电机控制算法（FOC、SVPWM、电流环速度环）姿态解算（四元数、欧拉角、互补滤波、卡尔曼）控制理论基础（状态观测器、PID参数整定、前馈补偿）

CYY1921

关注

原创 2026-07-12 15:10:47 287 阅读 AI 写同款· GEO 优化 ›

FOC伺服电机控制中前馈控制方案的原理和设计方法 FOC伺服电机前馈控制原理及应用摘要：前馈控制通过"预见性补偿"提升系统性能，其核心是根据系统动力学模型提前计算控制量。在FOC伺服系统中，主要采用速度前馈（补偿粘性摩擦）和加速度前馈（补偿惯性），结合运动学模型计算出转矩指令。前馈控制能显著减小跟踪误差、提高响应速度、降低对PID的依赖，并提升系统带宽。但效果依赖于模型准确性，需结合反馈控制共同作用。实践中需通过系统辨识获取准确模型参数，并进行参数调优以获得最佳性能。阅读详情

第一性原理的“三观”

控制本质：让“实际值”听话地等于“目标值”。
数学本质：微分方程（描述变化规律） + 线性代数（坐标变换） + 概率论（处理噪声）。
物理本质：能量守恒（电机扭矩=电功率）、牛顿力学（F=ma）、电磁感应（动生电动势）。

第一部分：电机控制算法（FOC & SVPWM）

1.1 第一性原理（物理模型）

电机为什么转？ 初中物理：通电导体在磁场中受力 F = BIL（左手定则）。
直流电机好控制：因为电枢磁场和永磁体磁场天然垂直（正交），扭矩 = Kt * I。
交流电机（BLDC/PMSM）的痛点：定子磁场在旋转，转子也在旋转。如果直接通正弦波，磁场夹角不固定，扭矩会脉动。
FOC（矢量控制）的核心物理逻辑：通过算法，把旋转的交流信号“冻结”住，强行构造出一个“虚拟直流电机”模型。这样，我就能像控制直流电机一样，独立控制“励磁（D轴）”和“扭矩（Q轴）”。

1.2 关键技术点拆解（通俗版）

Clark变换：把三相（U,V,W）的数学投影，合并成两相静止坐标系（α,β）。（数学：等幅值变换，矩阵乘法）
Park变换：把静止的α,β坐标系，“乘以转子角度θ的旋转矩阵”，得到旋转坐标系（D,Q）。此时，电流变成了直流量。
SVPWM（空间矢量脉宽调制）：逆变器只有8种开关状态（6个有效矢量+2个零矢量）。第一性原理是“伏秒平衡”——在一个开关周期内，让两个相邻有效矢量与零矢量进行组合，合成的平均效果等于我想要的任意方向的电压矢量。
- 画图秘诀：在复平面上画一个正六边形，内切圆就是最大不失真电压圆。

1.3 代码分段思路（电流环）

（代码部分为AI生成）

// 阶段1：ADC采样（物理层）-> 获取三相电流 Ia, Ib, Ic (Ic = -Ia - Ib)
// 阶段2：Clarke变换（数学投影）
float I_alpha = Ia;
float I_beta = (Ia + 2*Ib) / 1.732; 

// 阶段3：Park变换（解耦，需要转子角度θ，来自编码器）
float cos_theta = cos(angle); 
float sin_theta = sin(angle);
float I_d = I_alpha * cos_theta + I_beta * sin_theta;  // 励磁分量（希望为0）
float I_q = -I_alpha * sin_theta + I_beta * cos_theta; // 扭矩分量（希望跟随目标）

// 阶段4：PID计算（PI控制器，这里就是简单的离散累加）
float V_d = PI_Controller(I_d_ref - I_d, &pid_d); 
float V_q = PI_Controller(I_q_ref - I_q, &pid_q);

// 阶段5：逆Park变换（把电压矢量从旋转坐标变回静止坐标）
float V_alpha = V_d * cos_theta - V_q * sin_theta;
float V_beta = V_d * sin_theta + V_q * cos_theta;

// 阶段6：SVPWM实现（伏秒平衡计算）
// 计算扇区，计算X,Y,Z，计算T1,T2，赋值给定时器比较寄存器
SVPWM_Generate(V_alpha, V_beta);

第二部分：姿态解算（四元数 & 滤波）

2.1 第一性原理（物理/几何模型）

姿态是什么？ 就是刚体在三维空间中的旋转。
欧拉角的痛点（万向节死锁）：想象一下你拿着手机，当俯仰角达到±90°时，横滚和偏航的旋转轴重合了，自由度丢失。数学上表现为三角函数分母为零。
四元数的物理本质：它不是一个抽象数字，而是“轴-角”表示法的复数扩展。任何一个旋转，都可以理解为“绕某个空间单位向量 [x,y,z] 旋转 θ 度”。四元数 = cos(θ/2) + sin(θ/2)(xi + yj + zk)。它没有任何奇异性，适合积分。

2.2 关键技术点拆解

陀螺仪（高频）：测角速度。第一性原理：角度 = ∫角速度 dt。缺点：积分累积漂移（温飘）。
加速度计（低频）：测重力矢量。第一性原理：静止时，重力方向就是“下”。缺点：运动加速度会干扰（噪声大）。
互补滤波（Complementary Filter）：本质是一阶低通+高通滤波器。“短时间相信陀螺仪，长时间相信加速度计”。数学公式：角度 = 0.98(角度 + gyrodt) + 0.02*(accel_angle)。（系数和为1，保证幅频特性互补）。
卡尔曼滤波（终极解法）：第一性原理是“最优贝叶斯估计”。它不关心哪个传感器好哪个坏，它只关心协方差矩阵。预测（根据物理模型推一步）-> 更新（根据测量值修正一步）。如果陀螺仪方差小，K增益就小，更信陀螺仪；反之则更信加速度计。

2.3 代码分段思路（姿态更新）

（代码部分为AI生成）

// 阶段1：传感器读取（物理层）
gyro = Read_Gyro(); // rad/s
accel = Read_Accel(); // m/s^2

// 阶段2：加速度计求姿态（提供绝对参考，仅适用于静止/低速）
// 利用重力在三个轴的分量，反推横滚和俯仰（atan2函数）
float roll_acc = atan2(accel.y, accel.z);
float pitch_acc = atan2(-accel.x, sqrt(accel.y*accel.y + accel.z*accel.z));

// 阶段3：四元数微分方程更新（核心：一阶龙格-库塔法）
// 物理：q_dot = 0.5 * q ⊗ ω (四元数乘法)
quaternion q_new = q_old + 0.5 * dt * (q_old ⊗ gyro);

// 阶段4：利用加速度计修正（互补滤波在四元数层面的实现）
// 计算理论重力方向 vs 实测重力方向，做叉积得到误差
float error = cross_product(gravity_predicted, gravity_measured);
// 将误差通过PI控制器补偿到陀螺仪角速度上（消除漂移）
gyro_corrected = gyro + Kp * error + Ki * sum_error;

// 阶段5：归一化（数学上保证四元数模长为1，防止病态矩阵）
q_new = normalize(q_new);

// 阶段6：输出欧拉角（如果需要给位置环用）
float yaw = atan2(2*(q.w*q.z + q.x*q.y), 1 - 2*(q.y*q.y + q.z*q.z));

第三部分：控制理论基础（观测器 & 前馈 & PID）

3.1 第一性原理（数学/动力学模型）

PID：
- P（比例）：“现在”。误差越大，打得越狠。物理学本质：弹簧力 F = kx。
- I（积分）：“过去”。消除静差。物理学本质：累加器，对抗恒定重力负载。
- D（微分）：“未来”。预测趋势，抑制震荡。物理学本质：阻尼器 F = c*v（速度越快，阻力越大）。
前馈补偿（Feedforward）：
- 第一性原理：开环预测。PID是“出了错再改”，前馈是“我预判了你的预判”。
- 实例：电机要克服重力举起10kg物体。如果等PID看到位置误差再加大电流，反应慢且会超调。正确的物理做法：根据动力学方程 扭矩 = 重力矩 + 惯性矩，直接把基础电流算好给出去，PID只需要补偿剩下的5%外部扰动即可。
状态观测器（Luenberger Observer / Kalman Filter）：
- 第一性原理：“软传感器”。有些物理量测不准或者太贵（比如速度微分噪声大）。
- 逻辑：建立一个数学模型（如：位置 = 上次位置 + 速度*dt）。利用模型算出“估计位置”，利用传感器测出“实际位置”。两者做差，乘以一个增益（观测器增益），把误差反馈给模型中的“速度”，让估计速度无限逼近真实速度。

3.2 代码分段思路（位置-速度-电流三环串级）

（代码部分为AI生成）

// 阶段1：前馈计算（基于物理模型：牛顿第二定律）
// 目标加速度 = (Speed_cmd - Speed_last) / dt
float Torque_feedforward = Inertia * Target_Acceleration + Gravity_Compensation;

// 阶段2：位置环（P控制）-> 生成速度目标
float speed_target = Kp_pos * (pos_target - pos_feedback);
// 限制最大速度（防飞车）
speed_target = saturate(speed_target, MAX_SPEED);

// 阶段3：速度环（PI + 前馈）-> 生成扭矩目标（即电流环的Iq_ref）
float speed_error = speed_target - speed_feedback;
float torque_pid = Kp_speed * speed_error + Ki_speed * integral_speed_error;
float torque_total = torque_pid + Torque_feedforward; // 前馈叠加！
float i_q_target = torque_total / (Kt * 磁场系数); // 转换为电流

// 阶段4：电流环（PI高速执行，通常几十KHz）执行第一部分讲的FOC。
FOC_Update(i_q_target, 0); // Id=0控制

// 阶段5：（进阶）状态观测器估算速度，以替代微分器
// 建立状态方程 x(k+1) = A*x(k) + B*u(k)
// 位置_est(k+1) = 位置_est(k) + 速度_est(k)*dt + 0.5*加速度*dt^2
// 速度_est(k+1) = 速度_est(k) + 加速度*dt
// 误差 = 编码器位置 - 位置_est
// 速度_est = 速度_est + L * 误差  (L即为观测器增益)
float velocity_estimated = Observer_Update(encoder_pos, current_cmd);

第四部分：给小白的三条“打通任督二脉”实用建议

工程落地建议：

建议一：务必记住“采样时间（Ts）”是最重要的“物理常量”

第一性原理：离散控制是数字世界的积分器。
坑：把DSP的定时器中断频率设成10kHz，但电流环PID的积分项累加却放在1kHz的主循环里。
对策：所有积分项 integral += error * Ts，这个 Ts 必须精确对应中断时间。写代码时，把 Ts 作为一个全局常量，且每次中断触发时，利用定时器捕获计数值计算真实 dt，用以补偿中断响应延迟。

建议二：参数整定的“倒序法”和“临界比例度法”

错误做法：上来就调位置环P，结果震荡爆炸。
正确物理顺序（由内向外）：电流环（最内层）-> 速度环 -> 位置环（最外层）。内环没调好，外环不可能好。
实操：
1. 先给电流环（Iq）一个阶跃信号，只调P，直到电流波形上升沿不震荡（阻尼比≈0.707）。
2. 速度环只用P，给目标速度，增大P直到等幅震荡，记录震荡周期 Tc 和增益 Kc。
3. 根据齐格勒-尼科尔斯表格，Kp = 0.45*Kc，Ki = 0.54*Kc/Tc。这个公式来源于经典控制理论的根轨迹法，绝不是瞎猜。

建议三：离散化的“双线性变换（Tustin）”

第一性原理：模拟域的积分器 1/s 在数字域不能直接用 T/(z-1)（前向欧拉），因为系统会不稳定（把左半平面的极点映射到单位圆外）。
必杀技代码写法：对于积分项，用 增量式PID 或 后向欧拉。
(代码部分为AI生成)
c

// 积分限幅（Anti-windup）必须加！
integral += error * Ts;
if (integral > MAX) integral = MAX;
if (integral < -MAX) integral = -MAX;

建议四：工作中遇到“震荡”，先查“相位延迟”

姿态解算为什么飘？检查四元数更新时，陀螺仪数据是否经过了低通滤波（低通会带来相位滞后）。
FOC为什么高速没力？检查SVPWM的过调制区，当电压矢量进入六边形边界时，需要做“最小相位误差”处理，否则电压饱和会引起电流环震荡。

最后的最后（终极实战心法）

“不要试图一次性写出完美的卡尔曼滤波或FOC。第一性原理在代码中体现为‘分模块验证’。

先点灯测中断频率（确保Ts准确）。

开环给SVPWM看电机能不能转（验证硬件和坐标变换方向，方向错了，闭环就是正反馈！）。

锁住转子，测电流环的阶跃响应（验证PI正反）。

最后，永远要加安全保护（过流、过压、过热），因为再完美的算法也扛不住物理世界的短路。