本例介紹了如何利用傳感器融合實(shí)現(xiàn)基于傳感器融合的汽車自適應(yīng)巡航控制器，并且可用于在彎道上行駛的車輛。1：結(jié)合了傳感器融合和自適應(yīng)巡航控制器（ACC）的控制系統(tǒng)。提供了兩個(gè)ACC的變體：一個(gè)經(jīng)典控制器和一個(gè)來(lái)自模型預(yù)測(cè)控制工具箱的自適應(yīng)巡航控制系統(tǒng)塊。2：使用自動(dòng)駕駛工具箱生成的合成數(shù)據(jù)，在閉環(huán)Simulink模型中測(cè)試控制系統(tǒng)。3：配置軟件在環(huán)仿真的代碼生成設(shè)置，并自動(dòng)生成控制算法的代碼。
43.1 緒論自適應(yīng)巡航控制系統(tǒng)是一種根據(jù)前方路面交通狀況修改被控車輛速度的控制系統(tǒng)。與普通巡航控制一樣，駕駛員為汽車設(shè)定一個(gè)理想的速度；此外，如果前方車道上有其他車輛行駛速度較慢，自適應(yīng)巡航控制系統(tǒng)可以使被控車輛減速，控制車輛實(shí)現(xiàn)穩(wěn)定跟隨前車車輛行駛。要想讓ACC正常工作，被控車輛必須確定前方車道是彎道還是直道，以及哪輛車是 "領(lǐng)頭車"，也就是在車道上排在被控車輛前面的車輛。從被控車輛的角度看，一個(gè)典型的場(chǎng)景如下圖所示。被控車輛（藍(lán)色）沿著一條彎曲的道路行駛。一開(kāi)始，領(lǐng)頭車是粉色車。然后，紫色車切入被控車輛的車道，成為領(lǐng)頭車。過(guò)了一會(huì)兒，紫色車換到另一條車道，粉色車又成為領(lǐng)頭車。之后，粉色車依然是領(lǐng)頭車。ACC設(shè)計(jì)必須對(duì)路面上領(lǐng)頭車的變化做出反應(yīng)。目前的ACC設(shè)計(jì)主要依靠從毫米波雷達(dá)上獲得的測(cè)距和測(cè)速，其設(shè)計(jì)在直線道路上工作效果最好。在 "使用模型預(yù)測(cè)控制的自適應(yīng)巡航控制系統(tǒng)"（模型預(yù)測(cè)控制工具箱）中給出了這樣一個(gè)系統(tǒng)的例子。從先進(jìn)的駕駛輔助系統(tǒng)(ADAS)設(shè)計(jì)轉(zhuǎn)向更加自動(dòng)級(jí)別更高的系統(tǒng)，ACC必須解決以下挑戰(zhàn)：1 估算靠近被控車輛且相對(duì)被控車輛有較著橫向運(yùn)動(dòng)的車輛的相對(duì)位置和速度。2 估算被控車輛前面的車道，找出被控車輛前面的哪輛車是同一車道中最近的一輛車。3 反應(yīng)環(huán)境中其他車輛的侵略性舉動(dòng)，特別是當(dāng)其他車輛切入被控車輛車道時(shí)。這個(gè)例子展示了在現(xiàn)有ACC設(shè)計(jì)中主要增加兩個(gè)功能來(lái)應(yīng)對(duì)這些挑戰(zhàn)：增加一個(gè)傳感器融合系統(tǒng)和更新基于模型預(yù)測(cè)控制（MPC）的控制器設(shè)計(jì)。同時(shí)使用視覺(jué)和毫米波雷達(dá)傳感器的傳感器融合和跟蹤系統(tǒng)具有以下優(yōu)點(diǎn)：1 它將視覺(jué)傳感器獲得的較好的位置和速度橫向測(cè)量與毫米波雷達(dá)傳感器的測(cè)距和測(cè)距率測(cè)量相結(jié)合。2 視覺(jué)傳感器可以探測(cè)車道，提供車道相對(duì)于被控車輛的橫向位置估計(jì)，并定位場(chǎng)景中其他車輛相對(duì)于被控車輛車道的位置。這個(gè)例子假設(shè)了理想的車道探測(cè)。先進(jìn)的MPC控制器增加了對(duì)環(huán)境中其他車輛更激進(jìn)的操縱做出反應(yīng)的能力。與使用具有恒定增益的PID設(shè)計(jì)的經(jīng)典控制器相比，MPC控制器在保持嚴(yán)格的安全距離約束的同時(shí)調(diào)節(jié)被控車輛的速度，控制體驗(yàn)上更平順，(小編注MPC目前還沒(méi)有聽(tīng)說(shuō)哪家用在了量產(chǎn)車上)。因此，當(dāng)環(huán)境快速變化時(shí)，控制器可以采用類似于人類駕駛員的方式來(lái)實(shí)施更積極的機(jī)動(dòng)。43.2 測(cè)試臺(tái)模型和仿真結(jié)果概述要打開(kāi)Simulink主模型，請(qǐng)使用以下命令。open_system('ACCTestBenchExample')1 ACC with Sensor Fusion，它對(duì)傳感器融合進(jìn)行建模，控制車輛的縱向加速度。該組件允許選擇經(jīng)典或模型預(yù)測(cè)控制版本的設(shè)計(jì)。2 車輛與環(huán)境子系統(tǒng)，它對(duì)被控車輛的運(yùn)動(dòng)進(jìn)行建模，并對(duì)環(huán)境進(jìn)行建模。毫米波雷達(dá)和視覺(jué)傳感器的仿真為控制子系統(tǒng)提供融合數(shù)據(jù)。要在運(yùn)行模型之前運(yùn)行相關(guān)的初始化腳本，在Simulink模型中，單擊 "運(yùn)行設(shè)置腳本"，或者在命令提示符下，鍵入以下內(nèi)容：腳本加載Simulink模型所需的某些常量，如車輛和ACC設(shè)計(jì)參數(shù)。默認(rèn)的ACC是經(jīng)典控制器。腳本還創(chuàng)建了定義控制系統(tǒng)參考模型的輸入和輸出所需的總線。這些總線必須在模型編譯前在工作區(qū)中定義。當(dāng)模型編譯時(shí)，額外的Simulink總線會(huì)由各自的塊自動(dòng)生成。要繪制仿真結(jié)果并描繪被控車輛的周圍環(huán)境，包括被跟蹤的對(duì)象，可以利用Bird's-Eye
Scope觀測(cè)。Bird's-Eye Scope是一個(gè)模型級(jí)的可視化工具，它可以用來(lái)從Simulink工具條中打開(kāi)。在 "仿真 "選項(xiàng)卡上，在"回顧結(jié)果 "下，單擊鳥(niǎo)眼范圍。打開(kāi)范圍后，點(diǎn)擊查找信號(hào)，設(shè)置信號(hào)。下面的命令運(yùn)行仿真到15秒，得到中間的仿真畫(huà)面，再一直運(yùn)行下去。sim('ACCTestBenchExample','StopTime','15') %Simulate 15seconds sim('ACCTestBenchExample') %Simulate to end of scenarioans= Simulink.SimulationOutput:logsout: [1x1Simulink.SimulationData.Dataset] tout: [151x1 double]SimulationMetadata: [1x1Simulink.SimulationMetadata] ErrorMessage: [0x0 char]鳥(niǎo)眼范圍顯示了傳感器融合的結(jié)果。它顯示了毫米波雷達(dá)和視覺(jué)傳感器如何探測(cè)其傳感器覆蓋區(qū)域內(nèi)的車輛。它還顯示了由多對(duì)象跟蹤器塊保持的目標(biāo)跟蹤信息。黃色的跟蹤顯示的是最重要的物體（MIO）：被控車輛前方最近的跟蹤，在其車道上。我們看到，在場(chǎng)景開(kāi)始時(shí)，最重要的對(duì)象是被控車輛前方的快速行駛的汽車。當(dāng)過(guò)往的汽車離慢速行駛的汽車越來(lái)越近時(shí)，它越過(guò)了左側(cè)車道，傳感器融合體系識(shí)別出它是MIO。這輛車離被控車輛更近，速度比它慢很多。ACC必須控制被控車輛減速。在下面的經(jīng)典ACC系統(tǒng)的成果中：- 中間圖顯示了被控車輛與領(lǐng)先車輛之間的相對(duì)距離。在這個(gè)例子中，來(lái)自跟蹤和傳感器融合系統(tǒng)的原始數(shù)據(jù)被用于ACC設(shè)計(jì)，沒(méi)有進(jìn)行后處理?？梢灶A(yù)期看到一些 "尖峰"（中間圖），這是由于傳感器模型的不確定性，特別是當(dāng)另一輛車切入或離開(kāi)被控車輛車道時(shí)。要查看仿真結(jié)果，請(qǐng)使用以下命令：helperPlotACCResults(logsout,default_spacing,time_gap) - 在前11秒，領(lǐng)先車遠(yuǎn)遠(yuǎn)領(lǐng)先于被控車輛（中圖）。被控車輛加速并達(dá)到駕駛員設(shè)定的速度V_set（上圖）。- 11秒至20秒，另一輛車成為領(lǐng)先車，當(dāng)該車切入被控車輛車道時(shí)（中圖）。當(dāng)領(lǐng)頭車與被控車輛之間的距離較大時(shí)（11-15秒），被控車輛仍以駕駛員設(shè)定的速度行駛。當(dāng)距離變小時(shí)（15-20秒），被控車輛會(huì)減速以保持與領(lǐng)先車的安全距離（上圖）。- 20～34秒，前車移動(dòng)到另一條車道，新的領(lǐng)頭車出現(xiàn)（中圖）。由于領(lǐng)先車和被控車輛之間的距離很大，被控車輛在27秒時(shí)加速，直到達(dá)到駕駛員設(shè)定的速度。然后，被控車輛繼續(xù)以駕駛員設(shè)定的速度行駛（上圖）。- 底部圖顯示，加速度在[-3,2]m/s^2范圍內(nèi)。平穩(wěn)的瞬態(tài)行為表明，駕駛員的舒適度是令人滿意的。在基于MPC的ACC設(shè)計(jì)中，基本的優(yōu)化問(wèn)題是通過(guò)跟蹤駕駛員設(shè)定的速度來(lái)制定的，但必須強(qiáng)制執(zhí)行與領(lǐng)先車的安全距離。MPC控制器設(shè)計(jì)在自適應(yīng)巡航控制器一節(jié)中進(jìn)行了描述。要運(yùn)行帶有MPC設(shè)計(jì)的模型，首先激活MPC變體，然后運(yùn)行以下命令。這一步需要使用Model Predictive Control Toolbox軟件。可以使用以下代碼檢查該許可證的存在。如果不存在代碼，則描繪了類似結(jié)果的樣本。hasMPCLicense =license('checkout','MPC_Toolbox');if hasMPCLicensecontroller_type= 2;sim('ACCTestBenchExample','StopTime','15') %Simulate 15seconds sim('ACCTestBenchExample') %Simulate to end of scenarioelseload data_mpcend
-->Convertingmodel to discrete time.-->Assumingoutput disturbance added to measured output channel #2 is integrated white noise.Assumingno disturbance added to measured output channel #1.-->The"Model.Noise" property of the "mpc" object is empty. Assumingwhite noise on each measure ans =Simulink.SimulationOutput:logsout: [1x1 Simulink.SimulationData.Dataset] tout: [151x1double]
SimulationMetadata: [1x1Simulink.SimulationMetadata]ErrorMessage: [0x0 char]
-->Convertingmodel to discrete time.-->Assumingoutput disturbance added to measured output channel #2 is integrated white noise.Assumingno disturbance added to measured output channel #1.-->The"Model.Noise" property of the "mpc" object is empty.Assuming white noise on each measure
  在基于MPC的ACC的仿真結(jié)果中，與經(jīng)典ACC設(shè)計(jì)類似，實(shí)現(xiàn)了速度和間距控制的目標(biāo)。與經(jīng)典ACC設(shè)計(jì)相比，基于MPC的ACC采用全油門或制動(dòng)進(jìn)行加速或減速，因此更加激進(jìn)。這種行為是由于對(duì)相對(duì)距離的明確約束。當(dāng)?shù)缆飞习l(fā)生突然變化時(shí)，例如當(dāng)領(lǐng)先車變?yōu)槁嚂r(shí)，這種激進(jìn)行為可能是首選。為了使控制器的控制效果更加平順，打開(kāi)自適應(yīng)巡航控制系統(tǒng)塊的掩碼，并降低控制器行為參數(shù)的值。如前所述，中間圖中的尖峰是由于傳感器模型的不確定性造成的。要查看基于MPC的ACC的仿真結(jié)果，請(qǐng)使用以下命令。helperPlotACCResults(logsout,default_spacing,time_gap) 
下面將詳細(xì)介紹試驗(yàn)臺(tái)模型中各子系統(tǒng)的功能。帶傳感器融合的自適應(yīng)巡航控制器子系統(tǒng)包含兩個(gè)主要組成部分：2 自適應(yīng)巡航控制器子系統(tǒng)open_system('ACCTestBenchExample/ACC with Sensor Fusion')
43.3 跟蹤和傳感器融合跟蹤和傳感器融合子系統(tǒng)處理來(lái)自車輛和環(huán)境子系統(tǒng)的視覺(jué)和雷達(dá)探測(cè)，并生成被控車輛周圍環(huán)境的綜合情況圖。同時(shí)，它還能向ACC提供被控車輛前方車道上最近的車的估計(jì)。 open_system('ACCWithSensorFusionMdlRef/Tracking and Sensor Fusion')
跟蹤和傳感器融合子系統(tǒng)的主要塊是多對(duì)象跟蹤器塊，其輸入是所有傳感器探測(cè)的組合列表和預(yù)測(cè)時(shí)間。多對(duì)象跟蹤器塊的輸出是確認(rèn)的跟蹤列表。 探測(cè)融合模塊將視覺(jué)和毫米波雷達(dá)探測(cè)信息融合起來(lái)。預(yù)測(cè)時(shí)間由車輛與環(huán)境子系統(tǒng)中的時(shí)鐘來(lái)驅(qū)動(dòng)。 探測(cè)聚類塊對(duì)多個(gè)毫米波雷達(dá)探測(cè)進(jìn)行聚類，因?yàn)楦櫰飨Ｍ總€(gè)物體每個(gè)傳感器最多探測(cè)一個(gè)目標(biāo)。 findLeadCar MATLAB函數(shù)塊利用確認(rèn)的跟蹤列表和道路的曲率，在同一車道上找到離被控車輛最近并在其前方的哪輛車。這輛車稱為領(lǐng)先車，當(dāng)車輛駛?cè)牒婉偝霰豢剀囕v前方的車道時(shí)，可能會(huì)發(fā)生變化。該功能提供了領(lǐng)先車相對(duì)于被控車輛的位置和速度，以及最重要物體（MIO）跟蹤的索引。 43.4 自適應(yīng)巡航控制器自適應(yīng)巡航控制器有兩種變體：一種是經(jīng)典設(shè)計(jì)（默認(rèn)），一種是基于MPC的設(shè)計(jì)。對(duì)于這兩種設(shè)計(jì)，均采用以下設(shè)計(jì)原則：配備ACC的車輛（被控車輛）使用傳感器融合來(lái)估計(jì)與前車的相對(duì)距離和相對(duì)速度。ACC使被控車輛以駕駛員設(shè)定的速度行駛，或與領(lǐng)先車保持安全距離。領(lǐng)先車與被控車輛之間的安全距離定義為：
其中默認(rèn)間距，和時(shí)隙為設(shè)計(jì)參數(shù)，為被控車輛的縱向速度。ACC根據(jù)以下輸入生成被控車輛的縱向加速度： - 被控車車的縱向速度。 - 領(lǐng)頭車與被控車輛之間的相對(duì)距離（來(lái)自跟蹤與傳感器融合系統(tǒng)）。 - 領(lǐng)頭車和被控車之間的相對(duì)速度（來(lái)自跟蹤和傳感器融合系統(tǒng)）。 考慮到被控車輛的物理限制，縱向加速度被約束在[-3,2]范圍內(nèi)。 在經(jīng)典的ACC設(shè)計(jì)中，如果相對(duì)距離小于安全距離，那么首要目標(biāo)是減速并保持安全距離。如果相對(duì)距離大于安全距離，那么主要目標(biāo)是在保持安全距離的同時(shí)達(dá)到駕駛員設(shè)定的速度。 這些設(shè)計(jì)原則是通過(guò)Min和Switch塊來(lái)實(shí)現(xiàn)的。 open_system('ACCWithSensorFusionMdlRef/Adaptive Cruise Controller/ACC Classical')
在基于MPC的ACC設(shè)計(jì)中，基本的優(yōu)化問(wèn)題是在約束條件下，通過(guò)跟蹤駕駛員設(shè)定的速度來(lái)制定的。該約束條件強(qiáng)制要求相對(duì)距離總是大于安全距離。 要配置自適應(yīng)巡航控制系統(tǒng)塊，請(qǐng)使用幫助者ACCSetUp文件中定義的參數(shù)。例如，ACC設(shè)計(jì)的線性模型 ，并從車輛動(dòng)力學(xué)中獲得。兩個(gè)Switch塊實(shí)現(xiàn)了簡(jiǎn)單的邏輯，以處理來(lái)自傳感器的大數(shù)（例如，當(dāng)傳感器沒(méi)有探測(cè)到MIO時(shí)，它可能會(huì)返回Inf）。 open_system('ACCWithSensorFusionMdlRef/Adaptive Cruise Controller/ACC Model PredictiveControl')
43.5 車輛與環(huán)境車輛與環(huán)境子系統(tǒng)由兩部分組成： 1 車輛動(dòng)力學(xué)和全球坐標(biāo) 2 執(zhí)行器和傳感器仿真 open_system('ACCTestBenchExample/Vehicle and Environment') 車輛動(dòng)力學(xué)子系統(tǒng)使用自動(dòng)駕駛工具箱中的自行車模型-力輸入塊對(duì)車輛動(dòng)力學(xué)進(jìn)行建模。車輛動(dòng)力學(xué)，輸入（縱向加速度）和前轉(zhuǎn)向角，用以下方法。 在狀態(tài)向量中，表示橫向速度，表示縱向速度，表示偏航角。車輛的參數(shù)在幫助程序ACCSetUp文件中提供。 車輛動(dòng)力學(xué)的輸出（如縱向速度和橫向速度）是基于車身固定坐標(biāo)的。為了得到車輛在運(yùn)動(dòng)過(guò)程中的跟蹤，車身固定坐標(biāo)通過(guò)以下關(guān)系轉(zhuǎn)換成全局坐標(biāo)。 偏航角和偏航角率也換算成度的單位。 駕駛員轉(zhuǎn)向模型的目標(biāo)是通過(guò)控制前部轉(zhuǎn)向角，使車輛保持在自己的車道上，并沿著彎曲的道路行駛。這個(gè)目標(biāo)是通過(guò)駕駛偏航角誤差和側(cè)向位移誤差為零來(lái)實(shí)現(xiàn)的（見(jiàn)下圖），其中
所需的偏航角率由Vx/R給出。 執(zhí)行器和傳感器仿真子系統(tǒng)生成跟蹤和傳感器融合所需的合成傳感器數(shù)據(jù)。在運(yùn)行此示例之前，使用駕駛場(chǎng)景設(shè)計(jì)器應(yīng)用程序創(chuàng)建一個(gè)具有彎曲道路和多個(gè)執(zhí)行器在道路上移動(dòng)的場(chǎng)景。然后將該場(chǎng)景中的道路和執(zhí)行器保存到場(chǎng)景文件ACCTestBenchScenario.mat中。要了解如何定義場(chǎng)景，請(qǐng)點(diǎn)贊、轉(zhuǎn)發(fā)此文，小編后續(xù)不斷更新。 open_system('ACCTestBenchExample/Vehicle and Environment/Actors and Sensor Simulation')
被控車輛的運(yùn)動(dòng)是由控制系統(tǒng)控制的，并不是從場(chǎng)景文件中讀取的。取而代之的是，被控車輛的姿態(tài)、速度、偏航角和偏航率被接收為來(lái)自車輛動(dòng)力學(xué)塊的輸入，并使用packEgo MATLAB函數(shù)塊打包成一個(gè)單一的執(zhí)行器姿態(tài)結(jié)構(gòu)。 Scenario Reader 塊從場(chǎng)景文件 ACCTestBenchScenario.mat 讀取交通參與者姿勢(shì)數(shù)據(jù)。該塊將場(chǎng)景的世界坐標(biāo)中的執(zhí)行器姿勢(shì)轉(zhuǎn)換為被控車輛坐標(biāo)。被上傳到由塊生成的總線上。 在這個(gè)例子中，使用了視覺(jué)探測(cè)生成塊和毫米波雷達(dá)探測(cè)生成塊。 這兩個(gè)傳感器都是遠(yuǎn)距離和前向的，并提供了對(duì)被控車輛前部的良好覆蓋范圍，這是ACC的需要。傳感器使用被控車輛坐標(biāo)中的交通參與者姿勢(shì)來(lái)生成被控車輛前方的車輛探測(cè)列表。最后，以一個(gè)時(shí)鐘塊為例，說(shuō)明車輛將有一個(gè)集中的時(shí)間源。時(shí)間由多對(duì)象跟蹤器塊使用。 43.6 場(chǎng)景創(chuàng)建駕駛場(chǎng)景設(shè)計(jì)器應(yīng)用允許定義道路和在道路上移動(dòng)的車輛。在本例中， 定義了兩條曲率不變的平行道路。要定義道路， 需要定義：道路中心、道路寬度和傾斜角度（如果需要）。道路中心是通過(guò)沿圓弧取樣點(diǎn)選擇的，跨度為60度的恒定曲率半徑的轉(zhuǎn)彎。 定義了場(chǎng)景中的所有車輛。為了定義車輛的運(yùn)動(dòng)，通過(guò)一組航點(diǎn)和速度來(lái)定義它們的跟蹤。定義航點(diǎn)的快速方法是選擇前面定義的道路中心的一個(gè)子集，并在道路中心的左邊或右邊偏移，以控制車輛行駛的車道。 這個(gè)例子顯示了四輛汽車：一輛快速行駛的汽車在左側(cè)車道上，一輛緩慢行駛的汽車在右側(cè)車道上，一輛汽車在馬路對(duì)面駛來(lái)，一輛汽車從右側(cè)車道上開(kāi)始行駛，但隨后移動(dòng)到左側(cè)車道上以超越緩慢行駛的汽車。 場(chǎng)景可以使用駕駛場(chǎng)景設(shè)計(jì)器應(yīng)用程序進(jìn)行修改，并重新保存到同一個(gè)場(chǎng)景文件ACCTestBenchScenario.mat中。當(dāng)仿真重新運(yùn)行時(shí)，場(chǎng)景閱讀器塊會(huì)自動(dòng)拾取更改。如果要以編程方式構(gòu)建場(chǎng)景，可以使用助記場(chǎng)景授權(quán)功能（helperScenarioAuthoring）。
43.7 控制邏輯代碼生成雖然整個(gè)模型不支持代碼生成，但 ACCWithSensorFusionMdlRef 模型被配置為支持使用 Embedded Coder 軟件生成 C 代碼。要檢查是否有訪問(wèn)Embedded Coder的權(quán)限，請(qǐng)運(yùn)行。 hasEmbeddedCoderLicense = license('checkout','RTW_Embedded_Coder')可以為模型生成一個(gè)C函數(shù)，并通過(guò)運(yùn)行探索代碼生成報(bào)告。 if hasEmbeddedCoderLicense rtwbuild('ACCWithSensorFusionMdlRef')end可以使用軟件在環(huán)(SIL)仿真來(lái)驗(yàn)證編譯后的C代碼的行為是否符合預(yù)期。要在SIL模式下仿真ACCWithSensorFusionMdlRef引用的模型，請(qǐng)使用。 if hasEmbeddedCoderLicenseset_param('ACCTestBenchExample/ACC with Sensor Fusion',... 'SimulationMode','Software-in-the-loop (SIL)')end當(dāng) 運(yùn)行ACCTestBenchExample模型時(shí)，將為ACCWithSensorFusionMdlRef模型生成、編譯和執(zhí)行代碼。這使能夠通過(guò)仿真來(lái)測(cè)試編譯代碼的行為。 43.8 結(jié)論本例展示了如何在彎道上用傳感器融合實(shí)現(xiàn)一個(gè)集成的自適應(yīng)巡航控制器（ACC），在Simulink中使用自動(dòng)駕駛工具箱生成的合成數(shù)據(jù)進(jìn)行測(cè)試，對(duì)其進(jìn)行組件化，并自動(dòng)生成代碼。
|
