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

说明书

技术领域

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

背景技术

传统的离散型机器人(如串、并联机器人)均采用刚性关节和连杆结构，用于实现在自由空间内的多自由度运动，但由于其一般只具有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=Ls]]>

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

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

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

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

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