最近的工作重心回到到算法上之后蚓土，陸陸續(xù)續(xù)開(kāi)始學(xué)習(xí)一些自動(dòng)駕駛的控制算法被廓。目前工作的方向主要是online trajectory generation和底層控制。
對(duì)于在線(xiàn)路徑規(guī)劃，一個(gè)重要的概念是其應(yīng)該能提供給車(chē)輛一個(gè)速度和方向控制命令脊阴，使底層控制器跟隨這個(gè)輸入去執(zhí)行運(yùn)動(dòng)控制操作槽驶。
人工勢(shì)場(chǎng)法（Artificial Potential Field Method）是一個(gè)典型的在線(xiàn)路徑算法责嚷。其運(yùn)用了'水往低處流'的思路，很自然的能夠理解車(chē)輛路徑的產(chǎn)生規(guī)律掂铐。
話(huà)不多說(shuō)罕拂，直接上Matlab代碼看一下。
close all;clear
%% Copyright@http://www.reibang.com/u/1f9b23cfe862
density = 0.2;
Grid_X = 0:density:10;
Grid_Y = 0:density:10;
Basic_Z = ones(length(Grid_X), length(Grid_Y));
P0 = 20;
a = 20; %斥力影響因素
b = 10; %引力影響因素
%%
Goal = [10,10]; %目標(biāo)
Obs = [3,2;
3,3;
5,7;
5.3,6;
6,6;
2,4;
3,8;
4,7;
8,9]; %障礙物坐標(biāo)
for k1 = 1: length(Grid_X)
for k2 =1:length(Grid_Y)
X_c = Grid_X(k1);
Y_c = Grid_Y(k2);
rre =[];
rat = sqrt((Goal(1)-X_c)^2 +(Goal(2)-Y_c)^2);
Y_rre = [];
Y_ata = [];
for k3 = 1:length(Obs)
rre(k3) = sqrt((Obs(k3,1)-X_c)^2 +(Obs(k3,2)-Y_c)^2);
Y_rre(k3) = a*(1/rre(k3) - 1/P0 ) *1/(rre(k3)^2); %基本斥力場(chǎng)公式
if isinf(Y_rre(k3))==1|| Y_rre(k3)>150 %為顯示效果做的限制處理
Y_rre = 150;
end
end
Y_ata = b*rat; %基本引力場(chǎng)公式全陨，注意這里引力只有一個(gè)值
Field_rre(k1,k2) = sum(Y_rre);
Field_ata(k1,k2) = Y_ata;
end
end
SUM = Field_rre + Field_ata;
surf(Grid_X,Grid_Y,SUM) %總力場(chǎng)
下面兩個(gè)圖是基于以上代碼最后的路徑和總勢(shì)場(chǎng)爆班。 [10,10] 是目標(biāo)點(diǎn)，而[0,0]是我們的初始位置辱姨。
Path
Field
其實(shí)該方法有很多局限性柿菩，比如如果障礙物很靠近終點(diǎn)（如[9,9]），則可能無(wú)法達(dá)到雨涛。如果某個(gè)地方應(yīng)力與斥力相等碗旅，則可能陷入布局最優(yōu)。因此有很多改進(jìn)法镜悉。后續(xù)再談祟辟。
