中国科大力作 | 高性能四旋翼无人机复杂环境自主飞行规划器

标题：PE-Planner: A Performance-Enhanced Quadrotor Motion Planner for Autonomous Flight in Complex and Dynamic Environments
代码：https://github.com/USTC-AIS-Lab/PE-Planner

1、介绍

这篇文章介绍了一种名为PE-Planner的四旋翼运动规划器，用于在复杂和动态环境中进行自主飞行。该规划器由全局规划和局部规划组成。全局规划首先通过动力学路径搜索生成初始轨迹，然后使用B样条轨迹优化对其进行细化。而局部规划器基于模型预测轮廓控制(MPCC)框架，同时考虑四旋翼动力学、估计的扰动、全局参考轨迹、控制成本、时间成本和安全约束，以生成实时的控制输入。实验证明，PE-Planner在速度、安全性和抗扰动能力方面均表现出色。综合来看，该规划器通过全局和局部规划的结合，实现了四旋翼无人机在复杂环境下的高性能自主飞行。

2、主要贡献

PE-Planner的完整结构如图2所示。仿真和实验都证明了PE-Planner的性能增强，显示了增强的速度、安全性和对干扰的鲁棒性。主要贡献可以概括如下:

• 提出了一种新型的四旋翼运动规划器，无缝集成了规划和控制方法，以显着提高在复杂和动态环境下自主飞行的速度，安全性和鲁棒性。
• 这是第一个将控制障碍函数和干扰观测器技术结合到MPCC约束设计中的工作。与现有方法相比，该方法能较好地处理动态障碍和强外部干扰。此外，这种以控制为导向的设计很好地诠释了MPCC的潜力，以及我们整合规划和控制的技术路线。
• 在Gazebo模拟和现实场景中进行的大量实验验证了PE-Planner的速度，安全性和鲁棒性的增强。

3、全局规划器

主要介绍了全局规划器的设计，用于为局部规划器生成参考轨迹。采用运动学路径搜索生成初始无碰撞轨迹。随后，提出了一种改进的b样条轨迹优化方法来细化轨迹。具体内容如下：

3.1、动态路径搜索算法

动态路径搜索算法，用于生成四旋翼无人机的初始避障轨迹。该算法主要包含以下步骤：

A*算法改进 ：在基本的A*算法基础上，引入了四旋翼无人机的动力学模型，包括位置、速度和加速度的约束。

状态空间离散化 ：将加速度离散化为多个可能的值，用于生成不同状态转移，从而搜索最佳轨迹。

代价函数设计 ：定义了代价函数，综合考虑轨迹的控制代价和持续时间代价，用于评估和选择最佳轨迹。

最优轨迹搜索 ：采用Pontryagin最小原理求解从当前点到目标点的最优时间，并估计其代价，以用于A*算法的搜索。

平滑轨迹生成 ：最终，算法输出一个平滑且避障的轨迹，符合四旋翼无人机的动力学约束。

3.2、B样条轨迹优化

基于B样条的轨迹优化方法，旨在对全局规划器生成的初始轨迹进行进一步优化，以得到更平滑且安全的参考轨迹。具体步骤包括：

立方均匀B样条函数 ：首先介绍了立方均匀B样条函数，用于参数化轨迹。该函数具有良好的性质和易用性。

轨迹平滑性计算 ：给出了计算立方均匀B样条轨迹平滑性的近似方法，即通过相邻控制点的差值来近似表示。

轨迹优化问题建模 ：建立了轨迹优化问题，目标是最小化轨迹的平滑性和与障碍物的距离的代价，同时考虑起始和终止位置的约束。

参考轨迹的弧长参数化 ：根据优化结果，推导出轨迹弧长与时间的关系，实现了参考轨迹的弧长参数化，避免了后续重参数化步骤。

平滑且安全的参考轨迹 ：通过轨迹优化，得到了既平滑又安全的参考轨迹，作为局部规划器的输入。

这部分详细介绍了基于B样条的轨迹优化方法，用于对全局规划器生成的初始轨迹进行进一步优化，得到适用于后续局部规划器使用的平滑且安全的参考轨迹。

4、局部规划器

作者提出了一种局部规划器，将局部轨迹规划与跟踪控制相结合，以获得更好的性能。基于MPCC框架，进一步同时考虑参考轨迹、时间成本、控制成本、动力学模型、避碰和干扰估计来实时生成控制输入。

4.1、四旋翼无人机的动力学模型

四旋翼无人机的动力学模型，具体包括以下内容：

状态空间定义 ：将四旋翼无人机的状态空间定义为 x=[p^T,v^T,q^T]^T，其中 p 和 v 分别表示位置和速度，q 表示单位四元数。

输入定义 ：定义输入为 u=[T,ω^T]^T，其中 T 表示集体推力，ω 表示机体角速度。

动力学方程 ：给出四旋翼的动力学方程，包括位置、速度、姿态、推力和角速度之间的关系。

状态预测 ：采用欧拉方法对状态方程进行离散化，以预测未来的状态。

避障约束简化 ：在避障约束设计中，为了简化，忽略动力学方程中的扰动项 σ。

动态扩展 ：为了估计外部扰动，对动力学方程进行了扩展，加入了 z1 和 z2 两个状态变量。

这部分内容为后续的局部规划器设计和模型预测控制奠定了基础，定义了四旋翼无人机的状态空间和动力学模型，为轨迹生成和控制提供了理论基础。在这里推荐 基于PX4实现的四旋翼建模与控制系列教程 。

4.2、MPCC

基于模型预测轮廓控制（MPCC）框架的局部规划器设计，具体包括以下内容：

优化问题构建 ：定义了MPCC的优化问题，最小化跟踪误差和轮廓误差的平方，同时考虑时间成本、控制成本、安全约束等。

状态预测 ：利用欧拉方法对状态方程进行离散化，以预测未来的状态。

避障约束 ：构建控制屏障函数（CBF），以确保静态和动态避障的安全性，并将这些约束加入到MPCC优化问题中。

扰动估计 ：设计广义比例积分观测器（GPIO）来估计外部扰动，并在线更新MPCC的预测模型，以应对扰动。

参数设置 ：给出MPCC的参数设置，包括预测步数、预测步长等。

轨迹跟踪 ：MPCC框架可以跟踪动态不可行的轨迹，以较小的误差进行接近最优时间跟踪。

这部分内容通过MPCC框架实现了局部规划器和跟踪控制器的统一，能够实时生成轨迹和控制输入，同时考虑避障、扰动估计等因素，以实现安全高效的飞行。

4.3、基于CBF的静态/动态避碰

主要涉及以下内容：

简化假设 ：简化四旋翼的碰撞域为一个半径为r的球体，动态障碍物简化为一系列椭圆体。

距离计算 ：定义距离函数li,k，以计算四旋翼到动态椭圆体边缘的距离。

CBF定义 ：构建控制屏障函数（CBF）来保证四旋翼与静态和动态障碍物保持安全距离。

相对度定义 ：通过定义一个辅助变量T，使CBF具有相同的相对度，便于构建约束。

安全集定义 ：定义安全集C，由静态和动态避障约束的交集构成。

约束条件 ：通过在MPCC中加入CBF约束，确保四旋翼始终位于安全集C内。

4.4、扰动观测器

主要介绍了扰动观测器的设计，用于估计四旋翼飞行过程中可能遇到的外部扰动，具体包括以下内容：

动力学扩展 ：扩展四旋翼的动力学模型，加入了扰动σ、σ的导数z1和z2，以更准确地描述四旋翼的动力学。

观测器设计 ：设计广义比例积分观测器（GPIO），用于估计扩展动力学模型中的状态变量。

更新预测模型 ：在MPCC框架中，仅考虑第一步预测时估计的扰动σ0，忽略后续预测步中的扰动，以简化问题。

参数设置 ：通过极点配置获得GPIO的增益矩阵G1、G2和G3。

动态响应 ：扰动观测器能快速准确地估计外部扰动，并使四旋翼及时调整姿态和推力，以应对扰动。

5、实验

主要进行了以下实验：

在静态密集环境中，将PE-Planner与Fast-Planner和MPCC-DC进行了比较，结果显示PE-Planner在保证安全距离的同时，能够以更高的平均速度飞行。在动态环境中，PE-Planner与Dynamic Planning和MPCC-DC进行了比较，结果表明PE-Planner在存在静态障碍物的情况下，依然能保持超过90%的成功率。在扰动场景中，将PE-Planner与Fast-Planner以及不带GPIO的PE-Planner进行了比较，结果显示带有GPIO的PE-Planner对扰动具有更强的鲁棒性。在复杂环境中进行了实际飞行实验，PE-Planner实现了6.83m/s的最高速度和14.07m/s^2的最高加速度，验证了其在复杂环境中的表现。

6、总结

本文介绍了一种名为PE-Planner的新型四旋翼无人机运动规划器，旨在提升复杂动态环境下的自主飞行性能。该规划器包括全局规划器和局部规划器，全局规划器通过动力学路径搜索和B样条轨迹优化生成初始参考轨迹，而局部规划器基于模型预测轮廓控制框架，考虑四旋翼动力学、扰动估计、安全约束等，以生成实时控制输入。实验结果表明，PE-Planner在速度、安全性和抗干扰能力方面均优于现有运动规划器。具体而言，PE-Planner在复杂环境中实现了超过6.8m/s的高速飞行，展现出卓越的鲁棒性和安全性。本文的创新点在于将控制屏障函数和扰动观测器技术引入模型预测控制框架，以处理动态障碍物和强外扰。仿真和实验结果证实了PE-Planner的优越性能，为无人机在复杂环境中的自主飞行提供了有效解决方案。

本文仅属于学术交流，若有侵权请联系删除！

目前，四旋翼无人机在农业植保、物流配送、竞技飞行及科研实验等领域得到广泛应用。 许多公司、科研机构与无人机爱好者都会开发自己的飞控平台，在众多开源飞控平台中，PX4以其完善的功能、优异的性能以及友好的生态脱颖而出。

不仅是科研机构大量使用PX4，许多公司也会基于PX4进行改进完善，实现工业级的需求。 因此，能够基于PX4进行二次开发对于学习和工作都有着重要的价值和意义。 飞控平台是一个复杂的软硬件系统，软件系统大致可以分为 嵌入式、导航与控制 三大部分，每一部分都需要专业的知识和经验。 本课程针对控制这一大模块， 分三步走 ，旨在让学员能够学懂PX4的控制器系统并具备二次开发的能力。

第一步，飞起来

课程拥有配套的无人机平台，支持PX4代码，通过选择合适的硬件系统，该无人机具备良好的飞行性能。该无人机搭配光流与GPS等模块，支持室内室外飞行。课程将介绍组装、传感器与遥控器校准、参数调试、代码下载及实际飞行测试等基本内容，让学员在最短的时间内完成整机飞行测试。对于初接触飞控的小白，如果自行摸索，这个过程可能要花费数月时间，而且内容零散难以形成完整的知识体系，实验测试甚至有炸机风险。

第二步，读代码

这个过程是向PX4学习的过程。PX4有着完善的功能，但这也意味着有复杂的代码。初次阅读可能无从下手，找不到重点。课程会 先简要介绍 PX4整体代码内容，然后 重点介绍 控制模块相关内容。

控制模块可以分为 控制逻辑 和 控制算法 两大部分，逻辑用于处理各种输入输出的选择、切换等，算法是输入输出具体关系的数学表示。