Journal of Intelligent & Robotic Systems 论文阅读记录 —— Zhong XunYu 基于安全A*以及自适应窗口的动态环境路径规划算法


论文为: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*算法原理

文章在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的规则如下:

  1. 起点为第一个Keypoint,记为O.
  2. O的下一个节点为M,判断是否为goal,若是跳转⑤
  3. M的下一个节点为N,判断是否为goal,若是跳转⑤
  4. 计算ON之间任一节点的R(i),如果 R ( i ) ≥ ε R(i)\geqε R(i)ε
    则将M加入Keypoint中。然后令O=M,跳转②。如果 R ( i ) ⩽ ε R(i)\leqslant ε R(i)ε,则沿着P向后移动几个节点,跳转③。
  5. 把goal添加到KP中去。

计算ON之间的节点时,采取如下:
d x = X n − X o dx = X_n - X_o dx=XnXo d y = Y n − Y o dy = Y_n -Y_o dy=YnYo
∣ d x ∣ ≥ ∣ d y ∣ |dx| \geq |dy| dxdy 时 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={rwindidirwindi<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(xGtxRyGtyR)θGt=arctan(xGtxRyGtyR)+πθGt=arctan(xGtxRyGtyR)πxGtxRxGt<xRxGt<xRyGtyRyGt<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} vTRGt, ω ⩽ Φ ∗ 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}.也就是每个子目标点。其中转换目标点的方法如下:

  1. 从S开始,则子目标点 G t = K P 1 G_t = KP_1 Gt=KP1
  2. 当机器人当前位置到子目标点 G t ( K P i ) G_t(KP_i) Gt(KPi)的距离小于阈值 ∣ R G t ∣ m i n |RG_t|_{min} RGtmin.则更换子目标点为下一个点 K P i + 1 KP_{i+1} KPi+1
  3. 当两个关键点之间的普通节点也在窗口内时,选择下一个关键点,即使还没到阈值,也进行更换。如图9a。
  4. 当有多个关键点在窗口中时,选择最后的关键点。如图9b
    在这里插入图片描述
    局部路径规划和全局路径规划结合后,并且结合关键点提取。整个流程如下:
    在这里插入图片描述
    文章后边就是进行仿真以及实际实验了/具体可以看文章。
  • 0
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值