CN108036789B - 一种野外机器人航迹推算方法 - Google Patents

Abstract

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

Description

一种野外机器人航迹推算方法

技术领域

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

背景技术

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

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

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

发明内容

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

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

步骤一：对采样点序号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；以及

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

本发明与现有技术相比，其优点在于：1)引入转动阻力系数描述打滑现象，使运动学模型更加精确；2)基于保证估计的思想设计了航向角与转动阻力系数的估计方法，能够在提升航向角估计精度的同时，求解出转动阻力系数；3)引入了地形检测，在地形发生明显变化的时候，本发明会调整转动阻力系数的过程噪声包络矩阵，这种自适应机制能够保证转动阻力系数估计的平稳性，同时缩减收敛时间，适用于地形复杂的场景；4)利用估计出来转动阻力系数可以克服打滑效应对航迹推算的影响，比传统的航迹推算方法更加精确。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为根据本发明的野外机器人航迹推算方法的流程图；

图2为根据本发明一实施例的野外机器人航迹推算方法的地形相似度仿真结果；

图3为根据本发明一实施例的野外机器人航迹推算方法的航向角仿真结果；

图4为根据本发明一实施例的野外机器人航迹推算方法的转动阻力系数仿真结果；以及

图5为根据本发明一实施例的野外机器人航迹推算方法的航迹推算仿真结果。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

本发明提供了一种野外机器人航迹推算方法，首先引入了转动阻力系数描述打滑现象，并基于保证估计的思想设计了航向角与转动阻力系数的估计算法，能够在提升航向角估计精度的同时求解出转动阻力系数；同时引入了地形检测以自动调整估计算法的参数，能够保证转动阻力系数估计的平稳性，同时缩减收敛时间。相比于已有的航迹推算方法，由于克服了打滑效应对航迹推算的影响，本发明提出的航迹推算方法更适用于野外环境，特别是地形复杂情况下的移动机器人定位，精度更高且稳定性更强。

如图1所示，本发明的基于卡尔曼滤波的野外机器人航迹推算方法的流程如下：

S10、初始化

对采样点序号、后验状态估计椭球、地形特征向量、过程噪声和电子罗盘噪声的包络矩阵、采样间隔、车轮半径以及车身宽度进行初始化。具体如下：

采样点序号k＝0，后验状态估计椭球

的初始化如下：椭球中心

中的两个元素需要根据实际情况确定，椭球包络矩阵

记号

表示椭球集，椭球集的第1个元素表示椭球中心，第2个元素表示椭球包络矩阵，地形特征向量p_k＝O_8×1，过程噪声和电子罗盘噪声的包络矩阵Q_k和R_k分别为2行2列对角阵和标量，需要根据实际情况确定，采样间隔T，车轮半径Φ，车身宽度B需要根据实际情况确定。其中，下标k表示采样点序号，O_8×1为8行1列的零向量，

为航向角的后验状态估计椭球的中心，

为转动阻力系数的后验状态估计椭球的中心。

在本发明中，单引号上标表示矩阵的转置，例如，A′_k为矩阵A_k转置。

S20、采集传感器数据

将采样点序号自增，并采集加速度计、摄像头、左右轮编码器与电子罗盘的数据。具体如下：

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

S30、地形检测

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

3.1)消除加速度数据集合的直流分量：将加速度数据集合的所有元素分别减去该集合所有元素的均值：

得到消除了直流分量的加速度数据集合

3.2)提取地面照片的主色：从地面照片像素矩阵M_k中随机抽取50个像素并求平均，得到地形主色的红绿蓝分量l_R,k，l_G,k与l_B,k。

3.3)求取地形特征向量

其中的元素如下：

3.4对地形特征向量进行归一化处理。

3.5判断地形是否变化：计算地形相似度距离：

其中，ω_i∈(0,1]为每个特征分量的权重。如果

则判断地形没有变化，否则判断地形发生了显著变化。

S40、调整转动阻力系数的过程噪声包络矩阵

根据步骤S20中对地形是否发生显著变化的判断，调整转动阻力系数的过程噪声包络矩阵：如果地形发生显著变化，在接下来五个采样点内将转动阻力系数的过程噪声包络矩阵，即Q_k主对角线第二个元素，乘以10倍；如果地形没有发生变化，则保持原来的包络矩阵。

S50、状态预测

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

计算先验状态估计椭球

如下：

其中，状态转移方程f(·)具体为：

其中矩阵

为状态转移方程f(·)的雅克比矩阵，

tr(·)表示矩阵的迹。

S60、状态更新

根据步骤S10中获得的电子罗盘噪声包络矩阵，步骤S20中获得的机器人航向角以及步骤S50中获得的先验状态估计椭球，进行状态更新，得到后验状态估计椭球，具体如下：

6.1)计算新息∈_k如下：

其中，C＝[1 0]为观测矩阵。

6.2)计算新息包络矩阵W_k如下：

其中，

msvm(·)表示矩阵的最大奇异值。

6.3)计算后验状态估计椭球

如下：

其中，健康指示函数δ_k与后验状态估计椭球的预包络矩阵如下所示：

6.4)消除粗大误差：如果δ_k≤0，表明电子罗盘出现故障，计算后验状态估计椭球

如下：

P_k＝P_k,k-₁；

7.航迹推算定位

根据步骤S10获得的采样间隔、车轮半径以及车身宽度，步骤S20中获得的左右车轮的旋转速度以及步骤S60中获得的后验状态估计椭球，进行航迹推算定位，具体如下：

计算东向坐标e_k，北向坐标n_k如下：

其中，

为

向量中第一个元素。

8.重复步骤S20至S70，可以获得每一个采样点的机器人位置坐标。

为了验证本发明，我们采用软件MATLAB对进行仿真实验，设置2000个采样点，采样时间间隔为0.1秒，轮胎半径为35厘米，车架宽度为65厘米，转动阻力系数为4，在第1001个采样点变为2。同时，我们利用加速度传感器和摄像头分别采集水泥地和草地两种地形的数据各1000组，与MATLAB进行联合实验，模拟移动机器人切换地形。地形相似度仿真结果如图2所示，可见在地形发生变化的时候，地形相似度距离会发生短暂的跳跃性变化；在地形不发生变化的时候，地形相似度距离会稳定的维持在一个相对较小的值。

航向角仿真结果如图3所示，可以看出真实值包含在估计结果的上下界内，而且估计结果的中点与真实值基本重合；转动阻力系数仿真结果如图4所示，初始值设置为14，可以任意设定，估计结果能够在状态突变后很快的收敛到真实值。航迹推算仿真结果如图5所示，实线是基于本发明的东向、北向定位误差，虚线是基于传统航迹推算的东向、北向定位误差。由于利用转向阻力系数的估计值补偿的打滑效应，本发明的航迹推算误差相对较小。由上仿真可以验证本发明的有效性。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种野外机器人航迹推算方法，其特征在于，包括以下步骤：

步骤一：对采样点序号k、后验状态估计椭球

地形特征向量P_k、过程噪声和电子罗盘噪声的包络矩阵Q_k和R_k、采样间隔T、车轮半径Φ以及车身宽度B进行初始化，其中，后验状态估计椭球

的椭球中心

中的

为航向角的后验状态估计椭球的中心和

为转动阻力系数的后验状态估计；

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

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

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

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

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

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

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

2.根据权利要求1所述的野外机器人航迹推算方法，其特征在于，所述步骤三包括以下子步骤：

2.1)消除加速度数据集合的直流分量：将加速度数据集合的所有元素分别减去该集合所有元素的均值：

得到消除了直流分量的加速度数据集合

2.2)提取地面照片的主色：从地面照片像素矩阵M_k中随机抽取50个像素并求平均，得到地形主色的红绿蓝分量l_R,k，l_G,k与l_B,k；

2.3)求取地形特征向量

其中的元素如下：

2.4对地形特征向量P_k进行归一化处理；以及

2.5判断地形是否变化：计算地形相似度距离

其中，ω_i∈(0 ， 1] 为每个特征分量的权重，如果

则判断地形没有变化，否则判断地形发生了显著变化。

3.根据权利要求2所述的野外机器人航迹推算方法，其特征在于，在步骤四中所述设定倍数为10倍。

4.根据权利要求3所述的野外机器人航迹推算方法，其特征在于，在步骤五中所述先验状态估计椭球

如下：

其中，状态转移方程f(·)具体为：

其中，矩阵

为状态转移方程f(·)的雅克比矩阵，

tr( · ) 表示矩阵的迹。

5.根据权利要求4所述的野外机器人航迹推算方法，其特征在于，在步骤六中状态更新过程如下：

5.1)计算新息∈_k：

其中，C＝[1 0]为观测矩阵；

5.2)计算新息包络矩阵W_k：

其中，

msvm( · ) 表示矩阵的最大奇异值；

5.3)计算后验状态估计椭球

其中，健康指示函数δ_k与后验状态估计椭球的预包络矩阵

如下所示：

其中，矩阵I_2×2为二维单位矩阵，以及

5.4)消除粗大误差：如果δ_k≤0，表明电子罗盘出现故障，计算后验状态估计椭球

如下：

P_k＝P_k,k-1。

6.根据权利要求5所述的野外机器人航迹推算方法，其特征在于，计算东向坐标e_k和北向坐标n_k如下：

其中，为

向量中第一个元素。

Images