4. 五轴直线插补（双旋转轴）
任务目标
实现五轴直线插补（X、Y、Z线性轴 + A、C旋转轴），在刀尖点坐标系下规划空间直线轨迹，并通过正运动学验证插补点对应的实际刀尖位置是否严格位于目标直线上。
实现方案
采用刀尖点坐标（TCP）插补策略：在工件坐标系中规划刀尖位置 P=[x,y,z] 的直线运动，同时用四元数球面线性插值（Slerp）处理刀轴姿态从起始方向到终止方向的旋转。每个插补周期通过逆运动学求解各轴位置，其中旋转轴角度由姿态插补结果分解得到。本案例聚焦插补算法本身，逆运动学采用简化的正交结构（A绕X轴、C绕Z轴），便于独立验证。
核心代码
import numpy as np
from scipy.spatial.transform import Rotation
def normalize(v):
return v / np.linalg.norm(v)
def slerp(q0, q1, t):
"""四元数球面线性插值"""
dot = np.dot(q0, q1)
if dot < 0:
q1 = -q1
dot = -dot
if dot > 0.9995:
return normalize(q0 + t * (q1 - q0))
theta = np.arccos(np.clip(dot, -1, 1))
return (np.sin((1-t)*theta)*q0 + np.sin(t*theta)*q1) / np.sin(theta)
def inverse_kinematics(pos, quat):
"""
简化正交五轴逆运动学：
旋转顺序：先C轴绕Z旋转，再A轴绕X旋转（工件侧）
返回 [x, y, z, a_deg, c_deg]
"""
# 从四元数提取欧拉角（Z-X约定，外旋）
r = Rotation.from_quat(quat)
# 分解为 C（绕Z）和 A（绕X），按 Z-X 外旋顺序
# 等效于先绕Z转C，再绕新X转A
euler = r.as_euler('ZX', degrees=True) # [c_deg, a_deg]
c_deg, a_deg = euler
return [pos[0], pos[1], pos[2], a_deg, c_deg]
def five_axis_line_interp(start_pose, end_pose, feedrate, dt):
"""
五轴直线插补（TCP模式）
start_pose/end_pose: [x, y, z, qx, qy, qz, qw] 位置+姿态四元数
feedrate: 刀尖线速度 mm/s
dt: 插补周期 s
返回: 各轴位置序列
"""
p0 = np.array(start_pose[:3], dtype=float)
p1 = np.array(end_pose[:3], dtype=float)
q0 = np.array(start_pose[3:], dtype=float)
q1 = np.array(end_pose[3:], dtype=float)
q0 = normalize(q0)
q1 = normalize(q1)
total_dist = np.linalg.norm(p1 - p0)
if total_dist < 1e-9:
return []
# 梯形速度规划（简化为匀速）
total_time = total_dist / feedrate
n_steps = int(np.ceil(total_time / dt)) + 1
axis_path = []
for i in range(n_steps):
t = min(i * dt / total_time, 1.0)
pos = p0 + t * (p1 - p0)
quat = slerp(q0, q1, t)
axis = inverse_kinematics(pos, quat)
axis_path.append(axis)
return np.array(axis_path)
# ========== 验证部分 ==========
def forward_kinematics(axis):
"""正运动学：由各轴位置求刀尖点位置与姿态"""
x, y, z, a_deg, c_deg = axis
# 旋转轴作用于刀具方向（初始沿Z）
r = Rotation.from_euler('ZX', [c_deg, a_deg], degrees=True)
# 刀尖位置 = 线性轴位置（假设旋转轴在刀尖点，即RTCP）
pos = np.array([x, y, z])
return pos, r
def verify_trajectory(axis_path, start_pose, end_pose, tol=1e-6):
"""验证所有插补点是否在目标直线上"""
p0 = np.array(start_pose[:3], dtype=float)
p1 = np.array(end_pose[:3], dtype=float)
direction = p1 - p0
line_len = np.linalg.norm(direction)
direction /= line_len
max_dev = 0.0
for axis in axis_path:
pos, _ = forward_kinematics(axis)
# 点到直线距离
vec = pos - p0
proj = np.dot(vec, direction)
closest = p0 + proj * direction
dev = np.linalg.norm(pos - closest)
max_dev = max(max_dev, dev)
return max_dev
# ========== 示例运行 ==========
if __name__ == "__main__":
# 起点：位置(0,0,50)，刀轴竖直(沿Z)
start = [0, 0, 50, 0, 0, 0, 1]
# 终点：位置(100,50,80)，刀轴倾斜（A=30°, C=45°）
r_end = Rotation.from_euler('ZX', [45, 30], degrees=True)
q_end = r_end.as_quat() # [qx, qy, qz, qw]
end = [100, 50, 80, q_end[0], q_end[1], q_end[2], q_end[3]]
feedrate = 50.0 # mm/s
dt = 0.01 # 10ms插补周期
path = five_axis_line_interp(start, end, feedrate, dt)
print(f"插补点数: {len(path)}")
# 验证1：刀尖点直线度
max_dev = verify_trajectory(path, start, end)
print(f"最大直线偏差: {max_dev:.2e} mm")
assert max_dev < 1e-6, "刀尖点偏离目标直线！"
# 验证2：终点姿态正确
final_pos, final_rot = forward_kinematics(path[-1])
expected_rot = Rotation.from_quat(normalize(np.array(end[3:])))
# 检查姿态误差（旋转矩阵差异的Frobenius范数）
err = np.linalg.norm(final_rot.as_matrix() - expected_rot.as_matrix())
print(f"终点姿态误差: {err:.2e}")
assert err < 1e-6, "终点姿态错误！"
# 验证3：起点位置正确
start_pos, _ = forward_kinematics(path[0])
assert np.allclose(start_pos, start[:3], atol=1e-8)
print("起点位置验证通过")
print("全部验证通过：五轴直线插补实现正确")
运行与验证
依赖：NumPy、SciPy（仅用Rotation类）。运行 python five_axis_interp.py 即可执行。
验证逻辑：
- 刀尖直线度：对每个插补点做正运动学，计算刀尖位置到目标直线的垂直距离，最大偏差应小于1e-6 mm（数值精度范围内）。
- 终点姿态：检查最后一个插补点的姿态旋转矩阵与目标姿态的误差小于1e-6。
- 起点位置：确认首点位置与给定起点一致。
通过标准：程序输出插补点数，三条断言全部通过且打印"全部验证通过"即表示插补算法正确。
关键说明
- 本案例采用RTCP（旋转刀具中心点）结构，即旋转轴交点与刀尖重合，因此线性轴位置直接等于刀尖位置，简化了正逆运动学推导。
- 姿态插补使用Slerp保证旋转路径均匀且无奇异；逆运动学按Z-X外旋顺序将姿态分解为C轴和A轴角度。
- 速度规划为匀速，实际工程可替换为梯形或S形加减速，不影响插补核心逻辑。