机器人关节无传感器FOC实战：5维状态量EKF转子位置与转速估算七步讲透

机器人关节与云台要在密封、高温、走线受限的壳体里省掉位置传感器，无传感器 FOC 是主流路线。反电势过零法只能给出每 60° 电角度一次的离散换相点，转速大范围变化时过零点还会产生相移，低速段更是信噪比不足；扩展卡尔曼滤波（EKF） 的做法是把三相电流、转子角速度与转角一起放进状态向量，用可测的电压和电流连续估算转子瞬时位置与速度，稳态下估计值几乎与真值重合。三条可落地的结论：第一，EKF 是"预报 + 校正"的两步递推，本质是用电机数学模型补全测不到的转角；第二，状态量取 5 维（i_a、i_b、i_c、ω、θ）、观测量取 3 维（三相电流）即可闭环；第三，非线性只来自 sin/cos(θ)，用雅可比矩阵线性化后就是标准卡尔曼递推。下面用四张表把模型、参数、闭环与落地步骤讲透。

一、关节为什么必须放弃"离散过零"这类位置法

无传感器位置检测的工程路线主要有三种：反电势过零检测（六步方波常用）、磁链/反电势积分观测器、以及基于状态估计的 EKF。三者对比如下，差异集中在"输出的是点还是连续量"。

对比维度	反电势过零检测	反电势积分/磁链观测	EKF 状态估计
输出形式	每 60° 电角度一个离散换相点	连续角度，但依赖积分初值	连续角度 + 角速度，均为状态量
转速适应性	转速大范围变化时过零点相移明显	低速积分漂移，高速受参数误差影响	预报模型自动跟随转速，全速域一致
低速性能	反电势太小，信噪比不足	积分误差累积，易发散	靠模型预测补位，低速可用
噪声处理	靠硬件滤波，易引入相位延迟	无显式噪声建模	用 Q/R 协方差显式建模过程与测量噪声
算力需求	极低（比较器即可）	中（积分与坐标变换）	高（每周期矩阵运算，需 FPU/DSP）
适用场景	风机、水泵类方波驱动	中高速、参数稳定的场合	要连续角度、要低速性能的关节与伺服

关键判断是位置信息的连续性。FOC 的 Park 变换每个 PWM 周期都要用到电角度 θ，若 θ 只在换相点更新，转速一变就会产生相位滞后，表现为电流环抖动、转矩脉动增大。EKF 每个采样周期都输出新的 θ 和 ω，这正是矢量控制需要的输入。

二、EKF 的两步循环：预报与校正

卡尔曼滤波在每一次采样里都分成两步：预报与校正。预报阶段用电机数学模型，由当前状态推算下一时刻状态会怎样变化，并同步推算观测量的预测值；校正阶段用实际测量到的电流，与预测的电流相减，差值乘上卡尔曼增益后反过来修正状态估计。每一步还要同步更新状态量的协方差——它衡量估计的准确度，增益正是由协方差推出来的：对估计没把握时协方差大、增益大、校正幅度大；对估计有把握时协方差小、增益小、测量噪声被压低。

三、建模：5 维状态量、3 维观测量

关节电机是非线性系统，变量之间不存在线性组合关系，因此要把模型里的非线性关系用泰勒级数（一般取到二级）展开逼近，这就是"扩展"卡尔曼滤波的由来。系统采用的电机模型建立在 abc 参考框架下，各量的定义如下表。

类别	符号与维数	物理含义	工程取值示例（关节电机）
状态量	x=(ia, ib, ic, ω, θ)，5 维	三相电流、转子角速度、转子转角	i 范围 ±5A；ω 0～315 rad/s；θ 0～2π
观测量	i=(ia, ib, ic)，3 维	实测三相电流	分流电阻 + 运放，采样与 PWM 同步
输入量	u=(ua, ub, uc)，3 维	各采样周期内相电压的平均值	由 PWM 占空比 × 母线电压反推
系统模型	xk+1=Fk(xk)·xk+Gk·uk+wk	状态转移，Fk 因含 sin/cos(θ) 而非线性	需用 Fk 对 xk 的雅可比矩阵
观测模型	ik+1=H·xk+1+vk	H 为线性观测矩阵，直接取三相电流	H=[I3×3 03×2]
过程噪声	wk，协方差 Q	模型误差（参数偏差、离散化误差）	按 Ls、Rs、J 偏差量级标定
测量噪声	vk，协方差 R	电流采样噪声与量化误差	按 ADC 有效位数与采样电阻温漂标定

这里最容易忽略的是输入量 u 的来源。EKF 需要的是相电压，而驱动器能直接给的是 PWM 占空比；用"占空比 × 母线电压"反推相电压时，必须扣除死区与管压降，否则低速小占空比时电压误差会明显放大，位置估计出现稳态偏差。

四、EKF 递推五步与雅可比矩阵

把连续模型用一阶欧拉离散，得到可直接编程的递推形式。电机电气方程与机械方程联立后，非线性只集中在反电势项（含 sin θ、cos θ），因此雅可比矩阵有解析解，不需要数值求导。

/* EKF 单周期递推（状态 x = [ia, ib, ic, w, theta]）
 * 参数：Rs 定子电阻, Ls 相电感, ke 反电势系数, J 转动惯量, B 阻尼, Ts 采样周期
 * 说明：仅示意数学结构，实际部署建议用定点/单精度 FPU 并做矩阵降维优化
 */

/* ---- ① 预报：状态外推（欧拉离散） ---- */
w_pred   = x[3] + Ts * (Te - B*x[3]) / J;          /* 机械方程：dω/dt=(Te-Bω)/J */
th_pred  = x[4] + Ts * w_pred;                      /* dθ/dt = ω */
for (k = 0; k < 3; k++) {
    /* 电气方程：L·di/dt = u - R·i - e，e 为含 sin/cos(θ) 的反电势 */
    e_k = ke * w_pred * sinf(th_pred - k*TWO_PI_3);
    x_pred[k] = x[k] + Ts * (u[k] - Rs*x[k] - e_k) / Ls;
}
x_pred[3] = w_pred;  x_pred[4] = th_pred;

/* ---- ② 预报协方差：P = F·P·Fᵀ + Q ---- */
jacobian_F(x_pred, F);          /* 解析雅可比：∂f/∂x，非线性项在 ∂e/∂θ 与 ∂e/∂ω */
mat_mul(F, P, FP);              /* FP = F·P */
mat_mul_T(FP, F, P_pred);       /* P_pred = F·P·Fᵀ */
mat_add(P_pred, Q, P_pred);

/* ---- ③ 计算卡尔曼增益：K = P⁻Hᵀ(H P⁻Hᵀ + R)⁻¹ ---- */
/* H = [I3x3 0]，故 HP⁻Hᵀ 取 P⁻ 左上 3x3 块，R 为 3x3 测量噪声阵 */
S = P_pred_top3x3 + R;
mat_inv3(S, S_inv);
mat_mul_Ht(P_pred, K);          /* 简化后 K 为 5x3 */
mat_mul(K, S_inv, K);

/* ---- ④ 校正：用实测电流 i_meas 修正状态 ---- */
for (k = 0; k < 3; k++) resid[k] = i_meas[k] - x_pred[k];   /* 新息 = 实测 - 预测 */
mat_mul_vec(K, resid, dx);
for (i = 0; i < 5; i++) x[i] = x_pred[i] + dx[i];

/* ---- ⑤ 更新协方差：P = (I - K·H)·P⁻ ---- */
mat_mul_KH(K, KH);              /* KH 为 5x5，H 选择前三列 */
mat_sub_identity(KH, IKH);
mat_mul(IKH, P_pred, P);

/* 输出：x[4]=转子转角 θ，x[3]=角速度 ω，直接送 Park 变换与速度环 */

两个工程要点：协方差矩阵 P 要做对称化与限幅，否则单精度浮点在长时间递推后会因舍入误差失去正定性而发散；角度 θ 要归一化到 [0, 2π)，并且越过 0/2π 边界时不能直接做差，否则新息会出现 2π 的跳变。

五、双闭环分工：电流环 P、速度环 PI

系统采用双闭环：内环检测相电流，与速度环给出的参考电流比较，做电流环（力矩）控制；外环用 EKF 估计的转速与环境/负载参考速度比较，做速度环控制。电流内环用比例调节，突加给定转速时迅速抬高占空比、消除电流偏差；速度外环用 PI 调节，稳态时构成无静差调速系统。

环节	调节器	输入	输出	设计要点
电流内环	P（比例）	参考电流 − 实测相电流	电压指令 / PWM 占空比	带宽取 PWM 频率的 1/10～1/5，20kHz PWM 对应 2～4kHz
速度外环	PI	参考转速 − EKF 估计转速	参考电流幅值	带宽取电流环的 1/10 以下，约 200～400Hz
EKF 观测器	状态估计	相电压 u、实测电流 i	转角 θ、角速度 ω	采样周期与电流环一致，Q/R 分别按参数偏差与采样噪声标定
速度参考	—	温度/负载指令	参考转速	热负荷大则升速，达到目标后降到负载匹配转速连续运行

六、七步落地设计

第 1 步：确认算力与采样率

5 维状态、3 维观测的 EKF 每个周期要做十余次 5×5 级矩阵运算。以 PWM 20kHz（T_s=50µs）计，单周期乘加量在数千次量级，裸跑定点 MCU 会很吃力，建议选带 FPU 的 STM32 F4/G4 系列或专用电机控制 DSP，并开启单精度硬件浮点。

第 2 步：标定电机参数 R_s、L_s、k_e、J

EKF 的预报精度完全依赖模型参数。R_s 用直流伏安法测，L_s 用 LCR 表或阶跃电流响应测，k_e 由空载反电势—转速曲线斜率得到，J 用已知惯量盘的加减速曲线反推。参数偏差会直接表现为 θ 的稳态误差。

第 3 步：把相电压反推做准

用占空比 × 母线电压得到 u，并扣除死区与开关管压降；母线电压要实测而不是取标称值，否则电池供电的关节在放电过程中估计会漂。

第 4 步：Q 与 R 分别标定

R 由电流采样噪声决定：分流电阻阻值、运放噪声、ADC 有效位数共同决定，可静态采样数百点统计方差。Q 由参数偏差与离散化误差决定，先给对角阵，再按"低速不发散、高速不滞后"微调。

第 5 步：给初值与启动策略

上电时 θ 未知，可用开环强拖或高频注入把转子拖到已知位置后再切入 EKF 闭环；否则初值误差大、P 初值也大，容易出现启动瞬间的角度跳变。

第 6 步：做数值稳定性处理

P 矩阵每周期对称化（P←(P+Pᵀ)/2）并限制对角元上下界；角度归一化到 [0,2π)；新息做角度环绕处理，避免 0/2π 边界跳变。

第 7 步：把估计值接进 FOC 与速度环

θ 送 Park/反 Park 变换，ω 送速度环反馈。EKF 与电流环同周期运行，速度环降频运行，CPU 占用才可控。

七、六个最容易踩的坑

现象	根因	处理
低速时角度估计缓慢漂移	相电压反推未扣死区与管压降，小占空比误差占比大	实测母线电压，按死区时间与管压降修正 u
长时间运行后发散	单精度浮点舍入使 P 失去正定性	P 每周期对称化并限幅，必要时改双精度或平方根滤波
过 0/2π 时电流尖峰	角度未做环绕处理，新息出现 2π 跳变	角度归一化 + 新息环绕修正
高速段估计滞后、转矩脉动大	Q 过小，模型过度自信，跟不上转速突变	适度增大 ω、θ 对应的 Q 分量
电流噪声大导致角度抖动	R 标定过小，测量噪声被过度信任	按 ADC 有效位数重标 R，必要时先做电流滑动平均
启动瞬间反转或堵转	θ 初值错误、P 初值过大	先用开环强拖或高频注入定位，再切 EKF 闭环

八、总结

对要求连续角度、低速性能与全速域一致性的机器人关节，EKF 的价值在于把"测不到的转角"变成可用数学模型连续估算的状态量：状态量取 5 维（i_a、i_b、i_c、ω、θ），观测量取 3 维（三相电流），非线性只来自反电势的 sin/cos 项，用雅可比矩阵线性化后即得标准卡尔曼递推。它替代的是位置传感器，不是电流环。

落地记住三点：相电压反推必须扣死区与管压降、P 矩阵必须对称化限幅、角度与新息必须做环绕处理。把这三件事做对，再让电流环与速度环各司其职，无传感器关节的低速平稳性和转速突变响应会明显优于过零检测方案。

还想接着看

• STM32高级定时器TIM1驱动机器人关节电机——有编码器时 FOC 的定时器与采样实现，可与本文无传感器方案互为对照
• 机器人关节无传感器BLDC驱动实战：ST72141K硬件换相与限流七步讲透——纯硬件过零换相的低成本路线
• 传感器数字滤波算法选型与实现：从限幅平均到卡尔曼滤波——卡尔曼滤波在传感测量侧的基础版本
• IR217x线性电流传感器在电机驱动中的设计指南——EKF 观测量（相电流）的采样前端设计

本文基于《基于扩展卡尔曼滤波的无位置传感器直流无刷电机控制》等技术资料整理优化，并结合机器人关节无传感器 FOC 的工程实践进行了系统化梳理。