两轮自平衡机器人

控制器采用瑞萨电子公司生产 的 RL78/G13 单 片 机 ，其 主 要 性 能 参数有 ： 内 置 高 速 片 上 振 荡 器 时 钟 ， 频 率 最 高 可 达 32MHz，64KB 的 flash ROM，4KB 的 数 据 内 存 ，4KB 的 RAM， 支 持 自 编 程 功 能 和 Onchip 调试功能，外设有定时器、ADC、SPI、IIC 和 UART 等。 编译环境为 Cubesuite+， 可以使用 Code generator 功能进行设置自动生成代码，方 便程序编写。
动 电 机 部 分 VIN 采 用 +12V 直 流 电 池 供 电 ， 同 时 模 块 上 安 装 了 LM7805 稳压器对 vin 的输入进行稳压，逻辑部分 VCC 采用+5V 供电， 机 器 人 运 行 时 通 过 单 片 机 向 该 模 块 1、2、3、4 脚 输 入 控 制 信 息 来 控 制 机器人电机的转速，其原理图如图 3 所示。
科教前哨
Science & Technology Vision
科技视界
科技·探索·争鸣
两轮自平衡机器人
龚 瑞 袁维君 曾依莹 （西华大学电气与电子信息学院，四川 成都 610039）
【摘 要】针对于现在流行的两轮自平衡车 ，设计了一个与其原理相似的研究性两轮自平衡机器人系统。 该系统以瑞萨 RL78/G13 单片机 为主控制器，通过 MPU6050 陀螺仪采集加速度和角速度数据，采用软件滤波和四元素融合算法得到机器人姿态，最后利用 PID 控制算法计算 电机的 PWM 值以控制电机的合理转动，使机器人保持平衡和运行。
速度通过霍尔传感器测量，小磁铁固定在转盘上，转盘与电机轴 相连，同步转动，小磁铁通过霍尔传感器时，霍尔传感器产生一个相应
作 者 简 介 ：龚 瑞 （1994.09.30 — ），男 ，汉 族 ，四 川 成 都 人 ，目 前 在 西 华 大 学 就 读 本 科 ，主 要 研 究 电 力 系 统 及 自 动 化 方 向 。
x,y,z 方向的分量（由此确定了旋转轴）。
四元数到欧拉角的转换
（13）
! " arctan 和 arcsin 的 结 果 是
-
π 2
,
π 2
，这并不能覆盖所有朝向（对
! " 于 θ 角
-π 2
,π 2
的 取 值 范 围 已 经 满 足 ）， 因 此 需 要 用 atan2 来 代 替
arctan。
其中，Kp 为比例系数；T1 为积分时间常数；TD 为微分时间常数。 采 用 积 分 分 离 式 PID，当 被 控 量 与 设 定 值 偏 差 较 大 时 ，取 消 积 分
作用；当被控量接近给定值时，引入积分控制，以消除静差，提高精度。
代码如下：
float PID_realize(float Set_Speed，float Actual_Speed)
抽取代表性的蛋制品实验让学生进行实际操作， 占总成绩的 40%，在 考 核 的 过 程 中 有 专 门 的 教 师 进 行 一 对 一 审 核 ，监 考 教 师 从 学 生的操作方法、实验步骤、细节处理等多个角度进行评定打分，确保可 以实现对学生技能的综合考核。 3.3 平时考核
平时考核主要由上课出勤率、实验设计、实验过程中的表现和实 验报告完成情况决定，占总成绩的 20%。
3 完善考核制度
课程考核采用“学习过程+学习成果”的考评模式，通过知识、技 能、职业素养给学生综合评定 。 具体考核方式为理论考核 、专业技能考 核和平时考核。 3.1 理论考核
理论考核的方式采用传统的 笔 试 方 式 ，占 总 成 绩 的 40%，考 试 内 容分为基础知识和综合应用能力两大类。 题型布置多样化，难易题的 比例分配合理，充分考核学生对知识的理解和掌握能力。 3.2 专业技能考核
电源电路采用 5V 线性直流稳压电路。 由于 RL78 单片机电源电 压 1.6V-5.5V,因此采用 5V 电源电路供电。LM7805 集成稳压器是常用 的固定输出电压+5V 的集成稳压器，最大输出电流为 1.5A。 它的内部 含有限流保护、过热保护和过压保护电路，采用了噪声低、温度漂移小 的基准源，工作稳定可靠。
Science & Technology Vision 科技视界 93
科技·探索·争鸣
Science & Technology Vision
科技视界
科教前哨
图 3 直流电机驱动原理图
的脉冲，计算出两个连续脉冲的间隔时间，就可以计算出被测转速。 直 流 电 机 驱 动 电 路 采 用 了 L298N 双 H 桥 直 流 电 机 驱 动 芯 片 ，驱
高校科技
Science & Technology Vision
科技视界
科技·探索·争鸣
示，并在演示过程中指出学生在实践过程中应注意的问题，使复杂抽 象的理论问题简单化、具体化。 例如通过演示发酵蛋品饮料的发酵过 程，使学生对饮料的发酵控制能够产生深刻的认识，并在学生实践过 程中进行录像， 实践结束后通过演示和学生实践的录像进行对比，使 学生自己发现实践过程的问题并找出解决问题的正确方法，提高了教 学效果。 2.2.4 充分利用整合校外资源
运动处理传感器采用 MPU6050，它是全 球 首 例 9 轴 传 感 器 ，集 成 3 轴 MEMS 陀螺仪，3 轴 MEMS 加速度计， 以及一个可扩展 的 数 字 运 动 处 理 器 DMP，可 用 IIC 接 口 连 接 一 个 第 三 方 数 字 传 感 器 ，比 如 磁 力 计。 MPU6050 对陀螺仪和加速度计分别用了三个 16 位的 ADC，将其 测量的模拟量转化为可输出的数字量。 为了精确跟踪快速和慢速的 运 动 ，传 感 器 的 测 量 范 围 都 是 用 户 可 控 的 ，陀 螺 仪 可 测 范 围 为±250， ±500 ，±1000 ，±2000°/ 秒 （dps ）， 加 速 度 计 可 测 范 围 为 ±2 ，±4 ，±8 ，±16g。
【关 键 词 】两 轮 平 衡 机 器 人 ；单 片 机 ；陀 螺 仪 ；四 元 数 融 合 ；PID
0 引言
2 平衡机器人力学模型
两轮自平衡机器人作为一种本征不稳定轮式移动机器人，具有多 变量、非线性、强耦合和参数不确定等特点，这使得它成为验证各种控 制算法的理想平台。 同时它运动灵活、结构简单，适于在狭小的空间工 作，有着广泛的应用前景。 两轮自平衡机器人能够完成多轮机器人无 法完成的复杂运动及操作，特别适用于工作环境变化大、任务复杂的 场合。 开展两轮自平衡机器人的研究对于提高我国在该领域的科研水 平、扩展机器人的应用背景等具有重要的理论及现实意义。
（8）
通过旋转轴和绕该轴旋转的角度可以构造一个四元数：
w=cos(α/2)
（9）
x=sin(α/2)cos(βx)
（10）
x=sin(α/2)cos(βy)
（11）
x=sin(α/2)cos(βz)
பைடு நூலகம்
（12）
其 中 ，α 是 绕 旋 转 轴 旋 转 的 角 度 ，cos(βx),cos(βy),cos(βz)为 旋 转 轴 在
（1）
·θ·RLJRL=GL+HTLR
（2）
其中，xRL 为水平位移；MRL 为左车轮质量；HL 为车身施加于车轮的
水 平 作 用 力 ；HTL 为 地 面 对 车 轮 的 水 平 作 用 力 ；θRL 为 左 轮 相 对 于 垂 直
分量的倾角；JRL 为左轮相对于 Z 轴转动惯量；GL 为左轮电机产生的扭
蛋制品加工技术在高职院校食品加工专业教学中占有重要地位。 随着教育观念和教学手段的发展，蛋制品加工技术课程在教学方式和 考核内容上都有了很大的进步。 但由于我国蛋制品加工技术尚不成 熟，在课程改革中仍有一些漏洞和难题 ，仍然需要不断的研究探索 。 因 此，发展蛋制品加工技术，坚持课程改革，更新和完善课程内容，对蛋
1 系统总体结构
两轮自平衡机器人主要由车身和左右两个驱动轮组成，两个驱动 轮的轴线位于同一条直线上，但由各自的电机独立驱动。 机器人倾斜 角度由姿态传感器检测， 速度检测系统由霍尔传感器和编码器组成， 为控制系统提供反馈信号。 两轮自平衡机器人平衡控制的基本思想 是：当测量倾斜角度的传感器检测到体产生倾斜时，控制系统根据测 得的倾角产生一个相应的力矩，通过控制电机驱动两个车轮向车身要 倒下的方向运动，以保持机器人自身的动态平衡。 系统主要由以下几 个模块组成：瑞萨 RL78/G13 单片机最小系统、电源模 块 硬 、姿 态 检 测 模块、电机驱动模块、速度检测模块，如图 1 所示。
4 系统软件设计
根据系统要求，需要完成的总体软件设计包括：单片机初始化，姿 态信息采集，速度检测，直流电机 PID 控制算法，系统软件流程如图 4 所示。
漂移十分严重。
四元数定义，即由四个元构成的数。 四元数既可以看作是四维空
间中的一个向量，又可看作一个超复数。
q=[w x y z]T
（7）
q 2=w2+x2+y2+z2=1
（15）
图 4 软件设计流程图
所谓姿态更新是指将运载体上惯性单元的输出，实时转换成运载 体的姿态。 这里的姿态通常指机体坐标系相对于导航坐标系的角位 置。
四元数算法通过加速度和角速度值融合计算得到航向角、俯仰角 和横滚角。 四元数算法计算量小，且算法简单，易于操作，是比较实用 的工程方法。 但其对有限转动引起的不可交换误差的补偿程度不够， 所以只适用于低动态运载体的姿态解算，对于高动态运载体，其算法
为了获得平衡机器人的平衡方程，需要分析其力学结构，平衡机
器人的主要构成是车身和左右两个车轮，影响平衡的参数有：重心、质
量、转动惯量、半径。 建立力学模型，如图 2 所示。
假设平衡机器人为刚体，左右两轮完全对称，并且忽略车轮与地