·56 北京科技大学学报 第34卷 Vir ro+gseVengsnro+gsvingshror (4) Gpa diag(gra'gpA'gPAs'8PA) 式中，r为足端D,在坐标系S下的坐标 Jn(0=diag(J,(0,),Jm(0,),J,(0),J(0,)）: 利用坐标变换ri,=gr哈=gp8rr哈和抬起 %=》:p=GwG:24=GnnG 腿的雅克比矩阵，出=J（0,),上式可以化 式(6)是雅克比公式的推广，揭示了机器人足端速 简为 度。、车体位姿速度、车体变形速度和关节 gsrgmdD (0)0:=vo-Vir"ro-gsrVingsnro 转角速度之间的关系0，称为全局速度方程，J。(0) (5) 称为全局雅克比矩阵 写成矩阵形式为 3分解运动速度控制 GsGpAJp (0)0=V-VspTo-GsrVrGsnFo (6) 分解运动速度控制是一种常用的机器人操作臂 式中： 控制方法，指各关节电机转速相互配合以获得机器 =r)r(r,)T(r,)r(r)门： 人沿任意直角坐标轴的运动.如图6(a)所示，这种 控制方法是一种基于直角坐标的控制，可以直观方 0=〖a)r(82)r(a)r(84)T: 便地在任务空间里进行轨迹规划，不需要进行复杂 Gsp diag (gsp'gsp'gsp'8sp): 的逆运动学求解，而是用计算雅克比矩阵的逆代替 ②→口→控制器 操作臂 16 J.(e) 控制器◆机器人 速度 方程 日传感器 传感器 a) b 图6速度分解控制框图.(a)机械臂：(b)腿式机器人 Fig.6 Velocity decomposition control:(a)manipulators:(b)legged robots 利用全局速度方程，可以将分解运动速度控制 代公式计算： 的方法推广到腿式移动机器人的运动控制中.首先 X1=X(21-J+1X). 在任务直角坐标空间中规划机器人的车身运动轨迹 式中j=0,1,…,n)为迭代次数，为单位矩阵，X 和足端运动轨迹，给出车身位姿矩阵Gs和足端轨 为迭代变量，初始值取X。=J满足11-J+1X|<1. 迹向量，对于本文提到的四足变结构机器人还要 当总迭代次数n足够大时，将迭代变量X。作为J 给出车身位形矩阵G·然后，根据这些直角坐标空 的近似值.由于采样时间很小，J和J,非常接 间中的给出量计算出相应的车体位姿速度、足 近，所以进行一到两次迭代就能满足大部分精度要 端速度和车体变形速度通过全局速度方程， 求，大大降低了计算量 求出关节坐标速度0，作为控制信号传给运动控制 下面使用速度分解的控制方法来计算让机器人 器，控制关节电机运动 完成车身原地收缩动作所需要的关节运动轨迹，如 整个过程的控制框图如图6(b)所示，可以看出 图7所示.己知机器人的腿部结构参数为山1= 该控制方法在实际的机器人运动控制过程中需要实 85mm、l2=335mm和l,3=440mm,前后腿胯关节的 时计算每个采样时间的全局雅克比的逆J。(). 距离为2l=800mm.假设运动时车身姿态保持水 实际应用中存在两个问题：一是计算量较大：二是当 平，四足着地，立足点位置不变，车身的高度为H= 机器人的某一条腿处于奇异位置时，全局雅克比的 750mm,车身宽度是关于运动时间t(单位：s)的函 逆不存在.对于后者，可以在作机器人的运动轨迹 数2b=1005-cos(mt/10)],t∈0,10] 规划时绕开腿部奇异位置进行规避.对于前者，可 计算时采用的采样周期为△T=0.01s,求全局 以采用迭代法近似计算全局雅克比的逆来达到减少 雅克比的逆迭代次数为一次，即J+,=J'(21- 计算量的目的.首先，计算初始状态时机器人的全 Jk+J).图8(a）、(b)和(c)展示的是用分解速度 局雅克比J。以及它的逆J。,对于第k+1个采样时 控制和全局速度方程求出的腿1的三个关节轨迹仿 刻的机器人全局雅克比的逆J,可以用下面的迭 真计算结果.图8(d)表示用速度分解的控制方北 京 科 技 大 学 学 报 第 34 卷 ^ VS SP ·r S Di + gSP ^ VS PAi g － 1 SP·r S Di + gSAi ^ VS Ai Di g － 1 SAi ·r S Di ． ( 4) 式中，r S Di 为足端 Di 在坐标系 S 下的坐标． 利用坐标变换 r S Di = gSAi ·r Ai Di = gSP gPAi ·r Ai Di 和抬起 腿的雅克比矩阵 ^ VS Ai Di r Ai Di = JDi ( θi ) θ · i，上式可以化 简为 gSP gPAi JDi ( θi ) θ · i = vS Di － ^ VS SP ·r S Di － gSP ^ VS PAi g － 1 SP·r S Di ， ( 5) 写成矩阵形式为 GSPGPA JD( θ) θ · = vS D － ^ VS SP ·r S D － GSP ^ VS PAG － 1 SP·r S D． ( 6) 式中: r S D = ［( r S D1 ) T ( r S D2 ) T ( r S D3 ) T ( r S D4 ) T ］T ; θ · = ［( θ · 1 ) T ( θ · 2 ) T ( θ · 3 ) T ( θ · 4 ) T ］T ; GSP = diag( gSP，gSP，gSP，gSP ) ; GPA = diag( gPA1 ，gPA2 ，gPA3 ，gPA4 ) ; JD( θ) = diag( JD1 ( θ1 ) ，JD2 ( θ2 ) ，JD3 ( θ3 ) ，JD4 ( θ4 ) ) ; vS D = r ·S D; ^ VS SP = G · SPG － 1 SP ; ^ VS PA = G · PAG － 1 PA ． 式( 6) 是雅克比公式的推广，揭示了机器人足端速 度 vS D、车体位姿速度 ^ VS SP、车体变形速度 ^ VS PA和关节 转角速度之间的关系 θ · ，称为全局速度方程，JD ( θ) 称为全局雅克比矩阵． 3 分解运动速度控制 分解运动速度控制是一种常用的机器人操作臂 控制方法，指各关节电机转速相互配合以获得机器 人沿任意直角坐标轴的运动． 如图 6( a) 所示，这种 控制方法是一种基于直角坐标的控制，可以直观方 便地在任务空间里进行轨迹规划，不需要进行复杂 的逆运动学求解，而是用计算雅克比矩阵的逆代替． 图 6 速度分解控制框图． ( a) 机械臂; ( b) 腿式机器人 Fig． 6 Velocity decomposition control: ( a) manipulators; ( b) legged robots 利用全局速度方程，可以将分解运动速度控制 的方法推广到腿式移动机器人的运动控制中． 首先 在任务直角坐标空间中规划机器人的车身运动轨迹 和足端运动轨迹，给出车身位姿矩阵 GSP和足端轨 迹向量 r S D，对于本文提到的四足变结构机器人还要 给出车身位形矩阵 GPA ． 然后，根据这些直角坐标空 间中的给出量计算出相应的车体位姿速度 ^ VS SP、足 端速度 vS D 和车体变形速度 ^ VS PA通过全局速度方程， 求出关节坐标速度 θ · ，作为控制信号传给运动控制 器，控制关节电机运动． 整个过程的控制框图如图 6( b) 所示，可以看出 该控制方法在实际的机器人运动控制过程中需要实 时计算每个采样时间的全局雅克比的逆 J － 1 D ( θ) ． 实际应用中存在两个问题: 一是计算量较大; 二是当 机器人的某一条腿处于奇异位置时，全局雅克比的 逆不存在． 对于后者，可以在作机器人的运动轨迹 规划时绕开腿部奇异位置进行规避． 对于前者，可 以采用迭代法近似计算全局雅克比的逆来达到减少 计算量的目的． 首先，计算初始状态时机器人的全 局雅克比 J0 以及它的逆 J － 1 0 ，对于第 k + 1 个采样时 刻的机器人全局雅克比的逆 J － 1 k + 1，可以用下面的迭 代公式计算: Xj + 1 = Xj ( 2I － Jk + 1Xj ) . 式中，j( j = 0，1，…，n) 为迭代次数，I 为单位矩阵，Xj 为迭代变量，初始值取 X0 = J －1 k 满足|I － Jk +1X0 | ＜ 1． 当总迭代次数 n 足够大时，将迭代变量 Xn 作为 J － 1 k + 1 的近似值． 由于采样时间很小，J － 1 k 和 J － 1 k + 1 非常接 近，所以进行一到两次迭代就能满足大部分精度要 求，大大降低了计算量． 下面使用速度分解的控制方法来计算让机器人 完成车身原地收缩动作所需要的关节运动轨迹，如 图 7 所 示． 已知机器人的腿部结构参数为 l1 = 85 mm、l2 = 335 mm 和 l3 = 440 mm，前后腿胯关节的 距离为 2l = 800 mm． 假设运动时车身姿态保持水 平，四足着地，立足点位置不变，车身的高度为 H = 750 mm，车身宽度是关于运动时间 t( 单位: s) 的函 数 2b = 100［5 － cos( πt /10) ］，t∈［0，10］． 计算时采用的采样周期为 ΔT = 0. 01 s，求全局 雅克比的逆迭代次数为一次，即 J － 1 k + 1 = J － 1 k ( 2I － Jk + 1 J － 1 k ) ． 图 8( a) 、( b) 和( c) 展示的是用分解速度 控制和全局速度方程求出的腿 1 的三个关节轨迹仿 真计算结果． 图 8 ( d) 表示用速度分解的控制方 ·56·