五轴机床 AB 双转台结构 正解与逆解模型

1. 结构定义（AB型双转台）

AB型五轴机床由三个直线轴（X, Y, Z）和两个旋转轴（A轴：绕X轴旋转，B轴：绕Y轴旋转）组成，工件安装在两个旋转轴上（A轴和B轴构成双转台），刀具仅通过XYZ移动。

• A轴：绕X轴旋转，角度记为 a（单位：弧度或度）
• B轴：绕Y轴旋转，角度记为 b
• 直线轴位置：(x,y,z)
• 工件坐标系下目标点：P=(Px​,Py​,Pz​)
• 刀具方向单位向量：D=(dx​,dy​,dz​)
• 从B轴到A轴的偏置向量：OBA​=(ox​,oy​,oz​)
• 从A轴到工件原点的偏置向量：OA​=(ax​,ay​,az​)

2. 正运动学模型（Forward Kinematics）

已知：直线轴位置 (x,y,z)，旋转轴角度 a,b

求：刀具与工件在空间中的相对位置和方向

（1）旋转矩阵构造

B轴旋转矩阵（绕Y轴）：

Ry(b) = 
[ cos(b), 0, sin(b)]
[   0   , 1,   0   ]
[-sin(b), 0, cos(b)]

A轴旋转矩阵（绕X轴）：

Rx(a) = 
[1,    0   ,    0   ]
[0, cos(a), -sin(a)]
[0, sin(a),  cos(a)]

总旋转矩阵（先B后A）： R = Rx(a) × Ry(b)

展开为：

R = [
  cos(b),                0               ,        sin(b)        ;
 -sin(b)*sin(a),  cos(a),  cos(b)*sin(a);
 -sin(b)*cos(a), -sin(a),  cos(b)*cos(a)
]

（2）工件原点在机床坐标系中的位置

设：

• B轴旋转中心为原点
• 从B轴到A轴的偏置：O_BA = (o_x, o_y, o_z)
• 从A轴到工件原点的偏置：O_A = (a_x, a_y, a_z)

则工件原点在机床坐标系中的位置为：

P_workpiece = R × (O_A) + Ry(b)^T × O_BA

更准确地： P_w = Ry(b)^T × O_BA + R × O_A

其中 Ry(b)^T = Ry(-b)

即： P_wx = o_x * cos(b) - o_z * sin(b) + R[0][0]*a_x + R[0][1]*a_y + R[0][2]*a_z
P_wy = o_y + R[1][0]*a_x + R[1][1]*a_y + R[1][2]*a_z
P_wz = o_x * sin(b) + o_z * cos(b) + R[2][0]*a_x + R[2][1]*a_y + R[2][2]*a_z

（3）刀具方向在工件坐标系中的表示

刀具方向在机床坐标系中为 D_tool=(0,0,−1)（假设Z向下）

在工件坐标系中，其方向为： D_workpiece = R^T × (0, 0, -1)^T

即取 R 的第三行取负：

d_x = sin(b)
d_y = -cos(b)*sin(a)
d_z = -cos(b)*cos(a)

3. 逆运动学模型（Inverse Kinematics）

已知：工件上目标点 P=(P_x,P_y,P_z)，刀具方向 D=(d_x,d_y,d_z)（单位向量，指向切削方向）

求：直线轴位置(x,y,z) 和旋转角 a,b

（1）求解旋转角 b 和 a

由正解中刀具方向关系：

d_x = sin(b)
d_y = -cos(b) * sin(a)
d_z = -cos(b) * cos(a)

解法：

① 求 B 轴角度 b： b = arcsin(d_x)
或更稳健地（避免除零）： b = atan2(d_x, sqrt(1 - d_x^2))
但更推荐： b = asin(clamp(d_x, -1, 1))

② 求 A 轴角度 a：

若 cos(b) ≠ 0（即 |b| < 90°）： a = atan2( -d_y, -d_z )

注意：atan2(y, x) 返回角度

（2）求解直线轴位置 (x, y, z)

目标是使刀尖到达工件点 P 在机床坐标系中的位置。

刀尖在机床坐标系中的位置为： P_tool = P_w + R × P

其中 P_w 是工件原点在机床坐标系的位置（含偏置）

但更直接的方法是：

设刀尖在机床坐标系中应位于： P_machine = R × (P + O_A) + Ry(-b) × O_BA

而刀具由 (x,y,z) 控制，故： x = P_machine_x
y = P_machine_y
z = P_machine_z

即： x = [cos(b)(P_x + a_x) + sin(b)(P_z + a_z)] + [o_xcos(b) - o_zsin(b)]
y = [-sin(b)sin(a)(P_x + a_x) + cos(a)(P_y + a_y) + cos(b)sin(a)(P_z + a_z)] + o_y
z = [-sin(b)cos(a)(P_x + a_x) - sin(a)(P_y + a_y) + cos(b)cos(a)(P_z + a_z)] + [o_xsin(b) + o_zcos(b)]