Hybrid Path Planning Based on Safe A* Algorithm and Adaptive Window Approach for Mobile Robot in Large-Scale Dynamic Environment
论文为:Hybrid Path Planning Based on Safe A* Algorithm and Adaptive Window Approach for Mobile Robot in Large-Scale Dynamic Environment
文章提出了一种基于 safe A*全局路径规划算法以及自适应窗口局部路径规划算法的混合算法。
safe A*
文章在A* 的代价函数中加入了一个风险值R(n)。
R
(
n
)
=
α
/
r
2
R(n) = α/r^2
R(n)=α/r2
其中r为到最近的障碍物节点的距离。α>0为风险因素。
最后,传统的A*的代价函数更改为:
F
(
n
)
=
G
(
n
)
+
H
(
n
)
+
R
(
n
)
F(n) = G(n) + H(n) + R(n)
F(n)=G(n)+H(n)+R(n)
这是将传统的map转换为加入了风险值的map。
然后根据新的F(n)得到的全局路径对比如下:
Keypoint
由 safe A*得到的全局路径点为:
P
a
t
h
=
{
S
,
P
1
,
P
2
,
P
3
.
.
.
P
w
,
G
}
Path = \{ S, P_1,P_2, P_3 ... P_w, G\}
Path={S,P1,P2,P3...Pw,G}
然后得到关键点后,
K
P
a
t
h
=
{
S
,
K
P
1
,
K
P
2
.
.
.
K
P
m
,
G
}
KPath = \{ S, KP_1, KP_2 ... KP_m, G\}
KPath={S,KP1,KP2...KPm,G}
在P中提取KP的规则如下:
- 起点为第一个Keypoint,记为O.
- O的下一个节点为M,判断是否为goal,若是跳转⑤
- M的下一个节点为N,判断是否为goal,若是跳转⑤
- 计算ON之间任一节点的R(i),如果
R
(
i
)
≥
ε
R(i)\geqε
R(i)≥ε
则将M加入Keypoint中。然后令O=M,跳转②。如果 R ( i ) ⩽ ε R(i)\leqslant ε R(i)⩽ε,则沿着P向后移动几个节点,跳转③。 - 把goal添加到KP中去。
计算ON之间的节点时,采取如下:
d
x
=
X
n
−
X
o
dx = X_n - X_o
dx=Xn−Xo
d
y
=
Y
n
−
Y
o
dy = Y_n -Y_o
dy=Yn−Yo
当
∣
d
x
∣
≥
∣
d
y
∣
|dx| \geq |dy|
∣dx∣≥∣dy∣ 时 s = |dx|-1
Δ
y
=
∣
d
y
d
x
∣
\Delta y = |\frac{dy}{dx}|
Δy=∣dxdy∣
⌊·⌋ is the integral function, i = 1,2,…,s.
当
∣
d
x
∣
<
∣
d
y
∣
|dx| <|dy|
∣dx∣<∣dy∣ 时 s = |dy|-1
Δ
x
=
∣
d
x
d
y
∣
\Delta x = |\frac{dx}{dy}|
Δx=∣dydx∣
上述公式示意图如下:
这一步得到的关键点,刚开始不明白为什么R(i)> ε才是关键点,这不距离障碍更近吗。后来想明白,正是因为距离障碍近才是关键点。
Motion Planning for Obstacle Avoidance
机器人规划周期为T。用LiDAR为传感器,其测量最大为 d m a x d_{max} dmax,视野范围为 [ Φ m i n , Φ m a x ] [\Phi_{min},\Phi_ {max}] [Φmin,Φmax],其分辨率为 Δ Φ \Delta_{\Phi} ΔΦ, 机器人朝向为 θ R \theta_R θR,记为 Φ r o b \Phi_{rob} Φrob.这里的 Φ \Phi Φ 是与机器人垂直方向的角度,而 θ \theta θ是机器人在实际坐标下的角度。LiDAR每次扫描后得到 { d 1 , d 2 , … … d n } \{d_1, d_2, …… d_n\} {d1,d2,……dn},对应的角度为 Φ i \Phi_i Φi.如图
The Travelable Direction in Planning Window
在LiDAR范围内设置一个虚拟的运动规划窗口,其大小为
r
w
i
n
r_{win}
rwin,在窗口中,
l
i
=
{
Φ
i
,
d
w
i
}
l_i = \{ \Phi_i, d_{w_i} \}
li={Φi,dwi},.其中
d
w
i
d_{w_i}
dwi表示如下:
d
w
i
=
{
r
w
i
n
d
i
≥
r
w
i
n
d
i
d
i
<
r
w
i
n
d_{wi} = \left\{\begin{matrix} r_{win} && d_i \geq r_{win}\\ d_i && d_i< r_{win} \end{matrix}\right.
dwi={rwindidi≥rwindi<rwin
由图6,也就表示小于
r
w
i
n
r_{win}
rwin的在这个方向有障碍。
由于障碍的存在,所以需要对
d
w
i
d_{wi}
dwi进行缩放。规则如下:
对任意的
d
w
i
d_{w_i}
dwi,若满足
{
d
w
j
<
r
w
i
n
d
w
j
+
1
=
r
w
i
n
\left\{\begin{matrix} d_{w_j}< r_{win}\\ d_{w_{j+1}}= r_{win} \end{matrix}\right.
{dwj<rwindwj+1=rwin
则在
[
Φ
j
,
Φ
j
+
θ
1
]
[\Phi_j, \Phi_j + \theta_1]
[Φj,Φj+θ1]中均为
d
w
j
d_{w_j}
dwj。其中
θ
1
=
a
r
c
t
a
n
(
r
r
o
b
d
w
j
)
\theta_1=arctan(\frac{r_{rob}}{d_{w_j}})
θ1=arctan(dwjrrob) 。同理,在j-1的位置,在
[
Φ
j
−
θ
2
,
Φ
j
]
[\Phi_{j}- \theta_2, \Phi_j ]
[Φj−θ2,Φj]中,也均为
d
w
j
d_{w_j}
dwj.
θ
2
\theta_2
θ2计算相同。
The Moving Direction for the Next Moment
所有的可行区域记为:
记robot的位置为
(
x
R
,
y
R
,
θ
R
)
(x_R, y_R,\theta_R)
(xR,yR,θR)下一个子目标点
G
t
G_t
Gt为
(
x
G
t
,
y
G
t
)
(x_{Gt},y_{Gt})
(xGt,yGt). 其相差角度为
θ
G
R
=
θ
G
t
−
θ
R
\theta_{GR} = \theta_{Gt} - \theta_R
θGR=θGt−θR 其中
θ
G
t
\theta_{Gt}
θGt 的计算如下:
{
θ
G
t
=
a
r
c
t
a
n
(
y
G
t
−
y
R
x
G
t
−
x
R
)
x
G
t
≥
x
R
θ
G
t
=
a
r
c
t
a
n
(
y
G
t
−
y
R
x
G
t
−
x
R
)
+
π
x
G
t
<
x
R
y
G
t
≥
y
R
θ
G
t
=
a
r
c
t
a
n
(
y
G
t
−
y
R
x
G
t
−
x
R
)
−
π
x
G
t
<
x
R
y
G
t
<
y
R
\left\{\begin{matrix} \theta_{Gt} = arctan(\frac{y_{Gt}-y_R}{x_{Gt}-x_R}) & x_{Gt}\geq x_R & \\ \theta_{Gt} = arctan(\frac{y_{Gt}-y_R}{x_{Gt}-x_R}) + \pi & x_{Gt}<x_R & y_{Gt} \geq y_R\\ \theta_{Gt} = arctan(\frac{y_{Gt}-y_R}{x_{Gt}-x_R}) - \pi & x_{Gt}<x_R & y_{Gt} < y_R \end{matrix}\right.
⎩⎨⎧θGt=arctan(xGt−xRyGt−yR)θGt=arctan(xGt−xRyGt−yR)+πθGt=arctan(xGt−xRyGt−yR)−πxGt≥xRxGt<xRxGt<xRyGt≥yRyGt<yR
下一个子目标点
G
t
G_t
Gt 的
l
t
=
{
Φ
t
,
d
w
t
}
l_t = \{\Phi_t, d_{w_t}\}
lt={Φt,dwt}。其中
Φ
t
≈
Φ
r
o
b
+
θ
G
R
\Phi_t \approx \Phi_{rob}+\theta_{GR}
Φt≈Φrob+θGR的。则向这个子目标点移动需要的最小的角度为:
为了满足路径的安全性,需要距离障碍最远,所以需要满足如下:
其中
Φ
l
e
f
∗
\Phi_{lef}^{*}
Φlef∗为左侧最后一个
d
W
G
t
=
r
w
i
n
d_{W_{Gt}} = r_{win}
dWGt=rwin。再减少一个就会小于
r
w
i
n
r_{win}
rwin,会遇到障碍。
Φ
r
i
g
∗
\Phi_{rig}^{*}
Φrig∗同理。
为了满足移动的平滑性,则要求和机器人当前角度差越小越好。如下:
同时考虑 12 13 14 三个条件。将最后的角度调整为如下:
如果没有障碍,或者是到目标点不会遇到障碍,就直接朝向走就行。
The Expected Velocity for the Next Moment
控制机器人的角速度
v
v
v以及线速度
ω
\omega
ω表示如下:
Φ
∗
\Phi^*
Φ∗即为当前朝向,
k
v
k_v
kv,
k
ω
k_{\omega}
kω是一个正参数。同时,如果
c
o
s
(
Φ
∗
−
Φ
r
o
b
)
<
0
cos(\Phi^*-\Phi_{rob})<0
cos(Φ∗−Φrob)<0,则令其为0。同时为了防止超调,
v
⩽
∣
R
G
t
∣
T
v\leqslant\frac{|RG_t|}{T}
v⩽T∣RGt∣,
ω
⩽
Φ
∗
T
\omega \leqslant \frac{\Phi^*}{T}
ω⩽TΦ∗。
Adaptive Adjustment of the Planning Window
当
∣
ψ
∣
t
|\psi|_t
∣ψ∣tj较大时。增大窗口半径,去寻找更好的前进方向,当较小时,减小半径,有利于在狭窄空间中搜索。
r
w
i
n
r_{win}
rwin更新规则如下:
然后
Δ
r
w
i
n
\Delta r_{win}
Δrwin和
ψ
\psi
ψ的关系如下:
Path Following Based on Key Path Points
在移动过程中如何选择下一个子目标点。关键点为: K P = { S , K P 1 , K P 2 , . . . K P m , G } KP = \{S,KP_1, KP_2, ... KP_m ,G \} KP={S,KP1,KP2,...KPm,G}.也就是每个子目标点。其中转换目标点的方法如下:
- 从S开始,则子目标点 G t = K P 1 G_t = KP_1 Gt=KP1
- 当机器人当前位置到子目标点 G t ( K P i ) G_t(KP_i) Gt(KPi)的距离小于阈值 ∣ R G t ∣ m i n |RG_t|_{min} ∣RGt∣min.则更换子目标点为下一个点 K P i + 1 KP_{i+1} KPi+1
- 当两个关键点之间的普通节点也在窗口内时,选择下一个关键点,即使还没到阈值,也进行更换。如图9a。
- 当有多个关键点在窗口中时,选择最后的关键点。如图9b
局部路径规划和全局路径规划结合后,并且结合关键点提取。整个流程如下:
文章后边就是进行仿真以及实际实验了/具体可以看文章。