Skip to content

About

FOC servo simulation for robot joint modules: SVPWM, cascaded loops, cogging compensation, FOC vs six-step

Resources

Stars

0 stars

Watchers

0 watching

Forks

Latest commit

 

History

1 Commit

Folders and files

Repository files navigation

foc-joint-sim · 关节模组 FOC 伺服仿真

ci python

一个不依赖任何机器人框架的永磁同步电机(PMSM)磁场定向控制仿真:从电机模型、SVPWM、 三环控制到齿槽转矩补偿,全部用 NumPy 手写,只为了把「关节模组电控」这件事讲清楚。

面向的岗位问题很具体:做关节模组,你得知道为什么位置阶跃会超调、齿槽转矩怎么补、 电流环带宽不够时 FOC 为什么还不如六步方波。 这个项目把这三个问题都做成了可复现的实验。

pip install -r requirements.txt   # numpy / matplotlib
python run_all.py                 # 跑完四个实验,图输出到 results/

1. 模型

项目 取值
极对数 p 7(14 极 12 槽,典型协作关节/云台构型)
相电阻 R 1.2 Ω
d/q 电感 Ld = Lq 0.9 mH(表贴式,无磁阻转矩)
永磁磁链 ψ 0.012 Wb
转矩常数 ke = 1.5·p·ψ 0.126 N·m/A
转动惯量 J / 阻尼 b 1.2e-4 kg·m² / 1.0e-4 N·m·s/rad
母线电压 Vdc 24 V
相电流限幅 5 A

反电势取 e_k = -ωe·ψ·sin(θe + φk),φk = 0 / -2π/3 / +2π/3。齿槽转矩按 τ_cog(θm) = Σ amp_k·sin(k·p·θm + ph_k) 叠加 1/2/3 次谐波,幅值 12 / 6 / 3 mN·m, 用来测试补偿算法是否真的有效。

积分在 α-β 静止坐标系下做 RK4(src/sim.py,dt_ctrl=1e-4、每拍 5 个子步、控制零阶保持)。 不在 a-b-c 三相下直接积分:浮点误差会累积出虚假的零序电流,dq 电流就再也对不上了。

2. 控制器

位置 P ──► 速度 PI ──► iq_ref(+齿槽补偿) ──► dq 电流 PI + 解耦前馈 ──► 电压圆限幅 ──► 逆 Park ──► SVPWM

几个工程细节,都不是教科书上会强调但真机上一定会踩的:

  • 条件积分抗饱和:输出饱和且误差仍在往饱和方向推时,停止积分;退出饱和立刻恢复。 早先用反算(back-calculation)版本,位置阶跃时会先攒一大坨积分再反向冲出去。
  • SVPWM 用 min-max 共模注入,母线利用率 1/√3,比正弦调制多 15%。
  • 电压圆限幅 |v_dq| ≤ Vdc/√3,而不是分别限 d、q 轴——分别限会得到不存在的电压矢量。
  • 齿槽补偿前馈:把 τ_cog 除以 ke 直接加到 iq_ref 上,抵消位置相关的转矩波动。
  • 解耦前馈 v_d = -ωe·Lq·iq、v_q = ωe·(Ld·id + ψ),否则高速时 dq 轴互相串扰。

3. 实验与结果

01 位置阶跃(90°,空载)

experiments/01_position_step.py → results/01_position_step.png

指标 值
上升时间(10→90%) 143 ms
超调 0 %
调节时间(±2%) 0.277 s
稳态误差 -0.01 %
峰值 q 轴电流 1.05 A(限幅 5 A,远未饱和)
峰值转速 1495 deg/s

02 齿槽转矩与补偿(6 deg/s 匀速爬 60°)

experiments/02_cogging.py → results/02_cogging.png

纹波峰峰值 RMS 恒定滞后
无补偿 0.442° 0.106° -0.40°
齿槽补偿 0.000° 0.000° -0.40°

补偿后纹波被完全吃掉,但恒定的 -0.4° 滞后还在——那是速度环的稳态误差(纯 P 位置环 + 带积分限幅的速度环在匀速下的必然结果),齿槽补偿本来就不该管它。这个区分很重要: 面试时把「纹波」和「滞后」分开说,说明你真的量过。

03 位置环 / 速度环带宽匹配

experiments/03_bandwidth.py → results/03_bandwidth.png

固定位置环增益,速度环按 ζ=1 整定(ki_w = J·wn²、kp_w = 2√(J·ki_w)),扫速度环带宽:

wn [rad/s] 20 30 40 60 80 120 160 240
阶跃后残余振荡 [deg] 133.5 89.6 59.2 19.5 5.17 0.32 0.05 0.03

结论:位置环带宽必须比速度环低 5–10 倍,否则会出现围绕某个中间频率的等幅振荡。 最初版本 kp_pos=12 + wn=30 就卡在 26 rad/s 的极限环上,位置永远停在目标附近抖。 这条经验直接对应真机上调参的顺序:先电流环,再速度环,最后才碰位置环。

04 FOC vs 六步方波

experiments/04_foc_vs_sixstep.py → results/04_foc_vs_sixstep.png

条件:20 rad/s 匀速、0.1 N·m 负载,统计 t > 1.2 s 的稳态段,用净转矩 τ_e - τ_cog 计算脉动。

方案 平均转速 平均 iq 转矩脉动 电流脉动
FOC(SVPWM) 20.0 rad/s 0.81 A 1 % 30 %
六步方波(电流斩波) 20.0 rad/s 0.81 A 191 % 174 %

踩过的坑:六步换相的导通表必须整体取负。本模型的反电势是 e_a = -ωψsinθe, 若按常规正号写表,电机一开始就直接反转、冲到 -101 rad/s,看起来像"控制器炸了", 实际是换相序列和反电势符号反了。修完后平均转速才落回 20.0 rad/s。

4. 已知限制

  • 表贴式电机(Ld = Lq),没有磁阻转矩;IPM 需要额外的 MTPA 查表。
  • 逆变器用理想开关,没建模死区、开关损耗与 PWM 谐波——真机上死区会让低速电流波形明显畸变。
  • 编码器可选量化(enc_counts),但没有建模延迟与齿槽误差。
  • 没有温度模型,R 随温度漂移(铜 0.4 %/K)会让低速观测器跑偏。

5. 如果继续做

  1. 接入 ROS 2:把 FOCController 包成 ros2_control 的 SystemInterface, 暴露 joint_state 与 position/velocity/effort 命令接口,正好串起简历里的 ROS 2 实验。
  2. 加 Sim2Real:用真机阶跃响应反推 J、b、ke(和 balance-sim2real 的 04/05 同一套方法)。
  3. 死区补偿 + 电流环带宽实测:给出死区电压与低速转矩脉动的关系曲线。

About

FOC servo simulation for robot joint modules: SVPWM, cascaded loops, cogging compensation, FOC vs six-step

Resources

Stars

0 stars

Watchers

0 watching

Forks

Releases

Packages

Contributors

Languages