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

摘要

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

主权项

1.一种移动机器人航向角估计方法，其特征在于包含以下步骤：(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不是粗大误差，则卡尔曼增益Kt为：因此可得到最优估计：其相应的最优估计误差协方差为如果θ_M，t是粗大误差，则：Kt＝[0 0]′，(3.4)噪声方差估计：计算三种相关函数与如下：其中，N为窗口长度系数，与分别为对应到与的最新的N个可用磁力计读数的采样点序号，为二次后向差分磁力计读数序列，然后可以得到噪声方差估计