文章名称： 《Perception and Avoidance of Multiple Small Fast Moving Objects for Quadrotors with Only Low-cost RGBD Camera》
论文单位：香港大学 ArcLab
github链接: Fast-dodging

一、关键内容概述

文章主要步骤：
(1)用Yolo-fastest 在RGB里检测动态物体，用深度图获得三维信息，用3D-soft跟踪动态物体
(2)通过生成最优光滑轨迹，预测目标的轨迹，从而快速躲避动态、静态障碍物

特点：
使用低成本深度相机代替事件相机；
提出了分层轨迹生成算法：运动学路径搜索+基于梯度优化；
填补了多小快速运动目标感知与避障领域空白；

指标：
可以RGB-D相机躲避多个快速运动小物体指标： 半径<0.035m; 速度>5m/s; 距离<5m

二、问题背景–微小动态物体躲避的研究

①延迟边界与最大感知距离、物体检测时间有关
②规划算法要可计算，且在导航中可行
③算法大小要适合上机部署，且可以实时运行
快速物体避障主要被分解为【物体预测】与【运动规划】两个部分。预测部分挑战更大，规划部分趋向更安全、更优的方向发展。

其中，【物体预测】的两个重要因素：
① 知觉延迟（perception latency）：多数用事件相机，但运动相机使用于距离近的动态物体更有效，否则容易有位置估计误差。激光雷达对于小物体而言，其稀疏分辨率和信息更新速率不够。
② 检测范围

三、论文主要内容

3.1 快速移动小物体的检测：

有直接基于空间信息聚类的目标检测方法，但：鲁棒性不强；对于小物体不适用于与背景分开；距离较近时不容易分辨。
因此文章中快速移动小物体的检测使用YOLO检测目标物体。

模型描述：
设在时刻$t$时，有$N$个物体进入了相机视野内。获取检测信息后，将获得的中心像素坐标设为$(u_c,v_c)$，物体的像素尺度为$(s_b,r_b)$，其中$s_b$表示检测框区域，$r_b$表示检测框横纵比。

根据检测框的ROI对应到深度图，获得物体的深度信息，记为$d$。
进一步可以得到物体在世界系下的坐标：
$$M_1{M}_2[x,y,z,1{]}^T=d[u,v,1{]}^T$$
其中$M_{1} $是相机内参矩阵,$M_{2}$是世界系到相机系的旋转矩阵。

在$t$时刻的检测结果可以被定义为${\zeta }_t$：
ζ t = { ( p i , c i ) ∣ i ∈ ( 1 , N ) } \zeta_{t} =\left\{ (p_i ,c_i)|i \in(1,N)\right\} ζt​={(pi​,ci​)∣i∈(1,N)}
$p_i$是物体在三维空间中的位置，$c_i=[(s_b^i,r_b^i)$是像素尺度向量。

3.2 快速移动的小物体跟踪–3D-SOFT算法：

3D-SOFT算法用于使用低成本深度相机获得高速物体轨迹。

3.2.1 状态估计：

分别对物体的空间运动和像素尺度进行状态估计。将世界帧内各物体的帧间位移近似为线性恒加速度模型，像素尺度的变化近似为一阶微分模型。
每个目标的运动模型如下：
$$P_{i_{\kappa }}=[p_{i_{\kappa }}^T{\dot{p}}_{i_{\kappa }}^T{\ddot{p}}_{i_{\kappa }}^T{]}^T$$
$${P_i}_{\kappa +1}={\mathbf{A}}^{P_i}{P_i}_{\kappa }+v_{\kappa }^{P_i},z_{\kappa }^{P_i}=H^{P_i}{P_i}_{\kappa }+{w_{\kappa }}^{P_i}$$
$$H^{P_i}=[I_{3\times 3}{I}_{3\times 3}{0}_{3\times 3}],A^{P_i}=\left[\begin{array}{ccc}{I}_{3\times 3}& \Delta t\cdot I_{3\times 3}& 0.5\Delta t^2{I}_{3\times 3}\\ 0_{3\times 3}& I_{3\times 3}& \Delta t\cdot I_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& I_{3\times 3}\end{array}\right]$$

其中$\kappa$表示第$\kappa$个时间戳，$\Delta t$是相邻两帧的时间间隔，$v$是正态分布的过程噪声，$w$是正态分布的测量噪声。

像素尺度模型$C_{i_{\kappa }}=[c_{i_{\kappa }}^T,{\dot{c}}_{i_{\kappa }}^T{]}^T$与如上公式类似，但状态转移矩阵A与测量矩阵H不同：
$$A^{C_i}=\left[\begin{array}{cc}{I}_{3\times 3}& \Delta t\cdot I_{3\times 3}\\ 0_{3\times 3}& I_{3\times 3}\end{array}\right],H^{C_i}=[I_{3\times 3}{0}_{3\times 3}]$$
基于以上模型公式，可以使用卡尔曼滤波对模型进行预测和更新。（对应3D-SOFT原写法步骤10，3.2.4中8）.滤波器的后验概率输出为${\hat{P}}_i$与${\hat{C}}_i$（对应3D-SOFT原写法步骤6，3.2.4中5）

3.2.2 数据关联：

像素尺度只与数据关联有关。

在将检测到的对象分配给现有目标时，先将${\hat{P}}_i$转换到像素坐标系，进一步根据${\hat{C}}_i$，得到当前图片的预测框。

分配代价矩阵结合了位置和颜色特征：
位置代价矩阵计算为每个检测点与现有目标的所有预测边界框之间的IOU距离；
颜色代价矩阵通过 每个检测ROI的HSV通道直方图与所有的预测ROI 的巴氏系数（Bhattacharyya Coefficient）；

巴氏系数（Bhattacharyya Coefficient）：
是对两个统计样本的重叠量的近似计算。巴氏系数可用来对两组样本的相关性进行测量。
巴氏距离与巴氏系数-百度百科
HSV(Hue, Saturation, Value)：
是根据颜色的直观特性创建的一种颜色空间, 也称六角锥体模型
HSV颜色模型-百度百科

检测盒与跟踪盒重叠面积占总面积的比值表示空间相关性，检测盒ROI与预测盒ROI中HSV直方图的对比表示颜色相关性。

物体可能外观相似，但在极短时间内（lightning
situation）的状态也不同；HSV空间的颜色信息可以区别在不同位置的相似目标。

对于第$i$个预测框和第$h$个检测框，代价矩阵(cost matrix)${\mathcal{F}}_{ih}$为：
$$\begin{gathered} {\mathcal{F}}_{ih}=a\ast f_{iou}(i,h)+b\ast f_{hsv}(i,h),(a+b)=1 \\ {f}_{iou}(i,h)=\frac{{\Omega }_{ih}}{A_i{A}_h-{\Omega }_{ih}}, \\ {f}_{hsv}(i,h)=1-\sqrt{1-\frac{1}{\sqrt{{\bar{H}}_i{\bar{H}}_h{n}_{max}^2}}\sum _{n=1}^{n_{max}}\sqrt{H_i(n)H_h(n)}} \end{gathered}$$

其中$A_i$,$A_h$分别是预测框与检测框区域，${\Omega }_{ih}$是$A_i$,$A_h$的重叠区域，$H()$是HSV直方图函数，$\hat{H}$是$H()$的均值，$n$是H、S、V通道的值。

${\mathcal{F}}_{ih}\in (0,1)$表示匹配程度，值越大匹配越好。

利用匈牙利算法(Hungarian algorithm)对分配问题进行优化求解（对应3D-SOFT步骤1-3）。

匈牙利算法:
是一种在多项式时间内求解任务分配问题的组合优化算法
匈牙利算法详解-CSDN

当检测与目标之间的代价小于门限$h_{min}$时，将停止作业（对应3D-SOFT原写法步骤5），以及连续三次丢失匹配的跟踪器将被删除（对应3D-SOFT原写法11-13，3.2.4中9-10）.

3.2.3 轨迹预测：

基于滤波得到的三维位置信息$p_{t_0}$，在当前时间$t_0$下的速度$v_{t_0}$以及加速度$a_{t_0}$，可以将第$i$个动态物体在未来时间$t$下的轨迹$p_b(t)$表示为:
$$p_b^i(t)=p_{t_0}^i+v_{t_0}^i(t-t_0)+\frac{1}{2}{a}_{t_0}^i(t-t_0{)}^2$$

3.2.4 3D-SOFT完整步骤：

输入：检测指标 ζ t = { ( P i , C i ) ∣ i ∈ ( 1 , N ) } \zeta_{t} =\left\{ (P_i ,C_i)|i \in(1,N)\right\} ζt​={(Pi​,Ci​)∣i∈(1,N)}，跟踪指标${\mathcal{X}}_t=\left\{({\hat{P}}_h,{\hat{C}}_h)|i\in (1,B)\right\}$

STEP1： 计算代价矩阵${\mathcal{F}}_{B\times N}$（通过${\mathcal{F}}_{ih}=a\ast f_{iou}(i,h)+b\ast f_{hsv}(i,h)$）

STEP2： 通过匈牙利算法进行预测和检测;

STEP3： 更新匹配的配对集${\mathcal{M}}_p$,不匹配检测集$U_d$, 不匹配跟踪器$U_t$;

STEP4： 对配对集${\mathcal{M}}_p$中的配对执行循环：
STEP5： 如果$\mathcal{F}(pair)>h_{min}$，用2个卡尔曼滤波器更新跟踪器，获得$(\hat{P},\hat{C})$
STEP6： else: $U_d\leftarrow U_d\cup (P,C)$ in pair

STEP7： 对$U_d$中的$(P,C)$执行循环：
STEP8： ${\mathcal{X}}_t\leftarrow {\mathcal{X}}_t\cup (\hat{P},\hat{C})$

STEP9： 对$U_t$中的$(\hat{P},\hat{C})$执行循环：
STEP10： 如果 $Ag{e}_{(\hat{P},\hat{C})}>Ag{e}_{max}$，则 ${\mathcal{X}}_t\leftarrow (\hat{P},\hat{C})$ \ ${\mathcal{X}}_t$

3.3 轨迹规划

提出了 具有运动学和空间约束的 基于梯度优化的 轨迹规划方法。
规划器输入：安全走廊、动态物体轨迹、车辆初始状态和目标状态；
输出： 是一个动态可行和安全的轨迹。

3.3.1 分级规划

开始阶段，通过一条无碰撞路径连接起始点并搜索目标。对三维混合A*算法进行了改进，实现了在点云上高效、直接地搜索运动学可行路径。

为了减少延迟，使用最近的点云而不是体素地图来构建KD-tree；

在安全检查函数中，我们与建议路径段上的样本点进行最近邻查询。如果与邻居的距离大于预先分配的安全裕度，则查询的样本点是安全的。

在安全检查函数中，我们与建议路径段上的样本点进行最近邻查询。如果与邻居的距离大于预先分配的安全裕度，则查询的样本点是安全的。

然后，用一系列首尾相连的多面体或球体建立安全走廊；允许轨迹在廊道内变化，这比直接在路径引导下优化轨迹更加灵活，可以提高计算效率。

定义$SFC$为：
一系列$m-1$个凸多面体，通过$m$个waypoint生成。每个多面体${\mathcal{H}}_j(0<j<m)$定义为:
$$H_j=\left\{x\in {\mathbb{R}}^3|{(x-{\hat{p}}_j^k)}^T{\vec{n}}_j^k\le 0,k=1,2,...,N_j\right\}$$
其中$N_j$是第i个多面体面的数量，${\hat{p}}_j^k$,${\vec{n}}_j^k$分别是点和面上对应的法向量。最后，在SFC约束下对轨迹进行了优化。

3.3.2 轨迹优化

红色圈内的黑点表示不安全的样本点，经过优化后可以都转为安全的。优化改变了原始路径点(黄色点)的位置，同时进行了轨迹碎片的时间分配。每个路径点必须停留在两个相邻多面体的重叠部分内，且轨迹仅在安全走廊内变化。

可以使用一系列多项式来在每个维度定义轨迹，如：
$$p_j(t)=\sum _{j=1}^{m-1}{C}_j^T\beta (t-T_{j-1}),T_{j-1}\le t<T_j$$
其中$C_j\in {\mathbb{R}}^{6\times 3}$ 为第$j$片的系数矩阵；$\beta (t)=[1,t,...,t^5{]}^T$是时间向量。
多项式段连接点的位置、速度、加速度等状态是连续的，多项式的阶数为5，因此四旋翼飞行器的体角将根据微分平面性沿计划轨迹平滑变化。

将优化问题公式定义如下:

$\rho$是整个轨迹飞行时间的权重,$[s]$表示$s$的一组最高阶导数；选择$s=2$,所以多项式段间连接点的位置，速度和加速度都相等。${\bar{p}}_j\in {\mathbb{R}}^{(s+1)\times 3}$为时间段条件，${\bar{p}}_s,{\bar{p}}_f\in {\mathbb{R}}^{(s+1)\times 3}$为起终点状态；$S_e,S_v,S_a,S_c,S_d$分别为能量代价、最大速度代价、最大加速度代价、与安全走廊的碰撞代价、与动态物体的碰撞代价；${\lambda }_v,{\lambda }_a,{\lambda }_c,{\lambda }_d$为相应的权重。$C$为总系数矩阵，表示$(C_1^T,C_2^T,...,C_{m-1}^T{)}^T\in {\mathbb{R}}^{2(m-1)s\times 3}$。

定义时间分配向量T为：

各种代价定义如下：

$L$是每段轨迹上的样本数，$N$是动态对象的数量，$d_s=e_{ctl}+e_{pos}+r_{sum}$是保证车辆轨迹与障碍物在任何时刻的安全性的安全半径，其中$+r_{sum}$为动态物体和车辆的特征半径，$e_{ctl}$为整个控制系统的估计响应误差，为$e_{pos}$目标位置估计误差。$e_{pos}$可以通过KF的后验估计协方差矩阵$P^{P_i}$估计得到，$e_{pos}$公式为：

为速度$v_{max}$、加速度$a_{max}$上界；通过离散形式${\hat{p}}_b^i=p_b^i(t_f)$，可以${\hat{p}}_b^i$通过转发对象位置得到。
$S_e$本质上是一个多项式的积分，在每次优化迭代中都很容易计算出封闭形式的结果。其他代价在我们的代码中以离散的形式积累，通过沿着轨迹迭代样本点.

为了有效解决优化问题，使用梯度求解。
每个轨迹段的转角$p$写为$\frac{\partial G(p,T)}{\partial p}$；
时间向量$T$的梯度写为$\frac{\partial G(p,T)}{\partial T}$；;

所需的梯度可以由梯度传播定律导出:

通过文献6计算$\frac{\partial C}{\partial p}$、$\frac{\partial C}{\partial p}$，$\frac{\partial G}{\partial C}$、$\frac{\partial G}{\partial T}$可以用代价的定义导出，目标函数的梯度可以最终获得。此外，文献[6]中的方法可以消除(17)中的约束条件，将优化问题转化为无约束问题，使用L-BFGS等优化算法可以有效求解。

文献6：
Geometrically constrained trajectory optimization for multicopters，arXiv

【TO BE CONTINUE】