Cheetah开源电机驱动控制解析

著名的MIT Cheetah Mini四足机器人开源了所有软硬件，包括机载电脑和关节驱动器的软硬件全部开源。这一篇就根据开源的驱动控制软件及相关论文，探索其核心控制算法框架。这篇文章需要结合源代码来看。
主要参考了lab 成员 Benjamin Katz 的论文《A low cost modular actuator for dynamic robots.》，以及公开的源代码https://os.mbed.com/users/benkatz/code/Hobbyking_Cheetah_Compact_DRV8323/。

1. 驱动器状态机

1.1 五种状态

总共有这五种状态：

• #define REST_MODE 0
  这个状态下，调用一次enter_menu_state()函数，这个函数做的操作是：禁用驱动IC DRV832x、串口打印menu、led=0;

• #define CALIBRATION_MODE 1
  这个状态下运行一次calibrate()函数，这个函数做的操作是：使能驱动IC DRV832x、led=1、调用确定相序函数order_phase()、调用标定函数calibrate()、led=0、串口打印提示、禁用驱动IC DRV832x；
  其中确定相序函数order_phase()和标定函数calibrate()具体实现后面再讲；

• #define MOTOR_MODE 2
  运行enter_torque_mode()函数一次，然后不断循环torque_control()和commutate()函数，控制频率为40khz；
  enter_torque_mode()函数主要是foc控制器的复位处理，具体操作是：使能驱动IC DRV832x、复位超压标志、调用reset_foc()函数复位foc控制器、led=1；
  其中torque_control()和commutate()两个函数是具体的FOC控制过程后面再讲；

• #define SETUP_MODE 4
  调用enter_setup_state()函数，该函数中通过串口打印Configuration Options；

• #define ENCODER_MODE 5
  串口不断打印机械位置、电气位置和原始位置；

1.2 通过串口操作

在串口中断中接收上位机发送过来的指令，根据不同的指令进行不同的操作。有如下几种情况：

• 无论当前处于什么状态，如果串口接收到Esc键，则返回REST_MODE；

• 如果当前状态为REST_MODE
  串口打印manu，可以通过如下指令进入相应的模式；
  m - Motor Mode
  c - Calibrate Encoder
  s - Setup
  e - Display Encoder
  z - Set Zero Position：重新设置机械零位，并写入flash

• 如果当前状态为SETUP_MODE
  串口打印Configuration Options，可以通过指令对参数进行设置，并写入flash
  b - Current Bandwidth (Hz)
  i - CAN ID
  m - CAN Master ID
  l - Current Limit (A)
  f - FW Current Limit (A)
  t = CAN Timeout (cycles)(0 = none)

• 如果当前状态为ENCODER_MODE
  串口会不断打印机械位置、电气位置和原始位置值，可以通过Esc键停止打印并返回REST_MODE

• 如果当前状态为MOTOR_MODE
  通过键入‘d’可以让foc的参考输入电流为0，或者Esc键返回REST_MODE

• 如果当前状态为CALIBRATION_MODE
  串口打印Calibration complete，可以通过Esc键返回REST_MODE

1.3 通过CAN操作

1.3.1 CAN接收

CAN接收也有两种情况：

• 如果当前状态是MOTOR_MODE
  就是正常的控制指令，期望位置、期望速度、期望力矩、kp、kd等，参见后面CAN通信协议；

• 如果当前状态不是MOTOR_MODE
  接收到的CAN指令分三种情况：
  data[0]~data[6]=0xFF, data[7]=0xFC时，进入MOTOR_MODE
  data[0]~data[6]=0xFF, data[7]=0xFD时，进入REST_MODE
  data[0]~data[6]=0xFF, data[7]=0xFE时，进入设置Zero_Position，与串口设置不一样，这里设置的zero position不会保存到flash；

1.3.2 CAN发送

每接收到一包数据后，会立刻返回数据，返回的数据是机械角度、机械转速、输出力矩；

1.4 状态之间的转换

状态之间的转换，可以通过CAN进行，也可以通过串口。
通过CAN操作时，各状态之间的关系如下：

其中模式切换时CAN的前7个字节都是0xFF，即data[0]~data[6]=0xFF，而在MOTOR_MODE模式下发送的控制命令则按CAN通信协议进行。

通过串口操作时，各状态之间的关系如下：

2. 几个重要函数

2.1 确定相序

确定相序函数是order_phase()，在CALIBRATION_MODE状态下调用。用于确定电机转向与编码器方向是否一致，如果不一致需要改变电机相序。
一种简单的确定相序的方法，首先让定子电压矢量缓慢地沿正方向旋转。如果位置传感器的测量到的位置是增加的，那么电机的相序是正确的。反之，如果位置传感器测量到的位置减少，那么需要改变电机两相的相序，只需要在固件里修改BC两相的顺序。

2.2 位置传感器标定

标定位置传感器的函数是calibrate()，在CALIBRATION_MODE状态下调用。这个函数有两个作用，首先是标定传感器的偏移，然后是对传感器数据进行线性化处理。
由于安装误差，实际上传感器零点不可能与转子零点完全重合，所以需要标定传感器的偏移。理想情况下，位置传感器的零点应该与转子d轴重合。
为了确定位置传感器的偏移，定子电压分别向前向后缓慢旋转一整个机械位置圈，然后记录下来转子位置，如下图所示。在定子电压旋转过程中，转子d轴紧密跟踪定制电流矢量。由于齿槽转矩和摩擦力的存在，电流矢量角与转子角之间存在一定的跟踪误差。位置传感器的电磁偏移就是定子电压向量和转子角度的平均偏差角度。

在传感器标定里面还有一个重要步骤，就是传感器的线性化。编码器IC位于PCB上的焊盘上。因此，编码器可以放置在距离转子旋转轴几百微米的位置。任何细微的安装偏差，都会带来编码器的非线性变化。而编码器的何机械角度误差都会被放大21倍（电机极对数），这对电机换相有很大的影响。
通过测量转子角度序列和电压角度序列之间的差异来测量非线性。在编码器信号中可以明显检测到两种频率的误差，分别是有电机齿槽转矩引起的高频误差，以及由于位置传感器的非线性引起的低频误差。
为了补偿位置传感器的非线性，首先要将由齿槽效应引起的高频误差消除，使用的是滑动平均滤波。因为齿槽效应引起的误差频率与电机的电频率一致。所以将滑动滤波的窗口大小设置成电周期大小，就能在电频率和所有电频率整数倍频率处取到零增益，从而可以完全滤除齿槽效应引起的误差。滤除齿槽效应后，就只剩下传感器偏心非线性的影响了，由于偏心的影响是固定的，所以可以将该信号用作查表来线性化位置传感器的输出。如下图所示，左边表示由齿槽引起的高频误差，右边表示滤掉高频之后，由传感器偏心引起的非线性误差。

2.3 闭环控制

通过CAN输入的控制命令包含了：期望位置Pdes、期望速度Vdes、前馈转矩tff、控制参数Kp、Kd。由代码可知，这些控制命令按如下控制框图的方式组合成闭环。其中，θm表示机械角度、dθm表示机械角速度、tref表示参考转矩、KT是转矩常数、iqref和idref分别表示q轴和d轴的参考电流，作为FOC控制的输入。
可以看出，这样的控制方式很灵活，既可以是纯位置控制、纯速度控制、纯转矩控制，也可以位置加转矩前馈、速度加转矩前馈等方式控制电机。

2.4 FOC控制

commutate()函数就是FOC控制的具体实现过程，分析该函数，主要的控制流程如下图所示：

Field Weakening
Field Weakening是一种可以让电机运行超过其基本速度的方法，可以有效地降低了电机的转矩常数，使其能够在给定的最大直流链路电压下达到更高的运行速度和更宽的功率带，同时降低了单位电流产生的转矩。
对于单极对数来说，在dq轴上的动态方程如下：

可以看出Vq中含有耦合项，这就可以用来做Field Weakening。通过在D轴施加一个反向的电流，就会在q轴产生正比于w和Ld的反向电压，以及反向电流id。这个反向电压可以抵消一部分由反电动势产生的电压，从而可以减小电机的转矩常数。正确的使用Field Weakening可以有效提高电机的最大性能。
如果不使用Field Weakening，一般情况下会令idref=0，而使用Field Weakening之后，idref<0。

dq0
派克正变换，将三相电流变换到dq轴：

abc
派克逆变换，将dq轴电压变换成三相电压：

PI Controller
d轴、q轴分别独立进行PI控制，有各自的PI参数。

Vref
由Vd、Vq计算参考电压：

Linearize
使逆变器的输出线性化，特别是小占空比的时候非线性很明显。
首先计算出占空比：

然后用linearize_dtc()函数线性化。

void linearize_dtc(float *dtc){
    /// linearizes the output of the inverter, which is not linear for small duty cycles ///
    float sgn = 1.0f-(2.0f*(dtc<0));
    if(abs(*dtc) >= .01f){
        *dtc = *dtc*.986f+.014f*sgn;
        }
    else{
        *dtc = 2.5f*(*dtc);
        }
    
    }

这个函数里面 (dtc<0) 应该写成 (*dtc<0)。

svm
这里的svm是矢量调制函数，用的是一种非常简单的方法，不知道算不算矢量调制：