使用TT电机做辆平衡车

vany5921

   之前介绍了日本玩家使用无刷电机做平衡车，但是无刷电机由于转速快并不是很容易控制，这次介绍一个人人都可以做的，利用3块钱的TT电机就能完成的平衡车，程序用到了卡尔曼滤波和PID控制，所以看起来很难，不过借助这个示例能很好的理解卡尔曼滤波是如何处理数据的。

整个驱动电路依然使用的是DRV8830，I2C地址分别位0x60和0x64，通过电池盒给驱动器供电



以下位细节图片，为了让小车更稳定自制了一个支架来固定电机





在第一轮测试中，通过应用卡尔曼滤波器计算M5StickC的倾角θ和角速度ω的估计值，并且基于这些值确定电动机供电电压以确定相对于地板的姿态并保持垂直。PID的参数主要通过供给的电压来决定。
加减值 = Kp X 误差 + Ki X 误差的累计 + Kd X 上次误差与本次误差的差。
误差是目标值和当前斜率之间的差值，对于小车来讲我们的目标是垂直（0°），因此误差是斜率θ（θ -  0°）
电压 = Kp X θ +Ki X ∑θ + Kd X ω
通过调整系数Kp，Ki，Kd以实现适当的电压供应，而不会出现过多的电源或电源短缺，防止小车抖动厉害或者反应迟钝
由于需要迅速了解小车的姿态，因此对M5StickC内置的SH200Q默认参数进行了设置，确保能快速对小车进行调整
加速度计频率从256Hz调整到1024Hz
陀螺仪的输出频率从500Hz调整到1000Hz
陀螺仪的低频截止频率从50Hz调整到250Hz
加速度计灵敏度从8G调整到4G
角速度传感器灵敏度从±2000°/sec 调整到 ±250°/sec


先设置SH200Q的参数
[mw_shl_code=arduino,true]uint8_t Gscale = GFS_25DPS;
uint8_t Ascale = AFS_4G;[/mw_shl_code]

PID的数值调整通过Blynk进行输入，依然先设置Blynk


将4个数字输入小组件添加到项目，同时添加一个蓝牙按钮


添加方法参考下图


最大值和最小值的设定可参考下列参数
Kp：-1000~1000、每次改变0.1    标签V0
Ki：-1000~1000、每次改变0.001  标签V1
Kd：-1000~1000、每次改变0.01   标签V2
Vmin：0~255、     每次改变1        标签V3


对于电机的控制通过改变I2C的地址来实现，表中的地址X在读时为1，在写时为0，在这里我们不需要读操作，X在这里代表0


向寄存器写入0x00，可用下表的数值设定电压，用高6位表示电压，低2位表示电机方向



基于卡尔曼滤波器的倾斜角和角速度的估计误差，通过PID控制向电动机提供电压并保持姿势。在定时器每2.5毫秒中断一次通过卡尔曼滤波器计算倾斜角和角速度的误差。通过将角速度误差与角速度相加来估算角速度，每10毫秒计算一次电源电压并驱动电机。在初始设置时，需要测量并减去传感器的测量值偏移。

[mw_shl_code=arduino,true]#define BLYNK_PRINT Serial
#define BLYNK_USE_DIRECT_CONNECT

#include <BlynkSimpleEsp32_BT.h>
#include <M5StickC.h>
#include <Ticker.h>
#include <Wire.h>

char auth[] = "Blynk";

int  VMIN = 1;
float Kp = 0.4;
float Ki = 0.1;
float Kd = 0.2;


float power = 0.0;

float thetaP, thetaI, thetaD;

#define WAIT 10
#define VMAX 63

int16_t accX = 0;
int16_t accY = 0;
int16_t accZ = 0;

int16_t gyroX = 0;
int16_t gyroY = 0;
int16_t gyroZ = 0;


int accXoffset = 0;
int accYoffset = 0;
int accZoffset = 0;


int gyroXoffset = 0;
int gyroYoffset = 0;
int gyroZoffset = 0;

Ticker tickerUpdate;



int sample_num = 100;
int meas_interval = 10;
float theta_deg = 0.0;
float theta_dot = 0.0;
float theta_dot2;

float theta_mean;
float theta_variance;
float theta_dot_mean;
float theta_dot_variance;


//=========================================================
//卡尔曼滤波器的变量
//=========================================================

//卡尔曼滤波
float theta_update_freq = 400; //Hz
float theta_update_interval = 1.0/double(theta_update_freq); //2.5msec
//向量X
//[[theta(degree)], [offset of theta_dot(degree/sec)]]
//向量X预测值
float theta_data_predict[2][1];
float theta_data[2][1];
//共分散行列
float P_theta_predict[2][2];
float P_theta[2][2];
//状態方程式の"A"
float A_theta[2][2] = {{1, -theta_update_interval}, {0, 1}};
//状態方程式の"B"
float B_theta[2][1] = {{theta_update_interval}, {0}};
//输出方程式"C"
float C_theta[1][2] = {{1, 0}};


//=========================================================
// 行列演算関数
//=========================================================

//行列の和
void mat_add(float *m1, float *m2, float *sol, int row, int column) {
    for(int i=0; i<row; i++) {
        for(int j=0; j<column; j++) {
            sol[i*column + j] = m1[i*column + j] + m2[i*column + j];   
        }   
    }
    return;
}

//行列の差
void mat_sub(float *m1, float *m2, float *sol, int row, int column){
    for(int i=0; i<row; i++) {
        for(int j=0; j<column; j++) {
            sol[i*column + j] = m1[i*column + j] - m2[i*column + j];   
        }   
    }
    return;
}

//行列の积
void mat_mul(float *m1, float *m2, float *sol, int row1, int column1, int row2, int column2){
    for(int i=0; i<row1; i++){
        for(int j=0; j<column2; j++){
            sol[i*column2 + j] = 0;
            for(int k=0; k<column1; k++) {
                sol[i*column2 + j] += m1[i*column1 + k]*m2[k*column2 + j];   
            }
        }   
    }
    return;
}

//转置矩阵计算
void mat_tran(float *m1, float *sol, int row_original, int column_original) {
    for(int i=0; i<row_original; i++) {
        for(int j=0; j<column_original; j++) {
            sol[j*row_original + i] = m1[i*column_original + j];   
        }   
    }
    return;
}

//行列の定数倍算出
void mat_mul_const(float *m1,float c, float *sol, int row, int column){
    for(int i=0; i<row; i++){
        for(int j=0; j<column; j++){
            sol[i*column + j] = c * m1[i*column + j];   
        }   
    }
    return;
}

//逆行列算出
void mat_inv(float *m, float *sol, int column, int row){
    //allocate memory for a temporary matrix
    float* temp = (float *)malloc( column*2*row*sizeof(float) );
   
    //make the augmented matrix
    for(int i=0; i<column; i++) {
        //copy original matrix
        for(int j=0; j<row; j++) {
            temp[i*(2*row) + j] = m[i*row + j];  
        }
        
        //make identity matrix
        for(int j=row; j<row*2; j++) {
            if(j-row == i) {
                temp[i*(2*row) + j] = 1;
            }   
            else {
                temp[i*(2*row) + j] = 0;   
            }
        }
    }

    //Sweep (down)
    for(int i=0; i<column; i++) {
        //pivot selection
        float pivot = temp[i*(2*row) + i];
        int pivot_index = i;
        float pivot_temp;
        for(int j=i; j<column;j++) {
            if( temp[j*(2*row)+i] > pivot ) {
                pivot = temp[j*(2*row) + i];
                pivot_index = j;
            }   
        }  
        if(pivot_index != i) {
            for(int j=0; j<2*row; j++) {
                pivot_temp = temp[ pivot_index * (2*row) + j ];
                temp[pivot_index * (2*row) + j] = temp[i*(2*row) + j];
                temp[i*(2*row) + j] = pivot_temp;   
            }   
        }
        
        //division
        for(int j=0; j<2*row; j++) {
            temp[i*(2*row) + j] /= pivot;   
        }
        
        //sweep
        for(int j=i+1; j<column; j++) {
            float temp2 = temp[j*(2*row) + i];
            
            //sweep each row
            for(int k=0; k<row*2; k++) {
                temp[j*(2*row) + k] -= temp2 * temp[ i*(2*row) + k ];   
            }   
        }
    }
        
    //Sweep (up)
    for(int i=0; i<column-1; i++) {
        for(int j=i+1; j<column; j++) {
            float pivot = temp[ (column-1-j)*(2*row) + (row-1-i)];   
            for(int k=0; k<2*row; k++) {
                temp[(column-1-j)*(2*row)+k] -= pivot * temp[(column-1-i)*(2*row)+k];   
            }
        }   
    }     
   
    //copy result
    for(int i=0; i<column; i++) {
        for(int j=0; j<row; j++) {
            sol[i*row + j] = temp[i*(2*row) + (j+row)];   
        }   
    }
    free(temp);
    return;
}


//传感器偏移计算
void offset_cal(){
  delay(1000);
  accXoffset = 0;
  accYoffset = 0;
  accZoffset = 0;
  gyroXoffset = 0;
  gyroYoffset = 0;
  gyroZoffset = 0;

  for(int i=0; i<10; i++) {
    M5.IMU.getAccelAdc(&accX,&accY,&accZ);
    M5.IMU.getGyroAdc(&gyroX,&gyroY,&gyroZ);
    delay(meas_interval);
    accXoffset += accX;
    accYoffset += accY;
    accZoffset += accZ;
    gyroXoffset += gyroX;
    gyroYoffset += gyroY;
    gyroZoffset += gyroZ;
  }

  accXoffset /= 10;
  accYoffset = accYoffset / 10 + 8192;
  accZoffset /= 10;
  gyroXoffset /= 10;
  gyroYoffset /= 10;
  gyroZoffset /= 10;
}


//从加速度传感器获取倾斜数据 [deg]
float get_acc_data() {
  M5.IMU.getAccelAdc(&accX,&accY,&accZ);
  //得到的传感器值用偏移减去
//倾斜角导出单位为deg
theta_deg  = atan( (float)(accZ - accZoffset) / (float)(-1 * accY - accYoffset) ) * 57.29578f;
  return theta_deg;
}


//用加速度传感器测定倾斜的偏差
void acc_init(){
  float theta_array[sample_num];
  for(int i=0; i<sample_num; i++) {
    theta_array = get_acc_data();
    delay(meas_interval);
  }

  //平均値
  theta_mean = 0;
  for(int i=0; i<sample_num; i++) {
    theta_mean += theta_array;
  }
  theta_mean /= sample_num;
   
  //分散
  float temp;
  theta_variance = 0;
  for(int i=0; i<sample_num; i++) {
    temp = theta_array - theta_mean;
    theta_variance += temp*temp;
  }
  theta_variance /= sample_num;
}


//x軸 角速度取得
float get_gyro_data() {
  M5.IMU.getGyroAdc(&gyroX,&gyroY,&gyroZ);
  //得到的传感器值用偏移减去
theta_dot = ((float) (gyroX - gyroXoffset)) * M5.IMU.gRes;
  return theta_dot;
}

//陀螺传感器偏移测量
void gyro_init() {
  float theta_dot_array[sample_num];
  for(int i=0;i<sample_num;i++) {
    theta_dot_array =  get_gyro_data();
    delay(meas_interval);
  }
   
  //平均値
  theta_dot_mean = 0;
  for(int i=0;i<sample_num;i++) {
    theta_dot_mean += theta_dot_array;   
  }
  theta_dot_mean /= sample_num;

  //分散
  float temp;
  theta_dot_variance = 0;
  for(int i=0; i<sample_num; i++) {
    temp = theta_dot_array - theta_dot_mean;
    theta_dot_variance += temp*temp;   
  }
  theta_dot_variance /= sample_num;
}


//=========================================================
//卡尔曼滤波算法处理
//=========================================================
void update_theta()
{     
    //用加速度传感器测量角度
float y = get_acc_data(); //degree
   
    //输入数据：角速度
float theta_dot_gyro = get_gyro_data(); //degree/sec
      
    //卡尔曼增益计算: G = P'C^T(W+CP'C^T)^-1
    float P_CT[2][1] = {};
    float tran_C_theta[2][1] = {};
    mat_tran(C_theta[0], tran_C_theta[0], 1, 2);//C^T
    mat_mul(P_theta_predict[0], tran_C_theta[0], P_CT[0], 2, 2, 2, 1);//P'C^T
    float G_temp1[1][1] = {};
    mat_mul(C_theta[0], P_CT[0], G_temp1[0], 1,2, 2,1);//CP'C^T
    float G_temp2 = 1.0f / (G_temp1[0][0] + theta_variance);//(W+CP'C^T)^-1
    float G1[2][1] = {};
    mat_mul_const(P_CT[0], G_temp2, G1[0], 2, 1);//P'C^T(W+CP'C^T)^-1
   
    //傾斜角推定値算出: theta = theta'+G(y-Ctheta')
    float C_theta_theta[1][1] = {};
    mat_mul(C_theta[0], theta_data_predict[0], C_theta_theta[0], 1, 2, 2, 1);//Ctheta'
    float delta_y = y - C_theta_theta[0][0];//y-Ctheta'
    float delta_theta[2][1] = {};
    mat_mul_const(G1[0], delta_y, delta_theta[0], 2, 1);
    mat_add(theta_data_predict[0], delta_theta[0], theta_data[0], 2, 1);
           
    //共分散行列算出: P=(I-GC)P'
    float GC[2][2] = {};
    float I2[2][2] = {{1,0},{0,1}};
    mat_mul(G1[0], C_theta[0], GC[0], 2, 1, 1, 2);//GC
    float I2_GC[2][2] = {};
    mat_sub(I2[0], GC[0], I2_GC[0], 2, 2);//I-GC
    mat_mul(I2_GC[0], P_theta_predict[0], P_theta[0], 2, 2, 2, 2);//(I-GC)P'
      
    //下一时刻的倾斜角的预测值计算: theta'=Atheta+Bu
    float A_theta_theta[2][1] = {};
    float B_theta_dot[2][1] = {};
    mat_mul(A_theta[0], theta_data[0], A_theta_theta[0], 2, 2, 2, 1);//Atheta
    mat_mul_const(B_theta[0], theta_dot_gyro, B_theta_dot[0], 2, 1);//Bu
    mat_add(A_theta_theta[0], B_theta_dot[0], theta_data_predict[0], 2, 1);//Atheta+Bu
   
    //下一时刻的共分散矩阵计算: P'=APA^T + BUB^T
    float AP[2][2] = {};   
    float APAT[2][2] = {};
    float tran_A_theta[2][2] = {};
    mat_tran(A_theta[0], tran_A_theta[0], 2, 2);//A^T
    mat_mul(A_theta[0], P_theta[0], AP[0], 2, 2, 2, 2);//AP
    mat_mul(AP[0], tran_A_theta[0], APAT[0], 2, 2, 2, 2);//APA^T
    float BBT[2][2];
    float tran_B_theta[1][2] = {};
    mat_tran(B_theta[0], tran_B_theta[0], 2, 1);//B^T
    mat_mul(B_theta[0], tran_B_theta[0], BBT[0], 2, 1, 1, 2);//BB^T
    float BUBT[2][2] = {};
    mat_mul_const(BBT[0], theta_dot_variance, BUBT[0], 2, 2);//BUB^T
    mat_add(APAT[0], BUBT[0], P_theta_predict[0], 2, 2);//APA^T+BUB^T

    //角速度
    theta_dot2 = theta_dot_gyro - theta_data[1][0];
}


//卡尔曼滤波器的初始设定初始姿势假设为0°(直立)
void ini_theta(){
  //初期設定開始 LED ON
  digitalWrite(10, LOW);

  //传感器偏移计算
offset_cal();
  
  //传感器偏移取得测量100次分散导出
acc_init();
  gyro_init();

  //卡尔曼滤波器的初始设定初始姿势假设为0°(直立)
theta_data_predict[0][0] = 0;
  theta_data_predict[1][0] = theta_dot_mean;
  
  P_theta_predict[0][0] = 1;
  P_theta_predict[0][1] = 0;
  P_theta_predict[1][0] = 0;
  P_theta_predict[1][1] = theta_dot_variance;

  thetaI = 0.0;

  //初始设定完成 LED OFF
  digitalWrite(10, HIGH);  
}


const int motorL = 0x60;
const int motorR = 0x64;

long Speed;
long SpeedL, SpeedR;

//电动机驱动I2C控制 motor driver I2C
//Reference
//http://makers-with-myson.blog.so-net.ne.jp/2014-05-15
void writeMotorResister(byte vset, byte data1){
  int vdata = vset << 2 | data1;
  Wire.beginTransmission(motorL);
  Wire.write(0x00);
  Wire.write(vdata);
  Wire.endTransmission(true);
  
  Wire.beginTransmission(motorR);
  Wire.write(0x00);
  Wire.write(vdata);
  Wire.endTransmission(true);
}


//数据接收
BLYNK_WRITE(V0) {
  Kp = param.asFloat();
  Serial.println("Kp!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}

BLYNK_WRITE(V1) {
  Ki = param.asFloat();
  Serial.println("Ki!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}

BLYNK_WRITE(V2) {
  Kd = param.asFloat();
  Serial.println("KD!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}

BLYNK_WRITE(V3) {
  VMIN = param.asInt();
  //Serial.println("OFFSET!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}

void setup() {
  pinMode(10, OUTPUT);
  pinMode(39, INPUT);

  Serial.begin(115200);
  M5.begin();
  M5.Axp.ScreenBreath(8);
  M5.Lcd.fillScreen(RED);
  M5.IMU.Init();

  //卡尔曼过滤器初始设置
  ini_theta();

  Blynk.setDeviceName("Blynk");
  Blynk.begin(auth);

  Wire.begin(0, 26, 400000); //SDA, SCL
  writeMotorResister(0x00, 0x00);

  //用卡尔曼滤波器导出角度
  tickerUpdate.attach(theta_update_interval, update_theta);
}


void loop() {
  Blynk.run();
  //tickerUpdate.detach();

  if(digitalRead(39) == LOW){
    //Blynk.disconnect();
    writeMotorResister(0x00, 0x00);
    ini_theta();
  }

  if(Blynk.connected()){
    M5.Lcd.fillScreen(GREEN);
  }else{
    M5.Lcd.fillScreen(RED);
  }

  thetaP = theta_data[0][0] / 90.0;
  thetaI += thetaP;
  thetaD = theta_dot2 / 250.0;

  power = thetaP * Kp + thetaI * Ki + thetaD * Kd;
  power = max(-1.0, min(1.0, power));

  int Vdata = ((float)(VMAX - VMIN) * fabs(power)) + VMIN;

  Serial.print(power);
  Serial.print(", ");
  Serial.print(Vdata);
  Serial.print(", ");
  Serial.print(theta_data[0][0]);
  Serial.print(", ");
  Serial.println(theta_dot2);

  /*
  Serial.print(accX - accXoffset);
  Serial.print(", ");
  Serial.print(accY - accYoffset);
  Serial.print(", ");
  Serial.print(accZ - accZoffset);
  Serial.print(", ");
  Serial.print(gyroX - gyroXoffset);
  Serial.print(", ");
  Serial.print(gyroY - gyroYoffset);
  Serial.print(", ");
  Serial.println(gyroZ - gyroZoffset);
  */

  if(fabs(theta_data[0][0]) > 60 || Vdata == 0){
    writeMotorResister(0x00, 0x00);
  }else if(power > 0){
    writeMotorResister(byte(Vdata), 0x01);
  }else if(power < 0){
    writeMotorResister(byte(Vdata), 0x02);
  }
  
  //tickerUpdate.attach(theta_update_interval, update_theta);
  delay(WAIT);
}[/mw_shl_code]

沧海笑1122

blynk实现参数调试真是一个好办法，学习并收藏。