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

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

[0004] 本發(fā)明是為了目前采用的運(yùn)動障礙規(guī)避方法難W準(zhǔn)確預(yù)測隨機(jī)運(yùn)動障礙的運(yùn)動 狀態(tài)的問題，而提出的一種基于虛擬膨化的UUV(無人水下航行器)航行過程中的運(yùn)動障礙 的規(guī)避方法。
[0005] -種基于虛擬膨化的UUV航行過程中的運(yùn)動障礙的規(guī)避方法按W下步驟實(shí)現(xiàn)：
[0006] 步驟一:設(shè)運(yùn)動障礙航向與引導(dǎo)航向的夾角為headAngle，根據(jù)headAngle確定UUV 與運(yùn)動障礙相向航行或UUV與運(yùn)動障礙同向航行；引導(dǎo)航向是指從UUV當(dāng)前位置到下一個非 運(yùn)動障礙航路點(diǎn)的連線所形成的向量，非運(yùn)動障礙航路點(diǎn)是指不是依托于運(yùn)動障礙形成的 環(huán)境中靜態(tài)環(huán)境信息的頂點(diǎn)；
[0007] 步驟二：當(dāng)UUV檢測到與運(yùn)動障礙為相向航行時，采用膨化和矩形虛擬障礙調(diào)整 UUV的臘向，規(guī)避運(yùn)動障礙；
[000引步驟立：當(dāng)UUV檢測UUV與運(yùn)動障礙為同向航行時，根據(jù)運(yùn)動障礙至ljuuv當(dāng)前所在位 置的連線與運(yùn)動障礙前進(jìn)方向的夾角β，分為UUV追擊運(yùn)動障礙和運(yùn)動障礙追擊UUV兩種位 置關(guān)系；
[0009] 步驟四：當(dāng)UUV追擊運(yùn)動障礙時，采用膨化和矩形虛擬障礙調(diào)整UUV的臘向，規(guī)避運(yùn) 動障礙；
[0010] 步驟五：當(dāng)運(yùn)動障礙追擊UUV時，根據(jù)UUV與運(yùn)動障礙相對位置變化，UUV不斷調(diào)整 自身運(yùn)動速度，并采用膨化和矩形虛擬障礙輔助調(diào)整UUV的臘向，規(guī)避運(yùn)動障礙。
[0011]發(fā)明效果：
[0012]本發(fā)明使用蟻群算法作為最基本的動態(tài)規(guī)劃算法，其中規(guī)劃算法的觸發(fā)機(jī)制是動 態(tài)規(guī)劃的關(guān)鍵，首先該機(jī)制要控制好動態(tài)規(guī)劃算法的觸發(fā)時機(jī)，使UUV能及時規(guī)劃出新航路 避開運(yùn)動障礙物，或者在沒有運(yùn)動障礙物時合理觸發(fā)算法修正原有航路。其次該機(jī)制還應(yīng) 能根據(jù)UUV所處的具體環(huán)境，抑制不必要的觸發(fā)，因?yàn)樗惴ㄓ|發(fā)過于頻繁會降低UUV感知環(huán) 境的靈敏性。本發(fā)明根據(jù)UUV與運(yùn)動障礙間不同的位置關(guān)系對運(yùn)動障礙做虛擬膨化并設(shè)計 規(guī)劃算法觸發(fā)規(guī)則。
[0013] 本發(fā)明的目的是在運(yùn)動障礙靜態(tài)化策略的基礎(chǔ)上，使用虛擬膨化方法將運(yùn)動障礙 的趨勢考慮到動態(tài)規(guī)劃當(dāng)中，使UUV總能沿著威脅減小的方向生成規(guī)避航路，UUV行進(jìn)在同 時存在靜態(tài)目標(biāo)和運(yùn)動障礙的環(huán)境中，即使同時遭遇多個運(yùn)動障礙也能安全避碰，本發(fā)明 的仿真效果圖如圖14和圖15所示。
[0014] 本發(fā)明將同向航行規(guī)避方法和相向航行規(guī)避方法組合使用，由于兩種方法均是根 據(jù)運(yùn)動障礙與UUV的相對位置關(guān)系和速度關(guān)系，針對運(yùn)動障礙各自生成了虛擬障礙，W輔助 調(diào)節(jié)UUV的臘向。因此當(dāng)環(huán)境中出現(xiàn)多個運(yùn)動障礙時，UUV會判斷與感知范圍內(nèi)每一個運(yùn)動 障礙的航向關(guān)系和速度關(guān)系，并按照相應(yīng)的規(guī)則同時生成對應(yīng)運(yùn)動障礙的虛擬障礙;然后， 采用基本蟻群路徑規(guī)劃算法，在復(fù)雜的多運(yùn)動障礙環(huán)境下規(guī)劃出UUV安全航路。還需指出， 在同時遭遇多個不同航速或航向的運(yùn)動障礙時，UUV可能出現(xiàn)速度調(diào)節(jié)沖突，為了保證UUV 的安全，當(dāng)出現(xiàn)此類沖突時，均采用調(diào)節(jié)指令中較大的速度作為UUV的當(dāng)前航速。可W看出， 兩種方法組合使用后可W成功并優(yōu)化地規(guī)避多方向、多速度情況下的運(yùn)動障礙，達(dá)到了UUV 規(guī)避能力倍增的效果。
【附圖說明】
[0015] 圖1為引導(dǎo)航向示意圖；圖中1是靜態(tài)障礙，2是運(yùn)動障礙，3是引導(dǎo)航向，4是布放 點(diǎn)，5是回收點(diǎn)，必1是必經(jīng)點(diǎn)1;
[0016]圖2為相向航行示意圖；圖中1是headAngle，2是運(yùn)動障礙，3引導(dǎo)航向，4是UUV臘 向，5是運(yùn)動障礙航向，0是UUV;
[0017]圖3為同向航行不意圖；圖中1是headAngle，2是運(yùn)動障礙，3引導(dǎo)航向，4是UUV臘 向，5是運(yùn)動障礙航向，0是UUV;
[0018] 圖4為運(yùn)動障礙膨化并生成虛擬障礙示意圖；圖中1是headAngle,2是運(yùn)動障礙，3 是引導(dǎo)航向，4是L，5是運(yùn)動障礙航向，6是Μ，7是運(yùn)動障礙，0是UUV;
[0019] 圖5為UUV與運(yùn)動障礙的位置關(guān)系示意圖；圖中1是headAngle,2是運(yùn)動障礙，3是運(yùn) 動障礙航向，4是引導(dǎo)航向，5是L，6是Μ，7是運(yùn)動障礙，0是UUV;
[0020] 圖6為同向航行時UUV追擊運(yùn)動障礙示意圖；圖中1是引導(dǎo)航向，2是headAngle,3是 運(yùn)動障礙航向，4是UUV臘向，5是M，6是追擊判定線1，7是運(yùn)動障礙，8是運(yùn)動障礙被感知范圍 (UUV在該圓域內(nèi)，可感知到該運(yùn)動障礙），0是UUV;
[0021] 圖7為同向航行時運(yùn)動障礙追擊UUV示意圖；圖中1是UUV臘向，2是headAngle,3是 運(yùn)動障礙航向，4是引導(dǎo)航向，5是M，6是追擊判定線1，7是運(yùn)動障礙，8是運(yùn)動障礙被感知范 圍(UUV在該圓域內(nèi)，可感知到該運(yùn)動障礙），0是UUV;
[0022] 圖8為UUV追擊運(yùn)動障礙時生成虛擬障礙示意圖；圖中1是靜態(tài)障礙，2是膨化線，3 是引導(dǎo)航向，4是headAngle, 5是運(yùn)動障礙航向，6是L，7是M，8是追擊判定線1，9是UUV臘向， 10是運(yùn)動障礙被感知范圍，0是UUV;
[0023] 圖9為UUV追擊運(yùn)動障礙時虛擬障礙沿著運(yùn)動障礙前進(jìn)方向生成的示意圖；圖中1 是靜態(tài)障礙，2是膨化線，3是引導(dǎo)航向，4是headAngle，5是運(yùn)動障礙航向，6是L，7是α，8是Μ, 9是追擊判定線1，10是運(yùn)動障礙被感知范圍，11是UUV臘向，0是UUV;
[0024] 圖10為運(yùn)動障礙追擊UUV時的場景1示意圖；圖中1是UUV臘向，2是運(yùn)動障礙航向，3 是headAngle，4是靜態(tài)障礙，5是膨化線，6是引導(dǎo)航向，7是Μ，8是追擊判定線1，9是運(yùn)動障礙 被感知范圍，0是UUV;
[0025] 圖11為運(yùn)動障礙追擊UUV時的場景2示意圖；圖中1是運(yùn)動障礙航向，2是UUV臘向，3 是headAngle，4是靜態(tài)障礙，5是膨化線，6是引導(dǎo)航向，7是運(yùn)動障礙被感知范圍，8是追擊判 定線1，9是M，0是UUV;
[0026] 圖12為場景2后運(yùn)動障礙追擊UUV時的場景3示意圖；圖中1是運(yùn)動障礙航向，2是 headAngle, 3是UUV臘向，4是靜態(tài)障礙，5是膨化線，6是引導(dǎo)航向，7是運(yùn)動障礙被感知范圍， 8是M，9是追擊判定線1，0是UUV;
[0027] 圖13為快速運(yùn)動障礙追擊UUV時的場景3示意圖；圖中1是UUV臘向，2是膨化線，3是 靜態(tài)障礙，4是運(yùn)動障礙航向，5是引導(dǎo)航向，6是虛擬障礙，7是M，8是追擊判定線1，9是運(yùn)動 障礙被感知范圍，0是UUV;
[002引圖14為UUV規(guī)避單個運(yùn)動障礙示意圖；圖中必1是必經(jīng)點(diǎn)1、必2是必經(jīng)點(diǎn)2、必3是必 經(jīng)點(diǎn)3，必4是必經(jīng)點(diǎn)4，1是運(yùn)動障礙；
[0029] 圖15為UUV規(guī)避多個運(yùn)動障礙示意圖；圖中1、2、3、4、5、6、7是運(yùn)動障礙。
【具體實(shí)施方式】
【具體實(shí)施方式】 [0030] 一：一種基于虛擬膨化的UUV航行過程中的運(yùn)動障礙的規(guī)避方法包 括W下步驟：
[0031] 引導(dǎo)航向是指從UUV當(dāng)前位置到下一個非運(yùn)動障礙航路點(diǎn)的連線所形成的向量的 方向。所謂非運(yùn)動障礙航路點(diǎn)，是指該航路點(diǎn)不是依托于運(yùn)動障礙形成的，而是環(huán)境中靜態(tài) 環(huán)境信息的頂點(diǎn)，如靜態(tài)目標(biāo)的頂點(diǎn)(P2)、必經(jīng)點(diǎn)，回收點(diǎn)等。當(dāng)UUV運(yùn)行到圖1所示位置時， 遭遇1號運(yùn)動障礙，UUV觸發(fā)動態(tài)規(guī)劃算法后形成新的規(guī)避航路，點(diǎn)P1是UUV為避開1號運(yùn)動 障礙形成的依托于運(yùn)動障礙形成的航路點(diǎn)，而點(diǎn)P2是靜態(tài)目標(biāo)膨化頂點(diǎn)。于是根據(jù)定義此 時引導(dǎo)航向?yàn)橄蛄空僭频姆较?。UUV為無人水下航行器。
[0032] 獲得UUV自身幾何尺寸和航速V(運(yùn)個航速是UUV自身可調(diào)節(jié)的，避碰過程中會不斷 變化，即UUV真實(shí)采用的速度）、巡航速度為Vu、最大速度為Vu_max;使用UUV傳感器測得運(yùn)動障 礙半徑obs_r(在建模處理的時候，按照障礙物的最大寬度將障礙物都建成圓形模型），UUV 到運(yùn)動障礙中屯、點(diǎn)的直線距離Μ，運(yùn)動障礙速度vb;
[0033] 步驟一:設(shè)運(yùn)動障礙航向與引導(dǎo)航向的夾角為headAngle，根據(jù)headAngle確定UUV 與運(yùn)動障礙相向航行或UUV與運(yùn)動障礙同向航行；引導(dǎo)航向是指從UUV當(dāng)前位置到下一個非 運(yùn)動障礙航路點(diǎn)的連線所形成的向量，非運(yùn)動