Fast-Tracker: A Robust Aerial System for Tracking Agile Target in Cluttered Environments

Abstract/摘要

本文给出了在目标行动未知,稠密环境未知的情况下,无人机跟踪的系统解决方案.
本文主要包括两部分:

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

2. 跟踪轨迹规划:传统规划结构,前端采用考虑运动学的搜索方法,后端采用时空优化和无碰撞的轨迹优化方法

   Instruction/介绍

   在无人机跟踪领域,首先要在有限传感器条件下识别目标和周围障碍物,然后规划出可行的安全轨迹.此外,为了处理意外情况,需要高频率的重规划过程.我们提出一个目标意图未知,周围环境未知的无人机跟踪系统框架.
   目标轨迹预测及重定位:
   将目标近似为刚体,假设目标运动速度和加速度有界且连续.对于观测过的历史轨迹使用polynomial regression,然后用Bernstein basis polynomial进行动态约束,生成的轨迹作为目标未来运动预测.此外,针对障碍物遮挡,传感器范围限制,目标运动未知时,无人机难以定位目标的情况,我们设计了目标重定位策略.
   跟踪轨迹生成:
   设计一个考虑当前目标观测和未来运动预测的启发式搜索函数,然后在已搜索的路径上生成安全走廊,后端优化在安全走廊内生成时空优化安全轨迹.
   本文贡献:
   我们将上述方法集成到无人机系统上,采用多个相机提升传感器性能.在真实环境中实验,并与其他方法相比,更有优势.本文贡献如下:

3. 轻量级且目标运动未知的基于贝塞尔多面体约束的运动预测方法.

4. 前端:一个考虑目标的运动学搜索方法;后端:时空轨迹优化方法
5. 结合传感器,形成系统解决方案,并进行评估

目标运动预测

带约束的贝塞尔预测轨迹

采用贝赛尔曲线描述目标预测轨迹,n阶贝塞尔曲线表示如下:

将观测到的世界坐标系下的目标位置及时间戳存入长度为L的FIFO队列中,其中.时间范围是等于当前时刻.当获取到新的目标观测,调整历史观测生成新的目标预测轨迹.预测的时间范围是.如图3

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

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

使目标轨迹和观测距离差最小。为避免过拟合，增加二阶正交项，为该项权重。

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

是贝塞尔曲线阶次，是时间尺度。
贝塞尔曲线的二范数正定，所以该问题是带约束的QP问题。我们采用OOQP开源求解器求解该问题。

目标重定位

在无人机丢失目标观测后，需要重新定位目标位置。在多数情况下，无人机快速到达目标最后观测位置，并沿预测轨迹运动可以重新获取到目标。详细步骤如下：

1. 采用路径搜索算法得到飞机当前位置到目标丢失最后观测位置的最短路径,然后离散最后预测轨迹。如果预测轨迹上有障碍，采用路径搜索算法生成无障碍路径.和构成重定位路径
2. 基于生成安全走廊
3. 在安全走廊中生成时空优化和无障碍轨迹，作为重定位轨迹。
4. 在重定位步骤中，如果在重定位轨迹上有新的障碍，则重新生成新的轨迹。

过程如图4：

安全轨迹生成

目标导向的运动学跟踪路径搜索

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

• 无人机状态向量:
• 控制输入：加速度，用，将加速度离散，得到离散序列
• 拓展邻节点的时间序列为

拓展节点公式如下：

其中是上一帧状态向量，给出可以得到基于运动学的拓展节点。每次拓展步骤可以生成个拓展节点
损失函数：
每个节点的损失函数为,其中表示初始状态到当前状态的实际消耗，是使搜索更快速的启发式函数。
采用最小化能量-时间损失函数如下：

因为拓展节点由离散输入组成，定义其损失函数为,若路径由个拓展节点组成，则

启发式损失函数分为两部分：

其中是无人机当前状态到目标状态的距离。其中目标状态由跟踪目标的当前状态和预测时刻后状态的中间量得到，公式如下：

为权重，由预测轨迹得到，速度量是的一阶导数。
两点边界值问题（OBVP）用于求解最小化动态损失函数的最优路径。定义最小化损失函数为到的OBVP距离，即,过程描述见下图：

是时间惩罚函数，是期望拓展时间的和，是权重，该项能够加速搜索，因为它倾于选择当前节点的相邻区域，而非整个状态空间。增加该项通过牺牲得到最优解而降低计算量，但在结果中我们仍然能够找到可行且满足条件的解。

时空优化轨迹生成

在后端，我们采用文章[2]“Z. Wang, C. Xu, and F. Gao, “Generating large-scale trajectories effi-
ciently usingdouble descriptions of polynomials””中的优化方法，在安全走廊中生成时空优化轨迹。
安全走廊定义如下：

其中表示小方块

损失函数如下：

其中平滑项如下:

是保证在相交范围内的对数约束条件，定义如下：

其中为常数，为所有的单位向量,为自然对数。
用于调整轨迹的激进程度，定义如下：

其中为最大速度，为最大加速度。

Results/实验结果

应用详情

四旋翼硬件平台同论文”Teach-repeat-replan”,目标是头戴二维码移动的人.用三个有限视场角的相机检测目标.因为视场角较宽,无需使四旋翼面向目标,而是朝向轨迹切线方向,以保证能够观测到轨迹方向的障碍.系统总运行时间接近20ms,设置整个系统重规划频率为15HZ.

实验详情

我们在室内未知复杂环境中进行多次实验.室内环境如下图.在该环境中,目标平均速度1.3m/s,飞机能够安全平滑动态的跟踪目标


在室外环境的稠密树林中,试验场景如下图,目标运动速度更快,在该场景下,四旋翼最快速度达到3m/s

基准比较

将目标运动预测和跟踪轨迹规划方法与其他方法相比.采用的仿真环境,随机生成140个障碍.将我们的工作与论文[10]”J. Chen, T. Liu, and S. Shen, “Tracking a moving target in cluttered environments using a quadrotor”做对比.为了比较,给目标的对地真实位置添加高斯噪声,参数为(),高斯噪声的标准差分别为用来表示环境中的低噪声,中噪声,高噪声.我们对比目标将来位置和预测轨迹的平均距离误差.平均误差计算公式如下:

其中是当前时间,是采样间隔,是采样步长,每个场景预测超过15000个点.对比结果如下表:

相比于论文[10]中的方法,本文的方法预测精度更高,计算时间0.5ms,比论文[10]的0.3ms有所增加.

此外,与论文[9]”B. Jeon, Y. Lee, and H. J. Kim, “Integrated motion planner for real- time aerial videography with a drone in a dense environment””对比跟踪轨迹规划方法.
将目标未来轨迹的对地真实位置作为两种规划方法的输入.
定义：
有效跟踪时间：无人机到达与目标距离在平面小于3m的时间。
跟踪频率：在整个跟踪的总时间中，有效跟踪时间的占比
设置目标不同的平均速度和最大速度为不同的跟踪场景，在每个场景200个跟踪任务中，对比计算时间和跟踪频率，结果如下表：

相比于论文[9]，我们的方法计算量更小，并且能更好更有效的跟踪目标。结果表明高频的重规划对目标突然加速度等不可预知场景有较好的反应。此外如下图，目标轨迹为黑色虚线，蓝色曲线及扇形是本文规划轨迹和加速度，红色为论文[9]的方法，实验表明我们的方法更节能

Future/未来研究方向

未来研究方向是在跟踪过程中避开动态障碍，并且将会提供物体检测方法，使系统能够应用与更多场景。