⽬标⾏动及稠密环境未知情况下，⽆⼈机跟踪的系统解决⽅案

来源：深蓝学院-深蓝前沿教育

本文是对《Fast-Tracker：A Robust Aerial System for Tracking Agile Target in Cluttered Environments》的⽂献解读。（文末附原文章链接）

Author:Zhichao Han*, Ruibin Zhang*, Neng Pan*, Chao Xu, and Fei Gao

Editer:柯⻄

1. ⽬标轨迹预测：利⽤观测到的⽬标历史信息,在考虑⽬标动态约束的情况下预测未来轨迹；

2. 跟踪轨迹规划：传统规划结构,前端采⽤考虑运动学的搜索⽅法,后端采⽤时空优化和⽆碰撞的轨迹优化⽅法。

在⽆⼈机跟踪领域，⾸先要在有限传感器条件下识别⽬标和周围障碍物，然后规划出可⾏的安全轨迹。此外，为了处理意外情况，需要⾼频率的重规划过程。我们提出⼀个⽬标意图未知，周围环境未知的⽆⼈机跟踪系统框架。

将⽬标近似为刚体，假设⽬标运动速度和加速度有界且连续，对于观测过的历史轨迹使⽤polynomial regression，然后⽤Bernstein basis polynomial进⾏动态约束，⽣成的轨迹作为⽬标未来运动预测。此外，针对障碍物遮挡，传感器范围限制，⽬标运动未知时，⽆⼈机难以定位⽬标的情况，我们设计了⽬标重定位策略。

将上述⽅法集成到⽆⼈机系统上，采⽤多个相机提升传感器性能，在真实环境中实验，并与其他⽅法相⽐，更有优势，该⽂贡献如下:

1. 轻量级且⽬标运动未知的基于⻉塞尔多⾯体约束的运动预测⽅法；
2. 前端：⼀个考虑⽬标的运动学搜索⽅法;后端：时空轨迹优化⽅法；

3. 结合传感器，形成系统解决⽅案，并进⾏评估。

采⽤⻉赛尔曲线描述⽬标预测轨迹，n阶⻉塞尔曲线性质如下：

该⽅法将观测到的世界坐标系下的⽬标位置及时间戳存⼊⻓度为L的FIFO队列中。其中，历史时间范围是，等于当前时刻。当获取到新的⽬标观测，调整历史观测⽣成新的⽬标预测轨迹 。预测的时间范围是 ，如图3。

历史观测的可信度随时间递减，因此，观测到的时间越靠前，在损失函数中权重越低。添加权重 判断不同时间戳观测的置信度。采⽤双曲切线tanh(x)计算权重：

函数值随 值的增加快速衰减，可以有效判断不同观测的有效性。整体损失函数包括两部分：

1.使⽬标轨迹和观测距离差最小；

为了保证⽬标预测轨迹的动态可⾏性，预测速度和加速度约束为]，]。根据⻉塞尔曲线的凸包属性和hodograph属性（详⻅运动规划课程第六节），运动学约束如下：

n是⻉塞尔曲线阶次， st是时间尺度。⻉塞尔曲线的⼆范数正定，所以该问题是带约束的QP问题。

1. 采⽤路径搜索算法得到⻜机当前位置到⽬标丢失最后观测位置的最短路径，然后离散最后预测轨迹。如果预测轨迹上有障碍，采⽤路径搜索算法⽣成⽆障碍路径  ·.和  构成重定位路径Pr；

4. 在重定位步骤中，如果在重定位轨迹上有新的障碍，则重新⽣成新的轨迹。

运动学搜索⽅法基于hybrid  A*算法，主要包括邻节点拓展和启发式函数设计两部分：

邻节点拓展：

·⽆⼈机状态向量：

·控制输⼊：输⼊为加速度，将加速度离散，得到离散序列

·拓展邻节点的时间序列为

其中是上⼀帧状态向量，给出 可以得到基于运动学的拓展节点。每次拓展步骤可以⽣成  个拓展节点。

启发式损失函数：

1. 距离损失函数

·：⽆⼈机当前状态Xc到⽬标状态Xg的距离。

·⽬标状态Xg ：由跟踪⽬标的当前状态Xtc和预测时刻后状态的中间量得到。

·预测状态：由预测轨迹 得到，速度量是 的⼀阶导数，   其中 采⽤Xc到Xg的OBVP距离，转化为两点边界值问题（OBVP），⽤于求解最⼩化动态损失函数的最优路径。

2. 时间损失函数

 是时间惩罚函数，  是期望拓展时间的和，是权重，该项能够加速搜索，因为它倾于选择当前节点的相邻区域，⽽⾮整个状态空间。

在后端，该⽂采⽤了⽂章[2] Z. Wang, C. Xu, and F. Gao, Generating large-scale trajectories effi-ciently usingdouble descriptions of polynomials 中的优化⽅法，如下图，在安全⾛廊中⽣成时空优化轨迹 。

四旋翼硬件平台同论⽂"Teach-repeat-replan",⽬标是头戴⼆维码移动的⼈.⽤三个有限视场⻆的相机检测⽬标.因为视场⻆较宽,⽆需使四旋翼⾯向⽬标,⽽是朝向轨迹切线⽅向,以保证能够观测到轨迹⽅向的障碍。

系统总运⾏时间接近20ms,设置整个系统重规划频率为15HZ。