1.一种面向动态医药场景下的机械臂路径规划与重规划方法,其特征在于,所述方法包括以下步骤:S100:定义机器臂工作三维空间,确定初始点和目标点位置,使用深度相机获得环境点云信息,分别追踪静态障碍物点云信息以及动态障碍物点云信息;
S200:将初始点位置设置为随机树的初始节点,目标点位置设置为随机树的目标节点;
S300:基于非均匀采样自适应步长RRT*算法进行初始路径规划:基于高斯函数随机生成一个数,根据该数和预设的高斯函数约束阈值之间的大小关系,在全局采样空间或局部采样空间中随机采样,并根据随机采样的点进行自适应步长扩展生成新节点;
S400:判断静态障碍物点云最近的点云与新节点距离是否满足碰撞阈值,若满足则将新节点加入随机树,进行修剪重连,若不满足碰撞阈值,则废弃该新节点;返回S300重新采样,直至在预设最大采样次数内得到安全无碰撞的初始路径;
S500:基于RRT*结合虚拟势场对动态障碍物进行实时避障:得到初始的安全无碰撞路径后,结合路径决策控制器来选择机械臂运动路径,根据动态障碍物点云信息实时判断初始路径节点是否存在危险节点,若存在危险节点,则重规划局部路径,当实时判断初始路径节点中不存在危险节点时,则执行初始路径并判断机械臂运动规划是否完成,直至机械臂完成从初始节点到目标节点的运动;
S300包括:
S310:基于高斯函数随机生成一个数,当随机生成的数大于预设的高斯函数约束阈值时,根据启发函数选择随机树中最小的节点,构建局部采样空间,并进行随机采样;其中,局部采样空间是根据启发函数选择出的节点与随机树的目标节点构建的立方体采样空间;
S320:当随机生成的数小于或等于预设的高斯函数约束阈值时,在全局采样空间随机采样;其中,全局采样空间是三维空间中机械臂能到达的所有空间位置;
S330:根据随机采样的点,引导随机树扩展方向,并找到离随机采样点最近的树节点;
S340:每次采样的点会根据最近的树节点与障碍物之间的距离通过预设的步长区间范围[a,b]来自适应的设置扩展步长,生成一个最近的树节点到随机采样点方向步长为扩展步长的新节点;
S340中自适应步长具体为:
;
其中,、分别是预设的步长区间范围的最小值和最大值,是一个比例系数,表示所有障碍物最近点云到随机采样点的距离之和。
2.根据权利要求1所述的方法,其特征在于,S310中启发函数为:;
其中,表示从随机树的初始节点到第n个节点的实际路径长度,表示第n个节点到随机树目标节点的欧式距离,作为权重因子;
局部采样空间的范围为,其中:
;
;
;
其中,为根据启发函数选择的随机树中最小的节点,为随机树的目标节点。
3.根据权利要求2所述的方法,其特征在于,S400还包括:当在预设最大采样次数内未生成初始路径,则终止算法,路径规划失败。
4.根据权利要求3所述的方法,其特征在于,S500中判断规划路径节点是否存在危险节点,包括:设置一个半径为的球体作为动态障碍物的危险区域,当动态障碍物移动时,初始路径中的节点进入了动态障碍物的危险区域则都将视为危险节点。
5.根据权利要求4所述的方法,其特征在于,S500中若存在危险节点,则重规划局部路径,具体为:S510:获取路径中危险节点,将随机树中第一个危险节点的父节点当作重规划的初始节点,将随机树中最后一个危险节点的子节点当作目标节点;
S520:追踪动态障碍物点云信息,将初始节点作为树的根节点,向目标节点进行扩展,并随机采样得到随机采样点;
S530:通过人工势场法构建虚拟势场,目标节点对重规划中随机树产生引力,动态障碍物和静态障碍物对重规划中随机树分别产生斥力和,根据引力、静态障碍物斥力以及动态障碍物斥力得到虚拟势场合力;
S540:根据随机采样的点,引导随机树扩展方向,并找到离随机采样点最近的树节点,根据随机采样点、最近的树节点和虚拟势场合力组成随机树扩展策略进行扩展,生成新节点并对其进行碰撞检测,如果满足碰撞条件则添加到局部路径规划随机树中,反之则废弃,直到在预设最大采样次数内生成一条最终的局部路径,基于初始路径中可用的部分和生成的局部路径,完成路径规划;若在预设最大采样次数内未形成局部路径,则规划失败。
6.根据权利要求5所述的方法,其特征在于,S530中,虚拟势场合力引力F具体为:;
其中,引力具体为:
;
其中,表示引力的最大值,表示比例因子,表示重规划随机树中节点到目标节点的距离,当重规划随机树中节点到目标节点的距离越远时,就越大;
静态障碍物斥力以及动态障碍物斥力具体为:
;
;
其中,是最大斥力,表示比例因子,表示节点到静态障碍物斥力点的距离,将静态障碍物离随机树节点最近的点云作为斥力点,将动态障碍物离随机树节点最近的点云作为斥力点,表示节点到动态障碍物斥力点的距离,表示产生斥力的安全距离;如果节点离斥力点的距离越近,那么障碍物对其产生的斥力就越大。
7.根据权利要求6所述的方法,其特征在于,S540中新节点的生成方式具体为:;
其中,是权重因子,来调节RRT*与虚拟势场作用时的权重。