三维刚体运动与机械臂POE运动学建模

写在前面

不同于工程界常用的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$且$\Vert \omega \Vert =1$。

图1. $SO(3)$和$so(3)$的关系

定义$\omega \theta \in {\mathbb{R}}^3$为旋转$R$的指数坐标(exponential coordinates)，由罗德里格斯公式(Rodrigues’ formula)可以轻松计算出，当$\Vert \omega \Vert =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$，$\Vert \omega \Vert =1$和$\theta \in \mathbb{R}$，使得$R=e^{\hat{\omega }\theta }$¹。

SE(3)和螺旋理论

接下来，我们继续讲刚体运动/变换(rigid motion/transformation)，包括平动和旋转。刚体运动的构形空间由特殊欧氏群(special Euclidean group) S E ( 3 ) = { ( p , R ) } = R 3 × S O ( 3 ) SE(3)=\{(p,R)\}=\mathbb R^3\times SO(3) SE(3)={(p,R)}=R3×SO(3)表示，相应的李代数为$se(3)$。类似的，我们将$SE(3)$中的构形记作$g\in SE(3)$，并定义运动旋量(twist)为$\hat{\xi }\in se(3)$，即
$$\hat{\xi }=\left[\begin{array}{cc}\hat{\omega }& v\\ 0& 0\end{array}\right]\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)$($\Vert \omega \Vert =1$)和$\theta \in \mathbb{R}$，使得$g=e^{\hat{\xi }\theta }$。

对于刚体运动，计算$g=e^{\hat{\xi }\theta }$稍微复杂一些，需要分类讨论，证明见参考文献1的41页命题2.8。
$$g=e^{\hat{\xi }\theta }=\{\begin{array}{rlr}& \left[\begin{array}{cc}I& v\theta \\ 0& 1\end{array}\right]，& \omega =0，\\ & \left[\begin{array}{cc}{e}^{\hat{\omega }\theta }& (I-e^{\hat{\omega }\theta })(\omega \times v)+\omega {\omega }^{T}v\theta \\ 0& 1\end{array}\right]，& \omega \ne 0。\end{array}$$

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

图2. 螺旋运动

定义平动和旋转的比率为$h:=d/\theta \in \mathbb{R}$，称为pitch，则螺旋运动在齐次坐标(homogeneous coodinates)下表示为
$$g\left[\begin{array}{c}p\\ 1\end{array}\right]=\left[\begin{array}{cc}{e}^{\hat{\omega }\theta }& (I-e^{\hat{\omega }\theta })q+h\theta \omega \\ 0& 1\end{array}\right]\left[\begin{array}{c}p\\ 1\end{array}\right]。$$

因此，相应的运动旋量为
$$\hat{\xi }=\left[\begin{array}{cc}\hat{\omega }& -\omega \times q+h\omega \\ 0& 0\end{array}\right]。$$

机械臂运动学

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

对于常见机械臂，我们只需关注回转型和棱柱型即可，它们的运动旋量分别表示为
$$\xi =\left[\begin{array}{c}-\omega \times q\\ \omega \end{array}\right](h=0)，\xi =\left[\begin{array}{c}v\\ 0\end{array}\right](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_0{l}_1}({\theta }_1)g_{l_1{l}_2}({\theta }_2)\cdots g_{l_{n-1}{l}_n}({\theta }_n)g_{l_{n}t}，$$
其中$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({\varphi }_i)T_x(a_i)R_x({\alpha }_i)$。对于回转关节，${\varphi }_i={\theta }_i$；对于棱柱关节，$d_i={\theta }_i$。

注意：${\xi }_i$转化为$(d_i,{\varphi }_i,a_i,{\alpha }_i)$的方法不唯一，这里给出一个参考文献²。

为了研究${\xi }_{i-1,i}$和${\xi }_i$之间的关系，我们先定义伴随变换(adjoint transformation)。如果说$g_{ab}$定义两个坐标系A，B之间的刚体运动变换，即$g_a=g_{ab}{g}_b$，那么伴随变换则定义了它们之间的刚体速度(rigid body velocity)变换，即$V_{ab}^a={\mathrm{Ad}}_{g_{ab}}{V}_{ab}^b$，其定义为
$${\mathrm{Ad}}_{g_{ab}}=\left[\begin{array}{cc}{R}_{ab}& {\hat{p}}_{ab}{R}_{ab}\\ 0& R_{ab}\end{array}\right]。$$

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

---

1. Murray, R. M. (1994). A mathematical introduction to robotic manipulation. CRC press. ↩︎

2. Wu, L., Crawford, R., & Roberts, J. (2017). An Analytic Approach to Converting POE Parameters Into D–H Parameters for Serial-Link Robots. IEEE Robotics and Automation Letters, 2(4), 2174–2179. https://doi.org/10.1109/LRA.2017.2723470 ↩︎