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

摘要

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

主权项

一种连续型机器人空间路径跟踪方法，其特征在于：其包括按顺序执行的下列步骤：步骤一、初始化的S01阶段：进行蛇臂位置和姿态初始化，使蛇臂处于垂直状态,其末端点到达系统坐标系{S}原点OS；步骤二、判断路径起点是否在原点上方的S02阶段：判断路径起点是否处在系统坐标系原点OS的上方，若判断结果为“是”，则进入下一步S03阶段，否则下一步进入S04阶段；步骤三、蛇臂上升至路径起点的S03阶段：在路径起点处于系统坐标系{S}原点OS上方时，则末段蛇臂直线前进至路径起点，然后下一步进入S05阶段；步骤四、蛇臂直接弯曲的S04阶段：路径起点若处于系统坐标系{S}原点OS下方，则按照S06阶段方法求解关节段变量，并控制末段关节段进行弯曲，然后下一步进入S05阶段；步骤五、蛇臂前进一步的S05阶段；蛇臂基座前进一个步长；步骤六、求解蛇臂末节关节段变量的S06阶段：采用基于已知点的搜索算法计算末节关节段变量，并记录；步骤七、更新蛇臂姿态的S07阶段：蛇臂各个关节段按照关节段变量进行更新姿态，蛇臂末节采用S06阶段求得的关节段变量，其它各关节依次采用蛇臂末节关节段经过此位置时记录的关节段变量；步骤八、判断路径跟踪是否完成的S08阶段：判断蛇臂末端是否到达路径末端，如果判断结果为“是”，则整个流程至此结束，否则下一步重新进入S05阶段，继续跟踪过程。