4. 位置环的软件架构：控制周期与中断、位置环算法实现、前馈控制与补偿。
好，我们进入第四章。前面几章我们把位置环的物理基础、数学模型和传感器都聊透了。这一章，咱们得落地了——看看这些算法到底怎么在芯片里跑起来的。
很多刚入行的工程师，算法仿真做得漂漂亮亮，一上机就崩。为什么？因为软件架构没想清楚。控制周期怎么定？中断优先级怎么配？前馈加在哪里？这些细节，才是区分“能跑”和“跑得稳”的关键。
核心观点：位置环的软件架构，本质上是“在有限的计算资源下，用确定的时序，完成确定的控制律”。
4.1 控制周期与中断：系统的“心跳”
位置环是个实时系统。它的“心跳”就是控制周期。我见过有人把位置环放在主循环里，用延时函数凑周期。结果电机一转，波形跟心电图似的。嗯，这肯定不行。
正确的做法，是用硬件定时器触发中断，在中断服务函数里执行位置环。这样周期才准，抖动才小。
控制周期怎么选？
说白了，这是个权衡。周期越短，控制越细腻，但CPU负担重。周期太长，系统响应慢，容易丢步。
我个人习惯，先看机械系统的谐振频率。根据香农采样定理，控制频率至少是系统带宽的10倍。举个例子：
- 普通伺服（带宽 50-100 Hz）： 控制周期 1-2 ms
- 高速高精（带宽 200-500 Hz）： 控制周期 0.2-0.5 ms
- 大型重载（带宽 10-30 Hz）： 控制周期 5-10 ms
| 应用场景 | 典型带宽 | 推荐控制周期 | 中断优先级 |
|---|---|---|---|
| 小型机器人关节 | 100-200 Hz | 0.5-1 ms | 最高 |
| 数控机床进给轴 | 50-100 Hz | 1-2 ms | 高 |
| 大型龙门吊 | 10-30 Hz | 5-10 ms | 中 |
| 3D打印机 | 20-50 Hz | 2-5 ms | 中 |
我的小技巧： 中断服务函数里只做“必须实时”的事——读编码器、算位置误差、输出控制量。那些滤波、日志、通讯，扔到后台任务去。我曾经见过一个项目，在中断里打印调试信息，结果周期直接翻倍，电机嗡嗡响。
4.2 位置环算法实现：从公式到代码
好，周期定了，中断配好了。接下来就是核心——位置环算法怎么实现？
我们以最经典的比例-前馈（P+FF）结构为例。为什么不用PID？因为位置环通常不加积分项——积分容易引起超调和震荡，尤其在启停阶段。你想想看，位置环的误差是位置偏差，不是速度偏差，积分会累积出大问题。
算法流程：
- 读取当前实际位置（从编码器或光栅尺）
- 计算位置误差：
e_pos = target_pos - actual_pos - 计算前馈量：
ff = Kff * target_vel（速度前馈） - 计算比例控制量：
p_out = Kp * e_pos - 输出速度指令：
vel_cmd = p_out + ff - 限幅保护：
vel_cmd = clamp(vel_cmd, -max_vel, max_vel)
代码实现大概长这样：
// 位置环中断服务函数 (1ms 周期)
void PositionLoop_ISR(void)
{
// 1. 读取当前位置
int32_t actual_pos = Encoder_GetPosition();
// 2. 计算位置误差
int32_t error_pos = target_pos - actual_pos;
// 3. 速度前馈计算
int32_t vel_ff = (int32_t)(K_FF * target_vel);
// 4. 比例控制
int32_t p_out = (int32_t)(K_P * error_pos);
// 5. 合成速度指令
int32_t vel_cmd = p_out + vel_ff;
// 6. 限幅
if (vel_cmd > MAX_VEL) vel_cmd = MAX_VEL;
if (vel_cmd < -MAX_VEL) vel_cmd = -MAX_VEL;
// 7. 输出到速度环
SpeedLoop_SetTarget(vel_cmd);
}
注意： 上面的代码为了清晰做了简化。实际工程中，K_P 和 K_FF 通常是浮点数，但为了性能，我建议转成定点数运算。另外，target_vel 需要从轨迹规划器获取，而不是自己微分——微分会放大噪声。
4.3 前馈控制与补偿：让系统“预判”未来
纯比例控制有个毛病——它总是“事后诸葛亮”。误差出来了才去纠正。对于高速运动，这会导致很大的跟随误差。
前馈控制就是解决这个问题的。它根据目标轨迹，提前计算出需要的控制量，直接叠加到输出上。
常见的前馈类型：
- 速度前馈： 最常用。根据目标速度，补偿掉速度环的稳态误差。
- 加速度前馈： 用于补偿惯性力。在高速启停时效果明显。
- 摩擦力前馈： 补偿静摩擦和库仑摩擦。低速时特别重要。
- 重力前馈： 垂直轴专用。抵消重力影响。
我记得有一次调一个垂直轴，怎么调都有下坠。后来加了重力前馈，问题立刻解决。你想想看，电机不仅要克服惯性，还要托着负载不掉下来。比例环再强，也扛不住一直输出偏置。
前馈的整定原则：
前馈系数不是越大越好。太大了，系统会“过冲”，反而震荡。我一般这样调：
- 先关掉前馈，调好比例增益，让系统稳定。
- 从小到大加前馈系数，观察跟随误差。
- 当误差减小到原来的 30%-50% 时，基本就到位了。
- 再往上加，如果误差不再减小甚至反弹，说明前馈过大了。
避坑指南： 我曾经在一个项目中，前馈系数调得刚刚好，但一换负载就崩。后来发现，前馈系数应该跟负载惯量关联。如果负载变化大，建议做在线惯量辨识，动态调整前馈系数。
4.4 软件架构总览：一张图说清楚
说了这么多，咱们用一张图把整个软件架构串起来。从轨迹规划到位置环，再到速度环和电流环，每一层都有自己的周期和任务。
从这张图你能看到，整个控制链路是分层的。每一层都有自己的控制周期，越靠近电机，周期越短。位置环的输出不是直接给电机，而是作为速度环的输入。这种级联结构，让每一层各司其职，互不干扰。