CN103692440B - 一种连续型机器人空间路径跟踪方法 - Google Patents

Abstract

一种连续型机器人空间路径跟踪方法。其包括初始化、判断路径起点是否在原点上方、蛇臂上升至路径起点、蛇臂直接弯曲、蛇臂前进一步、求解蛇臂末节关节段变量、更新蛇臂姿态、判断路径跟踪是否等阶段。本发明提供的连续型机器人空间路径跟踪方法能够实现对空间任意给定路径的跟踪；采用这种方法能够实现在整个跟踪过程中以路径作为参考，机器人整体形态始终与路径一致，行进覆盖的范围较小，安全性高。

Description

一种连续型机器人空间路径跟踪方法

技术领域

本发明属于自动控制技术领域，特别是涉及一种连续型机器人空间路径跟踪方法。

背景技术

传统的离散型机器人(如串、并联机器人)均采用刚性关节和连杆结构，用于实现在自由空间内的多自由度运动，但由于其一般只具有5～7个自由度，自由度数目有限，因此对工作空间受限的环境适应性不强；与离散型机器人不同，连续型机器人为“无脊椎”的柔性结构，机器人采用形状可以灵活改变的结构，而不具有任何刚性的关节和连杆；这种新型的仿生机器人具有良好的弯曲性能，可以柔顺而灵活地改变自身的形状，其优良的弯曲特性甚至可以和蛇体、象鼻子以及章鱼触角等生物器官媲美；由于连续型机器人的外形可以灵活改变，因此具有根据环境障碍物的状况而改变自身形状的能力，对工作空间受限的环境具有独特的适应能力；其应用前景广阔，可以应用于飞机油箱检查、多障碍物工业环境内的作业、弯曲管道和塌陷建筑物内的侦查和搜救、核电站内部管路的维护、人体疾病的诊疗等场合。

连续型机器人为由多个关节段构成的串联机器人，其关节段具有结构上的约束，能进行空间的弯曲和旋转运动；在空间结构约束很强的环境中，为避免机器人触碰造成损伤或潜在危险，需要对机器人到达目标区域的行进路径进行规划；路径为由若干个类似连续型关节的曲线构成，满足其结构约束，是连续型机器人到达目标区域的最终姿态。

在这种技术背景下，目前仍缺少简洁有效的能够对连续型机器人空间路径进行跟踪的方法。

发明内容

为了解决上述问题，本发明的目的在于提供一种连续型机器人空间路径跟踪方法，实现机器人跟踪规划的三维空间路径，最终到达目标区域，并且尽量减小行进的覆盖范围。

为了达到上述目的，本发明提供的连续型机器人空间路径跟踪方法包括按顺序执行的下列步骤：

步骤一、初始化的S01阶段：进行蛇臂位置和姿态初始化，使蛇臂处于垂直状态,其末端点到达系统坐标系{S}原点O_S；

步骤二、判断路径起点是否在原点上方的S02阶段：判断路径起点是否处在系统坐标系原点O_S的上方，若判断结果为“是”，则进入下一步S03阶段，否则下一步进入S04阶段；

步骤三、蛇臂上升至路径起点的S03阶段：在路径起点处于系统坐标系{S}原点O_S上方时，则末段蛇臂直线前进至路径起点，然后下一步进入S05阶段；

步骤四、蛇臂直接弯曲的S04阶段：路径起点若处于系统坐标系{S}原点O_S下方，则按照S06阶段方法求解关节段变量，并控制末段关节段进行弯曲，然后下一步进入S05阶段；

步骤五、蛇臂前进一步的S05阶段；蛇臂基座前进一个步长；

步骤六、求解蛇臂末节关节段变量的S06阶段：采用基于已知点的搜索算法计算末节关节段变量，并记录；

步骤七、更新蛇臂姿态的S07阶段：蛇臂各个关节段按照关节段变量进行更新姿态，蛇臂末节采用S06阶段求得的关节段变量，其它各关节依次采用蛇臂末节关节段经过此位置时记录的关节段变量；

步骤八、判断路径跟踪是否完成的S08阶段：判断蛇臂末端是否到达路径末端，如果判断结果为“是”，则整个流程至此结束，否则下一步重新进入S05阶段，继续跟踪过程。

在S06阶段中，所述的计算关节段变量的方法为基于已知点的搜索算法求解关节段变量，其包括按顺序执行的下列步骤：

步骤一、将路径各段均分，求出分点在路径第一关节段坐标系下坐标的S601阶段：

步骤(a):设关节段长度为L，基座步进的步长为s，则蛇臂完成跟踪一个路径段时基座步进次数w为：

$w=\frac{L}{s}$

步骤(b):将路径各段均分为w份，在各路径段坐标系中求解出分点坐标；

步骤(c):求解路径分点在路径第一关节段坐标系下的坐标，即将求解出分点坐标，再经过齐次变换转化到路径第一关节段坐标系下坐标；

步骤二、求解蛇臂末端关节段旋转角度的S602阶段：

每次基座步进后，以对应的路径分点为蛇臂末端点要达到的目标点，此点与蛇臂末端关节段构成一个平面，由此确定旋转角度

步骤三、求解蛇臂末端关节的弯曲角度的S603阶段：

采用数值算法求解弯曲角度按照规定步长遍历弯曲角度，每次求出蛇臂末端点坐标，并求解末端点到路径的距离，选取对应最小距离的弯曲角度。

在S07阶段中，在所述的更新姿态时，记录每次跟踪时蛇臂末端关节段的关节段变量，其与路径位置对应，当其余关节段末端到达此位置时，关节段变量相同，直接载入数据即可，不需重复求解，相当于其它关节段跟随末端关节段的轨迹前进。

在S601阶段的步骤(c)中，所述的求解路径分点在路径第一关节段坐标系下的坐标的方法如下：

路径关节段的坐标系为{B_g}(g＝0,1,2,…,n)，路径中第m段的关节段变量为设路径第m段第k个分点为C_m,k,则该点对应的弯曲角度为可表示为：

${\mathrm{\θ}}_{C_{m,k}}=\frac{k}{w}{\mathrm{\θ}}_{p_m}$

在路径段坐标系{B_m-1}中，该分点对应的坐标为：

可得坐标系{B_m-1}到坐标系{B₀}的齐次变换矩阵为将分点C_m,k坐标转换到路径坐标系{B₀}中，转换关系为：

在S602阶段中，所述的求解蛇臂末端关节段旋转角度的方法如下：

蛇臂第N段起始端处坐标系为{A_N-1}，在此坐标系中，以路径分点C_m,k为目标点，能够确定一个过C_m,k点和轴的平面；蛇臂关节段N在此平面内，平面与轴的夹角即为旋转角度若已知C_m,k在坐标系{A_N-1}中坐标，则容易求解,设坐标为

路径坐标系{B₀}到蛇臂关节段坐标系{A_N-1}的齐次变换矩阵为：

故有：

由上式可求出由几何关系易得：

由于旋转角度根据投影点所在象限不同，b取值如表1所示：

表1 b取值情况

在S603阶段中，所述的求解蛇臂末端关节的弯曲角度的方法如下：

在平面内采用数值算法求解末端关节的弯曲角度；具体步骤如下：

(Ⅰ)求蛇臂末端点坐标

弯曲角度取值范围由于计算过程中处于分母上，故当时取较小值代替；设该角度步进的步长为△θ，则第i次步进后蛇臂末端关节的弯曲角度为：

${\mathrm{\θ}}_{R_{\mathrm{Ni}}}=\left\{\begin{array}{cc}{10}^{-5},i=0& \\ i\·\mathrm{\Δ\θ},i,i=\mathrm{1,2,3}\·\·\·& \end{array}\right.$

末端点Q_i坐标为可用下式表示；

(Ⅱ)计算蛇臂末端点到路径的距离

跟踪路径第m段时，求出弯曲角度第i次步进后蛇臂末端点Q_i到路径第m段所有分点的距离d_ik(k＝1,2,3,…,w)，取最小值作为近似的距离值d_i；

d_i＝min d_ik k＝1,2,3,…,w

选取距离值d_i(i＝0,1,2,3,…,1000)中最小值，设第t个值为最小值，则弯曲角度为：

${\mathrm{\θ}}_{R_N}=\left\{\begin{array}{cc}{10}^{-5},t=0& \\ t\·\mathrm{\Δ\θ},t,t=\mathrm{1,2,3}\·\·\·& \end{array}\right..$

本发明提供的连续型机器人空间路径跟踪方法能够实现对空间任意给定路径的跟踪；采用这种方法能够实现在整个跟踪过程中以路径作为参考，机器人整体形态始终与路径一致，行进覆盖的范围较小，安全性高。

本发明提供的连续型机器人空间路径跟踪方法，跟踪路径时采用末端点逼近的方法，使蛇臂各关节段的末端点逼近路径；本发明提出基于已知点的搜索算法进行路径跟踪：将路径均分，由分点确定关节段的旋转平面；在该平面中，采用距离最短策略搜索求解关节段的弯曲角度；多关节段跟踪时，仅求解末端关节段的关节变量，其余关节段重复末端关节段运动过的轨迹。

本发明的优点：

(1)时间复杂度低

进行路径跟踪时，仅需求解末端关节段的关节变量，其余关节段重复末端关节段运动过的轨迹，降低了求解的空间复杂度；因此，该方法时间复杂度低，跟踪时具有较好的实时性。

(2)空间包络小

该方法能使连续型机器人的蛇臂在行进过程中始终在路径附近，跟踪过程中覆盖的空间区域较小，为在空间狭小环境中进行和工作提供可能性。

(3)控制简单，易于编程实现

本发明提出的基于已知点的搜索算法是一种数值方法，无需设计复杂的控制器，控制收敛且性能稳定；因此，该方法具有控制简单，并易于编程实现的特点。

附图说明

图1：连续型机器人结构示意图。

图2：坐标系示意图。

图3：单关节段运动学模型示意图。

图4：四关节段弯曲模型示意图。

图5：为本发明提供的连续型机器人空间路径跟踪方法的流程图。

图6：为本发明提供的连续型机器人空间路径跟踪方法的路径跟踪过程示意图。

图7：为本发明提供的连续型机器人空间路径跟踪方法的求解路径分点坐标示意图。

图8：为本发明提供的连续型机器人空间路径跟踪方法的求解蛇臂末端关节的旋转角度示意图。

图9：为本发明提供的连续型机器人跟踪三关节段共面路径的仿真实验。

图10：为本发明提供的连续型机器人跟踪三关节段异面路径的仿真实验。

具体实施方式

下面结合附图和具体实施例对本发明提供的连续型机器人空间路径跟踪方法进行详细说明。

1技术方案

1.1连续型机器人本体结构分析

连续型机器人是一种柔顺、灵活性高的新型仿生机器人，一般由柔性机构1、伸缩机构2、控制系统3和移动平台4组成，如图1所示；由于柔性机构1类似蜿蜒的蛇，这里称为蛇臂；蛇臂通常为多个结构单元组成的串联机构，每个结构单元称作关节段，具有旋转和弯曲两个自由度；伸缩机构2具有一个自由度，可实现一维的精确运动，伸缩机构2上的升降平台称为基座，其为蛇臂行进提供基础；控制系统3为控制的核心，实现对蛇臂的运动路径规划、路径跟踪控制和检测信号处理及远传。

1.2连续型机器人运动学分析

1.2.1定义坐标系

在被检环境的入口中心处建立系统坐标系{S}(以入口朝上为例)，设机器人蛇臂含有N个关节段，规划的路径含有n个关节段；初始位置时，机器人蛇臂末端到达系统坐标系{S}原点O_S，此时在伸缩机构始端建立坐标系{A_b}，依次在蛇臂每个关节段的起始端处建立坐标系{A_f}(f＝0,1,2,…,N)，同理建立路径关节段坐标系{B_g}(g＝0,1,2,…,n)，如图2所示；

1.2.2单关节段运动学分析

连续型机器人蛇臂由多个关节段组成，每个关节段均具有相同的特性，现以蛇臂第一关节段为例进行单关节段运动学分析，如图3所示；坐标系的Z轴分别垂直于该坐标系所在的平面，指向单关节段的切线方向；机器人单关节段的运动可以分解为自身的弯曲运动和以坐标系{A₀}的Z_A0轴为旋转轴的旋转运动，分别称为弯曲自由度和旋转自由度；单关节段弯曲曲线为等曲率连续光滑曲线，关节段弯曲角度为弯曲圆弧对应的圆心角(右下标R₁表示机器人蛇臂第一关节段)，取值范围为[0,π]，关节段旋转角度为平面OO_A0O_A1与X_A0O_A0Z_A0的夹角取值范围为[0,2π)；单关节段的姿态由和决定，故称其为关节段变量。

设单关节段长度为L，在坐标系{A₀}中O_A1点坐标为可得公式(1)。

单关节段坐标系{A₀}与{A₁}间的转换可用齐次变换矩阵T进行表示，具体实现为：坐标系{A₀}到{A₁}的平移、绕Z_A0轴旋转角度、绕Y_A0轴旋转角度和绕Z_A0轴旋转角度，如式(2)所示；

式中，

1.2.3多关节段运动学分析

连续型机器人的蛇臂为由多个关节段串联组成，规定离基座最近的关节段为关节段一，其后依次为关节段二、关节段三等；坐标系{A_f}的轴指向蛇臂中心轴线的延伸方向，由于每个关节段的末端与下一关节段的起始端通过连接件连接，可认为两个坐标系重合，图4所示为四个关节段弯曲模型(关节段Joint Segment,简称JS)。

设蛇臂第h个关节段的弯曲角度和旋转角度分别为和(右下标R表示机器人蛇臂,h∈[1,N])。坐标系{A₀}到坐标系{A_f}的齐次变换矩阵为：

设某点在坐标系{A_f}下坐标为^AfP，则该点在坐标系{A0}下坐标^A0P为：

在坐标系{A_b}中坐标^AbP为：

在系统坐标系{S}中坐标^SP为：

以上式中，f＝0,1,2…,N。f＝0时，为单位阵。

1.3连续型机器人路径跟踪控制方法

1.3.1连续型机器人作业的总体思路

连续型机器人作业的具体过程如下：

(1)根据欲到达的目标点，进行路径规划；路径是基于机器人的运动特性规划的，由整数个关节段组成，是跟踪完成后机器人的最终形态；

(2)控制机器人跟踪规划的路径，最终使终端到达目标点，机器人各关节段与路径完全重合；

(3)终端设备进行作业。

其中，(1)中路径规划结果为蛇臂各关节段的变量值(右下标中的P表示路径)和蛇臂基座上升距离dis；跟踪路径时，基座以恒定步长步进，需实时求出各关节段的关节段变量值。

路径跟踪的关键为保证机器人行进轨迹偏离规划路径尽量小，本发明提供的方法为基于末端点逼近的跟随跟踪方法；该方法的基本思想：蛇臂对路径跟踪时，各关节段自由度一起运动，即蛇臂基座随伸缩机构进行运动，而蛇臂各段改变弯曲角度和旋转角度，使各关节段末端点均逼近路径；蛇臂共有N个关节，第N关节段首先跟踪路径第1段，基座每前进一个步长，第N关节段经过旋转和弯曲后保证其末端点离路径最近；完成第一段路径跟踪后，第N关节段跟踪路径第2段，此时第N-1关节段依照之前第N关节段的关节段变量跟随跟踪路径第1段，实现跟踪路径。

如图5、图6所示，本发明提供的连续型机器人空间路径跟踪方法包括按顺序执行的下列步骤：

步骤一、初始化的S01阶段：进行蛇臂位置和姿态初始化，使蛇臂处于垂直状态,其末端点到达系统坐标系{S}原点O_S；

步骤二、判断路径起点是否在原点上方的S02阶段：判断路径起点是否处在系统坐标系原点O_S的上方，若判断结果为“是”，则进入下一步S03阶段，否则下一步进入S04阶段；

步骤三、蛇臂上升至路径起点的S03阶段：在路径起点处于系统坐标系{S}原点O_S上方(图6(a))时，则末段蛇臂直线前进至路径起点(图6(b))，然后下一步进入S05阶段；

步骤四、蛇臂直接弯曲的S04阶段：路径起点若处于系统坐标系{S}原点O_S下方(图6(d))，则按照S06阶段方法求解关节段变量，并控制末段关节段进行弯曲，然后下一步进入S05阶段；

步骤五、蛇臂前进一步的S05阶段；蛇臂基座前进一个步长；

步骤六、求解蛇臂末节关节段变量的S06阶段：采用基于已知点的搜索算法计算末节关节段变量，并记录；

步骤七、更新蛇臂姿态的S07阶段：蛇臂各个关节段按照关节段变量进行更新姿态，蛇臂末节采用S06阶段求得的关节段变量，其它各关节依次采用蛇臂末节关节段经过此位置时记录的关节段变量；

步骤八、判断路径跟踪是否完成的S08阶段：判断蛇臂末端是否到达路径末端，如果判断结果为“是”，则整个流程至此结束，否则下一步重新进入S05阶段，继续跟踪过程。

在S06阶段中，所述的计算关节段变量的方法，为基于已知点的搜索算法求解关节段变量，其包括按顺序执行的下列步骤：

步骤一、将路径各段均分，求出分点在路径第一关节段坐标系下坐标的S601阶段：

步骤(a)设关节段长度为L，基座步进的步长为s，则蛇臂完成跟踪一个路径段时基座步进次数w为：

$w=\frac{L}{s}---\left(7\right)$

步骤(b)将路径各段均分为w份，在各路径段坐标系中求解出分点坐标；

步骤(c)求解路径分点在路径第一关节段坐标系下的坐标，即将求解出分点坐标，再经过齐次变换转化到路径第一关节段坐标系下坐标；

步骤二、求解蛇臂末端关节段旋转角度的S602阶段：

每次基座步进后，以对应的路径分点为蛇臂末端点要达到的目标点，此点与蛇臂末端关节段构成一个平面，由此确定旋转角度

步骤三、求解蛇臂末端关节的弯曲角度的S603阶段：

采用数值算法求解弯曲角度按照一定步长遍历弯曲角度，每次求出蛇臂末端点坐标，并求解末端点到路径的距离，选取对应最小距离的弯曲角度。

在S07阶段中，在所述的更新姿态时，需要记录每次跟踪时蛇臂末端关节段的关节段变量，其与路径位置对应，当其余关节段末端到达此位置时，关节段变量相同，直接载入数据即可，不需重复求解，相当于其它关节段跟随末端关节段的轨迹前进；因此，该计算方法仅计算末段关节段的关节段变量，不随蛇臂关节段数量的增加而加大搜索计算量，具有计算复杂度低、操作简单的特点。

在S601阶段的步骤(c)中，所述的求解路径分点在路径第一关节段坐标系下的坐标的方法如下：

路径关节段的坐标系为{B_g}(g＝0,1,2,…,n)，路径中第m段的关节段变量为设路径第m段第k个分点为C_m,k,如图7所示,则该点对应的弯曲角度为可表示为：

${\mathrm{\θ}}_{C_{m,k}}=\frac{k}{w}{\mathrm{\θ}}_{p_m}---\left(8\right)$

在路径段坐标系{B_m-1}中，该分点对应的坐标为：

可得坐标系{B_m-1}到坐标系{B₀}的齐次变换矩阵为将分点C_m,k坐标转换到路径坐标系{B₀}中，转换关系为：

在S602阶段中，所述的求解蛇臂末端关节段旋转角度的方法如下：

蛇臂第N段起始端处坐标系为{A_N-1}，在此坐标系中，以路径分点C_m,k为目标点，可以确定一个过C_m,k点和轴的平面；蛇臂关节段N在此平面内，平面与轴的夹角即为旋转角度如图8所示；若已知C_m,k在坐标系{A_N-1}中坐标，则容易求解,设坐标为

路径坐标系{B₀}到蛇臂关节段坐标系{A_N-1}的齐次变换矩阵为：

故有：

由式(14)可求出由几何关系易得：

由于旋转角度根据投影点所在象限不同，b取值如表1所示。

表1 b取值情况

在S603阶段中，所述的求解蛇臂末端关节的弯曲角度的方法如下：

在平面内采用数值算法求解末端关节的弯曲角度；具体步骤如下：

(Ⅰ)求蛇臂末端点坐标

弯曲角度取值范围由于计算过程中处于分母上，故当时取较小值代替；设该角度步进的步长为△θ，则第i次步进后蛇臂末端关节的弯曲角度为：

${\mathrm{\θ}}_{R_{\mathrm{Ni}}}=\left\{\begin{array}{cc}{10}^{-5},i=0& \\ i\·\mathrm{\Δ\θ},i=\mathrm{1,2,3}\·\·\·& \end{array}\right.---\left(17\right)$

末端点Q_i坐标为可用式(18)表示。

(Ⅱ)计算蛇臂末端点到路径的距离

由于路径为空间的曲线，不易求得空间一点到曲线的距离，这里采用数值方法求解距离；跟踪路径第m段时，求出弯曲角度第i次步进后蛇臂末端点Q_i到路径第m段所有分点的距离d_ik(k＝1,2,3,…,w)，取最小值作为近似的距离值d_i。

d_i＝min d_ik k＝1,2,3,…,w   (20)

选取距离值d_i(i＝0,1,2,3,…,1000)中最小值，设第t个值为最小值，则弯曲角度为：

${\mathrm{\θ}}_{R_N}=\left\{\begin{array}{cc}{10}^{-5},t=0& \\ t\·\mathrm{\Δ\θ},t=\mathrm{1,2,3}\·\·\·& \end{array}\right.---\left(21\right)$

1.4连续型机器人路径跟踪实验

为验证本发明提出的连续型机器人空间路径跟踪方法，进行了如下实验：

(1)共面路径仿真实验

连续型机器人跟踪三关节段共面路径的仿真实验如图9所示，其中，图9(a)、(b)、(c)依次为跟踪路径第一、二、三段关节段，(d)为跟踪完成后与路径重合；跟踪过程中蛇臂末关节段的末端点始终在路径上，其他关节段重复其行为，可保证蛇臂沿着路径行进，最终达到与路径重合。

(2)异面路径仿真实验

连续型机器人跟踪三关节段异面路径的仿真实验如图10所示，其中，图10(a)、(b)、(c)依次为跟踪路径第一、二、三段关节段，(d)为跟踪完成后与路径重合，(e)为跟踪过程中蛇臂行经的空间包络。

(3)样机实验

连续型机器人跟踪三关节段共面路径的样机实验证明了提出方法的适用性。

以上实验可以证明本发明提出的连续型机器人路径跟踪算法能够实现对给定路径的跟踪，具有时间复杂度低、易于软硬件实现和跟踪包络小的特点。

Claims (6)

1.一种连续型机器人空间路径跟踪方法，其特征在于：其包括按顺序执行的下列步骤：

步骤一、初始化的S01阶段：进行蛇臂位置和姿态初始化，使蛇臂处于垂直状态,其末端点到达系统坐标系{S}原点O_S；

步骤二、判断路径起点是否在原点上方的S02阶段：判断路径起点是否处在系统坐标系原点O_S的上方，若判断结果为“是”，则进入下一步S03阶段，否则下一步进入S04阶段；

步骤三、蛇臂上升至路径起点的S03阶段：在路径起点处于系统坐标系{S}原点O_S上方时，则末段蛇臂直线前进至路径起点，然后下一步进入S05阶段；

步骤四、蛇臂直接弯曲的S04阶段：路径起点若处于系统坐标系{S}原点O_S下方，则按照S06阶段方法求解关节段变量，并控制末段关节段进行弯曲，然后下一步进入S05阶段；

步骤五、蛇臂前进一步的S05阶段；蛇臂基座前进一个步长；

步骤六、求解蛇臂末节关节段变量的S06阶段：采用基于已知点的搜索算法计算末节关节段变量，并记录；

步骤七、更新蛇臂姿态的S07阶段：蛇臂各个关节段按照关节段变量进行更新姿态，蛇臂末节采用S06阶段求得的关节段变量，其它各关节依次采用蛇臂末节关节段经过此位置时记录的关节段变量；

步骤八、判断路径跟踪是否完成的S08阶段：判断蛇臂末端是否到达路径末端，如果判断结果为“是”，则整个流程至此结束，否则下一步重新进入S05阶段，继续跟踪过程。

2.根据权利要求1所述的连续型机器人空间路径跟踪方法，其特征在于：在S06阶段中，所述的计算关节段变量的方法为基于已知点的搜索算法求解关节段变量，其包括按顺序执行的下列步骤：

步骤一、将路径各段均分，求出分点在路径第一关节段坐标系下坐标的S601阶段：

步骤(a):设关节段长度为L，基座步进的步长为s，则蛇臂完成跟踪一个路径段时基座步进次数w为：

$w=\frac{L}{s}$

步骤(b):将路径各段均分为w份，在各路径段坐标系中求解出分点坐标；

步骤(c):求解路径分点在路径第一关节段坐标系下的坐标，即将求解出分点坐标，再经过齐次变换转化到路径第一关节段坐标系下坐标；

步骤二、求解蛇臂末端关节段旋转角度的S602阶段：

每次基座步进后，以对应的路径分点为蛇臂末端点要达到的目标点，此点与蛇臂末端关节段构成一个平面，由此确定旋转角度

步骤三、求解蛇臂末端关节的弯曲角度的S603阶段：

采用数值算法求解弯曲角度按照规定步长遍历弯曲角度，每次求出蛇臂末端点坐标，并求解末端点到路径的距离，选取对应最小距离的弯曲角度。

3.根据权利要求1所述的连续型机器人空间路径跟踪方法，其特征在于：在S07阶段中，在所述的更新姿态时，记录每次跟踪时蛇臂末端关节段的关节段变量，其与路径位置对应，当其余关节段末端到达此位置时，关节段变量相同，直接载入数据即可，不需重复求解，相当于其它关节段跟随末端关节段的轨迹前进。

4.根据权利要求2所述的连续型机器人空间路径跟踪方法，其特征在于：在S601阶段的步骤(c)中，所述的求解路径分点在路径第一关节段坐标系下的坐标的方法如下：

路径关节段的坐标系为{B_g}(g＝0,1,2,…,n)，路径中第m段的关节段变量为(m＝1,2,3,…,n)；设路径第m段第k个分点为C_m,k,则该点对应的弯曲角度为可表示为：

${\mathrm{\θ}}_{C_{m,k}}=\frac{k}{w}{\mathrm{\θ}}_{p_m}$

在路径段坐标系{B_m-1}中，该分点对应的坐标为：

可得坐标系{B_m-1}到坐标系{B₀}的齐次变换矩阵为将分点C_m,k坐标转换到路径坐标系{B₀}中，转换关系为：

5.根据权利要求4所述的连续型机器人空间路径跟踪方法，其特征在于：在S602阶段中，所述的求解蛇臂末端关节段旋转角度的方法如下：

蛇臂第N段起始端处坐标系为{A_N-1}，在此坐标系中，以路径分点C_m,k为目标点，能够确定一个过C_m,k点和轴的平面；蛇臂关节段N在此平面内，平面与轴的夹角即为旋转角度若已知C_m,k在坐标系{A_N-1}中坐标，则容易求解,设坐标为

路径坐标系{B₀}到蛇臂关节段坐标系{A_N-1}的齐次变换矩阵为：

故有：

由上式可求出由几何关系易得：

由于旋转角度根据投影点所在象限不同，b取值如表1所示：

表1 b取值情况

6.根据权利要求5所述的连续型机器人空间路径跟踪方法，其特征在于：在S603阶段中，所述的求解蛇臂末端关节的弯曲角度的方法如下：

在平面内采用数值算法求解末端关节的弯曲角度；具体步骤如下：

(Ⅰ)求蛇臂末端点坐标

弯曲角度取值范围由于计算过程中处于分母上，故当时取较小值代替；设该角度步进的步长为△θ，则第i次步进后蛇臂末端关节的弯曲角度为：

${\mathrm{\θ}}_{R_{\mathrm{Ni}}}=\left\{\begin{array}{c}{10}^{-5},i=0\\ i\·\mathrm{\Δ\θ},i=\mathrm{1,2,3}...\end{array}\right.$

末端点Q_i坐标为可用下式表示；

(Ⅱ)计算蛇臂末端点到路径的距离

跟踪路径第m段时，求出弯曲角度第i次步进后蛇臂末端点Q_i到路径第m段所有分点的距离d_ik(k＝1,2,3,…,w)，取最小值作为近似的距离值d_i；

d_i＝min d_ik   k＝1,2,3,…,w

选取距离值d_i(i＝0,1,2,3,…,1000)中最小值，设第t个值为最小值，则弯曲角度为：

${\mathrm{\θ}}_{R_N}=\left\{\begin{array}{c}{10}^{-5},t=0\\ t\·\mathrm{\Δ\θ},t=\mathrm{1,2,3}...\end{array}\right..$