自制轮腿机器人（1）：FOC无刷电机电流环控制（有感）

目录

前言

一、STM32工程创建

二、电流检测

三、SVPWM

四、电流环测试结果

---

前言

         本文章为个人学习笔记。先前复刻过一个开源轮腿项目，但是效果不好且一直找不到问题，归根结底还是没有对这个项目有自己的理解，遂决定从零学习、搭建这个项目。在此将我的学习经历分享出来，希望可以帮助到正在学习相关内容的同志们。

        首先是这个项目最基础的部分——无刷电机的力矩控制，由于很多数学模型的最终控制目标都是电机的转矩，所以做好一个良好的电流控制底层是非常重要的，控制好了电流就相当于控制好了力矩，我们的实物就能更好地去贴合仿真。在这里我想从理论到硬件及代码，再到实现效果，给大伙分析一下我对这一部分知识的理解，所有的工程文件我都开源到了gitee供大家参考（链接在文末）。如果存在一些理解错误的地方、用词不规范的地方，或者存在还可以改进的地方，期待各位大佬前来指正。

参考文章：SVPWM算法原理及详解

                  SimpleFOC（八）—— 理论+实践 深度分析SVPWM

                  SimpleFOC移植STM32（五）—— 电流采样及其变换

                【自制FOC驱动器】深入浅出讲解FOC算法与SVPWM技术

                  foc学习笔记2——svpwm

                  foc学习笔记3——电流环

参考视频：（十）小白FOC实物篇：电流环调节及PI参数整定

参考文档：SVPWM的原理及法则推导和控制算法详解第五修改版 提取码：2617

参考开源项目：【开源啦！】无刷轮腿平衡机器人

一、STM32工程创建

        我这里使用了STM32CubeMX来配置芯片以及生成工程，芯片选用最常见的F103C8T6。

        首先是时钟树，我的板子虽然画了晶振电路，但是并没有采用外部晶振（当时手里没有晶振了），这里将主频设置成64MHz就好了。

        下载方式选择SW，使用ST-Link下载器。

        ADC选择单次采样就够用了，选择两相电流采样的方式，正好对应两个ADC，选择合适的通道勾选即可。

         接下来是定时器的配置，TIM2用来发出PWM波，这里的频率即为载波频率，载波频率越高电机的运动就越趋近于一个标准圆，也就是说电机运动的越平滑，但是过高也会导致MOS管的开关损耗比较大。TIM1、TIM3都用于定时中断。

         然后是CAN总线的配置，配置时如果提示预分频的问题，可以先去时钟树将APB1预分频调到4，配置好后再回时钟树将预分频改回来。

        PB6、PB7是软件I2C使用的引脚，PC13 是LED引脚，PA4是电机驱动芯片的使能引脚，PA3是按键引脚。

         最后是串口的配置，可以用来给我们查看电流的波形，首先配置成异步模式（Asynchronous），然后将波特率设置为115200。

        配置完成后创建Keil5工程即可，注意命名以及文件地址不能有中文，另外我习惯将左侧的Code Generator栏中的“Generate peripheral initialization as a pair of ".c/.h' files per peripheral”选项勾选上，可以将配置部分分为一个个.c/.h文件，方便查看。

二、电流检测

        首先来看硬件部分，参考这篇文章，采用内置电流采样（在线电流采样）方式，采集任意两相电流后即可通过KCL得到第三相的电流。选择带PWM抑制功能的INA240A2运放，以下来简要概括一下相关信息以及使用方法，详情可以去嘉立创看看数据手册。

• 名字里的A2代表放大倍数为50倍，另外A1、A3、A4分别代表20、100、200倍。
• 引脚REF1、REF2用于配置输出基准电压，即0A时的输出电压，不可高于VS或低于GND。这里直接连接3.3V和GND，理论在采样电阻上的电流为0A时应该输出1.65V。
• IN+、IN-代表了输入方向，实际测试出驱动端（U_CS）为高端，电机端（U）为低端。

        接下来是电流检测的代码部分，这一部分代码我都放在了FOC_ADC.c文件中。

        全局变量以及宏定义：

#define Gain 50								//运放增益
#define R    0.01							//采样电阻
#define NUM  64								//滑动均值滤波倍数

//只采用两相采样
float A_Voltage , B_Voltage;							//采样电压
float A_offset , B_offset;								//偏置电压
float FOC_Current_q[NUM] = {0};						//数据缓冲区
float FOC_Current_d[NUM] = {0};
FOC_Struct FOC_Current,FOC_Voltage;

        首先是读取ADC的数据并转化为电压值，只需要简单地调用库函数即可。

        需要注意的一点就是 A 相和 B 相的选择，比如说我的PWM1、PWM2、PWM3分别用来输出A、B、C三相，而PWM1对应 U_CS，经过运放输出的U_OUT连接到ADC2的引脚PA7上，那么ADC2采样的结果就应该是A相的电压，这里是一个小坑，当时没有注意到导致调试许久没有结果...

void ADC_GetVoltage(void)
{
	uint16_t ADC1_data;
	uint16_t ADC2_data;
	HAL_ADC_Start(&hadc1);
	HAL_ADC_Start(&hadc2);

	ADC1_data = HAL_ADC_GetValue(&hadc1);
	ADC2_data = HAL_ADC_GetValue(&hadc2);
	
	B_Voltage = (float)ADC1_data / 4096 * 3.3;
	A_Voltage = (float)ADC2_data / 4096 * 3.3;
}

         我们在硬件设计中，将基准电压配置成了1.65V，但是由于硬件问题，实际值会存在少许偏差，所以我们需要先采集多次0A时的电压值，来对基准电压进行校正。

void Get_offset(void)
{
	uint16_t i;
	
	A_offset = 0;
	B_offset = 0;
	//取1000次采样的平均值
	for(i = 0;i < 1000;i++)
	{
		ADC_GetVoltage();
		A_offset += A_Voltage;
		B_offset += B_Voltage;
	}
	A_offset = A_offset / 1000;
	B_offset = B_offset / 1000;
}

         使用检测的电压与基准电压的差 / 电阻 / 增益即为相电流，使用基尔霍夫电流定律（KCL）：Ia + Ib + Ic = 0，求出第三相的电流。

//计算相电流
void ADC_GetCurrent(void)
{
	ADC_GetVoltage();
	FOC_Current.A = (A_Voltage - A_offset) / R / Gain;
	FOC_Current.B = (B_Voltage - B_offset) / R / Gain;
	FOC_Current.C = 0 - FOC_Current.A - FOC_Current.B; 		// KCL
}

         相比于不断变化的三相正弦波，我们更希望去观测和控制一个线性的变量，这时候就需要灵活运用Clark变换、Park变换以及它们的逆变换，来将三相正弦波转化成线性变量供我们控制，也可以将我们的控制量转换给SVPWM部分，最终拟合出控制电机运动的三相正弦波。

        首先是Clark变换，可以将三相坐标系（A、B、C）转换为固定的平面直角坐标系（α、β）

        由几何关系可得

         然后是Park变换，可以将固定坐标系（α、β）转换为随转子运动的坐标系（Q、D）。这里的D轴代表了转子的径向，我们需要控制其为零，Q轴代表了转子的切向，是我们的期望值。

        由于我们是通过控制合成电压矢量的旋转来控制电机转子的旋转的，而电机一般都有多个电极对，有n个电极对就意味我们的合成电压矢量转n圈才能使转子转一圈，所以我们这里计算所用到的角度θ并非是转子的机械角度，而应该是它的电角度，即 机械角度 * 极对数 。

         由几何关系可得

 

         了解了变换过程后，我们就可以用代码将其实现出来了

//计算Id、Iq
void ADC_GetFOCcurrent(float angle)
{
	float cos_angle,sin_angle;
	static uint8_t p;							//指针
	static uint8_t nub;
	float sum_q,sum_d;
	uint8_t i;
	
	
	ADC_GetCurrent();
	
	//clarke变换
	FOC_Current.alpha = FOC_Current.A - 0.5*(FOC_Current.B + FOC_Current.C);
	FOC_Current.beta = _SQRT3_2*(FOC_Current.B - FOC_Current.C);
	//park变换
	cos_angle = _cos(angle);
	sin_angle = _sin(angle);
	FOC_Current_d[p] = FOC_Current.alpha * cos_angle + FOC_Current.beta * sin_angle;
	FOC_Current_q[p] = FOC_Current.beta * cos_angle - FOC_Current.alpha * sin_angle;
	//滑动均值滤波
	p++;
	if(p >= NUM)p = 0;
	for(i = 0;i < NUM;i++)
	{
		sum_q += FOC_Current_q[i];
		sum_d += FOC_Current_d[i];
	}
	FOC_Current.q = sum_q / NUM;
	FOC_Current.d = sum_d / NUM;
	
	nub++;
	if(nub >= 10)
	{
//		printf("{A_Voltage}%f\r\n",A_Voltage);
//		printf("{B_Voltage}%f\r\n",B_Voltage);
		printf("{Iq}%f\r\n",FOC_Current.q);
		printf("{Id}%f\r\n",FOC_Current.d);
		nub = 0;
	}

}

         可能是我的硬件设计忽略了隔离的问题，也可能是F103的ADC性能有限，开环测试时我检测到的电流噪声非常大，所以我视情况使用了一个64倍的滑动均值滤波，以下为开环情况下 Id 添加滤波算法前后的波形图。

        滑动均值滤波：设置一个容量为 N 的数据缓冲区，N 就是滤波倍数，每当有一个新数据到来时，若缓冲区已满，则替换掉最老的数据（先进先出），然后取平均值输出。

         至此电流检测部分就已经完成了，可以等下一节SVPWM完成后，开环跑起来测试一下各个环节的检测结果，我的结果就是：三相的电流波形为存在一定噪声的正弦波，Iq 在0A附近，Id 大约在一个值附近（空载）。

三、SVPWM

        SVPWM大致就是根据合成电压矢量所在的不同区域来控制三相PWM波的占空比组合，从而达到控制电机旋转的目的。我们主要做的事情就两件，一是判断现在合成电压矢量所在的扇区，二是根据现在的扇区控制每一相的PWM占空比。

        首先我们需要对合成电压矢量所在的扇区进行判断，上文所说逆变换的应用就是在这里，我们需要先将我们设定的 Uq、Ud 进行Park逆变换：

 

        得到Uα、Uβ 之后就可以直接使用结论：

         不过这样的结论使用起来未免有些麻烦，所以我们可以进行进一步的推导，发现扇区位置只由下列三式决定：

         我们再定义A、B、C三个变量，分别对应U1、U2、U3，若U1 > 0 则 A = 1，反之A = 0，以此类推。随后可以定义一个序号N = 4*C+2*B+A，参照下列表格即可得到扇区号。

        接下来就可以按照每一个扇区去控制PWM的占空比了，不过在此之前，我们需要先了解一下这个电压空间矢量图。注意扇区的分布，各个电压矢量的分布，以及每个扇区的箭头方向，将箭头起点方向的电压矢量定义为 Ux，将箭头终点方向的电压矢量定义为 Uy，他们在一个载波周期（Ts）内的持续时间分别为 Tx 和 Ty，随即可以得到零电压矢量的持续时间为（Ts - Tx - Ty），又由于零电压矢量分为 U7 和 U0，所以 T7 = T0 =（Ts - Tx - Ty）/ 2 。

         接下来就可以使用这张表得到每个扇区内 Ux、Uy以及 U7 的持续时间 Tx、Ty、T7 了。这里的 U1、U2、U3 就是上面那三个，然后令Ts = 1，我们就可以将结果从持续时间转换为占空比。

         最后参照这个7段式SVPWM的开关切换顺序表，从上往下依次为A、B、C三相，再依据电压空间矢量图对应 Tx 和 Ty，就可以求出在任何情况下，每一相PWM波的占空比了。

        例如 I 区中，A相的高电平持续了T4+T6+T7，B相持续了T6+T7，C相持续了T7。而由电压空间矢量图得知 I 区中的T4是Tx，T6是Ty，所以A相持续时间为Tx+Ty+T7，B相持续时间为Ty+T7，C相持续时间为T7。以此可以类推出其余的扇区，这里我发现一个小结论，就是最长的是Tx+Ty+T7，第二长的是Ty+T7，最短的是T7，可以节省一点大家看图对照的时间。

        以下就是我实现出来的代码，在FOC_Control.c中，可以供大家参考一下。

        先解释一下：这里我是直接将Ts = 1代入计算的，求出来的Ta、Tb、Tc都是占空比，只是为了方便就没有用特殊的方式表示。

//SVPWM
void SVPWM_SetVoltage(float Uq, float Ud, float angle_el)
{
	float cos_angle,sin_angle;
	float Udc;
	float U1,U2,U3;
	float Ta,Tb,Tc;
	float Tx,Ty,T7;
	uint8_t A,B,C;
	uint8_t N;
	
	Udc = voltage_power_supply;			//母线电压
	cos_angle = _cos(angle_el);
	sin_angle = _sin(angle_el);
	//park逆变换
	FOC_Voltage.alpha = Ud * cos_angle - Uq * sin_angle;
	FOC_Voltage.beta 	= Ud * sin_angle + Uq * cos_angle;
	//判断区号
	U1 = FOC_Voltage.beta;
	U2 = _SQRT3_2*FOC_Voltage.alpha - 0.5*FOC_Voltage.beta;
	U3 = -(_SQRT3_2*FOC_Voltage.alpha + 0.5*FOC_Voltage.beta);
	
	A = (U1 > 0)?1:0;
	B = (U2 > 0)?1:0;
	C = (U3 > 0)?1:0;
	
	N = 4*C + 2*B + A;
	//计算每一相的占空比
	switch(N)
	{
		case 3:			//扇区I	x = 4;y = 6
		{
			Tx = _SQRT3*U2/Udc;
			Ty = _SQRT3*U1/Udc;
			T7 = (1 - Tx - Ty)/2;
			
			Ta = Tx + Ty + T7;
			Tb = Ty + T7;
			Tc = T7;
		}break;
		case 1:			//扇区II	x = 2;y = 6
		{
			Tx = -_SQRT3*U2/Udc;
			Ty = -_SQRT3*U3/Udc;
			T7 = (1 - Tx - Ty)/2;
			
			Ta = Ty + T7;
			Tb = Tx + Ty + T7;
			Tc = T7;
		}break;
		case 5:			//扇区III	x = 2;y = 3
		{
			Tx = _SQRT3*U1/Udc;
			Ty = _SQRT3*U3/Udc;
			T7 = (1 - Tx - Ty)/2;
			
			Ta = T7;
			Tb = Tx + Ty + T7;
			Tc = Ty + T7;
		}break;
		case 4:			//扇区IV	x = 1;y = 3
		{
			Tx = -_SQRT3*U1/Udc;
			Ty = -_SQRT3*U2/Udc;
			T7 = (1 - Tx - Ty)/2;
			
			Ta = T7;
			Tb = Ty + T7;
			Tc = Tx + Ty + T7;
		}break;
		case 6:			//扇区V	x = 1;y = 5;
		{
			Tx = _SQRT3*U3/Udc;
			Ty = _SQRT3*U2/Udc;
			T7 = (1 - Tx - Ty)/2;
			
			Ta = Ty + T7;
			Tb = T7;
			Tc = Tx + Ty + T7;
		}break;
		case 2:			//扇区VI	x = 4;y = 5
		{
			Tx = -_SQRT3*U3/Udc;
			Ty = -_SQRT3*U1/Udc;
			T7 = (1 - Tx - Ty)/2;
			
			Ta = Tx + Ty + T7;
			Tb = T7;
			Tc = Ty + T7;
		}break;
		default:
		{
			Ta = 0;
			Tb = 0;
			Tc = 0;
		}
		
	}
	//计算占空比
	
	//设置CCR寄存器控制PWM占空比
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Ta*PWM_Period);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Tb*PWM_Period);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Tc*PWM_Period);
}

         完成了这些，我们就可以手动给Uq、Ud赋值，让电机开环跑起来了，需要注意的是赋值不要太大（我给Uq 2V，Ud 0V），不要让电机开环转太久，简单验证一下功能即可。

四、电流环测试结果

        在开始电流控制之前，我们需要先查看Flash里的电机参数，如果没有的话，就需要让电机进行自动标定，以获得电机的转向、极对数和零点偏移量，完成之后存进Flash里。也可以长按按键进入自动标定，这一部分在中断的代码里。

        自动标定的代码参考这篇文章，使用的是简易版的SVPWM，这篇文章对这种SVPWM实现方式有讲解，我这里就不过多解说了。

        标定好参数后就可以定时进行PID控制了，目标量的最大值我都设置成了电机的额定值。由于还有其他的中断任务，直接将控制写进中断回调函数里会互相影响，所以采用中断设置标志位，然后在主函数里执行的方式。

//设置目标量，注意单位
void setTargetVotage(float new_target)
{
			
	//电流闭环
	if(new_target < 0)new_target = -new_target;
	if(new_target > 1.19)new_target = 1.19;
	Current_target = new_target;
	
//	//力矩闭环（采用can通讯时使用）
//	if(new_target < 0)new_target = -new_target;
//	if(new_target > 0.73)new_target = 0.73;
//	Current_target = new_target / KT;
}


void loopFOC(void)
{

	//设置电压q、d
	//开环
//	FOC_Voltage.q = 2;
//	FOC_Voltage.d = 0;
	//闭环
	FOC_Voltage.q = PID_Controller(&PID_current_q,(Current_target - FOC_Current.q)); 
	FOC_Voltage.d = PID_Controller(&PID_current_d,(0 - FOC_Current.d));
	//SVPWM
	SVPWM_SetVoltage(FOC_Voltage.q, FOC_Voltage.d, electrical_angle);
}

         主循环中执行中断任务

if(TIM_1ms)
{
	Key_Process();
	Led_Process();
	Motor_OfflineCheckProcess();
	if(HAL_GetTick()%2)
	{
		CAN_SendState(shaft_angle, shaft_velocity);
	}
			
	TIM_1ms = 0;
}
		
if(TIM_100us)
{
	//获取转子角度和角速度
	shaft_angle = shaftAngle();
//	shaft_velocity = shaftVelocity();
	//获取电角度
	electrical_angle = electricalAngle();
	//计算反馈电流
	ADC_GetFOCcurrent(electrical_angle);
			
	setTargetVotage(test_target);				//手动测试：test_target ; can接收：Torque_target
	loopFOC();
			
	TIM_100us = 0;
}

        这里附上PID的代码，使用结构体的方式来减少工作量。

PID_Struct  PID_current_q,PID_current_d,PID_velocity,PID_angle;


void PID_Init(PID_Struct *PID,float P,float I,float D,float Max_I,float Max_Out)
{
	PID->Kp = P;
	PID->Ki = I;
	PID->Kd = D;
	PID->Max_Integral = Max_I;
	PID->Max_Output = Max_Out;
	PID->error_prev = 0;
	
}


float PID_Controller(PID_Struct *PID,float error)
{
	float output;
	static float error_prev;
	
	PID->integral = PID->integral + PID->Ki * error;
	PID->integral = _constrain(PID->integral, -PID->Max_Integral, PID->Max_Integral);
	
	output = PID->Kp * error + PID->integral + PID->Kd * (error - error_prev);
	output = _constrain(output, -PID->Max_Output, PID->Max_Output);
	
	error_prev = error;
	
	return output;
}

         想要有一个好的PID控制效果，就必须有好参数，通过参数整定的方法就可以获得大致准确的参数，再结合实际效果进行细调即可。我们使用以下公式进行参数整定：

        L ：相间电感

        R：相间电阻

        T：控制周期

        B：带宽，数值上等于电机电角度的每秒最大转速（电角度）

        我们可以在购买电机的商家里找一下电机的参数表，比如我使用的是6010无刷电机，店家给出的参数表如下：

        其中可以直接找到我们需要的相间电阻及电感，还有上文中用到的额定电流和额定扭矩。而我们的带宽 B 则需要用到最大转速，用275除以60秒得到每秒最大转速，约为4.583转每秒，再乘极对数14转化为电角度情况下的转速，约为64，这就是带宽的值。最后根据我们对定时器的配置确定控制周期T，我使用的周期是0.1ms。

        代入以上结果计算出

        

        将结果写进PID初始化中，限幅的值不能超过母线电压/√3

​PID_Init(&PID_current_q,5.2397,0.4785,0,13,13);
PID_Init(&PID_current_d,5.2397,0.4785,0,13,13); 

        我们从D轴开始调试，因为这样电机不会转动，更方便我们观察电流响应。

        将Q轴期望设置为0，D轴期望设置为 0.2A 

void loopFOC(void)
{
	FOC_Voltage.q = PID_Controller(&PID_current_q,(0 - FOC_Current.q)); 
	FOC_Voltage.d = PID_Controller(&PID_current_d,(Current_target - FOC_Current.d));
	//SVPWM
	SVPWM_SetVoltage(FOC_Voltage.q, FOC_Voltage.d, electrical_angle);
}

        然后将代码烧录进去后上电，在串口调试助手中看到 D 轴电流波形如下：

         发现效果并不是很理想，我降低了一些Kp、Ki值，最终调试结果为Kp=3.2397、Ki=0.1385（跟理论值差的比较大，不太清楚是哪里的原因）。调试后的 D 轴电流波形如下：

        同时 Q 轴电流波形如下：

         两个轴的电流误差均在0.01A左右，个人认为这个结果还可以接受，如果有更好的条件可以进一步提高ADC采样率以及控制速率，误差应该可以更小些。

        D轴电流响应曲线没问题后，就可以给Q轴赋值了（别忘了Q轴电流才是真正的期望，D轴电流是无效的，需要控制在0A）

void loopFOC(void)
{
	FOC_Voltage.q = PID_Controller(&PID_current_q,(Current_target - FOC_Current.q)); 
	FOC_Voltage.d = PID_Controller(&PID_current_d,(0 - FOC_Current.d));
	//SVPWM
	SVPWM_SetVoltage(FOC_Voltage.q, FOC_Voltage.d, electrical_angle);
}

         下载代码上电测试一下，由于一直给电机切向力矩，所以电机会一直加速到满速，电流波形产生严重失真，我们用手将电机堵转后，电流迅速稳定在期望值附近，同时手上感受到恒定的力。

        电流波形如图，从上至下依次为 Iq、Id

         我们再用手将电机堵转着上电看看波形，顺序依旧是 Iq、Id

        可以看到输出电流满足了我们的期望，并且可以输出稳定的力矩。

        至此，我们已经完成了对无刷电机比较基础的电流控制，后续可以继续完成速度以及位置环，不过本人暂时还用不到，就没有进行这一部分的调试。下一步打算开始matlab仿真的搭建，验证物理模型及控制算法的合理性，并进行一些参数的调试。

        这是本人第一次发文章，你们能够看完就是对我最大的支持，最后在此奉上我的工程：

        wxynb_gitee/FOC电流环