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

时间：2023-07-29 17:37:01 fsg电位器an1575z51

基础理论知识：

参考以下链接：

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倍的最高信号频率，以确保信号不混合
陀螺仪的原始数据和加速度计的原始数据
根据MPU根据6050手册，相应的传感器原始数据需要从以下寄存器中获取：
Registers 67 to 72 – Gyroscope Measurements
Registers 59 to 64 – Accelerometer Measurements
初始化MPU6050
步骤如下：

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

在本程序中，自抗扰控制器设计简要分为以下几个部分：

线性扩张状态观测器（LESO）

对比参考扩张状态观测器（ESO）作用：估计系统中存在的不确定性（即ADRC里面常说的的总扰动）
这里使用LESO而非ESO主要是为了解决了参数整定复杂的问题，而LESO可以实现对总扰动的快速跟踪，从而为设计控制律进行补偿创造了可能。

这里我们针对于内环的角速度和角加速度进行估计，由于获取的原始数据噪音较大，需要适当地调节采样周期实现尽量降低噪声的影响。

控制器固定参数有：A、B
控制器输入输出参数有：w（总扰动）、u（控制器最终输出）
需要从MPU6050中获取的运行参数有：

SpeEst（角速度的状态估计）、AccEst（角加速度的状态估计）
需要调节的参数有：beta1、beta2、beta3，代表各扩张状态观测器的反馈增益

bata1和1/h是同一个数量级，过大会带来振荡甚至发散

beta2过小会带来发散，过大会产生高频噪声

beta3过大会产生振荡；过小会降低跟踪速度
首先采用如下公式计算角速度观测误差：

$\tilde{x}=x-\hat{x}$

角速度观测误差 = 输出角速度 - 角速度的状态估计量
然后更新角速度状态估计量

最新的角速度的状态估计量 = （之前获取的角速度的状态估计量 + 角速度观测误差 * beta1）* 控制周期
更新角加速度状态估计量

$\begin{aligned}\dot{x} &=A x+B u+E h \\y &=C x\end{aligned}$

函数中已知A、B、C三个矩阵的数值：

$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]$

最新的角加速度的状态估计量 = （B * 控制器最终输出 - A * 之前的角加速度的状态估计量 + 系统总扰动 + 角速度观测误差 * beta2）* 控制周期
更新系统总扰动

系统总扰动 = 角速度观测误差 * beta3 * 控制周期

跟踪微分器（TD）

该部分作用：防止目标值突变而安排的过渡过程，产生跟踪信号和微分信号，滤除噪声。

采用离散系统最速控制综合函数（记为ADRC_fhan）作为跟踪微分器的核心函数，该函数作用是起到一个缓冲作用，使得状态变量可以快速跟踪上系统输入。
- 首先采用如下公式作为fhan函数（ADRC_fhan），h0为积分步长
  
  $h=\text { fhan }\left(x_{1}(k)-v(t), x_{2}(k), r, h_{0}\right)$
  
  fhan函数可以参考如下公式：
  
  $\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.$
  锐单商城拥有海量元器件数据手册、IC替代型号，打造电子元器件IC百科大全！

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

芯片选型：

相关文章