程序员经验分享-hwhale

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

2023-05-16

目录

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

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

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

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

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

  • 解决secureCRT账号密码正确，无法连接服务器，那大概因为不支持新的密钥交换算法

    连接比较新版本的linux类服务器 xff0c 是否出现下面这些问题 xff1f 或者是openstack新建centos7镜像的时候 xff0c 无法连接新创建的centos7系统 我百度或者谷歌好像都没有找到答案啊 xff0c 所以才写

  • 树莓派SSH连接-SSH服务安装与开机自动启动

    1 SSH连接 SSH连接比Telnet远程桌面连接使用更为安全 xff0c 已经成为行业标准 使用SSH连接树莓派 xff0c 可以对树莓派进行远程控制与编程开发 xff0c 在没有桌面环境的条件下使用SSH连接是非常合适的选择 第2节和

  • 【VSCode Git】stage和stash的区别

    VSCode Git stage和stash的区别 问题来源 用vscode提交变更的文件时 xff0c 会发现2个相似的选项 Stage Changes 和 Stash Changes xff0c 乍一看不知道用哪个 xff0c 它们有什

  • canal文档

    简介 github地址 canal k n l xff0c 译意为水道 管道 沟渠 xff0c 主要用途是基于 MySQL 数据库增量日志解析 xff0c 提供增量数据订阅和消费 canal 工作原理 canal 模拟 MySQL slav

  • 自然拼读与词根词缀简版

    词根词缀 词根词缀重点 1 ab abs 表示远离 或否定 2 ac acr 表示尖 xff0c 酸 xff1b 3 aer aero 表示空气 xff0c 天空 4 am 表示爱 5 ambi ambul 表示周围 xff1b 6 ani

  • MySQL递归查询上下级菜单

    正文 在传统的后台管理系统里面经常会需要展示多级菜单关系 xff0c 今天我们来学一下如何使用一条SQL语句展示多级菜单 现在我们有一张corpinfo单位表 xff0c 里面有一个belong字段指向上级单位 xff0c 首先来看一下现在

  • 基于ESP32双无刷FOC电机的瓦力平衡机器人（2）

    恍恍惚惚中 xff0c 感觉瓦力已经慢慢悠悠的向我走来 xff0c 看了他的孤独 xff0c 感觉自己的也就不算什么了 断断续续搞了差不多两周的时间 xff0c 总算是把这些底层模块都调通了 xff08 虽然还完全看不出任何瓦力的影子 xf

  • 嵌入式 职位描述 职位要求

    来于智联招聘 前程无忧 xff0c 有关工作经验 xff0c 管理经验 xff0c 学历一并删掉 xff0c 只剩职位描述 职位要求 看看自己还缺些什么 嵌入式软件工程师 关专业 xff0c 本科或以上学历 xff1b 2 基础扎实 xff

  • 信道脉冲响应CIR

    博客写作技巧 xff1a 遇到的问题 如何解决问题 需要那种帮助 信道脉冲响应 xff1a CIR 问题 xff1a 场强测量系统需要获取场强和信道信息 xff0c 那么CIR是什么 xff1f 如何利用CIR反映信道特性 xff1f 解决

  • OpenMV数据打包发送以及STM32对数据的解析（串口方式）

    今天尝试了使用Openmv用串口发送数据 xff0c 32接收 xff0c 遇到了一些坑 xff0c 但是最后还是实现了 xff0c 难住我的地方并不是关于传输的代码 xff0c 而是那个板子串口3不知道因为什么原因接收到的数据是错误的 x

  • linux下 c++ 服务器开发（一）

    苦逼的c 43 43 程序员还没找到工作 xff0c 所以顺便开始写服务器练手 对内容不满意不要喷我我是写给自己看的 xff08 把自己犯得错误记下来 xff09 1 我的电脑是win10的 xff0c 所以先去网上下虚拟机 xff0c 我

  • 最优化算法——常见优化算法分类及总结

    之前做特征选择 xff0c 实现过基于群智能算法进行最优化的搜索 xff0c 看过一些群智能优化算法的论文 xff0c 在此做一下总结 最优化问题 在生活或者工作中存在各种各样的最优化问题 xff0c 比如每个企业和个人都要考虑的一个问题

  • 利用手机摄像头采集图片运行ORB-SLAM2

    一 ROS配置安装 二 ORB SLAM2配置安装 可参考前文 ROS仿真环境安装与配置 身在江湖的郭大侠的博客 CSDN博客 三 Android手机摄像头与ROS建立通信 GitHub有个开源的项目 xff0c 可以通过wifi将摄像头捕

  • VINS_FUSION

    VINS FUSIO xff2e 意义 VINS Fusion在VINS Mono的基础上 xff0c 添加了GPS等可以获取全局观测信息的传感器 xff0c 使得VINS可以利用全局信息消除累计误差 xff0c 进而减小闭环依赖 此外 x

  • VINS_FUSION编译运行

    一 ROS安装 见前文 二 ceres安装 GitHub地址 xff1a GitHub ceres solver ceres solver A large scale non linear optimization library 14 0

  • Turtlebot2简单控制

    遥控 遥控前为了让turtlebot接受命令 xff0c 需要启动 roslaunch turtlebot bringup minimal lauch 键盘操作命令 xff1a roslaunch turtlebot teleop keyb

  • turtlebot2利用turtlebot_exploration_3d进行自主建图

    安装octomap ros和rviz插件 sudo apt get install ros indigo octomap 源码安装 xff1a turtlebot exploration 3d 本机为Ubuntu16对应的ros版本为kin

  • CSDN周赛第十二期

    CSDN的比赛难度相对而且比较简单 xff0c 适合小白练手 xff0c 而且刷题本身在于锻炼思维 xff0c 写出题目不难 xff0c 但是需要拥有缜密的思维才能通过全部的用例 1 豚鼠排名榜 已知字符A B C 每个字符都有自己的权值q

  • rocon app

    规格 xff1a Rapp指代rocon app或者叫robot app 用在机器人的控制中Robotics in Concert xff0c 这是通过 Rapp Manager 安装和执行的元数据 它旨在允许更高级别的控制器采用提供所需公

  • 矩阵按键的两种扫描方法

    1 实验目的 掌握两种按键扫描方法 xff1a 行扫描 xff0c 行列扫描 xff08 高低电平翻转 xff09 2 实验流程图 3 代码分析 xff08 1 xff09 行扫描 include 34 stm32f10x h 34 u16

随机推荐

  • 计算机二进制的浮点表示法

    计算机二进制的浮点表示法 0 1 3 61 0 3 组成 符号位 0表示正 1表示负 指数位 2的n次方 xff0c n 43 127 对于4字节浮点数 xff0c 此处为127 再换算成二进制 尾数位 直接用 过程 将整数部分换算成二进制

  • STM32延时函数的三种方法——最好掌握第三种

    单片机编程过程中经常用到延时函数 xff0c 最常用的莫过于微秒级延时delay us 和毫秒级delay ms 1 普通延时法 这个比较简单 xff0c 让单片机做一些无关紧要的工作来打发时间 xff0c 经常用循环来实现 xff0c 不

  • 【常用模块】HC-05蓝牙串口通信模块使用详解（实例：手机蓝牙控制STM32单片机）

    HC 05蓝牙串口通信模块应该是使用最广泛的一种蓝牙模块之一了 为什么呢 xff1f 因为HC05模块是一款高性能主从一体蓝牙串口模块 xff0c 可以不用知道太多蓝牙相关知识就可以很好的上手 说白了 xff0c 只是个蓝牙转串口的设备 x

  • uln2003驱动电路

    一 uln2003有什么作用 ULN2003是大电流驱动阵列 xff0c 多用于单片机 智能仪表 PLC 数字量输出卡等控制电路中 可直接驱动继电器等负载 输入5VTTL电平 xff0c 输出可达500mA 50V ULN2003是高耐压

  • 简单超声波测距

    用到模块 hc sr04超声波模块 xff0c stm32开发板 本实验通过超声波测距模块得到长度 直接打印到窗口显示 xff0c 故主要用到定时器函数 xff0c 串口函数 hcsr04 c 只需要提供一个 10uS以上脉冲触发信号 xf

  • Javaer,你必须要了解的ExecutorService

    ExecutorService初接触 之前做的一个功能里有一个耗时操作 xff1a 处理数据库里对应的记录 xff0c 然后将每个处理后的结果做个排序 恕本人小白 xff0c 刚开始直接用单线程处理 xff01 你敢信 xff1f xff0

  • 平衡自行车-理论篇

    原文链接 xff1a http nicekwell net blog 20180121 ping heng zi xing che li lun pian html 一 模型分析 1 倒立摆2 自行车的平衡控制 2 1 怎样的状态才叫平衡2

  • 魔百盒CM201-1刷机教程

    家里有一块魔百盒CM201 1一直在家积灰 xff0c 由于看到网上教程可以刷各种系统 xff0c 所以想着玩来试试看 先刷一个电视版安卓系统看 盒子样子大概就是下面这样 xff1a 拿到手之后就迫不及待的将外壳拆掉了 xff0c 下面这样

  • RK3288刷机教程：安装Ubuntu 16.04

    网上有很多基于瑞芯微RK3288芯片的板子 xff0c 个人感觉配置都非常不错 xff01 然后就淘了两块玩玩 如下图所示 xff1a 然后可以看到 xff0c 各种接口也比较全乎 xff01 有HDMI和VGA视频输出接口 xff0c 两

  • ros串口通讯（读取串口数据）

    ros串口通讯是非常重要的通讯手段 xff0c 通常跟下位机或者各种usb口外设都是通过串口进行通讯的 那么我们跟着教程来学习一下如何读取手机通过无线串口发送给电脑的数据 这里我通过一个usb ttl工具将蓝牙连接到电脑上 xff0c 然后

  • No package ‘orocos-bfl‘ found

    目录 问题 xff1a 原因 xff1a 解决办法 xff1a 问题 xff1a 在编译ros工程的时候 xff0c 出现如下错误提示 xff1a No package 39 orocos bfl 39 found 如下图所示 xff1a

  • 人工智能（AI）入门

    人工智能的入门学习需要具备的知识结构 xff1a 一 编程语言选择 推荐python xff0c 原因有二 xff0c 其一 xff0c 语法简单易学 xff1b 其二 xff0c 有丰富的库支持 二 算法设计基础 人工智能的研究内容集中在

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

    正在更新中 这篇文章要跟大家一起完全搞明白卡尔曼滤波 xff0c 连一个标点符号也不放过 xff0c 完完全全理解明白 如果你看不懂 xff0c 那说明我写的不好 本文是看了dr con博士的视频后做的 xff0c 建议可以去看看 如果哪里

  • ROS发布tf坐标

    我们写个小程序来发布一个坐标系 xff1a 坐标系消息格式 xff1a std msgs Header header 头信息 uint32 seq 序列号 time stamp 时间戳 string frame id 坐标 ID strin

  • pop_back()的用法及运行机制

    vector在c 43 43 中非常好用 xff0c 简单的说 xff0c vector是一个能够存放任意类型的动态数组 能够增加和压缩数据 一般使用push back 和pop back 函数将数据存放进容器末尾 如下例程 xff1a i

  • Gazebo启动不开

    问题 xff1a 按照书上的指引 xff0c 启动gazebo仿真软件 当然记得运行roscore rosrun gazebo ros gazebo 结果我在这个页面等了三分钟一点儿动静也没有 查阅资料 xff0c 说明这是因为model库

  • Gazebo仿真小例程一（通过例程熟悉整个仿真步骤）

    目录 1 编辑urdf文件 xff08 1 xff09 dynamic标签 xff08 2 xff09 gazebo标签 xff08 3 xff09 transmission标签 xff08 4 xff09 ros control插件 2

  • Arduino ide配置esp32硬件支持(配置esp32的arduino开发环境）

    ESP32学习导航帖 前言 当我们用arduino ide基于esp32开发板进行程序开发的时候 xff0c arduino ide按照默认安装之后是无法直接给esp32下载程序的 xff0c 也不支持esp32相关的库 这主要是默认的ar

  • AS5600磁编码器的使用以及简单的滤波算法（arduino）

    目录 前言 实践 示例一 xff1a 发现IIC设备 示例二 xff1a 读取AS5600原始数据 示例三 xff1a 对读取到的AS5600原始数据进行低通滤波 1 一阶滤波算法的原理 2 编程实现 前言 AS5600磁编码器常用于电机的

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

    目录 平衡小车电机位置测试小实验 1 编码器脉冲计数 PID控制算法 平衡小车PID调参实验 位置环 2 编码器计数转换角度 小车整体的动力学建模 通过特征值判断系统动态特性 龙伯格观测器 平衡小车电机位置测试小实验 1 编码器脉冲计数 c

热门标签