前置學(xué)習(xí)內(nèi)容：自動(dòng)駕駛控制算法

【自動(dòng)駕駛】【零基礎(chǔ)】基礎(chǔ)自動(dòng)駕駛控制算法筆記_免費(fèi)教學(xué)錄影帶的博客-CSDN博客

自動(dòng)駕駛決策規(guī)劃第一章

自動(dòng)駕駛決策規(guī)劃算法第一章_免費(fèi)教學(xué)錄影帶的博客-CSDN博客_自動(dòng)駕駛五次多項(xiàng)式

自動(dòng)駕駛決策規(guī)劃第二章——理論篇

自動(dòng)駕駛決策規(guī)劃算法第二章——Apollo EM Planner理論篇_免費(fèi)教學(xué)錄影帶的博客-CSDN博客

?4、代碼實(shí)踐部分

（1）感知模塊搭建

首先是感知模塊的搭建。由于要避障，所以需要感知，需要看到障礙物

首先來(lái)到prescan加傳感器，這里選擇AIR理想傳感器，注意這里還要把范圍改成60m

?Prescan探測(cè)到障礙物會(huì)將其用矩形框框出來(lái)，沒(méi)辦法給出障礙物準(zhǔn)確位置，是一種簡(jiǎn)化計(jì)算。矩形框中心和障礙物中心不一定一致，BBox輸出框的外圍信息，Center輸出框中心，CoG輸出重心，這里選擇Center。

?Prescan處理動(dòng)態(tài)障礙物的坐標(biāo)系與我們的不一樣，它以北為正坐標(biāo)軸，以北為0°，而世界坐標(biāo)系是以東為0°，下面代碼要進(jìn)行轉(zhuǎn)換

?接下來(lái)放置障礙物，在探測(cè)范圍外面

確定傳感器與車的相對(duì)位置?，拖放一個(gè)box在車上

其x為11.6，車的坐標(biāo)為9.48。

車的位置和傳感器相對(duì)位置大概是2.16，因此需要在判斷障礙物的坐標(biāo)時(shí)要加上2.16。（個(gè)人理解，這里把車原點(diǎn)繞著2.16半徑的圓，保證圓內(nèi)無(wú)障礙物）

接下來(lái)build然后regenerate回到matlab

首先可以看到上面設(shè)置的AIR傳感器的輸出，先處理輸出數(shù)據(jù)（障礙物在傳感器坐標(biāo)系下的極坐標(biāo)range，theta，障礙物的速度大小velocity，障礙物的速度方向與x軸的夾角heading）

先把輸出的數(shù)據(jù)拉出來(lái)，準(zhǔn)備進(jìn)行坐標(biāo)轉(zhuǎn)換

?新建一個(gè)子模塊來(lái)處理，輸入定位信息，當(dāng)前車的位置xy以及方向heading

去外面把線都連進(jìn)來(lái)，輸入為location info和傳感器信息

?接下來(lái)寫一個(gè)函數(shù)來(lái)處理傳感器信息，輸出障礙物的位置速度以及方向

function [obs_x_set_gcs,obs_y_set_gcs,obs_velocity_set_gcs,obs_heading_set_gcs] = ...fcn(host_x,host_y,host_heading_xy,obs_range,obs_theta,obs_velocity,obs_velocity_heading) %gcs 全局坐標(biāo)系global coordinate system %該函數(shù)是感知模塊，輸出障礙物的位置，速度大小，速度方向與世界坐標(biāo)系x軸的夾角 % 輸入:定位信息host_x, host_y, host_heading_xy 世界坐標(biāo)系 % 障礙物在傳感器坐標(biāo)系下的極坐標(biāo)range，theta % 障礙物的速度大小velocity % 障礙物的速度方向與x軸的夾角heading % 輸出:障礙物在世界坐標(biāo)系下的坐標(biāo)x y v v與x軸的夾角heading % 注意輸出的heading是障礙物的速度heading，不是障礙物的幾何heading。是障礙物速度方向不是障礙物幾何方向 % 幾何heading并不重要，因?yàn)閜rescan會(huì)用一個(gè)框表示%傳感器最多輸出32個(gè)障礙物信息，但是事先不知道有多少個(gè)障礙物，所以要做個(gè)緩沖% %輸出初始化 obs_x_set_gcs = ones(32,1) * nan; obs_y_set_gcs = ones(32,1) * nan; obs_velocity_set_gcs = ones(32,1) * nan; obs_heading_set_gcs = ones(32,1) * nan; % %傳感器極坐標(biāo)的角度順時(shí)針為正，世界坐標(biāo)是逆時(shí)針為正 theta_transfer = -obs_theta * pi/180; %再加上角度轉(zhuǎn)弧度 % %速度的方向以北為0°，世界坐標(biāo)系是以東為0° heading_transfer = (-obs_velocity_heading + 90 * ones(32,1)) * pi/180; % % 世界坐標(biāo)系下障礙物距離車的x和y距離 x_transfer = obs_range .* cos(theta_transfer + host_heading_xy); y_transfer = obs_range .* sin(theta_transfer + host_heading_xy); %計(jì)算障礙物的世界坐標(biāo) for i = 1:32if obs_range(i) ~= 0obs_x_set_gcs(i) = x_transfer(i) + host_x + 2.16*cos(host_heading_xy);obs_y_set_gcs(i) = y_transfer(i) + host_y + 2.16*sin(host_heading_xy);obs_heading_set_gcs(i) = heading_transfer(i);obs_velocity_set_gcs(i) = obs_velocity(i);end end

這樣感知模塊就寫完了，我們現(xiàn)在獲得了障礙物在全局坐標(biāo)系下的位置，速度以及方向，將他們打包輸出出去，作為perception info

（2）規(guī)劃信號(hào)前處理

接下來(lái)進(jìn)行規(guī)劃模塊預(yù)處理

定位模塊加入加速度

?進(jìn)入planning模塊，增加planning_result輸出

決策規(guī)劃模塊增加三個(gè)輸入，感知信息，上次規(guī)劃信息以及當(dāng)前時(shí)間，

當(dāng)前時(shí)間使用time模塊

?將輸出的result加延遲1s作為上個(gè)處理結(jié)果輸入給決策規(guī)劃模塊

?然后新建EM Planner模塊，把輸入信息加進(jìn)去

?將該有的輸入輸入進(jìn)去整理好

?至此，規(guī)劃信號(hào)前處理完畢

（3）規(guī)劃起點(diǎn)算法

決策規(guī)劃的第一步是確立規(guī)劃起點(diǎn)，根據(jù)上一節(jié)的結(jié)論，分為第一次運(yùn)行和不是第一次運(yùn)行，第一次運(yùn)行的時(shí)候以當(dāng)前車位置為起點(diǎn)，后面再運(yùn)行的時(shí)候以上次規(guī)劃的結(jié)果，本周期的規(guī)劃起點(diǎn)為

新建一個(gè)函數(shù)確定起點(diǎn)并拼接上一段規(guī)劃的軌跡

接下來(lái)寫一下函數(shù)，算法運(yùn)行流程如下：如果是第一次運(yùn)行，獲取當(dāng)前車輛的信息作為規(guī)劃起點(diǎn)，然后設(shè)置規(guī)劃時(shí)間為0+100ms；如果不是第一次運(yùn)行，說(shuō)明我們已經(jīng)有上個(gè)周期的軌跡了，這個(gè)軌跡是帶有時(shí)間戳信息的，根據(jù)當(dāng)前時(shí)間可以查到車輛根據(jù)上個(gè)軌跡應(yīng)該所處的位置(pre_x_desire,pre_y_desire)，然后計(jì)算當(dāng)前位置和上個(gè)周期該時(shí)間對(duì)應(yīng)位置之間的橫縱向誤差lon_err和lat_err，如果縱向誤差大于2.5，橫向誤差大于0.5，則認(rèn)為控制沒(méi)跟上，這種情況我們使用車輛動(dòng)力學(xué)模型推出規(guī)劃起點(diǎn)（這里使用質(zhì)點(diǎn)模型外推）（注意：這里推出來(lái)的是當(dāng)前時(shí)間往后100ms的位置作為規(guī)劃起點(diǎn)）。如果誤差合理，我們認(rèn)為上個(gè)周期該時(shí)間對(duì)應(yīng)位置作為規(guī)劃起點(diǎn)，接下來(lái)計(jì)算拼接軌跡，假設(shè)pre_trajectory_time的第j個(gè)等于當(dāng)前周期后100ms，如果j大于20，就把j-19到j(luò)這20個(gè)軌跡點(diǎn)提取出來(lái)作為拼接軌跡，如果j小于20，就從起點(diǎn)開始能拼多少就多少個(gè)。

最終可以得到規(guī)劃起點(diǎn)以及拼接軌跡。

function [plan_start_x,plan_start_y,plan_start_heading,plan_start_kappa,plan_start_vx,plan_start_vy,plan_start_ax,plan_start_ay,...plan_start_time,stitch_x,stitch_y,stitch_heading,stitch_kappa,stitch_speed,stitch_accel,stitch_time] = ...fcn(pre_trajectory_x,pre_trajectory_y,pre_trajectory_heading,pre_trajectory_kappa,pre_trajectory_velocity,...pre_trajectory_accel,pre_trajectory_time,current_time,host_x,host_y,host_heading_xy,host_vx,host_vy,host_ax,host_ay) % 該函數(shù)將計(jì)算規(guī)劃的起點(diǎn)以及拼接軌跡的信息 % 輸入 上一周期的規(guī)劃結(jié)果信息pre_trajectory x , y, heading, kappa,velocity, accel,time % 本周期的絕對(duì)時(shí)間current_time % 定位信息host x, y, heading, kappa，vx，vy, ax, ay % 輸出 規(guī)劃起點(diǎn)信息plan_start x, y, heading, kappa, vx,vy, ax, ay(vx, vy, ax, ay)是世界坐標(biāo)系的 % 待拼接的軌跡信息，一般在上一周期的軌跡中的current_time+100ms，往前取20個(gè)點(diǎn)。 % 當(dāng)規(guī)劃完成后，本周期的規(guī)劃結(jié)果和stitch_trajectory一起拼好發(fā)給控制%輸出初始化 stitch_x = zeros(20,1); stitch_y = zeros(20,1); stitch_heading = zeros(20,1); stitch_kappa = zeros(20,1); stitch_speed = zeros(20,1); stitch_accel = zeros(20,1); % 時(shí)間為負(fù)代表沒(méi)有對(duì)應(yīng)的拼接軌跡 stitch_time = zeros(20,1)*-1;%聲明全局變量 persistent is_first_run; if isempty(is_first_run)%代表代碼首次運(yùn)行is_first_run = 0;% 按照軌跡拼接算法，應(yīng)該用車輛動(dòng)力學(xué)遞推，但是首次運(yùn)行車輛的速度都是0，所以沒(méi)必要遞推了plan_start_x = host_x;plan_start_y = host_y;plan_start_heading = host_heading_xy;plan_start_kappa = 0;plan_start_vx = 0;plan_start_vy = 0;plan_start_ax = 0;plan_start_ay = 0;%規(guī)劃的起點(diǎn)的時(shí)間應(yīng)該是當(dāng)前的時(shí)間+100msplan_start_time = current_time + 0.1; else%該函數(shù)不是首次運(yùn)行x_cur = host_x;y_cur = host_y;heading_cur = host_heading_xy;kappa_cur = 0;%vx vy ax ay 要轉(zhuǎn)換成世界坐標(biāo)系vx_cur = host_vx * cos(heading_cur) - host_vy * sin(heading_cur);vy_cur = host_vx * sin(heading_cur) + host_vy * cos(heading_cur);ax_cur = host_ax * cos(heading_cur) - host_ay * sin(heading_cur);ay_cur = host_ax * sin(heading_cur) + host_ay * cos(heading_cur); % 規(guī)劃周期dt = 0.1;%由于不是首次運(yùn)行，有上個(gè)時(shí)刻的軌跡了，找到上一周期軌跡規(guī)劃的本周期車輛應(yīng)該在的位置for i = 1: length(pre_trajectory_time)if(pre_trajectory_time(i) <= current_time && pre_trajectory_time(i+1)>current_time)break;endend%上一周期規(guī)劃的本周期的車應(yīng)該在的位置pre_x_desire = pre_trajectory_x(i);pre_y_desire = pre_trajectory_y(i);pre_heading_desire = pre_trajectory_heading(i);%計(jì)算橫縱向誤差% 切向量tor = [cos(pre_heading_desire);sin(pre_heading_desire)];%法向量nor = [-sin(pre_heading_desire);cos(pre_heading_desire)];%誤差向量d_err = [host_x;host_y] - [pre_x_desire;pre_y_desire];%縱向誤差lon_err = abs(d_err' * tor);%橫向誤差lat_err =abs(d_err' * nor);%縱向誤差大于2.5，橫向誤差大于0.5，認(rèn)為控制沒(méi)跟上if(lon_err>2.5 || lat_err > 2.5)%此分支處理控制未跟上的情況：規(guī)劃起點(diǎn)使用運(yùn)動(dòng)學(xué)（質(zhì)點(diǎn)模型）外推plan_start_x = x_cur + vx_cur * dt + 0.5 * ax_cur * dt *dt;plan_start_y = y_cur + vy_cur * dt + 0.5 * ay_cur * dt *dt;plan_start_vx = vx_cur + ax_cur*dt; plan_start_vy = vy_cur + ay_cur*dt; plan_start_ax = ax_cur;plan_start_ay = ay_cur;plan_start_kappa = kappa_cur;plan_start_time = current_time + 0.1;else% 此分支表示控制能跟的上的規(guī)劃，開始拼接for j = 1:length(pre_trajectory_time)if(pre_trajectory_time(j) <= current_time +0.1 && pre_trajectory_time(j+1)>current_time +0.1)break;endend%注：此算法要能運(yùn)行需要trajectory的點(diǎn)有很高的密度plan_start_x = pre_trajectory_x(j);plan_start_y = pre_trajectory_y(j);plan_start_heading = pre_trajectory_heading(j);plan_start_kappa = pre_trajectory_kappa(j);% 這里的pre_trajectory的速度和加速度是指軌跡的切向速度，切向加速度plan_start_vx = pre_trajectory_velocity(j) * cos(plan_start_heading);plan_start_vy = pre_trajectory_velocity(j) * sin(plan_start_heading);%pre_trajectory的加速度是切向加速度，要想計(jì)算ax,ay還要計(jì)算法向加速度%計(jì)算軌跡在current_time+100ms這個(gè)點(diǎn)的切向量法向量tor = [cos(plan_start_heading);sin(plan_start_heading)];nor = [-sin(plan_start_heading);cos(plan_start_heading)];a_tor = pre_trajectory_accel(j) * tor;a_nor = pre_trajectory_velocity(j)^2 * plan_start_kappa * nor;plan_start_ax = a_tor(1) + a_nor(1);plan_start_ay = a_tor(2) + a_nor(2);plan_start_time = pre_trajectory_time(j);% 計(jì)算拼接軌跡%j 表示 current_time + 0.1的時(shí)間點(diǎn)在 pre_trajectory_time的編號(hào)%拼接是往從j開始，往前拼接20個(gè)，也就是pre_trajectory(j-1),j-2,j-3,... .j-19%分兩種情況，如果j小于20，意味著前面的軌跡點(diǎn)不夠20個(gè)%如果j >= 20,意味著前面的點(diǎn)多于20個(gè)%還有一個(gè)細(xì)節(jié)需要考慮，pre_trajectory x(j) y(j) ....是規(guī)劃的起點(diǎn)，那么在軌跡拼接時(shí)%需不需要在stitch_trajectory中把pre_trajectory x(j) y(j) ....包含進(jìn)去%答案是否定的，不應(yīng)該包進(jìn)去，因?yàn)橐?guī)劃的起點(diǎn)會(huì)在規(guī)劃模塊計(jì)算完成后的結(jié)果中包含，如果在拼接的時(shí)候%還要包含，那就等于規(guī)劃起點(diǎn)包含了兩次%除非本周期計(jì)算的軌跡不包含規(guī)劃起點(diǎn)，那么stitch_trajectory可以包含規(guī)劃起點(diǎn)。%如果本周期計(jì)算的軌跡包含規(guī)劃起點(diǎn)，那么stitch_trajectory就不可以包含規(guī)劃起點(diǎn)。%我們選擇規(guī)劃包含起點(diǎn)，拼接不包含起點(diǎn)的做法%把起點(diǎn)去掉j = j - 1;if j >= 20stitch_x = pre_trajectory_x(j-19:j);stitch_y = pre_trajectory_y(j-19:j);stitch_heading = pre_trajectory_heading(j-19:j);stitch_kappa = pre_trajectory_kappa(j-19:j);stitch_speed = pre_trajectory_velocity(j-19:j);stitch_accel = pre_trajectory_accel(j-19:j);stitch_time = pre_trajectory_time(j-19:j);else %能拼多少就拼多少個(gè)stitch_x(20-j+1:20) = pre_trajectory_x(1:j);stitch_y(20-j+1:20) = pre_trajectory_y(1:j);stitch_heading(20-j+1:20) = pre_trajectory_heading(1:j);stitch_kappa(20-j+1:20) = pre_trajectory_kappa(1:j);stitch_speed(20-j+1:20) = pre_trajectory_velocity(1:j);stitch_accel(20-j+1:20) = pre_trajectory_accel(1:j);stitch_time(20-j+1:20) = pre_trajectory_time(1:j);endendend

接下來(lái)連接輸入輸出

（4）index2s算法

接下來(lái)做的是把直角坐標(biāo)轉(zhuǎn)化為自然坐標(biāo)，規(guī)劃起點(diǎn)和障礙物都是以世界坐標(biāo)給定的，都需要投影到參考線上（自然坐標(biāo)），坐標(biāo)轉(zhuǎn)換之前已經(jīng)講過(guò)了，直接套公式即可，比較麻煩的是求s，因?yàn)樾枰煌／B加，如果有10個(gè)障礙物，每個(gè)障礙物的s都需要疊加，希望找到一種快速計(jì)算s的方法。因?yàn)閰⒖季€referenceline是一堆離散點(diǎn)，所以可以實(shí)現(xiàn)找到離散點(diǎn)編號(hào)對(duì)應(yīng)的s，比如1號(hào)點(diǎn)對(duì)應(yīng)s=0，2號(hào)點(diǎn)s對(duì)應(yīng)0.1之類的，三號(hào)點(diǎn)s對(duì)應(yīng)0.3。有了這個(gè)對(duì)應(yīng)關(guān)系，算s就比較簡(jiǎn)單了，因?yàn)檎系K物要計(jì)算匹配點(diǎn)，前面已經(jīng)求過(guò)了匹配點(diǎn)的編號(hào)，所以一旦有了匹配點(diǎn)的編號(hào)可以立刻得到s是什么，有了匹配點(diǎn)的s再計(jì)算投影點(diǎn)的s就比較簡(jiǎn)單了，對(duì)于障礙物越多的情況越顯著。

所以我們要做這個(gè)表，命名為：index2s表。在制作之前，首先要確定自然坐標(biāo)系的坐標(biāo)原點(diǎn)的x，y。因?yàn)橐话銇?lái)說(shuō)自然坐標(biāo)系的原點(diǎn)不是以referenceline的第一個(gè)點(diǎn)為坐標(biāo)原點(diǎn)，一般是以車輛定位投影到referenceline上的投影點(diǎn)作為坐標(biāo)原點(diǎn)，這樣的話index是負(fù)數(shù)，我們要先計(jì)算坐標(biāo)原點(diǎn)的x和y，然后再去計(jì)算index和s的對(duì)應(yīng)表。

打開referenceline provider模塊，把批量處理投影點(diǎn)模塊復(fù)制一份，然后把車輛當(dāng)前位置以及參考線信息輸入進(jìn)去，輸出接上一個(gè)選擇第一個(gè)index的selector，獲取第一個(gè)元素。

然后使用bus輸出出去

?回到EM Planner模塊，處理一下info

?接下來(lái)新建一個(gè)function來(lái)寫index2s函數(shù)

function index2s = fcn(path_x, path_y, origin_x, origin_y, origin_match_point_index) %該算法將計(jì)算出index與s的轉(zhuǎn)換關(guān)系，index2s(i)表示當(dāng)編號(hào)為i時(shí)，對(duì)應(yīng)的弧長(zhǎng)s n = 181; %參考線有180個(gè)點(diǎn)，所以index2s也是180 index2s = zeros(n,1); %這個(gè)循環(huán)計(jì)算的是以軌跡起點(diǎn)為坐標(biāo)原點(diǎn)的index2s的轉(zhuǎn)換關(guān)系 for i = 2:181index2s(i) = sqrt((path_x(i) - path_x(i-1))^2 + (path_y(i) - path_y(i-1))^2) + index2s(i-1); end % 在計(jì)算起點(diǎn)到坐標(biāo)原點(diǎn)的弧長(zhǎng)s0，減去s0，就得到了以給定坐標(biāo)原點(diǎn)為起點(diǎn)的index2s的關(guān)系 s0 = CalcSFromIndex2S(index2s,path_x, path_y,origin_x, origin_y, origin_match_point_index); %計(jì)算完s0再用原index2s減去s0 index2s = index2s - ones(n,1) * s0; endfunction s = CalcSFromIndex2S(index2s,path_x, path_y,proj_x, proj_y, proj_match_point_index) %該函數(shù)將計(jì)算給定index2s的映射關(guān)系后，計(jì)算點(diǎn)(proj_x, proj_y)所對(duì)應(yīng)的弧長(zhǎng) %判斷投影點(diǎn)在匹配點(diǎn)的前面還是后面，向量的內(nèi)積可以判斷前后 vector_1 = [proj_x; proj_y] - [path_x(proj_match_point_index);path_y(proj_match_point_index)];% 投影點(diǎn)和匹配點(diǎn)之間的誤差的向量 % 匹配點(diǎn)的切線的方向向量vector_2，使用匹配點(diǎn)和匹配點(diǎn)前面的一個(gè)點(diǎn)之間的向量做近似 if proj_match_point_index < length(path_x) % 說(shuō)明不是最后一個(gè)點(diǎn)vector_2 = [path_x(proj_match_point_index+1);path_y(proj_match_point_index+1)] - ...[path_x(proj_match_point_index);path_y(proj_match_point_index)]; else % 如果是最后一個(gè)點(diǎn)，就拿這個(gè)點(diǎn)減去前面那個(gè)點(diǎn)vector_2 = [path_x(proj_match_point_index);path_y(proj_match_point_index)] - ...[path_x(proj_match_point_index-1);path_y(proj_match_point_index-1)]; end %使用向量?jī)?nèi)積判斷投影點(diǎn)在匹配點(diǎn)的前面還是后面if vector_1' * vector_2 > 0% 投影點(diǎn)在匹配點(diǎn)的前面s = index2s(proj_match_point_index) + sqrt(vector_1' * vector_1); else% 投影點(diǎn)在匹配點(diǎn)的后面s = index2s(proj_match_point_index) - sqrt(vector_1' * vector_1); end end

有了這個(gè)模塊，就不需要每次去加載，只需要查表就可以獲取s

?（5）規(guī)劃起點(diǎn)的坐標(biāo)轉(zhuǎn)換

接下來(lái)要把規(guī)劃起點(diǎn)轉(zhuǎn)換到SL坐標(biāo)系

首先求投影點(diǎn)，之前寫過(guò)了，直接復(fù)制過(guò)來(lái)用

接下來(lái)進(jìn)行坐標(biāo)轉(zhuǎn)換

新建一個(gè)函數(shù)，創(chuàng)建子系統(tǒng)

?對(duì)于靜態(tài)障礙物，只需要s和l即可，對(duì)于\dot{s},\dot{l}這些不關(guān)心。如果對(duì)于動(dòng)態(tài)障礙物來(lái)說(shuō)，需要輸出\dot{s},\dot{l}這些，而對(duì)于規(guī)劃起點(diǎn)，還需要加速度信息，不僅要\dot{s},\dot{l}，還需要\ddot{s},\ddot{l}，不同的變量對(duì)于坐標(biāo)轉(zhuǎn)換的要求不同，我們寫一個(gè)通用的算法

首先設(shè)置輸入

?然后先寫轉(zhuǎn)s和l的算法

function [s_set,l_set] = fcn(x_set,y_set,frenet_path_x,frenet_path_y,...proj_x_set, proj_y_set, proj_heading_set, proj_match_point_set,index2s)% 該函數(shù)將計(jì)算世界坐標(biāo)系下的x_set，y_set上的點(diǎn)在frenet_path下的坐標(biāo)s 1 % 輸入x_set, y_set待坐標(biāo)轉(zhuǎn)換的點(diǎn) % frenet_path_x,frenet_path_y frenet坐標(biāo)軸 % proj_x, y, heading, kappa,proj_match_point_index待坐標(biāo)轉(zhuǎn)換的點(diǎn)的投影點(diǎn)的信息 % index2s frenet_path的index與s的轉(zhuǎn)換表%由于不知道有多少個(gè)點(diǎn)需要做坐標(biāo)轉(zhuǎn)換，所以需要做緩沖 n = 128;%最多處理128個(gè)點(diǎn) %輸出初始化 s_set = ones(n,1)*nan; l_set = ones(n,1)*nan; for i = 1:length(x_set)if isnan(x_set(i))break;end%計(jì)算s，寫個(gè)子函數(shù)s_set(i) = CalcSFromIndex2S(index2s,frenet_path_x,frenet_path_y,proj_x_set(i), proj_y_set(i),...proj_match_point_set(i));n_r = [-sin(proj_heading_set(i));cos(proj_heading_set(i))];r_h = [x_set(i);y_set(i)];r_r = [proj_x_set(i);proj_y_set(i)];l_set(i) = (r_h - r_r)' * n_r;endfunction s = CalcSFromIndex2S(index2s,path_x, path_y,proj_x, proj_y, proj_match_point_index) %該函數(shù)將計(jì)算給定index2s的映射關(guān)系后，計(jì)算點(diǎn)(proj_x, proj_y)所對(duì)應(yīng)的弧長(zhǎng) %判斷投影點(diǎn)在匹配點(diǎn)的前面還是后面，向量的內(nèi)積可以判斷前后 vector_1 = [proj_x; proj_y] - [path_x(proj_match_point_index);path_y(proj_match_point_index)];% 投影點(diǎn)和匹配點(diǎn)之間的誤差的向量 % 匹配點(diǎn)的切線的方向向量vector_2，使用匹配點(diǎn)和匹配點(diǎn)前面的一個(gè)點(diǎn)之間的向量做近似 if proj_match_point_index < length(path_x) % 說(shuō)明不是最后一個(gè)點(diǎn)vector_2 = [path_x(proj_match_point_index+1);path_y(proj_match_point_index+1)] - ...[path_x(proj_match_point_index);path_y(proj_match_point_index)]; else % 如果是最后一個(gè)點(diǎn)，就拿這個(gè)點(diǎn)減去前面那個(gè)點(diǎn)vector_2 = [path_x(proj_match_point_index);path_y(proj_match_point_index)] - ...[path_x(proj_match_point_index-1);path_y(proj_match_point_index-1)]; end %使用向量?jī)?nèi)積判斷投影點(diǎn)在匹配點(diǎn)的前面還是后面if vector_1' * vector_2 > 0% 投影點(diǎn)在匹配點(diǎn)的前面s = index2s(proj_match_point_index) + sqrt(vector_1' * vector_1); else% 投影點(diǎn)在匹配點(diǎn)的后面s = index2s(proj_match_point_index) - sqrt(vector_1' * vector_1); end end

?接下來(lái)寫第二個(gè)模塊，計(jì)算

function [s_dot_set,l_dot_set,dl_set] = fcn(l_set,vx_set,vy_set,proj_heading_set,proj_kappa_set)%該函數(shù)將計(jì)算frenet坐標(biāo)系下的s_dot，l_dot, dl/ ds n = 128; %輸出初始化 s_dot_set = ones(n,1 )*nan ; l_dot_set = ones(n,1) *nan; dl_set = ones (n,1)*nan;for i = 1: length(l_set)if isnan(l_set(i))breakendv_h = [vx_set(i);vy_set(i)];n_r = [-sin(proj_heading_set(i)) ;cos(proj_heading_set(i))];t_r = [cos(proj_heading_set(i)) ;sin(proj_heading_set(i))];l_dot_set(i) = v_h'*n_r;s_dot_set(i) = v_h'*t_r/(1 - proj_kappa_set(i)*l_set(i)) ;%%%向量法做cartesian與frenet的轉(zhuǎn)換要更簡(jiǎn)單，但是也有缺點(diǎn)，向量法必須依賴速度加速度%l’= l_dot/s_dot但是如果s_dot = 0此方法就失效了if abs(s_dot_set(i))<1e-6dl_set(i) = 0;elsedl_set(i) = l_dot_set(i)/s_dot_set(i);end end

?再寫最后一個(gè)模塊，計(jì)算s兩點(diǎn)，l兩點(diǎn)

function [s_dot2_set,l_dot2_set,ddl_set] = fcn(ax_set,ay_set,proj_heading_set,proj_kappa_set,l_set,s_dot_set,dl_set)%由于不知道有多少個(gè)點(diǎn)需要做坐標(biāo)轉(zhuǎn)換，設(shè)一個(gè)最大值做緩沖 n = 128; %輸出初始化 s_dot2_set = ones(n,1)*nan; l_dot2_set = ones(n,1)*nan; ddl_set = ones(n,1) *nan; for i = 1 : length(l_set)if isnan(l_set(i))break;enda_h = [ax_set(i) ;ay_set(i)];n_r = [-sin(proj_heading_set(i)) ; cos(proj_heading_set(i))];t_r = [cos(proj_heading_set(i)) ;sin(proj_heading_set(i)l;%近似認(rèn)為dkr/ds 為0簡(jiǎn)化計(jì)算l_dot2_set(i) = a_h'*n_r - proj_kappa_set(i) * (1 - proj_kappa_set(i) * l_set(i)) * s_dot_set(i)^2;s_dot2_set(i) = (1/(l - proj_kappa_set(i) * l_set(i)))* (a_h' * t_r + 2 * proj_kappa_set(i) * dl_set(i) * s_dot_set(i)^2;%要考慮加速度為0的情況if abs(s_dot2_set(i)) < 1e-6ddl_set(i) = 0;elseddl_set(i) = (l_dot2_set(i) - dl_set(i)*s_dot2_set(i))/(s_dot_set(i)^2);endend

?然后把所有輸出整合起來(lái)

?得到模塊，連接輸入輸出

輸出選擇

?至此，得到規(guī)劃起點(diǎn)的SL，接下來(lái)處理障礙物

（6）障礙物處理與坐標(biāo)轉(zhuǎn)換

新建一個(gè)函數(shù)來(lái)寫

function [obs_x_set_final, obs_y_set_final, obs_velocity_set_final, obs_heading_set_final] = ...fcn(host_x,host_y,host_heading_xy,obs_x_set_gcs,obs_y_set_gcs,obs_velocity_set_gcs,obs_heading_set_gcs) %該函數(shù)將篩選障礙物，縱向[-10,60]橫向[-10,10]的障礙物才會(huì)被考慮 %該函數(shù)只是一種單車道的臨時(shí)辦法，考慮到多車道情況，即使障礙物距離較遠(yuǎn)也應(yīng)該考慮 %EM P1anner完全體是多車道并行計(jì)算的，每個(gè)車道都生成參考線然后并行計(jì)算出多條軌跡，再選擇最優(yōu)的軌跡作為輸出%傳感器最多可以識(shí)別32個(gè)障礙物 n = 32; %輸出初始化 obs_x_set_final = ones(n,1) *nan ; obs_y_set_final = ones(n,1) *nan; obs_velocity_set_final = ones(n,1)*nan; obs_heading_set_final = ones(n,1)*nan;for i = 1: length(obs_x_set_gcs)if isnan(obs_x_set_gcs(i))break;end%自車的heading的方向向量與法向量tor = [cos(host_heading_xy);sin(host_heading_xy)];nor = [-sin(host_heading_xy) ; cos(host_heading_xy)] ;%障礙物與自車的距離向量vector_obs = [obs_x_set_gcs(i) ;obs_y_set_gcs(i)] - [host_x; host_y] ;%障礙物縱向距離lon_distance = vector_obs'*tor;lat_distance = vector_obs'*nor;if (lon_distance < 60) && (lon_distance > -10)&& (lat_distance > -10) && (lat_distance < 10)obs_x_set_final(count) = obs_x_set_gcs (i);obs_y_set_final(count) = obs_y_set_gcs(i);obs_heading_set_final(count) = obs_heading_set_gcs(i);obs_velocity_set_final (count) = obs_velocity_set_gcs(i);count = count + 1;end end

?接下來(lái)計(jì)算障礙物投影點(diǎn)

復(fù)制通用轉(zhuǎn)換模塊，然后把障礙物的信息作為xy輸入進(jìn)去，在輸入給Cartesian2Frenet模塊

?注意，Cartesian2Frenet復(fù)制過(guò)來(lái)注釋掉下面，我們只需要靜態(tài)障礙物的s和l即可

?最后把s和l使用bus輸出出來(lái)，vx，vy，ax，ay都接地

?此外還需要篩選一下靜態(tài)障礙物和動(dòng)態(tài)障礙物

新建一個(gè)函數(shù)

function [static_obs_x_set,static_obs_y_set,dynamic_obs_x_set, dynamic_obs_y_set,dynamic_obs_vx_set,dynamic_obs_vy_set] = ...fcn(obs_x_set_gcs,obs_y_set_gcs,obs_heading_set_gcs,obs_velocity_set_gcs)%該函數(shù)將分類靜態(tài)障礙物和動(dòng)態(tài)障礙物 %輸出初始化 n = 32; static_obs_x_set = ones(n,1)*nan; static_obs_y_set = ones(n,1)*nan; dynamic_obs_x_set = ones(n,1) *nan; dynamic_obs_y_set = ones(n,1) *nan; dynamic_obs_vx_set = ones (n,1)*nan; dynamic_obs_vy_set = ones(n,1) *nan;count_static = 1; count_dynamic = 1; for i = 1:length(obs_x_set_gcs)if abs(obs_velocity_set_gcs(i))< 0.1static_obs_x_set(count_static) = obs_x_set_gcs(i);static_obs_y_set (count_static) = obs_y_set_gcs(i);count_static = count_static + 1;elsedynamic_obs_x_set (count_dynamic) = obs_x_set_gcs(i) ;dynamic_obs_y_set(count_dynamic) = obs_y_set_gcs(i);count_dynamic = count_dynamic + 1;end end

?然后吧這些全部打包成一個(gè)子系統(tǒng)

?設(shè)置輸入和輸出

?到目前，動(dòng)態(tài)規(guī)劃之前的準(zhǔn)備工作已經(jīng)做完了，下面說(shuō)正式的動(dòng)態(tài)規(guī)劃算法

（7）動(dòng)態(tài)規(guī)劃主算法

新建一個(gè)函數(shù)來(lái)寫動(dòng)態(tài)規(guī)劃的代碼

函數(shù)輸入為規(guī)劃起點(diǎn)信息，靜態(tài)障礙物信息，以及動(dòng)態(tài)規(guī)劃系數(shù)

function [dp_path_s,dp_path_l] = fcn(obs_s_set,obs_l_set,plan_start_s,plan_start_l,...plan_start_dl,plan_start_dll,w_cost_collision,w_cost_smooth_dl,w_cost_smooth_ddl,w_cost_smooth_dddl,w_cost_ref, row,col,sample_s,sample_l)%該函數(shù)為路徑?jīng)Q策算法動(dòng)態(tài)規(guī)劃的主函數(shù) % 輸入: obs s 1 set篩選后的障礙物s1信息 % plan_start s 1 d1 ddl規(guī)劃起點(diǎn)信息 % w_cost_obs,w_cost_smooth_dl,w_cost_smooth_ddl,w_cost_smooth_dddl,w_cost_ref動(dòng)態(tài)規(guī)劃代價(jià)權(quán)重 % row col 動(dòng)態(tài)規(guī)劃采樣點(diǎn)的行數(shù)和列數(shù) % sample_s, sample_l 采樣的s 1 長(zhǎng)度 % 輸出: dp_path_l, dp_path_dl, dp_path_s動(dòng)態(tài)規(guī)劃的路徑s1%動(dòng)態(tài)規(guī)劃最優(yōu)的數(shù)據(jù)結(jié)構(gòu)是結(jié)構(gòu)體矩陣，但是simulink不支持結(jié)構(gòu)體矩陣，所以動(dòng)態(tài)規(guī)劃算法寫的相對(duì)難以理解%聲明二維矩陣變量node_cost，node_cost(i,j)表示從起點(diǎn)出發(fā)，到行i列j節(jié)點(diǎn)的最小代價(jià)為node_cost(i, j) node_cost = ones(row, col)*inf ; %初始化為無(wú)窮大 %聲明二維矩陣變量pre_node_index pre_node_index(i,j)表示從起點(diǎn)到行i列j的節(jié)點(diǎn)的最優(yōu)路徑中前一個(gè)節(jié)點(diǎn)的行號(hào)為 %pre_node_index(i, j) %例: 比如一條最優(yōu)路徑為起點(diǎn) -> node(1,1) -> node(2,2) -> node(3,3) %則 pre_node_index(3,3) = 2 (最優(yōu)路徑的前一個(gè)節(jié)點(diǎn)是node(2,2)取它的行號(hào)) % pre_node_index(2,2)=1 (最優(yōu)路徑的前一個(gè)節(jié)點(diǎn)是node(1,1）取它的行號(hào)) % pre_node_index(1,1) =0 (起點(diǎn)的行號(hào)默認(rèn)是0)% 其實(shí)pre_node_index就是當(dāng)找到最小代價(jià)的路徑后從終點(diǎn)到起點(diǎn)反向選擇出最優(yōu)路徑的 %如果用結(jié)構(gòu)體矩陣完全沒(méi)有必要弄得這么麻煩，但是simulink不支持 pre_node_index = zeros(row,col); %初始化，所有節(jié)點(diǎn)的上個(gè)最優(yōu)節(jié)點(diǎn)的行號(hào)都是0，也就是起點(diǎn) %先計(jì)算從起點(diǎn)到第一列的cost for i = 1:row % 使用一個(gè)子函數(shù)計(jì)算第一層的Costnode_cost(i,1) = CalcStartCost(plan_start_s,plan_start_l,...plan_start_dl,plan_start_dll,i,sample_s,sample_l,w_cost_collision,...w_cost_smooth_dl,w_cost_smooth_ddl,w_cost_smooth_dddl,w_cost_ref,...obs_s_set,obs_l_set,row); end% 動(dòng)態(tài)規(guī)劃主代碼 for j = 2:colfor i = 1 : row%計(jì)算當(dāng)前node(i,j)的s lcur_node_s = plan_start_s + j *sample_s;cur_node_l = ((row + 1)/2 - i) *sample_l;%遍歷前一列的節(jié)點(diǎn)for k = 1 :row%計(jì)算前一列節(jié)點(diǎn)的s lpre_node_s = plan_start_s + (j - 1) * sample_s;pre_node_l = ((row + 1)/2 - k) * sample_l;cost_neighbour = CalcNeighbourCost(pre_node_s,pre_node_l,cur_node_s,cur_node_l,w_cost_collision,...w_cost_smooth_dl,w_cost_smooth_ddl,w_cost_smooth_dddl,w_cost_ref,obs_s_set,obs_l_set);% 起點(diǎn)到上一個(gè)節(jié)點(diǎn)的最小代價(jià)為node_cost(k,j-1)%再加上node(k,j-1)到node(i,j)的代價(jià) (k = 1,2,3,4.. .row)中最小的cost_temp = pre_min_cost + cost_neighbour ;if cost_temp < node_cost(i,j)node_cost(i,j) = cost_temp;%把最優(yōu)路徑上一列節(jié)點(diǎn)的行號(hào)記錄下來(lái)pre_node_index(i,j)= k;endendend end%找到node_cost最后一列中，cost最小的，記代價(jià)最小的行號(hào)為index index = 0; min_cost = inf; for i = 1:rowif node_cost(i, end)< min_costmin_cost = node_cost(i, end);index = i ;end end%動(dòng)態(tài)規(guī)劃最優(yōu)路徑初始化 dp_node_list_row = zeros(col, 1) ; %從后往前逆推 cur_index = index ; for i = 1:col%記錄cur_index前面節(jié)點(diǎn)的行號(hào)pre_index = pre_node_index(cur_index, end - i + 1) ;%把cur_index放到dp_node_list_row對(duì)應(yīng)的位置dp_node_list_row(col - i + 1) = cur_index;%再把pre_index賦給cur_index進(jìn)行下一步遞推cur_index = pre_index ; end%輸出初始化，由于事先不知道橫縱向的采樣列數(shù)，因此需要做緩沖 dp_path_s = ones(15,1)*-1; dp_path_l = ones(15,1)*-1; for i = 1:coldp_path_s(i) = plan_start_s + i *sample_s;dp_path_l(i) = ((row + 1)/ 2 - dp_node_list_row(i)) * sample_l; endendfunction cost = CalcNeighbourCost(pre_node_s,pre_node_l,cur_node_s,cur_node_l,w_cost_collision,...w_cost_smooth_dl,w_cost_smooth_ddl,w_cost_smooth_dddl,w_cost_ref,obs_s_set,obs_l_set)% 該函數(shù)將計(jì)算相鄰兩個(gè)節(jié)點(diǎn)之間的cost start_l = pre_node_l; start_dl = 0; start_ddl = 0; % ．．． % ．．．row = 3 中間的行號(hào)= (row + 1)/2 = 2 % ．．． end_l = cur_node_l; end_dl = 0; end_ddl = 0; start_s = pre_node_s; end_s = cur_node_s; coeff = CalcQuinticCoeffient(start_l, start_dl, start_ddl, end_l, end_dl, end_ddl, start_s, end_s); a0 = coeff(1); a1 = coeff(2); a2= coeff(3); a3 = coeff(4); a4 = coeff(5); a5 = coeff(6); %取10個(gè)點(diǎn)計(jì)算cost ds = zeros(10,1); l = zeros(10,1); dl = zeros(10,1); ddl = zeros(10,1); dddl = zeros (10,1); for i = 1:10ds(i) = start_s + (i-1)*sample_s/10; end l = a0 * ones (10,1) + a1* ds + a2 * ds.^2+ a3 * ds.^3 + a4 * ds.^4 + a5 * ds. 5; dl = al * ones(10,1)+ 2* a2 * ds + 3 * a3 * ds.^2+ 4* a4 * ds.^3 +5 * a5 * ds.^4; ddl = 2 * a2* ones(10,1)+6 * a3 * ds + 12 * a4 * ds.^2+ 20 * a5 * ds.^3; dddl = 6 * ones(10,1) * a3 + 24 * a4 * ds + 60 * a5 * ds.^2; cost_smooth = w_cost_smooth_dl * (dl'* dl) + w_cost_smooth_ddl * (ddl' * ddl) + w_cost_smooth_dddl * (dddl' * dddl); cost_ref = w_cost_ref * (l' *l); cost_collision = 0; for i = 1:length(obs_s_set)if isnan(obs_s_set(i))break;enddlon = ones (10,1)* obs_s_set(i) - ds ;dlat = ones(10,1)* obs_l_set(i) - l;%這里做了非常簡(jiǎn)化的質(zhì)點(diǎn)模型，認(rèn)為障礙物就是一個(gè)點(diǎn)。（可以這么做的原因：動(dòng)態(tài)規(guī)劃不需要特別復(fù)雜，只需要開辟凸空間，具體避障要在二次規(guī)劃中做）square_d = dlon.^2 + dlat.^2; % 近似算法，并不是真正的距離，因?yàn)閐x^2+dy^2不是距離cost_collision_once = CalcObsCost(w_cost_collision,square_d) ;cost_collision = cost_collision + cost_collision_once; end cost = cost_collision + cost_smooth + cost_ref; endfunction obs_cost = CalcObsCost(w_cost_collision,square_d) %該函數(shù)將計(jì)算障礙物的距離代價(jià) %暫定超過(guò)4米的cost為0在4到3米的cost為1000/squard_d,在小于3米的cost為w_cost_collisioncost = 0; for i = 1 : length(square_d)if (square_d(i)< 16 && square_d(i) > 9)cost = cost + 1000/ square_d(i);elseif (square_d(i)<9)cost = cost + w_cost_collision;end end obs_cost = cost; endfunction cost = CalcStartCost(begin_s,begin_l,begin_dl,begin_ddl,cur_node_row,sample_s,sample_l,...w_cost_collision,w_cost_smooth_dl,w_cost_smooth_ddl,w_cost_smooth_dddl,w_cost_ref,...obs_s_set,obs_l_set,row) %該函數(shù)將計(jì)算起點(diǎn)到第一層的cost start_l = begin_l; start_dl = begin_dl; start_ddl = begin_ddl; % ．．． % ．．．row = 3 中間的行號(hào)= (row + 1)/2 = 2 % ．．． end_l = ((row + 1)/2 - cur_node_row)*sample_l; end_dl = 0; end_ddl = 0; start_s = begin_s; end_s = begin_s + sample_s; coeff = CalcQuinticCoeffient(start_l, start_dl, start_ddl, end_l, end_dl, end_ddl, start_s, end_s); a0 = coeff(1); a1 = coeff(2); a2= coeff(3); a3 = coeff(4); a4 = coeff(5); a5 = coeff(6); %取10個(gè)點(diǎn)計(jì)算cost ds = zeros(10,1); l = zeros(10,1); dl = zeros(10,1); ddl = zeros(10,1); dddl = zeros (10,1); for i = 1:10ds(i) = start_s + (i-1)*(end_s - start_s)/10; end l = a0 * ones (10,1) + a1* ds + a2 * ds.^2+ a3 * ds.^3 + a4 * ds.^4 + a5 * ds. 5; dl = al * ones(10,1)+ 2* a2 * ds + 3 * a3 * ds.^2+ 4* a4 * ds.^3 +5 * a5 * ds.^4; ddl = 2 * a2* ones(10,1)+6 * a3 * ds + 12 * a4 * ds.^2+ 20 * a5 * ds.^3; dddl = 6 * ones(10,1) * a3 + 24 * a4 * ds + 60 * a5 * ds.^2; cost_smooth = w_cost_smooth_dl * (dl'* dl) + w_cost_smooth_ddl * (ddl' * ddl) + w_cost_smooth_dddl * (dddl' * dddl); cost_ref = w_cost_ref * (l' *l); cost_collision = 0; for i = 1:length(obs_s_set)if isnan(obs_s_set(i))break;enddlon = ones (10,1)* obs_s_set(i) - ds ;dlat = ones(10,1)* obs_l_set(i) - l;%這里做了非常簡(jiǎn)化的質(zhì)點(diǎn)模型，認(rèn)為障礙物就是一個(gè)點(diǎn)。（可以這么做的原因：動(dòng)態(tài)規(guī)劃不需要特別復(fù)雜，只需要開辟凸空間，具體避障要在二次規(guī)劃中做）square_d = dlon.^2 + dlat.^2; % 近似算法，并不是真正的距離，因?yàn)閐x^2+dy^2不是距離cost_collision_once = CalcObsCost(w_cost_collision,square_d) ;cost_collision = cost_collision + cost_collision_once; end cost = cost_collision + cost_smooth + cost_ref; endfunction obs_cost = CalcObsCost(w_cost_collision,square_d) %該函數(shù)將計(jì)算障礙物的距離代價(jià) %暫定超過(guò)4米的cost為0在4到3米的cost為1000/squard_d,在小于3米的cost為w_cost_collisioncost = 0; for i = 1 : length(square_d)if (square_d(i)< 16 && square_d(i) > 9)cost = cost + 1000/ square_d(i);elseif (square_d(i)<9)cost = cost + w_cost_collision;end end obs_cost = cost; endfunction coeff = CalcQuinticCoeffient(start_l, start_dl, start_ddl, end_l, end_dl, end_ddl, start_s, end_s) % l = a0 + a1*s + a2*s ^2 + a3*s ^3 + a4*s^4 + a5*s^5 % l’ = a1+ 2* a2* s + 3 * a3 * s ^2+ 4 * a4 * s^3 +5*a5 * s ^4 % l''= 2* a2+ 6 * a3 * s + 12 * a4 * s ^2+20 * a5 * s^3 start_s2 = start_s * start_s; start_s3 = start_s2 * start_s; start_s4 = start_s3 * start_s; start_s5 = start_s4 * start_s; end_s2 = end_s * end_s; end_s3 = end_s2 * end_s; end_s4 = end_s3 * end_s; end_s5 = end_s4 * end_s; A = [1, start_s, start_s2, start_s3, start_s4, start_s5;0 1, 2*start_s, 3 * start_s2, 4 *start_s3, 5*start_s4;0 0, 2, 6 * start_s, 12 *start_s2, 20*start_s3;1, end_s, end_s2, end_s3, end_s4, end_s5;0 1, 2*end_s, 3 * end_s2, 4 *end_s3, 5*end_s4;0 0, 2, 6 * end_s, 12 *end_s2, 20*end_s3;]; B = [start_l;start_dl;start_ddl;end_l;end_dl;end_ddl;]; coeff = A/B;end

連接好輸入輸出

注意這里設(shè)置9行6列，行數(shù)必須為奇數(shù)。縱向采樣長(zhǎng)度sample_s=10代表每10m采樣1列，6列正好是60m，傳感器探測(cè)范圍也就是60m。橫向采樣長(zhǎng)度sample_l為1m。行與行之間間距10m，列與列之間間距1m。

（8）后處理

動(dòng)態(tài)規(guī)劃完畢后，需要后處理

這里的dp_path_s和dp_path_l太少了，只反映了五次多項(xiàng)式的起點(diǎn)和終點(diǎn)，還不能稱之為合格的軌跡，所以需要多采樣幾個(gè)點(diǎn)，增密變成合格的軌跡。

function [dp_path_s_final,dp_path_l_final,dp_path_dl_final,dp_path_ddl_final] = fcn(...dp_path_s_init,dp_path_l_init,plan_start_s,plan_start_l,plan_start_dl,plan_start_ddl) %該函數(shù)將增密dp_path_s, dp_path_l %由于事先不知道動(dòng)態(tài)規(guī)劃采樣的行和列，也就不知道動(dòng)態(tài)規(guī)劃計(jì)算的曲線有多長(zhǎng)，因此需要做緩沖 %每1“米”采樣一個(gè)點(diǎn)，一共采60個(gè) ds = 1; %輸出初始化 dp_path_s_final = ones(60,1) * nan; dp_path_l_final = ones(60,1) * nan; dp_path_dl_final =ones(60,1) * nan; dp_path_ddl_final =ones(60,1) * nan;%設(shè)置五次多項(xiàng)式的起點(diǎn) start_s = plan_start_s; start_l = plan_start_l; start_dl = plan_start_dl ; start_ddl = plan_start_ddl ;s_cur = []; l_temp = []; dl_temp =[]; ddl_temp = [];% 每1m采樣一次，直到多項(xiàng)式終點(diǎn)，退出循環(huán)for i = 1 : length(dp_path_s_init)if dp_path_s_init(i) ==-1break; endfor j = 1: 10000%采樣ss_node = start_s + ds*(j - 1) ;if s_node < dp_path_s_init(i)s_cur = [s_cur,s_node];elsebreakendendend_s = dp_path_s_init(i) ;end_l = dp_path_l_init(i) ;end_dl = 0;end_ddl = 0;coeff = CalcQuinticCoeffient(start_l, start_dl, start_ddl, end_l, end_dl, end_ddl, start_s, end_s);a0 = coeff(1);a1 = coeff(2);a2= coeff(3);a3 = coeff(4);a4 = coeff(5);a5 = coeff(6); l = a0 * ones(1, length(s_cur)) + a1 * s_cur + a2 * s_cur.^2 + a3 * s_cur.^3 +...a4 * s_cur.^4 + a5 * s_cur.^5;dl = a1 * ones(1,length(s_cur)) + 2 *a2 * s_cur + 3 * a3 * s_cur.^2 + ...4 * a4 * s_cur.^3 +5 * a5* s_cur.^4;ddl = 2 * a2 * ones(1,length(s_cur)) + 6 * a3 * s_cur + 12 * a4 * s_cur.^2 + ...20 * a5 * s_cr.^3;%把l dl ddl的結(jié)果保存l_temp = [l_temp,1];dl_temp = [dl_temp, dl];ddl__temp = [ddl_temp, ddl];% end的值賦值給start做下一步循環(huán)start_s = end_s;start_l = end_l;start_dl = end_dl;start_ddl = end_ddl;s_cur = [];% 每次算完五次多項(xiàng)式后清空 end for k = 1:length(l_temp)if k == 61break;enddp_path_s_final(k) = plan_start_s + ds * (k - 1) ;dp_path_l_final(k)= l_temp(k);dp_path_dl_final(k) = dl_temp(k) ;dp_path_ddl_final(k) = ddl_temp(k); endendfunction coeff = CalcQuinticCoeffient(start_l, start_dl, start_ddl, end_l, end_dl, end_ddl, start_s, end_s) % l = a0 + a1*s + a2*s ^2 + a3*s ^3 + a4*s^4 + a5*s^5 % l’ = a1+ 2* a2* s + 3 * a3 * s ^2+ 4 * a4 * s^3 +5*a5 * s ^4 % l''= 2* a2+ 6 * a3 * s + 12 * a4 * s ^2+20 * a5 * s^3 start_s2 = start_s * start_s; start_s3 = start_s2 * start_s; start_s4 = start_s3 * start_s; start_s5 = start_s4 * start_s; end_s2 = end_s * end_s; end_s3 = end_s2 * end_s; end_s4 = end_s3 * end_s; end_s5 = end_s4 * end_s; A = [1, start_s, start_s2, start_s3, start_s4, start_s5;0 1, 2*start_s, 3 * start_s2, 4 *start_s3, 5*start_s4;0 0, 2, 6 * start_s, 12 *start_s2, 20*start_s3;1, end_s, end_s2, end_s3, end_s4, end_s5;0 1, 2*end_s, 3 * end_s2, 4 *end_s3, 5*end_s4;0 0, 2, 6 * end_s, 12 *end_s2, 20*end_s3;]; B = [start_l;start_dl;start_ddl;end_l;end_dl;end_ddl;]; coeff = A/B;end

算法寫好后，連接輸入輸出

接下來(lái)要把path轉(zhuǎn)回去，轉(zhuǎn)到直角坐標(biāo)系上去，發(fā)給控制去用

function [x_set,y_set,heading_set,kappa_set] = ...fcn(s_set,l_set,dl_set,ddl_set,frenet_path_x,frenet_path_y,frenet_path_heading,frenet_path_kappa,index2s) %通用算法frenet轉(zhuǎn)frenet %由于不知道有多少個(gè)(s,l)要轉(zhuǎn)化成直角坐標(biāo)，因此做緩沖 %輸出初始化 x_set = ones (128,1) *nan; y_set = ones (128,1) *nan; heading_set = ones (128,1)*nan; kappa_set = ones (128,1) *nan; for i = 1 : length(s_set)if isnan(s_set(i))break;end%計(jì)算(s,l)在frenet坐標(biāo)軸上的投影[proj_x,proj_y,proj_heading, proj_kappa] = CalcProjPoint(s, frenet_path_x, frenet_path_y, frenet_path_heading,...frenet_path_kappa,index2s);nor = [-sin(proj_heading) ;cos(proj_heading)];point = [proj_x ; proj_y] + l_set(i) *nor;x_set(i) = point(1);y_set(i) = point(2) ;heading_set(i) = proj_heading + atan(dl_set(i)/(l - proj_kappa * l_set(i)));% 近似認(rèn)為kappa' == 0, frenet轉(zhuǎn)cartesian見(jiàn)那個(gè)博客kappa_set(i) = ((ddl_set(i) + proj_kappa * dl_set(i)* tan(head_set(i) - proj_heading)) *...(cos(heading_set(i) - proj_heading)^2)/(1 - proj_kappa * l_set(i)) + proj_kappa) *...cos(heading_set(i) - proj_heading)/(1 - proj_kappa * l_set(i));endendfunction [proj_x, proj_y,proj_heading, proj_kappa] = CalcProjPoint(s,frenet_path_x,frenet_path_y, frenet_path_heading,frenet_path_kappa,index2s) %該函數(shù)將計(jì)算在frenet坐標(biāo)系下，點(diǎn)(s,l)在frenet坐標(biāo)軸的投影的直角坐標(biāo)(proj_x,proj_y, proj_heading. proj_kappa)' %先找匹配點(diǎn)的編號(hào) match_index = 1; while index2s (match_index)< smatch_index = match_index + 1; end match_point = [frenet_path_x(match_index) ;frenet_path_y(match_index)]; match_point_heading = frenet_path_heading(match_index) ; match_point_kappa = frenet_path_kappa(match_index) ; ds = s - index2s(match_index); match_tor = [cos(match_point_heading) ;sin(match_point_heading)]; proj_point = match_point + ds * match_tor; proj_heading = match_point_heading + ds * match_point_kappa; proj_kappa = match_point_kappa; proj_x = proj_point(1); proj_y = proj_point(2); end

這樣就完成了從dp_path從動(dòng)態(tài)規(guī)劃里面出來(lái)，直到整個(gè)一條曲線把它采樣完成以及轉(zhuǎn)換成直角坐標(biāo)系。

注意：dp_path_s_final理論上不需要自然坐標(biāo)轉(zhuǎn)直角坐標(biāo)，因?yàn)楹竺孢€有個(gè)二次規(guī)劃，動(dòng)態(tài)規(guī)劃畢竟是粗解，不是最優(yōu)解，最終算法是動(dòng)態(tài)規(guī)劃出來(lái)再二次規(guī)劃再轉(zhuǎn)成直角坐標(biāo)。還沒(méi)講到二次規(guī)劃，所以暫時(shí)先用粗解轉(zhuǎn)化生成path。

但是路徑不完整，沒(méi)有時(shí)間戳也沒(méi)有速度。還是沒(méi)法發(fā)給控制，所以有必要規(guī)劃一個(gè)速度，然后從路徑和速度合成軌跡，這才是控制所要的完整的軌跡。

下面寫一個(gè)簡(jiǎn)單的速度規(guī)劃，后面再細(xì)講。直接把老王模型里面的復(fù)制過(guò)來(lái)用

?最后發(fā)出去就完成了規(guī)劃的流程，速度規(guī)劃后面重新寫一遍

把emplanner的輸出作為決策規(guī)劃模塊的輸出，然后再輸入給控制

?規(guī)劃輸出的不是一個(gè)一個(gè)點(diǎn)，而是一群軌跡的時(shí)候，控制也要改，現(xiàn)在去修改控制接口

解除注釋，我們要先進(jìn)行一些處理，新建一個(gè)函數(shù)

function [xr,yr,thetar,kappar,vr,ar] = fcn(trajectory_x,trajectory_y,trajectory_heading,...trajectory_kappa,trajectory_speed,trajectory_accel,trajectory_time,current_time,ar) %該函數(shù)根據(jù)規(guī)劃的軌跡計(jì)算出期望跟蹤的點(diǎn) %由于控制也是有延遲，所以控制發(fā)出的期望跟蹤的點(diǎn)是current_time + 0.01; control_time = current_time + 0.01; %規(guī)劃發(fā)出的軌跡有一部分是拼接的，在剛啟動(dòng)的時(shí)候，stitch_time = -1,因此要先把它去掉 start_index = 1; while (trajectory_time(start_index)== -1)start_index = start_index + 1; end % 使用interp1插值計(jì)算得到xr yr thetar, kappar, vr, ar % 由于interp1在端點(diǎn)會(huì)得到nan，因此需要防一手 % 解釋，為什么需要trajectory_time(start_index) ~= 0 % 因?yàn)樵趧倖?dòng)時(shí)規(guī)劃先執(zhí)行(100ms)，當(dāng)規(guī)劃完畢的時(shí)候控制已經(jīng)執(zhí)行了10次 % 那么在最開始的時(shí)候，規(guī)劃還沒(méi)給結(jié)果是控制已經(jīng)執(zhí)行了10次，在這10次中控制跟蹤的軌跡是空軌跡 % simulink空軌跡表示為0，0，0，0，0，0.. .. . .,這樣trajectory_time % 也全是0，trajectory_time不單調(diào)遞增 % 而interp1(x, y,x0) 要求x必須要單調(diào)遞增，否則會(huì)報(bào)錯(cuò)if control_time > trajectory_time (start_index) && trajectory_time(start_index)~= 0xr = interp1(trajectory_time (start_index: end) , trajectory_x (start_index:end) , control_time);yr = interp1(trajectory_time(start_index:end) , trajectory_y(start_index:end) , control_time);thetar = interp1(trajectory_time(start_index:end), trajectory_heading(start_index:end), control_time) ;kappar = interp1(trajectory_time(start_index: end) , trajectory_kappa(start_index:end) , control_time);vr = interp1(trajectory_time(start_index:end), trajectory_speed(start_index:end) , control_time);ar = interp1(trajectory_time(start_index:end), trajectory_accel(start_index : end) , control_time); elsexr = trajectory_x(start_index);yr = trajectory_y(start_index);thetar = trajectory_heading(start_index);kappar = trajectory_kappa(start_index);vr = trajectory_speed(start_index);ar = trajectory_accel(start_index) ; end

在規(guī)劃里，我們用-1來(lái)表示沒(méi)有拼接軌跡

同時(shí)把planning_result的數(shù)據(jù)輸入給控制

?然后再把函數(shù)輸出接給desire，讓控制去跟蹤

控制模塊改好之后可以編譯解決一下bug，點(diǎn)擊運(yùn)行，可以看到可以正常的避障（這里可以多放幾個(gè)障礙物，更加直觀）

發(fā)現(xiàn)車有點(diǎn)晃，改一下這里的列為4，sample_s為15，這樣就不晃了

?還是有點(diǎn)晃，但是好多了，后面二次規(guī)劃再平滑

四、二次規(guī)劃

1、輕決策與重決策

二次規(guī)劃：求解二次型值最小的x，并且滿足約束

無(wú)需在意二次規(guī)劃的約束形式

?二次規(guī)劃要求解空間是凸的，而動(dòng)態(tài)規(guī)劃開辟了凸空間

動(dòng)態(tài)規(guī)劃雖然叫規(guī)劃，但是是做決策用的，如下圖所示，動(dòng)態(tài)規(guī)劃算出一條粗解，粗解在哪個(gè)凸空間內(nèi)，那么最終解就用它作為解空間（Apollo決策核心思想）

動(dòng)態(tài)規(guī)劃與決策的關(guān)系

決策算法分為重決策算法和輕決策算法，重決策算法基于人給定的規(guī)則，輕決策算法基于代價(jià)函數(shù)

重決策：根據(jù)人給定的規(guī)則判斷（綜合與障礙物的距離，相對(duì)速度，周圍環(huán)境信息）給出決策人事先寫好場(chǎng)景與決策的映射關(guān)系

假設(shè)決策：right nudge（向右繞）

?重決策優(yōu)點(diǎn)：①計(jì)算量小；②在感知不強(qiáng)的情況下仍能做決策（融合了人的智慧）

缺點(diǎn)：①場(chǎng)景太多了，人無(wú)法完全覆蓋

②人給出的決策所開辟的凸空間未必滿足約束

凸空間約束過(guò)于嚴(yán)苛，二次規(guī)劃搜不到滿足動(dòng)力學(xué)約束的解（這里是非凸，僅演示用）

輕決策算法：無(wú)先驗(yàn)規(guī)劃，空間離散化，設(shè)計(jì)Cost function，動(dòng)態(tài)規(guī)劃算法求解離散空間的最優(yōu)路徑，該最優(yōu)路徑開辟凸空間

?優(yōu)點(diǎn)：①無(wú)人為規(guī)則， 可以處理復(fù)雜場(chǎng)景

②有粗解，通過(guò)設(shè)計(jì)代價(jià)函數(shù)，可以使粗解“滿足”硬約束（無(wú)碰撞，最小曲率），這樣使二次規(guī)劃求解成功的機(jī)率大大增加（因?yàn)榇纸庠谕箍臻g內(nèi)部，所以該凸空間的“扭曲”程度至少有粗解兜底），大大緩解了基于人為規(guī)則的決策所造成的凸空間扭曲情況

缺點(diǎn)：①?gòu)?fù)雜場(chǎng)景計(jì)算量很大；②依賴預(yù)測(cè)（速度規(guī)劃時(shí)會(huì)講到）；③要求周圍環(huán)境全知（感知，定位要求高）④代價(jià)函數(shù)設(shè)計(jì)/最優(yōu)解未必符合人的駕駛習(xí)慣

L2和高速L3主要用重決策，L4用輕決策算法

目前學(xué)術(shù)研究的熱點(diǎn)是，輕決策和重決策結(jié)合的方向，博弈問(wèn)題（Apollo 7.0的預(yù)測(cè)模塊內(nèi)加入）。還有就是深度學(xué)習(xí)做決策，規(guī)則來(lái)兜底的方法。

2、二次規(guī)劃算法

二次規(guī)劃算法比較簡(jiǎn)單，但是編程麻煩，細(xì)節(jié)多

如圖所示，紅色點(diǎn)是動(dòng)態(tài)規(guī)劃出來(lái)的粗解，每個(gè)離散點(diǎn)si都有上下界[lmini,lmaxi]，這些界限構(gòu)成了凸空間

二次規(guī)劃的求解空間就在此凸空間中搜索

已知

?求滿足：f(s)二階導(dǎo)數(shù)連續(xù)

f(s)盡可能平滑（f(s)的各階導(dǎo)數(shù)的平方盡可能小）

Apollo3.5/5.0新增：f(s)盡可能在凸空間的中央

?分段加加速度優(yōu)化法

假設(shè)連接與的曲線f(s)的三階導(dǎo)數(shù)恒為（常數(shù)）

在與之間的曲線f(s)的四階導(dǎo)數(shù)及以上皆為0

那么，可以進(jìn)行有限項(xiàng)的泰勒展開（注意四階及其后面都消掉了）

?寫成矩陣形式：

?如果要滿足二階導(dǎo)數(shù)連續(xù)要滿足的等式約束

則分段加加速度約束為

?記為（等式約束）

下面來(lái)看不等式約束，上面已經(jīng)知道每個(gè)路徑點(diǎn)的上下界

?直接拿上下界來(lái)作為不等式約束

?實(shí)際這樣做并不準(zhǔn)確，因?yàn)槠嚥皇琴|(zhì)點(diǎn)，有體積，需要對(duì)汽車的四個(gè)角點(diǎn)做約束

?注意這里的d1和d2并不是ab，是質(zhì)心到汽車前后邊界線的距離

然后得到四個(gè)角點(diǎn)

?存在三角函數(shù)，非線性，不得不做一些近似處理：

這種近似是對(duì)安全性有利的，會(huì)將小車近似成大車

直接拿角點(diǎn)放到上下界來(lái)作為約束不合適，因?yàn)榭赡芷渌屈c(diǎn)會(huì)碰到

?做的保守一點(diǎn)

對(duì)于上屆

?對(duì)于下界

?個(gè)人理解：碰撞檢測(cè)算法不只一種，這個(gè)只是其中一種，還有基于膨脹圓碰撞等等方法

得到不等式約束及其矩陣形式

?總的不等式約束為

?記為，其中A為8n*3n，x為3n*1，b為8n*1

代價(jià)函數(shù)Cost function

?約束為

3、代碼實(shí)踐

首先需要繼續(xù)修bug，

進(jìn)入EM Planner的投影通用模塊

?主要改動(dòng)為：增加全局變量pre_x_set，pre_y_set用來(lái)判斷幀與幀之間的障礙物是否為同一個(gè)，幀與幀之間的物體的位置距離過(guò)大，認(rèn)為是兩個(gè)障礙物

function [match_point_index_set,proj_x_set, proj_y_set,proj_heading_set,proj_kappa_set] = ...fcn(x_set,y_set,frenet_path_x,frenet_path_y,frenet_path_heading,frenet_path_kappa) %該函數(shù)將批量計(jì)算x_set，y_set中的xy，在frenet_path下的投影的信息 %輸入 x_set,y_set，待投影的點(diǎn)的集合 %x_set,y_set,frenet_path_x,frenet_path_y,frenet_path_heading,frenet_path_kappa %曲線在直角坐標(biāo)系下的x，y，heading，kappa %輸出：match_point_index_set 匹配點(diǎn)在frenet_path下的編號(hào)的集合(即從全局路徑第一個(gè)點(diǎn)開始數(shù)，第幾個(gè)點(diǎn)是匹配點(diǎn)) % proj_x y heading kappa 投影的x,y,heading,kappa的集合% 由于事先不知道x_set中到底有多少個(gè)點(diǎn)需要投影，因此事先聲明一個(gè)最大的數(shù)量的點(diǎn)作為緩沖 n = 128; % 并且由于不知道投影點(diǎn)的個(gè)數(shù)，所以也不知道輸出的個(gè)數(shù)，因此輸出也要做緩沖，用nan表示不存在的點(diǎn) % 輸出最多輸出128個(gè) % 輸出初始化 match_point_index_set = ones(n,1) * nan; proj_x_set = ones(n,1) * nan; proj_y_set = ones(n,1) * nan; proj_heading_set = ones(n,1) * nan; proj_kappa_set = ones(n,1) * nan;%找匹配點(diǎn)，需要利用上一個(gè)周期的結(jié)果作為下一個(gè)周期遍歷的起點(diǎn)，因此需要聲明兩個(gè)全局變量 persistent is_first_run; persistent pre_match_point_index_set; persistent pre_frenet_path_x; persistent pre_frenet_path_y; persistent pre_frenet_path_heading; persistent pre_frenet_path_kappa; persistent pre_x_set; persistent pre_y_set; %算法主函數(shù)入口if isempty(is_first_run)%該if分支表示函數(shù)首次運(yùn)行is_first_run = 0;pre_frenet_path_x = frenet_path_x;pre_frenet_path_y = frenet_path_y;pre_frenet_path_heading = frenet_path_heading;pre_frenet_path_kappa = frenet_path_kappa;%要記錄首次運(yùn)行計(jì)算的匹配點(diǎn)的結(jié)果供下個(gè)周期運(yùn)行，先初始化pre_match_point_index_set = ones(n,1) * nan;%對(duì)x_set,y_set的點(diǎn)做遍歷，找到他們的匹配點(diǎn)for i = 1 : length(x_set)if isnan(x_set(i))break;end%首次運(yùn)行時(shí)，沒(méi)有任何提示，只能從frenet path的第一個(gè)點(diǎn)開始找start_search_index = 1;% 聲明increase_count，用于表示在遍歷時(shí)distance連續(xù)增加的個(gè)數(shù)increase_count = 0;% 開始遍歷min_distance = inf;for j = start_search_index : length(frenet_path_x)distance = (x_set(i) - frenet_path_x(j))^2 + (y_set(i) - frenet_path_y(j))^2;if distance < min_distancemin_distance = distance;match_point_index_set(i) = j;increase_count = 0;elseincrease_count = increase_count + 1;end%如果distance連續(xù)增加50次就不要再遍歷了，節(jié)省時(shí)間if increase_count > 50break;endend%如何通過(guò)匹配點(diǎn)計(jì)算投影點(diǎn)，請(qǐng)參見(jiàn)《自動(dòng)駕駛控制算法第七講》%或《自動(dòng)駕駛決策規(guī)劃算法》第一章第三節(jié)%取出匹配點(diǎn)的編號(hào)match_point_index = match_point_index_set(i);%取出匹配點(diǎn)的信息match_point_x = frenet_path_x(match_point_index);match_point_y = frenet_path_y(match_point_index);match_point_heading = frenet_path_heading(match_point_index);match_point_kappa = frenet_path_kappa(match_point_index);%計(jì)算匹配點(diǎn)的方向向量與法向量vector_match_point = [match_point_x;match_point_y];vector_match_point_direction = [cos(match_point_heading);sin(match_point_heading)];%聲明待投影點(diǎn)的位矢vector_r = [x_set(i);y_set(i)];%通過(guò)匹配點(diǎn)計(jì)算投影點(diǎn)vector_d = vector_r - vector_match_point;ds = vector_d' * vector_match_point_direction;vector_proj_point = vector_match_point + ds * vector_match_point_direction;proj_heading = match_point_heading + match_point_kappa * ds;proj_kappa = match_point_kappa;%計(jì)算結(jié)果輸出proj_x_set(i) = vector_proj_point(1);proj_y_set(i) = vector_proj_point(2);proj_heading_set(i) = proj_heading;proj_kappa_set(i) = proj_kappa;end%匹配點(diǎn)的計(jì)算結(jié)果保存，供下一個(gè)周期使用pre_match_point_index_set = match_point_index_set;pre_x_set = x_set;pre_y_set = y_set; else%此if分支表示不是首次運(yùn)行%對(duì)每個(gè)x_set上的點(diǎn)做處理for i = 1 : length(x_set)%不是首次運(yùn)行，對(duì)于點(diǎn)x_set(i),y_set(i)來(lái)說(shuō),start_search_index =%pre_match_point_index_set(i)start_search_index = pre_match_point_index_set(i);%上個(gè)周期匹配點(diǎn)的編號(hào)作為本周期搜索的起點(diǎn)% 聲明increase_count，用于表示在遍歷時(shí)distance連續(xù)增加的個(gè)數(shù)%%%%%%%%%%對(duì)于障礙物檢測(cè)而言，當(dāng)感知第一次檢測(cè)到障礙物時(shí)，算法并不是首次運(yùn)行，此時(shí) %pre_match_point_index_set的值是nan，如果還用上一時(shí)刻的結(jié)果作為本周期搜索的起點(diǎn)%必然會(huì)出問(wèn)題，所以要修改% 增加：判斷幀與幀之間的障礙物是否為同一個(gè)square_dis = (x_set(i) - pre_x_set(i))^2 + (y_set(i) - pre_y_set(i))^2;if square_dis > 36%幀與幀之間的物體的位置距離過(guò)大，認(rèn)為是兩個(gè)障礙物start_search_index = nan;end% 聲明increase_count_limitincrease_count_limit = 5;if isnan(start_search_index)%沒(méi)有上個(gè)周期的結(jié)果，那就不能只檢查5次了increase_count_limit = 50;%搜索起點(diǎn)也要設(shè)為1start_search_index = 1;endincrease_count = 0;% 開始遍歷%這里多一個(gè)步驟，判斷遍歷的方向%計(jì)算上個(gè)周期匹配點(diǎn)的位矢vector_pre_match_point = [frenet_path_x(start_search_index);frenet_path_y(start_search_index)];vector_pre_match_point_direction = ...[cos(frenet_path_heading(start_search_index));sin(frenet_path_heading(start_search_index))];%判斷遍歷的方向flag = ([x_set(i);y_set(i)] - vector_pre_match_point)' * vector_pre_match_point_direction;min_distance = inf;if flag > 0.001for j = start_search_index : length(frenet_path_x)distance = (x_set(i) - frenet_path_x(j))^2 + (y_set(i) - frenet_path_y(j))^2;if distance < min_distancemin_distance = distance;match_point_index_set(i) = j;increase_count = 0;elseincrease_count = increase_count + 1;end%如果distance連續(xù)增加increase_count_limit次就不要再遍歷了，節(jié)省時(shí)間if increase_count > increase_count_limitbreak;endendelseif flag < - 0.001for j = start_search_index : -1 : 1distance = (x_set(i) - frenet_path_x(j))^2 + (y_set(i) - frenet_path_y(j))^2;if distance < min_distancemin_distance = distance;match_point_index_set(i) = j;increase_count = 0;elseincrease_count = increase_count + 1;end%如果distance連續(xù)增加increase_count_limit次就不要再遍歷了，節(jié)省時(shí)間if increase_count > increase_count_limitbreak;endendelsematch_point_index_set(i) = start_search_index;end%如何通過(guò)匹配點(diǎn)計(jì)算投影點(diǎn)，請(qǐng)參見(jiàn)《自動(dòng)駕駛控制算法第七講》%或《自動(dòng)駕駛決策規(guī)劃算法》第一章第三節(jié)%取出匹配點(diǎn)的編號(hào)match_point_index = match_point_index_set(i);%取出匹配點(diǎn)的信息match_point_x = frenet_path_x(match_point_index);match_point_y = frenet_path_y(match_point_index);match_point_heading = frenet_path_heading(match_point_index);match_point_kappa = frenet_path_kappa(match_point_index);%計(jì)算匹配點(diǎn)的方向向量與法向量vector_match_point = [match_point_x;match_point_y];vector_match_point_direction = [cos(match_point_heading);sin(match_point_heading)];%聲明待投影點(diǎn)的位矢vector_r = [x_set(i);y_set(i)];%通過(guò)匹配點(diǎn)計(jì)算投影點(diǎn)vector_d = vector_r - vector_match_point;ds = vector_d' * vector_match_point_direction;vector_proj_point = vector_match_point + ds * vector_match_point_direction;proj_heading = match_point_heading + match_point_kappa * ds;proj_kappa = match_point_kappa;%計(jì)算結(jié)果輸出proj_x_set(i) = vector_proj_point(1);proj_y_set(i) = vector_proj_point(2);proj_heading_set(i) = proj_heading;proj_kappa_set(i) = proj_kappa;end%匹配點(diǎn)的計(jì)算結(jié)果保存，供下一個(gè)周期使用pre_match_point_index_set = match_point_index_set;pre_frenet_path_x = frenet_path_x;pre_frenet_path_y = frenet_path_y;pre_frenet_path_heading = frenet_path_heading;pre_frenet_path_kappa = frenet_path_kappa;pre_x_set = x_set;pre_y_set = y_set; end

EM Planner新建函數(shù)

function [l_min,l_max] = fcn(dp_path_s,dp_path_l,static_obs_s_set,static_obs_l_set,static_obs_length,static_obs_width) % 該函數(shù)將輸出每個(gè)離散的dp_path_s中的點(diǎn)s所對(duì)應(yīng)的l的邊界l_min, l_max %輸入動(dòng)態(tài)規(guī)劃的曲線dp_path_s dp_path_l % 障礙物中心點(diǎn)的坐標(biāo)static_obs_s_set,static_obs_l_set % 障礙物的長(zhǎng)寬static_obs_length,static_obs_width % 輸出l_min l_max l 的上下界%注意一般真實(shí)障礙物投影到frenet后，長(zhǎng)寬會(huì)變得扭曲，在這里近似用直角坐標(biāo)的static_obs_length,static_gobs width的值代替frenet坐標(biāo)下的值 %所以此em plannner不能處理參考線曲率過(guò)大的場(chǎng)景(r >= 200) Apollo的EM可以，但是需要大改，細(xì)節(jié)很多，這里暫時(shí)不做%大曲率場(chǎng)景的避障，參考線模塊，還有障礙物投影模塊，速度規(guī)劃模塊要做很多特殊處理。 %輸出初始化如果無(wú)障礙l的邊界默認(rèn)為±6 l_min = ones(60,1)*-6; l_max = ones(60,1)*6;%對(duì)每個(gè)障礙物做遍歷 for i = 1 : length(static_obs_s_set)if isnan(static_obs_s_set(i))break; end%計(jì)算障礙物尾部和頭部的sobs_s_min = static_obs_s(i) + static_obs_length/2;obs_s_max = static_obs_s(i) - static_obs_length/2 ;%找到obs_s_min，obs_s _max在dp_path_s 在 dp_path_s中的位置start_index = FindNearIndex(dp_path_s,obs_s_min);end_index = FindNearIndex(dp_path_s,obs_s_max);%判斷dp_path_l在障礙物的上面還是下面，(path在obs上面即決策為向左繞，否則為向右繞)centre_index = FindNearIndex(dp_path_s,static_obs_s_set(i)) ;if start_index ==1 && end_index == 1% 障礙物已經(jīng)完全在host的后面continue;endpath_l = dp_path_l(centre_index);if path_l > static_obs_s_set(i)%意味著決策是向左繞過(guò)障礙物for j = start_index :end_index% l_max(j)是所有決策為向左繞過(guò)障礙物的l邊界的最大值l_max(j) = min(l_max(j), static_obs_l_set(i) - static_obs_width/2);endelse%意味著決策是向右繞過(guò)障礙物for j = start_index :end_index% l_min(j)是所有決策為向左繞過(guò)障礙物的l邊界的最大值l_min(j) = max(l_min(j), static_obs_l_set(i) + static_obs_width/2);endendendendfunction y = FindNearIndex(dp_path_s,obs_s) %該函數(shù)將找到在dp path s上的所有點(diǎn)中，與obs_s最近的點(diǎn)，并返回該點(diǎn)在dp_path_s的編號(hào) %這里要注意dp path s上的點(diǎn)是動(dòng)態(tài)規(guī)劃的結(jié)果，必然大于等于0，小于等于59 %而obs_s是障礙物在整個(gè)referenceline上的投影所得到的坐標(biāo)，所以obs_s有可能小于0，也有可能大于59 if dp_path(1) >= obs_s% 障礙物在車的后面y = 1;return; elseif dp_path_s(end) < obs_s% 障礙物在車的前面過(guò)遠(yuǎn)y = 60;return; elseindex = 1;while dp_path_s(index) < obs_sindex = index + 1;end%循環(huán)退出的條件是dp_path_s(index) >= obs_s 判斷一下index和 index-1到底哪個(gè)點(diǎn)離obs_s更近if dp_path_s(index) - obs_s > obs_s - dp_path_s(index-1)y = index - 1;elsey = index;end endend

這里障礙物長(zhǎng)寬暫時(shí)用5和2

下面開始正式寫二次規(guī)劃算法

function [qp_path_l,qp_path_dl,qp_path_ddl] = ... fcn(l_min,l_max,w_cost_l,w_cost_dl,w_cost_ddl,w_cost_dddl,w_cost_centre,w_cost_end_l,w_cost_end_dl,w_cost_end_ddl,...host_d1,host_d2,host_w,...plan_start_l,plan_start_dl,plan_start_ddl,dp_path_l_final) % 路徑二次規(guī)劃 % 0.5*x'Hx + f'*x = min % subject to A*x <= b % Aeq*x = beq % lb <= x <= ub; % 輸入：l_min l_max 點(diǎn)的凸空間 % w_cost_l 參考線代價(jià) % w_cost_dl ddl dddl 光滑性代價(jià) % w_cost_centre 凸空間中央代價(jià) % w_cost_end_l dl dd1 終點(diǎn)的狀態(tài)代價(jià) (希望path的終點(diǎn)狀態(tài)為(0,0,0)) % host_d1,d2 host質(zhì)心到前后軸的距離 % host_w host的寬度 % plan_start_l,dl,ddl 規(guī)劃起點(diǎn) % 輸出 qp_path_l dl ddl 二次規(guī)劃輸出的曲線 coder.extrinsic("quadprog"); % 待優(yōu)化的變量的數(shù)量 n = 60;% 輸出初始化 qp_path_l = zeros(n,1); qp_path_dl = zeros(n,1); qp_path_ddl = zeros(n,1); % H_L H_DL H_DDL H_DDDL Aeq beq A b 初始化 H_L = zeros(3*n, 3*n); H_DL = zeros(3*n, 3*n); H_DDL = zeros(3*n, 3*n); H_DDDL = zeros(n-1, 3*n); H_CENTRE = zeros(3*n, 3*n); H_L_END = zeros(3*n, 3*n); H_DL_END = zeros(3*n, 3*n); H_DDL_END = zeros(3*n, 3*n); Aeq = zeros(2*n-2, 3*n); beq = zeros(2*n-2, 1); A = zeros(8*n, 3*n); b = zeros(8*n, 1); % 期望的終點(diǎn)狀態(tài) end_l_desire = 0; end_dl_desire = 0; end_ddl_desire = 0;% Aeq_sub ds = 1;%縱向間隔 Aeq_sub = [1 ds ds^2/3 -1 0 ds^2/6;0 1 ds/2 0 -1 ds/2]; % A_sub; d1 = host_d1; d2 = host_d2; w = host_w; A_sub = [1 d1 0;1 d1 0;1 -d2 0;1 -d2 0;-1 -d1 0;-1 -d1 0;-1 d2 0;-1 d2 0]; % 生成Aeq for i = 1:n-1% 計(jì)算分塊矩陣左上角的行和列row = 2*i - 1;col = 3*i - 2;Aeq(row:row + 1,col:col + 5) = Aeq_sub; end % 生成A for i = 1:nrow = 8*i - 7;col = 3*i - 2;A(row:row + 7,col:col + 2) = A_sub; end% 視頻里用的找(s(i) - d2,s(i) + d1)的方法過(guò)于保守，在這里舍棄掉 % 只要找到四個(gè)角點(diǎn)所對(duì)應(yīng)的l_min l_max 即可 % 。。。。。。。。。。。。。。 % [ . ]<- % [ ] % 。。。。。。。。。。。。。 front_index = ceil(d1/ds); back_index = ceil(d2/ds); % ceil向上取整 ceil(3) = 3 ceil(3.1) = 4 % 生成b for i = 1:n% 左前 右前的index = min(i + front_index,60)% 左后 右后的index = max(i - back_index,1)index1 = min(i + front_index,n);index2 = max(i - back_index,1);b(8*i - 7:8*i,1) = [l_max(index1) - w/2;l_max(index1) + w/2;l_max(index2) - w/2;l_max(index2) + w/2;-l_min(index1) + w/2;-l_min(index1) - w/2;-l_min(index2) + w/2;-l_min(index2) - w/2;]; end %生成 lb ub 主要是對(duì)規(guī)劃起點(diǎn)做約束 lb = ones(3*n,1)*-inf; ub = ones(3*n,1)*inf; lb(1) = plan_start_l; lb(2) = plan_start_dl; lb(3) = plan_start_ddl; ub(1) = lb(1); ub(2) = lb(2); ub(3) = lb(3); % for i = 2:n % lb(3*i - 1) = - pi/8; % ub(3*i - 1) = pi/8; % lb(3*i) = -0.1; % ub(3*i) = 0.1; % end % 生成H_L,H_DL,H_DDL,H_CENTRE for i = 1:nH_L(3*i - 2,3*i - 2) = 1;H_DL(3*i - 1,3*i - 1) = 1;H_DDL(3*i, 3*i) = 1; end H_CENTRE = H_L; % 生成H_DDDL; H_dddl_sub = [0 0 1 0 0 -1]; for i = 1:n-1row = i;col = 3*i - 2;H_DDDL(row,col:col + 5) = H_dddl_sub; end % 生成H_L_END H_DL_END H_DDL_END H_L_END(3*n - 2,3*n - 2) = 1; H_DL_END(3*n - 1,3*n - 1) = 1; H_DDL_END(3*n,3*n) = 1; % 生成二次規(guī)劃的H H = w_cost_l * (H_L'*H_L) + w_cost_dl * (H_DL'*H_DL) + w_cost_ddl * (H_DDL'*H_DDL)...+w_cost_dddl * (H_DDDL'*H_DDDL) + w_cost_centre * (H_CENTRE'*H_CENTRE) + w_cost_end_l * (H_L_END'*H_L_END)...+w_cost_end_dl * (H_DL_END'* H_DL_END) + w_cost_ddl * (H_DDL_END'*H_DDL_END); H = 2 * H; % 生成f f = zeros(3*n,1);centre_line = 0.5 * (l_min + l_max); centre_line = dp_path_l_final; for i = 1:nf(3*i - 2) = -2 * centre_line(i); endf = w_cost_centre * f; % 終點(diǎn)要接近end_l dl ddl desire f(3*n - 2) = f(3*n - 2) -2 * end_l_desire * w_cost_end_l; f(3*n - 1) = f(3*n - 1) -2 * end_dl_desire * w_cost_end_dl; f(3*n) = f(3*n) -2 * end_ddl_desire * w_cost_end_ddl;X = quadprog(H,f,A,b,Aeq,beq,lb,ub); % 這里沒(méi)有對(duì)曲率做強(qiáng)制約束，并不好 % 可以用 |dl(i+1) - dl(i)|/ds <= kappa_max 近似去約束曲率 for i = 1:nqp_path_l(i) = X(3*i - 2);qp_path_dl(i) = X(3*i - 1);qp_path_ddl(i) = X(3*i); endend

報(bào)錯(cuò)：

下面分析原因，新建可視化模塊

?

function fcn(dp_s,dp_l,qp_l,l_min,l_max,obs_s_set,obs_l_set,start_s,start_l,start_dl) coder.extrinsic("plot","axis","figure","drawnow","cla","fill");obs_length = 5; obs_width = 2; host_l = 6; host_w = 1.63; cla; host_p1_l = start_l + sin(atan(start_dl)) * host_l /2 + host_w/2 * cos(atan(start_dl)); host_p1_s = start_s + cos(atan(start_dl)) * host_l /2 - host_w/2 * sin(atan(start_dl)); host_p2_l = start_l + sin(atan(start_dl)) * host_l /2 - host_w/2 * cos(atan(start_dl)); host_p2_s = start_s + cos(atan(start_dl)) * host_l /2 + host_w/2 * sin(atan(start_dl)); host_p3_l = start_l - sin(atan(start_dl)) * host_l /2 + host_w/2 * cos(atan(start_dl)); host_p3_s = start_s - cos(atan(start_dl)) * host_l /2 - host_w/2 * sin(atan(start_dl)); host_p4_l = start_l - sin(atan(start_dl)) * host_l /2 - host_w/2 * cos(atan(start_dl)); host_p4_s = start_s - cos(atan(start_dl)) * host_l /2 + host_w/2 * sin(atan(start_dl)); host_area_x = [host_p1_s;host_p2_s;host_p4_s;host_p3_s]; host_area_y = [host_p1_l;host_p2_l;host_p4_l;host_p3_l]; fill(host_area_x,host_area_y,[0 1 1]); hold on plot(dp_s,dp_l,'r');plot(dp_s,qp_l,'b'); plot(dp_s,l_min,dp_s,l_max); axis([-8 70 -10 10]);for i = 1:length(obs_s_set)if isnan(obs_s_set(i))break;endobs_p1_s = obs_s_set(i) + obs_length/2;obs_p1_l = obs_l_set(i) + obs_width/2;obs_p2_s = obs_s_set(i) + obs_length/2;obs_p2_l = obs_l_set(i) - obs_width/2;obs_p3_s = obs_s_set(i) - obs_length/2;obs_p3_l = obs_l_set(i) + obs_width/2;obs_p4_s = obs_s_set(i) - obs_length/2;obs_p4_l = obs_l_set(i) - obs_width/2;obs_area_s = [obs_p1_s;obs_p2_s;obs_p4_s;obs_p3_s];obs_area_l = [obs_p1_l;obs_p2_l;obs_p4_l;obs_p3_l];fill(obs_area_s,obs_area_l,[0 0 0]); end

?

其中藍(lán)色和紅色線為上下界lmin,lmax，藍(lán)色線為二次規(guī)劃的軌跡線?

把w_cost_dl調(diào)大

?

下面來(lái)看四個(gè)問(wèn)題

①二次規(guī)劃崩潰問(wèn)題

②車控制不穩(wěn)，抖動(dòng)劇烈問(wèn)題

③決策不穩(wěn)定，朝令夕改問(wèn)題

④速度規(guī)劃如何影響路徑規(guī)劃

Q1：在靠近障礙物時(shí)，二次規(guī)劃崩潰

根源在于：兩個(gè)約束，碰撞約束和規(guī)劃起點(diǎn)約束相互矛盾

?規(guī)劃起點(diǎn)要么是動(dòng)力學(xué)遞推要么是軌跡拼接而來(lái)，不能保證規(guī)劃起點(diǎn)滿足碰撞約束

?使用拼接時(shí)，規(guī)劃起點(diǎn)的約束與碰撞約束可能會(huì)發(fā)生矛盾

思考：為什么在無(wú)避障時(shí)不會(huì)崩潰

?

規(guī)劃的線貼著邊界

思考：為什么規(guī)劃的路徑會(huì)貼著邊界

為什么是橙色的線而不是綠線

?答：二次規(guī)劃性質(zhì)決定

?帶約束的二次規(guī)劃的最小值只會(huì)在極小值或者邊界點(diǎn)獲得

下面來(lái)看避障問(wèn)題

如果不帶約束，極小值點(diǎn)就是中間的線

?如果帶約束，必然貼著約束邊界

?所以二次規(guī)劃的曲線天然有往極小值靠攏的趨勢(shì)，必然是貼著邊界約束

實(shí)際有必要貼著邊界嗎，沒(méi)必要。

所以采用以下改進(jìn)：

①規(guī)劃起點(diǎn)不管了，障礙物放大一點(diǎn)做安全緩沖

藍(lán)色障礙物膨脹到紫色，起點(diǎn)與紫色碰撞，并未和藍(lán)色碰撞

?具體在代碼上的做法就是改for循環(huán)

?②改極小值點(diǎn)的位置

改下H

?加上權(quán)重，把centre的權(quán)重調(diào)大

?這樣可以讓車往中心跑

?

等于二分之一，

?可以保證幾何穩(wěn)定，缺點(diǎn)是wcentre難調(diào)，調(diào)小了不起作用，調(diào)大了路徑不平滑

如圖所示，wcentre調(diào)大是棕色曲線，調(diào)小是紫色曲線

?也就是說(shuō)centre_line本來(lái)不是平滑曲線，如果調(diào)大就越接近c(diǎn)entre_line導(dǎo)致其越來(lái)越不平滑

而這樣做

?優(yōu)點(diǎn)就是，dp_path比上面那種平滑，曲線平滑相對(duì)好

?缺點(diǎn)：dp_path幾何不穩(wěn)定

dp_path每一幀與每一幀得幾何形狀都會(huì)有巨大變化，若二次規(guī)劃以dp_path作為centre line，qp path也會(huì)震蕩

?本身動(dòng)態(tài)規(guī)劃得解就會(huì)動(dòng)來(lái)動(dòng)去，而二次規(guī)劃又是在趨近去它，所以自然也會(huì)動(dòng)來(lái)動(dòng)去

相反就很穩(wěn)定

?那么，到底該選擇哪種呢？

老王推薦選擇穩(wěn)定的，防止晃來(lái)晃去，至于調(diào)參難可以和第二個(gè)問(wèn)題一起解決

Q2：方向盤大幅擺動(dòng)，車抖的厲害

根源：車規(guī)劃的路徑會(huì)天然往邊界靠攏的趨勢(shì)

?

?

?

解決：不允許反復(fù)橫跳，加約束

?新的問(wèn)題：約束越多，求解越慢（反直覺(jué)）

?因?yàn)榧s束越多，初值越難找

?解決：削二次規(guī)劃的規(guī)模，由60->20，二次規(guī)劃求解完畢后在進(jìn)行插值增密

最終Q1，Q2合起來(lái)的改進(jìn)方案

①削qp規(guī)模，再插值增密

②增加的約束

③規(guī)劃起點(diǎn)不做約束

④改，合理分配權(quán)重

合理就是需要大的地方大，需要小的地方小

?

Q3：決策“朝令夕改”的問(wèn)題

?解決：Matlab里面暫時(shí)無(wú)解

Apollo里面使用打標(biāo)簽的方法：

上一幀

?這一幀：只對(duì)無(wú)標(biāo)簽的obs作動(dòng)態(tài)規(guī)劃決策，若障礙物已有標(biāo)簽，直接沿用標(biāo)簽的決策

Matlab對(duì)結(jié)構(gòu)體，字符串的支持有點(diǎn)弱，盡快切C++

Q4：速度規(guī)劃對(duì)路徑規(guī)劃的影響

現(xiàn)在的規(guī)劃path只處理靜態(tài)障礙物，這樣做有問(wèn)題

?這個(gè)case，速度規(guī)劃無(wú)論怎么規(guī)劃，都會(huì)碰撞（除非倒車）

怎么辦呢？

已知上一幀的速度規(guī)劃為10m/s勻速，上一幀的path為直線

這一幀

?若path不考慮動(dòng)態(tài)，則速度規(guī)劃必?zé)o解

path規(guī)劃要拿上一幀的軌跡去判斷與動(dòng)態(tài)障礙物的交互

?上一幀的path + speed planning 會(huì)影響到這一幀的path planning

EM planner是SL planning與ST planing反復(fù)迭代的過(guò)程：

?

下面來(lái)到simulink進(jìn)行改正

增加兩個(gè)輸入來(lái)限制dl和ddl

?

更新后的二次規(guī)劃代碼，增加了約束

function [qp_path_s,qp_path_l,qp_path_dl,qp_path_ddl] = ... fcn(l_min,l_max,w_cost_l,w_cost_dl,w_cost_ddl,w_cost_dddl,w_cost_centre,w_cost_end_l,w_cost_end_dl,w_cost_end_ddl,...host_d1,host_d2,host_w,...plan_start_s,plan_start_l,plan_start_dl,plan_start_ddl,...delta_dl_max,delta_ddl_max) % 路徑二次規(guī)劃 % 0.5*x'Hx + f'*x = min % subject to A*x <= b % Aeq*x = beq % lb <= x <= ub; % 輸入：l_min l_max 點(diǎn)的凸空間 % w_cost_l 參考線代價(jià) % w_cost_dl ddl dddl 光滑性代價(jià) % w_cost_centre 凸空間中央代價(jià) % w_cost_end_l dl dd1 終點(diǎn)的狀態(tài)代價(jià) (希望path的終點(diǎn)狀態(tài)為(0,0,0)) % host_d1,d2 host質(zhì)心到前后軸的距離 % host_w host的寬度 % plan_start_l,dl,ddl 規(guī)劃起點(diǎn) % 輸出 qp_path_l dl ddl 二次規(guī)劃輸出的曲線 coder.extrinsic("quadprog"); % 待優(yōu)化的變量的數(shù)量 n = 20;% 輸出初始化 qp_path_l = zeros(n,1); qp_path_dl = zeros(n,1); qp_path_ddl = zeros(n,1); qp_path_s = zeros(n,1);% H_L H_DL H_DDL H_DDDL Aeq beq A b 初始化 H_L = zeros(3*n, 3*n); H_DL = zeros(3*n, 3*n); H_DDL = zeros(3*n, 3*n); H_DDDL = zeros(n-1, 3*n); H_CENTRE = zeros(3*n, 3*n); H_L_END = zeros(3*n, 3*n); H_DL_END = zeros(3*n, 3*n); H_DDL_END = zeros(3*n, 3*n); Aeq = zeros(2*n-2, 3*n); beq = zeros(2*n-2, 1); A = zeros(8*n, 3*n); b = zeros(8*n, 1); % 更新：加入 dl(i+1) - dl(i) ddl(i+1) - ddl(i) 的約束 A_dl_minus = zeros(n - 1,3*n); b_dl_minus = zeros(n - 1,1); A_ddl_minus = zeros(n - 1,3*n); b_ddl_minus = zeros(n - 1,1); for i = 1:n-1row = i;col = 3*i - 2;A_dl_minus(row,col:col+5) = [0 -1 0 0 1 0];b_dl_minus(row) = delta_dl_max;A_ddl_minus(row,col:col+5) = [0 0 -1 0 0 1];b_ddl_minus(row) = delta_ddl_max; end % -max < a*x < max => ax < max && -ax < -(-max) A_minus = [A_dl_minus;-A_dl_minus;A_ddl_minus;-A_ddl_minus]; b_minus = [b_dl_minus;b_dl_minus;b_ddl_minus;b_ddl_minus]; % 期望的終點(diǎn)狀態(tài) end_l_desire = 0; end_dl_desire = 0; end_ddl_desire = 0;% Aeq_sub ds = 3;%縱向間隔 for i = 1:nqp_path_s(i) = plan_start_s + (i-1)*ds; endAeq_sub = [1 ds ds^2/3 -1 0 ds^2/6;0 1 ds/2 0 -1 ds/2]; % A_sub; d1 = host_d1; d2 = host_d2; w = host_w; A_sub = [1 d1 0;1 d1 0;1 -d2 0;1 -d2 0;-1 -d1 0;-1 -d1 0;-1 d2 0;-1 d2 0]; % 生成Aeq for i = 1:n-1% 計(jì)算分塊矩陣左上角的行和列row = 2*i - 1;col = 3*i - 2;Aeq(row:row + 1,col:col + 5) = Aeq_sub; end % 生成A for i = 2:nrow = 8*i - 7;col = 3*i - 2;A(row:row + 7,col:col + 2) = A_sub; end% 視頻里用的找(s(i) - d2,s(i) + d1)的方法過(guò)于保守，在這里舍棄掉 % 只要找到四個(gè)角點(diǎn)所對(duì)應(yīng)的l_min l_max 即可 % 。。。。。。。。。。。。。。 % [ . ]<- % [ ] % 。。。。。。。。。。。。。 front_index = ceil(d1/ds); back_index = ceil(d2/ds); % ceil向上取整 ceil(3) = 3 ceil(3.1) = 4 % 生成b for i = 2:n% 左前 右前的index = min(i + front_index,60)% 左后 右后的index = max(i - back_index,1)% l_min l_max 都是60個(gè)點(diǎn)index1 = min(3*i - 2 + front_index,n);index2 = max(3*i - 2 - back_index,1);b(8*i - 7:8*i,1) = [l_max(index1) - w/2;l_max(index1) + w/2;l_max(index2) - w/2;l_max(index2) + w/2;-l_min(index1) + w/2;-l_min(index1) - w/2;-l_min(index2) + w/2;-l_min(index2) - w/2;]; end %生成 lb ub 主要是對(duì)規(guī)劃起點(diǎn)做約束 lb = ones(3*n,1)*-inf; ub = ones(3*n,1)*inf; lb(1) = plan_start_l; lb(2) = plan_start_dl; lb(3) = plan_start_ddl; ub(1) = lb(1); ub(2) = lb(2); ub(3) = lb(3); for i = 2:nlb(3*i - 1) = - 2; %約束 l'ub(3*i - 1) = 2;lb(3*i) = -0.1; %約束 l''ub(3*i) = 0.1; end % 生成H_L,H_DL,H_DDL,H_CENTRE for i = 1:nH_L(3*i - 2,3*i - 2) = 1;H_DL(3*i - 1,3*i - 1) = 1;H_DDL(3*i, 3*i) = 1; end H_CENTRE = H_L; % 生成H_DDDL; H_dddl_sub = [0 0 1 0 0 -1]; for i = 1:n-1row = i;col = 3*i - 2;H_DDDL(row,col:col + 5) = H_dddl_sub; end % 生成H_L_END H_DL_END H_DDL_END H_L_END(3*n - 2,3*n - 2) = 1; H_DL_END(3*n - 1,3*n - 1) = 1; H_DDL_END(3*n,3*n) = 1; % 生成二次規(guī)劃的H 因?yàn)閐s ！= 1 所以 dddl = delta_ddl/ds; H = w_cost_l * (H_L'*H_L) + w_cost_dl * (H_DL'*H_DL) + w_cost_ddl * (H_DDL'*H_DDL)...+w_cost_dddl * (H_DDDL'*H_DDDL)/ds + w_cost_centre * (H_CENTRE'*H_CENTRE) + w_cost_end_l * (H_L_END'*H_L_END)...+w_cost_end_dl * (H_DL_END'* H_DL_END) + w_cost_ddl * (H_DDL_END'*H_DDL_END); H = 2 * H; % 生成f f = zeros(3*n,1);centre_line = 0.5 * (l_min + l_max); % 此時(shí)centre line 還是60個(gè)點(diǎn) % centre_line = dp_path_l_final; for i = 1:nf(3*i - 2) = -2 * centre_line(3*i - 2); end % 避免centreline權(quán)重過(guò)大影響軌跡平順性 for i = 1:nif abs(f(i)) > 0.3f(i) = w_cost_centre * f(i);end end % 終點(diǎn)要接近end_l dl ddl desire f(3*n - 2) = f(3*n - 2) -2 * end_l_desire * w_cost_end_l; f(3*n - 1) = f(3*n - 1) -2 * end_dl_desire * w_cost_end_dl; f(3*n) = f(3*n) -2 * end_ddl_desire * w_cost_end_ddl;X = quadprog(H,f,[A;A_minus],[b;b_minus],Aeq,beq,lb,ub);for i = 1:nqp_path_l(i) = X(3*i - 2);qp_path_dl(i) = X(3*i - 1);qp_path_ddl(i) = X(3*i); endend

增加一個(gè)函數(shù)來(lái)增密qp_path，因?yàn)槲覀円呀?jīng)知道導(dǎo)數(shù)，這里是精密插值，不是簡(jiǎn)單的插值

function [qp_path_s_final,qp_path_l_final, qp_path_dl_final,qp_path_ddl_final] = fcn(qp_path_s,qp_path_l,qp_path_dl,qp_path_ddl) % 該函數(shù)將增密qp_path n_init = 20; % 增密的點(diǎn)的個(gè)數(shù) n = 501; % 輸出初始化 qp_path_s_final = zeros(n,1); qp_path_l_final = zeros(n,1); qp_path_dl_final = zeros(n,1); qp_path_ddl_final = zeros(n,1); ds = (qp_path_s(end) - qp_path_s(1))/(n-1); index = 1; for i = 1:nx = qp_path_s(1) + (i-1) * ds;qp_path_s_final(i) = x;while x >= qp_path_s(index)index = index + 1;if index == n_initbreak;endend% while 循環(huán)退出的條件是x<qp_path_s(index)，所以x對(duì)應(yīng)的前一個(gè)s的編號(hào)是index-1 后一個(gè)編號(hào)是indexpre = index - 1;cur = index;% 計(jì)算前一個(gè)點(diǎn)的l l' l'' ds 和后一個(gè)點(diǎn)的 l''delta_s = x - qp_path_s(pre);l_pre = qp_path_l(pre);dl_pre = qp_path_dl(pre);ddl_pre = qp_path_ddl(pre);ddl_cur = qp_path_ddl(cur);% 分段加加速度優(yōu)化 qp_path_l_final(i) = l_pre + dl_pre * delta_s + (1/3)* ddl_pre * delta_s^2 + (1/6) * ddl_cur * delta_s^2;qp_path_dl_final(i) = dl_pre + 0.5 * ddl_pre * delta_s + 0.5 * ddl_cur * delta_s;qp_path_ddl_final(i) = ddl_pre + (ddl_cur - ddl_pre) * delta_s/(qp_path_s(cur) - qp_path_s(pre));% 因?yàn)榇藭r(shí)x的后一個(gè)編號(hào)是index 必有x < qp_path_s(index),在下一個(gè)循環(huán)中x = x + ds 也未必大于% qp_path_s(index)，這樣就進(jìn)入不了while循環(huán)，所以index 要回退一位index = index - 1; end

?

最后來(lái)到Frenet2Cartesian模塊，更改輸入

?把里面的180改成600，一共500多個(gè)點(diǎn)

?

畫圖模塊也更新一下

function fcn(dp_s,dp_l,qp_s,qp_l,l_min,l_max,obs_s_set,obs_l_set,start_s,start_l,start_dl) coder.extrinsic("plot","axis","figure","drawnow","cla","fill");obs_length = 5; obs_width = 2; host_l = 6; host_w = 1.63; cla; host_p1_l = start_l + sin(atan(start_dl)) * host_l /2 + host_w/2 * cos(atan(start_dl)); host_p1_s = start_s + cos(atan(start_dl)) * host_l /2 - host_w/2 * sin(atan(start_dl)); host_p2_l = start_l + sin(atan(start_dl)) * host_l /2 - host_w/2 * cos(atan(start_dl)); host_p2_s = start_s + cos(atan(start_dl)) * host_l /2 + host_w/2 * sin(atan(start_dl)); host_p3_l = start_l - sin(atan(start_dl)) * host_l /2 + host_w/2 * cos(atan(start_dl)); host_p3_s = start_s - cos(atan(start_dl)) * host_l /2 - host_w/2 * sin(atan(start_dl)); host_p4_l = start_l - sin(atan(start_dl)) * host_l /2 - host_w/2 * cos(atan(start_dl)); host_p4_s = start_s - cos(atan(start_dl)) * host_l /2 + host_w/2 * sin(atan(start_dl)); host_area_x = [host_p1_s;host_p2_s;host_p4_s;host_p3_s]; host_area_y = [host_p1_l;host_p2_l;host_p4_l;host_p3_l]; fill(host_area_x,host_area_y,[0 1 1]); hold on plot(dp_s,dp_l,'r');plot(qp_s,qp_l,'b'); plot(dp_s,l_min,dp_s,l_max); axis([-8 70 -10 10]);for i = 1:length(obs_s_set)if isnan(obs_s_set(i))break;endobs_p1_s = obs_s_set(i) + obs_length/2;obs_p1_l = obs_l_set(i) + obs_width/2;obs_p2_s = obs_s_set(i) + obs_length/2;obs_p2_l = obs_l_set(i) - obs_width/2;obs_p3_s = obs_s_set(i) - obs_length/2;obs_p3_l = obs_l_set(i) + obs_width/2;obs_p4_s = obs_s_set(i) - obs_length/2;obs_p4_l = obs_l_set(i) - obs_width/2;obs_area_s = [obs_p1_s;obs_p2_s;obs_p4_s;obs_p3_s];obs_area_l = [obs_p1_l;obs_p2_l;obs_p4_l;obs_p3_l];fill(obs_area_s,obs_area_l,[0 0 0]); end

?下面run一下看下效果

?方向盤轉(zhuǎn)角

總結(jié)

以上是生活随笔為你收集整理的自动驾驶决策规划算法第二章——Apollo EM Planner实践篇的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。