一、原理说明
1.1、电流采样
1.1.1、为什么要采样电流
  FOC（Field-Oriented Control），即磁场定向控制，磁场大小与绕组中的电流成正比，所以对磁场的控制在程序上就是对电流的控制。前几节的程序并没有使用电流环，而是直接使用电压值，确实可以达到还行的效果。但是这样做有两个问题：

1、不能保证i_d为零，i_q等于目标值，因此电机并不能工作在效率最高的状态，力矩控制也是不准确的。

2、相电压施加在电感上产生相电流，电压和电流有相位差且并不恒定，低速运行时相位差对电机的影响不明显，但是当速度快了或者速度变化率高了以后，相位差的影响就会非常明显。

1.1.2、电流采样方式
电流采样主要有三种方式：
低侧电流采样
高侧电流采样
内置电流采样

1.1.2.1、低侧电流采样


低侧电流检测可能是最常见的电流检测技术，主要是因为它既不需要高性能的PWM抑制运放（如内置），也不需要支持高压的运放（如高侧），采样电阻在低侧MOS和GND之间，确保了运放输入端的电压非常低。这种方法的缺点是，必须在下桥臂MOS打开时检测电流，PWM频率通常为20k～50khz，这意味着低侧MOS的开关频率为每秒20k～50k次，因此PWM设置与ADC采集之间的同步非常重要。

1.1.2.2、高侧电流采样

  高侧电流检测可能是最不常见的电流检测技术，因为它需要支持高压的运放，采样电阻在高侧MOS和直流电源电压之间，使放大器的输入端始终有高电压。 这种方法的另一个缺点和低侧电流采样一样，需要同步PWM和ADC。



1.1.2.3、内置电流采样



  内置电流检测（InlineCurrentSense）是使用起来最简单但是最精准的技术。 采样电阻串联在电机相线上，检测的电流始终都是电机相电流，因为电感中的电流不会突变，所以无论PWM占空比的状态如何，采样到的电流都是连续稳定的。

  这种方法非常适合Arduino，采样程序变得简单了，这应该是考虑到了MEGA328P微弱的性能以及跨平台时程序的适配。内置电流检测的缺点主要在于芯片，需要比常规放大器更好的PWM抑制功能的高精度双向运放，简单的说就是硬件成本高。

1.2、电流变换
本节增加电流环，主要增加了以下功能，
1、AD转换获取电流值Ia和Ib，
2、通过Clark变换得到Iα和Iβ ，
3、获取电机角度，通过Park变换得到Id和Iq，
4、Id、Iq不能突变，同时为减少干扰，做平滑滤波，
5、Id、Iq与设定值比较，通过PID运算得到Vd 和Vq,

1.2.1、AD转换
  为了与官方代码保持一致，AD转换采用简单的单通道转换模式，主程序循环一次获取一次A/B相的电流。



1.2.2、clark变换



1.2.3、Park变换



  Park变换中的“θ”是电角度，由读出的编码器角度转变而来



1.2.4、LPF运算
dc_current模式只有 Iq 的LPF运算
foc_current模式有 Iq 和 Id 的LPF运算



1.2.5、PID运算
dc_current模式只有q轴电流的PID运算
foc_current模式有q轴电流和d轴电流两个变量的PID运算
电流环使用 P、I 两个参数。



二、电流采样参数设置
2.1、INA240介绍
INA240有四个型号，INA240A1是20倍放大倍数，INA240A2是50倍放大倍数。



电机的电流是正弦波，对于采样的差分信号来说，负半周期为负值，为方便处理，需要叠加上偏置电压以保证全周期的输出为正值。



2.2、电流实际波形
下图为电机静止时A/B相运放输出端波形，VCC=3.3V，所以此时电压=1.65V，



下图为电机转动时运放输出端波形



2.3、SimpleFOC V2.0.3 的参数



采样电阻0.01 Ω
运放为INA240A2，放大倍数50
背面需分别短接A0/A2至输出
C1_OUT=INA_VCC/2 + 0.01 * I * 50
如果INA_VCC=3.3V，C1_OUT=1.65 + 0.01 * I * 50，电流范围（-3.3A,3.3A）
如果INA_VCC=5.0V，C1_OUT=2.50 + 0.01 * I * 50，电流范围（-5A,5A）
B相采样输入在PCB上为走线方便做了反向输入，所以放大倍数需乘以 -1（移植后的代码已经做了处理）。

2.4、PowerShield V0.2 的参数



采样电阻0.001 Ω
运放为INA240A2，放大倍数50
C1_OUT=1.65 + 0.001 * I * 50，电流范围（-33A,33A）

2.5、SimpleMotor 的参数



采样电阻0.001 Ω
运放为INA240A1，放大倍数20
C1_OUT=1.65 + 0.001 * I * 20，电流范围（-82.5A,82.5A）

三、扭矩模式
  SimpleFOC中有三种扭矩模式：
Voltage mode - the simplest one
DC current mode - 1xPID controller + 1xLPF
FOC current mode - 2xPID controller + 2xLPF filters

3.2.1、电压模式(Voltage mode)
电压模式是之前示例中一直使用的模式，
因为不用采样电流，所以硬件简洁，程序简单，执行速度快，
适合低速电机控制



3.2.2、直流模式(DC current mode)
检测电流大小，实现电流环控制
只控制Iq，设置Vd=0，
像控制直流电机一样控制无刷直流电机的电流



3.2.3、FOC模式(FOC current mode)
控制Iq和Id，使转子中的磁力与永磁场精确偏移90度，从而确保最大转矩，
唯一真正的转矩控制方法，
保证电机始终工作在高效状态



3.2.4、三种模式对比



四、硬件介绍
4.1、原理图



4.2、SimpleMotor方案
4.2.1、准备清单





带编码器云台电机可以是AS5600，也可以是TLE5012B。

4.2.2、接线



只展示M1的接线，M2的接线根据原理图自行连接。

4.3、STM32方案
4.3.1、准备清单






带编码器云台电机可以是AS5600，也可以是TLE5012B。

4.3.2、接线



Shield V2.0.3的背面按照上图短接



对照 Shield V2.0.3 的原理图：




如果是AS5600编码器，如下



如果是TLE5012B编码器，如下





只展示M1的接线，M2的接线根据原理图自行连接。


五、程序演示





注意：
1、voltage_limit最大只能设置为供电电压的 1/√3（12V限制为6.9，24V限制为13.8），
2、如果检测到的极对数与实际不符，停止继续操作，否则电机会堵转。


六、代码升级说明

1、优化PID子程序。之前代码把位置PID、速度PID和电流PID_q_d 各自做为独立的函数调用，本次升级只写一个函数，把PID相关参数定义为结构体，与官方代码保持一致。

2、优化LPF子程序。与PID的做法相同，只写一个函数，把相关参数定义为结构体，与官方代码保持一致。

3、增加了对AS5047P、MA730编码器的支持（目前共支持AS5600、TLE5012B、AS5047P、MA730四种磁编），使用代码时只需在 MyProject.h 中把使用的型号设置为1即可。

4、官方最新的SimpleFOC库V2.2.0（2021年12月14日）版本新增加了sensor->update()函数，并因此对角度处理做了大改。
之前调试代码的时候，就发现官方代码中角度读取处理的不太好，特别是速度模式上电的时候电机会突然转一下然后停止，当时按照我的理解改进了这部分代码，最新版本中官方修补了这个问题，我本来是打算把这个部分升级到官方一致，但是修改了一天后发现官方的代码仍然很混乱（相信这部分代码官方以后还会再升级），所以此次放弃了这部分的升级，仍保持之前的设计思路，只做简单优化。

5、升级零点检测，在已知电机参数的情况下，通过设置 零点偏移角度和方向，可以跳过检测。与官方代码保持一致。

6、优化了while主循环中的计时方式，由原来的timer4中断累加修改为systick查询。当前代码中只有一个串口接收中断。与官方代码保持一致。

7、电流采样修改为三相采样。之前只写了两相采样，觉得两相就可以三相似乎浪费，调试ESP32drive的时候仔细看了下代码，发现三相多了一个平均处理，应该会比两相更可靠一点点，所以这次升级为第三相可选择，与官方代码保持一致。

8、升级后的代码可以很好的实现电流闭环，解决了之前发现的所有问题，所有功能都经过了本人的实际测试，当前代码在本人看来已无懈可击，虽为开源但也要认真负责。

————————————————
版权声明：loop222