文献类型：专利

专利类型：发明专利

是否失效：否

是否授权：否

申 请 号：CN201710415515.1

申 请 日：20170606

发 明 人：吕文君 李泽瑄

申 请 人：安徽天路航空科技股份有限公司

申请人地址：235100 安徽省淮北市淮北经济开发区龙湖高新区龙兴路21号

公 开 日：20181012

公 开 号：CN107218917B

代 理 人：杜彬

代理机构：11357 北京同辉知识产权代理事务所(普通合伙)

语　　种：中文

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

主 权 项：1.一种移动机器人航向角估计方法，其特征在于包含以下步骤：(1)利用陀螺仪检测移动机器人的航向角速度，利用磁力计检测移动机器人的航向角；(2)利用状态增广法建立如下的航向角状态空间模型： 其中，θ_t为航向角，b_G，t为陀螺仪漂移误差，为增广航向角向量，T为采样间隔，为系统转移矩阵，为输入矩阵，为陀螺仪检测的航向角速度，为系统噪声矩阵，n_G，t为陀螺仪内在噪声，w_G，t为陀螺仪漂移噪声，为增广系统噪声向量，θ_M，t为磁力计检测的航向角，