KUKA机器人的空间变换问题

在kuka机器人系统的FRAME和POS数据结构包含六个参数：{X ,Y,Z,A,B,C}。这些参数正好对应角度坐标系表示中的六个参数, X,Y,Z,alpha,beta,gama,且alpha=A,beta=B,gama=C。

假设在机器人系统中，{A}是参考坐标系，{B}是工具坐标系，并且相对A的表示为{x,y,z,a,b,c},并且{B}坐标系统有一点BP,那么其在{A}下坐标AP可以表示为：

AP =RxyzBP+APORG，其中 Rxyz=R(a).R(b).R(c),APORG=(x,y,z)

1 固定角坐标系表示法

Rxyz=Rz.Ry.Rx.(x,y,z)

2 欧拉角坐标系表示法

Rz'y'x'=Rz.Ry.Rx.(x,y,z)

3 相互关系

固定角过程可以看做坐标点逐步变换的过程（从右向左计算）；与欧拉角过程则是坐标系动态变换的过程（从左向右计算）

4 矩阵表示

(AP,1) =([R,APorg],[0,1])(BP,1)

5 逆解

平移量XYZ容易获取，注意问题在于角度量的计算，省去过程，答案如下

1) B!=+/-90degree时：

    B =atan2(-r31,sqrt(r11**2+r21**2))

    A = atan2(r21/cB,r11/cB)

    C = atan2(r32/cB,r33/cB)

2) B = 90degree 时：

    A =0degree

    C = atan2(r12,r22)

3)B = -90degree 时：

    A = 0degree

    C = -atan2(r12,r22)