基于交互式多模型秩滤波的移动机器人组合导航算法

王 磊，程向红，李双喜

（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