1.一种船舶舱室焊接机械臂避障运动路径规划方法,其特征在于:包括以下步骤:步骤一、设定两棵随机树的起点和目标点:根据机械臂的起点确定随机树T1的起点xstart1和随机树T2的目标点xgoal2,根据机械臂的焊接终点确定随机树T2的起点xstar2和随机树T1的目标点xgoal1;
步骤二、双向生长生成随机树:两个起点xstart1和xstar2与其各自对应的目标点xgoal1和xgoal2进行双向生长,分别生成两棵随机树T1和T2;
步骤三、生成采样点xrand并对其进行目标偏置:在工作空间中生成随机点,根据预设的概率P,将随机点分别向目标点xgoal1或目标点xgoal2偏置,分别得出预设概率P下的采样点xrand;
所述步骤三具体为:
由于RRT*算法在三维复杂环境下路径搜索存在随机性大和搜索时间长的问题,因此采用双目标偏置策略如式(1)来进行随机采样: (1)
式中,rand为0~l问的随机数,p为设定的目标偏置阈值,size(map)为地图的大小;
当rand的值大于设定的目标偏置阈值时,将随机采样点作为采样点xrand;否则,将目标点xgoal1和xgoal2作为各自随机树的采样点xrand;
步骤四、寻找最近节点xnear:对于任一随机树T1或者T2,找到距离步骤三中每个采样点xrand最近的节点xnear,即与该采样点xrand距离最小的节点xnear;
步骤五、采用改进的人工势场函数为每个最近的节点xnear计算其所受的引力与斥力的合力Ft;
所述步骤五具体为:
目标点xgoal对最近节点xnear施加吸引力Fai,同时采样点xrand也对最近节点xnear施加吸引力Faj,在最近节点xnear周围的障碍物对其施加排斥力Fri,综合吸引力Fai和排斥力Faj,确定节点扩展的最终方向Ft:人工势场包括引力场函数Uatt如式(2)所示、斥力场函数Urep如式(3)所示,则合力势场函数如式(4)所示: (2)
(3)
(4)
其中,Ka为引力因子;Kr为斥力因子;d为机械臂与障碍物的距离;d0为障碍物的斥力势场范围; Pr为机械臂末端的位置坐标;Pg为目标点位置坐标;a为改进因子,d(Pr‑Pg)为机械臂末端与目标点的相对距离;
每个最近的节点xnear所受的引力与斥力的合力Ft为: ;
步骤六、根据计算得到的合力Ft方向,从最近的节点xnear向该方向以固定步长s进行扩展,生成新节点 xnew;
步骤七、对生成的新节点xnew进行碰撞检测,即判断新节点xnew与其父节点的连接路径是否与障碍物发生干涉,若发生碰撞则放弃当前扩展,返回步骤三重新进行采样和扩展;若未发生碰撞则优化新节点 xnew与最近节点xnear之间的路径长度,进入步骤八;
步骤八、设定最大迭代次数nmax,重复执行步骤三至步骤七,在每次迭代中检查两棵随机树T1和T2是否成功生长并完成二者之间的路径连接,若在最大迭代次数nmax内,两棵随机树T1和T2路径成功连接,即两棵随机树从各自起点xstart1和xstar2向各自目标点xgoal1和xgoal2生长过程中相遇并连接时,生成初步路径,停止采样;若未能在最大迭代次数nmax内连接两棵树,则返回步骤三。
2.根据权利要求1所述的船舶舱室焊接机械臂避障运动路径规划方法,其特征在于:所述步骤七中优化新节点 xnew与最近节点xnear之间的路径长度具体为:将新节点xnew以任一最近节点xnear作为父节点加入与最近节点xnear对应的随机树T1或T2中;再以新节点xnew为中心,半径r的范围内搜索随机树T1或T2中的节点,选择能够使从起点xstart1和xstar2分别到新节点xnew路径最短的节点xmin,并将新节点xnew的父节点更新为xmin,以优化路径。
3.根据权利要求1所述的船舶舱室焊接机械臂避障运动路径规划方法,其特征在于:还包括对初步路径长度和转折次数的优化,具体为:遍历步骤八中初步路径中除了起点xstart1、目标点xgoal1以外的中间节点,移除部分中间节点,检查移除中间节点后的初步路径是否与障碍物无碰撞,若无碰撞,则更新初步路径,若发生碰撞则恢复中间节点,选择其他中间节点进行移除,直至初步路径与障碍物无碰撞,完成初步路径长度和转折次数的优化,另一起点xstar2、目标点xgoal2的初步路径采用相同的优化方法。
4.根据权利要求1所述的船舶舱室焊接机械臂避障运动路径规划方法,其特征在于:还包括对初步路径直线性的优化,具体为:对除了起点xstart1、目标点xgoal1以外的不相邻的两个中间节点进行路径连接,检查该路径是否与障碍物无碰撞,若无碰撞,则移除不相邻的两个中间节点之间的节点,并更新初步路径,若发生碰撞则取消该不相邻的两个中间节点的路径连接,重新选择不相邻的中间节点进行路径连接,直至连接路径未与障碍物碰撞,完成初步路径直线性的优化,另一起点xstar2、目标点xgoal2的初步路径采用相同的优化方法。
5.根据权利要求1所述的船舶舱室焊接机械臂避障运动路径规划方法,其特征在于:还包括对初步路径平滑度的优化,具体为:初步路径平滑度预优化:
从初步路径的目标点xgoal1开始,与除了起点xstart1以外的中间节点进行逐一连接,若目标点xgoal1与该中间节点的连接路径未与障碍物碰撞,则移除目标点xgoal1与该中间节点之间的所有节点,并更新路径,若目标点xgoal1与该中间节点的连接路径发生碰撞,则目标点xgoal1与上一个不发生碰撞的中间节点连接,并且将该发生碰撞的中间节点作为新的目标点重复上述连接步骤,直至连接路径未与障碍物碰撞,完成该路径初步平滑度的优化,该优化方法与从初步路径的目标点xgoal2开始,与除了起点xstar2以外的中间节点进行逐一连接的初步路径优化方法相同;
然后使用B样条曲线对初步路径平滑度预优化后的路径平滑拟合,具体为:
;
式中:Pi为曲线的控制点,Fi,k(t)为k阶B样条基函数。