1. 位置环规划为什么值得单独拎出来讲搞过FOC的人都有一个共识电流环是根基速度环是骨架位置环才是真正决定“手感”的那一层。很多朋友调电流环调得挺顺波形也漂亮一到位置环就发现要么响应慢半拍要么到位时抖一下要么走个小距离来回蹭。问题往往不在PID参数本身而在于位置指令是怎么给出来的。传统做法无非两种阶跃给定和梯形/S形速度规划。阶跃给定响应最快但加速度理论上无穷大机械冲击大到位后容易振荡梯形规划稍微好点但加速度突变点依然存在对皮带、丝杆、联轴器这些柔性传动不友好。S形规划能解决加速度突变但实现起来参数多、计算量大在STM32F103这种资源有限的片子上跑起来有点吃力。我这次要聊的方案是用一个Sigmoid函数直接生成位置环的平滑过渡曲线。它的核心优势是一个公式、两个参数、天然平滑、计算量小而且非常适合用Q16定点数在MCU上实现。整套东西我在STM32F103RCT6驱动无刷电机的平台上实测过从位置指令生成到最终转子运动全程没有出现明显的抖动和过冲。这篇文章适合谁看如果你正在做PMSM/BLDC的FOC控制已经跑通了电流环和速度环正在为位置环的平滑性发愁或者你对定点数运算在电机控制里的落地感兴趣想找一个具体案例来理解Q格式的用法那这篇内容应该能给你一些直接能抄的思路。我会从数学原理讲到Q16定点实现再到代码结构和实测现象尽量把每个“为什么”都说清楚。2. Sigmoid函数到底凭什么能用来做位置规划2.1 从生物神经元到电机位置曲线Sigmoid函数的标准形式是[ \sigma(x) \frac{1}{1 e^{-x}} ]它的形状是一条从0平滑上升到1的S形曲线中间陡、两头缓。这个特性最早是在神经网络里当激活函数用的因为它能把任意实数映射到(0,1)区间而且导数连续。把它搬到位置规划上思路就很直接了把归一化的时间映射到Sigmoid的自变量输出就自然是一条从0到1的平滑位置曲线。假设我们要从位置A走到位置B总行程Δ B - A那么任意时刻的位置可以写成[ pos(t) A \Delta \cdot \sigma(k \cdot (t - t_0)) ]其中k控制曲线的陡峭程度也就是“加速快慢”t0是曲线中心点对应的时间。t从0到T的过程中pos从A平滑过渡到B。2.2 为什么它比梯形和S形规划更省事我列一个对比表把三种常见位置规划方式放在一起看规划方式加速度连续性参数数量计算量实现难度适用场景阶跃给定不连续冲击0极低极低点位要求极低梯形速度加速度突变3vmax, acc, dec低低通用点位S形速度加速度连续5加加速度限制中高中高高精度柔性传动Sigmoid位置加速度连续2k, T低低资源受限MCU关键点在于Sigmoid曲线的一阶导和二阶导都是连续的意味着速度和加速度都不会突变。而它的计算只需要一次指数运算如果用定点数查表或者近似连指数运算都能省掉。2.3 参数k和T分别控制什么很多人第一次用Sigmoid做规划容易把k和T搞混。我用实际调试的经验来说T决定总过渡时间。T越大整个运动越慢但曲线形状不变。k决定曲线的“陡峭度”。k越大中间段越陡起步和停止越突然k越小曲线越平缓加速越柔和。有一个经验公式可以参考当k·T ≈ 10~14时曲线两端已经足够平缓起点和终点的速度接近零。如果k·T太小比如小于6起点速度不为零会有明显的“弹射感”如果太大比如超过20中间段几乎变成阶跃失去平滑意义。我在STM32F103上实测对于一个小型云台电机行程90度T取0.3sk取40k·T12效果比较理想。这个值不是固定的要根据负载惯量和机械柔性来调。注意k和T不是独立的真正影响曲线形状的是它们的乘积。调参时先定T满足节拍要求再调k满足平滑度要求。3. Q16定点数为什么不用浮点又怎么落地3.1 STM32F103的浮点困境STM32F103RCT6是Cortex-M3内核没有硬件浮点单元。所有float运算都是软件模拟一次浮点乘法可能要几十个周期一次指数运算更是上百周期。在电流环这种20kHz的场合浮点还能勉强撑住但位置环如果每1ms算一次还要做指数运算CPU负载会明显上升。更麻烦的是浮点运算在中断里执行时间不确定可能导致控制周期抖动。对于电机控制这种对时序敏感的场景定点数是更稳妥的选择。3.2 Q16格式的基本规则Q16的意思是用一个32位有符号整数低16位表示小数部分高16位表示整数部分。实际值 整数存储值 / 65536。比如1.0 存储为 655360.5 存储为 327680.1 存储为 65540.1×65536≈6553.6取整-2.5 存储为 -163840Q16的精度是1/65536≈0.0000153对于位置规划来说完全够用。范围是-32768到32767对于归一化到0~1的Sigmoid输出绰绰有余。3.3 Q16下的乘法与溢出处理两个Q16数相乘结果是Q32格式需要右移16位才能回到Q16。这里有个坑如果直接乘中间结果可能溢出32位。比如两个接近1.0的Q16数相乘65536×655364294967296已经超过int32的最大值2147483647。解决办法有两种用int64中间变量乘完再右移。STM32F103支持64位运算但速度慢。先右移再乘牺牲一点精度。比如(a8)*(b8)0相当于Q8×Q8Q16。我在实际代码里用的是第一种因为位置环频率不高1kHz64位运算的开销可以接受。如果要在更高频率下跑可以用第二种但要注意精度损失。3.4 Sigmoid的定点近似直接算e^{-x}在定点上很麻烦。我的做法是查表线性插值把x的范围比如-8到8分成256段每段存一个Q16的Sigmoid值。查表时根据x找到相邻两个表项做线性插值。这样一次Sigmoid计算只需要几次乘加和一次查表在STM32F103上大概几十个周期就能完成。表的大小是256×4字节1KB对于RCT6的256KB Flash来说毫无压力。如果你想要更高精度可以分512段或1024段但1KB的表已经能把误差控制在0.001以内对位置环来说足够了。实操心得查表法的精度不仅取决于表长还取决于x的范围。如果x超出表范围直接取端点值。因为Sigmoid在|x|8时已经非常接近0或1误差可以忽略。4. 从公式到代码位置环规划的完整实现4.1 整体控制结构先理清楚位置环在整个FOC里的位置。我的结构是三层串级位置环1kHz输入目标位置和当前位置输出目标速度。速度环5kHz输入目标速度和当前速度输出目标电流q轴分量。电流环20kHz输入目标电流和采样电流输出SVPWM占空比。位置环的输出不是直接给速度环而是经过Sigmoid规划后的位置指令再差分得到速度前馈。这样速度环跟踪的是一条平滑的速度曲线而不是阶跃速度指令。具体来说每个位置环周期做这几件事计算当前规划时间t。用Sigmoid函数算出归一化位置σ。实际位置指令 起始位置 行程×σ。位置指令差分得到速度前馈。位置误差 位置指令 - 实际位置经过P控制器得到速度修正。最终速度指令 速度前馈 速度修正。4.2 Q16定点Sigmoid查表实现先定义表长和范围#define SIGMOID_TABLE_SIZE 256 #define SIGMOID_X_MIN (-8.0f) #define SIGMOID_X_MAX (8.0f) #define SIGMOID_X_STEP ((SIGMOID_X_MAX - SIGMOID_X_MIN) / (SIGMOID_TABLE_SIZE - 1)) static const int32_t sigmoid_table[SIGMOID_TABLE_SIZE] { ... };表的生成可以在PC上算好直接以常量数组形式放进代码。每个表项是Q16格式的Sigmoid值。查表函数int32_t sigmoid_q16(int32_t x_q16) { // 把Q16的x转换成表索引 // x_q16 / 65536.0f 得到实际x // (x - X_MIN) / X_STEP 得到浮点索引 int32_t x_int x_q16 16; // 整数部分 int32_t x_frac x_q16 0xFFFF; // 小数部分 // 计算索引这里用定点运算避免浮点 // index (x - X_MIN) / X_STEP // 预先算好 1/X_STEP 的Q16值 int32_t index_q16 (x_q16 - (int32_t)(SIGMOID_X_MIN * 65536)) * INV_STEP_Q16 16; int32_t idx index_q16 16; int32_t frac index_q16 0xFFFF; if (idx 0) return sigmoid_table[0]; if (idx SIGMOID_TABLE_SIZE - 1) return sigmoid_table[SIGMOID_TABLE_SIZE - 1]; // 线性插值 int32_t y0 sigmoid_table[idx]; int32_t y1 sigmoid_table[idx 1]; return y0 (((y1 - y0) * frac) 16); }这段代码里INV_STEP_Q16是1/X_STEP的Q16表示。整个函数没有一次浮点运算全部是整数乘加和移位。4.3 位置规划器的状态机位置规划器需要管理一次运动的生命周期typedef enum { PLAN_IDLE, PLAN_RUNNING, PLAN_DONE } plan_state_t; typedef struct { plan_state_t state; int32_t start_pos_q16; int32_t delta_pos_q16; int32_t k_q16; int32_t T_q16; int32_t t_q16; int32_t dt_q16; int32_t pos_cmd_q16; int32_t vel_ff_q16; } pos_planner_t;启动一次规划void planner_start(pos_planner_t *p, int32_t target_pos_q16) { p-start_pos_q16 current_pos_q16; p-delta_pos_q16 target_pos_q16 - current_pos_q16; p-t_q16 0; p-state PLAN_RUNNING; }每个周期更新void planner_update(pos_planner_t *p) { if (p-state ! PLAN_RUNNING) return; p-t_q16 p-dt_q16; if (p-t_q16 p-T_q16) { p-pos_cmd_q16 p-start_pos_q16 p-delta_pos_q16; p-vel_ff_q16 0; p-state PLAN_DONE; return; } // 计算Sigmoid自变量k * (t - T/2) int32_t x_q16 (p-k_q16 * (p-t_q16 - (p-T_q16 1))) 16; int32_t sigma_q16 sigmoid_q16(x_q16); int32_t new_pos p-start_pos_q16 ((p-delta_pos_q16 * sigma_q16) 16); // 速度前馈 (new_pos - old_pos) / dt p-vel_ff_q16 ((new_pos - p-pos_cmd_q16) 16) / p-dt_q16; p-pos_cmd_q16 new_pos; }这里有个细节速度前馈用差分计算而不是对Sigmoid求导。差分更简单而且和位置指令严格一致不会出现前馈和指令不匹配的问题。4.4 参数整定的实操步骤我调这套东西的步骤是这样的先定T。根据机械节拍要求比如要求0.5秒内走完90度那T就取0.5秒。注意T是过渡时间不是总时间实际到位时间会略大于T。初选k。让k·T≈10比如T0.5k20。观察起步和停止。如果起步有冲击减小k如果中间段太慢增大k。微调位置环P。位置环的P增益不要太大因为前馈已经提供了主要的速度指令。P的作用只是消除稳态误差。看电流波形。如果q轴电流在起步和停止时有尖峰说明加速度还是太大继续减小k。我实测的一组参数T0.3sk40位置环P8速度环PI0.5/0.01电流环PI1.2/0.005。这套参数下90度行程的到位时间约0.35秒超调小于0.5度电流峰值约为额定值的1.5倍。常见问题如果发现位置始终到不了目标检查Sigmoid表的范围是否覆盖了k·T/2。当k·T12时x的最大值约为6在-8到8范围内没问题。但如果k·T超过16x会超出表范围导致端点值被截断位置出现偏差。5. 实测现象与排查记录5.1 起步抖动多半是k太大第一次跑的时候我用的是k80T0.2sk·T16。结果电机起步时“咯噔”一下声音很明显。用示波器看q轴电流起步瞬间有一个尖峰。原因分析k·T16时Sigmoid曲线在起点附近的变化率仍然较大对应的加速度不为零。虽然理论上Sigmoid的导数在无穷远处才为零但实际工程中要求起点加速度足够小。解决把k降到40k·T8起步明显柔和。但k·T8时曲线两端不够平缓停止时又有点慢。最后折中取k50T0.25k·T12.5效果比较平衡。5.2 到位振荡位置环P太大平滑了起步之后发现到位时电机在目标位置附近来回蹭幅度大概1度左右。一开始以为是速度环的问题调了半天没效果。后来把位置环P从20降到8振荡消失。原因是Sigmoid规划已经提供了速度前馈位置环P只需要修正很小的跟踪误差。P太大反而会把前馈的平滑性破坏掉。5.3 定点溢出乘法顺序很关键在计算delta_pos_q16 * sigma_q16时如果delta_pos_q16比较大比如对应360度sigma_q16接近65536乘积可能超过int32范围。我的处理是先把delta_pos_q16右移8位乘完再左移8位。这样损失一点精度但避免了溢出。对于位置控制来说1/256的精度损失完全可以接受。int32_t delta_shifted p-delta_pos_q16 8; int32_t pos_offset (delta_shifted * sigma_q16) 8;5.4 常见问题速查表现象可能原因排查方向解决方法起步冲击大k太大或T太小看k·T是否14减小k或增大T到位振荡位置环P太大观察位置误差符号减小P依赖前馈位置到不了Sigmoid表范围不够检查k·T/2是否8扩大表范围或减小k运动中间卡顿查表插值精度不够看表长是否128增加表长到256或512速度前馈噪声大差分周期太短检查dt是否0.5ms增大dt或加低通滤波定点溢出乘法中间结果超32位检查大数相乘先移位再乘5.5 一个容易被忽略的细节初始位置对齐位置环规划的前提是当前转子位置已知。对于有感FOC编码器或霍尔能提供绝对位置没问题。但对于无感FOC转子初始位置检测本身就有误差位置环规划再平滑起点错了后面全错。我的做法是在位置环使能之前先做一次转子初始位置检测把检测结果作为位置环的起始位置。如果检测误差较大可以在低速下先跑一段速度环让观测器收敛后再切位置环。6. 还能怎么扩展几个实用的改进方向6.1 变参数Sigmoid起步柔、中间快、停止稳固定k的Sigmoid曲线是对称的但实际机械往往希望起步更柔、中间更快、停止更稳。可以把k做成随时间变化的前1/3段用较小的k让起步柔和。中间1/3段用较大的k加快中间段。后1/3段再用较小的k让停止平稳。实现上就是根据t/T的值查不同的k或者用分段函数。代价是参数变多调参复杂度上升。对于大多数应用固定k已经够用。6.2 前馈加加速度限制Sigmoid规划的速度前馈是平滑的但加速度前馈速度前馈的差分在起点和终点仍然有较小突变。如果机械对加加速度jerk敏感可以对速度前馈再做一次Sigmoid平滑相当于两级串联。这样加加速度也连续但响应会变慢。6.3 多段连续运动的位置拼接如果要做连续轨迹比如从A到B再到C可以在第一段规划还没完全结束时就开始第二段。关键是保证位置和速度在拼接点连续。我的做法是在PLAN_DONE之前如果收到新目标就把当前位置作为新起点重新启动规划。由于Sigmoid在终点速度接近零拼接时的速度不连续很小实测几乎看不出来。6.4 在FPGA上实现定点Sigmoid如果位置环要在FPGA上跑Q16定点数同样适用。FPGA的优势是可以做全并行查表和插值一个时钟周期就能出一个Sigmoid值。表可以放在Block RAM里256×16bit只占一小块。对于多轴联动每个轴一个独立的规划器互不干扰。我在实际项目里对比过STM32F103软件查表一次Sigmoid计算约2微秒FPGA硬件查表一次约20纳秒。对于1kHz的位置环两者都绰绰有余但FPGA能支持更高的环路频率。6.5 和MPTA控制的配合如果电机要跑MPTA最大转矩电流比位置环输出的速度指令最终会变成iq和id的参考值。Sigmoid规划只负责位置和速度的平滑不改变MPTA的电流分配逻辑。两者是正交的可以独立调试。我个人在实际操作中的体会是位置环的平滑性七分靠规划三分靠PID。规划做得好PID参数可以放得很宽系统依然稳定规划做得糙PID再精细也补不回来。Sigmoid加Q16这套组合最大的价值就是把复杂的轨迹规划压缩成了一个公式和一张表在资源受限的MCU上也能跑出接近伺服驱动器的运动品质。最后再分享一个小技巧调参时把位置指令、实际位置、速度前馈、q轴电流四个量同时用串口传到上位机画波形。光看电机转是看不出问题的波形一摆出来哪里不平滑、哪里有过冲一目了然。我习惯用1kHz的采样率传波特率115200一帧8个字节四个量刚好32字节传一秒钟的数据也就3.6KB上位机用Python的matplotlib实时画调试效率比盲调高十倍不止。
