一种机械臂避障路径规划方法

本发明涉及机器人运动规划，具体地说是一种机械臂避障路径规划方法。

背景技术：

1、近年来，气动测量技术在汽车零件的测量方面有着广泛应用，其具有抗干扰能力强，操作简单，可清洁零件等优点。目前，其测量方式为人工测量，人工测量不仅成本高、效率低而且测量精度也难以保证。随着我国机械行业的快速发展，气动测量系统与机械臂的融和使用可以有效地解决上述问题。机械臂具有高精度、高灵活性的特性，可以使测量装置适应不同的测量环境，提高测量装置的使用价值。

2、采用路径规划方法对机械臂进行路径规划，可以有效提高机械臂完成作业任务的效率。近年来路径规划算法层出不穷，其中人工势场法(apf)与快速扩展随机树算法(rrt)应用最为广泛。

3、传统的人工势场法由于强烈的目标导向性，导致该算法容易陷入局部最优和目标不可达问题，这使得传统算法在复杂环境中做路径规划规划时，很难规划出可行路径。很多学者提出了优化算法对其进行改进，例如低震荡人工势场-自适应快速扩展随机树法，解决了人工势场法易陷入局部最小值和路径震荡的问题；将传统人工势场法与细菌进化算法结合来解决局部最小值的问题；还有对传统势场法的引力势函数进行改进，使其具有优化能力，可规划出平滑路径。

4、但上述方法都存在规划效率较低的问题。传统的快速扩展随机树算法(rrt)存在缺乏目标导向性导致路径规划过程中规划效率低、规划路径往往不是最优的问题。许多学者提出了改进算法，例如rrt*算法使得rrt算法具有了渐进优化的能力；rrt*-connect算法，在算法具有优化能力的基础上，同时引入目标导向策略，使得该算法规划时间大大降低。

技术实现思路

1、本发明的目的是提供一种机械臂避障路径规划方法，能够解决现有规划方法规划效率较低的技术问题。

2、为了实现上述目的，本发明采用如下技术方案。

3、一种机械臂避障路径规划方法，包括以下步骤：

4、s01、初始化工作空间的环境信息，获得机械臂在工作空间的中的起始点、目标点、障碍物坐标，并建立机械臂的运动学模型，分别求出正、逆运动学的解。

5、s02、根据环境信息，建立机械臂与障碍物的碰撞检测模型。

6、s03、采用改进的人工势场法，规划机械臂从起始点到目标点的无碰撞路径。

7、s04、检测步骤s03中路径规划过程是否陷入局部最小值，若陷入局部最小值，则采用改进的快速拓展随机树算法继续进行路径规划跳出局部最小值，若未陷入局部最小值，则继续步骤s03。

8、s05、在跳出局部最小值后，继续步骤s03，直至实现全部路径。

9、进一步地，改进的人工势场法在斥力势函数中引入机械臂末端点与目标点的相对距离和斥力调节因子，用于改变目标点附近障碍物的斥力影响，当距离目标点越近时，机械臂所受到障碍物的斥力越小，使得机械臂末端点能顺利到达目标点。

10、进一步地，改进后的斥力势函数为：

11、

12、式中：urep表示斥力势函数，kr为斥力调节因子，d为机械臂与障碍物的距离，d0为障碍物的斥力势场范围，pr为机械臂末端的位置坐标，pg为目标点位置坐标，a为改进因子，d(pr-pg)为机械臂末端与目标点的相对距离。

13、机械臂的每个关节都不能和障碍物产生碰撞，对单个关节i来说，其斥力势函数为：

14、

15、式中：uir为第i个关节的斥力函数，kr为斥力调节因子，dn为机械臂与障碍物的距离，d0为障碍物的斥力势场范围。

16、机械臂的斥力势能总和为：

17、进一步地，斥力调节因子的取值为非负且不等于0，斥力调节因子根据目标点在障碍物斥力范围内的位置不同做调节。

18、进一步地，步骤s04中检测改进的人工势场法在规划路径过程中陷入局部最小值时，以机械臂末端点为圆心，设置检测圆圈，若机械臂在检测周期内逃出检测圈，则未陷入局部最小值；若机械臂在检测周期内未逃出检测圈，则判定为陷入局部最小值。

19、进一步地，检测改进的人工势场法在规划路径过程中陷入局部最小值的方法，具体包括以下步骤：

20、(1)以机械臂末端点为圆心，设置检测圆圈，设路径规划平均速度的四分之一为v，检测周期为t，则检测圆圈的半径r＝vt；

21、(2)每个检测周期开始时，获取路径点的坐标(x1,y1),每个检测周期结束时，获取路径点的坐标为(x2,y2)；

22、(3)计算两个路径点的距离d；

23、(4)判断d与r的大小，若d大于r，未陷入局部最小值，若d小于r，则判定改进的人工势场法陷入局部最小值。

24、进一步地，步骤s04中，采用改进的快速拓展随机树算法进行路径规划，包括以下步骤：

25、(1)改进的快速拓展随机树算法在进行随机采样点的生成时，使随机采样点有一定几率为目标点，将随机采样点定为目标点的概率设置为p；

26、(2)在改进的快速拓展随机树算法规划出n个可行路径点之后，计算出离障碍物最远的路径点，并将该点设为起始点，切换改进的人工势场法继续做路径规划，直到完成路径规划；

27、(3)在完成路径规划之后，对采用改进的快速拓展随机树算法规划的路径做优化处理。

28、进一步地，采用改进的快速拓展随机树算法进行路径规划的具体步骤包括：

29、(1)初始化机械臂的初始点qinit,末端点qgoal，状态空间c，扩展步长s,偏置概率p，扩展点数n,最大扩展点数n；

30、(2)在[0-1]中任取一个概率值prand,如果prand≤p,则令目标点qgoal＝qrand，如果prand＞p，则在自由状态空间中cfree随机采样一个状态点qrand；

31、(3)在扩展树tk中找到距离qrand最近的点，并将其命名为qnearest，并沿qnearest-qrand方向前进一个步长s，得到点qnew，连接qnearest和qnew并检测是否发生碰撞，若碰撞，则舍去，并回到第(2)步，若不碰撞则进行下一步；

32、(4)检测qnew是否进入目标区域，如果进入目标区域，则将qnew与qgoal纳入扩展树中，结束算法运行，如果没有进入目标区域，则将qnew纳入tk，并将其个数赋值给n，若n≥n，则结束算法运行，若n＜n则返回第(2)步，并重复以上过程。

33、进一步地，对采用改进的快速拓展随机树算法规划的路径进行优化时，从采用改进的快速拓展随机树算法所规划路径段的起始点开始，依次与其余路径点连接，并判断是否发生碰撞，若发生碰撞，则从碰撞点的上一个路径点继续执行此操作，直到连接最后一个路径点。

34、进一步地，对采用改进的快速拓展随机树算法规划的路径进行优化的具体步骤包括：

35、(1)设改进的快速拓展随机树算法规划出的路径点为n，其中n∈(1,10)；

36、(2)当n＝1时，依次连接n与n+a并检测其是否发生碰撞，若发生碰撞则停止连接下个点，其中a∈(2,10)；

37、(3)直接连接n与n+a-1作为第一段路径；

38、(4)依次连接n+a与n+a+b并检测其是否发生碰撞，若发生碰撞则停止连接下个点，其中b∈(2,10)；

39、(5)直接连接n+a-1与n+a+b-1作为第二段路径；以此类推，直到连接上最后一个点。

40、一种融合机械臂避障路径规划方法的气动测量系统，包括机器人模块、上位机模块、送料机模块和测量模块。其中机器人模块包括机械臂和机器人控制器，机器人控制器用于控制机器人完成抓取、运送、放置等任务。上位机模块通过串口与机器人模块和测量模块完成通信，上位机模块对机械臂路径规划进行计算，向机器人控制器传输计算数据，使得机器人控制器驱动机械臂完成抓取任务，上位机模块通过对测量模块的监控完成检测数据的收集。送料机模块上安装有传感器，传感器与机器人控制器连接，当被测工件触发传感器，机器人控制器驱动机器人抓取工件。气动测量仪模块采用底座式v型测头，负责对待测工件进行测量，并将数据反馈给上位机。

41、进一步地，上位机模块对机械臂进行路径规划，首先搭建机械臂的运动模型及作业环境模型，在作业环境模型中设置起始点和目标点。运用改进的apf-rrt算法，生成一条从起始点到目标点的可行路径。将可行路径的路径点信息以及抓取点、投放点信息生成运动指令导入机器人控制器。

42、进一步地，机器人控制器接收上位机模块的运动指令，控制机械臂完成以下步骤：

43、(1)机械臂末端点运动至起始点，并由起始点移动至抓取点，抓取待测工件；

44、(2)机械臂完成抓取后，机械臂回到起始点；

45、(3)机械臂按规划路径运动至末端点，并由末端点移动至至投放点，将待测工件投放至气动测量仪模块；

46、(4)机械臂移动至起始点准备下次作业。

47、进一步地，送料机模块根据机器人模块一个作业周期时间及传送带速度，设置相邻两工件的安全距离，使待测工件晚于机械臂到达抓取点，时间设置为1秒。待测工件到达抓取点后触发控制机械臂的传感器，机械臂完成抓取任务。

48、进一步地，气动测量仪模块在机械臂完成投放后，对待测工件进行测量，完成测量后将测量数据反馈给上位机模块，完成数据收集。

49、与现有技术相比，本发明具有以下有益效果：

50、1、采用改进的人工势场法(apf)-快速拓展随机树算法(rrt)对机械臂进行路径规划，该算法能适应各种复杂环境，且相较于传统apf法和rrt算法具有规划时间短、规划成功率高等优点；

51、2、有效解决了apf法目标不可达和局部最小值的问题，能高效地规划一条平滑的作业路径；

52、3、本发明通过将机械臂融入气动测量系统，通过机器人代替人工测量，大大的提高了测量效率，解决了人工测量带来的成本高、精度低的缺点。

53、附图说明

54、图1是本发明中一种机械臂避障路径规划方法的效果图；

55、图2是检测改进的人工势场法未陷入局部最小值的示意图；

56、图3是检测改进的人工势场法陷入局部最小值的示意图；

57、图4是改进的快速拓展随机树算法路径段优化前的示意图；

58、图5是改进的快速拓展随机树算法路径段优化后的示意图；

59、图6是本发明中一种融合机械臂避障路径规划方法的气动测量系统的结构示意图；

60、图7是机械臂抓取待测零件的步骤示意图；

61、图8是机械臂投放待测零件的步骤示意图；

62、图9是改进的apf-rrt算法对机械臂避障路径规划的流程图；