一種uuv對圓形障礙物幾何繞行的二維航路規(guī)劃方法
【專利摘要】本發(fā)明提供的是一種UUV對圓形障礙物幾何繞行的二維航路規(guī)劃方法。一：從使命文本讀取航路起點Ob、航路終點Oe和各圓形障礙物的參數(shù)；二：對各圓形障礙物進(jìn)行膨脹處理，計算膨脹后的各圓形障礙物的參數(shù)；三：建立繞行點集合S，令規(guī)劃當(dāng)前點Oc為起點Ob，并放入繞行點集合S中；四：如果規(guī)劃當(dāng)前點Oc是航路終點Oe，或者規(guī)劃當(dāng)前點Oc和航路終點Oe可視，轉(zhuǎn)步驟六，否則執(zhí)行步驟五；五：對距規(guī)劃當(dāng)前點Oc最近的圓形障礙物進(jìn)行幾何繞行，得到繞行點并放入繞行點集合S中，更新規(guī)劃當(dāng)前點Oc，轉(zhuǎn)步驟四；六：將航路終點Oe放入繞行點集合S中，規(guī)劃結(jié)束。本發(fā)明通過簡單的幾何原理實現(xiàn)對圓形障礙物的繞行，可以使UUV在復(fù)雜多圓形障礙環(huán)境中快速、高效的獲得一條安全無碰的二維航路。
【專利說明】
-種uuv對圓形障礙物幾何繞行的二維航路規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及的是一種是UUV航路規(guī)劃方法，具體的是一種UUV對圓形障礙物幾何繞 行的二維航路規(guī)劃方法。
【背景技術(shù)】
[0002] 航路規(guī)劃是水下無人航行器(Unmanned化derwater Vehicle，UUV)的關(guān)鍵技術(shù)之 一，是UUV自主能力的重要體現(xiàn)。航路規(guī)劃是指在已知障礙環(huán)境下，規(guī)劃出一條從起點出發(fā) 繞過所有障礙物并到達(dá)終點的無碰路徑。根據(jù)空間維度，航路規(guī)劃可分為二維航路規(guī)劃和 Ξ維航路規(guī)劃。其中，二維航路規(guī)劃是Ξ維航路規(guī)劃的基礎(chǔ)，并且在UUV的應(yīng)用也更為廣泛， 是UUV航路規(guī)劃技術(shù)研究的熱點。目前，UUV的航路規(guī)劃方法很多，但是如何在復(fù)雜的障礙環(huán) 境下既快速又可行的獲得一條無碰路徑，特別是規(guī)劃方法能夠適于工程應(yīng)用，仍然是一個 難點。
[0003] 與本發(fā)明相關(guān)的已有技術(shù)為"基于幾何算法的水下航行器路徑規(guī)劃"（《海軍工程 大學(xué)學(xué)報》，2009,21(6) :41-44頁），其中提到了考慮圓形障礙物時水下航行器基于幾何算 法的路徑規(guī)劃，但該文獻(xiàn)對圓形障礙物繞行的航路規(guī)劃方法與本發(fā)明不同。

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

[0004] 本發(fā)明的目的在于提供一種計算簡單、規(guī)劃效率高、規(guī)劃速度快的UUV對圓形障礙 物幾何繞行的二維航路規(guī)劃方法。
[0005] 本發(fā)明的目的是運樣實現(xiàn)的：
[0006] 步驟一:從使命文本讀取航路起點Ob、航路終點Oe和各圓形障礙物的參數(shù)；
[0007] 步驟二:對各圓形障礙物進(jìn)行膨脹處理，計算膨脹后的各圓形障礙物的參數(shù)；
[000引步驟S :建立繞行點集合S，令規(guī)劃當(dāng)前點Oc為起點Ob，并放入繞行點集合S中；
[0009] 步驟四：如果規(guī)劃當(dāng)前點Oc是航路終點Oe，或者規(guī)劃當(dāng)前點Oc和航路終點Oe可視， 轉(zhuǎn)步驟六，否則執(zhí)行步驟五；
[0010] 步驟五:對距規(guī)劃當(dāng)前點0。最近的圓形障礙物進(jìn)行幾何繞行，得到繞行點并放入 繞行點集合S中，更新規(guī)劃當(dāng)前點0。，轉(zhuǎn)步驟四；
[001。 步驟六:將航路終點Oe放入繞行點集合S中，規(guī)劃結(jié)束。
[0012] 本發(fā)明還可W包括；
[0013] 1、對圓形障礙物進(jìn)行幾何繞行的方法為：
[0014] (1)、判斷規(guī)劃當(dāng)前點0。是否在圓形障礙物上，如果在，令點化= 0c;否則，求解規(guī)劃 當(dāng)前點0。和圓屯、0的連線與圓周的交點，并令其為點化；
[0015] (2)、求解航路終點Oe和圓屯、0的連線與圓周的交點，并令其為點化；
[0016] (3)、求解Z化0化的角平分線與圓周的交點，并令其為點化；
[0017] (4)、求解過點化的圓的切線與過點化的圓的切線的交點，并令其為點化；
[001引（5)、計算過點化的圓的切線與過點化的圓的切線的交點，并令其為點化；
[0019] (6)、將點〇1、點〇4、點〇5、點〇2作為圓形障礙物的繞行點放入繞行點集合S中，并更 新規(guī)劃當(dāng)前點Oc為點化，繞行結(jié)束。
[0020] 2、對圓形障礙物進(jìn)行膨脹處理的方法是在正常規(guī)劃結(jié)束障礙物幾何形狀的基礎(chǔ) 上，按照障礙物形狀邊緣W安全半徑ruuv向外擴展出一個安全半徑區(qū)域。
[0021] 本發(fā)明利用幾何原理進(jìn)行UUV的二維航路規(guī)劃，在環(huán)境模型上采用了簡單的幾何 模型，在計算無碰路徑時采用簡單的幾何原理對障礙物進(jìn)行繞行，避免了其他規(guī)劃方法需 要建立地圖、循環(huán)捜索無碰路徑所引起的信息量大、計算復(fù)雜的問題，不僅規(guī)劃效率高、規(guī) 劃速度快，而且原理簡單、計算量小，易于工程實現(xiàn)。
[0022] 本發(fā)明與【背景技術(shù)】"基于幾何算法的水下航行器路徑規(guī)劃(《海軍工程大學(xué)學(xué)報》， 2009,21(6):41-44頁Γ的主要區(qū)別在于：
[0023] 1、本發(fā)明對圓形障礙物進(jìn)行了膨脹處理，而【背景技術(shù)】"基于幾何算法的水下航行 器路徑規(guī)劃"沒有進(jìn)行膨脹處理。對圓形障礙物進(jìn)行膨脹處理，避免了 UUV沿規(guī)劃航路航行 時與圓形障礙物的碰撞，提高了 UUV的航行安全性。
[0024] 2、本發(fā)明和【背景技術(shù)】"基于幾何算法的水下航行器路徑規(guī)劃"對單個圓形障礙物 的繞行方法不同。本發(fā)明的繞行方法如圖4和圖5所示，形成的是多條線段組成的繞行航路； 而【背景技術(shù)】形成的是線段和圓弧相結(jié)合的繞行航路，而圓弧航路不利于UUV的航路跟蹤控 審IJ。本發(fā)明只有線段的繞行航路更利于UUV的航路跟蹤控制，可W提高UUV的航路跟蹤效果。
[0025] 本發(fā)明的有益效果在于：
[0026] 1、環(huán)境模型采用的是幾何空間模型，相比于傳統(tǒng)的柵格、地圖模型，所需規(guī)劃信息 量少，規(guī)劃效率高，特別適合復(fù)雜多障礙物的環(huán)境。
[0027] 2、對圓形障礙物的繞行算法只應(yīng)用到了幾何原理，計算簡單、易于工程實現(xiàn)，而且 計算量非常小，規(guī)劃速度快。
[00%] 3、WUUV的外形尺寸為安全半徑對圓形障礙物進(jìn)行了膨脹處理，避免了UUV沿規(guī)劃 航路航行時與圓形障礙物的碰撞，提高了 UUV的航行安全性。
[0029] 4、形成的繞行航路只有線段，利于UUV的航路跟蹤控制，可W提高UUV的航路跟蹤 效果。
【附圖說明】
[0030] 圖1規(guī)劃環(huán)境模型中的圓形障礙物示意圖；
[0031 ]圖2圓形障礙物膨脹處理的安全半徑示意圖；
[0032] 圖3圓形障礙物的膨脹示意圖；
[0033] 圖4規(guī)劃當(dāng)前點不在圓形障礙物上時對圓形障礙物的繞行示意圖；
[0034] 圖5規(guī)劃當(dāng)前點在圓形障礙物上時對圓形障礙物的繞行示意圖；
[0035] 圖6 UUV對圓形障礙物的幾何繞行的流程圖；
[0036] 圖7判斷兩點連成線段與圓形障礙物是否相交的流程圖；
[0037] 圖8 UUV對圓形障礙物幾何繞行的二維航路規(guī)劃流程圖；
[0038] 圖9利用本發(fā)明進(jìn)行UUV對圓形障礙物的幾何繞行的效果圖。
【具體實施方式】
[0039] 下面舉例對本發(fā)明進(jìn)行詳細(xì)說明。
[0040] 結(jié)合圖1介紹UUV二維航路規(guī)劃的環(huán)境模型。
[0041] 本發(fā)明中航路規(guī)劃的環(huán)境模型采用的是二維幾何空間模型。設(shè)規(guī)劃的航路起點為 Ob，航路終點為Oe，化和Oe分別用二維坐標(biāo)表示為：
[0042] 0b=(x〇b,y〇b);0e=(x〇e，y〇e) (1)
[0043] 另設(shè)航路規(guī)劃過程中每一步用到的規(guī)劃當(dāng)前點為0。，用二維坐標(biāo)表示為：
[0044] 0c=(x〇c,y〇c) (2)
[0045] 設(shè)二維幾何空間中存在一定數(shù)量的圓形障礙物，如圖1所示，設(shè)圓形障礙物為 Zcirc，其參數(shù)化表示為：
[0046] Zcirc= (Xcirc'ycirc'r) (3)
[0047] 式中，（Xcirc，ycirc)表示圓屯、的二維坐標(biāo)，r表示圓形障礙物的半徑。
[0048] 結(jié)合圖2和圖3介紹圓形障礙物膨脹模型的建立方法。
[0049] 進(jìn)行航路規(guī)劃時，一般是把UUV當(dāng)作質(zhì)點來考慮的，因此規(guī)劃的航路可能會距障礙 物較近。但是實際上，UUV是有幾何尺寸的實體，當(dāng)規(guī)劃的航路距障礙物較近時，很有可能導(dǎo) 致UUV與障礙物發(fā)生碰撞。為此在進(jìn)行航路規(guī)劃時，設(shè)置一個安全半徑來防止UUV沿規(guī)劃航 路航行時與障礙物發(fā)生碰撞。本發(fā)明采用的方法是WUUV的外形尺寸的外接圓半徑ruuv為安 全半徑(見圖2所示），然后在正常圓形障礙物的幾何形狀的基礎(chǔ)上，按照其形狀邊緣W半徑 ruuv向外擴展出一個安全半徑區(qū)域。圖3給出了圓形障礙物向外擴展安全半徑后的膨脹示意 圖。
[0050] 膨脹后圓形障礙物的參數(shù)化表示為：
[0051] Z'circ= (Xcirc，ycirc，r' ) (4)
[0化2] 式中：（Xcirc，ycirc)仍然表示圓屯、的二維坐標(biāo);而=r+ruuv表示膨脹后的圓形障礙 物半徑。
[0053] 結(jié)合圖4、圖5、圖6介紹UUV對圓形障礙物的繞行方法。
[0054] 對圓形障礙物的繞行分為規(guī)劃當(dāng)前點在圓形障礙物上和不在圓形障礙物上兩種 情況，圖4給出了規(guī)劃當(dāng)前點不在圓形障礙物上的繞行示意圖，圖5給出了規(guī)劃當(dāng)前點在圓 形障礙物上的繞行示意圖。從圖4和圖5可W看出，對圓形障礙物的繞行，采用的是利用多條 圓的切線的交點作為繞行點繞行圓形障礙物的方法，并利用幾何原理求解各繞行點。
[0055] 圖6給出了圓形障礙物的繞行流程。
[0056] 步驟一：判斷規(guī)劃當(dāng)前點Oc是否在障礙物上，如果在障礙物上，令化= 0。，轉(zhuǎn)步驟 Ξ;否則執(zhí)行步驟二；
[0057] 步驟二:求解規(guī)劃當(dāng)前點Oc和圓屯、0的連線與圓的交點0i=(x日i，y日1)，〇1的位置坐 標(biāo)按式(5)計算，有兩個解，選取與當(dāng)前點Oc距離近的解作為點化的坐標(biāo)。
[0化引
[0化9]式中：kc表示規(guī)劃當(dāng)前點Oc和圓屯、Ο所連直線的斜率，并且有
[0060]步驟求解航路終點Oe和圓屯、0的連線與圓的交點〇2=(Χ日2,7日2)，〇2的位置坐標(biāo) 按式(6)計算，有兩個解，選取與航路終點Oe距離近的解作為點化的坐標(biāo)。
[0061 ]
(6)
[0062] 式中：ke表示航路終點Oe和圓屯、0所連直線的斜率，并且有
[0063] 步驟四：求解Z〇1〇〇2的角平分線Lo與圓的交點〇3 = (X03，y〇3)，〇3的位置坐標(biāo)按式 (7)計算，有兩個解，選取與點化(或點化)距離近的解作為點化的坐標(biāo)。
[0066]步驟五:分別求解過點化、〇2、化的圓的切線^、L2、L3,其切線方程分別按式(8)、式 (9)和式(10)計算。
[0070] 式中：kLl、t)Ll分別表不圓的切線1^1的斜率和截距;kL2、t)L2分別表不圓的切線L2的
[0071]斜率和截距;kL3、bL3分別表示圓的切線L3的斜率和截距。
[0072] 步驟六:計算切線。和1^3的交點化=(胡4,704)，〇4的位置坐標(biāo)按式（11)計算
[0073]
(11)
[0074] 步驟屯:計算切線L2和L3的交點0日=(x日日，y日日），0日的位置坐標(biāo)按式（12)計算
[0075]
(12)
[0076] 步驟八:將點化、〇4、〇5、〇2作為圓形障礙物的繞行點放入繞行點集合S中，并更新當(dāng) 前點Oc為化，繞行結(jié)束。
[0077] 結(jié)合圖7介紹判斷規(guī)劃當(dāng)前點0。和航路終點Oe是否可視的方法。
[0078] 點Oc和點Oe是指兩點不被任何圓形障礙物所阻擋。判斷兩點是否可視的方法就是 判斷兩點連線所形成的線段是否與所有的圓形障礙物相交，如果不與任何圓形障礙物相交 則表明兩點可視。判斷規(guī)劃當(dāng)前點0。和航路終點Oe是否可視的流程如圖7所示：。
[0079] 步驟一:選擇第一個圓形障礙物；
[0080] 步驟二:求解當(dāng)前點0。和航路終點Oe兩點連線和圓聯(lián)立的二次方程的根的判別式 A，求解方法如式(13)所示：
[0081]
[0082] 式中，kee和bee分別表示點0。和點Oe所連成直線的斜率和截距，并且有
，'bce = yw-kceX〇c;
[0083] 步驟Ξ:判斷根的判別式Δ是否大于等于0,如果大于等于0轉(zhuǎn)步驟四，否則轉(zhuǎn)步驟 六
[0084] 步驟四：求解當(dāng)前點0。和航路終點0廂點連線和圓的兩個交點的橫坐標(biāo)Xpcl和Xpc2， 求解方法如式(14)所示：
[0085]
U4、
[0086] 步驟五:判斷Xpcl的值在Xoc和Xoe之間或Xpc2值在Xoc和Xoe之間是否滿足，如果滿足轉(zhuǎn) 步驟屯，否則轉(zhuǎn)步驟六；
[0087] 步驟六:判斷是否還有圓形障礙物，如果有，選擇下一個圓形障礙物，轉(zhuǎn)步驟二，否 則轉(zhuǎn)步驟八；
[008引步驟屯:當(dāng)前點0。和航路終點Oe不可視，判斷結(jié)束；
[0089] 步驟八：當(dāng)前點0。和航路終點Oe可視，判斷結(jié)束。
[0090] 結(jié)合圖8介紹UUV對圓形障礙物幾何繞行的二維航路規(guī)劃的整個流程。
[0091] 步驟一:從使命文本讀取航路起點Ob、航路終點Oe和各圓形障礙物的參數(shù)；
[0092] 步驟二:建立各圓形障礙物膨脹模型，計算膨脹后的各圓形障礙物的參數(shù)，建立繞 行點集合S;
[0093] 步驟Ξ:令規(guī)劃當(dāng)前點0。為起點Ob，并放入繞行點集合S中；
[0094]步驟四：判斷規(guī)劃當(dāng)前點Oc是不是航路終點Oe，如果是轉(zhuǎn)步驟十，否則轉(zhuǎn)步驟五； [00%]步驟五:判斷規(guī)劃當(dāng)前點0。和航路終點Oe是否可視，如果可視轉(zhuǎn)步驟十，否則轉(zhuǎn)步 驟六；
[0096] 步驟六:捜索距規(guī)劃當(dāng)前點0。最近的圓形障礙物;捜索方法為首先找到阻礙當(dāng)前 點Oc和航路終點Oe連線的所有圓形障礙物，然后求解W上各圓形障礙物中屯、與當(dāng)前點Oc的 距離，距離最小的即為距規(guī)劃當(dāng)前點0。最近的圓形障礙物；
[0097] 步驟屯:對距規(guī)劃當(dāng)前點0。最近的圓形障礙物進(jìn)行幾何繞行；
[0098] 步驟八:將繞行點放入繞行點集合S中；
[0099] 步驟九:更新規(guī)劃當(dāng)前點0。，轉(zhuǎn)步驟四；
[0100] 步驟十:將航路終點Oe放入繞行點集合S中，規(guī)劃結(jié)束。
[0101] 圖9給出了利用本發(fā)明進(jìn)行UUV對圓形障礙物幾何繞行二維航路規(guī)劃的一個實現(xiàn) 案例。
[0102] 本案例中，共設(shè)置了 10個圓形障礙物，航路的起點Ob和航路的終點Oe已在圖中標(biāo) 出。首先，在規(guī)劃時對每個障礙物進(jìn)行了膨脹處理，各障礙物的膨脹邊界已在圖中用點劃線 標(biāo)出。然后，UUV對部分圓形障礙物進(jìn)行了繞行，得到了繞行點集合S={0b，Pi，P2，P3r-，Pi6， Oe}，并在圖中用虛線表示出了由繞行點組成的UUV繞行航路。
【主權(quán)項】
1. 一種UUV對圓形障礙物幾何繞行的二維航路規(guī)劃方法，其特征是： 步驟一:從使命文本讀取航路起點Ob、航路終點Oe和各圓形障礙物的參數(shù)； 步驟二:對各圓形障礙物進(jìn)行膨脹處理，計算膨脹后的各圓形障礙物的參數(shù)； 步驟三:建立繞行點集合S，令規(guī)劃當(dāng)前點0。為起點Ob，并放入繞行點集合S中； 步驟四：如果規(guī)劃當(dāng)前點是航路終點Oe，或者規(guī)劃當(dāng)前點和航路終點Oe可視，轉(zhuǎn)步驟 六，否則執(zhí)行步驟五； 步驟五:對距規(guī)劃當(dāng)前點0。最近的圓形障礙物進(jìn)行幾何繞行，得到繞行點并放入繞行點 集合S中，更新規(guī)劃當(dāng)前點0。，轉(zhuǎn)步驟四； 步驟六:將航路終點仏放入繞行點集合S中，規(guī)劃結(jié)束。2. 根據(jù)權(quán)利要求1所述的UUV對圓形障礙物幾何繞行的二維航路規(guī)劃方法，其特征是對 圓形障礙物進(jìn)行幾何繞行的方法為： (1) 、判斷規(guī)劃當(dāng)前點0。是否在圓形障礙物上，如果在，令點〇! = 〇?;否則，求解規(guī)劃當(dāng)前 點0。和圓心0的連線與圓周的交點，并令其為點〇1; (2) 、求解航路終點0e和圓心0的連線與圓周的交點，并令其為點〇2; (3) 、求解Z 0W02的角平分線與圓周的交點，并令其為點〇3; (4) 、求解過點(h的圓的切線與過點03的圓的切線的交點，并令其為點04; (5) 、計算過點02的圓的切線與過點03的圓的切線的交點，并令其為點05; (6) 、將點、點04、點05、點02作為圓形障礙物的繞行點放入繞行點集合S中，并更新規(guī)劃 當(dāng)前點0。為點〇2，繞行結(jié)束。3. 根據(jù)權(quán)利要求1或2所述的UUV對圓形障礙物幾何繞行的二維航路規(guī)劃方法，其特征 是對圓形障礙物進(jìn)行膨脹處理的方法是：在正常規(guī)劃結(jié)束障礙物幾何形狀的基礎(chǔ)上，按照 障礙物形狀邊緣以安全半向外擴展出一個安全半徑區(qū)域。
【文檔編號】G05D1/06GK105843234SQ201610312415
【公開日】2016年8月10日
【申請日】2016年5月12日
【發(fā)明人】陳濤, 徐達(dá), 張偉, 張宏瀚, 張勛, 周佳加
【申請人】哈爾濱工程大學(xué)