基于LQR控制的机器人自行车静态平衡研究

古文生 王峰 周全 熊超伟

摘要：针对本团队目前研究的机器人自行车静态平衡控制，使机器人自行车达到自动静止的目的。本文在对该系统进行动力学建模的基础上，设计了一種基于LQR控制的算法，完成机器人自行车静态平衡系统。该机器人自行车是在一辆自行车的基础上改装的，要使它能够静止不倒下，我们先让前轮摆到90度。利用陀螺仪等传感器进行实时检测系统的状态，然后STM32程序通过LQR算法输出控制力矩给前轮电机，改变当前机器人自行车的状态，使系统达到静止平衡的状态。

关键词：机器人自行车;静态平衡;动力学建模;LQR控制算法;STM32;陀螺仪

中图分类号：TP2

文献标识码：A

文章编号：1009-3044（2020）04-0192-03

收稿日期：2019-11-21

作者简介：古文生（1993—），男，广东韶关人，硕士研究生，研究方向为机器人和智能车辆动力学建模控制;王峰（1961—），男，美籍华人，特聘教授，研究生导师，主要研究方向为机器人和智能车辆建模控制;周全（1995—），男，湖北黄冈人，硕士研究生，研究方向为机器人和智能车辆动力学建模控制;熊超伟（1995—），男，江西丰城人，硕士研究生，研究方向为机器人和智能车辆动力学建模控制。

Research on Static Balance of Robot Bicycle Based on LQR Control GU Wen-sheng，Everett Xiaolin Wang，ZHOU Quan，XIONG Chao-wei

（School of Information Engineering，Guangdong University of Technology，Guangzhou 510006，China）

Abstract：Aiming at the static balance control of the robot bicycle studied by our team，the robot bicycle can achieve the purpose of au-tomatic static.On the basis of dynamic modeling of the system，an algorithm Based on LQR control is designed to complete the static bal-ance system of the robot bicycle.The robot bike is a modified version of a bicycle.To make it stationary and not fall down，we first put the front wheel at a 90-degree Angle.Using gyroscope and other sensors to detect the state of the system in real time，STM32 program outputs control torque to the front wheel motor through LQR algorithm，changes the current state of the robot bicycle，and makes the sys-tem reach the state of static balance.

Key words：Robot Bicycle;Static Balance;Dynamic Modeling;LQR Control Algorithm;STM32;Gyroscope.

机器人自行车是一种典型的欠驱动系统，要实现静态平衡，必须要通过某一个控制量改变当前自行车的状态，以抵消某个自由度产生的偏移，从而达到平衡。之前流行着一种实现机器人自行车静态平衡的方法，就是在自行车上安装一个高速运转的飞轮，通过这个飞轮达到平衡的目的。但是这种飞轮需要转很快才能达到平衡，需要消耗巨大能量。而本文介绍的方法是，机器人自行车切换到静态时候，直接把前轮转到90度，使前轮和车主轴呈现垂直的状态。通过控制前轮的电机，改变车的状态，达到静态平衡。这种方法解决了飞轮消耗能量的缺点[1]。

线性二次型[4]最优控制设计是基于状态空间技术设计一个优化的动态控制器，线性二次型（LQR）算法应用对象是现代控制理论中以状态空间形式给出的线性系统，而目标函数为对象状态和控制输入的二次型函数。LQR最优设计是指设计出的状态反馈控制矩阵K要使得二次型目标函数输出最小值，而K矩阵由Q，R唯一确定。所以在选取这两个矩阵的时候要特别注意。LQR可得到状态线性反馈的最优控制规律，易于构成闭环最优控制。

针对线性系统的控制算法设计问题[5-7]，如果该系统的性能指标是状态变量或者控制变量的二次型函数积分，那么这种动态系统的最优化问题称为线性系统二次型性能指标的最优控制问题，也就是线性二次问题。线性二次型求优问题可以写成统一的解析表达式和实现求解过程的规范化，并可简单地采用状态线性反馈控制律构成闭环最优控制系统，能够兼顾多项性能指标，因此得到特别的重视，为现代控制理论中发展较为成熟的一部分。

1 系统建模

在设计LQR控制算法之前，要先对整个系统进行建模，并推导出状态方程，才能够进行LQR控制器设计的工作。因此，这一部分要对机器人自行车进行建模[3]。下面先看看整个机器人自行车的模型，如图1所示。

图1用了三个坐标系对车子进行描述，其中坐标系00是大地基准坐标系，O1为描述车身状态的坐标系，O2为描述前轮状态的坐标系。为了能够表达机器人自行车所处的状态，这里选取向量q用于描述车身的状态，q=（q1，q2，q3），其中q表示车身倾斜角，即坐标系00和01两者的z轴之间的夹角;q2表示车头转向角度，也就坐标系O0和O2两者的x轴之间的夹角。q3表示前轮所转过的角度，系统开机时所处的初始状态为0度。

在动力学中，限制物体运动的条件称谓约束。约束又称谓非完整约束和完整约束。完整约束是指几何约束和可积分的微分约束的统称。非完整约束是指系统具有不可积分的动力学方程。对于完整约束，利用拉格朗日第二方程可以建立它的动力学方程。而对于非完整约束，主要有拉格朗日方程，罗斯方程，阿佩尔方程这三种建模方法。

我们这里选择用阿佩尔方程对机器人自行车进行建模。下面公式（1）就是阿佩尔方程。

其中G是系统的总加速度能，V是系统的总势能。q;为向量q在维度j的角加速度，q;为向量q在维度j的角度，rj为系统在j维度对用的力矩输出。我们把机器人自行车系统的G和V求出之后带进以上的阿佩尔方程就可以得出以下公式（2）的动力学方程。

其中M，C，G，K都是与向量q有关维度值相关的参数矩阵21。向量q在前面已经介绍过，这里不再重复写出来。下面把M，C，G，K展开来看看。

接下来我们要对该动力学方程进行变换，因为我们实现静态平衡时候，车头的转弯角度是固定在90度的，所以可以根据我们的实际情况对该方程进行一些简化。最后得到以下公式（6）为系统的状态方程[1]。

其中x=（q1，q3，q1，q3），因为我们要实现静态平衡，92作为车头转向角已经固定在90度了，所以我们只需要观测q1和q3相关值的变化就可以反映出当前系统的状态，所以向量x里面只有这两个参数有关的变量。对于A，B矩则是由M，C，G，K推导出来的。u是τ向量中的第三维度变量，也就是前轮转向力矩。因为其他两个维度没有力矩输出，所以这里只有这个力矩引入到了状态方程。有了状态方程，就可以进人到LQR控制器设计阶段，下面将进人LQR控制器设计部分。

2 LQR控制器设计

前面已经得到了机器人自行车的状态方程，现在可以开始设计LQR控制器了。根据LQR调节器的原理，针对状态方程i=Ax+Bu，通过确定最佳控制量u（k）=-Kx（t）中的矩阵K。使得控制性能指标L=∫「（xTQx+uTRu）dt得到极小值。对Q，R这两个x和u的加权矩阵的选取，我们需要令q1和q3得到最小。选取好Q，R矩阵之后，接下来就要求解RICCATI方程PA+APT-PBR-1BP+Q=0，求得矩阵P之后，就可以通过K=R-1BTP计算出反馈增益矩阵K。得到最优控制器u（k）=-Kx（t）。以下是LQR简要框图。

3 系统的实现

我们前面已经对机器人自行车完成了建模和控制器设计，我们这里就要进行系统的实现，我们选择STM32F405作为主控芯片，MPU6050陀螺仪作为检测车身状态的传感器，SMP8048电机作为力矩输出。下面图3是整个系统的组成框图。

从上图3可以看出系统的组成结构和工作流程。其中MPU6050陀螺仪的作用是检测机器人自行车的倾斜角和倾斜角速度等状态量，然后把这些值送到STM32主控芯片当中;而另一边的带编码器的电机，把编码信号经过电机驱动机转换后传回给主控芯片，主控芯片即获得电机转速。主控芯片根据这些回传数据进行判断当前状态是否平衡，然后经过LQR控制算法输出PWM信号和方向信号给电机驱动器，电机驱动器把这些信号转换成三相电等信号给电机。这就是整个系统的工作过程。图4是具体的软件组成框图。

完成硬件软件等工作之后，我们进行了一系列测试实验，经过我们的实验检测，，实现了静态平衡的目标，下图5为实物作品。

当机器人自行车往左边倾斜时，前轮立马向左边加速运.动，防止它往倒下。当它往右边倾斜时，前轮往右边加速，防止它往右边倒下，这就实现了静态平衡。

4 总结

机器人自行车要实现静态平衡，必须有外力支撑它，让它平衡不倒下，而我们这里把前轮摆到90度，给系统提供了支撑外力。我们的系统是一种典型的欠驱动系统，也就是说我们想要抵消车身不平衡时候造成的倾斜角偏移，是不能通过某个控制量直接抵消，而是借助其他控制量来抵消，这里我们是靠前轮力矩来实现这个目的。要如何控制前轮力矩抵消这一偏移，我们就设计了LQR控制器进行完成这些控制过程。因此，我们可以得出LQR控制器可以实现机器人自行车静态平衡的结論。参考文献：

[1]C.Xiong，Z.Huang，W.Gu，W.X.Wang，etc.Static Balancing of Robotic Bicycle through Nonlinear Modeling and Control[C]/2018 3rd International Conference on Robotics and AutomationEngineering（ICRAE）.GuangZhou：China，USBISBN：978-1-5386-9593-7.

[2]Trung-Kien Dao，Chih-Keng Chen.Sliding-mode control for the roll-angle tracking of an unmanned bicycle[J].Taylor&Francis.ISSN：0042-3114.

[3]郭磊，廖启贞，魏世民.自行车机器人动力学建模与MIMO反馈线性化[J].北京邮电大学学报，2007，30（1）：80-84

[4]刘璟，梁昔明.LQR控制与PID控制在单级倒立摆中的对比研究[J].控制理论与应用，2007（1）：13.

[5]张永立，李洪兴，苗志宏，程会锋.基于变增益LQR控制方法的二级倒立摆自动摆起[J]系统工程理论与实践，2011，31（7）：1341-1355.

[6]金鑫，钟祥，谢双义，等.大型风力发电机转矩LQR控制及载荷优化[J].电气系统保护与控制，2013（6）：93.

[7]Everett.X.Wang，JunchengZou，Gengping.Xue，YiJun.Liu，etc..Development of Ef fi cient Nonlinear Benchmark BicycleDynamics for Control Applications [J].IEEE，2236-2246.

[通联编辑：梁书]