1.1.4 pose数据相关函数

假设位姿pose数据p1在坐标系wobj0下的值为pose1（x，y，z，rz，ry，rx），其中，rz，ry和rx为欧拉角，则p1在wobj0下的位姿如图1-8所示，即先将pose数据沿着wobj0的x₀、y₀和z₀三个方向平移x、y和z距离，得到新的pose数据wobj0′。再将wobj0′绕着wobj0′的本地坐标系旋转欧拉角R（rz，ry，rx）。

注：欧拉角旋转过程为绕当前的动坐标系，即先绕z轴，其次绕旋转后新的y轴，最后绕旋转后新的x轴，具体参见1.1节中的图1-6。

1.PoseMult

如图1-9所示，坐标系p2在坐标系p1下的位姿表示为pose2，p1在坐标系p0下的位姿表示为pose1，那么坐标系p2在坐标系p0下的位姿可以表示为pose3，pose3=pose1*pose2。对于以上公式，ABB工业机器人RAPID编程提供了PoseMult函数来实现，即

pose3:=PoseMult(pose1,pose2)

简单来说，PoseMult（pose1，pose2）可以理解为一个与pose1相同的坐标系，在pose1坐标系下平移pose2.trans，再绕着新的坐标原点旋转pose2.rot，此时的新pose在p0坐标系下表示为pose3。

由前文所知，对于位姿数据，可以用pose表示，也可用齐次变换矩阵表示。若把图1-9中的pose1和pose2用齐次变换矩阵表示，则可以得到如下公式：

其中：

对于PoseMult函数，也可自行编写函数来实现，即将pose转化为4×4的齐次变换矩阵并完成2个矩阵的乘法，最后将4×4的矩阵转为pose类型的数据即可，具体代码如下：

2.姿态数据旋转及求逆

对于某位姿数据p1，若只需要绕自身原有姿态旋转一定角度（见图1-10），则可采用PoseMult（p1，[[0，0，0]，EulerZYX（Rz，Ry，Rx）]）实现，即不平移只旋转。实际上，位姿绕自身旋转，就是姿态数据的右乘。RAPID可以采用如下语句实现：

考虑到直接对四元数赋值不方便，上面指令语句也可写为

例如，绕姿态数据o0的Z轴旋转90°，可以使用如下代码：

根据四元数的定义，其乘法实现如式（1-14），可以自行编写RAPID代码实现2个四元数的乘法。

图1-10显示了p0.rot绕自身旋转了姿态数据o2后得到新的姿态数据p1.rot。假设o0：=p0.rot，o1：=p1.rot，则o1：=o0*o2。其中，o2为姿态绕o0旋转的姿态数据。那么绕姿态o1旋转一定角度后得到o0，这个旋转姿态就称为o2的逆，可以用o2-1表示。姿态数据乘姿态数据的逆，得到零姿态数据[1，0，0，0]，对应的欧拉角为RzRyRx：=[0，0，0]。

RAPID函数未提供四元数求逆函数。根据四元数的逆就是四元数的共轭四元数，即Q（w，x，y，z）-1=Q（w，-x，-y，-z），可以自行编写姿态数据求逆函数，具体代码如下：

3.PoseInv

位姿p0到位姿p1的变换为pose1，那么位姿p1到位姿p0的变化pose2就可以称为pose1的逆。图1-11显示了pose1与其逆（pose2）的关系。

pose1*pose1-1=[[0，0，0]，[1，0，0，0]]，也就是位姿乘其逆等于单位pose（单位矩阵）。单位pose如果用矩阵形式表示，则如下：

RAPID编程中提供了位姿数据pose求逆函数PoseInv，其使用方法如下：

pose2:=PoseInv (pose1)

其中，pose2称为pose1的逆。

对于1.1.4节第2部分中计算姿态数据的逆，也可使用PoseInv函数实现。其中，将pose数据中的trans部分设为[0，0，0]：

4.PoseVect

空间某点p₁的位置用（x，y，z）表示。对于形式如（x，y，z）的数据，也可以称为矢量v。已知p₁在坐标系1下的位置是pos1（x₁，y₁，z₁），坐标系1在坐标系0下的位姿表示是pose1，则p₁在坐标系0下的位置可以用pos2（x₂，y₂，z₂）表示，如图1-12所示。RAPID编程提供了以上数据关系转化的函数PoseVect，即pos2：=PoseVect（pose1，pos1），该函数的典型应用如图1-13所示，已知TCP坐标系mytool（mytool数据基于tool0），又已知新的TCP是在mytool坐标系的z方向延伸150mm，则新的TCP坐标系mytool2就可以用如下代码实现：

PoseVect（pose1，pos1）的实现就是将pose1转变为Matrix4形式的矩阵，以及将pos1转变为4×1的矩阵（为了齐次化，添加一个元素，即[x，y，z，1]T）。具体RAPID实现如下：