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

王 磊，程向紅，李雙喜

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

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

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

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

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

移動機器人；秩濾波；交互式多模型；組合導航

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

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

1 秩濾波算法

1.1 系統模型

考慮如下的非線性、非高斯離散系統：

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

1.2 秩采樣機制

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

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

1.3 秩濾波算法

第一步 時間更新：

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

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

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

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

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

4）狀態預測：

5）協方差陣預測：

第二步 量測更新

1）SVD因式分解：

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

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

4）量測預測：

5）預測新息協方差矩陣：

6）預測互協方差矩陣：

7）計算增益：

8）狀態估計：

9）協方差估計：

2 IMM-RKF算法

2.1 混合系統描述

設模型集合M，模型個數為r，k時刻的有效模式為mk, mk∈M，混合動態系統可描述為

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

其中，π為標量函數，滿足：

2.2 算法步驟

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

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

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

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

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

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

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

3 SINS/DGPS組合導航系統建模

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

3.1 SINS/DGPS非線性模型

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

速度誤差模型：

位置誤差模型：

SINS/DGPS組合導航系統非線性模型具體推導過程，可參考文獻[13][14]。

3.2 組合導航系統狀態方程和量測方程

取狀態變量為

慣性器件誤差方程為

式(29)～(32)組成了SINS/DGPS組合導航狀態方程。

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

4 仿真實驗

將提出的IMM-RKF算法應用于移動機器人SINS/DGPS組合導航系統，并進行仿真驗證。假設：初始位置為東經118°，北緯32°，海拔高度100 m，初始位置誤差為0 m；初始速度為0 m/s，初始速度誤差為0.05 m/s；SINS初始水平姿態角誤差為1°，航向角誤差為3°；陀螺隨機常值漂移為1 (°)/h，白噪聲隨機漂移為0.1(°)/h；加速度計偏置誤差為1 mg，白噪聲隨機漂移為0.1 mg。設所采用DGPS在通常環境中經、緯度測量誤差小于0.5 m，采樣周期為0.1 s，SINS采樣周期為1 ms。假設移動機器人在某區域中執行搜索任務，采用“割草機”行進模式，軌跡如圖2所示，運行時間為3600 s。為了模擬非結構化環境對傳感器測量精度的影響，在觀測信號中引入非高斯白噪聲，采用高斯混合模型產生[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算法進行濾波估計。根據前面所述仿真條件，將初始誤差方差矩陣P0、系統噪聲矩陣Q0和量測噪聲矩陣R0分別設置為：

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

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

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

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

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

圖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算法誤差較大。進一步的，將三種算法得到的位置估計誤差進行數值比較，采用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算法性能優于IMM-EKF和IMM-UKF算法。

5 結 論

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

（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]. 航空動力學報, 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] 傅惠民. 不完全數據秩分布理論[J]. 航空學報, 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組合導航濾波算法[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] 丁繼成, 黃衛權, 王野. 非高斯環境下的GPS自適應多徑抑制技術[J]. 航空學報, 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

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

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