LOS引導算法的UUV航行控制

中國船舶重工集團公司第七一五研究所 陳 俊

UUV在水下航行時，需要對其深度和航向進行控制。相對于定航向控制，定深控制更為復雜，通常需要對其俯仰角進行控制以達到控制深度的目的。針對定航向控制和定深航行中俯仰角難以控制的問題，使用LOS引導算法，為俯仰角的控制提供參考角度，使用改進的PID控制算法對其航向、深度進行控制，并結合模型，在Matlab中對設計的引導算法和控制控制算法進行仿真驗真，結果表明，引入了LOS引導算法的PID控制器具有良好的控制性能。

無人水下自主航行器（UUV）是一種能夠在水下自主航行的航行體，綜合了耐高壓材料技術、高速動（靜）密封技術、自主控制技術、水下目標定位技術、聲納及圖像識別技術等技術，能夠根據搭載的任務系統的不同執行不同的任務。在軍用方面，UUV已成為反潛戰、水雷與反水雷戰、水下載荷投送、偵查與識別的重要載體，多國海軍已有成型的裝備列裝部隊。而民用方面，UUV也被應用在海洋資源勘探、水下救援、海洋石油開發、水下通信等領域，促進了海洋經濟的發展。鑒于其在軍事及民用方面廣闊的應用場景，UUV近年來一直是各國科研技術人員研究的重點。

UUV在自主航行過程中，通常需要根據任務設定保持一定的速度，在某一深度沿著某個指定方向航行，這對UUV的自主航行能力提出了很高的要求。UUV控制系統設計的主要難點在于：系統模型高度非線性、波浪和海流的隨機性、強耦合性、強時滯性、運動學和動力學參數不確定性等，因此航行控制問題較為復雜。常規的PID控制器對航向控制能起到較好的效果，但深度控制往往因為俯仰角難以有效控制而導致振蕩，產生較大的誤差，且俯仰角不加以控制容易產生安全問題，一旦發生事故，可能造成巨大的財產損失。要獲得良好的控制效果，需對UUV的俯仰角進行控制，同時結合模型特點，采用一定的控制策略和安全管理策略，提升控制器的穩定性和安全性。

1 LOS引導算法設計

對于UUV的定深控制，需要控制UUV以一定的俯仰角度向設定的深度下潛或上浮，并且根據不同的深度偏差值及時調整俯仰角度，因此俯仰角的控制對于深度控制極其重要。過小的俯仰角無法快速地達到期望深度，過大的俯仰角雖然可以快速達到期望深度，但容易產生超調和振蕩，無法滿足精確定深的要求，甚至可導致安全問題。因此在定深控制的過程中，要根據深度誤差，實時調整期望的俯仰角度，控制UUV以合適的俯仰角運動，達到更好的動態特性。

為確保UUV安全準確地實現定深控制，設計俯仰角引導算法，在UUV航行過程中，實時提供參考俯仰角，并控制深度舵輸出，使得UUV跟蹤參考俯仰角，以平滑穩定的方式達到期望深度。

LOS(Line of sight)引導算法廣泛應用于軍事領域，是導彈制導中重要的引導算法。LOS引導算法一般包括三個點，即目標點、參考點和追蹤點，LOS引導算法是非常典型的三點式引導算法。在船舶運動控制中，LOS通常應用在路徑跟蹤和軌跡跟蹤中，用以為船舶航行提供參考的艏向角。其基本思想是根據船舶當前位置與期望值的橫向誤差值，得到一前向距離，由橫向誤差和前向距離計算期望的航向角，使得船舶沿著期望航向角航行，在航行過程中，實時更新期望航向角，最終使船舶駛向期望路徑并沿期望路徑航行。根據LOS的基本思想，將該引導算法應用到UUV的航行控制中，為俯仰角提供參考值。

圖1 LOS引導算法應用在UUV航行控制中

運用LOS算法，將深度控制轉化為俯仰角控制，因此UUV的水下深度和航向控制轉化為對航向角和俯仰角的控制。

2 控制算法及策略設計

2.1 控制算法

本控制算法基于一類UUV模型，在傳統PID控制算法的基礎上進行改進。

被控UUV的推進系統為主推帶舵的形式，其深度舵和航向舵垂直相交，舵的控制形式為電磁鐵的吸合帶動舵片運動，由于電磁鐵只有吸合和斷開兩種狀態，因此航向舵只有左/中/右三種狀態，深度舵只有上/中/下三種狀態。假定深度控制和航向控制解耦互不影響，針對航向和深度控制分別設計PID控制器。

航向控制為PID控制，計算給定航向和實際航向的偏差作為控制輸入，再由PID控制器對誤差量進行比例、積分、微分計算求和得到系統的輸出。深度控制為基于LOS引導算法的雙環控制，內環控制對象為俯仰角，外環控制對象為深度，內環的俯仰角由LOS引導算法給出參考角度，通過PID控制算法控制俯仰角來達到控制深度的目的。控制器結構如圖2、圖3所示。

圖2 深度控制器

圖3 航向控制器

2.2 控制策略

考慮到UUV的模型特殊性和安全問題，在控制器設計時運用一定的控制策略，實現控制效果和安全性的提升。

但是對于UUV來說，當俯仰角和橫滾角超過一定的角度時，容易產生安全問題，因此控制器設計時引入對允許的最大俯仰角psi_max和roll_max橫滾角，當檢測到俯仰角大于psi_max時，深度舵停止輸出，當橫滾角度大于roll_max時，航向舵停止輸出，防止因俯仰角和橫滾角過大造成的UUV傾覆等嚴重問題。

3 仿真分析

為驗證所設計的LOS引導算法和PID控制算法的有效性，根據已經建立的UUV運動模型，在Matlab中進行航行控制仿真，該仿真過程為：UUV以固定的轉速按照任務設定的深度和航向航行，并在不同時刻給定不同的任務值。仿真時，以初始狀態為0時刻，0時刻時，航向角為0°，UUV所處深度為0m，T=0-300周期內，期望深度為0m，期望航向為120°；T=300-1000周期內，期望深度為20m，期望航向為0°，T=1000-3000周期內，期望深度為45m，期望航向為270°。

設計的控制算法涉及的控制參數較多，主要控制參數為：控制周期T_c=0.15s，course_output_min=1.2，psi_output_min=4，course_error_int_max=9.2，psi_error_int_max=12，roll_max=10，psi_max=30，航向PID控制器參數分別為Kpc=-0.1，Kic=-0.05，Kdc=-1，LOS引導算法參數為Delta_min=5，Delta_max=10，k=1，俯仰角控制參數分別為Kpd=-0.4，Kid=-0.015，Kdd=-3。仿真結果如下圖所示，圖5、圖6中，紅色虛線部分為任務設定值，藍色實現為UUV實際深度和航向。

圖5 UUV深度軌跡

圖6 UUV航向角

從圖4可以看出，仿真過程中，UUV三維空間運動軌跡平滑順暢，宏觀上按照任務設定的航向和深度航行，航向和深度均能較快地達到期望值，且在穩定后能夠長時間保持，沒有明顯的超調和振蕩，從俯仰角曲線可以看出，在最大俯仰角的限制下，UUV俯仰角未出現超調和振蕩，在設定深度突變時，俯仰角及時增大，快速下潛至指定深度并能及時矯正至水平狀態，在期望深度附近保持水平的姿態航行。

圖4 UUV三維空間軌跡圖及俯仰角

從圖5和圖6可以看出，得益于俯仰角的平滑控制，UUV的深度值和航向值均能跟蹤任務設定值，響應速度快，無明顯超調和振蕩。

圖7的舵角輸出曲線表明，深度舵和航向舵輸出平滑，大部分時間處在中位狀態，也未出現左右舵或上下舵頻繁交替的情況，在任務產生變化時快速響應，實現了對UUV深度和航向的有效控制，同時也有效地避免因打舵產生的機械磨損，減少能耗，保護舵的驅動機構。

圖7 舵角輸出曲線

針對UUV水下航行控制問題，分別設計了航向PID控制器和基于LOS引導算法的深度PID控制器，其中深度控制器以俯仰角內環控制，外環為深度控制。在Matlab中的仿真結果表明，所設計的控制器能夠較好地完成水下定深和定航向控制任務，具有響應速度快，超調量小，控制穩定，輸出平滑等特點。