FOC 单电阻采样 位置环控制伺服电机_foc 位置环

最近在做微型伺服电机的控制，需要平滑地将电机定位到某个位置。伺服电机、编码器、PCB都是自制的。这里我把整个的流程和遇到的问题记录一下。

目录

硬件设计

STM32CubeMX配置

程序设计

测试遇到的问题

演示视频

---

硬件设计

这里自制了一个驱动板

（1）MOS管驱动芯片：L6205D

原理图如下：

内部结构： 

• L6205自带死区控制，同时集成了MOS管，大大缩小了PCB的体积。
• 有四个MOS管引脚，这里只用到三相，即三个，而该芯片的电流采样引脚为每两个MOS管一个电流引脚，所以这里只能选择单电阻采样。
• 关于死区的定义和死区时间的设置，可以参考我这篇文章：死区时间的分析与设置

我这里的微型伺服电机，最小7V就可以旋转了。而这个芯片最大能承受52V的电压，所以也可以驱动非微型的电机，但此时图中的双肖特基二极管和自举升压电容的封装都要修改。

（2）运放

电流采样需要运放，由于不知道单电阻采样和L6205的输出电流的范围和正负，所以这里把加法电阻R8加上，同理放大倍数不对的话，换一下反馈链路上的电阻R11即可。

（3）电机

这里控制的是一个微型伺服电机，用PROE画的，然后拿去加工、自己充磁。自制的没有编码器，所以还需要做编码器。

（4）磁编码器：TLV493D-A1B6

采用磁感应芯片TLV493D-A1B6，感应电机转子产生的正弦磁场，通过数学公式得出角度，编写算法，这样就自己做出一个磁编码器。该芯片采用I2C通信，由于FOC执行频率很高，所以需要用到DMA。下图是根据磁场获取角度的流程框图。求角度需要实现arctan算法，这里基于CORDIC原理实现了一种定点arctan算法，后续会写一篇博客介绍。

 （5）CPU芯片：STM32F302K8U6

需要用到的引脚有：电机三相的3个PWM引脚、ADC采样1个引脚、I2C通信2个引脚、串口调试2个引脚、LED指示2个引脚、电源、SWDIO、SWCLK。这里不外接晶振了，采用内部晶振。

所以这里没必要用更多引脚的STM32F302R8TX。但MCSDK并不支持STM32F302K8U6，不能使用软件生成代码，所以需要自己移植修改电机库的代码。正好电机库的代码非常乱，即使你没选上的功能，那些函数、结构体都给你加上，这里我从底层做起，一点一点地移植代码。

最终硬件如图所示：

之前画过一个三电阻采样的四层板，元件特别多，高低频分开布线，十分复杂。而这里用了L6205，如图所示，板子体积缩小了不少，二层足矣。

STM32CubeMX配置

首先打开MCSDK(我使用的版本为MotorControl Workbench 5.4.7)，选择STM32F302R8TX，生成单电阻采样的代码，大部分都参考这个配置即可。

（1）定时器TIM1

由于L6205自带死区控制和互补输出，所以配置如下：

定时器设置为中央对齐模式3，也就是向上向下计数时都产生一个比较中断，因为可能要在这里面调整波形。而单电阻采样是一个PWM周期要有两个采样点，这里设置TRGO2触发ADC采样，配置通道5和通道6的上升沿产生TRGO2信号，通道5和6的上升沿，即CCR值在单电阻采样的函数中计算并设置。通道4是用来单电阻的PWM波形变形的，其比较值为HTMIN，即电流采样最小脉宽时间(死区时间+上升时间+ADC触发延时时间+采样时间)。

 生成代码后，L6205自带死区控制，程序中的死区时间设置为0。查询L6205手册，上升时间设置为250ns。对应下面两个宏定义。

#define SW_DEADTIME_NS 0
#define TRISE_NS 250

（2）I2C

对于磁感应芯片，需要设置I2C为快速模式，还有根据TLV493D-A1B6芯片手册的参数，设置上升时间和下降时间，最后再打开DMA传输。

（3）USART

主要用作上位机调试。这里不打开中断，因为经过后续测试发现，如果其中断优先级比ADC中断要低，则程序很容易进不了中断；如果其优先级比ADC中断要高，则电机的旋转噪声又不太对劲。所以干脆在while循环里读取USART。

（4）ADC

MCSDK生成的FOC程序都为ADC左对齐，这里我设置为右对齐。稍后会有分析。

程序设计

（1）状态机

我的应用中没有过压、过流检测等，不需要用到那么多状态的切换，为了简化代码，这里我仅设置M_INIT和M_TEST两个状态。中频任务中需要运行的初始化函数，在main函数中初始化电机后依次执行：

	R1F30X_TurnOnLowSides(pwmcHandle);
	PWMC_CurrentReadingCalibr(pwmcHandle, CRC_START);
	R1F30X_SwitchOnPWM(pwmcHandle);

（2）角度校正和计算

①校正

由于编码器是自制的根据磁场感应的角度传感器，电机转子在制作的时候也是随机贴上去的，所以感应出来的角度和电机的电角度有一个固定偏差，这需要校正。

这里我无反馈，设置一个固定电流值，软件循环模拟一个角度值，让电机仅用SVPWM缓慢转一圈，此时磁传感器的值和模拟值的误差即电机的角度偏差。

这个校正值并不是每一个电机都一样的，所以需要保存到Flash中，再接一个EEPROM有些大材小用了，所以利用STM32内置Flash的最后几K存储，这个ST官方有相应例程：STM32F3xx微控制器中的EEPROM模拟。

②计算

电角度是在ENC_CalcAngle函数中计算的，这里我的磁编码器写的arctan算法输出的角度范围是0~65535，直接将它赋值给int16变量转到-32768~32767范围。

（3）ADC采样

MCSDK生成的代码默认是左对齐的，ADC精度是12位的，也就是左移4位作为电流了。不知道大家是什么情况，但是我的实际测试，单电阻的采样电流是恒大于0的，如图中CH2所示。

 但是在获取电流函数中：

void R1F30X_GetPhaseCurrents( PWMC_Handle_t * pHdl, ab_t * pStator_Currents )

直接拿JDR寄存器中的注入结果判断溢出，溢出范围是int16范围，而我采样的电流再左移的话是uint16范围的，所以我这个肯定溢出了。

ST代码这样写的前提是保证无电流时运放有1.65V偏置，然后电流有正有负。不知道大家单电阻是不是有正有负，那我的是正的话干脆就把ADC改为右对齐了。

所以大家看前面我的实物图中，右上角运放电路的加法电阻R8已经去掉了，最开始电压有超过3.3V，所以反馈电阻R11我也改小了。

而在PWMC_SetPhaseVoltage函数中，计算定时器CCR值得时候其实除以了131072或262144，这两个数怎么来的，可以参考我的这篇文章：SVPWM函数PWMC_SetPhaseVoltage解析。它们都是32768的倍数，32768代表的其实就是ADC左对齐，所以电流值就是Q15格式的了，在计算CCR值时要转回Q0格式。但是这里我设置为了右对齐，这里不改这个系数，直接在R1F30X_GetPhaseCurrents函数中将最后计算出的电流左移3位。

当然不做任何修改也可以，后边的PID参数调好就行。

（4）位置环

这里我没有添加速度环，而位置环是通过设定位置和实际位置的误差计算出Iq的参考值。

位置控制其实是根据PID调节的，虽然能到达最终位置，但是中间的加速度太快了，我需要一个平滑的旋转过程。用速度环的话，要考虑刚开始和结束的加速度的限制，所以匀速可能又太慢了，效率低。

最终决定加上S曲线控制位置，但S曲线算法又有不少浮点和除法运算，不太适合再加到FOC上，所以我写了一种定点S曲线查表算法，大概思路就是通过S曲线的方程获取一定数量的浮点数，浮点数转为Q16定点数，用的时候就整数乘法，效率高些，乘完右移16位使用。这是我最终的位置环结构体：

typedef struct
{
	PID_Handle_t *PIPos;
	int16_t  hSetPosition;   //设定机械位置
	int16_t  hStartPosition; //起始机械位置
	int16_t  hCurPosition;   //当前机械位置
	int16_t  hError;         //初始机械位置与目标位置的差
	int16_t  hMaxTorque;     //最大力矩
	uint16_t hCurIndex;      //当前S曲线表的索引位置
	uint16_t hStep;          //S曲线调整的时间
	uint8_t  bIsChanging;    //正在调整位置
} PosControl_Handle_t;

具体实现见博客：FOC 位置环S曲线之定点查表算法

最终工程结构如下：

 相比由MCSDK生成的默认工程，我一个个文件移植，去掉了不需要用的文件、宏定义和代码。这样其实也便于我理解整个代码结构，也更容易修改代码。

（5）上位机

①TLV493D-A1B6

转子在旋转时，TLV493D会产生X、Y、Z轴的磁场，三个正弦波，可以通过算法求出当前转子的绝对角度。这里也写了一个程序来验证角度的准确性。

 ②位置环

程序界面很简单，因为我大部分是用JLINK在线仿真，在watch窗口中改变量值调试的。

测试遇到的问题

（1）微型伺服电机电感小

我控制的微型伺服电机电感比较小，我在反Park变换后，对Vα和Vβ乘以一个系数，它才有转矩。出现这种情况实际是位置环的最大转矩限制和位置环PID参数没调好。

同时我发现，电感小的电机甚至可以不用电流采样，让Ia、Ib都恒为0都行，西门子的一款微型电机就是这样做的，这仅适合小电感和反电势小的电机，这里还是位置控制，所以不用采样完全没问题。当然如果有人为外力干扰，电流也会比较大，所以还要加个过流检测。

（2）PID调节

定点PID应该根据电机的不同，先调整hKxDivisorPOW2的大小，否则有可能怎么调参数最后PID输出都溢出。调节时先调好电流环，再调节速度环。

另外我把代码中的wDischarge变量去掉了，它是如果输出超过了限制范围，将超出的范围加到下一次积分项中，个人觉得没有必要。

（3）串口

串口就是和上位机通信用作调试用了，实际测试发现中断执行会卡死，收发不了数据。如果把串口优先级设置的比ADC中断优先级还高，电机性能也会受一些影响，所以最终我放在while循环中读取。

还有数据若发得太快，QT收到时则会有粘包问题，所以还需要设计一个简单的协议。

（4）ADC采样

在初始化ADC完之后校准一下，结果更准确一些。

HAL_ADCEx_Calibration_Start(&hadc1,ADC_SINGLE_ENDED);

演示视频