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

假定有三个坐标系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}$