五轴机床 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)]