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

论文为：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)=\alpha /r^2$$
其中r为到最近的障碍物节点的距离。α>0为风险因素。
最后，传统的A*的代价函数更改为：
$$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)\ge \epsilon$
   则将M加入Keypoint中。然后令O=M，跳转②。如果 $R(i)⩽\epsilon$，则沿着P向后移动几个节点，跳转③。
5. 把goal添加到KP中去。

计算ON之间的节点时，采取如下：
$$dx=X_n-X_o$$ $$dy=Y_n-Y_o$$
当$|dx|\ge |dy|$ 时 s = |dx|-1
$$\Delta y=|\frac{dy}{dx}|$$

⌊·⌋ is the integral function, i = 1,2,…,s.

当$|dx|<|dy|$ 时 s = |dy|-1
$$\Delta x=|\frac{dx}{dy}|$$

上述公式示意图如下：

这一步得到的关键点，刚开始不明白为什么R(i）> ε才是关键点，这不距离障碍更近吗。后来想明白，正是因为距离障碍近才是关键点。

Motion Planning for Obstacle Avoidance

机器人规划周期为T。用LiDAR为传感器，其测量最大为$d_{max}$,视野范围为$[{\Phi }_{min},{\Phi }_{max}]$,其分辨率为${\Delta }_{\Phi }$, 机器人朝向为${\theta }_R$,记为${\Phi }_{rob}$.这里的$\Phi$ 是与机器人垂直方向的角度，而$\theta$是机器人在实际坐标下的角度。LiDAR每次扫描后得到 { d 1 , d 2 , … … d n } \{d_1, d_2, …… d_n\} {d1​,d2​,……dn​},对应的角度为${\Phi }_i$.如图

The Travelable Direction in Planning Window

在LiDAR范围内设置一个虚拟的运动规划窗口，其大小为$r_{win}$,在窗口中， l i = { Φ i , d w i } l_i = \{ \Phi_i, d_{w_i} \} li​={Φi​,dwi​​},.其中 $d_{w_i}$表示如下：
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​={rwin​di​​​di​≥rwin​di​<rwin​​
由图6，也就表示小于$r_{win}$的在这个方向有障碍。
由于障碍的存在，所以需要对$d_{wi}$进行缩放。规则如下：
对任意的$d_{w_i}$，若满足$$\{\begin{array}{c}{d}_{w_j}<r_{win}\\ d_{w_{j+1}}=r_{win}\end{array}$$
则在 $[{\Phi }_j,{\Phi }_j+{\theta }_1]$中均为$d_{w_j}$。其中${\theta }_1=arctan(\frac{r_{rob}}{d_{w_j}})$ 。同理，在j-1的位置，在$[{\Phi }_j-{\theta }_2,{\Phi }_j]$中,也均为$d_{w_j}$.${\theta }_2$计算相同。

The Moving Direction for the Next Moment

所有的可行区域记为：

记robot的位置为$(x_R,y_R,{\theta }_R)$下一个子目标点$G_t$为$(x_{Gt},y_{Gt})$. 其相差角度为${\theta }_{GR}={\theta }_{Gt}-{\theta }_R$ 其中${\theta }_{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​−xR​yGt​−yR​​)θGt​=arctan(xGt​−xR​yGt​−yR​​)+πθGt​=arctan(xGt​−xR​yGt​−yR​​)−π​xGt​≥xR​xGt​<xR​xGt​<xR​​yGt​≥yR​yGt​<yR​​
下一个子目标点$G_t$ 的 l t = { Φ t , d w t } l_t = \{\Phi_t, d_{w_t}\} lt​={Φt​,dwt​​}。其中${\Phi }_t\approx {\Phi }_{rob}+{\theta }_{GR}$的。则向这个子目标点移动需要的最小的角度为：

为了满足路径的安全性，需要距离障碍最远，所以需要满足如下：

其中${\Phi }_{lef}^{\ast }$为左侧最后一个$d_{W_{Gt}}=r_{win}$。再减少一个就会小于$r_{win}$，会遇到障碍。${\Phi }_{rig}^{\ast }$同理。
为了满足移动的平滑性，则要求和机器人当前角度差越小越好。如下：

同时考虑 12 13 14 三个条件。将最后的角度调整为如下：

如果没有障碍，或者是到目标点不会遇到障碍，就直接朝向走就行。

The Expected Velocity for the Next Moment

控制机器人的角速度$v$以及线速度$\omega$表示如下：

${\Phi }^{\ast }$即为当前朝向，$k_v$, $k_{\omega }$是一个正参数。同时，如果$cos({\Phi }^{\ast }-{\Phi }_{rob})<0$,则令其为0。同时为了防止超调，$v⩽\frac{|R{G}_t|}{T}$, $\omega ⩽\frac{{\Phi }^{\ast }}{T}$。

Adaptive Adjustment of the Planning Window

当$|\psi {|}_t$j较大时。增大窗口半径，去寻找更好的前进方向，当较小时，减小半径，有利于在狭窄空间中搜索。$r_{win}$更新规则如下：

然后$\Delta r_{win}$和$\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$
2. 当机器人当前位置到子目标点$G_t(K{P}_i)$的距离小于阈值$|R{G}_t{|}_{min}$.则更换子目标点为下一个点 $K{P}_{i+1}$
3. 当两个关键点之间的普通节点也在窗口内时，选择下一个关键点，即使还没到阈值，也进行更换。如图9a。
4. 当有多个关键点在窗口中时，选择最后的关键点。如图9b
   
   局部路径规划和全局路径规划结合后，并且结合关键点提取。整个流程如下：
   
   文章后边就是进行仿真以及实际实验了/具体可以看文章。