袁 健（1.國家海洋監(jiān)測設(shè)備工程技術(shù)研究中心；2.山東省海洋環(huán)境監(jiān)測技術(shù)重點(diǎn)實(shí)驗(yàn)室；3.山東省科學(xué)院海洋儀器儀表研究所，青島 266001）

欠驅(qū)動(dòng)式水下監(jiān)測機(jī)器人航路點(diǎn)軌跡跟蹤控制技術(shù)研究

袁 健1，2，3
（1.國家海洋監(jiān)測設(shè)備工程技術(shù)研究中心；2.山東省海洋環(huán)境監(jiān)測技術(shù)重點(diǎn)實(shí)驗(yàn)室；3.山東省科學(xué)院海洋儀器儀表研究所，青島 266001）

欠驅(qū)動(dòng)式水下剖面監(jiān)測機(jī)器人要實(shí)現(xiàn)對預(yù)定剖面軌跡航路點(diǎn)的跟蹤觀測，其航路點(diǎn)的軌跡跟蹤控制技術(shù)是實(shí)現(xiàn)高精度剖面觀測的關(guān)鍵。研究思路為：首先采用物理建模和實(shí)驗(yàn)驗(yàn)證方式對水下機(jī)器人進(jìn)行動(dòng)力學(xué)建模；其次通過設(shè)計(jì)分布式控制器A和控制器B以及兩者之間的通信協(xié)議，實(shí)現(xiàn)對通信時(shí)延的實(shí)時(shí)計(jì)算；然后通過擾動(dòng)前饋和逐次逼近轉(zhuǎn)換方法，設(shè)計(jì)能有效抑制臍帶纜等外擾動(dòng)并解決通信網(wǎng)絡(luò)時(shí)延的網(wǎng)絡(luò)化前饋-反饋?zhàn)顑?yōu)軌跡跟蹤控制方法。其中為實(shí)現(xiàn)平滑控制通過設(shè)置跟蹤誤差閾值和控制切換面，設(shè)計(jì)一種能實(shí)現(xiàn)無抖振航路點(diǎn)軌跡跟蹤的分段復(fù)合有限時(shí)間跟蹤控制方法。研究步驟為機(jī)器人動(dòng)力學(xué)建模、前饋-反饋?zhàn)顑?yōu)跟蹤控制和分段復(fù)合有限時(shí)間跟蹤控制算法設(shè)計(jì)以及控制方法的仿真和實(shí)驗(yàn)驗(yàn)證，分步進(jìn)行并驗(yàn)證設(shè)計(jì)方法的有效性，為欠驅(qū)動(dòng)式水下剖面監(jiān)測機(jī)器人的軌跡跟蹤控制技術(shù)提供理論依據(jù)。

欠驅(qū)動(dòng)式；水下剖面監(jiān)測機(jī)器人；軌跡跟蹤；有限時(shí)間跟蹤控制

1　引言

海洋環(huán)境監(jiān)測在開發(fā)海洋資源、預(yù)警海洋災(zāi)害、保護(hù)海洋環(huán)境等方面都有著重大意義［1，2］。海洋監(jiān)測技術(shù)發(fā)展已進(jìn)入立體綜合監(jiān)測階段。航空、航天遙感技術(shù)在一定程度上解決了從空中到海洋表層的實(shí)時(shí)和準(zhǔn)實(shí)時(shí)的同步、大面積觀測問題，海面觀測臺(tái)站、海上錨泊資料浮標(biāo)、水下潛標(biāo)、坐底式的海床基觀測系統(tǒng)等，可解決某一特定海區(qū)的定點(diǎn)觀測，調(diào)查船和漂流浮標(biāo)雖然可做斷面、剖面觀測，但又無法對特定區(qū)域，例如危險(xiǎn)區(qū)域、敏感區(qū)域進(jìn)行接近觀測［3-5］。而海洋水下剖面觀測平臺(tái)，則成為填補(bǔ)觀測空白的手段之一。AUV已廣泛應(yīng)用于海洋科學(xué)考察和海洋開發(fā)領(lǐng)域，在觀測海洋內(nèi)部過程中的作用日益顯著。在自主水下航行器（AUV）的開發(fā)和利用方面已有眾多的科研機(jī)構(gòu)參與，如美國伍茲霍爾研究所、大西洋大學(xué)、法國海洋開發(fā)研究院、俄羅斯希爾紹夫海洋資源研究院等國際知名科學(xué)院所都開展了AUV的研究和開發(fā)工作。伍茲霍爾海洋研究所（WHOI）在研究水下觀測平臺(tái)方面一直得到美國海軍方面的經(jīng)費(fèi)支持，技術(shù)水平一直處于領(lǐng)先水平。其中，“遠(yuǎn)程環(huán)境監(jiān)測裝置”（Remote Environmental Monitoring Units， REMUS）是最有代表性的水下觀測平臺(tái)，是一種低成本的海洋環(huán)境監(jiān)測和調(diào)查的多任務(wù)作業(yè)平臺(tái)。其研究得到美國海洋大氣局和海軍研究局的經(jīng)費(fèi)支持，目的是為了支持長期水下生態(tài)學(xué)環(huán)境觀測（LEO-15）和自動(dòng)海洋采樣網(wǎng)絡(luò)（AOSN）［6，7］。俄羅斯科學(xué)院遠(yuǎn)東分院目前正在研究利用太陽能作動(dòng)力的自主式水下移動(dòng)平臺(tái)。該平臺(tái)的突出特點(diǎn)是它在上浮時(shí)，太陽能電池板浮出水面，將太陽能轉(zhuǎn)化成電能為蓄電池充電，并完成觀測數(shù)據(jù)的發(fā)送、指令接收及定位［2］。中科院沈陽自動(dòng)化所開發(fā)了可用于大范圍水下環(huán)境監(jiān)測的水下滑翔機(jī)器人，并成功進(jìn)行了湖上試驗(yàn)。上海交通大學(xué)海洋工程國家重點(diǎn)實(shí)驗(yàn)室開展了深海水下機(jī)器人的產(chǎn)業(yè)化研制［8］；哈爾濱工程大學(xué)智能水下機(jī)器人重點(diǎn)實(shí)驗(yàn)室借助其在水下機(jī)器人領(lǐng)域的綜合優(yōu)勢，開展了AUV水下組合導(dǎo)航、動(dòng)力定位以及智能控制方法的研發(fā)［9］；西北工大近年在AUV的協(xié)同導(dǎo)航與協(xié)同控制研究方面取得了大量的科研成果［10，11］；浙江大學(xué)海洋工程系開展了深海水下移動(dòng)傳感網(wǎng)和水下運(yùn)載器的研發(fā)，并成功的進(jìn)行了海試；中國海洋大學(xué)信息學(xué)院開展了水下傳感器網(wǎng)絡(luò)組網(wǎng)實(shí)驗(yàn)與自主水下航行器的SLAM導(dǎo)航與協(xié)調(diào)控制方法的研究［12-14］；中國科學(xué)院海洋研究所研制了用于近海海洋環(huán)境檢測的輕型遙控機(jī)器人，可以檢測腐蝕電位、溫度、深度等16個(gè)參數(shù)［9］，北京大學(xué)工學(xué)院、北京理工大學(xué)自動(dòng)化學(xué)院和北航自動(dòng)化學(xué)院在水下多智能體系統(tǒng)的協(xié)調(diào)控制方法方面進(jìn)行了深入的研究，等。我國對海洋特定區(qū)域的海洋環(huán)境斷面、剖面觀測仍然存在一定的技術(shù)局限，一是水下監(jiān)測設(shè)備的采樣能力不足，監(jiān)測頻率低，機(jī)動(dòng)監(jiān)測能力不足。二是是僅限于海水表面的監(jiān)測，對一定深度的海水缺乏動(dòng)態(tài)監(jiān)測手段；或者僅限于某些固定海域的垂直剖面監(jiān)測，缺乏精細(xì)化監(jiān)測/觀測能力。海洋水下剖面觀測機(jī)器人平臺(tái)，成為填補(bǔ)觀測空白的手段之一。水下剖面監(jiān)測機(jī)器人要實(shí)現(xiàn)對預(yù)定剖面軌跡航路點(diǎn)的跟蹤觀測，其軌跡跟蹤控制技術(shù)是實(shí)現(xiàn)高精度剖面觀測的關(guān)鍵技術(shù)，但由于海流擾動(dòng)等因素的影響，很難精確控制機(jī)器人的位置和姿態(tài)［15，16］；另一方面，水下機(jī)器人自身蓄電池能量有限，綜合考慮外界洋流影響、航路點(diǎn)跟蹤精度和驅(qū)動(dòng)能量消耗的前饋-反饋?zhàn)顑?yōu)跟蹤控制技術(shù)是需要解決的關(guān)鍵科學(xué)問題之一。

圖1為項(xiàng)目組設(shè)計(jì)的水下剖面監(jiān)測機(jī)器人控制器［17-19］，包括置于船載控制箱內(nèi)的控制器1和置于水下機(jī)器人儀器艙內(nèi)的控制器2， 二者通過臍帶電纜進(jìn)行通信，控制器1實(shí)時(shí)采集船載控制器控制面板的遙控控制指令，并將控制指令按照自定義的通信協(xié)議進(jìn)行編碼和組裝成幀，經(jīng)由通信模塊發(fā)送到水下控制器2；水下控制器2實(shí)時(shí)接收船載控制器1發(fā)送的協(xié)議幀，并進(jìn)行協(xié)議解碼，按照協(xié)議格式解析出控制指令，進(jìn)而控制繼電器驅(qū)動(dòng)板上的對應(yīng)的繼電器的開關(guān)動(dòng)作，從而實(shí)現(xiàn)對水下機(jī)器人運(yùn)動(dòng)位置和姿態(tài)的控制［20］。該控制器可以實(shí)現(xiàn)手動(dòng)操控機(jī)器人和機(jī)器人的快速自動(dòng)軌跡跟蹤控制的自動(dòng)切換。水下機(jī)器人控制器一般采用集中控制方式，僅在水下機(jī)器人儀器艙內(nèi)設(shè)有一個(gè)控制器，通過多芯臍帶電纜將岸上的控制命令發(fā)送到水下控制器進(jìn)行采集，搖桿狀態(tài)與電纜的芯數(shù)相對應(yīng)，這樣會(huì)增加臍帶電纜的重量，對水下機(jī)器人的姿態(tài)控制帶來了外界不利影響。供電方式多采用船載220VAC電源直接供電方式，通過臍帶電纜為水下機(jī)器人供電，水下機(jī)器人儀器艙內(nèi)設(shè)有電壓轉(zhuǎn)換模塊；但由于交流電會(huì)對通信質(zhì)量帶來影響，使通信誤碼率較高，水下推進(jìn)器出現(xiàn)誤動(dòng)作的幾率較高。另外，若船載直接轉(zhuǎn)換為水下控制器電路板所需的工作電壓，則電壓在傳輸過程中由于電容效應(yīng)會(huì)出現(xiàn)壓降，達(dá)不到控制板電路板的正常工作電壓?？刂破髦g較多采用485通信方式，其傳輸距離多能達(dá)到1000米左右，由于受到水下擾動(dòng)的影響，其傳輸距離進(jìn)一步縮短；485通信方式難以滿足剖面監(jiān)測具有的大范圍、高深度監(jiān)測的特點(diǎn)。一般情形下光纜通信方式傳輸距離能達(dá)到幾公里，而且光纜通信質(zhì)量好，誤碼率極低，質(zhì)量輕，對水下剖面監(jiān)測機(jī)器人帶來的擾動(dòng)小，有利于機(jī)器人的精確控制，但較長的通信距離會(huì)引入較大的控制時(shí)滯［21，22］，導(dǎo)致控制性能下降甚至發(fā)散，不能準(zhǔn)確跟蹤預(yù)定的航路點(diǎn)［9］。如何克服時(shí)滯對控制性能的不利影響，設(shè)計(jì)高速通信網(wǎng)絡(luò)情形下的帶有時(shí)滯的、能有效抑制洋流影響的網(wǎng)絡(luò)化最優(yōu)跟蹤控制方法是需要解決的關(guān)鍵科學(xué)問題之一。為了減小機(jī)器人到達(dá)預(yù)定航路點(diǎn)的時(shí)間，研究有限時(shí)間的軌跡點(diǎn)跟蹤控制方法［23，24］，設(shè)計(jì)一種快速無抖振的水下剖面監(jiān)測機(jī)器人控制器是需要解決的關(guān)鍵問題之一。

圖1　水下剖面監(jiān)測機(jī)器人控制結(jié)構(gòu)圖

2　欠驅(qū)動(dòng)式機(jī)器人動(dòng)力學(xué)建模

3　航路點(diǎn)軌跡跟蹤控制算法設(shè)計(jì)

研究在水下機(jī)器人系統(tǒng)動(dòng)力學(xué)模型（1）的基礎(chǔ)上，實(shí)現(xiàn)航路點(diǎn)軌跡跟蹤控制算法的設(shè)計(jì)和驗(yàn)證。主要包括：

3.1分段復(fù)合有限時(shí)間跟蹤控制算法設(shè)計(jì)

水下擾動(dòng)源多、機(jī)器人動(dòng)力學(xué)系統(tǒng)非線性強(qiáng)，同時(shí)由于工作環(huán)境的復(fù)雜性和未知性，使得傳統(tǒng)的控制方式不能很好地解決外界擾動(dòng)情形下的運(yùn)動(dòng)控制，并且傳統(tǒng)的變結(jié)構(gòu)控制方法會(huì)導(dǎo)致控制器產(chǎn)生較大的抖振。水下機(jī)器人的有限時(shí)間軌跡控制器就是設(shè)計(jì)控制方法使水下機(jī)器人在有限的時(shí)間內(nèi)跟蹤上預(yù)定的參考軌跡點(diǎn)和參考控制輸入。下圖為分段復(fù)合有限時(shí)間跟蹤控制方法與其他跟蹤控制方法的對比示意圖。首先在水下機(jī)器人坐標(biāo)系下定義位姿誤差方程，為減小機(jī)器人到達(dá)預(yù)定航路軌跡點(diǎn)的時(shí)間，設(shè)計(jì)一種分段復(fù)合有限時(shí)間的軌跡點(diǎn)跟蹤控制方法，能實(shí)現(xiàn)機(jī)器人軌跡的快速無抖振控制。為減小在預(yù)定軌跡點(diǎn)的位置抖動(dòng)，在預(yù)定軌跡點(diǎn)處的兩側(cè)設(shè)置一個(gè)切換控制邊界層并設(shè)定切換控制邊界層的寬度，用來減緩有限時(shí)間控制器的過沖影響。切換控制邊界層的寬度需要根據(jù)跟蹤性能的過沖狀態(tài)進(jìn)行設(shè)置，寬度較大時(shí)，系統(tǒng)運(yùn)行到邊界層的時(shí)間雖較短，但在邊界層內(nèi)過渡時(shí)間較長，不利于有限時(shí)間內(nèi)的跟蹤；寬度較小時(shí)，系統(tǒng)運(yùn)行到邊界層的時(shí)間雖較長，極易產(chǎn)生運(yùn)動(dòng)過沖，導(dǎo)致系統(tǒng)震蕩。在切換控制邊界層內(nèi)外采用不同的跟蹤控制方法：切換控制邊界層外，采用有限時(shí)間跟蹤控制方法可以實(shí)現(xiàn)軌跡的快速跟蹤，在切換控制邊界層內(nèi)采用平滑控制方法，防止運(yùn)動(dòng)狀態(tài)的抖動(dòng)，使機(jī)器人平滑運(yùn)動(dòng)到預(yù)定航路軌跡點(diǎn)。本文提出的復(fù)合有限時(shí)間跟蹤控制方法綜合了平滑控制方法和有限時(shí)間控制方法的優(yōu)點(diǎn)，能夠?qū)崿F(xiàn)有限時(shí)間內(nèi)的無抖振跟蹤控制。

圖2　分段復(fù)合有限時(shí)間跟蹤控制方法與其他跟蹤控制方法的比較

3.2前饋-反饋?zhàn)顑?yōu)跟蹤控制方法設(shè)計(jì)

水下剖面監(jiān)測機(jī)器人軌跡跟蹤控制系統(tǒng)為持續(xù)海流干擾作用下的控制系統(tǒng)，控制性能即要求有一定的控制精度，又要求盡可能節(jié)能，利用二次型性能指標(biāo)的最優(yōu)控制方法來研究其協(xié)同控制問題是合理而有效的。對外擾動(dòng)進(jìn)行建模進(jìn)行系統(tǒng)前饋補(bǔ)償，能有效抑制外擾動(dòng)的影響。由于通信網(wǎng)絡(luò)帶來的時(shí)滯會(huì)導(dǎo)致跟蹤控制性能下降甚至發(fā)散，擬通過一種逐次逼近轉(zhuǎn)換方法對具有時(shí)滯的網(wǎng)絡(luò)化水下機(jī)器人控制系統(tǒng)模型（1）進(jìn)行軌跡跟蹤控制方法設(shè)計(jì)。對于水下機(jī)器人的線性系統(tǒng)方程（1），考慮軌跡跟蹤誤差和節(jié)能控制，選取有限時(shí)域的二次型性能指標(biāo)：

統(tǒng)計(jì)學(xué)是收集、整理、分析、解釋數(shù)據(jù)并從數(shù)據(jù)中得出結(jié)論的科學(xué)。對統(tǒng)計(jì)學(xué)的學(xué)習(xí)離不開對大量數(shù)據(jù)的處理與分析，而借助統(tǒng)計(jì)軟件是當(dāng)今統(tǒng)計(jì)學(xué)教學(xué)的主流方向。SPSS是一款優(yōu)秀的統(tǒng)計(jì)軟件，具有操作簡單、易于上手，統(tǒng)計(jì)分析結(jié)果專業(yè)、科學(xué)、準(zhǔn)確、美觀等諸多優(yōu)點(diǎn)。在教學(xué)中利用SPSS統(tǒng)計(jì)軟件不僅能方便快捷地得出準(zhǔn)確的分析數(shù)字與精美的整理結(jié)果，而且由于SPSS軟件是以變量及其數(shù)據(jù)結(jié)構(gòu)為基礎(chǔ)，菜單加窗口式操作的統(tǒng)計(jì)分析模式，在提升統(tǒng)計(jì)學(xué)教學(xué)效果方面可發(fā)揮其獨(dú)到的優(yōu)越性。

4　軌跡跟蹤控制方法的軟硬件實(shí)現(xiàn)和實(shí)驗(yàn)驗(yàn)證

主要研究分布式控制器的軟硬件設(shè)計(jì)、控制算法的軟硬件實(shí)現(xiàn)以及岸邊實(shí)驗(yàn)站進(jìn)行海上試驗(yàn)和改進(jìn)。具體為：

4.1分布式控制器的軟硬件設(shè)計(jì)

4.1.1控制器（控制器1和控制器2）電路結(jié)構(gòu)設(shè)計(jì)

水下剖面監(jiān)測機(jī)器人控制器包含置于船載控制箱內(nèi)的控制器1和置于水下機(jī)器人儀器艙內(nèi)的控制器2兩部分。控制器1和控制器2上還包括有串口通信單元、串口/光纖轉(zhuǎn)換器以及存儲(chǔ)器單元。該控制器可以實(shí)現(xiàn)手動(dòng)操控機(jī)器人和機(jī)器人的快速自動(dòng)軌跡跟蹤控制的自動(dòng)切換。圖3為船載控制器（控制器1）結(jié)構(gòu)圖，圖4為水下運(yùn)動(dòng)控制器（控制器2）結(jié)構(gòu)圖。

控制器1實(shí)時(shí)采集船載控制器控制面板的遙控控制指令，并將控制指令按照自定義的通信協(xié)議進(jìn)行編碼和組裝成幀，經(jīng)由通信模塊發(fā)送到水下控制器2。水下控制器2實(shí)時(shí)接收船載控制器1發(fā)送的協(xié)議幀，并進(jìn)行協(xié)議解碼，按照協(xié)議格式解析出控制指令，進(jìn)而控制繼電器驅(qū)動(dòng)板上的對應(yīng)的繼電器的開關(guān)動(dòng)作，繼而驅(qū)動(dòng)推進(jìn)電機(jī)進(jìn)行工作，從而實(shí)現(xiàn)對水下機(jī)器人運(yùn)動(dòng)姿態(tài)的控制?？刂破?和控制器2包括有串口通信模塊、串口/光纖轉(zhuǎn)換器和電壓轉(zhuǎn)換和穩(wěn)壓模塊。用于水下機(jī)器人控制器和船載控制器間的相互通信。通過電壓轉(zhuǎn)換和穩(wěn)壓模塊將供電電壓轉(zhuǎn)換為機(jī)器人所需的工作電壓，并提供其與控制器1間的遠(yuǎn)程數(shù)據(jù)通信，保證信號(hào)傳輸?shù)臒o失真。

圖3　船載控制器（控制器1）結(jié)構(gòu)圖

圖4　水下運(yùn)動(dòng)控制器（控制器2）結(jié)構(gòu)圖

4.1.2控制器A和控制器B之間的通信協(xié)議設(shè)計(jì)

針對控制器A和B間通信的具體要求，設(shè)計(jì)的通信協(xié)議幀的幀頭和幀尾均由1個(gè)字節(jié)組成，幀頭用于區(qū)分是控制數(shù)據(jù)幀和定位傳感器數(shù)據(jù)幀，序號(hào)字段用于區(qū)分不同數(shù)據(jù)幀，相同序號(hào)的控制幀和傳感數(shù)據(jù)幀表示同一個(gè)控制-傳感序列，控制命令字由1個(gè)字節(jié)組成，如圖5（a）所示?？刂破鰾 實(shí)時(shí)接收船載控制器A 發(fā)送的協(xié)議幀，并進(jìn)行協(xié)議解碼，按照自定義的協(xié)議格式的相應(yīng)字段提取發(fā)送時(shí)間戳信息控制命令字，計(jì)算得到?？刂破鰾檢測控制命令字的最高位，如果為0，則為手動(dòng)操控指令，進(jìn)而控制繼電器驅(qū)動(dòng)板上的對應(yīng)的繼電器的開關(guān)動(dòng)作，繼而驅(qū)動(dòng)推進(jìn)電機(jī)進(jìn)行工作，從而實(shí)現(xiàn)對水下機(jī)器人的前進(jìn)、后退、上升、下潛、左右旋轉(zhuǎn)等運(yùn)動(dòng)控制；如果為1，則為自動(dòng)軌跡跟蹤控制，該協(xié)議的低7位表示此時(shí)設(shè)定的軌跡點(diǎn)。例如，控制器B 接收到控制命令字為0*****XX，首先檢測最高位，這里為0，表示手動(dòng)操控，然后檢測最后兩位的狀態(tài)，如果后兩位全為1，則為前進(jìn)指令；如果后兩位全為0，則為后退指令；如果為10 則為左旋指令，如果為01 則為右旋指令。接收到上升和下沉指令時(shí)，控制命令字為0***XX**，檢測最高位后，再檢測第3 和第4 位的狀態(tài)，如果全為1，則為上升指令；如果全為0，則為下降指令；為避免機(jī)器人的橫滾，當(dāng)最高位為0 時(shí)，設(shè)定其他形式則為無效的控制字，予以丟棄?？刂破鰾 接收到控制命令字為1******* 時(shí)，表示自動(dòng)軌跡跟蹤控制，低7位組成的二進(jìn)制數(shù)表示需要設(shè)定的軌跡點(diǎn)，此時(shí)只控制垂向電機(jī)的正反轉(zhuǎn)?？刂破鰾 發(fā)出控制指令，控制繼電器驅(qū)動(dòng)板上的對應(yīng)垂向的繼電器的開關(guān)動(dòng)作，繼而驅(qū)動(dòng)推進(jìn)垂向電機(jī)進(jìn)行工作。自動(dòng)軌跡跟蹤控制模式下，控制器B 實(shí)時(shí)檢測定位傳感器的值，通過控制器B 的具有比較功能的端口，與設(shè)定跟蹤控制軌跡點(diǎn)的預(yù)定值進(jìn)行比較，利用在控制器B 中的自動(dòng)軌跡跟蹤控制方法，實(shí)現(xiàn)對水下機(jī)器人軌跡跟蹤控制。循環(huán)冗余檢測（CRC）也由1個(gè)字節(jié)組成，控制器B通過CRC檢測算法計(jì)算并檢測協(xié)議最未位，并與接收到的CRC值進(jìn)行對比，完成對數(shù)據(jù)幀的差錯(cuò)檢測，出現(xiàn)錯(cuò)誤則丟棄該數(shù)據(jù)包，不進(jìn)行機(jī)器人操控。圖5（b）為控制器B發(fā)送給控制器A的定位傳感器數(shù)據(jù)通信協(xié)議格式?？刂破鰾將發(fā)送時(shí)刻和計(jì)算得到的加入到數(shù)據(jù)幀中并發(fā)送給控制器A，控制器A接收數(shù)據(jù)幀，按圖5（b）所示的幀格式進(jìn)行解幀處理，提取發(fā)送時(shí)間戳信息、和傳感器信息，計(jì)算得到和表征機(jī)器人位置的傳感器數(shù)據(jù)，這樣就可以獲得時(shí)延和的值。

圖5?。╝） 控制器A發(fā)送給控制器B的控制通信協(xié)議格式

圖5?。╞） 控制器B發(fā)送給控制器A的定位傳感器數(shù)據(jù)通信協(xié)議格式

4.2軌跡跟蹤控制方法的軟硬件實(shí)現(xiàn)

4.2.1船載遙控網(wǎng)絡(luò)化控制器的軟硬件實(shí)現(xiàn)

在船載遙控跟蹤控制模式下，控制器A首先檢測其他各個(gè)單元的狀態(tài)，然后啟動(dòng)通信模塊，與控制器B進(jìn)行通信連接。船載控制器按一定時(shí)間間隔采集單片機(jī)端口的遙控柄開關(guān)狀態(tài)數(shù)據(jù)和手動(dòng)/自動(dòng)切換狀態(tài)，并按照自定義的協(xié)議幀格式，進(jìn)行打包組裝成幀，然后通過串口通信模塊以及串口/光纖轉(zhuǎn)換器，發(fā)送到水下控制器（控制器B），軟件流程如圖7所示。控制器B實(shí)時(shí)接收船載控制器A發(fā)送的協(xié)議幀，并進(jìn)行協(xié)議解碼，按照通信協(xié)議格式的相應(yīng)位解析出控制指令，檢測通信協(xié)議控制命令字段的最高位，如果為0，則為手動(dòng)操控指令；根據(jù)遙控指令控制繼電器驅(qū)動(dòng)板上的對應(yīng)的繼電器的開關(guān)動(dòng)作，繼而驅(qū)動(dòng)推進(jìn)電機(jī)進(jìn)行工作，從而實(shí)現(xiàn)對水下機(jī)器人的前進(jìn)、后退、上升、下潛、左右旋轉(zhuǎn)等運(yùn)動(dòng)控制。

圖7　控制器A軟件程序流程

4.2.2自動(dòng)軌跡跟蹤控制方法的軟硬件實(shí)現(xiàn)

基于設(shè)計(jì)的軌跡跟蹤控制算法，計(jì)算出機(jī)器人的驅(qū)動(dòng)力和力矩，并在AVRMega128單片機(jī)上實(shí)現(xiàn)控制算法，根據(jù)推進(jìn)器的布局，將其分配到對應(yīng)軸的推進(jìn)電機(jī)上，從而驅(qū)動(dòng)相應(yīng)的電動(dòng)機(jī)并帶動(dòng)螺旋槳按照給定的驅(qū)動(dòng)力和力矩轉(zhuǎn)動(dòng)，實(shí)現(xiàn)軌跡的自動(dòng)跟蹤控制?？刂破鰾接收到控制命令字段為1*******時(shí)，表示自動(dòng)軌跡跟蹤控制，低7位組成的二進(jìn)制數(shù)表示需要設(shè)定的軌跡點(diǎn)。在自動(dòng)軌跡跟蹤控制模式下，控制器B實(shí)時(shí)檢測定位傳感器的值并轉(zhuǎn)化為與設(shè)定值一致的統(tǒng)一的格式，通過控制器B的Mega128單片機(jī)的具有比較功能的端口，與跟蹤控制軌跡點(diǎn)的設(shè)定值進(jìn)行計(jì)算比較；考慮到定位設(shè)備性能和水下機(jī)器人自身控制導(dǎo)致的水下定位誤差， 設(shè)置一個(gè)控制閾值， 該閾值之內(nèi)認(rèn)為機(jī)器人達(dá)到了設(shè)定的位置， 不需要對機(jī)器人進(jìn)行控制。

5　采取的研究方案

項(xiàng)目的研究目標(biāo)是在水下機(jī)器人系統(tǒng)動(dòng)力學(xué)模型的基礎(chǔ)上，實(shí)現(xiàn)控制算法的設(shè)計(jì)和實(shí)驗(yàn)驗(yàn)證。具體的研究步驟為機(jī)器人動(dòng)力學(xué)建模、前饋-反饋?zhàn)顑?yōu)跟蹤控制和分段復(fù)合有限時(shí)間跟蹤控制算法設(shè)計(jì)以及控制方法的仿真、控制方法的軟硬件實(shí)現(xiàn)和實(shí)驗(yàn)驗(yàn)證，分步進(jìn)行以此驗(yàn)證提出設(shè)計(jì)方法的有效性（圖8所示）。

5.1水下剖面監(jiān)測機(jī)器人動(dòng)力學(xué)建模

水下剖面監(jiān)測機(jī)器人控制系統(tǒng)是一種實(shí)現(xiàn)機(jī)器人剖面監(jiān)測和閉環(huán)反饋的隨動(dòng)控制器，其由水下控制器、定位傳感器（長基線和IMU組合導(dǎo)航提供水下位置，GPS提供水面下水點(diǎn)位置坐標(biāo)，并將數(shù)據(jù)轉(zhuǎn)化到統(tǒng)一的坐標(biāo)系下）和驅(qū)動(dòng)電機(jī)（2組電機(jī)，水平和垂直方向各一組）組成。水下機(jī)器人擾動(dòng)源多、動(dòng)力學(xué)系統(tǒng)非線性強(qiáng)，同時(shí)由于工作環(huán)境的復(fù)雜性和未知性，使得傳統(tǒng)的控制方式不能很好地解決外界擾動(dòng)情形下的運(yùn)動(dòng)控制，需要建立水下機(jī)器人的動(dòng)力學(xué)模型，將外擾動(dòng)建模為外系統(tǒng)并進(jìn)行前饋控制，實(shí)現(xiàn)對外界擾動(dòng)的有效抑制。項(xiàng)目組前期已對水下剖面機(jī)器人進(jìn)行了動(dòng)力學(xué)建模，同時(shí)還需綜合考慮水下有限的的驅(qū)動(dòng)能量和跟蹤誤差，建立能充分考慮以上因素的系統(tǒng)動(dòng)力學(xué)方程。

圖8　項(xiàng)目研究技術(shù)路線

5.2航路點(diǎn)軌跡跟蹤控制方法設(shè)計(jì)

由于網(wǎng)絡(luò)帶來的時(shí)滯會(huì)導(dǎo)致跟蹤控制軌跡震蕩甚至發(fā)散，擬通過逐次逼近轉(zhuǎn)換方法將具有時(shí)滯的網(wǎng)絡(luò)化水下機(jī)器人控制系統(tǒng)方程轉(zhuǎn)化為一系列無時(shí)滯的控制系統(tǒng)方程，并基于轉(zhuǎn)化后的系統(tǒng)方程，研究有限時(shí)間的軌跡點(diǎn)跟蹤控制方法，設(shè)計(jì)一種分段復(fù)合式快速無抖振的水下剖面監(jiān)測機(jī)器人控制方法。通過控制器A和控制器B通信協(xié)議的設(shè)計(jì)，獲得通信時(shí)延和，然后基于建立的機(jī)器人動(dòng)力學(xué)模型和獲得的時(shí)延和，設(shè)計(jì)一種網(wǎng)絡(luò)化的水下機(jī)器人軌跡跟蹤控制方法，用于船載實(shí)時(shí)遙控水下機(jī)器人實(shí)現(xiàn)對給定航路點(diǎn)軌跡的跟蹤。通過Matlab進(jìn)行時(shí)滯和控制算法仿真，研究不同通信時(shí)延對控制性能的影響。

5.3水下剖面監(jiān)測機(jī)器人控制器仿真和軟硬件設(shè)計(jì)驗(yàn)證

采用Matlab軟件驗(yàn)證通信時(shí)延對軌跡跟蹤誤差的影響，分析可容忍的最大通信時(shí)延，為控制算法的設(shè)計(jì)提供實(shí)驗(yàn)支撐，并進(jìn)行控制算法的比較研究和驗(yàn)證，進(jìn)一步改進(jìn)最優(yōu)跟蹤控制方法，得出可以實(shí)現(xiàn)的最優(yōu)的控制算法?；谔岢龅能壽E跟蹤控制方法和系統(tǒng)動(dòng)力學(xué)模型，進(jìn)行水下機(jī)器人運(yùn)動(dòng)姿態(tài)的位置和姿態(tài)控制仿真驗(yàn)證。

5.4控制方法的軟硬件實(shí)現(xiàn)和實(shí)驗(yàn)驗(yàn)證

在山東省科學(xué)院岸邊實(shí)驗(yàn)站進(jìn)行水下機(jī)器人剖面監(jiān)測軌跡跟蹤實(shí)驗(yàn)，以此驗(yàn)證所設(shè)計(jì)的剖面軌跡跟蹤控制方法的有效性，并根據(jù)實(shí)驗(yàn)中出現(xiàn)的問題進(jìn)行改進(jìn)。將建立的水下機(jī)器人動(dòng)力學(xué)模型在Mega128單片機(jī)控制器上進(jìn)行實(shí)現(xiàn)，將其作為控制對象，設(shè)計(jì)控制器水上部分和水下部分的通信控制協(xié)議以及基于該通信協(xié)議的網(wǎng)絡(luò)化的機(jī)器人控制器。預(yù)定航路點(diǎn)位置坐標(biāo)由預(yù)編程寫入到機(jī)器人的控制器中，機(jī)器人的實(shí)際水下位置由長基線應(yīng)答器和陀螺儀、加速度計(jì)提供，作為控制系統(tǒng)的檢測反饋信號(hào)，水下控制器檢測該反饋信號(hào)，并與預(yù)訂航路點(diǎn)位置進(jìn)行比較，基于提出的軌跡跟蹤方法進(jìn)行控制器輸出解算，輸出對應(yīng)的直流電機(jī)的控制量，通過驅(qū)動(dòng)力輸出分配模塊，將相應(yīng)控制量轉(zhuǎn)化為相應(yīng)的12VDC到24VDC輸出電壓，控制直流電機(jī)旋轉(zhuǎn)，產(chǎn)生相應(yīng)的推進(jìn)力，控制機(jī)器人向指定位置運(yùn)動(dòng)。

6　結(jié)論

欠驅(qū)動(dòng)式水下監(jiān)測機(jī)器人航路點(diǎn)軌跡跟蹤控制技術(shù)是當(dāng)前水下機(jī)器人領(lǐng)域具有挑戰(zhàn)的研究課題。項(xiàng)目結(jié)合海洋環(huán)境剖面監(jiān)測的實(shí)際需求，研究水下剖面監(jiān)測機(jī)器人對設(shè)定航路點(diǎn)軌跡的跟蹤控制方法及其實(shí)驗(yàn)，在被控對象和實(shí)驗(yàn)方法上具有一定的特色。設(shè)計(jì)的控制方法考慮到軌跡跟蹤的實(shí)際需求和控制對象的控制特性，設(shè)計(jì)一種能有效抑制外擾動(dòng)的能量最優(yōu)的跟蹤控制方法和一種快速實(shí)現(xiàn)軌跡跟蹤的分段復(fù)合有限時(shí)間跟蹤控制方法。通過設(shè)計(jì)控制器A和B以及通信協(xié)議，實(shí)現(xiàn)對通信時(shí)延的計(jì)算；通過前饋控制和逐次逼近轉(zhuǎn)換方法，提出能有效抑制外擾動(dòng)并解決通信網(wǎng)絡(luò)時(shí)滯的網(wǎng)絡(luò)化前饋-反饋?zhàn)顑?yōu)跟蹤控制方法。通過設(shè)置跟蹤誤差閾值和控制切換面，提出能夠?qū)崿F(xiàn)航路點(diǎn)軌跡跟蹤的無抖振分段復(fù)合有限時(shí)間跟蹤控制方法。項(xiàng)目組研制了一臺(tái)用于海洋剖面監(jiān)測水下機(jī)器人，并在岸邊實(shí)驗(yàn)站進(jìn)行了實(shí)驗(yàn)。利用本單位的實(shí)驗(yàn)水槽和岸邊實(shí)驗(yàn)站可以進(jìn)行一些水下機(jī)器人相關(guān)參數(shù)的進(jìn)一步測定，獲取與機(jī)器人建模相關(guān)的水動(dòng)力參數(shù)，完善機(jī)器人的動(dòng)力學(xué)方程；利用長基線和IMU作為組合導(dǎo)航位置傳感器進(jìn)行定位信息的實(shí)時(shí)獲取。下一步課題組將在多AUVs的協(xié)同控制技術(shù)方面開展研究，重點(diǎn)研究基于水下通信MODEM的多個(gè)AUV的協(xié)同組網(wǎng)控制方法，研究基于多智能一致性的（consensus）的AUV最優(yōu)編隊(duì)控制方法及其實(shí)驗(yàn)驗(yàn)證。研究無GPS、北斗等情形下的基于加速度計(jì)和陀螺儀的慣性導(dǎo)航技術(shù)和自主導(dǎo)航濾波算法研究。開展AUV的SLAM方法研究，選取合適的傳感器進(jìn)行SLAM的算法設(shè)計(jì)及其實(shí)驗(yàn)驗(yàn)證。

［1］ Wood S， Nulph A， Howell B. Application of autonomous underwater vehicles［J］. Sea Technology， 2004， 45（12）： 10-14.

［2］ Ageev M D. Application of solar and wave energies for long-range autonomous underwater vehicles［J］. Advanced Robotics， 2002， 16（1）： 43-55.

［3］ Hemond H F， Mueller A V， Hemond M. Field Testing of Lake Water Chemistry with a Portable and an AUV-based Mass Spectrometer［J］. Journal of the American Society for Mass Spectrometry， 2008， 19（10）： 1403-1410.

［4］ Shome S N， et al. AUV for shallow water applications： Some design aspects［A］. Proceedings of the 18th International Offshore and Polar Engineering Conference［C］， 2008.429-435.

［5］ Spiewak J M， Jouvencel B， Fraisse P. A new design of AUV for shallow water applications： H160［A］. Proceedings of The Sixteenth International Offshore and Polar Engineering Conference［C］，2006.233-238.

［6］ Leonard N E， Fiorelli E. Virtual leader， artificial potentials and coordinated control of groups［J］. IEEE Conference on Decision and Control， Orlando， FL， USA， 2001，（3）： 2968-2973.

［7］ Leonard N E， Paley D A， Davis R E， et al. Coordinated control of an underwater glider fleet in an adaptive ocean sampling field experiment in Monterey Bay［J］. Journal of Field Robotics， 2010，27（6）： 718-740.

［8］ Feng Z P， et al. Formation Control of Underwater Mobile Sensing Network［J］.Journal of Shanghai Jiaotong university，2009，E-14（5）. ［9］ 李曄，龐永杰，唐旭東.一種智能水下機(jī)器人進(jìn)行大范圍海洋環(huán)境監(jiān)測的方案與實(shí)驗(yàn)［J］. 熱帶海洋學(xué)報(bào)， 2009， 28（4）： 12-16.

［10］ 閆偉， 嚴(yán)衛(wèi)生， 王銀濤， 姚堯.通信時(shí)延下欠驅(qū)動(dòng)自主水下航行器編隊(duì)控制［J］. 火力與指揮控制， 2011， （6）：52-55 .

［11］ 崔榮鑫， 徐德民， 嚴(yán)衛(wèi)生. 僅利用位置信息的自主水下航行器主從式編隊(duì)控制方法［J］. 兵工學(xué)報(bào)， 2008， 29（8）： 980-984.

［12］ 袁健，唐功友. 基于一致性和虛擬結(jié)構(gòu)的多自主水下航行器編隊(duì)控制［J］. 智能系統(tǒng)學(xué)報(bào)，2011， （3）：248-253.

［13］ Yuan Jian， Tang Gong-You. Finite-time Tracking Control Algorithms based on Variable Structure for Mobile Robots［A］. Chinese Control Conference［C］.Beijing， 2010.419-423.

［14］ Yuan Jian， Tang Gong-You. Formation Control for Mobile Multiple Robots with Hierarchical Virtual Structures［A］. IEEE International Conference on Control and Automation［C］.Xiamen，2010.393-398.

［15］ M.S. Naik， S.N. Singh. State-dependent Riccati Equation-based Robust Dive Plane Control of AUV with Control Constraints［J］. Ocean Engineering. 2007， 34（11-12）： 1711-1723.

［16］ Zhang Hong-bin， Yuan Jian. An Improved Controller based on Chattering-free Trajectory-tracking Control for Underwater Profile Monitoring Vehicle［J］.Applied Mechanics and Materials， 2014，（568-570） ： 944-947.

［17］ 袁健，周忠海 等. 一種網(wǎng)絡(luò)化海洋水質(zhì)監(jiān)測系統(tǒng)［P］. 中國專利，201220737710.9，2013.07.10，.

［18］ 周忠海，袁健 等. 一種海洋水質(zhì)監(jiān)測數(shù)據(jù)采集系統(tǒng)［P］. 中國專利， 201320581982.9， 2014.03.18.

［19］ 周忠海，袁健 等. 一種遙控水質(zhì)監(jiān)測水下機(jī)器人系統(tǒng)［P］. 中國專利， 201320568687.X，2014.03.04，.

［20］ Kim K， Choi H S. Analysis on the controlled nonlinear motion of a test bed AUV-SNUUV I［J］. Ocean Engineering， 2007， 34（8-9）：1138-1150.

［21］ Zhang Bao-lin， Tang Gong-you， Yue Dong. Optimal disturbance rejection control for singularly perturbed composite systems with time-delay［J］. Asian Journal of Control， 2009，11（3）：327-335.

［22］ 唐功友，蓋紹婷.時(shí)滯系統(tǒng)在高速網(wǎng)絡(luò)下的最優(yōu)擾動(dòng)抑制［J］，控制理論與應(yīng)用，2009，27（7）：965-970.

［23］ Zhou Zhong-Hai， Yuan Jian， Zhang Wen-Xia， Zhao Jin-Ping. Chattering-free Finite-time Tracking Control Algorithms with Variable Structure for Mobile Robots［A］.Chinese Control and Decision Conference［C］.Taiyuan， 2012.1337-1342.

［24］ Yuan Jian， Zhang Wen-Xia， Zhou Zhong-Hai. A Compound Finite-time Trajectory-tracking Control Algorithm for Mobile Robot with System and Measurement Noise， International Journal of Sensing［J］.Computing and Control， 2012， 2（1）： 38-49.

（責(zé)任編輯：張 萌）

Waypoint Trajectory-tracking Control Methods with High-resistance to Disturbance for Under-actuated Underwater Profile Monitoring Vehicle

YUAN Jian1，2，3
（1. National Engineering and Technological Research Center of Marine Monitoring Equipment；2. Qingdao， Shandong Provincial Key Laboratory of Ocean Environment Monitoring Technology；3. Institute of Oceanographic Instrumentation of Shandong Academy of Sciences ，Qingdao 266001）

Under-actuated underwater profile monitoring vehicle can achieve the observations on apredetermined profile waypoint trajectory， the trajectory-tracking controltechnology is the key technology to achieve high-precision profile observations. The controller A， controller B and their communication protocol will be designedto calculate the real-time time delay； using physical modeling and experimentalverification to establish the dynamics model of the underwater vehicle； byconstructing a feedforward control and using delay-free transformation， we designa networked feed-forward and feedback optimal trajectory-tracking control methodconsidering the external disturbances rejection and communication time delay. Tocarry out the smooth control， we construct a tracking error threshold and aswitching surface to design a free-chattering segmented compound control methodfor quick trajectory-tracking. The detailed steps are dynamics modeling，feed-forward and feedback optimal tracking control design，multistage finite-timetracking control design， simulations and experimental verification of the controlmethods. We verify the effectiveness of the proposed methods step by step. The control scheme provides a theory basis for trajectory-tracking control technologyof underwater profile monitoring vehicle.

Under-actuated；Underwater profile monitoring vehicle；Trajectory-tracking control；Finite-time tracking control

中圖文獻(xiàn)號(hào)：TP273；U69A

10.3969/j.issn.1003-8256.2016.01.006

山東省自然科學(xué)基金（ZR2012FL18）；青島科技發(fā)展計(jì)劃（13-1-4-172-jch）；山東省海洋生態(tài)環(huán)境與防災(zāi)減災(zāi)重點(diǎn)實(shí)驗(yàn)室開放課題（201308）；山東省科學(xué)院博士基金（201244）

袁?。?980-），男，山東榮成人，博士，助理研究員，研究方向?yàn)樗聦?dǎo)航與控制。