CN110125942A - 一种用于移动型蛇形机械臂的平面轨迹跟踪方法 - Google Patents

Abstract

本发明属于自动控制的技术领域，公开了一种用于移动型蛇形机械臂的平面轨迹跟踪方法，包括在目标点和末端关节点的初始位置点之间设置多个子目标点；将末端关节点移至第一个子目标点，从末端关节点至始端关节点依次迭代计算每个关节点所需移动到的位置点，根据始端关节点对应位置点得到基座所需移动到的位置点，将其移动并固定，判断始端关节点与相邻关节点之间的角度是否满足角度约束，若是，将始端关节点移至下一个子目标点；否则，将与之相邻的关节点移至角度约束范围内，从始端关节点至末端关节点重新依次迭代计算每个关节点所需移动到的位置点，将末端关节点移至下一个子目标点；重复上述步骤，完成末端关节点到所有子目标点和目标点的移动。

Description

一种用于移动型蛇形机械臂的平面轨迹跟踪方法

技术领域

本发明属于自动控制的技术领域，具体涉及一种用于移动型蛇形机械臂的平面轨迹跟踪方法。

背景技术

随着社会需求的不断提高，机器人技术在社会各个领域得到了广泛的应用，超冗余蛇形机械臂由于具有身体细长、环境适应性强等优点，在航天航空制造、核辐射管道、核电设备检修和地震灾后救援等领域有广泛的应用。具有移动基座的蛇形机械臂显著扩大机械臂的操作空间，增强了机械臂的操作灵活性。然而随着机械臂自由度的增加，带来了其运动学逆解不唯一，运动学求解困难，计算量大等特点。因此学者们在冗余机械臂逆运动学方面展开了积极的研究。

传统技术中移动机械臂路径跟踪方法有三种方式：第一种是利用传统的基于雅可比矩阵的数值优化法，这种方法在自由度多时计算量很大，且容易出现奇异性问题。第二种为智能算法其大幅度提高了机器人逆运动学的求解效率，但是对求数值解的实时性提高不明显，也很少把关节角的限定范围作为适应度函数的约束条件。第三种是末端跟随启发式算法，基本思想是给定一条无碰撞的机械臂末端轨迹，仅控制末端的运动，余下的机械臂跟随末端运动，其缺点为近端关节的潜在运动能力消失，路径的曲率不能太大，且需要从特定的位形开始运动，降低了其灵活性。

发明内容

本发明提供了一种用于移动型蛇形机械臂的平面轨迹跟踪方法，解决了现有控制方法的计算量大、实时性差、灵活性低等问题。

本发明可通过以下技术方案实现：

一种用于移动型蛇形机械臂的平面轨迹跟踪方法，包括以下步骤：

步骤一、在目标点和机械臂的末端关节点的初始位置点之间设置多个子目标点；

步骤二、将末端关节点移至第一个子目标点，从末端关节点至始端关节点依次迭代计算每个关节点所需移动到的位置点；

步骤三、根据此时的始端关节点对应的位置点计算得到机械臂的基座所需移动到的位置点，将基座移至其对应的位置点固定，判断此时的始端关节点与其相邻的关节点之间的角度是否满足角度约束，若是，将此时的末端关节点移至下一个子目标点；否则，将与始端关节点相邻的关节点移至角度约束范围内，从始端关节点至末端关节点重新依次迭代计算每个关节点所需移动到的位置点，将此时的末端关节点移至下一个子目标点；

步骤四、重复步骤二至三，完成末端关节点到所有子目标点和目标点的移动；

步骤五、判断此时的末端关节点是否到达目标点，若是，根据此时获得的各个关节点的位置点坐标，控制末端关节点移动到达目标点，否则，重复步骤二至三，直至末端关节点到达目标点。

进一步，所述基座所需移动到的位置点通过将始端关节点对应的位置点在基座的移动方向上进行投影获得。

进一步，从所述末端关节点的初始位置点到目标点，将两者之间的距离等间隔划分多个子目标点，将所述末端关节点由远至近从第一个子目标点向最后一个子目标点，再向目标点移动。

进一步，根据相邻关节点之间的角度约束、相邻关节点之间的连杆长度和相邻关节点之间的移动约束，利用纯几何解析法计算所述步骤二和三中各个关节点所需移动到的位置点，再利用圆周边界法判断所述位置点是否满足相邻的关节点之间的角度约束θ，若是，保留所述位置点，否则，用圆周边界法获得的边界位置点替代所述位置点。

进一步，利用纯几何解析法计算所述步骤二和三中各个关节点所需移动到的位置点包括：

记相邻三个关节点分别为p_i+1(x₁,y₁)、p_i(x₂,y₂)、p_i-1(x₆,y₆)，当关节点p_i+1(x₁,y₁)移动到p’_i+1(x₃,y₃)时，下一时刻相邻的关节点p’_i(x₄,y₄)要保证落在上一时刻的关节点p_i(x₂,y₂)和p_i+1(x₁,y₁)的连线上，同理关节点p’_i-1(x₅,y₅)要落在关节点p_i-1(x₆,y₆)、p_i(x₂,y₂)的连线上，三个杆向量的方向定义为

已知上一时刻相邻三个关节点分别为p_i+1(x₁,y₁)、p_i(x₂,y₂)、p_i-1(x₅,y₅)和目标关节点p’_i+1(x₃,y₃)，利用如下方程式计算下一时刻关节点p’_i(x₄,y₄)，

其中，θ表示相邻的关节点之间的角度约束，l_e表示相邻关节点p_i+1(x₁,y₁)、p_i(x₂,y₂)之间的连杆的长度。

进一步，利用圆周边界法判断所述位置点是否满足相邻的关节点之间的角度约束θ包括以下步骤：

步骤Ⅰ、利用如上文所述的方法计算下一时刻关节点p’_i-1(x₅,y₅)；

步骤Ⅱ、利用如下方程式，判断关节点p’_i-1(x₅,y₅)是否满足相邻的关节点之间的角度约束θ，若是，保留步骤Ⅰ计算得到的下一时刻关节点p’_i-1(x₅,y₅)，否则，执行步骤Ⅲ；

步骤Ⅲ、以关节点P’_i(x₄,y₄)为圆心，关节点p’_i(x₄,y₄)、p’_i+1(x₃,y₃)之间连杆的长度为半径作圆，以所述连杆关于圆心对称的另一个半径为中心线，左右向外各延伸角度θ所在的半径与圆周的交点记为p”_i-1(m₁,n₁)、p”'_i-1(m₂,n₂)，分别计算交点p”_i-1(m₁,n₁)、p”'_i-1(m₂,n₂)与p'_i-1(x₅,y₅)之间距离，用距离小的交点替代步骤Ⅰ计算得到的下一时刻关节点p'_i-1(x₅,y₅)。

进一步，所述机械臂包括基座和多个等长的连杆，多个所述连杆之间通过万向节相连，所述基座能够沿单一方向移动，也通过万向节与连杆相连。

一种基于上文所述的用于移动型蛇形机械臂的平面轨迹跟踪方法的控制系统，其特征在于：包括处理器，所述处理器与路径规划模块、关节更新模块、关节约束处理模块和基座进给约束处理模块相连，

所述路径规划模块用于对机械臂末端关节点的初始位置点与目标点之间的路径规划，获得多个子目标点，

所述关节更新模块用于对多个所述子目标点，从机械臂的末端关节点至始端关节点，再从始端关节点到末端关节点依次迭代计算获取各个关节点所需移动到的位置点，

所述关节约束处理模块用于根据圆周边界法，判断关节更新模块计算获得的关节点对应的位置点是否满足相邻关节点之间的角度约束，若是，保留关节更新模块计算获得的关节点对应的位置点，否则，用圆周边界法计算获得的边界位置点替代保留关节更新模块计算获得的关节点对应的位置点，

所述基座进给约束处理模块用于将关节更新模块获取的始端关节点对应的位置点在基座的移动方向进行投影，获取基座所需移动的位置点。

本发明有益的技术效果在于：

通过在目标点和机械臂的末端关节点的初始位置点之间设置多个子目标点，将末端关节点由远至近从第一个子目标点向最后一个子目标点，再向目标点移动，对每个子目标点，均采用从末端关节点至始端关节点，再从始端关节点至末端关节点，利用纯几何解析法依次迭代计算更新后的各个关节点的位置，对运动轨迹进行过渡规划，并防止单个步长内基座移动距离过大，实现运动轨迹的平滑跟踪，以及对基座进给和机械臂运动的精确控制，从而缩小蛇形机械臂的位形偏移量，节省了运动能量，同时在计算的过程中，考虑了关节点之间的角度约束、与连杆之间的移动约束以及基座进结的限制，采用圆周边界法确定满足关节点之间的角度约束的更新位置，综合考虑了机械臂运动过程中的多种实际情况，最大限度地利用了机械臂的能力，使得对其的控制更加符合实际情况。

附图说明

图1是本发明的蛇形机械臂的结构示意图；

图2是本发明的总体流程示意图；

图3是本发明的蛇形机械臂的各个参数定义示意图；

图4是本发明的在移动约束下，蛇形机械臂的关节点位置更新示意图；

图5是本发明的超过角度约束，蛇形机械臂的关节点位置更新示意图；

图6是本发明的基座移动后，蛇形机械臂的关节点位置更新示意图；

图7是本发明的实施例中的蛇形机械臂可达运动空间示意图；

图8是采用本发明的方法进行平面轨迹跟踪的结果示意图，其中，标识(a)表示无角度约束，标识(b)表示无角度约束；

图9是采用本发明的方法进行平面轨迹跟踪，无角度约束的蛇形机械臂的关节角度变化曲面示意图；

图10是采用本发明的方法进行平面轨迹跟踪，有角度约束的蛇形机械臂的关节角度变化曲面示意图；

其中，1-基座，2-连杆，3-万向节。

具体实施方式

下面结合附图及较佳实施例详细说明本发明的具体实施方式。

本发明所研究的蛇形机械臂包括基座1和多个等长的连杆2，如图1所示，相邻的连杆2之间通过万向节3连接，基座1也通过万向节3和与其最近的连杆2即始端的连杆2连接，与末端的连杆2相连的万向3节还与执行器相连，同时，基座1本身可以装在可移动的平台或者直线导轨上，也可以与伸缩机构相连，实现基座1在水平方向和竖直方向上的移动，而以万向节3作为关节点可以实现连杆之间的多个角度和方向的调整，从而实现对执行器的多姿态调整。

如图2所示，本发明提供了一种用于移动型蛇形机械臂的平面轨迹跟踪方法，采用纯几何解析法，推导了移动蛇形机械臂的末端跟随运动，同时满足关节约束和基座进给约束，实现了运动过程中机械臂的各个关节点都在落在前一时刻的相邻关节点的连线上，缩小机械臂的位形偏移量，使机械臂的运动更协调，同时用纯几何解析法代替运动学逆解，使得机械臂的迭代步骤和推算方法更加明确，物理意义直观，并且求解速度快，可满足实时运动和避障的需求。主要包括以下步骤：

步骤一、在目标点和末端关节点的末端关节点的初始位置点之间设置多个子目标点，可从该末端关节点的初始位置点到目标点，将两者之间的距离等间隔划分多个子目标点，将末端关节点由远至近从第一个子目标点向最后一个子目标点，再向目标点移动，这样，通过对运动轨迹进行过渡规划，使得机械臂可以一步一步向目标点移动，并防止单个步长内基座移动距离过大，避免关节角度和基座位置达到约束饱和，可以更加精密地控制基座的移动，提高对基座的位置约束的管控，同时实现蛇形机械臂的运动轨迹的平滑跟踪和精确控制。该子目标点的间隔设定满足机械臂基座移动的位置约束，其大小主要由对机械臂的末端关节点监测的采样时间内基座移动的距离决定。

步骤二、将末端关节点移至第一个子目标点，从末端关节点至始端关节点依次迭代计算每个关节点所需移动到的位置点，主要是根据相邻关节点之间的角度约束、相邻关节点之间的连杆长度和相邻关节点之间的移动约束，利用纯几何解析法计算各个关节点所需移动到的位置点，从而缩小蛇形机械臂的位形偏移量，节省了运动能量，具体如下：

以万向节和与之连接的连杆作为组成蛇形机械臂的单位模块，定义p_i为第i个单位模块的万向节即关节点位置，l_i为第i个单位模块的连杆长度，θ为第i个单位模块和第i-1个单位模块之间的夹角，i＝1...N，在蛇形机械臂所在的平面上，以与基座相连的关节点为原心，建立直角坐标系，如图3所示，根据几何关系，可推导各个关节点位置关系为：其中p_i-1(x_i-1,y_i-1)，p_i(x_i,y_i)分别为第i-1个单位模块的前后两个关节点的位置坐标，θ_i为连杆i和连杆i-1之间的夹角，通过上述方程即可迭代计算求出末端关节点对应的执行器的平面位置。

为了保证每一步迭代计算更新过程中，蛇形机械臂的相对关节移动小，规定移动约束如下：如图4所示，当关节点p_i+1(x₁,y₁)移动到p'_i+1(x₃,y₃)时，下一时刻相邻的关节点p'_i(x₄,y₄)要保证落在上一时刻的关节点p_i(x₂,y₂)和p_i+1(x₁,y₁)的连线上，同理关节点p'_i-1(x₅,y₅)要落在关节点p_i-1(x₆,y₆)、p_i(x₂,y₂)的连线上。

定义四个杆向量的方向为 已知上一时刻相邻三个关节点分别为p_i+1(x₁,y₁)、p_i(x₂,y₂)、p_i-1(x₅,y₅)和目标关节点p'_i+1(x₃,y₃)，利用如下方程式计算下一时刻关节点p'_i(x₄,y₄)，

其中，θ表示相邻的关节点之间的夹角即角度约束，l_e表示相邻关节点p_i+1(x₁,y₁)、p_i(x₂,y₂)之间的连杆的长度。

步骤三、根据此时的始端关节点对应的位置点计算得到机械臂的基座所需移动到的位置点，可通过将上述始端关节点对应的位置点在基座的移动方向上进行投影获得，然后，将基座移至其对应的位置点固定，判断此时的始端关节点与其相邻的关节点之间的角度是否满足角度约束，若是，将此时的末端关节点移至下一个子目标点；否则，将与始端关节点相邻的关节点移至角度约束范围内，从始端关节点至末端关节点重新依次迭代计算每个关节点所需移动到的位置点，将此时的末端关节点移至下一个子目标点，其迭代计算的方法与步骤二中所述的方法一致，而角度约束判断主要是利用圆周边界法判断所述位置点是否满足相邻的关节点之间的角度约束θ，若是，保留所述位置点，否则，用圆周边界法获得的边界位置点替代所述位置点，从而最大限度地利用了蛇形机械臂的能力，如图5所示，具体如下所示：

步骤Ⅰ、利用如步骤二所述的方法计算下一时刻关节点p'_i-1(x₅,y₅)；

步骤Ⅱ、利用如下方程式，判断关节点p'_i-1(x₅,y₅)是否满足相邻的关节点之间的角度约束θ，若是，保留步骤Ⅰ计算得到的下一时刻关节点p'_i-1(x₅,y₅)，否则，执行步骤Ⅲ；

步骤Ⅲ、以关节点p'_i(x₄,y₄)为圆心，关节点p'_i(x₄,y₄)、p'_i+1(x₃,y₃)之间连杆的长度为半径作圆，以所述连杆关于圆心对称的另一个半径为中心线，左右向外各延伸角度θ所在的半径与圆周的交点记为p”_i-1(m₁,n₁)、p”'_i-1(m₂,n₂)，分别计算交点p”_i-1(m₁,n₁)、p”'_i-1(m₂,n₂)与p'_i-1(x₅,y₅)之间距离，用距离小的交点替代步骤Ⅰ计算得到的下一时刻关节点p'_i-1(x₅,y₅)。

不管是从末端关节点到始端关节点迭代计算还是从始端关节点至末端关节点迭代计算获得的各个关节点的位置点，都需要利用上述方法进行角度约束判断，由于角度约束判断至少需要相邻的三个关节点，因此，与始端关节点和末端关节点最近的关节点无法进行角度约束判断，其余的关节点都需要。

由于基座仅能在水平方向或者竖直方向即单一方向移动，但是从末端关节点到始端关节点迭代计算获得的始端关节点，相对迭代计算之前的始端关节点不一定仅是水平方向或者竖直方向的改变，而始端关节点与基座直接相连，即基座的运动与始端关节点的移动一致，因此，基座所需移动的位置点可能会使迭代计算得到的始端关节点发生改变，如果计算得到的始端关节点相对于计算前不在水平方向或者竖直方向，则需要将其在基座的移动方向进行投影，将其拉回至水平方向或者竖直方向上，如图6所示，所以，需要对基座移动后，始端关节点与其相邻的两个关节点进行角度约束判断，若满足角度约束，无需再进行从从始端关节点至末端关节点迭代计算，否则，在将相邻关节点用圆周边界法获得的边界位置点替代后，还需要进行从从始端关节点至末端关节点迭代计算。

步骤四、重复步骤二至三，完成末端关节点到所有子目标点和目标点的移动；

步骤五、判断此时的末端关节点是否到达目标点，若是，根据此时获得的各个关节点的位置点坐标，控制末端关节点移动到达目标点，否则，重复步骤二至三，直至末端关节点到达目标点。

判断目标点p_t到达的收敛准则可设置为末端关节点p'_N与目标点p_t之间的误差在一定的范围内，即||p_t-p'_N||≤ε，若满足，机械臂的末端关节点向下一目标点移动。

本发明还提供了一种基于上文所述的用于移动型蛇形机械臂的平面轨迹跟踪方法的控制系统，包括处理器，该处理器与路径规划模块、关节更新模块、关节约束处理模块和基座进给约束处理模块相连，该路径规划模块用于对机械臂末端关节点的初始位置点与目标点之间的路径规划，获得多个子目标点，该关节更新模块用于对多个所述子目标点，从机械臂的末端关节点至始端关节点，再从始端关节点到末端关节点依次迭代计算获取各个关节点所需移动到的位置点，该关节约束处理模块用于根据圆周边界法，判断关节更新模块计算获得的关节点对应的位置点是否满足相邻关节点之间的角度约束，若是，保留关节更新模块计算获得的关节点对应的位置点，否则，用圆周边界法计算获得的边界位置点替代保留关节更新模块计算获得的关节点对应的位置点，该基座进给约束处理模块用于将关节更新模块获取的始端关节点对应的位置点在基座的移动方向进行投影，获取基座所需移动的位置点。

以基座在竖直方向的最大进给距离为60cm，基座每个步长的移动距离为3mm，蛇形机械臂的各个连杆共15节，由14个万向节及关节点相连，相邻两个关节点的最大相对转角为30，单位模块由连杆和万向节组成，长度为5cm，直径为3cm，则蛇形机械臂的总长为75cm为例，根据图2所示的定义，可以计算出末端关节点连接的执行器的可达运动区域，约为100×40²cm的平面区域，如图7所示。

按照上文所述的方法，通过仿真数据输出值做相应的对比，验证结果的可行性。从图8(a)可以看出在相邻关节点之间无角度约束的时候，末端关节点跟踪给定的轨迹，其余关节点的运动趋于在圆形轨迹内侧，从末端到始端各个关节点的波动范围逐渐减小，这表明在连杆上的移动约束能够实现最小能耗的运动，实现快速运动。当考虑角度约束时候，从图8(b)可以看出，机械臂的轨迹会移出圆形轨迹之外，但保证了关节点之间的角度约束，符合实际物理系统的运动情况。

图9、图10分别给出了有无关节点之间的角度约束限制情况下关节点位置即角度变化仿真结果。图9为在无角度约束的情况下蛇形机械臂的各个关节的角度，可以看出有部分关节角超过了角度约束30°。图10为有角度约束情况下蛇形机械臂的关节角度，可以看出，关节点对应的关节角度始终能限制在运动范围之内，在100采样点出，末端关节点首先达到极限值，其余关节点随后依次达到极限。在不考虑角度约束时，蛇形机械臂的关节更新情况依次落在连杆的连线上，运动平稳性提高，末端关节角度明显大于始端关节角度，关节的运动量从末端到始端逐渐减小，蛇形机械臂的位形偏移量减小，使得蛇形机器臂的运动更协调。

通过采集仿真实验数据，验证了本发明的方法能有效的实现目标轨迹的实时跟踪，具有如下特点：1)依据在杆上的原则，利用纯几何解析法确定关节点更新位置，从而缩小蛇形机械臂的位形偏移量，节省了运动能量；2)采用圆周边界法确定满足关节点之间的角度约束的更新位置，最大限度地利用了机械臂的能力；3)采用投影法和运动规划方法满足基座直线移动位置约束。

虽然以上描述了本发明的具体实施方式，但是本领域的技术人员应当理解，这些仅是举例说明，在不背离本发明的和实质的前提下，可以对这些实施方式做出多种变更或修改，因此，本发明的保护范围由所附权利要求书限定。

Claims (8)

1.一种用于移动型蛇形机械臂的平面轨迹跟踪方法，其特征在于包括以下步骤：

步骤一、在目标点和机械臂的末端关节点的初始位置点之间设置多个子目标点；

步骤二、将末端关节点移至第一个子目标点，从末端关节点至始端关节点依次迭代计算每个关节点所需移动到的位置点；

步骤三、根据此时的始端关节点对应的位置点计算得到机械臂的基座所需移动到的位置点，将基座移至其对应的位置点固定，判断此时的始端关节点与其相邻的关节点之间的角度是否满足角度约束，若是，将此时的末端关节点移至下一个子目标点；否则，将与始端关节点相邻的关节点移至角度约束范围内，从始端关节点至末端关节点重新依次迭代计算每个关节点所需移动到的位置点，将此时的末端关节点移至下一个子目标点；

步骤四、重复步骤二至三，完成末端关节点到所有子目标点和目标点的移动；

步骤五、判断此时的末端关节点是否到达目标点，若是，根据此时获得的各个关节点的位置点坐标，控制末端关节点移动到达目标点，否则，重复步骤二至三，直至末端关节点到达目标点。

2.根据权利要求1所述的用于移动型蛇形机械臂的平面轨迹跟踪方法，其特征在于：所述基座所需移动到的位置点通过将始端关节点对应的位置点在基座的移动方向上进行投影获得。

3.根据权利要求1所述的用于移动型蛇形机械臂的平面轨迹跟踪方法，其特征在于：从所述末端关节点的初始位置点到目标点，将两者之间的距离等间隔划分多个子目标点，将所述末端关节点由远至近从第一个子目标点向最后一个子目标点，再向目标点移动。

4.根据权利要求1所述的用于移动型蛇形机械臂的平面轨迹跟踪方法，其特征在于：根据相邻关节点之间的角度约束、相邻关节点之间的连杆长度和相邻关节点之间的移动约束，利用纯几何解析法计算所述步骤二和三中各个关节点所需移动到的位置点，再利用圆周边界法判断所述位置点是否满足相邻的关节点之间的角度约束θ，若是，保留所述位置点，否则，用圆周边界法获得的边界位置点替代所述位置点。

5.根据权利要求4所述的用于移动型蛇形机械臂的平面轨迹跟踪方法，其特征在于利用纯几何解析法计算所述步骤二和三中各个关节点所需移动到的位置点包括：

记相邻三个关节点分别为p_i+1(x₁,y₁)、p_i(x₂,y₂)、p_i-1(x₆,y₆)，当关节点p_i+1(x₁,y₁)移动到p′_i+1(x₃,y₃)时，下一时刻相邻的关节点p′_i(x₄,y₄)要保证落在上一时刻的关节点p_i(x₂,y₂)和p_i+1(x₁,y₁)的连线上，同理关节点p′_i-1(x₅,y₅)要落在关节点p_i-1(x₆,y₆)、p_i(x₂,y₂)的连线上，三个杆向量的方向定义为

已知上一时刻相邻三个关节点分别为p_i+1(x₁,y₁)、p_i(x₂,y₂)、p_i-1(x₅,y₅)和目标关节点p′_i+1(x₃,y₃)，利用如下方程式计算下一时刻关节点p′_i(x₄,y₄)，

其中，θ表示相邻的关节点之间的角度约束，l_e表示相邻关节点p_i+1(x₁,y₁)、p_i(x₂,y₂)之间的连杆的长度。

6.根据权利要求5所述的用于移动型蛇形机械臂的平面轨迹跟踪方法，其特征在于利用圆周边界法判断所述位置点是否满足相邻的关节点之间的角度约束θ包括以下步骤：

步骤Ⅰ、利用如权利要求5所述的方法计算下一时刻关节点p′_i-1(x₅,y₅)；

步骤Ⅱ、利用如下方程式，判断关节点p′_i-1(x₅,y₅)是否满足相邻的关节点之间的角度约束θ，若是，保留步骤Ⅰ计算得到的下一时刻关节点p′_i-1(x₅,y₅)，否则，执行步骤Ⅲ；

步骤Ⅲ、以关节点p′_i(x₄,y₄)为圆心，关节点p′_i(x₄,y₄)、p′_i+1(x₃,y₃)之间连杆的长度为半径作圆，以所述连杆关于圆心对称的另一个半径为中心线，左右向外各延伸角度θ所在的半径与圆周的交点记为p″_i-1(m₁,n₁)、p″′_i-1(m₂,n₂)，分别计算交点p”_i-1(m₁,n₁)、p”'_i-1(m₂,n₂)与p'_i-1(x₅,y₅)之间距离，用距离小的交点替代步骤Ⅰ计算得到的下一时刻关节点p'_i-1(x₅,y₅)。

7.根据权利要求1所述的用于移动型蛇形机械臂的平面轨迹跟踪方法，其特征在于：所述机械臂包括基座和多个等长的连杆，多个所述连杆之间通过万向节相连，所述基座能够沿单一方向移动，也通过万向节与连杆相连。

8.一种基于权利要求1所述的用于移动型蛇形机械臂的平面轨迹跟踪方法的控制系统，其特征在于：包括处理器，所述处理器与路径规划模块、关节更新模块、关节约束处理模块和基座进给约束处理模块相连，

所述路径规划模块用于对机械臂末端关节点的初始位置点与目标点之间的路径规划，获得多个子目标点，

所述关节更新模块用于对多个所述子目标点，从机械臂的末端关节点至始端关节点，再从始端关节点到末端关节点依次迭代计算获取各个关节点所需移动到的位置点，

所述关节约束处理模块用于根据圆周边界法，判断关节更新模块计算获得的关节点对应的位置点是否满足相邻关节点之间的角度约束，若是，保留关节更新模块计算获得的关节点对应的位置点，否则，用圆周边界法计算获得的边界位置点替代保留关节更新模块计算获得的关节点对应的位置点，

所述基座进给约束处理模块用于将关节更新模块获取的始端关节点对应的位置点在基座的移动方向进行投影，获取基座所需移动的位置点。