張玉婷，陳褒丹,任 佳,Vladimir Shikhin

(1. 海南大學(xué) 信息科學(xué)技術(shù)學(xué)院，海南 ?？?70228；2.莫斯科動(dòng)力學(xué)院 自動(dòng)化與計(jì)算機(jī)工程學(xué)院，莫斯科)

?

基于STM32的無(wú)人艇自主導(dǎo)航控制系統(tǒng)的設(shè)計(jì)

張玉婷1，陳褒丹1,任 佳1,Vladimir Shikhin2

(1. 海南大學(xué) 信息科學(xué)技術(shù)學(xué)院，海南 ?？?70228；2.莫斯科動(dòng)力學(xué)院 自動(dòng)化與計(jì)算機(jī)工程學(xué)院，莫斯科)

針對(duì)無(wú)人艇遠(yuǎn)程操控和自主導(dǎo)航問(wèn)題，設(shè)計(jì)了基于STM32的無(wú)人艇自主導(dǎo)航控制系統(tǒng).船載系統(tǒng)以STM32微控制器為核心控制單元，采用互補(bǔ)濾波算法完成了無(wú)人艇陀螺儀角速度數(shù)據(jù)、加速度數(shù)據(jù)和磁力計(jì)數(shù)據(jù)的解算，并通過(guò)串級(jí)PID算法對(duì)航向角進(jìn)行閉環(huán)控制，最后通過(guò)LABVIEW軟件平臺(tái)設(shè)計(jì)開(kāi)發(fā)出地面站控制系統(tǒng)，實(shí)現(xiàn)了對(duì)無(wú)人艇的遠(yuǎn)程操控和自主導(dǎo)航功能.

無(wú)人艇； 姿態(tài)解算； PID控制器設(shè)計(jì)； 地面站設(shè)計(jì)

在當(dāng)今海洋戰(zhàn)略格局中，水面無(wú)人艇扮演的角色日益重要，它集船舶設(shè)計(jì)、智能控制、遠(yuǎn)程通信、運(yùn)動(dòng)控制和探測(cè)偵查等專業(yè)技術(shù)為一體[1].目前，水面無(wú)人艇主要應(yīng)用于軍事領(lǐng)域，主要執(zhí)行反潛、掃雷、反恐、緝私、精確打擊、海上搜救、水文地理勘測(cè)和通信等任務(wù)[2-4].但無(wú)人艇在民用領(lǐng)域的發(fā)展尚處于起步階段，其原因在于無(wú)人艇控制系統(tǒng)的智能化程度不高，應(yīng)用拓展性不強(qiáng).

文獻(xiàn)[5]進(jìn)行了模型船控制方面的研究，基于PC104/386工控機(jī)設(shè)計(jì)開(kāi)發(fā)了一個(gè)船舶運(yùn)動(dòng)控制試驗(yàn)平臺(tái)，并在海上環(huán)境通過(guò)了對(duì)該平臺(tái)的測(cè)試.沈陽(yáng)航天新光公司研發(fā)的“天象一號(hào)”是我國(guó)第一艘真正意義上的無(wú)人艇[6]，其具有自穩(wěn)定功能，能夠在復(fù)雜和惡劣的海洋環(huán)境中進(jìn)行工作，并采用增強(qiáng)型動(dòng)力系統(tǒng)的設(shè)計(jì)，將其續(xù)航時(shí)間提高到20 d左右.哈爾濱工程大學(xué)在水下機(jī)器人技術(shù)研究領(lǐng)域取得了大量成果，目前己經(jīng)完成了無(wú)人艇概念設(shè)計(jì)、艇體設(shè)計(jì)和實(shí)艇建造等工作[7-9].目前我國(guó)對(duì)水面無(wú)人艇的研究?jī)H僅處于起步階段，很多關(guān)鍵領(lǐng)域的難題需要攻克，尤其是在復(fù)雜海況下自動(dòng)導(dǎo)航和路徑規(guī)劃方面的研究[10].

筆者設(shè)計(jì)了基于STM32的無(wú)人艇自主導(dǎo)航控制系統(tǒng)，該系統(tǒng)使用三軸陀螺儀、三軸加速度計(jì)和三軸磁力計(jì)進(jìn)行姿態(tài)測(cè)量，采用互補(bǔ)濾波算法將各姿態(tài)數(shù)據(jù)進(jìn)行融合，使用GPS實(shí)現(xiàn)位置追蹤，通過(guò)PID控制算法實(shí)現(xiàn)對(duì)無(wú)人艇航向姿態(tài)的控制.實(shí)驗(yàn)證明該無(wú)人艇穩(wěn)定性好，可通過(guò)地面站系統(tǒng)進(jìn)行遠(yuǎn)程操控，同時(shí)實(shí)現(xiàn)自主導(dǎo)航的功能，適用于海洋環(huán)境的監(jiān)測(cè)以及海事搜救.

1 系統(tǒng)結(jié)構(gòu)與硬件組成

無(wú)人艇自主導(dǎo)航控制系統(tǒng)以32位微控制器STM32F103VC為核心控制單元，采用集成三軸陀螺儀、三軸加速度計(jì)的姿態(tài)板MPU6050和HMC5883L三軸磁力計(jì)作為無(wú)人艇的姿態(tài)測(cè)量系統(tǒng)，UBLOX NEO-M8N GPS模塊組成無(wú)人艇的位置測(cè)量系統(tǒng).各模塊通過(guò)I2C總線與主控制器通信，主控制器將姿態(tài)測(cè)量系統(tǒng)采集到的原始數(shù)據(jù)進(jìn)行融合處理，通過(guò)數(shù)據(jù)解算得到當(dāng)前無(wú)人艇的航向角、俯仰角和橫滾角.姿態(tài)和位置數(shù)據(jù)通過(guò)PID控制器運(yùn)算輸出相應(yīng)的PWM信號(hào)驅(qū)動(dòng)舵機(jī)，從而實(shí)現(xiàn)無(wú)人艇位置與航向的控制.無(wú)人艇硬件電路框架圖如圖1所示.

2 自主導(dǎo)航與控制系統(tǒng)設(shè)計(jì)

系統(tǒng)開(kāi)始運(yùn)行后，首先進(jìn)行各模塊的初始化，采集并上傳各傳感器信號(hào)，系統(tǒng)時(shí)間等；再將各傳感器數(shù)據(jù)融合解算出當(dāng)前無(wú)人艇姿態(tài)，每3 ms進(jìn)入一次中斷，輸出PWM信號(hào)實(shí)現(xiàn)對(duì)舵機(jī)的航向控制；無(wú)人艇當(dāng)前姿態(tài)數(shù)據(jù)每50 ms上傳一次，同時(shí)監(jiān)聽(tīng)地面站是否有指令發(fā)出.程序流程圖如圖2所示.

2.1 姿態(tài)解算 三軸加速度計(jì)、三軸陀螺儀和三軸磁力計(jì)是慣性測(cè)量中最常用的傳感器.陀螺儀采集無(wú)人艇在航向、俯仰、橫滾3個(gè)方向的角速度，數(shù)據(jù)通過(guò)時(shí)域積分得到各個(gè)方向的角度.陀螺儀動(dòng)態(tài)特性良好，但由于易受噪聲、溫度變化等因素的干擾，導(dǎo)致測(cè)量精度較低，存在累積漂移誤差，通過(guò)多次積分誤差增大，使得測(cè)量的角度與實(shí)際角度存在較大差異.加速計(jì)和磁力計(jì)靜態(tài)特性良好，不存在累計(jì)誤差問(wèn)題，但容易受機(jī)體振動(dòng)和外部磁場(chǎng)等因素的干擾，使其動(dòng)態(tài)可信度降低.從頻域的角度分析來(lái)看，陀螺儀適用于高頻段的姿態(tài)測(cè)量，加速度計(jì)、磁力計(jì)適用于低頻段的姿態(tài)測(cè)量.因此，單獨(dú)使用陀螺儀或者加速度計(jì)和磁力計(jì)，都存在一定偏差，需要對(duì)姿態(tài)測(cè)量系統(tǒng)進(jìn)行多傳感器數(shù)據(jù)融合，從而求解出準(zhǔn)確的姿態(tài)信息.卡爾曼濾波算法和互補(bǔ)濾波算法是多慣性器件姿態(tài)解算中的常用算法.卡爾曼濾波算法計(jì)算量大，對(duì)核心處理器的運(yùn)算速度和精度有很高的要求；互補(bǔ)濾波算法對(duì)慣性器件的精度要求較低，而且簡(jiǎn)單可靠.根據(jù)各慣性器件在頻域上的特點(diǎn)，采用互補(bǔ)濾波算法對(duì)姿態(tài)角進(jìn)行解算，算法如圖3所示.

將加速度數(shù)據(jù)進(jìn)行低通濾波，過(guò)濾掉短周期內(nèi)加速度數(shù)據(jù)的波動(dòng)，減少了船體晃動(dòng)對(duì)姿態(tài)解算的影響.陀螺儀在低頻段時(shí)，具有較大的測(cè)量誤差，所以需要對(duì)陀螺儀數(shù)據(jù)進(jìn)行高通濾波，再將處理后的數(shù)據(jù)進(jìn)行融合計(jì)算.

低通濾波器在頻域上的傳遞函數(shù)為

(1)

高通濾波器在頻域上的傳遞函數(shù)為

(2)

在地理坐標(biāo)系中，加速度計(jì)和磁力計(jì)分別測(cè)得當(dāng)前地理坐標(biāo)系下的重力矢量和正北方向的磁矢量，陀螺儀在船體坐標(biāo)系下測(cè)得當(dāng)前無(wú)人艇的姿態(tài)，對(duì)于同一個(gè)向量，在不同坐標(biāo)系下表示時(shí)，其大小和方向一定相同.加速度計(jì)和磁力計(jì)對(duì)陀螺儀數(shù)據(jù)的修正實(shí)質(zhì)上是對(duì)地理坐標(biāo)系與船體坐標(biāo)系相互轉(zhuǎn)換所生成的旋轉(zhuǎn)矩陣的修正.

將旋轉(zhuǎn)矩陣矢量v與加速度計(jì)測(cè)得的重力矢量a作叉積得到俯仰和橫滾角誤差ea，

(3)

無(wú)人艇的航向定義在地理坐標(biāo)系的X-O-Y平面，將加速度計(jì)修正后的旋轉(zhuǎn)矩陣矢量m與磁力計(jì)測(cè)得正北方向的磁矢量w作叉積得到航向角誤差eh，

(4)

叉積的模為角度誤差的正弦，在角度很小的情況下可近似于角度，且與陀螺儀積分誤差成正比，加權(quán)后與陀螺儀角增量疊加修正誤差.g為陀螺儀實(shí)測(cè)出的角速度，Δθ為修正后的陀螺儀角速度，γ和λ分別為加速度計(jì)和磁力計(jì)糾正系數(shù)，

Δθ=g·Δt+γ·ea+λ·eh，

(5)

再將修正后的陀螺儀角速度采用一階龍格庫(kù)塔法進(jìn)行四元數(shù)的更新，從而更新無(wú)人艇的姿態(tài).qi(n)為當(dāng)前四元數(shù)分量，qi(n+1)為更新后的四元數(shù)分量，

(6)

2.2 PID控制器設(shè)計(jì) 位置控制是在水平面內(nèi)對(duì)無(wú)人艇規(guī)劃航跡偏差的控制.通過(guò)GPS全球定位系統(tǒng)獲得當(dāng)前無(wú)人艇的經(jīng)度、緯度和時(shí)間等信息，算出當(dāng)前位置與目標(biāo)航點(diǎn)的距離與夾角，并結(jié)合無(wú)人艇自身解算出的姿態(tài)信息作為航跡偏差的反饋，從而控制無(wú)人艇的舵機(jī)改變航向，使得無(wú)人艇按規(guī)劃航線自主航行.

為了提高航向控制的穩(wěn)定性，采用串級(jí)PID控制算法，將航向角角度與角速度分別進(jìn)行PID控制.外環(huán)角度控制器的輸出作為內(nèi)環(huán)角速度控制器的輸入，不僅可以準(zhǔn)確控制姿態(tài)角同時(shí)也能增加角度控制的阻尼系數(shù)，讓角度變化緩慢進(jìn)行，實(shí)現(xiàn)角度的平穩(wěn)轉(zhuǎn)換.根據(jù)PID控制器的原理，設(shè)kp，ki，kd分別為比例、積分和微分系數(shù)，調(diào)整比例系數(shù)可以提高系統(tǒng)的響應(yīng)速度，減小穩(wěn)態(tài)誤差；調(diào)整積分系數(shù)可以減小數(shù)據(jù)上下震蕩的誤差；調(diào)整微分系數(shù)可以預(yù)先處理和修正信號(hào)，加快控制系統(tǒng)的速度，減少調(diào)整時(shí)間.

Δφ=kp(φ-φd)+ki∫(φ-φd)dt ，

(7)

(8)

2.3 地面站軟件設(shè)計(jì) 地面站是無(wú)人艇自主導(dǎo)航控制系統(tǒng)的重要組成部分，能夠?qū)崿F(xiàn)對(duì)無(wú)人艇航行狀態(tài)的自主控制和實(shí)時(shí)監(jiān)測(cè)功能，基于LABVIEW的地面站軟件控制系統(tǒng)可以滿足無(wú)人艇組裝測(cè)試階段的實(shí)驗(yàn)要求和航行階段的操縱控制與自主導(dǎo)航的要求.地面站主要功能有：

1) 實(shí)時(shí)監(jiān)測(cè) 實(shí)時(shí)監(jiān)測(cè)無(wú)人艇工作時(shí)的系統(tǒng)狀態(tài)，包括工作模式、電池電壓和舵機(jī)電壓等，顯示當(dāng)前無(wú)人艇的GPS位置信息，監(jiān)控航向PID參數(shù)；

2) 智能分析 通過(guò)采集航向PID的目標(biāo)值，當(dāng)前值，控制量參數(shù),分析其變化趨勢(shì),并結(jié)合其他傳感器信息統(tǒng)一分析,從而做出控制決策，保證無(wú)人艇的航行安全，如油門(mén)控制、檔位控制和航向控制等；

3) 電子地圖導(dǎo)航 電子地圖導(dǎo)航功能通過(guò)MapInfo公司推出的基于Active技術(shù)的可編程控件Mapx來(lái)實(shí)現(xiàn).將MapInfo格式的地圖文件導(dǎo)入LABVIEW軟件平臺(tái)，實(shí)現(xiàn)對(duì)地圖圖層的加載、居中、平移和放縮等操作，并通過(guò)電子地圖寫(xiě)入目標(biāo)點(diǎn)坐標(biāo)，實(shí)現(xiàn)GPS導(dǎo)航功能；

圖6 地面站主界面

4) 故障診斷 對(duì)無(wú)人艇上搭載的傳感器、電路等其他設(shè)備進(jìn)行狀態(tài)監(jiān)控，對(duì)故障及時(shí)警報(bào)并采取處理措施；

5) 數(shù)據(jù)處理、保存與顯示 直觀顯示各傳感器采集到的數(shù)據(jù),并實(shí)現(xiàn)實(shí)時(shí)數(shù)據(jù)存儲(chǔ).

地面站功能結(jié)構(gòu)如圖5所示.

2.3.1 地面站主界面設(shè)計(jì) 為了滿足無(wú)人艇組裝測(cè)試階段的實(shí)驗(yàn)要求和航行階段的操縱控制與自主導(dǎo)航的要求，地面站主界面包括6個(gè)模塊，分別為串口設(shè)置模塊、系統(tǒng)狀態(tài)模塊、航向PID監(jiān)控模塊、控制區(qū)域模塊、GPS位置信息模塊和PID參數(shù)設(shè)置模塊，地面站主界面如圖6所示.

2.3.2 通信協(xié)議 無(wú)人艇要實(shí)現(xiàn)與地面站控制系統(tǒng)有效、可靠、無(wú)差錯(cuò)地通信，離不開(kāi)通信協(xié)議的支持，數(shù)據(jù)幀格式和字節(jié)數(shù)都是在通信協(xié)議中定義.地面站控制系統(tǒng)根據(jù)約定的通信協(xié)議將無(wú)人艇發(fā)送的數(shù)據(jù)進(jìn)行數(shù)據(jù)解析，并把解析后數(shù)據(jù)更新到波形和數(shù)值顯示.同時(shí)地面站控制系統(tǒng)也將按照約定的通信協(xié)議將操作命令數(shù)據(jù)封裝成數(shù)據(jù)幀發(fā)送給無(wú)人艇，無(wú)人艇再按照通信協(xié)議解析，并執(zhí)行相應(yīng)操作,此過(guò)程提高了數(shù)據(jù)傳輸?shù)目煽啃?、安全性以及傳輸?shù)娜蒎e(cuò)能力.本文所采用幀格式如表1所示.

表1 幀格式

3 系統(tǒng)調(diào)試與實(shí)現(xiàn)

無(wú)人艇在海南省三亞市三亞灣進(jìn)行了海上測(cè)試，測(cè)試內(nèi)容包括無(wú)人艇的運(yùn)動(dòng)姿態(tài)的穩(wěn)定性，航向PID控制的精準(zhǔn)性，無(wú)線數(shù)據(jù)傳輸?shù)姆€(wěn)定性和地面站與無(wú)人艇命令交互的及時(shí)性.測(cè)試無(wú)人艇采用長(zhǎng)為3.8 m，重量120 kg的沖鋒艇，測(cè)試條件為東北風(fēng)5～6級(jí)，浪高1 m，涌高1.5 m，海溫21.2～26.9 ℃.

經(jīng)過(guò)多次重復(fù)的實(shí)驗(yàn)發(fā)現(xiàn)，在船舶眾多電磁環(huán)境較復(fù)雜的三亞灣港口，無(wú)人艇姿態(tài)測(cè)量數(shù)據(jù)基本未受到影響，無(wú)人艇能快速準(zhǔn)確地執(zhí)行來(lái)自地面站的命令，當(dāng)風(fēng)和海浪將無(wú)人艇吹離規(guī)劃目標(biāo)點(diǎn)時(shí)，無(wú)人艇能快速地自主修正航向.

圖7 測(cè)試路徑

測(cè)試結(jié)果表明，無(wú)人艇可以對(duì)地面站控制系統(tǒng)的指令做出快速響應(yīng)，自主調(diào)整航向到達(dá)目標(biāo)點(diǎn)；航行速度可在0～2 m·s-1范圍內(nèi)靈活變化；無(wú)人艇航行姿態(tài)比較穩(wěn)定，在有風(fēng)浪的海面具備良好的適航性.測(cè)試路徑如圖7所示.

在無(wú)人艇姿態(tài)測(cè)試中，綠色線代表航向角，紅色線代表俯仰角，藍(lán)色線代表橫滾角.由圖8可知在測(cè)試時(shí)間內(nèi)，地面站能精確的跟蹤無(wú)人艇姿態(tài)的變化，各個(gè)方向上的姿態(tài)角數(shù)據(jù)平滑變化，無(wú)突跳情況出現(xiàn).

在無(wú)人艇航向PID控制測(cè)試中，綠色線代表地面站輸出的目標(biāo)角度，紅色線代表無(wú)人艇的當(dāng)前實(shí)際角度.由圖9可知在測(cè)試時(shí)間內(nèi)，航向角在達(dá)到穩(wěn)態(tài)后，綠色線和紅色線基本重合，無(wú)人艇的實(shí)際航向角度基本等于地面站輸出的目標(biāo)航向角度，測(cè)試結(jié)果的誤差在允許范圍內(nèi)，進(jìn)而證明了無(wú)人艇自主導(dǎo)航控制系統(tǒng)的可行性.

4 小 結(jié)

設(shè)計(jì)了基于STM32的無(wú)人艇自主導(dǎo)航控制系統(tǒng)，對(duì)無(wú)人艇相關(guān)硬件及整體參數(shù)的構(gòu)成進(jìn)行了說(shuō)明，對(duì)控制模塊所涉及算法進(jìn)行了描述，并基于地面站對(duì)各模塊進(jìn)行調(diào)試.實(shí)驗(yàn)證明該無(wú)人艇穩(wěn)定性好，可通過(guò)地面站系統(tǒng)進(jìn)行遠(yuǎn)程操控，同時(shí)實(shí)現(xiàn)自主導(dǎo)航的功能，適用于海洋環(huán)境的監(jiān)測(cè)以及海事搜救.

[1] Yan R, Pang S, Sun H, et al. Development and missions of unmanned surface vehicle[J]. Journal of Marine Science and Application, 2010, 9(4): 451-457.

[2] Ellenrieder K D V. Development of a USV-based Bridge Inspection System[C]. Washington: IEEE Press, 2015.

[3] Liu Y, Noguchi N. Development of an unmanned surface vehicle for autonomous navigation in a paddy field[J]. Engineering in Agriculture Environment & Food, 2015, 9(1):21-26.

[4] Scott G P, Henshaw C G, Walker I D, et al. Autonomous Robotic Refueling of an Unmanned Surface Vehicle in Varying Sea States[C]. Hamburg: IEEE Press, 2015.

[5] 任光，牛井超，顏德文，等. 新型船舶運(yùn)動(dòng)控制平臺(tái)及海上試驗(yàn)[J]. 中國(guó)造船，2002,43(1):64-69.

[6] 劉鐵軍，劉文遠(yuǎn). 航天新光天人船 奉獻(xiàn)科技奧運(yùn)[J]. 航天工業(yè)管理，2008(8):46-46.

[7] 吳恭興. 無(wú)人艇操縱性與智能控制技術(shù)研究[D]. 哈爾濱：哈爾濱工程大學(xué)，2011.

[8] 廖煜雷. 無(wú)人艇的非線性運(yùn)動(dòng)控制方法研究 [D]. 哈爾濱：哈爾濱工程大學(xué)，2012.

[9] 曾薄文. 噴水推進(jìn)水面無(wú)人艇的非線性控制方法研究[D]. 哈爾濱：哈爾濱工程大學(xué)，2012.

[10] 馮浩明. 水面無(wú)人船航行控制器開(kāi)發(fā)研究[D]. 哈爾濱：哈爾濱工程大學(xué)，2013.

USV’s Autonomous Control System Design Based on STM32

Zhang Yuting, Chen Baodan, Ren Jia, Vladimir Shikhin

(1. College of Information Sciences and Technology, Hainan University, Haikou 570228, China;2. Institute of Automatics and Computer Engineering, Moscow Power Engineering Institute, Russia)

Aiming at the problem of the Unmanned Surface Vessel’s (USV) autonomous navigation, a autonomous navigation control system for USV based on STM32 was designed. The STM32 microprocessor was used as the core control unit of shipboard system. The complementary filtering algorithm was used to calculate gyro angular velocity data, accelerometer angel data and magnetometer heading data, cascade PID controller was used for the closed loop control; LABVIEW software was used to design and develop a ground control station, which achieve the function of the USV’s remote control and autonomous navigation.

USV; position calculation; PID controller design; ground control station design

2016-05-12

國(guó)家國(guó)際科技合作專項(xiàng)(2015DFR10510)；國(guó)家自然科學(xué)基金(61562018)；海南省高等學(xué)?？茖W(xué)研究項(xiàng)目(HNKY2014-04)；海南省重點(diǎn)實(shí)驗(yàn)室和工程技術(shù)研究中心建設(shè)專項(xiàng)項(xiàng)目(gczx2014004)

張玉婷(1992-)，女，重慶榮昌人，海南大學(xué)2014級(jí)碩士研究生，研究方向：海洋通信，E-mail:17982894@qq.com

陳褒丹(1962-)，男，海南海口人，高級(jí)工程師，研究方向：海洋通信，E-mail:1417252623@qq.com

1004-1729(2016)03-0221-07

TP 181

A DOl：10.15886/j.cnki.hdxbzkb.2016.0034