文献阅读之无人机防御性驾驶-提前感知快速飞行的轨迹规划

大家好，欢迎大家关注我的知乎专栏- 慢慢悠悠小马车。

强烈推荐大家深度阅读原文《RAPTOR Robust and Perception-aware Trajectory Replanning for Quadrotor Fast Flight》，开放源代码 https://github.com/HKUST-Aerial-Robotics/Fast-Planner。建议先阅读《Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths》，是同一个团队的前期工作，可参考 CSDN。

原文逻辑非常清晰，这里按章节翻译，加上了自己的理解。

摘要

无人机高速导航问题是个巨大的挑战。现有方法在有限的时间内给出的轨迹无法保证可行性和质量。有些方法没有考虑环境感知，这正是高速飞行的瓶颈。本文提出了RAPTOR——一种鲁棒的且perception-aware重规划框架，来解决这个问题，达到快而安全的效果。该框架有3个重点：1是path-guided optimization(PGO)算法，规划出可行的轨迹；2是perception-aware规划策略和risk-aware trajectory refinement算法，用来主动观测和避开可能威胁到无人机行进的未知障碍物；3是yaw angle规划，通过调整轨迹点的朝向以便观测到周围环境。2、3点主要应用于有限视场角(FOV)的传感器，如摄像头。

一简介

作者在前文中提出了PGO算法，通过实时提取周围环境的若干结构路径来引导目标路径避开局部极小点，提高重规划成功率，通过并行优化提高轨迹质量。但缺少对环境的感知，使得PGO在复杂环境、高速飞行中表现不佳。为此，本文将其扩展成为了一种perception-aware重规划框架，新增重点体现在：

一种risk-aware trajectory refinement算法，识别PGO优化后轨迹附近的潜在危险未知区域。通过保证到这种区域的最小安全距离（足够刹停）、使其提前可见（纳入视场角内），使隐藏在这种区域的障碍物提前被探测到并可规避。
对yaw angle进行两步规划。第一步：在离散状态空间中搜索可最大化information gain和smoothness的yaw angle最优序列；第二步：优化该序列。这样轨迹的yaw angle限定了无人机可以更主动的观测到更多的环境信息，迭代的应用到接下来的周期规划中。

我觉得这2种方法的目的是相近的，2是1的泛化。1更关注未知区域的避障；2更关注观测到尽可能多的区域。但这2者对无人车可能都比较难导入，1要调整轨迹点的位置，但无人车在结构化环境中受车道线约束，可以车道内微调或变道，情况更复杂，或者在开放场景应用。2要调整轨迹点的朝向，但无人车是非完整约束，无法在限定下一个轨迹点位置的前提下使当前点不朝向下一个点，是冲突的。

二相关工作

无人机的轨迹规划可以分为2类：hard-constrained和gradient-based optimization。

前者的典型代表是minimum-snap，轨迹表示为分段多项式（piecewise polynomial trajectories），限定分段处连续等条件构造为带约束的二次规划问题（quadratic programming, QP），每一段的时间间隔必须是已知的。分段多项式的时间间隔分配失误常会导致偏离预期的结果。为此，fast marching和kinodynamic search方法用来搜索更优的初始路径和时间分配。

GTO方法属于后者，将轨迹生成构造为一个非线性优化问题，常以smoothness、safety、dynamic feasibility作为优化的目标函数。

三框架概述

如图3，topological paths searching输出多条体现周围环境结构的路径，触发一一对应的并行的轨迹优化path-guided trajectory optimization。优化后的最优轨迹，输入给risk-aware trajectory refinement，对轨迹点的位置做精细调整（不改朝向），得到position trajectory。该position trajectory输入给yaw angle planning，优化出yaw trajectory。position trajectory和yaw trajectory结合就是完整的轨迹，可以供控制模块跟踪。

这里我有2个问题：

全局的参考轨迹起到了什么作用？
将position和yaw解耦分开处理的适用性。无人机可以这样是因为其飞行方向和其朝向是独立无关的吗？无人车是非完整约束，应该是无法应用的。

四 path-guided trajectory optimization

详见前文，这里仅贴图（4、5）辅助理解。

五 topological paths searching

详见前文，这里仅贴图（6、8、9）辅助理解。2个算法的内容也和前文完全相同。

六 risk-aware trajectory refinement

该方法以PGO输出的最优轨迹为输入，通过精调部分轨迹点的位置，提供足以避开可能存在障碍物的未知空间的刹停距离，来提高安全性。主要分2个步骤，checking visibility status, iterative refinement，见算法3。

A checking visibility status

frontier intersecting point

如图10，该点即PGO输入轨迹上位于已知空间和未知空间分界面上的点，Pf。最坏的情况就是Pf后面紧邻障碍物，而其是没有被观测到的。若能尽早的观测到Pf，就有更多的未知区域变成已知。FrontierIntersection()以离散的时间步长沿PGO轨迹搜索并输出Pf及其对应的时刻tf。

visibility metric

我们期望Pf的visibility level（表示为ψ）需要大于ψmin。定义了从点P到点Pf的visibility level表示为方程5，即线段P→Pf到周围障碍物的最短距离。因为在第四章节中创建了ESDF保存了栅格到周围障碍物的距离，所以是O(1)时间复杂度。

critical view direction

存在这样的一个轨迹点Pc及其对应的时刻tc，当t<tc时ψ(Pi,Pf)<ψmin，当t≥tc时ψ(Pi,Pf)≥ψmin，则向量Vc、即Pc→Pf的朝向就是critical view direction，即恰好达到ψmin时的朝向。CriticalView()输出(Vc,Pc,tc)。

B iterative refinement

最坏的情况就是Pf后面紧邻障碍物。为了确保安全，需要调整轨迹点的位置，令无人机以最大加速度减速可以避免碰撞。（第5-7行，方程6）如果Pc到Pf可以刹停，返回Pc。否则，需要迭代检查其他轨迹点（第8-15行）。Pf应该更早的被观测到，比如点Ps(ts < tc)。但Ps处的速度Vs是未知的，需要对其在一定范围内迭代试探。Vs初值设置为（t0,tf）段的平均速度（第8行），然后调整轨迹并检查是否满足安全间距（第10-15行）。若不满足，将Vs乘以一个略大于1的因子继续迭代。

RefineTrajectory()将方程7-9转换为惩罚函数（方程10-11）作为优化的软约束，目标函数如方程14。方程10惩罚了轨迹点相对于原轨迹的横向偏移，即位置的变动大小，方程11惩罚了刹停需要的安全距离和实际存在的间距的差异。先利用方程14的后半部分计算初始的ts，再进一步调整（第11-12行）。如图11所示。

这里我理解了思路，但细节很复杂没搞明白。尤其是Vs为什么要递增，这样岂不是需要更大的刹停距离吗？

七 yaw angle planning

无人机常会用有限FOV的传感器。为了提高飞行安全，本文规划了轨迹的yaw angle以主动探测环境。规划过程分为图搜索和轨迹优化2部分。

A graph search

图搜索问题建模为根据smoothness和未知空间的information gain（IG）搜索一个yaw angle序列。保持轨迹点的位置不变，将其朝向进行采用，构建一幅如图12的图。采用Dijkstra算法搜索一条令smoothness和IG最大的yaw序列，即令方程15取最小。结合方程16来看，某个点离轨迹越远，其IG越小，所以方程15的第一项是对视野中偏离轨迹的点的处罚，即使更多的点分布在轨迹的近处，相当于最大化沿轨迹的视野分布。方程15的第2项是惩罚相邻2点的朝向差异，是平滑项。

定义IG为轨迹点视野内能被观测到的当前尚未观测到的voxels对轨迹的影响总和。能被观测到体现为该voxel没有被障碍物遮挡，尚未观测到体现为该voxel在无人机当前位置的视野之外。本文设计了如下3点以加速计算：1、FOV内的voxels被重采样；2、不同朝向的IG并行计算；3、缓存重复覆盖的voxel的IG，牺牲空间复杂度来减小时间复杂度。如方程16，设计离轨迹越近（考虑横向和纵向距离）的visible voxel有更大的IG。如图13。

B yaw angle optimization

将yaw angle随时间的变化用uniform B-spline表示。目标函数如方程17，第1项是平滑项，第2项是令朝向调整尽量小，第3、4项是dynamic feasibility，令角速度和角加速度尽量小。

八测试

测试无人机安装了Intel RealSense Depth Camera D435，硬件平台是Intel Core i7-8550U CPU。轨迹优化采用了非线性优化器NLopt。采用了FASTER、EWOK、RE Traj作为横评对比算法。本文算法效果见图15-20，生动的体现了“防御性驾驶”的提前让位观测前方和转弯的视角盲区、预防“鬼探头”的思想。