第5期 王建彬，等：动力学解析的四轮全向移动机器人电机解耦控制 ·571- 得动力学方程如下： 其中矩阵Z中各元素的表达式为 mx+F1sin(0+61)-F2sin(0-82) 2 ,C0s282 -F3sin(0+83)+F4sin(8-84)=0 n3* 1=J1+ rr m -+p2(m n216c0s28 my-F1cos(0+81）+F2cos(0-82)+ 2 m +p2(-m+- F3cos(0+63)-F4cos(0-64)=0 16cos5,sin 2 J0-F l-F2l2 F3ls-Fl=0 (3) m -16c0s5.sin.m J+Fr=T -2 J202+F2r=T2 r m .c0s28, J303 F3r =T3 Ja+Fr=T 矩阵W中各元素的表达式为 进一步整理为拉格朗日标准形式有 Cw Mg=Ex-H'(q)入 (4 01=Cm+ n 式中： H(q)= w=pmr2 4n2sinδ2 1) cos(8+8)sin(9+8,)-1T00 0 ws pmr2 1 n(、 -+ -)8 cos(0-82)sin(0-62)-l0r00 sinδ，cos0, cos(9+83)sin(9+83)-l00r0 0,s、Pr2 18 cos(0-84)sin(0-6a)-l000r M=diag(m,m,J小h)1-0r 「00 限于篇幅，其他系数未列出，其中J(i=1,2,3, 为单位对角方阵，A=[F,F,F,F],T= 4)为折算到各个驱动电机的转动惯量，cm和c.分 [T,T2T3T4]T。 别为各个驱动电机和对应轮子的阻尼系数，n为电 式(4)两端乘以S(q),且由式(1)、(3)可得图 1 1所示的移动机器人动力学模型： 机齿轮减速比，p=2(c0sd,+sin6,) T=S"(q)Mq 令w=v,则式(6)进一步化简得到机器人四轮 (5) 速度与其驱动力矩间的状态方程： 1.2各电机输入输出量间耦合关系的推导 对式(1)两端求导，并将式(5)代入可得 ω=A0+Br (9) i=-(s(q)Ms(q))-Is"(q)MS(q)v+(6) 式中：状态矩阵A和B为4×4的方阵，且 (S(q)MS(q))-17 A=Z-W.B=Z- (10) 取4×4的方阵Z和M分别为 由式(8)和(10)可知，系统状态矩阵A是关于各轮 31222324 转速的非线性矩阵，因此本文在实际计算中，用其在 22252623 Z=S(q)MS(q)= (7) 平衡点附近的近似线性化矩阵A来代替，从而有 23262522 @=A@+BT (11) 24232231」 由式(11)可知，四轮驱动机器人的电机控制系统是 1011021030d 一个4输入4输出的耦合系统，需要进行解耦以达 -10210110503 W=S(g)MS(g)= (8) 到对4个电机的独立控制。假设系统的输出方程为 -10310510102 Y=Cw,则由线性系统理论知识可得系统的传递函 数矩阵为得动力学方程如下： ｍｘ ¨ ＋ Ｆ１ ｓｉｎ（θ ＋ δ１ ） － Ｆ２ ｓｉｎ（θ － δ２ ） － Ｆ３ ｓｉｎ（θ ＋ δ３ ） ＋ Ｆ４ ｓｉｎ（θ － δ４ ） ＝ ０ ｍｙ ¨ － Ｆ１ ｃｏｓ（θ ＋ δ１ ） ＋ Ｆ２ ｃｏｓ（θ － δ２ ） ＋ Ｆ３ ｃｏｓ（θ ＋ δ３ ） － Ｆ４ ｃｏｓ（θ － δ４ ） ＝ ０ Ｊθ ¨ － Ｆ１ ｌ １ － Ｆ２ ｌ ２ － Ｆ３ ｌ ３ － Ｆ４ ｌ ４ ＝ ０ Ｊ１φ ¨ １ ＋ Ｆ１ ｒ ＝ Ｔ１ Ｊ２φ ¨ ２ ＋ Ｆ２ ｒ ＝ Ｔ２ Ｊ３φ ¨ ３ ＋ Ｆ３ ｒ ＝ Ｔ３ Ｊ４φ ¨ ４ ＋ Ｆ４ ｒ ＝ Ｔ４ （３） 进一步整理为拉格朗日标准形式有 Ｍｑ ¨ ＝ Ｅτ － Ｈ Ｔ （ｑ）λ （４） 式中： Ｈ（ｑ） ＝ ｃｏｓ（θ ＋ δ１ ） ｓｉｎ（θ ＋ δ１ ） － ｌ ｒ ０ ０ ０ ｃｏｓ（θ － δ２ ） ｓｉｎ（θ － δ２ ） － ｌ ０ ｒ ０ ０ ｃｏｓ（θ ＋ δ３ ） ｓｉｎ（θ ＋ δ３ ） － ｌ ０ ０ ｒ ０ ｃｏｓ（θ － δ４ ） ｓｉｎ（θ － δ４ ） － ｌ ０ ０ ０ ｒ é ë ê ê ê ê ê ê ù û ú ú ú ú ú ú Ｍ ＝ ｄｉａｇ（ｍ，ｍ，Ｊ，Ｊ１ ，Ｊ２ ，Ｊ３ ，Ｊ４ ），Ｉ ＝ ０ ０ ０ Ｉ′ ４×４ é ë ê ê ù û ú ú Ｉ＇ 为 单 位 对 角 方 阵， λ ＝ ［Ｆ１ Ｆ２ Ｆ３ Ｆ４ ］ Ｔ ， τ ＝ ［Ｔ１ Ｔ２ Ｔ３ Ｔ４ ］ Ｔ 。 式（４）两端乘以 Ｓ Ｔ （ｑ） ，且由式（１）、（３）可得图 １ 所示的移动机器人动力学模型： τ ＝ Ｓ Ｔ （ｑ）Ｍｑ ¨ （５） １．２ 各电机输入输出量间耦合关系的推导 对式（１）两端求导，并将式（５）代入可得 ｖ · ＝ － （Ｓ Ｔ （ｑ）ＭＳ（ｑ）） － １Ｓ Ｔ （ｑ）ＭＳ · （ｑ）ｖ ＋ （Ｓ Ｔ （ｑ）ＭＳ（ｑ）） － １τ （６） 取 ４ × ４ 的方阵 Ｚ 和 Ｍ 分别为 Ｚ ＝ Ｓ Ｔ （ｑ）ＭＳ（ｑ） ＝ ｚ１ ｚ２ ｚ３ ｚ４ ｚ２ ｚ５ ｚ６ ｚ３ ｚ３ ｚ６ ｚ５ ｚ２ ｚ４ ｚ３ ｚ２ ｚ１ é ë ê ê ê ê ê ê ù û ú ú ú ú ú ú （７） Ｗ ＝ Ｓ Ｔ （ｑ）ＭＳ · （ｑ） ＝ ｗ１ ｗ２ ｗ３ ｗ４ － ｗ２ ｗ１ ｗ５ ｗ３ － ｗ３ ｗ５ ｗ１ ｗ２ － ｗ４ － ｗ３ － ｗ２ ｗ１ é ë ê ê ê ê ê ê ù û ú ú ú ú ú ú （８） 其中矩阵 Ｚ 中各元素的表达式为 ｚ１ ＝ Ｊ ＾ １ ＋ Ｊ１ ｎ ２ ＋ ｒ ２ ｎ ２ ［ ｍ １６ ｃｏｓ ２ δ １ ＋ ρ ２ （ｍ ＋ ｃｏｓ ２ δ ２ Ｌ ２ Ｊ）］ ｚ２ ＝ ｒ ２ ｎ ２ ［ ｍ １６ｃｏｓδ １ ｓｉｎδ ２ ＋ ρ ２ （ － ｍ ＋ ｓｉｎδ １ ｃｏｓδ ２ Ｌ ２ Ｊ）］ ｚ３ ＝ ｒ ２ ｎ ２ ［ － ｍ １６ｃｏｓδ １ ｓｉｎδ ２ ＋ ρ ２ （ － ｍ ＋ ｓｉｎδ １ ｃｏｓδ ２ Ｌ ２ Ｊ）］ ｚ４ ＝ ｒ ２ ｎ ２ ［ － ｍ １６ ｃｏｓ ２ δ １ ＋ ρ ２ （ｍ ＋ ｃｏｓ ２ δ ２ Ｌ ２ Ｊ）］ … 矩阵 Ｗ 中各元素的表达式为 ｗ１ ＝ ｃｍ ＋ ｃｗ ｎ ２ ｗ２ ＝ ρｍｒ ２ ４ｎ ２ （ １ ｓｉｎδ ２ ＋ １ ｃｏｓδ １ ）θ · ｗ３ ＝ ρｍｒ ２ ４ｎ ２ （ － １ ｓｉｎδ ２ ＋ １ ｃｏｓδ １ ）θ · ｗ４ ＝ － ρｍｒ ２ ２ｎ ２ · １ ｃｏｓδ １ θ · … 限于篇幅，其他系数未列出，其中 Ｊ ＾ ｉ （ ｉ ＝ １，２，３， ４ ）为折算到各个驱动电机的转动惯量， ｃｍ 和 ｃｗ 分 别为各个驱动电机和对应轮子的阻尼系数， ｎ 为电 机齿轮减速比， ρ ＝ １ ２（ｃｏｓ δ １ ＋ ｓｉｎ δ ２ ） 。 令 ω ＝ ｖ ，则式（６）进一步化简得到机器人四轮 速度与其驱动力矩间的状态方程： ω · ＝ Ａ ～ ω ＋ Ｂτ （９） 式中：状态矩阵 Ａ ～ 和 Ｂ 为 ４ × ４ 的方阵，且 Ａ ～ ＝ Ｚ －１Ｗ，Ｂ ＝ Ｚ －１ （１０） 由式（８）和（１０）可知，系统状态矩阵 Ａ ～ 是关于各轮 转速的非线性矩阵，因此本文在实际计算中，用其在 平衡点附近的近似线性化矩阵 Ａ 来代替，从而有 ω · ＝ Ａω ＋ Ｂτ （１１） 由式（１１）可知，四轮驱动机器人的电机控制系统是 一个 ４ 输入 ４ 输出的耦合系统，需要进行解耦以达 到对 ４ 个电机的独立控制。 假设系统的输出方程为 Ｙ ＝ Ｃω ，则由线性系统理论知识可得系统的传递函 数矩阵为 第 ５ 期 王建彬，等：动力学解析的四轮全向移动机器人电机解耦控制 ·５７１·