[发明专利]一种移动机器人航向角估计方法

说明书

本发明公开了一种移动机器人航向角估计方法，包括以下步骤：利用陀螺仪检测移动机器人的航向角速度，利用磁力计检测移动机器人的航向角；利用状态增广法建立航向角状态空间模型；利用抗差自校正卡尔曼滤波进行数据融合以获取航向角的最优估计。本发明与现有技术相比，具有以下优点：1)陀螺仪的漂移误差能够被较好的补偿，同时利用磁力计消除了航向角估计的累计误差；2)滤波器系数随着传感器噪声的变化而变化，进一步提升了估计精度；3)错误的磁力计读数能被识别并隔离，进一步提升了估计可靠性。

技术领域

本发明涉及机器人技术领域，特别是涉及一种移动机器人航向角估计方法。

背景技术

移动机器人是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统。随着机器人性能不断地完善，移动机器人的应用范围大为扩展，不仅在工业、农业、国防、医疗、服务等行业中得到广泛的应用，而且在排雷、搜捕、救援、辐射和空间领域等有害与危险场合得到很好的应用。因此，移动机器人技术已经得到世界各国的普遍关注。

移动机器人的航向角的精确检测是其实现路线规划、导航与避障的前提。传统的方法存在以下问题：1)基于陀螺仪或者编码器的的航向角估计方法存在累计误差，随着时间的推移误差变得越来越大；2)基于磁力计的航向角检测方法虽然没有累计误差，但是其精度会受到环境中的铁磁物质的影响，因此存在很大的不确定性。近年来，一部分研究将陀螺仪与磁力计进行数据融合，利用两种传感器的互补特性可以得到更为可靠的航向角估计。但是，他们的共同缺点是没有考虑传感器噪声的变化与磁力计失效情况。所以，有必要提出一种新的方法来提升航向角估计的精度。

发明内容

为了克服陀螺仪、磁力计两种传感器的不足，同时考虑传感器噪声的变化与磁力计失效情况，本发明提出了一种移动机器人航向角估计方法，包括以下步骤：

(1)利用陀螺仪检测移动机器人的航向角速度，利用磁力计检测移动机器人的航向角；

(2)利用状态增广法建立如下的航向角状态空间模型

其中，θ_t为航向角，b_G,t为陀螺仪漂移误差，为增广航向角向量，T为采样间隔，为系统转移矩阵，为输入矩阵，为陀螺仪检测的航向角速度，为系统噪声矩阵，n_G,t为陀螺仪内在噪声，w_G,t为陀螺仪漂移噪声，为增广系统噪声向量，θ_M,t为磁力计检测的航向角，为观测矩阵，v_M,t为磁力计内在噪声，ε_M,t为铁磁物质引起的突变噪声，下标t表示该变量位于第t个采样点，噪声n_G,t、w_G,t与v_M,t都为高斯白噪声；

(3)利用抗差自校正卡尔曼滤波进行数据融合以获取航向角的最优估计，步骤如下：

(3.1)状态预测估计：使用上一时刻的最优估计来预测下一状态：

其中，是在采样点t-1时的最优估计，是预测估计，相应的预测估计误差协方差其中P_t-1是最优估计误差协方差，是方差的估计；

(3.2)粗大误差识别：磁力计读数被认定为粗大误差，如果满足以下条件

其中，为新息序列，是v_M,t方差的估计；

(3.3)状态最优估计：如果θ_M,t不是粗大误差，则卡尔曼增益K_t为

因此可得到最优估计