基于STM32的水下機器人設(shè)計

王 雷 ，楊慶喜 ，程 勇 ，張璇琛 ，鄭 磊

（1.中國科學(xué)院等離子體物理研究所，合肥 230031；2.中國科學(xué)技術(shù)大學(xué) 科學(xué)島分院，合肥 230026）

隨著陸上資源開發(fā)殆盡，越來越多的國家開始將目光放在海洋。相比海洋環(huán)境惡劣危險，人類的潛水深度有限，不能直接完成深海及海底資源的探測任務(wù)。目前的狀況是只有少數(shù)國家有能力對海洋資源進行初步開采。水下機器人強度高、抗高水壓、可操控，能夠完成深水水域的探測和分析任務(wù)，并將探測數(shù)據(jù)返回到陸地上，已逐漸成為人類進行水下資源開發(fā)的主要工具[1]。

水下機器人按照與水面支持系統(tǒng)之間的聯(lián)系方式不同可以分為2種：有纜水下機器人（ROV），ROV通常需要電纜從母艦接受動力，并且通過電纜傳輸控制信號；無纜水下機器人（AUV），通常稱作自治水下機器人或者智能水下機器人，能夠依靠自身的智能控制系統(tǒng)進行決策和控制[2]。本款水下機器人旨在設(shè)計一款在淺水水域工作的，能夠?qū)崿F(xiàn)遠程無線控制和無線圖像傳輸?shù)乃聶C器人。

1 系統(tǒng)概述

水下機器人設(shè)計總體分為三部分：結(jié)構(gòu)系統(tǒng)設(shè)計、控制系統(tǒng)設(shè)計和控制算法及軟件設(shè)計，其系統(tǒng)層次結(jié)構(gòu)如圖1所示。

圖1 水下機器人系統(tǒng)結(jié)構(gòu)Fig.1 Architecture of the underwater robot

1.1 硬件結(jié)構(gòu)設(shè)計

水下機器人結(jié)構(gòu)設(shè)計包括外殼支撐結(jié)構(gòu)、密封艙、推進器及外部天線設(shè)計。外殼支撐結(jié)構(gòu)負責將水下機器人電氣系統(tǒng)分別固定到機器人機體上，同時為保證電氣系統(tǒng)安全運行，需要為電氣系統(tǒng)設(shè)計防水密封艙。水下機器人為6自由度剛體，其不僅需要在6個自由度上可以平穩(wěn)運行，還必須保證在外界擾動條件下正常工作。因此在推進器布置上綜合考慮抗干擾和提供運動動力的問題，垂直方向上布置3個電機提供水下機器人垂直方向推力，在水平方向布置2個電機，控制水下機器人前進和轉(zhuǎn)向運動。因為電磁波是橫波，水是良導(dǎo)體，趨膚效應(yīng)嚴重影響電磁波在水下傳輸，因此需要為水下機器人設(shè)計浮標天線以保證水下機器人依靠電磁波正常通信。結(jié)構(gòu)設(shè)計如圖2所示。

圖2 水下機器人結(jié)構(gòu)Fig.2 Structure of the underwater robot

1.2 控制系統(tǒng)設(shè)計

水下機器人需要布置各類傳感器來獲取自身的狀態(tài)，其中水壓傳感器獲取水下深度，姿態(tài)傳感器獲取姿態(tài)角，GPS獲取運動空間坐標，同時加入溫濕度傳感器來檢測密封艙密封狀態(tài)?？刂破髯鳛檎麄€水下機器人的控制中心，一方面對傳感器數(shù)據(jù)進行融合，獲取水下機器人的運動狀態(tài)，在外界干擾條件下做出控制指令維持水下機器人機身自平衡；另一方面通過接收遙控器指令響應(yīng)相應(yīng)的動作。

1.3 控制算法設(shè)計

水下機器人平穩(wěn)運行是水下機器人完成水下任務(wù)的關(guān)鍵，因此水下機器人控制算法的核心是平衡控制[3]。水下機器人運動傳感器可以獲取水下機器人實時姿態(tài)角，水壓傳感器可以獲取水下機器人水下運動深度，將傳感器數(shù)據(jù)作為控制算法的輸入，通過PID自適應(yīng)控制算法輸出驅(qū)動推進器，實現(xiàn)水下機器人平穩(wěn)運行。

2 系統(tǒng)電氣組成和實現(xiàn)

水下機器人電氣架構(gòu)主要包括傳感器、控制器和驅(qū)動器三部分[4]，系統(tǒng)結(jié)構(gòu)框圖如圖3所示。

圖3 水下機器人系統(tǒng)框圖Fig.3 Block diagram of the underwater robot system

2.1 傳感器

2.1.1 深度傳感器

為保證水下機器人平穩(wěn)運行，必須獲取水下機器人在水下運動的空間姿態(tài)，包括水下深度、空間姿態(tài)角、GPS坐標等。本控制系統(tǒng)采用MS5803-1BA獲取水下壓力。MS5803壓力傳感器支持SPI和I2C總線通訊，是一款高精度的壓力測量傳感器，壓力測量范圍為 10～1300 mbar，其精度可達到 0.012 mbar。其水深計算公式為

式中：P為水壓傳感器測得的壓強；Pair為當?shù)卮髿鈮簭?；ρ為水密度；g為重力加速度。

2.1.2 姿態(tài)傳感器

水下機器人控制的核心是姿態(tài)控制，為提高水下機器人的控制精度，一款性能良好的姿態(tài)傳感器就顯得尤為重要[5]，該水下機器人采用MPU9250作為姿態(tài)傳感器。MPU9250是一款9軸的運動跟蹤傳感器，包括一組3軸加速度計，一組3軸陀螺儀另一組是AK8963 3軸磁力計，并在其中融合動態(tài)運動處理器DMP。通過姿態(tài)融合算法，將加速度計、陀螺儀、磁力計數(shù)據(jù)融合成四元數(shù)，再由四元數(shù)轉(zhuǎn)化為歐拉角，經(jīng)過卡爾曼濾波將運動噪聲濾除后將歐拉角作為運動控制的輸入量通過控制算法轉(zhuǎn)化為電機輸出達到控制水下機器人姿態(tài)的目的。

2.1.3 GPS模塊

該水下機器人通過GPS來獲取空間經(jīng)緯度坐標，從而為實現(xiàn)自動返航提供支持。GPS傳感器采用ATDM332D-5N系列模塊，該模塊基于中科微第四代低功耗GNSS SOC單芯片-AT6558，支持美國GPS和中國的北斗衛(wèi)星導(dǎo)航系統(tǒng)。水下機器人內(nèi)置GPS模塊，可以獲取到經(jīng)緯度坐標，通過設(shè)置起始坐標，對比水下機器人當前坐標，結(jié)合避障算法，從而實現(xiàn)機器人自動返航。

2.1.4 溫濕度傳感器

水下機器人密封艙用來安裝電子設(shè)備和檢測儀器，因此密封艙需要有足夠的強度和可靠的密封性。為保證水下機器人運行狀態(tài)的安全可靠，在密封艙中安裝溫濕度傳感器來檢測密封性能，采用SHT10溫濕度傳感器檢測密封艙內(nèi)部溫濕度狀態(tài)。SHT10包括一個電容性聚合體測濕敏感元件、一個用能隙材料制成的測溫元件，傳感器內(nèi)部有一個精度高達14位的A/D轉(zhuǎn)換器，適應(yīng)串行接口電路實現(xiàn)無縫連接。

2.2 控制器

水下機器人采用了較多的傳感器，為保證姿態(tài)控制的實時性，采用單獨的控制器來實現(xiàn)水下機器人傳感器數(shù)據(jù)解析和水下機器人電機控制。水下機器人采用STM32作為控制系統(tǒng)控制芯片，其中采用STM32F103CBT6作為電機控制器，STM32F407VGT6作為傳感器數(shù)據(jù)融合的控制器。兩片控制芯片之間采用串口進行數(shù)據(jù)交換。

STM32F103CBT6產(chǎn)生 5路 PWM輸出，分別控制2個水平推進電機和3個垂直推進電機，STM32F407VGT6則負責采集運動傳感器、水壓傳感器、GPS、溫濕度傳感器、激光測距傳感器及遙控器PPM輸出信號并將數(shù)據(jù)匯總、解析為控制信號后通過串口發(fā)送到STM32F103控制器中。STM32F103響應(yīng)控制指令，通過電子調(diào)速器來驅(qū)動無刷電機，起到控制機器人姿態(tài)的作用。

2.3 電源系統(tǒng)

水下機器人通過6節(jié)高倍率26650鋰電池供電，單節(jié)電池容量達到5200 mA，可持續(xù)提供10 A電流放電。供電系統(tǒng)采用鋰電池串并聯(lián)方案，將3節(jié)鋰電池串聯(lián)后得到電機驅(qū)動所需的12 V電壓，采用2組12 V電源并聯(lián)提高電源的電流輸出能力，同時提高水下機器人的續(xù)航能力。電源系統(tǒng)可以持續(xù)提供20 A放電電流，達到整個電氣系統(tǒng)對電流電壓的需求。電池供電系統(tǒng)架構(gòu)如圖4所示。

圖4 電源系統(tǒng)架構(gòu)Fig.4 Architecture of the power system

STM32采用3.3 V供電，因12 V轉(zhuǎn)化到3.3 V壓降較大，傳統(tǒng)LDO穩(wěn)壓器如AMS1117會產(chǎn)生較大的熱量，為提高系統(tǒng)的可靠性，采用DC-DC電壓轉(zhuǎn)化方案。電源電壓轉(zhuǎn)化電路采用LM2576穩(wěn)壓芯片來保證電源的安全可靠。電源轉(zhuǎn)換電路如圖5所示。

圖5 LM2576電路Fig.5 LM2576 circuit

3 軟件系統(tǒng)架構(gòu)

水下機器人軟件系統(tǒng)包括控制程序和測試軟件設(shè)計兩部分。

3.1 控制程序

水下機器人控制程序可分為主程序、傳感器數(shù)據(jù)采集和算法程序三部分。其中主程序負責水下機器人各個電氣部件的初始化，傳感器數(shù)據(jù)采集獲取機器人實時狀態(tài)，算法程序負責將傳感器數(shù)據(jù)融合，轉(zhuǎn)化為控制量輸出。

主程序初始化包括系統(tǒng)時鐘、串口初始化、定時器中斷初始化、以及各個傳感器部件的初始化。其中用于密封艙漏水檢測的溫濕度傳感器，可以作為整個水下機器系統(tǒng)的異常檢測。如果沒有異常，系統(tǒng)開始響應(yīng)定時器中斷程序。定時器中斷程序中包含傳感器數(shù)據(jù)刷新和控制算法程序，這樣可以保證系統(tǒng)執(zhí)行的實時性。之后通過串口將控制指令發(fā)送到推進器控制MCU，執(zhí)行相應(yīng)的動作?？刂葡到y(tǒng)流程如圖6所示。

圖6 控制程序執(zhí)行流程Fig.6 Control program execution flow chart

水下機器人控制算法采用數(shù)字PID控制器。數(shù)字PID控制器具有結(jié)構(gòu)簡單、參數(shù)整定方便、能滿足大多數(shù)控制性能要求等優(yōu)點[6]。水下機器人控制的核心是姿態(tài)控制，因此PID控制效果直接決定了機器人能否平穩(wěn)運行。水下機器人控制對象包括俯仰角（pitch）控制、橫滾角（roll）控制、偏航角（yaw）控制、深度控制，另外水下機器人需要對遙控器控制指令做出響應(yīng)。

3.2 測試軟件

圖7 上位機界面Fig.7 Host computer interface

測試軟件采用LabVIEW編寫，上位機界面如圖7所示。LabVIEW是美國國家儀器公司開發(fā)的圖形化編輯語言，因其簡單快捷，極大提高了編程效率，降低硬件測試平臺開發(fā)的難度。通過LabVIEW中的VISA驅(qū)動，搭建出基于串口通信的上位機測試軟件，并通過2塊433 MHz無線數(shù)傳模塊建立起上位機和水下機器人之間的通信，實時顯示水下機器人運動狀態(tài)，同時可以便捷地對PID參數(shù)進行在線修改，提高了PID參數(shù)調(diào)試的效率。

水下機器人采用Keil5軟件對STM32控制器進行編程，通過ST-Link下載器將程序下載到控制器，使用LabVIEW上位機在線對PID參數(shù)進行修改。上位機姿態(tài)角顯示如圖8所示。

圖8 上位機姿態(tài)角顯示Fig.8 Attitude angle display by LabVIEW

4 系統(tǒng)測試

4.1 水下機器人靜態(tài)調(diào)試

鑒于水下機器人的應(yīng)用場景，因此必須對水下機器人進行水下調(diào)試，找出水下機器人最優(yōu)的PID參數(shù)[7]。在靜水水域?qū)λ聶C器人的姿態(tài)角PID參數(shù)進行調(diào)試，采用的方法是試驗湊試法，先對比例參數(shù)進行整定，通過上位機觀察系統(tǒng)的響應(yīng)曲線，直至得到反應(yīng)快、超調(diào)小的響應(yīng)曲線。若在比例控制下穩(wěn)態(tài)誤差沒有辦法滿足要求，再加入積分環(huán)節(jié)。PI控制可以消除系統(tǒng)穩(wěn)態(tài)誤差，加入D控制會提高系統(tǒng)的動態(tài)響應(yīng)性能。水下測試如圖9所示，經(jīng)驗證水下機器人可以達到良好的靜態(tài)性能，在水下可以做到長時間懸浮。

圖9 水下機器人靜態(tài)調(diào)試Fig.9 Underwater robot static debugging

4.2 水下機器人動態(tài)調(diào)試

在保證水下機器人靜態(tài)性能的前提下，對水下機器人的運動性能進行測試，主要測試水下機器人運行過程中的姿態(tài)糾正和定深控制以及長距離運行中的航行角控制，經(jīng)測試其在水中的機動性和指向性都是可以滿足設(shè)計要求的。在長距離測試中同樣測試了密封艙的防水性能和視頻的傳輸，均得到良好的性能。

5 結(jié)語

本文介紹了一款基于STM32的水下機器人系統(tǒng)架構(gòu)，采用STM32作為傳感器數(shù)據(jù)的處理中心并控制推進器達到機器人精確控制的目的。水下機器人內(nèi)置的433 MHz無線模塊可以將水下機器人的運動狀態(tài)反饋到上位機進行顯示，同時上位機可以在線修改水下機器人參數(shù)。

經(jīng)測試，所設(shè)計的系統(tǒng)可以達到良好的運動性能，滿足設(shè)計要求，可以實現(xiàn)遠距離無線控制，并將實時圖像遠距離傳送到APP進行顯示。系統(tǒng)長時間運行可靠，具有較強的安全性，可以作為淺層水域探測的工具。

[1]邢進.水下機器人運動姿態(tài)控制技術(shù)的研究[D].山東：山東科技大學(xué)，2008.

[2]姜嬋娟.遙控式水下機器人PID運動控制算法優(yōu)化研究[D].黑龍江：哈爾濱工程大學(xué)，2010.

[3]劉瓊.微型航姿系統(tǒng)的設(shè)計與姿態(tài)解算算法研究[D].重慶：重慶郵電大學(xué)，2016.

[4]Vasilescu I，Varshavskaya P，Kotay K，et al.Autonomous modular optical underwater robot（AMOUR）design，Prototype and feasibility study[C]//IEEE International Conference on Robotics and Automation，IEEE，2006：1603-1609.

[5]劉鎮(zhèn)，姜學(xué)智.PID控制器參數(shù)整定方法綜述[J].電力系統(tǒng)自動化，1997（8）：79-83.

[6]Youakim D，Ridao P，Palomeras N，et al.Autonomous underwater free-floating manipulation using moveIt[J].IEEE Robotics&Automation Magazine，2017（99）：1.

[7]李曄，劉建成，沈明學(xué).Dynamics model of underwater robot motion control in 6 degrees of frccdom[J].哈爾濱工業(yè)大學(xué)學(xué)報：英文版，2005，12（4）：456-459.