在上一講《Coursera自動駕駛課程第6講：Vehicle Longitudinal Control》中我們了解了如何使用PID算法進行汽車縱向控制。

本講我們繼續學習新的模塊：汽車橫向控制。具體地，我們將學習三種控制算法：Pure pursuit，Stanley，MPC。

B站視頻鏈接：https://www.bilibili.com/video/BV1WE411D74g?p=1

---

文章目錄

• • • 1. Introduction to Lateral Control
    • • 1.1 Overview
      • 1.2 Two Types of Control Design
      • 1.3 Driving Controller
    • 2. Geometric Steering Control - Pure Pursuit
    • • 2.1 Geometric path tracking
      • 2.2 Pure pursuit
    • 3. Geometric Steering Control - Stanley Controller
    • • 3.1 Stanley Controller Approach
      • 3.2 Heading control & Combined steering law
      • 3.3 Case Study
    • 4. Advanced Vehicle Control Methods （難點）
    • • 4.1 Overview
      • 4.2 Receding Horizon Control
      • 4.3 MPC Structure
      • 4.4 Vehicle Lateral Control

1. Introduction to Lateral Control

1.1 Overview

在設計汽車橫向控制器時，通常需要進行以下三步：

定義車輛所處位置和預期路徑之間的偏差。
選擇一種控制設計策略，將偏差降至零，同時仍滿足輸入限制（如輸入轉向角限制等）。
考慮車輛本身的動態限制和所需的行駛特性。

下面介紹三種參考路徑定義：

• 直線段序列。
• 一系列緊密間隔的航點。
• 一系列連續的參數化曲線來定義路徑。

在上述三種路徑跟蹤情況下，控制器都試圖消除車輛與預期路徑之間的偏差，并使車輛航向與路徑方向對齊。

---

1.2 Two Types of Control Design

下面介紹兩種類型控制器：

• 第一類控制器是幾何控制器（Geometric Controllers），常見的有：Pure pursuit和Stanley。
• 另一類控制器稱為動態控制器（Dynamic Controllers），常見的有：MPC 控制。

后面我們以自行車模型為基礎來討論汽車橫向控制。

如下圖所示，我們將使用線段作為參考路徑，在圖中以實心黑線顯示。還可以看到一條平行于路徑但穿過前軸中心的黑色虛線。為了進行橫向控制，我們重新定義了相對于當前路徑線段的航向$\delta$和$\psi$。

請注意，根據我們的控制器設計，我們可以在后軸中心，前軸中心或重心處放置車輛參考系。

---

1.3 Driving Controller

如上一節所述，我們將介紹兩種類型誤差：航向誤差和軌跡偏移誤差。

航向誤差為沿路徑參考點的路徑方向與車輛實際航向之差。 航向誤差變化率有助于我們了解航向誤差如何隨時間變化，并且可以從自行車模型方程中進行計算。

這里，我們介紹相對于前軸的航向誤差率。對于直線段，所需的航向變化率為零，可以將其刪除。 這是因為參考航向對于一條直線不是隨時間變化的，并且實際上等于零。最終我們得到航向誤差變化率為：
$$\dot{\psi }(t)=\frac{?v_f(t)\sin \delta (t)}{L}$$

另一種類型的誤差是軌跡偏移誤差。定義為車輛上的參考點與所需路徑上的最接近點之間的距離。其公式為：
$$\dot{e}(t)=v_f(t)\sin (\psi (t)?\delta (t))$$

從該方程中，我們可以看到，隨著速度的增加，誤差變化更快。

---

2. Geometric Steering Control - Pure Pursuit

2.1 Geometric path tracking

幾何路徑跟蹤控制器使用車輛運動學的幾何形狀和參考路徑來跟蹤參考路徑。

幾何路徑跟蹤控制器依賴于沿所需路徑的參考點，該參考點可以是用于計算航向和軌跡偏移誤差的參考點，也可以是沿路徑在車輛前方一定距離處的超前點，一此處以紅色顯示。

---

2.2 Pure pursuit

下面我們介紹Pure pursuit。其核心思想是可以將參考點放置在路徑上距車輛前方固定距離的位置，并且可以使用恒定的轉向角計算與該點相交所需的轉向命令。

在這種方法中，后軸的中心為車輛參考點，我們將后軸中心與目標參考點之間距離定義為$l_d$。 車輛的車身方向與連接線之間的角度為$\alpha$。

為了構造Pure pursuit控制器，我們再次引入瞬時旋轉中心的概念。 軌跡上的目標點，后軸中心和瞬時旋轉中心形成一個三角形，其半徑長度為$R$，目標點和后軸中心長度為$l_d$。 從幾何關系以及正弦定律我們可以得出路徑曲率：
$$\kappa =\frac{1}{R}=\frac{2\sin \alpha }{l_d}$$

現在，讓我們看一下自行車模型，以計算跟蹤此弧線所需的轉向角：
$$\delta ={\tan }^{?1}\left(\frac{2L\sin \alpha }{l_d}\right)$$

軌跡偏移誤差$e$在這里定義為航向矢量和目標點之間的橫向距離，因此我們可以得出曲率與誤差之間關系：
$$\kappa =\frac{2}{l_d^2}e$$

隨著誤差的增加，曲率也隨之增加，使車輛更快速地返回至參考路徑。

該式也表明，Pure pursuit控制器以類似于比例控制的方式工作，以使用路徑曲率作為控制器的輸出來校正軌跡偏移誤差。比例增益為$\frac{2}{l_d^2}$。因此，隨著$l_d$的增加，比例增益減小。

我們也可以對Pure pursuit制器進行修改。 我們可以根據車輛的速度改變距離$l_d$。 使其與車輛前進速度成正比：
$$l_d=K_{dd}{v}_f$$

綜合以上式子，我們得到轉向角和車速之間的關系：
$$\delta ={\tan }^{?1}\left(\frac{2L\sin \alpha }{K_{dd}{v}_f}\right)$$

---

3. Geometric Steering Control - Stanley Controller

3.1 Stanley Controller Approach

Stanley Controller 是一種幾何路徑跟蹤控制器。最初是由斯坦福大學的Gabe Hoffman開發的，并以斯坦利車名字命名。其主要特點有：

• 將車輛參考點切換到前軸中心。
• 同時考慮航向對齊和軌跡偏移誤差。
• Stanley控制器為其輸出設置了上限，使輸出落在最大轉向角范圍內。

---

3.2 Heading control & Combined steering law

我們先介紹下如何進行航向控制的。

在左圖中可以看到軌跡偏移誤差是相對于前軸測量的。 Stanley控制器通常會有以下三個需求：

• 為了消除相對于路徑的航向誤差，轉向角直接等于航向角：$\delta (t)=\psi (t)$
• 為了消除軌跡偏移誤差，使用了比例控制，其增益$k$滿足以下公式：$\delta (t)=ta{n}^{?1}(\frac{ke(t)}{v_f(t)})$。與誤差基本成正比，與速度成反比，控制函數為反正切函數。
• 最后，轉向角會限制在一個取值范圍內：$\delta (t)\in [{\delta }_{min},{\delta }_{max}]$。

為了消除航向誤差和軌跡偏移誤差，最終得到的汽車轉向角為：
$$\delta (t)=\psi (t)+ta{n}^{?1}(\frac{ke(t)}{v_f(t)}),\delta (t)\in [{\delta }_{min},{\delta }_{max}]$$

現在讓我們看一下對于不同的誤差系統要求的汽車轉向角是多少。

對于航向誤差，轉向命令指向與航向誤差相反的方向，從而使車輛轉彎以糾正與路徑未對準的情況。 例如，對于較大的航向誤差，如果航向誤差超過最大轉向角，則控制器將會發出最大轉向指令。

對于較大的軌跡偏移誤差，此時反正切角將會接近$\frac{\pi }{2}$。因此，我們可以將Stanley控制器輸出為航向誤差加上$\frac{\pi }{2}$。
$${\tan }^{?1}\left(\frac{ke(t)}{v_f(t)}\right)\approx \frac{\pi }{2}\to \delta (t)\approx \psi (t)+\frac{\pi }{2}$$

我們討論以下誤差的變化情況。=我們代入上式，可以得到誤差變化率為：
$$\begin{array}{rl}\dot{e}(t)=?v_f(t)\sin (\psi (t)?\delta (t))& =?v_f(t)\sin \left({\tan }^{?1}\left(\frac{ke(t)}{v_f(t)}\right)\right)\\ & =\frac{?ke(t)}{\sqrt{1+{\left(\frac{ke(t)}{v_f}\right)}^2}}\end{array}$$

當跨軌偏移誤差較小時，上式可以簡化為：

$$\dot{e}(t)\approx ?ke(t)$$

上式最有趣的是誤差變化率與速度完全無關。

---

3.3 Case Study

下面我們來看兩個仿真例子。

第一個例子，對于較大的初始軌跡偏移誤差，讓我們假設初始誤差為5米，最大允許轉向角為25度，前進速度為5m/s。
仿真結果顯示了Stanley控制器如何糾正較大的軌跡偏移誤差并收斂到所需路徑。較大的初始誤差會導致較大的轉向指令，從而使車輛迅速轉向行駛路徑。然后，航向誤差和跨軌誤差項達到平衡，車輛沿直線繼續朝向路徑行駛。

我們還可以在不同的速度下運行相同的模擬。因此，讓我們嘗試2m/s，5m/s和10m/s的速度。結果顯示了Stanley控制器的主要特征。在所有情況下，都可以看到朝向路徑的轉彎，直線前進以及路徑的指數衰減。速度越高，汽車在到達路徑之前行駛的距離就越遠。

現在讓我們看看第二個仿真例子，初始時車輛的航向誤差很大。
仿真結果表明航向誤差已由Stanley控制器校正。首先，在糾正航向誤差時，轉向指令已超過其極限。然后，隨著軌跡偏移誤差開始增加，轉向命令將繼續校正汽車的行駛方向，使其與路徑對齊。最后，汽車像進入指數收斂段。實際上，它具有全局穩定性功能，這意味著無論初始條件如何，控制器都將引導汽車返回原路。

然而在實際情況中，需要對Stanley控制器進行一些調整。

• 由于速度項位于反正切內的分數的分母中，因此低速往往會導致轉向命令放大。這導致方向盤劇烈晃動，這對于駕駛員的舒適性而言是不希望的。因此，為了消除此問題并提高解決方案在低速下的穩定性，可以添加了一個常數，該常數可以在現場進行調整。
• 在較高的速度下，我們存在一個問題，即轉向命令需要緩慢變化以確保橫向力不會過大。即使在速度上如此縮放，斯坦利在高速下的反應仍然過于激進，因此還增加了航向阻尼項。這實質上將航向誤差控制部分轉換為PD控制器。
• 最后，對于具有高曲率的彎曲路徑，控制器無法很好地跟蹤它們，因為在幾何控制器的推導中未考慮動力學。正如我們在縱向控制中看到的，通過向控制器添加前饋項，可以提高系統性能并將誤差更快收斂為0。

---

4. Advanced Vehicle Control Methods （難點）

4.1 Overview

MPC指的是一種控制設計方法，可以在每個時間步長上以數值方式優化和解決問題。

用于自動駕駛汽車的縱向和橫向控制是MPC的一個常見應用。模型預測控制通常也可以稱為Receding horizon approach，因為控制器會在每個時間步長基于固定的有限長度生成一個執行器信號。MPC的主要優勢有：

• MPC控制器的公式簡單，只需定義目標函數和相關約束，然后使用已知的求解器對其進行優化。

• 由于許多汽車子系統具有嚴格的執行器約束和多樣化的性能目標，因此MPC已成為車輛控制的主要工具。 **控制器可以明確地應用于車輛及其子系統的線性或非線性模型，這意味著即使模型隨時間變化或改進，也可以使用相同的方法。

當然MPC與其它控制方法相比，其缺點則是需要更多的計算資源。

---

4.2 Receding Horizon Control

在開始設計MPC控制器之前，讓我們仔細研究一下Receding Horizo??n的概念。Receding Horizon Control解決了每個時間步長上的固定大小優化問題，該問題根據目標約束和車輛的當前狀態確定從當前時間到horizon結束要應用的最佳控制輸入。然而，在實現中出現的一個問題是，由于優化可能需要花費一段時間，因此開始優化時的車輛狀態將不同于優化完成時的車輛狀態。

因此，我們必須在優化中使用預測狀態來做為實際控制輸入。現在我們定義一些術語符號：

• 首先，我們定義receding horizon長度$T$。
• 然后，我們將優化初始狀態設置為在$t$時刻的$x$優化結束時的預測狀態。
• 接下來，我們使用上次優化中確定的控制輸入$u_{t?1}$，在車輛從時間$t$的當前狀態$x_{t?1}$移動到$t$時刻。
• 最后，我們應用receding horizon優化的第一時間步驟中的控制信號，并為下一個時間步驟重復該過程。
  

---

4.3 MPC Structure

我們可以使用下方左圖來可視化MPC結構。

我們有兩個主要模塊，一個是優化器模塊，它是核心MPC組件，另一個是動態模型。 該模型從時間$t$減去一開始獲取過去的輸入和狀態，并獲取優化器的輸出，該輸出是地平線上每個時間步長的輸入$U$的當前序列。 然后，該模型在下一個時間步輸出預測狀態，將其與參考軌跡進行比較，并作為預測誤差傳遞到優化器中。

優化器還接收更新的約束和要使用的成本函數，這些約束可以預先確定，也可以根據運行模式的變化而變化。 優化器解決其優化問題，然后重復該過程。

現在，讓我們看一下線性MPC。

• 我們使用狀態空間來表示離散形式的運動模型。 未來狀態$x_{t+1}$與當前狀態$x_t$和執行器信號$u_t$線性相關。 注意，$A$和$B$是系數矩陣，并且被假定為是不變的。
  $$x_{t+1}=A{x}_t+B{u}_t$$
• MPC試圖找到輸入的控制策略$U$。
  $$U=\{u_{t|t},u_{t+1|t},u_{t+2|t},???\}$$
• 目標函數可以定義為：
  $$J(x(t),U)=\sum _{j=t}^{t+T?1}{x}_{j|t}^{T}Q{x}_{j|t}+u_{j|t}^{T}R{u}_{j|t}$$

在從零到非零的控制輸入的狀態偏差上具有二次誤差，類似于最優控制理論的優化問題，并在控制性能和輸入積極性之間進行權衡。 注意，矩陣Q和R稱為權重矩陣，可以進行適當選擇以實現特定類型的響應。

相反，如果我們需要跟蹤參考信號（例如所需軌跡），則可以修改公式以包括相對于所需狀態的誤差增量$x$。
$$\begin{gathered} \delta x_{j|t}=x_{j|t,des}?x_{j|t} \\ J(x(t),U)=\sum _{j=t}^{t+T?1}{x}_{j|t}^{T}Q{x}_{j|t}+u_{j|t}^{T}R{u}_{j|t} \end{gathered}$$

下面我們給出其對應的優化公式，是一個具有封閉形式的解決方案，即線性二次調節器或LQR。 封閉式解決方案使用完整狀態反饋，這意味著在控制響應中使用了所有狀態。 LQR解決方案定義了一個控制增益矩陣$K$，該矩陣可以根據狀態空間模型的$A$和$B$矩陣以及成本函數的$Q$和$R$矩陣進行計算。
$$\begin{gathered} \underset{U?\{u_{t\mid t}{u}_{t+1\mid t,\dots \}}}{\min }J(x(t),U)=x_{t+T\mid t}^{\mathrm{T}}{Q}_f{x}_{t+T\mid t}+\sum _{j=t}^{t+T?1}{x}_{j\mid t}^{\mathrm{T}}Q{x}_{j\mid t}+u_{j\mid t}^{\mathrm{T}}R{u}_{j\mid t} \\ s.t.x_{j+1\mid t}=A{x}_{t\mid t}+B{u}_{t\mid t},t\le j\le t+T?1 \end{gathered}$$

現在讓我們看一下非線性MPC：

在更一般的情況下，其目標函數是狀態和receding horizon輸入的任何可微的非線性函數。 對于這樣的一般優化問題，不存在封閉形式的解決方案。 因此，我們必須依靠數值優化來找到解決方案。

---

4.4 Vehicle Lateral Control

現在，讓我們看一下MPC控制器在自動駕駛汽車上進行軌跡跟蹤的效果。

MPC與之前介紹的反饋結構相同，但我們將油門，制動和轉向命令作為回路中的低級控制器輸出。 MPC控制器的輸入，包括參考路徑和速度以及每個時間步的車輛狀態。 MPC控制器的輸出是遵循所需軌跡所需的橫向和縱向力。 之后低級控制器轉換為相應的油門，斷開和轉向命令，最后，在每個時間步長上將操縱信號施加到車輛上，并通過反饋回路獲得新的車輛狀態。

在這里我們定義了跟蹤所需軌跡的成本函數，包括最小化與所需軌跡的偏差和控制命令幅度。接下來，我們根據先前視頻中開發的橫向和縱向模型定義車輛的運動約束。我們還對輪胎力施加最大限制，以將其限制在線性輪胎區域內。

現在讓我們看一下控制效果。

對雙車道變更場景進行仿真，右圖顯示了帶有MPC控制的模擬操縱結果，參考軌跡為藍色，實際車輛軌跡為紅色。我們可以看到，MPC控制器的跟蹤性能非常好，略有滯后，但沒有超調或振蕩。可以看到模型預測控制器的輸出（橫向和縱向力）平滑變化。

同樣，繪制了在雙車道變更操縱期間的車輛偏航率。

---

總結，在本講我們學習了：

• 如何定義橫向路徑跟蹤問題；
• 如何將pure pursuit跟蹤和Stanley控制方法；
• MPC控制方法。

總結

以上是生活随笔為你收集整理的Coursera自动驾驶课程第7讲：Vehicle Lateral Control的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。