一种基于模糊PID的立方体机器人控制系统①

张舰栋 章 政 黄卫华 李 磊 陈 阳

(*武汉科技大学机器人与智能系统研究院 武汉430081)

(**武汉科技大学冶金自动化与检测技术教育部工程研究中心 武汉430081)

(***武汉科技大学信息科学与工程学院 武汉430081)

0 引言

立方体机器人是近20 年出现的一种结构和运动方式独特的新型机器人[1-3],其驱动电机、控制系统、传感器以及电源等模块均包含在立方体内部,具有全封闭[3-4]或半封闭[5-6]的立方体外壳。与传统的轮式、腿式、履带式等机器人相比,立方体机器人具有结构简单、外观新颖和组装方式灵活多变等特点,在太空探索、编队表演、未来无人机平衡控制等领域具有广阔的应用前景。

立方体机器人通过安装在立方体内部某个面的动量轮的旋转产生重心偏移,基于动量守恒完成立方体机器人的单棱边、单点自平衡或翻滚等可控运动,可以等效为一个多自由度空间动量轮倒立摆系统[7-9]。此外,受限于立方体机器人独特的外观结构和运动方式,现有的立方体机器人缺乏直接的外部环境测量传感器,导致难以获取精确的姿态数据,存在抗干扰能力较差、控制精度不高等问题。因此,动力学建模和自平衡控制一直是立方体机器人的研究重点和难点。文献[1]设计了一种金属壳和一个装有动力摆机构组成的立方体系统,采用模糊控制方法设计了一种基于Sugeno 模糊推理型智能积分模糊平衡控制器,可实现以棱边为支点的平衡。文献[3,4]设计了一种由内部3 个两两相交垂直的动量轮和驱动电机组成的立方体机器人装置,对立方体机器人以棱边和角点为支点的平衡问题做了较系统的分析,并对立方体机器人实现以角点为支点平衡的原理进行了介绍。文献[5]先采用拉格朗日方法和凯恩方法建立了立方体机器人的单边动力学模型,通过比较两种方法的建模结果和数值仿真分析,从理论上验证了所建立模型的正确性。文献[6]针对立方体机器人单点平衡,设计了自抗扰控制器实现了立方体机器人的单点平衡控制。

上述研究主要是研究立方体机器人在平面上的建模和控制,本文针对立方体机器人坡面自平衡控制系统进行研究。

考虑到立方体机器人具有非线性、欠驱动、强耦合、多变量等复杂特性,难以获取其精确的数学模型,并且受限于其特殊的自平衡方式,采用经典的控制方法存在控制精度不高、抗干扰能力较差等问题。因此,本文将模糊逻辑用于立方体机器人控制系统中,实现其在坡面自平衡控制。首先,基于自主设计的一种内部集成的单动量轮棱边平衡立方体机器人,采用拉格朗日法建立了以棱边平衡于坡面的立方体机器人动力学模型,并对立方体机器人的稳定性、能控性及能观性进行分析。然后,设计了一种基于模糊PID的角速度-角度串级控制系统。最后,仿真和实测实验验证了所设计系统的动、静态特性。

1 立方体机器人机械结构设计

在保证立方体机器人结构强度可靠性以及功能完整性的情况下,本文设计了一种单面安装动量轮内部集成的立方体机器人,由立方体本体、一面上的动量轮、直流电机、传感器、供电模块以及主控单元构成。立方体机器人结构如图1 所示。

图1 立方体机器人结构图

立方体机器人的主控单元采用32 位低功耗ARM 微控制器,动量轮直径为0.08 m,姿态观测传感器选用MPU 6050,完成对立方体姿态数据的实时采集,采用陀螺仪/加速度计互补滤波得到立方体机器人倾角最优估计。立方体机器人的主要参数以及数据如表1 所示。

表1 立方体机器人的主要参数表

2 立方体机器人坡面平衡建模

立方体机器人以一条棱边为支点在斜面上的动态平衡等效模型可视作为一阶倒立摆模型。为了便于简化立方体机器人建模与控制器设计,本文对立方体机器人做出如下假设。

(1) 立方体机器人以棱边为支点的所在边与接触面无滑动。

(2) 立方体机器人视为刚体且质量均匀对称。

(3) 忽略空气阻力以及各种摩擦力。

立方体机器人以一条单棱边为支点在坡面上平衡时,由电机驱动动量轮产生回复力实现平衡控制,因此立方体机器人的系统模型可简化为其平面二维模型,如图2 所示。立方体机器人位于倾角为γ的坡面上,其中M为立方体框架及电池的总质量,其重心G的位置坐标为(x1,y1)。立方体机器人与坡面接触点记为点A,点A正下方与水平面接触点记为坐标系原点O,立方体绕点A进行转动,转动惯量为I1,m为动量轮的质量,其重心P的位置坐标为(x2,y2),动量轮转过的角度为β,转动惯量为I2。立方体机器人相对于纵轴的偏转角为θ,点A与立方体机器人重心G的距离为l1,与动量轮重心P的距离为l2,与坡面底点B的距离为l3。

图2 立方体机器人在坡面上平衡的坐标系定义

立方体机器人在坡面上平衡的拉格朗日方程为

式中,L为拉格朗日算子,q为系统的广义坐标,T为系统的总动能,V为系统的总势能。

拉格朗日方程由广义坐标qi以及拉格朗日算子L所表示为

式中,i=1,2,3…n;Qi为系统沿广义坐标方向的外力,设立方体机器人的2 个广义坐标分别是θ和β。

立方体机器人在坡面上的总动能为

其中,立方体机器人机体的动能为

动量轮的动能为

则立方体机器人在坡面上平衡的总动能为

立方体机器人在坡面上的总势能为

其中,立方体机器人机体的势能为

动量轮的势能为

则立方体机器人在坡面上平衡的总势能为

由此可得,立方体机器人在坡面上平衡的拉格朗日算子为

立方体机器人在坡面上平衡的广义力矩为

式中,U为动量轮的驱动力矩。

将式(11)、(12)带入式(2),则有:

由此可得:

将式(14)代入式(2),则有:

由此可得立方体机器人在坡面上平衡的动力学模型为

为了便于分析和计算,将式(16)表示为

当立方体机器人处于平衡位置时,各个状态变量的初始值为0,即:

3 基于模糊PID的立方体机器人串级控制系统设计

稳定性是控制系统正常工作的必要条件,对于式(19)所示的立方体机器人系统,其特征根分别为λ1=0、λ2=8.1521、λ3=-8.1521,即立方体机器人系统有一个极点λ3在S平面的右半平面上,因此系统在平衡点处是不稳定的。此外,式(19)的能控矩阵和能观矩阵均为满秩,即所设计的立方体机器人在坡面上既能控且能观。因此,可以设计相应的控制器,实现其坡面自平衡调节。

立方体机器人复杂的结构和独特的自平衡方式,使其难以获取精确的动力学模型,导致采用常规的PID 控制方法存在控制精度不高、姿态不易收敛且抗干扰能力较差等问题。因此,本文将模糊逻辑与PID 相结合构成立方体机器人的角速度-角度串级控制系统,系统结构如图3 所示。

图3 立方体机器人控制系统结构图

为了及时响应立方体机器人机体角度的变化且克服不确定性扰动,选择立方体机器人的机体角度θ作为副变量构成基于PID 控制的副回路;为了弥补立方体机器人动力学模型不精确的问题和增强系统的鲁棒性,以动量轮角速度为主变量并采用模糊PID 控制方式,构成基于模糊PID的角速度-角度串级控制系统,从而使得立方体机器人系统具有良好的控制品质。

动量轮角速度输入量偏差e及偏差变化率ec通过量化因子Ke、Kec进行一次量化处理,输出量Δkp、Δki、Δkd经过解模糊化比例因子k1、k2、k3进行一次比例变换。取动量轮角速度输入量偏差e和偏差变化率ec,输出量Δkp、Δki、Δkd模糊子集为{NB,NM,NS,Z,PS,PM,PB},分别代表负大,负中,负小,零,正小,正中,正大。论域为[-3,3],量化等级为{ -3,-2,-1,0,1,2,3}。输入量偏差e和偏差变化率ec隶属函数选为双边高斯型,输出量Δkp、Δki、Δkd隶属函数选为三角形,使用重心法进行解模糊:

式中,ucen为面积中心对应的横坐标,A(ui) 为ui处的隶属度,n为相关隶属度的个数。系统的输入输出的函数隶属度曲线如图4、图5 所示。

图4 e、ec 隶属函数

图5 Δkp、Δki、Δkd 隶属函数

根据动量轮角速度环kp、ki、kd3 个参数的作用、相互之间的互连关系及对输出特性的影响,可以归纳出在不同e和ec的情况下,动量轮角速度环kp、ki、kd的自整定要求,从而可得模糊规则:

上述49 条模糊控制规则形成一个模糊控制规则矩阵,根据模糊输入量决策出模糊输出量

将Δkp、Δki、Δkd代入式(21),求出系统最终的控制参数kp、ki、kd。

其中,kp0、ki0、kd0为PID 初始值。

4 实验与结果分析

4.1 对比仿真实验

设立方体机器人的初始状态为(5,0,0)T,本文所设计的基于模糊PID的立方体机器人串级控制系统和基于普通PID的立方体机器人控制系统的系统输出响应仿真曲线如图6 所示。

由图6 可知,相比于基于普通PID 控制的系统,本文所设计的基于模糊PID的串级控制系统具有调节时间短、超调小、响应速度快等特性。

图6 模糊PID 串级控制与普通PID 仿真对比实验图

4.2 稳定性实测实验及结果分析

将立方体机器人放置在倾斜角度为23 °的坡面上,如图7 所示,采样周期为5 ms。稳态时立方体机器人的机体角度跟随和动量轮角速度实测输出曲线如图8 和图9 所示。

图7 立方体机器人实物实验图

图8 立方体机器人稳态时的机体角度跟随曲线

图9 立方体机器人稳态时动量轮角速度输出曲线

由图8、图9 可知,立方体机器人在坡面上稳定时,动量轮的角速度在0 附近波动,机体角度值在设定值附近小幅度波动,立方体机器人具有良好的稳定性。

4.3 抗扰性实测实验及结果分析

立方体机器人在坡面上稳态平衡过程中,给立方体机器人一个瞬间的干扰,使其机体角度值发生变化,立方体机器人系统的机体角度跟随和动量轮角速度波形抗扰输出响应如图10 和图11 所示。

由图10 和图11 可知,对于立方体机器人坡面上稳定过程中突然出现的外部干扰,立方体机器人系统能够对扰动做出快速且有效的调整,保证立方体机器人的稳定控制。

图10 扰动情况下机体角度跟随曲线

图11 扰动情况下动量轮角速度输出曲线

4.4 鲁棒性实测实验及结果分析

立方体机器人在坡面上稳态平衡过程中,给立方体机器人的立方体框架上悬挂一个重物,改变立方体框架的质量,系统的输出响应如图12 和图13所示。

图13 不同系统参数下动量轮角速度输出曲线

由图12、图13 可知,基于模糊PID的动量轮立方体机器人控制系统,在系统参数发生变化时,仍然对立方体机器人系统的平衡控制具有良好的适应性,验证了本文控制算法对系统参数发生变化时有良好的鲁棒性。

5 结论

本文自主研制了一种以立方体棱边为支点的立方体机器人,针对其在坡面上自平衡动力学建模及控制问题,设计了基于模糊PID 控制的角速度-角度串级控制系统。首先,基于Lagrange 方程建立了立方体机器人坡面动力学模型并进行了稳定性和能控性、能观性的理论分析。在此基础上,选择立方体机器人的机体角度为副变量、动量轮角速度为主变量,构成基于模糊PID的立方体机器人串级控制系统。相比于常规的PID 单回路立方体机器人控制系统,由于增加了角度副回路以及模糊逻辑的作用,坡面自平衡立方体机器人系统可以更好地克服由于对象非线性、各种扰动等不确定因素对系统的影响,具有良好稳定性、抗干扰以及鲁棒性。