[发明专利]一种面向INS/WSN室内移动机器人紧组合导航的迭代扩展RTS均值滤波方法

说明书

技术领域

一种面向INS(惯性导航系统)／WSN(无线传感器网络)室内移动机器人紧组合导航的迭代扩展RTS均值滤波方法，属于复杂环境下组合定位技术领域。 

背景技术

近年来，随着计算机技术、信息技术、通讯技术、微电子技术和机器人技术的飞速发展，移动机器人技术的研究与应用取得了长足的进步，使其在许多场合被寄予了替代人类自动执行某些日常性与危险性任务的厚望，如物流仓储的搬运机器人，恶劣工作环境的生产机器人等。机器人的导航与定位作为实现机器人智能化和完全自主化的关键技术，逐渐成为目前该领域的研究热点。然而，在外界无线电信号微弱、电磁干扰强烈等一系列复杂室内环境中，对智能移动机器人导航信息获取的准确性、实时性及鲁棒性有很大的影响。如何将室内环境下获取的有限信息进行有效的融合以满足智能移动机器人高导航精度的要求，消除外界环境的影响，具有重要的科学理论意义和实际应用价值。 

在处理组合导航数据融合的问题时，最常用的方法是使用卡尔曼滤波(Kalman Filter，KF)，然而由于KF是面向线性系统设计的，因此仅对线性定常系统有很好的滤波效果。为了能够实现非线性系统的滤波问题，许多学者通过雅克比矩阵将非线性的系统转化为线性系统进行滤波，从而产生了扩展卡尔曼滤波器(Extended Kalman Filter，EKF)。EKF对非线性系统有很好的滤波效果，是目前实际生产中应用较为广泛的一种滤波方法。但是由于EKF在滤波过程中假设系统的状态噪声和观测噪声均为标准白噪声，所以并不完全符合实际应用中的情况。 

发明内容

发明目的：针对上述现有技术，提供一种面向INS／WSN室内移动机器人紧组合导航的迭代扩展RTS均值滤波方法，提高室内移动机器人在复杂室内环境中的导航精度。 

技术方案：一种面向INS／WSN室内移动机器人紧组合导航的迭代扩展RTS均值滤波方法，包括下列步骤： 

步骤1)，首先通过迭代扩展卡尔曼滤波将INS、WSN在本地相对坐标系中获取的导航信息进行数据融合： 

构建迭代扩展卡尔曼滤波器的系统方程，所述迭代扩展卡尔曼滤波的系统方程以 INS每一时刻在本地相对坐标系中东向和北向的位置、速度和加速度误差作为状态变量，系统方程如式(1)所示： 

其中，δP_E，k为k时刻的东向位置误差，δP_N，k为k时刻的北向位置误差，δV_E，k为k时刻的东向速度误差，δV_N，k为k时刻的北向速度误差，δAcc_E，k为k时刻的东向加速度误差，δAcc_N，k为k时刻的北向加速度误差，T为采样周期，ω为系统噪声； 

所述迭代扩展卡尔曼滤波的观测方程以每一时刻INS和WSN各自测量的未知节点与参考节点之间的距离的平方差和速度差作为观测量，观测方程如式(2)所示：