MATLAB 里的駕駛場(chǎng)景設(shè)計(jì)器Driving Scenario Designer下文簡(jiǎn)稱 DSD是我這幾年做自動(dòng)駕駛彎道仿真的默認(rèn)起點(diǎn)。它不神秘本質(zhì)上就是一個(gè)帶道路編輯器的場(chǎng)景裝配臺(tái)你畫出中心線、拉出車道、擺上幾臺(tái)車、配好速度軌跡按下運(yùn)行就能看到車沿著你畫的線跑起來(lái)。真正卡住人的從來(lái)不是“按鈕在哪”而是“為什么這么點(diǎn)”——為什么中心點(diǎn)要多密、為什么速度剖面必須跟路徑點(diǎn)一一對(duì)應(yīng)、為什么仿真步長(zhǎng)稍大一點(diǎn)車就切出車道。下面我把自己反復(fù)折騰出來(lái)的一整套流程攤開講從環(huán)境自查到代碼級(jí)拆解彎道怎么畫、速度怎么算、坑怎么填全部是可復(fù)制的東西。這篇內(nèi)容適合兩類人看剛接手駕駛仿真、需要在一小時(shí)內(nèi)跑出第一條彎道的新手已經(jīng)在用 DSD 但總被“車飛出去”“傳感器啥也測(cè)不到”“導(dǎo)出 Simulink 后場(chǎng)景不動(dòng)”折磨的同行。全文以 MATLAB 腳本為主線圖形界面只作為校驗(yàn)和兜底手段因?yàn)槟_本可以版本管理、可以參數(shù)化、可以批量跑這三點(diǎn)在真實(shí)測(cè)試流程里比“點(diǎn)得爽”重要得多。1. 先把工具的能力邊界摸清楚1.1 彎道為什么是仿真里最“貴”的場(chǎng)景直線段仿真幾乎沒有技術(shù)含量車速恒定、航向恒定、車道邊界平行任何一套邏輯都能跑出漂亮結(jié)果。彎道就不一樣了它同時(shí)把四個(gè)維度的復(fù)雜度擰在一起幾何上曲率隨弧長(zhǎng)變化車輛模型需要持續(xù)輸出橫向加速度感知上車道線從直線變成圓弧擬合算法要用二次或三次曲線去逼近控制上預(yù)瞄距離和前視點(diǎn)必須隨曲率調(diào)整否則純跟蹤算法會(huì)切內(nèi)道評(píng)價(jià)上橫向誤差、航向誤差、舒適性指標(biāo)全都跟曲率強(qiáng)相關(guān)。這就是為什么很多人直線跑得挺順一進(jìn)彎就原形畢露。DSD 在這方面提供的價(jià)值不是“畫個(gè)彎”而是把道路幾何、車輛運(yùn)動(dòng)學(xué)和傳感器模型綁在同一個(gè)時(shí)間軸上。你畫的中心線會(huì)被擬合成帶曲率連續(xù)性的道路車輛沿軌跡行駛時(shí)每一幀的位置、朝向、速度都是自洽的雷達(dá)和攝像頭模型拿到的也是同一套真值。這一點(diǎn)很關(guān)鍵——如果道路用一套坐標(biāo)、車輛用另一套坐標(biāo)、傳感器再用第三套你后面排查問題會(huì)瘋掉。另一個(gè)容易被忽略的點(diǎn)是彎道對(duì)時(shí)步的敏感性。直線上車輛朝某個(gè)方向勻速走步長(zhǎng) 0.1 秒的影響無(wú)非是位置跳了一米彎道上同樣的步長(zhǎng)會(huì)讓航向角每次跳好幾度視覺上就是車在一格一格地“拐硬彎”傳感器打出去的光束也會(huì)跟著抖動(dòng)。所以彎道場(chǎng)景做得好不好很大程度取決于你對(duì)采樣率的把控而不是畫得多漂亮。1.2 圖形界面和腳本各自該干哪一半我見過不少人一上來(lái)就打開 App鼠標(biāo)拖著中心點(diǎn)一個(gè)個(gè)點(diǎn)點(diǎn)完發(fā)現(xiàn)想改半徑只能全部推倒重來(lái)。這屬于用錯(cuò)了工具的分工。我的習(xí)慣是探索性設(shè)計(jì)用界面可復(fù)現(xiàn)場(chǎng)景用腳本。第一次試某個(gè)彎道形態(tài)、想快速看看車道寬度和轉(zhuǎn)彎半徑比例是否合理打開drivingScenarioDesigner拖兩下確實(shí)快但一旦形態(tài)確定立刻把中心點(diǎn)坐標(biāo)抄進(jìn)腳本后面所有迭代都在代碼里做。界面的另一個(gè)正確用法是反向校驗(yàn)。腳本跑出來(lái)的場(chǎng)景可以一鍵喂回 AppdrivingScenarioDesigner(scenario)把變量加載進(jìn)去你就能在圖形界面里看到道路邊界、車道線、車輛軌跡、傳感器覆蓋范圍疊在一起的樣子。有些問題在命令行里看不出來(lái)比如兩條車道線在彎心處交叉、車輛軌跡貼著路緣擦過去圖形界面上一眼就能發(fā)現(xiàn)。這種“腳本生成 界面審查”的雙向流程比純界面或純腳本都高效。還有一個(gè)功能值得單獨(dú)提App 的 Tools 菜單里有一個(gè)針對(duì)道路和車輛時(shí)步的檢查項(xiàng)。它會(huì)把你的速度、采樣時(shí)間、道路曲率放在一起算一遍告訴你當(dāng)前配置下車輛每步移動(dòng)的距離是否超出了合理范圍。這個(gè)檢查我在項(xiàng)目里救過好幾次場(chǎng)——用高車速配 0.1 秒步長(zhǎng)時(shí)它會(huì)直接標(biāo)紅提示省得你后面花兩小時(shí)去查“為什么車會(huì)瞬移”。2. 動(dòng)手前的環(huán)境自查與最小驗(yàn)證2.1 工具箱、版本與常見依賴DSD 不是一個(gè)獨(dú)立產(chǎn)品它掛在自動(dòng)駕駛工具箱體系下同時(shí)依賴場(chǎng)景建模的基礎(chǔ)設(shè)施。裝環(huán)境時(shí)至少確認(rèn)這幾塊自動(dòng)駕駛相關(guān)工具箱提供drivingScenario、vehicle、trajectory、各類傳感器生成器、導(dǎo)航與地圖相關(guān)工具箱部分道路擬合和曲線工具在這里、以及基礎(chǔ)的 MATLAB 本體。如果你的流程要接 Simulink 做閉環(huán)那 Simulink 和對(duì)應(yīng)的場(chǎng)景讀取模塊也得在。版本差異是個(gè)真實(shí)的坑。同一個(gè)函數(shù)在不同年份的版本里參數(shù)名會(huì)變尤其是傳感器生成器那一塊屬性名和返回值順序調(diào)整過好幾次。我的經(jīng)驗(yàn)是先鎖定一個(gè)版本再寫代碼把版本號(hào)寫在腳本頭部注釋里別今天用新版本跑通、明天換臺(tái)機(jī)器用舊版本報(bào)錯(cuò)然后花半天懷疑人生。另外函數(shù)簽名不確定的時(shí)候別硬猜直接在命令行敲help 函數(shù)名看當(dāng)前這個(gè)版本到底怎么寫的比在網(wǎng)上翻三年前的帖子靠譜得多。道路擬合部分需要說明一下它的原理。DSD 在根據(jù)你給的中心點(diǎn)生成道路時(shí)用的是回旋線clothoid擬合也就是曲率隨弧長(zhǎng)線性變化的曲線。這個(gè)東西在公路設(shè)計(jì)里是標(biāo)準(zhǔn)做法——直線段曲率是 0圓曲線曲率是常數(shù) 1/R中間用回旋線過渡保證曲率連續(xù)。好處是車輛沿路行駛時(shí)方向盤轉(zhuǎn)角不會(huì)突變壞處是你給的中心點(diǎn)如果本身跳變很大擬合出來(lái)的道路可能偏離你預(yù)期的形狀。理解這一點(diǎn)你就知道為什么中心點(diǎn)不能隨手點(diǎn)了。2.2 三行代碼驗(yàn)證環(huán)境是否真的可用在正式畫彎道之前我建議先跑一個(gè)最小閉環(huán)確認(rèn)整條鏈路是通的。三行代碼就夠scenario drivingScenario(SampleTime, 0.05, StopTime, 5); road(scenario, [0 0 0; 50 0 0], Lanes, lanespec([1 1])); drivingScenarioDesigner(scenario);這三行的意思是建一個(gè)采樣時(shí)間 0.05 秒、總時(shí)長(zhǎng) 5 秒的場(chǎng)景鋪一條從原點(diǎn)出發(fā)、沿 x 軸方向長(zhǎng) 50 米的直路中心線左右各一條車道然后把它丟進(jìn)圖形界面里看一眼。如果界面正常打開、道路顯示為兩條平行車道線說明基礎(chǔ)環(huán)境沒問題。這一步的價(jià)值在于把所有環(huán)境問題前置。我見過太多次興沖沖寫了兩百行彎道代碼一運(yùn)行報(bào)“函數(shù)不存在”才開始排查是哪個(gè)工具箱沒裝。用最小用例先探路成本三十秒收益是后面省掉半小時(shí)。如果這三行都跑不通那問題一定在環(huán)境層面跟你的彎道邏輯毫無(wú)關(guān)系方向很明確。順手再驗(yàn)證一下推演機(jī)制while advance(scenario) p pose(scenario.Actors(1)); fprintf(t%.2f x%.2f y%.2f\n, scenario.CurrentTime, p(1), p(2)); end這段用來(lái)說明advance是怎么推進(jìn)時(shí)間的——每次調(diào)用把場(chǎng)景往前推一個(gè)采樣步長(zhǎng)返回 false 表示到StopTime了。后面所有的仿真循環(huán)都是這個(gè)骨架。3. 彎道場(chǎng)景的三層建模道路、車輛、傳感器3.1 道路層中心點(diǎn)密度決定了一切道路是場(chǎng)景的地基彎道尤其如此。核心接口是road(scenario, roadCenters, Lanes, lanespec([...]))其中roadCenters是一個(gè) N×3 的矩陣每行是一個(gè)中心點(diǎn)的 [x, y, z] 坐標(biāo)z 用來(lái)表達(dá)高程和橫坡。DSD 會(huì)按順序把這些點(diǎn)連成一條帶曲率連續(xù)性的中心線再按lanespec往兩側(cè)鋪車道。中心點(diǎn)密度這里有兩條經(jīng)驗(yàn)值。第一彎道上相鄰中心點(diǎn)的間距控制在曲率半徑的 1/10 到 1/5 之間。半徑 60 米的彎點(diǎn)間距 6 到 12 米比較合適。間距太大回旋線擬合出來(lái)的形狀跟你腦子里想的不一樣間距太小擬合會(huì)放大數(shù)值噪聲道路出現(xiàn)細(xì)微抖動(dòng)車輛的航向角跟著抖。第二直線段不需要這么密30 米一個(gè)點(diǎn)完全夠用因?yàn)橹本€曲率恒為零擬合結(jié)果沒有任何自由度。lanespec的參數(shù)形式是lanespec([左車道數(shù), 右車道數(shù)])。lanespec([1 1])是雙向兩車道lanespec([0 1])是只有右側(cè)一條車道。這里有個(gè)容易搞混的地方車道的左右是相對(duì)于中心線的前進(jìn)方向定義的右車道的中心線在中心線右側(cè)偏移半個(gè)車道寬默認(rèn)車道寬 3.6 米所以偏移是 1.8 米。如果你的車要跑在右車道正中間初始位置就不能放在中心線上。高程那一列別隨便填。很多人圖省事全部寫 0平面場(chǎng)景下沒問題但如果你想驗(yàn)證車輛在帶橫坡的彎道上是否會(huì)有姿態(tài)變化第三列就要按橫向坡率填。常規(guī)公路彎道超高在 2% 到 6% 之間半徑 60 米這種中速?gòu)澣?4% 比較常見。具體做法是讓外彎側(cè)比內(nèi)彎側(cè)高高差 車道寬 × 坡率。3.2 車輛層位置、朝向與速度剖面車輛用vehicle(scenario, Position, [x y z])創(chuàng)建能配置的包括尺寸、類別、初始朝向。位置坐標(biāo)是世界坐標(biāo)系下的米朝向是相對(duì) x 軸逆時(shí)針的角度弧度。初始位姿的坑在于它必須跟軌跡的第一個(gè)點(diǎn)一致。trajectory的行為是讓車輛從當(dāng)前位姿出發(fā)去追擊你給的路徑點(diǎn)如果第一個(gè)路徑點(diǎn)跟初始位置差了五米車輛會(huì)在第一步就瞬移到那個(gè)點(diǎn)上后續(xù)的軌跡全部錯(cuò)位。我踩過這個(gè)坑現(xiàn)象是車輛在起點(diǎn)閃一下然后整條軌跡看起來(lái)像整體平移了一段。真正決定彎道表現(xiàn)的是速度剖面。trajectory(actor, waypoints, speed)的第三個(gè)參數(shù)可以是標(biāo)量全程勻速也可以是與waypoints行數(shù)相同的向量每點(diǎn)一個(gè)速度。用向量的時(shí)候必須嚴(yán)格對(duì)齊行數(shù)多一個(gè)少一個(gè)都會(huì)報(bào)錯(cuò)這個(gè)報(bào)錯(cuò)信息還挺模糊的容易讓人卡住。速度剖面的設(shè)計(jì)原則是從橫向加速度約束倒推。車輛過彎時(shí)受到的橫向加速度近似為 a v2/R這個(gè)值直接決定乘客的舒適感。工程上取 1.5 到 2.5 m/s2 作為舒適上限超過 3 m/s2 就明顯不適了。所以給定半徑 R彎中允許的最大車速是 v √(a·R)。這個(gè)公式是整個(gè)速度設(shè)計(jì)的錨點(diǎn)后面具體算的時(shí)候會(huì)用到。3.3 傳感器層裝在哪、看多遠(yuǎn)傳感器模型不是必須的但彎道場(chǎng)景不加傳感器價(jià)值至少少一半。常用的兩個(gè)是雷達(dá)數(shù)據(jù)生成器和視覺檢測(cè)生成器。雷達(dá)的參數(shù)重點(diǎn)是安裝位置、視場(chǎng)角和量程。安裝位置用MountingLocation單位是米坐標(biāo)是相對(duì)車身、以車輛中心為原點(diǎn)的車身坐標(biāo)系x 向前、y 向左、z 向上。量產(chǎn)車的毫米波雷達(dá)一般裝在前保險(xiǎn)杠縱向 3 到 4 米、橫向 0、高度 0.4 到 0.8 米。視場(chǎng)角用FieldOfView格式是 [水平角, 俯仰角]角雷達(dá)通常水平 90 度、俯仰 10 到 20 度。視覺檢測(cè)器的關(guān)鍵參數(shù)是量程、檢測(cè)概率和誤檢率。量程要跟你的場(chǎng)景尺度匹配彎道半徑 60 米、總長(zhǎng)一百多米的話量程 60 米是個(gè)合理起點(diǎn)。檢測(cè)概率默認(rèn) 0.9 左右意味著每幀有 10% 概率漏檢如果你想驗(yàn)證跟蹤算法的魯棒性可以調(diào)到 0.7 制造壓力。彎道下傳感器有個(gè)特有現(xiàn)象內(nèi)彎側(cè)目標(biāo)被遮擋外彎側(cè)目標(biāo)提前暴露。這是幾何決定的雷達(dá)和攝像頭都一樣。如果你的場(chǎng)景里放了多輛車做評(píng)估時(shí)要意識(shí)到這個(gè)不對(duì)稱性——同一輛目標(biāo)車在彎道入口和彎道出口的檢測(cè)穩(wěn)定性可能完全不同別把它當(dāng)成算法的穩(wěn)定性問題。4. 完整實(shí)操五分鐘搭出一條 90 度彎道4.1 第一步 用腳本生成中心線先定義基本幾何參數(shù)。我要的是一條從原點(diǎn)出發(fā)、走 30 米直線、然后左轉(zhuǎn) 90 度的彎彎道半徑 60 米。這幾個(gè)數(shù)字決定了整條路的位置。R 60; % 彎道半徑單位 m lineLen 30; % 入彎前直線長(zhǎng)度單位 m nLine 7; % 直線段中心點(diǎn)數(shù)量 nArc 10; % 彎道段中心點(diǎn)數(shù)量 % 直線段沿 x 方向y 恒為 0 sLine linspace(0, lineLen, nLine); ptsLine [sLine, zeros(nLine, 1), zeros(nLine, 1)]; % 彎道段圓心位于 (lineLen, R)從 θ0 轉(zhuǎn)到 θπ/2 theta linspace(0, pi/2, nArc); ptsArc [lineLen R*sin(theta), R - R*cos(theta), zeros(nArc, 1)]; % 拼接時(shí)去掉重復(fù)的接縫點(diǎn) roadCenters [ptsLine; ptsArc(2:end, :)];這段代碼的幾何關(guān)系值得說清楚。圓心選在 (30, 60)起點(diǎn)是 (30, 60-60) (30, 0)正好接上直線段的終點(diǎn)終點(diǎn)是 (3060, 60) (90, 60)。參數(shù)方程用 sin 和 (1-cos) 的組合起點(diǎn)處切線方向是 x終點(diǎn)處切線方向是 y也就是標(biāo)準(zhǔn)的左轉(zhuǎn) 90 度。拼接時(shí)去掉ptsArc的第一個(gè)點(diǎn)因?yàn)槟且稽c(diǎn)和直線段最后一點(diǎn)完全重合重復(fù)的中心點(diǎn)會(huì)讓后續(xù)的弧長(zhǎng)計(jì)算出現(xiàn)零長(zhǎng)度段某些函數(shù)會(huì)直接報(bào)錯(cuò)。如果換成右轉(zhuǎn)、或者轉(zhuǎn) 45 度只需要改theta的范圍比如linspace(0, -pi/2, nArc)就是右轉(zhuǎn)。半徑和直線長(zhǎng)度改了圓心位置要跟著改這個(gè)是唯一的聯(lián)動(dòng)關(guān)系。4.2 第二步 鋪車道與擺車scenario drivingScenario(SampleTime, 0.02, StopTime, 25); road(scenario, roadCenters, Lanes, lanespec([1 1]));采樣時(shí)間我選了 0.02 秒比默認(rèn)的 0.05 更細(xì)。原因后面第 5 節(jié)會(huì)展開簡(jiǎn)單說就是彎道上每步位移要控制住不然航向角跳變明顯。lanespec([1 1])是雙向兩車道視覺上完整一些。接著擺車。為了讓車跑在右車道中心而不是壓著中心線先用中心點(diǎn)的法向量往外偏 1.8 米dx gradient(roadCenters(:,1)); dy gradient(roadCenters(:,2)); nor [-dy, dx] ./ vecnorm([-dy dx], 2, 2); % 單位法向量指向左側(cè) laneCenters roadCenters(:,1:2) - 1.8 * nor; % 右車道中心 laneCenters(:,3) 0;gradient算的是每個(gè)點(diǎn)處切線的差分近似[-dy, dx]旋轉(zhuǎn) 90 度得到法向。注意方向這個(gè)法向指向左側(cè)所以減去 1.8 才是右側(cè)車道中心。這個(gè)偏移用在整個(gè)路徑上而不是只偏起點(diǎn)——彎道里法向是連續(xù)變化的只偏起點(diǎn)會(huì)讓車輛越跑越往外。然后創(chuàng)建車輛并把初始位置設(shè)到路徑第一個(gè)點(diǎn)ego vehicle(scenario, ... ClassID, 1, ... Position, laneCenters(1,:), ... Length, 4.6, Width, 1.9, Height, 1.5);4.3 第三步 配軌跡與速度速度剖面按前面說的橫向加速度倒推。我取舒適上限 1.5 m/s2半徑 60 米aLatMax 1.5; vCorner sqrt(aLatMax * R); % 約 9.49 m/s折合 34 km/h vEnter 14; % 入彎前車速m/s sDecelFrom 10; % 從弧長(zhǎng) 10 m 處開始減速vCorner sqrt(1.5 × 60) 9.49米每秒大概 34 公里每小時(shí)。入彎前 14 米每秒50 公里每小時(shí)所以需要在直線段內(nèi)減速。按弧長(zhǎng)重采樣路徑點(diǎn)再逐點(diǎn)填速度s [0; cumsum(vecnorm(diff(roadCenters), 2, 2))]; sUniform linspace(0, s(end), 240); waypoints [interp1(s, laneCenters(:,1), sUniform), ... interp1(s, laneCenters(:,2), sUniform), ... zeros(numel(sUniform),1)]; speedProfile vCorner * ones(size(sUniform)); idx1 sUniform sDecelFrom; speedProfile(idx1) vEnter; idx2 sUniform sDecelFrom sUniform lineLen; speedProfile(idx2) vEnter (vCorner - vEnter) * ... (sUniform(idx2) - sDecelFrom) / (lineLen - sDecelFrom); trajectory(ego, waypoints, speedProfile);這里有兩個(gè)關(guān)鍵動(dòng)作。一是按弧長(zhǎng)等間距重采樣把原來(lái)疏密不均的中心點(diǎn)變成間隔均勻的點(diǎn)總長(zhǎng)約 124 米240 個(gè)點(diǎn)間距約 0.52 米這樣速度剖面在空間上的分布才是均勻的。二是速度向量長(zhǎng)度必須等于路徑點(diǎn)行數(shù)代碼里都用sUniform驅(qū)動(dòng)天然對(duì)齊。入彎的減速段是線性的實(shí)際車上會(huì)做成加加速度受限的 S 形曲線但用于仿真驗(yàn)證橫向控制線性減速已經(jīng)夠用而且更容易分析。4.4 第四步 運(yùn)行與可視化bep birdsEyePlot(XLimits, [-20 130], YLimits, [-40 100]); lp laneBoundaryPlotter(bep, DisplayName, 車道邊界); tp trackPlotter(bep, DisplayName, 自車, MarkerFaceColor, red); plotLaneBoundary(lp, scenario.LaneBoundaries); while advance(scenario) p pose(ego); tp(p(1), p(2), p(3)); drawnow limitrate end跑起來(lái)之后你應(yīng)該能看到車從原點(diǎn)出發(fā)沿直線跑 30 米在彎道入口減速然后貼著彎道內(nèi)側(cè)平穩(wěn)轉(zhuǎn)過 90 度。如果車輛在中途跳出車道或者航向角劇烈擺動(dòng)直接跳到第 6 節(jié)找對(duì)應(yīng)癥狀。5. 代碼逐段拆解與參數(shù)怎么算5.1 圓弧參數(shù)化與切線方向圓弧參數(shù)方程用的是(圓心 R·sinθ, 圓心_y - R·cosθ)這種組合而不是常見的(R·cosθ, R·sinθ)。這個(gè)選擇不是隨意的是因?yàn)楹笳咴?θ0 處的切線方向是 y起點(diǎn)朝向不對(duì)你還得額外加一個(gè)旋轉(zhuǎn)矩陣。用 sin 和 (1-cos) 的形式天然滿足“起點(diǎn)在原點(diǎn)方向、切線沿 x”的需求省去一堆坐標(biāo)變換。對(duì)于任意起始朝向的彎道更通用的做法是先算一個(gè)旋轉(zhuǎn)矩陣再作用到點(diǎn)上heading0 pi/6; % 起始朝向30 度 Rmat [cos(heading0) -sin(heading0); sin(heading0) cos(heading0)]; ptsArcRotated (Rmat * ptsArc(:,1:2));不過實(shí)際項(xiàng)目里我一般不用這套通用解因?yàn)閳?chǎng)景都是圍繞自車坐標(biāo)系搭建的讓自車從原點(diǎn)沿 x 出發(fā)后面所有幾何都能用最簡(jiǎn)單的表達(dá)式寫。保持這個(gè)約定能省掉大量調(diào)試坐標(biāo)系的時(shí)間。還有一個(gè)細(xì)節(jié)trajectory計(jì)算車輛朝向是靠前后路徑點(diǎn)的差分所以路徑點(diǎn)的順序必須是從起點(diǎn)到終點(diǎn)單調(diào)推進(jìn)。如果你不小心把點(diǎn)排反了車輛會(huì)試圖倒著走或者干脆原地打轉(zhuǎn)。中心點(diǎn)拼接順序錯(cuò)了是新手常見的失誤癥狀是車輛在起點(diǎn)來(lái)回抽搐。5.2 弧長(zhǎng)重采樣為什么必須做不重采樣會(huì)發(fā)生什么你給的中心點(diǎn)可能是 7 個(gè)直線點(diǎn)加 10 個(gè)彎道點(diǎn)間距分別是 5 米和 10.5 米而速度向量如果按點(diǎn)數(shù)配每個(gè)點(diǎn)之間代表的空間距離完全不同。線性插值速度的話減速的“陡峭程度”就會(huì)隨點(diǎn)間距變化彎道入口那一段的減速度可能比你設(shè)計(jì)的大兩三倍。重采樣的本質(zhì)是把時(shí)間維度的設(shè)計(jì)轉(zhuǎn)換到空間維度。速度剖面本質(zhì)上是 s弧長(zhǎng)的函數(shù)只有等間距采樣速度曲線在空間上的斜率才有物理意義。cumsum(vecnorm(diff(...)))這段就是算累積弧長(zhǎng)然后interp1在弧長(zhǎng)域上做線性插值把原來(lái)不均勻的點(diǎn)映射到均勻網(wǎng)格上。采樣密度怎么定我給的經(jīng)驗(yàn)值是空間分辨率 0.5 米左右對(duì)應(yīng)總長(zhǎng) 124 米就是 240 個(gè)點(diǎn)。這個(gè)密度下60 米半徑的彎道每?jī)牲c(diǎn)之間的轉(zhuǎn)角約 0.5 度視覺上足夠平滑。密度再高收益不大反而增加計(jì)算量低于 0.3 米的間距就要注意數(shù)值噪聲了。5.3 橫向加速度反推限速這張表是我常用的速查取橫向加速度上限 1.5 m/s2彎道半徑 R (m)彎中限速 v (m/s)換算 (km/h)典型場(chǎng)景205.519.7停車場(chǎng)掉頭306.724.1城市小半徑路口508.731.2城市主干道轉(zhuǎn)彎609.534.2郊區(qū)匝道8011.039.4快速路匝道10012.244.1高速互通如果你的場(chǎng)景是驗(yàn)證緊急工況可以把上限提到 2.5 m/s2車速相應(yīng)提高約 29%因?yàn)?√(2.5/1.5) ≈ 1.29。但要注意此時(shí)車輛模型的輪胎側(cè)偏特性可能進(jìn)入非線性區(qū)仿真結(jié)果的參考價(jià)值會(huì)下降。反過來(lái)用也很有價(jià)值給定你希望跑的車速反推需要多大的半徑。比如驗(yàn)證某個(gè)彎道 60 km/h 的通過性v 16.7 m/s在 1.5 m/s2 約束下需要 R v2/a 278 米。這個(gè)數(shù)字一算出來(lái)你就知道60 km/h 過 60 米半徑的彎?rùn)M向加速度會(huì)到 4.6 m/s2屬于相當(dāng)激進(jìn)的工況。5.4 仿真主循環(huán)的時(shí)序控制采樣時(shí)間選 0.02 秒不是拍腦袋。判斷標(biāo)準(zhǔn)是單步位移量與彎道曲率的關(guān)系每步車輛前進(jìn)的距離 Δs v·Δt如果 Δs 大到讓離散化后的軌跡明顯偏離圓弧航向角就會(huì)出現(xiàn)階躍。拿彎中 9.5 m/s 算0.02 秒步長(zhǎng)對(duì)應(yīng) Δs 0.19 米圓弧上對(duì)應(yīng)的圓心角是 0.19/60 ≈ 0.18 度完全可以忽略。如果用 0.05 秒Δs 0.475 米轉(zhuǎn)角 0.45 度還湊合用 0.1 秒Δs 0.95 米轉(zhuǎn)角接近 1 度視覺上就能看出車輛“一格一格拐”。高速工況下這個(gè)差異更明顯——30 m/s 配 0.1 秒步長(zhǎng)單步走了 3 米彎道上基本沒法看。代價(jià)是計(jì)算量。0.02 秒步長(zhǎng)、25 秒場(chǎng)景就是 1250 步比 0.05 秒多了 2.5 倍。如果場(chǎng)景里有多個(gè)傳感器、每幀都要做檢測(cè)跑一次可能要幾十秒。我的折中方案是調(diào)試階段用 0.02批量回歸用 0.05前者保證可視化正確后者保證吞吐量。真要卡精度可以在彎道段動(dòng)態(tài)調(diào)整步長(zhǎng)但這就復(fù)雜了一般場(chǎng)景用不上。另外StopTime要留余量。按 124 米總長(zhǎng)、平均速度 10 m/s 估大約 12.4 秒能跑完我給 25 秒是有富裕的。如果StopTime設(shè)小了車輛跑到一半仿真就結(jié)束你可能誤以為是軌跡斷了。6. 踩坑實(shí)錄彎道仿真最常見的六類問題6.1 車切出車道或“飛出去”這是最高頻的問題原因通常有三個(gè)按出現(xiàn)頻率排序。第一路徑點(diǎn)順序錯(cuò)亂或存在重復(fù)點(diǎn)。拼接中心線時(shí)如果接縫點(diǎn)沒去重cumsum里會(huì)出現(xiàn)零長(zhǎng)度段interp1在這一點(diǎn)上會(huì)產(chǎn)生 NaN軌跡就斷了。表現(xiàn)是車輛在某個(gè)位置突然消失或跳到遠(yuǎn)處。檢查方法很簡(jiǎn)單跑之前打一行any(diff(s) 0)返回 true 就說明有非遞增的弧長(zhǎng)。第二速度剖面長(zhǎng)度與路徑點(diǎn)數(shù)不匹配。這個(gè)報(bào)錯(cuò)通常直接報(bào)“向量長(zhǎng)度不一致”但如果你用了廣播機(jī)制不小心讓它跑通了速度就會(huì)錯(cuò)位到別的點(diǎn)上車輛會(huì)在彎心附近莫名其妙加速。第三車輛初始位姿與路徑首點(diǎn)偏離。前面提過偏離超過一兩米車輛就會(huì)瞬移。檢查方法是打印ego.Position和waypoints(1,:)對(duì)比差的應(yīng)該是零。還有一種“飛出去”是真的物理原因——速度太快軌跡函數(shù)算出來(lái)的轉(zhuǎn)向角跟不上。這時(shí)候車輛不是瞬移而是沿著切線方向沖出去路徑變成一條直線?？吹竭@個(gè)現(xiàn)象先降速再談別的。6.2 傳感器檢測(cè)不到目標(biāo)傳感器返回空結(jié)果排查順序建議這樣走。先確認(rèn)調(diào)用時(shí)機(jī)傳感器生成器需要在advance之后、用當(dāng)前時(shí)刻的scenario.Actors調(diào)用如果你在循環(huán)外面調(diào)那永遠(yuǎn)拿到的是初始狀態(tài)。再確認(rèn)坐標(biāo)系MountingLocation用的是車身坐標(biāo)系不是世界坐標(biāo)系寫錯(cuò)了傳感器就裝到車頂上或者車尾去了。然后是視場(chǎng)角。彎道場(chǎng)景里如果你的傳感器水平視場(chǎng)只有 30 度那內(nèi)彎側(cè)的目標(biāo)在很長(zhǎng)一段距離里都進(jìn)不了視野這是幾何決定的不是 bug。想驗(yàn)證這一點(diǎn)把視場(chǎng)角調(diào)到 120 度再跑一遍如果檢測(cè)突然出現(xiàn)了說明就是視野問題。最后是版本的返回值順序。不同年份的版本里視覺檢測(cè)器的返回值可能是[detections, numDetections, image]也可能是別的排列用之前help一下確認(rèn)別照著舊帖子抄。6.3 導(dǎo)出 Simulink 之后的斷層場(chǎng)景在腳本里跑得好好的導(dǎo)出成模型之后車不動(dòng)或者場(chǎng)景不變問題通常出在這幾處。場(chǎng)景數(shù)據(jù)沒跟著模型走。導(dǎo)出時(shí)場(chǎng)景是保存成一個(gè)數(shù)據(jù)文件或工作區(qū)變量的Simulink 里的場(chǎng)景讀取模塊需要指向這個(gè)變量。如果你換了機(jī)器或者清了工作區(qū)模型找不到數(shù)據(jù)就會(huì)用默認(rèn)空?qǐng)鼍氨憩F(xiàn)就是路沒畫出來(lái)、車也不動(dòng)。采樣時(shí)間不匹配。模型里的求解步長(zhǎng)跟場(chǎng)景的SampleTime如果不是整數(shù)倍關(guān)系車輛位置會(huì)出現(xiàn)插值誤差彎道上表現(xiàn)為軌跡抖動(dòng)。建議把模型的固定步長(zhǎng)設(shè)成與場(chǎng)景采樣時(shí)間一致或者成整數(shù)倍。道路參數(shù)沒有同步。有些導(dǎo)出方式只把車輛軌跡帶過去道路幾何要單獨(dú)配置。這時(shí)候模型里車輛是沿著一條不存在的路在跑視覺上就是懸浮。檢查方法是在模型里把道路預(yù)覽打開看邊界線是否顯示?，F(xiàn)象最可能原因快速驗(yàn)證車輛起點(diǎn)閃爍后整體偏移初始位姿與首路徑點(diǎn)不一致打印兩者差值彎道中車輛沿切線沖出速度超過曲率允許上限按 √(aR) 重算限速軌跡在某個(gè)位置斷裂路徑點(diǎn)重復(fù)導(dǎo)致弧長(zhǎng)非遞增any(diff(s)0)傳感器全程無(wú)檢測(cè)調(diào)用時(shí)機(jī)在循環(huán)外或視場(chǎng)過窄加大視場(chǎng)角重跑導(dǎo)出后場(chǎng)景空白場(chǎng)景數(shù)據(jù)未隨模型加載檢查模塊指向的變量彎道上航向角一跳一跳采樣步長(zhǎng)過大步長(zhǎng)減半觀察7. 從單車彎道走向多車交互與批量測(cè)試7.1 編排一輛對(duì)向車單車跑通之后加一輛對(duì)向車是驗(yàn)證感知和決策邏輯的下一步。做法是復(fù)制路徑生成邏輯把車道偏移反過來(lái)oppLane roadCenters(:,1:2) 1.8 * nor; % 左側(cè)車道 oppLane(:,3) 0; % 對(duì)向車從彎道出口往入口跑路徑需要反轉(zhuǎn) oppWaypoints flipud(oppLane); oppSpeed 8 * ones(size(oppWaypoints,1), 1); opp vehicle(scenario, ClassID, 1, Position, oppWaypoints(1,:)); trajectory(opp, oppWaypoints, oppSpeed);關(guān)鍵點(diǎn)是路徑必須反轉(zhuǎn)。對(duì)向車的行駛方向跟自車相反如果不翻轉(zhuǎn)它會(huì)從彎道出口倒著開回入口雖然位置對(duì)但朝向完全錯(cuò)看起來(lái)像在倒車。翻轉(zhuǎn)之后還要檢查速度向量的長(zhǎng)度也跟著變這一點(diǎn)容易漏。時(shí)間上的編排更有講究。你想讓兩車在彎心交會(huì)還是錯(cuò)開交會(huì)時(shí)機(jī)取決于兩車進(jìn)入場(chǎng)景的時(shí)間差。trajectory支持延遲啟動(dòng)可以在路徑前面補(bǔ)一段靜止點(diǎn)或者干脆調(diào)整初始位置。我一般用初始位置控制直觀一些把對(duì)向車的起點(diǎn)往遠(yuǎn)處挪讓它晚一點(diǎn)進(jìn)入交互區(qū)域。7.2 把場(chǎng)景參數(shù)化以便批量跑單次仿真說明不了什么真正的價(jià)值在于跑一批參數(shù)組合。把整個(gè)搭場(chǎng)景過程包成函數(shù)是最省事的做法function scenario buildCurveScenario(R, vEnter, aLatMax, dt) if nargin 4, dt 0.02; end vCorner sqrt(aLatMax * R); ... % 內(nèi)部按前面的邏輯生成道路、車輛、軌跡 end這樣外層就能用嵌套循環(huán)掃參數(shù)results []; for R [40 60 80 100] for v0 [10 12 14 16] sc buildCurveScenario(R, v0, 1.5, 0.05); metric runAndEvaluate(sc); % 自己寫的評(píng)估函數(shù) results [results; R, v0, metric]; end end評(píng)估函數(shù)里我通常記三個(gè)指標(biāo)最大橫向誤差車輛實(shí)際位置到車道中心線的距離峰值、最大橫向加速度從速度平方除以曲率算、航向誤差收斂時(shí)間從入彎到誤差穩(wěn)定在設(shè)計(jì)值以內(nèi)的時(shí)間。這三個(gè)放在一起基本能判斷某個(gè)控制參數(shù)在彎道上的適應(yīng)性。批量跑有個(gè)坑drivingScenario對(duì)象是有狀態(tài)的跑完之后不能直接復(fù)用必須重新創(chuàng)建。循環(huán)里每次都調(diào)buildCurveScenario就是因?yàn)檫@個(gè)別圖省事在循環(huán)外建一次然后反復(fù)advance。另外每次創(chuàng)建場(chǎng)景的隨機(jī)種子要固定傳感器模型里有隨機(jī)成分種子不固定的話同一組參數(shù)跑兩次結(jié)果不一樣你會(huì)誤以為算法不穩(wěn)定。我個(gè)人在實(shí)際操作中的體會(huì)是彎道仿真這件事八成的時(shí)間花在“把幾何和速度對(duì)上”兩成花在“看結(jié)果”。第一版場(chǎng)景能跑起來(lái)之后別急著加傳感器、加多車先把單車軌跡跑順、把橫向誤差曲線畫出來(lái)確認(rèn)車輛確實(shí)貼著車道中心走。這個(gè)基線穩(wěn)了后面加什么都是增量驗(yàn)證基線不穩(wěn)后面每加一個(gè)元素你都要重新懷疑一遍到底是誰(shuí)的問題。