復雜路況下無人駕駛路徑跟蹤模型預測控制研究*

李 駿，萬文星，郝三強，秦 武，，劉霏霏

（1. 華東交通大學機電與車輛工程學院，南昌 330000；2. 建新趙氏科技有限公司，寧波 315000）

前言

近年來，傳感器技術、通信技術和軟硬件的快速發展，為無人駕駛技術的研究和工程應用領域的落地提供了有效的支持。無人駕駛車輛技術被譽為最終解決交通擁擠、減少交通事故、提高道路通行能力的有效方案，該技術主要包括感知融合、決策規劃和運動控制等3大關鍵技術，其中車輛運動控制是無人駕駛領域內的核心技術之一。而在具有直角轉彎、連續彎道和弧形彎等路徑曲率變化較大的復雜路況下，保證路徑跟蹤精度和行駛穩定性是無人車駕駛車輛能夠應用于各種場景所需要解決的一個難點。

無人駕駛車輛路徑跟蹤原理是在車輛上選取一點作為控制點，跟蹤決策層規劃出的幾何曲線或給定的參考路徑，使控制點與期望路徑間的橫向誤差和航向角偏差盡可能小或者逐漸收斂到零。無人駕駛車輛路徑跟蹤控制已經成為了無人駕駛車輛技術的研究熱點，國內外學者針對無人駕駛路徑跟蹤控制的精度、穩定性和安全性問題，開展了大量的研究。

目前，應用較多的路徑跟蹤控制方法有經典的PID反饋控制、預瞄控制、最優控制和基于機器學習的控制方法等。PID 控制不需要建立精確的物理模型，廣泛應用于工程領域，但在車輛運動控制過程中存在多變量系統和時變系統，使用PID控制效果難以得到保證。預瞄控制以純跟蹤算法為代表，該算法的關鍵在于前視距離和預瞄點的確定。最優控制對建立的模型精確度要求較高，根據建立的精確模型，能夠達到控制目標最優，但同樣因為車輛運動過程中參數時變和模型非線性，模型的準確性無法保證，因此最優控制在車輛運動控制中的應用存在局限性。機器學習的方法是基于大量的實驗數據驅動，通過大數據建立控制模型，得到和實測值相近的控制輸出。該算法的不足在于控制效果完全依賴于所采集數據的完整度。

以上控制算法對參數和環境的依賴度較高，在變化的環境中不能完全適應新的車輛狀態，且在試驗驗證時未考慮執行器和車輛的狀態約束。模型預測控制算法（MPC）具有對未來軌跡的預測和處理多約束條件的優勢，在車輛路徑跟蹤控制方面得到了廣泛的應用。文獻［14］中提出了一種基于非線性模型預測的橫縱向綜合控制的路徑跟蹤方法，通過所提出的控制算法計算出縱向力、側向力和橫擺力矩，利用障礙函數法求解輪胎力的最優分配。仿真結果表明，該方法能顯著提高跟蹤精度和行駛穩定性，但非線性模型會增加控制器的計算時間，實時性得不到保證。文獻［15］中提出了線性時變模型預測控制算法，減少了控制器的計算時間，提高了車輛運行的實時性，保證了跟蹤精度。文獻［16］中為兼顧路徑跟蹤精度和實時性，改進了3 自由度車輛模型，在模型預測控制算法中將橫擺速度跟蹤誤差加入到二次規劃性能指標，同時考慮車輛質心側偏角對車輛行駛穩定性的影響，由此在提高跟蹤精度的同時保證了行駛穩定性。文獻［17］中在傳統模型預測控制算法的基礎上，利用遺傳算法求解車輛行駛最優時域參數和加入前輪側偏角約束，改善了高速行駛、低附著系數時車輛跟蹤性能和平穩性。Ju等在研究分布式電動汽車路徑跟蹤問題時，提出了利用橫擺力矩對跟蹤偏差補償和車輛穩定性進行協調控制的方法，采用MPC 控制算法給出前輪轉角，將跟蹤偏差和橫擺角速度偏差的兩個滑動面設置為控制目標，最后將兩個組合面合成一個具有權重系數的協調滑動面，算出橫擺力矩，對跟蹤誤差進行修正。文獻［19］中提出了一種將模型預測控制算法與PID 速度控制器相結合的協調控制方法，以8 自由度的車輛模型作為預測模型，通過MPC 控制器生成最優前輪轉角，以嵌入模型預測控制優化求解中的PID 控制器生成車輛的驅動力/制動力，協調控制車輛的路徑跟蹤和速度跟蹤性能。

綜上所述，MPC 算法在簡單或雙移線路況下的仿真與試驗驗證能得到較好的路徑跟蹤效果和行駛穩定性。但是，目前基于MPC 算法的無人駕駛路徑跟蹤的研究還存在以下問題：在車輛橫向運動控制時，通常假定車輛縱向速度為恒定值，而忽略了縱向車速對橫向控制的影響；在直角轉彎、連續彎道和弧形彎的復雜路況下，考慮無人駕駛車輛路徑跟蹤的研究較少；絕大多數控制算法研究未考慮道路曲率對行駛速度的約束。

因此，本文基于駕駛員開車時會根據道路彎曲度調整車輛行駛速度的駕駛習慣，提出了一種改進的MPC 算法，提高MPC 算法在復雜路況下的跟蹤性能和行駛穩定性。該算法根據路徑彎曲度設計車輛縱向行駛速度，并將車輛的位置偏差、前輪轉角偏差和速度偏差作為二次規劃的性能指標。在測試場地進行了實車驗證，試驗結果表明：與縱向速度恒定的MPC 算法相比，改進MPC 算法時車輛路徑跟蹤精度提高了61.80%，驗證了改進模型預測控制算法的可行性和優越性，顯著改善了車輛平穩性。

1 無人駕駛控制問題描述

1.1 無人駕駛汽車平臺簡介

無人駕駛車輛平臺由某新能源汽車有限公司提供，該平臺是在某款純電動汽車的基礎上建立，其設備的布置實物圖如圖1 所示。該試驗平臺在保留原車車載傳感器的同時還后裝了無人駕駛所需要的位姿設備和環境感知傳感器，包括：雙GPS天線、前視攝像頭、4 線前向激光雷達、64 線激光雷達、工控機、慣導RT2000 等（見圖1（a））。車輛的定位采用實時動態載波相位差分技術，使用雙GPS天線和RT2000組成組合導航，讓無人駕駛車輛“知道”自己的位置和狀態；無人駕駛車輛的“眼睛”由前視攝像頭、4 線前向激光雷達、64線激光雷達以及車輛前裝的77 GHz毫米波雷達代替，通過多傳感器融合技術對車輛周圍環境進行實時感知；無人車的“大腦”由安裝在車輛后備箱的工控機代替，其對各傳感器傳輸過來的數據進行實時分析處理，并下達相應的控制指令；工控機下發控制指令時，首先通過USB 轉CAN 盒接入車身CAN 總線（見圖1（b）），再通過車身CAN 總線傳輸控制指令，以實現車輛的加減速、轉向等操作，因此CAN 總線相當于是無人駕駛車輛的“神經網絡”。

圖1 無人駕駛試驗平臺

1.2 無人駕駛汽車的控制結構

無人駕駛車輛運動控制主要分為橫向控制和縱向控制，其控制原理如圖2所示。無人駕駛車輛正常行駛時，車身傳感器感知周圍環境信息，將感知的數據輸入給工控機進行數據分析計算，得出車輛行駛的期望速度和期望位姿，同時車輛運動傳感器實時檢測車輛當前的速度和位姿信息，并將測量信息傳輸到工控機。工控機將計算得到的期望速度和期望位姿信息與傳感器測量的信息進行對比，得出相應加速踏板、制動或者轉向盤的控制量，最后通過CAN 總線控制車輛的執行機構以達到路徑跟蹤的目的。

圖2 無人駕駛運動控制原理圖

在無人駕駛運動控制中，一般認為縱向控制是對速度的控制，橫向控制主要是對轉向盤進行控制，研究時往往將兩者分開研究。在研究橫向控制時通常設定縱向速度（行駛速度）為恒定值，只對車輛的橫向控制進行研究，但對于復雜路況，不同的縱向速度會對橫向控制性能產生較大的影響。因此在復雜路況下，綜合考慮車輛的縱向控制和橫向控制對提高路徑跟蹤性能與車輛穩定性具有重要意義。

本文在研究路徑跟蹤控制時主要運用了無人駕駛車輛技術中的車輛定位和控制技術，圖3 所示為所搭建的路徑跟蹤試驗平臺。

圖3 路徑跟蹤試驗平臺

2 基于改進模型預測控制的無人駕駛車輛路徑跟蹤設計

一般情況下，以縱向速度恒定的模型預測控制（CMPC）算法設計的無人駕駛車輛控制器，在直線、雙移線等簡單路況下進行試驗，可以得到較好的路徑跟蹤效果。但在直角彎、連續彎道、弧形彎道等復雜路況下，無人駕駛車輛路徑跟蹤會出現較大的橫向誤差。針對復雜路況，駕駛員駕駛車輛時會根據行駛路徑彎曲程度進行相應的速度調整，以便更好地控制車輛沿著車道行駛。為了提高復雜行駛工況下的無人駕駛車輛路徑跟蹤精度，本文根據路徑的彎曲度設計車輛的行駛速度，即行駛速度是取決于路徑的彎曲度，而不是恒定值，因此研究改進的模型預測控制（MMPC）算法。

基于MMPC 算法的無人駕駛車輛路徑跟蹤設計，包括路徑彎曲度確定、參考速度設定和MPC 算法設計3部分。

2.1 路徑彎曲度確定

根據規劃好的期望路徑信息，將其等距離采樣，如圖4所示。在路徑上每間隔距離取一個點，每連續的3 個點進行一次彎曲度計算。在圖4 中從路徑的起始點D1 開始，先與D2、D3 點計算一次彎曲度；然后以D2 點作為基點與D3、D4 再計算一次彎曲度。依次循環計算每3個點之間的彎曲度。

圖4 期望路徑等距離采樣下彎曲度確定

每個點處的彎曲度計算公式可表示為

式中θ表示第段折線到第+ 1 段折線的彎曲度。根據采集或者規劃的期望路徑，通過離散化，計算得到每連續線段處的彎曲度值。為避免連續線段之間彎曲度變化過大，引起匹配的車速不連續或者突變而造成車輛行駛不穩定情況，需要對全程期望路徑的彎曲度值進行平滑處理：

2.2 參考點速度設定

彎道實際限速一般采用簡化計算，如Pomerleau等提出的彎道限速公式為

式中：為重力加速度；為道路半徑；μ為路面附著系數；為路面超高角度。當車輛在平坦的道路（= 0）上不發生滑移時，車輛轉彎行駛最大速度是根據向心力公式推導。

根據道路半徑可計算出路徑參考點的最大參考速度，當采用距離的值取不大時，可將連續的3 個點之間2點的連線看作3點所確定圓的切線，此時每3 個點連成兩段折線的中垂線所交的圓心角就等于彎曲度值，即

式中：d為采樣時間；為車輛繞著等速圓心的角速度；為車輛的行駛速度。由式（5）可得到每一段處車輛的轉彎半徑。聯合式（3）～式（5）可得到在平坦道路上車輛不發生滑移的最大速度為

式中μ為路面附著系數，可以根據工程經驗或文獻［21］獲得，通常情況下車輛正常行駛的路面附著系數范圍為0.7 ≤μ≤1。

在低速（30 km/h）下彎曲度小于3°時，一般可以認為是直線路段，可以按照設定的目標速度行駛；在彎曲度大于3°的路況下，道路彎曲度只限制最大速度，并沒有最小速度的限制，通過大量仿真對比并綜合考慮無人駕駛車輛的跟蹤效果，可將參考速度設定為

式中為常數。

根據式（6）和式（7）可計算出期望路徑下不同彎曲度所匹配的參考速度，并且能夠實現參考速度的連續變化。不需要通過離散的坐標信息擬合，計算出每一段路徑的道路半徑，以設置行駛速度。改進的模型預測控制算法可以在給定的期望路徑上根據路徑彎曲度匹配相應的期望行駛速度，實現在復雜工況下的變速行駛。

2.3 模型預測控制算法設計

模型預測控制的主要原理是利用建立的模型、系統的當前狀態和未來的控制量預測系統未來的輸出，通過在線滾動求解帶約束優化問題來實現對系統的控制目的。對于無人駕駛車輛的運動控制，控制算法的實時性是至關重要的。因此，本文采用線性時變模型預測控制算法作為車輛路徑跟蹤控制方法。該算法基于車輛動態的線性狀態模型，通過預測模型、滾動優化和反饋校正，實現了車輛路徑的自動跟蹤。

因此模型預測控制算法的設計可由車輛模型的建立、預測模型、滾動優化和反饋校正等4步構成。

（1）車輛模型的建立

車輛運動學模型從簡化的阿卡曼轉向幾何模型和車輛位姿關系的角度研究車輛的運動規律。因為其不涉及車輛的物理性質（如質量）和受力情況，所以當車輛低速（≤30 km/h）行駛在路況良好的路面上時，不需要考慮復雜的輪胎滑移及車輛側向動力學特性對車輛的影響，此時基于運動學模型設計的路徑跟蹤控制器也具備可靠的控制性能。建立車輛運動學模型，如圖5 所示，在大地坐標系下，設定車身與大地坐標系軸的夾角為車體的橫擺角，其中(，)和(，)分別為車輛前后軸中心坐標，為車輛后軸中心的速度，為等效前輪轉角，為車輛軸距。

圖5 車輛運動學關系示意圖

根據車輛運動與幾何關系，得到車輛的運動學模型表達式為

（2）預測模型

為減小計算量，保證控制量的實時性，將式（9）進行線性化和離散化處理。離散化處理，得到離散狀態空間方程：

為避免系統控制量出現突變的情況，對控制增量進行精確約束，將狀態量與控制量進行組合，重新構建新的狀態空間表達式：

通過觀察式（13），可看出在預測時域內任意時刻系統的狀態量和輸出量都可以通過系統的當前狀態量(|)和控制時域內的控制增量Δ()計算得到，即可實現模型預測控制算法中的“預測”功能。

（3）滾動優化

車輛在進行路徑跟蹤控制的過程中，模型預測控制器的每一采樣時刻都要求狀態偏差和控制增量最小，以獲得最佳的路徑跟蹤性能和車輛行駛穩定性。在設計路徑跟蹤控制器時采用如下形式的目標函數：

式中：、和? 分別表示控制量、控制增量和松弛因子的權重矩陣，通過調節不同權重矩陣得到不同的車輛跟蹤效果；為松弛因子，將松弛因子添加到目標函數中，是為了防止目標函數出現無解的情況。式（14）中等號右邊的第1 項反映了系統對參考路徑的跟蹤能力，即當權重矩陣較大時，車輛的跟蹤誤差更?。坏? 項反映了系統對控制量平緩變化的要求，即當權重矩陣較大時，計算出的控制量變化更平緩，車輛運行的過程也會更加平穩。

在實際控制過程中，需要將控制量和控制增量限定在合理范圍之內，即無人駕駛車輛的控制量和控制量增量的約束表達式為

為后續優化求解將約束條件中的變量統一，式（15）可以轉換為以下形式：

式中：U= I?(- 1)，I表示行數為N的列向量，(- 1)為上一時刻的控制量；、分別表示控制時域內控制量的最小值和最大值集合；Δ、Δ分別為控制時域內控制增量的最小值和最大值集合；A為N× N的單位下三角矩陣。

將目標函數轉化成標準的二次型，結合式（16）約束條件，求解以下優化問題：

在下一個控制周期，將上一控制周期的狀態信息作為系統的依據來預測下一控制時域內的輸出，再次通過上述優化求解得到式（19）序列。重復上述過程，完成在控制時域內向前采樣周期內的滾動優化，實現車輛前輪轉角和行駛速度的最優控制輸入。

3 試驗分析

3.1 試驗參數設置

本文試驗采用某新能源汽車有限公司提供的測試場地作為跟蹤路徑，利用定位設備采集廠區測試場地經緯度數據，得到的測試場地路徑如圖6 所示。圖6（a）為JOSM 軟件中處理得到的廠區測試地圖。從圖可知，該測試場地有直線行駛、直角轉彎、連續彎道和弧形彎復雜路況。為便于試驗驗證，將路徑經緯度坐標轉換成以紅點為原點的大地坐標，得到的大地坐標系下廠區路徑如圖6（b）所示。圖中箭頭表示車輛測試時的起始方向。

圖6 測試場地路徑

根據采集點的坐標信息，計算不同路段處的彎曲度，得到如圖7（a）所示的道路彎曲度變化特性；實車試驗的道路為干燥的水泥道路，路面附著系數取μ= 0.85；考慮到實時計算量和道路曲率精度，根據實車調試效果，采樣距離取=1 m；計算彎曲度ˉ時，為避免采樣路段間彎曲度變化過大而造成參考速度跳變，式（2）中取=5。將各參數具體值代入式（6），最大行駛速與彎曲度的關系式為

利用式（20）計算出無人駕駛車輛路徑跟蹤的全程參考速度。圖7（b）為道路彎曲度與最大參考速度的對應關系。

圖7 路徑彎曲度及彎曲度與最大參考速度的對應關系圖

根據文獻［22］和文獻［23］，考慮車輛加減速過程的平穩性和乘坐的舒適性，并結合調試效果，權重矩陣、和?選擇為

式中：為車輛期望速度；Δ為每個控制周期的速度變化量；Δ為每個控制周期前輪轉向角的變化量。

根據式（21）和式（22），得到控制量的約束條件和控制量增量約束條件：

3.2 模型預測控制下路徑跟蹤試驗分析

為驗證MMPC算法在復雜路況下的路徑跟蹤效果，根據車輛運動學模型和MMPC 算法，利用Python語言編寫控制程序，并將采集的測試場地地圖信息導入到編寫程序中作為無人駕駛車輛的期望路徑。在進行實車試驗時，MMPC 算法下的車輛期望行駛速度根據道路彎曲度進行設計，具體數值設定根據式（7），式中的常數取=0.75。而在CMPC 算法下的車輛期望行駛速度設置為恒定值5.6 m/s（20 km/h）。試驗得到CMPC 算法和MMPC 算法下的速度、航向角和橫向誤差，對比分析在復雜路況下CMPC 算法和MMPC算法的路徑跟蹤效果。

圖8（a）和圖8（b）分別為CMPC 和MMPC 算法下的實車路徑跟蹤和行駛速度的對比結果。由圖8（a）可知，兩種控制均能準確地實現車輛跟蹤期望路徑；但在直角彎、連續彎道和弧形彎的路況下，兩種控制算法下車輛出現一定的跟蹤偏差。從圖8（b）可知，在CMPC 算法下車輛行駛速度為恒定值，而在MMPC 控制下車輛的行駛速能夠根據行駛的路徑彎曲度進行相應調節。

圖8 CMPC和MMPC算法下實車路徑跟蹤和行駛速度

圖9（a）為CMPC 和MMPC 算法下的航向角變化規律。由圖可知，在彎道處，車輛行駛速度越小，車輛橫擺角速度越小，車輛穩定性和乘坐舒適性得到提高。圖9（b）為CMPC 和MMPC 算法下的橫向誤差變化規律。由圖中可知：與直線路況相比，車輛在直角彎、連續彎道等大彎曲度處橫向偏差明顯增大，表明不同行駛路況對無人駕駛車輛路徑跟蹤效果具有較大的影響；CMPC 算法下車輛最大路徑跟蹤誤差為0.89 m，而MMPC 算法下車輛最大路徑跟蹤誤差只有0.34 m，與CMPC 算法跟蹤誤差相比，減少了0.55 m，最大跟蹤程度提高了61.80%，車輛具有更高的路徑跟蹤精度。

圖9 CMPC和MMPC算法下航向角和橫向誤差

為更好地分析在直線行駛、直角轉彎、連續彎道和弧形彎等復雜路況下車輛路徑跟蹤效果，將圖8中對應的復雜工況局部放大，如圖10 所示。在直線路況下（圖10（a）），CMPC 算法和MMPC 算法下車輛具有較好的跟蹤效果，兩種控制算法下最大橫向誤差只有0.31 m。而在直角轉彎路況下（圖10（b）），CMPC 算法和MMPC 算法下車輛通過直角彎時都會出現提前入彎的情況，這是因為兩種算法中的預測時域N值不可調，N值較大時，車輛在較遠處的誤差權重較大，使得車輛橫向誤差均呈現先增大、再減小、再增大、后減小的變化趨勢；在CMPC 算法下車輛最大橫向誤差達到0.68 m，而在MMPC 算法下車輛橫向誤差維持在0.25～0.32 m，這是由于該算法的縱向速度根據路徑彎曲度進行相應調整所至。在連續彎道路況下（圖10（c）），MMPC 算法下車輛行駛速度相對較小，車輛橫擺角速度小、穩定性好，路徑跟蹤橫向誤差最大為0.31 m。這與CMPC 算法下車輛最大橫向誤差為0.68 m 相比，MMPC 算法下的車輛跟蹤誤差減小了0.37 m，提高了入彎、出彎時的跟蹤精度。MMPC 算法能夠提高跟蹤精度的原因是：連續彎道處的彎曲度值變化較大，車輛行駛速度比CMPC 算法下的車輛行駛速度要小，縮短了車輛達到期望轉角的調整距離，從而減小了跟蹤誤差。而在弧形路況下（如圖10（d）），CMPC 算法和MMPC算法均能對過彎時產生的橫向偏差進行快速調節，但MMPC 算法能進一步減小跟蹤誤差，提高車輛的跟隨效果。

圖10 圖8（a）中直線行駛、直角轉彎、連續彎道和弧形彎下局部放大圖

4 結論

本文針對復雜路況下無人駕駛車輛路徑跟蹤控制問題，提出了一種改進的模型預測控制算法（MMPC）。與縱向速度恒定的模型預測控制算法（CMPC）相比，MMPC 算法是根據路徑彎曲度設計變化的縱向速度，故縮短車輛收斂到期望航向角時的調整距離，減小了跟蹤誤差。通過無人駕駛車輛實車試驗，對比分析了在復雜路況下MMPC 算法和CMPC算法的路徑跟蹤效果，得到了以下結論：

（1）在彎曲度較小的路況下，MMPC 算法和CMPC 算法下無人駕駛車輛具有相同的跟蹤效果，在彎曲度較大的路況下，CMPC 算法下最大跟蹤誤差為0.89 m，而MMPC 算法下最大跟蹤誤差僅有0.34 m，相比于CMPC 算法跟蹤誤差減小了0.55 m，跟蹤精度提高了61.80%，實現了無人駕駛車輛具有更好的路徑跟蹤效果；

（2）車輛在入彎或過彎時，MMPC 算法能夠調節車輛行駛速度，使車輛的航向角變化率更加平緩，提高了車輛行駛穩定性和乘坐舒適性；

（3）車輛在入彎或者過彎時，MMPC 算法下車輛能夠減速行駛，更加符合人類駕駛員的駕駛行為。