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

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

Joint estimation of pose and rotation instantaneous center for a sliding steering robot

The present invention discloses a joint estimation method of position and rotating instantaneous center of sliding steering robot, including initialization step, data acquisition step of sensor, step of terrain detection, noise envelope matrix step of adjustment process, state prediction step, state update step, and estimation of position and rotating instantaneous center. Sudden. Compared with the existing technology, the present invention does not require the probability distribution of process noise and observed noise as a prior knowledge, which means that no large number of statistical experiments need to be done before the implementation of the invention, and it has strong robustness to the time variation of the probability distribution of the noise; moreover, because of the introduction of topographic detection, When the shape changes obviously, the invention will adjust the process noise envelope matrix of the rotating instantaneous center. This adaptive mechanism can ensure the stability of the rotating instantaneous center estimation and reduce the time of convergence, which is suitable for the complex terrain.

【技术实现步骤摘要】
一种滑移转向机器人的位姿和转动瞬心联合估计方法
本专利技术涉及机器人
，特别是涉及一种滑移转向机器人的位姿和转动瞬心联合估计方法。
技术介绍
滑移转向机制可以通过改变左右轮子或者履带的速度来控制移动机器人的方向，由于其具有很好的鲁棒性和灵活性，并且能够实现零半径转向，滑移转向机制被广泛的应用在野外机器人上。由于滑移转向机器人的运动控制与路径规划大多是基于运动模型的，所以运动模型的准确性势必会对机器人的控制性能产生较大的影响。但是，滑移转向机器人在移动过程中不可避免的出现打滑现象，为了建立准确的运动模型必须要引入转动瞬心，而转动瞬心往往随着地形的变化而变化。因此，如何实时的获取滑移转向机器人的转动瞬心成为了机器人领域中一项重要且具有挑战性的工作。目前，针对滑移转向机器人的位姿和转动瞬心联合估计方法的相关研究刚刚起步，已有的研究成果较少。已有文献主要采用的如扩展卡尔曼滤波与无色卡尔曼滤波要求噪声是高斯白噪声，这在实际中往往不容易满足，比如车轮发生形变以后将会为系统带来常值误差。另外，转动瞬心的过程噪声方差往往被设置成常值，如果设置成一个较大的值，在地形发生变化的时候，转动瞬心的估计值将会快速收敛，但是会产生较大的抖动；如果设置成一个较小的值，转动瞬心的估计值会相对平稳，但是在地形变化后其收敛过程较长，不适合地形复杂的应用场景。所以基于以上两个问题，本专利技术提出了一种滑移转向机器人的位姿和转动瞬心联合估计方法。
技术实现思路
本专利技术的目的在于提供一种滑移转向机器人的位姿和转动瞬心联合估计方法,以适应地形复杂的应用场景。为此，本专利技术提供了一种滑移转向机器人的位姿和转动瞬心联合估计方法，包括以下步骤：步骤一：对采样点序号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模块本文档来自技高网...

【技术保护点】
一种滑移转向机器人的位姿和转动瞬心联合估计方法，其特征在于，包括以下步骤：步骤一：对采样点序号k、后验状态估计椭球

【技术特征摘要】
1.一种滑移转向机器人的位姿和转动瞬心联合估计方法，其特征在于，包括以下步骤：步骤一：对采样点序号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为航向角的观测值，由电子罗盘采集获得；步骤三：根据步骤一中获得的地形特征向量以及步骤二中获得的加速度数据集合和地面照片像素矩阵进行地形检测，判断地形是否发生显著变化；步骤四：若判定地形发生显著变化，在接下来五个采样点内将转动瞬心的过程噪声包络矩阵乘以设定倍数；若地形没有发生显著变化，则保持原来的包络矩阵；步骤五：根据步骤一中获得的后验状态估计椭球、采样间隔以及车身宽度，步骤二中获得的左右车轮的旋转速度以及步骤四中调整后的过程噪声包络矩阵，进行状态预测，得到先验状态估计椭球；步骤六：根据步骤一中获得的观测噪声的包络矩阵，步骤二中获得的观测向量以及步骤五中获得的先验状态估计椭球，进行状态更新，得到后验状态估计椭球；以及步骤七：重复步骤二至步骤六，以输出每一个采样点的位姿与转动瞬心的后验状态估计椭球，其中，该后验状态估计椭球的中心就是位姿与转动瞬心的估计值。2.根据权利要求1所述的滑移转向机器人的位姿和转动瞬心联合估计方法，其特征在于，所述步骤二包括以下子步骤：2.1)消除加速度数据集合的直流分量：将加速度数据集合的所有元素分别减去该集合所有元素的均值，即得到消除了直流分量的加速度数据集合2.2)提取地面照片的主色：从地面照片像素矩阵Mk中随机抽取50个像素并求平均，得到地形主色的红绿蓝分量lR,k，lG,k与lB,k；2.3)求取地形特征向量其中的元素如下：

【专利技术属性】
技术研发人员：吕文君，刘葆林，贾晓敏，郑敏，李鲲，
申请(专利权)人：安徽省一一通信息科技有限公司，
类型：发明
国别省市：安徽,34