[发明专利]一种连续型机器人空间路径跟踪方法

权利要求书

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=Ls]]>

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

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

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

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

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

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

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