機器人學中的微分運動

寫在前面

微分運動指機構（例如機器人）的微小運動，可以用它來推導不同部件之間的速度關係。依據定義，微分運動就是微小的運動。因此，如果能夠在一個很小的時間段內測量或者計算這個運動，就能得到速度關係。

爲啥想講講這個微分運動呢，其實我的目的很簡單，和雅可比矩陣聯繫上，進而幫助解決機械臂的逆運動學問題。當機器人關節做微量運動時，機器人手座標系也會產生微量運動。而前面博客講機器人雅可比矩陣已經講到，這是機器人關節速度與末端機械手（末端座標系）速度之間的映射，由於微分運動除以dt即可得到速度，因此可以說雅可比矩陣是機器人關節微分運動與末端微分運動之間的映射。當計算得到機器人末端座標系的微分運動時，通過雅可比矩陣，則能計算得到各關節的微分運動，能解決逆運動學及速度控制的問題。以六軸機器人爲例，
$\begin{bmatrix} dx \\ dy \\ dz \\ \delta x \\ \delta y \\ \delta z \end{bmatrix} = \begin{bmatrix} \\ \\ 機器人\\雅可比 \\矩陣 \\ \\ \end{bmatrix} \begin{bmatrix} d\theta_1 \\ d\theta_2 \\ d\theta_3 \\ d\theta_4 \\ d\theta_5 \\ d\theta_6 \end{bmatrix}或者D = \boldsymbol{J}d\boldsymbol\theta……（1）$
其中，$D = [dx, dy, dz, \delta x, \delta y, \delta z]$表示六軸機器人末端機械手座標的微分運動，前三項表示機械手座標沿x，y，z軸的微分平移，後三項表示繞這三個軸的微分旋轉；$d\theta$表示各關節的微分運動。

座標系的微分運動

此處探討的微分運動是基於座標系而言的，再細化一下，對於本博客而言，是基於機器人末端座標系而言的。

座標系的微分運動可以分爲：微分平移、微分旋轉、微分變換（平移和旋轉的組合）。其中微分旋轉還分爲繞參考軸xyz的微分旋轉和繞一般軸q的微分旋轉。

微分平移

很簡單，就是座標系原點平移一個微分量，和之前的運動學公式一致，只是將參數改爲微分量，可以用$Trans(dx, dy, dz)$來表示，其含義是座標系原點沿着x，y，z軸做了微小平移。

繞參考軸xyz的微分旋轉

同樣，與運動學公式一致，通常用$Rot(q,d\theta)$（其中q爲x，y，z，對應的$d\theta$爲$\delta x,\delta y, \delta z$）來表示。此處注意一點，由於旋轉角度是微小量，故存在近似式
$sin\delta x = \delta x (用弧度來表示)\\ cos\delta x = 1……（2）$
故可以得到表示繞x，y，z軸的微分旋轉矩陣爲
$\operatorname{Rot}(x, \delta x)=\left[\begin{array}{cccc}{1} & {0} & {0} & {0} \\ {0} & {1} & {-\delta x} & {0} \\ {0} & {\delta x} & {1} & {0} \\ {0} & {0} & {0} & {1}\end{array}\right], \operatorname{Rot}(y, \delta y)=\left[\begin{array}{cccc}{1} & {0} & {\delta y} & {0} \\ {0} & {1} & {0} & {0} \\ {-\delta y} & {0} & {1} & {0} \\ {0} & {0} & {0} & {1}\end{array}\right], \operatorname{Rot}(z, \delta z)=\left[\begin{array}{cccc}{1} & {-\delta z} & {0} & {0} \\ {\delta z} & {1} & {0} & {0} \\ {0} & {0} & {1} & {0} \\ {0} & {0} & {0} & {1}\end{array}\right]……（3）$
例如$(\delta x)^2,\delta x\delta y$這樣的高階微分可忽略不計，故可將其略去，因此通過計算可以看出，幾個微分運動以不同的順序相乘可以得到同樣的結果。因此在微分運動中，可認爲相乘的順序不重要。這與前面大角度旋轉順序不能交換的原則不一樣。

繞一般軸q的微分旋轉

上面分析可知，在微分運動中，任意相乘的順序都能得到同樣的結果。因此，可認爲繞一般軸q的微分旋轉運動可以表示以任意順序繞3個座標軸的3個微分旋轉構成，如下
$Rot(q, d\theta) = Rot(x, \delta x)Rot(y, \delta y)Rot(z, \delta z)……（4）$
同樣，忽略所有高階微分後，可得到
${Rot}(q, \mathrm{d} \theta)={Rot}(x, \delta x) {Rot}(y, \delta y) {Rot}(z, \delta z)=\left[\begin{array}{cccc}{1} & {-\delta z} & {\delta y} & {0} \\ {\delta z} & {1} & {-\delta x} & {0} \\ {-\delta y} & {\delta x} & {1} & {0} \\ {0} & {0} & {0} & {1}\end{array}\right]……（5）$

座標系的微分變換

座標系的微分變換是微分平移和以任意次序進行微分旋轉的合成。用T表示微分變換前的座標系位姿，這裏默認T爲相對於固定參考座標系（基系）的位姿，下同，如果用位姿間的增量來進行表示的話，即用dT表示由於微分變換所引起的原座標系T的變化量，$[dx, dy, dz, \delta x, \delta y, \delta z]^T$是相對固定參考系（基系）的，則可以得到我們定義的位姿微分變化量，有
$[T + dT] = Trans(dx, dy, dz)Rot(q, d\theta)[T] \rightarrow[dT] = [Trans(dx, dy, dz)Rot(q, d\theta) - \boldsymbol{I}][T]……（6）$
公式（5）可寫爲
$[dT] = [\Delta][T] \\ [\Delta] = [Trans(dx, dy, dz)Rot(q, d\theta) - \boldsymbol{I}]……（7）$
我們將$\Delta$稱爲微分運動算子，座標系的變化可以用微分運動算子乘以座標系來實現，這個算子並不是我們之前講的變換矩陣或者座標系，它只是一個算子，使座標系實現微分變化。其公式及具體計算式爲
$\begin{aligned} \Delta &=\operatorname{Trans}(\mathrm{d} x, \mathrm{d} y, \mathrm{d} z) \times \operatorname{Rot}(q, \mathrm{d} \theta)-I \\ &=\left[\begin{array}{llll}{1} & {0} & {0} & {\mathrm{d} x} \\ {0} & {1} & {0} & {\mathrm{d} y} \\ {0} & {0} & {1} & {\mathrm{d} z} \\ {0} & {0} & {0} & {1}\end{array}\right]\left[\begin{array}{rrrr}{1} & {-\delta z} & {\delta y} & {0} \\ {\delta z} & {1} & {-\delta x} & {0} \\ {-\delta y} & {\delta x} & {1} & {0} \\ {0} & {0} & {0} & {1}\end{array}\right]-\left[\begin{array}{cccc}{1} & {0} & {0} & {0} \\ {0} & {1} & {0} & {0} \\ {0} & {0} & {1} & {0} \\ 0 & 0 & 0 & 1\end{array}\right] \\ &= \left[\begin{array}{cccc}{0} & {-\delta z} & {\delta y} & {d x} \\\delta z & 0 & -\delta x & dy \\ {-\delta y} & {\delta x} & {0} & {\mathrm{d} z} \\ {0} & {0} & {0} & {0}\end{array}\right] \end{aligned} ……(8)$
前面講到的$dT$爲通過微分變換後原座標系T的變化量，則結合$\Delta$，可以計算得到經過微分變換後座標系新位姿爲
$T_{new} = dT + T_{old}（原座標系T）……（9）$

微分運動算子：相對於固定參考座標系、相對於自身座標系

通常，我們將上面提到的、也就是公式（7）計算得到的微分運動算子$\Delta$定義爲相對於固定參考座標系U的微分運動算子，將其記爲$^{U}\Delta$。還存在另一種微分運動算子，即定義相對於自身座標系、使得可以在該座標系中計算出同樣的變化，將其記爲$^T{\Delta}$。 因爲微分算子$^T\Delta$是相對於當前座標系的，爲了求出該座標系的變化，必須右乘$^T\Delta$，因爲這兩者都描述的是該座標系中的相同變化，只是相對的座標系不一樣而已，因此結果都是相同的。於是有：
$\begin{aligned} &[\mathrm{d} T]=[\Delta] [T]=[T]\left[^{T} \Delta\right] \\ \rightarrow &\left[T^{-1}\right][\Delta][T]=\left[T^{-1}\right][T][^T \Delta] \\ \rightarrow &\left[^{T} \Delta\right]=\left[T^{-1}\right][\Delta][T] \end{aligned}……（10）$
由式(10)可以計算得到相對於當前座標系（本身座標系）的微分算子。

上面談到的都是座標系的微分變化，這個座標系可以是任意座標系，包括機器人末端座標系。下面的問題就是找出機器人關節的微分運動與機器人末端座標系的微分運動是如何相關聯的。從公式(1)中可以看出，向量D和微分算子$\Delta$中元素相同，由此可以得到向量D，再通過雅可比取逆計算，則可得到機器人關節的微分運動。

至此，已將座標系的微分運動與機器人的微分運動相互聯繫了起來。