基于rrt的动态障碍规避算法
【专利摘要】本发明公开了一种基于RRT的动态障碍规避算法，通过计算无人机的规避点，按照避撞前以规避点为目标点，避撞后以终点为目标点的避撞决策，即可生成规避路径。本发明规避算法能够充分利用原有航路信息，以人工势场法计算得到的避障点作为RRT的航路指引点，克服了RRT原有的随机性强的问题，其规划时间、航迹长度和安全程度等方面都得到了很大的提高。
【专利说明】基于RRT的动态障碍规避算法
【技术领域】
[0001] 本发明属于无人机空域动态障碍物规避【技术领域】，涉及一种基于RRT的动态障碍 规避算法。
【背景技术】
[0002] 无人机的成本低、易控制，因此近年来广泛应用于军事与民用领域，但是除了少量 的高空长航时等特种用途无人机外，大量的军、民用无人机的飞行高度在5000米以下，中 小型无人机的导航精度较差，基本不具备障碍规避能力。为了提高无人机的自主障碍规避 能力，全球各无人机研宄单位都付出了大量资金与科研力量。无人机路径规划算法是无人 机自主避障的核心技术，近年来也得到了广泛关注。
[0003] 按照面向对象的不同分类，无人机路径规划算法可以分为全局规划算法与局部规 划算法。当预先具备完整精确的环境信息时，全局规划算法可一次性规划出一条自起点到 终点的最优航迹，但是飞行过程若遇到突发威胁，全局规划算法的路径就无法满足避开威 胁的要求，必须结合局部规划算法生成实时规避路径。局部路径规划算法要处理的突发威 胁有两种：移动威胁与静态威胁。静态威胁指事先未探明的阻挡航路的地形、建筑物、禁飞 区等固定目标，对于这些目标局部路径规划算法根据其与本机的位置信息即可生成规避路 径。移动威胁指飞机、气球、飞鸟等在空中移动的目标，
[0004] 传统的飞机避障系统如TCAS系统采用的方式是发现移动障碍物立即进行规避动 作，爬升、下降或改速转向等，这样的方式虽然能够有效避开障碍物但是当障碍物在规避过 程中机动时，飞机又要重新规划避障路径并执行，能量耗费较大。
【发明内容】
[0005] 本发明的目的是提供一种基于RRT的动态障碍规避算法，解决了现有技术中存在 的路径规划存在局部最小值以及航迹收敛速度慢的问题。
[0006] 本发明所采用的技术方案是，基于RRT的动态障碍规避算法，通过计算无人机的 规避点，按照避撞前以规避点为目标点，避撞后以终点为目标点的避撞决策，即可生成规避 路径；
[0007] 其中，规避点的生成方法具体为：
[0008] 步骤一、根据t时刻本机位置pta,⑴，目标点位置p(t)以及本机速度v ta,⑴以及 目标点速度v(t)，计算出无人机的引力场；
[0009] 步骤二、再分别计算无人机指向目标单位的方向向量nKT的应用力Fattl(P)和无人 机相对于速度的单位方向n VKT的方向力F att2 (V);
[0010] 步骤三、计算无人机的斥力场；
[0011] 步骤四、计算无人机指向障碍物方向1?的斥力Frepl，以及无人机垂直于障碍物方 向n KQ丄的斥力Frep2;
[0012] 步骤五、根据式（8)求出合力 F，其中，F = Frep(p，v)+Fatt(p，V) (8);
[0013] 步骤六、根据步骤五中求出的合力的方向，移动目标点的位置p(t)至指定的所述 合力方向上，从而重新规划出新航点P (X，y, z)。
[0014] 进一步的，步骤一中计算引力场Uatt (p，V)的公式如下：
[0015] Uatt (p,v) =QpIIPtar (t)-p(t)I Im+avI IVtar (t)-V(t)I In (1)，
[0016] 其中，I |ptm (t)-p(t) I I是欧式距离，I |vtm (t)-v (t) I I是相对速度的幅值，a ￡)与 ^^是尺度正参数，m与n是正常数；当a v=0，m=2时与传统的引力场的相同的。
[0017] 进一步的，步骤二中Fattl(P)和Fatt2(V)分别根据下式（2)和（3)计算：
[0018] Fattl(P)=mapI IPtar(t) -p (t)I Im-1nKT(2)，
[0019]Fatt2(v) = n a v I I vtar (t) -v (t) | | n_1nVET(3)，
[0020] 当 ptar (t)乒 p (t)且 vtar (t)乒 v (t)时，Fatt(p，v) =Fattl(p)+Fatt2(v)〇
[0021] 进一步的，步骤三中计算斥力场Urep (p，v)的公式如下：
[0022]
【权利要求】
1. 基于RRT的动态障碍规避算法，其特征在于，通过计算无人机的规避点，按照避撞前 以规避点为目标点，避撞后以终点为目标点的避撞决策，即可生成规避路径； 其中，规避点的生成方法具体为： 步骤一、根据t时刻本机位置pto(t)，目标点位置p(t)以及本机速度vtm(t)以及目标 点速度V (t)，计算出无人机的引力场； 步骤二、再分别计算无人机指向目标单位的方向向量ηκτ的应用力F attl (P)和无人机相 对于速度的单位方向ηνκτ的方向力Fatt2(V); 步骤三、计算无人机的斥力场； 步骤四、计算无人机指向障碍物方向1?的斥力Frepl，以及无人机垂直于障碍物方向 nK。丄的斥力Fre;p2; 步骤五、根据式（8)求出合力F，其中，F = Frep (p，V)+Fatt (p，V) (8); 步骤六、根据步骤五中求出的合力的方向，移动目标点的位置p(t)至指定的所述合力 方向上，从而重新规划出新航点P (X，y，z)。
2. 如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的步骤一中计 算引力场Uatt (p，v)的公式如下： Uatt (P，V) = α p I I Ptar ⑴-p (t) I I -+ a v I I Vtar ⑴-V (t) I In (1)， 其中，I |ptm(t)-p(t) I I是欧式距离，I |vtm(t)_v(t) I I是相对速度的幅值，％与α ν 是尺度正参数，m与η是正常数；当α ν= 0, m = 2时与传统的引力场的相同的。
3. 如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的步骤二中 Fattl(P)和Fatt2 (ν)分别根据下式⑵和⑶计算： Fattl (P) = m a p I I ptar ⑴-p (t) I I "]% (2)， Fatt2 (ν) = η a v I I Vtar ⑴-ν (t) I I η-1ηνκτ (3)， 当 Ptar (t)乒 ρ (t)且 vtar (t)乒 ν (t)时，Fatt (ρ，ν) = Fattl (ρ) +Fatt2 (ν) 〇
4. 如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的步骤三中计 算斥力场U""(p，v)的公式如下：
其中，P C1是单个障碍物影响的最大距离范围，当无人机与障碍物的距离大于P J寸，斥 力势力场对无人机的运动不再有影响；PS(P，PdJ为本机与目标机之间的距离；Vffij为目标 与本机的相对距离；P m(Vffi))表示无人机从Vffij下降到〇所经过的距离，假设在时刻t，无人 机向障碍物运动，无人机与障碍物之间的最小距离为P s(p(t)，Ptjbs (t))，最大负向加速度 幅值为a_。
5. 如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的步骤四中计 算斥力Frepl和斥力F 的方法如下：
9 其中：FreD(p，v) = -grad(UreD(p，v))，
其中，vK〇(t) = [v(t)-vobs(t)]TnK0，
是指向障碍物的方向向量方向，v I是垂直于障碍物的方向 向量，若vKQ(t) <0,即无人机向远离障碍物的方向移动，无需碰撞规避；若vKQ(t) >0即无 人机在接近障碍物，则需要进行规避动作。
6.如权利要求1所述的基于RRT的动态障碍规避算法，其特征在于，所述的规避路径的 生成方法具体为： 以步骤六中得到的新航点P(x，y，z)为目标点XgMl，从当前起点\_向目标点移动； 将Xinit作为根结点，添加到航点集合T中； 在无人机的可行域中取随机点Xrand; 找出航点集合T中的与Xrand最近的节点Xmm，并用扩展； 根据公式（9)计算出临时节点，
如果临时节点在可行域Xfl^中，则临时节点做为RRT的叶子节点，即令新节 点人M ，贝丨獅口'新如撤点、齡T A ;测，舖劍;難射纖 机点，直到Xnew= X ^或X newe Area(X gMl)，则确定了从xinijlj X gMl的可行路径。
【文档编号】G05B13/04GK104516356SQ201510009379
【公开日】2015年4月15日 申请日期:2015年1月8日 优先权日:2015年1月8日
【发明者】赵春晖, 王晓华, 朱海锋, 梁彦, 吕洋, 潘泉, 程承 申请人:西北工业大学