平衡小车的控制算法（PID,LQR,MPC)及arduino程序导航贴_平衡车mpc

目录

平衡小车电机位置测试小实验

1.编码器脉冲计数

PID控制算法

平衡小车PID调参实验

位置环

2.编码器计数转换角度

小车整体的动力学建模

通过特征值判断系统动态特性

龙伯格观测器

---

平衡小车电机位置测试小实验

1.编码器脉冲计数

const byte LeftMotorInterruptP = 22;  //左电机编码器中断引脚
const byte LeftMotorCountP = 23;      //左电机编码器计数引脚
const byte RightMotorInterruptP = 18; //右电机编码器中断引脚
const byte RightMotorCountP = 19;     //右电机编码器计数引脚
 
const byte MotorDriverEn = 5;         //电机驱动器使能
const byte LeftMotorP1 = 15;          //左电机控制io口1
const byte LeftMotorP2 = 13;          //左电机控制io口2
const byte RightMotorP1 = 16;         //右电机控制io口1
const byte RightMotorP2 = 17;         //右电机控制io口2
 
volatile long LeftMotorCounter = 0;   //左电机中断计数位置
volatile long RightMotorCounter = 0;  //右电机中断计数位置
 
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;  //声明一个portMUX_TYPE类型的变量，利用其对主代码和中断之间的同步进行处理
portMUX_TYPE mux_1 = portMUX_INITIALIZER_UNLOCKED;
 
//左电机中断函数
void LeftMotorInterruptF() {
  portENTER_CRITICAL_ISR(&mux_1);
  delayMicroseconds(10);  //延时20ms作为消抖，如果是很稳定的中断可以不加或者加很少的消抖时间
  if(digitalRead(LeftMotorInterruptP) == LOW)  //因为是上拉触发，所以在消抖时间完后读取引脚高低电平，如果还是为低那么就代表出现了一次稳定的中断
  {
    if(digitalRead(LeftMotorCountP)==LOW)
        LeftMotorCounter++;
    else
        LeftMotorCounter--;
    Serial.print("左边电机位置：");Serial.println(LeftMotorCounter);
  }
  portEXIT_CRITICAL_ISR(&mux_1);
}
//右电机中断函数
void RightMotorInterruptF() {
  portENTER_CRITICAL_ISR(&mux);
  delayMicroseconds(10);  //延时2ms作为消抖，如果是很稳定的中断可以不加或者加很少的消抖时间
  if(digitalRead(RightMotorInterruptP) == LOW)    //因为是下拉触发，所以在消抖时间完后读取引脚高低电平，如果还是为低那么就代表出现了一次稳定的中断
  {
    if(digitalRead(RightMotorCountP)==LOW)
        RightMotorCounter--;
    else
        RightMotorCounter++;
    Serial.print("右边电机位置：");Serial.println(RightMotorCounter);
  } 
  portEXIT_CRITICAL_ISR(&mux);
}
 
 
void setup(){
  Serial.begin(115200);
  Serial.println("中断测试实验"); 
  
  //off motor enable
  pinMode(MotorDriverEn,OUTPUT);
  digitalWrite(MotorDriverEn,LOW);
 
  pinMode(LeftMotorP1,OUTPUT);
  pinMode(LeftMotorP2,OUTPUT);
  pinMode(RightMotorP1,OUTPUT);
  pinMode(RightMotorP2,OUTPUT);
  digitalWrite(LeftMotorP1,LOW);
  digitalWrite(LeftMotorP2,LOW);
  digitalWrite(RightMotorP1,LOW);
  digitalWrite(RightMotorP2,LOW);
 
  pinMode(LeftMotorInterruptP, INPUT_PULLUP);  //先把引脚设置为上拉输入模式
  pinMode(RightMotorInterruptP, INPUT_PULLUP);  //先把引脚设置为上拉输入模式
  pinMode(LeftMotorCountP,INPUT);
  pinMode(RightMotorCountP,INPUT);
  
  attachInterrupt(digitalPinToInterrupt(LeftMotorInterruptP), LeftMotorInterruptF, FALLING);
  attachInterrupt(digitalPinToInterrupt(RightMotorInterruptP), RightMotorInterruptF, FALLING);
}
 
void loop(){
  
}

PID控制算法

电机速度环和位置环请看这篇文章：

PID控制算法及arduino应用（电机调速和位置控制）_Allen953的博客-CSDN博客​​​​​​​

2.编码器计数转换角度

 
const int CountperCircuit = 390;      //390个脉冲为一圈
const double pi = 3.141592653;        //pi
 
const byte LeftMotorInterruptP = 22;  //左电机编码器中断引脚
const byte LeftMotorCountP = 23;      //左电机编码器计数引脚
const byte RightMotorInterruptP = 18; //右电机编码器中断引脚
const byte RightMotorCountP = 19;     //右电机编码器计数引脚
 
const byte MotorDriverEn = 5;         //电机驱动器使能
const byte LeftMotorP1 = 15;          //左电机控制io口1
const byte LeftMotorP2 = 13;          //左电机控制io口2
const byte RightMotorP1 = 16;         //右电机控制io口1
const byte RightMotorP2 = 17;         //右电机控制io口2
 
volatile long LeftMotorCounter = 0;   //左电机中断计数位置
volatile long RightMotorCounter = 0;  //右电机中断计数位置
volatile double LeftMotordeg = 0.0;   //左电机中断计数位置
volatile double RightMotordeg = 0.0;  //右电机中断计数位置
 
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;  //声明一个portMUX_TYPE类型的变量，利用其对主代码和中断之间的同步进行处理
portMUX_TYPE mux_1 = portMUX_INITIALIZER_UNLOCKED;
 
//左电机中断函数
void LeftMotorInterruptF() {
  portENTER_CRITICAL_ISR(&mux_1);
  delayMicroseconds(10);  //延时20ms作为消抖，如果是很稳定的中断可以不加或者加很少的消抖时间
  if(digitalRead(LeftMotorInterruptP) == LOW)  //因为是上拉触发，所以在消抖时间完后读取引脚高低电平，如果还是为低那么就代表出现了一次稳定的中断
  {
    if(digitalRead(LeftMotorCountP)==LOW)
        LeftMotorCounter++;
    else
        LeftMotorCounter--;
    LeftMotordeg = double(LeftMotorCounter)/double(CountperCircuit)*360.0;
    Serial.print("左边电机位置：");Serial.print(LeftMotordeg);Serial.println("'C");
  }
  portEXIT_CRITICAL_ISR(&mux_1);
}
//右电机中断函数
void RightMotorInterruptF() {
  portENTER_CRITICAL_ISR(&mux);
  delayMicroseconds(10);  //延时2ms作为消抖，如果是很稳定的中断可以不加或者加很少的消抖时间
  if(digitalRead(RightMotorInterruptP) == LOW)    //因为是下拉触发，所以在消抖时间完后读取引脚高低电平，如果还是为低那么就代表出现了一次稳定的中断
  {
    if(digitalRead(RightMotorCountP)==LOW)
        RightMotorCounter--;
    else
        RightMotorCounter++;
    RightMotordeg = double(RightMotorCounter)/double(CountperCircuit)*360.0;
    Serial.print("右边电机位置：");Serial.print(RightMotordeg);Serial.println("'C");
  } 
  portEXIT_CRITICAL_ISR(&mux);
}
 
 
void setup(){
  Serial.begin(115200);
  Serial.println("中断测试实验"); 
  
  //off motor enable
  pinMode(MotorDriverEn,OUTPUT);
  digitalWrite(MotorDriverEn,LOW);
 
  pinMode(LeftMotorP1,OUTPUT);
  pinMode(LeftMotorP2,OUTPUT);
  pinMode(RightMotorP1,OUTPUT);
  pinMode(RightMotorP2,OUTPUT);
  digitalWrite(LeftMotorP1,LOW);
  digitalWrite(LeftMotorP2,LOW);
  digitalWrite(RightMotorP1,LOW);
  digitalWrite(RightMotorP2,LOW);
 
  pinMode(LeftMotorInterruptP, INPUT_PULLUP);  //先把引脚设置为上拉输入模式
  pinMode(RightMotorInterruptP, INPUT_PULLUP);  //先把引脚设置为上拉输入模式
  pinMode(LeftMotorCountP,INPUT);
  pinMode(RightMotorCountP,INPUT);
  
  attachInterrupt(digitalPinToInterrupt(LeftMotorInterruptP), LeftMotorInterruptF, FALLING);
  attachInterrupt(digitalPinToInterrupt(RightMotorInterruptP), RightMotorInterruptF, FALLING);
}
 
void loop(){
  
}

小车整体的动力学建模

这个模型不是很严谨，因为不总是比较小，而且我们的目的也不是仅仅为了保持小车平衡，在保持平衡的同时也要使得趋于0。不过暂时粗略先用这个动力学模型来做，后面再重新建立更加准确的模型进行优化。

经过测量，我的平衡小车

质量：m=0.748kg

质心高度约为40mm：L=0.04m

重力加速度用9.8：g=9.8m/s^2

那么动力学模型即为：

通过特征值判断系统动态特性

 计算特征值

可以看到，两个特征值并不都小于0.也就是说系统不稳定。

这跟我们的经验也一样，如果平衡小车没有输入的话，它将发生倾倒。

然后我们看一下能控性：

也就是Co矩阵的秩，如果Co矩阵是满秩的话，就意味着这个系统是可控的。

 可以看到Co矩阵是满秩的，也就意味着我们的系统是可控的。

然后我们就可以设计控制器进行控制了。

 我们设计一个线性控制器

这时，我们的控制器就设计好了，输入u就如上图最后一行一样。

可是我们的z并不知道啊。我的平衡小车上面是有一个mpu6050模块，可以测得小车的倾角，也就是

但是我们的 。现在我们可以用mpu6050这个传感，也就是倾角，但是倾角的角加速度我们没有传感器可以测得。也就是说我们现在无法知道z的准确值。

由于我们的控制器u是z的函数，因此我们要想办法得知z。那么接下来我们将使用一个观测器来得到。

龙伯格观测器

推导

求出L以后，我们得出了观测器的表达式。

 其中为估计的初值，可以任意估计，因为随着时间增加，这个值会收敛到跟实际值几乎一样。

u为输入，这里我们的input为，也就是平衡小车轮子在水平方向的加速度。

y就是倾角。

那么我们的观测器可以写为具体的表达形式为：