一種基于虛擬膨化的運動障礙與uuv同向航行的規(guī)避方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及基于虛擬膨化的運動障礙與UUV同向航行的規(guī)避方法。
【背景技術(shù)】
[0002] 近年來，關(guān)于運動障礙規(guī)避策略的成果不計其數(shù)，總結(jié)起來大致分為3大類，第一 類是先預(yù)測運動障礙的運動趨勢，然后根據(jù)不同的趨勢作出相應(yīng)的對策;第二類是在動態(tài) 規(guī)避的瞬間，將運動障礙固化成靜態(tài)障礙規(guī)避，只要作出動態(tài)規(guī)避的頻率較快，算法就能不 斷更新規(guī)劃路線，直到避過運動障礙;第三類是采用學習算法，通過訓練使UUV具有智能避 障的能力；
[0003] 基于趨勢預(yù)測的策略能較好地規(guī)避運動狀態(tài)相對平穩(wěn)的運動障礙，但對于運動速 度或方向變化頻繁的運動障礙(或目標)則難以準確預(yù)測。將運動障礙固化成靜態(tài)障礙的策 略，沒有考慮運動趨勢，導(dǎo)致規(guī)劃出的航路不一定沿著威脅減小的方向生成?；趯W習算法 的避碰策略需要大量的訓練樣本，當在復(fù)雜的動態(tài)程度較高的未知環(huán)境中，其避碰效果難 以保證。

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

[0004] 本發(fā)明是為了目前采用的同向運動障礙規(guī)避方法難以準確預(yù)測運動障礙的運動 狀態(tài)的問題，而提出的一種基于虛擬膨化的運動障礙與UUV(無人水下航行器）同向航行的 規(guī)避方法。
[0005] -種基于虛擬膨化的運動障礙與UUV同向航行的規(guī)避方法按以下步驟實現(xiàn)：
[0006] 步驟一:設(shè)運動障礙航向與引導(dǎo)航向的夾角為headAngl e
UUV與運動障礙為同向航行；引導(dǎo)航向是指從UUV當前位置到下一個非運動障礙航路點的連 線所形成的向量，非運動障礙航路點是指不是依托于運動障礙形成的環(huán)境中靜態(tài)環(huán)境信息 的頂點；
[0007] 步驟二：當UUV檢測UUV與運動障礙為同向航行時，根據(jù)運動障礙到UUV當前所在位 置的連線與運動障礙前進方向的夾角β，分為UUV追擊運動障礙和運動障礙追擊UUV兩種位 置關(guān)系；
[0008] 步驟三：當UUV追擊運動障礙時，采用膨化和矩形虛擬障礙調(diào)整UUV的艏向，規(guī)避運 動障礙；
[0009] 步驟四：當運動障礙追擊UUV時，根據(jù)UUV與運動障礙相對位置變化，UUV不斷調(diào)整 自身運動速度，并采用膨化和矩形虛擬障礙調(diào)整UUV的艏向，規(guī)避運動障礙。
[0010] 發(fā)明效果：
[0011] 本發(fā)明使用蟻群算法作為最基本的動態(tài)規(guī)劃算法，其中規(guī)劃算法的觸發(fā)機制是動 態(tài)規(guī)劃的關(guān)鍵，首先該機制要控制好動態(tài)規(guī)劃算法的觸發(fā)時機，使UUV能及時規(guī)劃出新航路 避開運動障礙物，或者在沒有運動障礙物時合理觸發(fā)算法修正原有航路。其次該機制還應(yīng) 能根據(jù)UUV所處的具體環(huán)境，抑制不必要的觸發(fā)，因為算法觸發(fā)過于頻繁會降低UUV感知環(huán) 境的靈敏性。
[0012] 本發(fā)明的目的是在運動障礙靜態(tài)化策略的基礎(chǔ)上，使用虛擬膨化方法將運動障礙 的趨勢考慮到動態(tài)規(guī)劃當中，使UUV與運動障礙同向航行時總能沿著威脅減小的方向生成 規(guī)避航路。本發(fā)明的仿真效果圖如圖1~圖12所示。
【附圖說明】
[0013] 圖1為引導(dǎo)航向示意圖；圖中1是靜態(tài)障礙，2是運動障礙，3是引導(dǎo)航向，4是布放 點，5是回收點，必1是必經(jīng)點1;
[0014] 圖2為相向航行示意圖；圖中1是headAngle，2是運動障礙，3引導(dǎo)航向，4是UUV艏 向，5是運動障礙航向，0是UUV;
[0015] 圖3為同向航行時UUV追擊運動障礙示意圖；圖中1是引導(dǎo)航向，2是headAngle，3是 運動障礙航向，4是UUV艏向，5是M，6是追擊判定線1，7是運動障礙，8是運動障礙被感知范圍 (UUV在該圓域內(nèi)，可感知到該運動障礙），0是UUV;
[0016] 圖4為同向航行時運動障礙追擊UUV示意圖；圖中1是UUV艏向，2是headAngle，3是 運動障礙航向，4是引導(dǎo)航向，5是M，6是追擊判定線1，7是運動障礙，8是運動障礙被感知范 圍(UUV在該圓域內(nèi)，可感知到該運動障礙），0是UUV;
[0017] 圖5為UUV追擊運動障礙時生成虛擬障礙示意圖；圖中1是靜態(tài)障礙，2是膨化線，3 是引導(dǎo)航向，4是headAngle，5是運動障礙航向，6是L，7是M，8是追擊判定線1，9是UUV艏向， 10是運動障礙被感知范圍，0是UUV;
[0018] 圖6為UUV追擊運動障礙時虛擬障礙沿著運動障礙前進方向生成的示意圖；圖中1 是靜態(tài)障礙，2是膨化線，3是引導(dǎo)航向，4是headAngle，5是運動障礙航向，6是L，7是α，8是M， 9是追擊判定線1，10是運動障礙被感知范圍，11是UUV艏向，0是UUV;
[0019] 圖7為運動障礙追擊UUV時的場景1示意圖；圖中1是UUV艏向，2是運動障礙航向，3 是headAngle，4是靜態(tài)障礙，5是膨化線，6是引導(dǎo)航向，7是Μ，8是追擊判定線1，9是運動障礙 被感知范圍，〇是UUV;
[0020] 圖8為場景2后運動障礙追擊UUV時的場景3示意圖；圖中1是運動障礙航向，2是UUV 艏向，3是headAngle，4是靜態(tài)障礙，5是膨化線，6是引導(dǎo)航向，7是運動障礙被感知范圍，8是 追擊判定線1，9是Μ，0是UUV;
[0021] 圖9為快速運動障礙追擊UUV時的場景3示意圖；圖中1是UUV艏向，2是膨化線，3是 靜態(tài)障礙，4是運動障礙航向，5是引導(dǎo)航向，6是虛擬障礙，7是Μ，8是追擊判定線1，9是運動 障礙被感知范圍，〇是UUV;
[0022]圖10為運動障礙追擊UUV時的場景3示意圖；圖中1是UUV艏向，2是膨化線，3是靜態(tài) 障礙，4是運動障礙航向，5是引導(dǎo)航向，6是虛擬障礙，7是Μ，8是追擊判定線1，9是運動障礙 被感知范圍，〇是UUV;
[0023]圖11為UUV追擊運動障礙仿真效果圖，圖中1為運動障礙；
[0024]圖12為運動障礙追擊UUV仿真效果圖，圖中3為運動障礙。
【具體實施方式】
【具體實施方式】 [0025] 一：一種基于虛擬膨化的運動障礙與UUV同向航行的規(guī)避方法包括 以下步驟：
[0026] 引導(dǎo)航向是指從UUV當前位置到下一個非運動障礙航路點的連線所形成的向量的 方向。所謂非運動障礙航路點，是指該航路點不是依托于運動障礙形成的，而是環(huán)境中靜態(tài) 環(huán)境信息的頂點，如靜態(tài)目標的頂點(p 2)、必經(jīng)點，回收點等。當UUV運行到圖1所示位置時， 遭遇1號運動障礙，UUV觸發(fā)動態(tài)規(guī)劃算法后形成新的規(guī)避航路，點 ？1是1]1^為避開1號運動 障礙形成的依托于運動障礙形成的航路點，而點P2是靜態(tài)目標膨化頂點。于是根據(jù)定義此 時引導(dǎo)航向為向量的方向。UUV為無人水下航行器。
[0027] 獲得UUV自身幾何尺寸和航速v(這個航速是UUV自身可調(diào)節(jié)的，避碰過程中會不斷 變化，即UUV真實采用的速度）、巡航速度為 Vu、最大速度為Vu_max;使用UUV傳感器測得運動障 礙半徑〇bs_r (在建模處理的時候，按照障礙物的最大寬度將障礙物都建成圓形模型），UUV 到運動障礙中心點的直線距離M，運動障礙速度vb;
[0028] 步驟一:設(shè)運動障礙航向與引導(dǎo)航向的夾角為headAngl e， UUV與運動障礙為同向航行；引導(dǎo)航向是指從UUV當前位置到下一個非運動障礙航路點的連 線所形成的向量，非運動障礙航路點是指不是依托于運動障礙形成的環(huán)境中靜態(tài)環(huán)境信息 的頂點；
[0029] 步驟二：當UUV檢測UUV與運動障礙為同向航行時，根據(jù)運動障礙到UUV當前所在位 置的連線與運動障礙前進方向的夾角β，分為UUV追擊運動障礙和運動障礙追擊UUV兩種位 置關(guān)系；
[0030] 步驟三：當UUV追擊運動障礙時，采用膨化和矩形虛擬障礙調(diào)整UUV的艏向，規(guī)避運 動障礙；
[0031] 步驟四：當運動障礙追擊UUV時，根據(jù)UUV與運動障礙相對位置變化，UUV不斷調(diào)整 自身運動速度，并采用膨化和矩形虛擬障礙調(diào)整UUV的艏向，規(guī)避運動障礙。
【具體實施方式】 [0032] 二:本實施方式與一不同的是:所述步驟二中UUV追擊 運動障礙和運動障礙追擊UUV兩種位置關(guān)系的確定方法為，
[0033]
如圖3所示，為UUV追擊運動障礙
'，如圖4所示，為運 動障礙追擊UUV。
[0034] 圖3和圖4中運動障礙外側(cè)虛線圓的大小為UUV的感知半徑，當UUV處于該圓內(nèi)時， 對應(yīng)的運動障礙被UUV探知。追擊判定線1將UUV可能處于的位置根據(jù)運動障礙的前進方向 劃分為兩部分，當UUV處于后側(cè)時為UUV追擊運動障礙，處于前側(cè)則是運動障礙追擊UUV;追 擊判定線為過運動障礙中心點且垂直于運動速度方向的直線。
【具體實施方式】 [0035] 三:本實施方式與一或二不同的是:所述步驟三中采 用膨化和矩形虛擬障礙調(diào)整UUV的艏向，規(guī)避運動障礙的過程具體為：
[0036] 當UUV在后追擊運動障礙時，由圖3可以看出只需UUV到運動障礙的直線距離M2 R， UUV就是安全的。由于UUV和目標的航速以及相對位置關(guān)系在一定范圍內(nèi)是隨機的，因此運 動障礙不一定會對UUV的運動造成影響，所以剛一發(fā)現(xiàn)目標就觸發(fā)規(guī)劃是沒必要的。本發(fā)明 設(shè)計的方案是，如果UUV在運動障礙后方追擊，當UUV到運動障礙的直線距離距離Μ < C(C為 常數(shù)，一般取為2R~3R)時認為運動障礙對UUV的航行有影響，此時觸發(fā)規(guī)劃。
[0037] 在航路規(guī)劃算法調(diào)用前，需生成虛擬障礙以輔助調(diào)節(jié)UUV的航向。
[0038]
，引導(dǎo)航向與運動障礙航向角較大，沿引導(dǎo)航向 垂直方向生成一個長為L寬為2R的虛擬障礙，通常L取很大的值(仿真程序中L取值為5R)，引 導(dǎo)UUV從運動障礙后方繞行，如圖5所示;若headAngle^smallAngle時，運動障礙的航向與 引導(dǎo)航向幾乎重合，虛擬目標沿著運動障礙前進方向生成，引導(dǎo)UUV從運動障礙側(cè)面繞行， 如圖6所示。smallAngle是小角度判定值，
I op I為UUV到下一個非 運動障礙航路點的直線距離，R為運動障礙進行圓形膨化后的半徑。
[0039] 圖5所示情形還有兩點值得注意:首先，設(shè)圖中角度α為UUV到運動障礙的連線與引 導(dǎo)航向的夾角，]?\(308(€〇是1]1^到矩形虛擬障礙生成方向的直線距離。當]\1\(308(€〇〈1?時， 如果仍按垂直于引導(dǎo)航向方向生成虛擬障礙，則UUV會被虛擬障礙覆蓋，這將不能產(chǎn)生正確 的規(guī)劃結(jié)果。為了避免此類情況的發(fā)生，當MX C〇S(a)〈R時，虛擬障礙物沿著運動障礙前進 方向生成。其次，與引導(dǎo)航向垂直的方向其實有兩個，但為了迫使UUV從運動障礙后側(cè)繞行， 虛擬障礙應(yīng)沿著與運動障礙航向夾角較小的垂直方向生成。
【具體實施方式】 [0040] 四：本實施方式與一至三之一不同的是:所述步驟四 中規(guī)避運動障礙的過程具體為：
[0041 ]障礙物在后追擊UUV的情形，根據(jù)UUV與運動障礙的相對位置關(guān)系可劃分為三種觸 發(fā)場景；
[0042] 如圖7所示(場景1)，UUV(點〇)和下一個非運動障礙航路點p分別在運動障礙物航 向線的兩側(cè)，UUV到運動障礙物航向的距離為h，當R < h < 2R時觸發(fā)規(guī)劃，同時虛擬障礙沿運 動障礙物前進方向生成(先生成虛擬障礙，后觸發(fā)規(guī)劃）。此時要擺脫運動障礙的影響，UUV