基于双向贪心搜索算法的串联机器人末端轨迹规划方法与流程

本发明涉及机器人控制，尤其涉及机器人轨迹规划领域，更具体的说涉及一种基于双向贪心搜索算法的串联机器人末端轨迹规划方法。

背景技术：

1、串联机器人因其工作空间灵活、可扩展性强被大量应用于工业生产中的测量、焊接以及装配等任务中。测量或装配等任务可以描述为，机器人不发生碰撞的情形下末端以一定顺序经过给定位置，在各个位置完成指定任务。由于机器人逆运动学具有多解的性质，每个末端位置不一定对应单一的关节空间位置，同时由于机器人碰撞限制，关节空间的直线轨迹不一定可达。

2、目前，机器人末端的轨迹规划常用的方法为基于随机搜索实现，但是其一般情况下得到的轨迹较长，机器人运行效率较低并且每次得到的结果重复性较差。

技术实现思路

1、为了解决上述现有技术中存在的问题和不足，本发明提出了一种基于双向贪心搜索算法的串联机器人末端轨迹规划方法，通过起点和终点的双向贪心搜索轨迹规划方法，利用二进程并行方法降低了机器人末端轨迹规划所用的时间，同时在串联机器人位姿选取时采用贪心方法，减少了搜索空间，提高了机器人的运行效率，具有良好的工业应用价值。

2、为了实现上述发明目的，本发明的技术方案具体如下：

3、一种基于双向贪心搜索算法的串联机器人末端轨迹规划方法，包括如下步骤：

4、步骤s1.利用串联机器人的几何关系获取机器人的逆运动学模型；

5、步骤s2.根据机器人及环境的模型获得机器人关节位姿的物理可行域；

6、步骤s3.将空间点位按照距离关系进行排序，得到机器人的空间点位运行序列；

7、步骤s4.将点位均分为两份，分别从起点和终点出发，将串联机器人空间点位依次输入机器人逆运动学模型中，规划机器人半轨迹，得到两条半轨迹；

8、步骤s5.将所述两条半轨迹连接，最终获得机器人完整的轨迹。

9、作为优选地，所述步骤s1具体包括：

10、步骤s11.通过cad模型或机器人标定方法获取机器人几何关系；

11、步骤s12.利用所述机器人几何关系建立机器人运动学模型；

12、步骤s13.通过机器人运动学模型建立机器人逆运动学模型。

13、作为优选地，所述步骤s2具体包括：

14、步骤s21.在机器人关节位姿空间中随机选取大量关节位姿；

15、步骤s22.在选取的关节位姿中，计算关节位姿对应的机器人各个关节的位置和环境各个组件的位置，对cad模型两两进行碰撞检测；若发生碰撞则为不可行关节位姿，反之为可行关节位姿；

16、步骤s23.通过插补的方式获取整个串联机器人的物理可行域。

17、作为优选地，所述步骤s3具体包括：

18、步骤s31.根据机器人任务获取机器人末端的起点、终点以及需要经过的中间空间点位；

19、步骤s32.将中间空间点位向所述起点和终点的连线投影，沿连线均分上述空间点位，将其顺序作为两条原始序列；

20、步骤s33.从原始序列中的第一点位作为出发点出发，将其加入输出序列，并在原始序列中删除该点位；

21、步骤s34.选取所述原始序列中与出发点在测量方向距离小于阈值的一系列点，获得局部序列，若局部序列长度小于等于1，则回到步骤s33；

22、步骤s35.将所述局部序列按照与出发点空间距离从小到大进行排序，遍历该局部序列，若遍历的当前点位不是原始序列的当前第一点，则将其加入输出序列，否则停止遍历；

23、步骤s36.循环步骤s33-s35至原始序列为空，此时输出序列即为规划好的点位运行序列。

24、作为优选地，所述步骤s4具体包括：

25、步骤s41.采用双进程分别处理两个点位运行序列，分别从起点和终点出发，将两个点位序列中的机器人空间点位依次输入至机器人逆运动学模型中，获得两个关节空间序列；

26、步骤s42.以两个关节空间序列的起点和终点作为第一点分别建立两条半轨迹，分别遍历两个可行空间序列；

27、步骤s43.循环取上一位置起点，在当前关节空间序列中未加入半轨迹的点中随机选取一定量的位姿为终点，在位姿空间中取起点和终点位姿的直线轨迹，进行插值，计算每个插值点是否发生碰撞，若所有点均不发生碰撞，则该直线轨迹称为直线可行轨迹，并将该轨迹作为机器人可行轨迹，在所有可行轨迹中选取运行时间最短的机器人轨迹，加入半轨迹，继续循环，否则跳转至步骤s44。

28、步骤s44.使用随机搜索树方法在关节位姿空间中搜索可行轨迹，再尝试跳跃连接优化轨迹，将所述轨迹作为机器人轨迹，加入半轨迹序列。

29、作为优选地，所述步骤s5具体包括：

30、步骤s51.选取步骤s4中两条半轨迹的末端点称为中间点；

31、步骤s52.在关节位姿空间中取中间点的直线轨迹，进行插值，计算每个插值点是否发生碰撞，若所有点均不发生碰撞，则将该轨迹作为机器人中间轨迹；

32、步骤s53.将起点半轨迹、中间轨迹、终点半轨迹依次连接，最终得到完整的机器人轨迹。

33、作为优选地，所述步骤s22具体包括：

34、步骤s22.1.获取机器人各个关节及环境的准确cad模型；

35、步骤s22.2.将所述cad模型三角化，获取三角化cad模型的所有顶点和顶点法向；

36、步骤s22.3.将所述三角化cad模型顶点的坐标沿顶点法向移动固定值，得到cad模型外包络；

37、步骤s22.4.将所述三角化cad模型顶点的坐标沿顶点法向反向移动固定值，得到cad模型内包络；

38、步骤s22.5.将所述三角化cad模型、cad模型外包络以及cad模型内包络组合得到扩展cad模型；

39、步骤s22.6.计算所述扩展cad模型的最小凸体包络；

40、步骤s22.7.将各个组件位置表示为齐次矩阵，将齐次矩阵与各个组件的最小凸体包络的其次坐标相乘，计算各个组件最小凸体包络的世界坐标；

41、步骤s22.8.使用世界坐标下各个模型的最小凸体包络两两进行相交检测，若相交则说明发生碰撞，反之说明未碰撞。

42、作为优选地，对于完全机器人，逆运动学模型的解为分离解，几何可行空间为空集或有限集；对于冗余机器人，逆运动型模型的解为连续解，通过空间范围表示。

43、作为优选地，所述步骤s43具体包括：

44、步骤s43.1.在所述关节空间序列中按照到起点欧式距离加权随机选取终点位姿，计算该位姿是否在所述物理可行域中，若满足，则将其加入可行点序列，否则在物理可行域中去除其邻域，循环选取直至可行终点序列长度达到预设值或物理可行域为空；

45、步骤s43.2.遍历可行终点序列，选取终点与起点进行线性插值，得到个中间点，计算所述个中间点是否在所述物理可行域中，若不满足则为发生碰撞，否则未碰撞；其中，为终点机器人的关节坐标，为起点机器人的关节坐标，表示中间点坐标。

46、作为优选地，所述步骤s44具体包括：

47、步骤s44.1.以起点为根建立随机搜索树；

48、步骤s44.2.在物理可行域中以一定的概率分布函数随机取点，以该随机取得的点为中心，将随机搜索树中所有节点按照到该点的欧式距离由小到大进行排序，得到相应的节点序列，遍历所述节点序列，使用步骤s43的插值和检验方法检测当前点到当前树的所有节点是否有步骤s43所述的直线可行轨迹，若存在则将其作为该节点的子节点，结束循环；

49、其中，所述距离计算为：设所述物理可行域中随机选取的点的坐标为，所述随机搜索树中的任意节点的坐标为，距离计算为：。

50、步骤s44.3.从根节点出发遍历搜索树，使用步骤s43的插值和检验方法尝试其他已搜索到的节点与当前轨迹上节点是否直线可行，若可行且距离更短，则将其代替当前轨迹。

51、本发明的有益效果：

52、（1）本发明针对现有技术机器人末端轨迹规划效率较低的问题，提出了从起点和终点双向贪心搜索的轨迹规划方法，通过二进程并行方法降低了机器人末端轨迹规划所用的时间，同时在串联机器人位姿选取时采用贪心方法，减少了搜索空间，提高了机器人的运行效率，具有良好的工业应用价值。

53、（2）本发明针对现有技术在机器人末端轨迹规划时，搜索轨迹较长、运行效率较低的问题，提出了直线轨迹搜索和随机搜索树搜索结合的方法。在直线随机搜索中提出通过设计分布函数尽可能选取较短轨迹的方法，在随机搜索树搜索中提出将随机搜索树与图优化方法结合优化轨迹的方法，减少了机器人的搜索轨迹，提高了运行效率。