CN109282810A - 一种惯导和角度传感器融合的蛇形机器人姿态估计方法 - Google Patents

Abstract

本发明公开了一种惯导和角度传感器融合的蛇形机器人姿态估计方法，该方法包括以下步骤：S1、获取蛇形机器人关节模块的角度值，通过D‑H分析法对关节模块进行建模；S2、利用SVD分解法建立虚拟坐标系，将关节模块模型转移到虚拟坐标系中；S3、获取蛇形机器人关节模块的惯导信息，建立蛇形机器人关节模块运动学模型并根据扩展卡尔曼滤波算法建立关节模块的预测方程和观测方程；S4、利用扩展卡尔曼滤波算法，估算蛇形机器人的角速度，加速度，速度以及虚拟坐标系的姿态四元素；S5、根据姿态四元素利用MATLAB建立蛇形机器人动画模型。本发明可以精确地计算出蛇形机器人每个关节地运行情况，提高系统模型的准确性，可以更加直观地观察机器人的运动情况。

Description

一种惯导和角度传感器融合的蛇形机器人姿态估计方法

技术领域

本发明涉及机器人算法领域，具体是一种惯导和角度传感器融合的蛇形机器人姿态估计方法。

背景技术

随着科技的发展，机器人技术越来越先进，对于机器人的运动姿态需要采用一些姿态的评估方法进行评估。卡尔曼滤波算法是一种最优估计算法，它通过建立多维的线性系统状态方程和观测方程，通过协方差矩阵和卡尔曼滤波增益，实现对数据的融合，通过迭代计算进一步实现对系统观测变量的最优化估计。

但是机器人姿态是一种惯性导航，由于惯性导航是一种典型的非线性系统，不能直接采用卡尔曼滤波算法进行机器人的姿态评估。

因此很多企业也通过很多其他的方法对机器人的姿态进行评估。

例如，专利名称：机器人运动姿态估计方法，申请号：CN201610353912.6的专利公开提供了提供一种机器人运动姿态估计方法，该方法包括：双目摄像头采集双目视觉图像，以及惯性测量单元采集惯性数据；根据双目视觉图像更新预先获取的关键帧库和路标点库，并将路标点库中的每个路标点构建投影误差项；将惯性数据代入预先构建的惯性测量单元的运动误差模型中，获取惯性测量单元的运动模型估计误差项；将投影误差项和运动模型估计误差项代入高斯牛顿迭代模型，并对高斯牛顿迭代模型进行迭代计算，并将能够使得误差代价函数最小的解作为最优解，用于估计机器人在下一时刻的位置和姿态。该发明提供的机器人运动姿态估计方法，充分利用视觉数据和惯性数据，提高了对机器人运动姿态估计的精确度。

专利名称：基于机器视觉和惯导融合的移动机器人运动姿态估计方法，申请号：CN201110417967.6的专利公开了一种基于机器视觉和惯导融合的移动机器人运动姿态估计方法，该方法其步骤为：同步采集移动机器人双目相机图像及三轴惯导数据；提取前后帧图像特征并匹配估计运动姿态；利用惯导计算俯仰角和横滚角；建立卡尔曼滤波器模型融合视觉和惯导姿态估计；根据估计方差自适应调整滤波器参数；姿态修正的累积航位推算。本发明提出实时扩展卡尔曼滤波器姿态估计模型，利用惯导结合重力加速度方向作为补充，对视觉里程计三个方向姿态估计进行解耦，修正姿态估计的累积误差；根据运动状态采用模糊逻辑调整滤波器参数，实现自适应的滤波估计，降低加速度噪声的影响，有效的提高了视觉里程计的定位精度和鲁棒性。

以上两个专利都是使用了视觉传感器和惯导进行融合，有效地提高了姿态估计的精确度，但是视觉传感器都是需要双目摄像头，观测到的姿态也只是整体的机器人的姿态，在关节直径为60mm的蛇形机器人关节上无法装配，比较适合配置在体型较大的机器人身上，并且需要保持视觉传感器不会旋转，对于像蛇形机器人这种体积较小且多关节的机器人，不适合采用。

导致上述缺点的原因：

对于双目摄像头来说，为了得到距离信息，一般两个摄像头要保持着10-20cm的距离，而蛇形机器人的每一关节整体都比较小，直径大约为6cm左右，不适合放置双目摄像头；此外，蛇形机器人在运动的过程中可能是翻滚着前进，对于视觉传感器来说，用rolling步态或者由于蛇形机器人运动的波动较大，摄像头拍下的照片会出现抖动，获取到的图片会处于一直旋转的状态，无法正确地与前后帧图像匹配起来。惯导能够获得机器人自身的加速度和旋转信息，但是对于多关节的蛇形机器人，仅使用惯导和双目摄像头，无法正确计算出每个关节的姿态。

因此有待对现有的蛇形机器人的姿态评估方法进行进一步地改进。

发明内容

本发明目的是克服现有技术的不足，提供一种准确、可靠、有效的对蛇形机器人的姿态进行估计的算法。

本发明的技术方案是这样实现的：一种惯导和角度传感器融合的蛇形机器人姿态估计方法，该方法包括以下步骤：

S1、获取蛇形机器人所有关节模块的角度值，利用获取的角度值通过D-H分析法对蛇形机器人的所有关节模块在固定坐标系中进行建模，得到多个关节模块模型，所述蛇形机器人的相邻关节模块的为正交连接，所述固定坐标系为三维直接坐标系；

S2、利用SVD分解法建立一个虚拟坐标系，将步骤S1中得到的蛇形机器人的每个关节模块模型均转移到所述虚拟坐标系中；

S3、获取蛇形机器人所有关节模块的惯导信息，利用获取的惯导信息建立蛇形机器人所有关节模块运动学模型并根据扩展卡尔曼滤波算法建立蛇形机器人所有关节模块的预测方程和观测方程；

S4、利用S3中的扩展卡尔曼滤波算法，估算蛇形机器人的角速度，加速度，速度以及虚拟坐标系的姿态四元素；

S5、根据S4算出的姿态四元素利用MATLAB建立蛇形机器人动画模型。

优选地，在步骤S1和S2中，相邻两个正交关节模块模型在固定坐标系的转换由齐次转换矩阵分成四次变换，四次具体转换过程如下：

S1-1：沿Z_i-1移动距离a_i-1；

S1-2：沿X_i-1转动角度α_i-1；

S1-3：沿Y_i-1转动角度α_i-1；

S1-4：沿Z_i移动距离a_i；

优选地，由相邻两个正交关节模块模型在虚拟坐标系的转换，进一步得到N个关节模块的蛇形机器人第i个关节模块模型相对于第1个关节模块模型的转换关系，其数学表达式如下：

优选地，S2中建立虚拟坐标系的方法如下：

S2-1:在所述固定坐标系下找到蛇形机器人几何上的质心用第i个关节模块的质心减去构造出一个位置矩阵

S2-2:构造出矩阵的大小是n×3，n表示蛇形机器人的模块数量；

S2-3:对矩阵P进行SVD分解USV^T＝P,结合旋转矩阵V和质心可以得出虚拟坐标系相对于固定坐标系的齐次转换矩阵；

其中，在奇异值分解中，U和V都是单位矩阵分别表示P矩阵的左右奇异值矩阵，U矩阵的列向量是PP^T的特征向量，它们构成PP^T的标准正交基；

V的列向量是PP^T的特征向量，构成了PP^T的一组标准正交基；V是描述虚拟坐标系相对于固定坐标系的旋转矩阵，S对角线上的元素值是矩阵P的奇异值，S的平方是PP^T的特征值，用来描述惯性轴方向分量的大小；

S2-4:如果用表示第i个模块在当前坐标系的旋转矩阵，则左乘将每个关节模块的位置关系旋转到虚拟坐标系下。

优选地，蛇形机器人运动步态包括Rolling步态,在所述括Rolling步态前提下，S3中建立蛇形机器人所有关节模块的预测方程如下：

ε＝ωt；

建立的系统预测方程为：

优选地，蛇形机器人的每个关节模块都安装有单轴的角度传感器，3轴的加速度传感器和3轴的角度传感器；

当蛇形机器人的关节模块数量为n时，则在每一次迭代计算的过程中观测变量的维度为7n维，得到观测变量的向量形式为：

其中,α_k，γ_k分别表示在k时刻时蛇形机器人角度传感器，3轴的加速度和3轴的陀螺仪的测量值。

优选地，所述S4中蛇形机器人的角速度，加速度，速度以及虚拟坐标系的姿态四元素代表空间中的一次旋转，所述姿态四元素连续的两次旋转可以用这两个四元素的叉乘来表示，可以得到四元素在离散时间下的更新方程：

其中αΔ表示在t时间内的旋转角度，qΔ表示在时间t间隔内绕物体方向旋转了αΔ角度的四元素，表示在四元素在第k时刻相对固定坐标系的旋转矩阵。

与现有技术相比，本发明具有如下优点：

1.使用角度传感器和惯导数据进行融合，可以精确地计算出蛇形机器人每个关节地运行情况，可以进行运动分析和调试，避免了人工检查造成的不精准。

2.使用虚拟坐标系代替常规固定坐标系，并改进扩展卡尔曼滤波姿态估计的数学模型，提高系统模型的准确性。

3.适用于蛇形机器人这种多关节机器人，可以更加直观地观察机器人的运动情况。

附图说明

图1是本发明一种惯导和角度传感器融合的蛇形机器人姿态估计方法整体流图框图。

图2是本发明中两个相邻关节模块的转换图。

图3是本发明中第i个关节模块在虚拟坐标下的奇次转换矩阵。

图4是本发明蛇形机器人在固定坐标系和虚拟坐标系下的姿态对比图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步说明：

如图1所示，一种惯导和角度传感器融合的蛇形机器人姿态估计方法，该方法包括以下步骤：

S1、获取蛇形机器人所有关节模块的角度值，利用获取的角度值通过D-H参数标定法对蛇形机器人的所有关节模块在固定坐标系中进行建模，得到多个关节模块模型，所述蛇形机器人的相邻关节模块的为正交连接，所述固定坐标系为三维直接坐标系。D-H参数标定法在机器人的每个连杆上都固定一个坐标系，然后用4×4的齐次变换矩阵来描述相邻两连杆的空间关系。通过依次变换可最终推导出末端执行器相对于基坐标系的位姿，从而建立机器人的运动学方程。；

S2、利用SVD分解法(Singular Value Decomposition，奇异值分解是线性代数中一种重要的矩阵分解，是矩阵分析中基于特征向量对角化的推广，在信号处理、统计学等领域有重要应用)建立一个虚拟坐标系，将步骤S1中得到的蛇形机器人的每个关节模块模型均转移到所述虚拟坐标系中；

S3、获取蛇形机器人所有关节模块的惯导信息，利用获取的惯导信息建立蛇形机器人所有关节模块运动学模型并根据扩展卡尔曼滤波算法(卡尔曼滤波，Kalmanfiltering，是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法；由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程，扩展卡尔曼滤波算法适用于非线性系统)建立蛇形机器人所有关节模块的预测方程和观测方程；

其中，扩展卡尔曼滤波算法适用于非线性系统，其利用卡尔曼滤波算法的数据融合功能，实现多模块和多传感器系统的数据融合。

扩展卡尔曼滤波算法中时间更新方程又称预测方程，测量更新方程又称观测方程。时间更新方程主要作用是向前推算当前状态变量和误差的协方差估计值，以便为下一个时间状态构造先验估计，测量更新方程主要负责将系统测量变量和先验估计相结合从而校正系统状态估计。

S4、利用S3中的扩展卡尔曼滤波算法，估算蛇形机器人的角速度，加速度，速度以及虚拟坐标系的姿态四元素；

S5、根据S4算出的姿态四元素利用MATLAB建立蛇形机器人动画模型。

在步骤S1和S2中，相邻两个正交关节模块模型在固定坐标系的转换由齐次转换矩阵分成四次变换，四次具体转换过程如下：

S1-1：沿Z_i-1移动距离a_i-1；

S1-2：沿X_i-1转动角度α_i-1；

S1-3：沿Y_i-1转动角度α_i-1；

S1-4：沿Z_i移动距离a_i；

其中Z_i-1，a_i-1，X_i-1，α_i-1，Y_i-1，Z_i，a_i其中Z_i-1，a_i-1分别表示第i-1个关节模块在固定坐标系Z轴方向以及第i-1个关节模块的长度；

X_i-1，α_i-1分别表示第i-1个关节模块在固定坐标系X轴方向以及第i-1个关节模块的长度；

Y_i-1表示第i-1个关节模块在固定坐标系Y轴方向；

Z_i，a_i分别表示第i个关节模块在固定坐标系Z轴方向以及第i个关节模块的长度。

上式中，表示蛇形机器人第i个关节模块模型相对于第i-1个关节模块模型的转换关系；

Trans函数表示将一个点的坐标值(或一个位移量)从某一坐标系统转换到另一个坐标系统,其语法是：(trans pt from to[disp])；Rot函数为Rate of turning旋回转速，又称转向率。在物理中等于▽×符号，求向量旋度。

由相邻两个正交关节模块模型在虚拟坐标系的转换，进一步得到N个关节模块的蛇形机器人第i个关节模块模型相对于第1个关节模块模型的转换关系，其数学表达式如下：

S2中建立虚拟坐标系的方法如下：

S2-1:在所述固定坐标系下找到蛇形机器人几何上的质心用第i个关节模块的质心减去构造出一个位置矩阵

S2-2:构造出矩阵的大小是n×3，n表示蛇形机器人的模块数量；

S2-3:对矩阵P进行SVD分解USV^T＝P,结合旋转矩阵V和质心可以得出虚拟坐标系相对于固定坐标系的齐次转换矩阵；

其中，在奇异值分解中，U和V都是单位矩阵分别表示P矩阵的左右奇异值矩阵，U矩阵的列向量是PP^T的特征向量，它们构成PP^T的标准正交基；

V的列向量是PP^T的特征向量，构成了PP^T的一组标准正交基；V是描述虚拟坐标系相对于固定坐标系的旋转矩阵，S对角线上的元素值是矩阵P的奇异值，S的平方是PP^T的特征值，用来描述惯性轴方向分量的大小；

S2-4:如果用表示第i个模块在当前坐标系的旋转矩阵，则左乘将每个关节模块的位置关系旋转到虚拟坐标系下。

蛇形机器人运动步态包括Rolling步态,Rolling步态为蛇形机器人独有的运动步态,在所述括Rolling步态前提下，S3中建立蛇形机器人所有关节模块的预测方程如下：

ε＝ωt；

A参数表示蛇形机器人关节的运动振幅，ω参数控制蛇形机器人的运动速率。

建立的系统预测方程为：

状态变量θ_k-1和表示系统在k-1时刻模块间的连接角度和角速度，和表示系统在k时刻模块间的连接角度和角速度。

蛇形机器人的每个关节模块都安装有单轴的角度传感器，3轴的加速度传感器和3轴的角度传感器；

当蛇形机器人的关节模块数量为n时，则在每一次迭代计算的过程中观测变量的维度为7n维，得到观测变量的向量形式为：

其中,α_k，γ_k分别表示在k时刻时蛇形机器人角度传感器，3轴的加速度和3轴的陀螺仪的测量值。

所述S4中蛇形机器人的角速度，加速度，速度以及虚拟坐标系的姿态四元素代表空间中的一次旋转，所述姿态四元素连续的两次旋转可以用这两个四元素的叉乘来表示，可以得到四元素在离散时间下的更新方程,得到机器人整体位置的旋转情况：

其中αΔ表示在t时间内的旋转角度，qΔ表示在时间t间隔内绕物体方向旋转了αΔ角度的四元素，表示在四元素在第k时刻相对固定坐标系的旋转矩阵,表示蛇形机器人在K时刻的运动速率。

根据上述说明书的揭示和教导，本发明所属领域的技术人员还可以对上述实施方式进行变更和修改。因此，本发明并不局限于上面揭示和描述的具体实施方式，对发明的一些修改和变更也应当落入本发明的权利要求的保护范围内。此外，尽管本说明书中使用了一些特定的术语，但这些术语只是为了方便说明，并不对本发明构成任何限制。

Claims (7)

1.一种惯导和角度传感器融合的蛇形机器人姿态估计方法，其特征在于，该方法包括以下步骤：

S1、获取蛇形机器人所有关节模块的角度值，利用获取的角度值通过D-H分析法对蛇形机器人的所有关节模块在固定坐标系中进行建模，得到多个关节模块模型，所述蛇形机器人的相邻关节模块的为正交连接，所述固定坐标系为三维直接坐标系；

S2、利用SVD分解法建立一个虚拟坐标系，将步骤S1中得到的蛇形机器人的每个关节模块模型均转移到所述虚拟坐标系中；

S3、获取蛇形机器人所有关节模块的惯导信息，利用获取的惯导信息建立蛇形机器人所有关节模块运动学模型并根据扩展卡尔曼滤波算法建立蛇形机器人所有关节模块的预测方程和观测方程；

S4、利用S3中的扩展卡尔曼滤波算法，估算蛇形机器人的角速度，加速度，速度以及虚拟坐标系的姿态四元素；

S5、根据S4算出的姿态四元素利用MATLAB建立蛇形机器人动画模型。

2.如权利要求1所述的一种惯导和角度传感器融合的蛇形机器人姿态估计方法，其特征在于，

在步骤S1和S2中，相邻两个正交关节模块模型在固定坐标系的转换由齐次转换矩阵分成四次变换，四次具体转换过程如下：

S1-1：沿Z_i-1移动距离a_i-1；

S1-2：沿X_i-1转动角度α_i-1；

S1-3：沿Y_i-1转动角度α_i-1；

S1-4：沿Z_i移动距离a_i；

3.如权利要求2所述的一种惯导和角度传感器融合的蛇形机器人姿态估计方法，其特征在于，由相邻两个正交关节模块模型在虚拟坐标系的转换，进一步得到N个关节模块的蛇形机器人第i个关节模块模型相对于第1个关节模块模型的转换关系，其数学表达式如下：

4.如权利要求1所述的一种惯导和角度传感器融合的蛇形机器人姿态估计方法，其特征在于，S2中建立虚拟坐标系的方法如下：

S2-1：在所述固定坐标系下找到蛇形机器人几何上的质心用第i个关节模块的质心减去构造出一个位置矩阵

S2-2：构造出矩阵的大小是n×3，n表示蛇形机器人的模块数量；

S2-3：对矩阵P进行SVD分解USV^T＝P，结合旋转矩阵V和质心可以得出虚拟坐标系相对于固定坐标系的齐次转换矩阵；

其中，在奇异值分解中，U和V都是单位矩阵分别表示P矩阵的左右奇异值矩阵，U矩阵的列向量是PP^T的特征向量，它们构成PP^T的标准正交基；

V的列向量是PP^T的特征向量，构成了PP^T的一组标准正交基；V是描述虚拟坐标系相对于固定坐标系的旋转矩阵，S对角线上的元素值是矩阵P的奇异值，S的平方是PP^T的特征值，用来描述惯性轴方向分量的大小；

S2-4：如果用表示第i个模块在当前坐标系的旋转矩阵，则左乘将每个关节模块的位置关系旋转到虚拟坐标系下。

5.如权利要求1所述的一种惯导和角度传感器融合的蛇形机器人姿态估计方法，其特征在于，蛇形机器人运动步态包括Rolling步态，在所述Rolling步态下，S3中建立蛇形机器人所有关节模块的预测方程如下：

ε＝ωt；

建立的系统预测方程为：

6.如权利要求1所述的一种惯导和角度传感器融合的蛇形机器人姿态估计方法，其特征在于，

蛇形机器人的每个关节模块都安装有单轴的角度传感器，3轴的加速度传感器和3轴的角度传感器；

当蛇形机器人的关节模块数量为n时，则在每一次迭代计算的过程中观测变量的维度为7n维，得到观测变量的向量形式为：

其中，α_k，γ_k分别表示在k时刻时蛇形机器人角度传感器，3轴的加速度和3轴的陀螺仪的测量值。

7.如权利要求1所述的一种惯导和角度传感器融合的蛇形机器人姿态估计方法，其特征在于，

所述S4中蛇形机器人的角速度，加速度，速度以及虚拟坐标系的姿态四元素代表空间中的一次旋转，所述姿态四元素连续的两次旋转可以用这两个四元素的叉乘来表示，可以得到四元素在离散时间下的更新方程：

其中α_Δ表示在t时间内的旋转角度，q_Δ表示在时间t间隔内绕物体方向旋转了α_Δ角度的四元素，表示在四元素在第k时刻相对固定坐标系的旋转矩阵。