1. ODrive控制器架构概述
ODrive是一款高性能开源电机控制器，其核心控制算法采用经典的串级控制架构。在v0.5.6版本固件中，Controller模块负责实现从位置指令到扭矩输出的完整控制链。整个控制流程严格遵循"位置环→速度环→扭矩输出"的层级结构，这种设计在工业运动控制领域被广泛采用，因其具有良好的稳定性和可调性。
控制器内部使用turn（转）作为角度基本单位，1 turn等于2π弧度。这种单位选择在旋转机械控制中更为直观，特别是对于多圈绝对位置控制场景。所有相关参数（如速度、加速度）都基于这个单位体系进行计算，保持了整个控制链的数学一致性。
2. 核心数据结构与接口
2.1 状态量估计接口
控制器通过以下接口获取电机当前状态：
cpp复制auto pos_estimate_linear = pos_estimate_linear_src_.present();
auto pos_estimate_circular = pos_estimate_circular_src_.present();
auto vel_estimate = vel_estimate_src_.present();
这些接口抽象了底层传感器（如编码器）和估计算法（如锁相环PLL）的实现细节，使控制器只需关注控制逻辑本身。这种设计体现了良好的模块化思想，使得可以灵活更换不同的位置/速度估计方案而不影响控制核心。
2.2 控制模式枚举
控制器支持多种输入模式，通过input_mode参数选择：
INPUT_MODE_PASSTHROUGH：直接传递模式INPUT_MODE_POS_FILTER：位置滤波模式INPUT_MODE_VEL_RAMP：速度斜坡模式INPUT_MODE_TORQUE_RAMP：扭矩斜坡模式
每种模式对应不同的前馈处理策略，为不同应用场景提供灵活性。
3. 输入模式处理详解
3.1 直接传递模式(Passthrough)
在此模式下，控制器直接将输入指令传递给内部设定点：
cpp复制pos_setpoint_ = input_pos_;
vel_setpoint_ = input_vel_;
torque_setpoint_ = input_torque_;
数学上表示为：
θ_ref = θ_cmd
ω_ref = ω_cmd
τ_ff = τ_cmd
这种模式通常用于高级用户直接控制，或作为其他控制模式的基础。
3.2 位置滤波模式
这是最复杂的输入处理模式，实现了一个二阶跟踪滤波器：
cpp复制float delta_pos = input_pos_ - pos_setpoint_;
float delta_vel = input_vel_ - vel_setpoint_;
float accel = input_filter_kp_ * delta_pos + input_filter_ki_ * delta_vel;
torque_setpoint_ = accel * inertia;
vel_setpoint_ += dt * accel;
pos_setpoint_ += dt * vel_setpoint_;
其连续时间模型为：
θ̈ = ω_n²(θ_ref - θ) + 2ω_n(θ̇_ref - θ̇)
参数对应关系：
kp = ω_n²
ki = 2ω_n
这个滤波器本质上模拟了一个虚拟的"质量-阻尼"系统，通过合理设置ω_n(自然频率)可以平滑指令过渡，避免机械冲击。
4. 位置环实现分析
4.1 基本位置控制
位置环的核心代码极为简洁：
cpp复制pos_err = pos_setpoint_ - pos_estimate;
vel_des += pos_gain * pos_err;
数学表达式为：
e_θ = θ_ref - θ
ω_des = ω_ff + K_p^θ * e_θ
关键点：
- 位置环输出的是速度指令，而非直接扭矩
- 前馈速度ω_ff来自输入处理阶段
- 比例增益K_p^θ决定位置跟踪刚度
4.2 圆周位置处理
对于旋转应用，控制器提供了圆周位置误差计算：
cpp复制pos_err = wrap_pm(pos_err, circular_range);
数学上等效于：
e_θ = argmin|θ_ref - θ + kL|, k∈ℤ
这个处理确保：
- 误差始终取最短路径
- 支持无限旋转而不溢出
- 保持控制连续性
5. 速度环PI控制
5.1 基本实现
速度环采用经典的PI控制结构：
cpp复制v_err = vel_des - vel_estimate;
torque += vel_gain * v_err;
torque += vel_integrator_torque_;
数学模型为：
e_ω = ω_des - ω
τ = K_p^ω e_ω + K_i^ω ∫e_ω dt
5.2 抗饱和处理
积分器抗饱和是工业PI控制的必备特性：
cpp复制if (limited) {
vel_integrator_torque_ *= 0.99f;
} else {
vel_integrator_torque_ += vel_integrator_gain * dt * v_err;
}
这种处理：
- 防止输出饱和时积分器"卷绕"
- 保持0.99的衰减系数确保平滑恢复
- 仅在非饱和状态下进行积分
6. 惯量前馈与单位系统
6.1 惯量参数作用
惯量参数出现在加速度前馈计算中：
cpp复制torque = inertia * accel;
物理关系为：
τ = J·α
其中J是控制器使用的惯量参数，与SI单位的关系为：
J = J_SI * (2π)²
6.2 单位系统设计
整个控制器采用基于turn的单位体系：
- 角度：turn (1 turn = 2π rad)
- 速度：turn/s
- 加速度：turn/s²
- 惯量：N·m/(turn/s²)
这种设计：
- 使参数更符合工程师直觉
- 简化多圈位置计算
- 保持整个控制链单位一致
7. 工程实现要点
7.1 定时执行机制
控制器算法在固定时间间隔被调用：
- 典型控制周期为100μs-1ms
- 必须确保严格定时执行
- 离散化处理需要考虑采样时间dt
7.2 参数整定建议
基于串级控制特性，参数应按照从内环到外环的顺序整定：
- 先整定速度环PI参数
- 然后整定位置环比例增益
- 最后调整前馈参数
7.3 实时性保障
在STM32上的实现要点：
- 使用硬件定时器触发中断
- 关键计算使用浮点单元
- 避免在中断内进行复杂运算
- 合理设置中断优先级
8. 常见问题排查
8.1 振荡问题
现象：电机出现持续振荡
可能原因：
- 位置环增益过高
- 速度环积分过强
- 机械共振未被抑制
解决方案： - 降低位置环增益
- 调整速度环参数
- 增加低通滤波
8.2 跟踪误差大
现象：位置跟踪存在稳态误差
可能原因：
- 速度环积分不足
- 前馈参数不准确
- 负载扭矩变化大
解决方案： - 适当增加速度环积分
- 校准惯量参数
- 考虑加入负载观测器
8.3 响应迟钝
现象：系统响应缓慢
可能原因：
- 位置环增益过低
- 速度环限幅过小
- 前馈未启用
解决方案： - 逐步提高位置环增益
- 检查速度指令限幅
- 启用合适的前馈
9. 性能优化技巧
9.1 前馈调谐
精确的前馈可以显著提升跟踪性能：
- 惯量前馈补偿加速度需求
- 摩擦前馈补偿速度相关损耗
- 重力前馈补偿恒定负载
9.2 陷波滤波应用
对于机械共振问题：
- 识别共振频率
- 配置数字陷波滤波器
- 放置在速度环之后
9.3 自适应控制
对于变负载应用：
- 实现在线惯量辨识
- 自适应调整控制参数
- 使用扰动观测器
10. 扩展功能实现
10.1 位置轨迹规划
在控制器外部实现：
- 三次样条插值
- S曲线速度规划
- 实时更新设定点
10.2 安全监控
增强系统安全性：
- 位置超限检测
- 速度超限保护
- 温度监控
10.3 网络化控制
集成通信接口：
- CAN总线指令接口
- 实时状态反馈
- 参数在线调整
在实际项目中，我们通常需要根据具体机械特性和性能需求对控制器进行细致调参。ODrive的这种模块化设计使得我们可以方便地实验不同控制策略，同时其开源性允许我们深入理解每个环节的实现细节。对于STM32平台开发者而言，这份代码也提供了工业级运动控制的优秀参考实现。