CN107991110B - 一种履带式机器人滑动参数检测方法 - Google Patents

Abstract

本发明公开了一种履带式机器人滑动参数检测方法，通过引入最优估计理论与地形检测算法，能够获取机器人的滑动参数的估计值，该检测方法包括初始化步骤、采集传感器数据步骤、地形检测步骤、调整滑动系统的过程噪声方差的步骤、状态预测步骤、状态更新步骤和输出滑动系数的估计值的步骤。本发明在状态预测过程中引入无色变换，以保证在强非线性情况下的状态预测的准确性；而且由于引入了地形检测，在地形发生明显变化的时候，本发明会调整滑动系数的过程噪声方差，这种自适应机制能够保证滑动系数估计的平稳性，同时缩减收敛时间，适用于地形复杂的场景。

Description

一种履带式机器人滑动参数检测方法

技术领域

本发明涉及机器人技术领域，特别是涉及一种履带式机器人滑动参数检测方法。

背景技术

由于履带机器人具有优越的通过性和机动性，其在军事、农业、火星探测等领域得到广泛应用。履带机器人转向过程始终伴随着高速侧履带的滑转与低速侧履带的滑移，这就导致运动学模型的不准确性，进而为行驶轨迹的预测带来了更多的困难。

在论文“Le A T,Rye D C,Durrant-Whyte H F.Estimation of track-soilinteractions for autonomous tracked vehicles[C]//IEEE InternationalConference on Robotics and Automation,1997.Proceedings.IEEE,1997:1388-1393vol.2.”中，作者将滑动系数引入到传统的运动学模型中，可以提升存在打滑现象时候的运动学模型的准确度。

滑动系数是无法直接测得的，传统的方法是利用遗传算法离线推算出滑动系数，然而滑动参数往往随着地形的变化而变化的。因此，如何实时的获取履带机器人的滑动系数成为了机器人领域中一项重要且具有挑战性的工作。

发明内容

本发明的目的在于提供一种履带式机器人滑动参数检测方法，以实时获取履带你的滑动系数。

为此，本发明提供了一种履带式机器人滑动参数检测方法，包括以下步骤：

步骤一：对采样点序号k、后验状态估计后验误差协方差P_k、新息协方差ε_k、地形特征向量p_k、过程噪声和观测噪声的方差Q_k和R_k、采样间隔T以及车身宽度B进行初始化，其中，后验状态估计中的六个元素为：与分别表示东向坐标、北向坐标与航向角的后验状态估计，与为左右履带滑动比以及机器人本体滑动角的后验状态估计；

步骤二：将采样点序号自增k←k+1，并采集加速度计关于垂直于地面轴向的加速度数据，在一个采样周期内按照相等时间间隔采集N次，得到加速度数据集合{a_k,i},i＝1,…,N；利用朝向地面的摄像头拍摄地面照片，得到像素矩阵M_k；采集左右轮编码器数据，获得左右车轮的旋转速度v_L,k与v_R,k；采集电子罗盘数据与GPS模块，获得观测向量z_k＝[z_e,kz_n,k z_θ,k]′，其中z_e,k与z_n,k为东向坐标与北向坐标的观测值，由GPS模块采集获得，z_θ,k为航向角的观测值，由电子罗盘采集获得；

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

步骤四：若判定地形发生显著变化，在接下来五个采样点内将滑动系数的过程噪声方差乘以设定倍数；如果地形没有发生变化，则保持原来的方差；

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

步骤六：根据步骤一中获得的新息协方差、观测噪声方差,步骤二中获得的观测向量以及步骤五中获得的先验状态估计和先验误差协方差，进行状态更新，得到新息协方差ε_k、后验状态估计和后验误差协方差P_k；以及

步骤七：重复执行步骤二至步骤七，以获得每一个采样点的后验状态估计中第4至6元素即滑动系数的估计值。

本发明与现有技术相比，优点在于：1)针对运动学方程是非线性而观测方程是线性的情况，在状态预测过程中引入无色变换，以保证在强非线性情况下的状态预测的准确性；2)引入新息协方差估计器，在系统噪声统计特性发生或者模型参数发生变化的时候，能够保证估计方法的稳定性；3)由于引入了地形检测，在地形发生明显变化的时候，本发明会调整滑动系数的过程噪声方差，这种自适应机制能够保证滑动系数估计的平稳性，同时缩减收敛时间，适用于地形复杂的场景。

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

附图说明

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

图1是根据本发明的履带式机器人滑动参数检测方法的流程图；

图2示出了根据本发明一实施例的地形相似度仿真结果；以及

图3示出了根据本发明一实施例的滑动系数仿真结果。

具体实施方式

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

本发明提供了一种履带式机器人滑动参数检测方法，通过引入最优估计理论与地形检测算法，能够获取机器人的滑动参数的估计值。与已有的技术相比，优点在于：1)针对运动学方程是非线性而观测方程是线性的情况，在状态预测过程中引入无色变换，以保证在强非线性情况下的状态预测的准确性；2)引入新息协方差估计器，在系统噪声统计特性发生或者模型参数发生变化的时候，能够保证估计方法的稳定性；3)由于引入了地形检测，在地形发生明显变化的时候，本发明会调整滑动系数的过程噪声方差，这种自适应机制能够保证滑动系数估计的平稳性，同时缩减收敛时间，适用于地形复杂的场景。

如图1所示，本发明的基于卡尔曼滤波的履带式机器人滑动参数检测方法的实施步骤如下：

S10、初始化

对采样点序号、后验状态估计、后验误差协方差、新息协方差、地形特征向量、过程噪声和观测噪声的方差、采样间隔、车轮半径以及车身宽度进行初始化。具体如下：

采样点序号k＝0，后验状态估计中的六个元素需要根据实际情况确定，后验误差协方差P_k＝0.1×I_6×6，新息协方差ε_k＝O_3×3，地形特征向量p_k＝O_8×1，过程噪声和观测噪声的方差Q_k和R_k分别为6行6列对角阵和3行3列对角阵，需要根据实际情况确定，采样间隔T，车轮半径Φ，车身宽度B需要根据实际情况确定。其中，下标k表示采样点序号，I_6×6为6行6列的单位矩阵，O_8×1为8行1列的零向量，O_3×3为3行3列的零矩阵，与分别表示东向坐标、北向坐标与航向角的后验状态估计，与为左右履带滑动比以及机器人本体滑动角的后验状态估计。

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

S20、采集传感器数据

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

采样点序号k←k+1；采集加速度计关于垂直于地面轴向的加速度数据，在一个采样周期内按照相等时间间隔采集N次，得到加速度数据集合{a_k,i},i＝1,…,N；利用朝向地面的摄像头拍摄地面照片，得到像素矩阵M_k；采集左右轮编码器数据，获得左右车轮的旋转速度v_L,k与v_R,k；采集电子罗盘数据与GPS模块，获得观测向量z_k＝[z_e,k z_n,k z_θ,k]′，其中z_e,k与z_n,k为东向坐标与北向坐标的观测值，由GPS模块采集获得，z_θ,k为航向角的观测值，由电子罗盘采集获得。

S30、地形检测

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

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

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

3.3)求取地形特征向量其中的8个元素分别如下：

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

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

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

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

S40、调整滑动系数的过程噪声方差

根据步骤2中对地形是否发生显著变化的判断，调整滑动系数的过程噪声方差：如果地形发生显著变化，在接下来五个采样点内将滑动系数的过程噪声方差，即Q_k主对角线第4至6个元素，乘以10倍；如果地形没有发生变化，则保持原来的方差。

S50、状态预测

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

5.1)创建一组Sigma点集合如下：其中m为状态维度，λ为比例因子；

5.2)用5.1中的Sigma点预测状态，得到一组状态预测Sigma点的集合如下：其中，与分别表示集合与中的第i个元素，状态转移方程f(·)具体为：

5.3)对5.2中的状态预测Sigma点加权，得到先验状态估计与先验误差协方差，如下：

其中，W_i ^m与W_i ^c分别为均值与协方差的权重系数。

S60、状态更新

根据步骤S10中获得的新息协方差、观测噪声方差,步骤S20中获得的观测向量以及步骤S50中获得的先验状态估计和先验误差协方差，进行状态更新，具体如下：

6.1)计算新息∈_k如下：

其中，为观测矩阵。

6.2)计算新息协方差ε_k如下：

6.3)计算卡尔曼增益K_k如下：

6.4)计算后验状态估计如下：

6.5)计算后验误差协方差P_k如下：P_k＝(I_6×6-K_kC)P_k,k-1，其中，矩阵I_6×6为6维单位矩阵。

S70、重复步骤S20至S60，以获得每一个采样点的后验状态估计中第4至6元素即滑动系数的估计值。

为了验证本发明，我们采用软件MATLAB对进行仿真实验，设置2000个采样点，采样间隔为0.4秒，轮胎半径为35厘米，车架宽度为65厘米，滑动系数与初始为0.2、0.3、0.2，在第1001个采样点变为0.3、0.4、0.3。同时，我们利用加速度传感器和摄像头分别采集水泥地和草地两种地形的数据各1000组，与MATLAB进行联合实验，模拟移动机器人切换地形。地形相似度仿真结果如图2所示，可见在地形发生变化的时候，地形相似度距离会发生短暂的跳跃性变化；在地形不发生变化的时候，地形相似度距离会稳定的维持在一个相对较小的值。滑动系数仿真结果如图3所示，初始值分别设置为0、0、0，估计结果能够在状态突变后很快的收敛到真实值。由以上仿真可以验证本发明的有效性。

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

Claims (5)

1.一种履带式机器人滑动参数检测方法，其特征在于，包括以下步骤：

步骤一：对采样点序号k、后验状态估计后验误差协方差P_k、新息协方差ε_k、地形特征向量p_k、过程噪声和观测噪声的方差Q_k和R_k、采样间隔T以及车身宽度B进行初始化，其中，后验状态估计中的六个元素为：与分别表示东向坐标、北向坐标与航向角的后验状态估计，与为左右履带滑动比以及机器人本体滑动角的后验状态估计；

步骤二：将采样点序号自增k←k+1，采集加速度计关于垂直于地面轴向的加速度数据，并在一个采样周期内按照相等时间间隔采集N次，得到加速度数据集合{a_k,i},i＝1,…,N；利用朝向地面的摄像头拍摄地面照片，得到像素矩阵M_k；采集左右轮编码器数据，获得左右车轮的旋转速度v_L,k与v_R,k；采集电子罗盘数据与GPS模块，获得观测向量z_k＝[z_e,k z_n,kz_θ,k]′，其中z_e,k与z_n,k为东向坐标与北向坐标的观测值，由GPS模块采集获得，z_θ,k为航向角的观测值，由电子罗盘采集获得；

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

步骤四：若判定地形发生显著变化，在接下来五个采样点内将滑动系数的过程噪声方差乘以设定倍数；如果地形没有发生变化，则保持原来的方差；

步骤五：根据步骤一中获得的后验状态估计、后验误差协方差、采样间隔以及车身宽度，步骤二中获得的左右车轮的旋转速度以及步骤四中调整后的过程噪声方差，进行状态预测，得到先验状态估计与先验误差协方差；

步骤六：根据步骤一中获得的新息协方差、观测噪声方差,步骤二中获得的观测向量以及步骤五中获得的先验状态估计和先验误差协方差，进行状态更新，得到新息协方差ε_k、后验状态估计和后验误差协方差P_k；以及

步骤七：重复执行步骤二至步骤六，以获得每一个采样点的后验状态估计其中中第4至6元素即滑动系数的估计值。

2.根据权利要求1所述的履带式机器人滑动参数检测方法，其特征在于，所述步骤三包括以下子步骤：

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

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

2.3)求取地形特征向量其中的元素如下：

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

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

其中，ω_i∈(0,1]为每个特征分量的权重，如果：则判定地形没有显著变化，否则判定地形发生了显著变化。

3.根据权利要求2所述的履带式机器人滑动参数检测方法，其特征在于，所述步骤四中的设定倍数为10倍。

4.根据权利要求3所述的履带式机器人滑动参数检测方法，其特征在于，所述步骤五包括以下子步骤：

4.1)创建一组Sigma点集合如下：其中m为状态维度，λ为比例因子；

4.2)用4.1中的Sigma点预测状态，得到一组状态预测Sigma点的集合如下：其中，与分别表示集合与中的第i个元素，状态转移方程f(·)具体为：

4.3)对4.2中的状态预测Sigma点加权，得到先验状态估计与先验误差协方差，如下：

其中，W_i ^m与W_i ^c分别为均值与协方差的权重系数。

5.根据权利要求4所述的履带式机器人滑动参数检测方法，其特征在于，所述步骤六包括以下子步骤：

5.1)计算新息∈_k如下：其中，为观测矩阵；

5.2)计算新息协方差ε_k如下：

5.3)计算卡尔曼增益K_k如下：

5.4)计算后验状态估计如下：以及

5.5)计算后验误差协方差P_k如下：P_k＝(I_6×6-K_kC)P_k,k-1，其中，矩阵I_6×6为6维单位矩阵。