基于距離與視差信息的水下機(jī)器人避障方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及水下機(jī)器人智能控制技術(shù)領(lǐng)域，尤其是一種融合距離與視差信息的水 下機(jī)器人避障方法。
【背景技術(shù)】
[0002] 水下機(jī)器人在海難搜救、水下防入侵技術(shù)、物種跟蹤探測(cè)等領(lǐng)域具有廣泛的應(yīng)用 價(jià)值。在水下環(huán)境中，沉船等沉積物以及礁石等使得水下環(huán)境非常復(fù)雜，如果水下機(jī)器人不 能主動(dòng)、有效地躲避這些障礙，將有可能導(dǎo)致水下機(jī)器人執(zhí)行任務(wù)的失敗甚至導(dǎo)致水下機(jī) 器人損壞。
[0003] 在現(xiàn)有技術(shù)中檢索發(fā)現(xiàn)，中國(guó)專利申請(qǐng)?zhí)枮?01310488141. 8,名稱為：基于前視 聲納的水下機(jī)器人避碰方法，該方法前視聲納圖像數(shù)據(jù)引入機(jī)器人避障策略中，可以減小 機(jī)器人避碰盲區(qū)。但上述方法在設(shè)計(jì)無(wú)碰撞軌跡規(guī)劃算法時(shí)，并沒有考慮水下機(jī)器人的控 制輸入約束。在實(shí)際應(yīng)用中，水下機(jī)器人一般都是由電機(jī)驅(qū)動(dòng)，而電機(jī)驅(qū)動(dòng)器都有功率限 制，當(dāng)輸入信號(hào)超過(guò)這一限制，飽和發(fā)生，進(jìn)而使系統(tǒng)的動(dòng)態(tài)性能降低（例如，超調(diào)量、調(diào)節(jié) 時(shí)間以及震蕩次數(shù)等），甚至導(dǎo)致閉環(huán)系統(tǒng)不穩(wěn)定。
[0004] 再有，中國(guó)專利申請(qǐng)?zhí)枮?01210539473.X，名稱為：一種智能水下機(jī)器人的自主 避障方法，該方法將水下機(jī)器人運(yùn)動(dòng)目標(biāo)、障礙物和水下機(jī)器人控制性能進(jìn)行統(tǒng)一考慮來(lái) 實(shí)現(xiàn)避障任務(wù)。上述方法是基于距離信息設(shè)計(jì)的，但僅考慮距離信息是不夠的，因?yàn)閷?shí)時(shí)有 效的避障行為不僅需要距離信息，還需要視角信息，例如人們?cè)谝?guī)避障礙的過(guò)程中，有效 的避障路線是通過(guò)距離與視差調(diào)整來(lái)共同確立的。在水下三維環(huán)境中，方向性顯得更為重 要。因此，如何在水下復(fù)雜障礙物環(huán)境下，利用距離和視差信息對(duì)水下機(jī)器人設(shè)計(jì)無(wú)碰撞軌 跡優(yōu)化策略，以提高水下機(jī)器人軌跡規(guī)劃的有效性，顯得尤為重要。

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

[0005] 本發(fā)明目的在于提供一種可動(dòng)態(tài)調(diào)整壁障策略、路徑更優(yōu)、躲避效果好、可靠性高 的基于距離與視差信息的水下機(jī)器人避障方法。
[0006] 為實(shí)現(xiàn)上述目的，采用了以下技術(shù)方案：
[0007] 本發(fā)明主要包括水下機(jī)器人、探測(cè)聲吶、前向紅外攝像頭、側(cè)向紅外攝像頭以及尾 部紅外攝像頭，在水下機(jī)器人上安裝探測(cè)聲吶，在水下機(jī)器人的前端對(duì)稱安裝兩個(gè)前向紅 外攝像頭，在水下機(jī)器人頂部?jī)蓚?cè)位置分別安裝側(cè)向紅外攝像頭，在水下機(jī)器人尾部對(duì)稱 安裝尾部紅外攝像頭，所述避障方法包括以下步驟：
[0008] (1)根據(jù)水下機(jī)器人與目標(biāo)點(diǎn)的位置，以及海圖數(shù)據(jù)庫(kù)信息離線規(guī)劃出水下機(jī)器 人的全局參考軌跡，設(shè)定如下代價(jià)函數(shù)使得水下機(jī)器人在無(wú)障礙情況下能夠移動(dòng)到目標(biāo)點(diǎn)
[0009]
【主權(quán)項(xiàng)】
1. 一種基于距離與視差信息的水下機(jī)器人避障方法，包括水下機(jī)器人、探測(cè)聲吶、前向 紅外攝像頭、側(cè)向紅外攝像頭以及尾部紅外攝像頭，在水下機(jī)器人上安裝探測(cè)聲吶，在水下 機(jī)器人的前端對(duì)稱安裝兩個(gè)前向紅外攝像頭，在水下機(jī)器人頂部?jī)蓚?cè)位置分別安裝側(cè)向紅 外攝像頭，在水下機(jī)器人尾部對(duì)稱安裝兩個(gè)尾部紅外攝像頭，其特征在于，所述避障方法包 括以下步驟： (1) 根據(jù)水下機(jī)器人與目標(biāo)點(diǎn)的位置，以及海圖數(shù)據(jù)庫(kù)信息離線規(guī)劃出水下機(jī)器人的 全局參考軌跡，設(shè)定如下代價(jià)函數(shù)使得水下機(jī)器人在無(wú)障礙情況下能夠移動(dòng)到目標(biāo)點(diǎn)
式中，《>0與U>0為比例因子；
為參考軌跡，qQ(t)GR3與
分別表示參考軌跡的位置與速度向量； 所述水下機(jī)器人視為一個(gè)質(zhì)量為m的質(zhì)點(diǎn)，質(zhì)點(diǎn)位置為水下機(jī)器人中心點(diǎn)，質(zhì)點(diǎn)在 空間坐標(biāo)系中有三個(gè)自由度，即q(t) = [x(t)，y(t)，z(t)]T;通過(guò)反饋線性化，水下機(jī) 器人的運(yùn)動(dòng)學(xué)方程可以用二階積分器模型表示，其中速度與控制輸入向量分別表示為
與u(t) =[ux(t)，uy(t)，uz(t) ]T;控制輸入約束表示為-Uxmin<ux(t )<Uxmax、_Uymin< U y(t) < 1^眶與_U zmin< U z (t) < Uzmax，其中Uxmin>0、Uxmax>0、Uymin>0、Uymax>0、 uzmin>〇與uzniax>0為輸入飽和常數(shù)； (2) 探測(cè)聲吶作為全局障礙測(cè)量?jī)x器，監(jiān)測(cè)水下機(jī)器人周邊遠(yuǎn)距離環(huán)境信息，獲取水下 機(jī)器人與障礙物的相對(duì)距離信息； 紅外攝像頭作為局部障礙測(cè)量?jī)x器，監(jiān)測(cè)水下機(jī)器人周邊近距離環(huán)境信息，獲取近距 離障礙物的視角信息； (3) 判斷是否需要避障，如果需要避障，根據(jù)相對(duì)距離信息，利用預(yù)測(cè)控制調(diào)整避障策 略，構(gòu)造基于預(yù)測(cè)控制的優(yōu)化算法；如果不需要避障則按照原路線行駛； (4) 若水下機(jī)器人到達(dá)目標(biāo)點(diǎn)，則軌跡規(guī)劃結(jié)束，完成任務(wù)；若水下機(jī)器人未到達(dá)目標(biāo) 點(diǎn)，則重新執(zhí)行步驟（2)。
2. 根據(jù)權(quán)利要求1所述的基于距離與視差信息的水下機(jī)器人避障方法，其特征在于， 所述步驟（3)中的避障策略如下： 首先，定義障礙物集合；設(shè)定障礙物的空間范圍可通過(guò)多個(gè)球的交集/并集來(lái)表示；利 用球的疊加，最終描述出來(lái)的障礙物區(qū)域?yàn)椴灰?guī)則形狀或規(guī)則形狀；因此，第m(m= 1，…） 個(gè)障礙物包含在一個(gè)二維圓內(nèi)，用符號(hào)Bm(Om,pm)表不，其中Om(OmGR3)與pm(pm>〇)分 別表示此球的中心與半徑，進(jìn)而水下機(jī)器人與障礙物m的避碰約束為||q-〇m| |>Pm+ds; 其次，劃分障礙物所處的區(qū)域；設(shè)定d為障礙物與水下機(jī)器人表面最近距離，dsaft為避 障警戒距離，4為避障遠(yuǎn)距離，ds為避障近距離，d ; 當(dāng)(1>4^時(shí)，障礙物處于警戒線之外，不需要躲避障礙； 當(dāng)dsafe>心屯時(shí)，障礙物處于遠(yuǎn)距離避障區(qū)域，水下機(jī)器人利用探測(cè)聲吶反饋的距離 信息進(jìn)行避障，構(gòu)造基于距離信息的勢(shì)能函數(shù)
其中時(shí)，水下機(jī)器人可在不與障礙物發(fā)生碰撞前提下沿著障礙物影響域半徑 移動(dòng)； 當(dāng)士多心屯時(shí)，障礙物處于近距離避障區(qū)域，水下機(jī)器人利用紅外攝像頭反饋的視差 信息進(jìn)行避障，構(gòu)造視差函數(shù)作為預(yù)測(cè)控制算法的終點(diǎn)狀態(tài)控制器，前向紅外攝像頭 對(duì)方形障礙物的視角差為0i，側(cè)向紅外攝像頭對(duì)不規(guī)則形障礙物的視角差為0 2，水下機(jī) 器人運(yùn)動(dòng)方向角度為a，考慮水下機(jī)器人的運(yùn)動(dòng)方向，對(duì)視差進(jìn)行校正，得出新的視差^與 4，
構(gòu)造類似于
的視差函數(shù)以作為終點(diǎn)狀態(tài)控制器，其中k^X)為 加權(quán)值； 融合距離與視差信息構(gòu)造避障函數(shù)，
，作為預(yù)測(cè) 控制算法的終點(diǎn)狀態(tài)控制器；其中dsaft多d>ds。
3.根據(jù)權(quán)利要求1所述的基于距離與視差信息的水下機(jī)器人避障方法，其特征在于： 所述的預(yù)測(cè)控制的優(yōu)化算法，在任意采樣時(shí)刻tk，求解涉及狀態(tài)約束和控制約束的有限時(shí) 域開環(huán)優(yōu)化問(wèn)題得到一組控制序列u%得出預(yù)測(cè)時(shí)域內(nèi)的假定輸出，并將當(dāng)前的控制輸入 作用于被控對(duì)象；在下一采樣時(shí)刻tk+1，基于新的初始狀態(tài)重復(fù)上一時(shí)刻的優(yōu)化過(guò)程，不斷 滾動(dòng)優(yōu)化，從而形成閉環(huán)控制，完成無(wú)碰撞軌跡優(yōu)化任務(wù)。
【專利摘要】一種基于距離與視差信息的水下機(jī)器人避障方法，包括以下步驟：控制裝置根據(jù)作業(yè)任務(wù)和海圖數(shù)據(jù)庫(kù)信息對(duì)水下機(jī)器人進(jìn)行全局軌跡規(guī)劃；探測(cè)聲吶與攝像頭分別作為全局與局部障礙測(cè)量?jī)x器，以獲取水下機(jī)器人與障礙物的相對(duì)距離與視差信息；控制裝置進(jìn)而判斷是否需要避障，利用預(yù)測(cè)控制調(diào)整避障策略；若水下機(jī)器人達(dá)到目標(biāo)點(diǎn)，則軌跡規(guī)劃結(jié)束。本發(fā)明具有可動(dòng)態(tài)調(diào)整壁障策略、路徑更優(yōu)、躲避效果好、可靠性高等優(yōu)點(diǎn)，能夠應(yīng)對(duì)復(fù)雜障礙，提高了水下機(jī)器人執(zhí)行任務(wù)的可靠性。
【IPC分類】G05D1-10
【公開號(hào)】CN104571128
【申請(qǐng)?zhí)枴緾N201410827493
【發(fā)明人】閆敬, 楊晛, 關(guān)新平, 羅小元, 華長(zhǎng)春
【申請(qǐng)人】燕山大學(xué)
【公開日】2015年4月29日
【申請(qǐng)日】2014年12月26日