自適應(yīng)巡航控制模擬器：從原理到調(diào)參實戰(zhàn))
我做ACC仿真這個方向也算有幾年了剛開始時的確想過直接上現(xiàn)成的整車級仿真平臺但發(fā)現(xiàn)要么太重、要么根本沒法針對控制層做快速迭代。最后我從零搭了一套自適應(yīng)巡航控制ACC系統(tǒng)模擬器用Matlab實現(xiàn)了實時車輛跟隨可視化。這篇文章就把這套模擬器的完整思路、代碼結(jié)構(gòu)和調(diào)參經(jīng)驗全部拆開講清楚希望能給正在做ACC算法驗證、課程設(shè)計或者前期方案評估的人省點時間。1. 為什么自己搭A(yù)CC模擬器而不是直接用整車仿真平臺1.1 市面方案的三個痛點先說明一下我并不是否定現(xiàn)成工具的價值。Simulink里的自動駕駛工具箱能做到四五十個自由度的高保真車輛動力學(xué)仿真配合場景編輯器還能模擬傳感器融合、毫米波雷達(dá)回波、攝像頭目標(biāo)檢測那一整套東西。但這套東西放在我只想驗證ACC控制策略這個場景里大概率是殺雞用牛刀。第一個痛點是學(xué)習(xí)曲線太陡。從場景構(gòu)建、傳感器配置到總線信號映射光是把一條簡單的跟車工況跑通新手可能要折騰兩三天。而我要的其實很簡單前車按給定速度剖面行駛自車通過ACC算法跟隨實時顯示兩車距離和相對速度的變化。用高保真平臺來做這個事大量精力都耗在了與算法驗證無關(guān)的配置上。第二個痛點是模塊黑盒化。整車平臺里帶的一些ACC控制器模塊是封裝好的參數(shù)暴露得有限你很難真正看清控制器的每個中間變量。但做算法研究恰恰需要隨時查看期望加速度、期望間距、誤差項這些量。自己寫代碼就沒有這個問題所有變量都在工作空間里想看哪個看哪個。第三個痛點是計算開銷。跑一次高保真仿真尤其加了復(fù)雜場景和傳感器模型之后速度會明顯放慢。但ACC策略驗證至少需要跑幾十組不同工況的參數(shù)掃描一個工況實時跑不起來的話調(diào)參效率會非常低。1.2 這套模擬器到底解決了什么問題基于上面的考慮我在某個模擬項目X里用Matlab寫了一個輕量級ACC模擬器。它的定位非常明確把精力集中在ACC上層決策算法和下層車輛響應(yīng)特性的模擬上不做復(fù)雜傳感器仿真不做高精度動力學(xué)模型但保證控制策略驗證所需的閉環(huán)鏈路完整。它能做到下面幾件事配置前車的速度剖面包括勻速、勻加速、勻減速、停車再起步等工況用恒定車頭時距策略計算期望間距通過PD控制器計算期望加速度用一階慣性環(huán)節(jié)模擬發(fā)動機和制動系統(tǒng)的實際響應(yīng)延遲在主循環(huán)里實時更新自車與目標(biāo)車的速度和位置并用動畫實時顯示跟車過程仿真結(jié)束后繪制間距誤差、速度對比、加速度曲線方便做參數(shù)分析這套東西適合誰用呢如果你是學(xué)生正在做ACC相關(guān)的課程設(shè)計或畢設(shè)需要在論文里給出仿真驗證如果你是工程師想快速驗證一個新的控制算法比如MPC、模糊控制、滑?？刂苹蛘吣阒皇菍CC的工作原理好奇想具象地理解它那么這套模擬器都是很合適的起點。2. ACC控制算法拆解從跟車策略到車輛響應(yīng)2.1 先把ACC的分層架構(gòu)說清楚很多人第一次接觸ACC容易把它想成一個大號的定速巡航。實際上它可以拆成三個層次感知層負(fù)責(zé)獲取自車速度、與前車的相對距離、相對速度。在真實系統(tǒng)里這依賴毫米波雷達(dá)和攝像頭的數(shù)據(jù)融合但在我的模擬器里這些量直接從仿真狀態(tài)中讀取再人為疊加一點噪聲和延遲用來模擬傳感器的不完美特性。決策層是ACC的核心它根據(jù)感知信息計算出期望加速度。這一層又分兩步先確定期望車間距再計算達(dá)到這個間距所需要的加速度。大多數(shù)當(dāng)前量產(chǎn)ACC都采用恒定車頭時距策略后面我會詳細(xì)介紹。執(zhí)行層把期望加速度變成車輛的實際響應(yīng)。發(fā)動機和制動系統(tǒng)不可能瞬間響應(yīng)控制指令所以我把執(zhí)行層建模成一個一階慣性環(huán)節(jié)時間常數(shù)通常在0.3到0.6秒之間。這一步非常關(guān)鍵如果不加這個慣性環(huán)節(jié)控制效果會好到失真實際應(yīng)用時根本達(dá)不到。2.2 恒定車頭時距策略的數(shù)學(xué)邏輯恒定車頭時距策略簡稱CTH是目前ACC最主流的跟車策略。它的核心思想是期望間距不是固定值而是隨著自車車速線性增大。期望間距的計算公式是d_des d0 tau * v_ego其中d0是最小安全間距也就是兩車都停住時也必須保持的距離一般取2到5米tau是車頭時距單位是秒代表自車按照當(dāng)前速度行駛需要經(jīng)過多少秒到達(dá)前車當(dāng)前位置。注意這個時距本質(zhì)上是一個時間間隔概念不是真正要求的物理距離。tau取1秒時如果自車以108km/h也就是30m/s行駛期望間距就是30加d0大概33米左右。tau取2.2秒時同樣的速度下間距要求會拉到接近70米。CTH策略的優(yōu)點在于車速越高跟車距離越大給了駕駛員足夠的反應(yīng)時間車速越低跟車距離越小不容易被旁車加塞。實際量產(chǎn)車的tau一般在0.8到2.2秒之間可調(diào)運動模式和舒適模式的差別很大程度就體現(xiàn)在這個參數(shù)上?？刂颇繕?biāo)是讓實際間距d趨近于期望間距d_des同時讓相對速度趨近于零。所以控制誤差有兩項間距誤差e_distance d - d_des相對速度v_rel v_lead - v_ego。期望加速度的PD形式是a_des Kp * (d - d_des) Kd * (v_lead - v_ego)前車提速時相對速度為正自車需要加速跟上去前車減速時相對速度為負(fù)自車要減速保持安全距離。2.3 車輛縱向動力學(xué)模型別小看這一步控制器算出來的期望加速度不能直接當(dāng)作車輛的實際加速度。我見過不少初學(xué)者的模擬器直接把a_des用在運動學(xué)積分里曲線確實好看但一到實車驗證就崩了。原因在于車輛動力系統(tǒng)的響應(yīng)延遲和加速度限制。在我的模擬器里實際加速度a_act通過一階慣性環(huán)節(jié)逼近期望加速度da_act/dt (a_des - a_act) / tau_vehtau_veh是執(zhí)行器響應(yīng)時間常數(shù)我默認(rèn)取0.4秒。這個值的意思是如果期望加速度從0突然變成2m/s^2車輛實際加速度約經(jīng)過1.2秒左右才能上升到接近目標(biāo)值。這個響應(yīng)特性在Sampling時間尺度上非常關(guān)鍵。同時我還對期望加速度做了幅值限制最大加速度2.0m/s^2最大減速度3.5m/s^2。這對應(yīng)一般的舒適性邊界。如果算法算出-6m/s^2的急剎需求控制器會飽和在-3.5仿真結(jié)果就會如實反映自車剎不住而產(chǎn)生碰撞風(fēng)險。這里的碰撞風(fēng)險其實是評估算法質(zhì)量的重要信號絕對不能通過放寬減速度限制來掩蓋。運動學(xué)更新就是最基礎(chǔ)的積分等式v_new v_old a_act * dt x_new x_old v_new * dt這套模型雖然簡單但包含了做ACC算法驗證最核心的兩個要素控制目標(biāo)明確、執(zhí)行響應(yīng)有約束。后面你替換成MPC也好、模糊控制也好模型框架都不用動。3. Matlab代碼結(jié)構(gòu)初始化、主循環(huán)到實時動畫的實現(xiàn)細(xì)節(jié)3.1 工程文件組織我不喜歡把所有代碼都堆在一個腳本里后期會很難維護(hù)。這套模擬器結(jié)合數(shù)據(jù)流劃分成幾個文件每個文件職責(zé)單一方便替換和復(fù)用。工程文件結(jié)構(gòu)大概是這樣init_params.m集中配置所有仿真參數(shù)包括ACC參數(shù)、車輛參數(shù)、工況定義generate_lead_profile.m生成前車的速度剖面返回v_lead時間序列acc_controller.m計算期望加速度輸入是當(dāng)前測量量輸出a_desupdate_vehicle.m根據(jù)期望加速度更新實際加速度、速度、位置run_simulation.m主循環(huán)驅(qū)動整個仿真plot_results.m仿真結(jié)束后繪制結(jié)果曲線animate_visualization.m實時可視化跟車過程我特別建議把控制算法和數(shù)據(jù)存儲解耦。acc_controller.m里不負(fù)責(zé)保存歷史數(shù)據(jù)只管計算這樣以后想把PD換成MPC只需要在這個文件里改函數(shù)體其他地方完全不用動。3.2 主循環(huán)的骨架代碼下面給出run_simulation.m的核心邏輯片段。這是整套模擬器的骨架。% 加載參數(shù) init_params; % 生成前車速度剖面60秒仿真1Hz更新前車指令 T 60; dt 0.05; N T / dt; t (0:N-1) * dt; % 預(yù)分配數(shù)組 x_ego zeros(1, N); v_ego zeros(1, N); a_ego zeros(1, N); x_lead zeros(1, N); v_lead zeros(1, N); x_ego(1) 0; v_ego(1) 15; x_lead(1) 35; v_lead(1) 15; % 前車速度剖面先勻速再加速再勻速最后急減速 v_lead_profile [ones(1, 100) * 15, ... 15 0.5*(0:99), ... ones(1, 400) * 65 * 0.44704 15, ... % 大約到21.5m/s max(0, 21.5 - 1.0*(0:59)), ... zeros(1, 400)]; for k 1:N-1 % 讀取前車當(dāng)前速度模擬感知前車狀態(tài) v_lead(k1) v_lead_profile(k1); x_lead(k1) x_lead(k) v_lead(k1) * dt; % 計算實際間距和相對速度這里可以加傳感器噪聲 d_meas (x_lead(k) - x_ego(k)) 0.2 * randn; v_rel_meas (v_lead(k) - v_ego(k)) 0.15 * randn; % ACC控制器 a_des acc_controller(d_meas, v_rel_meas, v_ego(k), tau, d0, Kp, Kd, a_max, a_min); % 執(zhí)行層一階慣性 a_ego(k1) a_ego(k) dt / tau_veh * (a_des - a_ego(k)); % 運動學(xué)更新 v_ego(k1) v_ego(k) a_ego(k1) * dt; x_ego(k1) x_ego(k) v_ego(k1) * dt; end注意我這個前車速度剖面只是為了說明數(shù)據(jù)流怎么走實際使用中我會把生成邏輯獨立到generate_lead_profile.m里避免主循環(huán)里堆太多東西。acc_controller.m的內(nèi)容也很簡潔function a_des acc_controller(d_meas, v_rel_meas, v_ego, tau, d0, Kp, Kd, a_max, a_min) d_des d0 tau * v_ego; a_des Kp * (d_meas - d_des) Kd * v_rel_meas; a_des max(a_min, min(a_max, a_des)); end3.3 實時動畫是怎么實現(xiàn)的實時可視化是這套模擬器的門面。沒有可視化的仿真看起來就是一堆數(shù)字在跳動很難直觀感受到ACC是否跟上了前車。我的基本實現(xiàn)思路是把兩輛車畫成矩形前車用深色自車用淺色在迭代過程中不斷更新它們的位置同時動態(tài)顯示當(dāng)前車間距、自車速度、期望間距。核心繪圖代碼結(jié)構(gòu)是這樣的。figure(Position, [100 100 1200 600]); for k 1:500:N-1 % 每0.5秒刷新一次畫面避免動畫過快 clf; % 車道線和車輛位置 plot([-10 120], [0 0], k-, LineWidth, 2); hold on; % 前車矩形 rectangle(Position, [x_lead(k), 0.5, 4.5, 2], FaceColor, [0.3 0.3 0.3]); % 自車矩形 rectangle(Position, [x_ego(k), 0.5, 4.5, 2], FaceColor, [0.2 0.6 0.9]); % 間距標(biāo)注 d_now x_lead(k) - x_ego(k); text(min(x_lead(k), x_ego(k)) d_now/2, 3.5, ... sprintf(間距: %.1f m, d_now), HorizontalAlignment, center); % 速度信息面板 text(80, 8, sprintf(自車速度: %.1f m/s, v_ego(k))); text(80, 6.5, sprintf(前車速度: %.1f m/s, v_lead(k))); text(80, 5, sprintf(期望間距: %.1f m, d0 tau * v_ego(k))); axis equal; xlim([-10 max(120, x_lead(k)10)]); ylim([0 10]); grid on; drawnow; pause(0.02); end一定要加drawnow否則Matlab會認(rèn)為你還在準(zhǔn)備圖畫面不會刷新。如果要保存動畫可以在循環(huán)里用getframe配合VideoWriter寫視頻文件這個我后面做測試時會用到。3.4 參數(shù)配置表所有關(guān)鍵參數(shù)一覽init_params.m里的參數(shù)建議集中管理我用一張表把主要參數(shù)列出來方便后續(xù)對照。參數(shù)符號默認(rèn)值說明采樣周期dt0.05s控制與仿真步長仿真時長T60s總仿真時間車頭時距tau1.5s決定期望間距的增長速率最小安全間距d03m停車時保持的間距間距誤差增益Kp0.4PD控制的比例項增益相對速度增益Kd0.8PD控制的微分項增益最大期望加速度a_max2.0m/s^2舒適性上限最大期望減速度a_min-3.5m/s^2舒適性下限執(zhí)行器時間常數(shù)tau_veh0.4s一階慣性響應(yīng)時間采樣周期dt的選擇有一定講究。仿真步長至少要比系統(tǒng)最小時間常數(shù)小一個數(shù)量級tau_veh是0.4秒dt取0.05秒差不多是1/8滿足要求。取太小比如0.001秒仿真速度會嚴(yán)重拖慢取太大比如0.2秒一階慣性環(huán)節(jié)的離散化誤差會明顯變大。4. 參數(shù)調(diào)優(yōu)實戰(zhàn)車頭時距、PID增益怎么選4.1 基準(zhǔn)工況設(shè)計調(diào)參之前必須先定工況否則你根本不知道改參數(shù)是為了優(yōu)化哪個場景下的表現(xiàn)。我在模擬器里設(shè)計了一個包含多段特征的基準(zhǔn)工況前車全程60秒速度剖面分為五個階段初始勻速15m/s行駛5秒然后以0.5m/s^2的加速度勻加速20秒到約25m/s保持勻速20秒最后以1.0m/s^2的減速度勻減速到停止停止后保持到最后。自車初始速度15m/s初始間距35米。這個工況覆蓋了ACC最常見的幾個場景穩(wěn)態(tài)跟車、前車加速、前車減速、前車剎停。一個合格的ACC算法在這種復(fù)合工況下應(yīng)該做到穩(wěn)態(tài)間距誤差在合理范圍內(nèi)、加減速平滑、最終停車后與前車保持不小于安全間距。4.2 三個參數(shù)的實際表現(xiàn)對比先說車頭時距tau。我分別跑了tau1.0s、1.5s、2.2s三組其他參數(shù)不變。結(jié)果非常直觀。車頭時距tau穩(wěn)態(tài)跟車間距前車急剎時最小間距主觀舒適性1.0s約18m接近碰撞最小間距僅4.2m跟車緊密加速頻繁1.5s約28m11.5m適中2.2s約40m18m很寬松但容易被加塞tau取1.0s時穩(wěn)態(tài)跟車距離確實更近公路上看起來更貼得住但前車減速時自車需要很大的減速度最小間距一度逼近安全邊界。tau取2.2s時前車減速幾乎毫無壓力但問題在于穩(wěn)態(tài)間距被拉得很大實際道路上旁邊的車很容易并線進(jìn)來導(dǎo)致系統(tǒng)頻繁切換目標(biāo)。綜合來看1.5s左右是比較均衡的起點。再說Kp。Kp直接決定了間距誤差的校正強度。我試過0.15、0.4、0.9三檔。Kp0.15時間距誤差要累積到很大才能產(chǎn)生足夠的加速度修正前車提速后自車要花十幾秒才能重新跟上去明顯太肉。Kp0.9時控制器變得非常激進(jìn)稍微有一點間距誤差就會產(chǎn)生很大的加速度請求整個系統(tǒng)的加速度曲線頻繁沖擊上下限前后抖動明顯。Kp0.4附近是一個合理區(qū)間。最后是Kd。Kd的作用是阻尼相對速度變化。如果Kd太小說0.3自車接近前車時幾乎沒有減速預(yù)判會有沖過頭的感覺如果Kd太大會引發(fā)與前車相對速度測量噪聲的耦合我在后面章節(jié)會專門講這個問題。我調(diào)試下來的經(jīng)驗是Kd取Kp的1.5到2倍左右能獲得相對平順的跟車效果。4.3 判斷跟車效果的三項量化指標(biāo)光看動畫不夠最終評估必須量化。我自己常用的指標(biāo)有三個。間距誤差的均方根值也就是RMS誤差。它反映整體跟車精度。計算公式是把每個采樣時刻的實際間距與期望間距之差求平方和再平均開方。RMS誤差越小說明跟車精度越高一般控制在2米以內(nèi)算合格。最大加速度絕對值和抖振次數(shù)。這直接關(guān)系到乘客舒適性。我會統(tǒng)計一整段仿真中加速度絕對值的最大值當(dāng)它頻繁逼近設(shè)定限幅值時說明控制參數(shù)太激進(jìn)。最小車間距尤其是前車急減速工況下的最小間距。這是安全性的底線指標(biāo)。如果最小間距小于安全間距說明算法存在碰撞風(fēng)險必須增加前饋或者限制控制增益。這三項指標(biāo)往往會互相矛盾間距誤差最小化需要增大Kp但Kp過大會導(dǎo)致舒適性下降間距更大化更安全但容易被加塞。調(diào)參的本質(zhì)就是在這三者之間找平衡點。我的建議是安全指標(biāo)優(yōu)先在保證最小間距不低于安全邊界的前提下再去優(yōu)化RMS誤差和舒適性。5. 仿真中的真實問題抖動、急剎與噪聲延遲5.1 低速跟車時的加速度抖振問題第一組仿真跑完我注意到一個現(xiàn)象前車幾乎勻速行駛時自車加速度曲線仍然在兩三個采樣周期內(nèi)來回跳變幅度大概在正負(fù)0.3m/s^2之間。看起來不嚴(yán)重但明顯不自然實車上會因為這種頻繁請求導(dǎo)致油門和剎車反復(fù)切換。排查后原因有兩點。第一相對速度測量值疊加了隨機噪聲而Kd乘以這個含噪信號時噪聲被放大后直接進(jìn)入加速度命令。第二采樣周期雖然只有0.05秒但前車速度剖面的輕微波動在每一次控制周期里都被當(dāng)作有效信號處理導(dǎo)致控制器太敏感。解決辦法從兩方面入手。一是對前車速度信號做一次滑動平均濾波窗口取0.25秒可以有效壓低高頻噪聲二是在加速度命令輸出前增加一個死區(qū)邏輯當(dāng)期望加速度絕對值小于0.2m/s^2時直接輸出0避免微小誤差反復(fù)觸發(fā)執(zhí)行器。這兩個改動加進(jìn)去后抖振現(xiàn)象立刻消失跟車曲線變得平滑很多。5.2 前車急減速時自車剎不住的問題第二次排查的問題更嚴(yán)重。把前車減速度從1.0m/s^2提升到2.5m/s^2時自車在前車停止前并沒有完全剎停最小車間距跌到0附近。這說明純PD控制器本質(zhì)上是一個事后糾偏型控制器它必須等間距誤差或者相對速度誤差明顯出現(xiàn)之后才開始反應(yīng)。當(dāng)前車的減速強度超過了控制器能跟蹤的極限時碰撞風(fēng)險就不可避免。我加了一個前饋補償項核心思想是利用當(dāng)前相對速度和間距去估算一個安全制動需求。前饋加速度的近似計算方式a_ff -(v_rel^2) / (2 * max(d - safety_margin, 0.5))這個公式的邏輯很簡單如果自車以當(dāng)前相對速度接近前車按運動學(xué)關(guān)系要在剩余間距內(nèi)把相對速度減到零所需的減速度就是這個值。safety_margin我取1米作為傳感器誤差的保守余量。把它疊加到PD控制輸出上期望加速度變成a_des Kp * (d - d_des) Kd * v_rel a_ff改完以后前車急減速工況下的最小車間距從接近0提升到了3到4米安全性顯著改善。這里也讓我體會到ACC算法要做到優(yōu)秀光靠反饋是不夠的前饋和運動學(xué)預(yù)測能力同樣關(guān)鍵。5.3 傳感器噪聲和測量延遲的影響模擬第三個問題不嚴(yán)重但很有代表性。我在測量環(huán)節(jié)加了噪聲和延遲后發(fā)現(xiàn)系統(tǒng)開始變得手忙腳亂自車明明在做很平穩(wěn)的跟車但控制命令里總是摻著高頻的毛刺。根源在于Kd對噪聲的放大。延時則會惡化控制系統(tǒng)的相位裕度讓系統(tǒng)變得更容易震蕩。我做了這樣一組對比同樣的PD參數(shù)在理想測量條件下跟車效果很好但加上0.1秒測量延遲和標(biāo)準(zhǔn)差0.2m的距離噪聲后速度誤差曲線明顯增粗。這說明ACC算法驗證時忽略傳感器模型相當(dāng)于用一個過度理想的環(huán)境來測試得到的參數(shù)很可能在實際系統(tǒng)中完全不適用。所以在模擬器中給測量環(huán)節(jié)加入噪聲和延遲是必要的敬畏心。我在仿真里采用的方法是真實間距計算出來后先把它延遲0.1秒再加0.2m標(biāo)準(zhǔn)差的高斯隨機噪聲?？刂浦芷诒３?.05秒這樣既能模擬傳感器的不完美又不至于把噪聲加得過分夸張導(dǎo)致仿真失去參考意義。寫在最后的一點實操心得把這套模擬器從零搭完并完成多個工況驗證后我最深的體會是ACC控制算法的核心不在于代碼復(fù)雜度而在于你是否理解了控制目標(biāo)、車輛響應(yīng)特性和傳感器約束三者之間的關(guān)系。代碼很短但每一段背后都有現(xiàn)實中對應(yīng)的物理環(huán)節(jié)不做簡化就看不透本質(zhì)過度簡化又會得到不切實際的樂觀結(jié)果。如果你準(zhǔn)備在這個框架上繼續(xù)做擴(kuò)展比如換成MPC算法、加入隊列協(xié)同甚至多目標(biāo)切換建議先把這套模擬器完整跑一遍把所有變量都真正看一遍再動手改。這樣后面遇到的每一個奇怪現(xiàn)象你都能快速判斷是控制問題、車輛模型問題還是測量環(huán)節(jié)的問題。 Matlab實現(xiàn)自適應(yīng)巡航控制模擬器：從原理到調(diào)參實戰(zhàn)
我做ACC仿真這個方向也算有幾年了剛開始時的確想過直接上現(xiàn)成的整車級仿真平臺但發(fā)現(xiàn)要么太重、要么根本沒法針對控制層做快速迭代。最后我從零搭了一套自適應(yīng)巡航控制ACC系統(tǒng)模擬器用Matlab實現(xiàn)了實時車輛跟隨可視化。這篇文章就把這套模擬器的完整思路、代碼結(jié)構(gòu)和調(diào)參經(jīng)驗全部拆開講清楚希望能給正在做ACC算法驗證、課程設(shè)計或者前期方案評估的人省點時間。1. 為什么自己搭A(yù)CC模擬器而不是直接用整車仿真平臺1.1 市面方案的三個痛點先說明一下我并不是否定現(xiàn)成工具的價值。Simulink里的自動駕駛工具箱能做到四五十個自由度的高保真車輛動力學(xué)仿真配合場景編輯器還能模擬傳感器融合、毫米波雷達(dá)回波、攝像頭目標(biāo)檢測那一整套東西。但這套東西放在我只想驗證ACC控制策略這個場景里大概率是殺雞用牛刀。第一個痛點是學(xué)習(xí)曲線太陡。從場景構(gòu)建、傳感器配置到總線信號映射光是把一條簡單的跟車工況跑通新手可能要折騰兩三天。而我要的其實很簡單前車按給定速度剖面行駛自車通過ACC算法跟隨實時顯示兩車距離和相對速度的變化。用高保真平臺來做這個事大量精力都耗在了與算法驗證無關(guān)的配置上。第二個痛點是模塊黑盒化。整車平臺里帶的一些ACC控制器模塊是封裝好的參數(shù)暴露得有限你很難真正看清控制器的每個中間變量。但做算法研究恰恰需要隨時查看期望加速度、期望間距、誤差項這些量。自己寫代碼就沒有這個問題所有變量都在工作空間里想看哪個看哪個。第三個痛點是計算開銷。跑一次高保真仿真尤其加了復(fù)雜場景和傳感器模型之后速度會明顯放慢。但ACC策略驗證至少需要跑幾十組不同工況的參數(shù)掃描一個工況實時跑不起來的話調(diào)參效率會非常低。1.2 這套模擬器到底解決了什么問題基于上面的考慮我在某個模擬項目X里用Matlab寫了一個輕量級ACC模擬器。它的定位非常明確把精力集中在ACC上層決策算法和下層車輛響應(yīng)特性的模擬上不做復(fù)雜傳感器仿真不做高精度動力學(xué)模型但保證控制策略驗證所需的閉環(huán)鏈路完整。它能做到下面幾件事配置前車的速度剖面包括勻速、勻加速、勻減速、停車再起步等工況用恒定車頭時距策略計算期望間距通過PD控制器計算期望加速度用一階慣性環(huán)節(jié)模擬發(fā)動機和制動系統(tǒng)的實際響應(yīng)延遲在主循環(huán)里實時更新自車與目標(biāo)車的速度和位置并用動畫實時顯示跟車過程仿真結(jié)束后繪制間距誤差、速度對比、加速度曲線方便做參數(shù)分析這套東西適合誰用呢如果你是學(xué)生正在做ACC相關(guān)的課程設(shè)計或畢設(shè)需要在論文里給出仿真驗證如果你是工程師想快速驗證一個新的控制算法比如MPC、模糊控制、滑?？刂苹蛘吣阒皇菍CC的工作原理好奇想具象地理解它那么這套模擬器都是很合適的起點。2. ACC控制算法拆解從跟車策略到車輛響應(yīng)2.1 先把ACC的分層架構(gòu)說清楚很多人第一次接觸ACC容易把它想成一個大號的定速巡航。實際上它可以拆成三個層次感知層負(fù)責(zé)獲取自車速度、與前車的相對距離、相對速度。在真實系統(tǒng)里這依賴毫米波雷達(dá)和攝像頭的數(shù)據(jù)融合但在我的模擬器里這些量直接從仿真狀態(tài)中讀取再人為疊加一點噪聲和延遲用來模擬傳感器的不完美特性。決策層是ACC的核心它根據(jù)感知信息計算出期望加速度。這一層又分兩步先確定期望車間距再計算達(dá)到這個間距所需要的加速度。大多數(shù)當(dāng)前量產(chǎn)ACC都采用恒定車頭時距策略后面我會詳細(xì)介紹。執(zhí)行層把期望加速度變成車輛的實際響應(yīng)。發(fā)動機和制動系統(tǒng)不可能瞬間響應(yīng)控制指令所以我把執(zhí)行層建模成一個一階慣性環(huán)節(jié)時間常數(shù)通常在0.3到0.6秒之間。這一步非常關(guān)鍵如果不加這個慣性環(huán)節(jié)控制效果會好到失真實際應(yīng)用時根本達(dá)不到。2.2 恒定車頭時距策略的數(shù)學(xué)邏輯恒定車頭時距策略簡稱CTH是目前ACC最主流的跟車策略。它的核心思想是期望間距不是固定值而是隨著自車車速線性增大。期望間距的計算公式是d_des d0 tau * v_ego其中d0是最小安全間距也就是兩車都停住時也必須保持的距離一般取2到5米tau是車頭時距單位是秒代表自車按照當(dāng)前速度行駛需要經(jīng)過多少秒到達(dá)前車當(dāng)前位置。注意這個時距本質(zhì)上是一個時間間隔概念不是真正要求的物理距離。tau取1秒時如果自車以108km/h也就是30m/s行駛期望間距就是30加d0大概33米左右。tau取2.2秒時同樣的速度下間距要求會拉到接近70米。CTH策略的優(yōu)點在于車速越高跟車距離越大給了駕駛員足夠的反應(yīng)時間車速越低跟車距離越小不容易被旁車加塞。實際量產(chǎn)車的tau一般在0.8到2.2秒之間可調(diào)運動模式和舒適模式的差別很大程度就體現(xiàn)在這個參數(shù)上?？刂颇繕?biāo)是讓實際間距d趨近于期望間距d_des同時讓相對速度趨近于零。所以控制誤差有兩項間距誤差e_distance d - d_des相對速度v_rel v_lead - v_ego。期望加速度的PD形式是a_des Kp * (d - d_des) Kd * (v_lead - v_ego)前車提速時相對速度為正自車需要加速跟上去前車減速時相對速度為負(fù)自車要減速保持安全距離。2.3 車輛縱向動力學(xué)模型別小看這一步控制器算出來的期望加速度不能直接當(dāng)作車輛的實際加速度。我見過不少初學(xué)者的模擬器直接把a_des用在運動學(xué)積分里曲線確實好看但一到實車驗證就崩了。原因在于車輛動力系統(tǒng)的響應(yīng)延遲和加速度限制。在我的模擬器里實際加速度a_act通過一階慣性環(huán)節(jié)逼近期望加速度da_act/dt (a_des - a_act) / tau_vehtau_veh是執(zhí)行器響應(yīng)時間常數(shù)我默認(rèn)取0.4秒。這個值的意思是如果期望加速度從0突然變成2m/s^2車輛實際加速度約經(jīng)過1.2秒左右才能上升到接近目標(biāo)值。這個響應(yīng)特性在Sampling時間尺度上非常關(guān)鍵。同時我還對期望加速度做了幅值限制最大加速度2.0m/s^2最大減速度3.5m/s^2。這對應(yīng)一般的舒適性邊界。如果算法算出-6m/s^2的急剎需求控制器會飽和在-3.5仿真結(jié)果就會如實反映自車剎不住而產(chǎn)生碰撞風(fēng)險。這里的碰撞風(fēng)險其實是評估算法質(zhì)量的重要信號絕對不能通過放寬減速度限制來掩蓋。運動學(xué)更新就是最基礎(chǔ)的積分等式v_new v_old a_act * dt x_new x_old v_new * dt這套模型雖然簡單但包含了做ACC算法驗證最核心的兩個要素控制目標(biāo)明確、執(zhí)行響應(yīng)有約束。后面你替換成MPC也好、模糊控制也好模型框架都不用動。3. Matlab代碼結(jié)構(gòu)初始化、主循環(huán)到實時動畫的實現(xiàn)細(xì)節(jié)3.1 工程文件組織我不喜歡把所有代碼都堆在一個腳本里后期會很難維護(hù)。這套模擬器結(jié)合數(shù)據(jù)流劃分成幾個文件每個文件職責(zé)單一方便替換和復(fù)用。工程文件結(jié)構(gòu)大概是這樣init_params.m集中配置所有仿真參數(shù)包括ACC參數(shù)、車輛參數(shù)、工況定義generate_lead_profile.m生成前車的速度剖面返回v_lead時間序列acc_controller.m計算期望加速度輸入是當(dāng)前測量量輸出a_desupdate_vehicle.m根據(jù)期望加速度更新實際加速度、速度、位置run_simulation.m主循環(huán)驅(qū)動整個仿真plot_results.m仿真結(jié)束后繪制結(jié)果曲線animate_visualization.m實時可視化跟車過程我特別建議把控制算法和數(shù)據(jù)存儲解耦。acc_controller.m里不負(fù)責(zé)保存歷史數(shù)據(jù)只管計算這樣以后想把PD換成MPC只需要在這個文件里改函數(shù)體其他地方完全不用動。3.2 主循環(huán)的骨架代碼下面給出run_simulation.m的核心邏輯片段。這是整套模擬器的骨架。% 加載參數(shù) init_params; % 生成前車速度剖面60秒仿真1Hz更新前車指令 T 60; dt 0.05; N T / dt; t (0:N-1) * dt; % 預(yù)分配數(shù)組 x_ego zeros(1, N); v_ego zeros(1, N); a_ego zeros(1, N); x_lead zeros(1, N); v_lead zeros(1, N); x_ego(1) 0; v_ego(1) 15; x_lead(1) 35; v_lead(1) 15; % 前車速度剖面先勻速再加速再勻速最后急減速 v_lead_profile [ones(1, 100) * 15, ... 15 0.5*(0:99), ... ones(1, 400) * 65 * 0.44704 15, ... % 大約到21.5m/s max(0, 21.5 - 1.0*(0:59)), ... zeros(1, 400)]; for k 1:N-1 % 讀取前車當(dāng)前速度模擬感知前車狀態(tài) v_lead(k1) v_lead_profile(k1); x_lead(k1) x_lead(k) v_lead(k1) * dt; % 計算實際間距和相對速度這里可以加傳感器噪聲 d_meas (x_lead(k) - x_ego(k)) 0.2 * randn; v_rel_meas (v_lead(k) - v_ego(k)) 0.15 * randn; % ACC控制器 a_des acc_controller(d_meas, v_rel_meas, v_ego(k), tau, d0, Kp, Kd, a_max, a_min); % 執(zhí)行層一階慣性 a_ego(k1) a_ego(k) dt / tau_veh * (a_des - a_ego(k)); % 運動學(xué)更新 v_ego(k1) v_ego(k) a_ego(k1) * dt; x_ego(k1) x_ego(k) v_ego(k1) * dt; end注意我這個前車速度剖面只是為了說明數(shù)據(jù)流怎么走實際使用中我會把生成邏輯獨立到generate_lead_profile.m里避免主循環(huán)里堆太多東西。acc_controller.m的內(nèi)容也很簡潔function a_des acc_controller(d_meas, v_rel_meas, v_ego, tau, d0, Kp, Kd, a_max, a_min) d_des d0 tau * v_ego; a_des Kp * (d_meas - d_des) Kd * v_rel_meas; a_des max(a_min, min(a_max, a_des)); end3.3 實時動畫是怎么實現(xiàn)的實時可視化是這套模擬器的門面。沒有可視化的仿真看起來就是一堆數(shù)字在跳動很難直觀感受到ACC是否跟上了前車。我的基本實現(xiàn)思路是把兩輛車畫成矩形前車用深色自車用淺色在迭代過程中不斷更新它們的位置同時動態(tài)顯示當(dāng)前車間距、自車速度、期望間距。核心繪圖代碼結(jié)構(gòu)是這樣的。figure(Position, [100 100 1200 600]); for k 1:500:N-1 % 每0.5秒刷新一次畫面避免動畫過快 clf; % 車道線和車輛位置 plot([-10 120], [0 0], k-, LineWidth, 2); hold on; % 前車矩形 rectangle(Position, [x_lead(k), 0.5, 4.5, 2], FaceColor, [0.3 0.3 0.3]); % 自車矩形 rectangle(Position, [x_ego(k), 0.5, 4.5, 2], FaceColor, [0.2 0.6 0.9]); % 間距標(biāo)注 d_now x_lead(k) - x_ego(k); text(min(x_lead(k), x_ego(k)) d_now/2, 3.5, ... sprintf(間距: %.1f m, d_now), HorizontalAlignment, center); % 速度信息面板 text(80, 8, sprintf(自車速度: %.1f m/s, v_ego(k))); text(80, 6.5, sprintf(前車速度: %.1f m/s, v_lead(k))); text(80, 5, sprintf(期望間距: %.1f m, d0 tau * v_ego(k))); axis equal; xlim([-10 max(120, x_lead(k)10)]); ylim([0 10]); grid on; drawnow; pause(0.02); end一定要加drawnow否則Matlab會認(rèn)為你還在準(zhǔn)備圖畫面不會刷新。如果要保存動畫可以在循環(huán)里用getframe配合VideoWriter寫視頻文件這個我后面做測試時會用到。3.4 參數(shù)配置表所有關(guān)鍵參數(shù)一覽init_params.m里的參數(shù)建議集中管理我用一張表把主要參數(shù)列出來方便后續(xù)對照。參數(shù)符號默認(rèn)值說明采樣周期dt0.05s控制與仿真步長仿真時長T60s總仿真時間車頭時距tau1.5s決定期望間距的增長速率最小安全間距d03m停車時保持的間距間距誤差增益Kp0.4PD控制的比例項增益相對速度增益Kd0.8PD控制的微分項增益最大期望加速度a_max2.0m/s^2舒適性上限最大期望減速度a_min-3.5m/s^2舒適性下限執(zhí)行器時間常數(shù)tau_veh0.4s一階慣性響應(yīng)時間采樣周期dt的選擇有一定講究。仿真步長至少要比系統(tǒng)最小時間常數(shù)小一個數(shù)量級tau_veh是0.4秒dt取0.05秒差不多是1/8滿足要求。取太小比如0.001秒仿真速度會嚴(yán)重拖慢取太大比如0.2秒一階慣性環(huán)節(jié)的離散化誤差會明顯變大。4. 參數(shù)調(diào)優(yōu)實戰(zhàn)車頭時距、PID增益怎么選4.1 基準(zhǔn)工況設(shè)計調(diào)參之前必須先定工況否則你根本不知道改參數(shù)是為了優(yōu)化哪個場景下的表現(xiàn)。我在模擬器里設(shè)計了一個包含多段特征的基準(zhǔn)工況前車全程60秒速度剖面分為五個階段初始勻速15m/s行駛5秒然后以0.5m/s^2的加速度勻加速20秒到約25m/s保持勻速20秒最后以1.0m/s^2的減速度勻減速到停止停止后保持到最后。自車初始速度15m/s初始間距35米。這個工況覆蓋了ACC最常見的幾個場景穩(wěn)態(tài)跟車、前車加速、前車減速、前車剎停。一個合格的ACC算法在這種復(fù)合工況下應(yīng)該做到穩(wěn)態(tài)間距誤差在合理范圍內(nèi)、加減速平滑、最終停車后與前車保持不小于安全間距。4.2 三個參數(shù)的實際表現(xiàn)對比先說車頭時距tau。我分別跑了tau1.0s、1.5s、2.2s三組其他參數(shù)不變。結(jié)果非常直觀。車頭時距tau穩(wěn)態(tài)跟車間距前車急剎時最小間距主觀舒適性1.0s約18m接近碰撞最小間距僅4.2m跟車緊密加速頻繁1.5s約28m11.5m適中2.2s約40m18m很寬松但容易被加塞tau取1.0s時穩(wěn)態(tài)跟車距離確實更近公路上看起來更貼得住但前車減速時自車需要很大的減速度最小間距一度逼近安全邊界。tau取2.2s時前車減速幾乎毫無壓力但問題在于穩(wěn)態(tài)間距被拉得很大實際道路上旁邊的車很容易并線進(jìn)來導(dǎo)致系統(tǒng)頻繁切換目標(biāo)。綜合來看1.5s左右是比較均衡的起點。再說Kp。Kp直接決定了間距誤差的校正強度。我試過0.15、0.4、0.9三檔。Kp0.15時間距誤差要累積到很大才能產(chǎn)生足夠的加速度修正前車提速后自車要花十幾秒才能重新跟上去明顯太肉。Kp0.9時控制器變得非常激進(jìn)稍微有一點間距誤差就會產(chǎn)生很大的加速度請求整個系統(tǒng)的加速度曲線頻繁沖擊上下限前后抖動明顯。Kp0.4附近是一個合理區(qū)間。最后是Kd。Kd的作用是阻尼相對速度變化。如果Kd太小說0.3自車接近前車時幾乎沒有減速預(yù)判會有沖過頭的感覺如果Kd太大會引發(fā)與前車相對速度測量噪聲的耦合我在后面章節(jié)會專門講這個問題。我調(diào)試下來的經(jīng)驗是Kd取Kp的1.5到2倍左右能獲得相對平順的跟車效果。4.3 判斷跟車效果的三項量化指標(biāo)光看動畫不夠最終評估必須量化。我自己常用的指標(biāo)有三個。間距誤差的均方根值也就是RMS誤差。它反映整體跟車精度。計算公式是把每個采樣時刻的實際間距與期望間距之差求平方和再平均開方。RMS誤差越小說明跟車精度越高一般控制在2米以內(nèi)算合格。最大加速度絕對值和抖振次數(shù)。這直接關(guān)系到乘客舒適性。我會統(tǒng)計一整段仿真中加速度絕對值的最大值當(dāng)它頻繁逼近設(shè)定限幅值時說明控制參數(shù)太激進(jìn)。最小車間距尤其是前車急減速工況下的最小間距。這是安全性的底線指標(biāo)。如果最小間距小于安全間距說明算法存在碰撞風(fēng)險必須增加前饋或者限制控制增益。這三項指標(biāo)往往會互相矛盾間距誤差最小化需要增大Kp但Kp過大會導(dǎo)致舒適性下降間距更大化更安全但容易被加塞。調(diào)參的本質(zhì)就是在這三者之間找平衡點。我的建議是安全指標(biāo)優(yōu)先在保證最小間距不低于安全邊界的前提下再去優(yōu)化RMS誤差和舒適性。5. 仿真中的真實問題抖動、急剎與噪聲延遲5.1 低速跟車時的加速度抖振問題第一組仿真跑完我注意到一個現(xiàn)象前車幾乎勻速行駛時自車加速度曲線仍然在兩三個采樣周期內(nèi)來回跳變幅度大概在正負(fù)0.3m/s^2之間。看起來不嚴(yán)重但明顯不自然實車上會因為這種頻繁請求導(dǎo)致油門和剎車反復(fù)切換。排查后原因有兩點。第一相對速度測量值疊加了隨機噪聲而Kd乘以這個含噪信號時噪聲被放大后直接進(jìn)入加速度命令。第二采樣周期雖然只有0.05秒但前車速度剖面的輕微波動在每一次控制周期里都被當(dāng)作有效信號處理導(dǎo)致控制器太敏感。解決辦法從兩方面入手。一是對前車速度信號做一次滑動平均濾波窗口取0.25秒可以有效壓低高頻噪聲二是在加速度命令輸出前增加一個死區(qū)邏輯當(dāng)期望加速度絕對值小于0.2m/s^2時直接輸出0避免微小誤差反復(fù)觸發(fā)執(zhí)行器。這兩個改動加進(jìn)去后抖振現(xiàn)象立刻消失跟車曲線變得平滑很多。5.2 前車急減速時自車剎不住的問題第二次排查的問題更嚴(yán)重。把前車減速度從1.0m/s^2提升到2.5m/s^2時自車在前車停止前并沒有完全剎停最小車間距跌到0附近。這說明純PD控制器本質(zhì)上是一個事后糾偏型控制器它必須等間距誤差或者相對速度誤差明顯出現(xiàn)之后才開始反應(yīng)。當(dāng)前車的減速強度超過了控制器能跟蹤的極限時碰撞風(fēng)險就不可避免。我加了一個前饋補償項核心思想是利用當(dāng)前相對速度和間距去估算一個安全制動需求。前饋加速度的近似計算方式a_ff -(v_rel^2) / (2 * max(d - safety_margin, 0.5))這個公式的邏輯很簡單如果自車以當(dāng)前相對速度接近前車按運動學(xué)關(guān)系要在剩余間距內(nèi)把相對速度減到零所需的減速度就是這個值。safety_margin我取1米作為傳感器誤差的保守余量。把它疊加到PD控制輸出上期望加速度變成a_des Kp * (d - d_des) Kd * v_rel a_ff改完以后前車急減速工況下的最小車間距從接近0提升到了3到4米安全性顯著改善。這里也讓我體會到ACC算法要做到優(yōu)秀光靠反饋是不夠的前饋和運動學(xué)預(yù)測能力同樣關(guān)鍵。5.3 傳感器噪聲和測量延遲的影響模擬第三個問題不嚴(yán)重但很有代表性。我在測量環(huán)節(jié)加了噪聲和延遲后發(fā)現(xiàn)系統(tǒng)開始變得手忙腳亂自車明明在做很平穩(wěn)的跟車但控制命令里總是摻著高頻的毛刺。根源在于Kd對噪聲的放大。延時則會惡化控制系統(tǒng)的相位裕度讓系統(tǒng)變得更容易震蕩。我做了這樣一組對比同樣的PD參數(shù)在理想測量條件下跟車效果很好但加上0.1秒測量延遲和標(biāo)準(zhǔn)差0.2m的距離噪聲后速度誤差曲線明顯增粗。這說明ACC算法驗證時忽略傳感器模型相當(dāng)于用一個過度理想的環(huán)境來測試得到的參數(shù)很可能在實際系統(tǒng)中完全不適用。所以在模擬器中給測量環(huán)節(jié)加入噪聲和延遲是必要的敬畏心。我在仿真里采用的方法是真實間距計算出來后先把它延遲0.1秒再加0.2m標(biāo)準(zhǔn)差的高斯隨機噪聲?？刂浦芷诒３?.05秒這樣既能模擬傳感器的不完美又不至于把噪聲加得過分夸張導(dǎo)致仿真失去參考意義。寫在最后的一點實操心得把這套模擬器從零搭完并完成多個工況驗證后我最深的體會是ACC控制算法的核心不在于代碼復(fù)雜度而在于你是否理解了控制目標(biāo)、車輛響應(yīng)特性和傳感器約束三者之間的關(guān)系。代碼很短但每一段背后都有現(xiàn)實中對應(yīng)的物理環(huán)節(jié)不做簡化就看不透本質(zhì)過度簡化又會得到不切實際的樂觀結(jié)果。如果你準(zhǔn)備在這個框架上繼續(xù)做擴(kuò)展比如換成MPC算法、加入隊列協(xié)同甚至多目標(biāo)切換建議先把這套模擬器完整跑一遍把所有變量都真正看一遍再動手改。這樣后面遇到的每一個奇怪現(xiàn)象你都能快速判斷是控制問題、車輛模型問題還是測量環(huán)節(jié)的問題。