魚(yú)雷狀小型無(wú)人艇路徑跟蹤控制系統(tǒng)研制

翁 昱,曾慶軍,李 維,李 昂,戴曉強(qiáng)

(1.江蘇科技大學(xué) 自動(dòng)化學(xué)院,江蘇 鎮(zhèn)江 212100; 2.江蘇科技大學(xué) 計(jì)算機(jī)學(xué)院,江蘇 鎮(zhèn)江 212100)

無(wú)人艇[1]是依靠遙控或自動(dòng)控制方式在水面航行的無(wú)人智能化平臺(tái)。因其具有成本低、機(jī)動(dòng)性好、體積小等特點(diǎn),在軍民用領(lǐng)域有著較為廣泛的應(yīng)用前景[2]。國(guó)外,英國(guó)研制的“水獺”無(wú)人艇,長(zhǎng)約2 m,寬約1.08 m,最大時(shí)速6 kn,搭載多波束回聲測(cè)深儀等傳感器,可應(yīng)用于湖泊、河流測(cè)深等場(chǎng)景[3];國(guó)內(nèi),哈爾濱工程大學(xué)研制的“天象一號(hào)”無(wú)人艇,具有自主導(dǎo)航、自主避障等功能,可應(yīng)用于水文、氣象監(jiān)測(cè)等領(lǐng)域[4]。近年來(lái),隨著無(wú)人艇自動(dòng)化和智能化水平的提高,無(wú)人艇智能系統(tǒng)引起越來(lái)越多研究者的關(guān)注[5]。

無(wú)人艇路徑跟蹤控制系統(tǒng)[6]是實(shí)現(xiàn)無(wú)人艇自主化的基礎(chǔ)和核心,是無(wú)人艇安全航行的重要保障。趙夢(mèng)鎧等[7]針對(duì)固定雙槳驅(qū)動(dòng)的無(wú)人艇,采用模糊PID控制方法設(shè)計(jì)了一種自主直線路徑跟蹤系統(tǒng),該系統(tǒng)運(yùn)行穩(wěn)定,有效降低了目標(biāo)路徑的跟蹤誤差;柳晨光等[8]為了解決環(huán)境干擾下的精度、可靠路徑跟蹤問(wèn)題,設(shè)計(jì)了一種自適應(yīng)路徑跟蹤控制系統(tǒng),有效提高了系統(tǒng)的可靠性和跟蹤精度。

本文為了提高無(wú)人艇水域作業(yè)時(shí)的航向跟蹤精度,設(shè)計(jì)了一款魚(yú)雷狀小型無(wú)人艇路徑跟蹤控制系統(tǒng)。首先搭建了軟硬件系統(tǒng)框架,然后設(shè)計(jì)了LOS-MPC路徑跟蹤控制算法,在岸基控制系統(tǒng)中進(jìn)行坐標(biāo)系轉(zhuǎn)換,用采集視線法計(jì)算期望航向;在艇載控制系統(tǒng)中將航向誤差作為MPC控制器[9]的輸入,實(shí)時(shí)輸出舵機(jī)PWM信號(hào)以調(diào)整航向;最后開(kāi)展湖試實(shí)驗(yàn)驗(yàn)證系統(tǒng)的可靠性和跟蹤精度。

1 控制系統(tǒng)概述

魚(yú)雷狀小型無(wú)人艇控制系統(tǒng)具有手動(dòng)控制和自主巡航2種模式,由上位機(jī)和下位機(jī)控制系統(tǒng)組成。其中,上位機(jī)控制系統(tǒng)為岸基控制系統(tǒng),下位機(jī)控制系統(tǒng)為艇載控制系統(tǒng)。圖1是魚(yú)雷狀小型無(wú)人艇控制系統(tǒng)組成圖。岸基控制系統(tǒng)由岸基端計(jì)算機(jī)(岸基通信終端)和數(shù)傳電臺(tái)等設(shè)備組成,利用數(shù)傳通信方式接收GPS、慣導(dǎo)、溫濕度等數(shù)據(jù)并進(jìn)行顯示,通過(guò)4G信號(hào)接收?qǐng)D傳畫(huà)面信息,艇載控制系統(tǒng)以STM32F429芯片為主控芯片,通過(guò)RS232、RS485等通信方式對(duì)GPS、慣導(dǎo)等設(shè)備數(shù)據(jù)進(jìn)行采集和解算,將自身狀態(tài)回傳至岸基控制系統(tǒng)。

無(wú)人艇路徑跟蹤控制系統(tǒng)的核心是艇載主控軟件和岸基端計(jì)算機(jī),岸基端計(jì)算機(jī)根據(jù)期望路徑信息與無(wú)人艇GPS測(cè)得的當(dāng)前位置信息,利用視線(Line of Sight,LOS)制導(dǎo)算法實(shí)時(shí)計(jì)算出期望航向并下達(dá)指令;在艇載主控系統(tǒng)中建立預(yù)測(cè)模型,結(jié)合預(yù)測(cè)模型計(jì)算出預(yù)測(cè)時(shí)域內(nèi)系統(tǒng)狀態(tài)序列、輸出序列和控制序列,通過(guò)在線求解基于狀態(tài)量誤差和控制量的目標(biāo)函數(shù),輸出舵機(jī)控制信號(hào)調(diào)整無(wú)人艇航向。

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

艇載控制系統(tǒng)采用12 V鋰電池包供電,由單片機(jī)模塊控制,模塊以STM32F429芯片為主控芯片,該芯片主頻為180 MHz,具有性價(jià)比高、配置靈活、功耗低等特點(diǎn)[10]。豐富的通信外設(shè)為控制系統(tǒng)[11]提供了很大的便利,也為導(dǎo)航模塊提供了完整的通信條件,魚(yú)雷狀小型無(wú)人艇控制系統(tǒng)框架如圖2所示。

魚(yú)雷狀小型無(wú)人艇控制板如圖3所示,由最小系統(tǒng)、電壓轉(zhuǎn)換電路、驅(qū)動(dòng)電路、通信電路等組成。最小系統(tǒng)[12]主要包括MCU運(yùn)行所需的一些基本外圍電路,為控制系統(tǒng)提供基本條件;電源電路由12 V轉(zhuǎn) 5 V和5 V轉(zhuǎn)3.3 V的電壓轉(zhuǎn)換電路組成,為芯片和外設(shè)提供穩(wěn)定的電壓;驅(qū)動(dòng)電路主要由隔離電路等組成,通過(guò)控制芯片定時(shí)器產(chǎn)生PWM信號(hào),經(jīng)光耦隔離輸出穩(wěn)定的控制信號(hào),驅(qū)動(dòng)推進(jìn)器和舵機(jī)。

由于導(dǎo)航傳感器的外設(shè)接口不同,單片機(jī)控制芯片不能產(chǎn)生通信所需要的電平信號(hào),因此設(shè)計(jì)了如圖4所示的通信電路,分別將單片機(jī)輸出的TTL電平信號(hào)轉(zhuǎn)為RS232電平信號(hào)和RS485差分信號(hào),其中輸出的RS232電平信號(hào)與GPS接口相連,采集定位數(shù)據(jù);輸出的RS485差分信號(hào)與慣導(dǎo)設(shè)備接口相連,采集載體運(yùn)動(dòng)時(shí)的姿態(tài)數(shù)據(jù),MCU將采集到的數(shù)據(jù)進(jìn)行解析處理,通過(guò)數(shù)傳電臺(tái)上傳至岸基控制系統(tǒng)。

圖4 通信電路

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

無(wú)人艇路徑跟蹤軟件主要有GPS和慣導(dǎo)數(shù)據(jù)解算與存儲(chǔ)、路徑跟蹤算法的實(shí)現(xiàn)、舵機(jī)驅(qū)動(dòng)控制等功能。圖5是路徑跟蹤算法流程圖。在上位機(jī)中執(zhí)行LOS制導(dǎo)算法[13]程序,在下位機(jī)中進(jìn)行MPC控制算法的處理。首先導(dǎo)入地圖,設(shè)置目標(biāo)跟蹤點(diǎn),進(jìn)行坐標(biāo)系轉(zhuǎn)換,利用LOS制導(dǎo)方法生成接納圓,計(jì)算期望航向,給下位機(jī)下達(dá)控制指令。接著下位機(jī)接收到指令后,采集當(dāng)前狀態(tài)信息進(jìn)行卡爾曼濾波處理,上傳位置信息,同時(shí)將航向誤差作為MPC控制器輸入,輸出舵角控制指令并生成相應(yīng)PWM信號(hào),驅(qū)動(dòng)舵機(jī),調(diào)整航向;最后上位機(jī)實(shí)時(shí)計(jì)算當(dāng)前位置與期望位置的距離,當(dāng)達(dá)到接納圓后,進(jìn)行路徑更新,當(dāng)達(dá)到終點(diǎn)后,下達(dá)停止指令。

圖5 路徑跟蹤算法流程圖

3.1 上位機(jī)算法設(shè)計(jì)

上位機(jī)軟件是基于QT5平臺(tái)開(kāi)發(fā)的,采用C++語(yǔ)言編寫(xiě),上位機(jī)軟件按照預(yù)先約定的協(xié)議通過(guò)RS232串口通信方式與下位機(jī)進(jìn)行信息交互[14]。

上位機(jī)路徑跟蹤子程序主要有坐標(biāo)系轉(zhuǎn)換、LOS制導(dǎo)算法。其中,坐標(biāo)系轉(zhuǎn)換程序是調(diào)用百度地圖的JS交互接口,根據(jù)起始點(diǎn)坐標(biāo)經(jīng)緯度和結(jié)束坐標(biāo)經(jīng)緯度,在地圖上標(biāo)注目標(biāo)跟蹤點(diǎn),考慮到百度地圖提供的是基于BD09坐標(biāo)系下的經(jīng)緯度信息,而GPS所需的是基于WGS84坐標(biāo)系下的經(jīng)緯度信息,因此進(jìn)行坐標(biāo)系轉(zhuǎn)換處理。

LOS制導(dǎo)算法[15]根據(jù)距離的遠(yuǎn)近來(lái)跟蹤期望位置,再由自身位置計(jì)算出期望航向角,具有較好的實(shí)時(shí)性。圖6是LOS制導(dǎo)算法原理圖。

圖6 LOS制導(dǎo)算法原理圖

圖6中:P為無(wú)人艇當(dāng)前位置;Δ為無(wú)人艇可視距離;Pn+1為當(dāng)前目標(biāo)航向點(diǎn);ψlos為期望航向。

LOS制導(dǎo)算法具體步驟如下。

① 計(jì)算期望航向,公式為

(1)

式中:(xlos,ylos)為L(zhǎng)OS點(diǎn)位置;(x,y)為無(wú)人艇當(dāng)前位置。

② 更新目標(biāo)路徑點(diǎn)。判斷無(wú)人艇是否在以Pn+1為圓心,R為半徑的接納圓內(nèi),若滿足,則跟蹤下一目標(biāo)路徑點(diǎn)Pn+2,公式為

[xn+1-x(t)]2+[yn+1-y(t)]2≤R2

(2)

式中:R為接納圓半徑,一般取3～5倍無(wú)人艇的長(zhǎng)度。

3.2 下位機(jī)算法設(shè)計(jì)

Keil5是一款兼容單片機(jī)的C語(yǔ)言軟件開(kāi)發(fā)系統(tǒng),提供了包括C編輯器、庫(kù)管理等在內(nèi)的完整開(kāi)發(fā)方案,擁有強(qiáng)大的軟件仿真調(diào)試工具和方便易用的集成環(huán)境。因此下位機(jī)軟件基于Keil5軟件開(kāi)發(fā),采用C語(yǔ)言編寫(xiě)[16]。

下位機(jī)路徑跟蹤子程序主要是MPC控制程序,MPC控制程序過(guò)程如圖7所示。

圖7 MPC控制過(guò)程

具體算法步驟如下。

① 建立舵角小幅度操縱模型作為預(yù)測(cè)模型,本文采用的是一階野本(Nomoto)模型,其數(shù)學(xué)表達(dá)式為

(3)

式中:δ為舵角;ψ為航向角;r為轉(zhuǎn)首角速度;K、T為操縱性指數(shù)。

② 當(dāng)下位機(jī)接收到期望航向信息后,采集當(dāng)前航向信息,進(jìn)行卡爾曼濾波,并計(jì)算期望航向誤差作為MPC控制器的輸入。

③ 設(shè)置系統(tǒng)約束。由于無(wú)人艇的推進(jìn)器驅(qū)動(dòng)機(jī)構(gòu)和舵機(jī)受機(jī)械性能影響,其運(yùn)動(dòng)范圍和速率有限。因此,為了無(wú)人艇的作業(yè)安全,需對(duì)控制量輸入進(jìn)行約束,公式為

umin

(4)

式中:umin為控制量輸入下限;umax為控制量輸入上限;u(t+i)為t+i時(shí)刻的控制量。

④ 基于狀態(tài)量誤差和控制量,構(gòu)建在t時(shí)刻的模型預(yù)測(cè)控制目標(biāo)函數(shù),進(jìn)行尋優(yōu)求解,公式為

(5)

式中:Np為預(yù)測(cè)步長(zhǎng);x(t+i)為t+i時(shí)刻的預(yù)測(cè)狀態(tài);u(t+i-1)為t+i-1時(shí)刻的控制量;xo為目標(biāo)點(diǎn)狀態(tài)信息;Q為權(quán)重矩陣;R為權(quán)重參數(shù)。

⑤ 將第一個(gè)控制分量作為舵角控制指令,生成對(duì)應(yīng)的PWM信號(hào)作用于無(wú)人艇。

4 湖試實(shí)驗(yàn)

4.1 實(shí)驗(yàn)平臺(tái)設(shè)計(jì)

利用QT等軟件建立無(wú)人艇路徑跟蹤實(shí)驗(yàn)平臺(tái)。該平臺(tái)主要有5大功能區(qū)域,分別為當(dāng)前狀態(tài)信息顯示區(qū)、地圖顯示區(qū)、巡航信息顯示區(qū)、按鈕區(qū)、畫(huà)面顯示區(qū)。其中當(dāng)前狀態(tài)信息顯示區(qū)用于顯示無(wú)人艇的當(dāng)前狀態(tài),包括經(jīng)度、緯度、航向角等數(shù)據(jù);地圖顯示區(qū)用于顯示目標(biāo)點(diǎn)位置和無(wú)人艇當(dāng)前位置;巡航信息顯示區(qū)用于顯示自主巡航模式下起始點(diǎn)、目標(biāo)點(diǎn)等信息;按鈕區(qū)用于手動(dòng)模式下無(wú)人艇的運(yùn)動(dòng)控制;畫(huà)面顯示區(qū)用于實(shí)時(shí)顯示圖傳畫(huà)面。無(wú)人艇航行時(shí)將實(shí)時(shí)采集的數(shù)據(jù)信息通過(guò)數(shù)傳電臺(tái)發(fā)送給上位機(jī),上位機(jī)將采回的數(shù)據(jù)存入設(shè)計(jì)好的數(shù)據(jù)庫(kù)中,路徑跟蹤控制器根據(jù)預(yù)先設(shè)置的控制參數(shù)結(jié)合MPC控制算法得出舵角控制信號(hào)傳送給執(zhí)行結(jié)構(gòu),通過(guò)改變無(wú)人艇的航向?qū)崿F(xiàn)路徑跟蹤控制。

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

為了驗(yàn)證本文設(shè)計(jì)的無(wú)人艇路徑跟蹤系統(tǒng)在真實(shí)場(chǎng)景下的有效性和可靠性,研制了一款如圖8所示的魚(yú)雷狀小型無(wú)人艇實(shí)體樣機(jī),為了滿足系統(tǒng)需要,艏段安裝有探照燈,電子艙段安裝有GPS、慣導(dǎo)等設(shè)備,艉段安裝有舵機(jī)、推進(jìn)器等設(shè)備,垂推艙安裝了2個(gè)備用推進(jìn)器,在特殊情況下能實(shí)現(xiàn)下潛抵近監(jiān)測(cè)等功能。為了保證無(wú)人艇作業(yè)的安全性,在電子艙內(nèi)裝有電極式水浸傳感器,當(dāng)傳感器探頭接觸到水時(shí),上位機(jī)將發(fā)送暫停自主巡航任務(wù)的指令并發(fā)出警報(bào)提示返航。該無(wú)人艇具有遠(yuǎn)程控制、自主巡航控制、數(shù)據(jù)存儲(chǔ)等功能。

圖8 魚(yú)雷狀小型無(wú)人艇實(shí)體樣機(jī)

湖試實(shí)驗(yàn)開(kāi)始前,將無(wú)人艇放入水池進(jìn)行調(diào)試,驗(yàn)證所研制的無(wú)人艇能否滿足水密安全性等設(shè)計(jì)要求,測(cè)試無(wú)人艇通信是否正常,導(dǎo)航數(shù)據(jù)是否接收正常。調(diào)試結(jié)束后,在室外某水域進(jìn)行路徑跟蹤實(shí)驗(yàn),無(wú)人艇需要先將自身的位置導(dǎo)航到設(shè)定的路徑上,在上位機(jī)界面的地圖上設(shè)定目標(biāo)點(diǎn)進(jìn)行跟蹤。無(wú)人艇初始位置A的經(jīng)緯度是(120.559 783 9,31.891 803 74),起始點(diǎn)B的經(jīng)緯度是 (120.559 860 2,31.891 815 19),目標(biāo)點(diǎn)C的經(jīng)緯度是(120.560 150 1,31.892 097 47),圖9為無(wú)人艇湖試實(shí)驗(yàn)情境。

圖9 無(wú)人艇湖試實(shí)驗(yàn)情境

在執(zhí)行路徑跟蹤任務(wù)時(shí),多種傳感器互相配合進(jìn)行導(dǎo)航,實(shí)時(shí)記錄慣導(dǎo)和GPS數(shù)據(jù)。圖10為湖試實(shí)驗(yàn)結(jié)果,表1為無(wú)人艇部分實(shí)驗(yàn)數(shù)據(jù)。

表1 無(wú)人艇部分實(shí)驗(yàn)數(shù)據(jù)

圖10 湖試實(shí)驗(yàn)結(jié)果

由圖10(a)可以看出,由于無(wú)人艇存在一定的拐彎半徑且GPS有定位誤差,無(wú)人艇的跟蹤軌跡與直線路徑不完全吻合,與PID控制算法相比,本文控制方法下的軌跡曲線波動(dòng)較小。

由圖10(b)可以看出,與PID控制算法相比,本文控制方法下航向變化曲線波動(dòng)較小,雖然剛開(kāi)始航向角有較小的超調(diào),但系統(tǒng)穩(wěn)定后,航向角誤差波動(dòng)始終保持在較小范圍內(nèi)。

為了進(jìn)一步分析本文所述控制系統(tǒng)的控制性能,給出如表2所示的2種控制方法的平均跟蹤誤差和航向角絕對(duì)值的平均誤差。其中,平均跟蹤誤差的計(jì)算公式為

表2 控制性能指標(biāo)對(duì)比

(6)

式中:q(i)為第i時(shí)刻距離的偏差。

綜上可見(jiàn),與PID控制方法相比,本文算法的平均跟蹤誤差小且航向角絕對(duì)值的平均誤差也較小。因此,本文設(shè)計(jì)的路徑跟蹤控制器的路徑跟蹤精度優(yōu)于PID控制器。

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

為了提高無(wú)人艇在水域作業(yè)時(shí)的航向跟蹤精度,設(shè)計(jì)了一款魚(yú)雷狀小型無(wú)人艇路徑跟蹤控制系統(tǒng)。艇載控制系統(tǒng)以STM32F429為主控芯片,硬件上搭建串口通信等電路,采集自身狀態(tài)數(shù)據(jù),利用數(shù)傳電臺(tái)與岸基控制系統(tǒng)進(jìn)行信息交互,接收遠(yuǎn)程指令完成運(yùn)動(dòng)控制;軟件上在上位機(jī)中采用LOS制導(dǎo)算法,下位機(jī)中采用MPC控制算法設(shè)計(jì)路徑跟蹤程序;通過(guò)湖試實(shí)驗(yàn)表明系統(tǒng)整體運(yùn)行穩(wěn)定。所設(shè)計(jì)的控制系統(tǒng)既能為無(wú)人艇航行提供安全保障,也可為其他無(wú)人艇路徑跟蹤控制的應(yīng)用提供參考。