[发明专利]一种机械臂在线避障运动规划方法

摘要

本发明提供一种机械臂在线避障运动规划方法，涉及机械臂运动规划技术领域。该方法对机械臂和障碍物进行建模，通过逆解确定目标节点；预规划阶段，在关节空间内进行采样生成新节点，对新节点进行碰撞检测确定其是否添加进搜索树中，将搜索树中使得新节点成本最小的节点作为其父节点；从搜索树中规划出的前k个节点并送给轨迹规划器，直至搜索到目标节点进入重规划；重规划阶段，在路径节点附近进行采样以优化路径，当障碍物移动与路径冲突时则重新规划路径，同时将规划节点送给轨迹规划器，直至机械臂运动到目标点。本发明方法路径规划与机械臂运动同步进行，实现了在线避障运动，即使目标节点位置发生变化，也能够重新采样连接路径到达目标点。

主权项

1.一种机械臂在线避障运动规划方法，其特征在于：包括以下步骤：步骤1：利用D‑H法对六自由度机械臂进行建模，对障碍物使用轴向包围盒和球形包围盒技术建立模型，给出笛卡尔空间的起始点和目标点坐标；步骤2：初始化机械臂的工作环境和参数，确定障碍物位置，逆解确定关节空间中的机械臂运动路径的起始点q_init和目标点q_goal，确定扩展步长r，邻域半径R，并初始化仅包含起始节点的搜索树和最大迭代次数maxAttempts，进入预规划阶段；步骤3：预规划阶段，在关节空间内以概率α向目标点采样，以概率1‑α在自由空间内随机采样，得到新采样点q_rand，对搜索树中节点遍历求得距离q_rand最近的节点q_nearest，从q_nearest向q_rand以扩展步长r扩展得到新节点q_new；步骤4：计算距离新节点q_new小于R的邻域集合Q_near，在Q_near内找出节点成本与该节点到q_new距离之和最小的节点作为q_new的父节点q_parent；步骤5：对新节点q_new与其父节点q_parent形成的路径做碰撞检测，如果发生碰撞，则返回步骤3重新采样；否则将新节点q_new与其父节点q_parent形成的路径添加进搜索树中，并对邻域集合Q_near中每个节点进行遍历，如果邻域集合Q_near中节点q_near的成本小于新节点q_new的成本与两者形成路径距离之和，则用q_new代替q_near原来的父节点，并更新其对应的成本；步骤6：判断新节点q_new与目标节点之间的距离q_goal小于扩展步长r是否成立，如果成立并且q_goal与q_new形成的路径没有发生碰撞，则说明已经找到可到达目标点的路径，将q_goal添加进搜索树中并转到步骤8进入重规划阶段，否则执行步骤7；步骤7：判断搜索树中的节点数是否大于等于根据起点到目标点的距离及扩展步长确定的机械臂运动路径点数k，如果是，则在建立好的搜索树中规划出k个节点，形成机械臂运动的路径点序列，并将路径点序列送到轨迹规划器中控制机械臂运动，循环执行步骤3至步骤7直到跳出循环进入重规划阶段或达到最大迭代次数中止，否则，重新执行步骤3；步骤8：重规划阶段，更新目标点q_goal、机械臂位置q_a和障碍物位置；步骤9：在搜索树中从q_goal反向检索得到通向目标节点的路径，在此路径上选取一个节点作为信标q_beacon，以概率α在以R为半径，q_beacon为圆心的超椭球内随机采样，以概率1‑α在自由空间内随机采样，按照步骤3‑5的方法生成新节点并连接，规划出新路径；步骤10：按照三角形定理“两边之和大于第三边”去掉新路径中的中间点使得整条路径的总成本更小，对整条路径重新布线以优化路径；步骤11：在建立好的搜索树中重新规划出k个节点，形成机械臂运动的新路径点序列，当机械臂q_a运动到新路径的起始节点q₀时，将k个节点序列送到轨迹规划器中控制机械臂运动，并使用新路径的最后一个节点更新起始节点，即q₀＝q_k，循环执行步骤8至步骤11直到机械臂q_a运动到目标节点q_goal。