# 改进型人工势场法

1 篇文章 1 订阅
1 篇文章 0 订阅

## 1、人工势场法基本原理

U ( q ) = U a t t ( q ) + U r e p ( q ) U(q)=U_{att}(q)+U_{rep}(q)

F ( q ) = − ∇ U a t t ( q ) − ∇ U r e p ( q ) = F a t t ( q ) + F r e p ( q ) F(q)=-\nabla U_{att}(q)-\nabla U_{rep}(q)=F_{att}(q)+F_{rep}(q)

U a t t = 1 2 K a t t ( q − q g ) 2 U_{att}=\frac {1}{2}K_{att}(q-q_g)^2

F a t t = − ∇ U a t t ( q ) = − K a t t ( q − q g ) F_{att}=-\nabla U_{att}(q)=-K_{att}(q-q_g)
F a t t F_{att} 的方向指向目标点，与无人车当前位置到目标点位置的距离成线性关系。 F a t t F_{att} 可以分解为x轴方向的力与y轴方向的力：
F a t t x = − K a t t ( x − x g ) F_{attx}=-K_{att}(x-x_g)
F a t t y = − K a t t ( y − y g ) F_{atty}=-K_{att}(y-y_g)

U r e p = { 0 , if  ρ o b s ( q ) ≥ ρ 0 1 2 K r e p ( 1 ρ o b s ( q ) − 1 ρ 0 ) 2 , if  ρ o b s ( q ) < ρ 0 U_{rep}=\begin {cases} 0, & \text {if $\rho_{obs}(q)\geq\rho_{0}$} \\ \frac {1}{2}K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})^2, & \text{if $\rho_{obs}(q)<\rho_{0}$} \end{cases}

F r e p = − ∇ U r e p ( q ) = { 0 , if  ρ o b s ( q ) ≥ ρ 0 K r e p ( 1 ρ o b s ( q ) − 1 ρ 0 ) ( 1 ρ o b s 2 ( q ) ) q − q o b s ∣ ∣ q − q o b s ∣ ∣ , if  ρ o b s ( q ) < ρ 0 F_{rep}=-\nabla U_{rep}(q)=\begin {cases} 0, & \text {if $\rho_{obs}(q)\geq\rho_{0}$} \\ K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})(\frac{1}{\rho_{obs}^2(q)})\frac{q-q_{obs}}{||q-q_{obs}||}, & \text{if $\rho_{obs}(q)<\rho_{0}$} \end{cases}
F r e p F_{rep} 的方向指向障碍物点的反方向， F r e p F_{rep} 可以分解为x轴方向的力与y轴方向的力，如下所示：
F r e p x = { 0 , if  ρ o b s ( q ) ≥ ρ 0 K r e p ( 1 ρ o b s ( q ) − 1 ρ 0 ) ( 1 ρ o b s 2 ( q ) ) x − x o b s ∣ ∣ q − q o b s ∣ ∣ , if  ρ o b s ( q ) < ρ 0 F_{repx}=\begin {cases} 0, & \text {if $\rho_{obs}(q)\geq\rho_{0}$} \\ K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})(\frac{1}{\rho_{obs}^2(q)})\frac{x-x_{obs}}{||q-q_{obs}||}, & \text{if $\rho_{obs}(q)<\rho_{0}$} \end{cases}
F r e p y = { 0 , if  ρ o b s ( q ) ≥ ρ 0 K r e p ( 1 ρ o b s ( q ) − 1 ρ 0 ) ( 1 ρ o b s 2 ( q ) ) y − y o b s ∣ ∣ q − q o b s ∣ ∣ , if  ρ o b s ( q ) < ρ 0 F_{repy}=\begin {cases} 0, & \text {if $\rho_{obs}(q)\geq\rho_{0}$} \\ K_{rep}(\frac {1}{\rho_{obs}(q)}-\frac {1}{\rho_{0}})(\frac{1}{\rho_{obs}^2(q)})\frac{y-y_{obs}}{||q-q_{obs}||}, & \text{if $\rho_{obs}(q)<\rho_{0}$} \end{cases}

U ( q ) = U a t t ( q ) + ∑ i = 1 n U r e p ( q ) U(q)=U_{att}(q)+\sum_{i=1}^n U_{rep}(q)
F ( q ) = F a t t ( q ) + ∑ i = 1 n F r e p ( q ) F(q)=F_{att}(q)+\sum_{i=1}^n F_{rep}(q)

## 2、人工势场法缺陷

(1)容易陷入局部极小值。当无人车所受引力与斥力大小相等、方向相反时，无人车所受合力为0，此时无人车陷入了局部极小值点。在局部极小值点周围的各个位置的合力都指向局部极小值，导致无人车在局部极小值点周围振荡，无法自行走出该区域。
(2)统人工势场法引力部分与无人车和目标点位置距离成正比，当无人车距离目标点较远时，引力部分过大，可能导致无人车与障碍物相撞。
(3)目标点附近有障碍物时，无人车可能无法到达目标点。

## 3、人工势场法改进方法

### 3.1、人工势场法引力场改进

U r e p = { 1 2 K a t t ( q − q g ) 2 , if  ( q − q g ) ≤ d d K a t t ( q − q g ) , if  ( q − q g ) > d U_{rep}=\begin {cases} \frac{1}{2}K_{att}(q-q_g)^2, & \text {if $(q-q_g)\leq d$} \\ dK_{att}(q-q_g), & \text{if $(q-q_g)> d$} \end{cases}
U r e p = { − K a t t ( q − q g ) , if  ( q − q g ) ≤ d − d K a t t q − q g ∣ ∣ q − q g ∣ ∣ , if  ( q − q g ) > d U_{rep}=\begin {cases} -K_{att}(q-q_g), & \text {if $(q-q_g)\leq d$} \\ -dK_{att}\frac {q-q_g}{||q-q_g||}, & \text{if $(q-q_g)> d$} \end{cases}

### 3.2、模拟退火法

#### 3.2.1、模拟退火法原理

1、设置 x = S x=S ( S S 局部极小值或者起点)。
2、选择退火策略，设置 T = T 0 T=T_0
3、While( T ≥ T f T\geq T_f 并且没有逃离局部极小值)

3.1、产生一个随机点 x 1 = x + Δ x x_1=x+\Delta x ( Δ x \Delta x 为点 x x 附近一个随机点，距离点 x x 一个步长)。
3.2、计算 U ( x 1 ) U(x_1) ，即点 x 1 x_1 处的势能。
3.3、设置 Δ = U ( x 1 ) − U ( x ) \Delta=U(x_1)-U(x)
3.4、如果 Δ ≤ 0 \Delta \leq 0 ，设置 x = x 1 x=x_1
3.5、如果 Δ > 0 \Delta>0 ，以概率P设置 x = x 1 x=x_1 ，其中 P = e − Δ T , Δ = U ( x 1 ) − U ( x ) P=e^{-\frac{\Delta}{T}},\Delta=U(x_1)-U(x) （具体为： 设置一个随机概率a，当P>a时，设置 x = x 1 x=x_1 ，反之，这个随机点为失败的随机点，再重新设置随机点）
3.6、如果 U ( x 1 ) ≤ U ( S ) U(x_1)\leq U(S) ，成功逃离局部极小值。
4、如果没有逃离局部极小值，返回失败；否则，返回成功。

## 4、代码解释与链接

1、运行main.m即可，代码里面有详细的备注，大家应该都可以看得懂，欢迎大家一起交流。
2、在main.m最下面部分有对路径的优化算法，路径优化有两个算法，分别check_lineA.m和check_lineB.m。为由于楼主最近比较忙，所以没有时间更新对优化路径算法的解释，有兴趣的小伙伴可以自己先看看。

05-21
01-22 1万+
08-10 2045
05-18 907
07-24 501
05-15
04-15
04-13 2436
©️2021 CSDN 皮肤主题: 大白 设计师:CSDN官方博客

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、C币套餐、付费专栏及课程。