SMARRT论文笔记

论文链接：SMARRT: Self-Repairing Motion-Reactive Anytime RRT for Dynamic Environments

论文主要贡献

SMARRT只在小范围内进行碰撞检测，如果现有的路径不满足条件（如会碰撞），会在最大程度保存已探索路径的同时，对现有路径进行剪枝，并且进行重新规划。

问题描述

定义机器人的操作空间为$\mathcal{X}$，定义${\mathcal{X}}_{obs}\in \mathcal{X}$为障碍空间，${\mathcal{X}}_{free}=\mathcal{X}/{\mathcal{X}}_{obs}$是空闲空间。在$t$时刻的机器人位置记为$x_{robot}(t)\in {\mathcal{X}}_{free}$。定义轨迹如下。

定义1：
定义初始位置为$x_{init}$和最终位置$x_{goal}$，函数$\sigma :[t_0,t_f]\to {\mathcal{X}}_{free}$可以被定义为轨迹，如果其满足下述条件：
1、$\sigma (t)\in {\mathcal{X}}_{free},t\in [t_0,t_f].$
2、$\sigma (t_0)=x_{init},\sigma (t_f)=x_{goal}$.
$\sigma$满足机器人的动力约束

在在线操作过程中，障碍物空间的改变和时间或者机器人的位置成函数，即：$\Delta {\mathcal{X}}_{obs}=f(t,x_{robot})$。然后，定义动态环境如下。

定义2：
动态环境定义为存在不确定障碍变化的空间，也就是说定义1中的$f$函数未知。这就意味着，动态环境可以包含随机出现、消失或者移动的物体。
在这篇文章中，我们只考虑随机移动的障碍物。然后，定义如下重规划问题。

定义3（问题描述）：
给定${\mathcal{X}}_{obs},{\mathcal{X}}_{free},x_{goal},x_{robot(t_0)}=x_{init}$，以及未知的$\Delta {\mathcal{X}}_{obs}=f(t,x_{robot})$，然后就可以找到可行的轨迹$\sigma :[t_{cur},t_f]\to {\mathcal{X}}_{free}$，其中，$x_{robot}(t_{cur})$是开始，当$x_{robot}(t_{cur})=x_{goal}$，在计算轨迹$\sigma$并且$\Delta {\mathcal{X}}_{obs}\ne \varnothing$的时候，持续更新$x_{robot}(t_{cur})$。

算法流程

关键思路和总体流程
1）只在机器人的附近区域进行可行性检测
2）如果检测存在障碍物，快速进行重规划
提出了一种multi-resolution utility map来进行规划和重规划。首先，只根据静态障碍物，通过RRT算法生成初始路径（起点到终点），然后沿着初始路径前进，前进过程中进行动态障碍物的检测，如果机器人的行经路线近期可能会被阻挡，则对路径进行修剪，这会产生一些列的断点树，通过对这些断点树的修复来重新生成新的路径。多分辨率地图的好处在于可以高效的找到包含断点树数目多的地图区域，这些区域更有可能被修复从而产生新的路径。然后根据待选新路径的长度，来选择最优长度的路径。

multi-resolution utility map的初始化

定义工作空间为$\mathcal{A}\subset {\mathbb{R}}^2$，下面对应对这个二维空间的一个划分，实际上就是用一些不相交的集合$\mathcal{T}={\tau }_{\gamma }\subset {\mathbb{R}}^2:\gamma =1,...|\mathcal{T}|$来覆盖整个$\mathcal{A}$
1）$I({\tau }_{\gamma })\cap I({\tau }_{{\gamma }^{\prime }})=\varnothing ,\forall \gamma ,{\gamma }^{\prime }\in 1,...|\mathcal{T}|,\gamma \ne {\gamma }^{\prime }$
2）${\cup }_{\gamma =1}^{|\mathcal{T}|}{\tau }_{\gamma }=\mathcal{A}$
这里，$I({\tau }_{\gamma })表示$${\tau }_{\gamma }的内部$

对搜索区域$\mathcal{A}$进行递归分解，生成一个层次的多尺度拼接(MST)结构，如下图所示。MST的主要目的是在不同尺度上增量式地使用环境地图知识。

MST提供了两个基本的优势：
1）减少计算量
2）快速重规划
MST的最精细层单元尺寸由用户自行决定，如上图(b)所示。不是一般性的，单元的个数一般定义为偶数，沿着$x$和$y$轴的单元数目分别定义为$N_x$和$N_y$。将$\mathcal{A}$划分为4个大小一样的象限，每个象限额维度是$\frac{N_x}{2}\times \frac{N_y}{2}$，如图(d)所示。图©使通过将图(d)中的每一块进一步等分维度4小块得到的。然后每一层可以表示为 T l = { τ γ l : γ l = 1 , . . . ∣ T l ∣ , ∀ l ∈ 0 , . . . L } \mathcal{T}^{\mathcal{l}}=\{\tau_{{\gamma}^{\mathcal{l}}}: \gamma^{\mathcal{l}}=1,...|\mathcal{T}^{\mathcal{l}}|, \forall \mathcal{l} \in {0, ...L}\} Tl={τγl​:γl=1,...∣Tl∣,∀l∈0,...L}（MST一共有$L$层）。
在MST被构建出来之后，通过RRT来建立搜索树。当机器人状态被生成并且添加到树中，他们同时会被其所在的${\mathcal{T}}^0$层的单元索引（即用该单元索引来标记机器人现在的位置），若机器人在${\tau }_{{\gamma }^0}$的单元中，该索引记为 P γ = { p j ∣ p j ∈ τ γ 0 } P_{\gamma}=\{p_j|p_j\in \tau_{\gamma^0}\} Pγ​={pj​∣pj​∈τγ0​}，这里$p_j\in {\mathbb{R}}^2$代表了机器人状态在搜索树中的位置。

高效地可行性检测和基于危险的树剪枝

当初始的路径建立后，如下图a所示

机器人开始沿着这个路径前进，并且检测是否存在可能碰撞动态障碍物。为了实现实时避障，机器人需要高效地识别这些危险。碰撞检测是这种基于采样的运动规划算法的计算瓶颈所在。而且，在动态场景下，现有的障碍物信息随着障碍物的移动会很快失效。

基于这些原因，本文提出了feasibility checking horizon来进行高效地碰撞检测和基于危险的树剪枝。如下图所示：

首先，feasibility checking horizon的范围以基于机器人的运动速度、平均的重规划时间以及机器人将走过路径的长度。碰撞检测只在feasibility checking horizon的范围内进行，原因有两点：
1）以及提供了足够的时间给机器人进行安全地重规划
2）当障碍物离机器人较远时，进行碰撞是不必要的，否则会造成不必要的计算资源浪费。

移动障碍物的collision zone（碰撞域）的定义和feasibility checking horizon类似，但是它是基于障碍物以及障碍物的速度定义的。在feasibility checking horizon和collision zone的交集中的节点都被任务是高危节点，这些节点会被从现有的树中剪去，这样就形成了断点树。这种方法使得SMARR可以在最大程度上保留已经探索过的路径（树），从而快速修复搜索树。当这些高危节点在机器人即将去往的路径上时，机器人和障碍物的碰撞风险很高，并且迫切需要重规划。

假设$\sigma (t)$是机器人路径$t$时刻的点。记$t_u$是期望的重规划时间。由于机器人需要同时重规划和避障，我们将$t_h=2t_u$定义为可行性检查的时间范围，因为这提供了最小的避障时间。记$p_c=\sigma (t_c),p_f=\sigma (t_c+t_h)$分别是$t_c$时刻以及$t_c+t_h$时间段的的机器人位置。定义 O = { o 1 , o 2 , . . . , o ∣ O ∣ } \mathcal{O}=\{o_1,o_2,...,o_{|\mathcal{O}|}\} O={o1​,o2​,...,o∣O∣​}为障碍物集合，障碍物在$t_c$时刻的的速度为$v_i(t_c)$。然后collision zone的集合 R c = { R 1 c , R 1 c , . . . , R O ∣ c ∣ } \mathcal{R}^c=\{\mathcal{R}^c_1,\mathcal{R}^c_1,...,\mathcal{R}^{|c|}_{\mathcal{O}}\} Rc={R1c​,R1c​,...,RO∣c∣​}就可以构建起来了，collision zone中的每一个区域以该障碍物为圆心，半径$r_i^c=2t_u\times v_i(t_c)$。

如果由$p_c$和$p_f$构成的线段和${\mathcal{R}}^c$有重合，则需要重规划。多以，feasibility checking horizon ${\mathcal{R}}^f$以当前的机器人为圆心，半径为$r_h=d(p_c,p_f)$，这里，$r_h=d(p_c,p_f)$表示$p_c$和$p_f$之间的欧氏距离。

在动态物体附近的实时重规划

因为去除了无效的节点，原本的搜索树可能被分解为多个无效的断点树。SMARRT的核心特征就是可以找到包含了多个断点树的单元，从而快速连接断点，修复树，从而实时重规划。为了达到这个目的，首先需要识别出断点树。然后，根据utility map计算每个单元中的utility，进而选择合适的采样点修复断点树，使机器人完后后续的导航

断点树的识别

在剪断危险节点后，使用泛洪填充算法来识别断点树的集合。具体来说，从某个随机节点开始，算法递归地访问每一个属于同一棵树的节点。上述的过程重复进行，只到每一个节点都被访问过了，从而构建了一个树的集合 G = { G 1 , . . . , G ∣ G ∣ } \mathcal{G}=\{ \mathcal{G}^1,...,\mathcal{G}^{|\mathcal{G}|}\} G={G1,...,G∣G∣}。当每一个节点都被访问过了，它所属的对应单元就会记录下它所属的树，以便于通过multi-resolution utility map来进行快速索引。具体说来，由单元${\tau }_{{\gamma }^0}$索引的树集合定义为 G γ 0 = { G g ∣ ∃ p j ∈ P γ 0 s . t . p j ∈ G g , g = 1 , . . . , ∣ G ∣ } \mathcal{G}_{\gamma^0}=\{\mathcal{G}^g | \exist p_j \in P_{\gamma^{0}} s.t. p_j \in \mathcal{G}^g, g=1,...,|\mathcal{G}|\} Gγ0​={Gg∣∃pj​∈Pγ0​s.t.pj​∈Gg,g=1,...,∣G∣}（这里的$p_j$不是指机器人的位置，而是指这个单元的任意点）。Fig. 2(a)就是两个断点树的例子。

单元的utility的计算

在多分辨率地图中，每个单元格的utility自底向上计算。从$l=0$开始计算，utility地图${\mathcal{U}}^0$使用两步构建。对每一个单元${\tau }_{\gamma }^0$和它对应的断点树${\mathcal{G}}_{{\gamma }^0}$，从字母表集 S = { I , V } S=\{I,V\} S={I,V}中分配一个符号状态，其中$I$代表无效，$V$代表有效。具体说来，如果这个单元包含了来自不同断点树的节点或者它的邻居包含了来自不同断点树的节点，则该单元被记为$V$，因为在这些单元里产生采样点会同时修复搜索树。否则，该节点被标记为$I$。然后，${\tau }_{\gamma }^0$的utility可以用下式计算：
$$U({\tau }_{{\gamma }^0})=\{\begin{array}{ll}0& \text{if}s({\tau }_{{\gamma }^0})=I\\ \frac{1}{d(p_c,p_{{\gamma }^0})+d(p_{{\gamma }^0},p_g)}& \text{if}s({\tau }_{{\gamma }^0})=V\end{array}$$
这里，$p_c$表示机器人的当前位置，$p_g$表示机器人的目标位置。$p_{{\gamma }^0}$是单元${\tau }_{{\gamma }^0}$的中心位置，$s({\tau }_{{\gamma }^0})$为符号状态指示函数，$d(\cdot ,\cdot )$是两点之间的欧氏距离。如果0层没有有效的单元，这个过程在上面的层中进行，只到某一层有一个有效的单元。剩下的更高层的utility地图通过其包含的低层单元的utility的最大值构建，也就是说：
$$U({\tau }_{{\gamma }^l})=\underset{{\tau }_{{\gamma }^{l-1}}\in {\tau }_{{\gamma }^l}}{\max }U({\tau }_{{\gamma }^{l-1}})$$

智能采样的树修复

multi-resolution utility map然后被用来找到最好的单元，该单元可以快速地修复树，并且可以指导机器人前往目标的同时避开障碍物。具体说来，这种方法首先在第0层（包含了最小单元）读取机器人梭子单元及其邻域单元的utility，并且选择utility最高的单元。如果附近没有有效的单元，这种方法切换到更高一层来扩大搜索半径，只到有效单元被找到。被选中的单元的尺寸应该尽可能的小，也就是层数尽可能的低，并且utility的分数最高。最后，最后在这个单元内生成一个随机样本，来链接到其邻域单元内所有的断点树。

流程图

实验结果