王 磊，程向紅，李雙喜

（1. 安徽科技學(xué)院 電氣與電子工程學(xué)院，蚌埠 233100；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院 微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室，南京 210096）

基于交互式多模型秩濾波的移動機器人組合導(dǎo)航算法

王 磊1，程向紅2，李雙喜1

（1. 安徽科技學(xué)院 電氣與電子工程學(xué)院，蚌埠 233100；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院 微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室，南京 210096）

針對非結(jié)構(gòu)化環(huán)境下移動機器人組合導(dǎo)航系統(tǒng)中存在的時變或非高斯噪聲，將秩濾波器(rank Kalman filter, RKF)與交互式多模型算法(interactive multiple model filter, IMM)相結(jié)合，提出一種交互式多模型秩濾波算法(IMM-RKF)。秩濾波根據(jù)秩統(tǒng)計量相關(guān)原理確定采樣點和權(quán)值，可適用于具有非高斯噪聲的非線性系統(tǒng)；交互式多模型算法是解決結(jié)構(gòu)和參數(shù)易發(fā)生變化系統(tǒng)中狀態(tài)估計問題的重要途徑，能夠抑制組合導(dǎo)航系統(tǒng)中時變噪聲引起的導(dǎo)航參數(shù)估計誤差。仿真實驗表明，相比于交互式多模型擴展卡爾曼濾波(IMM-EKF)和交互式多模型無跡卡爾曼濾波(IMM-UKF)，提出的IMM-RKF算法能夠提高組合導(dǎo)航系統(tǒng)姿態(tài)、速度和位置估計精度。

移動機器人；秩濾波；交互式多模型；組合導(dǎo)航

近年來，移動機器人替代人類在危險、惡劣環(huán)境中作業(yè)的步伐越來越快，其工作場合也越來越多地面向野外和行星表面等室外非結(jié)構(gòu)化環(huán)境。由于室外非結(jié)構(gòu)化環(huán)境具有的多樣性、隨機性與復(fù)雜性等特點，移動機器人需要具有更高的自主性，通過自身攜帶的傳感器感知周圍的環(huán)境，對感知到的信息進行處理，獲取自身的位置和姿態(tài)信息，然后實時進行路徑規(guī)劃和導(dǎo)航控制，完成各項作業(yè)任務(wù)[1-2]。因此，導(dǎo)航技術(shù)是實現(xiàn)移動機器人自動化作業(yè)的關(guān)鍵。本文以捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）和差分全球定位系統(tǒng)（DGPS）構(gòu)成的SINS/DGPS組合導(dǎo)航系統(tǒng)為背景，研究適用于非結(jié)構(gòu)化環(huán)境下移動機器人組合導(dǎo)航的信息融合算法。

Kalman濾波器一般適用于系統(tǒng)為線性、噪聲統(tǒng)計特性服從高斯分布并完全已知的情況；傳統(tǒng)的擴展卡爾曼濾波(extended Kalman filter，EKF)及其改進方法只能處理弱非線性系統(tǒng)及高斯噪聲條件下的狀態(tài)估計問題。為此，人們提出了一系列以貝葉斯濾波理論為框架，基于數(shù)值積分近似的非線性狀態(tài)估計方法[3]，其中具有代表性的方法包括中心差分卡爾曼濾波[4](central difference Kalman filter，CDKF)、無跡卡爾曼濾波[5](unscented Kalman filter，UKF)、求積分卡爾曼濾波[6](quadrature Kalman filter，QKF)、容積卡爾曼濾波[7](cubature Kalman filter，CKF)等。它們的共同特點是均假設(shè)系統(tǒng)中噪聲服從高斯分布，即為高斯濾波器。秩濾波器(rank Kalman filter，RKF)[8]具有與高斯濾波器相同的濾波結(jié)構(gòu)，它通過秩統(tǒng)計量原理確定采樣點和權(quán)值，因此還適用于具有非高斯噪聲的系統(tǒng)。交互式多模型(Interacting Multiple Model, IMM)算法以廣義偽貝葉斯算法為基礎(chǔ)，利用Markov切換系數(shù)實現(xiàn)各個模型之間的交互，兼具計算復(fù)雜度低和估計精度高的特點[9-11]。本文將秩濾波器與交互式多模型算法相結(jié)合，提出一種交互式多模型秩濾波算法(IMM-RKF)，并將其應(yīng)用到移動機器人組合導(dǎo)航系統(tǒng)中。

1 秩濾波算法

1.1 系統(tǒng)模型

考慮如下的非線性、非高斯離散系統(tǒng)：

式中：xk∈Rn和zk∈Rκ分別為n維狀態(tài)向量與κ維觀測向量；f(·)與h(·)為非線性函數(shù)；wk為k時刻的過程噪聲向量，方差矩陣為Qk；vk為k時刻的量測噪聲向量，方差為Rk。

1.2 秩采樣機制

由秩統(tǒng)計量相關(guān)原理[12]可以求得k時刻非高斯分布對應(yīng)的采樣點集{Χi,k-1}，表示為

式中，Χi,k-1為狀態(tài)向量x的第i個采樣點，采樣點個數(shù)為2ρn個；n為狀態(tài)向量x的維數(shù)；ρ為采樣點層數(shù)；為矩陣Pk-1平方根的第l列；為估計誤差所服從分布的一維標準分布概率pj的分位點；pj為第j層采樣點對應(yīng)的概率，一般可取值為pj=( j-0.3)/( 2m+1.4)或pj= j/( 2m+2)；rj為第j層采樣點的修正系數(shù)，滿足

1.3 秩濾波算法

第一步 時間更新：

1）通過SVD對協(xié)方差矩陣Pk-1|k-1取平方根：

式中：S為對角矩陣，S = diag[s1,s2, …, sn]；一般地，協(xié)方差陣Pk-1|k-1為對稱矩陣，因此U = V，特征值為的特征向量由矩陣UUT的列向量表示。經(jīng)過SVD分解，得：

2）求解一維標準分布概率pj對應(yīng)的分位點，通過

秩采樣機制求取采樣點集{Χi,k-1|k-1}：

3）將采樣點集{Χi,k-1|k-1}進行非線性傳播：

4）狀態(tài)預(yù)測：

5）協(xié)方差陣預(yù)測：

第二步 量測更新

1）SVD因式分解：

2）根據(jù)秩采樣機制得到采樣點集{Χi,k|k-1}：

3）將采樣點集{Χi,k|k-1}引入觀測方程，并進行非線性傳播：

4）量測預(yù)測：

5）預(yù)測新息協(xié)方差矩陣：

6）預(yù)測互協(xié)方差矩陣：

7）計算增益：

8）狀態(tài)估計：

9）協(xié)方差估計：

2 IMM-RKF算法

2.1 混合系統(tǒng)描述

設(shè)模型集合M，模型個數(shù)為r，k時刻的有效模式為mk, mk∈M，混合動態(tài)系統(tǒng)可描述為

式中，xk為狀態(tài)向量，zk為量測向量，wk與νk分別為與系統(tǒng)模式相關(guān)的過程噪聲和觀測噪聲序列。k-1時刻模型mk-1通過Markov鏈轉(zhuǎn)移至k時刻系統(tǒng)的模型mk，依賴于狀態(tài)的模型轉(zhuǎn)移概率為

其中，π為標量函數(shù)，滿足：

2.2 算法步驟

IMM-RKF算法的步驟同IMM算法一樣，主要包括輸入交互、模型濾波、模型概率更新和輸出交互四個環(huán)節(jié)，區(qū)別在于模型濾波過程中采用的是秩濾波算法，其算法結(jié)構(gòu)如圖1所示。

圖1 IMM-RKF算法結(jié)構(gòu)圖（模型個數(shù)r = 3）Fig.1 Structure chart of IMM-RKF algorithm(number of models r = 3)

1）概率混合。由k-1時刻的模型概率與先驗的Markov轉(zhuǎn)移概率πji進行交互，計算混合概率：

2）估計混合。對于第j=1,2,…,r個模型，重新初始化狀態(tài)與協(xié)方差陣：

步驟2 模型濾波。在獲得新的量測zk之后，利用上一步計算得到的重初始化狀態(tài)和協(xié)方差陣，進行狀態(tài)估計更新，采用RKF進行估計。

其中，Λi(k)表示k時刻模型mi為匹配模型的似然函數(shù)：

步驟4 輸出交互。對各濾波器估計值進行概率加權(quán)融合得到輸出結(jié)果：

3 SINS/DGPS組合導(dǎo)航系統(tǒng)建模

由于差分GPS消除了GPS系統(tǒng)大部分公共誤差源的影響，從而能大大提高了定位精度。慣性導(dǎo)航系統(tǒng)具有自主性強、輸出導(dǎo)航參數(shù)完備的優(yōu)點，將二者組合起來是一種理想的組合導(dǎo)航方式。

3.1 SINS/DGPS非線性模型

選取東北天坐標系為導(dǎo)航系，SINS/GPS組合導(dǎo)航的平臺誤差角模型為[13-14]：

速度誤差模型：

位置誤差模型：

SINS/DGPS組合導(dǎo)航系統(tǒng)非線性模型具體推導(dǎo)過程，可參考文獻[13][14]。

3.2 組合導(dǎo)航系統(tǒng)狀態(tài)方程和量測方程

取狀態(tài)變量為

慣性器件誤差方程為

式(29)～(32)組成了SINS/DGPS組合導(dǎo)航狀態(tài)方程。

取慣性測量單元和DGPS輸出的速度差和位置差作為量測值，量測方程表示如下：

4 仿真實驗

將提出的IMM-RKF算法應(yīng)用于移動機器人SINS/DGPS組合導(dǎo)航系統(tǒng)，并進行仿真驗證。假設(shè)：初始位置為東經(jīng)118°，北緯32°，海拔高度100 m，初始位置誤差為0 m；初始速度為0 m/s，初始速度誤差為0.05 m/s；SINS初始水平姿態(tài)角誤差為1°，航向角誤差為3°；陀螺隨機常值漂移為1 (°)/h，白噪聲隨機漂移為0.1(°)/h；加速度計偏置誤差為1 mg，白噪聲隨機漂移為0.1 mg。設(shè)所采用DGPS在通常環(huán)境中經(jīng)、緯度測量誤差小于0.5 m，采樣周期為0.1 s，SINS采樣周期為1 ms。假設(shè)移動機器人在某區(qū)域中執(zhí)行搜索任務(wù)，采用“割草機”行進模式，軌跡如圖2所示，運行時間為3600 s。為了模擬非結(jié)構(gòu)化環(huán)境對傳感器測量精度的影響，在觀測信號中引入非高斯白噪聲，采用高斯混合模型產(chǎn)生[15-16]：

其中，α1=0.8，R1=diag{(0.05m/s)2, (0.05m/s)2, (0.05m/s)2,(0.5m)2, (0.5m)2, (0.5m)2}，R2=diag{(0.1m/s)2, (0.1m/s)2,(0.1m/s)2, (1m)2, (1m)2, (1m)2}。

圖2 移動機器人仿真運行軌跡Fig.2 Simulation trajectory of mobile robot

仿真過程中，分別采用交互式多模型擴展卡爾曼濾波(IMM-EKF)、交互式多模型無跡卡爾曼濾波(IMM-UKF)和IMM-RKF算法進行濾波估計。根據(jù)前面所述仿真條件，將初始誤差方差矩陣P0、系統(tǒng)噪聲矩陣Q0和量測噪聲矩陣R0分別設(shè)置為：

將IMM-EKF、IMM-UKF 和IMM-RKF算法中固定模型集合均設(shè)置為RM=(R0, 3R0, 6R0)，分別對三種算法估計所得到的姿態(tài)角誤差、速度誤差和位置誤差進行比較，結(jié)果如圖3～圖9所示。

圖3 俯仰角誤差曲線Fig.3 The pitch angle error curves

圖4 橫搖角誤差曲線Fig.4 The roll angle error curves

由圖3～圖5姿態(tài)角誤差曲線可以看出：IMM-EKF、IMM-UKF 和IMM-RKF三種算法所得到的水平姿態(tài)角誤差均能收斂到很小的值并且收斂速度較快；航向角誤差的修正效果不是很明顯，但也能看出逐漸收斂的趨勢，收斂速度較慢。從航向角誤差的收斂過程可以看出，IMM-RKF算法估計效果優(yōu)于其它兩種算法。

從圖6、圖7速度誤差曲線可以看出，由于GPS提供的速度信息對SINS的速度誤差進行修正，采用IMM-RKF算法速度誤差基本保持在0.2 m/s以內(nèi)，明顯優(yōu)于其它兩種算法。

圖5 航向角誤差曲線Fig.5 The heading angle error curves

圖6 東向速度誤差曲線Fig.6 East velocity error curves

圖7 北向速度誤差曲線Fig.7 North velocity error curves

圖8 北向誤差曲線Fig.8 The latitude error curves

圖9 東向誤差曲線Fig.9 The longitude error curves

從圖8、圖9可以看出，IMM-UKF與IMM-RKF算法位置誤差較為接近，IMM-EKF算法誤差較大。進一步的，將三種算法得到的位置估計誤差進行數(shù)值比較，采用IMM-EKF算法得到的東向估計誤差均方差為0.76 m，最大達到0.98 m，北向估計誤差均方差為0.91 m，最大達到1.06 m。采用IMM-UKF算法得到的東向估計誤差為0.07 m，最大為0.59 m，北向估計誤差均方差為0.51 m，最大為0.74 m。采用IMM-RKF算法得到的東向估計誤差為0.02 m，最大為0.39 m，北向估計誤差均方差為0.32 m，最大為0.47 m。可以看出，IMM-RKF算法性能優(yōu)于IMM-EKF和IMM-UKF算法。

5 結(jié) 論

本文針對非結(jié)構(gòu)化環(huán)境中移動機器人組合導(dǎo)航系統(tǒng)中存在的時變或非高斯噪聲，將秩濾波算法和交互式多模型算法相結(jié)合，提出了一種適用于非線性、非高斯的交互式多模型秩濾波算法。秩濾波算法通過秩統(tǒng)計量相關(guān)原理確定采樣點和權(quán)值，因此，它不受高斯分布條件限制，將其作為交互式多模型算法中與各個模型相匹配的子濾波器，并得到交互式多模型秩濾波算法。最后，通過移動機器人SINS/DGPS組合導(dǎo)航系統(tǒng)對交互式多模型秩濾波算法進行了仿真驗證，結(jié)果表明，該算法相比于交互式多模型擴展卡爾曼濾波算法和交互式多模型無跡卡爾曼濾波算法具有更高的估計精度，證明了其優(yōu)越性。

（References）：

[1] Nishiwaki K, Chestnutt J, Kagami S. Autonomous navigation of a humanoid robot over unknown rough terrain[J]. International Journal of Robotics Research, 2017,31(11): 1251-1262.

[2] Hiremath S A, van der Heijden G, Evert F K V, et al. Laser range finder model for autonomous navigation of a robot in a maize field using a particle filter[J]. Computers and Electronics in Agriculture, 2014, 100(1): 41-50.

[3] Wang Lei, Cheng Xiang-hong. Algorithm of Gaussian sum filter based on high-order UKF for dynamic state estimation[J]. International Journal of Control Automation and Systems, 2015, 13(3): 652-661.

[4] Lim J, Shin M, Hwang W. Variants of extended Kalman filtering approaches for Bayesian tracking[J]. International Journal of Robust and Nonlinear Control, 2017, 27(2):319-346.

[5] Urrea C, Mu?oz R. Joints position estimation of a redundant scara robot by means of the unscented kalman filter and inertial sensors[J]. Asian Journal of Control, 2016,18(2): 481-493.

[6] Vilà-Valls J, Closas P, García-Fernández á F. Uncertainty exchange through multiple quadrature Kalman filtering[J].IEEE Signal Processing Letters, 2016, 23(12): 1825-1829.

[7] Zhao Ying-wei. Performance evaluation of Cubature Kalman filter in a GPS/IMU tightly-coupled navigation system[J]. Signal Processing, 2016, 119: 67-79.

[8] 傅惠民, 肖強, 婁泰山, 等. 非線性非高斯秩濾波方法[J]. 航空動力學(xué)報, 2015, 30(10): 2318-2322.Fu Hui-min, Xiao Qiang, Lou Tai-shan, et al. Nonlinear and non-Gaussian rank filer method[J]. Journal of Aerospace Power, 2015, 30(10): 2318-2322.

[9] Zhou Ling-feng, Dong Yan-qin, Zhao Wang-yang, et al. Novel SCNS/RSINS tight-integrated alignment based on adaptive interacting multiple model filters on shipboard[J]. Journal of Chinese Inertial Technology, 2016, 24(4): 464-472.

[10] Foo P H, Ng G W. Combining the interacting multiple model method with particle filters for manoeuvring target tracking with a multistatic radar system[J]. IET Radar Sonar and Navigation, 2011, 5(3): 234-255.

[11] Wang Lei, Cheng Xiang-hong, Li Shuang-xi, et al.Adaptive interacting multiple model filter for AUV integrated navigation[J]. Journal of Chinese Inertial Technology, 2016, 24(4): 511-516.

[12] 傅惠民. 不完全數(shù)據(jù)秩分布理論[J]. 航空學(xué)報, 1993,14(11): 578-584.Fu Hui-min. Theory of incomplete data rank distributions[J]. Acta Aeronautica et Astronautica Sinica, 1993, 14(11):578-584.

[13] Zhang Qiu-zhao, Meng Xiao-lin, Zhang Shu-bi, et al.Singular value decomposition-based robust cubature Kalman filtering for an integrated GPS/SINS navigation system[J]. Journal of Navigation, 2015, 68(3): 149-155.

[14] 孫楓, 唐李軍. 基于cubature Kalman filter的INS/GPS組合導(dǎo)航濾波算法[J]. 控制與決策, 2012, 27(7):1032-1036.Sun Feng, Tang Li-jun. INS/GPS integrated navigation filter algorithm based on cubature Kalman filter[J]. Control and Decision, 2012, 27(7): 1032-1036.

[15] Outamazirt F, Fu Li, Lin Yan, et al. A new SINS/GPS sensor fusion scheme for UAV localization problem using nonlinear SVSF with covariance derivation and an adaptive boundary layer[J]. Chinese Journal of Aeronautics,2016, 29(2): 424-440.

[16] 丁繼成, 黃衛(wèi)權(quán), 王野. 非高斯環(huán)境下的GPS自適應(yīng)多徑抑制技術(shù)[J]. 航空學(xué)報, 2014, 35(8): 2234-2242.Ding Ji-cheng, Huang Wei-quan, Wang Ye. GPS adaptive multipath mitigation technique in Non-Gaussian environment[J]. Acta Aeronautica et Astronautica Sinica, 2014,35(8): 2234-2242.

IMM-RKF algorithm and its application in integrated navigation system for agricultural robot

WANG Lei1, CHENG Xiang-hong2, LI Shuang-xi1
(1. School of Electrical and Electronic Engineering, Anhui Science and Technology University, Bengbu 233100, China;2. Key Laboratory of Micro Inertial instrument and Advanced Navigation technology,Ministry of Education, Southeast University, Nanjing 210096, China)

To solve the problem that the mobile robot’s integrated navigation system has time-varying or non-Gaussian noises, a novel algorithm named interactive multiple model rank Kalman filter (IMM-RKF) is proposed, which combines the rank Kalman filter (RKF) with the interactive multiple model algorithm (IMM).The RKF determines the sigma points and their associated weights according to principle of rank statistics, and can be used in nonlinear system with non-Gaussian noise to solve the state estimation problems. The IMM algorithm can solve state estimation problems for the systems with the variable structure and parameters, and then reduce the estimation errors of navigation parameters caused by time-varying system noise in the integrated navigation system. Simulation results show that, compared with the interactive multiple model extend Kalman filter and the interactive multiple model unscented Kalman filter, the proposed IMM-RKF algorithm can significantly improve the attitude, velocity and position estimation precisions of the integrated system.

mobile robot; rank Kalman filter; interactive multiple model; integrated navigation

U666.1

：A

1005-6734(2017)03-0328-06

10.13695/j.cnki.12-1222/o3.2017.03.009

2017-03-01；

：2017-05-26

國家自然科學(xué)基金（61374215）；安徽省自然科學(xué)基金（1708085QF146）；東南大學(xué)微慣性儀表與先進導(dǎo)航技術(shù)教育部重點實驗室（B類）開放基金資助項目（SEU-MIAN-201701）；安徽科技學(xué)院人才穩(wěn)定項目（DQWD201601）

王磊（1984—），男，講師，博士，從事組合導(dǎo)航、多傳感器信息融合算法研究。E-mail: frank_408@163.com