标题：Fast Likelihood-based Collision Avoidance with Extension to Human-guided Navigation
作者：Ji Zhang | Chen Hu | Rushat Gupta Chadha | Sanjiv Singh
来源：Journal of Field Robotics. 2020.
代码：https://github.com/jizhang-cmu/ground_based_autonomy_slimmed
一、简介
我们提出一种计划方法，以在混乱复杂的环境中实现快速自主飞行。通常，在复杂环境中进行自主导航需要对由k连接的网格或概率方案生成的图进行连续搜索。当车辆行驶时，用来自车载传感器的数据更新图是昂贵的，尤其是在路径必须是运动学上可行的情况下，在图上的搜索也是如此。我们建议避免在线搜索以减少计算复杂度。我们的方法在两个单独的区域中对环境进行了不同的建模。障碍被认为在传感器范围内是确定性已知的，而在传感器范围之外是概率已知的。该方法不是搜索成本最低的路径（通常是最短的路径），而是最大化确定导航下一步的目标的可能性。有了这样的问题表述，轨迹库实现的在线方法可以使用调制解调器嵌入式计算机上的单个中央处理单元线程在0.2-0.3毫秒内确定路径。该方法支持使用和不使用先验地图的两种配置。两种配置均可用于规划目标。此外，后者可以允许人类通过定向输入进行导航的导航。在实验中，它使轻型无人机能够在复杂的森林环境中以10 m / s的速度飞行。
二、问题描述
定义Q⊂R作为车辆的配置空间，A ⊂Q表示车辆的当前位置，B⊂Q表示目标点。车辆上配置了用于感知的传感器，S⊂Q定义为传感器感知范围内的空间，对障碍物进行建模以使其S中确定可知，而在Q \ S中概率已知。考虑当车辆从A开始移动时，车辆在起步有多个方向可供选择。为方便起见，让我们在此步骤中将车辆状态命名为起始状态，表示为 Xs，很显然Xs的不同选择会导致不同的路线。在本文中约定，将PB(.)定义为车辆从给定状态成功到达B的概率。 与初始状态相关的概率为PB(Xs)。 我们的规划问题可以定义如下：
问题1 给定A、B∈ Q，S ⊂ Q及在Q中的障碍物，确定初始状态来最大化概率PB(Xs),
当车辆沿着路径行驶时，上述问题在每一步都得到解决，也就是说，车辆在导航过程中的每个瞬时时间都达到到达B的概率最大化。
三、方法
1.概率模型
所提出的方法使车辆成功从A行驶到B的可能性最大化。如问题定义中所述，传感器范围S内的障碍物被确定认为是已知的，因为从感知传感器获取了信息。如果现有地图可用，则S以外的障碍物被认为是概率已知的。否则，该情况等同于没有先验障碍。
图2将S表示为灰色区域。 将F ⊂ S为传感器边界（红色实线表示）。 给定初始状态xs,，A和B点连接的路径为黑色曲线。对于所有可能的路径，它们必定与F相交。有人认为车辆可以横向移动并且不与F相交。在这种情况下，由于问题的性质，必须根据车辆的运动学模型来扩展S，以使车辆不会横穿由S覆盖的区域。 定义车辆通过F时的状态为xf。 给定为Xx和P (Xf ∣Xs )的条件分布可以从感知传感器提供的障碍信息中得出。此外，可以从先验地图上的障碍物获得车辆从Xf,到达B的概率密度pB(Xf )。 我们有：
在此，将（1）中的符号PB(Xs)x改写为pB(Xs)来表示概率密度。 考虑n∈Z+个样本ξ，i = 1，2，…,n ，从p(Xf | Xs)开始绘制。 根据蒙特卡洛抽样理论Robert，2004年），我们可以建立
合并公式（2）和（3）以及假定n是一个常数可得：
2.局部概率
给定了初始状态Xs，车辆可以沿着不同的路径到达传感器边界F。 在这里，让我们将路径组命名为共享相同xs的一组路径。 考虑xs的离散模型。 图3给出了路径组的示例。 在顶行中，自上而下的视图中存在7个路径组，其中xs在向左或向右弯曲的路径的开始处。 中间的路径组对应于直线向前运动。 在底行中，以侧视图显示了5个路径组，其中路径向上或向下弯曲。考虑到水平和垂直方向，这个例子中总共有7 x 5 = 35 个路径组合，所有的路径止于F。
每条路径均由三次样条曲线来生成。组中的路径在水平和垂直方向上沿多个方向进行拆分。 在图3中，路径首先在35个方向（水平7个，垂直5个方向）上分割，每个路径又在35个方向上分割。 这导致一组中的35^2 = 1225条路径。 考虑35个路径组，总共有35×1225 = 42,875条路径。 图4一起显示了所有路径组，其中用颜色标记了组索引。 请注意，这些示例路径是根据车辆运动约束生成的，但是该方法不限于特定的运动模型，并且可以支持各种路径组配置。
组中的路径可视为从Xs到F的可行路径。 可以将路径末端的状态视为Xf的样本ξi, i =1、2、…n ，其中的Xf分布是从p（x f ∣ x s）得出的。在导航过程中，障碍物会被感知传感器遮挡某些路径。 图5给出了一个障碍物示例以及组中相应的无碰撞路径。 定义一个布尔函数c（ξi）表示路径间隙：
基于公式（4）可以计算出PB (Xs ):
将公式（6）应用于所有的路径组，并选择最高PB (Xs)的X*s来执行控制车辆。
3.全局概率
我们的环境是以体素 voxel来表示。 与传统体素表示不同，我们的体素同时包含位置和方向信息。 如图6(a)所示，基于恒定的角度间隔将体素分为多个方向，以δ表示。 角度间隔设置了车辆穿过体素的方向。 将xkj，j，k∈Z 作为体素的状态，其中j是体素指数，k是方向指数。 与xkj相关的位置被建模在体素内，并且将该方向建模为围绕xkj方向均匀分布在[− /2δ/2]内。 从xkj到达B的概率密度表示为pB(xkj)。
概率可在相邻体素之间传输。如图6(b)所示，考虑概率为xkj从相邻的体素传输到xjk，在左下角表示为jl，在右上角表示为jr。 令θk为与xjk相关的方向。 由于位置被建模为在体素内均匀分布，因此要传输到xjk的概率来自体素jl和jr中的灰色区域，其区域为体素的1 − tan 2θk和tan θk/2区域。灰色区域是通过在jl和jr中画一条平行于xjk方向的直线来确定的，该方向连接了体素j的左下和右上顶点。 从每个灰色区域，从三个相邻方向传输概率。 概率密度传输定义为：
在（7）中，rj表示由于障碍而引起的体素j的可穿越性，其中rj = 1表示完全清除，rj = 0表示完全闭塞。 wf和wy分别确定对应于向后运动和偏航调整的概率分布。 我们要求
三维（3D）情况是对2D情况的直接扩展，其中每个体素都具有图6（a）中表示的多层。 每个层都与一个俯仰角相关联。 令αl为层l l的间距，∈Z。我们将3D情况的体素状态修改为xl,kj， 从三个相邻的体素传输概率密度pB（xl,kj），两个体素与2D情况相同，第三个体素在体素j的正上方或正下方。 如果α1> 0，则可以过渡到体素j的体素在底部，如果α1<0，则体素在顶部。 以α1> 0为例。 让我们将体素j下面的体素表示为体素jb。 该区域可以通过遵循xl,kj的方向过渡到体素j的体素jb中，体素的面积∣ ∣（+）∕ tan sin cos 2αθθlk k。 相应地，体素jl和jr中可以过渡到体素j的区域具有(1-|tan a1 |(sin θk + cos θk)/2)(1-tan θk/2)和(1-|tan a1 |(sin θk + cos θk)/2)tan θk/2。 概率密度计算为:
其中：
wjl k表示障碍物的xjl k的可遍历性。Wp表示概率传输pB(xl-1,k j*)和pB(xl+1,k j*)到pB(xl,k)的转弯权重。
同样，我们要求在传输过程中不损失任何概率：
如果α1<0，则将（9）中的体素jb替换为体素j上方的体素，即体素ja。存在两种特殊情况。 首先，如果α1= 0，则车辆水平移动。 在3D情况下，概率从体素jl和jr传输，但不从体素ja或jb传输到体素j。 其次，如果θk= 0，则车辆平行于体素jr移动。 在2D和3D情况下，概率都从体素jl传输，而没有从体素jr传输到体素j。 在初始化期间，概率密度均匀分布在包含B的体素中的所有方向上。概率的传播是通过迭代过程进行的。 图7给出了2D环境中传播的概率密度的示例。体素越亮表示概率密度越高。
4.方法实现
4.2节中描述的路径组是离线生成的。为了进行碰撞检测，我们使用体素网格覆盖传感器范围S。体素和路径之间的对应关系已预先建立，并存储在邻接表中。 在邻接表中，每一行都与一个体素相关联，并由路径的索引组成，这些路径的路径被放置在体素中心的障碍物所遮挡。 在这里，考虑以车辆的半径来计算遮挡。 系统启动后，路径和邻接表将加载到车辆计算机内存中。 在线碰撞检测处理所有感知传感器的数据，并根据邻接表标记阻塞的路径。 然后，该算法遍历每组中的所有路径以基于（6）计算PB(Xs)并选择最高PB(Xs)的路径组。 该算法返回x * s为：
n是路径组中的路径数。 设 h ∈ Z+为路径组数，m ∈Z+为感知传感器数据点数。 在线处理的算法在Algorithm 1中进行了描述。
定理1. 在线处理算法的计算复杂度为O（mnh）。
定理1分析最坏情况下的计算复杂度，在这种情况下，每个感知传感器数据点都会阻塞每个组中的所有路径。 实际上，数据点可以阻塞一些路径，因此计算量要轻得多。 全局范围内的概率传递使用覆盖环境的第二个体素网格，仅在导航之前运行一次。这使用类似于A *算法（Zeng＆Church，2009）的实现方式，其中仅更新与open set中的体素相邻的体素中的概率密度。 如果包含A的体素中的概率密度变化小于阈值，则此过程终止。
在先验图不可用的情况下，我们可以选择使用启发式函数来计算（6）中的pB(ξi)：
其中Δpi和Δyi分别是ξi和目标方向在俯仰和偏航角之间的相对角度。 等式（12）实质上将路径规划器偏向目标方向。 注意，相同的功能可用于处理例如来自操纵杆控制器的人为输入。 结果是，车辆在导航期间由驾驶员进行引导，而车辆自身进行避障。