基于STM32的無(wú)人船控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

(陜西科技大學(xué) 電氣與控制工程學(xué)院，西安 710021)

0 引言

無(wú)人船(Unmanned Surface Vehicles, USV)是一種具有自主航行能力，并且可自主實(shí)現(xiàn)環(huán)境感知、目標(biāo)探測(cè)等任務(wù)的智能化水面機(jī)器人[1-2]。其在民用與軍用上都具有重要作用，可以代替人們?cè)谒嫱瓿晌ｋU(xiǎn)、繁重的任務(wù)，因此無(wú)人船受到越來(lái)越廣泛的關(guān)注[3]。國(guó)際上以美國(guó)為首的幾個(gè)國(guó)家在無(wú)人船研究方面起步較早，最初主要以軍用無(wú)人船為主。隨著科學(xué)技術(shù)的突飛猛進(jìn)，無(wú)人船也由半自動(dòng)化向智能化發(fā)展，由軍用向民用發(fā)展[4]。目前國(guó)外的無(wú)人船技術(shù)較為成熟，相比之下，我國(guó)的無(wú)人船控制技術(shù)還有很大提升空間，許多關(guān)鍵領(lǐng)域的難題還需要攻克[5]。

無(wú)人船要想在復(fù)雜多變的環(huán)境中安全有效地完成人們指定的任務(wù)，就必須實(shí)現(xiàn)穩(wěn)定可靠的自主航行功能[6]。常用的自主導(dǎo)航方式有：慣性導(dǎo)航[7]、天文導(dǎo)航[8]、特征匹配[9]等，這些單一的導(dǎo)航方式容易受到干擾，因此需要使用組合導(dǎo)航方式以實(shí)現(xiàn)更精確的導(dǎo)航效果[10]。在無(wú)人船自主航行方面，能夠?qū)崿F(xiàn)高效率的路徑跟蹤是非常重要的，其準(zhǔn)確性的提高與采用的控制方法密切相關(guān)。目前常用的控制方法有PID控制，神經(jīng)網(wǎng)絡(luò)，模糊控制等，由于這些算法或多或少存在一些不足，所以在實(shí)際應(yīng)用中還需要將這些方法做些改進(jìn)。

無(wú)人船的控制主要是通過(guò)嵌入式編程和通信技術(shù)相結(jié)合實(shí)現(xiàn)的，經(jīng)過(guò)各個(gè)模塊之間的配合完成一系列任務(wù)。本文設(shè)計(jì)了一種基于STM32的無(wú)人船控制系統(tǒng)，使用GPS/IMU(Inertial measurement unit，慣性測(cè)量單元)組合導(dǎo)航方式，通過(guò)自抗擾控制技術(shù)控制無(wú)人船的航向，結(jié)合4G通信技術(shù)實(shí)現(xiàn)了無(wú)人船的遠(yuǎn)程操控以及路徑跟蹤的功能。

1 總體方案設(shè)計(jì)

本設(shè)計(jì)以實(shí)現(xiàn)無(wú)人船的自主導(dǎo)航為目標(biāo)，使船只能夠沿已經(jīng)規(guī)劃好的路徑前行，完成無(wú)人船的路徑跟蹤任務(wù)。本系統(tǒng)以全球定位系統(tǒng)與捷聯(lián)慣導(dǎo)系統(tǒng)相結(jié)合作為無(wú)人船的導(dǎo)航定位部分；以STM32作為主控芯片，集成了無(wú)人船的直流電機(jī)、電源顯示、信息采集等模塊的工作；以4G網(wǎng)絡(luò)為傳播媒介，可實(shí)現(xiàn)無(wú)人船的遠(yuǎn)程操控以及實(shí)時(shí)監(jiān)視的功能，并且開發(fā)了上位機(jī)軟件，可在手機(jī)上安裝該軟件，操作極其方便，如圖1所示為該系統(tǒng)的方案設(shè)計(jì)。

2 系統(tǒng)硬件設(shè)計(jì)

無(wú)人船控制系統(tǒng)主要由導(dǎo)航定位模塊、人機(jī)交互模塊、電機(jī)控制模塊、4G通信模塊等幾部分組成，選取STM32F103作為主控芯片，圖2所示為該系統(tǒng)的硬件結(jié)構(gòu)圖。

圖2 硬件結(jié)構(gòu)圖

從圖中可以看到系統(tǒng)使用了兩個(gè)STM32F103控制芯片，左邊的STM32芯片主要用來(lái)讀取數(shù)據(jù)，并且會(huì)對(duì)GPS及IMU的數(shù)據(jù)進(jìn)行相關(guān)處理；右邊的STM32芯片主要用來(lái)控制無(wú)人船運(yùn)動(dòng)，可通過(guò)CAN總線控制電機(jī)驅(qū)動(dòng)；4G模塊可以上網(wǎng)，實(shí)現(xiàn)無(wú)人船的遠(yuǎn)程操控功能；其他硬件如指示燈等可以顯示無(wú)人船的各種狀態(tài)。

2.1 GPS/IMU組合導(dǎo)航模塊

考慮到實(shí)用性與精確性，所選取的GPS模塊的型號(hào)為GYGPSV1-7M，其主芯片是ublox芯片，穩(wěn)定性高、應(yīng)用廣泛。模塊的供電電壓為3 V和5 V均可，本次設(shè)計(jì)采用5 V供電。為了提高信息處理速率將該模塊的波特率設(shè)置為115 200 bps，將信息刷新頻率設(shè)置為5 Hz。該模塊主要通過(guò)串口通信方式實(shí)現(xiàn)其與STM32之間的數(shù)據(jù)傳輸，且?guī)в蠩EPROM掉電保存參數(shù)數(shù)據(jù)的功能和專用的數(shù)據(jù)備份電池。

系統(tǒng)使用的IMU模塊型號(hào)為Mini IMU AHRS，該模塊由3軸磁力計(jì)、3軸角速率、3軸加速度及氣壓高度計(jì)組成，可以更全面地獲取載體的當(dāng)前狀態(tài)。MiniIMU的控制器可以運(yùn)行在72 M的主頻上，擁有更快的處理速度，傳感器通過(guò)I2C接口與控制器連接，可以在第一時(shí)間讀取最新的數(shù)據(jù)，快速響應(yīng)姿態(tài)變化。IMU模塊工作電壓為4.0～7.0 V，本設(shè)計(jì)使用5 V工作電壓，該模塊通過(guò)串口向外界發(fā)送信息，波特率設(shè)置為115 200 bps。

如圖2所示,GPS和IMU模塊都是通過(guò)串口連接到數(shù)據(jù)采集板上。將數(shù)據(jù)獲取后，STM32便會(huì)進(jìn)一步解算數(shù)據(jù)，使無(wú)人船的定位信息更加準(zhǔn)確，之后再將該信息通過(guò)CAN總線發(fā)送給運(yùn)動(dòng)控制板，以便進(jìn)行下一步操作。

2.2 電機(jī)驅(qū)動(dòng)模塊

電機(jī)驅(qū)動(dòng)在無(wú)人船的運(yùn)動(dòng)控制中尤為重要，STM32通過(guò)電機(jī)驅(qū)動(dòng)器來(lái)控制無(wú)人船的左右推進(jìn)器，進(jìn)而影響其運(yùn)動(dòng)狀態(tài)。系統(tǒng)選取的驅(qū)動(dòng)器為RMDS-402有刷伺服電機(jī)驅(qū)動(dòng)器，該驅(qū)動(dòng)器的供電電壓為10～58 V，本次設(shè)計(jì)使用12 V供電電壓。其通信方式有CAN總線通信、RS485通信及RS232通信等方式，由于CAN總線適用于大數(shù)據(jù)量、短距離通信，可在多主多從或者各個(gè)節(jié)點(diǎn)平等的現(xiàn)場(chǎng)中使用，所以本次選用CAN總線通信方式效率最高。

運(yùn)動(dòng)控制板將接收來(lái)的信息經(jīng)過(guò)一系列計(jì)算，通過(guò)CAN總線發(fā)送給電機(jī)驅(qū)動(dòng)器相應(yīng)指令，驅(qū)動(dòng)器再通過(guò)改變PWM的值來(lái)改變直流電機(jī)的速度。因?yàn)榇簧嫌袃蓚€(gè)推進(jìn)器，所以需要兩個(gè)驅(qū)動(dòng)器來(lái)分別控制對(duì)應(yīng)的推進(jìn)器，使無(wú)人船的運(yùn)動(dòng)狀態(tài)發(fā)生改變。

2.3 4G通信模塊

本系統(tǒng)使用的4G通信模塊是華為公司生產(chǎn)的型號(hào)為ME909s-821的模塊，它的工作頻段為1 800 MHz或900 MHz，電源供電電壓為3.4～4.7 V。該模塊有很多外圍接口，包括LGA接口、UART接口、USB接口、USIM卡接口、GPIO接口等，模塊與控制器之間采用串口通信模式，波特率設(shè)置為115 200 bps。使用時(shí)把SIM卡插好，一切準(zhǔn)備就緒，將4G天線引出豎直向上放到無(wú)人船的外面，有利于信號(hào)遠(yuǎn)距離傳輸。

2.4 供電部分

由于系統(tǒng)使用的硬件模塊較多，所需供電電壓有多種，因此需要設(shè)計(jì)合理的供電系統(tǒng)。經(jīng)過(guò)多方面考慮，無(wú)人船采用額定電壓為24 V的蓄電池供電。當(dāng)無(wú)人船全速前進(jìn)時(shí)所需電流較大，因此將兩個(gè)相同的蓄電池并聯(lián)安裝，這樣不僅可以使船只有足夠的動(dòng)力還可以延長(zhǎng)船只運(yùn)行時(shí)間。

為了將蓄電池的供電合理應(yīng)用在無(wú)人船各個(gè)硬件模塊上，需要將24 V電壓依次轉(zhuǎn)換成12 V、5 V、3.3 V等，12 V電壓主要給推進(jìn)器供電，5 V電壓給GPS、IMU等模塊供電，3.3 V電壓主要給主芯片供電，如圖3所示為系統(tǒng)電源轉(zhuǎn)換部分的原理圖。

除了以上主要模塊之外，整個(gè)系統(tǒng)還設(shè)計(jì)了指示燈、液晶屏等硬件部分用來(lái)顯示無(wú)人船當(dāng)前狀態(tài)和電量等信息，這樣不僅可以方便人們隨時(shí)觀察，還起到了美化船只外觀的作用。

3 系統(tǒng)軟件設(shè)計(jì)

3.1 GPS/IMU的數(shù)據(jù)融合

本系統(tǒng)采用松組合方式來(lái)實(shí)現(xiàn)GPS與IMU的數(shù)據(jù)融合，該方式具有結(jié)構(gòu)簡(jiǎn)單，易于實(shí)現(xiàn)等優(yōu)點(diǎn)[11]，主要是將GPS和IMU分別測(cè)出的位置或速度信息通過(guò)卡爾曼濾波器實(shí)現(xiàn)數(shù)據(jù)融合，并且融合后的信息還會(huì)反饋回IMU，以達(dá)到消除捷聯(lián)慣導(dǎo)系統(tǒng)累積誤差的目的，其結(jié)構(gòu)原理如圖4所示。

圖4 GPS/IMU松組合結(jié)構(gòu)框圖

卡爾曼濾波器的輸入為組合導(dǎo)航的量測(cè)值，輸出為捷聯(lián)慣導(dǎo)系統(tǒng)的估計(jì)誤差值，其完整遞推過(guò)程如下所示：

式(1)是狀態(tài)預(yù)測(cè)方程，其中Xk/k-1為上一狀態(tài)的預(yù)測(cè)結(jié)果，Φk/k-1為系統(tǒng)參數(shù)，Xk-1為上一狀態(tài)的最優(yōu)結(jié)果：

Xk/k-1=Φk/k-1Xk-1

(1)

式(2)為預(yù)測(cè)方差陣，其中Pk/k-1是Xk/k-1對(duì)應(yīng)的協(xié)方差，Pk-1是Xk-1對(duì)應(yīng)的協(xié)方差，Γk-1是系統(tǒng)參數(shù)，Qk-1是系統(tǒng)過(guò)程的協(xié)方差：

(2)

式(3)是卡爾曼濾波增益，Kk為卡爾曼增益，Hk是系統(tǒng)參數(shù)，Rk是為噪聲的協(xié)方差：

(3)

式(4)為狀態(tài)估計(jì)觀測(cè)更新，其中Xk是當(dāng)前狀態(tài)的最優(yōu)化估計(jì)值，Zk為系統(tǒng)的測(cè)量值：

Xk=Xk/k-1+Kk(Zk-HkXk/k-1)

(4)

式(5)是估計(jì)均方差，其中Pk是Xk的協(xié)方差：

Pk=(I-KkHk)Pk/k-1

(5)

3.2 無(wú)人船路徑跟蹤

要實(shí)現(xiàn)無(wú)人船的路徑跟蹤，使船只可以從任意初始位置駛?cè)腩A(yù)先規(guī)劃好的路徑，在該過(guò)程中，主要通過(guò)控制船只的航向來(lái)實(shí)現(xiàn)無(wú)人船路徑跟蹤目的，故設(shè)計(jì)合理的航向控制器至關(guān)重要。

自抗擾控制器(Active Disturbance Rejection Control，ADRC)是一種不依賴被控對(duì)象精確模型的、能夠替代PID控制技術(shù)的新型實(shí)用數(shù)字控制技術(shù)。該控制器可以很好地控制無(wú)人船航向，通過(guò)規(guī)劃好的路徑與實(shí)際船舶位置的對(duì)比可以得到無(wú)人船的期望航向，將期望航向作為輸入，實(shí)際測(cè)量航向作為輸出，經(jīng)過(guò)控制器的作用使無(wú)人船實(shí)現(xiàn)路徑跟蹤的目的。

自抗擾控制器主要由跟蹤微分器、擴(kuò)張狀態(tài)觀測(cè)器、狀態(tài)誤差反饋控制律與擾動(dòng)估計(jì)補(bǔ)償四部分組成[12]，本次使用的是二階控制器，其結(jié)構(gòu)如圖5所示。

圖5 自抗擾控制器

式(6)和(7)為微分跟蹤器，其中v為系統(tǒng)輸入，即目標(biāo)航向，v1為v的跟蹤信號(hào)，v2是v1的微分，fhan(·)為最速綜合函數(shù)，參數(shù)r為控制量增益，決定了跟蹤速度，h和h0都為采樣步長(zhǎng)：

v1(k+1)=v1(k)+hv2(k)

(6)

v2(k+1)=v2(k)+hfhan(v1(k)-v,v2(k),r,h0)

(7)

式(8)～(11)為擴(kuò)張狀態(tài)觀測(cè)器，其中參數(shù)β01，β02，β03都是由采樣步長(zhǎng)來(lái)決定的，y為系統(tǒng)輸出，即實(shí)際航向，fal(·)是一種特殊的非線性結(jié)構(gòu)的濾波器：

e=z1(k)-y(k)

(8)

z1(k+1)=z1(k)+h(z2(k)-β01e)

(9)

z2(k+1)=z2(k)+h(z3(k)-β02fal(e,0.5,h)+bu(k))

(10)

z3(k+1)=z3(k)-hβ03fal(e,0.25,h)

(11)

式(12)～(14)為非線性組合:

e1=v1(k)-z1(k)

(12)

e2=v2(k)-z2(k)

(13)

u0=β01fal(e,0.5,h)+β02fal(e,0.25,h)

(14)

式(15)為擾動(dòng)補(bǔ)償?shù)目刂屏?

u(k)=u0-z3/b

(15)

3.3 上位機(jī)監(jiān)控軟件設(shè)計(jì)

無(wú)人船上位機(jī)軟件是基于Android系統(tǒng)采用Java語(yǔ)言開發(fā)的，這款A(yù)pp適用于Android 5.0以上的手機(jī)，穩(wěn)定性好，其運(yùn)行界面如圖6所示。

圖6 上位機(jī)軟件

無(wú)人船上位機(jī)軟件主要由以下幾部分組成：

1)地圖顯示區(qū)：該軟件添加了高德地圖應(yīng)用，可以用來(lái)顯示船只的航行情況，包括位置、軌跡等信息。

2)船只狀態(tài)顯示：當(dāng)無(wú)人船通過(guò)網(wǎng)絡(luò)與手機(jī)軟件成功連接上之后，軟件主頁(yè)就會(huì)顯示該船只的名字、電量、功率、行駛速度等信息，方便用戶更直觀地了解無(wú)人船的當(dāng)前情況。

3)路徑設(shè)置：用戶可通過(guò)手機(jī)觸屏在地圖上為船只設(shè)置預(yù)定路徑，設(shè)置好后，軌跡的經(jīng)緯度會(huì)通過(guò)4G網(wǎng)絡(luò)傳給無(wú)人船，之后點(diǎn)擊“開始導(dǎo)航”，無(wú)人船就可以按照預(yù)設(shè)路徑進(jìn)行導(dǎo)航。

4)導(dǎo)航模式：自動(dòng)導(dǎo)航模式下，地圖上會(huì)顯示兩條顏色不同的路徑，一條是設(shè)置好的預(yù)定路徑，另一條則是無(wú)人船的實(shí)際路徑，方便用戶觀察。在自動(dòng)導(dǎo)航過(guò)程中，用戶可以隨時(shí)退出無(wú)人船的導(dǎo)航模式。

5)遙控模式：該模式下是把手機(jī)當(dāng)做遙控器來(lái)控制無(wú)人船的行駛狀態(tài)，可控制無(wú)人船的前進(jìn)、后退、轉(zhuǎn)彎、停止等。

6)其他信息顯示：該軟件會(huì)在歷史數(shù)據(jù)中保留每一次船只的運(yùn)行狀態(tài)，包括時(shí)間、航速、路徑等信息，用戶也可以手動(dòng)刪除這些信息。如果船上裝了水質(zhì)模塊、雷達(dá)等，軟件也會(huì)相應(yīng)的顯示這些信息。

4 系統(tǒng)測(cè)試結(jié)果與分析

為測(cè)試所設(shè)計(jì)的無(wú)人船控制系統(tǒng)的可行性和穩(wěn)定性，在西安豐慶公園的人工湖里進(jìn)行了多次測(cè)試。在做測(cè)試之前，需要準(zhǔn)備的有：確保無(wú)人船的所有硬件模塊可以正常工作，各個(gè)接口供電正常，顯示屏與指示燈反應(yīng)的船只狀態(tài)要在正常范圍內(nèi)。檢查上位機(jī)軟件能否正常運(yùn)行，保證該軟件可以通過(guò)4G網(wǎng)絡(luò)連接到無(wú)人船的4G模塊上，確保蓄電池電量充足等。

4.1 航向跟蹤測(cè)試

可以通過(guò)上位機(jī)軟件給無(wú)人船發(fā)送目標(biāo)航向，無(wú)人船再通過(guò)航向控制器自動(dòng)調(diào)節(jié)船體后面的推進(jìn)器以達(dá)到控制無(wú)人船航向的目的，使船只可以保持在期望航向上。

將目標(biāo)航向角設(shè)置為200°作為無(wú)人船的航向跟蹤測(cè)試，圖7為無(wú)人船實(shí)際航向變化，從圖中可以看到無(wú)人船接收到指令時(shí)航向是100°左右，接著便會(huì)向目標(biāo)航向調(diào)節(jié)，20 s之后，可以穩(wěn)定在期望航向200°了，之后以該航行角繼續(xù)前行。如圖8所示為無(wú)人船實(shí)際航向角與期望航向角的航向差，從20 s之后計(jì)算無(wú)人船航向差的均方差為2.18°，說(shuō)明航向控制效果較好。

圖7 無(wú)人船航向變化

圖8 航向差

4.2 路徑跟蹤實(shí)驗(yàn)

可通過(guò)上位機(jī)軟件向無(wú)人船發(fā)送多個(gè)位點(diǎn)的坐標(biāo)，其中相鄰的位點(diǎn)可確定一條路徑。本次測(cè)試共有4個(gè)位點(diǎn)，因此是3條路徑，其方向則是從前一位點(diǎn)指向后一位點(diǎn)。無(wú)人船接收到指令后，再通過(guò)控制系統(tǒng)調(diào)節(jié)左右推進(jìn)器使船只能夠沿著期望路徑行駛。

試驗(yàn)設(shè)置的4個(gè)位點(diǎn)分別為：E108.898789°，N34.247206°；E108.898816°，N34.247089°；E108.899009°，N34.247105°；E108.898958°，N34.247251°。圖9所示為無(wú)人船的路徑跟蹤航跡圖，圖10為無(wú)人船的實(shí)際位置與期望路徑的垂直距離。由圖9、10可知完成本次路徑跟蹤測(cè)試，無(wú)人船共行駛了約2 min，從實(shí)際位點(diǎn)到期望初始位點(diǎn)約10 s，在整個(gè)跟蹤過(guò)程中計(jì)算的垂直距離均方差為0.53 m，因此在路徑跟蹤上無(wú)人船控制效果較好。

圖10 路徑跟蹤距離差

通過(guò)對(duì)無(wú)人船的航向跟蹤與路徑跟蹤的測(cè)試結(jié)果分析，本無(wú)人船控制系統(tǒng)具有較好的可行性與穩(wěn)定性。

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

本次設(shè)計(jì)的基于STM32無(wú)人船導(dǎo)航控制系統(tǒng)，采用了GPS/IMU組合導(dǎo)航實(shí)現(xiàn)了無(wú)人船的精確定位，通過(guò)設(shè)計(jì)自抗擾控制器實(shí)現(xiàn)了無(wú)人船的航向控制及路徑跟蹤的目的。系統(tǒng)各個(gè)模塊分工明確運(yùn)行穩(wěn)定，經(jīng)測(cè)試表明無(wú)人船的控制效果較好，實(shí)際路徑與預(yù)設(shè)路徑偏差小，達(dá)到了預(yù)期目標(biāo)。設(shè)計(jì)了基于Java開發(fā)的上位機(jī)軟件，其界面友好、功能實(shí)用可以安裝在手機(jī)上，方便人員使用，并結(jié)合4G網(wǎng)絡(luò)完成上位機(jī)與無(wú)人船之間的通信，實(shí)現(xiàn)了人機(jī)交互功能。本文通過(guò)對(duì)無(wú)人船遠(yuǎn)程操控及自主導(dǎo)航的測(cè)試，獲得了大量研究數(shù)據(jù)，證實(shí)了本系統(tǒng)的有效性及可行性，為該項(xiàng)目的后續(xù)研究奠定了基礎(chǔ)。