[发明专利]一种野外机器人航迹推算方法

说明书

本发明公开了一种野外机器人航迹推算方法，引入了转动阻力系数描述打滑现象，并基于保证估计的思想设计了航向角与转动阻力系数的估计算法，能够在提升航向角估计精度的同时求解出转动阻力系数，该推算方法包括初始化步骤、采集传感器数据步骤、地形检测步骤、调整过程噪声包络矩阵步骤、状态预测步骤、状态更新步骤、航迹推算定位步骤、以及输出机器人位置坐标的步骤。本发明引入了地形检测以自动调整估计算法的参数，能够保证转动阻力系数估计的平稳性，同时缩减收敛时间。相比于已有的航迹推算方法，由于克服了打滑效应对航迹推算的影响，因而更适用于野外环境，特别是地形复杂情况下的移动机器人定位，精度更高且稳定性更强。

技术领域

本发明涉及机器人技术领域，具体涉及一种野外机器人航迹推算方法。

背景技术

为了适应复杂多变的野外环境，机器人在野外作业时要具有更强的自主性，其中的重要的功能是能够根据环境来调整定位导航系统，使其具有一定的鲁棒性。

相比较于目前不太成熟的腿足式机器人，轮式、履带式机器人凭借其强大的稳定性与通过性获得了广泛的应用。然而，由于这类机器人大多是基于滑移转向机制的，所以其在转弯过程中不可避免的会发生打滑现象，这就导致运动学模型不够准确。

航迹推算作为一种快速、低成本、不易受外界影响的定位方式，深受机器人学家的欢迎。由于航迹推算是基于运动学模型的，所以运动学模型的准确性势必会对定位性能产生较大影响。所以有必要研究针对野外机器人的能够适应地形变换的航迹推算方式。

发明内容

本发明的目的在于提供一种野外机器人航迹推算方法，以提高航迹推算的准确度。

为此，本发明提供了一种野外机器人航迹推算方法，包括以下步骤：

步骤一：对采样点序号k、后验状态估计椭球地形特征向量p_k、过程噪声和电子罗盘噪声的包络矩阵Q_k和R_k、采样间隔T、车轮半径Φ以及车身宽度B进行初始化，其中，后验状态估计椭球的椭球中心中的为航向角的后验状态估计椭球的中心和为转动阻力系数的后验状态估计；

步骤二：将采样点序号自增k←k+1，采集加速度计关于垂直于地面轴向的加速度数据，在一个采样周期内按照相等时间间隔采集N次，得到加速度数据集合{a_k,i},i＝1,…,N；利用朝向地面的摄像头拍摄地面照片，得到像素矩阵M_k；采集左右轮编码器数据，获得左右车轮的旋转速度ω_L,k与ω_R,k；采集电子罗盘数据，获得机器人航向角y_k；

步骤三：根据步骤一中获得的地形特征向量以及步骤二中获得的加速度数据集合和地面照片像素矩阵进行地形检测，判断地形是否发生显著变化；

步骤四：若判定地形发生显著变化，在接下来五个采样点内将转动阻力系数的过程噪声包络矩阵，乘以设定倍数；若地形没有发生显著变化，则保持原来的包络矩阵；

步骤五：根据步骤一中获得的后验状态估计椭球、采样间隔、车轮半径以及车身宽度，步骤二中获得的左右车轮的旋转速度以及步骤四中调整后的过程噪声包络矩阵，进行状态预测，得到先验状态估计椭球；

步骤六：根据步骤一种获得的电子罗盘噪声包络矩阵，步骤二中获得的机器人航向角以及步骤五中获得的先验状态估计椭球，进行状态更新，得到后验状态估计椭球；

步骤七：根据步骤一获得的采样间隔、车轮半径以及车身宽度，步骤二中获得的左右车轮的旋转速度以及步骤六中获得的后验状态估计椭球，进行航迹推算定位，输出机器人位置坐标，即东向坐标e_k和北向坐标n_k；以及

步骤八：重复执行步骤二至步骤七，获得每一个采样点的机器人位置坐标。