第4章 速度运动学——雅可比矩阵
在数学上，正运动学方程在笛卡尔位置和姿态空间与关节位置空间之间定义了一个函数，速度之间的关系由这个函数的雅可比矩阵来决定。
雅可比矩阵出现在机器人操作的几乎各个方面：规划和执行光滑轨迹，决定奇异位形，执行协调的拟人动作，推导运动的动力学方程，力和力矩在末端执行器和机械臂关节之间的转换。
1.角速度：固定转轴情形
k θ
ω =（k 是沿旋转轴线方向的一个单位向量，θ 是角度θ对时间的倒数） 2.反对称矩阵
一个n n ?的矩阵S
被称为反对称矩阵，当且仅当0=+S S T
,我们用)3(so 表示所有
33?反对称矩阵组成的集合。
如果)3(so S ∈，反对称矩阵满足0=+ji ij s s 3,2,1,=j i ，所以ii S =0，S 仅包含三个独立项，并且每个33?的反对称矩阵具有下述形式：
????
?
?????---=0001
2
13
23s s s s s s S 如果T
z y x a a a a ),,(=是一个3维向量，我们将对应的反对称矩阵)(a S 定义为如下形式：
????
?????
?---=000
)(x
y x z
y z a a a a a a a S 反对称矩阵的性质
1）)()()(b S a S b a S βαβα+=+ 向量a 、b 属于3
R ，α、β为标量
2）p a p a S ?=)( 向量a 、b 属于3R ，p a ?表示向量叉乘
3）)()(Ra S R a RS T
=，左侧表示矩阵)(a S 的一个相似变换，这个公式表明：)(a S 在坐标系中经过R 旋转操作的矩阵表示与反对称矩阵)(a SR 相同，其中)(a SR 对应于向量a 被转过R 这种情形。
4）对于一个n n ?的反对称矩阵S ,以及任何一个向量n R X ∈，有0=SX X T
旋转矩阵的导数
)(θθ
SR R d d
= 公式表明：计算旋转矩阵的R 的导数，等同于乘以一个反对称矩阵S 的矩阵乘法操作。
3.角速度：一般情况
)())(()(t R t w S t R
= ，其中，矩阵))((t w S 是反对称矩阵，向量)(t w 为t 时刻旋转坐标系相对于固定坐标系上的点p 。 4.角速度求和
假定我们有112010...-=n n n R R R R ，则00,00)(n
n n R S R ω= ，其中 0,104
,303
,202,10
1,01,10134,30323,20212,10101,00,0......n
n n n
n n n R R R R ----+++++=+++++=ω
ω
ω
ωωωωωωωω
（0
2,1ω表示对应于1
2R 导数的角速度在坐标系0000z y x o 中的表达式） 5.移动坐标系上点的线速度
v r o Rp S o p R p
+?=+=+=ωω 110)( 其中，1
Rp r =是从1
o 到p 的向量在坐标系0000z y x o 的姿态中的表达式，v 是原点1o 运
动的速度。 6.雅可比矩阵的推导
当机器人运动时，关节变量i q 以及末端执行器的位置0
n o 和姿态0
n R 都将为时间的函数。
q
J =ξ 其中，ξ和J 由下式给出
??
????=00
n n v ωξ 和 ??????=w v J J J
向量ξ有时被称为体速度。矩阵J 被称为机械臂的雅可布矩阵。
角速度：
末端执行器相对于基座坐标系的总的角速度0
n ω：
011
01
01
22110
...-=-∑=+++=i i n i i n n n n
z q k R q k R q k q
ρρρρω 其中，关节i 为转动时，i ρ等于1,；而关节i 为平动时，i ρ等于0。这是因为
k R z i i 0101--=，当然，T k z )1,0,0(0
0==。
所以 )...(101-=n n w z z J ρρ
线速度：
i
n
vi q o J ??=0，雅可比矩阵的第i 列可以通过下列方式生成：固定除第i 个关节之外的所有关节，同时以单位速度驱动第i 个关节。
平动关节：
1-=i v z J i
转动关节：
)(11---?=i n i v o o z J i
小结：
雅可比矩阵的上半部分v J 由下式给出：
)...(1n v v v J J J =
其中，矩阵的第i 列i v J 为
()
?
?
?-?=---i z i o o z J i i n i v i 对于平动关节对于转动关节111 雅可比矩阵的下半部分为
)...(1n J J J ωωω=
其中，矩阵的第i 列??
?=-i
i
z J i i 对于平动关节对于转动关节0
1
ω
计算雅可比矩阵仅需知道单位向量i z 以及原点n o o ,...,1的坐标。
i z 相对于基座坐标系的坐标，可由0
i T 第3列中的3个元素给出，同时i o 由0
i T 第4列中的3个元素给出。
7.工具速度
末端执行器和工具坐标系之间的固定空间关系由下列恒定齐次变换矩阵给出：
?
?
????=106
d R T tool
由于两个坐标系之间的刚性连接，工具坐标系的角速度与末端执行器坐标系的角速度相等。
如果末端执行器坐标系以T
T T v ),(66ωξ=的体速度运动，那么工具坐标系的原点（它被
刚性固连到末端执行器坐标系中）的线速度由下式给出：
r v v tool ?+=66ω，r 是从末端执行器坐标系原点到工具坐标系原点的向量。
8.分析雅可比矩阵
前面推导的雅可比矩阵有时称为几何雅可比矩阵，这是为了区别于分析雅可比矩阵，后者表示为)(q J a ，它基于对末端执行器姿态的最小表示，令
??
?
???=)()(q q d X α表示末端执行器的姿态，其中，)(q d 是从基座坐标系原点到末端执行器坐标系原点的一般向量，α表示末端执行器坐标系相对于基座坐标系的姿态的最小表示。
分析雅可比矩阵)(q J a 可以根据几何雅可比计算如下
q q J a B I d a B I B d
v q q J a a )()(00)(00)()(??????=????????????=??????=??????=αα
αω （条件：0)(det ≠αB ）
9.奇点
所有可能的末端执行器速度是雅可比矩阵向量的线性组合：
n n q J q J q
J +++=...2211ξ 矩阵的秩（rank ）等于矩阵中线性独立的列（或行）的数目。因此，当6=rankJ ，末端执行器可以以任意速度运行。
与矩阵)(q J 的秩小于其最大值情况相对应的位形被称为奇点或奇异位型。
识别机械臂的奇点很重要，其原因如下：
对那些带有球型手腕机械臂的奇异位形进行解耦，即分解为两个更简单的问题。第一个问题是确定所谓的手臂奇点，也就是由于手臂（包括前三个或更多连杆）运动而产生的奇点；第二个问题是确定由球型手腕运动而引起的手腕奇点。
如果机械臂由一个3自由度手臂和一个3自由度球型手腕构成，我们将雅可比矩阵J 分解成如下33?的矩阵块：
???
?
??==22122111|)|(J J J J J J J Q P
因此，机械臂奇异位形的集合是满足0det 11=J 的手臂位形集合以及满足0det 22=J 的手腕位形集合的并集。
任何两个转动关节轴共线时，都将会产生奇点，这是因为两个转角相等但方向相反的旋转所对应的综合结果是末端执行器保持不动。 10.静态力/力矩关系
令T
z y x z y x n n n F F F F ),,,,,(=表示末端执行器的力和力矩向量。令τ表示对应的关节力矩向量。那么，F 和τ之间可以由下式联系起来：
F q J T )(=τ
其中，)(q J T
是机械臂雅可比矩阵的转置。
11.逆速度和加速度
逆速度问题是指，求解生成期望末端执行器速度所需的关节速度q
ξ1-=J q
对于6自由度机械臂，逆速度和逆加速度方程可被写为
q q J dt d q q J q
a a ??
? ??+=)()( 以及
???
??
???? ??-=-q
q J dt d X q J q a a )()(1 上述公式成立的条件是：0)(det ≠q J a