算法 - FOC - 技术栈

一、理论基础

[1.1 克拉克变换](#1.1 克拉克变换)

[1.1.1 先说结论](#1.1.1 先说结论)

[1.1.2 克拉克基本形式](#1.1.2 克拉克基本形式)

[1.1.3 等幅值克拉克变换](#1.1.3 等幅值克拉克变换)

[1.1.4 克拉克逆变换](#1.1.4 克拉克逆变换)

[1.2 帕克变换](#1.2 帕克变换)

[1.2.1 QD坐标系的引入](#1.2.1 QD坐标系的引入)

[1.2.2 帕克正变换与逆变换](#1.2.2 帕克正变换与逆变换)

二、开环速度控制

[2.1 核心思想](#2.1 核心思想)

[2.2 三相电压矢量](#2.2 三相电压矢量)

[2.3 机械角度与电角度的关系](#2.3 机械角度与电角度的关系)

[2.4 伪代码部分](#2.4 伪代码部分)

[2.4.1 预设部分](#2.4.1 预设部分)

[2.4.2 PWM](#2.4.2 PWM)

[2.4.3 克拉克与帕克逆变换](#2.4.3 克拉克与帕克逆变换)

[2.4.4 Uq和电角度生成](#2.4.4 Uq和电角度生成)

三、闭环位置控制

[3.1 控制思想](#3.1 控制思想)

根据输入位置得到三相电压

[3.2 伪代码](#3.2 伪代码)

[3.2.1 AS5600读取](#3.2.1 AS5600读取)

[3.2.2 位置环控制](#3.2.2 位置环控制)

一、理论基础

1.1 克拉克变换

1.1.1 先说结论

克拉克基本形式：

克拉克变换等幅值：

克拉克逆变换：

1.1.2 克拉克基本形式

目的：三相波形相位差比较难控制，利用克拉克变换降维解耦，映射到二维坐标上面；

三相电流波形 -> 相位差为120°的矢量 -> 二维坐标

对 α 轴的推导：

因：

故：

对 β 轴的推导：

因：

故：

矩阵化：

1.1.3 等幅值克拉克变换

从1.1.2 克拉克基本形式 计算发现;

上述图片中，由基尔霍夫电流定律可知：

IA+IB+IC=0

我们把这个值代入克拉克变换基本形式计算：

此时结论是：与输入的不等。

我们需要让 a 相电流直接等于 α 轴分量，从而可以只采集两路电流并省去一次额外的乘法/矩阵运算

这时候可以乘上 2/3 就可以实现相等，也就是等幅值形式

此时：

（结合基尔霍夫电流定律：，故

将代入上式：

所以，我们只需要知道其中两相电流，也就是 ib 和 ia，这时候就可以知道和

1.1.4 克拉克逆变换

1.2 帕克变换

1.2.1 QD坐标系的引入

之前克拉克变换时，是在定子线圈那里的，或者说是固定的。

但是转子是转动的，所以引入Q-D坐标系；

在转动过程中会与坐标系形成一定夹角，这里称为电角度；

1.2.2 帕克正变换与逆变换

帕克正变换：

矩阵化：

帕克正变换：

帕克逆变换：

逆变换展开式：

二、开环速度控制

2.1 核心思想

输入期望的速度

生成Uq和旋转电角度

根据电角度进行帕克变换生成分量

根据分量进行克拉克变换，生成三相分量

利用三相分量来控制PWM输出该为多少

2.2 三相电压矢量

上面提到过了帕克变换、克拉克变换，都是电流形式；

在实际控制时，一般是用PWM占空比来控制电压，所以乘上一个电阻R，转成电压形式；

电压的帕克逆变换矩阵形式：

派克逆变换展开式：

αβ 坐标系到 abc 坐标系(三相)的电压变换：

2.3 机械角度与电角度的关系

电角度 = 机械角度 x 极对数

**极对数：**电机磁极对数，1个N极和1个S极为一对磁极；

比如机械角度旋转一圈，但是里面N对磁极，切割了N次，发了N个周期的电；

2.4 伪代码部分

2.4.1 预设部分

cpp 复制代码

/* pwm输出引脚假设 
30Khz
int pwmA = 32;
int pwmB = 33;
int pwmC = 25;
*/
 
//限制幅值
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
 
float voltage_limit = 10;
float voltage_power_supply = 12.6; //供电电压
float shaft_angle=0, open_loop_timestamp=0;
float zero_electric_angle=0, Ualpha, Ubeta=0, Ub=0, Uc=0, dc_a=0, dc_b=0, dc_c=0;
 
 
// 电角度求解
// 机械角度*极对数
float _electricalAngle(float shaft_angle, int pole_pairs) {
  return (shaft_angle * pole_pairs);
}
 
// 归一化角度到 [0,2PI]
// 前面取余，后面映射到[0,2PI]
float _normalizeAngle(float angle){
  float a = fmod(angle, 2*PI);   //取余运算可以用于归一化，列出特殊值例子算便知
  return a >= 0 ? a : (a + 2*PI);

2.4.2 PWM

cpp 复制代码

// 设置PWM到控制器输出
void setPwm(float Ua, float Ub, float Uc) {
  //限制上限
  Ua = _constrain(Ua, 0.0f, voltage_limit);
  Ub = _constrain(Ua, 0.0f, voltage_limit);
  Uc = _constrain(Ua, 0.0f, voltage_limit);
 
 
  // 计算占空比
  // 限制占空比从0到1
  dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
  dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
  dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
 
  //写入PWM到PWM 0 1 2 通道
  ledcWrite(0, dc_a*255);
  ledcWrite(1, dc_b*255);
  ledcWrite(2, dc_c*255);
}

2.4.3 克拉克与帕克逆变换

cpp 复制代码

void setPhaseVoltage(float Uq,float Ud, float angle_el) {
  angle_el = _normalizeAngle(angle_el + zero_electric_angle);
  // 帕克逆变换
  Ualpha =  -Uq*sin(angle_el); 
  Ubeta =   Uq*cos(angle_el); 
 
  // 克拉克逆变换
  Ua = Ualpha + voltage_power_supply/2;
  Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
  Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
  setPwm(Ua,Ub,Uc);
}

2.4.4 Uq和电角度生成

cpp 复制代码

// 输入速度为rad/s
float velocityOpenloop(float target_velocity){
//计算循环间隔
unsigned long now_us = micros();
float Ts = (now_us - open_loop_timestamp) * 1e-6f;
 
// micros() 溢出时，修正Ts
if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
  
//根据给定的速度，每次循环计算一下新的轴角度(机械角度)
shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
 
 
float Uq = voltage_power_supply/3;
setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, 7));
open_loop_timestamp = now_us;  //用于计算下一个时间间隔
 
return Uq;
}

三、闭环位置控制

3.1 控制思想

根据输入位置得到三相电压

输入期望位置 ，与编码器（实测机械角度）输出的机械角度 计算得到误差 e ：

e = 期望位置 - 实测机械角度

误差 e 经 PID 运算 ，得到

（为比例系数）

实测机械角度转换为 电角度：电角度 = 机械角 * 极对数

电角度与经 帕克逆变换 ，得到

经 克拉克逆变换 ，最终得到三相电压

3.2 伪代码

3.2.1 AS5600读取

cpp 复制代码

int _ams5600_Address = 0x36; // AS5600的I2C地址


int _raw_ang_hi = 0x0c;  //这两个是未处理的数据
int _raw_ang_lo = 0x0d;

int _angle_hi = 0x0e;
int _angle_lo = 0x0f; //这两个是经过IC处理后的数据

int ledtime = 0;
int32_t full_rotations=0; // full rotation tracking;
float angle_prev=0; 

//这个是ESP32初始化的东西
void BeginSensor() {
  Wire.begin(19,18, 400000UL);
  delay(1000);
}


//I2C读取数据，并整合成16位；
word readTwoBytes(int in_adr_hi, int in_adr_lo)
{
  word retVal = -1;
 
  /* 读低位 */
  Wire.beginTransmission(_ams5600_Address);
  Wire.write(in_adr_lo);
  Wire.endTransmission();
  Wire.requestFrom(_ams5600_Address, 1);
  while(Wire.available() == 0);
  int low = Wire.read();
 
  /* 读高位 */  
  Wire.beginTransmission(_ams5600_Address);
  Wire.write(in_adr_hi);
  Wire.endTransmission();
  Wire.requestFrom(_ams5600_Address, 1);
  while(Wire.available() == 0);
  int high = Wire.read();
  
  retVal = (high << 8) | low;
  
  return retVal;
}


//这个是包含圈数的
word getRawAngle()
{
  return readTwoBytes(_raw_ang_hi, _raw_ang_lo);
}

//这个是不包含圈数的
float getAngle_Without_track()
{
  return getRawAngle()*0.08789* PI / 180;    //得到弧度制的角度
}


float getAngle()
{
    float val = getAngle_Without_track();
    float d_angle = val - angle_prev;
    //计算旋转的总圈数
    //通过判断角度变化是否大于80%的一圈(0.8f*6.28318530718f)来判断是否发生了溢出，如果发生了，则将full_rotations增加1（如果d_angle小于0）或减少1（如果d_angle大于0）。
    if(abs(d_angle) > (0.8f*6.28318530718f) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; 
    angle_prev = val;
    return (float)full_rotations * 6.28318530718f + angle_prev;
    
}

3.2.2 位置环控制

首先获取AS5600得到的位置反馈信息，

Kp = 0.133根据 3.1部分进行计算得到的；

_constrain 用于限制电压幅度；

_electricalAngle 用于计算电角度，这里PP是极对数；

setPhaseVoltage 用于逆解出三相电压；

cpp 复制代码

#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))

float _electricalAngle(){
  return  _normalizeAngle((float)(DIR *  PP) * getAngle_Without_track()-zero_electric_angle);
}

void setPhaseVoltage(float Uq,float Ud, float angle_el) {
  angle_el = _normalizeAngle(angle_el);
  // 帕克逆变换
  Ualpha =  -Uq*sin(angle_el); 
  Ubeta =   Uq*cos(angle_el); 
  // 克拉克逆变换
  Ua = Ualpha + voltage_power_supply/2;
  Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
  Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
  setPwm(Ua,Ub,Uc);
}

void loop() {
  // put your main code here, to run repeatedly:
  Serial.println(getAngle());
  float Sensor_Angle=getAngle();
  float Kp=0.133;
  
  //设置位置环输出
  //DIR为方向系数，1为顺时针，-1为逆时针
  //
  setPhaseVoltage(_constrain(Kp*(motor_target-DIR*Sensor_Angle)*180/PI,-6,6),0,_electricalAngle());
  serialReceiveUserCommand();
}