基于LQR 控制的两轮自平衡小车设计

林寿光

两轮自平衡小车具有便携、环保、操作简单、成本低等优点，常应用在空间探索、短途运输、商场和机场治安巡逻等领域［1−2］.本文提出了一种基于LQR 控制的自平衡小车设计软硬件方案，并利用Simulink 平台进行仿真分析，搭建硬件平台进行实际测试.

1 系统整体方案设计

本系统以STM32F103RCT6 处理器为主控芯片，采用MPU6050 陀螺仪模块获取角度等传感数据，运用卡尔曼滤波算法进行数据处理.然后通过LQR 控制算法，计算出左右两轮最终的PWM 占空比调整姿态，达到控制小车平衡的效果.使用OLED 显示小车参数，通过手机APP 和上位机实现人机交互，控制小车的运动姿态.同时为了增加功能的多样性，使用N10 激光雷达模块实现避障、跟随功能；通过读取电池电量，在小车电压过低时停止小车运动.为了使小车达到动态平衡，小车通过定时采集编码器和陀螺仪的数据，获取小车的角度和左右两轮的转动情况，动态调整小车姿态.系统整体设计方案如图1 所示.

图1 系统整体设计框图

2 硬件电路设计

两轮自平衡小车的硬件分为机械结构和硬件控制电路两部分.机械结构部分设计参考目前市面上较为成熟的自平衡车机械结构.主要硬件电路包括：主控器模块、姿态检测模块、电机及驱动电路、避障模块、蓝牙通信模块和显示模块.

2.1 主控器模块

控制器采用ST 公司的STM32F103−RCT6芯片，它是基于ARM 公司设计的Cortex−M3 内核，最高工作频率为72 MHz，具有ADC、USB、DMA、IIC、SPI、USART 等外围设备及通信接口.定时器可产生多路PWM 用以控制电机，ADC 可以读取电池电压.具有高性能、低功耗、接口丰富等特点.本设计主控器的最小系统如图2 所示.

图2 STM32F103 最小系统图

2.2 姿态检测模块

采用MPU6050 模块作为小车的姿态检测.芯片整合了3 轴陀螺仪和3 轴加速度计，通过内部的DMP 检测，使用库函数得到四元数，再根据公式即可解算出姿态角，最后通过IIC 通信送到主控芯片.

传感器测量的数据存在诸多不确定性，比如有很多噪声会造成陀螺仪的漂移、车体摆动等，对小车加速度计产生干扰，为了得到更确切的车体倾角值，本设计采用卡尔曼滤波算法对陀螺仪与加速度计的输出值进行数据融合［3−4］.MPU6050 通过IIC 与主控芯片实现通信，模块及接口电路图如图3 所示.

图3 MPU6050 模块及接口电路图

2.3 电机及驱动电路

2.3.1 电机选型

保持稳定是自平衡小车的主要功能.为了保证自平衡小车能够长时间在不动或低速的状态下工作，需要一款在低速稳定性较高，又能输出较大转矩的电机［5］.通过比较测试，本设计采用直流减速电机.减速电机型号选用MG513 型，集成500 线GMR 编码器，堵转电流最大不超过3 A.

电机的速度使用分辨率为500 的GMR 编码器输出AB 相进行测量，电机减速比为1∶30，使用编码器的计数测量功能，设置为一个周期计数4 次，可实现轮子转一圈输出60 000 个计数.测量精度是13 线霍尔编码器的38 倍，测量低速时比霍尔编码器效果更好.

2.3.2 电机驱动模块

本设计采用AT8236 模块作为电机驱动，峰值电流高达6 A，远高于TB6612 型.可通过对输入信号进行脉宽调制（PWM）控制电机转速.有正转、反转、制动和停止4 种控制模式.内部包含过流保护、短路保护、欠压锁定和过温保护.电机驱动及接口电路如图4 所示.

图4 电机驱动及接口电路图

2.4 避障模块

本设计通过N10 系列激光雷达实现小车的避障、跟随功能.采用TOF（time of flight）方案测距，通过测量调制激光的发射、返回时间差测量物体与传感器的相对距离.测量精度3 cm，角度分辨率是0.8°，具有360°全方位扫描角度，支持建图、导航等ROS 核心功能.扫描角度和抗干扰能力远远超过超声波避障模块，可以让避障、跟随功能效果更稳定.雷达接口电路如图5 所示，其中1 脚REC 为GPS 经纬度时分秒，2 脚PPS 为GPS 秒信号，3 脚和4脚为串口通信引脚.

图5 雷达接口电路

2.5 蓝牙通信模块

系统通信模块使用BT05 蓝牙串口模块.该模块主从模式一体，可以和带有蓝牙设备的PC 端、手机端进行通信连接.具有成本低、体积小、功耗低、收发灵敏性高等优点.

2.6 显示模块

OLED 模块配置IIC/SPI 接口，使用I/O 数量少，显示视角广、对比度高、功耗低、体积小，对比于LCD 显示屏，更适合应用于小车.因此，系统显示模块采用0.96 寸OLED 显示屏，用以显示小车的运行电压、倾角、电机速度和避障距离等.

3 软件设计

3.1 程序总体设计

软件设计采用模块化设计，方便代码的可移植性和修改，分为主程序和中断处理程序.

主程序：主要完成系统的初始化，实现功能模块的调度.初始化包括时钟初始化、定时器初始化、中断初始化及串口、编码器、ADC、OLED、传感器等外设模块初始化.

中断处理程序：模块功能主要在中断服务中完成，包括姿态传感器数据采集处理、电机编码器数据处理、电压检测、控制算法、雷达控制等.

两轮自平衡小车是一个不稳定的系统［6］，为了能够保证系统的正常运行，处理器需要不断检测车身的姿态信息和左右车轮转速，同时还需要对串口信息进行处理，判断自平衡小车需要执行的动作.除此之外，微处理器还需要对电池电量、OLED 显示、激光雷达测距等任务进行处理.根据各个任务对实时性的要求，完成自平衡小车的主要模块分析、分类，以及程序执行间分配，如表1 所示.根据上述对自平衡车的任务分析，设计自平衡车软件总体框架，如图6 所示.

表1 软件处理任务实时性分析

图6 自平衡小车软件总体设计

主要模块设计说明如下：

姿态控制.姿态控制是软件系统的核心部分.通过先解析控制端发送的运动控制命令，然后利用MPU6050 模块测到的倾角值和角加速度值作为平衡系统的输入进行运算，最后将控制系统的输出经电机驱动模块转变为PWM 波施加在电机上，用以控制平衡车正常运行.本设计采用LQR 作为姿态平衡控制算法.

卡尔曼滤波算法.该算法使用系统状态方程预测当前的值，使用传感器测出来的观测值对预测值进行修正.系统以MPU6050 测量到的信号作为系统观测值，首先对倾角和角加速度做先验估计，利用系统噪声对系统预测方差进行预测.利用预测方差值与角度测量噪声，计算当前系统的Kalman 增益.通过上一状态的最优角度、最优角加速度、系统卡尔曼增益对预测值进行更新，得到当前状态的最优角度及角速度解.最后更新协方差方程.

编码器测速.本设计的电机自带编码器测速模块能输出电机的实时速度.当电机旋转时，编码器会输出若干个脉冲方波.编码器测速是通过定时器的输入捕获模式实现，当开启定时器的输入捕获模式后，对编码器接口进行高电平捕获，捕获一个高电平，定时器计数器加1，最后读取定时器计数寄存器中的计算值，计算得到小车的实际车速.

蓝牙功能.蓝牙模块用于APP 与小车之间的通信，能实现控制小车的运动转向与查看数据、调参等功能.蓝牙模块与手机的通信通过无线蓝牙实现，与主控芯片的通信使用串口3 连接.

电池电压检测.平衡小车使用12 V 的航模电池.电池电压通过ADC 进行测量，由于STM32 芯片最大只能转换3.3 V 的模拟电压，所以需将电压通过电阻分压（本设计分压值采用1/11），再送到芯片进行测量.最终测到的电压需乘以11.

雷达功能.雷达在上电时会自动传回数据包，不需要进行额外设置，只需要在串口中断中进行接收即可.一帧数据有16 个点的位置信息，包括角度和距离.

3.2 控制算法设计

本设计采用LQR 作为姿态平衡控制算法，需要先推导出小车的数学模型，再代入系统状态方程解出反馈矩阵.

3.2.1 自平衡车数学模型

两轮自平衡小车的结构主要由车体和双轮两部分组成，本设计中使用的小车实物及受力分析如图7、图8 和图9 所示，通过分析两轮自平衡车的车轮、车身等各部分模型，建立它们的联系，得出整体的数学模型［7−8］.模型分析涉及的参数见表2.

表2 两轮自平衡小车具体参数

图7 两轮平衡小车实物

图8 小车正向运动模型

图9 小车转向运动模型

小车的正向运动表示为：

小车的转向运动表示为：

由式（1）、式（2）、式（3）可得系统的状态方程：

其中：vLO表示左轮无摩擦时线速度的大小，单位：rad/s；vRO表示右轮无摩擦时线速度的大小，单位：rad/s.

矩阵中的元素为：

3.2.2 LQR 控制算法

姿势平衡控制是自平衡小车系统的核心，良好的控制算法能够让系统的性能更加稳定和安全.常见的控制算法有PID 算法、LQR 算法等，本方采用LQR 算法.

LQR（Linear Quadratic Regulator），即线性二次调节器.是根据系统的状态空间模型设计优化的动态控制器.一旦系统的状态偏离平衡状态，LQR 可以在不消耗过多能量的情况下通过状态调整快速回归平衡状态［9］.

设系统（要求系统完全能控）的状态空间表达式为：

结合上述自平衡小车的数学模型及公式，式中：

LQR 设计就是为了找到一个最优控制量u(t)，用来保持两轮自平衡小车的平衡，

使二次型目标J函数达到极小值

式中：Q为n×n维半正定加权矩阵，R为n×n维正定加权矩阵.在工程实际运用中，Q和R是对称矩阵且常取对角阵.

为了使二次型目标函数J最小，需要计算出反馈矩阵K［10］：

其中：P是Riccati方程ATP+PA+Q−PBR−1BTP=0的解.

4 系统仿真与实际控制实验

4.1 系统仿真

LQR 的Q阵和R阵系数的选取直接影响系统性能［11］，而Q和R的选取一般无规律可循，通常采用仿真试错法进行设计.LQR 的Q阵和R阵确定后，反馈矩阵K一般不用手动计算，可以直接通过Matlab 中的lqr（）函数解出.

通过自平衡小车的数学模型及LQR 控制算法，由式（4）～式（9）代入程序.这里加权矩阵R不能太小，否则会导致控制量的急剧增大以至于超过系统的执行能力；矩阵R也不能太大，否则控制作用太小会影响控制性能.结合实际，矩阵R中对角线上的元素选为1 较合适，可以根据实际情况再进行合理的修改.具体步骤如下：

①根据实际选用小车的参数，主要程序如下所示：

②代入所选小车的实际参数，运行程序结果如下：

③然后利用Matlab/Simulink 对其进行仿真，仿真框图如图10 所示.

图10 Simulink 仿真框图

仿真结果如图11 所示，由scope 可以看到，当给小车一个10 度的冲击，车体回到平衡位置所需时间大约是2 s，移动了大约0.13 m，由此可见模型的建立正确且有效.

图11 Simulink 仿真输出波形图

4.2 实际控制实验

使用Keil5 编程，实验小车使用图7 实物底盘，以及上述分析的STM32F103 系列主控芯片和模块搭建而成.

其中LQR控制对应的C语言代码如下所示：

LQR 控制器的效果是使所有状态变量都为0，为了实现App 控制的便捷，引入“Tar⁃get_x_speed”“Target_angle_x”“Target_gyro_z”三个量，分别表示目标前进速度、校正后的平衡中值、目标转向速度.以App 控制小车前进为例，此时，目标是使得x_speed=Target_x_speed≠0，而LQR 控制器的效果是使所有状态变量都为0，那么，可以把“x_speed−Target_x_speed”作为新的状态变量，使它变为0 即等价于使得x_speed=Target_x_speed.

小车硬件平台如图12 所示，APP 波形界面如图13 所示，分别显示X、Y、Z三个轴的角度值.上位机显示界面如图14 所示，三个参数从上而下分别表示平衡小车倾角（单位：°）、雷达测距（单位：cm）和供电电池电压（单位：V）.

图12 小车硬件平台

图13 APP波形监控

图14 上位机显示界面

通过系统仿真及实物硬件平台测试结果表明，本设计能够很好地实现小车的平衡、前进、后退及转向等功能，可以在OLED、手机APP 及上位机三种渠道显示参数，同时还可以利用激光雷达模块实现避障、追随和电压检测等功能.硬件平台在人为触碰、负重改变等不同外部干扰情况下，均能快速恢复到平衡状态.

5 结语

根据两轮自平衡小车的数学模型，搭建了硬件平台，利用LQR 控制算法对小车进行姿态控制.通过Simulink 对系统仿真，并对硬件平台进行了实际验证测试，结果表明LQR控制算法下的自平衡小车能够在外部干扰的情况下，短时间内进行姿态纠正，重新恢复到平衡状态，具有稳定性高、抗干扰能力强、调节速度快等特点.同时具有避障、跟随、手机APP 遥控等功能，具有应用推广价值.