一種非線性路徑跟蹤方法的應用

劉來華

（中國船船重工集團有限公司第七一〇研究所，湖北 宜昌 443003）

0 引言

無人駕駛載具作為一種具有自主操作能力、機動靈活、行動隱蔽、無人員傷亡等特點的智能化平臺，部署機動、使用方便，可以在危險區域或者不適宜有人操作的區域獨立自主執行任務，具有良好的費效比和廣泛的應用前景。

無人駕駛載具主要包括：地面無人智能車輛、水面無人艇、空中無人機、水下機器人。其自主航行能力作為衡量裝備智能化程度高低的核心技術之一，直接影響裝備的價值，也受到越來越廣泛的重視。

路徑跟蹤是實現無人駕駛載具自主航行的關鍵技術之一，也是無人駕駛載具必須具備的基本功能之一。但是在復雜環境下的各種擾動給無人駕駛載具的路徑跟蹤控制精度帶來了難度和挑戰，其結果直接影響系統的智能化作業效果。

目前，國內外學者都對路徑跟蹤方法進行了深入的研究，形成的文獻主要分為 4類：世界常用算法介紹[1-3]、基于數學模型的控制仿真算法研究[4-6]、半實物仿真控制算法研究[7-9]、基于實物平臺的控制算法介紹說明[10-11]。本文的算法屬于第4類。

本文介紹了一種新型的非線性路徑跟蹤算法。該算法結合傳統的PID算法，不依賴數學模型、易于實現。經過計算機控制仿真試驗和實物樣機試驗驗試，通過對比發現：相對于傳統的線性PID控制方法，本文算法在控制精度、適應環境強擾動性等方面有顯著優勢，具有一定的市場應用價值。

1 常用線性制導控制算法

在路徑跟蹤技術中一般采用2種方法：直接法和間接法。

間接法是將無人駕駛載具的制導與控制問題分開為外環的制導回路和內環的控制回路。外環制導回路通常使用基于幾何學和運動學特性的策略產生期望加速度。內環控制跟隨外環產生的加速度指令，輸出轉向角度指令。通過雙環控制實現路徑跟蹤。

直接法是使用集成策略，內環和外環被集成設計為一體。可以應用一些現代控制和設計技術，依據數學模型和算法，根據輸入的航跡偏差直接產生控制舵機角度或轉向角度的輸出指令，實現路徑跟蹤。

在實際應用中，將內環和外環分開的方法更加常用。這種方法對于無人駕駛載具的內環控制更簡單且易于建立。在算法測試時，可以將內外環分開進行獨立的調試測試，調試簡單方便。圖1為常用經典內外環分開的雙環線性路徑跟蹤算法控制原理圖。

圖1 常用路徑跟蹤控制器Fig.1 Normal path tracking controller

線性控制器廣泛應用在無人駕駛算法的外環制導。通常情況下比例微分（PD）控制器用于控制航跡誤差，在環境無擾動且當目標路徑為直線時，這種簡單的策略可以提供很好的外環控制性能，具有較好的直線路徑跟蹤能力。

無人艇在實際航行過程中，高海況擾動會實時引起航速和航向的變化。同樣，無人機在飛行過程中也會受到氣流、風向的擾動而影響航速和航向。傳統的線性 PID控制算法在外環中包含航跡偏差及航跡偏差的變化率，但未考慮擾動帶來的航速大小和方向的實時改變。所以，從理論上就決定了其在高海況下或強對流天氣下的抗擾能力不強、控制精度不高。同時，當無人駕駛載具在需要對復雜的曲線或者圓形路徑進行緊密跟蹤時，傳統PID算法中航跡誤差的線性反饋無法提供滿意的性能。

2 新型非線性路徑跟蹤算法

在無人駕駛技術的路徑跟蹤方法方面，可參考導彈制導方法。

在導彈制導領域主要有視線導航法（1ine of sight，LOS）、純跟蹤制導法（pure pursuit guidance，PP）、平行接近制導法（constant beating guidance，CB）3種方法。視距導航LOS方法應用于地對空制導，PP方法應用于空對地制導，CB應用于空對空制導。其中，LOS方法以其良好的性能及控制效果在無人駕駛載具的路徑跟蹤領域得到了廣泛的應用。

本文所述的非線性路徑跟蹤算法采用 LOS方法，在路徑跟蹤控制算法的外環采用一種非線性算法計算產生橫向加速度，以取代傳統的線性PD控制器，內環控制器與傳統方法相同。此非線性制導率稱之為L1控制算法。

視線導航的控制方法（LOS）原理：通過在目標路徑上選取合適的參考導航點，引導無人駕駛載具跟蹤選取的參考導航點，使無人駕駛載具能夠逐漸接近目標路徑并最終沿著目標路徑前行，達到路徑跟蹤的目標。

LOS制導算法獨立于無人駕駛載具的數學模型，具有參數整定容易、計算開銷較小、穩定性較高、收斂性能較好和易于工程實現等特點，因而被廣泛應用于各類無人駕駛載具的路徑跟蹤控制算法研究中。

2.1 非線性L1算法原理

在目標路徑上選擇1個參考點，在每一個時間點定義1個圓周路徑。這個圓過參考點和無人駕駛載具的當前位置，且與航行速度相切，通過參考點產生一個橫向加速度指令as。無人駕駛載具的速度為V，轉向半徑為R，參考點的位置在目標路徑上，并與無人駕駛載具距離L1，L1可由用戶設定調節。

圖2 運動幾何學原理圖Fig.2 Principle diagram of kinematic geometry

根據運動學和幾何學，可以得出：橫向加速度

速度 V可由傳感器實時測量獲得，L1由用戶預先設定，即速度V和L1均為已知量。根據上述公式，只需要得出轉向角η值就可求出外環橫向加速度值as，隨后進行內環（角速率）的閉環控制，從而最終實現直線或曲線路徑跟蹤的目標。

2.2 求解轉向角

以直線跟蹤為例，轉向角η由2部分組成，即η=η1+η2，根據幾何學有：

即：

在求出η1和η2后，即可得出轉向角η的值。

圖3 直線跟蹤幾何關系圖Fig.3 Geometrical relation diagram of line path tracking

2.3 線性化處理

假設η角很小，則η1、η2也很小，根據幾何學：

因此，L1非線性算法進行線性化處理之后等價于1個PD控制器來控制航跡誤差，與傳統的線性控制算法完全相同。

在轉向角很小的情況下，兩者的控制效果和精度完全相同，即L1控制算法完全兼容傳統線性PD控制算法。

在轉向角較大的情況下，不適宜做線性化處理，兩者的控制效果和精度不同。

2.4 穩定性分析

本文以直線路徑跟蹤情況為例進行非線性李雅普諾夫穩定性分析。

系統模型被表示為下面的運動學公式：

制導率為

為了參考點有唯一存在性，假設：

則李雅普諾夫函數為

由模型圖有：

則式（9）變為

這是一個正定函數，對式（9）進行時間求導可得出：

使用公式（6）和公式（10），有：

最后由：

因為|η|＜π/2，所以L˙是半負定的。故應用不變集理論，由L=0得到的解就只有d=0、η2=0。因此L1算法對于 d ＜L1、|η|＜π/2的情況是漸進穩定的。

2.5 L1算法工程化處理

根據穩定性分析結果：L1算法對于 d ＜L1、|η|＜π/2的情況是漸進穩定的。在實際工程應用中經常會遇到 d ＞L1、|η|＞π/2的情況，需要進行工程化處理，以確保算法的控制結果保持收斂。

在|η|＞ π /2即轉向角大于 90°度時，可在軟件設計中對η進行限幅，使其滿足|η|＜π/2，即限制每次的轉向角不超過π/2。

在 d＞L1即距離目標路徑很遠時，可以將目標路徑跟蹤問題轉變為航點跟蹤問題，即跟蹤目標路徑中的起始航點。

航點跟蹤原理：無人駕駛載具與起始航點的位置已知，可以得出其航向A，速度的方向B可以由羅經或慣導設備獲取。獲得A和B值，則可求出轉向角η的值，隨后轉變為傳統的動態航向保持問題，最終達到跟蹤目標航點的目標。

3 L1路徑跟蹤算法仿真測試

選擇小型固定翼飛行器為仿真目標對象，進行空氣動力學建模。通過控制仿真的方法來對比在不同目標路徑和風速擾動條件下非線性L1算法與線性PID算法的性能差異。

非線性L1算法同樣適用于圓形路徑跟蹤。在圓形路徑上選擇參考導航點，航跡偏差為飛行器當前位置距圓心的距離與圓形直徑值的差值，采用的LOS原理及運動幾何學與直線路徑跟蹤完全相同。

3.1 直線路徑跟蹤的仿真效果對比

設定初始條件：巡航速度為25 m/s，L1=150 m，航跡偏差d=10 m。起始時無人飛行器的航向平行于直線路徑的方向，進行兩種控制算法的仿真試驗。

仿真結果表明：在起始轉向角很小的情況下，2種算法對于跟蹤直線路徑的性能和效果幾乎完全一樣。同時，也印證了本文2.3章節中的理論分析結論。

3.2 無風條件下圓形路徑跟蹤仿真

根據數學模型進行控制仿真，結果表明：使用PD控制器的穩態誤差約為40 m。穩態誤差的出現是因為航跡誤差的位置參考命令是拋物線形式的，而目標路徑為圓形。采用PID控制器后，積分器消除了穩態誤差，但在初始階段的誤差仍然存在。L1法對曲線路徑的跟蹤效果很好，在初始階段與目標曲線的偏差不超過5 m。

3.3 有風條件下曲線跟蹤仿真

有風條件下仿真結果表明：在東向風力為5 m/s情況下，對于 PD控制器，在初始階段后，航跡誤差范圍為30～60 m；對于PID控制器，航跡誤差范圍為-20～20 m。

當飛行器在順風區飛行時處于圓形路徑外，而在逆風區時則處于圓形路徑內。因為在順風區時，飛行器相對于慣性坐標系的速度增加。如果要保持對路徑的精確跟蹤，飛行器必須產生一個大的加速度指令。而增益不變的線性控制器的固有局限使其并不能立刻糾正這個誤差，導致航跡誤差偏大。

4 飛行器樣機測試

4.1 飛行器樣機參數

飛行器選用固定翼飛機，翼展長度2.5 m，重量約10 kg。在巡航速度為25 m/s，L1=150 m的設置條件下，對圓形路徑跟蹤進行測試，圓形路徑的半徑為250 m。

4.2 飛行器路徑跟蹤效果

在飛行器沿圓形路徑飛行時，航跡和目標路徑的橫向偏差 75%的時間保持在±2 m，而 96%的時間保持在±3 m。

圖4 飛行器跟蹤圓形路徑的實測結果Fig.4 Test result of flight vehicle traces circular path

測試結果表明：L1算法動態性能好、控制精度高、抗擾能力強，跟蹤圓形路徑的效果特別好。

5 無人船測試

5.1 抗浪仿真測試

無人船模型選用小型固定雙槳的雙體船運動模型。先建立波浪干擾下的運動模型，忽略風和流的影響，主要考慮波浪作用，對模型施加各個方向的波浪，對L1算法與傳統PID算法進行仿真對比測試。

抗浪仿真結果表明：傳統PID算法在隨浪和橫浪的作用下，路徑跟蹤偏差隨著海浪的增大而加劇變大；L1算法可同時對隨浪和橫浪進行抑制，減小路徑跟蹤靜差，且隨著浪高的增大效果愈明顯，具有較好的抗浪能力。

5.2 抗風仿真測試

無人船模型選用大型雙體船運動模型。忽略波浪的影響，對模型施加各個方向的風力，對L1算法與傳統PID算法進行仿真對比測試。

抗風仿真結果表明：在順風和橫風的作用下，傳統PD算法無法準確收斂到目標路徑，進而產生跟蹤靜差，且隨著風速增加而加劇；L1算法可同時發揮對順風和橫風的抑制作用，減小路徑跟蹤靜差，提高路徑跟蹤的精度，明顯地提高了抗風干擾能力。

5.3 無人船水庫航行測試

試驗船采用無人雙體船，船長3.5 m，寬2.5 m，采用固定雙推進器模式，推進器為電力驅動方式。控制系統通過分別獨立調節左右 2個推進器的轉速實現調速和轉向的目標。

雙體船的路徑跟蹤算法采用本文的非線性控制算法，設定巡航速度為 4 m/s、目標路徑包含 2段直線和1段曲線，曲線兩端分別連接2段直線。

在水庫水面有風浪流綜合影響的天氣下，實地進行無人船路徑跟蹤試驗，實時測量航跡并與目標路徑進行對比，測試本文算法的路徑跟蹤效果及抗擾動性能。

圖5 雙體船路徑跟蹤測試Fig.5 Path tracking test of catamaran

測試結果表明：L1非線性算法能在存在一定外部擾動的情況下，準確跟蹤直線路徑和曲線路徑，具有較強的環境適應能力。

6 結束語

本文介紹了一種非線性路徑跟蹤方法，闡述了該方法的運動學原理、穩定性分析、工程應用方法、與傳統PID線性算法的關系。

通過與傳統PID/PD路徑跟蹤算法進行全面分析對比，本文算法具有動態性能好、控制精確度高、抗擾能力強的特點。

該算法能穩定跟蹤直線路徑和曲線路徑，不依賴于數學模型、易于實現。適用于中低速無人艇、無人飛行器、無人智能小車的室外路徑跟蹤應用場景，具有一定的市場應用價值。