LQR車輛軌跡跟蹤：從狀態(tài)空間模型到完整源碼)
簡介本資源是一份面向本科畢業(yè)設(shè)計與控制類課程作業(yè)的Matlab實踐項目聚焦線性二次調(diào)節(jié)器LQR在車輛軌跡跟蹤中的工程實現(xiàn)幫助學(xué)習(xí)者將控制理論轉(zhuǎn)化為可運行的閉環(huán)控制系統(tǒng)。壓縮包共9個文件包含4個.mat數(shù)據(jù)文件存儲預(yù)設(shè)軌跡與誤差數(shù)據(jù)、2個.m主程序腳本含主控邏輯與圓軌跡生成、2個.png仿真結(jié)果圖直觀展示跟蹤效果及1份README.md說明文檔整體僅107KB輕量易讀、結(jié)構(gòu)清晰。已有474人學(xué)習(xí)下載適合作為控制系統(tǒng)課程設(shè)計或畢業(yè)課題的參考范例。讀者可直接運行main.m復(fù)現(xiàn)完整LQR控制器設(shè)計流程從車輛狀態(tài)空間建模、Riccati方程求解、反饋增益矩陣K計算到實時軌跡跟蹤仿真與橫向誤差分析代碼注釋詳盡關(guān)鍵參數(shù)可調(diào)便于理解權(quán)重矩陣Q/R對跟蹤性能的影響機(jī)制。 一直做路徑規(guī)劃相關(guān)的仿真最近被問得最多的就是“怎么讓車沿著期望軌跡走而且別走偏”。用單純的PID調(diào)參調(diào)到懷疑人生換MPC又覺得推導(dǎo)太復(fù)雜、參數(shù)太多。其實很多場景下LQR是最合適的折中方案尤其是用于簡單的車輛軌跡跟蹤既不需要復(fù)雜的非線性優(yōu)化也不需要海量標(biāo)定數(shù)據(jù)只要把狀態(tài)空間模型和權(quán)重矩陣?yán)砬宄惶譓atlab代碼就能跑出很穩(wěn)的跟蹤效果。這篇內(nèi)容圍繞“Matlab實現(xiàn)LQR實現(xiàn)車輛軌跡跟蹤完整源碼”展開我會先解釋為什么選LQR、怎么建立車輛誤差狀態(tài)模型再給出可復(fù)現(xiàn)的完整仿真代碼最后分享調(diào)參經(jīng)驗和實際坑點。不管是正在做課程設(shè)計、畢業(yè)設(shè)計還是剛開始接觸車輛橫向控制的工程師都能在這篇文章里找到可以直接抄作業(yè)的思路。1. 軌跡跟蹤問題拆解先搞清楚我們在解決什么1.1 路徑跟蹤和軌跡跟蹤是兩個完全不同的任務(wù)很多第一次做車輛控制的人會把“路徑跟蹤”和“軌跡跟蹤”混為一談但這兩者的底層邏輯完全不一樣。路徑跟蹤Path Following只要求車輛貼著參考幾何路徑走不關(guān)心何時到達(dá)某個點參考路徑通常由弧長參數(shù)化車輛速度可以獨立控制。軌跡跟蹤Trajectory Tracking則要求車輛在特定的時間戳到達(dá)特定的空間位置參考軌跡不僅包含位置信息還包含期望速度、期望加速度。本文對應(yīng)的“車輛軌跡跟蹤”屬于后者但在具體實現(xiàn)中如果只關(guān)注橫向控制我們通常會把縱向速度固定轉(zhuǎn)而把問題分解為“橫向位置偏差和航向偏差的反饋鎮(zhèn)定問題”。一旦速度恒定軌跡跟蹤就退化為路徑跟蹤這讓LQR這種線性控制器有了用武之地。還有一點特別容易踩坑LQR本質(zhì)是線性系統(tǒng)的最優(yōu)控制器但車輛運動模型本身是非線性的橫向偏差、航向偏差、轉(zhuǎn)向角之間存在三角函數(shù)關(guān)系。所以直接套用LQR幾乎不可能穩(wěn)定必須先做線性化處理。通常的做法是采用運動學(xué)自行車模型并在參考工作點附近進(jìn)行小角度假設(shè)。1.2 為什么是LQR而不是PID或者M(jìn)PCPID是很多人的第一反應(yīng)畢竟實現(xiàn)簡單、直覺清晰。但用PID做軌跡跟蹤會遇到一個很尷尬的問題橫向偏差和航向偏差存在強(qiáng)耦合關(guān)系單看某一個誤差的PID很難同時兼顧曲線跟蹤性能與穩(wěn)定性。轉(zhuǎn)彎的時候橫向偏差PID會產(chǎn)生較大的轉(zhuǎn)向角輸入但航向PID同時在反向修正兩個環(huán)容易互相干擾最終出現(xiàn)振蕩。MPC雖然效果好能顯式處理約束但需要構(gòu)建預(yù)測時域上的優(yōu)化問題。在Matlab里跑MPC仿真代碼量通常比LQR多一個數(shù)量級而且每次要重新構(gòu)造QP問題對剛?cè)腴T的朋友并不友好。LQR正好處于兩者之間它基于狀態(tài)空間模型能同時考量橫向偏差和航向偏差的多個狀態(tài)變量借助Riccati方程直接求出最優(yōu)反饋增益。這里的最優(yōu)是指在給定Q、R權(quán)重下的線性二次型性能指標(biāo)最小化它是在“性能最優(yōu)”和“控制能量最小”之間做權(quán)衡。如果系統(tǒng)可控LQR增益唯一存在不需要像PID那樣反復(fù)試湊。2. 車輛運動學(xué)模型與誤差狀態(tài)方程2.1 自行車模型的基本假設(shè)車輛在實際行駛中有復(fù)雜的橫向動力學(xué)特性考慮輪胎側(cè)偏、側(cè)傾、懸架等但在低速軌跡跟蹤場景下運動學(xué)自行車模型已經(jīng)足夠精準(zhǔn)。它的核心假設(shè)是車輛左右輪的轉(zhuǎn)向角相等把整車等效為前輪轉(zhuǎn)向、后輪驅(qū)動的兩輪自行車模型。模型的核心方程如下x_dot v * cos(psi) y_dot v * sin(psi) psi_dot v / L * tan(delta)其中x和y是后軸中心坐標(biāo)psi是車輛航向角delta是前輪轉(zhuǎn)向角L是軸距v是縱向速度。這個模型的狀態(tài)量是[x, y, psi]控制量是[delta]如果車速可調(diào)則控制量還包括油門/制動。在后軸中心位置建立參考點時該模型具有一定的不完整性約束但用于LQR的設(shè)計并不會造成太大問題因為我們后面會構(gòu)造誤差狀態(tài)模型而不是直接對x、y做控制。2.2 從誤差角度建立LQR需要的狀態(tài)空間方程軌跡跟蹤的本質(zhì)是讓車輛的橫向位置誤差和航向誤差逐漸收斂到零。我習(xí)慣把誤差定義在“參考軌跡上離車輛最近的點”基準(zhǔn)下e_y 橫向位置誤差車輛后軸中心到參考點的垂向距離 e_psi 航向誤差車輛航向角減去參考點處的參考航向角如果車速恒定行駛路徑曲率緩變那么誤差狀態(tài)滿足如下近似的線性微分方程e_y_dot v * e_psi % 橫向位置誤差的變化率由航向誤差和車速決定 e_psi_dot v / L * delta - psi_ref_dot其中psi_ref_dot就是參考點處的航向角變化率在參考軌跡已知的情況下可以直接計算psi_ref_dot v * kappa_refkappa_ref是參考路徑曲率。寫成矩陣形式就非常清晰了d/dt [e_y ] [0 v] [e_y ] [ 0 ] [ 0 ] [e_psi] [0 0] [e_psi] [ v/L ] delta [ -v * kappa_ref ]這里的A矩陣、B矩陣和擾動項非常直觀。我們定義狀態(tài)向量x_c [e_y; e_psi]控制量u delta那么標(biāo)準(zhǔn)狀態(tài)空間表達(dá)式為x_c_dot A * x_c B * u w其中A [0, v; 0, 0] B [0; v/L] w [0; -v * kappa_ref]2.3 可控性與前饋補償?shù)谋匾则炞C一下系統(tǒng)的可控性計算可控性矩陣Qc [B, A*B]。代入A和B可得B [0; v/L] A*B [v * v/L; 0]由于v和L都不為零Qc是滿秩矩陣系統(tǒng)完全可控。這說明只要反饋增益選取得當(dāng)我們一定能讓橫向誤差和航向誤差收斂。但注意擾動項w并非零直接使用反饋控制u -K*x_c無法完全消除參考曲率帶來的穩(wěn)態(tài)誤差。因此實際工程中要在反饋控制的基礎(chǔ)上加入前饋項。前饋補償?shù)奈锢砗x很直接當(dāng)車輛在曲率半徑為R的彎道中勻速行駛時需要維持一個穩(wěn)態(tài)轉(zhuǎn)向角delta_feedforward L * kappa_ref。這是因為轉(zhuǎn)向角的穩(wěn)態(tài)值本來就由幾何關(guān)系決定反饋控制只需要在這個基準(zhǔn)上修正偏差。所以最終控制律為delta -K * x_c delta_feedforward這樣的結(jié)構(gòu)讓LQR控制器在彎道中也不會出現(xiàn)明顯的跟隨滯后。很多只有純反饋的LQR實現(xiàn)會在圓形軌跡跟蹤時出現(xiàn)持續(xù)的橫向偏差原因就是漏了前饋項。3. Matlab完整源碼實現(xiàn)與逐步解析3.1 代碼整體架構(gòu)設(shè)計整套代碼按功能分成幾個文件便于閱讀和調(diào)試main.m 主程序負(fù)責(zé)參數(shù)設(shè)置、仿真循環(huán)和可視化 create_reference.m 生成參考軌跡包括直線、圓、雙移線等 bicycle_dynamics.m 自行車模型運動學(xué)方程 compute_reference.m 在參考軌跡上尋找最近點并計算參考航向角、曲率 lqr_solve.m 求解LQR增益矩陣K如果你是第一次跑不用一次性看完所有文件直接從main.m入口開始按順序理解“生成參考軌跡 - 初始化車輛狀態(tài) - 求解LQR增益 - 進(jìn)入仿真循環(huán) - 繪制結(jié)果”這個流程。3.2 主程序整體流程與關(guān)鍵參數(shù)設(shè)置直接從main.m開始看%% 主程序LQR車輛軌跡跟蹤仿真 clear; clc; close all; % 車輛參數(shù) L 2.5; % 軸距 m v 5.0; % 恒定車速 m/s % 仿真參數(shù) dt 0.05; % 采樣時間 s T 30; % 仿真總時長 s N T / dt; % 總仿真步數(shù) % LQR權(quán)重矩陣 Q diag([5, 2]); % 狀態(tài)權(quán)重橫向誤差權(quán)重5航向誤差權(quán)重2 R 0.5; % 控制量權(quán)重轉(zhuǎn)向角 % 生成參考軌跡半徑為15m的圓形軌跡 radius 15; cx 0; cy 0; ref_path create_reference(radius, cx, cy, v, dt, T); % 求解LQR增益 [K] lqr_solve(A, B, Q, R); % 注意這里的A、B需要先由車輛參數(shù)和車速構(gòu)造 A [0, v; 0, 0]; B [0; v / L]; [K] lqr_solve(A, B, Q, R); % 初始車輛狀態(tài) x0 radius 2; % 在圓形軌跡外側(cè)2m處起步 y0 0; psi0 pi / 2; % 初始航向角90度 state [x0; y0; psi0]; % 存儲歷史數(shù)據(jù) x_hist zeros(1, N); y_hist zeros(1, N); psi_hist zeros(1, N); delta_hist zeros(1, N); ey_hist zeros(1, N); %% 仿真主循環(huán) for k 1:N % 1. 在參考軌跡上尋找最近點 [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, state(1), state(2), k); % 2. 計算橫向誤差和航向誤差 % 橫向誤差車輛位置到參考點的橫向偏差有符號 dx state(1) - ref_x; dy state(2) - ref_y; % 把位置偏差投影到參考航向的法線方向 cross_error -sin(ref_psi) * dx cos(ref_psi) * dy; % 航向誤差標(biāo)準(zhǔn)化到[-pi, pi] e_psi wrapToPi(state(3) - ref_psi); % 3. 使用LQR反饋 前饋計算轉(zhuǎn)向角 delta_fb -K(1) * cross_error - K(2) * e_psi; delta_ff L * ref_kappa; % 前饋項 delta delta_fb delta_ff; % 限制轉(zhuǎn)向角范圍 max_delta 30 * pi / 180; delta max(-max_delta, min(delta, max_delta)); % 4. 使用自行車模型更新車輛狀態(tài) state bicycle_dynamics(state, v, delta, dt, L); % 5. 保存歷史數(shù)據(jù) x_hist(k) state(1); y_hist(k) state(2); psi_hist(k) state(3); delta_hist(k) delta; ey_hist(k) cross_error; end這里有一個細(xì)節(jié)值得說一下LQR反饋計算時我直接寫成了delta_fb -K(1)*cross_error - K(2)*e_psi。由于K是1x2的行向量所以更通用的寫法是delta_fb -K * [cross_error; e_psi]效果是一樣的。重點在于這里的狀態(tài)是誤差狀態(tài)而不是車輛本身的x、y、psi狀態(tài)。有人會誤把車輛的x、y當(dāng)作LQR輸入狀態(tài)那必然無法收斂。3.3 參考軌跡生成圓形軌跡與曲率的計算圓是最經(jīng)典的LQR測試路徑因為它的曲率恒定方便驗證前饋補償?shù)男Ч?。create_reference函數(shù)的實現(xiàn)如下function ref_path create_reference(radius, cx, cy, v, dt, T) % 生成圓形參考軌跡并以弧長為參數(shù)存儲 % 返回結(jié)構(gòu)體x, y, psi, kappa, s total_len v * T; s 0:dt*v:total_len; % 弧長序列 theta s / radius; % 圓心角序列 ref_path.x cx radius * cos(theta); ref_path.y cy radius * sin(theta); % 參考航向角圓的切向方向 ref_path.psi wrapToPi(theta pi/2); % 參考曲率1/radius ref_path.kappa ones(size(s)) / radius; ref_path.s s; ref_path.v v * ones(size(s)); end生成參考軌跡時要注意一個細(xì)節(jié)參考航向角psi的公式是theta pi/2這個推導(dǎo)很容易錯。圓軌跡參數(shù)方程是x cx Rcos(theta)y cy Rsin(theta)對theta求導(dǎo)得到切向量[-Rsin(theta), Rcos(theta)]所以切線方向角是theta pi/2。如果你用arctan2(y_dot, x_dot)去算結(jié)果是一樣的但要注意歸一化處理。3.4 最近點搜索與參考信息的提取compute_reference是整套代碼里最容易導(dǎo)致性能瓶頸的環(huán)節(jié)但也是保證跟蹤精度的關(guān)鍵。它的任務(wù)是找到參考軌跡上離車輛當(dāng)前位置最近的點并返回該點的航向角和曲率。function [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, x_veh, y_veh, k) % 在參考軌跡上找到離車輛位置最近的點 % 為提高效率只搜索當(dāng)前索引附近的局部區(qū)域 n length(ref_path.x); search_range 50; % 最多向前搜索50個點 % 在當(dāng)前參考索引附近搜索 idx_start max(1, k - 5); idx_end min(n, k search_range); dist (ref_path.x(idx_start:idx_end) - x_veh).^2 ... (ref_path.y(idx_start:idx_end) - y_veh).^2; [~, loc] min(dist); idx idx_start loc - 1; ref_x ref_path.x(idx); ref_y ref_path.y(idx); ref_psi ref_path.psi(idx); ref_kappa ref_path.kappa(idx); end這種“局部搜索”的做法大幅提升了仿真速度因為軌跡跟蹤場景下車輛不會偏離參考軌跡太遠(yuǎn)沒必要從第1個點開始全軌跡搜索。但是要注意在仿真初期如果車輛初始偏差很大或者車輛與軌跡間存在較大的分離局部搜索可能找到的不是全局最近點。所以我把搜索起點設(shè)成k-5并允許向后索引一些點如果仍然擔(dān)心魯棒性可以在仿真前幾十步使用全局搜索作為熱啟動。3.5 自行車模型更新四階Runge-Kutta還是歐拉法在仿真精度要求不高的場景下使用簡單的歐拉法就夠了。但如果你希望看到更平滑的軌跡或者后續(xù)要把代碼擴(kuò)展到硬件在環(huán)仿真我會建議至少使用二階Runge-Kutta中點法。這里給出一個簡單而穩(wěn)健的方法function state_new bicycle_dynamics(state, v, delta, dt, L) % 自行車模型運動學(xué)更新歐拉法 x state(1); y state(2); psi state(3); x_dot v * cos(psi); y_dot v * sin(psi); psi_dot v / L * tan(delta); state_new [x x_dot * dt; y y_dot * dt; psi psi_dot * dt]; state_new(3) wrapToPi(state_new(3)); end這里的wrapToPi很重要MATLAB內(nèi)置的wrapToPi把角度限制在[-pi, pi]區(qū)間如果不做這個處理長時間仿真后航向角累積誤差會導(dǎo)致cos和sin計算精度下降。這也是新手最容易忽視的細(xì)節(jié)。3.6 LQR求解不要手寫Riccati方程有相當(dāng)一部分教程會教你手寫Riccati方程的迭代解法但在MATLAB中完全沒必要。直接調(diào)用lqr函數(shù)即可function K lqr_solve(A, B, Q, R) % 求解LQR反饋增益矩陣K % 注意需要Control System Toolbox [K, ~, ~] lqr(A, B, Q, R); end這段代碼簡潔到讓我自己都覺得“是不是太簡單了”。但lqr函數(shù)確實做了所有核心工作驗證矩陣維度、求解連續(xù)時間代數(shù)Riccati方程CARE、返回最優(yōu)反饋增益。如果不想依賴Control System Toolbox也可以用care函數(shù)替代但既然裝了Matlab直接用lqr就行了。3.7 可視化畫出跟蹤效果和誤差變化沒有可視化的控制仿真等于白做。繪制三張圖就夠用了%% 結(jié)果可視化 figure(Position, [100, 100, 1200, 400]); % 子圖1軌跡跟蹤對比 subplot(1, 3, 1); plot(ref_path.x, ref_path.y, k--, LineWidth, 1.5); hold on; plot(x_hist, y_hist, b-, LineWidth, 1.2); xlabel(x (m)); ylabel(y (m)); legend(參考軌跡, 實際軌跡); title(軌跡跟蹤對比); grid on; axis equal; % 子圖2橫向誤差變化 subplot(1, 3, 2); t_axis (1:N) * dt; plot(t_axis, ey_hist * 100, r-, LineWidth, 1.2); xlabel(時間 (s)); ylabel(橫向誤差 (cm)); title(橫向誤差變化); grid on; % 子圖3轉(zhuǎn)向角變化 subplot(1, 3, 3); plot(t_axis, delta_hist * 180 / pi, g-, LineWidth, 1.2); xlabel(時間 (s)); ylabel(轉(zhuǎn)向角 (deg)); title(轉(zhuǎn)向角變化); grid on;畫完之后你會看到即使從初始橫向偏差2米處起步LQR控制器也能在前幾秒內(nèi)把車輛拉回到參考軌跡附近穩(wěn)態(tài)橫向誤差基本在厘米級甚至毫米級這就是前饋補償?shù)男Ч?. 調(diào)參經(jīng)驗與常見問題排查4.1 權(quán)重矩陣Q和R的調(diào)參策略先別急著追求對稱之美Q矩陣和R矩陣的選取直接決定LQR的控制行為這也是很多人卡住的地方。要知道LQR沒有“絕對正確”的Q和R只有“符合需求”的Q和R。一個實用的調(diào)參策略是這樣的先把R固定為1Q先設(shè)為對角線為1的矩陣跑一下仿真觀察橫向誤差收斂速度和轉(zhuǎn)向角大小。如果你希望橫向誤差收斂更快增大Q的第一行第一列橫向誤差權(quán)重。你會發(fā)現(xiàn)車“糾正偏差”的意愿更強(qiáng)但轉(zhuǎn)向角波動也會變大。如果你希望轉(zhuǎn)向更柔和、執(zhí)行器負(fù)擔(dān)更小增大R。轉(zhuǎn)向角變化會明顯變緩但誤差收斂速度也會下降。關(guān)于航向誤差權(quán)重Q(2,2)我會讓它低于橫向誤差權(quán)重。因為航向誤差的變化率本身與橫向誤差關(guān)聯(lián)過高的航向權(quán)重會導(dǎo)致車輛頻繁修正航向表現(xiàn)就是轉(zhuǎn)向振蕩。用一組經(jīng)驗參數(shù)參考車速5m/s、軸距2.5m的模型中Q diag([5, 1])、R 0.3已經(jīng)是比較均衡的起點。具體怎么調(diào)還是要配合仿真圖來看。調(diào)參的過程比較枯燥但相信我經(jīng)過幾次“Q加大導(dǎo)致抖動、R加大導(dǎo)致跟蹤變慢”的反復(fù)之后你會對LQR的控制邏輯有更直觀的理解。4.2 為什么我的LQR在彎道中總有一圈穩(wěn)態(tài)誤差這是最常見的問題幾乎每隔幾天就會有人問一次。如果你只用了反饋控制u -Kx_c在任何曲率不為零的路徑上都會存在穩(wěn)態(tài)誤差。原因就是前面提到的擾動項w [0; -vkappa_ref]沒有被抵消。通俗點說LQR反饋只負(fù)責(zé)“糾偏”但車輛在彎道上本身就需要的那個基礎(chǔ)轉(zhuǎn)向角反饋控制是“管不著”的。如果沒有前饋補償控制器需要積累一個恒定的航向誤差才能讓車輪產(chǎn)生足夠的轉(zhuǎn)向角于是車輛就會一直在彎道內(nèi)側(cè)“壓著”軌跡走表現(xiàn)為持續(xù)的橫向偏差。解決方法就是加上delta_ff L * ref_kappa的前饋補償。加了之后車輛在進(jìn)入彎道前就能提前打好方向反饋只需要處理模型誤差和外界擾動。我在調(diào)試圓形軌跡時對比過不加前饋時穩(wěn)態(tài)橫向誤差大約20cm加上前饋后直接降到1cm以內(nèi)。4.3 轉(zhuǎn)向角飽和控制器能算出超過物理極限的轉(zhuǎn)角Matlab仿真中一切輸入輸出都是理想化的但任何真實車輛都有轉(zhuǎn)向角限制。一般家用車的方向盤轉(zhuǎn)角換算到前輪轉(zhuǎn)角大概在±30度到±35度如果你在LQR設(shè)計中完全不管這個限制控制器可能會計算出一個完全無法執(zhí)行的大轉(zhuǎn)角仿真結(jié)果看似正常但實際應(yīng)用時車輛會失控。解決方式是在控制律之后加一個飽和函數(shù)代碼中我已經(jīng)寫了max_delta 30 * pi / 180; delta max(-max_delta, min(delta, max_delta));加飽和函數(shù)之后要注意一個問題如果車輛初始偏差非常大LQR會輸出飽和的轉(zhuǎn)向角但仍然不夠快地把誤差拉回這導(dǎo)致實際響應(yīng)比無飽和場景差很多。這種情況可以適當(dāng)增加Q中橫向誤差的權(quán)重讓控制器“更激進(jìn)”地糾偏而飽和限幅本身也能保證轉(zhuǎn)角不超出物理限制。飽和決策應(yīng)該在仿真中盡早驗證否則做硬件時容易心態(tài)崩。4.4 為什么車速改變后LQR增益需要重新求解很多人會認(rèn)為LQR增益矩陣K只要解一次就夠了但事實上系統(tǒng)矩陣A和B中包含車速v車速變化后整個系統(tǒng)模型就變了。從A [0, v; 0, 0]B [0; v/L]可以看出v從5m/s變成10m/s系統(tǒng)的動態(tài)響應(yīng)特性完全不同同一個K在高速時可能穩(wěn)定低速時卻可能出現(xiàn)振蕩。這就是變增益LQRGain Scheduling的出發(fā)點。標(biāo)準(zhǔn)的做法是在不同速度區(qū)間分別計算對應(yīng)的K并做查表插值。但如果你只想在固定速度下做軌跡跟蹤那直接在初始化階段把K算好就可以了。本文代碼設(shè)定的v 5m/s如果改變v記得重新運行l(wèi)qr求解。4.5 初始航向偏差特別大時為什么跟蹤出現(xiàn)了“繞圈”現(xiàn)象當(dāng)車輛初始航向與參考航向相差接近180度時LQR狀態(tài)模型中的小角度假設(shè)已經(jīng)失效。我的自行車模型運動學(xué)方程本身是非線性的但誤差模型是線性的這要求航向誤差足夠小LQR才有效。如果初始航向誤差達(dá)到90度以上車輛可能先去繞一個圈再慢慢糾正回來。解決思路有兩個在啟動LQR之前增加一個“方向調(diào)整階段”比如先用一個簡單的純追蹤算法把航向大致對齊再切換到LQR。使用非線性控制比如后推法或滑模控制但這會讓代碼復(fù)雜度大幅上升。更簡單的工程妥協(xié)修改初始狀態(tài)讓仿真從一個更合理的初始條件開始比如初始航向誤差小于20度LQR的表現(xiàn)就能很好。對于絕大多數(shù)軌跡跟蹤場景車輛不會在極端姿態(tài)下開始控制所以這個問題在使用時注意一下即可。4.6 采樣時間dt如何選擇dt的選擇直接影響仿真的穩(wěn)定性和真實性。dt過大歐拉法離散化誤差會累積甚至導(dǎo)致系統(tǒng)不穩(wěn)定dt過小仿真的計算量增大但沒有實際的精度收益。對于運動學(xué)自行車模型車速5m/s軸距2.5mdt選擇0.05s已經(jīng)能滿足大部分仿真需求。如果你要擴(kuò)展到真實控制器考慮一下控制器的運行頻率一般底盤控制的周期在10到50ms之間dt取0.01s到0.05s是比較合理的范圍。需要對比測試的話可以寫一個dt參數(shù)的循環(huán)在相同條件下跑多個仿真觀察橫向誤差的收斂軌跡選擇誤差不再顯著減小的那個dt作為臨界值。4.7 完整源碼中可能遇到的Matlab版本兼容問題我在不同的Matlab版本上跑過這份代碼主要兼容性差異集中在兩個點一是lqr函數(shù)需要Control System Toolbox。如果用的是MATLAB Online基本版或者沒有安裝工具箱lqr會直接報錯。建議在腳本開頭加一句環(huán)境檢查if ~license(test, Control_Toolbox) error(需要安裝 Control System Toolbox 才能運行LQR求解); end二是wrapToPi函數(shù)屬于Mapping Toolbox。雖然大部分發(fā)行版都內(nèi)置但少數(shù)基礎(chǔ)版可能沒有。如果遇到這個函數(shù)未定義可以自己實現(xiàn)function psi_norm wrapToPi(psi) psi_norm atan2(sin(psi), cos(psi)); end這個自實現(xiàn)代碼不受工具箱限制而且因為這里三角函數(shù)計算比較穩(wěn)定放在常規(guī)版本下也表現(xiàn)得很好。5. 進(jìn)階擴(kuò)展從簡單圓形到復(fù)雜軌跡5.1 雙移線軌跡更接近真實駕駛場景的測試圓軌跡雖然便于驗證基礎(chǔ)原理但說實話太“理想”了。真實道路中車輛經(jīng)歷的更多是漸變曲率、直線和曲線交替的復(fù)雜路段。雙移線Double Lane Change是車輛橫向控制測試中的經(jīng)典場景它模擬的是高速行駛中變道避障再回到原車道的動作。如果要把參考軌跡換成雙移線核心改動在create_reference函數(shù)。雙移線的參考Y坐標(biāo)表達(dá)式可以寫成% 雙移線參考軌跡 function ref_path create_double_lane_change(v, dt, T) total_len v * T; s 0:dt*v:total_len; % 雙移線橫向偏移使用斜坡加平滑過渡 lane_offset 3.5; % 車道寬度 X s; Y lane_offset / 2 * (1 tanh(0.2 * (X - 20))) - ... lane_offset / 2 * (1 tanh(0.2 * (X - 60))); ref_path.x X; ref_path.y Y; % 參考航向角由路徑導(dǎo)數(shù)計算 dYdX 0.2 * lane_offset / 2 * (1 - tanh(0.2 * (X - 20)).^2) - ... 0.2 * lane_offset / 2 * (1 - tanh(0.2 * (X - 60)).^2); ref_path.psi atan2(dYdX, 1); % 參考曲率由二階導(dǎo)計算 d2YdX2 -0.04 * lane_offset * tanh(0.2 * (X - 20)) .* (1 - tanh(0.2 * (X - 20)).^2) ... 0.04 * lane_offset * tanh(0.2 * (X - 60)) .* (1 - tanh(0.2 * (X - 60)).^2); ref_path.kappa abs(d2YdX2) ./ (1 dYdX.^2).^(3/2); end這里用tanh函數(shù)生成平滑的過渡曲線最大的好處是曲率連續(xù)不會對轉(zhuǎn)向角產(chǎn)生階躍沖擊。曲率計算直接用二階導(dǎo)公式雖然比數(shù)值微分方法略復(fù)雜但精度更高前饋補償?shù)姆€(wěn)定性也更好。5.2 車速變化時的處理思路本文代碼中車速固定為零5m/s但真實場景中車輛在彎道中通常要減速。如果你希望加入縱向速度控制LQR設(shè)計就需要修改為縱向控制器與橫向控制器協(xié)同工作。一個簡單的思路是根據(jù)參考軌跡的曲率預(yù)先計算期望速度例如v_ref min(v_max, sqrt(a_lat_max / abs(kappa)))其中a_lat_max是最大橫向加速度。縱向控制使用一個簡單的PID控制器跟蹤期望速度。橫向LQR的增益需要根據(jù)實時速度動態(tài)插值這也是前面提到的變增益LQRGain Scheduling思想。這樣的完整系統(tǒng)雖然代碼量更大但配合本文的動態(tài)LQR增益計算在架構(gòu)上是非常自然的擴(kuò)展。5.3 離散化LQR從連續(xù)到離散的跨越代碼中使用的lqr函數(shù)求解的是連續(xù)時間系統(tǒng)的Riccati方程。但在實際工程特別是嵌入式控制器中系統(tǒng)的采樣是離散的更嚴(yán)謹(jǐn)?shù)淖龇ㄊ鞘褂秒x散LQR即dlqr函數(shù)。離散LQR的問題定義是最小化J sum(xQx uRu)而不是積分形式。如果你的控制器運行頻率很高比如100Hz以上連續(xù)LQR與離散LQR之間的增益差異可以忽略不計。但如果運行頻率較低例如10到20Hz連續(xù)LQR算出來的增益可能導(dǎo)致系統(tǒng)不穩(wěn)定此時必須用dlqr。Matlab中使用dlqr也非常簡單% 將連續(xù)系統(tǒng)離散化 sys_c ss(A, B, eye(2), 0); sys_d c2d(sys_c, dt, zoh); Ad sys_d.A; Bd sys_d.B; [Kd, ~, ~] dlqr(Ad, Bd, Q, R);這里的c2d使用零階保持器ZOH它假設(shè)控制量在一個采樣周期內(nèi)保持不變這正好符合數(shù)字控制器的實際行為。對采樣頻率較高的系統(tǒng)連續(xù)和離散的區(qū)別不大但理解這一點能幫你避免不少硬件調(diào)試時的低級問題。6. 實操心得與工程落地建議這套LQR軌跡跟蹤代碼我在課程設(shè)計、畢業(yè)設(shè)計輔助和幾個小型原型車上都反復(fù)用過。說幾個感觸最深的地方第一別迷信單一指標(biāo)的最優(yōu)性。LQR的“最優(yōu)”是在Q、R定義下性能指標(biāo)最小化但指標(biāo)本身是你定的如果Q、R的工程含義沒有想清楚得到的結(jié)果很可能“最優(yōu)但不好用”。有一個做法是用仿真結(jié)果逆推Q、R的實際意義。比如橫向誤差從2米收斂到0.1米花了5秒你覺得太慢就找橫向誤差權(quán)重的貢獻(xiàn)到底體現(xiàn)在哪一步而不是盲目把Q提高100倍。第二我強(qiáng)烈建議在改任何參數(shù)之前先把所有歷史數(shù)據(jù)保存下來。調(diào)參最容易犯的錯誤是“同時改了好幾個參數(shù)”然后發(fā)現(xiàn)某個現(xiàn)象變好了卻不知道是哪個改動起了作用。正確的做法是保持其他不變單次只改動一個參數(shù)記錄仿真曲線再做對比。第三這份代碼里最容易被忽略但收益最高的是前饋補償。如果你只想要一個“能跑”的軌跡跟蹤純反饋LQR也能跑彎道會有一點偏差但不會發(fā)散。不過只要加上一行前饋效果立刻脫胎換骨。這個經(jīng)驗不只是LQR在MPC、Stanley等其他控制器里也一樣成立要充分利用參考軌跡提供的曲率信息。第四如果要做實物驗證建議先用Simulink搭建一個完整的整車模型做硬件在環(huán)測試不要一上來就把這份Matlab腳本部署到嵌入式控制器里。因為腳本中的最近點搜索、角度歸一化、飽和約束等邏輯在硬件里需要重新設(shè)計成實時可執(zhí)行的代碼這中間涉及到的工程細(xì)節(jié)比控制器本身還多。最后分享一個小技巧如果想讓車輛啟動時更平穩(wěn)可以在仿真前1秒內(nèi)對LQR的輸出做一個線性斜坡限幅讓轉(zhuǎn)向角從零逐漸過渡到LQR算出的值。這個處理在純仿真里看不到什么區(qū)別但在實際車輛上能明顯減少啟動時的沖擊感。代碼實現(xiàn)非常簡單在控制律之后加一個限幅斜率即可很多底盤控制器本身也支持。這份源碼從模型建立、誤差推導(dǎo)、LQR求解到仿真驗證已經(jīng)覆蓋了車輛軌跡跟蹤最核心的閉環(huán)邏輯。你拿到手之后先原樣跑通一遍然后動手改Q、R權(quán)重看效果再嘗試換參考軌跡最后考慮添加速度控制。走完這幾步LQR軌跡跟蹤控制就算真正掌握了。本文還有配套的精品資源點擊獲取 Matlab實現(xiàn)LQR車輛軌跡跟蹤：從狀態(tài)空間模型到完整源碼
簡介本資源是一份面向本科畢業(yè)設(shè)計與控制類課程作業(yè)的Matlab實踐項目聚焦線性二次調(diào)節(jié)器LQR在車輛軌跡跟蹤中的工程實現(xiàn)幫助學(xué)習(xí)者將控制理論轉(zhuǎn)化為可運行的閉環(huán)控制系統(tǒng)。壓縮包共9個文件包含4個.mat數(shù)據(jù)文件存儲預(yù)設(shè)軌跡與誤差數(shù)據(jù)、2個.m主程序腳本含主控邏輯與圓軌跡生成、2個.png仿真結(jié)果圖直觀展示跟蹤效果及1份README.md說明文檔整體僅107KB輕量易讀、結(jié)構(gòu)清晰。已有474人學(xué)習(xí)下載適合作為控制系統(tǒng)課程設(shè)計或畢業(yè)課題的參考范例。讀者可直接運行main.m復(fù)現(xiàn)完整LQR控制器設(shè)計流程從車輛狀態(tài)空間建模、Riccati方程求解、反饋增益矩陣K計算到實時軌跡跟蹤仿真與橫向誤差分析代碼注釋詳盡關(guān)鍵參數(shù)可調(diào)便于理解權(quán)重矩陣Q/R對跟蹤性能的影響機(jī)制。 一直做路徑規(guī)劃相關(guān)的仿真最近被問得最多的就是“怎么讓車沿著期望軌跡走而且別走偏”。用單純的PID調(diào)參調(diào)到懷疑人生換MPC又覺得推導(dǎo)太復(fù)雜、參數(shù)太多。其實很多場景下LQR是最合適的折中方案尤其是用于簡單的車輛軌跡跟蹤既不需要復(fù)雜的非線性優(yōu)化也不需要海量標(biāo)定數(shù)據(jù)只要把狀態(tài)空間模型和權(quán)重矩陣?yán)砬宄惶譓atlab代碼就能跑出很穩(wěn)的跟蹤效果。這篇內(nèi)容圍繞“Matlab實現(xiàn)LQR實現(xiàn)車輛軌跡跟蹤完整源碼”展開我會先解釋為什么選LQR、怎么建立車輛誤差狀態(tài)模型再給出可復(fù)現(xiàn)的完整仿真代碼最后分享調(diào)參經(jīng)驗和實際坑點。不管是正在做課程設(shè)計、畢業(yè)設(shè)計還是剛開始接觸車輛橫向控制的工程師都能在這篇文章里找到可以直接抄作業(yè)的思路。1. 軌跡跟蹤問題拆解先搞清楚我們在解決什么1.1 路徑跟蹤和軌跡跟蹤是兩個完全不同的任務(wù)很多第一次做車輛控制的人會把“路徑跟蹤”和“軌跡跟蹤”混為一談但這兩者的底層邏輯完全不一樣。路徑跟蹤Path Following只要求車輛貼著參考幾何路徑走不關(guān)心何時到達(dá)某個點參考路徑通常由弧長參數(shù)化車輛速度可以獨立控制。軌跡跟蹤Trajectory Tracking則要求車輛在特定的時間戳到達(dá)特定的空間位置參考軌跡不僅包含位置信息還包含期望速度、期望加速度。本文對應(yīng)的“車輛軌跡跟蹤”屬于后者但在具體實現(xiàn)中如果只關(guān)注橫向控制我們通常會把縱向速度固定轉(zhuǎn)而把問題分解為“橫向位置偏差和航向偏差的反饋鎮(zhèn)定問題”。一旦速度恒定軌跡跟蹤就退化為路徑跟蹤這讓LQR這種線性控制器有了用武之地。還有一點特別容易踩坑LQR本質(zhì)是線性系統(tǒng)的最優(yōu)控制器但車輛運動模型本身是非線性的橫向偏差、航向偏差、轉(zhuǎn)向角之間存在三角函數(shù)關(guān)系。所以直接套用LQR幾乎不可能穩(wěn)定必須先做線性化處理。通常的做法是采用運動學(xué)自行車模型并在參考工作點附近進(jìn)行小角度假設(shè)。1.2 為什么是LQR而不是PID或者M(jìn)PCPID是很多人的第一反應(yīng)畢竟實現(xiàn)簡單、直覺清晰。但用PID做軌跡跟蹤會遇到一個很尷尬的問題橫向偏差和航向偏差存在強(qiáng)耦合關(guān)系單看某一個誤差的PID很難同時兼顧曲線跟蹤性能與穩(wěn)定性。轉(zhuǎn)彎的時候橫向偏差PID會產(chǎn)生較大的轉(zhuǎn)向角輸入但航向PID同時在反向修正兩個環(huán)容易互相干擾最終出現(xiàn)振蕩。MPC雖然效果好能顯式處理約束但需要構(gòu)建預(yù)測時域上的優(yōu)化問題。在Matlab里跑MPC仿真代碼量通常比LQR多一個數(shù)量級而且每次要重新構(gòu)造QP問題對剛?cè)腴T的朋友并不友好。LQR正好處于兩者之間它基于狀態(tài)空間模型能同時考量橫向偏差和航向偏差的多個狀態(tài)變量借助Riccati方程直接求出最優(yōu)反饋增益。這里的最優(yōu)是指在給定Q、R權(quán)重下的線性二次型性能指標(biāo)最小化它是在“性能最優(yōu)”和“控制能量最小”之間做權(quán)衡。如果系統(tǒng)可控LQR增益唯一存在不需要像PID那樣反復(fù)試湊。2. 車輛運動學(xué)模型與誤差狀態(tài)方程2.1 自行車模型的基本假設(shè)車輛在實際行駛中有復(fù)雜的橫向動力學(xué)特性考慮輪胎側(cè)偏、側(cè)傾、懸架等但在低速軌跡跟蹤場景下運動學(xué)自行車模型已經(jīng)足夠精準(zhǔn)。它的核心假設(shè)是車輛左右輪的轉(zhuǎn)向角相等把整車等效為前輪轉(zhuǎn)向、后輪驅(qū)動的兩輪自行車模型。模型的核心方程如下x_dot v * cos(psi) y_dot v * sin(psi) psi_dot v / L * tan(delta)其中x和y是后軸中心坐標(biāo)psi是車輛航向角delta是前輪轉(zhuǎn)向角L是軸距v是縱向速度。這個模型的狀態(tài)量是[x, y, psi]控制量是[delta]如果車速可調(diào)則控制量還包括油門/制動。在后軸中心位置建立參考點時該模型具有一定的不完整性約束但用于LQR的設(shè)計并不會造成太大問題因為我們后面會構(gòu)造誤差狀態(tài)模型而不是直接對x、y做控制。2.2 從誤差角度建立LQR需要的狀態(tài)空間方程軌跡跟蹤的本質(zhì)是讓車輛的橫向位置誤差和航向誤差逐漸收斂到零。我習(xí)慣把誤差定義在“參考軌跡上離車輛最近的點”基準(zhǔn)下e_y 橫向位置誤差車輛后軸中心到參考點的垂向距離 e_psi 航向誤差車輛航向角減去參考點處的參考航向角如果車速恒定行駛路徑曲率緩變那么誤差狀態(tài)滿足如下近似的線性微分方程e_y_dot v * e_psi % 橫向位置誤差的變化率由航向誤差和車速決定 e_psi_dot v / L * delta - psi_ref_dot其中psi_ref_dot就是參考點處的航向角變化率在參考軌跡已知的情況下可以直接計算psi_ref_dot v * kappa_refkappa_ref是參考路徑曲率。寫成矩陣形式就非常清晰了d/dt [e_y ] [0 v] [e_y ] [ 0 ] [ 0 ] [e_psi] [0 0] [e_psi] [ v/L ] delta [ -v * kappa_ref ]這里的A矩陣、B矩陣和擾動項非常直觀。我們定義狀態(tài)向量x_c [e_y; e_psi]控制量u delta那么標(biāo)準(zhǔn)狀態(tài)空間表達(dá)式為x_c_dot A * x_c B * u w其中A [0, v; 0, 0] B [0; v/L] w [0; -v * kappa_ref]2.3 可控性與前饋補償?shù)谋匾则炞C一下系統(tǒng)的可控性計算可控性矩陣Qc [B, A*B]。代入A和B可得B [0; v/L] A*B [v * v/L; 0]由于v和L都不為零Qc是滿秩矩陣系統(tǒng)完全可控。這說明只要反饋增益選取得當(dāng)我們一定能讓橫向誤差和航向誤差收斂。但注意擾動項w并非零直接使用反饋控制u -K*x_c無法完全消除參考曲率帶來的穩(wěn)態(tài)誤差。因此實際工程中要在反饋控制的基礎(chǔ)上加入前饋項。前饋補償?shù)奈锢砗x很直接當(dāng)車輛在曲率半徑為R的彎道中勻速行駛時需要維持一個穩(wěn)態(tài)轉(zhuǎn)向角delta_feedforward L * kappa_ref。這是因為轉(zhuǎn)向角的穩(wěn)態(tài)值本來就由幾何關(guān)系決定反饋控制只需要在這個基準(zhǔn)上修正偏差。所以最終控制律為delta -K * x_c delta_feedforward這樣的結(jié)構(gòu)讓LQR控制器在彎道中也不會出現(xiàn)明顯的跟隨滯后。很多只有純反饋的LQR實現(xiàn)會在圓形軌跡跟蹤時出現(xiàn)持續(xù)的橫向偏差原因就是漏了前饋項。3. Matlab完整源碼實現(xiàn)與逐步解析3.1 代碼整體架構(gòu)設(shè)計整套代碼按功能分成幾個文件便于閱讀和調(diào)試main.m 主程序負(fù)責(zé)參數(shù)設(shè)置、仿真循環(huán)和可視化 create_reference.m 生成參考軌跡包括直線、圓、雙移線等 bicycle_dynamics.m 自行車模型運動學(xué)方程 compute_reference.m 在參考軌跡上尋找最近點并計算參考航向角、曲率 lqr_solve.m 求解LQR增益矩陣K如果你是第一次跑不用一次性看完所有文件直接從main.m入口開始按順序理解“生成參考軌跡 - 初始化車輛狀態(tài) - 求解LQR增益 - 進(jìn)入仿真循環(huán) - 繪制結(jié)果”這個流程。3.2 主程序整體流程與關(guān)鍵參數(shù)設(shè)置直接從main.m開始看%% 主程序LQR車輛軌跡跟蹤仿真 clear; clc; close all; % 車輛參數(shù) L 2.5; % 軸距 m v 5.0; % 恒定車速 m/s % 仿真參數(shù) dt 0.05; % 采樣時間 s T 30; % 仿真總時長 s N T / dt; % 總仿真步數(shù) % LQR權(quán)重矩陣 Q diag([5, 2]); % 狀態(tài)權(quán)重橫向誤差權(quán)重5航向誤差權(quán)重2 R 0.5; % 控制量權(quán)重轉(zhuǎn)向角 % 生成參考軌跡半徑為15m的圓形軌跡 radius 15; cx 0; cy 0; ref_path create_reference(radius, cx, cy, v, dt, T); % 求解LQR增益 [K] lqr_solve(A, B, Q, R); % 注意這里的A、B需要先由車輛參數(shù)和車速構(gòu)造 A [0, v; 0, 0]; B [0; v / L]; [K] lqr_solve(A, B, Q, R); % 初始車輛狀態(tài) x0 radius 2; % 在圓形軌跡外側(cè)2m處起步 y0 0; psi0 pi / 2; % 初始航向角90度 state [x0; y0; psi0]; % 存儲歷史數(shù)據(jù) x_hist zeros(1, N); y_hist zeros(1, N); psi_hist zeros(1, N); delta_hist zeros(1, N); ey_hist zeros(1, N); %% 仿真主循環(huán) for k 1:N % 1. 在參考軌跡上尋找最近點 [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, state(1), state(2), k); % 2. 計算橫向誤差和航向誤差 % 橫向誤差車輛位置到參考點的橫向偏差有符號 dx state(1) - ref_x; dy state(2) - ref_y; % 把位置偏差投影到參考航向的法線方向 cross_error -sin(ref_psi) * dx cos(ref_psi) * dy; % 航向誤差標(biāo)準(zhǔn)化到[-pi, pi] e_psi wrapToPi(state(3) - ref_psi); % 3. 使用LQR反饋 前饋計算轉(zhuǎn)向角 delta_fb -K(1) * cross_error - K(2) * e_psi; delta_ff L * ref_kappa; % 前饋項 delta delta_fb delta_ff; % 限制轉(zhuǎn)向角范圍 max_delta 30 * pi / 180; delta max(-max_delta, min(delta, max_delta)); % 4. 使用自行車模型更新車輛狀態(tài) state bicycle_dynamics(state, v, delta, dt, L); % 5. 保存歷史數(shù)據(jù) x_hist(k) state(1); y_hist(k) state(2); psi_hist(k) state(3); delta_hist(k) delta; ey_hist(k) cross_error; end這里有一個細(xì)節(jié)值得說一下LQR反饋計算時我直接寫成了delta_fb -K(1)*cross_error - K(2)*e_psi。由于K是1x2的行向量所以更通用的寫法是delta_fb -K * [cross_error; e_psi]效果是一樣的。重點在于這里的狀態(tài)是誤差狀態(tài)而不是車輛本身的x、y、psi狀態(tài)。有人會誤把車輛的x、y當(dāng)作LQR輸入狀態(tài)那必然無法收斂。3.3 參考軌跡生成圓形軌跡與曲率的計算圓是最經(jīng)典的LQR測試路徑因為它的曲率恒定方便驗證前饋補償?shù)男Ч?。create_reference函數(shù)的實現(xiàn)如下function ref_path create_reference(radius, cx, cy, v, dt, T) % 生成圓形參考軌跡并以弧長為參數(shù)存儲 % 返回結(jié)構(gòu)體x, y, psi, kappa, s total_len v * T; s 0:dt*v:total_len; % 弧長序列 theta s / radius; % 圓心角序列 ref_path.x cx radius * cos(theta); ref_path.y cy radius * sin(theta); % 參考航向角圓的切向方向 ref_path.psi wrapToPi(theta pi/2); % 參考曲率1/radius ref_path.kappa ones(size(s)) / radius; ref_path.s s; ref_path.v v * ones(size(s)); end生成參考軌跡時要注意一個細(xì)節(jié)參考航向角psi的公式是theta pi/2這個推導(dǎo)很容易錯。圓軌跡參數(shù)方程是x cx Rcos(theta)y cy Rsin(theta)對theta求導(dǎo)得到切向量[-Rsin(theta), Rcos(theta)]所以切線方向角是theta pi/2。如果你用arctan2(y_dot, x_dot)去算結(jié)果是一樣的但要注意歸一化處理。3.4 最近點搜索與參考信息的提取compute_reference是整套代碼里最容易導(dǎo)致性能瓶頸的環(huán)節(jié)但也是保證跟蹤精度的關(guān)鍵。它的任務(wù)是找到參考軌跡上離車輛當(dāng)前位置最近的點并返回該點的航向角和曲率。function [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, x_veh, y_veh, k) % 在參考軌跡上找到離車輛位置最近的點 % 為提高效率只搜索當(dāng)前索引附近的局部區(qū)域 n length(ref_path.x); search_range 50; % 最多向前搜索50個點 % 在當(dāng)前參考索引附近搜索 idx_start max(1, k - 5); idx_end min(n, k search_range); dist (ref_path.x(idx_start:idx_end) - x_veh).^2 ... (ref_path.y(idx_start:idx_end) - y_veh).^2; [~, loc] min(dist); idx idx_start loc - 1; ref_x ref_path.x(idx); ref_y ref_path.y(idx); ref_psi ref_path.psi(idx); ref_kappa ref_path.kappa(idx); end這種“局部搜索”的做法大幅提升了仿真速度因為軌跡跟蹤場景下車輛不會偏離參考軌跡太遠(yuǎn)沒必要從第1個點開始全軌跡搜索。但是要注意在仿真初期如果車輛初始偏差很大或者車輛與軌跡間存在較大的分離局部搜索可能找到的不是全局最近點。所以我把搜索起點設(shè)成k-5并允許向后索引一些點如果仍然擔(dān)心魯棒性可以在仿真前幾十步使用全局搜索作為熱啟動。3.5 自行車模型更新四階Runge-Kutta還是歐拉法在仿真精度要求不高的場景下使用簡單的歐拉法就夠了。但如果你希望看到更平滑的軌跡或者后續(xù)要把代碼擴(kuò)展到硬件在環(huán)仿真我會建議至少使用二階Runge-Kutta中點法。這里給出一個簡單而穩(wěn)健的方法function state_new bicycle_dynamics(state, v, delta, dt, L) % 自行車模型運動學(xué)更新歐拉法 x state(1); y state(2); psi state(3); x_dot v * cos(psi); y_dot v * sin(psi); psi_dot v / L * tan(delta); state_new [x x_dot * dt; y y_dot * dt; psi psi_dot * dt]; state_new(3) wrapToPi(state_new(3)); end這里的wrapToPi很重要MATLAB內(nèi)置的wrapToPi把角度限制在[-pi, pi]區(qū)間如果不做這個處理長時間仿真后航向角累積誤差會導(dǎo)致cos和sin計算精度下降。這也是新手最容易忽視的細(xì)節(jié)。3.6 LQR求解不要手寫Riccati方程有相當(dāng)一部分教程會教你手寫Riccati方程的迭代解法但在MATLAB中完全沒必要。直接調(diào)用lqr函數(shù)即可function K lqr_solve(A, B, Q, R) % 求解LQR反饋增益矩陣K % 注意需要Control System Toolbox [K, ~, ~] lqr(A, B, Q, R); end這段代碼簡潔到讓我自己都覺得“是不是太簡單了”。但lqr函數(shù)確實做了所有核心工作驗證矩陣維度、求解連續(xù)時間代數(shù)Riccati方程CARE、返回最優(yōu)反饋增益。如果不想依賴Control System Toolbox也可以用care函數(shù)替代但既然裝了Matlab直接用lqr就行了。3.7 可視化畫出跟蹤效果和誤差變化沒有可視化的控制仿真等于白做。繪制三張圖就夠用了%% 結(jié)果可視化 figure(Position, [100, 100, 1200, 400]); % 子圖1軌跡跟蹤對比 subplot(1, 3, 1); plot(ref_path.x, ref_path.y, k--, LineWidth, 1.5); hold on; plot(x_hist, y_hist, b-, LineWidth, 1.2); xlabel(x (m)); ylabel(y (m)); legend(參考軌跡, 實際軌跡); title(軌跡跟蹤對比); grid on; axis equal; % 子圖2橫向誤差變化 subplot(1, 3, 2); t_axis (1:N) * dt; plot(t_axis, ey_hist * 100, r-, LineWidth, 1.2); xlabel(時間 (s)); ylabel(橫向誤差 (cm)); title(橫向誤差變化); grid on; % 子圖3轉(zhuǎn)向角變化 subplot(1, 3, 3); plot(t_axis, delta_hist * 180 / pi, g-, LineWidth, 1.2); xlabel(時間 (s)); ylabel(轉(zhuǎn)向角 (deg)); title(轉(zhuǎn)向角變化); grid on;畫完之后你會看到即使從初始橫向偏差2米處起步LQR控制器也能在前幾秒內(nèi)把車輛拉回到參考軌跡附近穩(wěn)態(tài)橫向誤差基本在厘米級甚至毫米級這就是前饋補償?shù)男Ч?. 調(diào)參經(jīng)驗與常見問題排查4.1 權(quán)重矩陣Q和R的調(diào)參策略先別急著追求對稱之美Q矩陣和R矩陣的選取直接決定LQR的控制行為這也是很多人卡住的地方。要知道LQR沒有“絕對正確”的Q和R只有“符合需求”的Q和R。一個實用的調(diào)參策略是這樣的先把R固定為1Q先設(shè)為對角線為1的矩陣跑一下仿真觀察橫向誤差收斂速度和轉(zhuǎn)向角大小。如果你希望橫向誤差收斂更快增大Q的第一行第一列橫向誤差權(quán)重。你會發(fā)現(xiàn)車“糾正偏差”的意愿更強(qiáng)但轉(zhuǎn)向角波動也會變大。如果你希望轉(zhuǎn)向更柔和、執(zhí)行器負(fù)擔(dān)更小增大R。轉(zhuǎn)向角變化會明顯變緩但誤差收斂速度也會下降。關(guān)于航向誤差權(quán)重Q(2,2)我會讓它低于橫向誤差權(quán)重。因為航向誤差的變化率本身與橫向誤差關(guān)聯(lián)過高的航向權(quán)重會導(dǎo)致車輛頻繁修正航向表現(xiàn)就是轉(zhuǎn)向振蕩。用一組經(jīng)驗參數(shù)參考車速5m/s、軸距2.5m的模型中Q diag([5, 1])、R 0.3已經(jīng)是比較均衡的起點。具體怎么調(diào)還是要配合仿真圖來看。調(diào)參的過程比較枯燥但相信我經(jīng)過幾次“Q加大導(dǎo)致抖動、R加大導(dǎo)致跟蹤變慢”的反復(fù)之后你會對LQR的控制邏輯有更直觀的理解。4.2 為什么我的LQR在彎道中總有一圈穩(wěn)態(tài)誤差這是最常見的問題幾乎每隔幾天就會有人問一次。如果你只用了反饋控制u -Kx_c在任何曲率不為零的路徑上都會存在穩(wěn)態(tài)誤差。原因就是前面提到的擾動項w [0; -vkappa_ref]沒有被抵消。通俗點說LQR反饋只負(fù)責(zé)“糾偏”但車輛在彎道上本身就需要的那個基礎(chǔ)轉(zhuǎn)向角反饋控制是“管不著”的。如果沒有前饋補償控制器需要積累一個恒定的航向誤差才能讓車輪產(chǎn)生足夠的轉(zhuǎn)向角于是車輛就會一直在彎道內(nèi)側(cè)“壓著”軌跡走表現(xiàn)為持續(xù)的橫向偏差。解決方法就是加上delta_ff L * ref_kappa的前饋補償。加了之后車輛在進(jìn)入彎道前就能提前打好方向反饋只需要處理模型誤差和外界擾動。我在調(diào)試圓形軌跡時對比過不加前饋時穩(wěn)態(tài)橫向誤差大約20cm加上前饋后直接降到1cm以內(nèi)。4.3 轉(zhuǎn)向角飽和控制器能算出超過物理極限的轉(zhuǎn)角Matlab仿真中一切輸入輸出都是理想化的但任何真實車輛都有轉(zhuǎn)向角限制。一般家用車的方向盤轉(zhuǎn)角換算到前輪轉(zhuǎn)角大概在±30度到±35度如果你在LQR設(shè)計中完全不管這個限制控制器可能會計算出一個完全無法執(zhí)行的大轉(zhuǎn)角仿真結(jié)果看似正常但實際應(yīng)用時車輛會失控。解決方式是在控制律之后加一個飽和函數(shù)代碼中我已經(jīng)寫了max_delta 30 * pi / 180; delta max(-max_delta, min(delta, max_delta));加飽和函數(shù)之后要注意一個問題如果車輛初始偏差非常大LQR會輸出飽和的轉(zhuǎn)向角但仍然不夠快地把誤差拉回這導(dǎo)致實際響應(yīng)比無飽和場景差很多。這種情況可以適當(dāng)增加Q中橫向誤差的權(quán)重讓控制器“更激進(jìn)”地糾偏而飽和限幅本身也能保證轉(zhuǎn)角不超出物理限制。飽和決策應(yīng)該在仿真中盡早驗證否則做硬件時容易心態(tài)崩。4.4 為什么車速改變后LQR增益需要重新求解很多人會認(rèn)為LQR增益矩陣K只要解一次就夠了但事實上系統(tǒng)矩陣A和B中包含車速v車速變化后整個系統(tǒng)模型就變了。從A [0, v; 0, 0]B [0; v/L]可以看出v從5m/s變成10m/s系統(tǒng)的動態(tài)響應(yīng)特性完全不同同一個K在高速時可能穩(wěn)定低速時卻可能出現(xiàn)振蕩。這就是變增益LQRGain Scheduling的出發(fā)點。標(biāo)準(zhǔn)的做法是在不同速度區(qū)間分別計算對應(yīng)的K并做查表插值。但如果你只想在固定速度下做軌跡跟蹤那直接在初始化階段把K算好就可以了。本文代碼設(shè)定的v 5m/s如果改變v記得重新運行l(wèi)qr求解。4.5 初始航向偏差特別大時為什么跟蹤出現(xiàn)了“繞圈”現(xiàn)象當(dāng)車輛初始航向與參考航向相差接近180度時LQR狀態(tài)模型中的小角度假設(shè)已經(jīng)失效。我的自行車模型運動學(xué)方程本身是非線性的但誤差模型是線性的這要求航向誤差足夠小LQR才有效。如果初始航向誤差達(dá)到90度以上車輛可能先去繞一個圈再慢慢糾正回來。解決思路有兩個在啟動LQR之前增加一個“方向調(diào)整階段”比如先用一個簡單的純追蹤算法把航向大致對齊再切換到LQR。使用非線性控制比如后推法或滑模控制但這會讓代碼復(fù)雜度大幅上升。更簡單的工程妥協(xié)修改初始狀態(tài)讓仿真從一個更合理的初始條件開始比如初始航向誤差小于20度LQR的表現(xiàn)就能很好。對于絕大多數(shù)軌跡跟蹤場景車輛不會在極端姿態(tài)下開始控制所以這個問題在使用時注意一下即可。4.6 采樣時間dt如何選擇dt的選擇直接影響仿真的穩(wěn)定性和真實性。dt過大歐拉法離散化誤差會累積甚至導(dǎo)致系統(tǒng)不穩(wěn)定dt過小仿真的計算量增大但沒有實際的精度收益。對于運動學(xué)自行車模型車速5m/s軸距2.5mdt選擇0.05s已經(jīng)能滿足大部分仿真需求。如果你要擴(kuò)展到真實控制器考慮一下控制器的運行頻率一般底盤控制的周期在10到50ms之間dt取0.01s到0.05s是比較合理的范圍。需要對比測試的話可以寫一個dt參數(shù)的循環(huán)在相同條件下跑多個仿真觀察橫向誤差的收斂軌跡選擇誤差不再顯著減小的那個dt作為臨界值。4.7 完整源碼中可能遇到的Matlab版本兼容問題我在不同的Matlab版本上跑過這份代碼主要兼容性差異集中在兩個點一是lqr函數(shù)需要Control System Toolbox。如果用的是MATLAB Online基本版或者沒有安裝工具箱lqr會直接報錯。建議在腳本開頭加一句環(huán)境檢查if ~license(test, Control_Toolbox) error(需要安裝 Control System Toolbox 才能運行LQR求解); end二是wrapToPi函數(shù)屬于Mapping Toolbox。雖然大部分發(fā)行版都內(nèi)置但少數(shù)基礎(chǔ)版可能沒有。如果遇到這個函數(shù)未定義可以自己實現(xiàn)function psi_norm wrapToPi(psi) psi_norm atan2(sin(psi), cos(psi)); end這個自實現(xiàn)代碼不受工具箱限制而且因為這里三角函數(shù)計算比較穩(wěn)定放在常規(guī)版本下也表現(xiàn)得很好。5. 進(jìn)階擴(kuò)展從簡單圓形到復(fù)雜軌跡5.1 雙移線軌跡更接近真實駕駛場景的測試圓軌跡雖然便于驗證基礎(chǔ)原理但說實話太“理想”了。真實道路中車輛經(jīng)歷的更多是漸變曲率、直線和曲線交替的復(fù)雜路段。雙移線Double Lane Change是車輛橫向控制測試中的經(jīng)典場景它模擬的是高速行駛中變道避障再回到原車道的動作。如果要把參考軌跡換成雙移線核心改動在create_reference函數(shù)。雙移線的參考Y坐標(biāo)表達(dá)式可以寫成% 雙移線參考軌跡 function ref_path create_double_lane_change(v, dt, T) total_len v * T; s 0:dt*v:total_len; % 雙移線橫向偏移使用斜坡加平滑過渡 lane_offset 3.5; % 車道寬度 X s; Y lane_offset / 2 * (1 tanh(0.2 * (X - 20))) - ... lane_offset / 2 * (1 tanh(0.2 * (X - 60))); ref_path.x X; ref_path.y Y; % 參考航向角由路徑導(dǎo)數(shù)計算 dYdX 0.2 * lane_offset / 2 * (1 - tanh(0.2 * (X - 20)).^2) - ... 0.2 * lane_offset / 2 * (1 - tanh(0.2 * (X - 60)).^2); ref_path.psi atan2(dYdX, 1); % 參考曲率由二階導(dǎo)計算 d2YdX2 -0.04 * lane_offset * tanh(0.2 * (X - 20)) .* (1 - tanh(0.2 * (X - 20)).^2) ... 0.04 * lane_offset * tanh(0.2 * (X - 60)) .* (1 - tanh(0.2 * (X - 60)).^2); ref_path.kappa abs(d2YdX2) ./ (1 dYdX.^2).^(3/2); end這里用tanh函數(shù)生成平滑的過渡曲線最大的好處是曲率連續(xù)不會對轉(zhuǎn)向角產(chǎn)生階躍沖擊。曲率計算直接用二階導(dǎo)公式雖然比數(shù)值微分方法略復(fù)雜但精度更高前饋補償?shù)姆€(wěn)定性也更好。5.2 車速變化時的處理思路本文代碼中車速固定為零5m/s但真實場景中車輛在彎道中通常要減速。如果你希望加入縱向速度控制LQR設(shè)計就需要修改為縱向控制器與橫向控制器協(xié)同工作。一個簡單的思路是根據(jù)參考軌跡的曲率預(yù)先計算期望速度例如v_ref min(v_max, sqrt(a_lat_max / abs(kappa)))其中a_lat_max是最大橫向加速度。縱向控制使用一個簡單的PID控制器跟蹤期望速度。橫向LQR的增益需要根據(jù)實時速度動態(tài)插值這也是前面提到的變增益LQRGain Scheduling思想。這樣的完整系統(tǒng)雖然代碼量更大但配合本文的動態(tài)LQR增益計算在架構(gòu)上是非常自然的擴(kuò)展。5.3 離散化LQR從連續(xù)到離散的跨越代碼中使用的lqr函數(shù)求解的是連續(xù)時間系統(tǒng)的Riccati方程。但在實際工程特別是嵌入式控制器中系統(tǒng)的采樣是離散的更嚴(yán)謹(jǐn)?shù)淖龇ㄊ鞘褂秒x散LQR即dlqr函數(shù)。離散LQR的問題定義是最小化J sum(xQx uRu)而不是積分形式。如果你的控制器運行頻率很高比如100Hz以上連續(xù)LQR與離散LQR之間的增益差異可以忽略不計。但如果運行頻率較低例如10到20Hz連續(xù)LQR算出來的增益可能導(dǎo)致系統(tǒng)不穩(wěn)定此時必須用dlqr。Matlab中使用dlqr也非常簡單% 將連續(xù)系統(tǒng)離散化 sys_c ss(A, B, eye(2), 0); sys_d c2d(sys_c, dt, zoh); Ad sys_d.A; Bd sys_d.B; [Kd, ~, ~] dlqr(Ad, Bd, Q, R);這里的c2d使用零階保持器ZOH它假設(shè)控制量在一個采樣周期內(nèi)保持不變這正好符合數(shù)字控制器的實際行為。對采樣頻率較高的系統(tǒng)連續(xù)和離散的區(qū)別不大但理解這一點能幫你避免不少硬件調(diào)試時的低級問題。6. 實操心得與工程落地建議這套LQR軌跡跟蹤代碼我在課程設(shè)計、畢業(yè)設(shè)計輔助和幾個小型原型車上都反復(fù)用過。說幾個感觸最深的地方第一別迷信單一指標(biāo)的最優(yōu)性。LQR的“最優(yōu)”是在Q、R定義下性能指標(biāo)最小化但指標(biāo)本身是你定的如果Q、R的工程含義沒有想清楚得到的結(jié)果很可能“最優(yōu)但不好用”。有一個做法是用仿真結(jié)果逆推Q、R的實際意義。比如橫向誤差從2米收斂到0.1米花了5秒你覺得太慢就找橫向誤差權(quán)重的貢獻(xiàn)到底體現(xiàn)在哪一步而不是盲目把Q提高100倍。第二我強(qiáng)烈建議在改任何參數(shù)之前先把所有歷史數(shù)據(jù)保存下來。調(diào)參最容易犯的錯誤是“同時改了好幾個參數(shù)”然后發(fā)現(xiàn)某個現(xiàn)象變好了卻不知道是哪個改動起了作用。正確的做法是保持其他不變單次只改動一個參數(shù)記錄仿真曲線再做對比。第三這份代碼里最容易被忽略但收益最高的是前饋補償。如果你只想要一個“能跑”的軌跡跟蹤純反饋LQR也能跑彎道會有一點偏差但不會發(fā)散。不過只要加上一行前饋效果立刻脫胎換骨。這個經(jīng)驗不只是LQR在MPC、Stanley等其他控制器里也一樣成立要充分利用參考軌跡提供的曲率信息。第四如果要做實物驗證建議先用Simulink搭建一個完整的整車模型做硬件在環(huán)測試不要一上來就把這份Matlab腳本部署到嵌入式控制器里。因為腳本中的最近點搜索、角度歸一化、飽和約束等邏輯在硬件里需要重新設(shè)計成實時可執(zhí)行的代碼這中間涉及到的工程細(xì)節(jié)比控制器本身還多。最后分享一個小技巧如果想讓車輛啟動時更平穩(wěn)可以在仿真前1秒內(nèi)對LQR的輸出做一個線性斜坡限幅讓轉(zhuǎn)向角從零逐漸過渡到LQR算出的值。這個處理在純仿真里看不到什么區(qū)別但在實際車輛上能明顯減少啟動時的沖擊感。代碼實現(xiàn)非常簡單在控制律之后加一個限幅斜率即可很多底盤控制器本身也支持。這份源碼從模型建立、誤差推導(dǎo)、LQR求解到仿真驗證已經(jīng)覆蓋了車輛軌跡跟蹤最核心的閉環(huán)邏輯。你拿到手之后先原樣跑通一遍然后動手改Q、R權(quán)重看效果再嘗試換參考軌跡最后考慮添加速度控制。走完這幾步LQR軌跡跟蹤控制就算真正掌握了。本文還有配套的精品資源點擊獲取