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

摘要

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

主权项

一种面向室内移动机器人的IMU/WSN组合导航方法，其特征是，包括以下步骤：(1)选取导航区域内任意一个参考节点的位置作为坐标原点，并分别选取x向和y向构建相对坐标系；(2)在WSN系统的每个无线通信通道中加入第一局部滤波器，通过第一局部滤波器分别得到当前时刻该无线通信通道的未知节点与参考节点之间的距离平方和距离平方变化率的最优估计；(3)根据惯性测量单元IMU采集得到的传感器数据判断移动机器人的运动状态，当移动机器人处于静止状态时，利用第二局部滤波器对IMU的解算误差进行补偿，得到当前时刻IMU解算得到的未知节点与参考节点之间的距离平方和距离平方变化率的最优估计；(4)分别以步骤(2)和步骤(3)得到的当前时刻未知节点与参考节点之间的距离平方之差以及距离平方变化率之差作为观测量，构建主滤波器；(5)将惯性测量单元IMU采集到的当前时刻的未知节点的位置和速度与主滤波器输出的惯性测量单元IMU的解算误差做差，最终得到当前时刻的未知节点的位置、速度和姿态的最优估计；所述步骤(3)中第二局部滤波器的状态方程具体为：$\left\{\begin{array}{c}\delta {\stackrel{·}{V}}^n=-{\Theta }^n\times {C_b^n}^{\prime }{\hat{f}}^b+{▿}^n\\ \stackrel{·}{\phi }=-{ϵ}^n\\ \delta {\stackrel{·}{P}}^n={\mathrm{\delta V}}^n\\ {\stackrel{·}{▿}}^n=0\\ {\stackrel{·}{ϵ}}^n=0\end{array}\right.$其中，δVⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的速度误差变化率；φ＝[φ_E φ_N φ_U]为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的姿态误差变化率；δPⁿ为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差；为惯性测量单元IMU测量得到的理想导航坐标系下的东向、北向和天向三个方向的位置误差变化率；▽ⁿ为理想导航坐标系下的东向、北向和天向三个方向的加速度误差；为理想导航坐标系下的东向、北向和天向三个方向的加速度误差变化率；εⁿ为理想导航坐标系下的东向、北向和天向三个方向的角速度误差；为理想导航坐标系下的东向、北向和天向三个方向的角速度误差变化率；为加速度计测量得到的载体坐标下的x、y和z轴三个方向的加速度值；为载体坐标系到实际导航坐标系下的状态转移矩阵。