徐小強(qiáng) 盛 磊

(武漢理工大學(xué)自動(dòng)化學(xué)院 武漢 430070)

0 引 言

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

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

1 控制器總體設(shè)計(jì)

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

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

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

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

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

2 硬件設(shè)計(jì)

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

2.1 電阻反饋模塊電路設(shè)計(jì)

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

圖3 電阻反饋模塊電路圖

2.2 噴泵驅(qū)動(dòng)模塊電路設(shè)計(jì)

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

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

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

3 軟件設(shè)計(jì)

3.1 系統(tǒng)總體軟件設(shè)計(jì)

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

3.2 主要模塊程序設(shè)計(jì)

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

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

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

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

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

3.2.2噴泵位置控制模塊程序設(shè)計(jì)

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

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

3.2.3航向保持器程序設(shè)計(jì)

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

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

3.2.4航跡跟蹤模塊程序設(shè)計(jì)

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

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

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

4 實(shí)驗(yàn)結(jié)果

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

5 結(jié) 束 語(yǔ)

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