前言

grad_traj_optimization是基于梯度的在线安全轨迹生成以及轨迹优化框架。在给定车辆状态估计和环境感知信息后，运动规划模块在自由空间中生成从当前状态到目标状态的平滑且安全的轨迹，同时躲避遇见的障碍物。

一、原理

1.系统组成

系统组成如上图所示。论文中所使用的测试平台是四旋翼飞行器，利用视觉惯导系统（VINS）来估计自身状态，并利用双目相机实时构建稠密地图。相机输出的局部体素地图传给路径规划模块，规划模块从体素地图中查找机器人从当前状态到目标状态的初始安全路径，然后，轨迹优化模块根据光滑度度，安全性和动力学可行性对轨迹进行优化处理。最后利用几何控制器来跟踪生成的轨迹。

2.轨迹生成

利用基于采样的方法（RRT、RRT*等）找到安全、无碰撞的路径，通常路径是不够平滑的，并不适合空中机器人进行跟踪。

可以使用Minimum Snap来生成光滑的轨迹，其中的轨迹是用分段多项式函数来表示，轨迹生成问题可以表述为二次规划（QP）问题。使用闭式求解的方法可以将带约束的QP问题转换化无约束的QP问题，从而绕开了求解带约束的QP问题过程，这种方式提高了数值稳定性和计算效率。

轨迹规划分为基于硬约束和软约束两种方法。基于硬约束的方法是在八叉树地图生成飞行走廊，然后使用二次约束二次规划（QCQP）生成完全约束在走廊内的轨迹。然而基于硬约束的方法是认定所有的安全区域都是等价的，很可能会导致轨迹的某些地方距离障碍物太近，如果控制部分不能完全按照轨迹走，则依然会导致碰撞；另一个潜在问题在于如果大量的使用基于视觉的方法，则会产生较多的噪点，那么就会给规划问题带来麻烦(对噪声敏感)。基于软约束的方法则是利用环境中的梯度信息来增加机器人靠近障碍物的惩罚，将生成的原始轨迹往远离障碍物的方向推。

3.优化策略

轨迹的优化策略分为两步：

1. 仅以碰撞代价来优化轨迹，将离障碍物比较近的轨迹点往外推；
2. 根据当前轨迹点的位置重新分配时间和重新调整参数来优化轨迹，并通过添加平滑项和动态惩罚项来优化目标。

下图表示的是在稠密中生成的轨迹。颜色值代表着障碍物的距离值，越深代表着对应的碰撞代价越高。橙色区域属于缓冲区，即虽然靠近障碍物但仍然安全的区域，黑色区域则是障碍物。蓝线显示的轨迹是一条无碰撞但难以跟踪的直线轨迹。优化的第一步是将中间轨迹点推离附近的障碍物，如图中的绿线所示，优化的第二步则是重新分配时间和调整参数来优化轨迹，如红线所示。

二、演示

参考

[1] Fei Gao, Yi Lin and Shaojie Shen, Gradient-Based Online Safe Trajectory Generation for Quadrotor Flight in Complex Environments