基于STM32的無人艇航跡控制器的設計與實現(xiàn)

徐小強 盛 磊

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

0 引 言

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

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

1 控制器總體設計

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

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

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

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

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

2 硬件設計

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

2.1 電阻反饋模塊電路設計

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

圖3 電阻反饋模塊電路圖

2.2 噴泵驅(qū)動模塊電路設計

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

2.3 以太網(wǎng)通信模塊電路設計

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

3 軟件設計

3.1 系統(tǒng)總體軟件設計

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

3.2 主要模塊程序設計

3.2.1以太網(wǎng)通信模塊

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

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

表1 以太網(wǎng)通信協(xié)議報文格式

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

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

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

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

3.2.3航向保持器程序設計

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

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

3.2.4航跡跟蹤模塊程序設計

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

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

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

4 實驗結果

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

5 結 束 語

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