一种机器人倒立摆实验系统的研究

李璐君，韩 冰，张华良，刘广辉，李庆鑫

1(中国科学院 网络化控制系统重点实验室，中国科学院 沈阳自动化研究所机器人学国家重点实验室， 沈阳 110016)

2(中国科学院 机器人与智能制造创新研究院，沈阳 110169)

3(中国科学院大学， 北京 100049)

4(东北大学 机器人科学与工程学院， 沈阳 110819)

E-mail：hanbing@sia.cn

1 引 言

机器人教学系统是工程实验教学的平台，人们应以教育理念为指导，以培养学生分析能力、创造能力和实践能力为目的，对机器人进行设计和开发，使其具有教学适用性、开放性、可扩展性和友好的人机交互等特性[1].目前，我国高校机器人专业教育尚处于初步发展阶段.机器人教学系统的研究对促进机器人教育的发展具有重要意义.

当前，机器人教学课程所采用的大多数工业机器人的控制系统是封闭的，无法写入底层控制算法.这就限制了算法在机器人实际应用上的实验与验证.而只在计算机中对控制算法进行仿真并不能有力地证明机器人控制算法的实际性能以及发现算法在实际应用中的问题.因此，建立起一个开放式机器人学算法验证的教学系统迫在眉睫.

本文开发出了一种采用工业机器人硬件本体，并基于Matlab/Simulink软件建模和自动代码生成，进行开放式机器人学算法验证的教学系统，最后以机器人倒立摆的实现为例，介绍了教学系统的具体应用.

2 实验平台总体组成

实验平台的总体组成包括机器人倒立摆、机器人控制器、PC机，如图1(a)所示.实验系统可分成两个控制部分，即倒立摆控制部分和机器人控制部分，两部分共同组成一个闭环控制系统.PC机主要用于实验算法开发，并通过以太网将算法远程部署到机器人控制器对机器人进行实时控制，并在运行期间承担非实时数据通信.倒立摆的机械结构如图1(b)所示.包括编码器、摆杆、连接件、旋转轴等.摆杆绕着旋转轴转动，编码器检测倒立摆摆杆绕旋转轴偏移的角度，机器人法兰带动连接件进行水平移动，实现一阶倒立摆的控制.

图1 实验平台构造图

3 教学系统软件架构

本系统是基于windows系统下Matlab2016a/Simulink软件建模和自动代码生成所建立的机器人系统快速开发平台，允许科研人员和学生直接在MATLAB2016a及以上版本的Simulink 仿真环境下进行算法开发与验证工作.本系统不要求学生对整个机器人控制系统有深入的认识，让学生将精力集中于算法开发上，如倒立摆控制算法、机器人正逆解算法、插补算法等.

3.1 实验系统开发接口

本实验系统将机器人控制系统分为四层，最上层为应用层，第二层为运动规划层、第三层为控制层、第四层为驱动层.运动规划层与控制层提供了大量的二次开发接口，如EtherCAT通信接口、机器人模型接口、插补规划接口等.并支持学生对开发接口进行重写.

EtherCAT总线接口约有100多个，支持多条总线同时使用，每条总线又支持多种设备，如：数字io,模拟io,附加轴等.可利用一个总线管理带有数字io,模拟io,多组附加轴的双臂机器人甚至多机器人系统.本系统实现的EtherCAT接口将被管理的EtherCAT设备分为四个层次，并通过字符名称对不同层级的设备进行管理.我们利用相应接口并指定设备名称来创建一个EtherCAT设备，系统会自动对io、机器人等子设备进行分类并命名，方便底层对EtherCAT从站设备进行数据操作.

机器人模型接口包括管理接口、运动学模型接口及动力学模型接口.管理接口主要完成配置文件数据的解析和模型创建工作，即利用配置文件中配置的模型数据创建模型，可用于运动学与动力学模型求解.管理接口可管理多个模型数据，通过不同的名称即可实现对不同模型的操作，创建的模型数据全局有效.运动学模型接口包括正运动学接口和逆运动学接口，分别用于机器人正运动学和逆运动学的求解.模型可以用通用的数据解法或解析解法，完成关节角度与末端位置和姿态的相互转换.动力学模型接口包括正动力学接口和逆动力学接口.分别用于机器人正动力学和逆动力学的求解.正动力学模型可用作被控对象的数字模型，可以用于算法的仿真验证.而逆运动学模型一般可用于控制算法中力矩的前馈.在基于模型的控制算法中一般需要获取机器人的惯性矩阵、奥利科利矩阵，重力矩、摩擦力矩、外部力矩.所以动力学接口中还包括这部分数据的获取.

由于机器人的驱动装置提供的功率仅能保证关节的速度、加/减速度在一定的范围.因此，机器人某个关节从一个位置到另一个位置并不是简单的给定目标为位置就可以的，而是需要在当前位置与目标位置之间分割若干小段，以保证机器人关节的运动速度、加/减速度不超过最大限制，也就是对机器人的运动进行插补和规划.机器人基础规划插补接口能够获取机器人关节在每个规划时刻的规划位置、速度、加速度、加加速度，对机器人的运动进行基本的规划与插补.

3.2 实验系统接口模块

为了学生能够在MATLAB/Simulink下进行算法开发，如图2(a)所示，本系统提供了相对应的接口模块，并以机器人倒立摆控制实验为例，进行控制算法的开发与验证.

图2 接口模块与实验搭建图

利用上述提供的开发接口，我们搭建了如图2(b)所示MATLAB/Simulink实验工程.其中包括倒立摆算法模块、机器人控制模块、3维模型显示模块、安全模块等.四个开关分别控制仿真切换、倒立摆初始化、倒立摆控制的启停、移动控制.实验学生可根据需要对倒立摆算法模块和机器人控制模块进行重写，从而验证自己的控制算法.

4 机器人倒立摆控制实验

单纯的倒立摆控制算法这些年已经发展得十分成熟.但是，用六自由度机器人实现稳定控制倒立摆的研究还很少有人涉及.机器人倒立摆控制结合了动力学、机器人学、数据插补、控制工程等学科，在机器人教学方面有着重要意义.同时，该实验也对机器人系统的稳定性和平衡性研究有积极作用.

本文以机器人倒立摆的控制实验为例，介绍了开放式机器人教学系统的应用.该实验的目的，是通过实现最优控制算法在机器人倒立摆实验中的实际应用，培养学生设计、开发和验证机器人控制算法的能力，证明本文所提出的开放式机器人教学系统具有教学适用性、开放性、可扩展性和友好的人机交互等特性.

图3 机器人倒立摆控制系统结构框图

需要指出的是，机器人控制一般需要较高的控制频率，而倒立摆的控制频率可以低一些.鉴于倒立摆编码器的分辨率较低，倒立摆也没有必要采用更高的控制频率.因此，倒立摆与机器人可以采用不同的控制频率来保证较好的控制效果.由于机器人采用了比倒立摆更高的控制频率，所以需要对倒立摆的输出控制量进行插补再驱动机器人.

4.1 倒立摆控制算法实现

4.1.1 单级倒立摆的数学模型

倒立摆系统是一个典型的不稳定、多变量和非线性的被控对象,其控制算法可应用于军工、航天、行走机器人等领域[2].因此，对倒立摆系统的算法研究具有深远意义.PID控制简单、稳定性好，但对于倒立摆这样非线性、多变量、强耦合及参数不确定的控制系统，控制效果不是很理想[3-6].

线性二次型调节器(Linear Quadratic Regulator,LQR)是基于状态空间方法设计的动态控制器，其性能指标易于分析、处理和计算，便于工程实现.被广泛应用于机器人控制[7]、导弹控制[8]等工程领域.

单级倒立摆系统可抽象为小车与摆杆构成的系统，设小车质量为M，摆杆质量为m，摆杆质心位置为l，小车移动位移为x，摆杆的偏角为θ，小车上的外力为F.

根据牛顿力学原理，可得运动方程为：

(1)

(2)

式(2)中，y为输出矩阵；μ为控制输入矩阵.

4.1.2 单级倒立摆的数学模型

对于倒立摆系统的状态方程，可以通过最优控制率得出

μ=-Kz

(3)

(4)

式(4)中，Q,R为加权矩阵，是用来平衡状态变量和输入向量的权重，它们决定了系统误差与控制能量消耗的相对重要性.

由最优控制理论可知，使线性二次型最优控制指标J取得最小值的线性最优反馈增益矩阵为：

K=R-1BTP

(5)

则

μ(t)=-R-1BTP(t)x(t)

(6)

其中，P为黎卡提方程的解.P可由黎卡提方程P(t)A+ATP(t)-P(t)BR-1BTP(t)+Q=0解得[10].

我们使用MATLAB中的求解语句lqr()来求取最优反馈增益矩阵：

[K,P]=lqr(A,B,Q,R)

在MATLAB中求解lqr(A,B,Q,R)需要选择参数Q，R，其中Q1,1代表小车位置的权重，Q3,3代表摆杆角度的权重.

我们通过反馈的控制效果的好坏不断改变参数Q,R的取值，最终确定了Q,R的取值，并通过MATLAB求得K值.

4.2 机器人目标位置解算

机器人倒立摆控制系统中机器人主要负责带动倒立摆水平移动，准确执行倒立摆系统的期望输入，相当于一个线性伺服装置.由于机器人具有更高的采样频率，需要对倒立摆的期望输入进行插补.同时因为机器人采用位置控制，只能获得机器人末端法兰盘移动速度，需要得到倒立摆期望的控制输入和机器人末端移动速度之间的换算关系并对末端速度进行积分以获得位置偏移量.该位置偏移量为机器人末端位移，为了驱动机器人运动还需要利用机器人逆运动学方程求得关节上的位置偏移量.机器人控制流程图如图4所示.

图4 机器人控制流程图

为了保证机器人运动的平滑化，需要对倒立摆的期望输入进行插补，并依次执行插补点.由于倒立摆的期望输入为施加在倒立摆上的控制力，保证了该控制力的连续即可保证机器人末端运动轨迹的加速度连续.倒立摆控制周期与机器人控制周期的关系示意图，如图5所示.

图5 倒立摆控制周期与机器人控制周期关系

考虑机器人运动的平滑性要求，本系统采用了五次多项式对倒立摆的期望输入μ进行插补.机器人的控制量是机器人末端法兰盘的移动速度.经过多次试验与验证，我们发现机器人法兰盘的移动速度v和倒立摆的输入力矩μ之间成正比关系，其比例系数为1.对倒立摆的期望输入进行插补即可转换成对机器人末端法兰盘的速度进行插补.五次多项式表达式为：

v(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(7)

(8)

对插补的控制量进行积分，获取机器人期望的末端位移，从而获得期望的末端位置.再利用机器人的逆运动学方程便可获取机器人目标关节位置.进而实现机器人以倒立摆期望的控制力驱动倒立摆.

5 实验测试

本实现系统的实验过程如下：

1)建立MATLAB/Simulink 实验工程，并拖拽相关模块搭建完整的机器人倒立摆闭环控制系统.

2)通过相关接口分别设置倒立摆和机器人的控制周期.

3)在倒立摆控制模块中实现倒立摆控制算法，或直接调用系统提供的底层API接口.如LQR、PID等.

4)在机器人模块中实现目标位置的差补与目标位姿的逆解，或直接调用系统提供的底层API接口.

5)数据采集与分析，确定倒立摆或机器人的控制算法性能.

图6 倒立摆运行数据

如图6所示，在1s时加入外部扰动，机器人以倒立摆期望的控制力矩驱动倒立摆，在5s左右使倒立摆恢复平衡，回到初始位置.

机器人倒立摆控制效果图如图7所示.图7(a)是在稳定状态下的机器人倒立摆系统.图7(b)时对倒立摆施加一个推力进行干扰.倒立摆受到干扰后偏离平衡位置，如图7(c)所示.随后，机器人通过控制法兰盘水平移动将倒立摆重新移动到平衡点处，如图7(d)所示.到图7(e)时，机器人倒立摆系统已经重新恢复到稳定状态.

图7 倒立摆控制效果图

实验证明，机器人倒立摆系统具有一定的抗干扰性.受到扰动偏离平衡点时能够稳定的收敛回平衡点.该系统验证了最优控制算法用于机器人倒立摆控制系统的可行性，，从而证明，本文所提出的开放式机器人教学系统能够帮助学生在实际应用层面进行算法验证与改进，提高学生的开发能力与动手能力.

6 总 结

本文介绍了一种可用于机器人算法验证的开放式教学系统.并以机器人倒立摆实验为例，对该教学系统进行了具体应用.相比于传统的封闭式工业机器人控制系统，开放式的机器人控制系统能够帮助学生将所学到的专业知识应用到具体的工程项目中，具有更广泛的应用空间.在该系统的帮助下，学生可以通过理论与实践结合的实验课程提高自己的专业素质与实际能力.