自動駕駛控制算法——LQR控制算法

一、LQR 是什么？

Linear Quadratic Regulator（線性二次型調節器，簡稱 LQR）是一類基于狀態反饋的最優控制算法。
它針對“線性系統 + 二次型代價函數”這對經典組合，自動給出一組最優反饋增益，從而在誤差最小化與控制能量最小化之間取得全局最優折中。
在自動駕駛中，LQR 被廣泛用于橫向/縱向軌跡跟蹤、車道保持（LKA）、穩定性控制（VSC）、智能底盤協同控制等場景，兼具實時性、多變量耦合處理能力與理論最優性。

二、LQR 原理

2.1 線性狀態空間模型 (State–Space Model)

對連續時間系統，令

$\dot{x}(t)=Ax(t)+Bu(t)$

• $x(t)\in {\mathbb{R}}^n$：狀態向量（位置、姿態、速度、誤差等）
• $u(t)\in {\mathbb{R}}^m$：控制輸出（轉向角、驅動力、制動力等）
• $A$、$B$：系統動力學矩陣（可由自行車模型、三自由度模型或實驗辨識而得）

離散時間版本（采樣周期 $\Delta t$）同理：

$x_{k+1}=A_d{x}_k+B_d{u}_k$

2.2 二次型性能指標 $J$

設計目標：在 $t\in [0,\infty )$ 內最小化

$J={\int }_0^{\infty }(x^{\mathrm{T}}(t)Qx(t)+u^{\mathrm{T}}(t)Ru(t))dt$

記號	物理含義	工程調節思路
$Q?0$	狀態加權矩陣：懲罰偏離參考軌跡	誤差越關鍵，賦值越大
$R?0$	控制加權矩陣：懲罰控制能耗/激烈度	不希望頻繁/劇烈操舵就增大 $R$

2.3 代數黎卡提方程 (ARE)

最優解存在唯一正定矩陣 $P$，滿足

$A^{\mathrm{T}}P+PA?PB{R}^{?1}{B}^{\mathrm{T}}P+Q=0$

得到最優反饋增益

$K=R^{?1}{B}^{\mathrm{T}}P?u(t)=?Kx(t)$

2.4 特點總結

1. 最優性：對給定 $A,B,Q,R$ 全局最優；無局部極值困擾。
2. 多變量耦合：一次性考慮所有狀態與輸入，天然處理耦合。
3. 實時性高：上線前離線求 $K$，在線只需矩陣乘法。
4. 線性假設：需在工作點線性化；大幅偏離或強非線性場景下效果下降。
5. 無顯式約束：不直接處理飽和與障礙，需要配合限幅或外層規劃器。

2.5 一句話總結 LQR 原理：

LQR 就像一個“聰明的爸爸”，手里拽著你（小孩）一根繩子，在奔跑中努力讓你既不跑偏，又不會猛拉你太用力。

---

2.5.1 場景類比：小孩牽繩跑步

假設你（小孩）在一個廣場上跑步，有一個目標路線（比如一條藍色的直線軌跡），你爸（控制器）拽著你一根彈力繩，任務是讓你盡量沿著這條軌跡跑。你爸不能直接控制你，但可以通過拉扯繩子的方式調整你的方向。

現在他面臨兩個問題：

1. 你偏離軌跡越多（狀態誤差大），他就想越用力拉你回來
2. 但用力過猛你會摔倒（控制動作大 → 不舒適），他也不想那樣

于是他就想著：

“我要最優地拉這根繩子，讓我家孩子既快速回到軌跡，又不至于每次都摔個大跟頭。”

這時候他腦子里其實就運行著一個 LQR 控制器。

---

2.5.2 數學意義下的類比映射：

生活場景	控制系統術語
你的位置與方向	狀態向量 $x(t)$
你爸的拉力	控制輸入 $u(t)$
你和軌跡的偏離	狀態誤差（需要懲罰）
拉繩子的幅度大小	控制開銷（也要懲罰）
你爸的聰明程度	增益矩陣 $K$，控制律 $u=?Kx$
怎么權衡糾偏 vs 用力	懲罰矩陣 $Q,R$，定義什么是“好”
目標	讓總代價函數 $J$ 最小（偏差小 + 控制平滑）

2.5.3 更直白一點的解釋：

LQR 的工作流程可以理解為以下四步：

① 系統動態是什么？

你是個小車，你能跑、能轉彎，爸爸知道你每跑一步、每拐一個彎，會跑到什么位置 → 系統模型：

$\dot{x}(t)=Ax(t)+Bu(t)$

---

② 什么樣的軌跡才叫“好”？

當然是既偏差小，又別太用力操控你：

$J={\int }_0^{\infty }\left[x^{T}Qx+u^{T}Ru\right]dt$偏差太大 $x^{T}Qx$ 會被罰

• 控制太猛 $u^{T}Ru$也要罰
• 怎么罰？用矩陣 $Q,R$ 來定義，自己調！

---

③ 怎么做才最優？

解一個叫“黎卡提方程”的東西，得到一個增益 $K$，然后只要你一偏離，控制器就自動拉回：

$u(t)=?Kx(t)$

這個反饋矩陣 $K$ 就是控制器的“聰明勁兒”。

---

④ 最終結果如何？

• 如果你設計得好，系統運行起來就像小車沿軌道平穩前進，輕松糾偏
• 如果你太注重精度（$Q$太大），控制器可能瘋狂操作
• 如果你太怕費力（$R$太大），小車就變得慵懶、不追軌了

---

🖼? 圖像化理解你可以把 LQR 理解為：“我告訴系統什么是‘好’，它就自己計算出‘怎么做’”。

---

總結關鍵思想

關鍵詞	意義
誤差反饋	LQR 直接用當前誤差狀態來反饋控制
最優折中	不只糾錯，還要代價最小
增益矩陣	控制器的大腦，自動學會怎么拉得最好
提前離線求解	控制器上線后實時快如閃電（僅乘法）

2.6 案例分析 —— 車輛軌跡跟蹤

2.6.1 任務介紹

• 目標：讓車輛以 15 m/s 的恒速精準沿參考曲線（S 形）行駛，最小化橫向誤差 $e_y$ 和航向誤差 $e_{\psi }$。
• 模型：采用簡化自行車模型并在小角度、恒速條件下線性化。

2.6.2 控制流程

1. 實時感知車輛位置與姿態 → 計算誤差向量 $x=[e_y,{\dot{e}}_y,e_{\psi },{\dot{e}}_{\psi }{]}^{\mathrm{T}}$。

2. 狀態反饋：控制器輸出前輪轉角

   $\delta =?Kx$

3. 車輛執行 → 新狀態回饋 → 進入下一控制周期。

2.6.3 參數調優現象觀察

• 增大 $Q_{(e_y,e_{\psi })}$：曲線跟隨更緊，響應更快，但控制動作更激進，可能出現抖動。
• 增大 R：轉向更平滑、舒適，跟蹤誤差增大，可能在急彎處外偏。
• 平衡策略：先用 Bryson’s Rule 給初值，再基于仿真/現場試驗迭代微調。

2.6.4 不同 Q/R 組合的典型效果

方案	Q(diag)	R	穩態誤差	峰值轉角	舒適性	評述
A	$[10,1,10,1]$	0.1	0.05 m	3.2°	★★☆	誤差小，操舵略頻繁
B	$[2,0.5,2,0.5]$	1.0	0.15 m	1.1°	★★★	舒適度佳，誤差可接受
C	$[20,2,20,2]$	0.05	0.02 m	5.5°	★☆☆	極致精度，易飽和

圖中紅色曲線展示了在**非線性車輛模型（Kinematic Bicycle Model）**下，LQR 控制器如何調整前輪轉角，使車輛從偏離狀態逐步回到并跟蹤 S 型參考軌跡（黑色虛線）。

這說明即使模型是非線性的，基于線性近似設計的 LQR 控制器也能取得良好的控制效果，尤其在中等速度、路徑曲率適中的場景下非常適用。

2.7 LQR 參數解讀

2.7.1 常用術語

術語	含義
$P$	黎卡提方程解，衡量未來代價的“勢能”
$K$	狀態反饋增益矩陣
Closed-Loop Poles	閉環特征值，決定響應快慢與穩定性

2.7.2 權重矩陣 $Q,R$對性能的影響

• 大 $Q$ / 小 $R$：強調跟蹤精度 → 響應快、可能過沖。
• 小 $Q$ / 大 $R$：強調控制平滑 → 誤差大、舒適性高。

2.7.3 關鍵性能指標

• 穩態誤差
• 超調量 / 峰值轉角
• 90% 調整時間
• 控制能量 $\int u^{2}dt$

2.7.4 計算與信號流程圖

感知 → 計算狀態誤差 → LQR 內核（讀 $K$ 表 → 乘以 ?$K$）→ 輸出控制 → 車輛執行 → 反饋環。 （工程實現中常封裝成 ROS 控制節點或嵌入式任務）

2.7.5 典型作用總結

• 在緊急避障前提供平穩高精度跟蹤。
• 可作為 MPC 的終端控制器或非線性控制器的局部線性近似補償器。
• 在低速或大曲率場景下，可結合速度規劃降低側向加速度峰值。

2.7.6 設計準則與物理量對應關系

物理量/需求	建議權重調整
橫向誤差 ≤ 0.1 m	提高 $Q_{e_y}$
航向誤差 ≤ 0.5°	提高 $Q_{e_{\psi }}$
乘坐舒適度	提高 $R$ 以限制劇烈操舵
轉角飽和	事先加輸入限幅或適當增大 $R$
低抓地/濕滑路面	同時增大 $Q,R$并在外層降速

三、離散 LQR 推導（DLQR）

3.1 離散狀態空間模型

$x_{k+1}=A_d{x}_k+B_d{u}_k,k=0,1,\dots$

• $x_k\in {\mathbb{R}}^n$：第 $k$ 步系統狀態
• $u_k\in {\mathbb{R}}^m$：控制輸入
• $A_d,B_d$：離散化后的系統矩陣（可由零階保持 $A_d=e^{A\Delta t},B_d={\int }_0^{\Delta t}{e}^{A\tau }d\tau B$ 獲得，或直接辨識/數值近似）

3.2 無限時域二次型性能指標

$J={\sum }_{k=0}^{\infty }(x_k^{?}Q{x}_k+u_k^{?}R{u}_k),Q?0,R?0$

有限時域（Horizon NNN）只需將求和上限改為 $N?1$，末端加終端權 $x_N^{?}S{x}_N$。

3.3 動態規劃推導

1. 定義成本到終點（Cost-to-Go）函數

   $J_k(x_k)={\sum }_{i=k}^{\infty }(x_i^{?}Q{x}_i+u_i^{?}R{u}_i)$

2. Bellman 最優性原理

   $J_k(x)={\min }_u\left[x^{?}Qx+u^{?}Ru+J_{k+1}(A_{d}x+B_{d}u)\right]$

3. 二次型猜想 設$J_k(x)=x^{?}{P}_{k}x$，則遞推得到$P_k=Q+A_d^{?}{P}_{k+1}{A}_d?A_d^{?}{P}_{k+1}{B}_d(R+B_d^{?}{P}_{k+1}{B}_d{)}^{?1}{B}_d^{?}{P}_{k+1}{A}_d$_

4. 最優控制律**$u_k^{?}=?K_k{x}_k,K_k=(R+B_d^{?}Pk+1B_d{)}^{?1}{B}_d^{?}Pk+1A_d$

5. 無限時域收斂：若 $(A_d,B_d)$ 穩定可控且$(A_d,Q^{1/2})$ 可觀，則存在極限

   $P_k\underset{k\to \infty }{\to }{P}_{\infty }$

   其滿足離散代數黎卡提方程（DARE）

   $P=A_d^{?}P{A}_d?A_d^{?}P{B}_d(R+B_d^{?}P{B}_d{)}^{?1}{B}_d^{?}P{A}_d+Q$

   得到 恒定最優增益

   $K=(R+B_d^{?}P{B}_d{)}^{?1}{B}_d^{?}P{A}_d$

數值求解

• Scipy：scipy.linalg.solve_discrete_are(A_d,B_d,Q,R)
• MATLAB：[K,P,~] = dlqr(A_d,B_d,Q,R)
• 若做有限時域控制，可從終端條件 $P_N=S$ 向前遞推（Backward Riccati Sweep）。

3.4 DLQR 與 CLQR 的對應關系

連續 LQR	離散 LQR
ARE：$A^{?}P+PA?PB{R}^{?1}{B}^{?}P+Q=0$	DARE 上式
控制律：$u=?Kx,K=R^{?1}{B}^{?}P$	同型：$K=(R+B^{?}PB{)}^{?1}{B}^{?}PA$
解析穩定域靠 ARE 正定解	解析穩定域靠 DARE 正定解

四、iLQR（Iterative LQR）擴展

動機：LQR 只適用于線性近似；真實車輛模型$x_{k+1}=f(x_k,u_k)$ 往往非線性。
思路：在給定控制序列附近迭代線性化 + 二次化，每輪都做一次“時變 LQR” → 前向滾動更新 → 直至收斂。
本質上是 DDP（Differential Dynamic Programming）的簡化實現，計算量更輕，且易于嵌入 MPC/規劃器。

4.1 問題定義

• 非線性動力學：$x_{k+1}=f(x_k,u_k)$
• 分段代價：${?}_k(x_k,u_k)$，終端代價 ${?}_f(x_N)$
• 目標：$\underset{u_{0:N?1}}{\min }\sum _{k=0}^{N?1}{?}_k+{?}_{f}u0:N?1$

4.2 算法框架

1. 初始化：選一條可行控制序列 {uk(0)}\{u_k^{(0)}\}{uk(0)?}，正向仿真得到軌跡 {xk(0)}\{x_k^{(0)}\}{xk(0)?}。

2. 迭代循環 ($t=0,1,2,\dots$)：

   • 線性化：對每個步長 $k$

     $\delta x_{k+1}=A_k\delta x_k+B_k\delta u_k,A_k={\frac{?f}{?x}|}_{(x_k^{(t)},u_k^{(t)})},B_k={\frac{?f}{?u}|}_{(?)}$

   • 二次化代價：泰勒展開 ${?}_k$ 得二次型 $\delta x,\delta u$。

   • 后向遍歷（LQR backward pass）：求時變增益 $K_k^{(t)},k_k^{(t)}$（反饋 + 前饋）。

   • 前向滾動：更新控制

     $u_k^{(t+1)}=u_k^{(t)}+{\alpha }^{(t)}{k}_k^{(t)}+K_k^{(t)}(x_k^{(t+1)}?x_k^{(t)})$

     并重新仿真得 {xk(t+1)}\{x_k^{(t+1)}\}{xk(t+1)?}

   • 線搜索：調節 ${\alpha }^{(t)}\in (0,1]$ 滿足代價下降。

   • 收斂判據：$|J^{(t+1)}?J^{(t)}|<\epsilon$ 或迭代次數達上限。

4.3 簡化偽代碼

Input: f, ?, ?_f, horizon N, x0
Initialize U ← {u0,...,u_{N-1}} (zeros or heuristic)
while not Converged doX ← ForwardRollout(f, x0, U){A_k,B_k,?_x,?_u,?_xx,?_uu,?_ux} ← LinearizeAndQuadratize(X,U){K_k,k_k} ← BackwardPass(A,B,?_x,?_u,?_xx,?_uu,?_ux)U_new ← LineSearchAndUpdate(U,K,k)if CostDropSmall then breakU ← U_new
end while
return U, X

4.4 iLQR 與其它方法對比

方法	模型要求	約束處理	收斂性質	工程特點
LQR	線性	不顯式	一步求閉解	最快，局部有效
iLQR	可微非線性	軟約束/罰項	局部收斂	迭代求解，秒級
DDP	同上	軟約束/罰項	二階收斂	需二階導數
MPC	線/非線性	可顯式硬約束	在線優化	計算量大，最靈活

4.5 工程實踐要點

• 采樣周期：iLQR 得到的是開環控制序列；可每 τ\tauτ 秒重解一次，外層加反饋穩健性更佳（Receding Horizon iLQR）。
• 權重與正則：常在二次化階段對 Hessian 加 $\lambda I$ 保證正定。
• 約束：可在代價中加入 barrier / soft-constraint，也可外層投影。
• 軟件：
  • ilqr（Python）、traja/ilqr (基于 JAX)
  • acados 非線性 MPC 內含 iLQR kernel
  • C++ 實時實現可參考 mpc-lqr、pinocchio+croccodile

五、LQR 權重整定方法（參數選擇）

LQR 的設計核心在于選擇合適的權重矩陣 $Q$ 與 $R$，它們反映了控制器對誤差狀態與控制輸入的“偏好程度”。調得好，可以實現高精度+高舒適性；調不好則可能導致過度抖動、飽和、甚至失穩。本節介紹三種常用的權重調參策略。

---

5.1 ? Bryson’s Rule

📌 基本思想

將每個狀態量/控制量的“最大允許值”設為代價函數中的單位懲罰基準。

即：對每個分量設定其最大允許值 $?x_{i,\max }$、$?u_{\max }$，令：

$Q_{ii}=\frac{1}{x_{i,\max }^2},R=\frac{1}{u_{\max }^2}$

🧠 背景原理

• LQR 性能指標函數本質上是：

  $J=\int x^{?}Qx+u^{?}Rudt=\sum \text{(狀態懲罰)}+\text{(控制懲罰)}$

• 若我們將每個量歸一化到它的最大可接受值，那么所有項的權重就統一了 → 得到一個物理量解釋清晰的權重矩陣。

🔧 操作步驟

1. 明確各狀態量最大可接受偏差（如位置偏差 ≤ 0.2 m，偏航角 ≤ 3° 等）

2. 明確控制量最大允許范圍（如最大轉角 ≤ 25°）

3. 代入公式構建權重矩陣

   $Q=\mathrm{diag}\left(\frac{1}{x_1^2},\frac{1}{x_2^2},\dots \right),R=\frac{1}{u_{\max }^2}$

4. 若還需強調某些狀態量，可在此基礎上乘上放大系數（例如橫向位置乘以 5）。

🚗 自動駕駛應用案例

物理量	變量	允許最大值	權重項
橫向誤差	$e_y$	0.2 m	$Q_{1,1}=25$
航向角誤差	$e_{\psi }$	3° = 0.052 rad	$Q_{2,2}\approx 369$
轉向角	$\delta$	25° = 0.436 rad	$R=5.26$

5.2 🛠? Trial-and-Error 調參法

📌 基本思想

基于仿真/試驗觀察系統響應曲線，手動增減 $Q$/$R$ 中元素逐步調整控制行為。

🧠 背景原理

• LQR 是解析型反饋控制，其性能高度依賴于代價函數的構型
• 直接通過增大/減小某一項權重，可以預測性地影響系統的響應曲線特征（如超調、響應時間、震蕩等）

🔧 操作步驟

1. 初始設置（例如單位矩陣）：

   $Q=I,R=1$

2. 仿真運行系統，看如下響應指標：

   • 超調量
   • 穩態誤差
   • 峰值控制量
   • 抖動/頻繁操作

3. 觀察控制趨勢：

   • 跟蹤不準？→ 提高 $Q$ 的關鍵項
   • 抖動太大？→ 提高 $R$

4. 循環調整至滿足性能指標或舒適性要求

🛠 工具推薦

• Python + matplotlib + control 仿真框架
• MATLAB Simulink + scope 曲線可視化
• ROS 中動態配置 K 調參并采集駕駛行為數據

🚗 自動駕駛應用案例

觀察現象	處理方法
跟蹤誤差大	增加 $Q_{e_y},Q_{e_{\psi }}$
控制頻繁跳變	增加 $R$
航向角偏移慢	增加 $Q_{e_{\psi }}$
進入彎道偏移大	減小 $R$，提升反應能力

5.3 📐 LMI-Based 自動整定

📌 基本思想

將 LQR 權重選擇問題轉化為一個線性矩陣不等式（LMI）優化問題，自動尋找滿足性能指標下的最優權重。

🧠 背景原理

• LQR 本質是穩定性約束下的最優控制 → 可在 Lyapunov 函數約束下構建如下 LMI：

  $?\begin{array}{l}{A}_c=A?BK\text{穩定}\\ V(x)=x^{?}Px,P?0\\ \text{最小化?}\mathrm{tr}(QP)\end{array}$

• 通過控制 Lyapunov 函數下降速率 + 狀態/控制限幅約束，可自動搜索 $Q,R$ 滿足性能指標

🔧 操作步驟

1. 明確系統模型 $(A,B)$ 與性能指標（如響應時間、能耗上限等）
2. 將問題轉化為優化形式（minimize 某種 cost, subject to LMI）
3. 使用 LMI 優化工具（如 CVX, YALMIP）進行數值求解
4. 從求得的 $P$ 與優化變量中反推 $K$、$Q,R$

🛠 工具推薦

• MATLAB + YALMIP + SeDuMi
• Python + CVXPY（支持 SDP）
• SciML Ecosystem（Julia）也支持非線性 SDP

🚗 自動駕駛應用案例

• 自動整定用于滿足“最大橫向加速度不超過 2.5m/s2”約束
• 控制能量最小化 → 節能路徑跟蹤控制器
• 系統穩定裕度 ${\lambda }_{\min }(P)$ 最大化設計

六、python實現代碼

import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import solve_continuous_are# ----------------------------
# 1. 定義線性系統模型
# ----------------------------
# 狀態空間模型：dx/dt = A x + B u
A = np.array([[0, 1],[0, 0]])              # 雙積分器模型（位置+速度）
B = np.array([[0],[1]])                # 控制輸入為加速度# ----------------------------
# 2. 設置 LQR 權重矩陣
# ----------------------------
Q = np.array([[10, 0],             # 懲罰位置誤差更重[0, 1]])             # 懲罰速度誤差次之
R = np.array([[1]])                # 控制成本# ----------------------------
# 3. 求解黎卡提方程并計算最優反饋增益 K
# ----------------------------
P = solve_continuous_are(A, B, Q, R)         # 解代數黎卡提方程
K = np.linalg.inv(R) @ B.T @ P               # LQR 最優增益print("LQR Gain K =", K)# ----------------------------
# 4. 仿真控制系統響應
# ----------------------------
dt = 0.01                     # 時間步長
T = 5                         # 仿真總時間
steps = int(T / dt)
x = np.array([[1.0], [0.0]])  # 初始狀態：偏離1m，速度為0
trajectory = [x.flatten()]for _ in range(steps):u = -K @ x                # 狀態反饋控制律x_dot = A @ x + B @ u     # 系統動態x += x_dot * dttrajectory.append(x.flatten())trajectory = np.array(trajectory)# ----------------------------
# 5. 可視化結果
# ----------------------------
plt.figure(figsize=(8, 4))
plt.plot(trajectory[:, 0], label="Position $x_1$ (m)")
plt.plot(trajectory[:, 1], label="Velocity $x_2$ (m/s)")
plt.axhline(0, color='gray', linestyle='--')
plt.xlabel("Time Step")
plt.ylabel("State Value")
plt.title("LQR Control Response of a Double Integrator")
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()

小結對比

方法	優點	缺點	場景適配
Bryson’s Rule	簡潔直觀、物理意義明確	粒度粗、僅限對角權重	快速初始化，物理系統強適配
Trial-and-Error	靈活細致、性能可控	需反復仿真，調參經驗依賴重	小規模系統 + 仿真豐富的設計階段
LMI 自動整定	自動最優、支持約束	數學復雜，需 SDP 解器	多目標優化、高安全性應用、復雜耦合系統