基于模擬退火優化的直接航跡控制算法設計*

夏云鵬 楊田田

（1.中船重工第七一三研究所 鄭州 450000）（2.武漢理工大學 武漢 430070）

1 前言

無人船［1］（Unmanned Surface Vessel，USV），是一種無人操作的水面船。它具有機動性強、速度快、自主化、無人化等特點，其主要功能是代替人執行一些特殊的、對人有危險的任務。

精準的航跡控制是無人船執行各項任務的基礎保障，也是無人船運動控制領域的研究熱點。無人船在實際航行環境中會受到外部干擾的影響，導致產生模型不確定性的問題。

無人船航跡控制算法主要分為兩類，一類是直接控制算法［2～3］，控制精度高，控制算法直接作用于航跡，算法復雜度高；一類是間接控制算法［4～6］，其設計思路是將航跡控制問題轉化為航向控制問題，主要實現方式是將其分解為雙閉環控制系統，內環由傳統的航向控制算法構成，外環則使用航向解算算法，控制精度相對較低，但優點是可直接使用已有的自動舵技術，更有利于工程應用［7］。

針對無人船航跡控制中容易出現的非線性、時滯性問題，本文采用非線性模型預測控制（NMPC）算法對水面無人船進行直接航跡控制，并采用模擬退火算法對預測控制算法進行最優控制序列求解，進一步提高航跡控制的準確性和實時性。模型預測控制采用反饋校正，可應對多種不確定性問題（包括系統干擾、模型失配等），并通過滾動優化策略在線求解局部最優化問題，極大地減少了在線優化計算量，提高算法的響應速度。

2 基于模擬退火優化NMPC算法的直接航跡控制

2.1 控制器設計

針對無人船航跡控制中容易出現的非線性、時滯性問題，本節采用非線性模型預測控制（NMPC）算法對水面無人船進行直接航跡控制，并采用模擬退火算法對預測控制算法進行最優控制序列求解，進一步提高航跡控制的準確性和實時性。非線性模型預測控制器總體流程如圖1。

圖1 非線性模型預測控制器總體流程

基于NMPC的航跡控制器根據預設航跡信息以及無人船的初始狀態信息，直接控制無人船的航跡。在基于NMPC的水面無人船航跡控制系統中，航跡控制器的輸出量為無人船的舵角控制量以及航速控制量，收到無人船的狀態量反饋信號（包括無人船的位置坐標信息，以及航向角信息），對航跡控制器內部預測輸出值進行自我校正，并采用模擬退火算法進行局部尋優獲得下一時刻的無人船舵角控制量以及航速控制量信息［8］。NMPC航跡控制器主要包括無人船預測模型、目標函數以及約束信息三個方面。下面將從這三個方面來介紹控制器的設計內容。

基于非線性模型預測控制的無人船航跡控制系統的控制流程如圖2所示。首先對預設航跡信息進行離散化處理，獲得離散化后的第一個目標航跡點信息，基于NMPC的航跡控制器根據此目標航跡點信息進行動作，計算無人船舵角控制量以及所需航行速度，進行航跡跟蹤，獲取離散化后的航跡信息上的下一個航跡點，重復以上動作，直到經過離散化后的航跡信息上的最后一個點。

2.2 無人船預測模型

一般來說，無人船的運動情況需要使用六個自由度來描述，包括橫搖、縱搖、艏搖、橫蕩、縱蕩、垂蕩。為描述無人船在六自由度中的運動情況，需建立如圖3所示的無人船坐標系，包括固定坐標系o-x1y1z1（簡稱“定系”）和運動坐標系G-xyz（簡稱“動系”）。“定系”是指固定于地球上的慣性坐標系。“動系”是指跟隨船體運動的附體坐標系，原點設在無人船的船體重心G處，Gx為無人船水平面方向，Gy為無人船橫剖面方向，Gz為無人船的縱中剖面方向。正方向按右手系的規定。

圖2 直接航跡控制算法流程圖

圖3 無人船坐標系

表1 無人船運動參數

無人船在附體坐標系G-xyz和慣性坐標系o-x1y1z1中涉及到的運動參數如表1所示。其中X，Y，Z為無人船所受外力在附體坐標系G-xyz上的投影；K，M，N為無人船力矩在附體坐標系G-xyz上的投影；u，v，w為無人船的速度在附體坐標系G-xyz上的投影；p，q，r為無人船的角速度在附體坐標系G-xyz上的投影；(x，y，z) 為無人船重心在慣性坐標系o-x1y1z1的空間坐標；φ，θ，ψ為無人船在慣性坐標系o-x1y1z1中的三個歐拉角，φ稱為橫傾角、θ稱為縱傾角、ψ稱為方位角。

船舶在海洋上的航行情況既可以采用“動系”中的速度向量u，v，w和角速度向量p，q，r來表示，也可以使用定系中位置向量的導數，，及歐拉角姿態向量的導數來表示。研究者可根據自身課題研究需要確定表示方式。另外，船舶中的“動系”和“定系”之間還存在如下變換關系［5］。

其中，變換矩陣TV為

無人船的直接航跡控制器設計，重點關注無人船在水平面的運動，暫且忽略橫傾、縱傾和升沉對無人船運動的影響。因此將無人船的六自由度模型的式（1）和式（2）進行化簡，得到無人船的三自由度模型為

其中，為無人船位置信息的橫坐標的導數，為無人船位置信息的縱坐標的導數，為無人船艏向角的導數，ψ為無人船的艏向角，u為無人船速度的縱向分量，v為無人船速度的橫向分量，r為無人船的角速度。

水面無人船航行過程中，當滿足u?0且v≈0時，可忽略橫漂角對無人船運動的影響，此時方程可進一步簡化為

又已知無人船角速度r和舵角δ之間的關系可用式（5）表示。式（5）中，K表示無人船的旋回性指數，T表示無人船的追隨性指數。K、T的值可由無人船的Z形操縱性試驗獲得，具體計算方法見文獻［9］。

將式（5）帶入式（4）中，并以為采樣時間Ts進行離散化處理，可得差分方程為

在水面無人船的航跡控制中，若令狀態量的矩陣形式為χ=[xyψ]T、控制量的矩陣形式為u=[uδ]T，根據式（6）表示的無人船輸入控制量與輸出狀態量之間的關系，可以得出無人船的非線性離散模型的一般表示為

2.3 目標函數

基于非線性模型預測控制的航跡控制器中，目標函數的選取要能夠充分反映無人船航跡跟蹤參考航跡的程度。本文參考文獻［10］中闡述的軟約束方法，結合無人船航跡控制的預測模型，設計目標函數如下：

其中，Hp是MPC航跡控制器的預測時域，Hc是MPC航跡控制器的控制時域，ρ為權重系數，ε為松弛因子。

該目標函數既考慮了無人船對預設航跡的跟蹤能力，也考慮了控制量增量的約束，有效地避免了控制量的突變。另外，在目標函數中加入了松弛因子，可以防止出現沒有可行解的情況。

2.4 約束條件

根據無人船本身的控制特點，在輸出控制量的過程中要考慮舵角約束以及航速約束。本文中使用的無人船模型舵角以及舵角控制量增量約束為

本文中使用的無人船航速以及航速增量約束為

綜上所述，無人船非線性航跡控制問題可以用以下有限時域內的最優化問題來表示：

2.5 算法流程與步驟

優化算法的實時性以及準確性直接影響所設計的航跡控制器的控制效果，由于非線性目標函數較難求得解析解，此處針對式（11）所示的局部最優化問題，采用模擬退火算法進行最優控制序列求解，具體求解步驟如下：

1）初始化參數：初始溫度T0，降溫速率q，結束溫度Tend，對應于每個溫度T的迭代次數L，以當前時刻的控制量矩陣作為初始可行解矩陣u0；

2）對當前溫度T，求解局部最優解，即重復步驟3）至6）；

3）對當前可行解矩陣u0在控制量約束范圍內隨機產生一個新解u1；

4）對應于式（11）所示的目標函數J，求解其對應于可行解矩陣u0和u1的增量dJ=dJ(u1)-dJ(u0)；

5）根據判斷條件確定當前局部最優解，即若dJ＜0，則認為新解u1為最優解，用u1替代u0；否則，計算對于新解的接受概率exp(-df T)，當滿足接受概率exp(-df T)＞rand（rand為（0，1）區間上的隨機數）時，也認為新解u1為最優解，用u1替代u0；否則，仍認為u0為最優解。

設置終止條件，使得溫度T衰減至結束溫度Tend時，輸出當前的最優解u0，不滿足終止條件時，對當前溫度T按降溫速率進行衰減運算后，重復執行步驟2）。

3 仿真試驗

3.1 試驗船概況

本文中的實驗船的基本參數如表2所示。

表2 無人船主要參數

在式（6）中無人船的旋回性指數K和無人船的追隨性指數T可以通過對實驗船進行10°10°的Z形操縱性試驗來獲得。在平靜的湖面進行了實驗船的Z形（10°10°）試驗，獲得的實驗船艏向角和舵角關系變化曲線如圖3所示，根據文獻［11］中提到的計算方法，對無人船試驗數據進行擬合，求得實驗船的K、T參數的值為K=0.49，T=1.94。

3.2 仿真驗證

針對表2提出的試驗船參數，對設計的基于模擬退火算法改進的NMPC的航跡控制器進行航跡控制效果仿真。各項參數設置為

1）無人船初始狀態：位置坐標(0 ，-22)，航向角ψ=0°，航速v=0m/s。

2）預設曲線航跡信息：原點為(0 ，0)、半徑為20m的圓弧軌跡。期望跟蹤速度vd=3m/s。

3）設置采樣時間為50ms。

圖4 Z形操縱試驗結果

仿真試驗分別在無干擾和有干擾情況下進行仿真驗證。對于有干擾情況，外部干擾信號設置為無人船航速的25%，干擾信號對無人船的航跡控制系統產生的影響可用式（12）表示：

其中，Ts為采樣時間。此處取Ts=1。

無干擾條件下，對于預設圓弧形航跡，航跡跟蹤仿真實驗獲得的結果如圖5至圖7所示。

圖5 航跡跟蹤效果

圖6 狀態量誤差變化曲線

圖7 控制量變化曲線

由航跡跟蹤效果圖和狀態量誤差變化曲線可以看出，對于設定的圓形航跡，基于NMPC的直接航跡控制器可以根據當前時刻無人船的狀態信息，調整無人船的舵角控制量和航速控制量，快速地跟蹤到設定航跡，且最終的跟蹤誤差為零。由控制量變化曲線可以看出，在無人船由接收到設定圓形航跡信息時刻的位置進行航跡跟蹤時，舵角控制量以及航速控制量會立刻做出響應，直到無人船跟蹤到預設航跡上，航速穩定在期望航速上，舵角控制量以及航速控制量保持不變，實現預設圓形航跡的精確跟蹤控制。

對所設計的航跡控制器添加干擾信號，參考式（4）～（5）所示，設置對應于無人船實時航速的干擾信號，獲得的航跡控制曲線以及過程中的干擾信號曲線如圖8～圖11所示。

圖8 曲線航跡跟蹤效果（有干擾）

在有干擾的情況下，由航跡跟蹤效果圖可以看出，在無人船接收到預設航跡信息初期，航跡跟蹤效果較差，但是基于NMPC先預測后控制的思想，一段時間后基于NMPC-SA的航跡控制器對干擾信號進行了預測補償，控制效果慢慢變好，到后期基本上能通過舵角補償量實現對預設圓形航跡的精確跟蹤控制。

圖9 狀態量誤差變化曲線（有干擾）

圖10 控制量變化曲線（有干擾）

圖11 擾動信號曲線圖

4 結語

針對傳統的粒子群算法容易陷入局部最優解的問題，利用模擬退火算法的突跳特性提出了一種改進算法，通過加入多目標優化，提升了算法的實用性。仿真結果顯示，改進算法可以優化無人船自主航行的路徑平滑度，提高路徑的安全性，滿足無人船全局路徑規劃的要求。針對無人船航跡控制中容易出現的非線性、時滯性問題，提出了基于非線性模型預測控制（NMPC）算法對水面無人船進行直接航跡控制，并采用模擬退火算法對預測控制算法進行最優控制序列求解，提升了算法的實用性。仿真結果顯示，改進算法有效地提高了航跡控制的準確性和實時性。