[发明专利]一种适用于动态环境下的机械臂路径规划方法

权利要求书

1.一种适用于动态环境下的机械臂路径规划方法，其特征在于，包括路径规划阶段以及重新规划阶段：

路径规划阶段，是指根据当前的环境和目标点通过路径规划算法得到搜索树和路径点；根据搜索树和路径点用轨迹规划的方法对路径点进行插值，获得初始规划轨迹，完成机械臂运动控制；包括如下步骤：

步骤一、基于概率统计的运动不确定性建模来预测动态障碍物运动趋势：

A1.针对移动障碍物运动速度的大小和运动方向的不确定性，设每个移动障碍物的运动速度和方向的取值范围分别表示为[v_min，v_max]和[θ_min，θ_max]，在时间周期ΔT内速度和方向保持不变，在ΔT之后，二者可能会有改变，基于此，将障碍物速度的变化量和障碍物运动角度的变化量分别用正态分布描述：

采用正态分布描述移动障碍物速度v的变化量和障碍物运动角度θ的变化量：对于i≥0，有v(i+1)＝v(i)+Δv；θ(i+1)＝θ(i)+Δθ；其中均服从正态分布：

A2.采用极大似然法构建移动障碍物Δv，Δθ的期望和方差；确定速度和方向保持不变的间周期ΔT，统计ΔT时间周期内移动障碍物运动的速度和方向信息v(i)、θ(i)，(i＝1、2......n)构建样本集，i是指ΔT时间周期内的不同时刻；采用极大似然法构建Δv，Δθ的期望和方差对Δv有：对Δθ有：

在重新规划阶段：根据传感器监测的情况来判断机械臂当前工况是否需要重新规划路径；若当前工况被检测到阻断时，通过路径规划算法对初始路径进行修复，并将更新后的路径重新进行轨迹插值，使得机械臂能够连续平滑运行；

步骤二、初始路径预处理，具体是指：

B1、按照初始规划的轨迹移动，当检测到初始路径被动态障碍物阻断时，删除掉被障碍物影响的无效路径点，抛弃已经经过的路径点及其分支；

具体而言：机械臂按照初始规划的路径移动，当传感器检测到初始路径被动态障碍物阻断时，删除掉被障碍物影响的无效路径点，抛弃已经经过的路径点及其分支，整理后，路径分为V_valid和V_backward，基于该步骤以减少查询次数和计算量；

B2、基于前述步骤对机械臂参数进行计算,不断用新采集来的数据对样本集进行更新,以保证估计的准确性；当使用一定数量的数据取得所需的参数后；再利用3σ规则,即对正态随机变量来说,它的取值落在区间[-3e,3e],并根据此规律以i＝1,2,...,n时刻的移动障碍物的运动状态对i＝n+1时刻障碍物的速度,方向和位置的不确定性进行描述，持续利用更新后的数据进行轨迹插值，得到新的轨迹。