[科研理论]无人机底层控制算法PID、LQR、MPC解析

文章目录

  • [1. PX4飞控PID简介](#1. PX4飞控PID简介)
    • [1.1 位置控制器](#1.1 位置控制器)
    • [1.2 速度控制器](#1.2 速度控制器)
    • [1.3 加速度和yaw转到姿态](#1.3 加速度和yaw转到姿态)
    • [1.4 姿态控制器](#1.4 姿态控制器)
    • [1.5 角速率控制器](#1.5 角速率控制器)
  • [2. 线性二次型优化(LQR)控制](#2. 线性二次型优化(LQR)控制)
  • [3. 模型预测控制MPC/NMPC](#3. 模型预测控制MPC/NMPC)
    • [3.1 MPC](#3.1 MPC)
    • [3.2 NMPC](#3.2 NMPC)

1. PX4飞控PID简介

相关链接:PX4官方中文文档PID概念(不了解PID的可以看看这个或者翻自控书吧)

PX4使用了串级PID控制器,串级PID通过分而治之的策略,将复杂控制问题分解为多个简单子问题,在响应速度、抗干扰性、稳定性和工程实现上均优于单级PID。比如姿态环处理的IMU噪声不会影响到位置环,同时各环PID参数可独立调谐,内环保证局部稳定(如机体角速率收敛),外环在此基础上优化全局性能。避免单级PID因参数耦合导致的振荡或发散。

1.1 位置控制器

在位置外环中只使用了一个P控制器和一个限幅函数。使用当前的位置估计和期望位置相减得到误差,然后经过P控制器和限幅得到期望速度,输出到速度内环。

为什么外环只需要一个P控制环呢?

那是因为外环的核心任务是"设定目标",而非快速跟踪。核心任务是将位置误差线性转化为速度指令,无需积分或微分,速度内环的PID会补偿外环的不足,避免振荡和噪声放大。若外环加入I或D控制,积分会导致超调或延迟,微分会放大噪声,反而破坏系统稳定性,因此简化外环、强化内环是最优策略。

1.2 速度控制器

速度内环控制器就熟悉一点,PID大套餐上齐了。首先估计速度直接进入LPF(低通滤波器,防止高频干扰微分结果)然后计算微分,进入D环节,这一部分是防止输出突变 以及提前控制防止震荡 ,其实也不难理解,就是对当前的估计做了微分反映了数据的运动趋势;然后估计速度与期望速度作差得到误差进入积分环节,这部分是**消除稳态误差**(如抵抗恒定风阻导致的速度偏差),其实就是识别到误差长时间没有归零就加大矫正力度;最后是误差进入P环节**快速响应速度误差**。

**为什么内环使用PID控制器?**速度内环用PID控制,因为它需要快速精准地跟踪速度指令,比例项响应误差,积分项消除静差,微分项抑制扰动,确保内环动态性能足够支撑外环。

**总结:**外环P控制像"指挥官"设定宏观目标,内环PID像"执行者"处理细节,二者分工明确,避免单级PID的复杂性和不稳定性。

1.3 加速度和yaw转到姿态

由于无人机的yaw和速度加速度在动力学上天然解耦,yaw角在规划器中是可以单独规划的,不过常见规划器还是按照轨迹的切向方向来的。图中的yaw角也是直接从规划器来的,而不是像pitch、roll需要从加速度中提取。

1.4 姿态控制器

多旋翼姿态控制器基于四元数(quaternion)设计,通过误差四元数计算期望角速率,最终输出饱和的角速率命令。

1.误差计算(通过四元数乘法实现):

四元数乘法如下(实部虚部分开):

这里分为两路,提取了实部的符号乘以虚部保证最短旋转路径(具体可以参考四元数的几何表示,q和-q表示一样的终端状态,但是旋转方向有大小区别,q可以保证旋转最短角度):

2.P控制器构造

角速度 Ω 的本质是旋转偏差随时间的变化率。在控制系统中,我们希望角速度命令与姿态误差成比例关系,即**角速度的方向应对准旋转轴,大小应与旋转角度偏差成正比**。

观察四元数的几何表示:

可以看到w是影响旋转角度大小,虚部影响旋转轴方向还有旋转角度大小。当误差特别小时,可以使用小角度假设,此时的虚部等于1/2θn,那么根据前面的构造,可以进一步构造一个有物理意义的P系数即2P。

这个时候P的物理意义是姿态误差每增加一弧度,角速度增加P弧度每秒。最后再限幅保证安全。

这里为什么选择四元数而不选择欧拉角呢?

主要有下列几点原因:1.欧拉角有万向节死锁问题。2.四元数是线性计算,欧拉角是三角函数计算,更加复杂。3.通过四元数插值(如SLERP)可平滑过渡姿态,且误差四元数的实部符号能自动选择最短旋转路径。

这一环可以自动调参:自动调参,但是感觉对大轴距的飞机效果不好。

1.5 角速率控制器

K-PID:比例项(K)独立于积分(I)和微分(D)路径,直接作用于误差信号。无人机需要快速响应姿态误差(比例项K直接作用),同时通过积分消除稳态误差(如风力干扰),微分抑制振荡。

这一环可以自动调参:自动调参,但是感觉对大轴距的飞机效果不好。

计算完期望的体速率和推力之后,使用旋翼的模型计算每个电机的转速:

由电机转速得到力矩和推力公式:

对上式求逆得到每个电机转速:

2. 线性二次型优化(LQR)控制

无人机的底层控制算法中,LQR(Linear Quadratic Regulator,线性二次调节器)是一种基于状态空间模型的优化控制方法,广泛应用于姿态、位置等底层控制环节。LQR是一种最优控制算法,通过最小化一个"代价函数"来求解最优控制输入。其核心思想是:

  • 线性系统:无人机动力学模型在平衡点附近可线性化(如悬停状态附近)。
  • 二次代价函数:平衡状态误差和控制输入的能耗,实现"高性能+低能耗"的折中。

由于限制为一个线性系统,所以这个状态方程就不会太复杂,以无人机悬停为例。

无人机的状态方程(此时当做质点):

设x为无人机的位置、速度,A为无人机的状态转移矩阵,B为输入矩阵(描述控制输入对状态的影响),u为无人机的输入,这里应该是加速度。

代价函数为:

最小化跟踪误差以及输入加速度(节省能量)。Q:增大对角元素会强制对应状态快速收敛。R:增大对角元素会限制控制输入的幅值。

这个目标函数是一个标准的二次型优化问题(可以类比于二次函数),是有解析解的。下面是推导过程:

1.先构建一个关于状态向量的值函数(其中P 是对称正定矩阵,需要求解):

2.带入HJB方程(哈密顿-雅各比-贝尔曼方程)

这个方程表达了 从当前状态出发,若采取最优控制策略,所能达到的最小代价应满足某种微分方程。

再把值函数带入得到:

然后对这个HJB方程对输入u求导:

得到

再构造一个反馈增益矩阵K:

再将结果带入HJB方程得到:

最终求得

**总结:**LQR通过数学模型和优化理论,为无人机提供了一种高效、稳定的底层控制方案。其核心是状态反馈和代价函数最小化,适合需要精确建模和性能优化的场景。

3. 模型预测控制MPC/NMPC

MPC(Model Predictive Control,模型预测控制)是现代控制中非常重要的一个方法,特别适用于多变量系统、有约束系统或需要提前规划行为的场景,比如无人车路径跟踪、无人机避障、工业过程控制等。MPC 是一种滚动优化控制策略。它的基本思想是:

  • 使用系统模型预测未来状态变化(例如未来几秒或几个采样周期);
  • 基于预测结果最小化一个代价函数(类似 LQR 的目标函数);
  • 求解一个有限时域的优化问题,得到一组未来控制序列;
  • 只执行第一个控制输入,然后重新预测、重新优化,循环进行。

这就是 MPC 的"预测-优化-执行-滚动"机制。

如果系统方程是线性的就是MPC,如果非线性就是NMPC。

3.1 MPC

还是以无人机悬停为例,且暂时考虑一维模型,状态变量:

控制输入为加速度:

系统模型(离散)为:

代价函数为:

求解过程:

1.预测:

给定当前状态𝑥0,可以展开出整个时域的状态预测:

定义控制向量:

将整个状态预测写成矩阵形式:

2.优化求解

将预测状态代入代价函数:

代入状态展开:

展开为标准形式:

同时考虑一些必要的硬件约束,调用优化库进行求解得到数值解。

3.执行

执行求解后的u_0,即控制向量的第一个元素。

总流程

每个控制周期:

  1. 读取当前状态𝑥0建预测矩阵 𝑆𝑥,𝑆𝑢 构造代价函数和约束
  2. 解二次规划问题,得到控制向量 𝑢⋆
  3. 仅执行第一个控制输入 𝑢_0⋆
  4. 系统前进一步,滚动窗口,重复上述步骤

3.2 NMPC

仍然是无人机悬停为例,使用NMPC直接控制无人机的电机转速。

状态变量(12维):

控制变量(4维)

系统模型:

这个M就是1.5节的M矩阵。

代价函数:

然后再写出扩展过的系统模型,带入代价函数进行优化求解。

**总结:**LQR就是MPC无约束、无时域限制的特殊形式,而且由于形式简单,可以准确计算解析解,计算快、准。

相关推荐
**梯度已爆炸**16 分钟前
NLP文本预处理
人工智能·深度学习·nlp
胖大和尚20 分钟前
clang 编译器怎么查看在编译过程中做了哪些优化
c++·clang
uncle_ll21 分钟前
李宏毅NLP-8-语音模型
人工智能·自然语言处理·语音识别·语音模型·lm
Liudef0621 分钟前
FLUX.1-Kontext 高效训练 LoRA:释放大语言模型定制化潜能的完整指南
人工智能·语言模型·自然语言处理·ai作画·aigc
静心问道22 分钟前
大型语言模型中的自动化思维链提示
人工智能·语言模型·大模型
众链网络1 小时前
你的Prompt还有很大提升
人工智能·prompt·ai写作·ai工具·ai智能体
汀沿河1 小时前
2 大模型高效参数微调;prompt tunning
人工智能·深度学习·prompt
路溪非溪1 小时前
机器学习之线性回归
人工智能·机器学习·线性回归
钱彬 (Qian Bin)1 小时前
一文掌握Qt Quick数字图像处理项目开发(基于Qt 6.9 C++和QML,代码开源)
c++·开源·qml·qt quick·qt6.9·数字图像处理项目·美观界面
双叶8362 小时前
(C++)学生管理系统(正式版)(map数组的应用)(string应用)(引用)(文件储存的应用)(C++教学)(C++项目)
c语言·开发语言·数据结构·c++