技术摘要：
本发明属于三维空间中机器人路径规划以及生成技术领域，尤其涉及一种用于动态环境下的机械臂路径规划方法。包括路径规划阶段以及重新规划阶段：在路径规划阶段，根据当前的环境和目标点通过路径规划算法得到搜索树和路径点；根据上述信息用轨迹规划的方法对路径点进行  全部
背景技术：
在动态避障规划中，规划器需要根据更新后的环境重建或者修复路径。而动态环 境类似于静态环，通常将其视为真实世界更新与规划器系统交替进行。动态避障的必要过 程如下：  1、在静态的环境下进行机械臂的运动规划。2、开始机械臂运动。3、更新环境信息。 4、如果执行路径被监测到阻断，则规划器将重新规划或者修复当前路径以适应新的环境。 5、机械臂继续运动。在机械臂执行任务时，外界传感器(如摄像头)监测其工作环境中存在 运动的障碍物影响到机械臂执行路径时，如何快速地修正路径是机械臂在动态障碍物环境 下进行作业规划的关键。但受不同规划方案侧重点以及数据处理效率的不同，经常难以在 处理效率以及路径规划成功率上取得较好的平衡，特别是在动态避障问题上，传统方法不 仅耗时良久，且难以保证路径规划的有效性，限制了机械臂等机器人的发展。
技术实现要素：
本发明创造的目的在于，提供一种效率高于传统路径巡游方法，同时具有高有效 性，同时适用于动态环境下的机械臂路径规划方法。 为实现上述目的，本发明创造采用如下技术方案。 一种适用于动态环境下的机械臂路径规划方法，包括路径规划阶段以及重新规划 阶段： 路径规划阶段，是指根据当前的环境和目标点通过路径规划算法得到搜索树和路 径点；根据上述信息用轨迹规划的方法对路径点进行插值，获得初始规划路径，完成机械臂 运动控制；包括如下步骤： 步骤一、基于概率统计的运动不确定性建模来预测动态障碍物运动趋势： A1 .针对移动障碍物运动速度的大小和运动方向的不确定性，设每个移动障碍物 的运动速度和方向的取值范围分别表示为[vmin，vmax]和[θmin，θmax]，在时间周期ΔT内速度 和方向保持不变，在ΔT之后，二者可能会有改变，基于此，将障碍物速度的变化量和障碍物 运动角度的变化量分别用正态分布描述： 采用正态分布描述移动障碍物速度v的变化量和障碍物运动角度θ的变化量：对于 i≥0，有v(i 1)＝v(i) Δv；θ(i 1)＝θ(i) Δθ；其中 均服从 正态分布： A2.采用极大似然法构建移动障碍物Δv，Δθ的期望和方差；确定速度和方向保持 不变的间周期ΔT，统计ΔT时间周期内移动障碍物运动的速度和方向信息v(i)、θ(i)，(i＝ 4 CN 111546347 A 说　明　书 2/7 页 1、2......n)构建样本集；采用极大似然法构建Δv，Δθ的期望和方差 对Δ v有： 对Δθ有： 在重新规划阶段：根据传感器监测的情况来判断机械臂当前工况是否需要重新规 划路径；若当前工况被检测到阻断时，通过路径规划算法对初始路径进行修复，并将更新后 的路径重新进行轨迹插值，使得机械臂能够连续平滑运行； 步骤二、初始路径预处理，具体是指： B1按照初始规划的路径移动，当检测到初始路径被动态障碍物阻断时，删除掉被 障碍物影响的无效路径点，抛弃已经经过的路径点及其分支； 具体而言：机械臂按照初始规划的路径移动，当传感器检测到初始路径被动态障 碍物  (即黑色圆圈)阻断时，删除掉被障碍物影响的无效路径点，抛弃已经经过的路径点及 其分支，整理后，路径分为Vvalid和Vbackward，基于该步骤以减少查询次数和计算量； B2、基于前述步骤对机械臂参数进行计算,不断用新采集来的数据对样本集进行 更新, 以保证估计的准确性；当使用一定数量的数据取得所需的参数后；再利用3σ规则,即 对正态随机变量来说,它的取值落在区间[-3e,3e],并根据此规律以i＝1,2,...,n时刻的 移动障碍物的运动状态对i＝n 1时刻障碍物的速度,方向和位置的不确定性进行描述，持 续利用更新后的数据进行轨迹插值，得到新的路径规划。 对前述适用于动态环境下的机械臂路径规划方法的进一步改进，所述步骤B1还包 括： B1.1基于中继点的初始路径预处理 按照初始规划的路径移动，当检测到初始路径被动态障碍物阻断时，计算中继节 点的位置同时在更新后的场景中规划新路径，若到达中继点之前没有完成新路径规划，则 停止移动并等待新路径规划完成；否则机械臂将通过中继节点而不会暂停； 对于路径点V和中继节点qrelay，当前位置为qcurrent，qobstacle为障碍物边界与V相交 且距离最近的点，θ是动态障碍物和qcurrent的连线与动态障碍物速度方向vobstacle之间的夹 角，移动速度为vagent(vagent≤vmax)；当初始路径被中断时，动态障碍物的速度和运动角度的 变换分别定义为v′obstacle＝vobstacle Δv  andΔθ；找到障碍边界与路径相交的位置qobstacle， 中继节点的位置被定义为qrelay←qcurrent k，表示机械臂沿执行轨迹从当前位置前进k的距 离且 B1 .2当初始路径被动态障碍物阻断时，先计算出中继点的位置，在到达中继点时 重新判断障碍物与初始路径的关系，若检测到两者碰撞，则认为障碍物仍影响这初始路径， 利用Reconnect方法获取修复路径； 具体而言，判断在Vvaild和Vbackward所属的节点在拓展步长内，是否存在可以直接一 步连接Vvaild和Vbackward的节点且不会与障碍物发生碰撞，如果存在这样的节点，则将Vvaild的 和Vbackward的节点连接，以完成路径修复；当检测到路径被障碍物阻断时，生成中继节点并且 删除无效节点，最后连接以修复路径； 当Reconnect策略无法完成任务时，使用Regrow策略利用中继节点，不再考虑中继 5 CN 111546347 A 说　明　书 3/7 页 点之前的搜索树分支各节点；只查询中继点之后搜索树的末端到Vvalid代价最低的点作为新 的起始点，同时设置Vvalid为新目标，只要满足误差允许，则新的拓展树将扩展到Vvalid中的 任何点，进而修复路径方案； 对前述适用于动态环境下的机械臂路径规划方法的进一步改进，当使用Regrow方 法开始重新规划时，增加路径缓存的步骤，具体而言：随机选择路径缓存区中的关键点；使 用初始解决方案的信息来尽可能多地修复路径，定义Prand为在空间中随机采样的概率，则 Pgoal Pway Prand＝1，其中Pgoal表示向目标的拓展概率，Pway表示向Vvaild的拓展概率；在进行路 径缓存过程中，设置两个随机数p(p∈(0,1))和i(i＝1，2,...,n)；其中N是路径缓存区中的 点个数；若0