CN104374389A - 一种面向室内移动机器人的imu/wsn组合导航方法 - Google Patents

Abstract

本发明公开了一种面向室内移动机器人的IMU/WSN组合导航方法，包括：构建相对坐标系；得到当前时刻该无线通信通道的未知节点与参考节点之间的距离平方和距离平方变化率的最优估计；得到当前时刻IMU解算得到的未知节点与参考节点之间的距离平方和距离平方变化率的最优估计；构建主滤波器；最终得到当前时刻的未知节点的位置、速度和姿态的最优估计。本发明有益效：在WSN和低成本IMU中都采用了局部滤波器，有效的提高了传感器所采集数据的精度，有助于后续主滤波器进行数据融合过程中精度的提升。主滤波器与传统移动机器人定位领域的位置、速度和加速度误差相比，对系统的运行状态描述更加精确，有助于提高数据融合的精度。

Description

一种面向室内移动机器人的IMU/WSN组合导航方法

技术领域

本发明属于复杂环境下组合定位技术领域，具体涉及一种面向室内移动机器人的IMU/WSN组合导航方法。

背景技术

近年来，随着计算机技术、信息技术、通讯技术、微电子技术的飞速发展，面向小区域的目标跟踪技术的研究与应用，逐渐成为目前该领域的研究热点。然而，在外界无线电信号微弱、电磁干扰强烈等一系列复杂室内环境中，对目标载体导航信息获取的准确性、实时性及鲁棒性有很大的影响。如何将室内环境下获取的有限信息进行有效的融合以满足小区域目标高导航精度的要求，消除外界环境的影响，具有重要的科学理论意义和实际应用价值。

在现有的定位方式中，全球卫星导航系统(Global Navigation Satellite System,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。

近年来，WSN以其低成本、低功耗和低系统复杂度的特点在短距离局部定位领域表现出很大的潜力。伴随着全国范围内无线网络的普及和使用，学者们提出将基于WSN的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，WSN信号十分容易受到干扰而导致定位精度下降甚至失锁；除此之外，由于WSN采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于WSN的目标跟踪在室内导航领域仍旧面临很多挑战。

为了克服上述两种导航方法需要参考节点并容易产生失锁的缺点，有学者提出将惯性测量单元(Inertial measurement unit,IMU)应用于小区域目标跟踪领域。IMU具有全自主、运动信息全面、短时、高精度的优点，虽然可以实现自主导航，但误差随时间积累，长航时运行条件下将会导致导航精度严重下降。

发明内容

本发明的目的就是为了解决上述问题，提出了面向室内移动机器人的IMU/WSN组合导航方法，该方法能够有效的提高室内移动机器人的导航精度，可用于室内，地下矿井等密闭复杂环境下的长距离高精度目标定位跟踪。

为了实现上述目的，本发明采用如下技术方案：

一种面向室内移动机器人的IMU/WSN组合导航方法，包括以下步骤：

(1)选取导航区域内任意一个参考节点的位置作为坐标原点，并分别选取x向和y向构建相对坐标系；

(2)在WSN系统的每个无线通信通道中加入第一局部滤波器，通过第一局部滤波器分别得到当前时刻该无线通信通道的未知节点与参考节点之间的距离平方和距离平方变化率的最优估计；

(3)根据惯性测量单元IMU采集得到的传感器数据判断移动机器人的运动状态，当移动机器人处于静止状态时，利用第二局部滤波器对IMU的解算误差进行补偿，得到当前时刻IMU解算得到的未知节点与参考节点之间的距离平方和距离平方变化率的最优估计；

(4)分别以步骤(2)和步骤(3)得到的当前时刻未知节点与参考节点之间的距离平方之差以及距离平方变化率之差作为观测量，构建主滤波器；

(5)将惯性测量单元IMU采集到的当前时刻的未知节点的位置和速度与主滤波器输出的惯性测量单元IMU的解算误差做差，最终得到当前时刻的未知节点的位置、速度和姿态的最优估计。

所述步骤(2)中的第一滤波器的状态方程具体为：

$\left[\begin{array}{c}{\mathrm{\ρ}}_{k+1}^i\\ {\stackrel{\·}{\mathrm{\ρ}}}_{k+1}^i\end{array}\right]=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ρ}}_k^i\\ {\stackrel{\·}{\mathrm{\ρ}}}_k^i\end{array}\right]+W$

其中，为WSN系统的第i个无线通道在k时刻的距离未知节点的距离，为WSN系统的第i个无线通道在k时刻的距离未知节点的距离变化率，T为第一滤波器的采样周期，W为第一滤波器的状态噪声矩阵。

所述步骤(2)中的第一滤波器的观测方程具体为：

$\left[\begin{array}{c}{\widehat{\mathrm{\ρ}}}_k^i\\ {\widehat{\stackrel{\·}{\mathrm{\ρ}}}}_k^i\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ρ}}_k^i\\ {\stackrel{\·}{\mathrm{\ρ}}}_k^i\end{array}\right]+v_{\mathrm{loc}1}$

其中，为WSN系统实际测量得到的第i个无线通道在k时刻的距离未知节点的距离，为WSN系统实际测量得到的第i个无线通道在k时刻的距离未知节点的距离变化率，v_loc1为滤波器的观测噪声矩阵。

所述步骤(3)中根据惯性测量单元IMU采集得到的传感器数据判断移动机器人的运动状态的具体方法为：

通过将惯性测量单元IMU的加速度传感器采集到的数据计算当前移动机器人的加速度模值：

$\left|\right|\mathrm{Acc}\left|\right|=\sqrt{a_x^2+a_y^2+a_z^2}$

通过加速度模值完成对移动机器人运动状态的判断，即：

其中，a_x、a_y、a_z分别为惯性测量单元IMU所搭载的加速度传感器在载体坐标系下采集到的x、y和z三个方向的加速度值。

所述步骤(3)中第二滤波器的状态方程具体为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}^n=-{\mathrm{\Θ}}^n\×C_b^{n^{\′}}{\hat{f}}^b+{\▿}^n\\ \stackrel{\·}{\mathrm{\φ}}=-{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{\·}{P}}^n=\mathrm{\δ}{V}^n\\ {\stackrel{\·}{\▿}}^n=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}^n=0\end{array}\right.$

其中， ${\mathrm{\Θ}}^n\×=\left[\begin{array}{ccc}0& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 0& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 0\end{array}\right];$ δVⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差变化率；φ＝[φ_E φ_N φ_U]为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差变化率；δPⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差变化率；▽ⁿ为理想导航坐标系下的东向、北向和天向三个方向的加速度误差；为理想导航坐标系下的东向、北向和天向三个方向的加速度误差变化率；εⁿ为理想导航坐标系下的东向、北向和天向三个方向的角速度误差；为理想导航坐标系下的东向、北向和天向三个方向的角速度误差变化率；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵。

所述步骤(3)中第二滤波器的观测方程具体为：

$\mathrm{\δ}{\hat{V}}^n=\mathrm{\δ}{V}^n+v_{\mathrm{loc}2}$

其中，为当前时刻观测到的惯性测量单元IMU解算的速度误差，δVⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的x、y和z轴三个方向的速度误差，v_loc2为滤波器的观测噪声矩阵。

所述步骤(4)中主滤波器的状态方程具体为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}^n=-{\mathrm{\Θ}}^n\×C_b^{n^{\′}}{\hat{f}}^b+{\▿}^n\\ \stackrel{\·}{\mathrm{\φ}}=-{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{\·}{P}}^n=\mathrm{\δ}{V}^n\\ {\stackrel{\·}{\▿}}^n=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}^n=0\end{array}\right.$

其中， ${\mathrm{\Θ}}^n\×=\left[\begin{array}{ccc}0& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 0& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 0\end{array}\right];$ 为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差；δVⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差变化率；φ＝[φ_E φ_N φ_U]为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差变化率；δPⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差变化率；▽ⁿ为理想导航坐标系下的东向、北向和天向三个方向的加速度误差；为理想导航坐标系下的东向、北向和天向三个方向的加速度误差变化率；εⁿ为理想导航坐标系下的东向、北向和天向三个方向的角速度误差；为理想导航坐标系下的东向、北向和天向三个方向的角速度误差变化率；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵。

所述步骤(4)中主滤波器的观测方程具体为：

$\left[\begin{array}{c}{\mathrm{\δd}}_1^2\\ {\mathrm{\δd}}_2^2\\ .\\ .\\ .\\ {\mathrm{\δd}}_m^2\\ {\mathrm{\δ}\stackrel{\·}{d}}_1^2\\ {\mathrm{\δ}\stackrel{\·}{d}}_2^2\\ .\\ .\\ .\\ {\mathrm{\δ}\stackrel{\·}{d}}_m^2\end{array}\right]=\left[\begin{array}{c}2(x_k^I-x_1)\mathrm{\δx}+2(y_k^I-y_1)\mathrm{\δy}+2(z_k^I-z_1)\mathrm{\δz}-({\mathrm{\δx}}^2+{\mathrm{\δy}}^2+{\mathrm{\δz}}^2)\\ 2(x_k^I-x_2)\mathrm{\δx}+2(y_k^I-y_2)\mathrm{\δy}+2(z_k^I-z_2)\mathrm{\δz}-({\mathrm{\δx}}^2+{\mathrm{\δy}}^2+{\mathrm{\δz}}^2)\\ .\\ .\\ .\\ 2(x_k^I-x_m)\mathrm{\δx}+2(y_k^I-y_m)\mathrm{\δy}+2(z_k^I-z_m)\mathrm{\δz}-({\mathrm{\δx}}^2+{\mathrm{\δy}}^2+{\mathrm{\δz}}^2)\\ 2v{x}_k^I\mathrm{\δx}+2(x_k^I-x_1)\mathrm{\δvx}-2\mathrm{\δx\δvx}+2v{y}_k^I\mathrm{\δy}+2(y_k^I-y_1)\mathrm{\δvy}-2\mathrm{\δy\δvy}+2v{z}_k^I\mathrm{\δz}+2(z_k^I-z_1)\mathrm{\δvz}-2\mathrm{\δz\δvz}\\ 2v{x}_k^I\mathrm{\δx}+2(x_k^I-x_2)\mathrm{\δvx}-2\mathrm{\δx\δvx}+2v{y}_k^I\mathrm{\δy}+2(y_k^I-y_2)\mathrm{\δvy}-2\mathrm{\δy\δvy}+2v{z}_k^I\mathrm{\δz}+2(z_k^I-z_2)\mathrm{\δvz}-2\mathrm{\δz\δvz}\\ .\\ .\\ .\\ 2v{x}_k^I\mathrm{\δx}+2(x_k^I-x_m)\mathrm{\δvx}-2\mathrm{\δx\δvx}+2v{y}_k^I\mathrm{\δy}+2(y_k^I-y_m)\mathrm{\δvy}-2\mathrm{\δy\δvy}+2v{z}_k^I\mathrm{\δz}+2(z_k^I-z_m)\mathrm{\δvz}-2\mathrm{\δz\δvz}\end{array}\right]+v$

其中，为k时刻低成本IMU解算的未知节点的x、y和z向的位置，(x_i,y_i,z_i)为第i个参考节点的x、y和z向的位置；(δx,δy,δz)为k时刻低成本IMU解算的位置误差，即向量δPⁿ的标量元素；(δvx,δvy,δvz)为k时刻低成本IMU解算的速度误差，即向量δVⁿ的标量元素；为k时刻低成本IMU和WSN分别测量得到的未知节点与参考节点之间的距离平方的差；为k时刻低成本IMU和WSN分别测量得到的未知节点与参考节点之间的距离平方变化率的差；v为滤波器的观测噪声矩阵。

所述步骤(4)中主滤波器采用15维状态变量来对移动机器人的位置、速度和姿态进行描述，所述15维状态变量具体包括：在x、y、z三个方向的姿态角误差、位置误差、速度误差、加速度误差和角速度误差。

本发明的有益效果是：

1、本发明方法在WSN和低成本IMU中都采用了局部滤波器，有效的提高了传感器所采集数据的精度，有助于后续主滤波器进行数据融合过程中精度的提升。

2、主滤波器采用了姿态角误差、位置误差、速度误差、加速度误差和角速度误差作为状态变量，与传统移动机器人定位领域的位置、速度和加速度误差相比，对系统的运行状态描述更加精确，有助于提高数据融合的精度。

3、在主滤波器中以INS的测量值与其误差的差值替代参数的真实值，减少了传统紧组合方法由于泰勒展开后忽略二次项对定位精度造成的影响。

4、该紧组合方法中将距离平方变化率作为数据滤波器的观测向量，无需新添设备就可以完成对目标节点速度误差的预估，进而完成对目标节点的速度预估。

5、为了提高主滤波器的定位精度，在WSN的每一个通信信道与低成本IMU的解算过程中都加入了局部滤波器，更好的提高了传感器所采集数据的精度。

6、本发明使用MPU6050作为导航系统的惯性导器件，降低组合导航系统的成本。

7、可用于室内，地下矿井等密闭复杂环境下的高精度目标定位跟踪。

附图说明

图1为面向室内移动机器人的低成本IMU/WSN组合导航方法的系统示意图；

图2为面向室内移动机器人的低成本IMU/WSN组合导航方法的示意图；

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

具体实施方式：

下面结合附图与实施例对本发明做进一步说明：

单一的导航方式容易受到目标周围导航环境的影响，不能提供持续稳定的高精度导航信息,为了克服单一导航技术存在的不同程度的缺陷，本发明提出一种面向室内移动机器人的低成本IMU/WSN组合导航方法。

面向室内移动机器人的低成本IMU/WSN组合导航系统，如图1所示，包括参考节点部分和未知节点部分。参考节点部分由多个无线接收模块组成，主要负责接收未知节点所发出的距离测量命令并测量未知节点与参考节点之间的距离；未知节点部分由无线发送模块、无线路由模块、IMU模块和控制模块组成，其中无线发送模块主要负责无线测距命令的发送；无线路由模块是整个WSN与外界通信的接口；主要负责与控制模块进行数据通信；IMU模块主要负责通过自身的传感器来解算当前时刻的载体的位置、速度和姿态等导航信息；控制模块主要负责控制整个导航系统的其他模块，以及与外界进行数据通信。

面向室内移动机器人的低成本IMU/WSN组合导航方法的示意图如图2所示，以一个参考节点的位置作为坐标原点，并选取相对坐标系中的x向、y向和z向。在WSN系统的无线通信通道中与低成本IMU中利用局部滤波器分别对未知节点与参考节点之间的距离平方和距离平方变化率和低成本IMU静止状态下的解算误差进行预估。在此基础上，以每一时刻低成本IMU和WSN各自测量的未知节点与参考节点之间的距离平方之差和距离平方变化率之差作为观测量，构建采用15维状态向量的主滤波器，将低成本IMU采集到的当前时刻的未知节点的位置和速度与主滤波器输出的低成本IMU解算误差做差，最终得到当前时刻的未知节点的位置、速度和姿态的最优估计。

本发明方法的具体流程图如图3所示，包括以下步骤：

(1)选取任意一个参考节点的位置作为坐标原点，并选取相对坐标系中的x向和y向；

(2)在WSN系统的每个无线通信通道中加入了局部滤波器，通过局部滤波器得到当前时刻该无线通信通道的未知节点与参考节点之间的距离平方和距离平方变化率的最优估计，局部滤波器的状态方程具体为：

$\left[\begin{array}{c}{\mathrm{\ρ}}_{k+1}^i\\ {\stackrel{\·}{\mathrm{\ρ}}}_{k+1}^i\end{array}\right]=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ρ}}_k^i\\ {\stackrel{\·}{\mathrm{\ρ}}}_k^i\end{array}\right]+W---\left(1\right)$

其中，为WSN系统的第i个无线通道在k时刻的距离未知节点的距离，为WSN系统的第i个无线通道在k时刻的距离未知节点的距离变化率，T为滤波器的采样周期，W为滤波器的状态噪声矩阵；

局部滤波器的观测方程为：

$\left[\begin{array}{c}{\widehat{\mathrm{\ρ}}}_k^i\\ {\widehat{\stackrel{\·}{\mathrm{\ρ}}}}_k^i\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ρ}}_k^i\\ {\stackrel{\·}{\mathrm{\ρ}}}_k^i\end{array}\right]+v_{\mathrm{loc}1}---\left(2\right)$

其中，为WSN系统实际测量得到的第i个无线通道在k时刻的距离未知节点的距离，为WSN系统实际测量得到的第i个无线通道在k时刻的距离未知节点的距离变化率，v_loc1为滤波器的观测噪声矩阵。

(3)通过将低成本IMU的3个加速度传感器所采集到的数据通过下式进行计算，得到当前移动机器人的加速度模值：

$\left|\right|\mathrm{Acc}\left|\right|=\sqrt{a_x^2+a_y^2+a_z^2}---\left(3\right)$

通过加速度模值完成对移动机器人运动状态的判断，即：

在完成对移动机器人运动状态的判断之后，当移动机器人处于静止状态时，所采用的局部滤波器的状态方程具体为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}^n=-{\mathrm{\Θ}}^n\×C_b^{n^{\′}}{\hat{f}}^b+{\▿}^n\\ \stackrel{\·}{\mathrm{\φ}}=-{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{\·}{P}}^n=\mathrm{\δ}{V}^n\\ {\stackrel{\·}{\▿}}^n=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}^n=0\end{array}\right.---\left(5\right)$

其中， ${\mathrm{\Θ}}^n\×=\left[\begin{array}{ccc}0& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 0& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 0\end{array}\right];$ δVⁿ为低成本IMU测量得到的理想导航坐标系下的x、y和z轴三个方向的速度误差；φ为低成本IMU测量得到的理想导航坐标系下的x、y和z轴三个方向的姿态误差；δPⁿ为低成本IMU测量得到的理想导航坐标系下的x、y和z轴三个方向的位置误差；▽ⁿ为理想导航坐标系下的x、y和z轴三个方向的加速度误差；εⁿ为理想导航坐标系下的x、y和z轴三个方向的角速度误差；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵；

局部滤波器的观测方程具体为：

$\mathrm{\δ}{\hat{V}}^n=\mathrm{\δ}{V}^n+v_{\mathrm{loc}2}---\left(6\right)$

其中，为当前时刻观测到的低成本IMU解算的速度误差，由于本局部滤波器工作时，移动机器人处于静止状态，因此当前时刻的理论值为零，故低成本IMU测量得到的速度均为IMU的解算误差，v_loc2为滤波器的观测噪声矩阵。

(4)以每一时刻低成本IMU和WSN各自测量的未知节点与参考节点之间的距离平方之差和距离平方变化率之差作为观测量，构建采用15维状态向量的主滤波器，主滤波器的状态方程具体为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}^n=-{\mathrm{\Θ}}^n\×C_b^{n^{\′}}{\hat{f}}^b+{\▿}^n\\ \stackrel{\·}{\mathrm{\φ}}=-{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{\·}{P}}^n=\mathrm{\δ}{V}^n\\ {\stackrel{\·}{\▿}}^n=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}^n=0\end{array}\right.---\left(7\right)$

对移动机器人的导航信息解算误差的描述，传统上普遍采用三个方向的位置、速度和加速度的9维状态变量来描述，为了更加准确的描述移动机器人的运动状态和导航解算误差，本发明采用15维状态变量来对移动机器人的位置、速度和姿态进行描述；

主滤波器的观测方程为：

$\left[\begin{array}{c}{\mathrm{\δd}}_1^2\\ {\mathrm{\δd}}_2^2\\ .\\ .\\ .\\ {\mathrm{\δd}}_m^2\\ {\mathrm{\δ}\stackrel{\·}{d}}_1^2\\ {\mathrm{\δ}\stackrel{\·}{d}}_2^2\\ .\\ .\\ .\\ {\mathrm{\δ}\stackrel{\·}{d}}_m^2\end{array}\right]=\left[\begin{array}{c}2(x_k^I-x_1)\mathrm{\δx}+2(y_k^I-y_1)\mathrm{\δy}+2(z_k^I-z_1)\mathrm{\δz}-({\mathrm{\δx}}^2+{\mathrm{\δy}}^2+{\mathrm{\δz}}^2)\\ 2(x_k^I-x_2)\mathrm{\δx}+2(y_k^I-y_2)\mathrm{\δy}+2(z_k^I-z_2)\mathrm{\δz}-({\mathrm{\δx}}^2+{\mathrm{\δy}}^2+{\mathrm{\δz}}^2)\\ .\\ .\\ .\\ 2(x_k^I-x_m)\mathrm{\δx}+2(y_k^I-y_m)\mathrm{\δy}+2(z_k^I-z_m)\mathrm{\δz}-({\mathrm{\δx}}^2+{\mathrm{\δy}}^2+{\mathrm{\δz}}^2)\\ 2v{x}_k^I\mathrm{\δx}+2(x_k^I-x_1)\mathrm{\δvx}-2\mathrm{\δx\δvx}+2v{y}_k^I\mathrm{\δy}+2(y_k^I-y_1)\mathrm{\δvy}-2\mathrm{\δy\δvy}+2v{z}_k^I\mathrm{\δz}+2(z_k^I-z_1)\mathrm{\δvz}-2\mathrm{\δz\δvz}\\ 2v{x}_k^I\mathrm{\δx}+2(x_k^I-x_2)\mathrm{\δvx}-2\mathrm{\δx\δvx}+2v{y}_k^I\mathrm{\δy}+2(y_k^I-y_2)\mathrm{\δvy}-2\mathrm{\δy\δvy}+2v{z}_k^I\mathrm{\δz}+2(z_k^I-z_2)\mathrm{\δvz}-2\mathrm{\δz\δvz}\\ .\\ .\\ .\\ 2v{x}_k^I\mathrm{\δx}+2(x_k^I-x_m)\mathrm{\δvx}-2\mathrm{\δx\δvx}+2v{y}_k^I\mathrm{\δy}+2(y_k^I-y_m)\mathrm{\δvy}-2\mathrm{\δy\δvy}+2v{z}_k^I\mathrm{\δz}+2(z_k^I-z_m)\mathrm{\δvz}-2\mathrm{\δz\δvz}\end{array}\right]+v---\left(8\right)$

其中，为k时刻低成本IMU解算的未知节点的x、y和z向的位置，(x_i,y_i,z_i)为第i个参考节点的x、y和z向的位置；(δx,δy,δz)为k时刻低成本IMU解算的位置误差(即向量δPⁿ的标量元素)；(δvx,δvy,δvz)为k时刻低成本IMU解算的速度误差(即向量δVⁿ的标量元素)；为k时刻低成本IMU和WSN分别测量得到的未知节点与参考节点之间的距离平方的差；为k时刻低成本IMU和WSN分别测量得到的未知节点与参考节点之间的距离平方变化率的差；v为滤波器的观测噪声矩阵。

(5)将低成本IMU采集到的当前时刻的未知节点的位置和速度与主滤波器输出的低成本IMU解算误差做差，最终得到当前时刻的未知节点的位置、速度和姿态的最优估计。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (9)

1.一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，包括以下步骤：

(1)选取导航区域内任意一个参考节点的位置作为坐标原点，并分别选取x向和y向构建相对坐标系；

(2)在WSN系统的每个无线通信通道中加入第一局部滤波器，通过第一局部滤波器分别得到当前时刻该无线通信通道的未知节点与参考节点之间的距离平方和距离平方变化率的最优估计；

(3)根据惯性测量单元IMU采集得到的传感器数据判断移动机器人的运动状态，当移动机器人处于静止状态时，利用第二局部滤波器对IMU的解算误差进行补偿，得到当前时刻IMU解算得到的未知节点与参考节点之间的距离平方和距离平方变化率的最优估计；

(4)分别以步骤(2)和步骤(3)得到的当前时刻未知节点与参考节点之间的距离平方之差以及距离平方变化率之差作为观测量，构建主滤波器；

(5)将惯性测量单元IMU采集到的当前时刻的未知节点的位置和速度与主滤波器输出的惯性测量单元IMU的解算误差做差，最终得到当前时刻的未知节点的位置、速度和姿态的最优估计。

2.如权利要求1所述的一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，所述步骤(2)中的第一滤波器的状态方程具体为：

$\left[\begin{array}{c}{\mathrm{\ρ}}_{k+1}^i\\ {\stackrel{.}{\mathrm{\ρ}}}_{k+1}^i\end{array}\right]=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ρ}}_k^i\\ {\stackrel{.}{\mathrm{\ρ}}}_k^i\end{array}\right]+W$

其中，为WSN系统的第i个无线通道在k时刻的距离未知节点的距离，为WSN系统的第i个无线通道在k时刻的距离未知节点的距离变化率，T为第一滤波器的采样周期，W为第一滤波器的状态噪声矩阵。

3.如权利要求1所述的一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，所述步骤(2)中的第一滤波器的观测方程具体为：

$\left[\begin{array}{c}{\widehat{\mathrm{\ρ}}}_k^i\\ {\widehat{\stackrel{.}{\mathrm{\ρ}}}}_k^i\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{\ρ}}_k^i\\ {\stackrel{.}{\mathrm{\ρ}}}_k^i\end{array}\right]+v_{\mathrm{loc}1}$

其中，为WSN系统实际测量得到的第i个无线通道在k时刻的距离未知节点的距离，为WSN系统实际测量得到的第i个无线通道在k时刻的距离未知节点的距离变化率，v_loc1为滤波器的观测噪声矩阵。

4.如权利要求1所述的一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，所述步骤(3)中根据惯性测量单元IMU采集得到的传感器数据判断移动机器人的运动状态的具体方法为：

通过将惯性测量单元IMU的加速度传感器采集到的数据计算当前移动机器人的加速度模值：

$\left|\right|\mathrm{Acc}\left|\right|=\sqrt{a_x^2+a_y^2+a_z^2}$

通过加速度模值完成对移动机器人运动状态的判断，即：

其中，a_x、a_y、a_z分别为惯性测量单元IMU所搭载的加速度传感器在载体坐标系下采集到的x、y和z三个方向的加速度值。

5.如权利要求1所述的一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，所述步骤(3)中第二滤波器的状态方程具体为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{.}{V}}^n=-{\mathrm{\Θ}}^n\×C_b^{n^{\′}}{\hat{f}}^b+{\▿}^n\\ \stackrel{.}{\mathrm{\φ}}=-{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{.}{P}}^n=\mathrm{\δ}{V}^n\\ {\stackrel{.}{\▿}}^n=0\\ {\stackrel{.}{\mathrm{\ϵ}}}^n=0\end{array}\right.$

其中， ${\mathrm{\Θ}}^n\×=\left[\begin{array}{ccc}0& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 0& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 0\end{array}\right];$ δVⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差变化率；φ＝[φ_E φ_N φ_U]为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差变化率；δPⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差变化率；▽ⁿ为理想导航坐标系下的东向、北向和天向三个方向的加速度误差；为理想导航坐标系下的东向、北向和天向三个方向的加速度误差变化率；εⁿ为理想导航坐标系下的东向、北向和天向三个方向的角速度误差；为理想导航坐标系下的东向、北向和天向三个方向的角速度误差变化率；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵。

6.如权利要求1所述的一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，所述步骤(3)中第二滤波器的观测方程具体为：

$\mathrm{\δ}{\hat{V}}^n=\mathrm{\δ}{V}^n+v_{\mathrm{loc}2}$

其中，为当前时刻观测到的惯性测量单元IMU解算的速度误差，δVⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的x、y和z轴三个方向的速度误差，v_loc2为滤波器的观测噪声矩阵。

7.如权利要求1所述的一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，所述步骤(4)中主滤波器的状态方程具体为：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{.}{V}}^n=-{\mathrm{\Θ}}^n\×C_b^{n^{\′}}{\hat{f}}^b+{\▿}^n\\ \stackrel{.}{\mathrm{\φ}}=-{\mathrm{\ϵ}}^n\\ \mathrm{\δ}{\stackrel{.}{P}}^n=\mathrm{\δ}{V}^n\\ {\stackrel{.}{\▿}}^n=0\\ {\stackrel{.}{\mathrm{\ϵ}}}^n=0\end{array}\right.$

其中， ${\mathrm{\Θ}}^n\×=\left[\begin{array}{ccc}0& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 0& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 0\end{array}\right];$ 为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差；δVⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差变化率；φ＝[φ_E φ_N φ_U]为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差变化率；δPⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差变化率；▽ⁿ为理想导航坐标系下的东向、北向和天向三个方向的加速度误差；为理想导航坐标系下的东向、北向和天向三个方向的加速度误差变化率；εⁿ为理想导航坐标系下的东向、北向和天向三个方向的角速度误差；为理想导航坐标系下的东向、北向和天向三个方向的角速度误差变化率；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵。

8.如权利要求1所述的一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，所述步骤(4)中主滤波器的观测方程具体为：

$\left[\begin{array}{c}\mathrm{\δ}{d}_1^2\\ \mathrm{\δ}{d}_2^2\\ .\\ .\\ .\\ \mathrm{\δ}{d}_m^2\\ \mathrm{\δ}{\stackrel{.}{d}}_1^2\\ \mathrm{\δ}{\stackrel{.}{d}}_2^2\\ .\\ .\\ .\\ \mathrm{\δ}{\stackrel{.}{d}}_m^2\end{array}\right]=\left[\begin{array}{c}2(x_k^I-x_1)\mathrm{\δx}+2(x_k^I-y_1)\mathrm{\δy}+2(z_k^I-z_1)\mathrm{\δz}-(\mathrm{\δ}{x}^2+\mathrm{\δ}{y}^2+\mathrm{\δ}{z}^2)\\ 2(x_k^I-x_2)\mathrm{\δx}+2(y_k^I-y_2)\mathrm{\δy}+2(z_k^I-z_2)\mathrm{\δz}-(\mathrm{\δ}{x}^2+\mathrm{\δ}{y}^2+\mathrm{\δ}{z}^2)\\ .\\ .\\ .\\ 2(x_k^I-x_m)\mathrm{\δx}+2(y_k^I-y_m)\mathrm{\δy}+2(z_k^I-z_m)\mathrm{\δz}-(\mathrm{\δ}{x}^2+\mathrm{\δ}{y}^2+\mathrm{\δ}{z}^2)\\ 2v{x}_k^I\mathrm{\δx}+2(x_k^I-x_1)\mathrm{\δvx}-2\mathrm{\δx\δvx}+2v{y}_k^I\mathrm{\δy}+2(y_k^I-y_1)\mathrm{\δvy}-2\mathrm{\δy\δvy}+2v{z}_k^I\mathrm{\δz}+2(z_k^I-z_1)\mathrm{\δvz}-2\mathrm{\δz\δvz}\\ 2v{x}_k^I\mathrm{\δx}+2(x_k^I-x_2)\mathrm{\δvx}-2\mathrm{\δx\δvx}+2v{y}_k^I\mathrm{\δy}+2(y_k^I-y_2)\mathrm{\δvy}-2\mathrm{\δy\δvy}+2v{z}_k^I\mathrm{\δz}+2(z_k^I-z_2)\mathrm{\δvz}-2\mathrm{\δz\δvz}\\ .\\ .\\ .\\ 2v{x}_k^I\mathrm{\δx}+2(x_k^I-x_m)\mathrm{\δvx}-2\mathrm{\δx\δvx}+2v{y}_k^I\mathrm{\δy}+2(y_k^I-y_m)\mathrm{\δvy}-2\mathrm{\δy\δvy}+2v{z}_k^I\mathrm{\δz}+2(z_k^I-z_m)\mathrm{\δvz}-2\mathrm{\δz\δvz}\end{array}\right]+v$

其中，为k时刻低成本IMU解算的未知节点的x、y和z向的位置，(x_i,y_i,z_i)为第i个参考节点的x、y和z向的位置；(δx,δy,δz)为k时刻低成本IMU解算的位置误差，即向量δPⁿ的标量元素；(δvx,δvy,δvz)为k时刻低成本IMU解算的速度误差，即向量δVⁿ的标量元素；为k时刻低成本IMU和WSN分别测量得到的未知节点与参考节点之间的距离平方的差；为k时刻低成本IMU和WSN分别测量得到的未知节点与参考节点之间的距离平方变化率的差；v为滤波器的观测噪声矩阵。

9.如权利要求1所述的一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，所述步骤(4)中主滤波器采用15维状态变量来对移动机器人的位置、速度和姿态进行描述，所述15维状态变量具体包括：在x、y、z三个方向的姿态角误差、位置误差、速度误差、加速度误差和角速度误差。