當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

【控制】人工势场法及人工势场函数

發布時間：2023/12/31 编程问答 38 豆豆

生活随笔收集整理的這篇文章主要介紹了【控制】人工势场法及人工势场函数小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

人工勢場法-維基百科

人工勢場法是由Khatib提出的一種機器人路徑規劃算法。該算法將目標和障礙物分別看做對機器人有引力和斥力的物體，機器人沿引力與斥力的合力來進行運動。

該法結構簡單，便于低層的實時控制，在實時避障和平滑的軌跡控制方面，得到了廣泛應用，其不足在于存在局部最優解，容易產生死鎖現象，因而可能使移動機器人在到達目標點之前就停留在局部最優點。

From: 人工勢場法-維基百科

路徑規劃-人工勢場法（Artifical Potential Field）

引力場 (attractive/gravitation field)

常用的引力函數：
$Uatt(q)=12ξρ2(q,qgoal)U_{att}(q) = \frac{1}{2}\xi\rho^2(q,q_{goal})$

這里的 $ξ\xi$ 是尺度因子， $ρ(q,qgoal)\rho(q,q_{goal})$ 表示物體當前狀態與目標的距離。引力場有了，那么引力就是引力場對距離的導數（類比物理里面 $W = F X$ ）：
$Fatt(q)=??Uatt(q)=ξ(qgoal?q)F_{att}(q) = -\nabla U_{att}(q) = \xi(q_{goal}-q)$

斥力場 (repulsive field)

傳統的斥力場公式
$Urep(q)={12η(1ρ(q,qobs)?1ρ0)2,if?ρ(q,qobs)≤ρ00,if?ρ(q,qobs)>ρ0U_{rep}(q) = \left\{\begin{aligned} \frac{1}{2} \eta (\frac{1}{\rho(q, q_{obs})} - \frac{1}{\rho_0})^2, \quad \text{if}\ \rho(q, q_{obs}) \le \rho_0 \\ 0,\quad \text{if}\ \rho(q, q_{obs}) > \rho_0 \end{aligned}\right.$

其中，
$η\eta$ 是斥力尺度因子，
$ρ(q,qobs)\rho(q, q_{obs})$ 代表物體和障礙物之間的距離。
$ρ0\rho_0$ 代表每個障礙物的影響半徑。
換言之，離開一定的距離，障礙物就對物體沒有斥力影響。

斥力就是斥力場的梯度
$Frep(q)=??Urep(q)={η(1ρ(q,qobs)?1ρ0)?1ρ2(q,qobs)?ρ(q,qobs),if?ρ(q,qobs)≤ρ00,if?ρ(q,qobs)>ρ0\begin{aligned}F_{rep}(q) &= -\nabla U_{rep}(q)\\ &= \left\{\begin{aligned} &\eta (\frac{1}{\rho(q, q_{obs})} - \frac{1}{\rho_0}) \\ & \cdot \frac{1}{\rho^2(q,q_{obs})}\nabla\rho(q,q_{obs}),\quad & \text{if}\ \rho(q, q_{obs}) \le \rho_0 \\ & 0,\quad & \text{if}\ \rho(q, q_{obs}) > \rho_0 \end{aligned}\right.\end{aligned}$

總場

總的場就是斥力場合引力場的疊加，也就是 $U=U_{att}+U_{rep}$ ，總的力也是對對應的分力的疊加，如下圖所示：

From: 路徑規劃-人工勢場法（Artifical Potential Field）

【機器人路徑規劃】人工勢場法

1.概述

我們打兩個比方來說明人工勢場法的作用機理。首先，我們把構型空間比作一個電勢場平面，機器人（的當前構型）比作空間中一點。如果讓機器人的起點和障礙物帶正電荷，終點帶負電荷，機器人帶正電荷。由于同性電荷相斥，異性電荷相吸的原理，機器人將會在電場力的作用下沿著某條路徑向終點移動，并避開帶正電荷的障礙物，如圖1所示。

類似的，我們也可以把構型空間比作一個有起伏地形的區域。其中，起點和障礙物位于較高的區域，終點位于較低的區域，機器人視作一個球體。那么在重力的作用下，機器人將沿著某條軌跡從較高的起點滑落到較低的終點，并避開較高的障礙物。如圖2[2]所示。

以上的兩個例子其實就是電勢場與重力勢場的作用機制，電勢場和重力勢場都是自然勢場。而人工勢場法就是在已知起點、終點和障礙物位置的情況下，構建一個人工勢場來模仿這種作用機制。人工勢場法的優點在于，它其實是一種反饋控制策略，對控制和傳感誤差有一定的魯棒性；缺點在于存在局部極小值問題，因此不能保證一定能找到問題的解。

From: 【機器人路徑規劃】人工勢場法

Paper

M. Zhang, Y. Shen, Q. Wang and Y. Wang, “Dynamic artificial potential field based multi-robot formation control,” 2010 IEEE Instrumentation & Measurement Technology Conference Proceedings, 2010, pp. 1530-1534, doi: 10.1109/IMTC.2010.5488238.

close all;cleardensity = 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;endendY_ata = b*rat; %基本引力場公式，注意這里引力只有一個值Field_rre(k1,k2) = sum(Y_rre);Field_ata(k1,k2) = Y_ata;end endSUM = Field_rre + Field_ata; surf(Grid_X,Grid_Y,SUM) %總力場

Matlab 代碼

人工勢場算法 Matlab版源碼

自己編寫的 Matlab

1. 僅考慮引力的情況

% 自寫人工勢場法測試函數 % 說明： % X 軸：時間 time % Y 軸：小車橫坐標 % Z 軸：小車縱坐標 % 初始位置：[0 0]' % 目標位置：[10 10]'clearP0 = [0 0]'; Pt(:,1) = P0; PGoal = [10 10]'; Ut(:,1) = [0 0]';% 時間參數 tBegin = 0; tEnd = 10; dT = 0.01; T(1,1) = tBegin; times = (tEnd - tBegin)/dT;% 其他參數 xi = 0.8;for time=1:timesT(1,time+1) = T(1,time) + dT;% 引力FGravitation = xi * (PGoal - Pt(:,time));% 斥力FRepulsive = 0;% 合力Ut(:,time+1) = FGravitation + FRepulsive;% 更新位置Pt(:,time+1) = Pt(:,time) + dT * Ut(:,time); endfigure(1) plot3(T,Pt(1,:),Pt(2,:)) xlabel('Time'); ylabel('XPosition'); zlabel('YPosition'); grid onfigure(2) plot3(T,Ut(1,:),Ut(2,:)) xlabel('Time'); ylabel('XSpeed'); zlabel('YSpeed'); grid on

總結

以上是生活随笔為你收集整理的【控制】人工势场法及人工势场函数的全部內容，希望文章能夠幫你解決所遇到的問題。

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