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

天际线

作者：Jiaxin Qiu | 编辑：3DCV
添加微信：dddvision，备注：无人机，拉你入群。文末附行业细分群

标题：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样条轨迹优化对其进行细化。而局部规划器基于模型预测轮廓控制([color=var(--weui-LINK)]MPCC)框架，同时考虑四旋翼动力学、估计的扰动、全局参考轨迹、控制成本、时间成本和安全约束，以生成实时的控制输入。实验证明，PE-Planner在速度、安全性和抗扰动能力方面均表现出色。综合来看，该规划器通过全局和局部规划的结合，实现了[color=var(--weui-LINK)]四旋翼无人机在复杂环境下的高性能自主飞行。
图1：在复杂的动态环境中以370克的有效载荷飞行
2、主要贡献
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[color=var(--weui-LINK)]样条函数：首先介绍了立方均匀B样条函数，用于参数化轨迹。该函数具有良好的性质和易用性。

轨迹平滑性计算：给出了计算立方均匀B样条轨迹平滑性的近似方法，即通过相邻控制点的差值来近似表示。
轨迹优化问题建模：建立了轨迹优化问题，目标是最小化轨迹的平滑性和与障碍物的距离的代价，同时考虑起始和终止位置的约束。

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

平滑且安全的参考轨迹：通过轨迹优化，得到了既平滑又安全的参考轨迹，作为局部规划器的输入。
这部分详细介绍了基于B样条的轨迹优化方法，用于对全局规划器生成的初始轨迹进行进一步优化，得到适用于后续局部规划器使用的平滑且安全的参考轨迹。
4、局部规划器
作者提出了一种局部规划器，将局部轨迹规划与跟踪控制相结合，以获得更好的性能。基于MPCC框架，进一步同时考虑参考轨迹、时间成本、控制成本、动力学模型、避碰和干扰估计来实时生成控制输入。
4.1、四旋翼无人机的动力学模型
四旋翼无人机的动力学模型，具体包括以下内容：
状态空间定义：将四旋翼无人机的状态空间定义为 x=[p^T,v^T,q^T]^T，其中 p 和 v 分别表示位置和速度，q 表示单位[color=var(--weui-LINK)]四元数。
输入定义：定义输入为 u=[T,ω^T]^T，其中 T 表示集体推力，ω 表示机体角速度。
动力学方程：给出四旋翼的动力学方程，包括位置、速度、姿态、推力和角速度之间的关系。
状态预测：采用欧拉方法对状态方程进行离散化，以预测未来的状态。
避障约束简化：在避障约束设计中，为了简化，忽略动力学方程中的扰动项 σ。
动态扩展：为了估计外部扰动，对动力学方程进行了扩展，加入了 z1 和 z2 两个状态变量。
这部分内容为后续的局部规划器设计和模型预测控制奠定了基础，定义了四旋翼无人机的状态空间和动力学模型，为轨迹生成和控制提供了理论基础。在这里推荐基于PX4实现的四旋翼建模与控制系列教程。
4.2、MPCC
基于模型预测轮廓控制（MPCC）框架的局部规划器设计，具体包括以下内容：
优化问题构建：定义了MPCC的优化问题，最小化跟踪误差和轮廓误差的平方，同时考虑时间成本、控制成本、安全约束等。
MPCC公式
状态预测：利用欧拉方法对状态方程进行离散化，以预测未来的状态。

避障约束：构建控制屏障函数（CBF），以确保静态和动态避障的安全性，并将这些约束加入到MPCC优化问题中。
扰动估计：设计广义比例积分观测器（GPIO）来估计外部扰动，并在线更新MPCC的预测模型，以应对扰动。
参数设置：给出MPCC的参数设置，包括预测步数、预测步长等。
轨迹跟踪：MPCC框架可以跟踪动态不可行的轨迹，以较小的误差进行接近最优时间跟踪。
这部分内容通过MPCC框架实现了局部规划器和跟踪控制器的统一，能够实时生成轨迹和控制输入，同时考虑避障、扰动估计等因素，以实现安全高效的飞行。
4.3、基于CBF的静态/动态避碰
图3. 防撞示意图
主要涉及以下内容：
简化假设：简化四旋翼的碰撞域为一个半径为r的球体，动态障碍物简化为一系列[color=var(--weui-LINK)]椭圆体。
距离计算：定义距离函数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以其完善的功能、优异的性能以及友好的生态脱颖而出。

<span style="outline: 0px; max-width: 100%; letter-spacing: 0px; text-align: justify; visibility: visible; box-sizing: border-box !important; overflow-wrap: break-word !important;"/>
不仅是科研机构大量使用PX4，许多公司也会基于PX4进行改进完善，实现工业级的需求。因此，能够基于PX4进行二次开发对于学习和工作都有着重要的价值和意义。飞控平台是一个复杂的软硬件系统，软件系统大致可以分为嵌入式、导航与控制三大部分，每一部分都需要专业的知识和经验。本课程针对控制这一大模块，分三步走，旨在让学员能够学懂PX4的控制器系统并具备二次开发的能力。
飞控学习三步走
第一步，飞起来
课程拥有配套的无人机平台，支持PX4代码，通过选择合适的硬件系统，该无人机具备良好的飞行性能。该无人机搭配光流与GPS等模块，支持室内室外飞行。课程将介绍组装、传感器与遥控器校准、参数调试、代码下载及实际飞行测试等基本内容，让学员在最短的时间内完成整机飞行测试。对于初接触飞控的小白，如果自行摸索，这个过程可能要花费数月时间，而且内容零散难以形成完整的知识体系，实验测试甚至有炸机风险。

整机实物图，联系微信：cv3d007，可采购课程配套无人机硬件。<span style="outline: 0px; max-width: 100%; box-sizing: border-box !important; overflow-wrap: break-word !important;"/>「御风250」-四旋翼无人机（基于PX4）[硬件+代码]<span style="outline: 0px; max-width: 100%; color: rgb(255, 41, 65); box-sizing: border-box !important; overflow-wrap: break-word !important;"/>
第二步，读代码
这个过程是向PX4学习的过程。PX4有着完善的功能，但这也意味着有复杂的代码。初次阅读可能无从下手，找不到重点。课程会先简要介绍PX4整体代码内容，然后重点介绍控制模块相关内容。
控制模块可以分为控制逻辑和控制算法两大部分，逻辑用于处理各种输入输出的选择、切换等，算法是输入输出具体关系的数学表示。
PX4使用的控制算法是基于四元数的PID，但是相比于传统的单级PID要复杂很多，一是因为多个PID串联，二是因为是改进的PID，例如微分项不对期望进行微分且加入滤波，又如使用了饱和积分算法。
如果缺乏控制算法的知识是很难理解代码内容的。除了上述的控制算法，控制逻辑也至关重要。课程还会讲解PX4中各种模式的切换、一键起飞、一键降落等过程。为了实现一个良好的飞行效果与用户体验，这些模块都值得专门地研究。
PX4速度控制框图
第三步，改代码
尽管PX4已经很强大，但也不能说完美无缺，而且不可能完全满足需求。课程将以两个典型例子展开，帮助学员实现改代码的能力。
第一个例子是在科研领域热门的SO(3)控制。
PX4虽然使用了四元数作为姿态控制，但是其中涉及欧拉角的计算，并不能完全避免欧拉角奇异值的问题。基于SO(3)的控制器能够完美地避免欧拉角的缺陷，实现任意姿态的飞行控制，而且基于SO(3)的姿态控制能够方便地与轨迹规划结合。
第二个例子是自抗扰控制器(ADRC)，和PID一样，该控制算法在工业界得到广泛的应用和验证。
通过加入观测器，ADRC实现更鲁棒的控制，抗扰动能力更强。通过学习这个例子，学员不仅能够学会这两个控制器算法，而且能够实现算法与逻辑的对接，实现自定义数据格式与日志记录，为日后的创新打下坚实的基础。
除了以上三大步骤中的内容，通过学习此课程还可以收获一套高效的飞行控制开发流程。课程不仅有配套的四旋翼硬件平台，还提供该四旋翼对应的的数学模型。该模型通过修改PX4自带的Gazebo模型实现。
在实际开发中，不仅可以通过Gazebo仿真验证算法逻辑，而且可以使用Gazebo仿真验证算法效果，Gazebo仿真中得到的效果基本可以在实物上复现，加速前期开发流程，减小试错成本。
综上，此课程针对四旋翼控制这一领域，使用PX4平台进行二次开发，基本包含PX4控制全部内容，带领学员从入门到中级。最后，建议此课程与姊妹篇《零基础入门四旋翼建模与控制（MATLAB仿真）》一起学习，相互弥补。

学后收获（硬件需额外单独购买）

收获一台带模型的无人机硬件平台，实现从零到一的飞行；
收获一套完整的PX4软件开发平台，实现二次开发；
理解PX4整体框架，理解PX4控制逻辑与控制算法；
收获一套基于SO3与一套基于ADRC的高性能控制器；
该课程还会涉及Git等工具使用，在实战中快速上手，方便后期学习工作使用。

面向人群

刚入门与初级的四旋翼的本科生、研究生；
有一定飞控基础想了解PX4的学员；
对于完成论文实验有需求的学员；
对四旋翼飞行控制感兴趣的学员。

课程特色

课程以PX4为硬件平台，起点在世界第一梯队；
课程给出配套飞控系统，并且提供无人机模型参数，为控制器设计提供理论基础；
课程介绍完整的PX4控制器相关内容，建立一个完整的知识体系；
课程基于PX4进行二次开发，提供性能优异的SO(3), ADRC控制器例子。

开课时间
2023年3月23日晚上8点（周六），每周更新一章节。
课程答疑
本课程答疑主要在本课程对应的鹅圈子中答疑，学员学习过程中，有任何问题，可以随时在鹅圈子中提问。

整机实物图，联系微信：cv3d007，可采购课程配套无人机硬件。「御风250」-四旋翼无人机（基于PX4）[硬件+代码]
注：即使不够买硬件，课程同样提供了提前采集好的数据集，可以正常学习。
▲添加小助理cv3d007，咨询更多
备注：以上图片和视频部分来自网络，如果侵犯了您的权益，还请联系删除！