ADRC控制算法在多旋翼飞行器上的应用
ADRC控制算法在多旋翼飞行器上的应用
基础理论知识：
程序中涉及的部分知识点参考如下链接：
-
ADRC算法以及参数整定：
-
ADRC算法的程序实现：
-
线性扩张状态观测器（LESO）详解：
-
最速跟踪微分器（TD）详解：
-
非线性状态误差反馈（NLSEF）详解：
-
总程序设计参考如下链接：
简要概括为：
- 安排跟踪过程（跟踪微分器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
步骤如下：
adrc.c 自抗扰控制器设计：
在本程序中，自抗扰控制器设计简要分为以下几个部分：
-
线性扩张状态观测器（LESO）
对比参考扩张状态观测器（ESO）作用：估计系统中存在的不确定性（即ADRC里面常说的的总扰动）
这里使用LESO而非ESO主要是为了解决了参数整定复杂的问题，而LESO可以实现对总扰动的快速跟踪，从而为设计控制律进行补偿创造了可能。这里我们针对于内环的角速度和角加速度进行估计，由于获取的原始数据噪音较大，需要适当地调节采样周期实现尽量降低噪声的影响。
-
控制器固定参数有：A、B
-
控制器输入输出参数有：w（总扰动）、u（控制器最终输出）
-
需要从MPU6050中获取的运行参数有：
SpeEst（角速度的状态估计）、AccEst（角加速度的状态估计）
-
需要调节的参数有：beta1、beta2、beta3，代表各扩张状态观测器的反馈增益
β 1 \beta_{1} β1和1/h是同一个数量级，过大会带来振荡甚至发散
β 2 \beta_{2} β2过小会带来发散，过大会产生高频噪声
β 3 \beta_{3} β3过大会产生振荡；过小会降低跟踪速度
-
首先采用如下公式计算角速度观测误差：
x ~ = x − x ^ \tilde{x}=x-\hat{x} x~=x−x^
角速度观测误差 = 输出角速度 - 角速度的状态估计量
-
然后更新角速度状态估计量
最新的角速度的状态估计量 = （之前获取的角速度的状态估计量 + 角速度观测误差 * β 1 \beta_{1} β1）* 控制周期
-
更新角加速度状态估计量
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=⎣ ⎡000100010⎦ ⎤,B=⎣ ⎡0b0⎦ ⎤,E=⎣ ⎡001⎦ ⎤,C=[100]
最新的角加速度的状态估计量 = （B * 控制器最终输出 - A * 之前的角加速度的状态估计量 + 系统总扰动 + 角速度观测误差 * β 2 \beta_{2} β2）* 控制周期
-
更新系统总扰动
系统总扰动 = 角速度观测误差 * β 3 \beta_{3} β3* 控制周期
-
-
跟踪微分器（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=hx2y=x1+a0a1=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)
注意：这里可以用h0代替h，解决最速跟踪微分器速度超调问题 -
更新输入的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函数等，可实现“小误差大增益，大误差小增益”的效果。
调参过程：
以最简单的线性组合方法为例，大概有如下参数需要调节：
TD: δ h \delta h δh
ESO: B 01 B_{01} B01、 B 02 B_{02} B02、 B 03 B_{03} B03和观测器带宽 ω 0 \omega_{0} ω0
非线性反馈：（ β 1 \beta_{1} β1、 β 2 \beta_{2} β2）用 k p k_{p} kp和 k d k_{d} kd代替， α \alpha α
对于TD，一般的仿真模型 δ \delta δ可以尽量大一些，在100~500范围内基本相同，即使再大效果也基本不会有大的提升。h即仿真模型中的仿真步长。
ESO的三个参数和观测器带宽有关，依次设置为 3 w 0 3w_{0} 3w0、 3 w 0 2 3w_{0}^2 3w02、 w 0 3 w_{0}^3 w03就可以满足要求。
所以最终需要调节的参数只有四个： k p 、 k d 、 ω 0 、 α k_{p}、k_{d}、\omega_{0}、\alpha kp、kd、ω0、α。这时候就可以控制变量了。
基本规律是：
α \alpha α越小调节时间越短，但是过小会导致震荡。
ω 0 \omega_{0} ω0越小调节时间越长，震荡幅度越小。
k p k_{p} kp越大调节时间越短，震荡越大。
k d k_{d} kd效果不太明显，可在稳定后微调。
经验就是：
- 确保ADRC建模过程中没有错误
- 确保输入的测试信号的幅值对你的被控对象是合理的
- 慢悠悠调整参数
代码文件：
由于本工程还处于进行当中，欢迎各位贡献者参与其中共同完善。
有“AI”的1024 = 2048，欢迎大家加入2048 AI社区
更多推荐
5
0- 0
已为社区贡献12条内容
所有评论(0)