一、算法簡介

1986年Khatib首先提出人工勢場法，并將其應(yīng)用在機(jī)器人避障領(lǐng)域，而現(xiàn)代汽車可以看作是一個高速行駛的機(jī)器人，所以該方法也可應(yīng)用于汽車的避障路徑規(guī)劃領(lǐng)域。

二、算法思想

1、人工勢場法的基本思想是在障礙物周圍構(gòu)建障礙物斥力勢場，在目標(biāo)點(diǎn)周圍構(gòu)建引力勢場，類似于物理學(xué)中的電磁場。

2、被控對象在這兩種勢場組成的復(fù)合場中受到斥力作用和引力作用，斥力和引力的合力指引著被控對象的運(yùn)動，搜索無碰的避障路徑。

3、更直觀而言，勢場法是將障礙物比作是平原上具有高勢能值的山峰，而目標(biāo)點(diǎn)則是具有低勢能值的低谷。

?三、算法內(nèi)容

????????引力勢場主要與汽車和目標(biāo)點(diǎn)間的距離有關(guān)，距離越大，汽車所受的勢能值就越大；距離越小，汽車所受的勢能值則越小，所以引力勢場的函數(shù)為：

????????其中n為正比例增益系數(shù)，p(q，qg)為一個矢量，表示汽車的位置q和目標(biāo)點(diǎn)位置qg之間的歐幾里德距離|q-qg|，矢量方向是從汽車的位置指向目標(biāo)點(diǎn)位置。

????????響應(yīng)的引力Fatt（X）為引力場的負(fù)梯度：

????????決定障礙物斥力勢場的因素是汽車與障礙物間的距離，當(dāng)汽車未進(jìn)入障礙物的影響范圍時，其受到的勢能值為零；在汽車進(jìn)入障礙物的影響范圍后，兩者之間的距離越大，汽車受到的勢能值就越小，距離越小，汽車受到的勢能值就越大。斥力勢場的勢場函數(shù)為：

????????其中k為正比例系數(shù)，p(q，q。)為一矢量，方向為從障礙物指向汽車，大小為汽車與障礙物間的距離|q-q。|，p。為一常數(shù)，表示障礙物對汽車產(chǎn)生作用的最大距離。

????????響應(yīng)的斥力為斥力場的負(fù)梯度

?????????設(shè)車輛位置為(×，y)，障礙物位置為(xg, yg)。則引力勢場函數(shù)為：

?????????故有：

?????????斥力勢場函數(shù)為：

????????目標(biāo)不可達(dá)的問題。由于障礙物與目標(biāo)點(diǎn)距離太近，當(dāng)汽車到達(dá)目標(biāo)點(diǎn)時，根據(jù)勢場函數(shù)可知，目標(biāo)點(diǎn)的引力降為零，而障礙物的斥力不為零，此時汽車雖到達(dá)目標(biāo)點(diǎn)，但在斥力場的作用下不能停下來，從而導(dǎo)致目標(biāo)不可達(dá)的問題。

????????陷入局部最優(yōu)的問題。車輛在某個位置時，如果若干個障礙物的合斥力與目標(biāo)點(diǎn)的引力大小相等、方向相反，則合力為0，這將導(dǎo)致車輛不再“受力”，故無法向前搜索避障路徑。

四、算法缺陷與改進(jìn)

1、通過改進(jìn)障礙物斥力勢場函數(shù)來解決局部最優(yōu)和目標(biāo)不可達(dá)的問題；

?2、通過建立道路邊界斥力勢場以限制汽車的行駛區(qū)域，并適當(dāng)考慮車輛速度對斥力場的影響

% 人工勢場法 % 作者：Ally % 日期：2021/1/24 clc clear close all%% 初始化車的參數(shù) d = 3.5; % 道路標(biāo)準(zhǔn)寬度 W = 1.8; % 汽車寬度 L = 4.7; % 車長P0 = [0,-d/2,1,1]; % 車輛起點(diǎn)信息，1-2列位置，3-4列速度 Pg = [99,d/2,0,0]; % 目標(biāo)位置 Pobs = [15,7/4,0,0;30,-3/2,0,0;45,3/2,0,0;60,-3/4,0,0;80,7/4,0,0]; % 障礙物位置 P = [Pobs;Pg]; % 將目標(biāo)位置和障礙物位置合放在一起Eta_att = 5; % 計算引力的增益系數(shù) Eta_rep_ob = 15; % 計算斥力的增益系數(shù) Eta_rep_edge = 50; % 計算邊界斥力的增益系數(shù)d0 = 20; % 障礙影響距離 n = size(P,1); % 障礙與目標(biāo)總計個數(shù) len_step = 0.5; % 步長 Num_iter = 200; % 最大循環(huán)迭代次數(shù)%% ***************初始化結(jié)束，開始主體循環(huán)****************** Pi = P0; %將車的起始坐標(biāo)賦給Xi i = 0; while sqrt((Pi(1)-P(n,1))^2+(Pi(2)-P(n,2))^2) > 1i = i + 1;Path(i,:) = Pi; % 保存車走過的每個點(diǎn)的坐標(biāo)%計算車輛當(dāng)前位置與障礙物的單位方向向量、速度向量for j = 1:n-1 delta(j,:) = Pi(1,1:2) - P(j,1:2); % 用車輛點(diǎn)-障礙點(diǎn)表達(dá)斥力dist(j,1) = norm(delta(j,:)); % 車輛當(dāng)前位置與障礙物的距離unitVector(j,:) = [delta(j,1)/dist(j,1), delta(j,2)/dist(j,1)]; % 斥力的單位方向向量end%計算車輛當(dāng)前位置與目標(biāo)的單位方向向量、速度向量delta(n,:) = P(n,1:2)-Pi(1,1:2); %用目標(biāo)點(diǎn)-車輛點(diǎn)表達(dá)引力 dist(n,1) = norm(delta(n,:)); unitVector(n,:)=[delta(n,1)/dist(n,1),delta(n,2)/dist(n,1)];%% 計算斥力 % 在原斥力勢場函數(shù)增加目標(biāo)調(diào)節(jié)因子（即車輛至目標(biāo)距離），以使車輛到達(dá)目標(biāo)點(diǎn)后斥力也為0for j = 1:n-1if dist(j,1) >= d0F_rep_ob(j,:) = [0,0];else% 障礙物的斥力1，方向由障礙物指向車輛F_rep_ob1_abs = Eta_rep_ob * (1/dist(j,1)-1/d0) * dist(n,1) / dist(j,1)^2; F_rep_ob1 = [F_rep_ob1_abs*unitVector(j,1), F_rep_ob1_abs*unitVector(j,2)]; % 障礙物的斥力2，方向由車輛指向目標(biāo)點(diǎn)F_rep_ob2_abs = 0.5 * Eta_rep_ob * (1/dist(j,1) - 1/d0)^2; F_rep_ob2 = [F_rep_ob2_abs * unitVector(n,1), F_rep_ob2_abs * unitVector(n,2)]; % 改進(jìn)后的障礙物合斥力計算F_rep_ob(j,:) = F_rep_ob1+F_rep_ob2; endend% 增加邊界斥力勢場，根據(jù)車輛當(dāng)前位置，選擇對應(yīng)的斥力函數(shù)if Pi(1,2) > -d+W/2 && Pi(1,2) <= -d/2 %下道路邊界區(qū)域力場，方向指向y軸正向F_rep_edge = [0,Eta_rep_edge * norm(Pi(:,3:4))*(exp(-d/2-Pi(1,2)))];elseif Pi(1,2) > -d/2 && Pi(1,2) <= -W/2 %下道路分界線區(qū)域力場，方向指向y軸負(fù)向F_rep_edge = [0,1/3 * Eta_rep_edge * Pi(1,2).^2];elseif Pi(1,2) > W/2 && Pi(1,2) < d/2 %上道路分界線區(qū)域力場，方向指向y軸正向 F_rep_edge = [0, -1/3 * Eta_rep_edge * Pi(1,2).^2];elseif Pi(1,2) > d/2 && Pi(1,2)<=d-W/2 %上道路邊界區(qū)域力場，方向指向y軸負(fù)向F_rep_edge = [0, Eta_rep_edge * norm(Pi(:,3:4)) * (exp(Pi(1,2)-d/2))];end%% 計算合力和方向F_rep = [sum(F_rep_ob(:,1)) + F_rep_edge(1,1),...sum(F_rep_ob(:,2)) + F_rep_edge(1,2)]; % 所有障礙物的合斥力矢量F_att = [Eta_att*dist(n,1)*unitVector(n,1), Eta_att*dist(n,1)*unitVector(n,2)]; % 引力矢量F_sum = [F_rep(1,1)+F_att(1,1),F_rep(1,2)+F_att(1,2)]; % 總合力矢量UnitVec_Fsum(i,:) = 1/norm(F_sum) * F_sum; % 總合力的單位向量%計算車的下一步位置Pi(1,1:2)=Pi(1,1:2)+len_step*UnitVec_Fsum(i,:); % %判斷是否到達(dá)終點(diǎn) % if sqrt((Pi(1)-P(n,1))^2+(Pi(2)-P(n,2))^2) < 0.2 % break % end end Path(i,:)=P(n,:); %把路徑向量的最后一個點(diǎn)賦值為目標(biāo)%% 畫圖 figure len_line = 100;% 畫灰色路面圖 GreyZone = [-5,-d-0.5; -5,d+0.5; len_line,d+0.5; len_line,-d-0.5]; fill(GreyZone(:,1),GreyZone(:,2),[0.5 0.5 0.5]); hold on fill([P0(1),P0(1),P0(1)-L,P0(1)-L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],'b') %2號車% 畫分界線 plot([-5, len_line],[0, 0], 'w--', 'linewidth',2); %分界線 plot([-5,len_line],[d,d],'w','linewidth',2); %左邊界線 plot([-5,len_line],[-d,-d],'w','linewidth',2); %左邊界線% 設(shè)置坐標(biāo)軸顯示范圍 axis equal set(gca, 'XLim',[-5 len_line]); set(gca, 'YLim',[-4 4]); % 繪制路徑 plot(P(1:n-1,1),P(1:n-1,2),'ro'); %障礙物位置 plot(P(n,1),P(n,2),'gv'); %目標(biāo)位置 plot(P0(1,1),P0(1,2),'bs'); %起點(diǎn)位置 plot(Path(:,1),Path(:,2),'.b');%路徑點(diǎn)

學(xué)習(xí)自B站：小黎的Ally

視頻鏈接：路徑規(guī)劃與軌跡跟蹤系列算法學(xué)習(xí)_第6講_人工勢場法_嗶哩嗶哩_bilibili

總結(jié)

以上是生活随笔為你收集整理的学习笔记：人工势场法的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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