1. 自抗扰控制与TD为什么说它是工程实践的“稳定器”如果你正在捣鼓无人机飞控或者任何需要快速、精准跟踪指令的自动控制系统那你大概率已经听过“自抗扰控制”这个名字了。我第一次接触它是为了解决一个四旋翼无人机在强风干扰下的姿态抖动问题。传统的PID控制器在设定点突变时要么响应迟钝要么超调振荡得让人心惊胆战更别提那些突如其来的阵风了。后来我发现了自抗扰控制尤其是它的“先锋官”——跟踪微分器感觉像是打开了一扇新世界的大门。自抗扰控制英文叫Active Disturbance Rejection Control简称ADRC。它的核心思想非常“工程化”把系统内部所有不确定的动态和外部未知的干扰统统打包成一个“总扰动”然后实时估计并补偿掉。你可以把它想象成一个经验丰富的船长船被控对象在海上航行会遇到未知的风浪外部干扰船本身的发动机性能也会波动内部模型不确定性。这位船长不纠结于精确的风浪模型或发动机曲线他通过观察船的实际状态实时估算出一个“综合干扰力”然后猛打方向盘或调整油门直接把这个干扰力抵消掉让船始终沿着预定航线稳定前进。TD就是这个船长手里最关键的“观测仪”和“预判器”。TD全称跟踪微分器是ADRC的第一个核心环节。它的任务听起来简单做起来却非常精妙给定一个可能跳变甚至带噪声的指令信号v(t)TD要能快速无超调地跟踪上这个指令同时干净利落地给出这个指令的微分信号。举个例子你希望无人机从0度倾斜角瞬间转到30度。直接把这个30度的阶跃指令喂给电机机身会猛地一抖很可能失稳。TD的作用就是把这个“跳变”的指令平滑成一个“S”形的过渡轨迹让无人机沿着这条平滑的轨迹柔和地到达30度同时它还能准确地告诉你“嘿现在跟踪的速度是多少加速度趋势又是怎样”。这个平滑后的轨迹和它的微分信号才是真正送给后续控制器去执行的“理想指令”。所以对于飞控开发者来说理解TD不仅仅是看懂几个公式更是掌握如何让机器“柔和而坚定”地执行命令的关键。接下来我们就从它的理论基石——“最速控制综合函数”开始一步步拆解直到你能在单片机上跑起来的离散化代码。2. 理论基石最速控制综合函数到底在解决什么问题要理解TD的离散形式我们必须先回到它的源头最速控制综合函数。这个名字听起来有点唬人但其实它解决的问题非常直观就是我们初中物理就学过的“最短时间制动”问题。想象一下你开着一辆小车从A点出发要尽快到达B点并稳稳停住速度为零。这辆车的性能有限最大加速度是r最大刹车减速度是-r。你怎么开时间最短答案就是先以最大加速度r全力加速在某个恰到好处的时刻切换为最大减速度-r全力刹车正好在到达B点时速度减为零。这条先加速、后减速的轨迹就是“最速控制”轨迹。把这个物理问题数学化就得到了最速控制综合函数。它要计算的就是在当前时刻基于小车当前位置x1距离目标B点的位移、当前速度x2以及最大控制量加速度/减速度r我应该给出多大的控制指令u这个u必须在[-r, r]之间。连续系统下的最速控制综合函数表达式是许多资料的起点u -r * sign( x1 - v (x2 * |x2|) / (2*r) )这里v是目标位置。这个式子怎么理解呢核心是看sign()函数括号里的部分x1 - v当前位移误差。(x2 * |x2|) / (2*r)这是一个关键项。根据运动学公式v² 2*a*s这里(x2)² / (2*r)代表的物理意义是以当前速度x2用最大减速度r刹车直到停车所需要走过的距离。所以括号里整体的含义是当前位移误差 刹车所需距离。如果这个和大于0说明即使现在开始全力刹车也会冲过目标点所以必须立刻给出最大减速指令u -r。如果这个和小于0说明即使现在开始全力加速到达目标点时速度也减不到零或者还差得远所以应该继续加速u r。如果这个和等于0那就正好处在那个最优切换点上理论上控制量可以在r和-r之间切换。这就是最速控制的核心思想通过实时计算“位移误差”与“刹车距离”之和的符号来决定当前是该全力加速还是全力刹车。它保证了系统能以最短时间、无超调地到达目标。然而这个漂亮的连续时间公式并不能直接塞进数字控制器比如STM32里运行。我们的单片机是在离散的时间点上每隔h秒进行计算的所以必须把它“离散化”。而离散化之后情况就变得微妙和复杂起来这也是很多朋友感到困惑的地方。3. 从连续到离散关键推导与三种工况的深度剖析把连续系统公式离散化并不是简单地把微分dx/dt换成(x(k1)-x(k))/h。对于最速控制问题离散化后系统状态位置x1速度x2的演化会呈现出不同的模式主要取决于当前状态离目标点有多“远”。韩京清教授在他的经典论文中清晰地将其分为了三种工况这是理解离散TD的重中之重。我们设定离散系统模型如下x1(k1) x1(k) h * x2(k) x2(k1) x2(k) h * u(k)其中h是采样周期积分步长u(k)是第k步施加的控制量加速度且|u(k)| ≤ r。我们的目标仍然是寻找一系列u(k)使得系统从任意初始状态[x1(0), x2(0)]出发能在最少的步数N内到达原点[0, 0]即目标点且无超调。通过一系列矩阵运算和逆向推导这里不展开繁复的数学重点讲物理图景我们可以根据初始状态相对于原点的位置划分出三个不同的区域对应三种最优控制策略3.1 工况一一步到位区|x1| ≤ h²r 且 |x1| ≥ |h*x2| - h²r/2这个区域最靠近原点。如果系统的初始状态落在这个菱形区域内在位置-速度相平面图上它可以在一步之内直接到达原点并停下。此时的最优控制律很简单u - (x1 h*x2) / (h²/2)你可以把它理解为计算出一个“恰好”能在一步h时间内将当前状态[x1, x2]驱动到[0,0]的控制量。当然这个计算出的u必须进行限幅确保其绝对值不超过r。3.2 工况二两步调节区|x1| h²r 且 |x1| ≤ h²r |h*x2|如果状态离原点稍远一步到不了但可以通过两步到达。这个区域的控制策略是第一步施加一个±r的控制将状态驱动到“一步到位区”的边界上第二步再利用工况一的公式精准到达原点。判断逻辑是看x1和x2的符号关系决定第一步是加速还是减速。3.3 工况三多步跟踪区|x1| h²r |h*x2|这是最常见的情况初始状态离目标比较远。此时的最优策略就是让我们前面提到的“最速控制轨迹”离散化。系统会沿着一条离散的“最速逼近曲线”向原点运动。这条曲线由两条边界线a和b所界定a线最速轨迹线如果状态正好在这条线上那么一直以最大控制量u -r * sign(x1)运动就能在最后两步切换到工况二和工况一完美到达原点。b线临界线如果状态在这条线上那么需要先以u -r * sign(x1)运动一步就能落到a线上。如果当前状态在a线和b线之间最优控制量u就不再是简单的±r而是一个介于-r和r之间的值。论文中通过几何分析推导出此时的控制量计算公式为u - (x2 - sign(delta) * r*k*h) / h其中delta x1 h*x2k是一个与状态相关的整数步数估计。这个公式的意义是计算出一个控制量使得系统能在k步后速度刚好变为sign(delta)*r*h从而进入预定的切换序列。最终韩教授用他标志性的fhan函数最速控制综合函数离散形式优雅地将这三种工况统一到了一个简洁的表达式里。这个fhan函数正是TD离散实现的核心。4. TD离散化实现fhan函数详解与参数物理意义韩京清教授将上述复杂的分段最速控制综合函数精炼成了一个非常适合计算机实现的函数通常被称为fhan函数。这个函数就是离散跟踪微分器的心脏。我们来看看它的标准形式以及每个参数的“手感”。一个典型的fhan函数实现如下以C语言风格伪代码表示float fhan(float x1, float x2, float v, float r, float h) { float d r * h * h; float a0 h * x2; float y x1 - v a0; float a1 sqrt(d * (d 8 * fabs(y))); float a2 a0 sign(y) * (a1 - d) / 2.0; float sy (sign(yd) - sign(y-d)) / 2.0; float sa (sign(a2d) - sign(a2-d)) / 2.0; float fhan -r * (a2 / d - sign(a2)) * sa - r * sign(a2); return fhan; }然后TD的离散迭代方程就非常简洁了float TD(float v, float r, float h) { static float x1 0, x2 0; // 状态变量跟踪位置和微分 float fh fhan(x1, x2, v, r, h); x2 x2 h * fh; x1 x1 h * x2; return x1; // 返回跟踪后的信号x2即为微分信号 }现在我们来拆解fhan函数和TD参数这直接关系到你调参的直觉r—— 速度因子这是最重要的参数没有之一。它直接决定了跟踪的“快慢”。你可以把它理解为“最大允许加速度”。r越大TD为了跟上指令信号所能产生的“虚拟加速度”就越大跟踪速度就越快但同时对噪声也可能更敏感。调r的感觉就像调节油门的灵敏度r太小无人机响应慢吞吞r太大一点指令就导致剧烈动作可能引发振荡。我的经验是先从系统物理最大加速度的1/5到1/2开始试。h—— 积分步长采样周期这通常是你的控制循环周期。例如你的飞控主循环是500Hz那么h 0.002秒。r和h是耦合的。fhan函数内部很多项都有h²r这个组合。这意味着如果你改变了控制频率h为了保持相似的跟踪性能你需要同步调整r。一般来说h变小频率变高r可以适当增大。d—— 动态参数d r*h*h它有一个关键的物理意义它正比于在一步采样时间h内以最大加速度r所能产生的位移变化量。函数中的y、a1、a2等变量本质都是在和d进行比较来判断系统当前处于我们前面讲的三种工况中的哪一种。sy和sa—— 平滑开关函数fhan函数里sign函数和sy、sa这些操作其核心作用就是实现三种工况的平滑无缝切换。它避免了在工况边界处控制量u发生跳变使得输出非常平滑。这是手工实现分段函数时很难做到的优雅之处。理解这些后你再回头看fhan函数它就不再是一堆神秘的符号了。它是在每个计算周期h内根据当前跟踪误差(x1-v)和跟踪速度x2快速判断系统处于哪个“区域”然后给出一个最优的、平滑的控制量u去驱动虚拟的“跟踪状态”[x1, x2]让它们快速、无超调地逼近指令v。x1输出就是平滑后的指令x2输出就是高质量的微分信号。5. 工程实践在飞控中实现TD的步骤与避坑指南理论懂了代码也有了是不是就能直接用了别急我在实际把TD嵌入到无人机飞控时踩过好几个坑。这里分享一套完整的实现步骤和注意事项希望能帮你省点时间。第一步模块化封装首先将TD写成一个独立的结构体或类。这有利于管理状态变量也方便多个通道如滚转、俯仰、偏航独立使用。typedef struct { float v; // 输入指令 float r; // 速度因子 float h; // 采样周期 float x1; // 跟踪状态位置 float x2; // 跟踪状态速度即微分输出 float last_out; // 上一周期输出可选用于滤波 } TD_HandleTypeDef; void TD_Init(TD_HandleTypeDef *htd, float r, float h) { htd-r r; htd-h h; htd-x1 0; htd-x2 0; htd-last_out 0; } float TD_Calc(TD_HandleTypeDef *htd, float v) { htd-v v; float fh fhan(htd-x1, htd-x2, htd-v, htd-r, htd-h); htd-x2 htd-h * fh; htd-x1 htd-h * htd-x2; // 可以直接返回htd-x1作为平滑信号htd-x2作为微分信号 return htd-x1; }第二步参数整定——从“手感”到“科学”确定h首先固定你的控制周期。对于大多数无人机100Hz到500Hz是常见范围。先取一个值比如200Hz (h0.005)。初调r这是主要调试对象。找一个典型的阶跃指令比如让角度从0到30度。先设一个较小的r比如1观察x1跟踪指令的曲线。它应该缓慢上升。逐步增大r你会看到跟踪速度变快曲线变陡。直到你发现继续增大r跟踪曲线开始出现轻微的超调或前端有个很小的“毛刺”这其实是TD在试图以最快速度跟踪而产生的非线性特性并非振荡。把r回调到刚好不出现超调或毛刺的值附近。这个值通常是一个不错的起点。联调验证将TD平滑后的信号x1和微分信号x2送入你的控制器比如ADRC的ESO和NLSEF。观察整个系统的响应。如果系统响应太慢可以适当同时增大r和减小h提高控制频率。如果系统在高频指令下出现噪声放大可能是r太大或h太小需要反向调整。第三步必须避开的“坑”坑1初始状态突变。如果系统上电时指令v和TD内部状态x1相差巨大比如指令是0但TD内部状态未初始化TD会输出一个巨大的微分值x2可能导致执行器饱和。务必在初始化时将x1和x2设置为一个合理的初始值通常就是第一个输入指令v和0。坑2r和h的耦合忘记考虑。当你改变控制频率时一定要重新调整r。一个经验公式是保持r * h²在一个量级能大致维持相似的跟踪性能。坑3对噪声指令的处理。TD本身对噪声有一定滤波能力因为它是二阶系统但如果输入指令v本身带有高频噪声r太大时微分信号x2可能会放大噪声。如果指令噪声严重最好先对原始指令v进行一个低通滤波再把滤波后的值送给TD。不要指望TD能完美处理一切噪声。坑4离散计算精度。在资源受限的单片机如某些浮点能力弱的MCU上fhan函数中的sqrt和除法运算可能较慢。确保你的h、r取值不会导致计算溢出特别是d很小的时候。可以考虑使用查找表或近似算法来加速sqrt但对于飞控应用现在的STM32系列芯片直接计算通常没有问题。我在一个中型四旋翼上的实测结果是将姿态环的指令经过TD平滑后在快速打杆时电机声音从尖锐的“吱吱”声变成了更浑厚的“嗡嗡”声机身动作明显更顺滑对于突发阵风引起的姿态误差恢复过程也更平稳几乎没有肉眼可见的振荡。这背后就是TD这个“先锋官”为后续的控制器提供了干净、平滑且带有预见性微分的指令让整个控制回路变得更加“从容”。6. 超越基础TD的高级应用与变体探讨当你掌握了基本的TD实现后可以探索一些更高级的用法和变体这能让你的控制系统如虎添翼。1. 安排过渡过程这是TD最经典的应用。不仅仅用于阶跃指令对于任意变化的指令v(t)TD都能为其生成一个平滑的过渡信号x1(t)及其微分x2(t)。你可以把r理解为“对指令变化速度的期望限制”。r小过渡过程慢而平滑r大过渡过程快而激进。这相当于为你的系统动态地安排了一个合理的“参考轨迹”避免了直接跟踪突变指令带来的冲击。2. 微分信号提取TD是一个优秀的非线性微分器。相比传统的线性微分器如一阶差分[v(k)-v(k-1)]/h或速度-位置环节s/(Ts1)TD提取的微分信号x2有两个显著优点相位超前小、对噪声不敏感。线性微分器为了抑制噪声通常要加大滤波时间常数T但这会引入很大的相位滞后不利于控制。TD在跟踪x1的同时自然给出了x2这个x2的噪声水平比直接差分低得多而且几乎没有额外的相位滞后这对于需要高质量速度、加速度反馈的控制系统如云台增稳、机械臂关节控制价值巨大。3. 参数自适应与变参数TD固定的r和h可能无法应对所有工况。你可以根据系统状态动态调整TD参数变r策略当跟踪误差大时增大r以快速跟踪当接近目标时减小r以平滑收敛避免超调。这类似于非线性PID中的变增益思想。变h策略在高速动态阶段使用更小的h更高的计算频率以提高精度在稳态时使用较大的h以节省计算资源。但这需要你的控制器能处理变步长迭代。4. 与其他观测器结合TD可以和扩张状态观测器ESOADRC的另一个核心强强联合。ESO负责估计总扰动而TD负责为ESO提供高质量的、平滑的参考指令及其微分。在一些更复杂的架构中甚至可以使用多个TD级联来安排更高阶的过渡过程如位置、速度、加速度轨迹用于需要加加速度急动度限制的高性能运动控制。实现这些高级功能核心在于深刻理解fhan函数中r和h对系统动态特性的影响。我建议你先在Simulink或Python/Matlab上搭建模型反复调整参数观察x1、x2的曲线变化形成肌肉记忆。然后在一个简单的硬件平台如一个小车或一个舵机上做实操感受参数变化对实际物理系统的影响。这个过程积累下来的“手感”远比死记硬背公式更有价值。