基于距離與視差信息的水下機器人避障方法
【技術領域】
[0001] 本發(fā)明涉及水下機器人智能控制技術領域，尤其是一種融合距離與視差信息的水 下機器人避障方法。
【背景技術】
[0002] 水下機器人在海難搜救、水下防入侵技術、物種跟蹤探測等領域具有廣泛的應用 價值。在水下環(huán)境中，沉船等沉積物以及礁石等使得水下環(huán)境非常復雜，如果水下機器人不 能主動、有效地躲避這些障礙，將有可能導致水下機器人執(zhí)行任務的失敗甚至導致水下機 器人損壞。
[0003] 在現(xiàn)有技術中檢索發(fā)現(xiàn)，中國專利申請?zhí)枮?01310488141. 8,名稱為：基于前視 聲納的水下機器人避碰方法，該方法前視聲納圖像數(shù)據(jù)引入機器人避障策略中，可以減小 機器人避碰盲區(qū)。但上述方法在設計無碰撞軌跡規(guī)劃算法時，并沒有考慮水下機器人的控 制輸入約束。在實際應用中，水下機器人一般都是由電機驅動，而電機驅動器都有功率限 制，當輸入信號超過這一限制，飽和發(fā)生，進而使系統(tǒng)的動態(tài)性能降低（例如，超調(diào)量、調(diào)節(jié) 時間以及震蕩次數(shù)等），甚至導致閉環(huán)系統(tǒng)不穩(wěn)定。
[0004] 再有，中國專利申請?zhí)枮?01210539473.X，名稱為：一種智能水下機器人的自主 避障方法，該方法將水下機器人運動目標、障礙物和水下機器人控制性能進行統(tǒng)一考慮來 實現(xiàn)避障任務。上述方法是基于距離信息設計的，但僅考慮距離信息是不夠的，因為實時有 效的避障行為不僅需要距離信息，還需要視角信息，例如人們在規(guī)避障礙的過程中，有效 的避障路線是通過距離與視差調(diào)整來共同確立的。在水下三維環(huán)境中，方向性顯得更為重 要。因此，如何在水下復雜障礙物環(huán)境下，利用距離和視差信息對水下機器人設計無碰撞軌 跡優(yōu)化策略，以提高水下機器人軌跡規(guī)劃的有效性，顯得尤為重要。
【發(fā)明內(nèi)容】
[0005] 本發(fā)明目的在于提供一種可動態(tài)調(diào)整壁障策略、路徑更優(yōu)、躲避效果好、可靠性高 的基于距離與視差信息的水下機器人避障方法。
[0006] 為實現(xiàn)上述目的，采用了以下技術方案：
[0007] 本發(fā)明主要包括水下機器人、探測聲吶、前向紅外攝像頭、側向紅外攝像頭以及尾 部紅外攝像頭，在水下機器人上安裝探測聲吶，在水下機器人的前端對稱安裝兩個前向紅 外攝像頭，在水下機器人頂部兩側位置分別安裝側向紅外攝像頭，在水下機器人尾部對稱 安裝尾部紅外攝像頭，所述避障方法包括以下步驟：
[0008] (1)根據(jù)水下機器人與目標點的位置，以及海圖數(shù)據(jù)庫信息離線規(guī)劃出水下機器 人的全局參考軌跡，設定如下代價函數(shù)使得水下機器人在無障礙情況下能夠移動到目標點
[0009]
【主權項】
1. 一種基于距離與視差信息的水下機器人避障方法，包括水下機器人、探測聲吶、前向 紅外攝像頭、側向紅外攝像頭以及尾部紅外攝像頭，在水下機器人上安裝探測聲吶，在水下 機器人的前端對稱安裝兩個前向紅外攝像頭，在水下機器人頂部兩側位置分別安裝側向紅 外攝像頭，在水下機器人尾部對稱安裝兩個尾部紅外攝像頭，其特征在于，所述避障方法包 括以下步驟： (1) 根據(jù)水下機器人與目標點的位置，以及海圖數(shù)據(jù)庫信息離線規(guī)劃出水下機器人的 全局參考軌跡，設定如下代價函數(shù)使得水下機器人在無障礙情況下能夠移動到目標點
式中，《>0與U>0為比例因子；
為參考軌跡，qQ(t)GR3與
分別表示參考軌跡的位置與速度向量； 所述水下機器人視為一個質量為m的質點，質點位置為水下機器人中心點，質點在 空間坐標系中有三個自由度，即q(t) = [x(t)，y(t)，z(t)]T;通過反饋線性化，水下機 器人的運動學方程可以用二階積分器模型表示，其中速度與控制輸入向量分別表示為
與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) 探測聲吶作為全局障礙測量儀器，監(jiān)測水下機器人周邊遠距離環(huán)境信息，獲取水下 機器人與障礙物的相對距離信息； 紅外攝像頭作為局部障礙測量儀器，監(jiān)測水下機器人周邊近距離環(huán)境信息，獲取近距 離障礙物的視角信息； (3) 判斷是否需要避障，如果需要避障，根據(jù)相對距離信息，利用預測控制調(diào)整避障策 略，構造基于預測控制的優(yōu)化算法；如果不需要避障則按照原路線行駛； (4) 若水下機器人到達目標點，則軌跡規(guī)劃結束，完成任務；若水下機器人未到達目標 點，則重新執(zhí)行步驟（2)。
2. 根據(jù)權利要求1所述的基于距離與視差信息的水下機器人避障方法，其特征在于， 所述步驟（3)中的避障策略如下： 首先，定義障礙物集合；設定障礙物的空間范圍可通過多個球的交集/并集來表示；利 用球的疊加，最終描述出來的障礙物區(qū)域為不規(guī)則形狀或規(guī)則形狀；因此，第m(m= 1，…） 個障礙物包含在一個二維圓內(nèi)，用符號Bm(Om,pm)表不，其中Om(OmGR3)與pm(pm>〇)分 別表示此球的中心與半徑，進而水下機器人與障礙物m的避碰約束為||q-〇m| |>Pm+ds; 其次，劃分障礙物所處的區(qū)域；設定d為障礙物與水下機器人表面最近距離，dsaft為避 障警戒距離，4為避障遠距離，ds為避障近距離，d ; 當(1>4^時，障礙物處于警戒線之外，不需要躲避障礙； 當dsafe>心屯時，障礙物處于遠距離避障區(qū)域，水下機器人利用探測聲吶反饋的距離 信息進行避障，構造基于距離信息的勢能函數(shù)
其中時，水下機器人可在不與障礙物發(fā)生碰撞前提下沿著障礙物影響域半徑 移動； 當士多心屯時，障礙物處于近距離避障區(qū)域，水下機器人利用紅外攝像頭反饋的視差 信息進行避障，構造視差函數(shù)作為預測控制算法的終點狀態(tài)控制器，前向紅外攝像頭 對方形障礙物的視角差為0i，側向紅外攝像頭對不規(guī)則形障礙物的視角差為0 2，水下機 器人運動方向角度為a，考慮水下機器人的運動方向，對視差進行校正，得出新的視差^與 4，
構造類似于
的視差函數(shù)以作為終點狀態(tài)控制器，其中k^X)為 加權值； 融合距離與視差信息構造避障函數(shù)，
，作為預測 控制算法的終點狀態(tài)控制器；其中dsaft多d>ds。
3.根據(jù)權利要求1所述的基于距離與視差信息的水下機器人避障方法，其特征在于： 所述的預測控制的優(yōu)化算法，在任意采樣時刻tk，求解涉及狀態(tài)約束和控制約束的有限時 域開環(huán)優(yōu)化問題得到一組控制序列u%得出預測時域內(nèi)的假定輸出，并將當前的控制輸入 作用于被控對象；在下一采樣時刻tk+1，基于新的初始狀態(tài)重復上一時刻的優(yōu)化過程，不斷 滾動優(yōu)化，從而形成閉環(huán)控制，完成無碰撞軌跡優(yōu)化任務。
【專利摘要】一種基于距離與視差信息的水下機器人避障方法，包括以下步驟：控制裝置根據(jù)作業(yè)任務和海圖數(shù)據(jù)庫信息對水下機器人進行全局軌跡規(guī)劃；探測聲吶與攝像頭分別作為全局與局部障礙測量儀器，以獲取水下機器人與障礙物的相對距離與視差信息；控制裝置進而判斷是否需要避障，利用預測控制調(diào)整避障策略；若水下機器人達到目標點，則軌跡規(guī)劃結束。本發(fā)明具有可動態(tài)調(diào)整壁障策略、路徑更優(yōu)、躲避效果好、可靠性高等優(yōu)點，能夠應對復雜障礙，提高了水下機器人執(zhí)行任務的可靠性。
【IPC分類】G05D1-10
【公開號】CN104571128
【申請?zhí)枴緾N201410827493
【發(fā)明人】閆敬, 楊晛, 關新平, 羅小元, 華長春
【申請人】燕山大學
【公開日】2015年4月29日
【申請日】2014年12月26日