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

第一性原理的“三观”控制本质让“实际值”听话地等于“目标值”。数学本质微分方程描述变化规律 线性代数坐标变换 概率论处理噪声。物理本质能量守恒电机扭矩电功率、牛顿力学Fma、电磁感应动生电动势。第一部分电机控制算法FOC SVPWM1.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生成c// 阶段1ADC采样物理层- 获取三相电流 Ia, Ib, Ic (Ic -Ia - Ib) // 阶段2Clarke变换数学投影 float I_alpha Ia; float I_beta (Ia 2*Ib) / 1.732; // 阶段3Park变换解耦需要转子角度θ来自编码器 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; // 扭矩分量希望跟随目标 // 阶段4PID计算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; // 阶段6SVPWM实现伏秒平衡计算 // 计算扇区计算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生成c// 阶段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));第三部分控制理论基础观测器 前馈 PID3.1 第一性原理数学/动力学模型PIDP比例“现在”。误差越大打得越狠。物理学本质弹簧力 F kx。I积分“过去”。消除静差。物理学本质累加器对抗恒定重力负载。D微分“未来”。预测趋势抑制震荡。物理学本质阻尼器 F c*v速度越快阻力越大。前馈补偿Feedforward第一性原理开环预测。PID是“出了错再改”前馈是“我预判了你的预判”。实例电机要克服重力举起10kg物体。如果等PID看到位置误差再加大电流反应慢且会超调。正确的物理做法根据动力学方程扭矩 重力矩 惯性矩直接把基础电流算好给出去PID只需要补偿剩下的5%外部扰动即可。状态观测器Luenberger Observer / Kalman Filter第一性原理“软传感器”。有些物理量测不准或者太贵比如速度微分噪声大。逻辑建立一个数学模型如位置 上次位置 速度*dt。利用模型算出“估计位置”利用传感器测出“实际位置”。两者做差乘以一个增益观测器增益把误差反馈给模型中的“速度”让估计速度无限逼近真实速度。3.2 代码分段思路位置-速度-电流三环串级代码部分为AI生成c// 阶段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); // Id0控制 // 阶段5进阶状态观测器估算速度以替代微分器 // 建立状态方程 x(k1) A*x(k) B*u(k) // 位置_est(k1) 位置_est(k) 速度_est(k)*dt 0.5*加速度*dt^2 // 速度_est(k1) 速度_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结果震荡爆炸。正确物理顺序由内向外电流环最内层- 速度环 - 位置环最外层。内环没调好外环不可能好。实操先给电流环Iq一个阶跃信号只调P直到电流波形上升沿不震荡阻尼比≈0.707。速度环只用P给目标速度增大P直到等幅震荡记录震荡周期Tc和增益Kc。根据齐格勒-尼科尔斯表格Kp 0.45*KcKi 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正反。最后永远要加安全保护过流、过压、过热因为再完美的算法也扛不住物理世界的短路。秦时明月汉时关万里长征人未还。——《出塞》·王昌龄