[發明專利]一種威脅環境下基于導引速度場的航路規劃方法

在審

• 申請號: 201410639837.0
• 申請日: 2014-11-13
• 公開（公告）號: CN104406588A
• 公開（公告）日: 2015-03-11
• 發明（設計）人: 梁宵;孟光磊;田豐;陳國棟
• 申請（專利權）人: 沈陽航空航天大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/00;G06F19/00
• 代理公司: 沈陽火炬專利事務所(普通合伙) 21228
• 代理人: 王欣

1.一種威脅環境下基于導引速度場的航路規劃方法；其特征在于：其首先針對威脅進行預處理將威脅信息轉化為靜態障礙，之后計算出牽引速度、規避速度與導引速度，利用疊加原理對速度求和，最后根據仿真步長確定飛行航路；具體步驟如下：

步驟一：對威脅信息進行預處理，將威脅等效計算為特定高度的圓形靜態障礙；

步驟二：計算目標對智能體的牽引速度；

智能體的航路規劃任務在矢量場中表現為到達目標點G(x_t,y_t)，目標是牽引場場源，牽引場作用在規劃點上產生牽引速度V_T；目標點提供的牽引場作用區域為全局，大小為定值，方向由規劃點指向目標點：

V→T=w·r→t]]>

其中表示牽引速度，w表示牽引場權重，表示目標長單位方向矢量；在二維空間中，的表達式如下：

rtx=xt-xc(xt-xc)2+(yt-yc)2rty=yt-yc(xt-xc)2+(yt-yc)2]]>

其中(x_c,y_c)表示當前規劃點坐標，(x_t,y_t)表示目標點坐標，r_tx表示牽引場單位方向矢量沿x軸的分量，r_ty表示牽引場單位方向矢量沿y軸的分量；

步驟三：計算障礙對智能體的規避速度；

規避場作用在規劃點上產生規避速度V_P，導引場作用在規劃點上產生導引速度V_G，設規避速度如下：

V→Pi=0di>ri+ΔRwp(1+((di-ri)/L)2)·r→piri≤di≤ri+ΔRwp(di/ri)2·r→pidi<r]]>

其中表示第i個障礙對規劃點產生的規避速度，w_p表示規避場的權重系數，L表示規避場的作用系數，d_i表示第i個障礙中心與規劃點之間的距離，r_i表示第i個障礙區域的半徑，ΔR表示障礙區域規避場作用的范圍，表示規避場單位方向矢量；

在障礙區域場強作用邊緣令其場強大小為導引場場強的α倍，在障礙區域邊緣令其場強大小為導引場的β倍，設權重與威脅場作用系數的表達式：

wp=β·w,L=ΔRα/(β-1)]]>

參數β代表在障礙邊緣時，規劃點所受規避場場強與導引場場強的比值，β＞1，當β越大時，在規劃過程中，規劃點運動的軌跡距離威脅邊緣越遠；

參數α代表在障礙作用區域邊緣時，規劃點所受規避場場強與導引場場強的比值，α＞1，α值一般需要比較小，規劃點在進入規避場作用前后，場強變化平緩，從而使航路比較平滑；

步驟四：導引場與規避場同時產生，根據規避速度計算導引速度；

假定目標點的位置為p_t，規劃點當前位置為p_c，則牽引速度規避速度和導引速度導引速度為：

V→g=ϵ|V→p|·r→g]]>

其中ε表示導引場與規避場大小的比值，這里取ε＝1，表示導引場大小與規避場相等，表示導引場單位方向矢量；

導引速度方向垂直規避速度，導引速度的方向是由目標點出發，垂直于規避速度并指向目標點的一側；導引場的判斷方法如下：

r→g=V→g=R(π2)·r→pθ(V→g′,V→t)≤π2V→g′=R(-π2)·r→pθ(V→g,V→t)>π2]]>

其中R(φ)=cosφ-sinφsinφcosφ]]>代表旋轉矩陣，將單位矢量左乘該旋轉矩陣，即可得到順時針旋轉角度φ后的單位矢量；表示向量與之間的夾角；規避速度經過旋轉±π/2后得到垂直于規避速度的導引速度矢量和選擇指向目標點一側的導引場，用γ表示與的夾角，γ′表示與的夾角，可知γ+γ′＝π；因此，對的方向選取也可以通過以下公式進行判斷：

r→g=V→gsgn(V→g·V→t)=1V→g′sgn(V→g′·V→t)=1]]>

其中表示量向量的點乘，當兩向量的夾角為鈍角時，兩向量點乘結果小于0；sgn(m)表示取數值m的符號，計算公式為：

sgn(m)=-1m<00m=01m>0]]>

步驟五：對牽引速度、規避速度和導引速度進行矢量求和，并計算下一時刻航路點；

首先對牽引速度、規避速度和導引速度進行矢量求和，得到總的規劃速度之后，對規劃速度進行積分得到規劃航路；

步驟六：重復步驟二～步驟五，計算得到一系列航路點，完成航路規劃。