[发明专利]基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法

权利要求书

1.基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，其特征在于，包括以下步骤：

步骤S1：初始化工作空间环境，获取障碍物信息并确定冗余机械臂移动物品的起始位姿和目标位姿；

步骤S2：选取臂型角，通过逆运动学解算出冗余机械臂在所述起始位姿和目标位姿的关节变量；

步骤S3：利用MIRRT*-Connect算法在冗余机械臂自由构型空间内规划出一条从初始位姿运动到目标位姿的无碰撞路径；

步骤S4：通过迭代运算，对步骤S3所得路径进行平滑处理；

步骤S5：考虑冗余机械臂运动过程中与动态障碍物产生空间干涉，更新局部路径或改变臂型角避免碰撞；

步骤S6：利用上位机对冗余机械臂进行控制，使其按照最新路径进行运动；

所述基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法，对InformedRRT*算法引入动态采样域概率偏置来降低扩展树生长的盲目性和双向扩展、自适应步长和提高全局路径规划效率；通过最佳最近点、重选父节点及布线和迭代运算优化路径；利用零空间自运动和局部路径规划有效避免动态障碍物，保证机械臂运动过程的安全性；

所述动态采样域概率偏置具体可表述为：

其中，和分别为扩展树T_s和T_g的随机采样点和的均匀采样空间，P_sample∈[0,1]为随机数，α为动态采样域概率阈值，Θ为冗余机械臂构型空间，Θ(θ_goal,R_goal)和Θ(θ_start,R_start)分别是以中心θ_goal和θ_start，半径为R_goal和R_start的超球域，这里R_goal和R_start可分别表示为：

上式中，γ为冗余机械臂构型空间边界系数，N₁是能够作为扩展树有效采样点数量，N₂是采样到障碍物区域被舍弃的点的数量，β为决定半径大小变化快慢的系数；

所述重选父节点范围半径可表示为：

其中，m为冗余机械臂构型空间维度，Θ_ball为m维单位球域，ξ(·)代表高维空间的体积，为当前两扩展树上节点集，N(Θ^T)为其节点数量；

当首次找到解决方案时，采样范围将缩小到超椭球域，同时对无法构成更小成本路径的节点进行剪枝处理，所述半径会被进一步更新为：

其中Θ_ellipse为超椭球域；

在当前扩展树上距离采样点或上次扩展树新节点的所述最佳最近点可根据下式得到：

式中，为当前扩展树，θ_target为采样点或者上次扩展树的新节点，w_d为距离比例系数，w_c为混合比例系数，N_obs(Θ(θ,2R^*))为以当前扩展树上节点θ为中心，2R^*为半径的超球域内障碍物的数量；

在首次找到解决方案后，采样域进一步更新为超椭球域，其满足θ_ellipse～μ(Θ_ellipse)，经变换可进一步表述为：

θ_ellipse＝ZEθ_ball+θ_center

其中，θ_center＝(θ_start+θ_goal)/2为超椭球域中心点，θ_ball＝{θ∈Θ_ball|||θ||₂≤1}为在单位超球域的点，变换矩阵保证在超椭球域内均匀采样，可表示为：

其中，diag{·}为对角矩阵，c_best为当前解决方案的路径成本，超椭球以θ_start和θ_goal作为焦点，进一步有c_min＝||θ_start-θ_goal||₂；

另外，旋转矩阵可表示为：

Z＝Xdiag{1,…,1,|X|·|Y|}Y^T

其中，|·|代表矩阵行列式，通过奇异值分解是XΣY^T≡M的酉矩阵，其中M可表示为：

其中为单位矩阵的第一列；

以上即通过在单位超球域内均匀采样在经过变换、旋转和平移，可以得到超椭球域内的均匀采样点进一步描述为：

步骤S3中所述利用MIRRT*-Connect算法进行路径规划过程具体包括：

步骤S31、初始化扩展树T_s和T_g、树节点线集和树节点集和起始点θ_s、终止点θ_g、路径规划解决方案P_soln，路径最小成本c_best及相关参数；

步骤S32、根据c_best是否为初始值来判断选取采样方式，并通过采样函数获取当前扩展树T_s的采样点并检测是否通过碰撞检测，如果是则跳转到步骤S33，否则重新采样；

步骤S33、所述最佳最近点的搜寻，遍历树节点集获取采样点的最佳最近点

步骤S34、沿到指定方向进行扩展的到新节点对最佳最近点和新节点构成路径进行碰撞检测，如果未通过检测则跳转到步骤S38，否则执行步骤S35；

步骤S35、遍历以新节点为中心，R^*为半径的附近点集Θ_near，获取经过附近点集Θ_near中的节点θ_near从当前扩展树T_s根节点θ_start到新节点的路径成本，得到最小路径成本附近点集Θ_near中的节点θ_near作为父节点，并将节点θ_near和新节点连线添加到树节点线集中；

步骤S36、获取经过新节点从当前扩展树T_s根节点θ_start到附近点集Θ_near中的节点θ_near的路径成本，如果小于从当前扩展树T_s根节点θ_start到节点θ_near的原路径成本，则将新节点作为节点θ_near的新父节点，并在树节点线集中删除节点θ_near和原父节点的连线，添加新节点和节点θ_near的连线；

步骤S37、以当前扩展树T_s的新节点为生长方向对另一扩展树T_g进行扩展，并获取最佳最近点并进行碰撞检测，未通过碰撞检测则跳转到步骤S38，否则重选父节点、重布线、并判断新节点是否为新节点如果不是则继续扩展，直到扩展未通过碰撞检测，执行步骤S38，否则连接两树；

步骤S38、交换扩展树T_s和T_g；

步骤S39、判断新节点的加入是否找到路径规划解决方案P_soln，如果是更新当前路径规划解决方案P_soln，否则执行步骤S310；

步骤S310、利用c_best覆盖过时最小路径成本并计算当前路径规划解决方案P_soln的路径成本并覆盖c_best，判断c_best是否小于如果成立则对无法构成更小成本路径的节点进行剪枝处理，否则执行步骤S311；

步骤S311、判断是否达到循环结束条件，如果是则结束循环，返回扩展树T_s和T_g以及路径规划解决方案P_soln，否则跳转至步骤S32。