一种移动机器人航向角估计方法与流程

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

背景技术：

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

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

技术实现要素：

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

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

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

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

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

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

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

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

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

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

因此可得到最优估计

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

kt＝[00]′

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

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

本发明与现有技术相比，提供了一种更为精确且可靠的航向角估计方法，其优点在于：1)陀螺仪的漂移误差能够被较好的补偿，同时利用磁力计消除了航向角估计的累计误差；2)滤波器系数随着传感器噪声的变化而变化，进一步提升了估计精度；3)错误的磁力计读数能被识别并隔离，进一步提升了估计可靠性。

附图说明

图1为本发明流程图

图2为本发明实验结果图

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图与具体实施例对本发明进行详细说明。

如图1所示，本发明的具体实施步骤如下：

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

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

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

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

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

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

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

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

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

因此可得到最优估计

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

kt＝[00]′

(3.4)噪声方差估计：θm,t的差分序列包括陀螺仪和磁力计的噪声，因此噪声的统计特性可以通过差分序列的统计分析得出。考虑以下式子：

θm,t+1＝θt+1+vm,t+1+εm,t+1

θm,t＝θt+vm,t+εm,t

上式相减可得

从而可得

上式中包含陀螺仪和磁力计的误差项。为了消除bg,t，引入后向差分算子得到：

假设在所有采样点εm,t＝0，也就是说不存在铁磁干扰，将会是三个独立的滑动平均过程wg,t-1，ng,t-ng,t-1，vm,t+1-2vm,t+vm,t-1之和，因此，也是一个滑动平均过程，它的阶数等于三个中较高阶的阶数。

求取上式两边随机过程的相关函数可得

其中，表示数学期望。使用有限记忆方法，计算在采样点t时三种相关函数与如下

但是如果存在铁磁干扰，假设在采样点集合时磁力计的读数是不可用的，则对和来说，可用磁力计读数的采样点集合为：

进而，在采样点t时三种相关函数与可写为

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

采用本专利方法需要注意如下问题：首先，准确的噪声方差初始值和需要提前获取，因为这些初始值对粗大误差识别很关键。其次，噪声方差应当是随时间缓慢变化的，可以保证在一段较长时间里的和的计算是准确的。最后，εm,t在多数情况下应该是等于0的，否则将没有足够可用的磁力计读数来校正基于陀螺仪的航向角估计结果。

使用轮式移动机器人对本发明实施例进行测试，时间间隔为0.2秒，其结果如图2所示。首先，由于在滤波过程中增加了噪声方差估计，所以滤波器的参数会随着噪声方差的变化而变化，从而保证了滤波精度；其次，随着铁磁物质的出现，磁力计读数发生很大的偏差，传统方法如卡尔曼滤波则会跟踪错误的磁力计读数，使滤波结果变得极不可靠，而本专利方法中提出的抗差自校正卡尔曼滤波能够隔离错误的磁力计读数，虽然滤波误差会在短期内增大，但是当磁力计读数恢复正常时，滤波误差能够迅速变小，证明本专利方法有着较强的可靠性。

提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。