0
点赞
收藏
分享

微信扫一扫

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

腾讯优测 2022-11-18 阅读 203
平衡小车git引脚编码器编程语言


目录

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

​​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控制算法

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

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(){
  
}

小车整体的动力学建模

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

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

这个模型不是很严谨，因为

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

不总是比较小，而且我们的目的也不是仅仅为了保持小车平衡，在保持平衡的同时也要使得

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

趋于0。不过暂时粗略先用这个动力学模型来做，后面再重新建立更加准确的模型进行优化。

经过测量，我的平衡小车

质量：m=0.748kg

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

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

那么动力学模型即为：

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

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

 计算特征值

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

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

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

然后我们看一下能控性：

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

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

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

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

 我们设计一个线性控制器

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

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

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

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

，也就是平衡小车的控制算法（PID,LQR,MPC)及arduino程序导航贴_平衡小车_10但是我们的

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

 。现在我们可以用mpu6050这个传感平衡小车的控制算法（PID,LQR,MPC)及arduino程序导航贴_平衡小车_10，也就是倾角，但是倾角的角加速度我们没有传感器可以测得。也就是说我们现在无法知道z的准确值。

由于我们的控制器u是z的函数，因此我们要想办法得知z。那么接下来我们将使用一个观测器来得到平衡小车的控制算法（PID,LQR,MPC)及arduino程序导航贴_平衡小车_13。

龙伯格观测器

推导

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

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

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

 其中平衡小车的控制算法（PID,LQR,MPC)及arduino程序导航贴_git_16为估计的初值，可以任意估计，因为随着时间增加，这个值会收敛到跟实际值几乎一样。

u为输入，这里我们的input为平衡小车的控制算法（PID,LQR,MPC)及arduino程序导航贴_编码器_17，也就是平衡小车轮子在水平方向的加速度。

y就是倾角

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

。

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

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

举报

相关推荐

白泽平衡小车导航贴

平衡小车数据串口#include编程语言

基于arduino的循迹小车（含有PID算法）

循迹小车循迹PIDPIDgit#define编程语言

PID控制算法及arduino应用（电机调速和位置控制）

esp32arduinopid增量式位置式编程语言

PID小车平衡和跳跃的核心代码

数据示例代码初始化物联网yyds干货盘点

基于PID的智能小车循迹控制

自动驾驶PID智能小车循迹控制增量式PID人工智能

arduino小车跟随纸板程序

arduinogit#define连线编程语言

PID控制控制算法

示例代码Python控制算法HarmonyOS后端开发yyds干货盘点

卡尔曼滤波（Kalman filter)算法以及Arduino应用-mpu6050（导航贴）

单片机卡尔曼滤波算法arduinompu6050编程语言

matlab小车运动轨迹增量式PID控制

时间常数框图控制系统人工智能

Arduino白泽四足机器人（导航贴）

自动驾驶matlab机器学习3d打印上传编程语言
0 条评论