python 机械臂工具坐标系转笛卡尔坐标系

前言：导师主要是做图像处理方面的，然后接了一个与机械臂相关的项目，然后让我学习机械臂，我从去年寒假开始，自己搜集资料，看了零星一些文章，感觉自己学的不是很深入，学的也比较零碎，我希望能通过写博客的方式来监督自己的学习。
机械臂我们可以理想地把他作为刚体，以刚体的角度对机械臂进行研究。刚体参考点的位置和刚体的姿态称为刚体的位姿，其表示方式主要有：齐次变换法、矢量法、旋量法和四元数法。其中介绍及应用比较多的是齐次变换法，他能将运动、变换、映射和矩阵运算联系起来。此外齐次变换在研究空间机构动力学、机器人控制算法、计算机图形学和视觉信息处理方面也有广泛的运用。

位姿描述：

（1）位置的描述：

描述一个点的位置，有很多方法。描述点的位置需要基于一个坐标系，常见的坐标系有直角坐标系，极坐标系，圆柱坐标系。通常我们采用直角坐标系来描述一个点的位置。对于直角坐标系{A}，空间任意一点p的位置，我们可以用列矢量$(px,py,pz)^T$来表示，其中px,py,pz是点p在坐标系{A}下的三个分量，这三个分量便能唯一确定一点的位置。

图1-1

（2）方位的描述：

描述刚体的运动，不仅要描述某一点的位置，也要描述刚体的方位；为了描述空间某刚体B的方位，另外设一个直角坐标系{B}与刚体连接，用坐标系{B}的三个单位主矢量$X_b$,$Y_b$,$Z_b$,相对于坐标系{A}的方向余弦组成的3X3的矩阵
$_B^AR$ = $[^A{X_B},^A{Y_B},^A{Z_B}]$ =$\begin{bmatrix} r11&r12&r13\\ r21&r22&r23\\r31&r32&r33\end{bmatrix}$
$_B^AR$为旋转矩阵，其中上标A代表参考坐标系{A}，下标B代表被描述的坐标系{B}。$_B^AR$矩阵中含有9个元素，只有三个是独立的，他的三个列矢量都是单位主矢量，两两互相垂直。标量${r_{ij}}$可用每个矢量在参考坐标系中单位方向上的投影的分量来表示。$_B^AR$中的各个分量可用一对单位矢量的点积来表示。由两个单位矢量的点积可以得到二者之间的夹角的余弦，所以旋转矩阵的各个分量常被称作方向余弦。
绕x轴、y轴、z轴旋转$\theta$角度的矩阵分别为：
R(x,$\theta$)=$\begin{pmatrix} 1&0&0 \\ 0&cos\theta&-sin\theta \\ 0&sin\theta&cos\theta \end{pmatrix}$
R(y,$\theta$)=$\begin{pmatrix} cos\theta&0&sin\theta \\ 0&1&0 \\ -sin\theta&0&cos\theta \end{pmatrix}$
R(z,$\theta$)=$\begin{pmatrix} cos\theta&-sin\theta&0 \\ sin\theta&cos\theta&0 \\0&0&1 \end{pmatrix}$
通常情况下会用旋转矩阵来描述刚体的方位。

（3）位姿的描述：

为了完整描述刚体B在空间的位置与姿态，需要将刚体B与坐标系{B}固接。坐标系的原点一般选取物体的特征点，如质心，对称中心等。相对于参考系{A}，位置矢量和旋转矩阵分别描述坐标系{B}的原点位置和坐标轴的方位。所以B的位姿由坐标系{B}参考坐标系{A}来描述，即{B} = {$_B^AR$,$^A{X_B}$}

（4）手爪坐标系：

为了描述末端执行器的位置和姿态，规定一个与手爪固接的坐标系，称为手爪坐标系。z轴设定在接近物体的方向，称为接近矢量a(approach);y轴设置在设在两手指的联线方向，称为方向矢量o(orientation);x轴由右手法则确定，n称为法向矢量(normal)。如此，手爪的方位就由旋转矩阵[n,o,a]而手爪的位置由位置矢量p所规定，他代表手爪坐标系的原点，因此手爪的位姿可有四个独立矢量进行描述[ n,o,a,p],记为[T]=[ n,o,a,p],在后续正逆运动学中，会多次用到这个坐标系。

坐标变化：

在空间中一点p在不同坐标系下的描述是不同的，从一个坐标系到另一个坐标系描述之间的变化关系主要如下三种：

（1）平移变换：

假设坐标系{B}和{A}有着相同的方位，但是两个坐标系的原点不一致。用位置矢$^A{P_B}$来描述{B}相对于参考坐标系{A}的位置，如下图所示。把$^A{P_B}$称为{B}相对于{A}的平移矢量。如果点P在坐标系{B}中的位置为$^B{P}$,则他相对于{A}的位置$^B{P}$可以通过矢量相加得到：

$^ A{P}$=$^ B{P}$+$^A{P_B}$

这个矢量相加的等式称为坐标平移方程。

图2-1

（2）旋转变化：

坐标系{B}与{A}具有相同的坐标原点，却有着不同的方位，如下图所示。可以用旋转矩阵来描述{B}相对于{A}的方位。同样的，空间中一点P在坐标系{A}和{B}中具有以下变换关系：$^ A{P}$=$_B^A{R}$$^B{P}$ ,旋转矩阵与位置矢量的相乘称为坐标系旋转方程。

图2-2

（3）复合变化：

对于一般情况，坐标系{B}与坐标系{A}，两个坐标系，原点不重合，方位也不相同，如下图所示。结合上面两个方程，复合变换由坐标平移与坐标旋转共同作用得来的，得出他的一般方程如下：$^ A{P}$=$_B^A{R}$$^B{P}$+$^A{P_B}$

齐次坐标变换：

上式的复合变换对点$^B{P}$来说是非齐次的，但是可以把他转化成齐次的形式:
$\begin{pmatrix} ^A{P} \\ 1\end{pmatrix}$=$\begin{pmatrix} _B^AR &^A{P_B} \\ 0&1\end{pmatrix}$$\begin{pmatrix} ^B{P} \\ 1\end{pmatrix}$

（1）平移齐次坐标变换：

空间的任意点都有矢量ai+bj+ck来描述，其中i,j,k为轴x,y,z上的单位矢量，所以此点可以用平移齐次变换表示为：
Trans(a,b,c)=$\begin{pmatrix} 1&0&0&a \\ 0&1&0&b \\ 0&0&1&c \\ 0&0&0&1 \end{pmatrix}$

（2）旋转齐次坐标变换：

对应于轴x,y,z作转角为$\theta$的旋转变化，可得到如下齐次矩阵:

Rot(x,$\theta$)=$\begin{pmatrix} 1&0&0&0 \\ 0&cos\theta&-sin\theta&0 \\ 0&sin\theta&cos\theta&0 \\ 0&0&0&1 \end{pmatrix}$

Rot(y,$\theta$)=$\begin{pmatrix} cos\theta&0&sin\theta&0 \\ 0&1&0&0 \\ -sin\theta&0&cos\theta&0 \\ 0&0&0&1 \end{pmatrix}$

Rot(z,$\theta$)=$\begin{pmatrix} cos\theta&-sin\theta&0&0 \\ sin\theta&cos\theta&0&0 \\ 0&0&1&0 \\ 0&0&0&1 \end{pmatrix}$

（3）复合齐次变换：

空间某点既有平移变换又有旋转变化是，按照变化的先后顺序，可得到如下变化矩阵：
T=Trans($a_1$$,b_1$,$c_1$)*Trans($a_n$$,b_n$,$c_n$)Rot(y,$\theta$)Rot(x,$\varepsilon$)

参考

[1].John J.Craig《机器人学导论》[M]机械工业出版社, 2006:14~47
[2].熊有伦，《机器人技术基础》[M]华中理工大学出版社,1996:15~30
[3]马强.《六自由度机械臂轨迹规划研究》[D]哈尔滨工程大学2007:9~13
[4]马江.《六自由度机械臂控制系统设计与运动学仿真》[D]北京工业大学2009：31~33