简介：双闭环PID程序（位置环加速度环，即位置环与速度环双闭环）针对单闭环控制在电机未到位时长期满偏输出、导致速度过快的问题，提供多环协调控制方案。程序将位置环输出与速度环输出叠加：位置未到达预设值时位置环虽输出满偏，但速度环检测偏差后输出反向补偿值，抑制整体输出；接近目标时关闭速度环，避免影响最终定位。压缩包内含2个文件，1个C源文件与1个头文件，共1KB，代码量小、结构清晰，适合直接阅读、修改并移植到实际工程。已有21822人浏览学习，可供正在调试电机定位、学习PID多环嵌套的开发者参考，也可作为位置环+速度环叠加控制的实现样例，帮助理解多环间的配合与切换时机，以及高功率电机场景下如何避免超调与飞车。
1. 双闭环PID：为什么位置环要套加速度环
做运动控制的人对PID都不陌生，但大多数项目用的是位置环加速度环，或者速度环加电流环。位置环加加速度环这个组合相对少见，它解决的是高刚性、高抗扰的定位场景：外环位置环输出的是期望加速度，内环加速度环直接压住加速度误差。和速度环相比，加速度环对负载扰动的响应更快，位置跟踪刚性更好，代价是加速度反馈难获取、噪声敏感、整定门槛更高。这篇文章拆一份可直接落地的双闭环PID工程代码，覆盖环路结构、C语言实现、参数整定顺序和最常见的坑，适用对象是做运动平台、机械臂关节或精密转台的开发者。
2. 环路架构与参数域：位置环和加速度环各自该干什么
2.1 为什么外环是位置、内环是加速度：刚性与带宽的博弈
在双闭环控制里，环路顺序不是随意定的。外环输出作为内环的给定，内环的输出再去驱动执行器。位置环在最外层，它看的是“我期望的位置和实际位置差多少”，然后把这个误差映射成一个加速度指令；加速度环在里层，它看的是“我期望的加速度和实际加速度差多少”，然后输出力矩或电流信号。
这个拓扑有一个很直接的好处。如果只做位置单环，位置误差到力矩之间隔着一个电机电气时间常数、机械惯量、摩擦扰动，等效成开环特性就是一个二阶或三阶系统，很难压住扰动。有了加速度内环，位置环面对的对象就变成了“加速度跟踪环节”，这个东西接近一个一阶惯性或者纯比例环节，位置环的控制难度大幅下降。
从频域看，内环带宽要远高于外环。一般工程经验是内环带宽至少要达到外环的5到10倍。如果位置环闭环带宽是10Hz，加速度环至少要跑到50到100Hz。这个倍数关系决定了位置环的相位裕度，内环带宽不够的话，位置环提增益就会振荡。
位置环本身建议只做P加少量D。原因是位置误差为零时，加速度给定就应为零，P控制天然满足这个物理约束。位置环加I项反而容易造成稳态抖动，因为它在零点附近还在积分，破坏位置环的静态刚性。加速度环则需要PI，甚至PID，因为它面对的是摩擦力、重力负载等持续扰动，没有I项消除不了稳态加速度误差，也就消除不了匀速段的动态位置滞后。
2.2 位置环与加速度环的PID结构差异：参数怎么分家
位置环P参数的本质是把位置误差映射成加速度，这个比例系数乘上系统质量后，等价于一个虚拟弹簧刚度。它决定系统的自然频率，Kp越大，系统越“硬”，但过大会激发机械谐振。位置环的D项放在速度反馈上，而不是位置误差的微分上，这样给定值突变时不会产生微分冲击。
加速度环的Kp决定加速度跟踪的快速性，Ki负责消除恒定的加速度误差。内环稳定裕度和执行器饱和限幅都在这一个环节里管住。两环的时间常数差异非常大，位置环周期可以放1ms甚至2ms，加速度环周期要么与位置环相同但必须在同一个中断里先算内环再算外环，要么提高到500us级，具体取决于加速度反馈信号的更新率。
以下是位置环和加速度环各自的责任域对照：
| 环节 | 输入 | 输出 | 推荐控制结构 | 主要限制 |
|---|---|---|---|---|
| 位置环 | 位置给定与位置反馈误差 | 加速度给定 | P + 少量D（微分在反馈侧） | 输出限幅需低于执行器能力 |
| 加速度环 | 加速度给定与加速度反馈误差 | 电流/力矩指令 | PI或PID，带积分限幅 | 反馈噪声与带宽必须匹配 |
2.3 加速度反馈从哪来：传感器、虚拟差分还是状态观测器
加速度反馈是实现这套方案绕不开的第一个工程决策，有三种来源。第一种是真加速度计，频响高、噪声低，但要考虑安装共振和温度漂移；第二种是从编码器速度差分获得，零成本但没有真加速度计那么干净，因为差分会放大高频噪声，后面必须接低通滤波；第三种是用状态观测器或者扰动观测器去估计加速度，这个实现成本最高但抗噪声最好。
我一般会先用手头的编码器差分加一阶低通跑通整定流程，确认双闭环逻辑没问题后再决定要不要上加速度计。因为差分法只是噪声大而不是不能用，它可以帮你在不换硬件的情况下就把位置环和加速度环的Kp、Ki先整定出大致范围。加速度计的零偏在闭环里其实影响不大，加速度环的I项会自动补偿它。
3. C语言工程实现：数据结构、控制周期与限幅逻辑
3.1 双环状态结构体：把环路状态封装起来
一份能直接改到业务代码里的双闭环PID程序，至少要包含两部分：控制参数和控制状态。参数部分在初始化或调整时写入，状态部分在每次周期中断里更新。我习惯把两个环的状态分开定义，方便单独调试和查看。
typedef struct { float Kp; // 位置环比例增益，单位：1/s^2 float Kd; // 位置环微分增益，作用在速度反馈上 float Ts; // 位置环控制周期，单位：秒 float pos_ref; // 位置给定 float pos_fb; // 位置反馈 float vel_fb; // 速度反馈，用于微分项 float acc_ref; // 加速度给定输出 float out_max; // 输出限幅，单位：m/s^2 float prev_error; // 上一周期位置误差，用于看变化趋势 } PosLoop_t; typedef struct { float Kp; // 加速度环比例增益 float Ki; // 加速度环积分增益 float Kd; // 加速度环微分增益 float Ts; // 加速度环控制周期 float acc_ref; // 加速度给定 float acc_fb; // 加速度反馈 float integral; // 积分状态 float out; // 电流/力矩指令输出 float out_max; // 输出限幅，单位：对应驱动器满量程 float i_max; // 积分限幅，通常为out_max的20%左右 float prev_error; // 上一周期加速度误差，用于微分项 } AccLoop_t;这个设计的核心是把限幅参数放进结构体里，而不是散落在算法函数中。位置环的输出限幅out_max要留有余量，不能让位置环把执行器输出占满，否则内环就没有调节空间了。积分项独立限幅是一个关键工程细节，i_max设成out_max的20%到30%，这样可以大幅降低积分饱和造成的超调。
3.2 位置环算法：微分放在反馈侧
位置环计算中有一个很重要的处理：微分项不是对位置误差求差分，而是对速度反馈做比例增益。这样位置给定在阶跃变化时，输出不会因为误差突变而产生尖峰，因为速度反馈是有惯性的。
void PosLoop_Update(PosLoop_t *p) { float error = p->pos_ref - p->pos_fb; // 加速度给定 = P项（误差） - D项（速度反馈） // 这里用减号是因为速度反馈方向与误差减小方向一致 p->acc_ref = p->Kp * error - p->Kd * p->vel_fb; // 输出限幅：限制加速度指令在安全范围内 if (p->acc_ref > p->out_max) p->acc_ref = p->out_max; if (p->acc_ref < -p->out_max) p->acc_ref = -p->out_max; // 保存误差，仅用于调试或后续扩展 p->prev_error = error; }这段代码去掉Kd项之后就是最纯粹的位置P控制，系统等效为一个弹簧质量系统。Kp与系统闭环自然频率的关系是ωn² = Kp，这个式子可以用来确定初值：如果你希望位置环闭环带宽为10Hz，那么ωn约为62.8 rad/s，Kp初值就取4000左右。Kd的初值可以按临界阻尼估算，也就是2*sqrt(Kp)，但需要结合机械系统的阻尼情况微调。
限幅放在P和D合成之后而不是分开限幅，是因为位置环内部不需要单独限制P或D，需要限制的是执行器能承受的加速度指令总量。位置环输出的acc_ref会输入给加速度环，在同一个中断周期里继续执行。
3.3 加速度环算法：带积分分离和独立限幅
加速度环是整个双闭环里最需要打磨的环节。它直接面对执行器饱和问题，如果积分项在输出饱和后还在累加，系统会出现明显的过冲和振荡。下面这段代码实现了积分分离和两级限幅。
void AccLoop_Update(AccLoop_t *a) { float error = a->acc_ref - a->acc_fb; float p_out, d_out, i_out; // 比例项 p_out = a->Kp * error; // 微分项：用误差差分，但对反馈噪声敏感时可加低通 float error_deriv = (error - a->prev_error) / a->Ts; d_out = a->Kd * error_deriv; // 积分项：带积分分离，误差过大时停止积分 if (fabsf(error) < 0.2f * a->out_max) { a->integral += error * a->Ts; // 积分限幅：防止积分饱和 if (a->integral > a->i_max) a->integral = a->i_max; if (a->integral < -a->i_max) a->integral = -a->i_max; } else { a->integral = 0.0f; // 误差过大时清空积分，防止反冲 } i_out = a->Ki * a->integral; // 合成输出并限幅 a->out = p_out + i_out + d_out; if (a->out > a->out_max) a->out = a->out_max; if (a->out < -a->out_max) a->out = -a->out_max; a->prev_error = error; }这里是整套程序的灵魂，几个细节值得展开。积分分离的阈值用了0.2倍的out_max，意思是加速度跟踪误差超过输出能力的20%时，积分器直接清零而不是累加，这能避免一种常见的翻车现象：启动瞬间误差大，积分项迅速堆满，位置到达目标后积分还要反向放一大段，表现为过冲后回摆。
d_out用的是误差差分，和位置环的反馈微分策略不同。因为内环面对的是高频动态，反馈侧微分会在你还没看到误差变化的时候提前反应，这个在这里不一定是好事。如果你发现加速度反馈噪声较大，微分项会把噪声进一步放大，最简单的处理是给误差信号加一个截止频率在控制频率1/10处的一阶低通滤波器，或者干脆把Kd设成0，用PI控制。
3.4 双环联动调度：先算外环再算内环
双环程序的结构体定义好后，需要在控制中断里按固定顺序执行。顺序不能反过来，原因很简单：位置环要用当前周期的位置误差计算加速度给定，加速度环紧跟其后用这个给定计算力矩输出，它们必须在同一时刻基准下工作。
void ControlISR(void) { // 读取反馈量 ReadEncoder(&pos_fb, &vel_fb); // 编码器位置和速度 acc_fb = GetAccelerationFeedback(); // 加速度反馈 // 参数更新 if (param_update_flag) { LoadCtrlParams(&pos_ctrl, &acc_ctrl); param_update_flag = 0; } // 先外环后内环 PosLoop_Update(&pos_ctrl); AccLoop_Update(&acc_ctrl); // 输出到驱动器 WriteDAC(acc_ctrl.out); }控制周期的稳定性直接决定微分项的质量。位置环和加速度环共用同一个硬件定时器中断，中断里执行全套计算，运行时间要控制在中断周期的80%以内。如果用软件延时或while循环来凑时间，控制周期抖动会让微分项计算失真，尤其在高Kd时表现非常明显。
参数热更新用的是param_update_flag标志位，这比在中断里直接改参数安全。因为结构体里的Kp、Ki可能有多个字节，如果主循环刚好写到一半被中断打断，加速度环会用一个被撕裂的数值运行一个周期，这在高速运动时可能造成一次电流冲击。我一般会通过串口或者上位机把整包参数下发到缓冲区，然后一次性拷贝到运行参数区。
4. 参数整定与联调：从内环到外环的实操路径
4.1 加速度环先整定：位置环设成最弱
双闭环整定最忌讳一上来就同时调两个环。正确的顺序是先让内环稳定，再让外环逐步加强。先把位置环Kp设成非常小的值，比如理论值的1%到5%，让位置环基本不起作用，这样加速度环面对的输入是一个缓慢变化的信号，方便观察它自身的行为。
然后从小到大逐步加大加速度环Kp，观察加速度反馈能否又快又没有超调地跟踪加速度给定。这个过程看的是内环阶跃响应，可以用程序在加速度环给定上主动叠加一个方波信号，检查响应上升时间和超调量。理想情况是超调量不超过10%，上升时间对应的带宽大约等于目标内环带宽。
内环Kp稳定后，再逐步加Ki。Ki的作用是消除稳态加速度误差，表现为加速度反馈对给定信号的静态跟随精度。加Ki的过程中如果出现低频振荡，多半是积分增益太大加上了执行器饱和，这时需要同时检查i_max是否过小。一个经验值是i_max取out_max的20%，如果在这个限幅下稳态误差达不到要求，优先检查系统是否存在较大的持续外力，而不是继续加Ki。
4.2 位置环整定：从自然频率反推Kp初值
加速度环整定完成后，把位置环Kp按照带宽目标算出一个初值。比如目标位置环带宽8Hz，ωn约50，Kp就是2500。这个初值代入后观察位置阶跃响应，大多数情况下会出现一个几十毫秒到几百毫秒的慢振荡，这是外环带宽与内环剩余的相位滞后相互作用的结果。
如果出现振荡，分两种情况看。振荡频率接近内环带宽，那就是内环带宽不够，需要回头加强加速度环或提高加速度反馈的更新率；振荡频率明显低于内环带宽，比如1Hz到3Hz，那就是位置环Kp偏大，需要降下来或者加Kd。
位置环Kd加在速度反馈上，本质是一个阻尼项。在整定Kp时遇到振荡，加Kd比继续降Kp更划算，因为Kd不降低系统刚度。Kd的典型取值范围是Kp的0.05到0.2倍，具体要看速度反馈的噪声水平，噪声大时Kd加不上去，这就是为什么速度反馈必须要经过滤波。
4.3 联调检查点：看四条曲线而不是只看上位机数字
联调阶段我习惯记录四路信号：位置给定、位置反馈、加速度给定、加速度反馈。这里有一个参数表可以参考：
| 调整目标 | 优先调整参数 | 次要调整参数 | 预期表现 |
|---|---|---|---|
| 定位时间太长 | 位置环Kp加大 | 位置环Kd减小 | 阶跃响应上升沿变陡 |
| 末端振荡 | 位置环Kd加大 | 加速度环Kp减小 | 尾部振荡衰减更快 |
| 动态滞后大 | 加速度环带宽提高 | 加速度反馈滤波放宽 | 跟踪误差峰值下降 |
| 低速爬行抖动 | i_max减小 | 加速度环Ki减小 | 稳定位姿时输出微振消除 |
联调中一个容易被忽略的点是加速度给定曲线。如果加速度给定的形状出现明显的锯齿或高频毛刺，说明位置环Kp相对速度反馈噪声偏大，或者位置环的输出限幅在频繁触发。限幅触发是正常的，但如果在一个稳定的正弦跟踪过程中频繁撞限幅，就要考虑重新分配位置环和加速度环的增益了。
5. 双闭环PID避坑指南：五条实战踩坑记录
5.1 启动瞬间猛冲一下，然后才缓慢归位
现象：位置阶跃启动时执行器先往一个方向猛冲，过了目标之后又回摆，整体动作像是“先犯错再修正”。
原因：微分项对位置误差做了直接差分。位置给定阶跃时误差突变，差分值极大，微分项输出瞬间饱和。这通常发生在位置环Kd直接作用在pos_ref与pos_fb差值上的写法里，作用在速度反馈上就不会有这个问题。
解决：把位置环Kd从“误差差分”改成“速度反馈比例”，也就是本文3.2节的写法。如果已经改了还在冲，检查速度反馈是否经过滤波，滤波不足会导致速度反馈本底噪声过大，Kd被迫调低。
5.2 加速度反馈噪声大，内环一发力电机就发烫
现象：加速度环增益提到一定程度后，电机电流出现高频脉动，运行几分钟后电机明显发热，但位置环看起来还算稳定。
原因：加速度反馈用的是编码器速度差分，差分运算放大了编码器量化噪声。内环Kp把这些噪声送到了电流环，电流高频分量导致电机额外发热。
解决：给加速度反馈加一阶低通滤波器，截止频率设置为内环带宽的10倍左右，例如内环目标带宽100Hz，滤波器截止1kHz。同时需要把加速度环的Kd降下来或者置零，噪声环境下微分项是最先失控的。
5.3 位置环输出限幅设置过大，系统低速时蠕动
现象：系统低速跟踪时有明显的走走停停感，速度曲线不平滑，位置曲线呈阶梯状。
原因：位置环输出限幅设得太接近执行器极限，内环没有调节余量。当位置误差较大时，位置环直接输出限幅值，加速度环被推到饱和，积分项清空，误差变小后又从饱和状态退回来，形成反复的“饱和-退出”循环。
解决：位置环输出限幅设为执行器能力的80%到90%，留出10%以上的余量给加速度环调节。同时注意out_max的单位和加速度计/编码器单位一致，这里出问题的话整套参数都会变得难以理解。
5.4 参数整定在实验台上完美，上机后完全变样
现象：同样的双闭环程序，在配重块模拟负载时阶跃响应完美，换到实际负载后出现持续振荡或者跟踪误差变大。
原因：实际负载的惯量和结构谐振频率发生了变化，加速度环的等效被控对象从“纯惯量”变成了“惯量加谐振”。加速度计安装位置的不同也会改变反馈信号的相位延迟。
解决：上机后重新执行一次从内环到外环的整定流程，不要沿用实验台的参数。如果实际负载惯量变化范围超过3倍，最好加入负载惯量辨识或者把增益设成按负载切换的分段值。
5.5 控制周期抖动导致同样的参数反复出现不同结果
现象：用软件延时凑控制周期，有时同一组参数调出来表现完全不一样，甚至同一次运行的前后段性能都不同。
原因：控制周期不是固定值，位置环的P项和加速度环的I项都乘了Ts，Ts抖动会直接改变环路的增益和相位。看起来参数没变，实际等效带宽一直在变。
解决：用硬件定时器中断保证控制周期无抖动。所有控制计算放在中断里执行，中断内运行时间严格控制在周期时间的80%以内，主循环只做参数交互和数据记录。
6. 前馈补偿与验证手法：让双闭环的实际表现再上一个台阶
6.1 加速度前馈：位置环跟踪动态轨迹不那么费力
双闭环在静态定位上已经表现很好，但跟踪连续运动轨迹时，位置环P项产生的加速度给定量会和轨迹加速度相差一个相位。解决办法是在位置环输出上直接叠加规划加速度前馈：
pos_ctrl.acc_ref = pos_ctrl.Kp * pos_err - pos_ctrl.Kd * vel_fb + trajectory_ff_acc;这里trajectory_ff_acc来自上层轨迹规划器，通常是S曲线或正弦轨迹的二阶导数。前馈的好处是让加速度环提前知道下一步要多少加速度，位置环的P项只需要修正前馈不准确的部分。
6.2 一个低成本的验证方法：扫频对比
整定完成后做一个快速验证：给位置给定加一个频率逐步升高的正弦信号，从1Hz扫到位置环带宽的2倍，记录位置反馈与给定的幅值比和相位差。幅值比衰减到0.707（-3dB）处就是实际闭环带宽，这个值应该在目标带宽附近，如果差很多说明参数还有调整空间。
另外一个判断内环带宽的技巧是观察加速度给定曲线。正常情况下加速度给定的频率成分应该集中在位置环带宽附近，如果出现明显高于内环带宽的频率成分，说明执行器在做一些没什么意义的高频动作，需要检查是不是加速度反馈噪声带来的。
最后一个习惯性检查：把位置环Kp突然提高50%再降回来，观察系统是否发生不可恢复的振荡。如果恢复不了，说明加速度环阻尼不够，内环带宽和外环带宽的差距拉得还不够大。从那以后我每次做完双闭环整定，都强制走一遍这套扫频和突变测试，至少能确认参数不是只在某一条曲线下漂亮。希望帮到你。
本文还有配套的精品资源，点击获取
