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

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

本文是对《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

Publisher: ICRA 2021

Editer: 柯⻄

摘要

该⽂给出了在⽬标⾏动未知，稠密环境未知的情况下，⽆⼈机跟踪的系统解决⽅案。

该⽂主要包括两部分：

⽬标轨迹预测：利⽤观测到的⽬标历史信息,在考虑⽬标动态约束的情况下预测未来轨迹；
跟踪轨迹规划：传统规划结构,前端采⽤考虑运动学的搜索⽅法,后端采⽤时空优化和⽆碰撞的轨迹优化⽅法。

内容及贡献

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

目标轨迹预测及重定位

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

跟踪轨迹生成

设计⼀个考虑当前⽬标观测和未来运动预测的启发式搜索函数，然后在已搜索的路径上⽣成安全⾛廊，后端优化在安全⾛廊内⽣成时空优化安全轨迹。

论文贡献

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

轻量级且⽬标运动未知的基于⻉塞尔多⾯体约束的运动预测⽅法；
前端：⼀个考虑⽬标的运动学搜索⽅法;后端：时空轨迹优化⽅法；
结合传感器，形成系统解决⽅案，并进⾏评估。

目标运动检测

带约束的贝塞尔预测轨迹

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

（图片来自深蓝学院运动规划课程）

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

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

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

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

2.为避免过拟合，增加⼆阶正交项。

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

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

目标重定位

在⽆⼈机丢失⽬标观测后，需要重新定位⽬标位置。在多数情况下，⽆⼈机快速到达⽬标最后观测位置，并沿预测轨迹运动可以重新获取到⽬标。

详细步骤如下：

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

2. 基于Pr生成安全走廊；

3. 在安全⾛廊中⽣成时空优化和⽆障碍轨迹，作为重定位轨迹；

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 中的优化⽅法，如下图，在安全⾛廊中⽣成时空优化轨迹。

Results/实验结果

应用详情

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

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

实验详情

在室内未知复杂环境中进⾏多次实验，室内环境如下图。在该环境中,⽬标平均速度1.3m/s,⻜机能够安全平滑动态的跟踪⽬标。

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

附

code：https://github.com/ZJU-FAST-Lab/Fast-tracker

原⽂链接：

https://www.yuque.com/docs/share/adaea007-7bce-4421-806d-9b8c38b0870d?#

实验视频结果见链接：

https://www.bilibili.com/video/BV1xr4y1w7RJ

作者简介：柯西，毕业于哈尔滨工程大学，主要研究方向为无人机运动规划。

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、orb-slam3等视频课程）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近2000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~