機器人學回爐重造（2-5）：談一談空間雅可比與物體雅可比之間的關係

寫在前面

這篇博客主要是談一談空間雅可比與物體雅可比之間的關係，其實這兩個概念是在用旋量理論表示機器人運動學模型時出現的，我這裏主要是結合傳統的D-H（不管是改進還是標準DH）建模方法，將描述工具座標系的雅可比看作是空間雅可比，將描述末端連桿座標系處的雅可比看做是物體雅可比，然後找出兩者之間的關係。

這麼做的意義在於，在之前基於DH法求出的雅可比矩陣均是描述在各個連桿關節座標系下的，而通常情況下，我們建立機器人的連桿座標系，只是從各個關節出發，只考慮了各關節之間的運動關係，末端座標系也只是指最後一個關節座標系或者是末端連桿座標系，若還需要知道末端連桿上某一處（點）（也就是工具座標系）的雅可比呢，比如該處（點）上安裝了攝像頭或抓手，這時該如何求解呢？

準備知識

借用下面博客進行理解：

《Modern Robotics》閱讀筆記2——剛體的運動（二）

《Modern Robotics》閱讀筆記3——剛體的運動（三）

《Modern Robotics》閱讀筆記4——剛體的運動（四）

簡單進行理解，可以想象一個邊界無限大的運動剛體（即圖中橢圓），它與{b}系固連，其中$v_s$爲空間速度，$v_b$稱爲本體速度，$v_s$代表的就是，在某一瞬間時刻，這個剛體上恰好與{s}系原點對應的那一點的瞬時速度在{s}系下的表示。因此$v_s$應該包含兩個部分，一部分呢是{b}系原點與{s}系原點的相對速度，另一部分是{s}系原點相對於{b}系旋轉引起的速度。可以推導得到$[\omega_s,v_s]^T$與$[\omega_b, v_s]^T$之間的關係如下，推導過程見上面的筆記——剛體的運動（四），
$\begin{bmatrix}\omega_s \\v_s\end{bmatrix} = \begin{bmatrix}R_{sb} &0 \\ [p]R_{sb} & R_{sb}\end{bmatrix}\begin{bmatrix}\omega_b \\ v_b\end{bmatrix}$
其中，$R_{sb}$爲{b}系在{s}系中的表示，下同。

舉個例子具象理解

上述是抽象理解，具體來說，可見下圖

其中，{n}係爲機械臂連桿末端座標系，機械臂末端連桿可認爲是剛體，而{e}系呢，按照上面抽象理解就是{s}系，從外觀上看，你可以將{e}系理解爲固定在機械臂末端連桿上，但是需要注意的是，就某一瞬間而言，{e}系的速度$v_e$不應該理解爲相對於{e}系的速度（如果這樣理解的話速度就爲零了），應該理解爲在那一瞬間下的運動相對於一個與{e}系完全對齊的靜止座標系的速度。這與上面的抽象理解不謀而合。因此，根據速度與角速度公式可以推導得到，
$\begin{cases}v_e = R_{en}(v_n + \omega_n \times P_{ne}) \\ \omega_e = R_{en}\omega_n \end{cases}$
即（注意P的指向與符號，同時注意v和w誰在上誰在下）
$\begin{cases} v_e = R_{en}(v_n - P_{ne} \times \omega_n) \\ \omega_e = R_{en}\omega_n \end{cases}$
可得到
$\begin{bmatrix} v_e \\ \omega_e\end{bmatrix} = \begin{bmatrix} R_{en} & -[P_{ne}]R_{en} \\ O & R_{en} \end{bmatrix}\begin{bmatrix}v_n \\ \omega_n \end{bmatrix}$
可以得到空間雅可比，也就是描述機械臂末端某一{e}原點的雅可比$J_e$，與物體雅可比也就是描述機械臂末端連桿座標系{n}處的雅可比$J_n$，之間的關係，如下
$J_e\dot q = \begin{bmatrix}R_{en} & -[P_{ne}]R_{en} \\ O & R_{en} \end{bmatrix}J_n\dot q \Rightarrow J_e = \begin{bmatrix}R_{en} & -[P_{ne}]R_{en} \\ O & R_{en} \end{bmatrix}J_n$
這裏得到的$J_e$是瞬時狀態下相對於一個與{e}系完全對齊的靜止座標系的雅可比矩陣，同理$J_n$也是。此時，我們可以將其變換成相對於基系{0}，步驟如下

已知，
$^0J_e = \begin{bmatrix}R_{en} & -[^0P_{ne}]R_{en} \\ O & R_{en} \end{bmatrix} {^0J_n}$
其中，$^0P_{ne}$是指$P_{ne}$在基系{0}下的表示。

因爲
$[^0P_{ne}]^0\omega = ^0P_{ne}\times^0\omega =R_{0n}(^nP_{ne}\times^n\omega)=R_{0n}([^nP_{ne}]R_{0n}^T{^0\omega})$
因爲$^nP_{ne} = P_{ne}$，可得到
$[^0P_{ne}] = R_{0n}[P_{ne}]R_{0n}^T$
故
$^0J_e = \begin{bmatrix}R_{en} & -R_{0n}[P_{ne}]R_{0n}^TR_{en} \\ O & R_{en} \end{bmatrix} {^0J_n}$
也就是得到了，瞬時狀態下，固定於機械臂連桿末端某{e}原點處相對於基系{0}的雅可比$^0J_e$，與，機械臂末端連桿座標系{n}相對於基系{0}的雅可比$^0J_n$，之間的轉換關係。

總結

簡單記憶的話，

• 機械臂不同連桿（不同剛體）之間的雅可比轉換，只需要左乘旋轉矩陣構成的對角矩陣；

• 機械臂同一連桿（也就是在同一個剛體上）之間的雅可比轉換，左乘的矩陣不再是簡單的旋轉矩陣對角陣，而是
  $J_e = \begin{bmatrix}R_{en} & -[P_{ne}]R_{en} \\ O & R_{en} \end{bmatrix}J_n$

• 將上述矩陣統一變換到基系{0}下，記住$[^0 P_{ne}] \ne R_{0n}[^nP_{ne}]$，需要重新計算;

• 不同座標之間的速度變換、微分變換關係實際上就是雅可比映射得到。