姚金藝, 曾慶軍, 周啟潤(rùn), 朱志宇, 包靈卉
全驅(qū)動(dòng)AUV系統(tǒng)路徑跟蹤設(shè)計(jì)與實(shí)現(xiàn)
姚金藝, 曾慶軍, 周啟潤(rùn), 朱志宇, 包靈卉
(江蘇科技大學(xué) 電子信息學(xué)院, 江蘇 鎮(zhèn)江, 212003)
針對(duì)全驅(qū)動(dòng)自主水下航行器(AUV)水下搜救和回收等應(yīng)用需求, 提出一種基于全球定位系統(tǒng)(GPS)、多普勒計(jì)程儀(DVL)、光纖慣導(dǎo)和深度計(jì)及航路點(diǎn)跟蹤的AUV路徑跟蹤控制系統(tǒng)。結(jié)合自主研發(fā)的“探海Ι型”AUV樣機(jī), 介紹了AUV分布式系統(tǒng)組成結(jié)構(gòu)、基于MOOS庫(kù)的軟件開(kāi)發(fā)設(shè)計(jì)以及基于PC104總線架構(gòu)開(kāi)發(fā)的硬件設(shè)計(jì), 并針對(duì)AUV系統(tǒng)的路徑跟蹤功能, 闡述了航路點(diǎn)跟蹤原理及以GPS、DVL、光纖慣導(dǎo)和深度計(jì)為基礎(chǔ)的導(dǎo)航算法, 并設(shè)計(jì)了比例-積分-微分(PID)路徑跟蹤控制器, 最后進(jìn)行了路徑跟蹤湖試, 經(jīng)過(guò)對(duì)試驗(yàn)曲線的分析, 驗(yàn)證了該方法的有效性, 為今后進(jìn)一步優(yōu)化路徑跟蹤探明了一條可行的技術(shù)途徑。
自主水下航行器; 路徑跟蹤; 比例-積分-微分控制器; 航路點(diǎn)跟蹤
隨著科技不斷發(fā)展, 自主水下機(jī)器人(auto- nomous undersea vehicle, AUV)成為人類進(jìn)行海洋探索的重要工具, 在軍事和民用領(lǐng)域中朝著更為自主、更長(zhǎng)的作業(yè)時(shí)間、更高的隱蔽性、更強(qiáng)大的數(shù)據(jù)交互能力的趨勢(shì)前進(jìn)[1-2]。而路徑跟蹤作為AUV的重要功能, 對(duì)于AUV是否能夠精準(zhǔn)完成規(guī)定使命, 順利進(jìn)行回收布放等任務(wù)具有重要意義[3]。
目前, 世界上已經(jīng)有十幾個(gè)國(guó)家在執(zhí)行多種用途的AUV研制計(jì)劃, 有的甚至已經(jīng)可以定制并商業(yè)化運(yùn)作。國(guó)內(nèi)外針對(duì)AUV路徑跟蹤控制問(wèn)題, 特別是欠驅(qū)動(dòng) AUV 路徑跟蹤問(wèn)題的高水平研究主要集中在最近10年時(shí)間。目前針對(duì)AUV路徑跟蹤問(wèn)題, 大多將三維路徑進(jìn)行解耦, 分成水平面和垂直面進(jìn)行分別控制。Lapierre等[4-5]針對(duì)AUV的水平面路徑跟蹤問(wèn)題, 基于虛擬向?qū)Ы⒙窂礁櫿`差方程, 基于反步法設(shè)計(jì)艏向控制器。Oh等[6]采用視線引導(dǎo)法對(duì)航路點(diǎn)跟蹤建模, 通過(guò)求解線性約束優(yōu)化問(wèn)題來(lái)提高路徑跟蹤控制器性能。邊信黔等[7]采用滑模變結(jié)構(gòu)控制方法進(jìn)行AUV路徑跟蹤控制器設(shè)計(jì), 并考慮了AUV模型參數(shù)不確定性和海流干擾的影響。上述研究可以看出, 當(dāng)前對(duì)于AUV的路徑跟蹤研究主要集中在二維平面內(nèi), 絕大部分還停留于理論研究階段, 能真正應(yīng)用于實(shí)際系統(tǒng)只是少數(shù)。
文中結(jié)合自主研發(fā)的“探海Ⅰ型”AUV樣機(jī), 介紹了其結(jié)構(gòu)和軟硬件設(shè)計(jì), 并且針對(duì)路徑跟蹤功能, 闡述了航路點(diǎn)跟蹤原理[8], 以全球定位系統(tǒng)(global positioning system, GPS)、多普勒速度計(jì)程儀(Doppler velocity log, DVL)、光纖慣導(dǎo)和深度計(jì)為基礎(chǔ)的導(dǎo)航算法與比例-積分-微分(proportion-integral-derivative, PID)控制器設(shè)計(jì), 最后進(jìn)行湖試, 驗(yàn)證了路徑跟蹤功能。
“探海Ⅰ型”AUV系統(tǒng)主要由水面設(shè)備和水下設(shè)備組成, 水面設(shè)備由光端機(jī)(水上)、無(wú)線數(shù)傳設(shè)備、光滑環(huán)、光纖絞盤(pán)和岸基控制單元等組成。水下部分為AUV實(shí)體, 主要包括艏段, 艏部推進(jìn)段, 電子艙段, 艉部推進(jìn)段和主推進(jìn)段5部分。AUV的結(jié)構(gòu)平面圖如圖1所示。
AUV控制系統(tǒng)分為甲板單元和水下部分, 甲板單元的硬件設(shè)備主要包括水面控制箱(光端機(jī)和無(wú)線數(shù)傳設(shè)備)、監(jiān)控計(jì)算機(jī)以及水密光纖套組, 甲板單元的主要作用是:
圖1 “探海Ι型”自主水下航行器結(jié)構(gòu)平面圖
1) 將AUV采集到的視頻圖像和聲吶圖像、AUV的航向姿態(tài)、地理方位、漏水檢測(cè)和電源狀態(tài)等在甲板單元上顯示出來(lái);
2) 將上位機(jī)的控制指令下達(dá)給下位機(jī), 完成AUV的自主任務(wù)。
水下部分分為自動(dòng)駕駛單元、運(yùn)動(dòng)控制單元、導(dǎo)航單元與信息測(cè)量單元。
自動(dòng)駕駛單元主要包括主控系統(tǒng)和電源管理系統(tǒng)兩部分。主控系統(tǒng)為PC104工控主板, 其作為整個(gè)AUV的控制核心和樞紐, 主要完成各傳感器信息的采集、處理和傳輸, 并接收各種指令, 控制AUV完成相應(yīng)任務(wù)。主控系統(tǒng)與甲板單元和各設(shè)備之間的連接如圖2所示。
圖2 AUV設(shè)備連接圖
AUV實(shí)體樣機(jī)如圖3所示。導(dǎo)航單元以光纖慣導(dǎo)為核心, 通過(guò)串口通信接收DVL、深度計(jì)和GPS信息, 獲取AUV 當(dāng)前的位置信息, 進(jìn)行數(shù)據(jù)儲(chǔ)存或發(fā)送至主控單元; 此外, 導(dǎo)航單元還可以從主控單元接收傳感器的設(shè)置信息或?qū)Ш蕉ㄎ恍枰臄?shù)據(jù), 進(jìn)行自身校準(zhǔn)或轉(zhuǎn)發(fā)給相應(yīng)傳感器。
圖3 AUV實(shí)體樣機(jī)
文中軟件系統(tǒng)基于嵌入式PC104架構(gòu), 使用MOOS庫(kù)進(jìn)行應(yīng)用開(kāi)發(fā)。MOOS是麻省理工大學(xué)開(kāi)發(fā)的專門(mén)為水下機(jī)器人設(shè)計(jì)的C++開(kāi)源軟件架構(gòu),具有星型拓?fù)浣Y(jié)構(gòu)、模塊化和分布式的特點(diǎn)。軟件系統(tǒng)主要包括中央控制單元MOOSDB、傳感器數(shù)據(jù)采集單元、PID運(yùn)動(dòng)控制單元、pHelm行為控制單元和岸基控制單元, 各單元主要功能如圖4所示。
圖4 AUV軟件系統(tǒng)結(jié)構(gòu)圖
1) 中央控制單元MOOSDB: MOOSDB是整個(gè)MOOS軟件系統(tǒng)的核心, AUV在運(yùn)行時(shí)的所有數(shù)據(jù)均存于此數(shù)據(jù)庫(kù), 任何需要特定數(shù)據(jù)的應(yīng)用軟件通過(guò)訂閱此數(shù)據(jù)庫(kù)即可獲取相應(yīng)信息。
2) 傳感器數(shù)據(jù)采集單元: 主要對(duì)GPS地理坐標(biāo)采集、光纖慣導(dǎo)姿態(tài)和加速度信息采集、DVL三軸速度采集和深度計(jì)深度采集以及電機(jī)轉(zhuǎn)速信息等數(shù)據(jù)的采集, 為AUV實(shí)現(xiàn)運(yùn)動(dòng)控制和位置推算等功能提供數(shù)據(jù)。
3) PID運(yùn)動(dòng)控制單元: PID控制單元通過(guò)訂閱pHelm行為控制單元期望航向、期望速度和期望深度等信息, 采用控制算法, 輸出相應(yīng)的AUV各推進(jìn)器轉(zhuǎn)速。
4) pHelm行為控制單元: 根據(jù)從中央控制單元獲取傳感器數(shù)據(jù)信息計(jì)算得到AUV當(dāng)前的位置信息, 并將AUV不同的運(yùn)動(dòng)行為信息進(jìn)行融合。
5) 岸基控制單元: 即上位機(jī)監(jiān)控界面系統(tǒng), 主要通過(guò)光纖、WIFI和無(wú)線數(shù)傳電臺(tái)3種通信方式對(duì)AUV進(jìn)行設(shè)備監(jiān)控和任務(wù)使命下達(dá)。
圖5 AUV坐標(biāo)構(gòu)建圖
表1列舉了AUV位置分量、角度分量、線速度分量、角速度分量、力分量和力矩分量在相應(yīng)坐標(biāo)系下的符號(hào)含義。
表1 AUV運(yùn)動(dòng)參數(shù)定義
得到旋轉(zhuǎn)變換矩陣
可以將動(dòng)坐標(biāo)系坐標(biāo)轉(zhuǎn)化到大地坐標(biāo)中, 即
反變換后得
每一條使命路徑由若干個(gè)航路點(diǎn)構(gòu)成, 這些航路點(diǎn)在-平面內(nèi)定義。如圖6所示, 使命的基本目的是同一系列坐標(biāo)系航跡點(diǎn)。捕獲半徑是指AUV在捕獲半徑內(nèi)意味著到達(dá)目標(biāo)點(diǎn), 足夠接近半徑指在此半徑內(nèi)AUV可以通過(guò)減速等一系列決策以達(dá)到目標(biāo)點(diǎn)。
圖6 航路點(diǎn)跟蹤原理圖
與傳統(tǒng)的航路點(diǎn)控制算法相比, 文中采用的路徑跟蹤控制算法根據(jù)橫向距離實(shí)時(shí)切換跟蹤航路點(diǎn), 在應(yīng)對(duì)海流干擾時(shí)具有較好的路徑跟蹤能力。
AUV要想實(shí)現(xiàn)路徑跟蹤功能, 需要對(duì)水下推進(jìn)器有較為精確的控制能力, 必須實(shí)現(xiàn)推進(jìn)器的閉環(huán)控制。綜上所述, 文中AUV采用PID作為路徑跟蹤控制器, 如圖8所示。
圖8 比例-積分-微分控制框圖
欠驅(qū)動(dòng)AUV的路徑跟蹤控制主要將AUV解耦為水平面和縱平面控制, 通過(guò)設(shè)計(jì)艏向、深度、速度控制器輸出舵機(jī)舵角和推進(jìn)器轉(zhuǎn)速。而全驅(qū)動(dòng)型AUV將控制量分為平移控制量(軸、軸、軸)、轉(zhuǎn)矩控制量(軸、軸), 其中在航路點(diǎn)跟蹤模式下啟用軸速度控制、軸深度控制、軸轉(zhuǎn)矩縱傾角控制和軸轉(zhuǎn)矩航向角控制。
AUV路徑跟蹤依賴于航位推算。AUV水下航位推算系統(tǒng)包含光纖慣導(dǎo)的姿態(tài)數(shù)據(jù)采集和處理、深度計(jì)測(cè)量、水平經(jīng)緯度推算。其中水平經(jīng)緯度的推算采用AUV入水點(diǎn)經(jīng)緯度坐標(biāo)作為初始狀態(tài), 以AUV航向、速度和傳感器采樣時(shí)間作為依據(jù), 從而推出其在水下的位置。其中深度通道采用AUV深度計(jì)測(cè)量值。
首先計(jì)算AUV在GPS大地坐標(biāo)系下的位移增量
地球幾何形狀為橢球體, AUV的位移增量在緯線圈和子午圈中近似表示為一段圓弧, 通過(guò)除以曲率半徑可以得到經(jīng)緯度增量。其中, 緯線圈可以看作正圓, 并隨緯度增加而變小, 子午圈是一個(gè)扁平的橢圓, 長(zhǎng)半軸為, 短半軸為。
(8)
結(jié)合式(1), 在t(> 0) 時(shí)刻, AUV在水下的經(jīng)緯度可表示為[10]
為驗(yàn)證所研制的“探海Ⅰ型”AUV能否滿足設(shè)計(jì)要求, 進(jìn)行了相關(guān)湖試試驗(yàn)。試驗(yàn)地點(diǎn)選擇為某天然湖泊, 試驗(yàn)水域的水深范圍5~40 m, 湖底較平坦, 湖水流速不大于2 kn, 試驗(yàn)條件較好。
給定水面軌跡, 依賴航位推算系統(tǒng)進(jìn)行水面路徑跟蹤試驗(yàn), 設(shè)定AUV整個(gè)使命航速恒定為0.8 kn, 使命時(shí)間1 500 s, 初始位置GPS經(jīng)緯度為(120.317741, 31.109846), 起始航跡點(diǎn)GPS經(jīng)緯度為(120.319405, 31.109317), 目標(biāo)航跡點(diǎn)GPS經(jīng)緯度為(120.319405, 31.109317), 回收航跡點(diǎn)GPS經(jīng)緯度為(120.317 655, 31.109 634), 如圖9所示, A為起始點(diǎn)、B為回收點(diǎn)、C為目標(biāo)航路點(diǎn)。
圖9 水面航跡全球定位系統(tǒng)曲線
圖中, AUV根據(jù)GPS經(jīng)緯度信息逐漸接近預(yù)設(shè)航跡點(diǎn), 具體實(shí)現(xiàn)方法是通過(guò)航路點(diǎn)算法得到期望航向角, 將其作為PID閉環(huán)控制的輸入進(jìn)行路徑跟蹤控制, AUV期望航向與實(shí)際航向如圖10所示, 水面路徑跟蹤試驗(yàn)部分試驗(yàn)數(shù)據(jù)如表2所示, AUV總體上完成了水面航跡跟蹤任務(wù), 但由于試驗(yàn)當(dāng)天天氣造成GPS導(dǎo)航精度變差和湖面水流干擾, 航跡存在一些波動(dòng), AUV不能較好地沿著直線朝C點(diǎn)行進(jìn)。
圖10 期望航向與實(shí)際航向的對(duì)比曲線圖
表2 水面路徑跟蹤試驗(yàn)部分?jǐn)?shù)據(jù)
在上位機(jī)中給定巡航中心點(diǎn)和運(yùn)動(dòng)圖形, 如圖11所示, 依賴航位推算系統(tǒng)進(jìn)行水下路徑跟蹤試驗(yàn)。使命規(guī)劃: AUV航速0.8 kn, 下潛深度3 m, 初始位置GPS經(jīng)緯度為(120.317 474, 31.109 632), 起始航跡點(diǎn)GPS經(jīng)緯度為(120.318199, 31.109 256),回收航跡點(diǎn)GPS經(jīng)緯度為(120.317 421, 31.109 615),巡航圖形為六邊形, 巡航中心位置為(120.318199, 31.109 256), 自主導(dǎo)航時(shí)間為600 s。根據(jù)航位推算出的經(jīng)緯度畫(huà)出巡航軌跡如圖12所示。圖13為航跡的二維平面圖, 圖中不包含回收曲線。表3為部分試驗(yàn)數(shù)據(jù), 可以看出AUV進(jìn)行自主巡航的穩(wěn)定性很高, 巡航使命結(jié)束浮出水面返回回收點(diǎn)的過(guò)程中, 湖面起風(fēng), 水流波動(dòng)較大, 導(dǎo)致AUV存在部分軌跡雜亂。另外由于AUV自身的穩(wěn)心所帶來(lái)下潛時(shí)的晃動(dòng), 使得深度跟蹤存在一定的波動(dòng)。但是總體上使用航路點(diǎn)跟蹤方法進(jìn)行AUV水下路徑跟蹤的路徑辨識(shí)度較高。
圖11 上位機(jī)航跡規(guī)劃界面圖
圖12 水下六邊形自主巡航經(jīng)緯度軌跡
在AUV執(zhí)行路徑跟蹤使命時(shí), 通過(guò)GPS、光纖慣導(dǎo)、深度計(jì)和DVL多種傳感器進(jìn)行航位推算, 同時(shí)采用航路點(diǎn)跟蹤和PID控制器作為路徑跟蹤控制, 較好地解決了AUV導(dǎo)航精度不夠和洋流干擾的問(wèn)題。
圖13 水下六邊形自主巡航經(jīng)緯度軌跡二維平面圖
表3 水下路徑跟蹤試驗(yàn)部分?jǐn)?shù)據(jù)
路徑跟蹤作為AUV的重要功能, 一直是國(guó)內(nèi)外研究的熱點(diǎn)。文中通過(guò)光纖慣導(dǎo)、深度計(jì)、GPS和DVL等硬件設(shè)備, 以航路點(diǎn)跟蹤原理為基礎(chǔ), PID作為路徑跟蹤控制器, 實(shí)現(xiàn)了“探海Ⅰ型”AUV的路徑跟蹤功能, 并且通過(guò)湖試, 對(duì)航路點(diǎn)跟蹤原理、導(dǎo)航算法以及控制器的有效性進(jìn)行了驗(yàn)證。試驗(yàn)結(jié)果表明, 以PID作為控制器的航路點(diǎn)路徑跟蹤能夠較好地完成路徑跟蹤任務(wù)。當(dāng)前AUV采用常規(guī)的PID控制器, 今后將采用先進(jìn)的智能控制方法來(lái)提高AUV路徑跟蹤效果; 在路徑跟蹤應(yīng)用方面, 會(huì)將此AUV應(yīng)用于目標(biāo)探測(cè)、回收等實(shí)際任務(wù)需求, 并且開(kāi)展對(duì)欠驅(qū)動(dòng)AUV路徑跟蹤的研究工作。
[1] 王芳, 萬(wàn)磊, 李曄, 等. 欠驅(qū)動(dòng)AUV的運(yùn)動(dòng)控制技術(shù)綜述[J]. 中國(guó)造船, 2010, 51(2): 227-241.Wang Fang, Wan Lei, Li Ye, et al. A Survey on Development of Motion Control for Underactuated AUV[J]. Shipbuilding of China, 2010, 51(2): 227-241.
[2] 鐘宏偉. 國(guó)外無(wú)人水下航行器裝備與技術(shù)現(xiàn)狀及展望[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2017, 25(4): 215-225.Zhong Hong-wei. Review and Prospect of Equipment and Techniques for Unmanned Undersea Vehicle in Foreign Countries[J]. Journal of Unmanned Undersea Systems, 2017, 25(4): 215-225.
[3] 趙朝聞, 張淞, 李輝. 基于超短基線的AUV自主對(duì)接流程及算法[J]. 魚(yú)雷技術(shù), 2016, 24(3): 166-171.Zhao Chao-wen, Zhang Song, Li Hui. Autonomous Abutting Joint Flow and Control Algorithm for AUV Based on Ultra-short Baseline[J]. Torpedo Technology, 2016, 24(3): 166-171.
[4] Lapierre L, Soetanto D. Nonlinear Path-following Control of an AUV[J]. Ocean Engineering, 2007, 34(11): 1734- 1744.
[5] Lapierre L, Jouvencel B. Robust Nonlinear Path-following Control of an AUV[J]. IEEE Journal of Oceanic Engineering, 2008, 33(2): 89-102.
[6] Oh S R, Sun J. Path Following of Underactuated Marine Surface Vessels Using Line of Sight Based Model Predictive Control[J]. Ocean Engineering, 2010, 37(2-3): 289- 295.
[7] 邊信黔, 程相勤, 賈鶴鳴, 等. 基于迭代滑模增量反饋的欠驅(qū)動(dòng)AUV地形跟蹤控制[J]. 控制與決策, 2011, 26(2): 289-292, 296.Bian Xin-qian, Cheng Xiang-qin, Jia He-ming, et al. A Bottom-following Controller for Underactuated AUV Based on Iterative Sliding and Increment Feedback[J]. Control and Decision, 2011, 26(2): 289-292, 296.
[8] 高劍, 李勇強(qiáng), 李璐瓊, 等. 基于航路點(diǎn)跟蹤的AUV回收控制[J]. 火力與指揮控制, 2013, 38(8): 103-106.Gao Jian, Li Yong-qiang, Li Lu-qiong, et al. Autonomous Recovery of an AUV Based on Way-Point Tracking Control[J]. Fire Control & Command Control, 2013, 38(8): 103-106.
[9] 嚴(yán)浙平, 宋金雪. 模型輔助和洋流估計(jì)下的UUV導(dǎo)航定位方法[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2017, 25(5): 319-325.Yan Zhe-ping, Song Jin-xue. UUV Navigation and Positioning Method Based on Model-Aided and Ocean Current Estimation[J]. Journal of Unmanned Undersea Systems, 2017, 25(5): 319-325.
[10] 周佳加, 邊信黔, 王宏健, 等. 基于小波的無(wú)人潛航器航位推算算法研究[J]. 儀器儀表學(xué)報(bào), 2012, 33(3): 670-676.Zhou Jia-jia, Bian Xin-qian, Wang Hong-jian, et al. Study on Dead Reckoning Method for Unmanned Underwate Vehicle Based on Wavelet[J]. Chinese Journal of Scientific Instrument, 2012, 33(3): 670-676.
Design and Implementation of a Path Tracking System for Fully Actuated AUV
YAO Jin-yi, ZENG Qing-jun, ZHOU Qi-run, ZHU Zhi-yu, BAO Ling-hui
(School of Electronics and Information, Jiangsu University of Science and Technology, Zhenjiang 212003, China)
Aiming at the application requirements for such as recovery, underwater search and rescue, and so on, a path tracking control system for fully actuated autonomous undersea vehicle(AUV) is proposed based on global positioning system(GPS), Doppler velocity log(DVL), optical fiber inertial navigation, depth gauge and waypoint tracking. The distributed system structure of AUV, the software design based on MOOS, the hardware design based on PC104 bus architecture are introduced according to the independently developed T-Sea I type AUV prototype. The principle of waypoint tracking and the navigation algorithm based on GPS, DVL, optical fiber inertial navigation and depth gauge are discussed, and then a proportion-integral-derivative(PID) path tracking controller is designed. Lake test of path tracking is carried out and the test curves are analyzed to verify the validity of the proposed method.
autonomous undersea vehicle(AUV); path tracking; proportion-integral-derivative(PID) controller; waypoint tracking
TJ630; TB568
A
2096-3920(2019)04-0452-07
10.11993/j.issn.2096-3920.2019.04.014
姚金藝, 曾慶軍, 周啟潤(rùn), 等. 全驅(qū)動(dòng)AUV系統(tǒng)路徑跟蹤設(shè)計(jì)與實(shí)現(xiàn)[J]. 水下無(wú)人系統(tǒng)學(xué)報(bào), 2019, 27(4): 452-458.
2019-01-17;
2019-03-29.
國(guó)防基礎(chǔ)科研計(jì)劃項(xiàng)目(JCKY2017414C002); 國(guó)家自然科學(xué)基金資助項(xiàng)目(11574120); 江蘇省產(chǎn)業(yè)前瞻與共性關(guān)鍵技術(shù)項(xiàng)目(BE2018103).
姚金藝(1993-), 男, 在讀碩士, 研究方向?yàn)樗聶C(jī)器人運(yùn)動(dòng)控制.
(責(zé)任編輯: 許 妍)