机器人运动学和动力学/串联机械臂位置运动学

正向位置运动学

正向位置运动学问题可以表述为：给定不同的关节角度，末端执行器的位置是什么？考虑到前面的章节，答案相当简单：构造不同的变换矩阵，并以正确的方式组合它们，结果是 $_{bs}^{ee}T\,$，其中 $\{bs\}$ 是机器人机械臂的基座坐标系。

解

假设相邻连杆之间的相互方向矩阵是已知的。（由于每个连杆的固定参数是已知的，并且关节角度是给定问题，因此可以计算这些参数。一种可能的方法是使用 Denavit-Hartenberg 约定。）将最后一个坐标系与第一个坐标系联系起来的变换，因此，正向运动学问题的解，由复合齐次变换矩阵表示。轴是移动的，因此，复合齐次变换矩阵是通过预乘各个变换矩阵得到的

$_{bs}^{ee}T=\,_{0}^{N}T=\,_{0}^{1}T\,_{1}^{2}T\ldots \,_{N-2}^{N-1}T\,_{N-1}^{N}T$

例子

平面三连杆机械臂

一个平面三连杆机械臂。每个 $X_{i}$-轴沿着第 $i$ 个连杆。每个 $Y_{i}$-轴垂直于相应的 $X_{i}$-轴，使得正的 $\theta _{i}$ 对应于从 $X_{i}$ 到 $Y_{i}$ 的旋转。

下面的方程使用 3 × 3 姿态矩阵，因为这只是一个二维情况（参见右边的图）。

第一个连杆相对于参考坐标系的姿态由下式给出（回顾上一节中关于 z 轴的基本旋转）

$T_{1}(\theta _{1})={\begin{pmatrix}\cos(\theta _{1})&-\sin(\theta _{1})&0\\\sin(\theta _{1})&\cos(\theta _{1})&0\\0&0&1\end{pmatrix}}$

相对于第一杆件，第二杆件的位置由下式给出：

$T_{2}(\theta _{2})={\begin{pmatrix}\cos(\theta _{2})&-\sin(\theta _{2})&l_{1}\\\sin(\theta _{2})&\cos(\theta _{2})&0\\0&0&1\end{pmatrix}}$

这对应于一个角度为 $\theta _{2}$ 的旋转和距离为 $l_{1}$ 的平移，其中 $l_{1}$ 是第一杆件的长度。

相对于第二杆件，第三杆件的位置由下式给出：

$T_{3}(\theta _{3})={\begin{pmatrix}\cos(\theta _{3})&-\sin(\theta _{3})&l_{2}\\\sin(\theta _{3})&\cos(\theta _{3})&0\\0&0&1\end{pmatrix}}$

相对于第三杆件，末端执行器的位置由下式给出：

$T_{4}={\begin{pmatrix}1&0&l_{3}\\0&1&0\\0&0&1\end{pmatrix}}$

正向运动学问题的解法如下：

$_{bs}^{ee}T=\,_{0}^{4}T(\theta _{1},\theta _{2},\theta _{3})={\begin{pmatrix}c_{123}&-s_{123}&l_{1}c_{1}+l_{2}c_{12}+l_{3}c_{123}\\s_{123}&c_{123}&l_{1}s_{1}+l_{2}s_{12}+l_{3}s_{123}\\0&0&1\end{pmatrix}}$

因此

${\begin{pmatrix}x\\y\\1\end{pmatrix}}=T{\begin{pmatrix}0\\0\\1\end{pmatrix}}$

最终得到的 **运动学方程** 为

${\begin{array}{l}x=l_{1}\cos \theta _{1}+l_{2}\cos(\theta _{1}+\theta _{2})+l_{3}\cos(\theta _{1}+\theta _{2}+\theta _{3})\\y=l_{1}\sin \theta _{1}+l_{2}\sin(\theta _{1}+\theta _{2})+l_{3}\sin(\theta _{1}+\theta _{2}+\theta _{3})\end{array}}$

逆运动学

逆运动学问题是正运动学问题的反面，可以概括为：给定末端执行器的期望位置，可以使用哪些关节角组合来实现该位置？

逆运动学问题导致相同末端执行器位置和方向的两种不同解决方案的示例。

可以考虑两种类型的解决方案：闭式解和数值解。闭式解或解析解是一组完全描述末端执行器位置和关节角之间关系的方程。数值解是通过使用数值算法找到的，即使没有闭式解也可以存在。也可能存在多个解，或者根本没有解。

示例：平面三连杆机械臂

这个二维机械臂的逆运动学问题可以用代数方法很容易地解决。

从前面的结果（为简单起见，这里将忽略$l_{3}$上的位移）

$_{0}^{3}T={\begin{pmatrix}c_{123}&-s_{123}&0&l_{1}c_{1}+l_{2}c_{12}\\s_{123}&c_{123}&0&l_{1}s_{1}+l_{2}s_{12}\\0&0&1&0\\0&0&0&1\\\end{pmatrix}}$

现在假设给定的末端执行器方向如下所示

$_{bs}^{ee}T={\begin{pmatrix}c_{\phi }&-s_{\phi }&0&x\\s_{\phi }&c_{\phi }&0&y\\0&0&1&0\\0&0&0&1\\\end{pmatrix}}$

将这两个表达式相等得到

${\begin{array}{lll}c_{\phi }&=&c_{123}\\s_{\phi }&=&s_{123}\\x&=&l_{1}c_{1}+l_{2}c_{12}\\y&=&l_{1}s_{1}+l_{2}s_{12}\\\end{array}}$

如

${\begin{array}{lll}c_{12}&=&c_{1}c_{2}-s_{1}s_{2}\\s_{12}&=&c_{1}s_{2}+s_{1}c_{2}\\\end{array}}$,

将 $x$ 和 $y$ 的表达式分别平方，然后将它们相加，得到

$x^{2}+y^{2}=l_{1}^{2}+l_{2}^{2}+2l_{1}l_{2}c_{2}$

解出 $c_{2}$，得到

$c_{2}={\frac {x^{2}+y^{2}-l_{1}^{2}-l_{2}^{2}}{2l_{1}l_{2}}}$,

而 $s_{2}$ 等于

$s_{2}=\pm {\sqrt {1-c_{2}^{2}}}$,

最后，$\theta _{2}$

$\theta _{2}={\mbox{Atan2}}(s_{2},c_{2})$

注意: $s_{2}$ 符号的选择对应于上图中的两个解之一。

现在可以根据 $\theta _{1}$ 来解出 $x$ 和 $y$ 的表达式。为此，将它们写成如下形式

${\begin{array}{lll}x&=&k_{1}c_{1}-k_{2}s_{1}\\y&=&k_{1}s_{1}+k_{2}c_{1}\\\end{array}}$

其中 $k_{1}=l_{1}+l_{2}c_{2}$，以及 $k_{2}=l_{2}s_{2}$.

令

${\begin{array}{lll}r&=&{\sqrt {k_{1}^{2}+k_{2}^{2}}}\\\gamma &=&{\mbox{Atan2}}(k2,k1)\\\end{array}}$

然后

${\begin{array}{lll}k_{1}&=&r\cos \gamma \\k_{2}&=&r\sin \gamma \\\end{array}}$

将这些应用到上面关于 $x$ 和 $y$ 的等式中

${\begin{array}{lll}x/r&=&\cos \gamma \,c_{1}+\sin \gamma \,s_{1}\\y/r&=&\cos \gamma \,s_{1}+\sin \gamma \,c_{1}\\\end{array}}$,

或

${\begin{array}{lll}\cos(\gamma +\theta _{1})&=&{\frac {x}{r}}\\\sin(\gamma +\theta _{1})&=&{\frac {y}{r}}\\\end{array}}$

因此

$\gamma +\theta _{1}={\mbox{Atan2}}(y,x)$

因此

$\theta _{1}={\mbox{Atan2}}(y,x)-{\mbox{Atan2}}(k_{2},k_{1})$

注意：如果 $x=y=0$，$\theta _{1}$ 实际上变得任意。

$\theta _{3}$ 现在可以通过前两个方程求解 $s_{\phi }$ 和 $c_{\phi }$

$\theta _{3}=\phi -\theta _{1}-\theta _{2}={\mbox{Atan2}}(s_{\phi },c_{\phi })-\theta _{1}-\theta _{2}$