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

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

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

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

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

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

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

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

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

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

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

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

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

圖4 通信電路

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

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

圖5 路徑跟蹤算法流程圖

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

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

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

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

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

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

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

① 計算期望航向,公式為

(1)

式中:(xlos,ylos)為LOS點位置;(x,y)為無人艇當(dāng)前位置。

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

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

(2)

式中:R為接納圓半徑,一般取3～5倍無人艇的長度。

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

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

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

圖7 MPC控制過程

具體算法步驟如下。

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

(3)

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

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

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

umin

(4)

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

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

(5)

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

⑤ 將第一個控制分量作為舵角控制指令,生成對應(yīng)的PWM信號作用于無人艇。

4 湖試實驗

4.1 實驗平臺設(shè)計

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

4.2 路徑跟蹤實驗

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

圖8 魚雷狀小型無人艇實體樣機(jī)

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

圖9 無人艇湖試實驗情境

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

表1 無人艇部分實驗數(shù)據(jù)

圖10 湖試實驗結(jié)果

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

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

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

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

(6)

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

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

5 結(jié)束語

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