基于STM32的無人艇航跡控制器的設計與實現

徐小強 盛 磊

(武漢理工大學自動化學院 武漢 430070)

0 引 言

在海洋環境監測、海洋開發和現代化軍事裝備智能化發展的大背景下，無人艇作為水上的智能航行器在民用和軍用領域具有廣闊的應用前景[1-2].無人艇是一個橫跨多個學科，涉及船舶設計、網絡通信、信息融合、智能控制等多個領域的綜合設計，其中無人艇系統是一個很復雜的控制系統[3-4].在無人艇控制系統中，航跡跟蹤是無人艇自主完成各種復雜任務的關鍵技術之一，自主巡航、動態避碰等都是依賴于航跡跟蹤來實現的.文獻[5]結合無人艇的數學模型、自動操舵儀等設備設計了基于嵌入式平臺的無人艇控制仿真系統，仿真結果較真實的反映了無人艇的運行狀況.文獻[6]以某公司的APAX系列的工控機為硬件控制平臺，用Visual CJHJ開發無人艇控制系統軟件，完成來了無人艇傳感器實時數據的采集和無人艇的運動控制.文獻[7]采用廣義預測控制和PID控制相結合的方式，分別控制無人艇的轉艏運動和操舵運動，并采用了分離式控制方案，通過航向控制間接實現了無人艇的航跡跟蹤控制，通過Matlab仿真結果可知，該控制器具有良好的航向控制與航跡跟蹤的控制效果.文獻[8]采用STM32微控制器為控制單元，通過互補濾波算法完成對無人艇的姿態進行解算，通過PID控制算法實現對無人艇的姿態進行控制并在海上進行了實船實驗，實驗證明了該無人艇的穩定性好，適合海洋環境的監測及海事搜救.

目前，無人艇的控制器設計在仿真實驗中取得了很理想的控制效果，然而在實船實驗應用中達不到很好的控制效果.本文設計了基于STM32的無人艇航跡控制器，構建局域網進行網絡通信，通過Visual C++ 編寫船端監控軟件，采用分離式控制方案，編寫航向保持算法和航跡保持算法，使無人艇沿著指定的路徑進行航行，通過實船反復實驗，驗證了該航跡控制器的實用性.

1 控制器總體設計

無人艇航跡控制器采用分離式的設計方案，將航跡控制算法運行在工控機中的船端監控軟件中和單獨設計控制無人艇的噴水推進器的航向控制板卡.工控機采用Adipcom的BOX-102型號，其上運行通過Visual C++ 編寫的船端監控軟件進行無人艇進行統一管理.無人艇的真航向采用七維航測的 AHRS-Ⅱ型號的電羅經，更新頻率可配置，精度能達到0.01°，位置信息通過GPS定位模塊測得本船的實際位置.它們之間通過RS232串口與工控機相連，工控機中對應的解碼軟件通過串口讀取航向信息和位置信息并進行解析，并按照固定的協議格式推送給船端監控軟件使用.航跡控制器硬件組成框圖見圖1.

圖1 航跡控制器硬件組成框

航向控制板是控制無人艇的航向的核心控制板卡，它通過以太網通信接收船端監控軟件設定的航向，控制無人艇的噴水推進器擺動，達到控制無人艇航向的目的.航向控制板采用以高性能、低成本和低功耗的32位STM32F103增強型系列的芯片作為控制核心，以太網通信采用實現了全硬件TCP/IP協議棧的以太網控制器芯片W5500，高效的實現與船端監控軟件之間的UDP通信.無人艇的噴水推進器轉向裝置是一個12 V直流電機，通過液壓油傳導壓力控制噴水推進器的噴嘴方向，通過0～184 Ω電阻值作為碰嘴位置反饋.圖2為航向控制板的硬件組成框圖.

圖2 航向控制板的硬件組成框圖

系統運行后，工控機中相關解碼軟件將通過串口讀取無人艇的實時航向數據和實時GPS位置信息進行解碼，同時傳送給船端監控軟件.船端監控軟件根據不同的控制命令，解析出來待執行的路徑任務，每一個路徑任務都有一個待航行路徑點隊列，同時并開啟一個定時器任務，周期為2 000 ms，此任務負者判斷當前無人艇的經緯度信息是否與當前路徑隊列中的目標經緯度信息在誤差允許的范圍之內.當接近目標點時進行減速航行，避免無人艇轉向時可能存在的大范圍旋回.當不在誤差允許的范圍之內則按照設定的航速繼續航行.在朝著當前路徑隊列中的目標點航行時，經過計算出本無人艇的實際位置與目標位置的航向角作為設定航向.然后通過以太網按照自定義通信協議格式的UDP數據包發送到航向控制板，航向控制板接收到后進行解析出設定航向，開始設定航向任務，進行航向保持控制.航向控制板根據接收到的電羅經實時測量的本無人艇的真航向，進行PID算法的計算，計算出對應需調整的噴泵位置對應的電阻值.由STM32產生PWM波驅動電機驅動模塊去調整噴泵噴嘴的位置.航向控制板根據設定的航向，實時調整噴泵的位置進行航向保持.在這個動態調整的過程中，無人艇從而實現了航跡的跟隨控制.

2 硬件設計

航向控制板采用ST公司增強型32位微控制STM32F103作為主控制器，它是專為嵌入式應用而專門設計的基于ARM Cortex-M內核的芯片，具有高性能、低功耗和低成本的優勢，工作頻率最高為72 MHz.該型號芯片包含兩路12位的ADC，多個定時器和一個PWM定時器，還有兩個SPI等標準的通信接口.航向控制器通過W5500以太網芯片以UDP協議接收船端監控軟件發送過來的實時航向和設定航向，通過增量式PID算法，計算出設定的噴泵位置的電阻值，通過反饋控制輸出PWM波驅動連接噴泵的直流電機，控制噴嘴的位置.噴泵噴嘴的位置通過STM32進行分壓法測電阻的方式測得碰嘴的實時位置.

2.1 電阻反饋模塊電路設計

噴水推進器位置反饋電阻值范圍為0～190 Ω，通過測電壓的方式計算出對應的電阻值.圖3為電阻反饋模塊的電路圖.通過REF3030穩壓電源芯片,輸出標準穩定的3.0 V基準電壓，精準度在0.2%.它的最大電壓漂移為50 μA,輸出電流最大為25 mA.標準電壓輸出端連接一個由OPA365運算放大器組成的電壓跟隨器，跟隨標準電壓值，提高輸入電阻值到無限大，起到隔離的作用.標準高精度190 Ω電阻與待測電阻串聯起來，待采集的電壓經過電阻值分壓過后，范圍在0～1.5 V，STM32主控芯片自帶的ADC進行采集分壓，經過一定的處理后計算出對應的電阻值.

圖3 電阻反饋模塊電路圖

2.2 噴泵驅動模塊電路設計

噴泵是通過12 V的直流電機驅動液壓油進行控制碰嘴的位置，從噴泵左滿舵可以到右滿舵之間進行調控，實現無人艇的航向控制.筆者采用由大功率MOS管組成的H橋驅動電路DC-30A模塊,最高額定電流達到30 A，寬電壓輸入可達12～30 V.STM32主控芯片PB5和PB6引腳連接模塊上的A1、A2引腳，控制電機的轉向，A1為高電平同時A2為低電平是驅動電機正轉，A2為高電平同時A1為低電平是驅動電機反轉，當A1和A2同時為低電平時可使電機進入剎車狀態.主控芯片PB7是定時器4的PWM脈沖輸出引腳,連接模塊上的PA引腳，控制電機的轉速.電機驅動模塊上的驅動電壓采用無人艇上蓄電池電壓12 V，主控芯片通過控制電機轉動的時間來控制噴泵的位置，從而來實現航向調整.

2.3 以太網通信模塊電路設計

航向控制板與船端工控機之間的通信采用以太網通信，通信協議采用UDP協議進行數據的傳輸.筆者采用的某公司推出的高性能以太網接口芯片W5500芯片，它內部集成了全硬件TCP/IP協議棧、以太網數據鏈路層和以太網物理層.全硬件協議棧采用邏輯門電路來實現復雜的TCP/IP協議簇，通過它來實現以太網通信具有簡單快速、可靠性搞、安全性好等顯著優勢.主控芯片與W5500芯片通過SPI1接口進行通信，通過外部中斷引腳PA12來進行收發數據的中斷，提高數據處理的響應效率.通過RJ45接口與船端的路由器相連，實現網絡通信.

3 軟件設計

3.1 系統總體軟件設計

無人艇的航跡控制采用分離控制的思想，通過實時改變無人艇的航向來跟蹤設定的無人艇軌跡.航跡控制為最外層控制，航向保持控制為中間控制層，噴泵位置控制為最底層的執行機構控制層.航向控制板完成航向保持功能，同時驅動執行機構驅動噴泵位置轉向.船端監控軟件根據無人艇的設定軌跡進行航跡跟蹤，計算出實時跟蹤航向，發送到航向控制板，航向控制板完成.通過分離控制便于模塊化調試，簡化程序代碼的設計.

3.2 主要模塊程序設計

3.2.1以太網通信模塊

無人艇要實現船端到航向控制板，船端到岸端控制系統可靠、有效、無差錯的通信，就需要制定自定義的通信協議.通信協議格式見表1船端通信協議報文格式.

航向控制板上電之后初始化W5500的中斷向量表、SPI初始化的配置和加載網絡的參數(MAC地址、UDP通信模式、本機IP地址和端口等).當接收到數據后產生中斷同時置全局中斷標志W5500_Interrupt 為true.main函數檢測到中斷標志為真之后就去處理對應配置的SOCKET 端口的數據，同時按照表1自定義的通信協議解析出接收到的數據.航向控制板每接收到一幀數據包就需要進行一個對應ID的應答，從而確保數據被接收到.航向控制板向船端發送數據時直接通過調用編寫的Write_SOCK_Data_ACK_To_IP()函數，填充應答ID、航向控制軟件的目的IP地址和端口，將數據寫入到W5500的數據發送緩沖區發送出去.

表1 以太網通信協議報文格式

船端監控軟件中利用CAsyncSocket 類進行SOCKET編程完成UDP通信.第一步通過Create()函數創建綁定本機指定端口的SOCKET,并設定為UDP模式(SOCK_DGRAM).第二步調用SendTo()函數，向目的IP地址和端口發送指定長度的數據.接收數據通過OnReceive()中斷響應函數，利用ReceiveFrom()函數接收從目的IP地址和端口發送來的數據包.接收到的數據包依據自定義的協議進行解析，從而完成通信.

3.2.2噴泵位置控制模塊程序設計

噴泵位置控制模塊是負責調整無人艇航向控制的關鍵，它是基于反饋控制的原理來實現的.噴泵的位置通過電阻值來反映的，噴泵在中間的電阻值為92 Ω，左滿舵時位置反饋為2 Ω，右滿舵時位置反饋為184 Ω.通過STM32F103自身的ADC模塊進行采集位置電阻的分壓后計算得來.航向控制板初始化完成之后，判斷是否需要調整噴泵位置，需要調整的時，根據測得的噴泵位置電阻值與設定要達到的電阻值相比較后產生的誤差來調整執行電機進行正轉或者反轉.當噴泵位置被調整到與期望的電阻值位置在誤差(±1.5 Ω)允許的范圍內則結束調整.噴泵位置控制程序流程圖見圖4.期望的待設定電阻值是航向保持控制器的輸出值.

圖4 噴泵位置控制程序流程圖

3.2.3航向保持器程序設計

無人艇在水上航行受風、浪涌等多方面的影響，無人艇通過直接操作噴泵很難沿著固定的航向航行.航向保持器程序采用位置式PID算法，進行負反饋調節，使無人艇能沿著設定的航向進行航行.無人艇當前航向通過電羅經設備采集后進行處理，由船端監控軟件通過以太網實時發送到航向控制板.由STM32作主控的航向控制板通過W5500中斷后接收到無人艇的實際航向，位置式PID算法根據設定值比較得到的偏差計算出輸出值，輸出值經過歸一化處理后傳遞給噴泵位置控制程序，作為其設定值進行噴泵的位置控制，從而實現對無人艇的航向控制.在位置式PID控制算法中，綜合考慮無人艇的特點取采樣時間為1 000 ms.計算出來的輸出值經過歸一化處理，平移坐標零點到噴泵的位置零點值92，同時限制輸出在12～172，作為噴泵位置的電阻設定值.PID的比例系數、積分系數和微分系數通過實際無人艇水上實驗調整到合適的值.程序流程圖見圖5.

圖5 航向保持器程序流程圖

3.2.4航跡跟蹤模塊程序設計

無人艇的實際位置由GPS定位模塊測得經度和緯度，航跡跟蹤采用漸進直線跟蹤的方法，將跟蹤路徑的關鍵點的經緯度提取出來，存成路徑數組，不同的跟蹤路徑存成不同的路徑數組.航跡跟蹤模塊程序采用Visual C++ 編寫，運行在船端工控機上的控制軟件中，根據待跟蹤的目標點的經緯度，實時計算出待跟蹤航向，從而通過UDP協議編寫的Write_SOCK_Data_Message_To_IP()函數發送到基于STM32設計的航向控制板中，讓航向控制板完成航向保持的任務.

船端監控軟件中根據本無人艇的實際位置與跟蹤路徑的終點位置計算出實際距離，從而判斷無人艇是否航跡跟蹤完畢.每一段直線軌跡的跟蹤采用航向控制器實時跟蹤，由本船實際位置與跟蹤路徑數組中的下一個待航行的坐標點的經緯度計算出來對地航向角，無人艇周期性的計算待跟蹤的航向，然后發送到航向控制板，通過航向控制器來讓無人艇進行航向跟蹤.無人艇根據設定的位置誤差20m，從而判斷是否航向到下一個跟蹤路徑的坐標點，到達之后取出后面一個的路徑點作為新的跟蹤點.無人艇一個個坐標點位置的跟蹤最終完成整個路徑的跟蹤，從而實現了航跡的跟蹤.

通過曲線擬合的方式讓無人艇沿著分段的直線跟蹤航行，可以使無人艇完成各種復雜的跟蹤路徑.路徑中關鍵點的選取距離不能過短，否則無人艇航向調整沒有結束就要進行新的航向跟蹤，從而導致整個路徑跟蹤的失敗.

4 實驗結果

將已經完成的航向控制板與無人艇船端設備相連接，進行通電測試后，進行實船實驗.測試用的無人艇采用長約7.2 m，質量1.2 t左右，動力采用噴水推進的沖鋒艇，測試地點在武漢市黃陂區的木蘭湖水域，經實測整定和調試，航向保持器PID控制器的參數選擇為，比例系數為13，積分系數0.1，微分系數為10.經過十幾天的實船重復試驗，無人艇在有風和浪的影響下，依舊可以完成良好的折線航跡跟蹤、菱形和圓形路徑跟蹤、W形和復雜曲線的航跡跟蹤，驗證了無人艇航跡控制器的穩定性.軌跡跟蹤最大誤差大約5 m，無人艇在復雜水況下依舊能實現良好的跟蹤效果.航跡控制器能沿著指定的路徑航行為后續的雷達避碰、自主巡航提供良好的技術保障.

5 結 束 語

本文設計的無人艇航跡控制器，采用以STM32為主控芯片，通過W5500以太網通信模塊進行制定自定義協議格式進行通信，編寫噴泵位置控制程序和航向保持控制程序，完成了無人艇的航跡有效跟蹤.采用分離式的設計方法，將噴泵位置控制、航向保持控制和航跡跟蹤分開進行設計調試，實驗證明了該航跡控制器跟蹤誤差在允許的范圍內，穩定性良好，具有一定的實用價值.