一种连续型机器人空间路径跟踪方法
【专利摘要】一种连续型机器人空间路径跟踪方法。其包括初始化、判断路径起点是否在原点上方、蛇臂上升至路径起点、蛇臂直接弯曲、蛇臂前进一步、求解蛇臂末节关节段变量、更新蛇臂姿态、判断路径跟踪是否等阶段。本发明提供的连续型机器人空间路径跟踪方法能够实现对空间任意给定路径的跟踪；采用这种方法能够实现在整个跟踪过程中以路径作为参考，机器人整体形态始终与路径一致，行进覆盖的范围较小，安全性高。
【专利说明】一种连续型机器人空间路径跟踪方法
【技术领域】
[0001]本发明属于自动控制【技术领域】，特别是涉及一种连续型机器人空间路径跟踪方法。
【背景技术】
[0002]传统的离散型机器人(如串、并联机器人)均采用刚性关节和连杆结构，用于实现在自由空间内的多自由度运动，但由于其一般只具有5?7个自由度，自由度数目有限，因此对工作空间受限的环境适应性不强；与离散型机器人不同，连续型机器人为“无脊椎”的柔性结构，机器人采用形状可以灵活改变的结构，而不具有任何刚性的关节和连杆；这种新型的仿生机器人具有良好的弯曲性能，可以柔顺而灵活地改变自身的形状，其优良的弯曲特性甚至可以和蛇体、象鼻子以及章鱼触角等生物器官媲美；由于连续型机器人的外形可以灵活改变，因此具有根据环境障碍物的状况而改变自身形状的能力，对工作空间受限的环境具有独特的适应能力；其应用前景广阔，可以应用于飞机油箱检查、多障碍物工业环境内的作业、弯曲管道和塌陷建筑物内的侦查和搜救、核电站内部管路的维护、人体疾病的诊疗等场合。
[0003]连续型机器人为由多个关节段构成的串联机器人，其关节段具有结构上的约束，能进行空间的弯曲和旋转运动；在空间结构约束很强的环境中，为避免机器人触碰造成损伤或潜在危险，需要对机器人到达目标区域的行进路径进行规划；路径为由若干个类似连续型关节的曲线构成，满足其结构约束，是连续型机器人到达目标区域的最终姿态。
[0004]在这种技术背景下，目前仍缺少简洁有效的能够对连续型机器人空间路径进行跟踪的方法。
【发明内容】
[0005]为了解决上述问题，本发明的目的在于提供一种连续型机器人空间路径跟踪方法，实现机器人跟踪规划的三维空间路径，最终到达目标区域，并且尽量减小行进的覆盖范围。
[0006]为了达到上述目的，本发明提供的连续型机器人空间路径跟踪方法包括按顺序执行的下列步骤:
[0007]步骤一、初始化的SOl阶段:进行蛇臂位置和姿态初始化，使蛇臂处于垂直状态，其末端点到达系统坐标系{S}原点Os ；
[0008]步骤二、判断路径起点是否在原点上方的S02阶段:判断路径起点是否处在系统坐标系原点Os的上方，若判断结果为“是”，则进入下一步S03阶段，否则下一步进入S04阶段；
[0009]步骤三、蛇臂上升至路径起点的S03阶段:在路径起点处于系统坐标系{S}原点Os上方时，则末段蛇臂直线前进至路径起点，然后下一步进入S05阶段；
[0010]步骤四、蛇臂直接弯曲的S04阶段:路径起点若处于系统坐标系{S}原点Os下方，则按照S06阶段方法求解关节段变量，并控制末段关节段进行弯曲，然后下一步进入S05阶段；
[0011]步骤五、蛇臂前进一步的S05阶段；蛇臂基座前进一个步长；
[0012]步骤六、求解蛇臂末节关节段变量的S06阶段:采用基于已知点的搜索算法计算末节关节段变量，并记录；
[0013]步骤七、更新蛇臂姿态的S07阶段:蛇臂各个关节段按照关节段变量进行更新姿态，蛇臂末节采用S06阶段求得的关节段变量，其它各关节依次采用蛇臂末节关节段经过此位置时记录的关节段变量；
[0014]步骤八、判断路径跟踪是否完成的S08阶段:判断蛇臂末端是否到达路径末端，如果判断结果为“是”，则整个流程至此结束，否则下一步重新进入S05阶段，继续跟踪过程。
[0015]在S06阶段中，所述的计算关节段变量的方法为基于已知点的搜索算法求解关节段变量，其包括按顺序执行的下列步骤:
[0016]步骤一、将路径各段均分，求出分点在路径第一关节段坐标系下坐标的S601阶段:
[0017]步骤(a):设关节段长度为L，基座步进的步长为S，则蛇臂完成跟踪一个路径段时基座步进次数w为:
【权利要求】
1.一种连续型机器人空间路径跟踪方法，其特征在于:其包括按顺序执行的下列步骤: 步骤一、初始化的SOl阶段:进行蛇臂位置和姿态初始化，使蛇臂处于垂直状态，其末端点到达系统坐标系{S}原点Os ； 步骤二、判断路径起点是否在原点上方的S02阶段:判断路径起点是否处在系统坐标系原点Os的上方，若判断结果为“是”，则进入下一步S03阶段，否则下一步进入S04阶段；步骤三、蛇臂上升至路径起点的S03阶段:在路径起点处于系统坐标系{S}原点Os上方时，则末段蛇臂直线 前进至路径起点，然后下一步进入S05阶段； 步骤四、蛇臂直接弯曲的S04阶段:路径起点若处于系统坐标系{S}原点Os下方，则按照S06阶段方法求解关节段变量，并控制末段关节段进行弯曲，然后下一步进入S05阶段；步骤五、蛇臂前进一步的S05阶段；蛇臂基座前进一个步长； 步骤六、求解蛇臂末节关节段变量的S06阶段:采用基于已知点的搜索算法计算末节关节段变量，并记录； 步骤七、更新蛇臂姿态的S07阶段:蛇臂各个关节段按照关节段变量进行更新姿态，蛇臂末节采用S06阶段求得的关节段变量，其它各关节依次采用蛇臂末节关节段经过此位置时记录的关节段变量； 步骤八、判断路径跟踪是否完成的S08阶段:判断蛇臂末端是否到达路径末端，如果判断结果为“是”，则整个流程至此结束，否则下一步重新进入S05阶段，继续跟踪过程。
2.根据权利要求1所述的连续型机器人空间路径跟踪方法，其特征在于:在S06阶段中，所述的计算关节段变量的方法为基于已知点的搜索算法求解关节段变量，其包括按顺序执行的下列步骤: 步骤一、将路径各段均分，求出分点在路径第一关节段坐标系下坐标的S601阶段:步骤(a):设关节段长度为L，基座步进的步长为S，则蛇臂完成跟踪一个路径段时基座步进次数w为:
L
Mi ——
S 步骤(b):将路径各段均分为w份，在各路径段坐标系中求解出分点坐标； 步骤(C):求解路径分点在第一关节段坐标系下的坐标，即将求解出分点坐标，再经过齐次变换转化到第一关节段坐标系下坐标； 步骤二、求解蛇臂末端关节段旋转角度的S602阶段: 每次基座步进后，以对应的路径分点为蛇臂末端点要达到的目标点，此点与蛇臂末端关节段构成一个平面，由此确定旋转角度& 步骤三、求解蛇臂末端关节的弯曲角度的S603阶段: 采用数值算法求解弯曲角度I,:按照规定步长遍历弯曲角度，每次求出蛇臂末端点坐标，并求解末端点到路径的距离，选取对应最小距离的弯曲角度。
3.根据权利要求1所述的连续型机器人空间路径跟踪方法，其特征在于:在S07阶段中，在所述的更新姿态时，记录每次跟踪时蛇臂末端关节段的关节段变量，其与路径位置对应，当其余关节段末端到达此位置时，关节段变量相同，直接载入数据即可，不需重复求解，相当于其它关节段跟随末端关节段的轨迹前进。
4.根据权利要求2所述的连续型机器人空间路径跟踪方法，其特征在于:在S601阶段的步骤(C)中，所述的求解路径分点在第一关节段坐标系下的坐标的方法如下: 路径关节段的坐标系为{Bg} (g=0，l，2，…，n)，路径中第m段的关节段变量为(0Pm^J{m = \2,3,-,n);设路径第m段第k个分点为Cm，k，则该点对应的弯曲角度为，可表不为:
5.根据权利要求2所述的连续型机器人空间路径跟踪方法，其特征在于:在S602阶段中，所述的求解蛇臂末端关节段旋转角度的方法如下: 蛇臂第N段起始端处坐标系为{AN_J，在此坐标系中，以路径分点Cm, k为目标点，能够确定一个过Cm，k点和^^轴的平面；蛇臂关节段N在此平面内，平面与轴的夹角即为旋转角度％，若已知Cm，k在坐标系{AnJ中坐标，％则容易求解，设坐标为
6.根据权利要求2所述的连续型机器人空间路径跟踪方法，其特征在于:在S603阶段中，所述的求解蛇臂末端关节的弯曲角度的方法如下: 在平面内采用数值算法求解末端关节的弯曲角度；具体步骤如下: (I)求蛇臂末端点坐标 弯曲角度取值范围IeWM,由于计算过程中％处于分母上，故当时取较小值代替；设该角度步进的步长为Λ Θ，则第i次步进后蛇臂末端关节的弯曲角度为:
【文档编号】B25J9/16GK103692440SQ201310662862
【公开日】2014年4月2日 申请日期:2013年12月6日 优先权日:2013年12月6日
【发明者】牛国臣, 王力, 高庆吉, 胡丹丹 申请人:中国民航大学