移動機器人路徑跟蹤的方法及研究進展

梁婕

廣東技術師范大學 自動化學院，廣東廣州 510665

0 引言

在過去的幾十年里，機器人技術已經成為工程研究中最重要的領域之一，機器人的使用也越來越廣泛。此外，隨著當前科學技術的發展進步，機器人不僅變得更便宜，而且更有效、更快、更靈活、更智能，也極大地方便了人們的日常生活。隨著移動機器人的廣泛應用，對于機器人的研究也進一步深入。機器人在傳感器的作用下自動行駛，基本的操作過程主要是根據傳感器的信息檢測環境進行自身定位，建圖、路徑規劃、路徑跟蹤及導航，其中路徑跟蹤是非常重要的一個方面[1]。

路徑跟蹤是移動機器人[2]的一項基本任務，近年來受到了廣泛關注。路徑跟蹤的控制問題是設計移動機器人速度的控制律，使機器人的真實軌跡與參考軌跡之間的跟蹤誤差最小化。路徑誤差是由噪聲、外部干擾和來自內部和外部源的測量傳感器誤差造成的[3]。

路徑跟蹤方面的研究主要包含了以下幾個方面的內容：基于反饋誤差的控制算法，該類型算法只關注了系統的輸入輸出，而忽略了機器人的運動規律，因此這種算法更適合于沒有模型的系統；基于模型的控制算法，該類算法運用機器人的運動學模型以及動態模型來跟蹤規劃好的路徑；基于參考路徑和機器人之間的幾何關系的控制器，實現起來較為容易，該算法利用運動模型計算移動機器人的轉向角，優點是易于植入，且具有良好的魯棒性。

本文對其中的PID 控制、MPC 控制、LQR 控制及純追蹤算法以及斯坦利算法的現狀進行了歸納分析，幾種方法的優缺點如表1 所示，為今后的研究提供一定的參考。

表1 幾種方法的優缺點

1 理論模型

通常，移動機器人的路徑跟蹤系統由模型和控制器組成，根據運動學模型[4]對機器人進行建模，得到機器人的位置和航向角。路徑跟蹤控制器的目標是使移動機器人的橫向誤差和航向誤差收斂到零，從而穩定到達目標位置。其中，橫向跟蹤誤差是指機器人當前位置到參考路徑最近點的偏差，航向誤差是指機器人與參考路徑之間的航向差。移動機器人的運動學模型如圖1 所示。

該移動機器人[5]的輪子是由兩個平行的驅動輪和一個萬向輪組成，XOY為全局坐標系，XRCYR為局部坐標系，機器人的位姿由全局坐標系q=[x y θ]定義，其中，x和y是移動機器人在全局系統中的坐標點，θ為機器人的方向角度，C為機器人的中心，假設機器人沒有滑動和滾動。式（1）為移動機器人的運動學模型[6]：

其中，v和ω分別為機器人的線速度和角速度；x和y為機器人在全局坐標系中的坐標點；θ為機器人的方向角度。

2 算法控制

2.1 PID 控制

PID 控制器主要包含3 個部分：比例、積分、微分。標準比例積分導數（PID）控制器因其結構簡單、易于設計而在工業機器人領域得到了廣泛的應用。PID控制器可以實現穩定性和跟蹤控制，但跟蹤控制存在靜態誤差，其性能與比例項Kp的比例因子有關，而且PID 控制器需要建立過程的數學模型參數。

PID 控制的基本思路[7]是計算被控對象的輸入輸出差值，通過比例調節、積分調節、微分調節得到系統的控制量，利用控制量對機器人進行控制，使其跟隨設置好的路徑進行行駛。PID 控制的整體控制量的計算方法[8-9]如式（2）所示。所有這3 個控制器組合在一起，使它給出一個控制信號。作為一種反饋控制器，它在指定的水平上傳遞控制輸出，并保持輸出，使過程變量與閉環任務的期望輸出之間存在零誤差。在PID 控制中，最重要的就是設置好Kp、Ki、Kd3 個參數，如果選取的參數合適，就可以達到所期待的效果。

其中，u(t)為控制器輸出的控制量；Kp為比例系數；Ki為積分系數；Kd為微分系數；e(t)為實際值與目標值之間的差值。

PID 控制器被認為是一種廣泛應用的控制器，在控制系統中應用范圍很廣，因為其增益參數易于調整，所以PID 控制器的優點是簡單、穩定且易于實現[10]。但這種方法需要存儲每一次的偏差，計算量較大，且存在非線性、不精確和參數不準確等問題，因此，PID控制器限制了系統的實現設計和性能，此外，PID 控制器的參數整定需要一定的經驗和大量的實驗。

SIDI M H A 等人[11]對履帶式機器人進行建模并使用PID 控制器控制機器人進行跟蹤。TIWARI T 等人[12]提出了將非耦合線性PID 控制器與顯式條件相結合來控制移動機器人，提高了跟蹤的性能。該研究展示了一種利用遺傳算法（GA）進行調整的最優控制方法來定義模糊控制器的顯式輸入/輸出范圍。將遺傳算法應用到改進的PID 控制上，提高了控制精度和收斂速度[13]。KARTHIKA B 等人[14]設計了SDRE 控制器和SDC 矩陣，提高了兩輪自平衡機器人的平衡能力。文獻[15]通過遺傳算法對PID 控制參數進行整定，使系統穩定性得到提高。使用SSA 對PID 參數進行整定，優化后的控制器可以有效地完成路徑跟蹤[16]。經過FPGA 令移動機器人進行速度調節和基于PSO 的PID 算法進行智能方向調節，通過精細算法及控制系統優化，令移動機器人在工作中對路徑具有高精準選擇，提升移動機器人的工作效率[17]。采用遺傳算法優化PID參數，使得參數選取更加靈活，跟蹤性能更好[18]。

2.2 模型預測控制器

模型預測控制器（簡稱MPC）是一種基于模型優化的控制器，該控制器主要包含了3 個部分：模型預測、滾動優化、反饋校正。其中，最重要的部分是預測模型和優化函數。

通常，MPC 被表述為一個線性二次極小化問題。利用二次規劃求解器可以有效地求解該公式化的問題。橫向車輛引導任務被表述為一個沿參考曲線的約束最優控制問題，通過將系統動力學沿參考曲線進行線性化處理，推導出一個線性時變模型。性能準則由一個二次代價函數定義。

MPC 實現跟蹤的基本思路：

（1）建立移動機器人的運動學狀態方程；

（2）對非線性方程進行線性化處理；

（3）對模型進行離散化處理；

（4）建立預測方程，即將當前狀態量和上一個時刻的控制量組合成新的狀態量輸入；

（5）構造合適的代價函數；

（6）求解最優問題。

MPC 具有良好的魯棒性，且跟蹤誤差相對較小，但其收斂速度緩慢。

MPC 控制器的核心是在每個時間步長求解一個優化問題，從而找到一個最優的序列[19]。文獻[20]基于細化路徑和曲率，采用前后向積分策略設計了受輪胎-路面摩擦限制的最小時間縱向速度剖面。YUE X等人[21]提出了一種由模糊系統自適應調整參數的模型預測控制器，使機器人能夠在斜坡上準確、穩定地跟蹤給定路徑。近年來，計算硬件的相當大的進步促進了模型預測控制和非線性優化技術的實際應用，例如自動驅動領域的差分動力學編程。一些研究人員探索了模型預測控制（MPC）和DDP 在機器人的自動駕駛和自動控制中的可能性[22]。SUN C 等人[23]提出帶有切換跟蹤誤差的MPC 控制器，建立基于模糊邏輯的狀態分類器，從而更準確地跟蹤路徑。

2.3 線性二次調節控制器

線性二次調節器（簡稱LQR）是一種利用狀態空間方法分析系統的現代控制方法。其主要思想是通過優化代價函數式（3）來運行動態系統，找到一個控制函數，優化控制量，使得成本函數最小化。LQR 控制器采用狀態反饋控制器，用一組線性微分方程來解釋系統的動力學。LQR 實際上是一種流行的控制器，它通過反饋增益控制系統的閉環穩定性和提高系統的設計性能。LQR 由于其穩定性和魯棒性等優點，被廣泛應用于許多控制應用的系統設計中。其中，J為代價函數；Q、R分別為狀態權重矩陣和控制權重矩陣；x為當前系統狀態量；u為當前系統控制量。

LQR 實現路徑跟蹤的基本思路：

（1）選擇參數矩陣Q和R；

（2）求解Riccati 方程得到矩陣P；

（3）計算系統反饋矩陣K=R-1BP；

（4）計算控制量u=-Kx。

式（4）為Riccati 方程：

其中，A、B、Q、R皆為已知量，可以求解出矩陣P。

LQR 算法在跟蹤直線時性能良好，但在路徑曲率變化過快時會存在超調問題。文獻[24]采用預瞄PID對LQR 控制器進行轉角補償，從而消除穩態誤差，使得跟蹤精度更高。文獻[25]提出一種基于路徑跟蹤誤差的參數計算方法和一種基于車-路位置關系的參數調整規則，以此實現LQR 控制器的改進，提高控制器的適應性與控制精度。采用基于最優控制理論的線性二次調節器對機器人進行誤差自適應調整，能夠有效跟蹤路徑[26]。根據虛擬跟蹤路徑曲率、車輛車速和虛擬路徑相對航向偏差設計自適應預瞄算法，以此建立自適應預瞄前饋LQR 控制器，具有較好的跟蹤效果[27]。以側偏角最小為目標設計轉向控制規則?；谧顑灴刂评碚?，采用線性二次型調節器實現車輛軌跡跟蹤的最優控制[28]。

MPC 和LQR 都是基于模型預測的控制器，兩種方法有很多相似之處，但也有很多不同的地方。第一，LQR 主要是針對控制中的線性系統，而MPC 的研究對象更廣泛，既可以是線性系統，也可以是非線性系統；第二，因為兩種方法都存在優化問題，所以對于目標函數的設計是必要的，LQR 的目標函數是研究對象的狀態以及控制輸入的二次型函數，而MPC 的目標函數是將優化目標與權重相乘再求和；第三，LQR 是將一個控制周期內求出的最優解發給控制器，而MPC是滾動優化，將最優解序列中的第一個控制值發送給控制器。

2.4 純追蹤算法

純追蹤算法是一種基于移動機器人和路徑之間幾何關系的方法，其主要思想是由幾何關系的分析運動學模型和參考路徑，以及這些模型跟蹤的路徑。幾何路徑跟蹤方法主要有純追蹤算法以及斯坦利方法，已經得到了廣泛而實際的應用。其主要優點是采用簡單的幾何模型，根據當前狀態及時反饋，從而滿足移動機器人的實時需求，實現簡單，魯棒性好。

純追蹤算法如圖2 所示[29]，OXY為慣性坐標系，O為機器人左右輪的中心坐標，r為機器人的轉向半徑，ld為前瞻距離，(x, y)為前瞻點。

由幾何關系[30]可得：

其中，x、y為前瞻點的橫縱坐標；ld為前瞻距離；r為轉向半徑；γ為曲率。

圓弧曲率與前瞻距離的關系為：

阿克曼轉向模型可得到機器人的前輪轉角為：

其中，L為機器人的軸距；δ為機器人的轉向角；r為機器人的轉向半徑；x為機器人與前瞻點之間的橫向誤差。

純追蹤算法的性能主要取決于前瞻距離的選擇[30]，如果選擇的前瞻距離過小，機器人的跟蹤精度相對較高，但會導致跟蹤不穩定，容易產生振蕩；如果選擇的前瞻距離過大，機器人跟蹤相對穩定，路徑會比較平滑，但是容易產生切角問題。由此可見，前瞻距離的選擇對于移動機器人的路徑跟蹤至關重要，因此要合理選擇前瞻距離。

純追蹤算法的步驟可以概括為以下幾點：

（1）選擇合適的前瞻距離；

（2）以機器人為中心畫圓，將其與路徑的交點記為前瞻點，也即目標點；

（3）計算移動機器人的偏轉角度；

（4）控制機器人沿著路徑跟蹤；

（5）更新位置，重復上述幾個步驟。

純追蹤算法實現起來較為簡單，易于植入，且魯棒性強[31]，但當機器人的速度發生顯著變化時，該算法會產生較大的誤差。

對于純追蹤算法的研究有很多，文獻[32]給出了預瞄距離的確定方案，結合模糊理論設計出一種變預瞄距離的路徑跟蹤模糊控制器。AHN J 等人[33]提出了一種方法，通過考慮車輛和道路之間的關系啟發式地選擇一個有預見性的點，使用該方法可以穩定收斂于期望路徑。文獻[34]研究了基于車速和GPS 軌跡與當前車輛位置的最短距離的兩次多項式函數的先進純跟蹤算法，該算法可以在車輛跟蹤理想路徑時標定車輛的航向角和轉向角，減少橫向誤差。將速度作為一個計算前瞻距離的參數來提高系統的穩定性，將比例控制器添加到純追蹤轉向角的計算算法，提高算法在曲線階段的跟蹤精度[35]。文獻[36]提出了一種新的基于優化前瞻距離的純追蹤算法OLDPPA，通過實驗，證明了該算法的有效性。文獻[37]提出了一個更健壯的、有預見性的距離調優策略的幾何路徑跟蹤控制器，控制器可以適應不同的速度，執行更穩定，證明了沒有必要添加所有相關變量的模糊輸入，以及兩個變量是足以調整先行的距離。文獻[38]將PID 與純追蹤算法結合起來可以減少橫向角誤差并輸出穩定的控制角度。將純追蹤作為基本控制器，添加比例積分控制器，提高了轉向的穩定性，并使用低通濾波器平滑最終的轉向角[39]。將模糊控制器與純追蹤算法結合起來，速度與橫向誤差作為輸入，前瞻距離作為輸出，可以快速消除路徑跟蹤誤差，提高精度[40]。文獻[41]提出基于雙切圓巡線與純跟蹤結合的路徑跟蹤算法，降低了收斂時間。張宜寶等人[42]設計改進型純追蹤模型并提出基于模糊PID 的轉向控制方法。

純追蹤算法結構簡單，有良好的魯棒性，但過于依賴前瞻距離，雖然以上研究取得了一些進展，但前瞻距離與眾多因素的關系難以描述，導致前視距離最優值難以獲取，因而該算法多適用于速度較慢的路徑跟蹤。

2.5 斯坦利（Stanley）算法

斯坦利算法是基于橫向跟蹤誤差的非線性反饋函數，利用移動機器人的位姿和參考軌跡之間的相對幾何關系生成轉向角指令，從而控制移動機器人向目標位置行駛。該算法的主要優點是不會出現如純追蹤算法那樣的切角問題，但其只關注當前的一個狀態，魯棒性相對來說比較差。斯坦利算法示意圖如圖3 所示。

控制原理可表示為：

其中，δ為期望轉角；δe為橫向誤差引起的期望轉角；θ為航向偏差引起的期望轉角。

若只考慮橫向誤差對期望轉角的影響，忽略航向誤差的作用，那么：

其中，k為增益系數；e為橫向誤差；v為機器人速度。

若只考慮航向誤差對期望轉角的影響，忽略橫向誤差的作用，那么：

其中，θ為航向偏差。

在計算期望轉角時，需要綜合考慮這兩個方面的影響，由此得到前輪期望轉角，如式（14）：

斯坦利算法的流程如下所述：

（1）得到當前時刻機器人的位姿信息；

（2）找到距離當前位置最近的點；

（3）獲取最近點和當前位置的航向角，計算橫向誤差和航向誤差；

（4）根據公式計算前輪轉角；

（5）更新機器人位姿信息并進行循環。

將幾何路徑跟蹤方法中的純追蹤方法與斯坦利方法結合[43]起來，可以有效地減少切角問題，更快地收斂到規劃好的路徑上。文獻[44]提出了一種新的通過模糊控制和粒子群算法改進斯坦利算法的方法，修改控制增益自適應的跟蹤誤差，提高跟蹤精度。WANG L 等人[45]將多種群遺傳算法應用到斯坦利算法上，獲得了更好的跟蹤效能。文獻[46]采用純追蹤算法對Stanley 算法中車輪轉角進行改進，從而計算出合適的前輪轉角，提高了跟蹤的平滑性。文獻[47]提出一種增益系數自適應的Stanley 模型路徑跟蹤算法，設計模糊推理和解模糊化過程，實時確定控制模型增益系數，提高Stanley 模型對不同曲率路徑的自適應能力。

3 結論與展望

隨著科技的發展進步，移動機器人技術已經得到了迅速發展。同時，機器人的廣泛應用也引起越來越多的人研究與它相關的技術。本文主要關注的是控制算法。在性能標準方面，跟蹤誤差是評估控制器性能最常見的特征。根據控制點的位置和所使用的控制方法，跟蹤誤差的確定可能因研究的不同而不同。在回顧了幾種算法后，可以得出結論：每一種控制算法都有其自身的優缺點，對于控制算法的設計應該將被控對象和應用場景考慮在內，針對被控對象使用合適的控制算法，設計相應的控制器。此外，控制器的精度和穩定性仍是研究的重難點，需要不斷地進行完善，以達到更好的跟蹤效果。目前，絕大多數的控制器都會使用兩種或兩種以上的算法以便為機器人的路徑跟蹤提供更好地性能，對以往算法的回顧可以幫助我們更好的進行新的工作。此外，將智能優化算法、機器學習及深度學習等領域的知識應用到運動控制領域也是一個可以深入研究的方向。