一种针对四轮机器人的转动瞬心估计方法与流程

本发明涉及机器人技术领域，特别是涉及一种针对四轮机器人的转动瞬心估计方法。

背景技术：

四轮机器人是一种常见的野外机器人，它可以通过改变左右轮子的速度来控制机器人的方向，具有很好的鲁棒性和灵活性，并且能够实现零半径转向。但是，四轮机器人在移动过程中不可避免的出现打滑现象，这就导致运动学模型的不准确性。

在论文“mandowa,moralesj,pedrazas.approximatingkinematicsfortrackedmobilerobots[j].internationaljournalofroboticsresearch,2005,24(10):867-878.”中，作者首次提出了转动瞬心运动学模型，做法是将左右轮子与机器人本体的转动瞬心的位置坐标引入到传统的运动学模型中，以提升运动学模型在存在打滑现象的准确度。

转动瞬心本身可以看作对四轮机器人打滑现象的一种描述方式，而且转动瞬心往往随着地形的变化而变化。因此，如何实时的获取四轮机器人的转动瞬心成为了野外机器人领域中一项重要且具有挑战性的工作。

技术实现要素：

本发明的目的在于提供一种针对四轮机器人的转动瞬心估计方法，以实时获取四轮机器人的转动瞬心。

为此，本发明提供了一种针对四轮机器人的转动瞬心估计方法，包括以下步骤：

步骤一：对采样点序号k、后验状态估计后验误差协方差pk、新息协方差εk、地形特征向量pk、过程噪声和观测噪声的方差qk和rk、采样间隔t以及车身宽度b进行初始化，其中，后验状态估计中的与分别表示东向坐标、北向坐标与航向角的后验状态估计，与为转动瞬心的3个运动学参数的后验状态估计；

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

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

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

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

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

步骤七：重复步骤二至步骤六，以输出每一个采样点的后验状态估计中第4至6元素即转动瞬心的估计值。

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

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

附图说明

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

图1是根据本发明的针对四轮机器人的转动瞬心估计方法的流程图；

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

图3是根据本发明一实施例的转动瞬心仿真结果。

具体实施方式

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

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

如图1所示，本发明的基于卡尔曼滤波的四轮机器人的转动瞬心估计方法的具体实施步骤如下：

s10、初始化

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

采样点序号k＝0，后验状态估计中的六个元素需要根据实际情况确定，后验误差协方差pk＝0.1×i6×6，新息协方差εk＝o3×3，地形特征向量pk＝o8×1，过程噪声和观测噪声的方差qk和rk分别为6行6列对角阵和3行3列对角阵，需要根据实际情况确定，采样间隔t，车轮半径φ，车身宽度b需要根据实际情况确定。其中，下标k表示采样点序号，i6×6为6行6列的单位矩阵，o8×1为8行1列的零向量，o3×3为3行3列的零矩阵，与分别表示东向坐标、北向坐标与航向角的后验状态估计，与为转动瞬心的3个运动学参数的后验状态估计。

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

s20、采集传感器数据

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

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

s30、地形检测

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

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

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

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

以及

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

3.5)判断地形是否变化：计算地形相似度距离d(pk,pk-1)：

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

s40、调整转动瞬心的过程噪声方差

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

s50、状态预测

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

5.1)创建一组sigma点集合xk-1，如下：其中m为状态维度，λ为比例因子。

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

5.3)对5.2中的状态预测sigma点加权，得到先验状态估计与先验误差协方差pk,k-1：

以及

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

s60、状态更新

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

6.1)计算新息ek如下：其中，为观测矩阵。

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

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

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

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

s70、重复步骤s20至s60，可以获得每一个采样点的后验状态估计中第4至6元素即转动瞬心的估计值。

为了验证本发明，我们采用软件matlab对进行仿真实验，设置2000个采样点，采样间隔为0.4秒，轮胎半径为35厘米，车架宽度为65厘米，转动瞬心初始为21.6、-21.6、10，在第1001个采样点变为43.2、-43.2、5。同时，我们利用加速度传感器和摄像头分别采集水泥地和草地两种地形的数据各1000组，与matlab进行联合实验，模拟移动机器人切换地形。

地形相似度仿真结果如图2所示，可见在地形发生变化的时候，地形相似度距离会发生短暂的跳跃性变化；在地形不发生变化的时候，地形相似度距离会稳定的维持在一个相对较小的值。转动瞬心仿真结果如图3所示，初始值分别设置为65、-65、20，估计结果能够在状态突变后很快的收敛到真实值。由以上仿真可以验证本发明的有效性。

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