• 写在前面
• 三维刚体运动
  • SO(3)和李群
  • SE(3)和螺旋理论
• 机械臂运动学
  • 前向运动学
  • POE和DH参数的关系

写在前面

不同于工程界常用的DH参数(Denavit-Hartenberg parameters)，学术界比较常用的机械臂建模方法为指数乘积公式POE(The product of exponentials formula)。后者的优点是在运动学建模上比DH参数更简洁，可以和李群理论联系起来，方便扩展到动力学建模；缺点是建立的模型和实际机械臂数学上等效，但难以对应关节坐标系。由于该方法的相关介绍在网上比较少，所以我在这篇文章讲一下。

三维刚体运动

本节默认大家对群有一定了解，不懂的参考这里。简单的说群(group)是一种集合加上一种运算的代数结构，而对于李群来说，相应的集合和运算分别为流形(manifold)和左乘。

SO(3)和李群

首先，我们回顾一下特殊正交群(special orthogonal)$SO(3)$，这是一个李群(Lie group)，同时也是所有3D旋转矩阵(rotation matrix)的集合。特殊正交群$SO(3)$对应的李代数(Lie algebra)为所有反对称矩阵(skew-symmetric matrix)的集合$so(3)$。李代数对应李群的正切空间，它描述了李群局部的导数。如果说$SO(3)$是个流形(manifold)，那么$so(3)$是流形对应的切空间(tangent space)。

对于刚体旋转，我们通常把$SO(3)$当作构形空间(configuration space)，那么刚体的角速度显然对应$so(3)$，两者的关系如图1所示。

注意：图中$\omega$范数为$\theta$，相当于$\omega\theta$且$\|\omega\|=1$。

定义$\omega \theta\in\mathbb R^3$为旋转$R$的指数坐标(exponential coordinates)，由罗德里格斯公式(Rodrigues' formula)可以轻松计算出，当$\|\omega\|=1$时，

$R=e^{\hat \omega\theta}=I+\hat \omega\sin\theta+\hat \omega^2(1-\cos \theta)。$

这意味着，如果我们绕着轴$\omega$以单位速度旋转$\theta$时间，那么净旋转可表示为$e^{\hat \omega\theta}$。显然指数坐标不唯一，有如下命题成立。

命题1：给定$R\in SO(3)$，存在$\omega\in\mathbb R^3$，$\|\omega\|=1$和$\theta\in\mathbb R$，使得$R=e^{\hat \omega\theta}$^[1]。

SE(3)和螺旋理论

接下来，我们继续讲刚体运动/变换(rigid motion/transformation)，包括平动和旋转。刚体运动的构形空间由特殊欧氏群(special Euclidean group)$SE(3)=\{(p,R)\}=\mathbb R^3\times SO(3)$表示，相应的李代数为$se(3)$。类似的，我们将$SE(3)$中的构形记作$g\in SE(3)$，并定义运动旋量(twist)为$\hat \xi\in se(3)$，即

$\hat \xi =\begin{bmatrix} \hat \omega & v\\ 0&0 \end{bmatrix}\in \mathbb R^{4\times 4}，$

旋量坐标(twist coodinates)为$\xi\in\mathbb R^6$。定义$\xi\theta\in\mathbb R^6$为刚体变换$g$的指数坐标，也有如下命题成立。

命题2：给定$g\in SE(3)$，存在$\hat \xi\in se(3)$($\|\omega\|=1$)和$\theta\in\mathbb R$，使得$g=e^{\hat \xi\theta}$。

对于刚体运动，计算$g=e^{\hat \xi\theta}$稍微复杂一些，需要分类讨论，证明见参考文献1的41页命题2.8。

$g=e^{\hat \xi \theta}=\left\{ \begin{aligned} &\begin{bmatrix}I&v\theta\\ 0&1\end{bmatrix}，&\omega=0，\\ &\begin{bmatrix}e^{\hat \omega\theta}&(I-e^{\hat\omega\theta})(\omega\times v)+\omega\omega^Tv\theta\\ 0&1\end{bmatrix}，&\omega\neq 0。 \end{aligned} \right.$

考虑一个螺旋形式的刚体运动，即绕着一个轴旋转$\theta$同时沿着轴向平动$d$，这样的刚体运动称为螺旋运动(screw motion)，如图2所示。

定义平动和旋转的比率为$h:=d/\theta\in\mathbb R$，称为pitch，则螺旋运动在齐次坐标(homogeneous coodinates)下表示为

$g\begin{bmatrix} p\\ 1 \end{bmatrix}=\begin{bmatrix} e^{\hat \omega \theta}&(I-e^{\hat \omega \theta})q +h\theta \omega\\ 0&1 \end{bmatrix}\begin{bmatrix} p\\ 1 \end{bmatrix}。$

因此，相应的运动旋量为

$\hat \xi =\begin{bmatrix} \hat \omega & -\omega\times q+h\omega\\ 0&0 \end{bmatrix}。$

机械臂运动学

机械臂可以看作是多个刚体组成的多体结构，各个刚体之间由关节(joint)连接。基本关节分为六类，称为低副(lower pairs)，包括回转型(revolute)、棱柱型(prismatic)、螺旋型(helical)、圆柱形(cylindrical)、球型(spherical)和平面型(planar)关节。

对于常见机械臂，我们只需关注回转型和棱柱型即可，它们的运动旋量分别表示为

$\xi =\begin{bmatrix} -\omega\times q\\ \omega \end{bmatrix}(h=0)，\xi =\begin{bmatrix} v\\ 0 \end{bmatrix}(h=\infty),$

其中$\omega\in\mathbb R^3$是旋转轴向的单位向量，$q\in\mathbb R^3$是轴上任意一点，$v\in\mathbb R^3$是平动方向的单位向量。所有向量均关于基坐标系$S$(base frame)给出。

前向运动学

机械臂操作中，末端执行器的构形是我们最关心的，因此定义末端坐标系$T$(tool frame)。给定一系列关节坐标(joint variables)$\theta\in\mathcal Q$，$\mathcal Q$是关节空间(joint space)，前向动力学(forward kinematics)由映射$g_{st}:\mathcal Q\to SE(3)$表示，该映射描述了末端坐标系相对基坐标系的构形。

考虑单自由度机械臂，即只有一个关节，其前向动力学为

$g_{st}(\theta)=e^{\hat \xi \theta}g_{st}(0)。$

这意味着，通过一个螺旋运动$e^{\hat \xi \theta}$，机械臂末端构形由$g_{st}(0)$变为$g_{st}(\theta)$。

同理，对于$n$自由度机械臂，其前向动力学为

$g_{st}(\theta)=e^{\hat \xi_1 \theta_1}e^{\hat \xi_2 \theta_2}\cdots e^{\hat \xi_n \theta_n}g_{st}(0)，$

称为指数乘积公式(the product of exponentials formula)。

POE和DH参数的关系

DH参数也可以写成螺旋运动的组合

$g_{st}(\theta)=g_{l_0l_1}(\theta_1)g_{l_1l_2}(\theta_2)\cdots g_{l_{n-1}l_n}(\theta_n)g_{l_nt}，$

其中$g_{l_{i-1}l_i}=e^{\hat \xi_{i-1,i}\theta_i}g_{l_{i-1}l_i}(0)$由四个刚体运动组合而成，即$T_z(d_i)R_z(\phi_i)T_x(a_i)R_x(\alpha_i)$。对于回转关节，$\phi_i=\theta_i$；对于棱柱关节，$d_i=\theta_i$。

注意：$\xi_i$转化为$(d_i,\phi_i,a_i,\alpha_i)$的方法不唯一，这里给出一个参考文献^[2]。

为了研究$\xi_{i-1,i}$和$\xi_i$之间的关系，我们先定义伴随变换(adjoint transformation)。如果说$g_{ab}$定义两个坐标系A，B之间的刚体运动变换，即$g_a= g_{ab}g_b$，那么伴随变换则定义了它们之间的刚体速度(rigid body velocity)变换，即$V_{ab}^a=\operatorname{Ad}_{g_{ab}}V_{ab}^b$，其定义为

$\operatorname{Ad}_{g_{ab}}=\begin{bmatrix} R_{ab}&\hat p_{ab} R_{ab}\\ 0&R_{ab} \end{bmatrix}。$

$\xi_{i-1,i}$和$\xi_i$之间的关系可表示为$\xi_i=\operatorname{Ad}_{g_{l_0 l_{i-1}}(0)}\xi_{i-1,i}$，证明见参考文献1的93页。