三次轨迹多项式在无人机上实现的一点小想法

最近实验涉及到了无人机的室内飞行，在调试飞机的同时，对路径规划算法的文献进行了一些查阅。Shen的一篇文章中提到了一些无人机室内感知与路径规划算法。文章发表在2016年的ICRA，名为"online generation of collision-free trajectories for quadrotor flight in unknown clutter environment"。文章中对无人机的pose只取了位置和偏航（x，y，z，yaw）。飞机的状态估计为双目视觉和雷达+UKF里程计，光流估计速度。
以下内容都截取自文章，英文以截图形式展现。

Introduction

现在无人机复杂环境的路径规划的难点在于非凸障碍物和可穿越（安全）空间的有效建模。早期方法是MILP，然后使用mixed-integer quadratic programming（混合整数二次规划）来建模非凸障碍物表面。但是这种方法的劣势在于当障碍物多了之后，计算复杂度大了很多。

然后又说了为了有效使用计算量，更多的局部（local）和反应式（reactive）方法被使用，缺点是无法全局优化。
提到“RRT+QP"的方案，RRT方案网上仿真有很多，包括集成在ROS的功能包，我试着跑了跑，感觉效果还不错。终于在末尾提到了自己的方案”用octree-based方法来表示环境中的物体来生成无障碍轨迹，然后QP优化“。继承了混合整数方法的准度，但是弥补了计算速度的缺失，还解决了RRT在狭窄的空间内生成轨迹无效的问题。

Method

octree-based

我觉得，简单理解就是用体素将深度信息表示出来，然后图b是作者之前方法，图a是改进之后的。明显看出可行空间利用的更加充分，因为黄色（蓝色）格子的交点是生成轨迹的主要参考。

可行轨迹连接

具体不多说，给个图表明意思。细节直接看原文。

飞行长廊约束下的轨迹生成

用二次规划方法生成安全轨迹，多项式轨迹指定平面输出，文中只考虑了平移避障，不考虑yaw。
N阶多项式生成有数学公式，我试了一个三次的。





然后在每个点有速度的时候，结果是这样。

为了安全期间，开始做实验可以将所有的目标位置的速度都设为0，结果是这样。

当然不同的点根据环境可以设置为不同的速度，这个方法在shen的另外一篇文章中有提过。
接下来文章讲到了怎么将环境的障碍约束和QP结合起来，引入了一种迭代的思想，公式给出：

定义也清晰给出并且证明了，数学知识不复杂，应该可以看懂。
说到这，我想到了ROS中的导航功能包，很多文章的思路可以基于movebase做一些开发。
欢迎同行们一起交流~