[发明专利]基于RRT的动态障碍规避算法

权利要求书

1.基于RRT的动态障碍规避算法，其特征在于，通过计算无人机的规避点，按照避撞前以规避点为目标点，避撞后以终点为目标点的避撞决策，即可生成规避路径；

其中，规避点的生成方法具体为：

步骤一、根据t时刻本机位置p_tar(t)，目标点位置p(t)以及本机速度v_tar(t)以及目标点速度v(t)，计算出无人机的引力场；

步骤二、再分别计算无人机指向目标单位的方向向量n_RT的应用力F_att1(p)和无人机相对于目标速度的单位方向n_VRT的方向力F_att2(v)；

步骤三、计算无人机的斥力场；

步骤四、计算无人机指向障碍物方向n_RO的斥力F_rep1，以及无人机垂直于障碍物方向n_RO⊥的斥力F_rep2；

步骤五、根据式(8)求出合力F，其中，F＝F_rep(p，v)+F_att(p，v) (8)；

其中，F_rep(p,v)是斥力的合力，F_att(p,v)是引力的合力；

步骤六、根据步骤五中求出的合力的方向，移动目标点的位置p(t)至指定的所述合力方向上，从而重新规划出新航点P(x,y,z)。

2.如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的步骤一中计算引力场U_att(p，v)的公式如下：

U_att(p，v)＝α_p||p_tar(t)-p(t)||^m+α_v||v_tar(t)-v(t)||ⁿ(1)，

其中，||p_tar(t)-p(t)||是欧式距离，||v_tar(t)-v(t)||是相对速度的幅值，α_p与α_v是尺度正参数，m与n是正常数；当α_v＝0，m＝2时与传统的引力场是相同的。

3.如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的步骤二中F_att1(p)和F_att2(v)分别根据下式(2)和(3)计算：

F_att1(p)＝mα_p||p_tar(t)-p(t)||^m-1n_RT(2)，

F_att2(v)＝nα_v||v_tar(t)-v(t)||^n-1n_VRT(3)，

当p_tar(t)≠p(t)且v_tar(t)≠v(t)时，F_att(p，v)＝F_att1(p)+F_att2(v)；

其中，||p_tar(t)-p(t)||是欧式距离，||V_tar(t)-V(t)||是相对速度的幅值，α_p与α_v是尺度正参数，m与n是正常数。