[运动规划算法]基于飞行走廊的轨迹优化

---

简介

Btraj是一个在线的四旋翼无人机的运动规划框架，主要用于未知环境中的自主导航。基于机载状态估计和环境感知，采用基于快速行进（FM *）的路径搜索方法，从基于构建的欧氏距离场（ESDF）中寻找路径，以实现更好的时间分配。通过对环境进行膨胀生成四旋翼无人机的飞行走廊。使用伯恩斯坦多项式将轨迹表示为分段Bezier曲线，并将整个轨迹生成问题转换成凸问题。通过使用Bezier曲线，可以确保轨迹的安全性和高阶动力学可行性，可以将轨迹完全限制在安全区域内。

---

一、介绍

运动规划问题可分为前端的路径查找和后端的轨迹优化。对于前端来说，搜索出的路径通常是不够光滑或者受机器人本身的动力学约束限制而无法执行。因此，后端的优化模块生成平滑轨迹是必不可少的。

有许多方法可以帮助优化路径以生成光滑的轨迹。基于梯度的方法（如CHOMP）将轨迹优化问题表述为对安全性和平滑性的不利影响的非线性优化。Minimum Snap算法将轨迹用分段多项式函数表示，轨迹生成问题被表述为一个二次规划（QP）问题。

安全性和动力学可行性是通过迭代地添加对违禁极值的约束并重新求解凸问题来实现的，但以这种方式需要经过多次迭代之后才能获得可行解。另一种方案是沿轨迹采样时不断添加额外的约束，随着四旋翼无人机速度的增加，添加的约束也就越多。在本文中使用伯恩斯坦多项式来表示分段轨迹，是利用了伯恩斯坦多项式的属性，确保轨迹执行上受到安全性和动力学可行性约束。

---

二、原理

飞行走廊生成

在获取到路径规划出的原始路径后，提取环境中的自由空间构建用于后端优化的飞行走廊。充分利用好自由空间来找到解空间并获得最优解对于生成轨迹来说是同样重要的。因此，我们的方法是初始化飞行走廊并进行膨胀。对于路径中的每个节点，可以通过查询ESDF轻易获得从节点位置到最近障碍物的距离。在每个节点处，我们获得了一个安全空间的球体以抵御最近的障碍物，并将飞行走廊初始化为该球体的内接立方体。接着通过查询沿轴方向x，y，z的相邻网格来扩大每个多维数据集，直到它到达最大的自由体积为止（如图3所示）。由于我们将飞行走廊初始化为一系列密集节点，因此某些节点可能包含相同的放大立方体，这是不必要的，但会增加后端优化的复杂性。因此，需要修剪掉走廊中所有重复的立方体。路径搜索、初始的飞行走廊生成和膨胀后的飞行走廊生成过程如图4所示。

---

三、项目演示

[运动规划算法]基于飞行走廊的轨迹优化

---

参考

[1] F Gao, W Wu, Y Lin, S Shen，Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial

---