一種非線性路徑跟蹤方法的應(yīng)用

劉來華

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

0 引言

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

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

路徑跟蹤是實現(xiàn)無人駕駛載具自主航行的關(guān)鍵技術(shù)之一，也是無人駕駛載具必須具備的基本功能之一。但是在復(fù)雜環(huán)境下的各種擾動給無人駕駛載具的路徑跟蹤控制精度帶來了難度和挑戰(zhàn)，其結(jié)果直接影響系統(tǒng)的智能化作業(yè)效果。

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

本文介紹了一種新型的非線性路徑跟蹤算法。該算法結(jié)合傳統(tǒng)的PID算法，不依賴數(shù)學(xué)模型、易于實現(xiàn)。經(jīng)過計算機控制仿真試驗和實物樣機試驗驗試，通過對比發(fā)現(xiàn)：相對于傳統(tǒng)的線性PID控制方法，本文算法在控制精度、適應(yīng)環(huán)境強擾動性等方面有顯著優(yōu)勢，具有一定的市場應(yīng)用價值。

1 常用線性制導(dǎo)控制算法

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

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

直接法是使用集成策略，內(nèi)環(huán)和外環(huán)被集成設(shè)計為一體。可以應(yīng)用一些現(xiàn)代控制和設(shè)計技術(shù)，依據(jù)數(shù)學(xué)模型和算法，根據(jù)輸入的航跡偏差直接產(chǎn)生控制舵機角度或轉(zhuǎn)向角度的輸出指令，實現(xiàn)路徑跟蹤。

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

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

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

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

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

在無人駕駛技術(shù)的路徑跟蹤方法方面，可參考導(dǎo)彈制導(dǎo)方法。

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

本文所述的非線性路徑跟蹤算法采用 LOS方法，在路徑跟蹤控制算法的外環(huán)采用一種非線性算法計算產(chǎn)生橫向加速度，以取代傳統(tǒng)的線性PD控制器，內(nèi)環(huán)控制器與傳統(tǒng)方法相同。此非線性制導(dǎo)率稱之為L1控制算法。

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

LOS制導(dǎo)算法獨立于無人駕駛載具的數(shù)學(xué)模型，具有參數(shù)整定容易、計算開銷較小、穩(wěn)定性較高、收斂性能較好和易于工程實現(xiàn)等特點，因而被廣泛應(yīng)用于各類無人駕駛載具的路徑跟蹤控制算法研究中。

2.1 非線性L1算法原理

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

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

根據(jù)運動學(xué)和幾何學(xué)，可以得出：橫向加速度

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

2.2 求解轉(zhuǎn)向角

以直線跟蹤為例，轉(zhuǎn)向角η由2部分組成，即η=η1+η2，根據(jù)幾何學(xué)有：

即：

在求出η1和η2后，即可得出轉(zhuǎn)向角η的值。

圖3 直線跟蹤幾何關(guān)系圖Fig.3 Geometrical relation diagram of line path tracking

2.3 線性化處理

假設(shè)η角很小，則η1、η2也很小，根據(jù)幾何學(xué)：

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

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

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

2.4 穩(wěn)定性分析

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

系統(tǒng)模型被表示為下面的運動學(xué)公式：

制導(dǎo)率為

為了參考點有唯一存在性，假設(shè)：

則李雅普諾夫函數(shù)為

由模型圖有：

則式（9）變?yōu)?/p>

這是一個正定函數(shù)，對式（9）進行時間求導(dǎo)可得出：

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

最后由：

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

2.5 L1算法工程化處理

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

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

在 d＞L1即距離目標(biāo)路徑很遠時，可以將目標(biāo)路徑跟蹤問題轉(zhuǎn)變?yōu)楹近c跟蹤問題，即跟蹤目標(biāo)路徑中的起始航點。

航點跟蹤原理：無人駕駛載具與起始航點的位置已知，可以得出其航向A，速度的方向B可以由羅經(jīng)或慣導(dǎo)設(shè)備獲取。獲得A和B值，則可求出轉(zhuǎn)向角η的值，隨后轉(zhuǎn)變?yōu)閭鹘y(tǒng)的動態(tài)航向保持問題，最終達到跟蹤目標(biāo)航點的目標(biāo)。

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

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

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

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

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

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

3.2 無風(fēng)條件下圓形路徑跟蹤仿真

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

3.3 有風(fēng)條件下曲線跟蹤仿真

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

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

4 飛行器樣機測試

4.1 飛行器樣機參數(shù)

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

4.2 飛行器路徑跟蹤效果

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

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

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

5 無人船測試

5.1 抗浪仿真測試

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

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

5.2 抗風(fēng)仿真測試

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

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

5.3 無人船水庫航行測試

試驗船采用無人雙體船，船長3.5 m，寬2.5 m，采用固定雙推進器模式，推進器為電力驅(qū)動方式。控制系統(tǒng)通過分別獨立調(diào)節(jié)左右 2個推進器的轉(zhuǎn)速實現(xiàn)調(diào)速和轉(zhuǎn)向的目標(biāo)。

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

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

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

測試結(jié)果表明：L1非線性算法能在存在一定外部擾動的情況下，準(zhǔn)確跟蹤直線路徑和曲線路徑，具有較強的環(huán)境適應(yīng)能力。

6 結(jié)束語

本文介紹了一種非線性路徑跟蹤方法，闡述了該方法的運動學(xué)原理、穩(wěn)定性分析、工程應(yīng)用方法、與傳統(tǒng)PID線性算法的關(guān)系。

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

該算法能穩(wěn)定跟蹤直線路徑和曲線路徑，不依賴于數(shù)學(xué)模型、易于實現(xiàn)。適用于中低速無人艇、無人飛行器、無人智能小車的室外路徑跟蹤應(yīng)用場景，具有一定的市場應(yīng)用價值。