Page 9 - 《振动工程学报》2026年第3期

第 3 期 陈 刚，等： 驾驶机器人换挡机械手非线性动力学特性分析 609

T
q =( x ij y ij θ ij x pc y pc θ pc ) ；i = 1，2，3；j = 1，2 不含关节间隙的换挡机械手系统理想刚体模型
（13） 在广义坐标系下的牛顿⁃欧拉方程为：
在未考虑换挡机械手的关节间隙时，每个关节 Mq ̈ = g c （17）
受 两 个 方 程 约 束 ，故 换 挡 机 械 手 的 运 动 学 约 束 方 式 中 ， M 为 系 统 的 质 量 矩 阵 ， M = diag( m ij，m ij，I ij，
程为： m pc，m pc，I pc ) ( i = 1，2，3；j = 1，2 )，其中，m ij 和 I ij 分别
é ê ê e i1 - r i1 u i1 - e Ai ù ú ú 表示连杆 ij 的质量和惯量，m pc 和 I pc 分别表示手杆的
ê ê ú ú
ê ê e i2 - r i2 u i2 - l i1 u i1 - e Ai ú ú 质量和惯量； g c 为换挡机械手系统的广义外力。
ê
H = ê êl 11 u 11 + l 12 u 12 - l 22 u 22 - l 21 u 21 - e A2 ú ú = 0 在考虑驾驶机器人换挡机械手相应关节处的间
ê
l
ê ê 11 u 11 + l 12 u 12 - l 32 u 32 - l 31 u 31 - e A3 ú ú
ê ê ú ú 隙时，换挡机械手的相应关节处解除运动学约束，取
ê ê e pc - r pc u pc - l 11 u 11 - l 12 u 12 ú ú
ë θ pc - θ 12 û 而代之的是该关节处以关节接触力的形式进行约
（14） 束，即将几何约束转化为力约束。每多考虑一处关
T
式中， e ij 表示连杆 l ij 质心的全局坐标，e ij =( x ij y ij) ； 节间隙，换挡机械手在该关节处增加 2 个自由度，于
T 是系统约束方程数量相应减少。
e Ai 表示关节 A i 的全局坐标，e Ai =( x ij y ij) ； e pc 表示
考虑关节间隙的驾驶机器人换挡机械手动力学
手杆的质心位置的全局坐标； u ij 和 u pc 表示分别连杆
模型为 ［17］ ：
T
l ij 和 手 杆 l pc 的 单 位 方 向 向 量 ，u ij =( cosθ ij sinθ ij) ， Mq ̈ + J q λ = g （18）
T
T
u pc =( cosθ pc sinθ pc) 。 式中， λ 为拉格朗日乘子向量； g 为广义力向量， g =
分别对运动学约束方程中的广义坐标 q 和时间 g c + g d，其中 g d 为关节间隙接触力的等效力。

t 求一阶导数，得到系统雅可比矩阵 J q 和矩阵 γ，进 合并式（16）和（18），得到如下非线性动力学微
一步导出系统的速度约束方程和加速度约束方程。 分代数方程：
g
系统的速度约束方程为： ( M J q T )( ) ( ) （19）
q ̈
=
J q q ̇ = 0 （15） J q 0 λ γ
式中， J q 为换挡机械手系统的速度传递矩阵（即雅可 由于运动学约束方程和速度约束方程只在第一
比矩阵），其表达式如下： 个积分步骤得到满足，式（19）为一个不稳定系统，采

ê é M 11 0 0 0 0 0 0 ù 用 Baumgarte 方法稳定约束，得到的非线性动力学
ú
ú
ê ê ú 方程表示为：
ê ê N 11 M 12 0 0 0 0 0 ú
ú
ê ê 0 0 M 21 0 0 0 0 ú
g
ê ê ú ( M J q T )( ) ( )
q ̈
ú
ê ê 0 0 N 21 M 22 0 0 0 ú = （20）
ú
ê ê 0 0 0 0 0 0 ú J q 0 λ γ′
J q = ê ê M 31 ú，
ú
ê ê 0 0 0 0 N 31 M 32 0 ú 式中， γ'= γ - 2αH - β H，其中 α 和 β 为 Baumgarte
2
ú
0 0 0 ú
ê ê-N 11 -N 12 N 21 N 22 ú
ê ê ú 修正因子。
ê ê -N 11 -N 12 0 0 N 31 N 32 0 ú 求解式（20）的非线性动力学方程，可以得到换
ú
ú
ê ê ê N 11 N 12 0 0 0 0 N pc ú
ë 0 K 1 0 0 0 0 K 2 û 挡机械手末端位移误差为：
T
) ， ì 2 ∂x p ∂x p )
式 中 ，M ij =[ I 2 × 2 r ij w ij]，w ij =( sinθ ij - cosθ ij ï ï Δl 1k + Δθ 1k
ï Δx p =
) T ï ∑( ∂l 1k ∂θ 1k
ï ï
k = 1
N pc =[ I 2 × 2 r pc w pc]， w pc =( sinθ pc -cosθ pc ， N ij = í （21）
ï ï 2 ∂y p ∂y p )
ï Δy p =
[ 0 2×2 l ij w ij] ， K 1 =[0 0 -1] ， K 2 =[0 0 1]。 ï ∑( ∂l 1k Δl 1k + ∂θ 1k Δθ 1k
ï ï
î
k = 1
对运动学约束方程中的时间求二阶导数，得到
进一步求导可以得到末端速度误差和加速度误
换挡机械手系统的加速度约束方程为：
差分别为：
J q q ̈ = γ （16）
ì 2 ∂v px ∂v px ∂v px )
ï ï
̇
ù ú ú
式中， γ 为力向量，且 γ =-êê ê é ê ( J q q ̇ ) q ̇ + 2J qt q ̇ + J tt ， ï ï Δv px = ∑ Δl 1k + ∂θ 1k Δθ 1k + ̇ Δθ 1k
ë q û ï ï k = 1( ∂l 1k ∂θ 1k
í
其中， q ̇ 为广义速度， J qt 为雅可比矩阵对时间的一阶 ï ï 2 ∂v py ∂v py ∂v py )
̇
ï ï Δv py = ∑ Δl 1k + Δθ 1k + Δθ 1k
̇
导数， J tt 为矩阵 H 对时间 t 的二阶导数； q ̈ 为广义加 ï ï k = 1( ∂l 1k ∂θ 1k ∂θ 1k
î
速度。 （22）