本发明属于机器人，特别涉及一种适用于蛇形臂机器人的避障路径规划方法。
背景技术：
1、蛇形臂机器人是一种新型的仿生机器人，借助线驱动的驱动方式灵活地改变自身的形状与角度，从而实现良好的弯曲性能。蛇形臂机器人在许多领域有着广泛的应用前景，细长的形状和多自由一种适用于蛇形臂机器人的避障路径规划方法度的变形使其能够顺利进入狭窄的作业区域，尤其是复杂、危险的非结构化工作环境，例如有坍塌、辐射、泄露等不适宜人类进入的场合。在现场工作时，需要用远端的遥控设备，通过蛇形臂机器人上搭载的摄像头进行手动操作，这需要对操作人员进行一系列操作上的训练，具有一定的操作门槛。为了使蛇形臂的操作更加智能化，需要使其具备一定的路径自主规划能力。目前常见的路径规划方法有：a*方法、rrt方法、智能搜索算法等，这些方法虽然成熟，但其规划出的路径，难以直接被蛇形臂机器人使用。原因之一是因为蛇形臂机器人在运动过程中会遇到许多障碍物，因此需要进行避障路径规划，而目前的规划方法不能满足适用于蛇形臂机器人的避障路径规划方法，实现不了对蛇形臂机器人避障路径的高效规划；因此，如何提供一种使用于蛇形臂机器人的路径规划方法称为本领域技术人员目前需要解决的问题。
技术实现思路
1、本发明的目的是提出一种适用于蛇形臂机器人的避障路径规划方法，是一种rrt搜索方法的衍生方法，在保证稳定性的前提下，尽可能提高方法效率。
2、为了实现上述任务，本发明采用以下技术方案：
3、一种适用于蛇形臂机器人的避障路径规划方法，包括以下步骤：
4、步骤1，建立包含障碍物的环境模型，并给定起始点与目标点，初始化路径生长网络
5、所述的障碍物环境模型是包含可能与蛇形臂机器人运动过程中发生碰撞关系的所有物体全部位置信息的模型，位置信息的描述方法不做限制，可采用几何特征表示法、栅格网络表示法等成熟的方法。
6、所述的起始点为蛇形臂机器人的末端点在运动开始前的位置。
7、所述的目标点为蛇形臂机器人末端点最终要到达的位置。
8、所述的路径生长网络为路径搜索过程中所记录的路径信息的一种表现形式，具体到程序实现中，为一种特殊的数据结构。
9、所述的路径生长网络数据结构，具体结构如图4所示。
10、所述的路径生长网络数据结构，包含但不限于如下五种类属性：(1)路径节点；(2)起始点；(3)目标点；(4)转角限位；(5)单节路径最短距离；(6)起始延伸方向。
11、所述的路径生长网络数据结构，包含但不限于如下五种类方法：(1)初始化方法；(2)添加节点方法；(3)路径夹角计算方法；(4)路径节点连接方法；(5)路径延伸方法。
12、所述的路径生成网络共有两段，第一段从起始点向外延伸，称为起始端路径网络；第二段从目标点向外延伸，称为目标端路径网络。
13、所述的路径节点，是路径生长网络的一种组成要素，具体到程序实现中，为一种特殊的数据结构。
14、所述的路径节点数据结构，包含但不限于如下五种类属性：(1)序号；(2)坐标；(3)子节点序号；(4)父节点序号；(5)连接关系表。
15、所述的路径节点数据结构，包含但不限于如下五种类方法：(1)初始化方法；(2)添加子节点序号的方法；(3)添加父节点序号的方法；(4)修改连接关系表的方法；(5)根据子节点序号查找对应父节点序号的方法。
16、所述的转角限位，为路径生长网络在延伸的过程中，允许通行的相邻路径段之间的最大夹角，其具体的数值由所述蛇形臂机器人的结构或控制参数决定。
17、所述的单节路径最短距离，为路径生长网络中，除与起始点与目标点为端点的其他所有路径段所允许的最短距离，其具体的数值受所述蛇形臂机器人单节连杆长度的影响，一般取所述蛇形臂机器人单节活动连杆中长度的最大值。
18、所述的起始延伸方向，为所述的蛇形臂机器人末端到达所述起始点时，末端连杆的方向。
19、所述的添加节点方法，为将新节点添加到所述路径绳长网络中的方法函数。
20、所述的路径夹角计算方法，为计算路径生长网络中相邻路径段之间夹角的方法函数。
21、所述的路径节点连接方法，为判断新节点与路径生长网络连接是否合理的方法函数。若合理，则调用所述的添加新节点方法，同时自动更改相关路径节点的父节点序号、子节点序号与连接关系表，并返回逻辑值1；若不合理，则直接返回逻辑值0。
22、所述的路径延伸方法，为将路径网络向采样点延伸的方法函数，应包含如下过程：(1)寻找路径网络中到所述采样点的代价最小的节点，称为广义最近节点；(2)从广义最近节点向采样点延伸，找到新节点；(3)调用所述的路径节点连接方法。
23、所述的连接关系表，为一种包含了当前节点通行信息的二维数组结构，其第一列为当前节点的子节点序号，第一行为当前节点的父节点序号，子节点序号所在行与父节点序号所在列交叉的位置记录0或1,0表示不允许从列所在父节点通过当前节点到达行所在子节点，1表示允许从列所在父节点通过当前节点到达行所在子节点。
24、所述的根据子节点序号查找对应父节点序号的方法，其主要目的是根据当前节点的某一个子节点序号，查找对应的父节点序号，并且要求该父节点序号通过当前节点到达所述的子节点的允许通行状态为1。
25、步骤2，判断起始点与目标点是否可以直接相连
26、步骤2.1将目标点设为新节点，调用所述起始端路生长网络的路径节点连接方法，判断将目标点加入到起始端路径生长网络的合理性，若允许加入，则跳转步骤2.2，否则，跳转步骤3。
27、步骤2.2将起始点设为新节点，调用所述目标端路径生长网络的路径节点连接方法，结束搜索过程。
28、步骤3，在环境中随机生成采样点。
29、在四种不同的采样方式中随机选择1种生成随机采样点。
30、所述的采样点，是指用于引导上述路径生长网络在环境中延伸的点，每有一个采样点，路径生长网络就有一个距采样点最近的节点，向采样点所在的方向生长；
31、所述的路径生长网络中的生长，是指从某一个节点为起点，向某个特定的方向移动一定的距离后，以到达的位置作为路径生长网络新节点的位置，并尝试将新节点添加到该路径生长网络中，同时建立连接关系。
32、所述的四种不同的采样方式如下：
33、方式(1)：选取所述目标点作为起始端路径生长网络的采样点，选取所述起始点作为目标端路径生长网络的采样点；
34、方式(2)：选取所述目标点附近的随机点作为起始端路径生长网络的采样点，选取所述起始点附近的随机点作为目标端路径生长网络的采样点；
35、方式(3)：分别在环境空间中随机取点，作为两段路径生长网络的采样点；
36、方式(4)：选取另一个路径生长网络的最新节点，作为当前路径生长网络的采样点。
37、步骤4，根据采样点延伸路径生长网络
38、步骤4.1寻找到达采样点代价最小的路径节点；
39、所述的代价最小，包含两个要素：(1)节点到达采样点的欧式距离应尽可能的小；(2)节点的延伸方向应尽可能的接近采样点所在的方向。
40、所述的节点延伸方向，为当前节点的某一个父节点指向当前节点的方向a，其与采样点所在方向c的接近程度可用两者之间的夹角度量θ，计算方法如下：
41、
42、其中，θ表示当前节点的父节点到当前节点的延伸方向与当前节点到采样点的延伸方向之间的夹角；向量a表示当前节点的父节点到当前节点的方向向量；向量c表示当前节点到采样点的方向向量(图6)。
43、采用下式对所述的两个代价要素进行统一处理：
44、
45、其中，dθ为判断当前节点向采样点移动的代价指标，称为当前节点到采样点的广义距离；d为当前节点到采样点的欧式距离；dall为所述目标点与所述起始点之间的距离；θmax为两方向向量理论上的最佳夹角；gd与gθ分别为所述要素(1)与所述要素(2)的权重因子。
46、按照所述的广义距离dθ，寻找到达采样点代价最小的路径节点，设为延伸节点。
47、步骤4.2以所述的延伸节点为起点，向尽可能靠近采样点的方向e移动一个规定的步长，将移动后到达的位置设为新节点所在位置(图7)。
48、对所述的尽可能靠近采样点的方向e，应按照如下步骤进行：
49、步骤4.2.1分如下两种情况，计算夹角θ：
50、(1)若延伸节点为路径生长网络的第一个节点，则不存在父节点。这时计算所述路径生长网络的起始延伸方向与延伸节点到采样点所在方位之间的夹角θ；
51、(2)若延伸节点不是路径生长网络的第一个节点，则必然拥有至少一个父节点，计算所有父节点到延伸节点方向a与延伸节点到采样点方向c的夹角，然后取绝对值最小的夹角作为θ；
52、步骤4.2.2分如下两种情况，计算延伸方向向量e：
53、(1)若θ≤θlimit，则取e＝c；
54、(2)若θ>θlimit，则按照下式计算延伸方向向量e：
55、e＝rot(ω,θlimit)c
56、其中，表示以为转轴，以θlimit为旋转角度得到旋转矩阵。
57、所述的转轴的计算方式如下：
58、
59、步骤4.2.3以所述的延伸节点为起点，以方向e为延伸方向，以规定的大小移动一个的步长，将移动后到达的位置设为新节点所在位置。
60、所述的步长规定的大小，分以下两种情况讨论：
61、(1)若延伸节点为所述路径生长网络的第一个节点，则步长大小可任意选取；
62、(2)若延伸节点不是所述路径生长网络的第一个节点，则步长大小的选取不能小于所述单节路径最短距离。
63、步骤5，进行新节点的碰撞检测
64、若新节点没有与环境发生碰撞，继续进行步骤6,；否则，跳转到步骤3。
65、步骤6，将新节点添加到路径生长网络中
66、调用所述路径生长网络的路径节点连接方法以完成该步骤。
67、步骤7，搜索结束判定
68、所述搜索结束的判定原则如下：
69、(1)若两端路径中分别存在至少一个路径节点作为过渡节点，可以使得两端路径的中任一节点通过对面路径生长网络的过渡节点添加到对面路径生长网络中，则说明这两段路径可以成功对接，路径生长网络构建成功，可继续进行步骤8。
70、(2)若直到搜索过程超过给定限制条件(如最大路径节点数、最大搜索次数、最大搜索时间)仍未使得两端路径生长网络对接成果，则说明本轮搜索失败。
71、步骤8，提取最终路径
72、指从两端路径网络中提取有用的路径节点，按照特定序列，组成一组用于机器人通行的特定路径，具体方法如下：
73、分别从两端路径的过渡节点作为当前节点出发，以当前节点的最小父节点作为前进方向，直到到达根节点为止。将两段路径序列首位相接，可得到最终的路径节点序列。
74、进一步地，可以根据搜索过程的实际情况并参考步骤3中各种采样方式的特点修改各方式的选择概率，或增加新的采样方式；
75、一般地，步骤3中四种采样方式的特点如下：
76、方式(1)：可以让路径生长网络快速接近目标点；
77、方式(2)：可以让路径生长网络快速到达目标点附近，该方法会比方法(1)中的接近速度慢一些，但进入死循环的概率会比方法(1)小很多；
78、方式(3)：可以扩大路径的搜索范围，主要用于防止路径生长网络搜索陷入死循环；
79、方式(4)：可以让两端路径更快地进行连接。
80、进一步地，步骤4.1中，可以根据搜索过程的实际情况和所述蛇形臂机器人的运动能力，修改权重因子gd与gθ的值，以获得更好的搜索效果；
81、进一步地，步骤4.1中，可以根据环境空间的实际情况和起始点与目标点之间的距离，选其他适当的常数来代替所述的dall，以获得可能出现的更好的搜索效果。
82、进一步地，步骤4.2.2中，若搜索空间只局限于二维平面中，为简化计算，延伸方向e可按照下式计算：
83、e＝[cosθe,sinθe]t
84、其中，θe为延伸方向与参考坐标系x轴正方向的夹角，其值计算如下：
85、
86、其中，θa为步骤4.2中所述的节点延伸方向a与参考坐标系x轴正方向之间的夹角。
87、本发明的有益效果：通过改进的使用与蛇形臂机器人的rrt避障路径规划方法，解决了当前搜索算法效率与搜索路线的通过性难以同时满足的现状，既保证了被规划的路径的较高的通过性能，又提高了路径的搜索效率，降低了进一步优化后续路径的难度。