【机器人学】微分变换与雅可比矩阵

假定有三个坐标系W、A、B，其中W为基坐标系。

变换矩阵的左乘和右乘

当坐标系A变换成坐标系B时，可左乘一个矩阵，也可右乘一个矩阵
${^W}T{_B} = {^W}T{_U} {^W}T{_A}$
${^W}T{_B} = {^W}T{_A} {^A}T{_B}$

对于左乘的情况，变换矩阵为
${^W}T{_U} = Trans(\Delta{_x},\Delta{_y},\Delta{_z})Rot(f,\Delta{_\theta})$
表示的是一个基于基坐标系的平移和绕轴旋转的变换，式中的变量均以基坐标系为参考坐标系。

对于右乘的情况，变换矩阵为
${^A}T{_B} = Trans({^T}\Delta{_x},{^T}\Delta{_y},{^T}\Delta{_z})Rot({^T}f,{^T}\Delta{_\theta})$
表示的是一个基于A坐标系（联体坐标系）的平移和绕轴旋转的变换，变换矩阵刚好为B坐标系在A坐标系下的表示，式中的变量均以A坐标系为参考坐标系。

微分变换

当变换前后的两个坐标系非常接近时，变换矩阵简化为

$\begin{aligned} {^W}T{_U} &= Trans(d{_x},d{_y},d{_z})Rot(f,d{_\theta}) \\ &= \begin{bmatrix} 0 & -f{_z}f{_\theta} & f{_y}f{_\theta} & d{_x} \\ f{_z}f{_\theta} & 0 & -f{_x}f{_\theta} & d{_y} \\ -f{_y}f{_\theta} & f{_x}f{_\theta} & 0 & d{_z} \end{bmatrix} \\ &= \begin{bmatrix} 0 & -\delta{_z} & \delta{_y} & d{_x} \\ \delta{_z} & 0 & -\delta{_x} & d{_y} \\ -\delta{_y} & \delta{_x} & 0 & d{_z} \end{bmatrix} \end{aligned}$

$\begin{aligned} {^A}T{_B} &= Trans({^T}d{_x},{^T}d{_y},{^T}d{_z})Rot({^T}f,{^T}d{_\theta}) \\ &= \begin{bmatrix} 0 & -{^T}f{_z}{^T}f{_\theta} & {^T}f{_y}{^T}f{_\theta} & {^T}d{_x} \\ {^T}f{_z}{^T}f{_\theta} & 0 & -{^T}f{_x}{^T}f{_\theta} & {^T}d{_y} \\ -{^T}f{_y}{^T}f{_\theta} & {^T}f{_x}{^T}f{_\theta} & 0 & {^T}d{_z} \end{bmatrix} \\ &= \begin{bmatrix} 0 & -{^T}\delta{_z} & {^T}\delta{_y} & {^T}d{_x} \\ {^T}\delta{_z} & 0 & -{^T}\delta{_x} & {^T}d{_y} \\ -{^T}\delta{_y} & {^T}\delta{_x} & 0 & {^T}d{_z} \end{bmatrix} \end{aligned}$

两种坐标系下微分变换平移和旋转的关系

由于左乘和右乘两种变换是等价的，即

${^W}T{_U} {^W}T{_A} = {^W}T{_A} {^A}T{_B}$

其中

${^W}T{_A} = \begin{bmatrix} n{_x} & o{_x} & a{_x} & p{_x} \\ n{_y} & o{_y} & a{_y} & p{_y} \\ n{_z} & o{_z} & a{_z} & p{_z} \\ 0 & 0 & 0 & 1 \end{bmatrix}$

得到基坐标系下和联体坐标系下微分变换之间的关系

$\begin{aligned} {^A}T{_B} &= {^W}T{_A}{^{-1}} {^W}T{_U} {^W}T{_A} \\ &= \begin{bmatrix} 0 & -\delta.a & \delta.o & \delta.(p \times n)+d.n \\ \delta.a & 0 & -\delta.n & \delta.(p \times o)+d.o \\ -\delta.o & \delta.n & 0 & \delta.(p \times a)+d.a \end{bmatrix} \\ &= \begin{bmatrix} 0 & -{^T}\delta{_z} & {^T}\delta{_y} & {^T}d{_x} \\ {^T}\delta{_z} & 0 & -{^T}\delta{_x} & {^T}d{_y} \\ -{^T}\delta{_y} & {^T}\delta{_x} & 0 & {^T}d{_z} \end{bmatrix} \end{aligned}$

由此可得到基坐标系下和联体坐标系下平移旋转微分量之间的关系

$\begin{bmatrix} {^T}d{_x} \\ {^T}d{_y} \\ {^T}d{_z} \\ {^T}\delta{_x} \\ {^T}\delta{_y} \\ {^T}\delta{_z} \end{bmatrix} = \begin{bmatrix} n{_x} & n{_y} & n{_z} & (p \times n){_x} & (p \times n){_y} & (p \times n){_z} \\ o{_x} & o{_y} & o{_z} & (p \times o){_x} & (p \times o){_y} & (p \times o){_z} \\ a{_x} & a{_y} & a{_z} & (p \times a){_x} & (p \times a){_y} & (p \times a){_z} \\ 0 & 0 & 0 & n{_x} & n{_y} & n{_z} \\ 0 & 0 & 0 & o{_x} & o{_y} & o{_z} \\ 0 & 0 & 0 & a{_x} & a{_y} & a{_z} \end{bmatrix} \begin{bmatrix} d{_x} \\ d{_y} \\ d{_z} \\ \delta{_x} \\ \delta{_y} \\ \delta{_z} \end{bmatrix}$

$\begin{bmatrix} {^T}d \\ {^T}\delta \end{bmatrix} = \begin{bmatrix} R{^T} & -R{^T}S(p) \\ 0 & R{^T} \end{bmatrix} \begin{bmatrix} d \\ \delta \end{bmatrix}$

$\begin{bmatrix} d{_x} \\ d{_y} \\ d{_z} \\ \delta{_x} \\ \delta{_y} \\ \delta{_z} \end{bmatrix} = \begin{bmatrix} n{_x} & o{_x} & a{_x} & (p \times n){_x} & (p \times o){_x} & (p \times a){_x} \\ n{_y} & o{_y} & a{_y} & (p \times n){_y} & (p \times o){_y} & (p \times a){_y} \\ n{_z} & o{_z} & a{_z} & (p \times n){_z} & (p \times o){_z} & (p \times a){_z} \\ 0 & 0 & 0 & n{_x} & o{_x} & a{_x} \\ 0 & 0 & 0 & n{_y} & o{_y} & a{_y} \\ 0 & 0 & 0 & n{_z} & o{_z} & a{_z} \end{bmatrix} \begin{bmatrix} {^T}d{_x} \\ {^T}d{_y} \\ {^T}d{_z} \\ {^T}\delta{_x} \\ {^T}\delta{_y} \\ {^T}\delta{_z} \end{bmatrix}$

$\begin{bmatrix} d \\ \delta \end{bmatrix} = \begin{bmatrix} R & -S{^T}(p)R \\ 0 & R \end{bmatrix} \begin{bmatrix} {^T}d \\ {^T}\delta \end{bmatrix}$

$R = \begin{bmatrix} n{_x} & o{_x} & a{_x} \\ n{_y} & o{_y} & a{_y} \\ n{_z} & o{_z} & a{_z} \end{bmatrix}, S(p) = \begin{bmatrix} 0 & -p{_z} & p{_y} \\ p{_z} & 0 & -p{_x} \\ -p{_y} & p{_x} & 0 \end{bmatrix}$

微分变换的无序性

绕各个轴旋转的变换矩阵分别为

$Rot(x, \delta{_x}) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & -\delta{_x} & 0 \\ 0 & \delta{_x} & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ Rot(y, \delta{_y}) = \begin{bmatrix} 1 & 0 & \delta{_y} & 0 \\ 0 & 1 & 0 & 0 \\ -\delta{_y} & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ Rot(z, \delta{_z}) = \begin{bmatrix} 1 & -\delta{_z} & 0 & 0 \\ \delta{_z} & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

XYZ和ZYX旋转结果分别为

$Rot(x, \delta{_x}) Rot(y, \delta{_y}) Rot(z, \delta{_z}) = \begin{bmatrix} 1 & -\delta{_z} & \delta{_y} & 0 \\ \delta{_x}\delta{_y}+\delta{_z} & 1-\delta{_x}\delta{_y}\delta{_z} & -\delta{_x} & 0 \\ -\delta{_y}+\delta{_x}\delta{_z} & \delta{_y}\delta{_z}+\delta{_x} & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

$Rot(z, \delta{_z}) Rot(y, \delta{_y}) Rot(x, \delta{_x}) = \begin{bmatrix} 1 & -\delta{_z}+\delta{_x}delta{_y} & \delta{_x}\delta{_z}+\delta{_y} & 0 \\ \delta{_x} & 1+\delta{_x}\delta{_y}\delta{_z} & -\delta{_x}+\delta{_y}\delta{_z} & 0 \\ -\delta{_y} & \delta{_x} & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$

在忽略高阶无穷小的前提下，两式结果相同。另外，用同样的方法容易验证微小平移和微小旋转之间与变换顺序无关。在忽略高阶无穷小的前提下(即多个变分相乘的项)，微分变换与次序无关，即微分变换具有无序性。

雅可比矩阵

雅可比矩阵为笛卡尔空间与关节空间的速度之间的关系

$\begin{bmatrix} v \\ \omega \end{bmatrix} = \begin{bmatrix} J_{11} & J_{12} & ... & J_{1n} \\ J_{21} & J_{22} & ... & J_{2n} \\ J_{31} & J_{32} & ... & J_{3n} \\ J_{41} & J_{42} & ... & J_{4n} \\ J_{51} & J_{52} & ... & J_{5n} \\ J_{61} & J_{62} & ... & J_{6n} \\ \end{bmatrix} \begin{bmatrix} q^{\prime}_{1} \\ q^{\prime}_{2} \\ ... \\ q^{\prime}_{n-1} \\ q^{\prime}_{n} \\ \end{bmatrix}$

可得到微分运动量之间的关系

$dx = \begin{bmatrix} d \\ \delta \end{bmatrix} = J(q)dq$

转动关节旋转时，以当前位置的连杆坐标系为参考坐标系，则微分运动的连杆坐标系绕其z轴旋转，该旋转运动在该坐标系下的微分平移和微分旋转矢量为

${^i}d = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix}, {^i}\delta = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}q{_i}$

由此基于当前位置的连杆坐标系关于该关节的雅可比向量为

$^{i}J_{i} = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & 1 \end{bmatrix}^{T}$

乘上基坐标系下和联体坐标系下平移旋转微分量之间关系的矩阵，转换到机器人基坐标系为

$J_{i} = \begin{bmatrix} (p_{i} \times a_{i})_{x} & (p_{i} \times a_{i})_{y} & (p_{i} \times a_{i})_{z} & a_{ix} & a_{iy} & a_{iz} \end{bmatrix}^{T}$

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 212,657评论 6赞 492
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 90,662评论 3赞 385
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 158,143评论 0赞 348
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 56,732评论 1赞 284
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 65,837评论 6赞 386
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 50,036评论 1赞 291
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 39,126评论 3赞 410
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 37,868评论 0赞 268
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 44,315评论 1赞 303
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 36,641评论 2赞 327
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 38,773评论 1赞 341
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 34,470评论 4赞 333
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 40,126评论 3赞 317
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 30,859评论 0赞 21
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 32,095评论 1赞 267
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 46,584评论 2赞 362
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 43,676评论 2赞 351