球形机器人的建模与控制研究.pdfVIP

自动化仪表第 37 卷第 6 期 2016 年 6 月 修改稿收到日期:2015 -11 -21。 第一作者王志群(1990 - ),男,现为北京交通大学控制科学与工程 专业在读硕士研究生;主要从事移动机器人控制方向的研究。 球形机器人的建模与控制研究 Modeling of Spherical Robot and Research on Relevant Control 王志群1 刘 蕾2 杨 彬2 董 春2 (北京交通大学电子信息工程学院1,北京 100044;北京交通大学电气工程学院2,北京 100044) 摘 要: 设计了一种通过三个惯性轮驱动的球形机器人,基于角动量守恒定律实现机器人移动。 利用四元数及 Kane 方程建模方法, 建立了球形机器人的完整动力学模型,给出了控制其运动的微分方程组,进而设计一个自适应模糊滑模变结构控制器,以实现对参数 及动力学模型不精确的球形机器人的位置控制。 为了减弱系统的抖振,通过对滑模控制器中的切换项进行模糊逼近,使切换项连续 化。 轨迹跟踪的仿真和试验表明,该控制器在参数不确定和动力学模型不精确的系统中有良好的表现。 关键词: 球形机器人 动力学模型 四元数 Kane 方程 滑模变结构控制 自适应模糊控制 减振 轨迹跟踪 中图分类号: TH7;TP242 文献标志码: A DOI:10.16086/ j. cnki. issn 1000 -0380.201606020 Abstract: A spherical robot driven by three inertia wheels is designed; the motion of robot is implemented based on the law of conservation of angular momentum. Complete dynamics model of the robot is built by using quaternion and Kane equation modeling method; the differential equations that are controlling the motion are given, thereby an adaptive fuzzy sliding mode variable structure controller is designed to implement position control of the spherical robot, of which the parameters and dynamics model are not accurate enough. In order to weaken the buffeting vibration of the system, the switching items of the sliding mode controller are fuzzy approximated to make these items continuum. The simulation and experiments of trajectory tracking show that this controller has good manifestation in the system of which the parameters are undetermined either the dynamics model is inaccurate. Keywords: Spherical robot Dynamics model Quaternion Kane equation Sliding mode variable structure control Adaptive fuzzy control Vibration reduction Trajectory tracking 0 引言 球形机器人由球壳及内部驱动机构组成,它的运 动原理主要为两种:一种是通过内部驱动机构改变系 统的重心位置,如轮式质量块驱动1、位移质量块驱 动2、两自由度摆驱动3等;另一种是改变系统的角 动量,如双转子结构4、三自由度陀螺5等。 文献6提出的动力学模型,只适用于球形机器 人的直线运动;而文献7摆杆驱动的球形机器人则 采用反馈线性化的方法,设计直线轨迹跟踪的控制器。 此外,直线轨迹跟踪控制器的设计方法还有:滑模变结 构方法8、自适应分层滑模控制方法9、自适应神经 模糊和滑模变结构控制相融合的方法10等。 文献 11对球形机器人的模型进行线性化,忽略内部驱动 机构的动力学效应,设计了一个滑模变结构轨迹跟踪 控制器;文献12则基于解耦的动力学模型并忽略机 器人的横向及纵向旋转,设计了一个模糊控制器进行 运动控制。 本文将研究使用惯性轮驱动的球形机器人,并设 计滑模控制器实现平面轨迹跟踪。 为了解决抖振的问 题,采用模糊系统对滑模控制器的切换项进行逼近,从 而实现控制信号的连续化。 1 运动学与动力学建模 1. 1 运动学建模 受文献13、文献14的启发,本文设计了球形 机器人结构。 为便于建模及仿真,在此假设装配了 3 个惯性轮及相对应数目配重块的球形机器人的质心, 刚好与球壳中心重合。 设惯性坐标系为1(x1,y1, z1)。 坐标系2(x2,y2,z2)与惯性坐标系1平行,原 点与球心重合,坐标系3(x3,y3,z3)为球体坐标系, 与坐标系2同原点,但是坐标系随着球的旋转而发 生相应的旋转。 对于坐标系2与坐标系3之间的 旋转和平移关系,可用四元数表示如下: 2 3q 1 3q = q0 q = q0 + q1i + q2j + q3k(1) 2 3q 2 = 1 3q 2 = q2 0 + q2+ q2 2 + q2 3 = 1(2) 式中:1 3q 和 2 3q 分别为坐标系3相对坐标系1和坐 标系2的旋转四元数。 97 球形机器人的建模与控制研究 王志群,等 PROCESS AUTOMATION INSTRUMENTATION Vol 37 No 6 June 2016 由文献15可知,空间角速度与旋转四元数之间 的关系可表示为: 0 2 3 = 2 2 3q 2 3q - (3) 式中:23为坐标系3中角速度向量在坐标系2中 的描述;q - 为共轭四元数。 由式(3)可计算得到球壳在坐标系2的角速度 和角加速度: 2 1 = 2 3 2 1 = 2 3 (4) 式中:21及21分别为球形机器人第 1 部分(即球壳) 的角速度及角加速度向量在坐标系2中的描述。 球心的速度为: 2V 1 = 2V I +212rG/ I= 2 1 Rsk2(5) 式中:2V1为球形机器人第 1 部分(即球壳)质心位置 的线速度向量在坐标系2中的描述;2VI为球形机器 人与地面接触点间的速度向量;2rG/ I为球形机器人几 何中心到球壳与地面接触点间的位置向量在坐标系 2中的描述;Rs为球壳半径。 x y = 2Rs - q2q3q0- q1 q1- q0q3- q2 q 0,q 1,q 2,q 3 T(6) 式中:x 及 y 分别为球壳质心位置的两个线速度分量 在坐标系1中的描述。 3 1 = 3 2q 0 2 1 3 2 q - 3 1 = 3 2 0 2 1 3 2 q - (7) 式中:3 2q 为坐标系2 相对坐标系3 的旋转四元 数;3 2q - 为相对应的共轭四元数。 由于配重块与球壳相连,因此它们的角速度及角 加速度都与球壳相等,即: 3 i = 3 1 3 i = 3 1 (8) 式中:i =2,3,4。 3 个惯性轮相对于坐标系1的角速度及角加速 度分别为: 35,36,37 = 31+ xi3,31+ yj3,31+ zk3(9) 35,36,37 = 31+ xi3 +31 xi3,31+ yj3 +31 yj3,31+ zk3 +31 zk3 (10) 式中:x、y、z分别为 3 个惯性轮相对球壳旋转的角 速度; x、 y、 z分别为对应的角加速度。 各惯性轮及配重块质心处的线速度及加速度为: 2V i = 2V I + 2 3 q(313li)2 3q - (11) 式中:i = 2,3,4,5,6,7;3li为机器人内部第 i 部分结构 的质心相对于球体中心的位置矢量在坐标系3的 描述。 2 i = d dt (2Vix)i2+ d dt (2Viy)j2+ d dt (2Viz)k2(12) 式中:i =1,2,3,4,5,6,7。 1. 2 动力学建模 球形机器人系统可由 9 个参数q0,q1,q2,q3,x,y, x,y,z来描述,但是这些参数之间并不相互独立。 以下由 Kane 法来推导机器人运动微分方程。 选择q 1,q 2,q 3,x,y,z这 6 个速度分量,组成 广义速度向量 U,这样配重块与球之间的相互作用都 只是内力,将不会出现在运动方程式中。 坐标系4、 5、6的原点分别位于3 个惯性轮的重心位置,3 个 坐标系都有一个轴与电机轴向平行,并且 3 个坐标系 都与3平行。 由此可知,球形机器人各部位所受到 的惯性力和力矩如下所示: 2R i = - mi2i(13) 式中:i =1,2,3,4,5,6,7;2R i 为球形机器人各部位所 受的惯性力在坐标系2中的描述。 3M i= - 3I i 3 i- 3 i ( 3I i 3 i) (14) 式中:i = 1,2,3,4;3M i 为球形机器人各部位所受 的惯性矩在坐标系3中的描述;3Ii为球形机器人各 部位的转动惯量矩阵在坐标系3中的描述。 电机转子的惯性矩为: 3M 5= - 3I 5( 3 1+ xi3) - 3 1 ( 3I 5 3 5) 3M 6= - 3I 6( 3 1+ yj3) - 3 1 ( 3I 6 3 6) 3M 7= - 3I 7( 3 1+ zk3) - 3 1 ( 3I 7 3 7) (15) 电机产生的主动力矩可分别表示为: 3M rel1= Txi3 3M rel2= Tyj3 3M rel3= Tzk3 (16) 式中: Tx、Ty、Tz为 3 个电机的输出转矩。 根据 Kane 法,计算出机器人各部位的线速度及角 速度的偏导数为: 2Vj i= (2Vi) (Uj) 2j i= (2i) (Uj) (17) 08 球形机器人的建模与控制研究 王志群,等 自动化仪表第 37 卷第 6 期 2016 年 6 月 式中:i =1,2,3,4,5,6,7;j =1,2,3,4,5,6。 电机转子在坐标系4、5、6中相对角速度的 偏导数分别为: 3j rel1= (xi3) (Uj) 3j rel2= (yj3) (Uj) 3j re/3= (zk3) (Uj) (18) 式中:j =1,2,3,4,5,6。 广义主动力和广义惯性力分别为: Fj= 7 i =1 2R i 2Vj i+ 7 i =1 3M i 3j i+ 7 i =1 3M reli 3j reli (19) 式中:j = 1,2,3,4,5,6;2Ri为球形机器人各部位所产 生的主动力在坐标系2中的描述;3Mi为球形机器人 各部位所产生的主动力矩在坐标系3中的描述。 F j = 7 i =1 2R i 2Vj i+ 7 i =1 3M i 3j i (20) 式中:j =1,2,3,4,5,6。 式(19)和式(20)由 Kane 法表述为: Fj+ F j =0(21) 式中:j =1,2,3,4,5,6。 将式(21)化为如下球形机器人全状态运动学方程: A(p) p + C(p,p ) = (22) p = q0,q1,q2,q3,x,y,x,y,z T (23) 式中:pR9为球形机器人全状态向量;A(p)R9 9为 正定质量惯性矩阵;C(p,p )R9 为科氏力、离心力和 重力之和;R9为控制力矩矩阵。 由于 A(p)是非奇 异矩阵,在式(22)两端同乘 A -1,可转化为: p = f(p,p ) + g(p)u (24) 式中:f(p,p )为(9 1)矩阵;g(p)为(9 3)矩阵;u 为 (3 1)矩阵。 2 控制系统设计 考虑建模误差、参数变化以及其他不确定因素, 式(22)可以改写成以下形式: (A + A) p + C(p,p ) + C(p,p) + D = (25) 式中:A、C 为建模误差及参数不确定;D 为测量噪声 及其他干扰。 根据式(24)、式(25),可改写成以下形式: p = f(p,p ) + f(p,p) + g(p) + g(p)u (26) 式中:f(p,p )和 g(p)大小未知,但各状态量的二范 数均有上界。 如果将它们视为外界扰动 d(p,p ,),由 于 d 为未知,取 d 上下界为 dL和 dU,设上下界绝对值 中的最大值小于一个已知数 E。 则式(26)可改写为: p = f(p,p ) + g(p)u + d(p,p,t) (27) 为了跟踪球形机器人系统的运动轨迹,系统输出 矩阵设计为 H = x,y,z T,对输出矩阵取二阶微分, 可得: H = x ,y , z = x ,y ,2 3z T (28) 把式(27)中关于 x 、y 、 z的等式代入式(28),可 得到如下二阶非线性系统: H = H1(p,p ) + H 2(p) + d(p,p ,t) Y = H (29) 式中:HR3,H2R3 3,dR3,可由式(24)得到。 2. 1 滑模控制器设计 取球形机器人的位置及角度指令,分别为 xd、yd、 zd,则轨迹的跟踪误差为: e = x - xd,y - yd,z- zd T = ex,ey,ez T 定义切换函数为: S = sx,sy,s z T = (e x+ k1ex),(e y + k2ey),(e z + k3e z) T,k 1,k2,k30 则: S = e x+ k1e x),( e y+ k2e y),( e z+ k3e z) T = H -H d+ KH (30) 式中:Hd= xd,yd,zd T;K = k 1,k2,k3 T。 将滑模控制律设计为: u = H -1 2 (p)H d- KH - H1(p,p ) - u sw (31) 式中:usw= xsgn(sx),xsgn(sy), zsgn(sz) T,且 x E,y E, z E。 由式(29)和式(30),可得: S = H1(p,p ) + H 2(p)u - H d+ KH + d(p,p ,t) (32) 将式(31)代入式(32),可得: S = d(p,p ,t) - u sw (33) 即:STS = STd(p,p ,t) - ( x|sx| + y|sy| + z|sz|)0 当干扰项 d(p,p ,t)的值较大时,滑模控制式(31) 中的切换项 usw增益也会比较大,这就造成抖振。 为了 减弱抖振,可采用模糊系统逼近 usw。 2. 2 自适应模糊滑模控制器设计 由于模糊系统具有万能逼近特性,利用自适应模糊 控制方法,通过将滑模控制器中的切换项进行模糊逼 近,可将切换项连续化,从而有效地降低抖振16。 分别为 3 个跟踪目标设计 3 个模糊系统,针对模 糊系统输入分别设计 3 个模糊子集,各有 3 条模糊规 则。 采用乘积推理机、单值模糊器和中心平均解模糊 器设计模糊系统17,模糊系统的输出为 F,引入模糊 基向量 (s) = 1(s),2(s),3(s) T,则式(31)所 表示的控制律可变为: 18 球形机器人的建模与控制研究 王志群,等 PROCESS AUTOMATION INSTRUMENTATION Vol 37 No 6 June 2016 u = H -1 2 (p)H d- KH - H1(p,p ) - F(S) (34) 式中:F(S)为模糊系统输出矩阵;F(S)为理想的模 糊系统输出矩阵;(sx)、(sy)、(s z)为 3 个三维模 糊基向量,向量 JT x,J T y,J T z根据自适应律而变化。 理想 的 F ( s | J) 为 F ( s | J) = sgn ( s), 其 中 E。 自适应律为: J = s(s)(35) 式中: 0。 稳定性证明如下: 设最优参数为:J=min jE sup|F(s|J) - sgn(s)|。 其中:E 为 J 的集合,sup|F(s|J) - sgn(s) |为取函数 F(s|J) - sgn(s)的上确界,则由式(33)可得: S = d(p,p ,t) - u sw= F (S) + d(p,p ,t) - F(S) 其中:F (S) = F(S) -F(S),j = J-J。 Lyapunov 函数为:2V = (STS + -1 x J T xJ x, -1 y J T yJ y, -1 J T zJ z T),则: V = STS + -1 x J T xJ x, -1 y J T yJ y, -1 J T zJ z T = STF (S) + -1 x J T xJ x, -1 y J T yJ y, -1 z J T zJ z T + STd(p,p ,t) - F(S) = STd(p,p,t) - x|sx|,y|sy|, z|sz| T 0 3 仿真及试验 设 x 方向的位置指令为 xd=0. 1 sin t,y 方向的位 置指令为 yd=0.1 cos t,球壳绕竖直转轴 z 方向的旋转 角速度指令为 zd= x 2 d+ y 2 y/ Rd=0.1,其中 Rd为球形 机器人进行圆周运动时轨迹的半径。 取 K = k1,k2, k3 T = 30,30,30 T,干扰项 d(p,p ,t) = 10 sin t, 10sin t,10 sin t T。 定义切 换函数的 隶属 函 数 为 z(s) = exp( - s2),N(s) = 1 + exp5(s + 3) -1, p(s) = 1 + exp5(s -3) -1;切换函数 s y、sz的隶属 函数都与 sx相同。 设向量 JT x、J T y、J T z中各元素的初始值都为 0. 1。 采 用控制律式(34)和自适应律式(35),自适应参数选取 x= 150,150,150 T,则仿真结果图 1 图 3 所示。 图 1 为利用自适应模糊控制方法,将跟踪位置 x 滑模控制器中的切换项进行模糊逼近。 将原先的切换 项连续化,能够有效降低控制系统的抖振。 图 2 为圆周运动轨迹,2 条轨迹表明球形机器人 能够快速稳定地跟踪给定的轨迹。 图 3 为跟踪误差曲 线,采用曲线的形式,给出球形机器人进行圆周轨迹跟 踪时,在 x 方向、y 方向以及绕竖直转轴 z 方向的转角 和跟踪误差。 图 1 x 方向的切换项及模糊逼近示意图 Fig. 1 The switching item and fuzzy approximation of direction x 图 2 圆周运动轨迹 Fig. 2 Circumferential motion trajectory 图 3 跟踪误差曲线 Fig. 3 The curve of tracking error 4 结束语 本文研究了一种通过 3 个惯性轮驱动的球形机器 人。 在对机器人进行运动分析的基础上,利用四元数 及 Kane 法,建立了球形机器人的完整动力学模型,并 进行了相应的仿真和试验研究。 为了实现对球形机器 人的轨迹跟踪控制,设计了一个自适应模糊滑模变结 构控制器。 通过将滑模变结构控制器中的切换项进行 模糊逼近,使切换项连续化,从而有效降低了抖振;对 满足一定条件的参考轨迹,实现了非完整机器人系统 的全局渐近轨迹跟踪控制。 仿真结果证明了该控制器 的有效性和正确性。 (下转第 85 页) 28 球形机器人的建模与控制研究 王志群,等 自动化仪表第 37 卷第 6 期 2016 年 6 月 型进行离线修正,或者利用实时数据进行在线修正。 综上所述,建立氧量软测量模型的步骤如下。 根据使用要求,确定模型的性能参数。 确定所使用的软测量模型。 选取模型的辅助输入变量,分析辅助变量测量 的准确度。 根据选取的辅助变量和氧量,从现场选择样本 数据,并按模型要求对数据进行预处理。 将预处理后的数据代入模型,确定模型的参数。 利用新的样本数据对模型进行验证,如不满足 性能参数,则调整辅助变量或软测量模型;如满足性能 要求,则将新的样本数据带入模型进行参数修正。 4 结束语 烟气含氧量的软测量建模技术为火电机组的氧量 测量提供了新的方法,对于实时监测以及燃烧系统优 化调整具有重要的意义,尤其是可以在硬件测量出现 故障时,为工程实施提供重要的参考。 尽管获得了很 好的动态响应特性,但是在精度方面,由于目前软测量 预测结果仍然是以现场用氧量分析仪测量得到的数据 作为参考,因此并不能得出软测量可以取代硬件测量 结果的结论。 目前,氧化锆氧分析器仍是国际公认的 锅炉节能和环保的主要装置,因此笔者认为研究者应 该更多地从材料和硬件制作工艺方面考虑,改善测量 的精准度和响应速度。 参考文献 1 谷俊杰,孔德奇,高大明,等. 电站锅炉燃烧优化中最佳烟气含 氧量设定值的计算J. 华北电力大学学报,2007,34 (6): 61 -65. 2 周新刚,张文奎,郝卫东,等. 超临界 600MW 机组锅炉运行氧量 优化试验研究J. 热力发电,2012,41(10):28 -31. 3 龙文,梁昔明,龙祖强. 基于混合 PSO 优化的 LSSVM 锅炉烟气 含氧量预测控制J. 中南大学学报(自然科学版),2012,43 (3):980 -986. 4 赵征,曾德良,田亮,等. 基于数据融合的氧量软测量研究J. 中国电机工程学报,2005,25(7):7 -15. 5 陈彦桥,郭一,刘建民,等. 一种改进烟气含氧量软测量建模方 法与仿真J. 动力工程学报,2011,31(1):12 -17. 6 韩璞,王东风,翟永杰. 基于神经网络的火电厂烟气含氧量软测 量J. 信息控制,2001,30(2):189 -192. 7 刘千,韩璞,王东风. 基于 ANFIS 模型的烟气含氧量建模和预 测J. 计算机仿真,2014,31(10):437 -441. 8 湛腾西,郭观七. 电厂烟气含氧量的智能混合预测方法J. 仪 器仪表学报,2010,31(8):1826 -1833. 9 徐巧玲,林伟豪,赵超,等. 基于 KPCA - LS - SVM 的工业锅炉 烟气含氧量 预测 J. 计算 机与应 用化学,2012,29 (7): 834 -838. 10张曦,陈世和,陈锐民,等. 基于核偏最小二乘的电厂热力参数 预测与估计J. 中国电机工程学报,2011,05(31):127. (上接第 82 页) 参考文献 1 HALME A, SCHNBERG T, WANG Y. Motion control of a spherical mobile robotC / / International Workshop on Advanced Motion Control (AMC), 1996.259 -264. 2 JAVADI A H, MOJABI P. Introducing august: a novel strategy for an omnidirectional spherical rolling robotC / / IEEE International Conference on Robotics and Automation, 2002:3527 -3533. 3 ZGAN Q, ZHOU T, CHEN M,et al. Dynamic trajectory planning of a spherical mobile robotC / / IEEE International Conference on Robotics, Automation & Mechatronics (RAM), 2006:1 -6. 4 BHATTACHARYA S, AGRAWAL S K. Spherical rolling robot: A design and motion planning studies J. IEEE Transactions on Robotics and Automation, 2000, 16(6): 835 - 839. 5 OTANI T, URAKUBO T, MAEKAWA S, et al. Position and attitude control of a spherical rolling robot equipped with a Gyro C / / IEEE International Workshop on Advanced Motion Control (AMC), 2006:416 -421. 6 ZHAO B, LI M, YU H, et al. Dynamics and motion control of a two pendulumsdrivensphericalrobot C / / IEEE/ RSJInternational Conference on Intelligent Robots and Systems (IROS),2010.147 -153. 7 LIU D, SUN H, JIA Q. A family of spherical mobile robot: Driving ahead motioncontrolbyfeedbacklinearization C / / IEEE International Symposium on Systems and Control in Aerospace and Astronautics (ISSCAA), 2008.1 -6. 8 LIU B, YUE M, LIU R. Motion control of an underactuated spherical robot: A hierarchical sliding - mode approach with disturbance estimationC / / IEEE International Conference on Mechatronics and Automation (ICMA), 2012.1804 -1809. 9 YUE M, LIU B. Adaptive control of an underactuated spherical robot with a dynamic stable equilibrium point using hierarchical sliding mode approach J. International Journal of Adaptive Control and Signal Processing, 2014, 28(6): 523 -535. 10 KAYACAN E, RAMON H, SAEYS W. Adaptive neuro - fuzzy control of a spherical rolling robot using sliding - mode -