基于动态引擎的控制理论3D仿真实验平台设计*

张梅 王飞活 何婉红 高红霞



基于动态引擎的控制理论3D仿真实验平台设计*

张梅 王飞活 何婉红 高红霞

（华南理工大学自动化科学与工程学院）

为更好地理解和验证控制理论知识，采用开源物理引擎和Visual Studio 2005设计并实现倒立摆3D虚拟仿真实验平台。该平台既可通过设置PID参数或配置极点等方法验证控制理论，也可利用Matlab或VC++自行编写其他控制算法，并生成相应的动态链接库以供平台调用和测试。实验表明，该平台能够提供一个虚拟直观、操作性强、兼容性和开放性好的控制系统实验环境。

倒立摆；开源物理引擎；Matlab；虚拟仿真实验平台

0　引言

倒立摆系统是一种典型的高阶次、多变量、强耦合且绝对不稳定的非线性系统，对其控制的效果可以非常直观地体现控制理论的有效性和优缺点[1]，是验证各种控制理论的理想实验平台。本文结合开源物理引擎（open dynamic engine，ODE[2-3]）的3D图形技术和控制系统的仿真技术，利用Matlab与VC++混合编程，设计一款可以脱离Matlab运行环境的倒立摆3D仿真控制实验平台。该平台具有良好人机界面，操作性强、灵活开放，兼备了实体实验的直观显示倒立摆运行状况、降低实验成本和不受时间地点等因素制约等优点。

1　开发环境与倒立摆系统介绍

1.1基于Visual Studio 2005和ODE的动力学仿真

本文采用ODE和Visual Studio 2005进行倒立摆仿真实验平台的开发。ODE是一个可用于模拟关节连接的刚体动力学库，它有内建的碰撞检测系统，可快速灵活地模拟地面上的车辆、动物和虚拟环境中的可移动物体。

ODE使用Body、Geom和Joint 3个基本元素描述刚体，分别表示刚体本体、几何形状和刚体间的连接关系。其中刚体本体Body的属性包括质心位置、线速度、角速度、质量、质心和转动惯量等。1组Body构成1个刚体世界（World）。Geom表示刚体的几何形状，包括球、圆柱、长方形、平面、三角形网格等复杂形状。1组Geom形成1个碰撞空间（Space）。将若干个简单的Geom以一定Joint（2个刚体间的关节，表示刚体间的约束关系）进行连接，从而形成控制对象。1组Joint形成1个Joint群（Group）。最终，在一个虚拟空间中，可利用以上3要素构建倒立摆系统和它所在的物理世界。

1.2Matlab与VC++混合编程

Matlab是一款强大的数学分析软件，在数值计算、矩阵分析以及非线性系统建模与仿真等方面具有优势，但程序执行速度慢。VC++ 2005开发环境强大，程序执行速度快，具有友好的编程界面，但相对而言，其科学计算能力、图形显示功能不强。因此，本文结合这两款开发工具的优势，先利用Matlab编写例如求解状态反馈矩阵等数学计算方面的函数，生成“*.m”文件，然后通过Matlab编译器生成相应的动态链接库文件“*.dll”，以供VC调用。此方法设计的仿真控制平台可脱离Matlab环境，独立运行，操作简单、方便[4-6]。

1.3倒立摆系统的仿真问题

倒立摆系统的控制是使小车和摆杆尽快达到预期的平衡位置，同时要求它们不会有大的振荡幅度、速度和角速度，并且当倒立摆系统达到期望位置后，系统能克服一定范围的扰动而保持平衡。单级倒立摆系统示意图如图1所示。倒立摆系统的输入为小车的位移（即位置）和摆杆的倾斜角度期望值。计算机在每一个采样周期中，采集来自传感器的小车与摆杆的实际位置信号，与期望值比较后，通过控制算法得到控制量，在实际控制系统中将控制量通过数模转换驱动直流电动机实现倒立摆的实时控制。

图1单级倒立摆系统示意图

为了在ODE中将单级倒立摆系统表示出来，可将其看成由1个底盘、4个轮子和1个摆杆组成，这些物体有不同质量和物理属性，它们之间由一定的联接方式连接。

基于ODE设计单级倒立摆的仿真平台，就是将单级倒立摆实物模型分解为ODE的Geom、Body和Joint等基本元素。根据ODE的特点，所涉及的各Body、Joint等，只需定义其初始状态，运动过程中的状态变化由引擎自动计算获得。构建仿真平台的关键是Body和Joint在World中的构建和状态设置。

2　基于ODE的倒立摆仿真平台设计

基于ODE的倒立摆仿真平台由ODE引擎、倒立摆本体、3D显示模块和控制模块组成。各部分分别负责倒立摆动力学仿真、倒立摆仿真实物（关节）等的定义和封装、3D仿真图像场景显示、外接控制器接口。ODE开发demo会提供ODE引擎和一个封装了OpenGL的3D显示函数库drawstuff。ODE引擎可完成仿真系统的动力学、碰撞检测和仿真循环等。

平台设计主要是完成倒立摆本体的模块建立，包括利用ODE定义倒立摆本体和3D显示；构建1个控制接口模块，实现倒立摆系统的控制。倒立摆本体是由摆杆、小车和车轮等Body通过Joint关节连接而成。这里使用的是合页约束Joint（即Hinge Joint）。不同的Body之间用Joint连接，使小车Body、车轮Body和摆杆Body连接成为一个整体。

仿真平台结构如图2所示，该仿真平台的控制结构如图3所示。

图2　倒立摆仿真平台结构

图3　基于ODE的倒立摆控制系统结构

倒立摆控制系统一般装有很多传感器，如：水平仪、陀螺仪、码盘和加速度传感器等，可分别用于检测摆杆倾角、角速度、电机转角和倒立摆运动的加速度，为控制系统提供反馈信号[7]。而在本仿真平台中这些传感器信息可以通过ODE引擎本身已封装好的函数进行查询Joint和Body的状态得到，例如利用dBodyGetPosition、dBodyGetLinearVel、dJointGet- HingeAngle、dJointGetHingeAngleRate等获得倒立摆系统当前状态下的小车位移、速度、摆杆的角度、角加速度等，并作为倒立摆系统的反馈量添加到控制算法中，以实现倒立摆系统的实时控制。

控制接口模块接收4个输入信息，包括小车位移、速度、摆杆的角度和角加速度。通过选择平台事先设计好的控制器（如PID控制[8]、状态反馈[9]或滑模控制[10]等）并配置相关参数后，该控制器可根据接口模块获得的小车位移、速度、摆杆的角度和角加速度等的偏差，计算输出控制器的控制量，即加在小车上的外力，从而实现倒立摆的稳定和相关性能指标的要求。

3　软件设计与实现

3.1总体思想

该仿真平台拥有2条线程：主线程和仿真线程。

主线程建立程序的主窗口，即仿真平台的界面。该线程由MFC创建，主要用于MFC的菜单绘制、界面创建与显示、消息响应、消息处理、用户数据处理和用户函数调用等。

仿真线程由ODE创建，主要用于实时计算各刚体的动力学属性和数据、计算关节点的状态、碰撞检测与仿真、绘制倒立摆的实时3D状态、调整用户视区和角度等。

2条线程共享同一个倒立摆资源，为了保证数据同步，需要对线程进行同步处理。主线程在创建、销毁、重置倒立摆、切换控制算法的过程中，倒立摆资源呈挂起状态，此时仿真线程不得对倒立摆资源进行访问。

3.2控制方法切换

所有添加的控制算法信息（包括控制算法的名称、所在DLL名称、算法函数名称、需要参数个数、需要参数名称和状态等）均保存在平台安装目录下的CtrInfo.ini文件中，每次软件启动后，会读取并解释这个文件的所有算法状态。若有被选中的控制算法，会从该算法所在的DLL中加载该算法的函数指针，通过该函数指针访问对应控制算法。若用户更改控制算法，会将旧控制算法的状态更改为“未被使用”，而新选中的控制算法状态更改为“正在使用”，并重新加载新控制算法的函数指针，替换旧的函数指针，从而实现算法的切换。

3.3ODE仿真的主要步骤[2-3]

步骤1：创建一个仿真世界dynamics World；

步骤2：在World中创建物体Bodies，设置所有物体的状态，如位置等；

步骤3：在World中创建联结Joints，将Joint与body绑定，为所有的联结设置参数；

步骤4：按需要创建一个碰撞世界Space和碰撞几何体Geom;

步骤5：创建一个联结组Joints Group管理Joints；

步骤6：Loop

1) 按需给物体施加力，

2) 按需调整Joint的参数，

3) 调用碰撞检测，

4) 为每个碰撞点collision point创建一个接触联结，并将其放入到contact Joint Group，

5) 执行一个仿真步，刷新仿真系统，

6) 移除在contact Joint Group中所有联结Joints，

步骤7：销毁动态World和碰撞世界Space。

3.4倒立摆控制函数

用全局变量contfunc、pn、an、fs分别代表控制方法的函数指针、位置噪声、角度噪声、输出力饱和标识。它们都是在主线程中赋值，在仿真线程中读取和使用。

通过contfunc调用当前的控制算法，由控制算法计算并返回控制力的值；通过判断pn、an决定是否需要施加位置和角度干扰；通过判断fs决定是否需要设置力饱和输出的值。倒立摆控制函数流程如图4所示。

3.5仿真线程函数

设置变量renderer_run、renderer_ss和renderer_pause，分别控制仿真的运行、单步和暂停。这3个参数在主线程中改变，在仿真线程中使用。通过判断其表示运行状态的数值来控制程序的运行。另外，在倒立摆的角度超过±60º时，renderer_run变成0，此时系统已经超出控制范围，仿真线程退出。仿真线程函数流程如图5所示。

图4　倒立摆控制函数流程

图5　仿真线程函数流程

3.6用户自定义算法的实现

为提高本实验平台的开放性，能够自主编写不同的控制算法，本仿真平台提供相应接口，用户可添加自定义的新控制算法对倒立摆进行控制。其编写方法有2种：1) 通过Matlab编写控制算法并生成相应的C++动态链接库供Visual Studio 2005直接调用；2) 直接用VC++编写控制算法并生成DLL文件，通过调用DLL动态链接库技术实现对用户算法的调用。

其中，VC++与Matlab的混合编程步骤[4-6]（以计算极点配置下的状态反馈矩阵的算法为例）如下：

1) 利用Matlab编译器生成“.dll”文件

①Matlab编写计算反馈增益矩阵的m文件Pole.m；

②设置Matlab编译器，在Matlab命令框中先后执行下列语句

>>mex -setup

>>mbuild -setup

③对m文件进行编译，在Matlab命令框中先后执行下列语句

>> cd “m文件所在路径”

>>mcc -W cpplib:Pole -T link:libPole.m

2) Visual Studio 2005环境设置

①从Matlab编译生成的文件中选出Pole.h、Pole.dll和Pole.lib 3个文件，并复制到Visual Studio 2005工程目录下；

②将Matlab安装文件里的“extern”文件也复制到工程目录下；

③选择菜单“Project->Property Pages”

a) 在C/C++ ->General ->Directories下添加

“....externlibwin32microsoft”“....projectproject”（project为工程文件的名字）；

b) 在Linker->General ->Library Directories中添加

“....externlibwin32microsoft”

“....externinclude”

“....projectproject”

c) 在Linker-> Input -> Dependencies中添加

“mclmcrrt.lib”

“mclmcr.lib”

“Pole.lib”

④编写Visual Studio 2005调用Matlab函数的接口程序，读取状态反馈增益矩阵。

4　倒立摆系统仿真与实验

4.1仿真平台介绍

仿真平台主要完成界面编程、倒立摆本体程序建立和控制器接口模块实现等。

本平台使用一个虚拟场景演示自动控制实验中倒立摆的控制。这个控制实验在传统的教学中需要利用复杂和昂贵的设备来完成，实验资源有限，在一定程度上影响了教学效果。而利用该基于3D游戏引擎的控制系统实验平台控制系统的设计与仿真，不仅能让学生很好地理解理论知识，而且能够使学生感受到逼真的实验效果。

基于动态引擎的控制理论3D仿真实验平台界面如图6所示。控制目的是使小车和摆杆尽快到达平衡位置。系统在利用动态引擎中的状态测取函数获得小车实时状态值后，利用事先设定好的控制算法计算出控制量，然后作用于小车，从而保证小车位置及摆杆稳定。

图6　基于动态引擎的控制理论3D仿真实验平台界面

4.2系统功能介绍

本套仿真系统作为验证控制理论相关知识的实验平台，可通过如下2种方法实现控制算法的主要功能：1) 用户可通过选择系统自带的控制方式（如PID控制）设置控制参数；2) 可自己编写控制算法，达到控制目的，如图7所示。

图7　添加自定义控制算法

4.3仿真平台测试

仿真平台测试采用极点配置控制方法，动态仿真过程中某一时刻的仿真界面截图如图8所示。

图8　极点配置控制实验

图8中上下2条响应曲线分别为摆杆角度响应曲线和小车速度响应曲线。由响应曲线和系统动态仿真模型可见，在仿真开始后添加干扰，角度和速度曲线均出现较大幅度的震荡，但是在相应的状态反馈控制器的作用下，倒立摆系统逐渐趋于稳定状态，实现相应的控制效果。

5　结语

本文采用ODE和Visual Studio 2005实现倒立摆控制的3D仿真系统的设计。该平台可提供直观的3D动态仿真效果和操作性强的实验环境，可在虚拟实验平台上对控制理论知识进行验证，同时还可自行定制更多的控制算法来加强对控制理论知识的理解和掌握，极大地激发学习的积极性和主动性，有效增强动手实践和大胆创新的能力。这是将三维动态引擎技术以及虚拟实验平台应用到控制系统理论的创新性教学中的一个尝试。

参考文献

[1] 孔凡国,刘锡林,钟廷志.平面一级倒立摆的滑模控制研究[J].机械设计与制造,2011(3):133-135.

[2] Russell Smith. Open Dynamics Engine v0.5 user guide [EB/OL]. http://ode.org, Monday May 28, 2007.

[3] 薛方正,刘成军,李楠,等.基于ODE引擎的开放式仿人机器人仿真[J].机器人,2011,33(1):84-89.

[4] 刘维.精通Matlab与C/C++混合程序设计[M].3版.北京:北京航空航天大学出版社,2012.

[5] 田红霞.基于VC与Matlab的电力电子技术软件实验平台设计[J].实验室科学,2011,14(1):116-118.

[6] 赵志强,刘志成.VC++与Matlab混合编程技术应用分析[J].实验技术与管理,2009,26(1):99-101.

[7] 杨兴明,丁学明,张培仁,等.两轮移动式倒立摆的运动控制[J].合肥工业大学学报:自然科学版,2005,28(11):1485-1488.

[8] Lal Bahadur Prasad, Barjeev Tyagi, Hari Om Gupta. Optimal control of nonlinear inverted pendulum system using PID contorller and LQR: Performance analysis without and with disturbance input[J].International Journal of Automation and Computing, 2014,11(6):661-670.

[9] 刘秀文,郭伟,余波年.倒立摆状态反馈极点配置与LQR控制Matlab实现[J].现代电子技术,2011,34(10):88-90.

[10] Manar Lashin, Ahmed Ramadan, Hossam S Abbass, et al. Design of an optimized sliding mode control for loaded double inverted pendulum with mismatched uncertainties [C] // 19th IEEE International Conference On Methods and Models in Automation and Robotics, 2014:270-275.

The Design of 3D Simulation Platform for Control Theory Based on Open Dynamic Engine

Zhang Mei Wang Feihuo He Wanhong Gao Hongxia

(College of Automatic Science and Engineering, South China University of Technology)

To make users understand and verify the control theory, a virtual simulation experimental platform of single inverted pendulum is developed based on ODE(Open Dynamic Engine) and Visual Studio 2005. Users can verify the control theory by setting the PID parameter, configuration of poles or other ways. And they can also program control algorithm based on Matlab and C++, and then generate a dynamic link library for the platform to call it, which improve the openness of the experimental platform. The realization of this platform shows that it can provide a virtual but intuitive experimental environment of control system with strong operability and good compatibility and openness for the users. So that it can consolidate the theoretical knowledge of the users and make the effects of simulation be more realistic quality.

Inverted Pendulum; ODE; Matlab; Virtual Simulation Experimental Platform

张梅，女，1977年生，博士，副教授，研究方向：控制理论与控制工程。E-mail:zhangmei@scut.edu.cn

广东省精品课程“自动控制原理精品课程建设”项目；华南理工大学教学改革项目“基于设计的现代控制理论课程改革”。