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

已于 2022-05-24 19:01:52 修改
阅读量5.5k 收藏 94
点赞数 11
分类专栏： arduino 文章标签： 平衡小车
于 2022-04-22 21:57:03 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qqliuzhitong/article/details/124355565
版权

目录

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

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就是倾角。

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

Allen953
关注
专栏目录
MPC、PP、Stanley、LQR、PID等控制算法的MATLAB程序实现
06-14
控制算法包含了MPC、PP、Stanley、LQR、PID等； 包含m源文件以及mdl模型；
PID和LQR两轮平衡车公式推导.pdf
09-15
PID和LQR两轮平衡车公式推导，英文版，包含一部分中文注释， 有针对PID和LQR部分的建模和公式计算过程
【路径跟踪】PID和MPC控制算法路径跟踪【含Matlab源码 6842期】
Matlab领域的博客
08-10 561
PID和MPC控制算法路径跟踪 完整的代码，方可运行；可提供运行操作视频！适合小白！
简单明了的说明白PID算法和MPC算法原理和对比
weixin_62688777的博客
07-26 1526
适用范围：PID 控制器适用于较简单的系统和工业应用；而 MPC 更适合于需要考虑复杂约束和预测未来行为的系统。计算复杂度：PID 控制器计算简单，易于实现；MPC 控制器则需要解决优化问题，计算量较大。灵活性：PID 控制器灵活性较低，难以处理复杂的约束；MPC 控制器可以灵活地处理多种约束。控制性能：在简单系统中，PID 控制器可以提供满意的性能；而对于复杂系统，MPC 控制器可以提供更好的控制性能。选择哪种控制策略取决于具体的应用场景、系统特性以及可用资源等因素。
自动控制原理学习--平衡小车的控制算法（三）
weixin_43030192的博客
05-09 1153
上一节PID的simulin仿真，这一节用LQR一、模型二、LQRLQR属于现代控制理论的一个很重要的点，这里推荐B站的【】课程（up主DR_CAN），讲得很好，这里引用了他视频里讲LQR的ppt。LQR属于lost优化问题，L：Linear；Q：Quadratic二次型；R：regulater 调制器主要是优化状态空间下，保证稳定的同时，如何选取状态方程特征值。二、平衡小车LQR。
双轮平衡车实现自平衡功能
Robotway的博客
05-22 1612
在双轮小车上安装一个六轴陀螺仪传感器，本文示例将实现双轮小车自主平衡功能。
PID算法（本人自写验证可用平衡车）
02-17
void PID init { PID e0 0; PID e1 0; PID e2 0; PID x chuzhi 28000; PID x out 28000; PID y shedingzhi 0; PID kp 100; PID ki 28; PID kd 10000; PID2 e0 0; PID2 e1 0; PID2 e2 0; PID2 x chuzhi 27000; PID2 x out 27000; PID2 y shedingzhi 0; PID2 kp 1; PID2 ki 40; PID2 kd 100; }
Arduino平衡小车
qq_52812027的博客
03-27 1万+
Arduino平衡小车 1.概述 此Arduino平衡小车在主控方面由Arduino UNO R3和Arduino sensor shield v5.0传感器扩展板组成。采用TB6612FNG作为电源和电机之间的中介给带编码器的直流电机供电以及传送PWM信号。采用六轴MPU6050进行角度采集并传给Arduino由Arduino计算角度偏差以及角加速度。现在，先看一下车的总体图片（此小车采用5V电源给Arduino直接供电省去了降压模块）以便有一个大致了解。 2.平衡原理（参考亚博） 2.1角度环平衡原
arduino平衡车程序
08-25
基于arduino2560平衡车程序，绝对真实有用，还带有其他功能，比如语音模块，全贡献出来了
平衡小车之PID算法
热门推荐
nulidehahafr的博客
12-02 2万+
一、直立控制（PD算法） int balance(float Angle,float Gyro)//角度，角速度 {      float Bias,kp=300,kd=1;      int balance;      Bias=Angle-ZHONGZHI;       //求出平衡的速度中值      balance=kp*Bias+Gyro*kd;   //计算平衡控制的电机PWM PD...
arduino平衡小车教程
m0_46509684的博客
01-08 1860
Arduino平衡小车教程 平衡小车教程
平衡小车控制算法
04-09
这个文件包含了平衡小车控制运动的模型介绍，以及平衡小车 的控制代码和传感器的数据处理，内涵PID的视频教程和整个项目的代码，希望能帮助到广大网友，谢谢大家
Arduino平衡车项目，附详细资料源码。
08-09
Arduino平衡车PID控制程序源码，硬件模块资料，APP蓝牙控制，PC上位机控制资料详细资料。
两轮自平衡小车（卡尔曼滤波代码+PID调试文档）
04-01
两轮自平衡小车（卡尔曼滤波代码+PID调试文档），包含详细的代码，stm32为控制器。
平衡小车速度控制原理
09-18
平衡小车速度控制原理与PID控制，
STM32F0电机驱动，平衡小车PID控制算法
10-18
除了陀螺仪的采集代码，这个只有电机驱动和PID控制，
基于C++实现的常见自动驾驶控制算法源码(PID、Pure pursuit、Stanley、MPC、LQR).zip
07-26
基于C++实现的常见自动驾驶控制算法源码(PID、Pure pursuit、Stanley、MPC、LQR).zip基于C++实现的常见自动驾驶控制算法源码(PID、Pure pursuit、Stanley、MPC、LQR).zip基于C++实现的常见自动驾驶控制算法源码(PID...
论文+程序 基于动态车辆模型的 Baidu Apollo lqr和mpc横向控制算法Analyzed Baidu Apollo
05-16
深入分析了基于动态车辆模型的百度Apollo平台上的线性二次调节器(LQR)和模型预测控制(MPC)横向控制算法。通过对这两种算法的比较研究，揭示了它们在处理车辆横向控制问题时的性能差异和适用场景。文章提供了详细的...
基于STM32F103C8T6芯片+LQR算法开发的平衡小车项目源码及完整资料.zip
最新发布
09-18
基于STM32F103C8T6芯片+LQR算法开发的平衡小车项目源码及完整资料.zip 该项目旨在通过STM32F103C8T6微控制器（MCU）作为核心控制单元，结合线性二次调节器（LQR）算法，实现一个能够自主保持平衡并响应控制指令的...
pid lqr mpc 对比
11-03
PID，LQR和MPC都是控制系统中常用的控制算法。其中，PID是最基本的控制算法，LQR和MPC则是更加高级的控制算法。三者的主要区别如下： 1. PID控制器是一种反馈控制器，它通过测量系统输出与期望输出之间的误差来调整控制器的输出。LQR和MPC则是基于模型的控制算法，它们需要系统的数学模型来进行控制。 2. LQR和MPC都是优化控制算法，它们可以通过优化目标函数来得到最优的控制输入。而PID控制器则是通过调整比例、积分和微分系数来得到最优的控制输入。 3. LQR和MPC都可以处理多变量系统和非线性系统，而PID控制器只适用于单变量线性系统。 4. LQR和MPC都可以处理时变系统，而PID控制器则需要重新调整参数来适应时变系统。 综上所述，LQR和MPC相比于PID控制器具有更高的控制精度和更广泛的适用范围，但需要更多的计算资源和更复杂的数学模型。
评论 4
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Allen953

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值