一种滑移转向机器人的位姿和转动瞬心联合估计方法与流程

本发明涉及机器人技术领域，特别是涉及一种滑移转向机器人的位姿和转动瞬心联合估计方法。

背景技术：

滑移转向机制可以通过改变左右轮子或者履带的速度来控制移动机器人的方向，由于其具有很好的鲁棒性和灵活性，并且能够实现零半径转向，滑移转向机制被广泛的应用在野外机器人上。

由于滑移转向机器人的运动控制与路径规划大多是基于运动模型的，所以运动模型的准确性势必会对机器人的控制性能产生较大的影响。但是，滑移转向机器人在移动过程中不可避免的出现打滑现象，为了建立准确的运动模型必须要引入转动瞬心，而转动瞬心往往随着地形的变化而变化。因此，如何实时的获取滑移转向机器人的转动瞬心成为了机器人领域中一项重要且具有挑战性的工作。

目前，针对滑移转向机器人的位姿和转动瞬心联合估计方法的相关研究刚刚起步，已有的研究成果较少。已有文献主要采用的如扩展卡尔曼滤波与无色卡尔曼滤波要求噪声是高斯白噪声，这在实际中往往不容易满足，比如车轮发生形变以后将会为系统带来常值误差。

另外，转动瞬心的过程噪声方差往往被设置成常值，如果设置成一个较大的值，在地形发生变化的时候，转动瞬心的估计值将会快速收敛，但是会产生较大的抖动；如果设置成一个较小的值，转动瞬心的估计值会相对平稳，但是在地形变化后其收敛过程较长，不适合地形复杂的应用场景。所以基于以上两个问题，本发明提出了一种滑移转向机器人的位姿和转动瞬心联合估计方法。

技术实现要素：

本发明的目的在于提供一种滑移转向机器人的位姿和转动瞬心联合估计方法,以适应地形复杂的应用场景。

为此，本发明提供了一种滑移转向机器人的位姿和转动瞬心联合估计方法，包括以下步骤：

步骤一：对采样点序号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为航向角的观测值，由电子罗盘采集获得；

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

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

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

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

步骤七：重复步骤二至步骤六，以输出每一个采样点的位姿与转动瞬心的后验状态估计椭球，其中，该后验状态估计椭球的中心就是位姿与转动瞬心的估计值。

本发明与现有技术相比，优点在于：1)不需要过程噪声和观测噪声的概率分布作为先验知识，这意味着在实施本专利之前不需要做大量的统计实验，同时对于噪声概率分布时变的情况具有较强的鲁棒性；2)不需要过程噪声和观测噪声满足高斯白噪声这个条件，这是非常符合实际情况的，因为现实中高斯白噪声只是一个理想的情况，一般很难被满足；3)由于引入了地形检测，在地形发生明显变化的时候，本发明会调整转动瞬心的过程噪声包络矩阵，这种自适应机制能够保证转动瞬心估计的平稳性，同时缩减收敛时间，适用于地形复杂的场景；4)由于引入了粗大误差检测，所以能够减弱传感器故障对估计算法的影响，提升准确度。

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

附图说明

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

图1是根据本发明的滑移转向机器人的位姿和转动瞬心联合估计方法的流程图；

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

图3示出了根据本发明一实施例的转动瞬心估计仿真结果；以及

图4示出了根据本发明一实施例的位姿估计仿真结果。

具体实施方式

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

本发明提供了一种滑移转向机器人的位姿和转动瞬心联合估计方法，通过引入保证估计与地形检测算法，能够同时获取位姿和转动瞬心的估计值。与已有的技术相比，本发明不需要过程噪声和观测噪声的概率分布作为先验知识，这意味着在实施本专利之前不需要做大量的统计实验，同时对于噪声概率分布时变的情况具有较强的鲁棒性；另外，由于引入了地形检测，在地形发生明显变化的时候，本发明会调整转动瞬心的过程噪声包络矩阵，这种自适应机制能够保证转动瞬心估计的平稳性，同时缩减收敛时间，适用于地形复杂的场景。

如图1所示，本发明的基于卡尔曼滤波的滑移转向机器人的位姿和转动瞬心联合估计方法的流程如下：

s10、初始化

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

采样点序号k＝0，后验状态估计椭球的初始化如下：椭球中心中的六个元素需要根据实际情况确定，椭球包络矩阵pk＝0.1×i6×6，记号表示椭球集，椭球集的第1个元素表示椭球中心，第2个元素表示椭球包络矩阵，地形特征向量pk＝o8×1，过程噪声和观测噪声的包络矩阵qk和rk分别为6行6列对角阵和3行3列对角阵，需要根据实际情况确定，采样间隔t，车轮半径φ，车身宽度b需要根据实际情况确定。其中，下标k表示采样点序号，i6×6为6行6列的单位矩阵，o8×1为8行1列的零向量，与分别表示东向坐标、北向坐标与航向角的后验状态估计椭球的中心，与为转动瞬心的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消除加速度数据集合的直流分量：将加速度数据集合的所有元素分别减去该集合所有元素的均值，即得到消除了直流分量的加速度数据集合i＝1,…,n。

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

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

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

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

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

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

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

s50、状态预测

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

计算先验状态估计椭球如下：

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

其中，矩阵为状态转移方程f(·)的雅克比矩阵，tr(·)表示矩阵的迹。

s60、状态更新

根据步骤s10中获得的观测噪声包络矩阵，步骤s20中获得的机器人航向角以及步骤s50中获得的先验状态估计椭球，进行状态更新，得到后验状态估计椭球，具体如下：

6.1计算新息∈k如下：

其中，为观测矩阵。

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

其中，msvm·表示矩阵的最大奇异值。

6.3计算后验状态估计椭球如下：

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

其中，矩阵i6×6为6维单位矩阵；

6.4消除粗大误差：如果δk≤0，表明传感器出现故障，计算后验状态估计椭球如下：

pk＝pk,k-1。

s70、重复步骤s20至s60，可以获得每一个采样点的位姿与转动瞬心的后验状态估计椭球，这些椭球的中心就是位姿与转动瞬心的估计值。

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

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