一种蛇形机器人路径跟随规划方法与流程

本发明涉及蛇形机器人运动规划与控制技术领域，尤其涉及一种蛇形机器人路径跟随规划方法。

背景技术

蛇形机器人自由度多，非常灵活使得它可以应用与核工厂管道，飞机发动机及机翼油箱等复杂设备的检修。但蛇形机器人自由度多，很难求得其运动学逆解，因此其运动规划问题也较为复杂，一直是学者们研究的热点问题；通过文献调研发现针对蛇形机器人的运动规划主要通过以下几种方法解决：基于雅可比的迭代方法、几何关系及启发式迭代法和曲线规划法。

蛇形机器人虽然没有解析逆，但可以利用雅可比的广义逆迭代运动至目标点，通常利用的雅可比迭代本质上是一个最优化过程，而雅可比的广义逆就是这一优化问题的最优解；雅可比的迭代是在其运动学模型基础上进行迭代优化，这类算法计算量大，效率不高，因此很多学者将机器人视为多连杆结构，直接利用循环坐标下降等几何启发迭代式算法获取机器人路径，这类算法效率高，但考虑关节等约束比较困难，而且规划时容易陷入局部极小；曲线规划法是指，用一条曲线来代表机械臂的位形，通过规划不同时刻曲线的位置和形态来确定机械臂的位形，在每个时刻，控制机械臂与对应的位形曲线匹配，通常选用的曲线所含参数个数远小于机器人的自由度数目，使得机器人运动规划维数大大降低，但该算法使得机器人在运动时扫过的空间区域较大，不利于蛇形机器人在狭小封闭空间中开展作业。

因此，本领域的技术人员致力于开发蛇形机器人路径跟随规划方法，使蛇形机器人在运动过程中在空间中扫过的区域尽量小，以便能灵活的进入狭小封闭空间开展操作。

技术实现要素：

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是采用空间曲线路径跟随的方法规划蛇形机器人的运动，使蛇形机器人在运动过程中在空间中扫过的区域尽量小，以便能灵活的进入狭小封闭空间开展操作。

为实现上述目的，本发明提供了蛇形机器人路径跟随规划方法，包括以下主要步骤：

(1)蛇形机器人路径曲线生成，具体为根据所述蛇形机器人在空间中所要经过的点，或者根据空间障碍物等约束信息反求所述蛇形机器人机械臂的可行路径点，生成一条连续光滑无碰撞的路径曲线；

(2)蛇形机器人关节位置搜索，具体为所述蛇形机器人路径曲线生成以后，所述蛇形机器人每往前进一步，需搜索所述蛇形机器人机械臂的关节点与所述蛇形机器人路径曲线上的匹配点；

(3)蛇形机器人关节角度反解，具体为在一个控制周期内，所述蛇形机器人机械臂的关节点确定之后，可以根据所述关节点信息以及所述蛇形机器人运动学模型反求所述蛇形机器人在该时刻下对应的关节转角。

进一步地，所述步骤(1)包括离线预先生成和在线实时生成两种方式。

进一步地，所述步骤(1)中所述离线预先生成具体为当所述蛇形机器人工作环境参数已知时，根据所述蛇形机器人机械臂所要经过的点和障碍的位置，确定所述蛇形机器人在空间中所要通过的关键点，利用样条曲线将所述关键点连接起来形成所述蛇形机器人路径曲线。

进一步地，所述步骤(1)中所述离线预先生成也可以为当所述蛇形机器人工作环境参数已知时，通过a*算法、蚁群算法等自动路径规划算法自动生成所述蛇形机器人路径曲线。

进一步地，所述步骤(1)中所述在线实时生成具体为当所述蛇形机器人工作环境参数未知时，在所述蛇形机器人实时控制中，基于各类传感器实时获取环境特征并调整所述蛇形机器人路径曲线，所述蛇形机器人机械臂跟随所述蛇形机器人路径曲线运动滞后于所述蛇形机器人路径曲线的生成。

进一步地，所述样条曲线并不限定于某种形式，包括多项式、bezier、b样条曲线等。

进一步地，所述步骤(2)中当所述蛇形机器人路径曲线连续时，在t时刻所述蛇形机器人机械臂的关节中心c1(t)-cn(t)需落在所述蛇形机器人路径曲线上，同时相邻的所述关节点需满足杆长公式：

||ci(t)-ci-1(t)||＝li-1,i∈[2,n]

其中||g||为欧几里德范数，li-1为第i-1根杆的长度，当t＝0时，第一个所述关节点与所述蛇形机器人路径曲线起始点重合，c1(0)＝p1，所述蛇形机器人的驱动底座沿所述蛇形机器人路径曲线做进给运动，等时间间隔采样，所述驱动底座的位置已知，故c1(t)可由驱动进给量求得，对应时刻其余的所述关节点可由所述杆长公式逐个求得。

进一步地，所述杆长公式中的非线性方程可采用二分法搜索、牛顿法迭代法及梯度下降法等方法求解，可将杆长的相对误差ξ作为算法迭代终止的条件，ξ如下式所示：

当相对误差ξ小于给定阈值时，认为找到了所述蛇形机器人路径曲线上对应的关节点。

进一步地，所述步骤(2)中当所述蛇形机器人路径曲线离散时，在t时刻所述蛇形机器人机械臂关节中心c1(t)-cn(t)则需落在所述蛇形机器人路径曲线对应的离散点上，同时相邻的所述关节点仍需满足所述杆长公式，ci(t)表示在t时刻，所述蛇形机器人第i个关节在所述蛇形机器人路径曲线上的离散点pni，n为所述离散点pni在所有离散点中的序号，t+1时刻下所述蛇形机器人的机械臂底座作一个微小的进给，所述关节点也沿着所述蛇形机器人路径曲线的离散点往前进的方向移动相同的一个微小距离，可得到如下关系：

n(i+1)(t+1)-n(i)(t+1)＝n(i+1)(t)-n(i)(t)

记δn(i)(t)＝n(i+1)(t)-n(i)(t)，可得：

n(i+1)(t+1)＝δn(i)(t)+n(i)(t+1)

在t时刻第i个对应的所述关节点所在离散点的序号已知，故δn(i)(t)为已知量，称其为迭代步长，n(i)(t+1)可根据当前关节点所在离散点的序号n(i)(t)以及进给量求得，因此t+1时刻i+1关节点所在离散点的序号的估计值可以求出，再判断所述离散点是否满足杆长条件，若不满足条件，在所述离散点附近搜索即可。

进一步地，所述步骤(3)由获得某时刻下，所述蛇形机器人机械臂关节中心c1(t)-cn(t)坐标，反求相邻关节之间的转动角度，所述蛇形机器人机械臂每个关节为万向节，两旋转轴分别与yi，xi重合，相邻机械臂关节中心c1(t)与cn(t)之间连线与坐标系的zi-1轴重合，将关节i-1坐标系xi-1yi-1zi-1沿zi-1平移li-1得到坐标系xi-1’yi-1’zi-1’，将新的坐标系绕着yi-1’转αi得到坐标系xi-1”yi-1”zi-1”，最后将新得到的坐标系绕xi-1”旋转βi得到关节i处的坐标系xiyizi，根据以上关系，相邻z轴之间变换关系可表示为：

因此可求解出每个关节的转角如下式：

与现有技术相比，本发明具有如下的有益效果：

1、采用空间曲线路径跟随的方法规划蛇形机器人的运动，使机器人在运动过程中在空间中扫过的区域尽量小，以便蛇形机器人能灵活的进入狭小封闭空间开展操作。

2、将每次寻点搜索的迭代步长保存下来作为下次寻点的估计值，可以有效减少迭代计算次。

3、计算效率高，计算量小，编程实现简单，可进一步用于蛇形机器人复杂空间运动的实时控制。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明的一个较佳实施例中蛇形机器人的结构；

图2是本发明的一个较佳实施例中蛇形机器人的运动障碍空间中的关键点；

图3是本发明的一个较佳实施例中根据关键点生成路径曲线；

图4是本发明的一个较佳实施例中在离散的路径点上搜索蛇形机器人机械臂关节对应的点；

图5是本发明的一个较佳实施例中单节机械臂运动坐标系模型。

具体实施方式

以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。

在附图中，结构相同的部件以相同数字标号表示，各处结构或功能相似的组件以相似数字标号表示。附图所示的每一组件的尺寸和厚度是任意示出的，本发明并没有限定每个组件的尺寸和厚度。为了使图示更清晰，附图中有些地方适当夸大了部件的厚度。

如图1所示，在底座4的进给运动下，蛇形机器人的连杆和关节2沿规划的路径曲线3运动，即运动中的任意时刻，整个蛇形机械臂尽可能逼近路径曲线，该运动中，蛇形机械臂看起来始终都跟随其机械臂头部末端执行器1运动轨迹，只要机械臂头部末端执行器1能通过障碍物，则机械臂的其他部分就能跟随其头部的运动轨迹通过障碍物。

本方法是通过仿生研究，发现自然界中的一些动物如蛇等穿过狭小空间等复杂的地形时，几乎是沿着一条曲线通过，这些曲线拥有类似三角函数曲线的规律。

如图2所示，根据蛇形机器人的工作情况，获得一条可行的路径曲线，路径的生成可分离线预先生成和在线实时生成两种方式：当工作环境参数已知时，根据机械臂所要经过的位置和障碍的位置，确定机器人在空间中所要通过的关键点，根据关键点，利用样条曲线将这些点连接起来形成机器人的一条可行路径；另一方面，可以通过一些搜索算法如a*算法、蚁群算法等自动路径规划算法生成一条避开障碍物的可行路径曲线。在当获取环境参数后，通过a*算法、蚁群算法等算法自动生成路径轨迹，或者通过人工标记的方法，获得工作空间中需要通过的关键点。

当机器人工作环境未知时，需要在线实时生成路径，在机器人实时控制中，基于各类传感器实时获取环境特征并调整机器人前进路线，在线实时控制中，路径连续生成，蛇形机械臂跟随路径运动滞后于路径的生成。

如图3所示，其路径曲线可以根据机器人所需通过关键点，利用样条曲线生成，工作空间中需要通过的关键点为p1-pm，m为关键点的个数。蛇形机器人的初始姿态下其各关节中心ci以及超冗余度机械臂的起始点c1和末端点cn，n为关节数目，最后将p1-pm和c1-cn都作为曲线控制点，以生成路径曲线，这里的样条曲线并不限定与某种形式。常用的包括多项式，bezier，b样条曲线等。

路径生成后，蛇形机器人每往前进给一步，则需搜索机械臂的关节点与路径曲线上的匹配点，搜索机械臂关节匹配点可以分为两种路径曲线连续和离散两种情况。

当路径曲线连续时，为实现蛇形机器人跟随路径曲线，在t时刻所述蛇形机器人机械臂的关节中心c1(t)-cn(t)需落在蛇形机器人路径曲线上，同时相邻的关节点需满足杆长公式：

||ci(t)-ci-1(t)||＝li-1,i∈[2,n]

其中||g||为欧几里德范数，li-1为第i-1根杆的长度，当t＝0时，第一个关节点与所述蛇形机器人路径曲线起始点重合，c1(0)＝p1，蛇形机器人的驱动底座沿蛇形机器人路径曲线做进给运动，等时间间隔采样，驱动底座的位置已知，故c1(t)可由驱动进给量求得，对应时刻其余的关节点可由杆长公式逐个求得。

杆长公式中的非线性方程可采用二分法搜索、牛顿法迭代法及梯度下降法等方法求解，可将杆长的相对误差ξ作为算法迭代终止的条件，ξ如下式所示：

当相对误差ξ小于给定阈值时，认为找到了所述蛇形机器人路径曲线上对应的关节点。

如图4所示，当路径曲线离散时，为实现蛇形机器人跟随路径曲线，在t时刻其机械臂关节中心c1(t)-cn(t)则需落在对应的离散点上，同时相邻关节点仍需满足杆长条件，故因此先将路径曲线离散化处理，离散点可以是采用等参数法例如等弦长或者等弧长的采样取得，离散点之间的距离根据杆长、关节数及期望位置精度等确定，这一离散距离与本算法的求解效率和精度均有关。

与路径曲线连续时一样，机器人关节点与路径曲线匹配的目的是在已知上一个关节中心点根据如上所述杆长条件公式寻找下一个关节点的位置，不同的是这个关节点中心需要在一些离散的点中找出相应的关节中心点，通常可用二分法进行搜索查找，但是二分法需要设定初始搜索区间，搜索区间过小会导致最佳点落在区间外，区间过大会降低搜索效率，因此根据本专利一种用于蛇形机器人狭小封闭空间探测的路径跟随规划方法特点提出一种迭代搜索方法。

如图4所示，ci(t)表示在t时刻，蛇形机器人第i个关节在所述蛇形机器人路径曲线上的离散点pni，n为离散点pni在所有离散点中的序号，t+1时刻下蛇形机器人的机械臂底座作一个微小的进给，关节点也沿着蛇形机器人路径曲线的离散点往前进的方向移动相同的一个微小距离，可得到如下关系：

n(i+1)(t+1)-n(i)(t+1)＝n(i+1)(t)-n(i)(t)

记δn(i)(t)＝n(i+1)(t)-n(i)(t)，可得：

n(i+1)(t+1)＝δn(i)(t)+n(i)(t+1)

在t时刻第i个对应的关节点所在离散点的序号已知，故δn(i)(t)为已知量，称其为迭代步长，n(i)(t+1)可根据当前关节点所在离散点的序号n(i)(t)以及进给量求得，因此t+1时刻i+1关节点所在离散点的序号的估计值可以求出，再判断所述离散点是否满足杆长条件，若不满足条件，在离散点附近搜索即可。

由获得某时刻下，超冗余度机械臂关节中心c1(t)-cn(t)坐标，反求相邻关节之间的转动角度。

如图5所示，蛇形机器人机械臂每个关节为万向节，两旋转轴分别与yi，xi重合，相邻机械臂关节中心c1(t)与cn(t)之间连线与坐标系的zi-1轴重合，将关节i-1坐标系xi-1yi-1zi-1沿zi-1平移li-1得到坐标系xi-1’yi-1’zi-1’，将新的坐标系绕着yi-1’转αi得到坐标系xi-1”yi-1”zi-1”，最后将新得到的坐标系绕xi-1”旋转βi得到关节i处的坐标系xiyizi，根据以上关系，相邻z轴之间变换关系可表示为：

因此可求解出每个关节的转角如下式：

βi＝arcsin(-yi)

αi＝arcsin(xi/cos(βi))

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。