[0073] 参见图1,所示为本发明一种两轮自平衡机器人自适应滑模变结构控制方法的流程框图,包括以下步骤:
[0074] 步骤101:根据经典力学分析法和基于能量分析的Lagrange算法建模如下两轮自平衡机器人的动力学方程:
[0075]
[0076] 步骤102:并根据上述动力学方程设计出滑模变结构控制器;
[0077] 其中,动力学方程中,U为滑模变结构控制器的输出控制信号,θ为两轮自平衡机器人的车体偏角,ev=V-Vr为当前速度V和参考速度Vr的速度差,a1、b1、c1、d1、a2、b2、c2、d2为两轮自平衡机器人的模型参数,d1和d2为系统干扰。
[0078] 步骤103:采集传感信号并以此作为滑模变结构控制器的输入参数,其中,两轮自平衡机器人的当前速度信息为滑模变结构控制器的输入参数之一。
[0079] 步骤104:滑模变结构控制器根据其输入参数输出控制信号;
[0080] 步骤105:根据滑模变结构控制器输出控制信号以及电位器信号共同控制电机运动;根据滑模变结构控制器输出控制信号,对系统进行平衡以及速度控制,以及利用转向电位器信号,对系统进行转向控制;
[0081] 步骤106:检测两轮自平衡机器人的当前速度信息,并将其反馈至滑模变结构控制器的输入端,作为滑模变结构控制器的输入参数之一;
[0082] 重复步骤103至步骤106,根据传感信号参数以及反馈信号,滑模变结构控制器实时调整输出控制信号驱动两轮自平衡机器人运动。
[0083] 在上述步骤101中,对两轮自平衡机器人控制系统的数学模型在控制系统的研究中有着相当重要的地位,要提高系统的性能,首先需要知道系统的数学模型,而后才有可能对系统进行仿真处理,进而架构实际系统进行模拟测试。参见图2,所示为两轮自平衡机器人的整体机械结构框图,两轮自平衡机器人系统的机械结构主要由车体、左右两个驱动轮、驱动电机、编码器、以及传感器组成,传感器进一步包括陀螺仪、加速度计、转向电位器、速度传感器等,根据传感器信号控制驱动轮运动,机器人的运动轨迹与这两个驱动轮有关。左右两轮由各自电机独立驱动且两轮转轴轴线在同一条直线上,机器人车体可绕两轮转轴自由转动。当陀螺仪检测到车体产生倾斜时,控制系统根据测得的倾角产生一个相应的力矩,通过控制电机驱动两个车轮朝车身要倒下的方向运动,以保持两轮自平衡机器人自身的动态平衡。两轮自平衡机器人的运动主要由驱动两个轮子转动的电机产生的转动力矩所控制。
[0084] 现有技术中,仅采用经典力学分析法或基于能量分析的Lagrange方法对两轮自平衡机器人系统进行建模,本发明首先通过对两轮自平衡机器人的受力分析,然后采用基于能量分析的Lagrange方法建立两轮自平衡机器人的整体数学模型,两轮自平衡机器人整体的三维受力分析如图3-a所示,考虑到两轮自平衡机器人的运动是通过车轮转动实现,本发明技术方案采用的是一对同轴安装且参数相同的轮胎,所以左右轮的模型是一样的,故只考虑两轮自平衡机器人的二维受力情况,如图3-b所示,为了便于数学推导,将其进一步简化成如图3-c所示的形式。图3-a、图3-b和图3-c中涉及的参数说明如下表1所示。
[0085] 表1两轮自平衡机器人模型的符号说明
[0086]
[0087] 以下详细介绍本发明的两轮自平衡机器人的动力学方程推导过程,其中,本发明中所用到的其它符号说明如表2所示。
[0088] 表2两轮自平衡机器人模型的符号说明
[0089]
[0090] 首先,根据动量守恒原理,得到两轮自平衡机器人关于动量的方程式(2),根据能量守恒原理,得到两轮自平衡机器人关于能量的方程式(3)。
[0091]
[0092]
[0093] (2)式和(3)式中:P0表示两轮自平衡机器人的初始动量(Kg·m/s),E0为两轮自平衡机器人的初始能量(J),J为车体的转动惯量(Kg·m2)。
[0094] 分别对(2)式和(3)式进行求导得到关于合力F的方程(4)式和关于功率的方程(5)式:
[0095]
[0096]
[0097] 将(4)式代入(5)式,得到(6)式:
[0098]
[0099] 在控制两轮自平衡机器人的平衡及运动时,控制量是车轮的转动力矩,所以需要知道电机的输出力矩,直流电机模型中的输出力矩表达式为(7)式:
[0100]
[0101] 式(7)中Kt为电机的转矩常数(N·m·A),Ke为反电动势系数(V·s),Ua为电枢输入电压(V),w为电机输出角速度(rad/s),Ra为电枢电阻(Ω)。
[0102] 将(7)式电机模型中输出力矩表达式化成(8)式的形式:
[0103] F=CuU-CvV (8)
[0104] 其中:F=τe,
[0105] 将(8)式代入到(4)式得到(9)式:
[0106]
[0107] 由于θ和w较小,所以有(10)式:
[0108]
[0109] 定义:
[0110] ev=V-Vr (11)
[0111] 其中Vr为V的参考速度。
[0112] 结合(5)、(8)、(9)和(10)式,最终得出两轮自平衡机器人方程为:
[0113]
[0114] 其中
[0115]
[0116] 在步骤102中,根据上述动力学方程(公式12)来设计两轮自平衡机器人的滑模变结构控制器,具体过程如下:
[0117] 首先定义:
[0118] eθ=θ-θr (13)
[0119] 其中θr为θ的参考角度。
[0120] 根据两轮自平衡机器人整体的动力学方程(12)式,设计滑模变结构控制器,将滑模变量s的方程定义为(14)式:
[0121]
[0122] 其中 λ1>0,λ2>0满足Hurwitz稳定判据条件。
[0123] 采用上述技术方案设计滑模变结构控制器,将整体的滑模变量设计成速度滑模变量和角度滑模变量组成的矩阵形式,并将速度滑模变量设计成带积分形式,从而起到降低抖振的效果。
[0124] 定义李雅普诺夫能量函数为(15)式:
[0125]
[0126] 式(15)当中
[0127] 为确保整个两轮自平衡机器人系统稳定,即v的导数小于零。同时确保两轮自平衡机器人在速度和直立上同时稳定,即v1和v2的导数都小于零。
[0128] v1的导数为(16)式。
[0129]
[0130] 令
[0131]
[0132] 式(17)中常数ε1>0,表示系统的运动点趋近于切换面s=0的速率。ε1越小,趋近速度慢;ε1越大,则运动点到达切换面时具有较大的速度,引起的抖动也较大。
[0133] 在一种优选的实施方式中,式(17)中斜坡函数的表达式为(18)式。斜坡函数作为准滑动模态控制的一种方法,它的本质是在边界层外,采用切换控制,在边界层内,采用线性化反馈控制,减小了系统的抖振,从而使系统更加稳定。
[0134]
[0135] 将(17)式代入到(16)式得到(19)式。
[0136]
[0137] 式(19)表明滑动变量s1是渐进稳定的,满足李雅普诺夫稳定条件。
[0138] 由(17)式得到关于速度的变化率(20)式。
[0139]
[0140] 由于系统当中的速度和角度信号有一定的联系,在一种优选实施方式中,定义角度参考信号θr与速度V的关系式为(21)式。
[0141] θr=βV (21)
[0142] 对(21)式进行一次求导以及二次求导得到(22)式和(23)式。
[0143]
[0144]
[0145] 对角度误差(13)式进行一次求导以及二次求导得到(24)式和(25)式。
[0146]
[0147]
[0148] 令
[0149]
[0150] 将(26)式代入到(25)式中得到(27)式。
[0151]
[0152] 对(13)式方程和(24)式方程进行整理得到(28)式和(29)式。
[0153] eθ=θ-θr=θ-β(ev+Vr) (28)
[0154]
[0155] 将(20)式代入到(29)式得到(30)式。
[0156]
[0157] 式(30)中U为两轮自平衡机器人控制器的上一状态。
[0158] v2的导数为(31)式。
[0159]
[0160] 令
[0161]
[0162] 式(32)中常数ε2>0,和ε1的作用一样,β的取值必须满足(33)式。
[0163]
[0164] 结合(26)式得到β的值为(34)式。
[0165]
[0166] 由(32)式得到两轮自平衡机器人的最终滑模变结构控制器为(35)式。
[0167]
[0168] 在式(35)中,
[0169] Y=βb3,
[0170] Z=b2-βa33b3。其自适应项为 其中为拉盖尔基函数族。
[0171] 将(32)式代入到(31)式中得到(36)式。
[0172]
[0173] 式(36)表明滑动变量s2是渐进稳定的,满足李雅普诺夫稳定条件。说明式(35)中的滑模变结构控制器U在理论上是正确的。
[0174] 在一种优选实施方式中,在步骤102中,将滑模变结构控制器的输出控制信号的输出方程设定为:
[0175] 其中,ΔT为采样时间,Y=βb2,Z=b1-βc2b2, 为自适应项。
[0176] 在步骤103中,传感信号包括通过陀螺仪采集偏角信息和通过加速度计采集的加速度信息,在一种优选实施方式中,通过卡尔曼滤波算法对所述偏角信息和所述加速度信息进行数据融合。采用卡尔曼滤波算法进行数据融合主要采用以下公式,从而使系统控制更加精确。
[0177] X(k|k-1)=AX(k-1|k-1)+BU(k) (37)
[0178] P(k|k-1)=AP(k-1|k-1)A'+Q (38)
[0179] X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1)) (39)
[0180] Kg(k)=P(k|k-1)H'/(HP(k|k-1)H'+R) (40)
[0181] P(k|k)=(I-Kg(k)H)P(k|k-1) (41)
[0182] 参见图4,所示为实现上述控制方法的两轮自平衡机器人控制系统的硬件原理框图,包括电源模块、陀螺仪、加速度计、转向电位器、控制单元、第一电机驱动模块、第二电机驱动模块、第一电机、第二电机、第一编码器以及第二编码器,其他模块如按键模块、显示屏等在此不再赘述。
[0183] 在该系统中,电源模块用于整个系统提供供电电压;
[0184] 陀螺仪用于检测自平衡机器人车体偏角信息,并将该信息发送给控制单元;陀螺仪的偏角信息是重要参数,控制单元以此为基准控制输出控制信号。
[0185] 加速度计用于检测自平衡机器人的加速度信息,并将该信息发送给控制单元;
[0186] 转向电位器用于检测自平衡机器人的转向信息,并将该信息发送给控制单元;
[0187] 第一编码器和第二编码器用于检测自平衡机器人的速度信息,并将该信息发送给控制单元;第一编码器和第二编码器分别安装在第一驱动轮和第二驱动轮上,检测第一驱动轮和第二驱动轮的转速。
[0188] 控制单元根据偏角信息、加速度信息、转向信息和速度信息计算出输出控制信号,并发送给第一电机驱动模块和第二电机驱动模块;
[0189] 第一电机驱动模块和第二电机驱动模块根据上述输出控制信号输出PWM驱动信号使第一电机和第二电机转动。
[0190] 在一种优选实施方式中,参见图5,所示为两轮自平衡机器人控制系统中控制信号的示意图,控制单元进一步包括卡尔曼数据融合模块和滑模变结构控制器,滑模变结构控制器包括速度滑模变结构控制器和角度滑模变结构控制器,其中,
[0191] 卡尔曼数据融合模块用于将偏角信息和加速度信息进行数据融合,并将融合信息发送给角度滑模变结构控制器;
[0192] 角度滑模变结构控制器根据卡尔曼数据融合模块输出的融合信息和速度滑模变结构控制器的反馈信息输出控制信号;
[0193] 在一种优选实施方式中,速度滑模变结构控制器输出的反馈信息由以下反馈方程确定:
[0194] θr=βV,其中,θr为速度滑模变结构控制器反馈给角度滑模变结构控制器的反馈信息,V为当前速度,β为常量,通过选取β的值,从而使系统的速度和角度能够相互影响,当系统的倾角过大时,系统会自动降速,速度降低的同时,会自动回到平衡位置,从而保证系统的安全与稳定。
[0195] β为系统稳定的重要参数,由二轮机器人模型参数决定(表1、表2中部分参数),β值的选取通过求解方程 而得到,同时必须满足条件 所以最终本发明通过仿真的方式求得β值的范围。参见图6,所示为不
同两轮自平衡机器人模型参数下β值的仿真图,β值的范围值-0.15至0.15之间。
[0196] 在一种优选实施方式中,角度滑模变结构控制器的输出控制信号由以下输出方程确定:
[0197] 其中,ΔT为采样时间,Y=βb2,Z=b1-βc2b2, 为自适应项。
[0198] 在一种优选实施方式中,速度滑模变结构控制器根据速度信息和角度滑模变结构控制器的输出控制信号,输出反馈信息。
[0199] 在一种优选实施方式中,还包括调速手把,通过调速手把输出油门信号,并将该信号发送给控制单元。油门信号与参考速度Vr成一定比例关系,因此通过油门信号使参考速度Vr的值发生变化。本发明技术方案和现有的仅靠偏角信息控制速度方式相比,增加了一种控制方式,使得系统的速度控制方式多样化,同时增大了系统的安全系数。
[0200] 在一种优选实施方式中,还包括自适应负载检测模块,具有负载检测功能。负载检测模块采用的是迟滞函数,通过设定阈值来判断是否有负载,即当人站上去的时候指示灯会亮,阈值是根据编码器和电机的输出量来设定的。
[0201] 在一种优选实施方式中,还包括无线通信模块,与控制单元相连接,用于与上位机端进行通信,通过无线通信模块对数据进行处理和分析,来提高系统控制的精确度以及智能性。无线通信模块中的无线数据接收模块和发送模块采用芯片NRF24L01,RXF2401射频功率放大器。
[0202] 在一种优选实施方式中,控制单元采用飞思卡尔32位微控制MK60DN512ZVLQ10,速度传感器选用光电编码器,电机驱动采用BTN7971B半桥驱动芯片搭建的全桥驱动电路,电源模块采用24V、14Ah的可充电镍镉电池。陀螺仪和加速度计分别采用LPR510AL和MMA7260。
[0203] 在一种优选实施方式中,本发明的两轮自平衡机器人系统的驱动电机采用直流伺服电机,具体是EC90M485500RGOL型号的伺服电机,这是因为直流伺服电机具有优良的速度控制性能,它输出较大的转矩,直接拖动负载运行,同时它又受控制信号的直接控制进行转速调节。该直流电机的技术参数如下表3所示。
[0204] 表3 EC90M485500RGOL直流电机的技术参数
[0205]
[0206] 结合上表3,再根据UaIa=EaIa+Ia2Ra,PI=PM+PCua两个方程以及两轮自平衡机器人的固有技术参数测出系统中电机电枢两端的电阻Ra、电感La、电机时间常数Kt、粘性阻尼系数B、反电动势系数Ke、电机转子的转动惯量J,通过仪器测量出系 统中电 机电枢两端电 感La以 及机器人的重量M。最后通过以下公式
计算出两
轮自平衡机器人系统动力学方程以及滑模控制器中的参数,使得系统的控制更加精确。
[0207] 根据上述模型参数对β值进行系统仿真,参见图7,所示为在特定模型参数下β值的仿真图;从图7可以看出,β值为-0.14时,系统趋于稳定,期望的角度和速度之间的关系符合设定的值。
[0208] 为了进一步验证本发明技术方案所能达到的技术效果,在同样两轮自平衡机器人系统模型参数下,分别对本发明滑模变结构控制器和现有技术PID算法控制器进行数据仿真。参见图8-a,所示为在速度参考信号为正弦波下采用自适应滑模控制的速度跟踪仿真图,图8-b为在速度参考信号为正弦波下滑模控制速度误差仿真图,图8-c为在速度参考信号为正弦波下滑模控制角度误差仿真图,图8-d为在速度参考信号为正弦波下滑模控制器的输出量,从仿真图中可以看出,实际速度和角度跟踪误差很小,能够达到很好的跟踪效果,系统的响应速度非常快,由于自动控制当中的平衡移动原理,在速度跟踪的时候会出现相移现象,图9a-d为两轮自平衡机器人在速度参考信号为正弦波下采用现有技术PID控制算法的性能曲线,从图8和图9的对比可以看出,本设计的自适应滑模控制能够使系统响应速度更快,鲁棒性更强,从速度和角度误差可以看出,由于相移现象,所以采用自适应滑模控制,系统的速度跟踪误差稍大一点,但是在平衡位置的时候,采用PID控制,系统会出现轻微的抖动现象,效果明显不如采用自适应滑模控制器的效果,并且采用PID控制的角度跟踪误差更大,另外从控制器输出量可以看出,采用自适应滑模控制器,系统更稳定,优势明显,为了进一步验证本设计的滑模控制器的优势,由于方波信号当中有从0立即变化到1的时刻,能够更好的验证系统鲁棒性以及响应速度等特性,图10a-d为在速度参考信号为方波下采用自适应滑模控制的性能曲线,图11a-d为在速度参考信号为方波下采用PID控制的性能曲线,从图10和11中可以看出,系统在由0变到1时,系统采用自适应滑模控制的响应速度更快,速度跟踪效果更好,鲁棒性更强。
[0209] 以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以对本发明进行若干改进和修饰,这些改进和修饰也落入本发明权利要求的保护范围内。
[0210] 对所公开的实施例的上述说明,使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的,本发明中所定义的一般原理可以在不脱离本发明的精神或范围的情况下,在其它实施例中实现。因此,本发明将不会被限制于本发明所示的这些实施例,而是要符合与本发明所公开的原理和新颖特点相一致的最宽的范围。