ADRC控制算法在多旋翼飞行器上的应用

基础理论知识：

1. ADRC算法以及参数整定：
关于ADRC算法以及参数整定（调参）的一些心得体会_西涯先生的博客-CSDN博客_adrc控制算法
ADRC，全称叫做Active Disturbance Rejection Control，中文名是自抗扰控制技术。这项控制算法是由中科院的韩京清教授提出的。韩教授继承了经典PID控制器的精华，对被控对象的数学模型几乎没有任何要求，又在其基础上引入了基于现代控制理论的状态观测器技术，将抗干扰技术融入到了传统PID控制当中去，最终设计出了适合在工程实践中广泛应用的全新控制器。 一般来说，ADRC控制器包括三个组件：跟踪微分器，非线性状态反馈（非线性组合），扩张观测器。ADRC的框图如下（盗图自《从PID技术到"自抗扰控制"技术》）： 简单的介绍下ADRC的结构： 安排过渡过程就是跟踪微分器，主要目的就是为了输入量不要有跳变，便于实际系统实时跟踪。 非线性反馈实际上就是PD控制的进阶版本，PD控制计算控制律的方法是线性加权的，而非线性反馈则先用了非线性函数对误差和误差的微分进行处理，之后再进行加权。 扩张观测器（ESO）是一个状态观测器，一般观测器仅观测系统的状态，只有输出和输出的导数（速度）。但是这里对输出的导数的导数（加速度）也进行了观测，这里也就是所谓的扰动，对扰动进行了观测。观测器的状态量也由此扩张了一维，因此叫做扩张观测器。（目前对观测器的认识还非常粗浅，欢迎大家批评指正。） 扰动补偿部分，也就是框图中间的红框部分。这里是整个ADRC的核心部分，说成是ADRC的灵魂也不为过。扰动补偿的核心思想就是要把原系统通过控制律设计改造成积分器级联的二阶系统，这样即使是最普通的PD控制器也可以对其进行控制。在这个改造过程中，补偿量u综合考虑了系统内部扰动、外部扰动、模型不确定性等等。这个补偿的原理也可以详细阅读韩老师的《从PID技术到"自抗扰控制"技术》，非常简单，但是非常高明。 注意，这里介绍的是线性ADRC的参数整定方式。ADRC分为三部分，跟踪微分器的参数整定比较容易，很多地方都有介绍，这里就不再细说了。这里主要介绍其余参数的整定方法。首先线性ADRC的公式罗列如下： e = z 1 ( k ) − y ( k ) z 1 ( k + 1 ) = z 1 ( k ) + h ⋅ ( z 2 ( k ) − β 01 e )

https://blog.csdn.net/handsome_for_kill/article/details/88398467

2. ADRC算法的程序实现：
ADRC与Matlab/Similink/C++实现_Amanda1m的博客-CSDN博客_adrc matlab
写在前面ADRC控制算法主要分为三部分，跟踪微分器TD、观测器ESO和控制器，其中观测器分为线性观测器LESO和非线性观测器NLESO，控制器分为线性控制器和非线性控制器NLSEF。目录一、理论分析1.1 跟踪微分器1.2 观测器ESO1.3 控制器二、Matlab仿真三、C++和Simulink联调一、理论分析1.1 跟踪微分器1.2 观测器ESO1.3 控制器二、Matlab仿真2.1 .m文件实现2.2 S-Fun...

https://blog.csdn.net/m0_37764065/article/details/108665429

3. 线性扩张状态观测器（LESO）详解：
线性扩张状态观测器详解
2020/4/24 更正 感谢评论区老哥指出问题 之前正文这个地方漏了bu 2020/4/9 更新1：正好写基金画了几幅高清无码大图，这回看图不伤眼了 更新2：代码放出来给大伙图一乐，自己F5一下比啥教程都好使 ...

https://zhuanlan.zhihu.com/p/108466027

4. 最速跟踪微分器（TD）详解：
ADRC学习笔记（二）_飞翔的柯南的博客-CSDN博客_fhan函数
添加链接描述 源码 1.最速跟踪微分器TD 它的离散表达式为： 参数中：V(t)是目标值h、h0为积分步长，一般来说h可以等于h0，但是为了减少超调和减少震荡，才把他们分开，一般h0比h大，比如大20倍。当h0较大时，能够明显减少震荡，所以也叫滤波因子。减小h可以抑制噪声放大作用。r为速度因子，值越大，逼近速度越快，但是最好根据实际被控对象的可承受能力而定。 表达式中：其中fhan函数第一种表达式为： fhan函数第二种表达式为： 其中fsg函数表达式为： 两种fhan函数是等价的。最速跟踪微分器的作用是起到一个缓冲作用，比如输入一个阶跃信号，它会让信号有一定斜率，让执行机构不会一启动就进入最大马力，从而导致第一次及后面几次逼近设定值时出现超调。它的可调参数与实际系统 2.扩张状态观测器 其中函数fal的表达式为： 它的另一种表达式为 3.状态误差反馈律 3.1线性反馈率 其中e1=x1-z1,e2=x2-z2,e0为e1的积分。 3.2非线性反馈率 然而这种线性组合不一定最好。大量仿真研究表明, 采用 e 0,e 1,e 2 的适当非线性组合效果更好, 我们通常采用的非 线性组合有如下两种形式: 完成的二阶自抗扰控制器算法如下 这里 r0,β01,β02,β03,r,c,h1,b0 是控制器的参数, 其中 r0是根据过渡 过 程 快 慢 的 需 要 和 系 统 的 承 受 能 力 来 决 定的 ;

https://blog.csdn.net/weixin_38291293/article/details/97100236

5. 非线性状态误差反馈（NLSEF）详解：
【ADRC/Matlab实现】非线性状态误差反馈NLSEF_Amanda1m的博客-CSDN博客_非线性adrc
三、Matlab仿真 3.2 simulink仿真 找到一种非线性的控制组合代替传统的PID控制器的线性组合，获得更有效的误差反馈控制率，只需将误差信号换成关于误差的非线性函数如fst函数（fhan函数）和fal函数等，可实现"小误差大增益，大误差小增益"的效果。 ...

https://blog.csdn.net/m0_37764065/article/details/108678922

6. LADRC仿真测试
LADRC的学习--换被控对象进行仿真测试_浪客.剑心的博客-CSDN博客_ladrc
这篇文章主要根据清华大学的硕士陈星写的论文：自抗扰控制器参数整定方法及其热工过程中的应用 进行学习。 参考文献为： [1] Zhiqiang Gao. Scaling and bandwidth-parameterization based controller tuning[P]. American Control Conference, 2003. Proceedings of the 2003,2003.   [2] 陈星. 自抗扰控制器参数整定方法及其在热工过程中的应用[D].清华大学,2008.   通过上一篇文章搭建了LADC的Simulink仿真模型，通过对这个被控对象，表达式如下：   可以在简单输入波形的情况下，比如正弦波，方波，三角波等，通过调节omega_c一个参数就能得到较好的控制效果。   如图所示： 方波： 正弦波： 三角波： 通过对结果分析得出结论： 1.模拟仿真对信号的跟踪可以有很好的效果，在快，准，稳这几个方面上都可以极大的满足要求； 2.只用在一定范围内对一个参数omega_c调节，就可以实现对信号的稳定跟踪。 但是在几个地方还没有弄懂： 1.omega_c的调节范围是多少； 2.对哪些信号起到作用； 3.可以应用在哪些被控对象上； 4.如何在LADRC的基础上再扩张，事项更广的应用； 5.LADRC本质是什么； 6.如何编程实现离散LADRC，把它应用在实际生活中。 等等问题，现在我对ADRC还有很多疑问，接下来继续学习扩展，并且深入理解。   这里主要引用陈星的硕士论文对LADRC进行理解并学习。   为了进一步改善PID控制器在强干扰及不确定系统中的控制效果，在非线性PID控制的基础上，提出自抗扰控制（ADRC）的概念。其中 核心思想是利用扩张状态观测器(ESO)去 实时地估计对象的"内扰"和"外扰"，然后利用控制律对总扰动进行补偿，从而使ADRC具有较强的抗扰动能力，并且能够获得较好的性能的鲁棒性。   一般来说，控制系统大家都希望通过数学建模的方法来得到模型，但是无论模型建立的多么精确，实际系统中总会存在未知的动态扰动。实际模型与标称模型的偏差可以看作系统的 内扰。还有一些外部扰动，不饶噪声或者控制量扰动，称为 外绕，控制系统的内扰和外绕为系统的 总扰动。为了获得好的控制结果， 采取办法估计系统扰动然后抑制或者消除扰动带来的影响，这就是ADRC的核心思想。    ADRC的特点及其问题：   1.

https://blog.csdn.net/qq_38169460/article/details/97616270

7. 总程序设计参考如下链接：
ADRC自抗扰控制，有手就行_suitcalaw的博客-CSDN博客_adrc
关于ADRC的优点本人不会赘述，毕竟作为一个ADRC算法都推导不出来的应用工程师，最希望看到的就是有手就行的操作方法。ARC的缺点就显而易见，就是参数多，一环ADRC大概就有11个参数，但一个粗略的效果很快就出来。本文所有的言论仅以我最近的一次速度闭环控制经验之谈，并没有经过大量的实验验证其绝对正确性，慎用（注：文中公式来自于csdn用户：遥远的乌托邦，有稍作修改）。 ...

https://blog.csdn.net/suitcalaw/article/details/110795934

简要概括为：

• 安排跟踪过程（跟踪微分器TD）：为了输入量不要有跳变，便于实际系统实时跟踪。

• 扩张观测器（ESO）或线性扩张观测器（LESO）：对输出的导数的导数（加速度）也进行了观测，这里也就是所谓的扰动，对扰动进行了观测。

• 非线性反馈扰动补偿（非线性状态误差反馈NLSEF）：把原系统通过控制律设计改造成积分器级联的二阶系统。补偿方法充分运用特殊的“非线性”效应，先用了非线性函数对误差和误差的微分进行处理，之后再进行加权。

控制过程：

• 安排过渡过程

• 估计状态和总扰动（ESO方程）

• 控制量的形成

优点：

• 在飞行器上的应用主要解决了快速性和超调之间的矛盾，实现了无反馈也能无静差的控制。

• 不存在鲁棒性。

• 算法简单，参数易于调节。

硬件设计：

芯片选型：

程序设计：

main.c 主函数程序：

1. 初始配置： 使用 STM32CubeMX 完成基本的IO端口配置、定时器配置、外部中断配置和系统时钟配置等。

2. 定时任务： 添加定时函数回调函数，用于控制电机内环、电机外环与数据交换，电机锁定与解锁 定时函数由 HAL_TIM_PeriodElapsedCallback 函数实现 2.5ms 中断一次。
注：尽量避免使每个任务的开始时刻为整数倍关系，这样可以尽量避免同一时刻执行多个任务，保证计时准确。

3. 主函数内循环任务： 按照上面的定时函数配置

任务执行条件	任务执行内容	任务说明
存在待处理的数据帧	处理遥控器发来的数据	ㅤ
2ms定时任务	IMU预处理 电机内环控制 发送高速数据 将发送缓冲区的数据填入DMA发送并清空发送缓冲区	内环控制采用自抗扰控制的方式
10ms定时任务	电机外环控制	外环控制采用比例控制的方式
100ms定时任务	遥控信号与蓝牙信号监测	ㅤ
500ms定时任务	LED闪烁控制	ㅤ

mpu6050.c 姿态数据处理：

1. 设置MPU6050的陀螺仪满量程范围： fsr: 0,±250dps; 1, ±500dps; 2,±1000dps; 3,±2000dps

2. 设置MPU6050的加速度传感器满量程范围： fsr:0,±2g; 1,±4g; 2,±8g; 3,±16g

3. 设置MPU6050的数字低通滤波器的低通滤波频率： lpf:数字低通滤波频率(Hz)

4. 设置MPU6050的采样率： 自动设置LPF为采样率的一半
注：Nyquist定理规定，使用大于2倍的最高信号频率采样才能保证信号的不混叠

5. 获取陀螺仪原始数据和加速度计原始数据 根据MPU6050手册可知，需要从以下寄存器中获取对应的传感器原始数据： Registers 67 to 72 – Gyroscope Measurements Registers 59 to 64 – Accelerometer Measurements

6. 初始化MPU6050 步骤如下：

adrc.c 自抗扰控制器设计：

1. 线性扩张状态观测器（LESO）
对比参考扩张状态观测器（ESO）作用：估计系统中存在的不确定性（即ADRC里面常说的的总扰动） 这里使用LESO而非ESO主要是为了解决了参数整定复杂的问题，减少了需要整定的参数数量，而LESO可以实现对总扰动的快速跟踪，从而为设计控制律进行补偿创造了可能。
对于理想二阶LESO系统，需要整定的参数有三个：，，
这里我们针对于内环的角速度和角加速度进行估计，由于获取的原始数据噪音较大，需要适当地调节采样周期实现尽量降低噪声的影响。
• 控制器固定参数有：A、B
• 控制器输入输出参数有：w（总扰动）、u（控制器最终输出）
• 需要从MPU6050中获取的运行参数有：
SpeEst（角速度的状态估计）、AccEst（角加速度的状态估计）
• 需要调节的参数有：、、，代表各扩张状态观测器的反馈增益
和1/h是同一个数量级，过大会带来振荡甚至发散
过小会带来发散，过大会产生高频噪声
过大会产生振荡；过小会降低跟踪速度
• 首先采用如下公式计算角速度观测误差：
角速度观测误差 = 输出角速度 - 角速度的状态估计量
• 然后更新角速度状态估计量
最新的角速度的状态估计量 = （之前获取的角速度的状态估计量 + 角速度观测误差 * ）* 控制周期
• 更新角加速度状态估计量
函数中已知A、B、C三个矩阵的数值：
最新的角加速度的状态估计量 = （B * 控制器最终输出 - A * 之前的角加速度的状态估计量 + 系统总扰动 + 角速度观测误差 * ）* 控制周期
• 更新系统总扰动
系统总扰动 = 角速度观测误差 * * 控制周期

2. 跟踪微分器（TD）
该部分作用：防止目标值突变而安排的过渡过程，产生跟踪信号和微分信号，滤除噪声。
采用离散系统最速控制综合函数（记为ADRC_fhan）作为跟踪微分器的核心函数，该函数作用是起到一个缓冲作用，使得状态变量可以快速跟踪上系统输入。
• 这里定义了ADRC的三个参量：
• adrcR：快速跟踪因子
• adrcH：滤波因子（系统调用步长）
• adrcD：控制微分量（H * H * R）
adrcR：r越大，快速性越好，但是容易超调和引发振荡。
adrcH：h越大，静态误差越小，则意味着刚开始带来的超调越小，初始误差越小；但会导致上升过慢，快速性不好。
（注意：滤波因子参数应大于控制周期T）
• 首先采用如下公式作为fhan函数（ADRC_fhan），为积分步长
fhan函数可以参考如下公式：
其中fsg函数表达式为：
• 更新输入的参数，h为积分步长
注意：这里可以用代替，解决最速跟踪微分器速度超调问题
• 更新输入的参数
• 输出更新后的参数

3. 非线性状态误差反馈（NLSEF）
该部分作用：找到一种非线性的控制组合代替传统的PID控制器的线性组合，获得更有效的误差反馈控制率，只需将误差信号换成关于误差的非线性函数如fst函数（fhan函数）和fal函数等，可实现“小误差大增益，大误差小增益”的效果。
 

调参过程：

需要调节的参数：

基本规律是：

经验就是：

1. 确保ADRC建模过程中没有错误

2. 确保输入的测试信号的幅值对你的被控对象是合理的

3. 慢悠悠调整参数

代码文件：

GitHub - XDU-Educational-UAV/Drone_Master_ADRC: Main control terminal of educational UAV (ADRC Version)

version V1.02 HEAD M2 ↑ M3 \ / \ / \ / / \ / \ / \ M1 M4 电机M1至M4分别为逆顺逆顺。 默认为姿态模式,可通过地面站进行更改。 尽量不改动自动生成的代码文件，用户添加的代码文件在user文件夹中，与自动生成的代码文件分离。 自动生成的代码文件中加入的用户代码包括: 用户代码文件中存在的底层代码包括: 变量:全部小写或首字母大写 xxx/Xxx/XxxXxx 函数:首字母大写加下划线分隔 Xxx_Xxx() 宏定义:全部大写加下划线分隔 XXX_XXX 锁定与解锁均需要上位机发送正确的指令。 起飞后（即油门超过一定值）2秒内未接收到正确的遥控信号或飞机姿态倾角过大，将进入失控保护模式尝试平稳降落，重新收到信号或姿态恢复正常则可随时恢复控制。 解锁后2秒内未接收到正确的遥控信号自动锁定。 起飞前建议进行传感器校准。

https://github.com/XDU-Educational-UAV/Drone_Master_ADRC