基于rrt的動態(tài)障礙規(guī)避算法
【專利摘要】本發(fā)明公開了一種基于RRT的動態(tài)障礙規(guī)避算法，通過計算無人機的規(guī)避點，按照避撞前以規(guī)避點為目標點，避撞后以終點為目標點的避撞決策，即可生成規(guī)避路徑。本發(fā)明規(guī)避算法能夠充分利用原有航路信息，以人工勢場法計算得到的避障點作為RRT的航路指引點，克服了RRT原有的隨機性強的問題，其規(guī)劃時間、航跡長度和安全程度等方面都得到了很大的提高。
【專利說明】基于RRT的動態(tài)障礙規(guī)避算法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于無人機空域動態(tài)障礙物規(guī)避【技術(shù)領(lǐng)域】，涉及一種基于RRT的動態(tài)障礙 規(guī)避算法。

【背景技術(shù)】
[0002] 無人機的成本低、易控制，因此近年來廣泛應用于軍事與民用領(lǐng)域，但是除了少量 的高空長航時等特種用途無人機外，大量的軍、民用無人機的飛行高度在5000米以下，中 小型無人機的導航精度較差，基本不具備障礙規(guī)避能力。為了提高無人機的自主障礙規(guī)避 能力，全球各無人機研宄單位都付出了大量資金與科研力量。無人機路徑規(guī)劃算法是無人 機自主避障的核心技術(shù)，近年來也得到了廣泛關(guān)注。
[0003] 按照面向?qū)ο蟮牟煌诸?，無人機路徑規(guī)劃算法可以分為全局規(guī)劃算法與局部規(guī) 劃算法。當預先具備完整精確的環(huán)境信息時，全局規(guī)劃算法可一次性規(guī)劃出一條自起點到 終點的最優(yōu)航跡，但是飛行過程若遇到突發(fā)威脅，全局規(guī)劃算法的路徑就無法滿足避開威 脅的要求，必須結(jié)合局部規(guī)劃算法生成實時規(guī)避路徑。局部路徑規(guī)劃算法要處理的突發(fā)威 脅有兩種：移動威脅與靜態(tài)威脅。靜態(tài)威脅指事先未探明的阻擋航路的地形、建筑物、禁飛 區(qū)等固定目標，對于這些目標局部路徑規(guī)劃算法根據(jù)其與本機的位置信息即可生成規(guī)避路 徑。移動威脅指飛機、氣球、飛鳥等在空中移動的目標，
[0004] 傳統(tǒng)的飛機避障系統(tǒng)如TCAS系統(tǒng)采用的方式是發(fā)現(xiàn)移動障礙物立即進行規(guī)避動 作，爬升、下降或改速轉(zhuǎn)向等，這樣的方式雖然能夠有效避開障礙物但是當障礙物在規(guī)避過 程中機動時，飛機又要重新規(guī)劃避障路徑并執(zhí)行，能量耗費較大。


【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的是提供一種基于RRT的動態(tài)障礙規(guī)避算法，解決了現(xiàn)有技術(shù)中存在 的路徑規(guī)劃存在局部最小值以及航跡收斂速度慢的問題。
[0006] 本發(fā)明所采用的技術(shù)方案是，基于RRT的動態(tài)障礙規(guī)避算法，通過計算無人機的 規(guī)避點，按照避撞前以規(guī)避點為目標點，避撞后以終點為目標點的避撞決策，即可生成規(guī)避 路徑；
[0007] 其中，規(guī)避點的生成方法具體為：
[0008] 步驟一、根據(jù)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] 步驟五、根據(jù)式（8)求出合力 F，其中，F(xiàn) = Frep(p，v)+Fatt(p，V) (8);
[0013] 步驟六、根據(jù)步驟五中求出的合力的方向，移動目標點的位置p(t)至指定的所述 合力方向上，從而重新規(guī)劃出新航點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 ￡)與 ^^是尺度正參數(shù)，m與n是正常數(shù)；當a v=0，m=2時與傳統(tǒng)的引力場的相同的。
[0017] 進一步的，步驟二中Fattl(P)和Fatt2(V)分別根據(jù)下式（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)時，F(xiàn)att(p，v) =Fattl(p)+Fatt2(v)〇
[0021] 進一步的，步驟三中計算斥力場Urep (p，v)的公式如下：
[0022]

【權(quán)利要求】
1. 基于RRT的動態(tài)障礙規(guī)避算法，其特征在于，通過計算無人機的規(guī)避點，按照避撞前 以規(guī)避點為目標點，避撞后以終點為目標點的避撞決策，即可生成規(guī)避路徑； 其中，規(guī)避點的生成方法具體為： 步驟一、根據(jù)t時刻本機位置pto(t)，目標點位置p(t)以及本機速度vtm(t)以及目標 點速度V (t)，計算出無人機的引力場； 步驟二、再分別計算無人機指向目標單位的方向向量ηκτ的應用力F attl (P)和無人機相 對于速度的單位方向ηνκτ的方向力Fatt2(V); 步驟三、計算無人機的斥力場； 步驟四、計算無人機指向障礙物方向1?的斥力Frepl，以及無人機垂直于障礙物方向 nK。丄的斥力Fre;p2; 步驟五、根據(jù)式（8)求出合力F，其中，F(xiàn) = Frep (p，V)+Fatt (p，V) (8); 步驟六、根據(jù)步驟五中求出的合力的方向，移動目標點的位置p(t)至指定的所述合力 方向上，從而重新規(guī)劃出新航點P (X，y，z)。
2. 如權(quán)利要求1所述的基于RRT的動態(tài)障礙規(guī)避算法，其特征在于，所述的步驟一中計 算引力場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是相對速度的幅值，％與α ν 是尺度正參數(shù)，m與η是正常數(shù)；當α ν= 0, m = 2時與傳統(tǒng)的引力場的相同的。
3. 如權(quán)利要求1所述的基于RRT的動態(tài)障礙規(guī)避算法，其特征在于，所述的步驟二中 Fattl(P)和Fatt2 (ν)分別根據(jù)下式⑵和⑶計算： 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)時，F(xiàn)att (ρ，ν) = Fattl (ρ) +Fatt2 (ν) 〇
4. 如權(quán)利要求1所述的基于RRT的動態(tài)障礙規(guī)避算法，其特征在于，所述的步驟三中計 算斥力場U""(p，v)的公式如下：
其中，P C1是單個障礙物影響的最大距離范圍，當無人機與障礙物的距離大于P J寸，斥 力勢力場對無人機的運動不再有影響；PS(P，PdJ為本機與目標機之間的距離；Vffij為目標 與本機的相對距離；P m(Vffi))表示無人機從Vffij下降到〇所經(jīng)過的距離，假設在時刻t，無人 機向障礙物運動，無人機與障礙物之間的最小距離為P s(p(t)，Ptjbs (t))，最大負向加速度 幅值為a_。
5. 如權(quán)利要求1所述的基于RRT的動態(tài)障礙規(guī)避算法，其特征在于，所述的步驟四中計 算斥力Frepl和斥力F 的方法如下：
9 其中：FreD(p，v) = -grad(UreD(p，v))，
其中，vK〇(t) = [v(t)-vobs(t)]TnK0，
是指向障礙物的方向向量方向，v I是垂直于障礙物的方向 向量，若vKQ(t) <0,即無人機向遠離障礙物的方向移動，無需碰撞規(guī)避；若vKQ(t) >0即無 人機在接近障礙物，則需要進行規(guī)避動作。
6.如權(quán)利要求1所述的基于RRT的動態(tài)障礙規(guī)避算法，其特征在于，所述的規(guī)避路徑的 生成方法具體為： 以步驟六中得到的新航點P(x，y，z)為目標點XgMl，從當前起點\_向目標點移動； 將Xinit作為根結(jié)點，添加到航點集合T中； 在無人機的可行域中取隨機點Xrand; 找出航點集合T中的與Xrand最近的節(jié)點Xmm，并用擴展； 根據(jù)公式（9)計算出臨時節(jié)點，
如果臨時節(jié)點在可行域Xfl^中，則臨時節(jié)點做為RRT的葉子節(jié)點，即令新節(jié) 點人M ，貝丨獅口'新如撤點、齡T A ;測，舖劍;難射纖 機點，直到Xnew= X ^或X newe Area(X gMl)，則確定了從xinijlj X gMl的可行路徑。
【文檔編號】G05B13/04GK104516356SQ201510009379
【公開日】2015年4月15日 申請日期:2015年1月8日 優(yōu)先權(quán)日:2015年1月8日
【發(fā)明者】趙春暉, 王曉華, 朱海鋒, 梁彥, 呂洋, 潘泉, 程承 申請人:西北工業(yè)大學