江苏高等学校大学生实践创新训练计划立项项目

江苏省高等学校大学生实践创新训练计划立项项目总结报告在杜晓老师的悉心指导下， 在课题组成员的共同努力下， “两轮自平衡小车的设计与实现”项目已顺利完成，现总结如下：一、项目研究的研究背景两轮自平衡小车是一个高度不稳定系统，是一种特殊轮式移动机器人，其动力学系统具有多变量、非线性、强耦合、参数不确定性等特性，是研究各种控制方法的一个理想平台。两轮自平衡小车就像传统的倒立摆一样，其本身是一个自然不稳定体，必须采用适当的控制算法、施加合适控制手段才能使之稳定。项目组对于4轮机器人有丰富的控制经验，研究并应用过多种控制算法，熟悉轮式机器人的硬件设计，这些都是本项目的开展研究的基础。二、项目研究内容本项目研究目标为：设计一个高稳定度两轮自平衡小车。主要研究以下内容：（1）利用智能控制和最优控制对两轮自平衡小车的大范围可控问题进行探讨。（2）高稳定度自平衡小车的硬、软件设计。三、项目使用的方法和手段采用运算速度更高的处理器使得复杂的控制算法得以快速实现，进一步增强电机驱动电路的负载能力，利用陀螺仪、加速度传感器来监测车身所处的俯仰状态和状态变化率，通过高速中央处理器计算出适当数据和指令后，驱动电动机产生前进或后退的加速度来达到车体前后平衡的效果。四、取得的成果与价值本项目中提出的两轮自平衡小车的设计与实现可作为学生的研究性课题，是电子信息工程专业和电气工程及其自动化专业学生的课程设计和毕业设计很好的课题。 同时课题的研究对提高目前“单片机与接口技术” 、“计算机控制技术”等课程的教学效果，培养了我们在单片机应用系统设计、智能控制等方面的能力，可以加深我们对所学知识的理解 ,提高动手能力,锻炼发现问题、分析问题和解决问题的能力。这也将为我们就业和从事相关专业的工作打下一定基础。本项目取得的成果如下：1）掌握了直流电机驱动、控制技术；掌握了角度传感器、加速度传感器应用技术；3)学会了直流电机的PWM调速控制技术；4）深入学习了卡尔曼滤波器原理及滤波算法；5）进一步学习了单片机应用系统的设计。课题中设计的两轮自平衡小车，其软件编程基于 C51，该系统利用速度检测、倾角检测、加速度检测技术，并利用卡尔曼滤波算法进行数据融合，在单片机控制下实现了两轮小车的稳定精致、前进、后退、转弯等控制。下面分别进行详细介绍。两轮自平衡小车是一种自然不稳定体，其动力学系统具有多变量、非线性、强耦合等特性，是研究各种控制方法的理想平台。两轮小车的自平衡控制过程是利用微控制器对系统各种状态参数进行实时分析，使系统在水平方向或垂直方向上的位移和角度（角速度）的偏移量控制在允许的范围以内，使之在静态平衡点附近保持平衡，或者在一定范围内在运动过程中使小车保持平衡。因此小车控制的关键是姿态倾角、角速度、及车轮加速度测量的准确性、实时性，这直接决定小车的控制精度及稳定性。针对这一问题，本文采用高性能单片机 STM32F103C8T6 作控制器，通过安装在车轮轴上的光栅码盘等（每周输出 4200个AB相脉冲）实现小车位移、速度及运动方向的测量，用数字运动传感器 MPU-6050实现车体倾角及倾角角速度的测量，利用卡尔曼滤波算法进行数据融合，精确地计算出小车的实时姿态信息，再利用 PID控制算法实现小车的自平衡控制。两轮小车受力分析如图 1所示。图中小车质量为 m，车轮半径为 r、车速为v，车体自身水平作用力为 H、车轮受到地面摩擦力为 Hf，θ为倾角，车轮对车轴的等效转动惯量为 J，车体对车轮吊销转动惯量为 I，车轮转速为ω，电机电磁转矩为Tm，车体质心位置坐标（x,y），车体质量为ma，车轮对车身扭矩为T。在倾角θ较小的情况下，分析可得小车动力学方程可简化为：1）（1）式为两轮自平衡小车车体倾角与角加速度的微分 图1 小车受力分析方程。文献指出在小车质量一定的情况下，地面摩擦系数越大、电机扭矩越高，小车自平衡可控角度范围越大，越容易控制小车的平衡。车体倾角测量由陀螺仪和加速度传感器实现，单独使用陀螺仪或加速度传感器都无法完成倾角的测量。一方面，陀螺仪能够测量车体倾斜的角速度，对角速度积分得到车体倾角，但在实际应用中陀螺仪输出的信号有偏差或漂移，经过积分运算后会形成随时间增加而增大的积累误差，导致无法得到有效的倾角信号；另一方面，小车运动中车身会不断摆动，导致加速度传感器输出的信号中有很大的干扰，虽然通过数据平滑滤波可有效降低干扰，但滤波后的信号已无法实时反映车体倾角的变化，因此仅用加速度传感器也无法实现倾角测量。陀螺仪和加速度传感器输出的信号中含有高斯白噪声，使用卡尔曼滤波算法将陀螺仪和加速度传感器输出的数据进行优化处理，补偿加速度传感器的动态误差及陀螺仪的漂移误差，可得到准确的车体倾角。2.1 卡尔曼滤波器一般形式卡尔曼滤波算法实际上是一种求最优估计值的递推算法，是利用前一状态的估计值和当前的测量值来更新对状态变量的预测和估计，最终可以得到当前状态的最优估计值。对本系统而言，将陀螺仪输出的角速度作为预测值的控制量，加速度传感器输出的角度作为测量值，对车体当前倾角进行最优估计。记 是k时刻对状态的先验估计， 是k时刻得到观测值后对状态的后验估计， 是先验估计协方差， 为后验估计协方差， 为卡尔曼增益，则卡尔曼滤波方程如（ 2）～（6）式所示[3]。其中（2）式、（3）式为预测方程；（4）～（6）式为校正方程。其中矩阵 A、B是系统结构确定的矩阵，Q、R分别是系统噪声和观测噪声的协方差矩阵， H是测量系统的参数。（2）3）（4）（5）（6）2.2加速度传感器和陀螺仪数据融合算法记为车体k时刻的倾角（单位：rad），陀螺仪k时刻测得的角速度记为，加速度传感器k时刻测量到的车体倾角为 ， 为k时刻角速度（ rad/s）， 是k时刻角速度误差（样时间间隔为 dt，则预测方程为（ 7）、（8），更新方程为（ 9）～（11）。

rad/s），采（7）（8）（9）（10）（11）具体计算时， 的初值可简单地初始化为单位矩阵，为了加快收敛速度、尽快得到稳定值则通过实验确定；Q是观测噪声协方差矩阵，通过实验确定；度传感器测得的倾角的方差决定，如（12）式所示。

R由陀螺仪测得的角速度的方差及加速（12）两轮自平衡小车硬件电路主要由微控制器、测量模块、直流电机驱动模块、蓝牙模块等构成。其中测量模块包括传感器 MPU-6050、光栅编码器两部分。 MPU-6050用于测量车体倾角，同时也完成加速度测量；安装在左右两个车轮轴上的增量型光栅码盘经光栅编码器处理后， 分别输出两路 AB相脉冲信号，经单片机处理后可得到每个车轮的转速及方向；蓝牙模块接收手机或电脑发出的信号，控制小车的运动。小车的硬件系统结构如图2所示。选用意法半导体公司的STM32F103C8T6作为控制器，该控制器是一种基于CortexM3内核的高性能MCU，运行时钟最高为72MHz，运算速度高达1.25DMIPS/MHz，片上64K字节FlashROM、20K字节RAM，内含7个定时器，可输出多路PWM信号又可配置成增量型编码器接口[5]，尤其适合于车轮测速传感器输出的STM32F103C8T6AB相脉冲信号的处理。片上集成的I2C总线模块，光栅编码器光栅编码器MP6050方便了对传感器MPU-6050的控制。STM32F103C8T6（左）（右）微完成传感器MPU-6050输出的加速度信号和陀螺仪信控号的采集；对加速度信号和陀螺仪信号进行数据融蓝牙模块制左轮电机右轮电机器合，计算出车体实时倾角；处理两个车轮上光栅编码器输出的两路AB相信号，得到小车运动速度及方电源模块直流电机驱动模块向信息；利用数字PID算法输出PWM信号控制电机驱动模块实现小车姿态调整从而实现小车的自平衡控制。图2系统硬件结构框图图3为单片机及传感器接口电路，P1、P2分别接左右车轮的光栅传感器检测电路，检测后的AB相脉冲分别从PA0PA1、PA6PA7送入单片机；PA9PA10用以与蓝牙模块进行串行通信；PA5为电池电压检测输入；PA2PA3、PB0PB1分别为左右车轮的控制信号；PB6、PB7分别是左右车轮的PWM调速控制信号；PB10PB11与传感器MPU-6050相连，实现加速度和倾角的测量。图3 单片机及传感器接口电路图4为传感器 MPU-6050电路。MPU-6050是世界上首款 9轴运动传感器，它将3轴MEMS陀螺仪、3轴MEMS加速度计和一个可扩展的运动数字处理器（ DPM）集成在一个芯片上，其内部对陀螺仪和加速度计分别使用 3个16位ADC，检测输出的 16位数字量通过 I2C总线接口输出，为实现精确跟踪快速和慢速运动，陀螺仪和加速度传感器的测量范围都可设定 [6]。图4 MPU-6050 电路 图5 电机驱动电路图5为直流电机驱动电路。 TB6612FNG是一款新型驱动器件，能独立控制 2个直流电机，具有集成度高、输出驱动能力强等特点， 每通道输出最高 1.2A的连续驱动电流，启动峰值电流达 2A/3.2A(连续脉冲

/单脉冲)，有正转

/反转/制动/停止

4种电机控制模式，

PWM支持频率高达

100kHz[7]，在集成化、小型化的电机控制系统中，是一种理想的电机驱动器件。软件主要由传感器数据采集程序、卡尔曼滤波程序、车速及运动方向检测程序、直流电机 PWM控制程序、蓝牙串口模块数据处理程序等组成。 STM32F103C8T6

PID控制程序、内含PWM模块及I2C总线接口模块，因此直流电机

PWM

控制和传感器

MPU-6050

数据采集通过读写单片机内部的寄存器就可实现，将单片机内的定时器配置成增量型编码器接口，车速及运动方向的检测也很容易实现，不需要 CPU过多干预。因此，卡尔曼滤波算法及 PID控制算法的实现是软件编程的关键。根据（7）～（11）式卡尔曼滤波数据融合过程分成以下步骤：（1）设定数据更新频率。具体编程时由定时器定时产生所需要的时间间隔dt；（2）设定参数。主要是矩阵Q的确定，可取，一般由实验确定

q1、q2、q3，式（

11）中矩阵

取单位矩阵；（3）计算更新后的协方差矩阵；（4）将陀螺仪的更新后的协方差融入到加速度计的倾角误差估算； （5）求加速计新的协方差矩阵；（6）计算倾角误差、角速度误差、输出角速度。PID 控制的原理是根据系统的被调量实测值与设定值之间的偏差，利用偏差的比例、积分、微分三个环节的组合计算出控制量，程序设计中采用 PD组合控制方式，利用卡尔曼滤波器输出的车体倾角及角速度计算电机 PWM信号的占空比，同时利用小车位移及小车速度计算 PWM信号占空比增量值，如（ 13）、（14）式所示。（13）（14）两轮自平衡小车系统中变量多且相互耦合，因此调试过程比较繁琐，主要包括以下几方面：（1）确定零偏。关闭电机驱动，让车体垂直于水平桌面并保持静止，读取若干组MPU-6050中各通道的偏移量到PC机中，求取陀螺仪的零偏值及加速度传感器z轴最小值；（2）静止倒立小车确定加速度传感器z轴最大值，从而可以确定加速度传感器z轴输出值变化范围，进而确定加速度计的比例因子；（3）陀螺仪比例因子的确定。确定的目的是使由陀螺仪积分得到的车体倾角值与加速度计计算到的值一致；（4）PID控制中按照先比例后微分的步骤确定比例因子和微分因子。五、项目存在问题及经验教训项目中所设计的这种基于微控制器 STM32F103C8T6、采用卡尔曼数据融合算法的两轮自平衡小0干扰的情况下小车仍能保持平衡，小车在蓝牙设备发出的指令下可实现前进、倒退、转弯、停止等动作，控制过程稳定、可靠。测试结果表明利用卡尔曼滤波算法能有效地消除陀螺仪漂移及车体摆动等对加速度计的影响，准确计算出姿态倾角，