简介面向自动控制与机器人方向的两轮自平衡车非线性控制难题ADRC平衡车/AB平衡车压缩包提供了基于自抗扰控制ADRC算法的C与MATLAB源码可直接用于倒立摆建模、控制器设计与仿真调试适合本科高年级及工程技术人员快速上手。包体共22个文件含11个头文件、9个C源文件及文本说明源码覆盖ADRC核心、扩张状态观测器、PID、陀螺仪、编码器、电机驱动等模块压缩包仅35KB目录沿用ADRC-master结构轻量易读代码注释可辅助理解算法细节。已有483人学习。借助源码可对照理解ADRC如何估算并抑制系统内外干扰结合仿真环境验证平衡车控制参数并可将控制逻辑移植到Arduino等嵌入式平台有助于掌握非线性鲁棒控制的工程实现思路对深入学习先进控制理论有直接参考价值。1. 自抗扰平衡车源码包从EV3的LMS2012到MATLAB仿真的完整链路两轮平衡车的控制难点不在让它站起来而在站起来之后扛得住扰动。这个ADRC平衡车源码包里TWIP是Two-Wheel Inverted Pendulum的缩写bytecodes.h、lms2012.h、lmstypes.h这几个头文件暴露了它的运行平台——LEGO EV3而ADRC.cpp和ESO.cpp则是控制核心。它走的不是常见的PID加串级方案而是韩京清提出的自抗扰控制用扩张状态观测器把重力非线性、摩擦、重心偏移、电机增益变化统一打包成一个总扰动实时估计并补偿掉。整套代码是C写的配合工程化的CMakeLists.txt可以直接交叉编译MATLAB端的仿真脚本负责模型搭建和参数整定。适合正在做平衡车课程设计、想把两轮小车控制从PID换成ADRC、或者要在MATLAB里复现自抗扰控制效果的人。源码里从传感器融合到控制输出是一条完整的闭环比单独看ADRC论文容易落地得多。2. ADRC核心构成解析TD、ESO与NLSEF在两轮倒立摆模型上的落点2.1 为什么PID在两轮车上容易失效先把问题定性平衡车本质是倒立摆车体倾斜角θ的线性化动力学是θ (g/L)·θ b·u f。这里g/L项是重力引起的正反馈——角度越大倒得越快这是系统本身的不稳定极点PID要做的不是抑制而是对冲。传统PID用比例项顶住重力用微分项提供阻尼但有两个现实问题。第一b控制增益随电池电压、负载变化PID按固定增益设计电压一掉电机力矩变软Kp就显得过大车体开始高频抖动。第二摩擦和重心偏移是慢变扰动PID只能靠积分项慢慢攒攒多了就超调车体来回晃。实际调车的时候PID通常只能调成能站但推一下就倒的状态再提升鲁棒性就得牺牲响应速度。ADRC的出发点完全不同不去精确建模g、L、b到底是多少而是把模型偏差、外部推力和未建模动态合并成一个总扰动f_total用扩张状态观测器在线估计然后在控制量里反向补偿。这就是它能比PID少调很多参数、又比LQR抗模型失配的原因。2.2 扩张状态观测器ESO把不确定性变成可观测状态ESO的设计思路很直接。把二阶系统改写成状态方程x1 x2x2 f_total b0·ux3 f_totalx3 h扰动变化率其中x3就是扩张出来的状态。只要扰动变化率h有界观测器就能跟踪x3。观测器本身用输出y融合后的角度和输入u电机力矩来驱动状态更新。源码包里的ESO.cpp核心是离散迭代// ESO.cpp 离散更新dt为控制周期y为实测角度u为上一个周期的控制量 void ESO::update(float y, float u) { float e z1 - y; // 角度估计值与实测值之差 z1 dt * (z2 - beta1 * e); // 状态1角度估计 z2 dt * (z3 - beta2 * fal(e, 0.5f, 0.02f) b0 * u); // 状态2角速度估计 z3 dt * (-beta3 * fal(e, 0.25f, 0.02f));// 状态3总扰动估计 }这里fal是非线性函数作用是小误差高增益、大误差低增益避免微分冲击float ESO::fal(float e, float alpha, float delta) { if (fabs(e) delta) return e / powf(delta, 1.0f - alpha); // 线性区防抖 return (e 0 ? 1.0f : -1.0f) * powf(fabs(e), alpha); }beta1、beta2、beta3是观测器增益。常见做法是用高志强的带宽参数化避免逐个试beta1 3·wobeta2 3·wo²beta3 wo³wo是观测器带宽。wo越大对扰动的估计越快但对传感器噪声也越敏感。在两轮车上wo从20起步配合20ms的控制周期一般到50就是极限。2.3 平衡车模型阶次与TD的作用平衡车直立控制只需要角环用二阶ADRC就够x1是倾角x2是倾角速度x3是总扰动。不需要把车轮位置拉进来做三阶除非你要做位置闭环。源码包里ADRC.cpp的控制器输出形式是典型的PD 扰动补偿// ADRC.cpp 控制律ref为期望角度直立时为0 float ADRC::control(float ref, float y) { float e1 ref - z1; // 角度误差 float e2 0.0f - z2; // 角速度误差目标角速度为0 float u0 kp * e1 kd * e2; // 线性状态误差反馈 u (u0 - z3) / b0; // 减去扰动估计除以控制增益 return u; }至于跟踪微分器TD它的作用是给期望值安排过渡过程防止阶跃给定带来的超调。在纯直立任务里ref恒为0TD基本闲置只有当你做遥控前进后退或者原地转向时TD才用来平滑速度给定。源码包的ADRC.h里保留了TD接口这是很务实的取舍——不用的模块不占控制周期。参数z1、z2、z3来自ESO而不是传感器实测这带来一个实际问题如果ESO收敛太慢控制律用的是过期的状态车体会先倒下去再被拉回来。所以整定顺序永远是先调wo让z1贴合实测角度再调kp和kd最后才碰b0。b0是对真实控制增益b的粗估ESO会把误差(b - b0)·u吸收进总扰动因此b0偏大偏小一两倍都能工作这是ADRC在工程上最讨喜的一点。3. 源码逐模块拆解ESO.cpp、Gyroscope.cpp与TWIP.cpp的数据协作3.1 工程结构与数据流解压后能看到ADRC-master下是src目录加CMakeLists.txt.gitmodules说明它还依赖子模块。核心文件可以分成四组控制ADRC.cpp、ESO.cpp、PID.cpp、传感Gyroscope.cpp、Accelerometer.cpp、Encoders.cpp、融合KalmanFilter、整车装配TWIP.cpp、Motor.cpp、main.cpp。数据流是一条单向前向链传感器采集 → 滤波融合 → 状态估计 → 控制律 → 电机输出。main.cpp里通常在定时中断或高优先级线程里按固定周期调用TWIP的update接口。// main.cpp 主循环伪结构 int main() { TWIP twip; // 整车对象内部持有ESO与ADRC实例 twip.init(); // 传感器校准与控制参数初始化 while (running) { sleep_ms(20); // 50Hz控制周期 twip.update(); // 采集-融合-控制-输出一条龙 } }50Hz对两轮车是偏低的倒立摆自然频率大约1.5Hz控制频率至少是它的20倍。如果EV3的Linux调度抖动明显常见做法是传感器读取放一个线程、控制计算放另一个线程用带锁的共享变量交换数据或者换RT补丁。源码包不做这层抽象移植的时候得知道瓶颈在哪。3.2 传感器融合Gyroscope Accelerometer KalmanFilterGyroscope.cpp读的是角速度Accelerometer.cpp读的是三轴加速度。单独用陀螺仪积分角度零偏漂移会让车体缓慢倒向一边单独用加速度计atan2算出的角度在电机振动下噪声很大微分之后根本没法用。常见做法是用一维卡尔曼滤波融合状态就是角度和陀螺零偏两个量// KalmanFilter 预测更新Q_angle和Q_bias是过程噪声方差 void KalmanFilter::update(float acc_angle, float gyro_rate, float dt) { angle (gyro_rate - bias) * dt; // 用角速度积分做预测 P00 dt * (dt * P11 - P01 - P10 Q_angle); P01 - dt * P11; P10 - dt * P11; P11 Q_bias * dt; float y acc_angle - angle; // 加速度计角度作观测 float S P00 R_angle; float K0 P00 / S; float K1 P10 / S; angle K0 * y; // 更新角度 bias K1 * y; // 在线估计零偏 P00 - K0 * P00; P01 - K0 * P01; P10 - K1 * P00; P11 - K1 * P01; }注意这里的角度观测来自加速度计在静止或慢变时刻的输出振动剧烈时加速度计混入高频分量卡尔曼的R_angle如果设得太小融合结果会把振动带进去。实车调试时R_angle从0.05起调Q_angle在0.001量级Q_bias在0.003量级这个组合在大多数两轮车上都能压住零偏又不太吃噪声。3.3 控制闭环与电机输出Encoders.cpp负责读轮子编码器计算轮速。EV3上bytecodes.h提供的直接命令比抽象库少一层封装适合控制周期严格的情况。Motor.cpp拿到ADRC输出的占空比后映射到EV3电机命令。这里有个容易忽略的细节电机指令必须限幅ADRC输出的u在扰动大时会瞬间冲到饱和值不限幅的话电机会堵转发热编码器读数也会丢步。// Motor.cpp 限幅输出 float Motor::apply(float cmd) { if (cmd MAX_PWM) cmd MAX_PWM; if (cmd -MAX_PWM) cmd -MAX_PWM; set_power(cmd); // 底层走EV3的motor命令 return cmd; // 返回实际输出供ESO使用 }这个返回值很关键ESO里模型项用的是b0·uu必须填实际作用到电机上的值而不是限幅前的控制量。如果拿未限幅的值喂给ESO扰动估计会在饱和期间严重失真车体表现为推一下之后回不来。这也是源码包把限幅放在Motor::apply而不是ADRC::control里的原因之一。3.4 CMakeLists.txt与跨平台编译CMakeLists.txt用标准cmake语法组织target是嵌入式可执行文件而不是库。在ev3dev上编译时工具链指向arm-linux-gnueabi-g并且要链接ev3dev库才能解析lms2012.h里的函数mkdir build cd build cmake .. -DCMAKE_TOOLCHAIN_FILE../toolchain-ev3.cmake make -j4如果只改控制参数不动硬件抽象最省事的做法是直接在EV3上装g把CMakeLists.txt的target_link_libraries改成pthread和ev3dev-c。控制参数集中在ADRC.h和ESO.h顶部的宏定义里编译一次刷进去就能试不用每次重连传感器。4. MATLAB/Simulink两轮平衡车仿真ADRC参数整定与LQR对比4.1 在MATLAB里搭两轮倒立摆模型仿真的第一步不是写控制器而是把被控对象写对。两轮平衡车在直立状态下车体绕轮轴的动力学可以简化成θ (g/L)·sin(θ) b·u d其中L是质心到轮轴的距离b是控制增益d代表外部推力或重心偏移。MATLAB里直接写离散循环比搭Simulink模块更能看清每个状态的变化%% 两轮平衡车 ADRC 仿真主脚本 clear; clc; % 物理参数L为质心高度g重力加速度b0为控制器使用的增益估计 L 0.06; g 9.8; b0 80; dt 0.01; t 0:dt:8; N length(t); % 系统真实参数故意让实际增益与b0不一致模拟模型失配 b_real 65; % 真实增益比估计值小 theta zeros(1,N); w zeros(1,N); % ESO 状态初始化扩张状态 z3 估计总扰动 z1 0; z2 0; z3 0; % 控制器参数wo观测器带宽wc控制器带宽 wo 30; wc 8; beta1 3*wo; beta2 3*wo^2; beta3 wo^3; u 0; for k 1:N-1 % 被控对象真实模型含外部扰动 d d 0.6 * sin(2*t(k)); % 周期推力扰动 theta_2dot (g/L)*sin(theta(k)) b_real*u d; w(k1) w(k) theta_2dot * dt; theta(k1) theta(k) w(k1) * dt; % ESO用输出theta和上一拍控制量u估计状态 e z1 - theta(k1); z1 z1 dt * (z2 - beta1 * e); z2 z2 dt * (z3 - beta2 * fal(e,0.5,0.02) b0*u); z3 z3 dt * (-beta3 * fal(e,0.25,0.02)); % 控制律直立目标角度和角速度期望都为0 u0 wc^2 * (0 - z1) 2*wc * (0 - z2); u (u0 - z3) / b0; u max(-100, min(100, u)); % 输出限幅 end脚本里fal函数单独放在一个m文件里参数alpha和delta与源码包ESO.cpp一致这样仿真和实车用的是同一套非线性观测器。仿真时把b_real设成65而控制器里用80就是为了验证ESO能否吃掉模型失配——这是ADRC相对LQR的核心卖点。4.2 ADRC参数整定顺序与带宽法仿真里只有三个参数要调wo、wc、b0。推荐顺序是b0先按物理量粗估电机力矩除以转动惯量量级对上就行然后固定wc6、wo20跑一版看曲线再逐步加压。下表是两轮车模型上常用的参考范围。参数含义参考范围调大后的表现b0控制增益估计50100偏大响应变钝、低频抖动wo观测器带宽2050偏大扰动估计快但噪声放大wc控制器带宽512偏大回正快、易高频抖动观察两条曲线就够了theta曲线看超调量和收敛时间z3曲线看扰动的估计质量。如果z3能跟上预设的d 0.6·sin(2t)且波形不衰减说明wo足够如果theta在5秒内收进±2度且不被扰动打散说明wc合适。wo和wc之间一般保持wo ≥ 3·wc观测器必须比控制器快一拍否则补偿总比扰动慢半拍。4.3 与LQR的对比模型失配下的表现同一个模型换LQR控制只需要几行代码A [0 1; g/L 0]; B [0; 1/(m*L^2)]; Q diag([100, 1]); R 0.01; K lqr(A, B, Q, R); % 控制量 u -K * [theta; w]实现同样限幅后接入仿真循环LQR在模型精确时性能很漂亮R越小、Q越大回正越硬。但把A、B矩阵里的参数改掉20%再跑LQR的控制律仍然按旧模型算增益theta会持续出现12度的静差而ADRC的ESO在0.5秒内就能把失配量估进z3并补偿掉。对比实验的标准做法是同一个扰动序列、同一组初始角度分别记录theta轨迹画在同一个figure里。要注意LQR的Q/R权重和ADRC的wc不是等价的别拿调好的LQR和没调的ADRC比两种算法各调到稳定回正时间相近再比抗扰幅度。4.4 仿真结果提取把theta、z3、u存成MAT文件后续接FFT分析抖动频率也方便save(adrc_result.mat,t,theta,z3,u); figure; subplot(3,1,1); plot(t, theta*180/pi); ylabel(角度 deg); subplot(3,1,2); plot(t, z3); ylabel(总扰动估计); subplot(3,1,3); plot(t, u); ylabel(控制量);这里有个实用技巧把z3和真实扰动d画在一起如果仿真里d已知两条线重合度就是观测器质量的直接证据。实车上d不可见只能靠z3的平滑度间接判断。5. 实车移植验证ESO扰动估计、KalmanFilter融合与离线回放调试5.1 上电校准与状态检查实车和仿真的最大差异是传感器质量。Gyroscope.cpp里要先估算零偏上电静止1秒取陀螺仪输出均值作为offset。加速度计不需要额外校准用atan2(ax, az)算角度时只要车体竖直放置时输出接近0就算合格。// 启动校准静止均值作为陀螺零偏 float bias 0; for (int i 0; i 100; i) { bias gyro.read(); sleep_ms(10); } bias / 100.0f; gyro.setOffset(bias);校准之后把KalmanFilter输出和加速度计原始角度并行打印观察融合曲线是否比原始数据平滑、又没有明显滞后。滞后超过50ms就需要减小R_angle。5.2 通过ESO的z3判断控制质量ESO的z3在实车上是最好的诊断信号。车体静止直立时z3应该稳定在一个接近常数的值这个值就是重力分量和零偏的合力矩用手推一下车体z3会立刻跳变说明观测器响应正常。如果z3在被推之后长时间回不到原值wo太小或者控制周期抖动太大如果z3高频抖动则需要调大fal的delta或者降低wo。5.3 离线回放调试法实车只有一辆真车每调一次参数都冒着倒车风险。常见做法是把传感器数据和控制量记录成CSV拿回MATLAB里做离线回放脚本读CSV跑同一份ADRC代码把theta、z3、u画出来再叠加实车录像时间轴逐帧找问题。data readtable(log_20240601.csv); % 列time, theta_fused, gyro_z, duty, encoder_l, encoder_r res simADRC(data.theta_fused, data.duty, 0.01); plot(data.time, res.z3); hold on; plot(data.time, data.theta_fused*180/pi);如果离线仿真和实车响应趋势对得上说明模型参数基本可信对不上优先检查限幅逻辑和控制周期是否一致。这套方法能把实车调试从改了就跑变成先仿真验证再上真车对课程设计和毕业设计尤其省时间。整车直立稳定后再逐步追加转向控制和位置闭环这时候ADRC的TD才真正派上用场。本文还有配套的精品资源点击获取
