基于复合控制器的两轮机器人平衡控制研究

杨国欢

摘 要：两轮自平衡移动机器人是一种高阶次、不稳定、非线性的典型控制系统。以其为研究对象，采用Lagrange方程建立其动力学模型，经过线性化处理得到其一定约束条件下的线性化模型。采用线性二次型调节器与PID控制相结合的方法可有效克服线性化过程中约束条件对系统的影响，并且以数字信号处理器芯片TMS320LF2812为控制器核心，实现了两轮机器人较大倾角范围的动态平衡控制。物理实验表明：使用LQR与PID复合控制器对两轮机器人实体控制的有效性。

关键词：两轮自平衡机器人 Lagrange方程 LQR PID DSP2812

中图分类号：TP242 文献标识码：A 文章编号：1672-3791（2014）04（a）-0002-03

基于倒立摆模型的两轮自平衡机器人属于轮式机器人的范畴，并结合了自主移动的思想，其体积小、结构简单、运动灵活，适于在狭小和危险的空间内工作，在民用和军事上有着广泛的应用前景；同时由于其不稳定的动态特性，两轮自平衡机器人成为验证各种控制算法的理想平台，具有重要的理论意义。两轮自平衡机器人属于非线性、时变、欠驱动、非完整约束系统，控制问题是其研究的关键[1]。

国内外研究学者对移动轮式倒立摆模型及对两轮行走平衡控制技术进行了大量的研究，也提出了一些将此非线性系统线性化的方法。其中很多研究人员用近似线性化方法将机器人非线性模型线性化，再利用现代控制理论中极点配置或LQR等控制方法进行分析研究，仿真分析能获得很好的效果。但此线性化方法是假设两轮机器人俯仰角在一个小范围之类进行的，但实际中，机器人能控角度范围远大于此，采用此种方法在平衡点附近有很好效果，但当大于一定角度后系统失去控制。

本文主要介绍两轮自平衡机器人的结构设计，采用Lagrange方法建立数学模型，提出了将LQR与PID控制相结合的方法对两轮机器人进行姿态控制，物理实验验证了此方法不仅有很好的控制效果，而且实现了两轮机器人大倾角范围的平衡控制。

1 两轮自平衡机器人的动力学模型

1.1 两轮自平衡机器人的系统结构

两轮自平衡机器人系统的机械结构采用层状结构。底层有两个同型号、同轴的直流电机、姿态检测传感器、伺服驱动器和电源。在中间层有电源监控和转换模块和控制器模块。上层是机器人头部，可用来放置机器人视觉传感器以及将来扩展功能的部件。

两轮机器人的控制核心是TMS320F28

12DSP处理器，其AD口转换机器人上的姿态传感器的检测信号确定机器人姿态，并用QEP单元采集电机编码器上信号确定机器人速度和位移，通过设计的控制器计算得到电机电压，实现机器人平衡控制。

1.2 两轮自平衡机器人的动力学建模

由式可知，经过转换将原多输入系统分成两个单输入子系统，控制机器人的位移与俯仰角度，控制机器人的偏航角。面对两轮机器人的平衡控制，我们现在只需考虑机器人的俯仰角度和位移，因此将角度、角速度、位移及速度选为状态量，写出如下状态方程：

根据系统能控能观性原理在Matlab中进行计算，得出此系统能控能观。

2 基于LQR和PID的控制器设计

2.1 两轮机器人可控范围分析

由模型分析可知此机器人系统是可控的，但由于模型是在平衡点附近进行的线性化，角度为，而实际机器人倾斜角度比此值大得多，并且使用的倾角仪线性测量范围为。通过物理实验得：机器人倾斜的角度时，给电机最大控制量指令情况下，机器人实体完全可以控制。因此，设计的两轮机器人其平衡控制范围定为，远大于线性化时约束条件。本文所研究的LQR和PID复合控制器的控制算法很有研究意义[3]。

2.2 控制器设计

2.2.1 LQR控制器

对上面所描述的机器人模型，给定对于状态和控制的二次型性能指标函数定义为：

，

其中，权值矩阵和均为对称正定常值矩阵。其系统控制方法为找到最佳控制向量矩阵K，得到最佳控制量。这里选取Q=[100 0 0 0；0 100 0 0；0 0 100 0；0 0 0 100]，R=1。利用Matlab命令LQR（A，B，Q，R），得到系统反馈增益：

当给定初始俯仰倾角分别为和，其他状态量给零时，其零输入相应仿真曲线如图1和图2。

比较可知，初始角度相差很大的情况下，对于理论仿真来说，其变化情况并不大。但是在实际物理系统调试中，情况却不同。当倾斜角度增大时，其线性化过程中产生的误差也相应的增大，将会出现所设计的控制器就根本无法是机器人达到动态平衡的情况，而且对于LQR控制器的设计，模型的精确程度及物理系统参数准确性都要求相对严格。

2.2.2 PID控制器

对于经典PID控制，它是工业控制中的主要技术之一。在两轮机器人控制上，其主要优点是结构简单在控制器上易于实现，并且适应性强，不依赖于其线性化模型，兼顾了一定的鲁棒性。但由于经典的PID调节器为实现无差调节而引入积分作用后，不可避免地使系统的调节过程发生超调。适度的超调对于提高系统的快速性是有利的，但在机器人实体控制过程中，这样的超调也会使系统发生振荡，静态性能指标较差。如图所示，阶跃相应仿真曲线见图3。

2.2.3 LQR与PID复合控制器设计

以上分析了LQR与PID控制器在两轮机器人控制中的优缺点，对于两轮机器人来说，由于其非线性和不确定性，使用任何其中一种控制器进行控制都不能达到理想的效果。为了得到更快的控制速度和更好的动态平衡效果，根据两控制器各自的特点，本文设计了复合控制器：分别采用一个控制因子来限制两个控制器的输出，再将两控制量进行叠加。

假设两控制器输出分别为U1和U2，控制因子为K1和K2。考虑到，当倾斜角≤时，机器人接进平衡状态，系统状态方程的线性化误差较小，此时采用LQR控制效果较好，不需采用PID控制方法。当倾斜角≤≤时，机器人开始偏离平衡点附近，模型线性化会产生一定误差，控制器参数也存在误差，仅靠LQR控制已不能完全满足要求平衡控制要求，此时引入PID控制，使其各发挥一定的功能。当倾斜角≤≤时，机器人已经偏离平衡点很远，线性化的模型有较大的误差，其LQR控制器参数已不符合实际控制要求，此时只调用PID控制算法，使车体倾斜角度回到小于范围[4]。

3 两轮机器人实物控制效果

实验用的两轮机器使用的控制器为TI公司的DSP2812[5]，在CCS软件开发环境中编程。首先对DSP各个单元模块进行初始化，配置相应端口并开中断。等待中断，中断程序里对传感器进行采样，通过计算得到机器人的姿态和速度。调用控制算法，对机器人进行平衡控制，并将传感器数据传送到上位机。通过实验得到数据，绘制曲线。

实验总结：利用LQR与PID复合控制器控制算法可以使机器人获得很好的平衡效果，并且有较好的抗扰动效果，能较快的恢复动态平衡状态，有一定的鲁棒性。对于PID控制器，虽然能控制机器人平衡，但机器人在平衡处的振动较大，静态性能较差。而线性控制器LQR，在小角度范围有不错的控制效果，但对于较大扰动，角度超出线性化约束角度条件时，此控制算法已不能控制机器人的平衡。

4 结语

本文针对两轮机器人控制问题中重要的平衡控制问题进行了分析，通过分析非线性系统近似线性化控制方法和经典PID控制的优缺点，设计了基于LQR和PID的复合控制器，并通过实际物理实验验证了此控制方法的有效性，实现了两轮机器人在较大倾角范围内的动态平衡控制。

参考文献

[1] 李磊，叶涛，谭民，等.移动机器人技术研究现状与未来[J].机器人，2002，24（5）：475-480.

[2] 王晓宇.两轮自平衡机器人的研究[D]. 哈尔滨工业大学，2007.

[3] A.Salerno and J.Angeles.On the nonlinear controllability of a quasiholonomic mobile robot[C]//in Proc. IEEE ICRA，Taiwan，2003：3379-3384.

[4] Kong Xiangxuan.Research on the Control System of Two-Wheeled Self-Erect Mobile Robot[D].Shanghai Jiao Tong University，2007.

[5] 苏奎峰，吕强，耿庆峰，等.TMS320F28

12原理与开发[M].电子工业出版社，2005.

摘 要：两轮自平衡移动机器人是一种高阶次、不稳定、非线性的典型控制系统。以其为研究对象，采用Lagrange方程建立其动力学模型，经过线性化处理得到其一定约束条件下的线性化模型。采用线性二次型调节器与PID控制相结合的方法可有效克服线性化过程中约束条件对系统的影响，并且以数字信号处理器芯片TMS320LF2812为控制器核心，实现了两轮机器人较大倾角范围的动态平衡控制。物理实验表明：使用LQR与PID复合控制器对两轮机器人实体控制的有效性。

关键词：两轮自平衡机器人 Lagrange方程 LQR PID DSP2812

中图分类号：TP242 文献标识码：A 文章编号：1672-3791（2014）04（a）-0002-03

基于倒立摆模型的两轮自平衡机器人属于轮式机器人的范畴，并结合了自主移动的思想，其体积小、结构简单、运动灵活，适于在狭小和危险的空间内工作，在民用和军事上有着广泛的应用前景；同时由于其不稳定的动态特性，两轮自平衡机器人成为验证各种控制算法的理想平台，具有重要的理论意义。两轮自平衡机器人属于非线性、时变、欠驱动、非完整约束系统，控制问题是其研究的关键[1]。

国内外研究学者对移动轮式倒立摆模型及对两轮行走平衡控制技术进行了大量的研究，也提出了一些将此非线性系统线性化的方法。其中很多研究人员用近似线性化方法将机器人非线性模型线性化，再利用现代控制理论中极点配置或LQR等控制方法进行分析研究，仿真分析能获得很好的效果。但此线性化方法是假设两轮机器人俯仰角在一个小范围之类进行的，但实际中，机器人能控角度范围远大于此，采用此种方法在平衡点附近有很好效果，但当大于一定角度后系统失去控制。

本文主要介绍两轮自平衡机器人的结构设计，采用Lagrange方法建立数学模型，提出了将LQR与PID控制相结合的方法对两轮机器人进行姿态控制，物理实验验证了此方法不仅有很好的控制效果，而且实现了两轮机器人大倾角范围的平衡控制。

1 两轮自平衡机器人的动力学模型

1.1 两轮自平衡机器人的系统结构

两轮自平衡机器人系统的机械结构采用层状结构。底层有两个同型号、同轴的直流电机、姿态检测传感器、伺服驱动器和电源。在中间层有电源监控和转换模块和控制器模块。上层是机器人头部，可用来放置机器人视觉传感器以及将来扩展功能的部件。

两轮机器人的控制核心是TMS320F28

12DSP处理器，其AD口转换机器人上的姿态传感器的检测信号确定机器人姿态，并用QEP单元采集电机编码器上信号确定机器人速度和位移，通过设计的控制器计算得到电机电压，实现机器人平衡控制。

1.2 两轮自平衡机器人的动力学建模

由式可知，经过转换将原多输入系统分成两个单输入子系统，控制机器人的位移与俯仰角度，控制机器人的偏航角。面对两轮机器人的平衡控制，我们现在只需考虑机器人的俯仰角度和位移，因此将角度、角速度、位移及速度选为状态量，写出如下状态方程：

根据系统能控能观性原理在Matlab中进行计算，得出此系统能控能观。

2 基于LQR和PID的控制器设计

2.1 两轮机器人可控范围分析

由模型分析可知此机器人系统是可控的，但由于模型是在平衡点附近进行的线性化，角度为，而实际机器人倾斜角度比此值大得多，并且使用的倾角仪线性测量范围为。通过物理实验得：机器人倾斜的角度时，给电机最大控制量指令情况下，机器人实体完全可以控制。因此，设计的两轮机器人其平衡控制范围定为，远大于线性化时约束条件。本文所研究的LQR和PID复合控制器的控制算法很有研究意义[3]。

2.2 控制器设计

2.2.1 LQR控制器

对上面所描述的机器人模型，给定对于状态和控制的二次型性能指标函数定义为：

，

其中，权值矩阵和均为对称正定常值矩阵。其系统控制方法为找到最佳控制向量矩阵K，得到最佳控制量。这里选取Q=[100 0 0 0；0 100 0 0；0 0 100 0；0 0 0 100]，R=1。利用Matlab命令LQR（A，B，Q，R），得到系统反馈增益：

当给定初始俯仰倾角分别为和，其他状态量给零时，其零输入相应仿真曲线如图1和图2。

比较可知，初始角度相差很大的情况下，对于理论仿真来说，其变化情况并不大。但是在实际物理系统调试中，情况却不同。当倾斜角度增大时，其线性化过程中产生的误差也相应的增大，将会出现所设计的控制器就根本无法是机器人达到动态平衡的情况，而且对于LQR控制器的设计，模型的精确程度及物理系统参数准确性都要求相对严格。

2.2.2 PID控制器

对于经典PID控制，它是工业控制中的主要技术之一。在两轮机器人控制上，其主要优点是结构简单在控制器上易于实现，并且适应性强，不依赖于其线性化模型，兼顾了一定的鲁棒性。但由于经典的PID调节器为实现无差调节而引入积分作用后，不可避免地使系统的调节过程发生超调。适度的超调对于提高系统的快速性是有利的，但在机器人实体控制过程中，这样的超调也会使系统发生振荡，静态性能指标较差。如图所示，阶跃相应仿真曲线见图3。

2.2.3 LQR与PID复合控制器设计

以上分析了LQR与PID控制器在两轮机器人控制中的优缺点，对于两轮机器人来说，由于其非线性和不确定性，使用任何其中一种控制器进行控制都不能达到理想的效果。为了得到更快的控制速度和更好的动态平衡效果，根据两控制器各自的特点，本文设计了复合控制器：分别采用一个控制因子来限制两个控制器的输出，再将两控制量进行叠加。

假设两控制器输出分别为U1和U2，控制因子为K1和K2。考虑到，当倾斜角≤时，机器人接进平衡状态，系统状态方程的线性化误差较小，此时采用LQR控制效果较好，不需采用PID控制方法。当倾斜角≤≤时，机器人开始偏离平衡点附近，模型线性化会产生一定误差，控制器参数也存在误差，仅靠LQR控制已不能完全满足要求平衡控制要求，此时引入PID控制，使其各发挥一定的功能。当倾斜角≤≤时，机器人已经偏离平衡点很远，线性化的模型有较大的误差，其LQR控制器参数已不符合实际控制要求，此时只调用PID控制算法，使车体倾斜角度回到小于范围[4]。

3 两轮机器人实物控制效果

实验用的两轮机器使用的控制器为TI公司的DSP2812[5]，在CCS软件开发环境中编程。首先对DSP各个单元模块进行初始化，配置相应端口并开中断。等待中断，中断程序里对传感器进行采样，通过计算得到机器人的姿态和速度。调用控制算法，对机器人进行平衡控制，并将传感器数据传送到上位机。通过实验得到数据，绘制曲线。

实验总结：利用LQR与PID复合控制器控制算法可以使机器人获得很好的平衡效果，并且有较好的抗扰动效果，能较快的恢复动态平衡状态，有一定的鲁棒性。对于PID控制器，虽然能控制机器人平衡，但机器人在平衡处的振动较大，静态性能较差。而线性控制器LQR，在小角度范围有不错的控制效果，但对于较大扰动，角度超出线性化约束角度条件时，此控制算法已不能控制机器人的平衡。

4 结语

本文针对两轮机器人控制问题中重要的平衡控制问题进行了分析，通过分析非线性系统近似线性化控制方法和经典PID控制的优缺点，设计了基于LQR和PID的复合控制器，并通过实际物理实验验证了此控制方法的有效性，实现了两轮机器人在较大倾角范围内的动态平衡控制。

参考文献

[1] 李磊，叶涛，谭民，等.移动机器人技术研究现状与未来[J].机器人，2002，24（5）：475-480.

[2] 王晓宇.两轮自平衡机器人的研究[D]. 哈尔滨工业大学，2007.

[3] A.Salerno and J.Angeles.On the nonlinear controllability of a quasiholonomic mobile robot[C]//in Proc. IEEE ICRA，Taiwan，2003：3379-3384.

[4] Kong Xiangxuan.Research on the Control System of Two-Wheeled Self-Erect Mobile Robot[D].Shanghai Jiao Tong University，2007.

[5] 苏奎峰，吕强，耿庆峰，等.TMS320F28

12原理与开发[M].电子工业出版社，2005.

摘 要：两轮自平衡移动机器人是一种高阶次、不稳定、非线性的典型控制系统。以其为研究对象，采用Lagrange方程建立其动力学模型，经过线性化处理得到其一定约束条件下的线性化模型。采用线性二次型调节器与PID控制相结合的方法可有效克服线性化过程中约束条件对系统的影响，并且以数字信号处理器芯片TMS320LF2812为控制器核心，实现了两轮机器人较大倾角范围的动态平衡控制。物理实验表明：使用LQR与PID复合控制器对两轮机器人实体控制的有效性。

关键词：两轮自平衡机器人 Lagrange方程 LQR PID DSP2812

中图分类号：TP242 文献标识码：A 文章编号：1672-3791（2014）04（a）-0002-03

基于倒立摆模型的两轮自平衡机器人属于轮式机器人的范畴，并结合了自主移动的思想，其体积小、结构简单、运动灵活，适于在狭小和危险的空间内工作，在民用和军事上有着广泛的应用前景；同时由于其不稳定的动态特性，两轮自平衡机器人成为验证各种控制算法的理想平台，具有重要的理论意义。两轮自平衡机器人属于非线性、时变、欠驱动、非完整约束系统，控制问题是其研究的关键[1]。

国内外研究学者对移动轮式倒立摆模型及对两轮行走平衡控制技术进行了大量的研究，也提出了一些将此非线性系统线性化的方法。其中很多研究人员用近似线性化方法将机器人非线性模型线性化，再利用现代控制理论中极点配置或LQR等控制方法进行分析研究，仿真分析能获得很好的效果。但此线性化方法是假设两轮机器人俯仰角在一个小范围之类进行的，但实际中，机器人能控角度范围远大于此，采用此种方法在平衡点附近有很好效果，但当大于一定角度后系统失去控制。

本文主要介绍两轮自平衡机器人的结构设计，采用Lagrange方法建立数学模型，提出了将LQR与PID控制相结合的方法对两轮机器人进行姿态控制，物理实验验证了此方法不仅有很好的控制效果，而且实现了两轮机器人大倾角范围的平衡控制。

1 两轮自平衡机器人的动力学模型

1.1 两轮自平衡机器人的系统结构

两轮自平衡机器人系统的机械结构采用层状结构。底层有两个同型号、同轴的直流电机、姿态检测传感器、伺服驱动器和电源。在中间层有电源监控和转换模块和控制器模块。上层是机器人头部，可用来放置机器人视觉传感器以及将来扩展功能的部件。

两轮机器人的控制核心是TMS320F28

12DSP处理器，其AD口转换机器人上的姿态传感器的检测信号确定机器人姿态，并用QEP单元采集电机编码器上信号确定机器人速度和位移，通过设计的控制器计算得到电机电压，实现机器人平衡控制。

1.2 两轮自平衡机器人的动力学建模

由式可知，经过转换将原多输入系统分成两个单输入子系统，控制机器人的位移与俯仰角度，控制机器人的偏航角。面对两轮机器人的平衡控制，我们现在只需考虑机器人的俯仰角度和位移，因此将角度、角速度、位移及速度选为状态量，写出如下状态方程：

根据系统能控能观性原理在Matlab中进行计算，得出此系统能控能观。

2 基于LQR和PID的控制器设计

2.1 两轮机器人可控范围分析

由模型分析可知此机器人系统是可控的，但由于模型是在平衡点附近进行的线性化，角度为，而实际机器人倾斜角度比此值大得多，并且使用的倾角仪线性测量范围为。通过物理实验得：机器人倾斜的角度时，给电机最大控制量指令情况下，机器人实体完全可以控制。因此，设计的两轮机器人其平衡控制范围定为，远大于线性化时约束条件。本文所研究的LQR和PID复合控制器的控制算法很有研究意义[3]。

2.2 控制器设计

2.2.1 LQR控制器

对上面所描述的机器人模型，给定对于状态和控制的二次型性能指标函数定义为：

，

其中，权值矩阵和均为对称正定常值矩阵。其系统控制方法为找到最佳控制向量矩阵K，得到最佳控制量。这里选取Q=[100 0 0 0；0 100 0 0；0 0 100 0；0 0 0 100]，R=1。利用Matlab命令LQR（A，B，Q，R），得到系统反馈增益：

当给定初始俯仰倾角分别为和，其他状态量给零时，其零输入相应仿真曲线如图1和图2。

比较可知，初始角度相差很大的情况下，对于理论仿真来说，其变化情况并不大。但是在实际物理系统调试中，情况却不同。当倾斜角度增大时，其线性化过程中产生的误差也相应的增大，将会出现所设计的控制器就根本无法是机器人达到动态平衡的情况，而且对于LQR控制器的设计，模型的精确程度及物理系统参数准确性都要求相对严格。

2.2.2 PID控制器

对于经典PID控制，它是工业控制中的主要技术之一。在两轮机器人控制上，其主要优点是结构简单在控制器上易于实现，并且适应性强，不依赖于其线性化模型，兼顾了一定的鲁棒性。但由于经典的PID调节器为实现无差调节而引入积分作用后，不可避免地使系统的调节过程发生超调。适度的超调对于提高系统的快速性是有利的，但在机器人实体控制过程中，这样的超调也会使系统发生振荡，静态性能指标较差。如图所示，阶跃相应仿真曲线见图3。

2.2.3 LQR与PID复合控制器设计

以上分析了LQR与PID控制器在两轮机器人控制中的优缺点，对于两轮机器人来说，由于其非线性和不确定性，使用任何其中一种控制器进行控制都不能达到理想的效果。为了得到更快的控制速度和更好的动态平衡效果，根据两控制器各自的特点，本文设计了复合控制器：分别采用一个控制因子来限制两个控制器的输出，再将两控制量进行叠加。

假设两控制器输出分别为U1和U2，控制因子为K1和K2。考虑到，当倾斜角≤时，机器人接进平衡状态，系统状态方程的线性化误差较小，此时采用LQR控制效果较好，不需采用PID控制方法。当倾斜角≤≤时，机器人开始偏离平衡点附近，模型线性化会产生一定误差，控制器参数也存在误差，仅靠LQR控制已不能完全满足要求平衡控制要求，此时引入PID控制，使其各发挥一定的功能。当倾斜角≤≤时，机器人已经偏离平衡点很远，线性化的模型有较大的误差，其LQR控制器参数已不符合实际控制要求，此时只调用PID控制算法，使车体倾斜角度回到小于范围[4]。

3 两轮机器人实物控制效果

实验用的两轮机器使用的控制器为TI公司的DSP2812[5]，在CCS软件开发环境中编程。首先对DSP各个单元模块进行初始化，配置相应端口并开中断。等待中断，中断程序里对传感器进行采样，通过计算得到机器人的姿态和速度。调用控制算法，对机器人进行平衡控制，并将传感器数据传送到上位机。通过实验得到数据，绘制曲线。

实验总结：利用LQR与PID复合控制器控制算法可以使机器人获得很好的平衡效果，并且有较好的抗扰动效果，能较快的恢复动态平衡状态，有一定的鲁棒性。对于PID控制器，虽然能控制机器人平衡，但机器人在平衡处的振动较大，静态性能较差。而线性控制器LQR，在小角度范围有不错的控制效果，但对于较大扰动，角度超出线性化约束角度条件时，此控制算法已不能控制机器人的平衡。

4 结语

本文针对两轮机器人控制问题中重要的平衡控制问题进行了分析，通过分析非线性系统近似线性化控制方法和经典PID控制的优缺点，设计了基于LQR和PID的复合控制器，并通过实际物理实验验证了此控制方法的有效性，实现了两轮机器人在较大倾角范围内的动态平衡控制。

参考文献

[1] 李磊，叶涛，谭民，等.移动机器人技术研究现状与未来[J].机器人，2002，24（5）：475-480.

[2] 王晓宇.两轮自平衡机器人的研究[D]. 哈尔滨工业大学，2007.

[3] A.Salerno and J.Angeles.On the nonlinear controllability of a quasiholonomic mobile robot[C]//in Proc. IEEE ICRA，Taiwan，2003：3379-3384.

[4] Kong Xiangxuan.Research on the Control System of Two-Wheeled Self-Erect Mobile Robot[D].Shanghai Jiao Tong University，2007.

[5] 苏奎峰，吕强，耿庆峰，等.TMS320F28

12原理与开发[M].电子工业出版社，2005.