[发明专利]一种机械臂动态避障路径规划方法

权利要求书

1.一种机械臂动态避障路径规划方法，其特征在于，包括如下步骤：

步骤一、基于初始规划路径控制机械臂运动，利用传感器检测可能存在的动态障碍，当监测到下一路径点被动态障碍阻断时，删除路径规划搜索树中的失效点，抛弃已经经过的路径点及其分支，整理后得到位于失效点前侧的路径V_backward和位于失效点后侧的路径V_valid；基于搜索树查询V_backward和V_valid的路径点中是否存在相临近的点V_l和V_m，能够在检索范围内直接连接且不会与更新后的障碍发生碰撞，所述检索范围是指以V_l和V_backward中V_m两点的中点O_ε＝(V_l+V_m)/2为圆心，预设参数ε为半径的范围；如果存在这样的点，则将V_l和V_m连接，完成路径规划；若无法获得有效路径，则执行步骤二；

步骤二、基于进行节点生长以生成新的路径，根据动态障碍物和机械臂当前的运动位置和状态，生成一个位于机械臂和障碍物之间的无碰撞中继节点q_relay；查询q_relay到V_backward之间搜索树的最末端分支节点，并将这些节点根据父子关系的顺序设为N＝[N₁,N₂,......N_m]^T；为了从分支节点中确定一个生长起始节点q_start且使其生长到V_valid＝[V₁,V₂,......V_n]^T的代价更小，定义选择代价函数

其中

其中是指V₁和V_n两点的连线，ξ为障碍物安全系数；

d表示两点之间的距离，d(N_m,V₁)表示为N_m和V₁之间的距离，表示点线之间的距离；

计算各分支节点到V_valid＝[V₁,V₂,......V_n]^T的代价，选择代价函数值最小的点作为生长点q_start；

为了在V_valid＝[V₁,V₂,......V_n]^T中确定最佳的路径点以简化规划距离和时间，定义消耗代价函数选择在V_valid＝[V₁,V₂,......V_n]^T中消耗代价最小的点为路径修复的生长目标节点；

其中ρ(N_m,V_i)是指N_m,V_i和V_n的离散点曲率且ρ(N_m,V_n)＝1。

2.根据权利要求1所述机械臂动态避障路径规划方法，其特征在于，为便于确定动态障碍物与传感器运动关系，还包括用于构建运动不确定的动态障碍物运动模型的步骤，具体而言：采用正态分布描述障碍物的实时速度v(i)和障碍物运动角度θ(i)：建立第i+1个运动时间节点运动模型v(i+1)＝v(i)+Δv；θ(i+1)＝θ(i)+Δθ；

其中Δv为移动速度变化量；其中Δθ为移动方向变化量；

且均服从正态分布；

其中

μ_v,分别为Δv的期望和方差；μ_θ,分别为Δθ的期望和方差；

对Δv有

对Δθ有

不断采集数据更新样本集以保证模型的准确性，基于正态随机变量的3σ规则进行取值，建立运动障碍物的运动模型。