第6期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1061· 来完成驱动功能的钢丝绳分成两部分，在驱动线 行角度转换，建立从(a,By)到(9,0，-p)的转换关系。 孔中的部分，长度为h,处于两个导线圆盘之间的 根据旋转公式可求得旋转顺序为ZYX的旋 部分，长度分别为1和2。假设连续型机器人有 转变换矩阵为 n个关节，则其直立状态时驱动线长度为L=心 a11 a12 a13 (h+lo)。 R(@.B.y)=Rz(@)Rx(B)Rz(Y)= a21 22 a23 a31a32 a33 (6) cBca sBsyca-cysa sBcyca+sysa CBsa sBsysa +cyca sBcysa-syca -sB sycB cycB 由于姿态传感器测量Z轴旋转角度易受环境 磁场等因素的影响，所以这里避免使用角度α，联 合式(6)和式(1)中的旋转变换矩阵R,则可以得 到从姿态传感器角度到关节姿态的转换式(7)、 式(8)： Atan2(va+a,a3s),s0 0= 0 (7) 180 图5关节弯曲引起绳长变化示意图 Fig.5 Schematic of rope length changes caused by joint -Atan2(a32,-a31),s0≠0 bending 9= 0°，0=0° (8) 0°，0=180° 假设导线圆盘导线孔按逆时针进行编号，由 于每个关节由3根钢丝绳驱动，则相邻两孔夹角 1.24绳长空间到驱动信号空间的映射 为。=会在连续型机器人机械臂弯曲时，存在 连续型机器人绳长的变化控制最终将反映到 电机的驱动信号控制上，通过式(5)可以计算出 导线孔无线的情况，在该情况下，线长变化量为 连续型机器人从一个姿态转换到另一个姿态绳长 零，设第i关节段的关节变量为(0，)，由几何分 的变化量，这里选择直线模组驱动连续型机器人 析即可得到第关节，第j个孔的驱动线在该关节 的线长变化，直线模组导程为c,单位mm,设置 范围内的线长为 直线模组驱动器电机每转的脉冲个数为5，根据 L与=nH+cos2 -dsin ncos(+(j-D)a) 6 (3) 式(5)可计算出各个驱动绳长的变化值△1，设直 则线长变化量为 线模组速度为v,单位为mm/s,则可以得到绳长 A,=nk-beo号-dsin号casg,+U-ladj 变化量和步进电机驱动信号频率和脉冲的映射关 2n 系，如式(9)所示： 由于连续型机器人在进行关节运动时，越靠 m= .△ 近连续型机器人基座，穿过该关节的驱动线数量 (9) 越多，所有穿过该关节的驱动线线长都将随之改 f =-.v 变。在进行实际计算时，需要的是整体机械臂一 式中：m表示脉冲数目；f表示在设定速度v下的 个驱动线的变化量，则第导线孔驱动线总线长 频率，单位为Hz。 变化量△为 2操作方法研究 (5) 2.1系统设计 1.2.3ZYZ欧拉角求解 为了验证连续型机器人的运动学，搭建了三 为了测量连续型机器人的姿态，这里采用姿 关节的连续型机器人控制系统，如图6所示，连续 态传感器采集连续型机器人的姿态信息。由连续 型机器人平台由PC、MCU、电源、步进电机驱动 型机器人单关节段建模可知，其坐标旋转变换按 器、姿态传感器、直线模组、连续型机器人机械臂 照Z-Y-Z次序，分别为(，6，-p),而姿态传感器测 等组成。连续型机器人柔性机械臂由3个关节组 量的姿态角的旋转变换按照ZY-X次序，假设分 成，单关节长度220mm,驱动部分由9个直线模 别为（α，B,y),因此无法直接将姿态传感器的输出 组构成。 角度应用到连续型机器人的姿态测量上，需要进 由式(7)和式(8)可知，能够由姿态传感器得h l1 l2 n L = n· (h+l0) 来完成驱动功能的钢丝绳分成两部分，在驱动线 孔中的部分，长度为 ，处于两个导线圆盘之间的 部分，长度分别为 和 。假设连续型机器人有 个关节，则其直立状态时驱动线长度为 。 x h y z θ l2 l1 l0 /2 d/2 d/2 图 5 关节弯曲引起绳长变化示意图 Fig. 5 Schematic of rope length changes caused by joint bending α = 2π 3n (θi ,φi) 假设导线圆盘导线孔按逆时针进行编号，由 于每个关节由 3 根钢丝绳驱动，则相邻两孔夹角 为 。在连续型机器人机械臂弯曲时，存在 导线孔无线的情况，在该情况下，线长变化量为 零，设第 i 关节段的关节变量为 ，由几何分 析即可得到第 i 关节，第 j 个孔的驱动线在该关节 范围内的线长为 Li j =n [ H+l0 cos θi 2n −d sin θi 2n cos(φi +(j−1)α) ] (3) 则线长变化量为 ∆Li j = n { l0 − [ l0 cos θi 2n −d sin θi 2n cos(φi +(j−1)α) ]} (4) ∆lj 由于连续型机器人在进行关节运动时，越靠 近连续型机器人基座，穿过该关节的驱动线数量 越多，所有穿过该关节的驱动线线长都将随之改 变。在进行实际计算时，需要的是整体机械臂一 个驱动线的变化量，则第 j 导线孔驱动线总线长 变化量 为 ∆Lj = ∑n i=1 ∆Li j (5) 1.2.3 ZYZ 欧拉角求解 (φ, θ,−φ) (α, β, γ) 为了测量连续型机器人的姿态，这里采用姿 态传感器采集连续型机器人的姿态信息。由连续 型机器人单关节段建模可知，其坐标旋转变换按 照 Z-Y-Z 次序，分别为 ，而姿态传感器测 量的姿态角的旋转变换按照 Z-Y-X 次序，假设分 别为 ，因此无法直接将姿态传感器的输出 角度应用到连续型机器人的姿态测量上，需要进 行角度转换，建立从 (α, β, γ) 到 (φ, θ,−φ) 的转换关系。 根据旋转公式可求得旋转顺序为 Z-Y-X 的旋 转变换矩阵为 R(α,β,γ) = RZ(α)RY(β)RZ(γ) =   a11 a12 a13 a21 a22 a23 a31 a32 a33   =   cβcα sβsγcα−cγsα sβcγcα+ sγsα cβsα sβsγsα+cγcα sβcγsα− sγcα −sβ sγcβ cγcβ   (6) α R 由于姿态传感器测量 Z 轴旋转角度易受环境 磁场等因素的影响，所以这里避免使用角度 ，联 合式 (6) 和式 (1) 中的旋转变换矩阵 ，则可以得 到从姿态传感器角度到关节姿态的转换式 (7)、 式 (8)： θ =    Atan 2( √ a 2 31 +a 2 32,a33), sθ , 0 0 ◦ 180◦ (7) φ =    −Atan 2(a32,−a31), sθ , 0 0 ◦ , θ = 0 ◦ 0 ◦ , θ = 180◦ (8) 1.2.4 绳长空间到驱动信号空间的映射 c s ∆l v 连续型机器人绳长的变化控制最终将反映到 电机的驱动信号控制上，通过式 (5) 可以计算出 连续型机器人从一个姿态转换到另一个姿态绳长 的变化量，这里选择直线模组驱动连续型机器人 的线长变化，直线模组导程为 ，单位 mm，设置 直线模组驱动器电机每转的脉冲个数为 ，根据 式 (5) 可计算出各个驱动绳长的变化值 ，设直 线模组速度为 ，单位为 mm/s，则可以得到绳长 变化量和步进电机驱动信号频率和脉冲的映射关 系，如式 (9) 所示：    m = s c ·∆l f = s c · v (9) 式中：m 表示脉冲数目； f 表示在设定速度 v 下的 频率，单位为 Hz。 2 操作方法研究 2.1 系统设计 为了验证连续型机器人的运动学，搭建了三 关节的连续型机器人控制系统，如图 6 所示，连续 型机器人平台由 PC、MCU、电源、步进电机驱动 器、姿态传感器、直线模组、连续型机器人机械臂 等组成。连续型机器人柔性机械臂由 3 个关节组 成，单关节长度 220 mm，驱动部分由 9 个直线模 组构成。 由式 (7) 和式 (8) 可知，能够由姿态传感器得 第 6 期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1061·