CN104516356A - 基于rrt的动态障碍规避算法 - Google Patents

Abstract

本发明公开了一种基于RRT的动态障碍规避算法，通过计算无人机的规避点，按照避撞前以规避点为目标点，避撞后以终点为目标点的避撞决策，即可生成规避路径。本发明规避算法能够充分利用原有航路信息，以人工势场法计算得到的避障点作为RRT的航路指引点，克服了RRT原有的随机性强的问题，其规划时间、航迹长度和安全程度等方面都得到了很大的提高。

Description

基于RRT的动态障碍规避算法

技术领域

本发明属于无人机空域动态障碍物规避技术领域，涉及一种基于RRT的动态障碍规避算法。

背景技术

无人机的成本低、易控制，因此近年来广泛应用于军事与民用领域，但是除了少量的高空长航时等特种用途无人机外，大量的军、民用无人机的飞行高度在5000米以下，中小型无人机的导航精度较差，基本不具备障碍规避能力。为了提高无人机的自主障碍规避能力，全球各无人机研究单位都付出了大量资金与科研力量。无人机路径规划算法是无人机自主避障的核心技术，近年来也得到了广泛关注。

按照面向对象的不同分类，无人机路径规划算法可以分为全局规划算法与局部规划算法。当预先具备完整精确的环境信息时，全局规划算法可一次性规划出一条自起点到终点的最优航迹，但是飞行过程若遇到突发威胁，全局规划算法的路径就无法满足避开威胁的要求，必须结合局部规划算法生成实时规避路径。局部路径规划算法要处理的突发威胁有两种：移动威胁与静态威胁。静态威胁指事先未探明的阻挡航路的地形、建筑物、禁飞区等固定目标，对于这些目标局部路径规划算法根据其与本机的位置信息即可生成规避路径。移动威胁指飞机、气球、飞鸟等在空中移动的目标，

传统的飞机避障系统如TCAS系统采用的方式是发现移动障碍物立即进行规避动作，爬升、下降或改速转向等，这样的方式虽然能够有效避开障碍物但是当障碍物在规避过程中机动时，飞机又要重新规划避障路径并执行，能量耗费较大。

发明内容

本发明的目的是提供一种基于RRT的动态障碍规避算法，解决了现有技术中存在的路径规划存在局部最小值以及航迹收敛速度慢的问题。

本发明所采用的技术方案是，基于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)；

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

进一步的，步骤一中计算引力场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时与传统的引力场的相同的。

进一步的，步骤二中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)。

进一步的，步骤三中计算斥力场U_rep(p，v)的公式如下：

其中，ρ₀是单个障碍物影响的最大距离范围，当无人机与障碍物的距离大于ρ₀时，斥力势力场对无人机的运动不再有影响；ρ_s(p，p_obs)为本机与目标机之间的距离；v_RO为目标与本机的相对距离；ρ_m(v_RO)表示无人机从v_RO下降到0所经过的距离，假设在时刻t，无人机向障碍物运动，无人机与障碍物之间的最小距离为ρ_s(p(t)，p_obs(t))，最大负向加速度幅值为a_max。

进一步的，步骤四中计算斥力F_rep1和斥力F_rep2的方法如下：

其中：F_rep(p，v)＝-grad(U_rep(p，v))，

$F_{\mathrm{rep}1}=\frac{-\mathrm{\η}}{{({\mathrm{\ρ}}_s(p,p_{\mathrm{obs}})-{\mathrm{\ρ}}_m\left(v_{\mathrm{RO}}\right))}^2}(1+\frac{v_{\mathrm{RO}}\left(t\right)}{2a_{\max }})n_{\mathrm{RO}}---\left(6\right),$

$F_{\mathrm{rep}2}=\frac{\mathrm{\η}{v}_{\mathrm{RO}}(\mathrm{\α}+v_{\mathrm{RO}\⊥})}{\mathrm{\ρ}(p,p_{\mathrm{obs}})a_{\max }{(\mathrm{\ρ}(p,p_{\mathrm{obs}})-{\mathrm{\ρ}}_m\left(v_{\mathrm{RO}}\right))}^2}{n}_{\mathrm{RO}\⊥}---\left(7\right),$

其中，v_RO(t)＝[v(t)-v_obs(t)]^Tn_RO，

v_RO是指向障碍物的方向向量方向，v_RO⊥是垂直于障碍物的方向向量，若v_RO(t)≤0，即无人机向远离障碍物的方向移动，无需碰撞规避；若v_RO(t)＞0即无人机在接近障碍物，则需要进行规避动作。

进一步的，规避路径的生成方法具体为：

以步骤六中得到的新航点P(x,y,z)为目标点X_goal，从当前起点X_init向目标点移动；

将X_init作为根结点，添加到航点集合T中；

在无人机的可行域X_free中取随机点X_rand；

找出航点集合T中的与X_rand最近的节点X_near，并用X_near来扩展；

根据公式(9)计算出临时节点

$x_{\mathrm{temp}}^k=\left\{\begin{array}{ccc}{x}_{\mathrm{near}}^k+L\·\frac{(x_{\mathrm{rand}}^k-x_{\mathrm{near}}^k)}{\left|\right|x_{\mathrm{rand}}^k-x_{\mathrm{near}}^k\left|\right|}& \mathrm{if}& x_{\mathrm{rand}}^k\∈\mathrm{\Φ}\left(x_{\mathrm{near}}^k\right)k\\ x_{\mathrm{near}}^k+\left|r_k\right|\·L\·[\cos (r_k\·{\mathrm{\η}}_{\max }),\sin (r_k\·{\mathrm{\η}}_{\max })]& \mathrm{if}& x_{\mathrm{rand}}^k\∉\mathrm{\Φ}\left(x_{\mathrm{near}}^k\right)\end{array}\right.---\left(9\right),$

如果临时节点在可行域X_free中，则临时节点做为RRT的叶子节点，即令新节点则添加新节点X_new到航点集合T中；否则，舍弃该临时节点重复寻找随机点，直到X_new＝X_goal或X_new∈Area(X_goal)，则确定了从X_init到X_goal的可行路径。

本发明的有益效果是，本发明规避算法能够充分利用原有航路信息，以人工势场法计算得到的避障点作为RRT的航路指引点，克服了RRT原有的随机性强的问题，其规划时间、航迹长度和安全程度等方面都得到了很大的提高。

具体实施方式

下面对本发明进行详细说明。

本发明提供了一种基于RRT的动态障碍规避算法，通过计算无人机的规避点，按照避撞前以规避点为目标点，避撞后以终点为目标点的避撞决策，即可生成规避路径；

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

步骤一、根据t时刻本机位置p_tar(t)，目标点位置p(t)以及本机速度v_tar(t)以及目标点速度v(t)，计算出无人机的引力场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时与传统的引力场的相同的。

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

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)。

步骤三、计算无人机的斥力场U_rep(p，v)；

其中，ρ₀是单个障碍物影响的最大距离范围，当无人机与障碍物的距离大于ρ₀时，斥力势力场对无人机的运动不再有影响；ρ_s(p，p_obs)为本机与目标机之间的距离；v_RO为目标与本机的相对距离；ρ_m(v_RO)表示无人机从v_RO下降到0所经过的距离，假设在时刻t，无人机向障碍物运动，无人机与障碍物之间的最小距离为ρ_s(p(t)，p_obs(t))，最大负向加速度幅值为α_max。

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

其中：F_rep(p，v)＝-grad(U_rep(p，v))，

$F_{\mathrm{rep}1}=\frac{-\mathrm{\η}}{{({\mathrm{\ρ}}_s(p,p_{\mathrm{obs}})-{\mathrm{\ρ}}_m\left(v_{\mathrm{RO}}\right))}^2}(1+\frac{v_{\mathrm{RO}}\left(t\right)}{2a_{\max }})n_{\mathrm{RO}}---\left(6\right),$

$F_{\mathrm{rep}2}=\frac{\mathrm{\η}{v}_{\mathrm{RO}}{v}_{\mathrm{RO}\⊥}}{{\mathrm{\ρ}}_s(p,p_{\mathrm{obs}})a_{\max }{({\mathrm{\ρ}}_s(p,p_{\mathrm{obs}})-{\mathrm{\ρ}}_m\left(v_{\mathrm{RO}}\right))}^2}{n}_{\mathrm{RO}\⊥}$

当F＝0时，无人机陷入局部最小值。为了解决该问题，首先考虑如何使F_rep2在目标、障碍物与本机处于同一直线时不为0。最简单的做法就是此时给F_rep2指定一个值，本质与设定中间目标点的方法是一样的。

具体方法是在F_rep2的公式中引入相对位置因子α，F_rep2的计算公式可以改为：

$F_{\mathrm{rep}2}=\frac{\mathrm{\η}{v}_{\mathrm{RO}}(\mathrm{\α}+v_{\mathrm{RO}\⊥})}{\mathrm{\ρ}(p,p_{\mathrm{obs}})a_{\max }{(\mathrm{\ρ}(p,p_{\mathrm{obs}})-{\mathrm{\ρ}}_m\left(v_{\mathrm{RO}}\right))}^2}{n}_{\mathrm{RO}\⊥}---\left(7\right),$

其中，v_RO(t)＝[v(t)-v_obs(t)]^Tn_RO，v_RO是指向障碍物的方向向量方向，v_RO⊥是垂直于障碍物的方向向量，若v_RO(t)≤0，即无人机向远离障碍物的方向移动，无需碰撞规避；若v_RO(t)＞0即无人机在接近障碍物，则需要进行规避动作；

α＝n_ROn_RT，n^RO为从本机指向障碍物的单位方向向量，n^RT为从本机指向目标的单位方向向量，随着两个方向之间角度的变化α的值域为[-1,1]，当本机指向障碍物的方向与本机指向目标的方向相同，即时，α＝1；互相垂直，即时，α＝0；方向相反，即时，α＝-1。将方向角度引入斥力的计算后，可以保证斥力F_rep2在本机、障碍物以及目标为直线的情况下不为0，克服局部极小值的问题，为本机规划出航线以外的避障点。

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

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

规避路径的生成方法具体为：

以步骤六中得到的新航点P(x,y,z)为目标点X_goal，从当前起点X_init向目标点移动；

将X_init作为根结点，添加到航点集合T中；

在无人机的可行域X_free中取随机点X_rand；

找出航点集合T中的与X_rand最近的节点X_near，并用X_near来扩展；

根据公式(9)计算出临时节点

$x_{\mathrm{temp}}^k=\left\{\begin{array}{ccc}{x}_{\mathrm{near}}^k+L\·\frac{(x_{\mathrm{rand}}^k-x_{\mathrm{near}}^k)}{\left|\right|x_{\mathrm{rand}}^k-x_{\mathrm{near}}^k\left|\right|}& \mathrm{if}& x_{\mathrm{rand}}^k\∈\mathrm{\Φ}\left(x_{\mathrm{near}}^k\right)k\\ x_{\mathrm{near}}^k+\left|r_k\right|\·L\·[\cos (r_k\·{\mathrm{\η}}_{\max }),\sin (r_k\·{\mathrm{\η}}_{\max })]& \mathrm{if}& x_{\mathrm{rand}}^k\∉\mathrm{\Φ}\left(x_{\mathrm{near}}^k\right)\end{array}\right.---\left(9\right),$

如果临时节点在可行域X_free中，则临时节点做为RRT的叶子节点，即令新节点则添加新节点X_new到航点集合T中；否则，舍弃该临时节点重复寻找随机点，直到X_new＝X_goal或X_new∈Area(X_goal)，则确定了从X_init到X_goal的可行路径。

Claims (6)

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)；

步骤六、根据步骤五中求出的合力的方向，移动目标点的位置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)。

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

其中，ρ₀是单个障碍物影响的最大距离范围，当无人机与障碍物的距离大于ρ₀时，斥力势力场对无人机的运动不再有影响；ρ_s(p，p_obs)为本机与目标机之间的距离；v_RO为目标与本机的相对距离；ρ_m(v_RO)表示无人机从v_RO下降到0所经过的距离，假设在时刻t，无人机向障碍物运动，无人机与障碍物之间的最小距离为ρ_s(p(t)，p_obs(t))，最大负向加速度幅值为a_max。

5.如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的步骤四中计算斥力F_rep1和斥力F_rep2的方法如下：

其中：F_rep(p，v)＝-grad(U_rep(p，v))，

$F_{\mathrm{rep}1}=\frac{-\mathrm{\η}}{{({\mathrm{\ρ}}_s(P\·\mathrm{Pobs})-\mathrm{\ρm}\left(v_{\mathrm{RO}}\right))}^2}(1+\frac{v_{\mathrm{RO}}\left(t\right)}{{2a}_{\max }})n_{\mathrm{RO}}---\left(6\right),$

$F_{\mathrm{rep}2}=\frac{{\mathrm{\ηv}}_{\mathrm{RO}}(\mathrm{\α}+v_{\mathrm{RO}\⊥})}{\mathrm{\ρ}(p,p_{\mathrm{obs}})a_{\max }{(\mathrm{\ρ}(p,p_{\mathrm{obs}})-{\mathrm{\ρ}}_m\left(v_{\mathrm{RO}}\right))}^2}{n}_{\mathrm{RO}\⊥}---\left(7\right),$

其中，v_RO(t)＝[v(t)-v_obs(t)]^Tn_RO，

v_RO是指向障碍物的方向向量方向，v_RO⊥是垂直于障碍物的方向向量，若v_RO(t)≤0，即无人机向远离障碍物的方向移动，无需碰撞规避；若v_RO(t)＞0即无人机在接近障碍物，则需要进行规避动作。

6.如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的规避路径的生成方法具体为：

以步骤六中得到的新航点P(x，y，z)为目标点X_goal，从当前起点X_init向目标点移动；

将X_init作为根结点，添加到航点集合T中；

在无人机的可行域X_free中取随机点X_rand；

找出航点集合T中的与X_rand最近的节点X_near，并用X_near来扩展；

根据公式(9)计算出临时节点

$x_{\mathrm{temp}}^k=\left\{\begin{array}{ccc}{x}_{\mathrm{near}}^k+L\·\frac{(x_{\mathrm{rand}}^k-x_{\mathrm{near}}^k)}{\left|\right|x_{\mathrm{rand}}^k-x_{\mathrm{near}}^k\left|\right|}& \mathrm{if}& x_{\mathrm{rand}}^k\∈\mathrm{\Φ}\left(x_{\mathrm{near}}^k\right)\\ x_{\mathrm{near}}^k+\left|r_k\right|\·|\·L\·[\cos (r_k\·{\mathrm{\η}}_{\max }),\sin (r_k\·{\mathrm{\η}}_{\max })& \mathrm{if}& x_{\mathrm{rand}}^k\∉\mathrm{\Φ}\left(x_{\mathrm{near}}^k\right)\end{array}\right.---\left(9\right),$

如果临时节点在可行域X_free中，则临时节点做为RRT的叶子节点，即令新节点则添加新节点X_new到航点集合T中；否则，舍弃该临时节点重复寻找随机点，直到X_new＝X_goal或X_new∈Area(X_goal)，则确定了从X_init到X_goal的可行路径。