CN114888815A - 一种连续体机器人逆运动学求解方法 - Google Patents

Abstract

一种连续体机器人逆运动学求解方法，涉及连续体机器人运动控制领域，解决现有逆运动学方法存在着求解过程复杂，计算效率低，甚至存在奇异位形等问题，本发明通过位姿插值：蛇形臂关键节点预处理，蛇形臂关键节点逆向迭代和正向迭代计算以及判断误差值等步骤实现逆运动最终求解。本发明方法生成的位姿更加自然，连续体机器人的各个关节运动分布均匀。相较于雅可比矩阵的迭代式算法，本方法运动分布均匀，无奇异点，不存在奇异位姿等问题。将本发明所述的方法应用到连续体机器人的实时控制当中，可以提高运动控制的精度且实时性高，实现对连续体机器人的实时控制。

Description

一种连续体机器人逆运动学求解方法

技术领域

本发明涉及连续体机器人运动控制领域，具体涉及一种连续体机器人逆运动学求解方法。

背景技术

连续体机器人是一种新型的无关节的仿生机器人。连续体机器人相较于传统的刚性机器人而言，可以根据障碍物环境灵活的改变自身的形状，因此对狭小和非结构化的环境具有较强的适应能力。因而广泛的应用于外科微创手术，探索救援，航空检修等领域。连续体机器人的逆运动学是保障连续体机器人在狭小空间运行和作业的基础。

然而当前的逆运动学方法存在着求解过程复杂，计算效率低，甚至存在奇异位形等一系列问题。而快速可靠的逆运动学求解，是连续体机器人实时控制的基础。

为了解决上述问题，本发明的目的是提供一种简单有效的逆运动学求解方法，实现多段式连续体机器人快速有效的逆解。

发明内容

本发明为解决现有逆运动学方法存在着求解过程复杂，计算效率低，甚至存在奇异位形等问题，提供一种连续体机器人逆运动学求解方法。

一种连续体机器人逆运动学求解方法，该方法由以下步骤实现：

步骤一、位姿插值；

根据蛇形臂的初始位姿和目标位姿进行插值，获得蛇形臂路径上的中间目标点；

步骤二、蛇形臂关键节点预处理；

根据蛇形臂的当前位姿和中间目标点的位置，对蛇形臂的关键节点的位置进行调整；

步骤三、蛇形臂关键节点逆向迭代计算；

从蛇形臂的末端开始，根据上一个关键节点的位姿和下一个关键节点的坐标，逆向迭代求解蛇形臂的构型参数；

步骤四、蛇形臂关键节点正向迭代计算；

步骤五、判断误差；

根据蛇形臂的当前位姿和中间目标点的位置，判断蛇形臂末端距离中间目标点是否小于阈值，若否，则返回执行步骤三，若是，则执行步骤六；

步骤六、根据获得的蛇形臂的构型参数，蛇形臂运动到下一个目标点；

步骤七、判断蛇形臂是否到达路径终点，若是，则结束；若否，返回执行步骤二。

本发明的有益效果：

一、本发明所述的方法，将求解连续体机器人的逆运动学问题转化为关键节点的线性运算。同时通过启发式的正向迭代过程进一步减小了迭代过程的误差，优化了全局关键节点的位置，提高了算法的收敛速度。因此本方法的时间复杂度较低，运算效率高，可应用于连续体机器人的实时路径规划中，实现多段式连续体机器人的控制。

二、本发明所述的方法，无需设计复杂的控制器，控制收敛且性能稳定，因此该方法具有控制简单。

三、本发明所述的方法，生成的位姿更加自然。连续体机器人的各个关节运动分布均匀。相较于雅可比矩阵的迭代式算法，本方法运动分布均匀，无奇异点，不存在奇异位姿等问题。

四、将本发明所述的方法应用到连续体机器人的实时控制当中，可以提高运动控制的精度且实时性高，实现对连续体机器人的实时控制。

附图说明

图1为本发明所述的一种连续体机器人逆运动学求解方法中连续体机器人结构示意图；

图2为单段连续体机器人运动学分析示意图；

图3为多段连续体机器人运动学分析原理图；

图4为本发明所述的一种连续体机器人逆运动学求解方法的流程图；

图5为关键节点的预处理原理图；

图6为连续体机器人构型参数的求解原理图；

图7为连续体机器人关键节点的逆向迭代示意图；

图8为连续体机器人关键节点正向迭代示意图；

图9为连续体机器人直线轨迹路径跟踪仿真实验效果图；

图10为连续体机器人圆弧轨迹路径跟踪仿真实验效果图，(a)为连续体机器人圆弧轨迹跟随的前半部分(b)为连续体机器人圆弧轨迹跟随的后半部分。

具体实施方式

结合图1至图10说明本实施方式，一种连续体机器人的逆运动学求解方法，包括下列步骤：

S1、位姿插值：

设连续体机器人初始位姿为T₀，连续体机器人目标位姿为T₁,中间存在n个点，连续体机器人的位姿插值具体实现方式如下：

将齐次旋转矩阵转化为四元数，之后利用插值方法进行求解。设连续体机器人的当前位姿所对应的齐次旋转矩阵T为：

旋转矩阵T对应的四元数q₀为:

q0＝a₀i+b₀j+c₀k+w₀

式中，i、j、k为四元数虚数部分的三个单位，a₀、b₀、c₀为四元数虚部的数值，w₀为四元数实部的数值；

首先将齐次旋转矩阵转化为四元数，具体实施方式为：

设连续体机器人初始位姿T0和目标位姿T1对应的齐次旋转矩阵为：

q₀＝a₀i+b₀j+c₀k+w₀

q₁＝a₁i+b₁j+c₁k+w₁

引入中间变量t则第i个中间点对应的中间变量为：

两个四元数的夹角为：

设定阈值ε，若两个四元数的夹角小于ε，采用归一化线性插值的方法计算第i个中间点所对应的四元数。若两个四元数的夹角大于ε，则采用球面线性插值的方法进行插值。

若第i个中间点对应的齐次矩阵为T_i：

其中旋转矩阵R_i可以利用当前点处的四元数求得：

第i个中间点的坐标可以通过线性插值得到。p1(px1，py1，pz1)为连续体机器人初始位姿的坐标，p2(px2，py2，pz2)为连续体机器人目标位姿的坐标

S2、预处理，如图5所示。

在S2阶段中，根据连续体机器人当前关键节点的位置和中间目标点的位置对关键节点的位置进行调整，包括顺序执行下列的步骤：

步骤一：设连续体机器人目标点的坐标为p_end,连续体机器人当前末端点的坐标为p_ini,将连续体机器人末端点移动到目标点。此时连续体机器人末端点移动的距离为：

d_inv＝p_end-p_ini

步骤二：移动其他关键节点，作末端点到目标点的连线，其他关键节点的移动方向和末端点移动的方向一致。若连续体机器人共有n个关键节点，第i个关键节点的移动距离为：

S3、逆向迭代，如图6所示。

在步骤S3逆向迭代中，已知了第i个关键节点的位姿和第i-1个节点的坐标，求解第i段连续体机器人的具体方法如下：

设第i-1个关键节点的坐标为P(x₁,y₁,z₁)，第i个关键节点的坐标为O(x₀,y₀,z₀),O点的局部坐标系{Ti}，X轴的方向向量为n_x,Y轴的方向向量为n_y,Z轴的方向向量为n_z,平面XOY的方程为：

nz_x(x-x₀)+nz_y(y-y₀)+n_z(z-z₀)＝0

点P到平面XOY的距离为：

线段OP的长度为：

根据几何关系，可以求解得到第i段连续体机器人的弯曲角α_i，当点P位于平面XOY上方时候，α_i取正值，否则取负值。

设直线OP的方向向量为n₁,易求得OP和Y轴正半轴的夹角θ的余弦值

根据三余弦定理，可以求出第i段连续体机器人的方向角θi，当点P位于平面YOZ上方是θ_i取负值，否则取正值。

本实施方式中，如图7所示，具有i段的连续体机器人逆向迭代，在步骤S3逆向迭代中，通过逆向迭代求解连续体机器人的构型参数，包括顺序执行以下步骤：

步骤一、根据第i+1个关键节点的位姿和第i个关键节点的坐标，求解第i段连续体机器人的构型参数。

步骤二、根据第i+1个关键节点的坐标和i段连续体机器人的构型参数，求解第i个关键节点的坐标

步骤三、重复上述步骤，直到求得第一段连续体机器人的构型参数

S4、正向迭代，如图8所示。

具有i段连续体机器人的正向迭代，在S4正向迭代步骤中通过正向迭代减小和目标点的误差，设连续体机器人第i个关键节点的坐标为pi,连续体机器人第i段的长度为l，具体过程如下：

首先，固定连续体机器人的末端关键节点,将连续体机器人的第一个关键节点移动到初始位置p₁'；第n个关键节点的坐标和第i个关键节点的坐标分别为P_n和P_i；第一个关键节点的坐标为P₁；

移动其他关键节点。更新后的第i个关键节点位于第i个关键节点和第i-1个关键节点的连线上，移动距离取决于关键节点所处的相对位置，如下式所示：

其中

本实施方式中，如图1所示，对连续体机器人本体结构分析：

连续体机器人是一种具有较高的灵活性和较好柔顺性能的仿生机器人，一般由机器人本体1，驱动机构2，进给机构3组成。由于连续体机器人本体的结构类似于蜿蜒的蛇，因此又称之为蛇形臂。蛇形臂通常由多个关节段串联构成，每个关节段具有旋转和弯曲两个自由度。每个关节段的连接处为关键节点。进给机构具有一个自由度，可以实现一维的精确进给运动。驱动机构主要由电机和滑台构成，为连续体机器人实现驱动。

如图2所示，对单段连续体机器人的运动学分析；

第i段连续体机器人如图所示，连续体机器人和坐标轴的Z轴相切。连续体机器人单个关节的弯曲曲线可以近似的看成连续光滑的圆弧曲线。连续体机器人的单个关节的位姿可以用两个参数进行描述，弯曲角α和方向角θ，称之为连续体机器人的构型参数。

单个关键节点的坐标系{T_i}与{T_i-1}之间的坐标变换可以通过旋转矩阵得到，具体实现方式为：绕Z逆时针旋转θ角度，沿着Y轴平移l_i(1-cosα_i)/α_i,之后沿着Z轴平移l_isinα_i/α_i，最后绕X轴顺时针旋转α_i角度，如下式所示。

如图3所示，对多段连续体机器人运动学分析；

连续体机器人由多个关节串联而成，距离连续体机器人基座最近的关节为关键节点1，之后为关键节点2、关键节点3，连续体机器人末端为关节n；机器人基座坐标系{T1}和机器人末端的坐标系{Tn}之间的坐标变换关系如下式所示：

如图9和图10所示，对连续体机器人仿真实验：为了验证本实施方式提出的连续体机器人逆运动学求解方法，进行了以下实验：

首先，对连续体机器人直线轨迹逆运动学仿真实验：

连续体机器人直线轨迹逆运动学仿真实验如图9所示，连续体机器人初始位姿为位姿1，连续体机器人末端通过直线轨迹经过中间点B点到达最终目标点C点。连续体机器人在整个运动过程中运动平稳，连续性较高，没有位姿和构型的突变。

其次，对连续体机器人圆弧轨迹逆运动学仿真实验：

连续体机器人圆弧轨迹逆运动学仿真如图10所示，连续体机器人末端运动轨迹为圆弧，通过逆运动学求解得到了连续体机器人的构型参数。在整个过程中，连续体机器人运动分布均匀，没有不连续的突变。

通过以上实验可以充分的证明本实施方式所提出的连续体机器人逆运动学方法能够实现连续体机器人逆运动学的求解，具有求解简单，时间复杂度低和运动分布均匀等特点。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (4)

1.一种连续体机器人逆运动学求解方法，其特征是：该方法由以下步骤实现：

步骤一、位姿插值；

根据蛇形臂的初始位姿和目标位姿进行插值，获得蛇形臂路径上的中间目标点；

步骤二、蛇形臂关键节点预处理；

根据蛇形臂的当前位姿和中间目标点的位置，对蛇形臂的关键节点的位置进行调整；

步骤三、蛇形臂关键节点逆向迭代计算；

从蛇形臂的末端开始，根据上一个关键节点的位姿和下一个关键节点的坐标，逆向迭代求解蛇形臂的构型参数；

步骤四、蛇形臂关键节点正向迭代计算；

步骤五、判断误差；

根据蛇形臂的当前位姿和中间目标点的位置，判断蛇形臂末端距离中间目标点是否小于阈值，若否，则返回执行步骤三，若是，则执行步骤六；

步骤六、根据获得的蛇形臂的构型参数，蛇形臂运动到下一个目标点；

步骤七、判断蛇形臂是否到达路径终点，若是，则结束；若否，返回执行步骤二。

2.根据权利要求1所述的一种连续体机器人逆运动学求解方法，其特征在于：步骤二中，对蛇形臂的关键节点的位置进行调整的具体过程为：

首先，设定连续体机器人中间目标点的坐标为p_end，连续体机器人当前末端点的坐标为p_ini，将连续体机器人当前末端点移动到中间目标点；则连续体机器人末端点移动的距离为：

d_inv＝p_end-p_ini

然后，移动其他关键节点，并作当前末端点到中间目标点的连线，其他关键节点的移动方向和当前末端点移动的方向一致；若连续体机器人共有n个关键节点，则第i个关键节点的移动距离为：

式中，l_m为第m段连续体机器人的长度。

3.根据权利要求1所述的一种连续体机器人逆运动学求解方法，其特征在于：步骤三中蛇形臂关键节点逆向迭代计算的过程为：

步骤一、根据第i+1个关键节点的位姿和第i个关键节点的坐标，求解第i段连续体机器人的构型参数；

步骤二、根据第i+1个关键节点的坐标和i段连续体机器人的构型参数，求解第i个关键节点的坐标；

步骤三、返回执行步骤一，直到求得第一段连续体机器人的构型参数。

4.根据权利要求1所述的一种连续体机器人逆运动学求解方法，其特征在于：步骤四中，通过正向迭代减小和中间目标点的误差，设定连续体机器人第i个关键节点的坐标为p_i，连续体机器人第i段的长度为l，具体过程为：

首先，固定连续体机器人的末端关键节点，将连续体机器人的第一个关键节点移动到初始位置；

然后，移动其他关键节点，更新后的第i个关键节点位于第i个关键节点和第i-1个关键节点的连线上，移动距离取决于关键节点所处的相对位置，如下式所示：

其中：

式中，P_i第i个关键节点的坐标，P_i+1第i+1个关键节点的坐标，P₁为第一个关键节点的坐标，P₁'为初始位置坐标，l_i为第i段连续体机器人的长度。

Images