·400· 智能系统学报 第10卷 展现)。文献[6]针对移动机器人在传统控制器控 yp =lsin 0 lzsin(0 +0) (3) 制下自主运动时出现的不稳定状况，将模糊控制策 对末端坐标进行积分得到末端速度方程： 略引入移动机器人运动控制系统中。通过分析比较 xa=-l1m1sin81-2(ω1+w2)sin(8+82)(4) 不同的控制方法，设计了由速度误差率和速度误差 y'm=l101c0s81+l2(ω1+w2)os(6+62)(5) 变化率为输入、电机输出功率为输出的双输入单输 式中：w1=0'1,02=8'2,将式(4)和(5)整理并写成 出模糊逻辑控制器。文献[7]模仿人工预瞄驾驶行 如下矩阵形式（即雅克比矩阵[]）： 为，提出了移动机器人运动控制的模糊控制方法，实 验表明，该模糊控制方法可保证移动机器人快速、准 确地沿着各种参考轨迹行走，且具有良好的鲁棒性。 式中：S1=sin01,C,=cos01,S2=sin82,C2=cos62, 文献[8]在智能轮椅机器人中使用超声波的探测距 S12=sin(01+02),Ce=cos(01+02)。 离作为输入信号，经模糊控制技术处理后，输出机器 对二连杆机械臂末端速度方程(3)~(4)进行 人左右轮的转动速度来实现超声波避障。 求导得到末端加速度方程： 本文首先对二连杆投球机器人进行运动学建 x”+(l1S1+l2S2)a1+l2S12a2= 模，然后在此基础上对机器人的控制方法进行了研 -[(l,C1+l2C2)w21+l2C12w2+2l2w1ω2C2] 究，通过PID和模糊PID2种控制方法，实现二连杆 投球机器人运动轨迹的有效控制，用MATLAB软件 (7) 进行仿真，并对仿真结果进行对比分析。 y”d+(l1S1+l2S12)a1+l2S12a2= -[(L1S1+l2S12)w21=l2S12w22+2l2w1w2S12] 1二连杆投球机器人的运动学建模 (8) 1.1二连杆机械臂 式中：a1=0”1,2=0"”2,二连杆机械臂质心位置的 机械手臂是机器人重要构成部件。图1为二杆 加速度与关节处的变量之间关系如下： 机械臂结构图，其中日，和6，分别为关节1、2转角， (Ac +rS=-rCWi L1和2分别为第1、2根杆的长度，r,为关节1到第1 (9) (Aay-riCia1=-rSwi 根杆质心的距离，12为关节2到第2根杆质心的距 A2+(l1S+T2S2)a1+T2S2a2= 离，M.为负载质量。 (L1C1+l2C12)w21+l2C12w2+2l1w1ω2C2(10) A+(L1C,+r2C2)a1+r2C12a2= （化+，5a)a+4soa+2以@，o,Sa（1） 分别对机械臂两连杆的隔离体[o]进行力及力矩的 分析，求得机械臂的动力学方程。 1.2机械臂动力学方程 首先，对二连杆平面投球机器人的第1根连杆 应用牛顿定律，受力分析如图2所示。 图1二连杆机械臂结构 Fig.1 Dual-arm mechanical structure 式(1)是二连杆机械臂的矢量方程，其中R,、 R,是沿T1和T2方向上的矢量，R为合成矢量。由 于在二连杆机械臂中，各连杆的长度保持不变，但它 18 们各自的方位却随机构的运动而改变，所以矢量方 程对时间求导将闭环矢量方程分解成2个标量表达 图2第1根连杆受力分析 式，分别沿x、y方向分解。 Fig.2 The first arm force analysis R=R+R2 (1) R相应于x和y的坐标方程为 图2中，FoF2.分别代表第1根杆2端所受x x=l1c0s8+l2c0s(81+02) (2) 方向的力，F,、F,分别代表第1根杆2端所受y方展现［５］ 。 文献［６］针对移动机器人在传统控制器控 制下自主运动时出现的不稳定状况，将模糊控制策 略引入移动机器人运动控制系统中。 通过分析比较 不同的控制方法，设计了由速度误差率和速度误差 变化率为输入、电机输出功率为输出的双输入单输 出模糊逻辑控制器。 文献［７］模仿人工预瞄驾驶行 为，提出了移动机器人运动控制的模糊控制方法，实 验表明，该模糊控制方法可保证移动机器人快速、准 确地沿着各种参考轨迹行走，且具有良好的鲁棒性。 文献［８］在智能轮椅机器人中使用超声波的探测距 离作为输入信号，经模糊控制技术处理后，输出机器 人左右轮的转动速度来实现超声波避障。 本文首先对二连杆投球机器人进行运动学建 模，然后在此基础上对机器人的控制方法进行了研 究，通过 ＰＩＤ 和模糊 ＰＩＤ２ 种控制方法，实现二连杆 投球机器人运动轨迹的有效控制，用 ＭＡＴＬＡＢ 软件 进行仿真，并对仿真结果进行对比分析。 １ 二连杆投球机器人的运动学建模 １．１ 二连杆机械臂 机械手臂是机器人重要构成部件。 图 １ 为二杆 机械臂结构图，其中 θ１ 和 θ２ 分别为关节 １、２ 转角， ｌ １ 和 ｌ ２ 分别为第 １、２ 根杆的长度， ｒ１ 为关节 １ 到第 １ 根杆质心的距离， ｒ２ 为关节 ２ 到第 ２ 根杆质心的距 离， Ｍｐｌ 为负载质量。 图 １ 二连杆机械臂结构 Ｆｉｇ． １ Ｄｕａｌ⁃ａｒｍ ｍｅｃｈａｎｉｃａｌ ｓｔｒｕｃｔｕｒｅ 式（１） 是二连杆机械臂的矢量方程，其中 Ｒ１ 、 Ｒ２ 是沿 ｒ１ 和 ｒ２ 方向上的矢量， Ｒｐｌ 为合成矢量。 由 于在二连杆机械臂中，各连杆的长度保持不变，但它 们各自的方位却随机构的运动而改变，所以矢量方 程对时间求导将闭环矢量方程分解成 ２ 个标量表达 式，分别沿 ｘ、ｙ 方向分解。 Ｒｐｌ ＝ Ｒ１ ＋ Ｒ２ （１） Ｒｐｌ 相应于 ｘ 和 ｙ 的坐标方程为 ｘｐｌ ＝ ｌ １ ｃｏｓ θ１ ＋ ｌ ２ ｃｏｓ（θ１ ＋ θ２ ） （２） ｙｐｌ ＝ ｌ １ ｓｉｎ θ１ ＋ ｌ ２ ｓｉｎ（θ１ ＋ θ２ ） （３） 对末端坐标进行积分得到末端速度方程： ｘ′ｐｌ ＝ － ｌ １ω１ ｓｉｎ θ１ － ｌ ２（ω１ ＋ ω２）ｓｉｎ（θ１ ＋ θ２） （４） ｙ′ｐｌ ＝ ｌ １ω１ ｃｏｓ θ１ ＋ ｌ ２（ω１ ＋ ω２ ）ｃｏｓ（θ１ ＋ θ２ ） （５） 式中： ω１ ＝ θ′１ ，ω２ ＝ θ′２ ，将式（４）和（５）整理并写成 如下矩阵形式（即雅克比矩阵［９］ ）： ｘ′ｐｌ ｙ′ｐｌ é ë ê ê ù û ú ú ＝ － ｌ １ Ｓ１ － ｌ ２ Ｓ１２ － ｌ ２ Ｓ１２ ｌ １Ｃ１ ＋ ｌ ２Ｃ１２ ｌ ２Ｃ１２ é ë ê ê ù û ú ú ω１ ω２ é ë ê ê ù û ú ú （６） 式中： Ｓ１ ＝ ｓｉｎ θ１，Ｃ１ ＝ ｃｏｓ θ１，Ｓ２ ＝ ｓｉｎ θ２，Ｃ２ ＝ ｃｏｓ θ２， Ｓ１２ ＝ ｓｉｎ（θ１ ＋ θ２），Ｃ１２ ＝ ｃｏｓ（θ１ ＋ θ２） 。 对二连杆机械臂末端速度方程（３） ～ （４） 进行 求导得到末端加速度方程： ｘ″ｐｌ ＋ （ｌ １ Ｓ１ ＋ ｌ ２ Ｓ１２ ）α１ ＋ ｌ ２ Ｓ１２α２ ＝ － ［（ｌ １Ｃ１ ＋ ｌ ２Ｃ１２ ）ω２１ ＋ ｌ ２Ｃ１２ω２２ ＋ ２ｌ ２ω１ω２Ｃ１２ ］ （７） ｙ″ｐｌ ＋ （ｌ １ Ｓ１ ＋ ｌ ２ Ｓ１２ ）α１ ＋ ｌ ２ Ｓ１２α２ ＝ － ［（ｌ １ Ｓ１ ＋ ｌ ２ Ｓ１２ ）ω２１ ＝ ｌ ２ Ｓ１２ω２２ ＋ ２ｌ ２ω１ω２ Ｓ１２ ］ （８） 式中： α１ ＝ θ″１ ，α２ ＝ θ″２ ，二连杆机械臂质心位置的 加速度与关节处的变量之间关系如下： Ａｃ１ｘ ＋ ｒ１ Ｓ１α１ ＝ － ｒ１Ｃ１ω ２ １ Ａｃ１ｙ － ｒ１Ｃ１α１ ＝ － ｒ１ Ｓ１ω ２ １ { （９） Ａｃ２ｘ ＋ （ｌ １ Ｓ１ ＋ ｒ２ Ｓ１２ ）α１ ＋ ｒ２ Ｓ１２α２ ＝ （ｌ １Ｃ１ ＋ ｌ ２Ｃ１２ ）ω２１ ＋ ｌ ２Ｃ１２ω２２ ＋ ２ｌ １ω１ω２Ｃ１２ （１０） Ａｃ２ｙ ＋ （ｌ １Ｃ１ ＋ ｒ２Ｃ１２ ）α１ ＋ ｒ２Ｃ１２α２ ＝ （ｌ １ Ｓ１ ＋ ｌ ２ Ｓ１２ ）ω２１ ＋ ｌ ２ Ｓ１２ω２２ ＋ ２ｌ ２ω１ω２ Ｓ１２ （１１） 分别对机械臂两连杆的隔离体［１０］ 进行力及力矩的 分析，求得机械臂的动力学方程。 １．２ 机械臂动力学方程 首先，对二连杆平面投球机器人的第 １ 根连杆 应用牛顿定律［１１］ ，受力分析如图 ２ 所示。 图 ２ 第 １ 根连杆受力分析 Ｆｉｇ． ２ Ｔｈｅ ｆｉｒｓｔ ａｒｍ ｆｏｒｃｅ ａｎａｌｙｓｉｓ 图 ２ 中，Ｆ０１ｘ、Ｆ２１ｘ分别代表第 １ 根杆 ２ 端所受 ｘ 方向的力，Ｆ０１ｙ、Ｆ２１ｙ分别代表第 １ 根杆 ２ 端所受 ｙ 方 ·４００· 智 能 系 统 学 报 第 １０ 卷