上海證券交易所網(wǎng)址,優(yōu)化搜狐的培訓(xùn),寧波seo教程行業(yè)推廣,系統(tǒng)軟件開發(fā)流程基于MPC的智能車運(yùn)動預(yù)測和控制算法 Motion predication; Kinematic model //. MATLAB coding //. 加入求解步驟進(jìn)而得到自定義成本函數(shù)的可擴(kuò)展MPC控制器; //. 模型狀態(tài)空間方程線性化和離散化; //. 可與風(fēng)險場/人工勢場/決策/軌跡跟蹤等算法集成; //. 需要用到車輛運(yùn)動學(xué)模…基于MPC的智能車運(yùn)動預(yù)測和控制算法 Motion predication; Kinematic model //. MATLAB coding //. 加入求解步驟進(jìn)而得到自定義成本函數(shù)的可擴(kuò)展MPC控制器; //. 模型狀態(tài)空間方程線性化和離散化; //. 可與風(fēng)險場/人工勢場/決策/軌跡跟蹤等算法集成; //. 需要用到車輛運(yùn)動學(xué)模型的算法把方向盤交給代碼之前得先讓車知道自己是誰。在停車場倒庫的場景里我們給智能車構(gòu)建了一個自行車模型——別誤會這可不是兩輪自行車而是把四個輪子簡化為前后兩輪的數(shù)學(xué)模型。MATLAB里這么定義狀態(tài)向量states (x) [x(1); x(2); x(3); x(4)]; % X坐標(biāo),Y坐標(biāo),航向角,車速 controls (u) [u(1); u(2)]; % 前輪轉(zhuǎn)角,加速度這個簡化模型暗藏玄機(jī)前輪負(fù)責(zé)轉(zhuǎn)向后輪只管驅(qū)動。實(shí)際調(diào)試時會發(fā)現(xiàn)當(dāng)車速超過30km/h模型誤差開始明顯變大——這時候就得換動力學(xué)模型了但那是另一個故事。要讓MPC跑起來得把連續(xù)時間模型切成時間片。用前向歐拉法離散化時采樣時間Δt的選擇直接決定控制器是近視還是遠(yuǎn)視。舉個栗子dt 0.1; % 100ms控制周期 A_discrete eye(4) A_continuous*dt; B_discrete B_continuous*dt;代碼里的A_continuous來自雅可比矩陣線性化。有趣的是在高速場景下有些團(tuán)隊會給Δt做速度自適應(yīng)但這會讓預(yù)測時域變得不均勻增加QP求解難度。成本函數(shù)就像駕校教練的評分標(biāo)準(zhǔn)。下面這個函數(shù)既懲罰偏離路徑又限制急剎猛打方向function J custom_cost(X, U, ref) path_error sum((X(1:2,:) - ref(1:2,:)).^2); control_smooth sum(diff(U).^2); J 0.8*path_error 0.2*control_smooth; end權(quán)重的調(diào)整堪稱玄學(xué)——某自動駕駛公司工程師透露他們曾用強(qiáng)化學(xué)習(xí)自動調(diào)參結(jié)果訓(xùn)練出瘋狂點(diǎn)頭的加減速策略活像駕校新手。把風(fēng)險場引入MPC時可以在成本項(xiàng)里加勢能梯度。比如遇到突然竄出的行人pedestrian_risk exp(-norm(X(1:2)-ped_pos)/(2*sigma^2)); J J 50*pedestrian_risk;這個50的系數(shù)需要實(shí)際路測校準(zhǔn)太高會導(dǎo)致車輛在空曠路段也蛇形走位太低則可能反應(yīng)不足。完整MPC求解流程在代碼里呈現(xiàn)為循環(huán)優(yōu)化問題。注意看qp求解器的輸入如何隨時間窗滑動for k 1:N % 構(gòu)建QP矩陣 H ... % 二次型矩陣 f ... % 線性項(xiàng) % 添加道路邊界約束 A_ineq [road_constraints; dynamics_constraints]; b_ineq [road_bounds; zeros(dynamics_dim,1)]; [U_opt, fval] quadprog(H, f, A_ineq, b_ineq); % 執(zhí)行第一控制量 apply_control(U_opt(1:2)); end實(shí)際工程中90%的bug出在約束矩陣的維度對齊問題。有團(tuán)隊曾因索引偏移導(dǎo)致車輛畫龍排查三天后發(fā)現(xiàn)是b_ineq少了個轉(zhuǎn)置。當(dāng)集成軌跡跟蹤時會發(fā)現(xiàn)純運(yùn)動學(xué)模型在急彎處容易畫地圖——預(yù)測軌跡和實(shí)際軌跡偏差太大。這時需要引入路徑曲率前饋delta_feedforward atan(wheelbase * curvature); U(1) delta_feedforward delta_feedback;某自動駕駛測試視頻里沒有前饋項(xiàng)的車過彎時像醉漢左右搖擺加上后立刻變得絲滑可見底層控制的小細(xì)節(jié)決定用戶體驗(yàn)。最后留個思考題當(dāng)GPS信號丟失時如何用MPC的預(yù)測狀態(tài)做dead reckoning航位推算答案藏在狀態(tài)觀測器的設(shè)計里下次我們可以聊聊卡爾曼濾波如何與MPC暗通款曲。