1. 基础知识

基础知识主要是希望大家回顾一下大学物理里讲的速度矢量和角速度矢量部分的知识，用矢量形式去表示，还有叉乘的相关知识，这些我就不赘述了。

想象存在坐标系{A}和{B}，把{B}固连在某一刚体上，要求描述BQ^BQBQ相对于坐标系{A}的运动。
首先位置我们是可以确定的，用位置矢量APBORG^AP_{BORG}APBORG和旋转矩阵BAR^A_BRBAR来表示。那么求解速度也是一样的：
AVQ^AV_QAVQ = AVBORG^AV_{BORG}AVBORG + BAR^A_BRBAR BVQ^BV_QBVQ
但是要注意，上式只适用于两坐标系姿态保持不变的情况！！

我们再考虑一种情况：两坐标系原点重合且相对线速度为零，固定在坐标系{B}上的矢量BQ^BQBQ以角速度AΩB^A\Omega_BAΩB相对于坐标系{A}旋转。那么从{A}看固定在{B}中的矢量是如何随时间变化呢？可以根据下图分析：

首先，显然AQ^AQAQ的微分增量一定垂直于AΩB^A\Omega_BAΩB和AQ^AQAQ；
其次，可以看出微分增量的大小为|ΔQ\Delta QΔQ| = (|AQ^AQAQsin⁡θ\sin\thetasinθ|) (|AΩB^A\Omega_BAΩB| Δt\Delta tΔt)
回顾大学物理的知识，我们可以得到
QVQ^QV_QQVQ = AΩB^A\Omega_BAΩB ×\times× AQ^AQAQ
而且一般情况下，Q是相对于{B}变化的，这就要加上一个分量，得到下式
AVQ^AV_QAVQ = A(BVQ)^A(^BV_Q)A(BVQ) + AΩB^A\Omega_BAΩB ×\times× AQ^AQAQ
AVQ^AV_QAVQ = BARBVQ^A_BR^BV_QBARBVQ + AΩB^A\Omega_BAΩB ×\times× BARBQ^A_BR^BQBARBQ
实际上，我们也很容易将上式的情况扩展到原点不重合的情况，无非就是再加一个原点线速度的分量，那么最终我们就可以推导出从固定坐标系观测运动坐标系中的矢量导数的最终结果：
AVQ^AV_QAVQ = AVBORG^AV_{BORG}AVBORG + BARBVQ^A_BR^BV_QBARBVQ + AΩB^A\Omega_BAΩB ×\times× BARBQ^A_BR^BQBARBQ

2. 连杆的速度“传递”

就如同位姿可以通过连杆间的推导从基座标推导到末端坐标系一样，线速度与角速度也可以。

首先我们看角速度。
当两个ω矢量都是相对于相同坐标系的时候，二者可以相加。那么i+1的角速度就等于连杆i的角速度加一个由于i+1角速度引起的分量，参照坐标系{i}就可以推导出：
iωi+1^i\omega_{i+1}iωi+1 = iωi^i\omega_iiωi + i+1iR_{i+1}^iRi+1iR θ˙i+1\dot{θ}_{i+1}θ˙i+1i+1Z^i+1^{i+1}\hat{Z}_{i+1}i+1Z^i+1
式中
θ˙i+1\dot{\theta}_{i+1}θ˙i+1 i+1Z^i+1^{i+1}\hat{Z}_{i+1}i+1Z^i+1 = i+1(00θ˙i+1)^{i+1}\begin{pmatrix}0\\0\\ \dot{\theta}_{i+1}\end{pmatrix}i+1⎝⎛00θ˙i+1⎠⎞
上式中我们给两边同时左乘 ii+1R^{i+1}_iRii+1R，最终要得到连杆i+1的角速度相对于坐标系{i+1}的表达式：
i+1ωi+1^{i+1}\omega_{i+1}i+1ωi+1 = ii+1R_i^{i+1}Rii+1R iωi^i\omega_iiωi + θ˙i+1\dot{θ}_{i+1}θ˙i+1 i+1Z^i+1^{i+1}\hat{Z}_{i+1}i+1Z^i+1

接下来看线速度。
坐标系{i+1}的原点的线速度等于坐标系{i}原点的线速度加上一个由于连杆i+1的角速度引起的分量，这与我么再第0节推导的结果一样，不是很清楚的可以回到上面再仔细看看，因此我们可以得到
ivi+1^iv_{i+1}ivi+1 = ivi^iv_iivi + iωi^i\omega_iiωi ×\times× iPi+1^iP_{i+1}iPi+1
同样的两边左乘 ii+1R^{i+1}_iRii+1R，得到
i+1vi+1^{i+1}v_{i+1}i+1vi+1 = ii+1R_i^{i+1}Rii+1R(ivi+iωi×iPi+1)\left( \ ^iv_i+ ^i\omega_i \times ^iP_{i+1} \right)( ivi+iωi×iPi+1)

这两个重要的公式希望大家能够好好理解：
i+1ωi+1^{i+1}\omega_{i+1}i+1ωi+1 = ii+1R_i^{i+1}Rii+1R iωi^i\omega_iiωi + θ˙i+1\dot{θ}_{i+1}θ˙i+1 i+1Z^i+1^{i+1}\hat{Z}_{i+1}i+1Z^i+1
i+1vi+1^{i+1}v_{i+1}i+1vi+1 = ii+1R_i^{i+1}Rii+1R (ivi+iωi×iPi+1)\left( \ ^iv_i+ ^i\omega_i \times ^iP_{i+1} \right)( ivi+iωi×iPi+1)
以上是对于旋转关节的情况，如果是移动关节，那么公式就是下面的形式：
i+1ωi+1^{i+1}\omega_{i+1}i+1ωi+1 = ii+1R_i^{i+1}Rii+1R iωi^i\omega_iiωi
i+1vi+1^{i+1}v_{i+1}i+1vi+1 = ii+1R_i^{i+1}Rii+1R(ivi+iωi×iPi+1)\left( \ ^iv_i+ ^i\omega_i \times ^iP_{i+1} \right)( ivi+iωi×iPi+1) + d˙i+1\dot{d}_{i+1}d˙i+1 i+1Z^i+1^{i+1}\hat{Z}_{i+1}i+1Z^i+1

我们可以通过一个平面两连杆机械臂来理解这组公式。
机械臂的参数以及坐标系确定如下，

经过前面运动学的讲解，这里可以很容易地求出各个齐次变换矩阵
10T^0_1T10T = (c1−s100s1c10000100001)\begin{pmatrix} c_1 & -s_1 & 0 & 0 \\ s_1 & c_1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{pmatrix}⎝⎜⎜⎛c1s100−s1c10000100001⎠⎟⎟⎞

21T^1_2T21T = (c2−s20l1s2c20000100001)\begin{pmatrix} c_2 & -s_2 & 0 & l_1 \\ s_2 & c_2 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{pmatrix}⎝⎜⎜⎛c2s200−s2c2000010l1001⎠⎟⎟⎞

32T^2_3T32T = (100l2010000100001)\begin{pmatrix} 1 & 0 & 0 & l_2 \\ 0 & 1& 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{pmatrix}⎝⎜⎜⎛100001000010l2001⎠⎟⎟⎞
对各个连杆依次使用速度和角速度的两个公式：
i+1ωi+1^{i+1}\omega_{i+1}i+1ωi+1 = ii+1R_i^{i+1}Rii+1R iωi^i\omega_iiωi + θ˙i+1\dot{θ}_{i+1}θ˙i+1 i+1Z^i+1^{i+1}\hat{Z}_{i+1}i+1Z^i+1
i+1vi+1^{i+1}v_{i+1}i+1vi+1 = ii+1R_i^{i+1}Rii+1R (ivi+iωi×iPi+1)\left( \ ^iv_i+ ^i\omega_i \times ^iP_{i+1} \right)( ivi+iωi×iPi+1)
我们可以得到，
1ω1^1\omega_11ω1 = (00θ˙1)\begin{pmatrix} 0\\ 0\\ \dot\theta_1 \end{pmatrix}⎝⎛00θ˙1⎠⎞

1v1^1v_11v1 = (000)\begin{pmatrix} 0\\ 0\\ 0 \end{pmatrix}⎝⎛000⎠⎞

2ω2^2\omega_22ω2 = (00θ˙1+θ˙2)\begin{pmatrix} 0\\ 0\\ \dot\theta_1 + \dot\theta_2 \end{pmatrix}⎝⎛00θ˙1+θ˙2⎠⎞

2v2^2v_22v2 = (c2s20−s2c20000)\begin{pmatrix} c_2 & s_2 & 0\\ -s_2 & c_2 & 0\\ 0 & 0 & 0 \end{pmatrix}⎝⎛c2−s20s2c20000⎠⎞ (0l1θ˙10)\begin{pmatrix} 0\\ l_1\dot\theta_1\\ 0 \end{pmatrix}⎝⎛0l1θ˙10⎠⎞ = (l1s2θ˙1l1c2θ˙10)\begin{pmatrix} l_1s_2\dot{\theta}_1 \\ l_1c_2\dot{\theta}_1 \\ 0 \end{pmatrix}⎝⎛l1s2θ˙1l1c2θ˙10⎠⎞

3ω3^3\omega_33ω3 = 2ω2^2\omega_22ω2

3ω3^3\omega_33ω3 = (l1s2θ˙1l1c2θ˙1+l2(θ˙1+θ˙2)0)\begin{pmatrix} l_1s_2\dot{\theta}_1\\ l_1c_2\dot{\theta}_1 + l_2(\dot\theta_1 + \dot\theta_2)\\ 0 \end{pmatrix}⎝⎛l1s2θ˙1l1c2θ˙1+l2(θ˙1+θ˙2)0⎠⎞
为了得到相对于基座标的速度和角速度，我们再给出旋转矩阵30R^0_3R30R

30R^0_3R30R = 10R^0_1R10R 21R^1_2R21R 32R^2_3R32R = (c12−s120s12c120001)\begin{pmatrix} c_{12} & -s_{12} & 0\\ s_{12} & c_{12} & 0\\ 0 & 0 & 1 \end{pmatrix}⎝⎛c12s120−s12c120001⎠⎞

那么通过这个变换，我们就可以得到，

0v3^0v_30v3 = (−l1s1θ˙1−l2s12(θ˙1+θ˙2)l1c1θ˙1+l2c12(θ˙1+θ˙1)0)\begin{pmatrix} -l_1s_1\dot{\theta}_1 - l_2s_{12}(\dot{\theta}_1 + \dot{\theta}_2)\\ l_1c_1\dot{\theta}_1 + l_2c_{12}(\dot{\theta}_1 + \dot{\theta}_1)\\ 0\end{pmatrix}⎝⎛−l1s1θ˙1−l2s12(θ˙1+θ˙2)l1c1θ˙1+l2c12(θ˙1+θ˙1)0⎠⎞

3. 雅可比

紧接着上面的推导出的速度0v3^0v_30v3，我们来讲一下雅可比矩阵。
简单地说，雅可比矩阵就是多维形式的导数，在这里它就是速度的映射，是一个时变的线性映射：Y˙\dot{Y}Y˙ = 0J(X)X˙^0J(X) \dot{X}0J(X)X˙ 。
在机器人学中，我们通常有以下形式：
0v^0\boldsymbol{v}0v = 0J(Θ)Θ˙^0J(\Theta) \dot{\Theta}0J(Θ)Θ˙
0v^0\boldsymbol{v}0v = (0v0ω)\begin{pmatrix} ^0v\\^0\omega \end{pmatrix}(0v0ω)
0v^0v0v是3 ×\times× 1的线速度矢量，0ω^0\omega0ω是3 ×\times× 1的角速度矢量。

那么根据0v3^0v_30v3 = (−l1s1θ˙1−l2s12(θ˙1+θ˙2)l1c1θ˙1+l2c12(θ˙1+θ˙1)0)\begin{pmatrix} -l_1s_1\dot{\theta}_1 - l_2s_{12}(\dot{\theta}_1 + \dot{\theta}_2)\\ l_1c_1\dot{\theta}_1 + l_2c_{12}(\dot{\theta}_1 + \dot{\theta}_1)\\ 0\end{pmatrix}⎝⎛−l1s1θ˙1−l2s12(θ˙1+θ˙2)l1c1θ˙1+l2c12(θ˙1+θ˙1)0⎠⎞，我们就可以得到雅可比矩阵
0J^0J0J = (−l1s1−l2s12−l2s12l1c1+l2c12l2c12)\begin{pmatrix} -l_1s_1- l_2s_{12} & -l_2s_12\\ l_1c_1 + l_2c_{12} & l_2c_12\\ \end{pmatrix}(−l1s1−l2s12l1c1+l2c12−l2s12l2c12)
另外再提一下，雅可比矩阵参考坐标系的变换。
已知坐标系{B}中的雅可比矩阵，即
(BvBω)\begin{pmatrix}^Bv \\^B\omega\end{pmatrix}(BvBω) = Bv^B\boldsymbol{v}Bv = BJ(Θ)Θ˙^BJ(\Theta)\dot{\Theta}BJ(Θ)Θ˙
容易得到{B}中 6 ×\times× 1的速度矢量在{A}中的变换，
(AvAω)\begin{pmatrix}^Av \\^A\omega\end{pmatrix}(AvAω) = (BAR00BAR)\begin{pmatrix}^A_BR & 0\\0 & ^A_BR\end{pmatrix}(BAR00BAR) (BvBω)\begin{pmatrix}^Bv \\^B\omega\end{pmatrix}(BvBω)
因此可以得到
(AvAω)\begin{pmatrix}^Av \\^A\omega\end{pmatrix}(AvAω) = (BAR00BAR)\begin{pmatrix}^A_BR & 0\\0 & ^A_BR\end{pmatrix}(BAR00BAR) BJ(Θ)Θ˙^BJ(\Theta)\dot{\Theta}BJ(Θ)Θ˙
显然，可以完成雅可比矩阵的参考坐标系的转换
AJ^AJAJ = (BAR00BAR)\begin{pmatrix}^A_BR & 0\\0 & ^A_BR\end{pmatrix}(BAR00BAR) BJ(Θ)^BJ(\Theta)BJ(Θ)

4. 奇异性

根据线性代数中的知识，如果某个Θ\ThetaΘ值使得雅可比矩阵JJJ不可逆，那么这些位置就是机构的奇异位型（singularities of the mechanism） 或者简称 奇异性（singularitries）。
可以确定的是，工作空间的边界存在奇异位型。记得我们前面讲过机械臂两个轴共轴导致失去一个自由度的情况吗？这就是在工作空间内的奇异位型。在这种情况下，选择什么样的速度都无法使机械臂运动

5. MATLAB中的雅可比

多余的就不赘述了，直接讲如何在MATLAB中求雅可比以及雅可比的一些应用。

mdl_puma560
J = p560.jacob0(qn)  %qn是默认的位姿之一，其他的有qs、q0等。
%好了，求解雅可比就这么简单。。。一句话
%注意，jacob0()求解的是将关节速度映射到世界坐标系中的末端执行器空间速度
%如果要求末端执行器在自身空间内的速度的话，用下面这句
Jn = jaconb(qn);

上面我们提到了雅可比的奇异性，那么在MATLAB中怎么判断呢？
不要被太多名词迷惑了，本质上就是在问你，如何判断矩阵的奇异性。答案很简单——是否满秩。

rank(J) %就完啦，完啦，啦，a......
jsingu(J) %这句话可以自动分析奇异性，得到的结果是q6可由q4表示

6. 总结

雅可比这一篇大概是耗费时间最多，内容最多的一篇，但是我依然感觉自己写的仍然有点少。事实也确实是这样，雅可比在机器人学中是很重要的知识点，还有力域中的雅可比，这一篇我也不打算讲了，在后面的动力学部分再考虑整理一下吧。

今天下午去看了深圳草莓音乐节，站得腰酸背痛。回学校后想了想还是回实验室把这篇写完。周末终于解放了，回寝洗漱！

中正平和的机器人学笔记——4. 雅可比矩阵（附MTALB代码）相关推荐

中正平和的机器人学笔记——0. 前言
中正平和的机器人学笔记--0.前言一点唠叨的话没有想到最后还是入了机器人学的坑,心情复杂. 借鉴某同学课设时写csdn blog整理自己所学知识的方法,我也将这周学习到的机器人学的知识记录整理下来 ...
中正平和的机器人学笔记——1. 机械臂正运动学（附MATLAB代码）
中正平和的机器人学笔记--1.正运动学 0. 何为运动学?何为正运动学? 运动学主要研究的就是机械臂的运动特性(位置.速度.加速度等),但是不考虑使机械臂产生运动时施加的力和力矩(这部分是动力学).而 ...
中正平和的机器人学笔记——5. 机械臂动力学
0. 基础知识 0.1 线加速度上一篇中我们讲到了在坐标系{A}和坐标系{B}原点重合时,BQ^BQBQ的速度矢量的表示方式: AVQ^AV_QAVQ = BARBVQ^A_BR^BV_QBAR ...
柔性matlab机械臂运动空间代码,中正平和的机器人学笔记——2. 机械臂逆运动学（附MATAB代码）...
0. 预备知识(别跳过呀) 上一篇我们讲了正运动学,也就是通过齐次变换矩阵,知晓关节角后求得机械臂末端的位姿,并表示在基座标系下.这一篇我们就来讲一讲逆运动学问题--已知工具坐标系{T}相对于固定坐标 ...
机器人学matlab建模实例,中正平和的机器人学笔记——3.几个MATLAB实例
0. 轨迹学机器人学前最想知道的,就是如何把机器人末端执行器平滑地从A点移动到B点,也许不太准确,应当说从位姿A移动到位姿B. 0.1 关节空间运动考虑末端执行器在以下两个笛卡尔位姿之间移动下面 ...
中正平和的机器人学笔记——7. 一种气驱型柔性臂的逆运动学模型
1. 前言上一篇分析了Air-Octor的正运动学模型,这一篇是学习另一篇论文1的笔记,着重分析这种柔性臂的逆运动学模型,也就是通过kϕk_\phikϕ 和 ϕ\phiϕ 以及 sss 来求 l1 ...
中正平和的机器人学笔记——6. 一种气驱型柔性臂的正运动学模型
1. 前言柔性臂是当前机器人领域一个比较热的研究方向,国内外高校如MIT.Stanford.JHU和我校.上交.中科大.北航等等都有做研究.大致可以分为三类:气驱/绳驱,超弹性材料,智能材料(DE/ ...
【模糊综合评价的运用】——《电子舌技术在食用盐模糊感官评价中的应用》论文笔记（内附MATLAB程序）
[模糊综合评价的运用]--<电子舌技术在食用盐模糊感官评价中的应用>论文笔记(内附MATLAB程序) 本文目录 1.因素集:(评价指标集)与评价事物相关的因素. 2. 评语集:(评价的结果 ...
Python教学 | Python 中的循环结构（上）【附本文代码和数据】
查看原文:[数据seminar]Python教学 | Python 中的循环结构(上)[附本文代码和数据] (qq.com) Part1引言上期文章我们向大家介绍了 Python 程序控制结构中的分 ...

中正平和的机器人学笔记——4. 雅可比矩阵（附MTALB代码）