最近的工作重心回到到算法上之后，陸陸續續開始學習一些自動駕駛的控制算法。目前工作的方向主要是online trajectory generation和底層控制。

對于在線路徑規劃，一個重要的概念是其應該能提供給車輛一個速度和方向控制命令，使底層控制器跟隨這個輸入去執行運動控制操作。

人工勢場法(Artificial Potential Field Method)是一個典型的在線路徑算法。其運用了'水往低處流'的思路，很自然的能夠理解車輛路徑的產生規律。

話不多說，直接上Matlab代碼看一下。

close all;clear

%% Copyright@https://www.jianshu.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]; %目標

Obs = [3,2;

3,3;

5,7;

5.3,6;

6,6;

2,4;

3,8;

4,7;

8,9]; %障礙物坐標

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); %基本斥力場公式

if isinf(Y_rre(k3))==1|| Y_rre(k3)>150 %為顯示效果做的限制處理

Y_rre = 150;

end

end

Y_ata = b*rat; %基本引力場公式，注意這里引力只有一個值

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) %總力場

下面兩個圖是基于以上代碼最后的路徑和總勢場。 [10,10] 是目標點，而[0,0]是我們的初始位置。

Path

Field

其實該方法有很多局限性，比如如果障礙物很靠近終點(如[9,9])，則可能無法達到。如果某個地方應力與斥力相等，則可能陷入布局最優。因此有很多改進法。后續再談。

總結

以上是生活随笔為你收集整理的matlab人工势场法三维演示图,人工势场法（Artificial Potential Field Method）的学习的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。