【现代机器人学】基于指数积的机械臂正运动学

前言：对于一个开链机器人而言，末端执行器的位置和姿态可以通过关节角唯一确定出来。正运动学的问题就是当给定各关节位置，求出附着在末端执行器上的物体坐标系的位姿。
描述正运动学的方法主要是DH参数法，它是在每个关节上建立坐标系，通过坐标系的平移旋转，即矩阵的乘积来求得相对于基坐标系的末端位姿。
指数积(Product of Exponential,POE)的方法近些年才开始流行，成为新颖的建模方法。POE公式直接来自刚体运动的指数坐标表示，它无须建立连杆坐标系，只有基坐标系和末端坐标系是必需的，且它们可以任意选择。

注：以下专业名词的含义及相关公式可参考我的另一篇博客:
现代机器人学名词概念

一.指数积公式

1.1 第一种表达形式：相对基坐标系的螺旋轴

空间刚体位移Chasles-Mozzi定理：任何刚体运动都可通过绕空间某一固定螺旋轴SSS的运动来实现。
隐藏在POE公式背后的关键是：将每个关节的螺旋运动施加给后面的杆。为应用POE公式，首先需选择基坐标系{s}\left\{ s \right\}{s}和末端坐标系{b}\left\{ b \right\}{b}，并将机器人置于初始位置(或零位，即所有关节变量初值为0)，每个关节正向位移的方向指定。令M∈SE(3)M\in SE\left( 3 \right)M∈SE(3)表示末端坐标系相对于基坐标系的初始位形(机器人处于初始位置时)。
假定关节n对应的关节变量为θn\theta _nθn，末端坐标系M的位移可写成T(θ)=e[Sn]θnMT\left( \theta \right) =e^{\left[ S_n \right] \theta _n}M T(θ)=e[Sn]θnM
式中，T∈SE(3)T\in SE\left( 3 \right)T∈SE(3)为末端的新位形，Sn=(ωn,νn)S_n=\left( \omega _n,\nu _n \right)Sn=(ωn,νn)为表示在基坐标系中的关节n的旋量坐标，wn∈R3w_n\in R^3wn∈R3是沿关节轴正向的单位向量，vn=−wn×qnv_n=-w_n\times q_nvn=−wn×qn，qnq_nqn为关节轴上任一点，坐标值在基坐标系中进行度量。
若我们假设关节n-1也发生变化，则在以上的位移上再乘以矩阵指数e[Sn−1]θn−1e^{\left[ S_{n-1} \right] \theta _{n-1}}e[Sn−1]θn−1，按此推理，当所有的关节变量(θ1,⋯,θn)\left( \theta _1,\cdots ,\theta _n \right)(θ1,⋯,θn)都发生变化时，应满足

T(θ)=e[S1]θ1⋯e[Sn−1]θn−1e[Sn]θnMT\left( \theta \right) =e^{\left[ S_1 \right] \theta _1}\cdots e^{\left[ S_{n-1} \right] \theta _{n-1}}e^{\left[ S_n \right] \theta _n}M T(θ)=e[S1]θ1⋯e[Sn−1]θn−1e[Sn]θnM
上述方程就是对于n自由度开链机器人正向机器人正向运动学的指数积公式。其中的所有螺旋轴都是基于基坐标系的表示。

计算步骤：
一.当机器人处于初始位置时，末端位形M∈SE(3)M\in SE\left( 3 \right)M∈SE(3)；

二.当机器人处于初始位置时，相对基坐标系的螺旋轴S1,⋯SnS_1,\cdots S_nS1,⋯Sn，对应各个关节的螺旋运动；

三.关节变量(θ1,⋯,θn)\left( \theta _1,\cdots ,\theta _n \right)(θ1,⋯,θn)。

1.2 第二种表达形式：相对末端坐标系的螺旋轴

T(θ)=Me[β1]θ1⋯e[βn−1]θn−1e[βn]θnT\left( \theta \right) =Me^{\left[ \beta _1 \right] \theta _1}\cdots e^{\left[ \beta _{n-1} \right] \theta _{n-1}}e^{\left[ \beta _n \right] \theta _n} T(θ)=Me[β1]θ1⋯e[βn−1]θn−1e[βn]θn
上式是指数积公式的另一种表达形式，其中的关节角表示成机器人处于零位时各螺旋轴相对于末端坐标系(物体坐标系)的旋量坐标 βi\beta _iβi ，它也被称为指数积公式的物体坐标系表示形式。其中[βi]=[AdM−1]Si,i=1,⋯,n\left[ \beta _i \right] =\left[ Ad_{M^{-1}} \right] S_i,i=1,\cdots ,n [βi]=[AdM−1]Si,i=1,⋯,n

算例：对于6R开链机器人的正向运动学

第一步：当机器人位于初始位形时，建立基坐标系{s}\left\{ s \right\}{s}和末端坐标系{b}\left\{ b \right\}{b}，旋转轴作为zzz轴，坐标系的建立满足右手定则，此时可以得到末端位姿矩阵M=[RP01]∈SE(3)M=\left[ \begin{matrix} R& P\\ 0& 1\\ \end{matrix} \right] \in SE\left( 3 \right) M=[R0P1]∈SE(3)式中，R为末端坐标系相对于基坐标系的姿态变化，P为末端坐标系原点在基坐标系中的位移表示。
末端坐标系姿态任意选择，这里使基坐标系和末端坐标系姿态相同，即M=[100−0.0261010−1.1863001−0.65400001]M=\left[ \begin{matrix} 1& 0& 0& -0.0261\\ 0& 1& 0& -1.1863\\ 0& 0& 1& -0.6540\\ 0& 0& 0& 1\\ \end{matrix} \right] M=⎣⎢⎢⎡100001000010−0.0261−1.1863−0.65401⎦⎥⎥⎤
第二步：建立每个关节的螺旋轴，同样的，旋转轴作为zzz轴，此时可写出每个螺旋轴相对于基坐标系的单位向量w^\hat{w}w^。
这里选择[w1⋯w6]=[0000−1−1100−100010001]\left[ w_1\cdots w_6 \right] =\left[ \begin{matrix} 0& 0& 0\\ 0& -1& -1\\ 1& 0& 0\\ \end{matrix}\begin{matrix} -1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ \end{matrix} \right] [w1⋯w6]=⎣⎡0010−100−10−100010001⎦⎤

第三步：选择六个关节轴上的任一点 q1⋯q6q_1\cdots q_6q1⋯q6，为方便计算，我们将这些点选为螺旋轴与连杆的重合点，即将六个关节上的点分别作为 q1⋯q6q_1\cdots q_6q1⋯q6。
这里选择[q1⋯q6]=[0−0.1573−0.203700.0653−0.548300−0.0165−0.1518−0.1081−0.0261−0.7154−0.6714−1.1863−0.3140−0.6685−0.6540]\left[ q_1\cdots q_6 \right] =\left[ \begin{matrix} 0& -0.1573& -0.2037\\ 0& 0.0653& -0.5483\\ 0& 0& -0.0165\\ \end{matrix}\,\, \begin{matrix} -0.1518& -0.1081& -0.0261\\ -0.7154& -0.6714& -1.1863\\ -0.3140& -0.6685& -0.6540\\ \end{matrix} \right] [q1⋯q6]=⎣⎡000−0.15730.06530−0.2037−0.5483−0.0165−0.1518−0.7154−0.3140−0.1081−0.6714−0.6685−0.0261−1.1863−0.6540⎦⎤

再根据公式vn=−wn×qnv_n=-w_n\times q_nvn=−wn×qn，即可计算得到六个线速度vnv_nvn。
第四步：有了以上的数据，再根据指数坐标公式运算得到矩阵指数 e[S]θe^{\left[ S \right] \theta}e[S]θ，详细公式可参考我的文前的链接文章。到此即可得到指数积公式T(θ)=e[S1]θ1⋯e[Sn−1]θn−1e[Sn]θnMT\left( \theta \right) =e^{\left[ S_1 \right] \theta _1}\cdots e^{\left[ S_{n-1} \right] \theta _{n-1}}e^{\left[ S_n \right] \theta _n}M T(θ)=e[S1]θ1⋯e[Sn−1]θn−1e[Sn]θnM

最后说明一点：这里的关节角是指机器人从初始位置实际转动的角度，而不是机器人此刻的角度，因为对于一些工业机器人而言有的初始关节角是90°。

个人对POE指数积建模与DH参数法的理解：POE指数积建模需要扎实的数学几何知识，它的建模过程相比DH参数法要简单一些，因为它不必考虑首尾之间的坐标系应该怎么建立，而是确定螺旋轴方向和确定螺旋轴上的一点位置即可。它们之间的联系是指数积方法在确定螺旋轴方向的同时隐含了对连杆转角的确定，确定螺旋轴上的一点时隐含了对连杆长度的确定，总的来说，大同小异，但指数积方法更新颖，更灵活。