差速轉向式雙推無人艇航向與航速聯合控制研究

袁 健

（齊魯工業大學（山東省科學院），海洋儀器儀表研究所，山東省海洋監測儀器裝備技術重點實驗室，國家海洋監測設備工程技術研究中心，山東 青島 266100）

0 引言

無人水面艇（Unmanned Surface Vessel，USV）是一種智能化、自主化的海洋無人智能裝備，在水文環境探測和監測、海洋測繪、海洋維權、敵情偵測等領域將發揮重要作用.其配備先進的供電系統、控制系統、傳感器系統、通信系統，可進行大范圍海洋環境信息采集和監測[1-2]，軍事方面可執行多種戰爭和非戰爭軍事任務，如偵察、反潛、反特種作戰以及巡邏等作戰任務，在無人作戰系統占有重要位置，因此倍受世界各國的重視[3-5].面對復雜未知的海洋環境，如何設計一種穩定可靠的無人艇自主控制系統，如何實現無人艇的自主航行控制以及自主導航策略是無人水面艇自主控制研究的關鍵技術[6-8]，也是當下海洋無人系統智能控制的一個熱點.無人艇航行時受到海浪、海風等外擾動的影響，是一個非線性、不確定性和動態不穩定性系統，因此，在這種情形下，基于無人艇動態模型建模的控制律設計方法其實際的控制效果不佳.PID控制算法具有結構簡單，魯棒性強的優點，但當無人艇動態特性或所處環境發生變化時，其控制參數需進行適應性修訂，否則將導致控制效果變差，參數的修訂受到人員經驗、外界干擾等影響，不可控且低重復性，因此為提升傳統PID控制算法性能，增強其適應性，需要對傳統PID控制算法進行優化改進.文獻[9]研究了具有不確定性擾動的水面無人艇的航跡跟蹤控制，提出一種魯棒的軌跡跟蹤控制器設計方法用于處理外擾動不確定情形下的控制設計.文獻[10]提出一種修正的模糊神經網絡方法，用于實現無人艇的航向控制.文獻[11]采用模糊控制方法設計了無人艇模糊控制規則和推理，實現了不依賴于動力學模型的模糊PID航向控制算法，并應用于無人艇航控系統試驗.文獻[12]針對無人艇航向控制中存在的模型參數和外界干擾不確定性問題，基于反步控制思想設計無人艇的航向控制律，提出一種由舵角控制輸入控制航向的自適應航向控制算法，利用李雅普諾夫函數驗證該控制算法能夠使得閉環控制系統一致有界.文獻[13]建立了無人艇操縱運動模型，設計了一種基于軌跡線性化的快慢回路時標分離的無人艇航向控制方法，并將非線性干擾觀測器設計方法與軌跡線性化控制方法結合，提出基于非線性干擾觀測器的軌跡線性化航向控制方法.文獻[14]對無人艇的航向跟蹤控制系統和外部環干擾進行了數學建模，并基于分數階微積分方法設計了一種航向控制的分數階控制器，進一步給出了一種徑向基函數神經網絡學習算法，實現了該控制器的參數整定.

本文針對一種雙電推的無人艇航向和航速聯合控制問題，研究對象為大容量鋰電池為能源的，兩個涵道式推進器推進的小型無人水面監測艇，該無人艇的推進裝置由2個對稱設置于無人艇底的，可以正反向轉動的涵道式推進器組成.雙推無人艇航向控制與舵控無人艇不同，其通過控制左右推進器的轉速差進行航行控制，轉速差大則無人艇轉向幅度大，轉速差小則無人艇轉向幅度小，很難精確確定轉速差與轉動角度之間的關系式，另一方面，無人艇的精確動力學模型建模也非常困難，因此我們采用模糊PID控制方法，模擬人工操控無人艇的操作模式，對操控規則采用模式語言進行描述和模糊化處理，提出了航向和航速聯合控制的模糊PID閉環控制方法，通過構建模糊控制規則表，并以無人艇航向的實際誤差及誤差變化率作為模糊控制的輸入，PID的參數作為模糊輸出，實現PID控制器控制參數自適應調整，增強無人艇控制的環境應變能力.進一步根據系統控制流程編寫了閉環控制模塊軟件，并設計了下位機控制器和運動控制器的控制協議.最后通過岸邊實驗獲取了該無人艇的速度控制因子，并通過仿真和岸邊實驗驗證了所提控制方法的有效性.

1 控制系統設計

該雙推無人艇的控制系統軟件包括上位機控制軟件、下位機主控制軟件、下位機運動控制軟件，控制結構如圖1所示.上位機和下位機控制軟件分別安裝在兩塊PCM9310主板上，操作系統為ubuntu16.2，控制軟件采用MOOS-Ivp框架進行開發，該軟件框架為MIT和Oxford機器人實驗室共同開發和維護的基于C++的分布式機器人開源控制軟件系統框架[15].下位機運動控制器采用STM32F103ZC控制板.上位機與下位機主控制器之間通過無線網橋進行通信連接，下位機主控制器與運動控制器之間通過RS323串口進行數據通信.本文主要研究雙推無人艇航向與航速控制方法，主要涉及下位機控制軟件和運動控制器的算法和軟件設計，上位機的主要功能是實現用戶控制命令的下發和下位機采集數據的呈現，不在本文討論范圍之內.由于采用電子調速器控制推進電機與采用伺服電機控制推進電機的控制方式不同，電子調速器可以接受1 000-2 000 us的脈寬調制（PWM）信號，通過控制單片機的PWM引腳產生1 000-2 000 us的PWM信號輸出給電子調速器，電子調速器才能響應.其中1 000-1 500 us為電調控制直流無刷電機反轉信號，1 500 us到2 000 us為電調控制電機正轉信號，中值1 500 us為電機停止信號，并且PWM值與1 500 us的差值越大，則轉速越高，所以雙推無人艇航向控制與舵控無人艇不同，其通過控制左右推進器的轉速差進行航行控制，轉速差大則無人艇轉向幅度大，轉速差小則無人艇轉向幅度小，其關系式很難精確描述，為此我們針對雙推無人艇航速和航向聯合控制的需求，研究了MOOS-Ivp架構下相應軟件模塊的功能和系統組成，并配置了保證控制系統正常運行所需的軟件模塊，提出了航向和航速聯合控制的模糊PID閉環控制方法.其采用星型組織架構，以MoosDB為軟件體系的消息發布中心，其他模塊化相對獨立，并通過消息的訂閱和發布機制進行組織的軟件架構模式.該軟件架構可以實現各個軟件模塊的靈活配置，自由增減，通過配置文件即可實現對軟件模塊的配置，非常適合于復雜環境下無人艇分布式控制軟件的研發.

圖1 無人艇控制結構

1.1 MOOS的核心模塊

MOOS-Ivp軟件模塊安裝在下位機主控制器PCM9310板上.該軟件運行環境為ubuntu16.2，通過*.moos文件進行系統的初始化參數配置，可以選擇程序需要啟動的軟件模塊，以及實現對應模塊的參數初始化配置，通過團體名實現對模塊組的區分.本項目針對無人艇控制的具體需求，采用的MOOS軟件系統可以實現分布式控制，在上位機運行一個MOOSDB模塊、一個可視化用戶界面（pMarineViewer）模塊；其中MOOSDB模塊需要給定其團體名和IP地址，下位機需要啟動的MOOS軟件模塊為MOOSDB，pHelmIvp，pLogger，pMarinePID，pNodeReporter，pShare，uMS，uSimMarine，pCompass、pAct等模塊.其中，MOOSDB為MOOS軟件架構體系的核心，是其他模塊訂閱和分發消息的中心.pHelmIvp為系統的決策輸出模塊，通過采集uSimMarine模塊的輸出和上位機用戶輸入的設定速度和航向信息，并對輸入信息進行決策規劃，輸出期望速度desired_speed和期望航向desired_heading.pLogger模塊以文件的形式實現對系統運行過程中消息的全紀錄.pMarinePID為航速和航向PID控制模塊.pNodeReporter模塊設計無人艇節點相關信息的記錄.pShare通過UDP的方式實現兩個團體之間的數據通信，需要設置其端口號，如input為9 200，output為9 201.uMS模塊可以對各個模塊發布的信息進行統一的界面化現實.uSimMarine為系統仿真模塊，通過系統仿真模塊模擬真實無人艇的導航速度、航向的輸出.pCompass主要實現組合導航模塊的航向和航速信息獲取，模塊通過USB轉232串口模塊采集電子羅盤的航向角和GPS輸出的航速值，將航向角標準化到0-360°范圍，航速單位轉換到m/s.并將采集的這些信息通過Notify（參數1，參數2）函數發布到MOOSDB模塊.pAct模塊一方面實現對運動控制器上傳的電壓、電流數據幀進行分析處理和發布，一方面將pMarinePID模塊的輸出的期望PWM值（Desired_thrust）和期望轉動角度（Desired_rudder）進行打包裝幀處理，并通過串口將成幀數據發送到運動控制器.

1.2 下位機運動控制器

下位機運動控制器通過485接口實現對溫濕度、電壓電流模塊信息的采集，同時解析下位機主控器PCM9310下發的控制幀，解析出對應字段信息繼而驅動對應的IO口和PWM信號輸出，將左右PWM信號輸出給對應推進器的電調進行轉速控制.下位機運動控制器采用STM32F103ZC微處理器模塊，晶振：72 MHz，具有16路PWM輸出功能.本項目采用TIM3的2路PWM輸出端口對應左右推進器轉速的脈寬調制.由于所采用電調的工作頻率為50 Hz，所以在初始化程序中需要設置PWM通道的輸出頻率：PWM頻率為50 Hz，初始化PWM頻率的函數參數為（72 000 000，71，19 999），計算過程為：72 000 000/20 000/72＝50 Hz.

1.3 下位機主控制器與運動控制器的通信協議

該通信協議為下位機主控制器發送到運動控制器的協議，主要功能為實現對下位機主控制器pAct軟件模塊所封裝的控制信息下發給下位機運動控制器PCM9310板，如圖2所示.ASCII編碼方式，采用逗號分隔，其協議格式為：

圖2 下位主控制器與推進器控制器之間的協議格式

其中，$USV—表示數據幀開始；

手動自動—取值為A和M，A表示自動控制信號，M表示遙控器控制；

運行指令—取值為0，1，2，3，4.0表示前進，1表示停止，2表示后退，3表示左轉，4表示右轉；

左調速PWM—取值在1 000 us和2 000 us之間，其中1 000-1 500 us表示推進器反轉，1 500-2 000 us表示推進器正轉；

右調速PWM—取值在1 000 us和2 000 us之間，其中1 000-1 500 us表示推進器反轉，1 500-2 000 us表示推進器正轉；

CRC—CRC-16，UTF-8格式編碼；

這里給出一個示例，如：$USV，A，3，1 600，1 800，XX表示下位機主控制器發送的指令為的自動控制指令，左轉，左推進器PWM值為1 600 us，左推進器PWM值為1 800 us，XX表示兩個字節的CRC.

2 控制原理

2.1 雙推無人艇航向模糊控制原理

雙推無人艇航向控制與舵控無人艇不同，其通過控制左右推進器的轉速差進行航行控制，轉速差大則無人艇轉向幅度大，轉速差小則無人艇轉向幅度小.對于有舵的無人艇而言，當舵角為負值時，表示舵順指針轉動，此時無人艇逆時針轉角.同理，當舵角為正值時，表示舵逆指針轉動，此時無人艇順時針轉角.Moos-ivp自帶MarinePID模塊，可以實現基本的PID控制，其根據模塊訂閱的航向角（NAV-heading）、航速（NAV-speed）、期望速度（Desired_speed）、期望航向（Desired_heading）等信息，產生期望PWM值（Desired_thrust）、期望轉動角度（Desired_rudder）.傳統PID航向控制算法其控制原理為

其通過調節KP、KI、KD實現航向自動控制，其中KP影響系統響應速度和控制精度，過大易引起震蕩，過小則降低系統響應速度；KI起到消除系統穩態誤差的作用，過大會引起超調，過小則降低控制精度；KD用于預測偏差變化，適當的值可有效提升系統動態特性.但當無人艇動態特性或所處環境發生變化時，其控制參數需進行適應性修訂，否則將導致航向控制效果變差，然而參數的整定受到人員經驗、外界干擾等影響，不可控且低重復性，因此為提升傳統PID航向控制算法性能，增強其適應性，需要對傳統PID航向控制算法進行優化改進.所以由于海流、海浪和風的影響導致系統的PID控制參數需要進行適應性調節，利用模糊控制方法根據航向角度誤差和角速度誤差實時調整PID三個參數.基本的PID航向控制其適用于舵控的無人航行器，對于雙推差分調速的無人艇，由于沒有舵角控制信息，所以需要將產生的舵角控制信號區間映射到左右推進器所需的PWM控制信號的區間上.但這一映射關系由于轉向角度和推進器推力差之間以及航速之間的關系并不明確.因此需要實驗研究這一映射關系.在同一個轉速推力差的前提下，設置不同的航速，進行岸邊實驗，得到不同航速下兩個推進器轉速差值與轉向角度的關系曲線.為了使無人艇控制程序能適應不同的海流、風等擾動的影響，適應性調節PID算法的控制參數，采用模糊控制方法，以航行偏差和偏差率作為模糊控制器的輸入，PID的比例、積分和微分系數作為輸出，根據不同的航向偏差和偏差率適應性地輸出解模糊后的控制參數，模糊PID控制器結構如圖3所示.

圖3 模糊PID控制器結構

式中，c決定了隸屬度函數的中心點，σ決定了函數曲線的寬度.e（t）、（t）的隸屬度函數如圖4所示.

圖4 誤差和誤差導數的隸屬度函數

無人艇航向航速聯合控制方法控制無人艇轉向時，由于初始航向偏差較大，先給一個較大的差速轉角，讓轉向運動快速啟動、加速，待回轉角速度達到穩態后，產生一個適當的轉角維持等角速度回轉；當實際航向接近期望航向時，產生一個較小的差速轉角，直至達到期望航向和航速.接著進行模糊推理，并根據模糊規則，將其規則轉換為可以程序實現的模糊語句：If（e（t）isX）and（（t）is Y）then（KPis Z），設置模糊推理器，對KP、KI、KD分別進行解模糊處理，其中的解模糊后輸出結果如圖5所示.

2.2 控制方法

在所涉及的MOOS下位機軟件模塊中，我們設計了三個模塊參與了無人艇航向和航速控制，分別是：pHelmIvp、pAct、PIDmarine模塊.其中pHelmIvp模塊根據訂閱的無人艇的即時航向NAV-heading、航速NAV-speed以及設定的航速Desired_speed-debug和設定航向Desired_heading-debug信息，產生Desired_speed、Desired_heading；PIDmarine模塊訂閱NAV-heading、NAV-speed、Desired_speed、Desired_heading，經過PID運算產生Desired_thrust、Desired_rudder，進一步將Desired_rudder轉化為無人艇左右推進器的PWM值：thrustleft、thrustright，通過Notify（）函數進行發布.pAct模塊訂閱PIDmarine模塊發布的thrustleft、thrustright信息，并根據下位主控制與運動控制器之間的通信協議生成控制幀，裝幀完成后通過串口發給下位機運動控制器PCM9310板.pAct模塊還需完成航向信息（NAV-heading）、航速信息（NAV-speed）的數據采集，NAV-heading信息是通過串口采集的北微電子羅盤輸出的航向角信息，NAV-speed信息是通過串口采集的GPS模塊的航速經過轉換而實現的，即時二者信息進行發布，滿足PID控制模塊閉環控制的需求.并且pAct模塊通過串口采集運動控制器上傳的電壓、電流數據幀，并按照設計的協議進行數據字段的解析，解析出來的左右推進器的電壓、電流、主控制器的電壓和電流，以及控制艙的溫濕度信息進行消息的發布，其他軟件模塊均可對該信息進行訂閱和顯示.

2.2.1 航向控制

航向控制需要實現的功能是通過上位機給定一個航向角Desired_heading并發布，下位機PID模塊訂閱該期望航向角信息，以及pAct發布的NAV-heading信息，通過PID控制算法實現航向的保持，具體控制流程如圖5所示.我們采用電子羅盤反饋航向角信息（NAV-heading）.當PID模塊產生的無人艇的Desired_rudder為負值，其代表無人艇需要控制順時針轉動，此時要求無人艇右邊推進器的推力大于左邊推進器的推力，即右邊的PWM值大于左邊的PWM值.類推，當PID模塊產生的無人艇的Desired_rudder為正值，代表無人艇需要控制其逆時針轉動，此時要求左邊推進器的推力大于右邊推進器的推力，即左邊的PWM值大于右邊的PWM值.具體的左右推進器的PWM值產生方法為：在當前推進器PWM值Desired_thrust的基礎上，左邊推進器的PWM值=左邊推進器的當前PWM值＋rudder*500/30；右邊推進器的PWM值=右邊推進器的當前PWM值-rudder*500/30，其中rudder為經過PID產生的轉向角值（Desired_rudder）；500對應著推進器PWM差值為500 us且作用時間為1 s，此時無人艇最大轉向角度約為30°，左右推進器的PWM值的差值為500 us，并且經過實驗得知rudder角與PWM差值之間存在線性關系.由于PWM存在飽和的情況，我們設定經過計算后的左右推進器的PWM值（thrustleft、thrustright），若其值大于2 000 us，則其值取為2 000 us，若其值小于1 000 us，則其值取為1 000 us.

圖5 航向反饋的閉環航向控制方法

2.2.2 航速控制

航速控制需要實現的功能是通過上位機給定一個航速Desired_speed并發布的情形下，下位機PID模塊訂閱該期望航速信息，還需訂閱pAct發布的NAV-heading信息，通過PID控制算法實現無人艇航速的保持，具體控制流程如圖6所示.我們采用GPS反饋無人艇航速信息.通過設置一個速度因子變量Speed_factor，參數配置其是否為零決定了程序是采用常規控制還是模糊PID控制，當Speed_factor直接給定非零時，通過計算以下公式計算推進器的PWM值：推進器的PWM值＝Speed_factor*Desired_speed＋1 500，其中Desired_speed為通過上位機給定的期望速度，通過該公式可以計算給定的推進器的PWM值，計算完成后將Desired_thrust進行發布，從而實現直接驅動推進器產生給定的轉速，控制流程如圖7所示.當Speed_factor參數配置為直接給定零時，程序采用PID航速控制，通過pAct模塊采集GPS的速度反饋值，與上位機給定的速度值相減，通過PID計算delta_thrust，通過 Desired_thrust＝Desired_thrust＋delta_thrust計算 Desired_thrust.其中Speed_factor變量的給定，是通過下位機MarinePID模塊初始化參數里的Speed_factor參數進行初始化的，MarinePID模塊里的函數vclip（dval，0，Speed_factor）取出該值并賦值給m_Speed_factor，然后通過setspeedfactor（）方法實現對MarinePID模塊中該Speed_factor參數的設置.

圖6 Speed_factor=0時速度反饋的航速模糊PID控制方法

圖7 Speed_factor!=0時帶速度因子的常規PID航速控制方法

3 實驗與分析

岸邊實驗2：在給定的無人艇航速下和左右推進器PWM差值下，無人艇會做一個圓周運動，但是由于短時間內不容易測量轉向角，我們通過觀測無人艇圓周運動單位時間內的轉角變化，也即角速度是否恒定來決定是否啟用無人艇角速度測量.無人艇進行圓周運動時，初始運動狀態不穩定，待無人艇運動穩定后，程序測量兩次相同航向角時，無人艇所轉動的角度和所用的時間t，根據公式：角速度ω＝弧度變化量/所用時間，計算角速度ω＝2/一圈的時間，得到角速度ω.我們記錄了PWM中值固定情形下，不同轉速差值下的角速度值，如表2所示.通過實驗發現，當左推進器PWM＝1 350 us，右推進器PWM＝1 850 us時，無人艇有最大角速度為0.52 rad/s.

為了驗證提出航速和航向控制方法的有效性，一方面通過MOOS系統的模擬功能實現對控制方法有效性的驗證，在模擬實驗驗證通過的基礎上，進行了岸邊控制功能的實驗驗證，并通過岸邊實驗獲取了無人艇的轉角速度信息，進一步優化了無人艇的控制算法相關參數.

岸邊實驗1：該實驗是為了實現航速與PWM值關系曲線和Speed_factor參數的獲取，如圖8所示.在上位機設置PWM值為1 550 us到2 000 us的如表1所示的數值，pAct將自動獲取Desired_thrust并將兩個推進器的PWM設置為表1中的PWM值，通過GPS模塊測得無人艇航速如表1所示.根據速度與PWM值增量的線性關系，可以獲取無人艇系統的速度因子Speed_factor為25.Desired_thrust與推進器PWM值之間的轉換關系為：推進器PWM值＝k*Desired_thrust＋1 500，這里k＝5為轉化系數.將Speed_factor值寫入到下位機MOOS系統的MarinePID模塊的初始化參數中，或者將MOOS初始化文件的Speed_factor修改為25，為下一步模擬仿真做好準備.

表1 測得的PWM值與推進器轉速、航速的關系（1節海流速度）

表2 Delta_PWM＝100，150，200，250，300，400，500us時的角速度值

圖8 無人艇岸邊試驗

模擬仿真1：采用常規PID的航速控制和航向的閉環控制方式.在MOOS初始化配置中，將Speed_factor參數設置為125，通過計算Desired_thrust＝speed_factor*Desired_speed，計算期望的推進器PWM值，并寫入到數據幀的對應字段中.進一步運行系統相應模塊和MarineViewer界面，進行八邊形路徑點跟蹤，仿真結果如圖9所示.可以看到無人艇系統可以實現對八邊形航路點的跟蹤，圖9中最下面為無人艇的期望航向角以及實際的航向角的實時曲線.

圖9 無人艇的航路點跟蹤控制結果、期望航向角以及實際的航向角

模擬仿真2：采用模糊PID的航速控制和航向的閉環控制方式.在MOOS初始化配置中，將Speed_factor參數設置為0，通過計算Desired_thrust＝Desired_thrust＋delta_thrust，計算期望的推進器PWM值，并寫入到數據幀的對應字段中.進一步運行系統相應模塊和MarineViewer界面，進行八邊形路徑點的跟蹤，仿真結果如圖10所示.可以看到采用模糊PID的航速控制和航向的閉環控制模式，無人艇系統可以實現對八邊形航路點的跟蹤.當期望速度為3.5 m/s時，對比常規PID速度控制與模糊PID速度控制的控制精度，常規速度控制方法的速度誤差為1 m/s以內.當PID算法的比例系數KP＝0.5，KD＝2.0，KI＝0時，與常規PID速度控制方法相比，模糊PID速度跟蹤控制方法對期望速度的跟蹤精度要高一個數量級，速度誤差控制在0.1 m/s以內.

圖10 無人艇的航路點跟蹤控制結果、期望航向角以及實際的航向角

4 結論

從無人水面監測艇自主控制的工程設計需求出發，針對無人監測艇的自主控制系統的結構組成、各功能模塊的技術要求、下位機主控制模塊和運動控制模塊的設計及其通信控制協議進行了設計，基于MOOS-Ivp軟件平臺編寫了航向和速度聯合控制的分布式控制軟件模塊，對比了采用常規PID控制和模糊控制方法的無人艇航向航速控制效果，并在岸邊試驗站對該試驗艇的運動控制系統進行了實驗驗證，證明了基于模糊控制方法的無人艇自主控制方法的有效性.