基础理论知识：
程序中涉及的部分知识点参考如下链接：
-
ADRC算法以及参数整定： 关于ADRC算法以及参数整定（调参）的一些心得体会_西涯先生的博客-CSDN博客_adrc控制算法 -
ADRC算法的程序实现： ADRC与Matlab/Similink/C++实现_Amanda1m的博客-CSDN博客_adrc matlab -
线性扩张状态观测器（LESO）详解： 线性扩张状态观测器详解 -
最速跟踪微分器（TD）详解： ADRC学习笔记（二）_飞翔的柯南的博客-CSDN博客_fhan函数 -
非线性状态误差反馈（NLSEF）详解： 【ADRC/Matlab实现】非线性状态误差反馈NLSEF_Amanda1m的博客-CSDN博客_非线性adrc -
总程序设计参考如下链接： ADRC自抗扰控制，有手就行_suitcalaw的博客-CSDN博客_adrc
简要概括为：
- 安排跟踪过程（跟踪微分器TD）：为了输入量不要有跳变，便于实际系统实时跟踪。
- 扩张观测器（ESO）或线性扩张观测器（LESO）：对输出的导数的导数（加速度）也进行了观测，这里也就是所谓的扰动，对扰动进行了观测。
- 非线性反馈扰动补偿（非线性状态误差反馈NLSEF）：把原系统通过控制律设计改造成积分器级联的二阶系统。补偿方法充分运用特殊的“非线性”效应，先用了非线性函数对误差和误差的微分进行处理，之后再进行加权。
控制过程：
- 安排过渡过程
- 估计状态和总扰动（ESO方程）
- 控制量的形成
优点：
- 在飞行器上的应用主要解决了快速性和超调之间的矛盾，实现了无反馈也能无静差的控制。
- 不存在鲁棒性。
- 算法简单，参数易于调节。
硬件设计：
芯片选型：
主控芯片：STM32G030
姿态传感器：MPU6050
蓝牙通信模块：JDY-32双模蓝牙模块
程序设计：
main.c 主函数程序：
主函数主要完成硬件相关的初始配置，添加定时任务并按照分配好的时间周期并行执行。
-
初始配置：
使用 STM32CubeMX 完成基本的IO端口配置、定时器配置、外部中断配置和系统时钟配置等。 -
定时任务：
添加定时函数回调函数，用于控制电机内环、电机外环与数据交换，电机锁定与解锁
定时函数由 HAL_TIM_PeriodElapsedCallback 函数实现 2.5ms 中断一次。
- 注：尽量避免使每个任务的开始时刻为整数倍关系，这样可以尽量> 避免同一时刻执行多个任务，保证计时准确。
-
主函数内循环任务：
按照上面的定时函数配置
| 任务执行条件 | 任务执行内容 | 任务说明 |
|---|
| 存在待处理的数据帧 | 处理遥控器发来的数据 | | | 2ms定时任务 | IMU预处理
电机内环控制
发送高速数据
将发送缓冲区的数据填入DMA发送并清空发送缓冲区 | 内环控制采用自抗扰控制的方式 | | 10ms定时任务 | 电机外环控制 | 外环控制采用比例控制的方式 | | 100ms定时任务 | 遥控信号与蓝牙信号监测 | | | 500ms定时任务 | LED闪烁控制 | |
mpu6050.c 姿态数据处理：
对于MPU系列的IMU传感器，可以使用DMP运动库，也可以使用原始传感器数据处理后使用。这里我们采用直接读取原始传感器数据的方式。
需要先设置传感器相关参数后再使用
-
设置MPU6050的陀螺仪满量程范围：
fsr: 0,±250dps; 1, ±500dps; 2,±1000dps; 3,±2000dps -
设置MPU6050的加速度传感器满量程范围：
fsr:0,±2g; 1,±4g; 2,±8g; 3,±16g -
设置MPU6050的数字低通滤波器的低通滤波频率：
lpf:数字低通滤波频率(Hz) -
设置MPU6050的采样率：
自动设置LPF为采样率的一半
- 注：Nyquist定理规定，使用大于2倍的最高信号频率采样才能保证信号的不混叠
-
获取陀螺仪原始数据和加速度计原始数据
根据MPU6050手册可知，需要从以下寄存器中获取对应的传感器原始数据：
Registers 67 to 72 – Gyroscope Measurements
Registers 59 to 64 – Accelerometer Measurements -
初始化MPU6050
步骤如下：
Delay_100ms
复位MPU6050
唤醒MPU6050
关闭所有中断
I2C主模式关闭
关闭FIFO
设置CLKSEL,PLL X轴为参考
使能加速度计与陀螺仪
设置加速度计与陀螺仪的满量程范围
设置器件的采样率
adrc.c 自抗扰控制器设计：
在本程序中，自抗扰控制器设计简要分为以下几个部分：
Input
跟踪微分器
TD
非线性状态误差反馈
NLSEF
Object
Output
线性扩张状态观测器
LESO
-
线性扩张状态观测器（LESO） 对比参考扩张状态观测器（ESO）作用：估计系统中存在的不确定性（即ADRC里面常说的的总扰动）
这里使用LESO而非ESO主要是为了解决了参数整定复杂的问题，而LESO可以实现对总扰动的快速跟踪，从而为设计控制律进行补偿创造了可能。 这里我们针对于内环的角速度和角加速度进行估计，由于获取的原始数据噪音较大，需要适当地调节采样周期实现尽量降低噪声的影响。
-
控制器固定参数有：A、B -
控制器输入输出参数有：w（总扰动）、u（控制器最终输出） -
需要从MPU6050中获取的运行参数有： SpeEst（角速度的状态估计）、AccEst（角加速度的状态估计） -
需要调节的参数有：beta1、beta2、beta3，代表各扩张状态观测器的反馈增益 bata1和1/h是同一个数量级，过大会带来振荡甚至发散 beta2过小会带来发散，过大会产生高频噪声 beta3过大会产生振荡；过小会降低跟踪速度 -
首先采用如下公式计算角速度观测误差：
x
~
=
x
?
x
^
\tilde{x}=x-\hat{x}
x~=x?x^ 角速度观测误差 = 输出角速度 - 角速度的状态估计量 -
然后更新角速度状态估计量 最新的角速度的状态估计量 = （之前获取的角速度的状态估计量 + 角速度观测误差 * beta1）* 控制周期 -
更新角加速度状态估计量
x
˙
=
A
x
+
B
u
+
E
h
y
=
C
x
\begin{aligned}\dot{x} &=A x+B u+E h \\y &=C x\end{aligned}
x˙y?=Ax+Bu+Eh=Cx? 函数中已知A、B、C三个矩阵的数值：
A
=
[
0
1
0
0
0
1
0
0
0
]
,
B
=
[
0
b
0
]
,
E
=
[
0
0
1
]
,
C
=
[
1
0
0
]
A=\left[\begin{array}{lll}0 & 1 & 0 \\0 & 0 & 1 \\0 & 0 & 0\end{array}\right], \quad B=\left[\begin{array}{l}0 \\b \\0\end{array}\right], \quad E=\left[\begin{array}{l}0 \\0 \\1\end{array}\right], \quad C=\left[\begin{array}{lll}1 & 0 & 0\end{array}\right]
A=???000?100?010????,B=???0b0????,E=???001????,C=[1?0?0?] 最新的角加速度的状态估计量 = （B * 控制器最终输出 - A * 之前的角加速度的状态估计量 + 系统总扰动 + 角速度观测误差 * beta2）* 控制周期 -
更新系统总扰动 系统总扰动 = 角速度观测误差 * beta3 * 控制周期
-
跟踪微分器（TD） 该部分作用：防止目标值突变而安排的过渡过程，产生跟踪信号和微分信号，滤除噪声。 采用离散系统最速控制综合函数（记为ADRC_fhan）作为跟踪微分器的核心函数，该函数作用是起到一个缓冲作用，使得状态变量可以快速跟踪上系统输入。
-
这里定义了ADRC的三个参量：
- adrcR：快速跟踪因子
- adrcH：滤波因子（系统调用步长）
- adrcD：控制微分量（H * H * R）
adrcR：r越大，快速性越好，但是容易超调和引发振荡。 adrcH：h越大，静态误差越小，则意味着刚开始带来的超调越小，初始误差越小；但会导致上升过慢，快速性不好。 （注意：滤波因子参数应大于控制周期T） -
首先采用如下公式作为fhan函数（ADRC_fhan），h0为积分步长
f
h
=
?fhan?
(
x
1
(
k
)
?
v
(
t
)
,
x
2
(
k
)
,
r
,
h
0
)
f h=\text { fhan }\left(x_{1}(k)-v(t), x_{2}(k), r, h_{0}\right)
fh=?fhan?(x1?(k)?v(t),x2?(k),r,h0?) fhan函数可以参考如下公式：
{
d
=
r
h
2
a
0
=
h
x
2
y
=
x
1
+
a
0
a
1
=
d
(
d
+
8
∣
y
∣
)
a
2
=
a
0
+
sign
?
(
y
)
(
a
1
?
d
)
/
2
a
=
(
a
0
+
y
)
f
s
g
(
y
,
d
)
+
a
2
(
1
?
f
s
g
(
y
,
d
)
)
f
h
a
n
=
?
r
(
a
d
)
f
s
g
(
a
,
d
)
?
r
sign
?
(
a
)
(
1
?
f
s
g
(
a
,
d
)
)
\left\{\begin{array}{l}d=r h^{2} \\a_{0}=h x_{2} \\y=x_{1}+a_{0} \\a_{1}=\sqrt{d(d+8|y|)} \\a_{2}=a_{0}+\operatorname{sign}(y)\left(a_{1}-d\right) / 2 \\a=\left(a_{0}+y\right) \mathrm{fsg}(y, d)+a_{2}(1-\mathrm{fsg}(y, d)) \\\mathrm{fhan}=-r\left(\frac{a}{d}\right) \mathrm{fsg}(a, d)-r \operatorname{sign}(a)(1-\mathrm{fsg}(a, d))\end{array}\right.
????????????????????d=rh2a0?=hx2?y=x1?+a0?a1?=d(d+8∣y∣)
?a2?=a0?+sign(y)(a1??d)/2a=(a0?+y)fsg(y,d)+a2?(1?fsg(y,d))fhan=?r(da?)fsg(a,d)?rsign(a)(1?fsg(a,d))? 其中fsg函数表达式为：
f
s
g
(
x
,
d
)
=
(
sign
?
(
x
+
d
)
?
sign
?
(
x
?
d
)
)
/
2
\mathrm{fsg}(x, d)=(\operatorname{sign}(x+d)-\operatorname{sign}(x-d)) / 2
fsg(x,d)=(sign(x+d)?sign(x?d))/2 -
更新输入的x1参数，h为积分步长
x
1
(
k
+
1
)
=
x
1
(
k
)
+
h
x
2
(
k
)
x_{1}(k+1)=x_{1}(k)+h x_{2}(k)
x1?(k+1)=x1?(k)+hx2?(k) -
更新输入的x2参数
x
2
(
k
+
1
)
=
x
2
(
k
)
+
h
f
h
x_{2}(k+1)=x_{2}(k)+h f h
x2?(k+1)=x2?(k)+hfh -
输出更新后的x1参数
-
非线性状态误差反馈（NLSEF） 该部分作用：找到一种非线性的控制组合代替传统的PID控制器的线性组合，获得更有效的误差反馈控制率，只需将误差信号换成关于误差的非线性函数如fst函数（fhan函数）和fal函数等，可实现“小误差大增益，大误差小增益”的效果。
代码文件：
GitHub - XDU-Educational-UAV/Drone_Master_ADRC: Main control terminal of educational UAV (ADRC Version)
由于本工程还处于进行当中，欢迎各位贡献者参与其中共同完善。
|