一种面向动态医药场景下的机械臂路径规划与重规划方法

本发明属于机械臂路径规划，特别是涉及一种面向动态医药场景下的机械臂路径规划与重规划方法。

背景技术：

1、在医药工业发展中，为了能够进一步提高药物生产速度，机械臂作业成为提高效率的重要手段之一。在医药制药生产环境中，存在许多配药制药仪器（保温箱、震荡仪等）以及试管试剂，导致机械臂运动作业面临巨大挑战，找到一条从起始点到目标点的无碰撞最优路径是安全高效实现配药制药操作任务重要前提。

2、机械臂路径规划方法可以分为离线路径规划以及在线路径规划，两者主要区别在于离线路径规划需要预先知道在机械臂工作空间中障碍物的位姿信息，而在线路径规划是在运动过程中利用传感器感知环境信息从而进行实时规划。在线路径规划的方式主要依赖于人工势场法，根据已设定的运动目标和实时感知到的障碍物通过建立引力场和斥力场，从而实现动态环境下实时避障，这种方式能够很好的解决在非结构化环境下机械臂运动路径规划操作，但是当障碍物斥力与运动目标引力大小相同方向相反时，则会使得机械臂在实时规划时陷入局部最小值，导致机械臂路径规划失败。目前离线路径规划方法可以分为：基于搜索方式的规划算法（如a*算法、dijkstra算法等）、基于智能仿生方式的规划算法（蚁群算法、模拟退火算法等）、基于机器学习方式的规划算法（图神经网络算法、对抗神经网络规划算法等）以及基于概率采样方式的规划算法（rrt算法、prm算法等）。机械臂属于开链系统，其自由度高，基于搜索方式的规划算法计算复杂性随着规划空间的维度增加而迅速增长，该类算法难以应用于机械臂路径规划中。基于智能仿生方式的规划算法通过模拟大自然进化方式来对目标求最优解，这类方法需要合理设置参数且容易陷入局部最小值，此外该类方法随着维度的增加计算复杂度也会急剧增加。基于机器学习方式的方法通过采集的数据进行学习训练，得到机械臂近似最优的安全无碰撞路径，但是这类方法需要花大量时间进行训练且泛化性较差。基于概率采样的规划方法通过在规划空间中进行随机采样，通过节点之间的连接最终形成一条可行路径，这类算法适合解决高维空间中路径规划问题，但是基于概率采样的算法具有随机性，效率较低且无法保证是最短路径。

3、基于此，本发明提出一种面向动态医药场景下的机械臂路径规划与重规划方法。

技术实现思路

1、针对以上技术问题，本发明提供一种面向动态医药场景下的机械臂路径规划与重规划方法。

2、本发明解决其技术问题采用的技术方案是：

3、一种面向动态医药场景下的机械臂路径规划与重规划方法，所述方法包括以下步骤：

4、s100：定义机器臂工作三维空间，确定初始点和目标点位置，使用深度相机获得环境点云信息，分别追踪静态障碍物点云信息以及动态障碍物点云信息；

5、s200：将初始点位置设置为随机树的初始节点，目标点位置设置为随机树的目标节点；

6、s300：基于非均匀采样自适应步长rrt*算法进行初始路径规划：基于高斯函数随机生成一个数，根据该数和预设的高斯函数约束阈值之间的大小关系，在全局采样空间或局部采样空间中随机采样，并根据随机采样的点进行自适应步长扩展生成新节点；

7、s400：判断静态障碍物点云最近的点云与新节点距离是否满足碰撞阈值，若满足则将新节点加入随机树，进行修剪重连，若不满足碰撞阈值，则废弃该新节点；返回s300重新采样，直至在预设最大采样次数内得到安全无碰撞的初始路径；

8、s500：基于rrt*结合虚拟势场对动态障碍物进行实时避障：得到初始的安全无碰撞路径后，结合路径决策控制器来选择机械臂运动路径，根据动态障碍物点云信息实时判断初始路径节点是否存在危险节点，若存在危险节点，则重规划局部路径，当实时判断初始路径节点中不存在危险节点时，则执行初始路径并判断机械臂运动规划是否完成，直至机械臂完成从初始节点到目标节点的运动。

9、优选地，s300中基于高斯函数随机生成一个数，根据该数和预设的高斯函数约束阈值之间的大小关系，在全局采样空间或局部采样空间中随机采样，包括：

10、s310：基于高斯函数随机生成一个数，当该值大于预设的高斯函数约束阈值时，根据启发函数选择随机树中最小的节点，构建局部采样空间，并进行随机采样；其中，局部采样空间是根据启发函数选择出的节点与随机树的目标节点构建的立方体采样空间；

11、s320：当该值小于或等于预设的高斯函数约束阈值时，在全局采样空间随机采样；其中，全局采样空间是三维空间中机械臂能到达的所有空间位置。

12、优选地，s310中启发函数为：

13、；

14、其中，表示从随机树的初始节点到第n个节点的实际路径长度，表示第n个节点到随机树目标节点的欧式距离，作为权重因子；

15、局部采样空间的范围为，其中：

16、；

17、；

18、；

19、其中，为根据启发函数选择的随机树中最小的节点，为随机树的目标节点。

20、优选地，s300中根据随机采样的点进行自适应步长扩展生成新节点，包括：

21、s330：根据随机采样的点，引导随机树扩展方向，并找到离随机采样点最近的树节点；

22、s340：每次采样的点会根据最近的树节点与障碍物之间的距离通过预设的步长区间范围[a,b]来自适应的设置扩展步长，生成一个最近的树节点到随机采样点方向步长为扩展步长的新节点。

23、优选地，s340中自适应步长具体为：

24、；

25、其中，、分别是预设的步长区间范围的最小值和最大值，是一个比例系数，表示所有障碍物最近点云到随机采样点的距离之和。

26、优选地，s400还包括：

27、当在预设最大采样次数内未生成初始路径，则终止算法，路径规划失败。

28、优选地，s500中判断规划路径节点是否存在危险节点，包括：

29、设置一个半径为的球体作为动态障碍物的危险区域，当动态障碍物移动时，初始路径中的节点进入了动态障碍物的危险区域则都将视为危险节点。

30、优选地，s500中若存在危险节点，则重规划局部路径，具体为：

31、s510：获取路径中危险节点，将随机树中第一个危险节点的父节点当作重规划的初始节点，将随机树中最后一个危险节点的子节点当作目标节点；

32、s520：追踪动态障碍物点云信息，将初始节点作为树的根节点，向目标节点进行扩展，并随机采样得到随机采样点；

33、s530：通过人工势场法构建虚拟势场，目标节点对重规划中随机树产生引力，动态障碍物和静态障碍物对重规划中随机树分别产生斥力和，根据引力、静态障碍物斥力以及动态障碍物斥力得到虚拟势场合力；

34、s540：根据随机采样的点，引导随机树扩展方向，并找到离随机采样点最近的树节点，根据随机采样点、最近的树节点和虚拟势场合力组成随机树扩展策略进行扩展，找到新节点并对其进行碰撞检测，如果满足碰撞条件则添加到局部路径规划随机树中，反之则废弃，直到在预设最大采样次数内生成一条最终的局部路径，基于初始路径中可用的部分和生成的局部路径，完成路径规划；若在预设最大采样次数内未形成局部路径，则规划失败。

35、优选地，s530中，虚拟势场合力引力f具体为：

36、；

37、其中，引力具体为：

38、；

39、其中，表示引力的最大值，表示比例因子，表示重规划随机树中节点到目标节点的距离，当重规划随机树中节点到目标节点的距离越远时，就越大；

40、静态障碍物斥力以及动态障碍物斥力具体为：

41、；

42、；

43、其中，是最大斥力，表示比例因子，表示节点到静态障碍物斥力点的距离，将静态障碍物离随机树节点最近的点云作为斥力点，将动态障碍物离随机树节点最近的点云作为斥力点，表示节点到动态障碍物斥力点的距离，表示产生斥力的安全距离；如果节点离斥力点的距离越近，那么障碍物对其产生的斥力就越大。

44、优选地，s540中新节点的生成方式具体为：

45、；

46、其中，是权重因子，来调节rrt*与虚拟势场作用时的权重。

47、上述提出了一种非均匀采样自适应步长rrt*算法与rrt*结合虚拟势场的机械臂路径规划与重规划算法，该方法解决了传统路径规划算法效率低，规划时间长，实时性低以及动态环境下规划能力差的问题，可实现在医药场景下机械臂在静态障碍物以及动态障碍物约束下生成一条安全高效的运动路径。