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

权利要求书

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不是粗大误差，则卡尔曼增益K_t为：

因此可得到最优估计：

其相应的最优估计误差协方差为如果θ_M，t是粗大误差，则：

K_t＝[0 0]′，

(3.4)噪声方差估计：计算三种相关函数与如下：

其中，N为窗口长度系数，与分别为对应到与的最新的N个可用磁力计读数的采样点序号，为二次后向差分磁力计读数序列，然后可以得到噪声方差估计