双轮自平衡机器人的移动控制系统设计毕业论文.doc

湖南工业大学本科毕业设计 论文 双轮自平衡机器人的移动控制系统设计双轮自平衡机器人的移动控制系统设计 毕业论文毕业论文 目录目录 摘 要 ABSTRACT 第 1 章 绪论 1 1 1 研究背景 1 1 2 国内外研究现状 1 1 3 本设计这要研究内容 3 第 2 章 模型建立和理论分析 4 2 1 建立直流电机空间状态方程 4 2 2 车模的能控能观性 5 2 3 建立平衡车倒立摆模型及零极点分布 6 2 4 本章小结 8 第 3 章 系统任务分解及总体设计构架 9 3 1 任务分析 9 3 2 直立平衡控制 9 3 3 速度控制 9 3 4 方向控制 10 3 5 系统总体设计框架 10 第 4 章 硬件选型及硬件电路 11 4 1 主控芯片 STC12C5A60S2 11 4 2 电源模块 11 4 3 电机驱动模块 12 4 4 传感器模块 13 4 5 系统控制结构图 15 第 5 章 传感器数据融合与滤波 16 5 1 惯性传感器数据融合和滤波的必要性 16 I 湖南工业大学本科毕业设计 论文 I 5 2 卡尔曼数据融合 16 5 3 互补滤波 18 5 4 速度滤波 19 第 6 章 PID 控制 20 6 1 PID 基本介绍 20 6 2 数字 PID 21 6 3 PID 控制器的设计 21 第 7 章 软件设计 23 7 1 软件设计主流程图 23 7 2 外部中断程序设计 25 7 3 各子模块程序设计 25 第 8 章 机械人安装和调试 28 8 1 机械人安装 28 8 2 软件调试 28 总结 32 致谢 33 参考文献 34 附录一 Kalman 滤波程序 36 附录二 参数说明 37 II 湖南工业大学本科毕业设计 论文 II 0 湖南工业大学本科毕业设计 论文 0 第第 1 1 章章 绪论绪论 1 1研究背景 近年来 国内外多家科研单位和企业以及自平衡车的爱好者对自平衡理论 的发展做出了许多有益的贡献 大大推进了自平衡技术的发展 同时也出现了 一批有代表性的平衡机器人和载人代步车作品 在现代社会中 移动机器人应 用也越来越广泛 机器人面临的环境和任务也越来越复杂 人们迫切需求一些 机器人可以适应更复杂的环境 控制更智能化 成本更低廉 让人们真正享受 科技的红利 另一方面当今世界能源危机越来越严重 人们都在探寻节能且方便的出行 方式 二轮小车就可以比四轮节能 因而这个原因提高了人们研究的热情 同 时两轮车比较简单 出行潇洒自如 可以急转弯 狭窄空间运行比较灵活 受到 年轻消费者的追捧 因而有广泛的市场价值 当代社会数据处理芯片的速度越来越快 各类传感器飞速发展 而且集成 的元器件价格比较便宜 为平衡机器人研究奠定了物资基础 作为一个自动化毕业生 主要研究动态控制 设计合理的电路原理图 并 把设计的硬件和软件结合 达到自动控制的效果 且控制稳定可靠 而两轮自 平衡机器人就是一个复杂的动态控制过程 很注重应用性 对所学知识的一次 综合运用 可以大大提高我的分析和设计能力 所以研究这个课题 具有双重 价值 1 2国内外研究现状 2002 年 美国 Lego 公司的 Steve Hassenplug 设计了两轮自平衡传感式机器 人 Legway 这个设计采用了电机的差动驱动方式 它可以工作在倾斜面甚至不 规则表面上 可遥控操作 通过对电机进行遥控 Legway 可以在进行前进 后 退和转弯时保持平衡 可以实现零半径转弯和 U 型 1 回转 图 1 1 Legway 1 湖南工业大学本科毕业设计 论文 1 2002 年 三洋电机展示了可以靠上体倒立来保持平衡的双轮行走机器人 FLATHRU 移动速度方便 平地行走时最大 30cm s 可搬运重量可达 10kg 运行时间长达 1 小时 车轮中嵌有一个输入功率为 90W 的直流电动机 头部则嵌入了两个相同的电动机 为了检测上体的平衡情况 使用了 3 个陀螺 仪和 1 个 3 轴加速器传感器 美国发明家 Dean Kamen 开发的 SEGWAY HT 是一款实用的商业化的两 轮机器人 可以独立载人 并保持平衡的运行 它模仿人两脚平衡行走原理 人的小脑有灵敏控制细胞 可以快速精确知道重心位置 并控制脚肌的作用 保持平衡 用陀螺仪进行模拟神经 电机模仿两脚 其外观如图 1 2 所示 机 器人使用了 5 个陀螺仪和一个数据融合集成器解决了多参数的融合问题 同时 采用高速处理器以 20000 次 秒的速度调整车身姿态 使系统较好保持直立状态 图 1 2 美国 SEGWAY 中国科学技术大学研究出两轮代步电动车 利用在车内的 CPU 控制器 处 理平衡 速度 加速度传感器的测量数据 通过动力学理论建立合理的力学模 型 根据模型设置相应模糊自适应控制算法 输出 PWM 控制两个伺服电机的转 动 使电动车可以根据人体的重心偏移 自动前进 后退以及转弯从而保持重 心平衡和正常前进 该车可控制灵活 并可以任意转弯 运行速度快 行走距 离长 上面这些研究成果 给我们很多经验 提供了很多研究方法 也启发我们 创新 总结前人 开发新的合理的开发方案 从以上研究可以发现 一般选择 陀螺仪 加速度计 倾斜角传感器 超声测距仪 红外线等传感器 通过建立 平衡模型 并用先进的控制算法模拟 采用很快的控制器快速调整车模姿态 同时发现软件是控制核心 因为硬件多会提高成本且不稳定 通过较好的控制 算法可以在原有的硬件基础上大大提高产品性能 受此因此本文对于卡尔曼滤 波 互补滤波 PID 等都采用软件形式以节约成本 2 湖南工业大学本科毕业设计 论文 2 1 3 本设计主要研究内容 1 根据小车的实际运动特点建立小车的平衡力学模型 对非线性系统在平衡 点进行线性化处理 并建立小车的控制传递函数从理论上分析传递函数的 零极点分布 得出小车平衡的条件 并提出控制方案 2 对小车的运动控制进行分解方便各个击破 降低设计难度 使设计过程更 直观 3 硬件器件的选型和搭建 选用 STC12C5A60S2 单片机作为核心控制器 选 用 MPU 6050 获取车模姿态 倾斜角和角加速度信息 选用 L298N 作 为电机驱动 选择 12V 直流减速电机作为动力 选择 LM2596S 模块作为 MPU6050 L298N 等器 件的 5v 电源 选光电编码器读取小车的速度信息 红 外传感器识别小车的方向 4 研究惯性传感器的数据融合 分析了卡尔曼滤波和互补滤波两种融合算法 5 具体分析小车的反馈控制 PID 调节器 设计小车运动平衡的 PID 控制算法 具体分析 PID 各参数的影响为调试打下基础 6 搭建硬件 编写软件控制程序 结合硬件和软件进行实际调试 并不断改 进程序 达到理想效果 3 湖南工业大学本科毕业设计 论文 3 第二章第二章 模型建立和理论分析模型建立和理论分析 2 1 建立直流电机空间状态方程 两轮自平衡机器人的核心就是控制电机 所以研究电机的状态方程是解决 问题的首要 我们可以建立一个直流电机线性模型 参数见附录二 如图 2 1 图 2 1 直流电机模型 当电机加上电压后 电机转矩输出上图电流成正比关系如下 I m T m K 2 1 于此同时 电机线圈也会产生一个反电动势且正比于线圈转速关系方程如下 ee Uk w 2 2 根据图 2 1 和基尔霍夫定律可得到电路的微分方程如下 e di URiLU dt 2 3 根据牛顿第二定律 F ma 电机转轴的总转矩可以近似等于电机转轴的加速度 与负载转动惯量乘积 表达式如下 3 L dw TTJ dt mfaa dw TTKTJ dt 2 4 结合 2 1 2 2 2 3 2 4 可以推导出电机运动基本方程如下 e kdiRU iw dtLLL 2 5 f ma RRR k kTdw iw dtJJJ 4 湖南工业大学本科毕业设计 论文 4 2 6 研究一个简化的直流电机数学模型就可以达到自平衡要求 所以可以假设直 流电机的电感 L 和电机摩擦都忽略不计 所以 2 5 2 6 可以近似为 f K e KU iw RR 2 7 ma RR kTdw i dtJJ 2 8 把 2 7 代入 2 8 可得如下函数 mema RRR K KKTdw U dtJ RJ RJ 2 9 电机电感可以忽略 因此电流在线圈的电压也可忽略 这样在图 2 1 中电 流会趋于一个稳定值 改变电压一段时间后可以使小车加速到一个稳定的速度 值 w 这样我们在小车平衡速度控制时可以改变电机电压控制 为此电机动力 学方程可以写成一个空间状态方程 通过控制输入从而可以控制车模的位置 a U 和速度 w 也就是可以采用 PWM 电压调速根据外界干扰使小车偏离平衡位置动 态的调整输入电压 达到动态平衡效果 状态空间方程如下 0100 1 0 mem m RRR k k J R U k Tww J RJ 2 10 10 00 a U y Tw 2 11 2 2 分析车模的能控能能观性 系统能控能观是状态反馈 观测器设计的理论基础 同时系统能控能观理 论推导比较复杂 但在前人很多研究成果都有类似的推导 以下是本文参考的 车模系统状态方程 4 上式 5 湖南工业大学本科毕业设计 论文 5 以上公式符号涉及参数见 附件二 根据上式系统状态方程 结合组装的车模 以及电气物理参数 通过状态 方程处理多变量控制系统 状态方程可以很好的解决输入 状态量 输出之间 的变化关系 并通过分析系统的能控能观性 可以从理论上得出小车是否可以 控制在目标位置和速度 代入平衡机器人实际参数得到方程如下 在 Matlab 中 用 ctrb a b 命令 可求得系统的能控性矩阵 再用 rank 命令 可求得能控性矩阵的秩 用 Matlab 软件计算可知系统为满秩 因此系统为能控系统 同理用 obsv 和 obsvf 命令可以得到系统为满秩的 能观系统 2 3 建立平衡车倒立摆模型及零极点分布 我们都知道单摆可以在平衡位置左右摆动 在外力干扰下最后平衡在竖直 位置 这是因为单摆受到重力和悬线的产生的使重物回到平衡位置的回复力 大小为 F 同时受到空气阻力 单摆所受的回复力与位移方向sinmgmg 相同 所以单摆可以最后回到垂直位置 两轮小车要平衡可以看作重心固定的 轴 而车轮则可看作摆线 从而构建车模倒立摆模型 模型如图 2 2 所示 6 湖南工业大学本科毕业设计 论文 6 图 2 2 车模 倒立摆 受力分析 设小车轮轴到重心的长为 L 质量为 m 偏角为 外界对小车的干扰为 想 x t 小车运动加速度 a t 小车的杆子是可以看作不可伸长的刚体 通过小车 受力的平行四边形法则把力分解到缘杆和垂直杆的两个方向 在缘杆方向由于 杆的支撑可以保持平衡 在垂直杆方向小车的运动方程如下 2 12 2 t sin cos 2 Lgta ttLx t t 2 12 由于车模平衡一般在小范围变动 所以小于 10 度 这时 由 2 12 可得 2 13sin cos1 2 t 2 Lgta tLx t t 2 13 由于小车平衡时 a t 0 代入 2 13 可得 2 t 2 LgtLx t t 2 14 根据自动控制原理 先把 2 14 拉氏变换可得 5 2 s s LsgLx 2 15 两边除以并移项 可得系统的传递函数为 s 1 s 2 s H g x s s L 2 16 由 2 16 可得系统两极点为 由奈奎斯特稳定判据可知有一个极 g sp L 点在 S 正半轴 系统不稳定不能保持平衡 同时有倒立摆模型得知车模的加速 7 湖南工业大学本科毕业设计 论文 7 度的大小是由小车的倾角和小车角加速度反馈决定 因此在控制系统需要引入 小车倾角和角速度构成比例微分反馈控制环节 PD 如图 2 3 L X t Q S 1 2 LSg 12 kk s 图 2 3 系统 PD 反馈控制传递函数框图 在系统中加入了 PD 反馈调节后 系统的传递函数如 2 17 式 1 s 1 22 s H k x sk g ss LL 2 17 系统此时的两个极点为 由奈奎斯特稳定判据可得 22 4 k 221 2 kkLg sp L 系统要极点都在左半平面 只要 k1 g 且 k2 0 Sp 都在左半平面系统可稳定 2 4 本章小结 首先本章第一节建立直流电机线性模型 得出电机空间状态方程 充分论 证了通过控制输入从而可以控制车模的位置和速度 w 也就是可以采用 PWM a U 电压调速根据外界干扰使小车偏离平衡位置参数动态的调整输入电压 这点为 下面电机控制以及 PWM 电压调节搭下理论基础 第二节介绍了系统能控能观性 通过结合实际小车与前人分析的小车系统 状态空间方程 论证了小车的能控能观特点 从而让我们设计小车自平衡有了 顶层设计 式轮廓 第三节重点分析了小车的倒立摆模型 建立了控制传递函数 从而就传递 函数分析了系统两种状态下的极点分布 让我们知道系统在不加反馈控制时在 自然条件下是不可能平衡的 然而加了 PD 反馈控制器后 只要 g 且 0 系统可以稳定 为后面软件 PID 设计小车的数字式偏差控制 1 k 2 k 8 湖南工业大学本科毕业设计 论文 8 的增量 PID 奠定基础 所以本章是重要的理论分析章节 9 湖南工业大学本科毕业设计 论文 9 第第 3 3 章章 系统任务分解以及总体设计构架系统任务分解以及总体设计构架 3 1 任务分析 研究两轮机器人移动控制 就是要移动机器人可以平稳的行走 并可选择 行走路径 要达到这些目的 可以把小车的合运动分为平衡控制 速度控制 方向控制 然而控制器一般在同一时间只能做一件事 所以对这三种控制采取 分时控制 因此也要求控制器执行速度较快 同时采样时间很短 这样才能保 证宏观上小车可以三种控制同时进行的一种现象 3 2 直立平衡控制 在上一章已经分析了小车的电机空间状态方程和系统能控能观性 在上章 中我们得出结论 控制电机的电压可以控制车模保持在目标位置 而改变电压 就是改变车模的运动方向和产生前进的加速度 这样车模会有一个惯性力从而 使车模平衡 所以平衡控制主要是控制电机的正反转产生抵消车模的回复力 同时小车的外界干扰不确定 电机控制也会有误差 这样要稳定的保持平衡 还需要上一章分析的要在控制环加上 PD 反馈控制 这样会有较大的控制裕度 同时控制器也要时刻知道车模的倾角和角速度 所以就要求使用采集角度和角 速度的传感器 这样通过快速采集姿态信息并快速处理 输出 PWM 不断调节车 身 这样就会表现出动态平衡的状态 3 3 速度控制 要控制小车的速度 首先就要让控制器知道车模的速度 所以我们要用到 一种测量车模速度的传感器 同时传感器采集的信息要传到控制器 我们需要 2 个定时 计数器或 2 个外部中断来记录速度信息 电机的转速决定了车模前进的速度 而转速与电机电压成正比 所以我们 需要 2 路 PWM 改变电机电压控制车模速度 速度控制与平衡控制是分时进行的 在速度控制时假设小车处于平衡 通 过 PWM 给车加上一个设定的速度 但执行很短一段时间又要去执行平衡控制 平衡是最重要的 这样不断循环 就可达到匀速且平衡运动的目标 10 湖南工业大学本科毕业设计 论文 10 3 4 方向控制 小车方向控制相对简单些 我们要改变两轮车的方向 就要不同的驱动动 力源 所以就要两个直流减速电机 通过左右电机的差速 可以实现车模的转 向 这也是车速控制需要 2 路 PWM 和 2 个定时 计数器或 2 个外部中断的原因 同时我们要让小车自己选择行走方向 需要一个小车的 眼睛 可以采用红外 CCD 摄像头 超声波 无线蓝牙等充当机器人的 眼睛 3 5 系统总体设计框架 由以上任务分解 可得图 3 1 所示总体框架图 控制器需要收集车模的倾 角 方向 角速度 速度等信息 根据这些信息并经过控制器进行处理输出 PWM 控制左右电机以达到目的 图 3 1 系统总控制框图 11 湖南工业大学本科毕业设计 论文 11 第第 4 章章 硬件选型及硬件电路硬件选型及硬件电路 4 1 主控芯片STC12C5A60S2 根据第二第三章介绍 需要的硬件类型基本确定 现结合实际情况以及经济 成本等确定适合的硬件 在可以达到课题目的的前提下 做到充分利用硬件资 源 节约成本 所以选用 STC12C5A60S2 这款单片机基本可以达到本课题的 大致要求 而且价钱便宜适合本次开发研究 以下就是 STC12C5A60S2 单片机 的简单介绍 STC12C5A60S2 特点如下 1 增强型 8051CPU 1T 单时钟 单周期单片机 指令代码完全兼容 8051 开发 难度较低 同时 1T 单片机指令执行速度是 8051 的 8 12 倍 这样可以满足 小车快速的要求 2 工作电压 5 5V 3 3V 5V 单片机 电压范围较宽 且电压容易做到 支 持工作频率 0 35MHz 且有强抗静电 强抗干扰的特点 3 用户应用程序空间为 62K 片上集成 128 字节的 RAM 有 EEPROM 和看门 狗等 4 40 个通用 I O 口 复位后为 准双向口 弱上拉 可设置为四种模式 准双 向口 弱上拉 强推挽 强上拉 开漏 输入 高阻 每个 I 0 口可产生驱动电 流 20mA 5 下载方便 ISP IAP 下载 无需专用编程器或仿真器 仅通过 P3 0 P3 1 就可 直接下载 6 2 路 PWM PCA 可输出 PWM 方波 并可改变方波占空比 8 路 10 位高精度 A D 转换 转换 25 万次每秒 7 有两路与 8051 兼容的 16 位定时器 T0 和 T1 还有两路可由独立比特率发生 器做串口通讯的波特率发生器和 PCA 模块实现的 2 个 16 位定时器 4 2 电源模块 前面两章提到直流电机 因为需要大转矩 大转速 我们选用 12V 的大转 矩直流减速电机 同时小车需要自由移动因此选用 12V 暴力 9800 毫安聚合物 锂电池 该电池工作电流大可达 3A 可承受电机启动产生的大电流 带负载能 力强 基本满足平衡车的负载要求 同时单片机 电机驱动 传感器等芯片需 要 5V 电压 为了方便采用 12V 降压模块 LM2596S 它可输入 3V 40V 电压 12 湖南工业大学本科毕业设计 论文 12 输出 1 5V 35V 电压可调 且强抗干扰 输出纹波3 也会收敛 到 4 21 湖南工业大学本科毕业设计 论文 21 第第 6 6 章章 PIDPID 控制控制 6 1 PID 基本介绍 PID 是本课题的核心 在第二章建立倒立摆模型分析出 要使小车平衡 就要加入反馈控制 PID 就是一种经典的负反馈控制 同时 PID 也是运动控制 的核心 它可以对偏差信号通过比例 积分 微分使偏差信号逐步减少 从而 达到目标值 本文的角度目标值处于垂直位置 0 度 速度也就是设定的值 通过 PID 可以调整电机正反转和转速动态调整车模姿态在目标位置 有模拟和数字 PID 其中模拟 PID 控制框图如下 比比例例 P 积积分分 I 微微分分 D E t R t Y t 图 6 1 模拟 PID 控制框图 上图 r t 是给定值 y t 是实际输出值 e t 是两者偏差 则 e t r t y t 6 1 PID 控制方程如下 0 de t t y tk e tke t dtk pid dt 6 2 Kp 为比例系数 ki 为积分系数 kd 为微分系数 从上式可以知道 PID 控 制就是按偏差控制 e t 越大控制越强 下面介绍一下 P I D 的控制特点 对于比例主要调节系统的动态性能 值越大响应速度越快 但太大会出现 震荡影响系统稳定性 所以对 kp 在稳定的条件下尽量加大参数 一旦系统出现 偏差比例环节就会按照 kp 的大小调节系统较少偏差 比例是系统的粗调部分 对于积分部分就是把偏差的累积求和输出 e t 存在且较小时 比例在 e t 较小时输出较小 这时就这要靠积分控制了 因为积分输出会随着小偏差 22 湖南工业大学本科毕业设计 论文 22 不断增加控制系统 直到 e t 0 因此积分主要调节系统的稳态性能 降低系 统静态误差 但积分是一个不断累积的过程 过程比较慢因而降低系统的快速 性和增大超调量 对于微分环节 主要改善系统动态特性 根据偏差变化趋势进行控制 偏 差变化越大微分输出就越大 所以微分可以大幅提高系统动态特性 可以减小 超调和震荡 但它对噪声比较敏感 所以在加入微分前要先滤波 6 2 数字 PID 自平衡机器人采集的都是离散信号 同时本课题采用软件 PID 控制 所以 需要数字式 PID 对 6 2 式模拟 PID 离散化可得 由 可推 6 2 的离散模型为 0 0 K t e t dtTe i i 1 de te ke k dtT 1 0 k y kk e kTke iTke ke k pid i 6 3 同理可得 y k 1 的离散方程 1 1 1 1 2 0 k y kk e kTke iTke ke k pid i 6 4 结合 6 3 6 4 由 k 和 k 1 两状态 可得增量式 PID 控制算法如下 1 1 e k e k 2 1 2 kd y ky ky kke ke kTke ke k pi T 6 5 由式 6 5 可知 增量式 PID 只与 k k 1 k 2 三个状态有关 在陀螺仪和加速度计 所测量的数据我们就可根据这三个状态 控制小车的倾角和速度达到自平衡的 总效果 同时增量 PID 有自动收敛的功能 对测量误差和精度要求不高 因为 这些对控制量影响较小 6 3 PID 控制器的设计 根据第三章分析 小车可分为平衡控制 速度控制 方向控制因此我们要 根据这三个控制的特点 结合 PID 比例 积分 微分的特点 设计合适的控制 器 对于小车的平衡控制 在第二章建立的倒立摆模型中 就分析了在控制传 递函数中加入比例 积分反馈环节可以使车模不倒下 因此对于平衡控制采用 23 湖南工业大学本科毕业设计 论文 23 PD 控制 按第二章分析要求 Kp g Kd 0 比例可以快速调整车模倾角 是系 统不会倒下 但比例是粗调 很可能会过大或过小 同时电机转动有惯性 会 使车模产生震动 所以还要加入微分 使调节幅度可以根据偏差自动增大或减 小 PD 输出 相当在粗调基础上加上了细调 最终使车模平稳站立 对于小车的速度控制 也就是要求车模按照设定的速度匀速前进 这样匀 速运动时 车模的偏差可能就会很小 这样就要求控制系统可以有较小的静态 误差 所以偏差较大时采用比例 较小时用积分 积分可以消去静态误差 这 样采用 PI 调节就可以保持匀速运行 小车方向控制 主要是控制左右两轮的差速 本文主要读取检测红外线的 高低电平 判断目标路径方向 通过 PWM 产生左右差压控制方向 方向控制比 较简单 只是控制器根据红外测量值 控制制器输出一个方向信号就可以了 所以用普通的比例控制就可 小车的 PID 控制器设计框图如下 结合第四章硬件特性 图 6 2 PID 控制器框图 24 湖南工业大学本科毕业设计 论文 24 第第 7 7 章章 软件设计软件设计 软件是小车的 灵魂 本设计设及车模姿态信息滤波 增量式 PID 控制 读取陀螺仪和加速度计的角速度和角加速度 并对角速度角加速度处理得到单 个仪器测量的倾角和角速度 这里的倾角不能直接用于控制电机 因为波动 干扰很大 滤波后才能用 读取红外传感器高低电平和光电编码的脉冲信息都 要软件处理 7 1 软件设计主流程图 首先设计软件流程总图 总的流程图可以大大提高开发速度 使开发思路 清晰 避免缺漏 总流程思路是 在主程序不断循环检测红外传感器的地面灰 度信息并对信息作阀值处理 阀值处理 就是对返回的 8 路二进制高低电平数据 进行阀值处理 例如当小于 50 认为车体处在黑线中 可以保持姿态 当大于 50 小于 150 认为小车轻微偏离黑线需要调整 当大于 150 时需要大的调整 同时用 LCD1602 显示小车的角度 速度等信息 当定时器 5ms 中断响应时 在中断程序中读取其他传感器数据 滤波处理 PID 控制 PWM 输出等 5ms 更新调整车模姿态 软件流程总图如图 7 1 25 湖南工业大学本科毕业设计 论文 25 上上电电运运行行 初初始始化化 红外和显示模 块 5ms中中断断 mpu6050数据 读取及处理 滤滤波波 读读红红外外模模块块数数 据据 读读取取速速度度设设定定 和和实实际际值值 倾倾角角 速速度度 方方向向PID控控制制 左右PWM输出 左左右右电电机机驱驱动动 控控制制 车模倾角大 于40度 关关闭闭电电机机 结结束束 是 是是 否 否 图 7 1 软件流程总图 考虑到单片机计算相对较慢 采用 5ms 中断 每秒调整 200 次 并不断采集车 模地面反射的红外信号判断车模前进方向 经过后期调试基本可以达到预期效 果 7 2 外部中断程序设计 26 湖南工业大学本科毕业设计 论文 26 在总流程中 需要的车模速度信息 不能由主程序读取 因为采集速度信 息需要根据外部光电编码产生的脉冲计数 计数的时刻不确定 且计数不断进 行 所以需要外部中断来完成速度检测 具体就是采用外部中断 0 和外部中 2 每来一个外部脉冲 就加 1 5ms 后统计总数这样通过车轮走的距离和时间 就可知道速度 再清除脉冲计数变量值 7 3 各子模块程序设计 1 对于 mpu6050 数据处理 首先 mpu6050 有零点漂移 需要零点校正 所以先单独读取 mpu6050 处于 水平时 的 X Z 轴的角加速度值 Accel x Accel z 还有陀螺仪 Y 轴的角速度 值 Gyro y 用如下命令 Accel x Accel x 1189 Accel z Accel z 1600 Gyro y Gyro y 4 因为 X Z 轴的角加速度零点值分别为 1189 和 1600 角速度值为 4 处理 后才可以用于车模姿态测量 另外 mpu6050 寄存器存的数据是 16 位有符号的二进制数据 并且加速度 计用 2g 陀螺仪用的是 2000dps 最低分辨率分别为 16384 g 16 4LSB s 且 对 X Y 轴的角加速度取反正切 可以得到加速度计的倾角计算值 程序设计如 下 if Accel x 32764 x Accel x 16384 else x 1 Accel x 49152 16384 if Accel z 32764 z Accel z 16384 else 27 湖南工业大学本科毕业设计 论文 27 z Accel z 49152 16384 Angle az atan x z 180 3 14 计算出加速度计的测量倾角单位为度 角度的正负号 if Accel x32764 Angle az Angle az 2 软件滤波程序设计 对于 Kalman 滤波 就是根据第五章建立的状态方程和观测方程 结合 Kalman 五个递推估计离散方程可以写出软件程序具体见附件 一 5 对于互补滤波利用第五章互补滤波原理和式 5 12 用软件模拟高通和低 通滤波器如下 Angle 0 98 Angle Gyro y dt 0 02 Angle az 其中 Angle 为滤波后角度 Angle az 为加速度计计算角度 dt 为采样时间 对于速度滤波在第五章已经详细介绍就不在详叙 3 PID 软件设计 根据第六章增量式 PID 理论以及 PID 控制器设计思路 首先分析平衡控制 PD 控制器的软件实现 由式 6 5 1 1 e k e k 2 1 2 kd y ky ky kke ke kTke ke k pi T 因为平衡控制目标是小车保持在倾角 0 度 电机控制精度比较大 在 上面也分 析了经过 5ms 的调整 可以认为车模又有一个短时的处于倾 角 0 度位置 所以设 k 1 状态为初始态 0 当现态 k 有一个变化值时 e k 1 0 e k 可以理解为单位时间 T 角度变化量乘 e k 2 1 2 kd e ke k T 以 Kd 也就是 Kd 角速度 这样在 PWM 调节下 k 状态可达平衡 同样 k 1 状态也就类似 k 状态如此循环 就可不断调整车模达到平衡 关键语句 为 PWM Kp Angle Kd Gyro y 分析速度 PI 的软件实现 因为车模采集的信号为离散信号 通过每次都把 28 湖南工业大学本科毕业设计 论文 28 speed speed0 加给 position 这样就可得到速度增量的积分 speed0 是设 定速度值 speed r l speed mr speed ml 0 5 speed 0 7 speed0 车轮速度滤波 speed speed r l 0 3 速度 position speed speed0 积分得到位移 再根据第五章速度滤波介绍 速度滤波可以使速度收敛于 speed0 也就是相 当与 PID 里的偏差比例控制 得 PI 控制式为 PWM Ki position Kp speed 对于控制方向的比例控制较为简单 这里就不多说了 同时还有一些初始化 传感器模块的数据读取 显示 驱动以及 PWM 设计比较的固定 可以移植 按照芯片手册操作就可以了 29 湖南工业大学本科毕业设计 论文 29 第第 8 8 章章 机械安装和调试机械安装和调试 8 1 机械安装 对于机械安装主要是注意几个关键点 1 电机选择尽量保证功率 转速 力矩相同 不然会增加调试难度 甚至不 能平衡 2 车模车身固定时最好要用标准板 使车模相对比较对称 这样也有利于车 模平衡 本人制作时为了节约就自己做了一块板 不很标准 3 各连接线要用溶胶固定 以免运动中接触不良 4 车模车身不要太高 以免震动比较大 机械安装不是本科题重点 大概把握以上四点就差不多了 小车模型如图 8 1 所示 图 8 1 车模外形 8 2 软件调试 软件是关系到小车能否站起来的关键 1 首先检查 mpu6050 零点漂移 用过单独在 1602 显示 X Y 轴方向的角加 速度 还有 Z 轴方向角速度在水平时的数值 并多次测量取平均值 测试结果如 下图 8 2 rx 是加速度计 X 轴的角加速度测量值 rz 是 Y 轴加速度计测量的角 加速度值 gy 是陀螺仪 Y 轴角速度值 30 湖南工业大学本科毕业设计 论文 30 图 8 2 测量零漂值 2 mpu6050 滤波调试 由第七章滤波软件程序 可以在硬件上调试 我 们用串口和虚拟示波器观察比较滤波前后变化 同时在程序里加延时并用 1602 显示比较两者的稳定性 如下图所示 图 8 3 Kalman 滤波前和滤波后倾角变化比较曲线 图 8 3 黑色表示未经过滤波后的倾角测量变化曲线 白线表示滤波后倾角 测量变化曲线 31 湖南工业大学本科毕业设计 论文 31 图 8 4 Kalman 滤波前和滤波后角速度变化比较曲线 由图 8 4 可以看出滤波后得到的角速度曲线比较平滑 有利小车的平稳运行 图 8 5 倾角变化时有无滤波的比较 第一行左未滤波 第一行右滤 波 通过两曲线可以明显看出 滤波后倾角和角速度变化较平滑 滤去了很多尖脉 冲噪声 有利于减小车模震动 从图 8 4 可知 有滤波比较稳定 无滤波的会 有跳动 32 湖南工业大学本科毕业设计 论文 32 同时设计使用 STC12C5A60S2 作主控 该单片机速度还是不能很快执行卡 尔曼滤波算法 在开始上电时会有一段大约 1 分钟的收敛计算等待 基于此 我们还利用了第五章介绍的互补滤波 通过调试发现 虽然有滤波效果 但在 相同 PID 参数时互补滤波震动比较大 因此本设计最终采用卡尔曼滤波算法 3 PID 调试 对于 PID 调试 要根据车模参数调试 首先进行估计 否则漫无目的的调 就如同太海捞针 例如角度比例参数 kp 2 那么当角度很大的时候 假设为 30 度 此时控制量为 2 30 60 255 结果就是只要角度不为 0 度 那么电机就 会全速运行 以最大转矩调整车模 这时小车也会很快倒下 调试方法一般采用试凑法大概如下 1 先调平衡且对于平衡 PD 调节 先 调比例 P 由小到大 且考虑到 PWM 为 0 255 所以 kp 在 10 左右比较合适 因为偏离 1 度 PWM 输出为 10 偏离 30 度 PWM 为 300 而一般车模偏角在 10 度左右 当比例较小响应慢 较大会引起震动 通过不断比较得出合适的比 例值 微分主要阻止偏差变化 且角速度变化范围较大 所以 kd 要比 kp 小些 同时在比例调节出现震荡时 可以加大微分改善 在小车可以平衡后再调节速度 PI 速度先调积分 I 后调比例 P 且设定车 速要与速度控制周期一致 若控制周期 100ms 时 车速要求低速小于 1m s 速 度是累积的 慢慢会漂移变大 所以速度 ki 要小 为防止漂移过大造成控制量 PWM 大于 255 需要设定一个 PWM 上下限 4 红外参数及反馈调试 根据红外原理 检测黑白线返回高低电平 以黑线为目标线 返回低电平 当检测时高电平时 需要电机左右转动使小车在黑线上 转动要不断试凑才可 以使车模不会偏 33 湖南工业大学本科毕业设计 论文 33 总总 结结 两轮自平衡移动控制机器人系统设计 本文涉及面广 有数学建模理论 有很多力学分析原理 有经典控制理论传递函数分析 有现代控制理论状态空 间方程 有工业控制经常用到的 PID 有先进的 Kalman 滤波 mpu6050 高级芯 片的使用 电路设计 单片机等 是一个综合性很强的研究课题 可以很好地 联系运用大学四年我们自动化学习的知识 通过研究这个系统提高了我的综合 分析能力 同时在研究过程中让我学到了很多东西 总的来说本课题研究成果 如下 1 建立小车模型 并对小车运动任务分解 设计了以单片机 STC12C5A60S2 控制器为核心的硬件体系 并通过软件实现快速执行并调整车模姿态任务 使小车完成直立 行走等任务 2 分析了惯性传感器的特点 并根据卡尔曼滤波原理 设计了滤波算法 同 时调试时也采用了互补滤波算法 与卡尔曼滤波比较 并采用对平衡有利 的卡尔曼滤波算法 较好的解决了数据漂移和动态误差的问题 3 根据增量式 PID 设计了角度 速度反馈控制 使小车可以在电机干扰 外 界干扰的复杂环境下平衡运动 4 对单片机的开发能力和 C 语言编程能力 有较大提高 有很多像 I2C PWM 进行了自我学习 5 设计实践操作要求很高 在整个机械安装 调试 焊接过程中提高了实 践能力 了解了很多芯片 选择适合小车的硬件 6 设计的循迹红外模块 由于时间关系没有加入调试 希望后期可以做好这 个模块 7 调试发现左右电机不对称 可能是商家发错货了 在相同电压下电机转速 不同 也因为时间紧没再买新的电机 所以有些任务没有实现 在后期要 调整电机 完成任务 34 湖南工业大学本科毕业设计 论文 34 致 谢 感谢辛勤栽培我的所有湖南工业大学老师 是老师的辛勤教导 让我学习了很 多专业知识 思想素质也得到很大的提高 真正实现人的一次蜕变 感谢我的指导老师肖伸平教授 感谢给了我这个机会研究这个课题 并细心指 导 从这个毕业设计中提高了我的综合能力 感谢我的队友刘任慧等班上同学 我们一起讨论 相互学习 让我们的研究增 加了乐趣 很多时候提高了研究信心 在问题讨论过程中增进了我们友谊 感谢我的大学 湖南工业大学一所工科类特色学校 为我提供良好的学习环境 和美丽的校园环境 希望学校人才辈出 感谢我的父母 是他们的辛劳供我上学 你们辛苦了 希望我毕业后能够发扬吃苦耐劳精神 继续学习潜行钻研 以报答这些我要感 谢的人 学生签名 日 期 0 湖南工业大学本科毕业设计 论文 0 参考文献 1 袁泽睿 两轮自平衡机器人控制算法的研究 D 哈尔滨工业大学工学硕士学位论 文 2006 6 2 张吉昌 单轴双轮自平衡代步车的研究与设计 D 中国海洋大学硕士学位论文 2009 6 3 李发海 电机与拖动基础 M 北京 清华大学出版社 第四版 2012 6 9 23 4 周毅漳 基于自抗扰控制的双轮自平衡机器人 D 福建 福建农林大学硕士学位论 文 2010 6 5 潘丰 自动控制原理 M 北京 机械工业出版社 2010 8 5 陈进 单级倒立摆的模糊控制应用 A 北京 工业仪表与自动化装置 2008 6 7 寸晓菲 基于飞思卡微控制器的智能循迹小车 A 荆门 荆楚理工学院学报 2012 04 8 赵建伟 两轮自立式自平衡移动机器人的控制系统设计 J 北京 北京工业大学报 2009 01 9 屠运武 自平衡控制系统的建模与仿真 J 北京 系统仿真学报 2009 04 10 宁慧英 基于光电传感器的智能小车自动循迹控制系统 J 辽宁沈阳 仪表技术与 传感器 2012 01 11 张强 基于模糊控制的双轮自平衡小车的研究 D 山西 太原 太原理工大学硕士 学位论文 2011 5 13 李荣伟 李鑫 两轮自平衡小车的设计 A 江苏 常熟 常熟理工学院 1008 2794 2012 0076 06 12 第七届全国大学生 飞思卡尔 杯智能汽车竞赛电磁组直立行车参考设计方案 版本 1 0 2011 12 22 14 MPU 6000 and MPU 6050 Register Map and Descriptions Revision 3 2 15 冯智勇 基于陀螺仪及加速度计信号融合的姿态角度测量 A 重庆 西南大学学 1 湖南工业大学本科毕业设计 论文 1 报 自然科学版 2011 第36卷 第4期 16 Greg Welch and Gary Bishop TR 95 041 Department of Computer Science Uuniversity of North Carolina at Chapel Hill NC 27599 3175 17 杨兴明 两轮移动式倒立摆的运动控制 A 安徽 合肥 合肥工业大学 1003 5060 2005 18 欧伟明 何静 凌云 刘剑 单片机原理与应用 M 北京 电子工业出版社 2009 7 19 余忠宇 模糊控制理论在两轮移动式倒立摆中的应用及实现 D 合肥工业大学 2011 6 10 20 霍亮 两轮自平衡电动车的关键技术研究 D 哈尔滨工业大学 2010 3 4 21 何雅静 两轮自平衡小车变结构控制性能改进的研究 D 西安电子科技大学 2001 1 6 2 湖南工业大学本科毕业设计 论文 2 附录一 附录一 KalmanKalman 滤波程序滤波程序 void Kalman Filter float Accel float Gyro Angle Gyro Q bias dt 先验估计 Pdot 0 Q angle PP 0 1 PP 1 0 Pk 先验估计误差协方差的微分 Pdot 1 PP 1 1 Pdot 2 PP 1 1 Pdot 3 Q gyro PP 0 0 Pdot 0 dt Pk 先验估计误差协方差微分的积分 PP 0 1 Pdot 1 dt 先验估计误差协方差 PP 1 0 Pdot 2 dt PP 1 1 Pdot 3 dt Angle err Accel Angle zk 先验估计 PCt 0 C 0 PP 0 0 PCt 1 C 0 PP 1 0 E R angle C 0 PCt 0 K 0 PCt 0 E K 1 PCt 1 E t 0 PCt 0 t 1 C 0 PP 0 1 PP 0 0 K 0 t 0 后验估计误差协方差 PP 0 1 K 0 t 1 PP 1 0 K 1 t 0 PP 1 1 K 1 t 1 Angle K 0 Angle err 后验估计 Q bias K 1 Angle err 后验估计 Gyro y Gyro Q bias 输出值 后验估计 的微分 角速度 3 湖南工业大学本科毕业设计 论文 3 附录二 模型参数说明 1 直流电机模型符号表 直流电机在回路中有电流通过时产生的感应转矩 m T 直流电机的转矩系数 m k i 直流电机回路电流 直流电机在回路中有电流时的感应电动势 e U 直流电机在回路的两端电压U 直流电机的反电动系数 e k 直流电机的转速w R 直流电机的回路电阻 L 直流电机的回路电感 直流电机的转矩总和T 直流电机转矩摩擦力与电机转轴的速度比例关系 f k 直流电机负载转矩 a T 直流电机机械转动部分的转动惯量 R J 2 机器人系统的数学模型符号表 小车的任意轮子的质量 W M r 车轮半径 车身重心到通过小车两轮的轮子中心轴的距离l 重力加速度g 直流电机的转矩系数 m k 直流电机反电动势系数 e k 车轮绕车轴的转动惯量 W J 车身及电机不动部分 两个电机定子 以及小车两个轴承的外圈部分通过小车 P J 两轮的轮中心转动惯量的总和 车身 两个减速箱的不转动部分 两个电机的定子 以及小车两个轴承外圈部 P M 分质量总和 直流电机回路的电阻R 4 湖南工业大学本科毕业设计 论文 4 2014 届届 本科毕业设计 论文 资料本科毕业设计 论文 资料 第二部分第二部分 过程管理资料过程管理资料 5 湖南工业大学本科毕业设计 论文 5 过程管理资料目录过程管理资料目录 一 2014 届毕业设计 论文 课题任务书 1 二 湖南工业大学本科毕业设计 论文 开题报告 3 三 本科毕业设计 论文 中期报告 8 四 毕业设计 论文 指导教师评阅表 11 五 毕业设计 论文 评阅教师评阅表 12 六 毕业设计 论文 答辩及最终成绩评定表 13 6 湖南工业大学本科毕业设计 论文 6 2014 届毕业设计 论文 课题任务书届毕业设计 论文 课题任务书 学院 部 电气与信息工程学院 专业 方向 自动化 指导教师肖伸平学生姓名肖春元 课题名称双轮自平衡机器人移动控制系统设计 内 容 及 任 务 内容 以 STC12C5A60S2 为主控器 设计一个双轮平衡循迹控制系统 实 现小车的直线行走 原地转弯 循迹移动的功能 此系统采用闭环 控制系统 检测经过卡尔曼融合后的加速度计和陀螺仪的值 再通 过 PID 控制电机 使系统维持动态平衡 任务 1 查阅平衡小车控制的相关论文和书籍 分析控制理论和数据滤 波原理 制作车模 完成硬件电路设计 2 软件设计和系统整体调试 3 按时完成开题报告 中期检查报告等过程材料 4 完成与课题相关的实验 5 完成毕业设计论文 报告 拟 达 到 的 要 求 或 技 术 指 标 技术要求 1 平衡小车能站立并能承受一定的扰动 2 能采集规定的路径信息 提取特征值 3 按规定的路线循迹行走 4 LCD 显示屏显示路径和角度信息 7 湖南工业大学本科毕业设计 论文 7 起止日期工作内容 2013 12 01 2013 12 31熟悉设计任务书 撰写开题报告 2014 01 01 2014 03 05系统总体方案设计 2014 03 06 2014 03 20硬件系统设计 2014 03 21 2014 04 10系统软件设计 2014 04 11 2014 04 20系统调试 完善设计 2014 04 21 2014 05 10撰写论文 进 度 安 排 2014 05 10 2014 05 20论文修改 毕业答辩 主 要 参 考 资 料 1 何雅静 两