简介一份基于自抗扰控制ADRC的两轮自平衡车控制程序包面向自动化、机器人学方向的高校学生与嵌入式开发者旨在解决两轮车动力学建模、外部扰动抑制与实时平衡控制等工程问题。资源共22个文件压缩包仅35KB主体为C代码含11个头文件与9个源文件还附有工程说明文本与版本管理配置打开即可查看算法结构。代码按功能清晰拆分为ADRC控制器、PID控制器、扩张状态观测器ESO、卡尔曼滤波等核心算法模块并封装了陀螺仪、加速度计、电机、编码器等底层驱动接口便于二次开发与功能裁剪。已有483人学习下载。借助这套源码读者可以深入理解自抗扰控制相比经典PID在抗扰性和自适应性上的优势结合MATLAB/Simulink离线仿真进行参数整定再移植至Arduino、树莓派等硬件平台实现从模型验证到实物落地的全流程闭环非常适合作为课程设计、毕业设计或相关科研课题的参考起点。1. 用 MATLAB 从零搭 ADRC 平衡车先建模再谈抗扰两轮 AB 平衡车是少有的“模型失配”随处可见的环节加配重、轮胎打滑、重心偏移参数马上变。ADRC 不去精确辨识全部参数而是把未能建模部分合成一项总扰动由扩张状态观测器在线估计。这篇文章给出 MATLAB 中复现 ADRC 平衡车的完整路径从简化动力学到二阶模型再到 TD、ESO 与非线性反馈的离散实现最后用闭环脚本完成参数整定与抗扰验证。适合已用 PID 或 LQR 完成直立、面对负载变化想少改增益的人也适合刚学自抗扰、拿不准代码该写在 Simulink 还是脚本里的 MATLAB 用户。代码只用基础 MATLAB 就能跑。2. ADRC 平衡车的动力学模型化简与 MATLAB 状态空间表达2.1 为什么 ADRC 平衡车只需要一个二阶主导模型自抗扰不拒绝模型它把模型切成“知道的部分”和“不知道的部分”。对平衡车而言核心量是倾角 θ 和倾角角速度 θ̇车轮加速度可以直接由电机速度环给定。忽略电机动态和车轮滑移后小角度范围的动力学可以写成θ̈ (g/l)·θ - (b/l)·θ̇ (1/l)·a_w d其中 g 是重力加速度l 是车体质心到轮轴距离b 是等效粘滞阻尼a_w 是车轮加速度d 是外部力矩换算出来的等效加速度扰动。把控制输入记为 ua_wADRC 再做一个关键改写把“可以确定的那部分增益”记为 b0剩余全部误差都合并为 f变成θ̈ f b0·u这里 f 包含了未被建模的重力非线性、摩擦、重心偏移、外部扰动甚至还包括 b0 与实际 1/l 的偏差。这样的二阶写法就是 ESO 的立足点u 进模型的系数只需要一个近似值 b0。为什么近似也能成立因为 z3 把 f 在线看住了这是 ADRC 平衡车和 LQR 在思路上最大的不同LQR 把模型误差当成性能损失ADRC 把它当成可观测状态。2.2 在 MATLAB 里建立最小状态空间对象下面代码给出一个最小被控对象。注意这里的输入是车轮加速度不是 PWM如果你手上是实物建议让底盘先跑一个内环速度控制器把输入量统一成加速度指令。% balance_model.m 建立平衡车最小线性模型 g 9.81; % 重力加速度m/s^2 l 0.10; % 质心到轮轴距离m b 0.03; % 等效粘滞阻尼N*m*s/rad b0 1/l; % ADRC 名义控制增益后文可以偏离 % 状态 x [theta; theta_dot] % 输入 u 车轮加速度 A [0 1; g/l -b/l]; B [0; 1/l]; C eye(2); D [0; 0]; sys ss(A, B, C, D); fprintf(开环极点: %.3f, %.3f\n, real(eig(A))); fprintf(能控性矩阵秩: %d\n, rank(ctrb(sys))); fprintf(能观性矩阵秩: %d\n, rank(obsv(sys)));上述ss建立了连续时间状态空间对象ctrb和obsv分别返回能控性、能观性矩阵。开环极点里必然有一个正实部因为车体在不控制时像倒摆一样向外倒下两个秩都满秩意味着状态可测、输入可到ADRC 的“扩张”才有基础。l 和 b 看似简单实际里 l 往往是最大误差来源建议把它写成变量而不是字面常量。表平衡车仿真常用参数参数符号取值范围备注质心到轮轴距离l0.08~0.15 m直接影响 b0载人车会更大等效阻尼b0.01~0.10 N·m·s/rad摩擦和粘滞的综合结果控制周期dt0.0005~0.002 s实物 ECU 常取 1 ms加速度输入上限u_max2~5 m/s²由电机力矩和轮胎附着决定这个表里的u_max在 ADRC 里也重要因为 b0 取得过大时补偿量z3/b0会在限幅处憋住输出曲线出现方波振荡。这些参数不必一开始就精确先按表中间值给之后用脚本扫描。2.3 模型校验先看开环不稳定再决定用 ESO把状态空间对象搭好后最值得看的是开环初始响应。给 0.05 rad约 2.9°的初始倾角车应该在 0.5 秒内迅速倒下这个发散速度决定了对控制周期和传感器延迟的容忍度。figure(Color, w); initial(sys, [0.05; 0], 2); grid on; xlabel(Time (s)); ylabel(Angle (rad)); title(开环角度发散);曲线发散得越快说明需要的观测器带宽越高。如果 l0.1 m发散时间常数大约是 sqrt(l/g)0.1 s也就是传感器必须能在 10 ms 内给出可信角度否则 ESO 会把采样延迟当成扰动的一部分观测相位滞后。能在 MATLAB 里提前看到这层约束总好过上车以后拿示波器一遍遍试。由于能观性矩阵秩是满的理论上用编码器和 IMU 的角度、角速度足够恢复全部状态但这不等于可以直接用卡尔曼滤波ADRC 里的 z3 会把传感器零偏和车体倾斜混在一起所以实物中仍然要先把角速度零偏处理掉。模型完成了下一章进入 ADRC 的三个可执行组件。3. ADRC 平衡车核心三件在 MATLAB 里的离散实现TD、ESO 与 NLSEF3.1 跟踪微分器在平衡车上的正确使用位置把 ADRC 拆开就是跟踪微分器、扩张状态观测器、非线性状态误差反馈三个块。TD 负责把参考信号变成平滑过渡ESO 负责把模型误差与外部扰动合成 z3NLSEF 负责给出控制增量。很多人在平衡车上不加 TD因为直立控制的角度参考是常数 0加了 TD 反而把 0 变成一条缓慢趋近的曲线恢复速度变慢。TD 应该放在外环也就是速度或位置控制环。当目标位置是一个阶跃信号时位置控制器直接拿阶跃做误差会得到一个大加速度。TD 先把这个阶跃平滑成一条 S 形轨迹再让内环去追。线性 TD 的实现非常简单r 控制平滑速度% td_update.m 线性跟踪微分器 % x1 跟踪 vx2 是 x1 的导数 function [x1, x2] td_update(x1, x2, v, r, h) x2 x2 h * (-r^2 * (x1 - v) - 2 * r * x2); x1 x1 h * x2; end这段代码只在每个控制周期执行一次。r加大时过渡过程变短但它本质是加速度上限的软限制不能随便设太大否则和平移阶跃没有区别。做平衡车位置控制时我一般先把 r 放在 2~4看小车起步是不是有可接受的加速度如果电机发出尖叫就把 r 减半。h是控制周期和后面所有离散函数共用同一时间基准。3.2 扩张状态观测器z3 究竟在观察什么ESO 的 z1 跟踪倾角 θz2 跟踪角速度 θ̇z3 跟踪总扰动 f。输入端只有两个量测量 y 和控制 u。它不关心扰动来自重力、摩擦还是外界推力只要这个扰动在状态方程里影响 θ̈z3 就有机会估计出来。以下是离散 ESO 的标准写法% fal.m 韩京清 fal 函数 function f fal(e, alpha, delta) if abs(e) delta f e / delta^(1 - alpha); else f sign(e) * abs(e)^alpha; end endfal 函数在小误差附近用线性段避免原点斜率无穷大在大误差时用幂次压缩减弱噪声影响。α 通常在 0.25~0.75 之间取δ 取 0.01~0.05。接下来把它套进 ESO% eso_update.m 扩张状态观测器一步更新 function z eso_update(z, y, u, h, p, b0) % z [角度估计; 角速度估计; 总扰动估计] e z(1) - y; z(1) z(1) h * (z(2) - p.beta1 * e); z(2) z(2) h * (z(3) - p.beta2 * fal(e, p.alpha1, p.delta) b0 * u); z(3) z(3) h * (-p.beta3 * fal(e, p.alpha2, p.delta)); endz1 表达式里只有测量误差反馈z2 里加入了 b0*u意味着模型已经知道自己输出了多少控制量z3 里没有 u 项它需要靠 z2 的误差反推扰动。如果 u 没有反馈到 ESOz3 会把控制作用也当成扰动输出会有明显直流平移。这在接线的 S-Function 中非常常见所以独立验证时先把 z3 稳定值打印出来。β 用带宽法确定表ESO 参数初始值与作用参数表达式初值范围作用β13ω₀30~60角度估计收敛速度β23ω₀²200~1000角速度估计平滑度β3ω₀³500~4000扰动估计响应速度ω₀观测器带宽8~20 rad/s三条 β 的总标尺3.3 非线性反馈与扰动补偿u 不再只靠误差去凑控制律分成两步。第一步用状态误差生成一个无扰动时的控制量 u0第二步把 z3 的影响直接扣除。写成代码如下% adrc_ctrl.m ADRC 控制律 function [u, u0] adrc_ctrl(z, ref, p, b0) e1 ref(1) - z(1); e2 ref(2) - z(2); u0 p.kp * fal(e1, p.alpha3, p.delta) p.kd * fal(e2, p.alpha4, p.delta); u (u0 - z(3)) / b0; end这里z(3)/b0是补偿项它占总控制的比重远大于 u0尤其是在持续受力的斜坡路面或滑坡工况。由于 z3 在起步阶段是 0初始控制主要由 kp、kd 提供所以 b0 的符号必须和实际模型一致。按照通常定义b0 取 1/l 为正。如果你用的被控对象输入是 PWM 而非加速度b0 的单位和数值都会变化这也是参数失配测试关注的目标。4. 在 MATLAB/Simulink 中把 ADRC 平衡车闭环跑起来主循环与参数整定4.1 最小闭环脚本先把整条链路在一个循环里走通下面这段代码将前面三个函数拼成闭环。被控对象仍然使用简化二阶模型并加一个正弦扰动模拟路况变化。这里使用欧拉法控制周期取 1 ms硬件上已经接近主流单片机的主循环节奏。% main_loop.m clear; clc; g 9.81; l 0.10; b 0.03; b0 1/l; p.beta1 24; p.beta2 192; p.beta3 512; p.alpha1 0.5; p.alpha2 0.25; p.alpha3 0.5; p.alpha4 0.25; p.delta 0.01; p.kp 20; p.kd 10; dt 0.001; T 3; n round(T / dt); x [0.1; 0]; % 初始倾角 0.1 rad z [0; 0; 0]; u 0; logbuf zeros(n, 3); for k 1:n t (k-1) * dt; z eso_update(z, x(1), u, dt, p, b0); [u, ~] adrc_ctrl(z, [0; 0], p, b0); d 0.2 * sin(5 * t) 0.1 * (t 2.0); x2_dot g/l * x(1) - b/l * x(2) b0 * u d; x(1) x(1) dt * x(2); x(2) x(2) dt * x2_dot; logbuf(k, :) [x(1), x(2), z(3)]; end figure(Color, w); plot((0:n-1)*dt, logbuf(:,1)*180/pi, LineWidth, 1.2); grid on; xlabel(Time (s)); ylabel(Angle (deg)); title(ADRC 平衡车角度响应);注意循环顺序先 ESO 更新再算控制律最后积分被控对象。如果反过来控制量用的还是上一拍测量会在 z3 变化剧烈的瞬间造成一拍额外延迟。1 ms 步长下这样还不致命到 5 ms 步长时就会出现明显相位滞后。提示欧拉积分在 dt1 ms、频率低于 10 Hz 的刚体模型上是够用的但换到带柔性轮或弹性联轴器的平台建议把被控对象换成ode45积分或 RK4。4.2 Simulink 中的模块接线与 3 个高频错误Simulink 做 ADRC 的优点是调试 z3 方便缺点是很多人把反馈线拉错。常见做法是控制器用一个 MATLAB Function 块观察z作为状态y 从 IMU 角度接口来u 输出给底层速度环。被控对象也可以直接用刚才的模型封装成一个 S-Function。关键接线顺序与脚本保持一致ESO 块的输出连着控制律块的输入控制律块的输出再分两路一路进被控对象另一路回到 ESO 的 u 端。最容易犯的错误有三个一是角度单位不一致把陀螺仪积分得到的 rad 后处理成角度又没在 ESO 里改回弧度b0 差 57 倍二是把 ESO 的观测值 z1 直接接到角度反馈而不是测量值 y导致整个反馈变成了观测自锁不能发现传感器故障三是 Simulink 变步长求解器与控制模块的采样周期不统一窗口放大后 u 波形是台阶状z3 是毛刺状。先在脚本里把逻辑跑对再迁到 Simulink 生成代码能省掉一大半这类问题。4.3 参数整定顺序先观测器再控制律最后动 b0ADRC 收敛性看起来只和 β 有关但整套控制系统需要按顺序调。先用 b01/l 固定kp、kd 设到能让 0.1 rad 初始倾角回正的程度然后加扰动看 z3 是否跟踪最后才处理 b0 偏移。没有这个顺序参数耦合会让前 5 步都在互相掩盖问题。表ADRC 平衡车参数整定表参数初始值加大后的效果过大的风险r2~4外环跟踪更快电机加速度达到上限ω₀8~20z3 跟踪更快噪声放大kp20~40角度回正更硬低频振荡kd8~15阻尼更强高频抖b00.5~1.5 倍名义值补偿强度变化控制饱和与高频振铃kd 是平衡车最容易出现“控制量抖”的源头。角度反馈经过 ESO 后已经比直接差分平滑如果再叠加高频脚步扰动kd 大大会让 PWM 在正负之间来回变化。碰到这种情况先看 u 的曲线如果抖动频率和传感器噪声中心频率相近衰减 kd 比加滤波更有效。5. 用 MATLAB 验证 ADRC 平衡车的抗扰边界阶跃扰动与参数失配测试5.1 用同一个循环跑三组工况量化 ISE把 main_loop.m 的循环体包一层for case_id 1:3分别做理想模型、阶跃扰动、b0 失配三种测试。每次更改的条件写在独立行里方便回放。for case_id 1:3 b0 1/l; if case_id 2 d 0.2 * (t 2); % 2 秒后加常值扰动 elseif case_id 3 b0 0.5 / l; % 模拟质心高度变化 d 0.2 * (t 2); end % 循环内记录 angle 并计算 ISE ise trapz(t_log, angle_log(:, 1).^2); end三组结果并排画在同一张图上就能看出 z3 对 b0 失配的补偿能力。推荐记录三个指标最大倾角、恢复时间、ISE。如果 case_id3 的最大倾角已经比 case_id2 大一倍说明 ω₀ 没有覆盖这个失配量再调 b0 之前先把观测器带宽升上去。此时不要靠 kp 硬压否则会掩盖 b0 偏差换到实车时一受载就暴露。5.2 用 z3 与扰动的对比判断 ESO 是否“看对”了z3 是总扰动估计不是某个物理量的独立估计所以对比时把真实扰动 d 和 z3 画在同一个坐标系里z3 应在一个控制周期后靠近 d 的包络。如果 z3 的直流中心偏移小于 10%就说明 b0 的符号与幅值方向正确如果 z3 出现明显振荡条纹则把 β3 降低一个数量级再跑。实际在硬件中 z3 还会包含陀螺零偏这会表现为一个常数偏移可以在控制律里额外加一个零点校准或者让 ESO 里的 α1α21退化成线性 ESO 去观察它。验证到这一步ADRC 平衡车的参数边界基本就清楚了。保存仿真参数为状态文件后嵌入式代码里只保留一个load参数结构体的接口后续联调时直接更新参数不用重新编译逻辑。我一般还会把 3 秒内的 u 饱和次数打印出来超过 5 次就说明加速度上限接近极限所有参数都要往保守方向回退。本文还有配套的精品资源点击获取