基于人工势场法的路径规划
\qquad
路径规划是移动机器人领域的一个重要组成部分,人工势场法是机器人路径规划算法中一种简单有效的方法。
\qquad
势场法的基本思想是在移动机器人的工作环境中构造一个人工势场,势场中包括斥力极和吸引极,不希望机器人进入的区域和障碍物定义为斥力极,目标及建议机器人进入的区域定义为引力极,使得在该势场中的移动机器人受到其目标位姿引力场和障碍物周围斥力场的共同作用,朝目标前进。
\qquad
机器人在工作空间中的位置为
X
=
[
x
y
]
T
X=[x\quad y]^T
X=[xy]T,通常的目标函数势场函数(引力场函数)为:
U
a
t
(
X
)
=
1
2
k
(
X
−
X
g
o
a
l
)
2
(
1
)
{ U_{at}(X) = \frac12k(X-X_{goal})^2 \qquad (1) }
Uat(X)=21k(X−Xgoal)2(1)
\qquad
其中k为位置增益函数,
(
X
−
X
g
o
a
l
)
(X-X_{goal})
(X−Xgoal)是机器人与目标
X
g
o
a
l
X_{goal}
Xgoal之间的相对位置。相应的吸引力为目标势场函数的负梯度:
F
a
t
(
X
)
=
−
g
r
a
d
[
U
a
t
(
X
)
]
=
−
k
(
X
−
X
g
o
a
l
)
(
2
)
{ F_{at} (X)= -grad[U_{at}(X)]=-k(X-X_{goal}) \qquad (2) }
Fat(X)=−grad[Uat(X)]=−k(X−Xgoal)(2)
\qquad
当机器人达到目标的过程中,这个力线性收敛于零。
\qquad
斥力场函数为:
U
r
e
(
X
)
=
{
1
2
η
[
1
ρ
−
1
ρ
0
]
2
(
X
−
X
g
o
a
l
)
n
ρ
≤
ρ
0
0
ρ
>
ρ
0
(
3
)
U_{re}(X)= \begin{cases} \frac12 \eta \left[ \frac{1}{\rho} -\frac{1}{\rho_{0}} \right]^{2}(X-X_{goal})^{n} &&\text{$\rho\leq\rho_{0}$} \\ \qquad\qquad\qquad 0 &&\text{$\rho>\rho_{0}$} \end{cases} \qquad (3)
Ure(X)=⎩⎨⎧21η[ρ1−ρ01]2(X−Xgoal)n0ρ≤ρ0ρ>ρ0(3)
\qquad
其中
η
\eta
η为位置增益系数,
ρ
\rho
ρ为机器人在空间的位置X与障碍物之间的最短距离,
ρ
0
\rho_{0}
ρ0是一个常数,代表障碍物的影响距离。
n
n
n是一个大于零的任意常数,由于引入机器人与目标之间的相对距离,保证整个势场仅在目标点
X
g
o
a
l
X_{goal}
Xgoal全局最小。
\qquad
当
X
≠
X
g
o
a
l
X \ne X_{goal}
X̸=Xgoal时,斥力可写为:
F
r
e
(
X
)
=
−
g
r
a
d
[
U
r
e
(
X
)
]
=
{
F
r
e
1
+
F
r
e
2
ρ
≤
ρ
0
0
ρ
>
ρ
0
(
4
)
F_{re}(X)=-grad[U_{re}(X)]= \begin{cases} F_{re1}+F_{re2}&&\text{$\rho\leq\rho_{0}$}\\ \qquad 0&&\text{$\rho>\rho_{0}$} \end{cases} \qquad (4)
Fre(X)=−grad[Ure(X)]={Fre1+Fre20ρ≤ρ0ρ>ρ0(4)
\qquad
其中
F
r
e
1
=
η
[
1
ρ
−
1
ρ
0
]
1
ρ
2
∂
ρ
∂
X
(
X
−
X
g
o
a
l
)
n
(
5
)
F_{re1}=\eta[\frac{1}{\rho}-\frac{1}{\rho_{0}}]\frac{1}{\rho^{2}}\frac{\partial {\rho}}{\partial X}(X-X_{goal})^{n} \qquad (5)
Fre1=η[ρ1−ρ01]ρ21∂X∂ρ(X−Xgoal)n(5)
∂
ρ
∂
X
=
[
∂
ρ
∂
x
∂
ρ
∂
y
]
T
(
6
)
\frac{\partial {\rho}}{\partial X}=[\frac{\partial {\rho}}{\partial x}\quad \frac{\partial {\rho}}{\partial y}]^{T}\qquad(6)
∂X∂ρ=[∂x∂ρ∂y∂ρ]T(6)
F
r
e
2
=
−
n
2
η
[
1
ρ
−
1
ρ
0
]
2
(
X
−
X
g
o
a
l
)
n
−
1
(
7
)
F_{re2}=-\frac{n}{2}\eta[\frac{1}{\rho}-\frac{1}{\rho_{0}}]^{2}(X-X_{goal})^{n-1} \qquad (7)
Fre2=−2nη[ρ1−ρ01]2(X−Xgoal)n−1(7)
\qquad 势场函数 U r e U_{re} Ure, U a t U_{at} Uat使得机器人所受的合力 F s u m F_{sum} Fsum将驱使机器人远离障碍物,逼近目标。矢量 F r e 1 F_{re1} Fre1的方向为从障碍物指向机器人,矢量 F r e 2 F_{re2} Fre2的方向为从机器人指向目标。显然 , 当 F r e 1 F_{re1} Fre1对机器人产生斥力时, F r e 2 F_{re2} Fre2对机器人产生朝向目标的吸引力。
参考论文
\qquad
基于改进的势场函数的移动机器人路径规划.王会丽等