4、动态切换核心算法：位置补偿算法、速度前馈算法、加速度前馈算法、切换点的平滑处理

好，咱们直接切入正题。电子齿轮比的动态切换，说白了就是让从轴在运行中突然改变跟随比例，还不能让机器抖一下、顿一下。这活儿听着简单，做起来坑不少。我最早做这个功能时，以为就是改个系数就完事了，结果电机当场飞车，吓得我赶紧拍急停。后来才明白，核心在于四个字：补偿与平滑。

这一章，我把动态切换的四个核心算法拆开揉碎了讲。你跟着我的思路走一遍，以后遇到类似问题，心里就有底了。

4.1 位置补偿算法：别让轴“跳”一下

先想一个问题：电子齿轮比从 1:1 切换到 1:2，从轴的位置会怎样？

假设主轴走了 1000 个脉冲，从轴原本应该跟 1000 个。切换后，同样的主轴位置，从轴要跟 2000 个。如果你在切换瞬间直接改比例，从轴会认为“我欠了 1000 个脉冲”，然后猛地追上去——这就是位置跳变。

怎么解决？位置补偿。

我的做法是：在切换时刻，记录下主轴和从轴的当前位置，然后计算一个“虚拟主轴位置”。这个虚拟位置，是假设从轴一直按新比例跟随应该到达的位置。然后，把从轴的实际位置和虚拟位置做个差值，这个差值就是补偿量。

公式其实不复杂：

// 切换时刻记录
old_ratio = current_ratio;   // 旧比例
new_ratio = target_ratio;    // 新比例
master_pos_snapshot = master_encoder_pos;  // 主轴位置快照
slave_pos_snapshot = slave_encoder_pos;    // 从轴位置快照

// 计算虚拟主轴位置（假设从轴一直按新比例走）
virtual_master_pos = slave_pos_snapshot / new_ratio;

// 计算位置补偿量
position_offset = master_pos_snapshot - virtual_master_pos;

// 切换后的实际跟随计算
slave_target_pos = (master_encoder_pos - position_offset) * new_ratio;

嗯，这里要注意：position_offset 只在切换瞬间计算一次，之后保持不变。它就像一个“偏置”，让从轴在新比例下，从当前位置平滑起步，而不是跳到一个新位置。

4.2 速度前馈算法：让跟随更“跟脚”

位置补偿解决了“跳位置”的问题，但还有一个问题：速度突变。

你想想看，比例变了，从轴的目标速度也变了。如果只靠位置环去追，从轴会有一个加速过程，这个过程会产生跟随误差。说白了，就是主轴跑得快，从轴追得慢，位置误差就大了。

速度前馈就是干这个的。它直接把主轴速度乘以新比例，作为从轴速度环的前馈量。这样，从轴的速度环在切换瞬间就知道“我要跑多快”，而不是等位置环慢慢算出来。

// 速度前馈计算
master_velocity = get_master_velocity();  // 获取主轴速度（单位：脉冲/周期）
feedforward_velocity = master_velocity * new_ratio;

// 速度环输入 = 位置环输出 + 前馈量
velocity_loop_input = position_loop_output + feedforward_velocity;

我在项目中遇到过一个问题：主轴速度信号有噪声，直接做前馈会导致从轴速度抖动。后来我加了一个一阶低通滤波器，截止频率设在 50Hz 左右，效果立竿见影。

4.3 加速度前馈算法：对付“急加速”场景

速度前馈搞定了匀速段，但遇到主轴急加速或急减速呢？

举个例子：主轴从 1000rpm 在 10ms 内加速到 2000rpm，加速度很大。这时候，从轴不仅要跟上速度，还要跟上加速度的变化。如果只靠速度前馈，从轴的加速度响应会滞后，导致切换瞬间出现“过冲”或“欠冲”。

加速度前馈就是用来补偿这个滞后的。它把主轴的加速度乘以新比例，再乘以一个惯性补偿系数，直接加到电流环的给定上。

// 加速度前馈计算
master_acceleration = get_master_acceleration();  // 获取主轴加速度
inertia_compensation = system_inertia_ratio;      // 系统惯量比（需整定）
feedforward_acceleration = master_acceleration * new_ratio * inertia_compensation;

// 电流环给定 = 速度环输出 + 加速度前馈
current_loop_input = velocity_loop_output + feedforward_acceleration;

说实话，加速度前馈的整定比速度前馈麻烦。因为加速度信号对噪声极其敏感，稍微有点毛刺，电流环就会抖。我个人的习惯是：先用速度前馈把稳态误差压到 1% 以内，如果还不够，再考虑加加速度前馈。而且，加速度前馈的系数我一般从 0.5 开始试，慢慢往上加。

4.4 切换点的平滑处理：别让算法“硬切换”

算法都准备好了，但还有一个细节：什么时候切换？

如果你在主轴速度最高点切换，即使有前馈，从轴也会有一个冲击。因为前馈算法需要时间建立，切换瞬间的误差无法完全消除。

我的做法是：选择主轴速度的过零点或低速段作为切换点。比如，主轴每转一圈，速度会经过两次过零（正转和反转之间）。在这些点切换，冲击最小。

如果应用不允许等待过零点，那就用“软切换”：

• 比例渐变：在几个控制周期内，让比例从旧值线性过渡到新值。比如，10ms 内完成切换，每个周期增加 10% 的新比例。
• 权重混合：切换瞬间，从轴的目标位置 = 旧比例计算值 × (1-α) + 新比例计算值 × α。α 从 0 到 1 渐变。

// 软切换：比例渐变
float alpha = 0.0f;  // 混合系数
float step = 0.1f;   // 每个周期增加 0.1

for (int i = 0; i < 10; i++) {
    alpha += step;
    if (alpha > 1.0f) alpha = 1.0f;
    
    // 混合计算
    slave_target = (1 - alpha) * slave_target_old + alpha * slave_target_new;
    
    // 输出到驱动器
    set_slave_position(slave_target);
    
    // 等待下一个控制周期
    wait_for_next_cycle();
}

我曾经在一个包装机械项目里用过软切换，效果很好。但要注意：软切换的时间不能太长，否则主轴已经跑出去很远了，从轴还在慢慢过渡，反而会产生大误差。一般我控制在 5-10 个控制周期内完成。

4.5 算法组合与流程图

这四个算法不是孤立的，它们需要组合使用。下面这张图，是我在实际项目中总结的算法调用流程：

从图上你能看到，整个流程是串行的。先判断切换点是否合适，如果不合适就等待；然后依次执行位置补偿、速度前馈、加速度前馈。每一步都有它的作用，跳一步都不行。

4.6 实战中的避坑指南

最后，分享几个我踩过的坑：

• 位置补偿的溢出问题：如果主轴编码器是 32 位的，补偿量计算时要注意数据类型。我吃过一次亏，补偿量算出来是负数，结果从轴反向飞车。后来统一用 64 位整数计算，再也没出过问题。
• 前馈系数的整定顺序：先整定速度前馈，再整定加速度前馈。不要一上来两个一起调，出了问题你都不知道是哪个参数导致的。
• 软切换的时间不要超过 20ms：我试过 50ms 的软切换，结果主轴已经跑完一个工位了，从轴还在半路上。对于高速应用，软切换时间越短越好。
• 切换点检测要加滞回：如果主轴速度在过零点附近抖动，切换点会反复触发。加一个 5% 的滞回区间，能有效避免这个问题。

好了，这一章的内容就到这儿。四个算法，一个流程图，几个避坑点，够你消化一阵子了。记住：动态切换的核心不是算法有多复杂，而是你能不能把每个细节都做到位。

---

公众号:蓝海资料掘金营，微信deep3321