CN103471595A - 一种面向ins/wsn室内移动机器人紧组合导航的迭代扩展rts均值滤波方法 - Google Patents

Abstract

本发明公开了一种面向INS／WSN室内移动机器人紧组合导航的迭代扩展RTS均值滤波方法，首先通过迭代扩展卡尔曼滤波将INS、WSN在本地相对坐标系中获取的导航信息进行数据融合，在此基础上提高导航信息的输出周期，将小周期内的数据先通过迭代扩展RTS平滑之后再进行均值滤波，最后将数据输出。迭代扩展RTS均值滤波算法的提出，有效提高了室内移动机器人的导航精度。

Description

一种面向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)所示： 

其中，Δvx为INS和WSN测量得到的东向速度之差；Δvy为INS和WSN测量得到的北向速度之差； $\mathrm{\Δ}{\mathrm{\ρ}}_i^2={\left({\mathrm{\ρ}}_i^{\mathrm{ins}}\right)}^2-{\left({\mathrm{\ρ}}_i^{\mathrm{wsn}}\right)}^2,{\mathrm{\ρ}}_i^{\mathrm{ins}}=\sqrt{{(x^{\mathrm{INS}}-x_{\mathrm{Ri}})}^2+{(y^{\mathrm{INS}}-y_{\mathrm{Ri}})}^2}$ 为INS测量的未知节点与参考节点之间的距离，(x_Ri，y_Ri)为已知第i个节点在相对坐标系中的位置，(x^INS，y^INS)为当前时刻INS自身测量得到的未知节点的位置，

为WSN通过测量TOA得到的未知节点与第i个参考节点之间的距离；其中，i=1，2，…，N，N为参考节点数目；v为系统的观测噪声； 

步骤2)，将所述迭代扩展卡尔曼滤波器工作的滤波周期定义为小周期，根据所述步骤1)经迭代扩展卡尔曼滤波后的数据，将小周期内的未知节点的位置、速度、加速度数据进行迭代扩展卡尔曼RTS平滑处理，提高导航信息的输出周期，得到INS测量误差的最优估计值，步骤如下： 

步骤2.1)，采用迭代扩展卡尔曼滤波的递归过程，作为迭代扩展卡尔曼RTS平滑的前向预测部分，得到的未知节点状态预估，其步骤为： 

X_k+1|k=AX_k|k   (3) 

其中，A为系统矩阵，X_k|k表示k时刻的系统状态，X_k+1|k表示在k时刻基础上预估的下一时刻的系统状态； 

步骤2.2)，在所述步骤2.1)的基础上进行n次迭代： 

$X_{k|k}^{n+1}=X_{k|k}^n+K_k^n[z_k-h^n\left(X_{k|k}^n\right)-H^n\left(X_{k|k}^n\right)\×(X_{k|k-1}-X_{k|k}^n)]---\left(4\right)$

$K_{k+1}^n=P_{k|k-1}{H}^n{\left(X_{k|k}^n\right)}^T{[H^n\left(X_{k|k}^n\right)P_{k|k-1}{H}^n{\left(X_{k|k}^n\right)}^T+R]}^{-1}---\left(5\right)$

$P_{k+1}^n=[I-K_{k+1}^n{H}^n\left(X_{k|k}^n\right)P_{k+1|k}]---\left(6\right)$

其中，n=1，2，...，j；为第k时刻进行第n步迭代时的滤波器增益，z_k为第k时刻系统的观测向量，

为第k时刻进行第n步迭代时的滤波器观测矩阵，

为第k时刻进行第n步迭代时的滤波器观测矩阵的雅克比矩阵，为第k时刻进行第n步迭代时的滤波器的概率矩阵，R为过噪声和测量噪声协方差矩阵； 

系统状态的最终状态预估如下： 

$X_{k+1|k+1}=X_{k|k}^j---\left(7\right)$

$P_{k+1|k+1}=P_{k+1}^j---\left(8\right)$

步骤2.3)，将导航解的输出周期定义为大周期，在大周期内对所述式(7)和式(8)得到的最终状态预估做逆向平滑，得到INS测量误差的最优估计，其步骤为： 

$K_k^S=P_{k+1|k+1}{\mathrm{AP}}_{k+1|k}^{-1}---\left(9\right)$

$X_k^S=X_{k+1|k+1}+K_k^S(X_{k+1}^S-X_{k+1|k})---\left(10\right)$

$P_k^S=P_k+K_k^S(P_{k+1}^S-P_{k+1|k}){\left(K_k^S\right)}^T---\left(11\right)$

其中，为逆向平滑过程中的状态变量，为逆向平滑过程中的滤波器增益，

为逆向平滑过程中的概率； 

步骤3)，将同一时刻的INS测量得到的未知节点的位置、速度、加速度，与所述步骤2)迭代扩展卡尔曼RTS平滑得到的INS测量误差的最优估计作差，得到小周期内INS测量的未知节点的位置、速度和加速度最优估计； 

步骤4)，将所述步骤3)得到的小周期内INS测量的未知节点的位置、速度和加速度最优估计的均值作为大周期的导航信息输出，得到最终的导航信息。 

有益效果： 

1.与传统的扩展卡尔曼RTS(Rauch-Tung-Striebel)平滑滤波(ERTS)相比，在平滑算法的前向滤波过程中采用迭代扩展卡尔曼滤波算法，以提高前向滤波的精度。 

2.将同一时刻的INS测量得到的未知节点的位置、速度和加速度与迭代扩展卡尔曼RTS平滑得到的INS测量误差的最优估计作差，得到小周期内INS测量的未知节点的位置、速度和加速度最优估计；并将小周期内得到的INS测量的未知节点的位置、速度和加速度最优估计的均值作为大周期的导航信息输出，得到相对精确地导航信息，减小了平滑算法需要的区间，提高了平滑算法的实时性。 

3.本发明方法可用于室内，地下矿井等密闭复杂环境下的移动机器人高精度目标定位跟踪。 

附图说明

图1是用于面向INS／WSN室内移动机器人紧组合导航的迭代扩展RTS均值滤波算法的系统示意图； 

图2是用于面向INS／WSN室内移动机器人紧组合导航的迭代扩展RTS均值滤波算法的控制方法示意图； 

图3是本发明的方法流程图。 

具体实施方式

下面结合附图对本发明做更进一步的解释。 

如图1所示，面向INS／WSN室内移动机器人紧组合导航的迭代扩展RTS均值滤波方法，包括参考(RS)节点部分和未知(BN)节点部分。参考节点部分由参考节点无线网 络接收模块、超声测距模块和时间同步模块组成，其中，参考节点无线网络接收模块主要负责完成远程命令的收发，在接到远程命令之后首先控制时间同步模块完成测距的时间同步，接着打开超声测距模块测量相应的距离信息，最后，将超声测距模块测量得到的距离信息通过参考节点无线网络接收模块返回给未知节点；未知节点部分由未知节点无线网络接收模块、INS导航模块和码盘组成，其中，未知节点上的未知节点无线网络接收模块主要负责完成未知节点与参考节点之间的距离信息的采集；INS导航模块主要完成的是加表信息的采集，码盘主要用于测量移动载体的切线速度。 

如图2所示，该方法首先进行导航信息数据融合： 

构建迭代扩展卡尔曼滤波的系统方程以INS每一时刻在本地相对坐标系中东向和北向的位置、速度和加速度误差作为状态变量；构建观测方程以每一时刻INS和WSN各自测量的未知节点与参考节点之间的距离的平方差和速度差作为观测量，对导航信息进行数据融合；在此基础上提高导航信息的输出周期，将同一时刻的INS测量得到的未知节点的位置、速度和加速度与迭代扩展卡尔曼RTS平滑得到的INS测量误差的最优估计作差，得到小周期内INS测量的未知节点的位置、速度和加速度最优估计；最后将小周期内得到的INS测量的未知节点的位置、速度和加速度最优估计通过均值滤波得到的均值作为大周期的导航信息输出，得到精确地导航信息。 

如图3所示，一种面向INS／WSN室内移动机器人紧组合导航的迭代扩展RTS均值滤波算法，具体步骤如下： 

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

步骤1.1)，通过INS采集加表信息，在采集到加表信息之后进行解算，获得INS测量的BN节点的位置和速度信息，测量RN节点到BN节点之间的距离； 

步骤1.2)，BN节点通过超声测距模块测量得到BN节点与步骤1.1)中相应的RN节点之间的距离。通过采集INS磁力计信息和码盘数据得到的BN点在本地相对坐标系中的速度信息，我们将此信息看作WSN测量的速度信息； 

步骤1.3)，在获取到上述信息之后，构建迭代扩展卡尔曼滤波器的系统方程，该系统方程以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为采样周期，ω为系统噪声； 

步骤1.4)，迭代扩展卡尔曼滤波的观测方程以每一时刻INS和WSN各自测量的未知节点与参考节点之间的距离的平方差和速度差作为观测量：通过INS测量每个时刻未知节点在x和y方向，即东向和北向的位置，将得到的位置信息与参考节点的位置信息通过距离公式计算出INS测量得到的未知节点和第i个RN节点之间的距离，通过WSN中包含的超声测距模块测量出未知节点和第i个RN节点之间的距离；观测方程如式(2)所示： 

其中，Δvx为INS和WSN测量得到的东向速度之差，Δvx=vx^ins-vx^wsn；Δvy为INS和WSN测量得到的北向速度之差，Δvy=vy^ins-vy^wsn；

为INS测量的未知节点与参考节点之间的距离，(x_Ri，y_Ri)为已知第i个节点在相对坐标系中的位置，(x^INS，y^INS)为当前时刻INS自身测量 得到的未知节点的位置，

为WSN通过测量TOA得到的未知节点与第i个参考节点之间的距离；以

作为滤波器的观测量；其中，i=1，2，…，N，N为参考节点数目；v为系统的观测噪声； 

步骤2)，将迭代扩展卡尔曼滤波器工作的滤波周期定义为小周期，导航数据经步骤1)迭代扩展卡尔曼滤波后，将小周期内的未知节点的位置、速度、加速度数据进行迭代扩展卡尔曼RTS平滑处理，提高导航信息的输出周期，得到INS测量误差的最优估计值，步骤如下： 

步骤2.1)，采用迭代扩展卡尔曼滤波的递归过程，作为迭代扩展卡尔曼RTS平滑的前向预测部分，得到的未知节点状态预估，该状态预估包括未知节点的位置预估、速度预估和加速度预估，其步骤为： 

X_k+1|k=AX_k|k   (3) 

其中，A为系统矩阵，X_k|k表示k时刻的系统状态，X_k+1|k表示在k时刻基础上预估的下一时刻的系统状态； 

其中，由式(1)可得： 

$A=\left[\begin{array}{cccccc}1& T& \frac{T^2}{2}& 0& 0& 0\\ 0& 1& T& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& T& \frac{T^2}{2}\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& 0& 1\end{array}\right];X_{k|k}=\begin{array}{c}\left[\begin{array}{c}{\mathrm{\δP}}_{E,k}\\ \mathrm{\δ}{V}_{E,k}\\ \mathrm{\δ}{\mathrm{Acc}}_{E,k}\\ \mathrm{\δ}{P}_{N,k}\\ \mathrm{\δ}{V}_{N,k}\\ \mathrm{\δ}{\mathrm{Acc}}_{N,k}\end{array}\right];X_{k+1|k}=\left[\begin{array}{c}\mathrm{\δ}{P}_{E,k+1}\\ \mathrm{\δ}{V}_{E,k+1}\\ \mathrm{\δ}{\mathrm{Acc}}_{E,k}\\ \mathrm{\δ}{P}_{N,k+1}\\ \mathrm{\δ}{V}_{N,k+1}\\ \mathrm{\δ}{\mathrm{Acc}}_{N,k}\end{array}\right];\end{array}$

步骤2.2)，在步骤2.1)的基础上进行n次迭代： 

$X_{k|k}^{n+1}=X_{k|k}^n+K_k^n[z_k-h^n\left(X_{k|k}^n\right)-H^n\left(X_{k|k}^n\right)\×(X_{k|k-1}-X_{k|k}^n)]---\left(4\right)$

$K_{k+1}^n=P_{k|k-1}{H}^n{\left(X_{k|k}^n\right)}^T{[H^n\left(X_{k|k}^n\right)P_{k|k-1}{H}^n{\left(X_{k|k}^n\right)}^T+R]}^{-1}---\left(5\right)$

$P_{k+1}^n=[I-K_{k+1}^n{H}^n\left(X_{k|k}^n\right)P_{k+1|k}]---\left(6\right)$

其中，n=1，2，...，j；其中，矩阵上的符号“n”代表第n次迭代；j为迭代次数，可以取任意整数；

为第k时刻进行第n步迭代时的滤波器增益，z_k为第k时刻系统 的观测向量，

为第k时刻进行第n步迭代时的滤波器观测矩阵，

为第k时刻进行第n步迭代时的滤波器观测矩阵的雅克比矩阵，

为第k时刻进行第n步迭代时的滤波器的概率矩阵；R为过噪声和测量噪声协方差矩阵； 

系统状态的最终状态预估如下： 

$X_{k+1|k+1}=X_{k|k}^j---\left(7\right)$

$P_{k+1|k+1}=P_{k+1}^j---\left(8\right)$

该最终状态预估包括未知节点的最终位置预估、最终速度预估和最终加速度预估； 

步骤2.3)，将导航解的输出周期定义为大周期，在大周期内对式(7)和式(8)得到的状态预估做逆向平滑，得到INS测量误差的最优估计，其步骤为： 

$K_k^S=P_{k+1|k+1}{\mathrm{AP}}_{k+1|k}^{-1}---\left(9\right)$

$X_k^S=X_{k+1|k+1}+K_k^S(X_{k+1}^S-X_{k+1|k})---\left(10\right)$

$P_k^S=P_k+K_k^S(P_{k+1}^S-P_{k+1|k}){\left(K_k^S\right)}^T---\left(11\right)$

其中，矩阵上的符号“S”代表逆向平滑；

为逆向平滑过程中的状态变量，

为逆向平滑过程中的滤波器增益，

为逆向平滑过程中的概率；测量误差的最优估计包括未知节点的位置测量误差的最优估、速度测量误差的最优估和加速度测量误差的最优估； 

步骤3)，将同一时刻的INS测量得到的未知节点的位置、速度、加速度，分别对应与步骤2)迭代扩展卡尔曼RTS平滑得到的INS位置测量误差的最优估、速度测量误差的最优估和加速度测量误差的最优估作差，得到小周期内INS测量的未知节点的位置、速度和加速度最优估计； 

步骤4)，将步骤3)得到的小周期内INS测量的未知节点的位置、速度和加速度最优估计通过均值滤波处理，得到的均值作为大周期的导航信息输出，得到最终的导航信息。 

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。 

Claims (1)

1.一种面向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)所示：

其中，Δvx为INS和WSN测量得到的东向速度之差；Δvy为INS和WSN测量得到的北向速度之差； $\mathrm{\Δ}{\mathrm{\ρ}}_i^2={\left({\mathrm{\ρ}}_i^{\mathrm{ins}}\right)}^2-{\left({\mathrm{\ρ}}_i^{\mathrm{wsn}}\right)}^2,{\mathrm{\ρ}}_i^{\mathrm{ins}}=\sqrt{{(x^{\mathrm{INS}}-x_{\mathrm{Ri}})}^2+{(y^{\mathrm{INS}}-y_{\mathrm{Ri}})}^2}$ 为INS测量的未知节点与参考节点之间的距离，(x_Ri，y_Ri)为已知第i个节点在相对坐标系中的位置，(x^INS，y^INS)为当前时刻INS自身测量得到的未知节点的位置，

为WSN通过测量TOA得到的未知节点与第i个参考节点之间的距离；其中，i=1，2，…，N，N为参考节点数目；v为系统的观测噪声；

步骤2)，将所述迭代扩展卡尔曼滤波器工作的滤波周期定义为小周期，根据所述步骤1)经迭代扩展卡尔曼滤波后的数据，将小周期内的未知节点的位置、速度、加速度数据进行迭代扩展卡尔曼RTS平滑处理，提高导航信息的输出周期，得到INS测量误差的最优估计值，步骤如下：

步骤2.1)，采用迭代扩展卡尔曼滤波的递归过程，作为迭代扩展卡尔曼RTS平滑的前向预测部分，得到的未知节点状态预估，其步骤为：

X_k+1|k=AX_k|k   (3)

其中，A为系统矩阵，X_k|k表示k时刻的系统状态，X_k+1|k表示在k时刻基础上预估的下一时刻的系统状态；

步骤2.2)，在所述步骤2.1)的基础上进行n次迭代：

$X_{k|k}^{n+1}=X_{k|k}^n+K_k^n[z_k-h^n\left(X_{k|k}^n\right)-H^n\left(X_{k|k}^n\right)\×(X_{k|k-1}-X_{k|k}^n)]---\left(4\right)$

$K_{k+1}^n=P_{k|k-1}{H}^n{\left(X_{k|k}^n\right)}^T{[H^n\left(X_{k|k}^n\right)P_{k|k-1}{H}^n{\left(X_{k|k}^n\right)}^T+R]}^{-1}---\left(5\right)$

$P_{k+1}^n=[I-K_{k+1}^n{H}^n\left(X_{k|k}^n\right)P_{k+1|k}]---\left(6\right)$

其中，n=1，2，...，j；

为第k时刻进行第n步迭代时的滤波器增益，z_k为第k时刻系统的观测向量，

为第k时刻进行第n步迭代时的滤波器观测矩阵，

为第k时刻进行第n步迭代时的滤波器观测矩阵的雅克比矩阵，

为第k时刻进行第n步迭代时的滤波器的概率矩阵；R为过噪声和测量噪声协方差矩阵；

系统状态的最终状态预估如下：

$X_{k+1|k+1}=X_{k|k}^j---\left(7\right)$

$P_{k+1|k+1}=P_{k+1}^j---\left(8\right)$

步骤2.3)，将导航解的输出周期定义为大周期，在大周期内对所述式(7)和式(8)得到的最终状态预估做逆向平滑，得到INS测量误差的最优估计，其步骤为：

$K_k^S=P_{k+1|k+1}{\mathrm{AP}}_{k+1|k}^{-1}---\left(9\right)$

$X_k^S=X_{k+1|k+1}+K_k^S(X_{k+1}^S-X_{k+1|k})---\left(10\right)$

$P_k^S=P_k+K_k^S(P_{k+1}^S-P_{k+1|k}){\left(K_k^S\right)}^T---\left(11\right)$

其中，

为逆向平滑过程中的状态变量，

为逆向平滑过程中的滤波器增益，

为逆向平滑过程中的概率；

步骤3)，将同一时刻的INS测量得到的未知节点的位置、速度、加速度，与所述步骤2)迭代扩展卡尔曼RTS平滑得到的INS测量误差的最优估计作差，得到小周期内INS测量的未知节点的位置、速度和加速度最优估计；

步骤4)，将所述步骤3)得到的小周期内INS测量的未知节点的位置、速度和加速度最优估计的均值作为大周期的导航信息输出，得到最终的导航信息。

Images