《Python机器人基础原理》第二十八篇焊接作业仿真——笛卡尔空间直线轨迹规划生成末端执行器直线运动位姿序列1. 实际工业应用场景周三下午某汽车零部件焊接工段。一台3-DOF垂直关节臂基座偏航θ₁ 大臂俯仰θ₂ 小臂俯仰θ₃搭载焊枪末端执行器负责在汽车排气管法兰盘上沿直线焊缝进行MAG焊接。机器人构型参数大臂长450 mm、质量4.2 kg质心距基座0.22 m小臂长350 mm、质量2.1 kg质心距肘部0.18 m末端焊枪通过腕部法兰刚性连接质量0.8 kgTCP工具中心点位于法兰正下方120 mm处。关节限位θ₁±180°、最大角速度2.8 rad/s、最大角加速度7.0 rad/s²θ₂±130°、最大角速度2.2 rad/s、最大角加速度5.0 rad/s²θ₃±150°、最大角速度3.0 rad/s、最大角加速度8.0 rad/s²。焊缝是一条空间直线段起点在基座坐标系中的位置为 (0.35 m, 0.12 m, 0.30 m)终点为 (0.55 m, 0.12 m, 0.30 m)长度200 mm位于水平面内。要求焊枪TCP沿此直线以恒定线速度 0.05 m/s 匀速运动焊枪姿态保持垂直向下焊枪轴线与重力方向平行TCP始终垂直于工件表面。小赵接到任务在离线仿真中验证这条直线轨迹是否可行——生成TCP的笛卡尔空间直线位姿序列通过逆运动学求解各关节角度随时间的变化输出关节空间的位置/速度/加速度曲线校验全程是否超出关节限位。他先在纸上画了个草图TCP从(0.35, 0.12, 0.30)到(0.55, 0.12, 0.30)沿Y0.12的水平线走200 mm。速度0.05 m/s需要4.0秒。小李路过看了一眼你打算怎么让TCP走直线直接给起点终点关节角度做插值对关节空间五次多项式。那TCP走的是直线吗小赵愣了一下。关节空间里做插值TCP在笛卡尔空间里走的是曲线。你的焊枪火焰轨迹会弯焊缝直接报废。……那怎么办笛卡尔空间直线规划。先在笛卡尔空间把TCP的直线轨迹按时间采样每个采样点通过逆运动学算出对应的关节角度。这样TCP走的是真直线但各关节的速度不会均匀——映射回来之后是非线性的。逆解失败怎么办比如某个点够不到离线校验工具的正确行为是直接报出来——哪一段不可达、为什么。不能偷偷用上一个点的角度填上那样后面的速度差分全是假的。本篇解决的问题给定TCP起点和终点的笛卡尔坐标及恒定姿态在笛卡尔空间生成直线位置序列姿态恒定四元数Slerp退化为恒等映射相关代码为后续姿态渐变场景预留通过逆运动学求解各关节角度输出关节空间轨迹曲线全程校验限位逆解不可达点直接中断并报告。⚠️ 声明本篇全部代码为课程离线仿真运行在普通PC上用于离线焊接轨迹规划校验。不可直接用于真实机器人控制器、焊接电源或产线设备。真实工业现场需结合焊接工艺参数电流、电压、送丝速度、实时逆解、奇异点回避、外部轴同步。代码作者不对直接使用本代码于真实设备造成的任何后果负责。2. 痛点分析课程模块 本篇对应的工程痛点第2章 位姿描述与坐标变换 → 2.3 旋转矩阵与四元数 姿态表示需从旋转矩阵转换为四元数为后续球面线性插值Slerp打基础第3章 连杆参数及运动学求解 → 3.2 逆运动学 笛卡尔空间每个采样点需通过逆运动学求解关节角度解析解存在多解需选最近解第6章 轨迹规划与控制 → 6.5 笛卡尔空间轨迹规划 关节空间插值无法保证TCP走直线→必须在笛卡尔空间规划→逆解映射本篇代码解决 L2 离线笛卡尔轨迹规划校验层。输入TCP起点/终点位姿、运动时间、采样频率。在笛卡尔空间线性插值生成位置序列调用逆运动学求解器逐点计算关节角度输出关节空间轨迹曲线、限位校验报告、不可达点报告。不覆盖焊接工艺参数建模熔池动力学、热变形、实时迭代逆解、奇异点回避策略、力控焊接接触力反馈、姿态渐变焊缝如船形焊、环缝需启用Slerp。3. 核心逻辑讲解物理直觉想象你手里拿着一支笔要在纸上画一条直线。你不会去想手腕转多少度、肘关节弯多少度——你只关心笔尖在纸上的轨迹。这就是笛卡尔空间规划先确定末端TCP要走的路径再反推每个关节该怎么动。但机器人不是人的手臂它有严格的关节限位和速度上限。所以你需要把笛卡尔路径翻译成关节的运动指令。翻译的过程就是逆运动学。关键难点笛卡尔空间的匀速直线运动映射回关节空间后各关节的角速度不是常数。靠近奇异构型时某个关节需要快速旋转来补偿TCP的小位移——这就是奇异点问题。本篇的3-DOF垂直关节臂在完全伸直或完全折叠时会出现奇异代码中通过限位校验和不可达检测来暴露这个问题。位置线性插值 姿态处理TCP位置从起点到终点做线性插值p(t) p₀ (p_T - p₀) · (t/T_total)。每一项含义p₀是起点坐标p_T - p₀是总位移向量t/T_total是归一化时间0到1乘起来就是当前时刻的位置。匀速直线运动的直接结果。关于姿态本篇焊缝要求焊枪始终垂直向下起点和终点的姿态四元数完全相同q₀ q₁。因此四元数球面线性插值Slerp退化为恒等映射——每个采样点输出相同的四元数。代码中transforms.py实现了完整的Slerp含最短路径检测和线性退化保护但本篇不调用它参与逆解。这是有意为之的预留设计当后续场景需要姿态渐变如船形焊、角焊缝、法兰环缝起弧段时Slerp结果将作为逆解的腕部方向约束输入。本篇重点验证位置→逆解→关节轨迹的核心链路。逆运动学几何法将3-DOF垂直关节臂投影到垂直平面。已知TCP的(x, y, z)腕部在TCP正上方tcp_offset处因为焊枪垂直向下所以腕部位置为(x, y, z tcp_offset)。- θ₁基座偏航由腕部在基座平面的投影决定θ₁ atan2(y, x)- 腕部到基座的水平距离 r √(x² y²)高度 h z tcp_offset- 腕部到基座的直线距离 d √(r² h²)- 余弦定理求θ₃cos(θ₃) (d² - L₁² - L₂²) / (2·L₁·L₂)- θ₂ atan2(h, r) - atan2(L₂·sin(θ₃), L₁ L₂·cos(θ₃))可达性检查d L₁ L₂外不可达或 d |L₁ - L₂|内不可达/奇异时该点不可达直接中断。业务逻辑 → 代码映射输入TCP起点位姿 (x0,y0,z0, q0)、终点位姿 (xT,yT,zT, qT)、运动时间 T_total、采样数 N↓步骤1笛卡尔空间位置线性插值p(t) p0 (pT - p0) · (t/T_total)姿态q(t) q0本篇恒定Slerp预留↓步骤2逆运动学求解几何法逐点对每点 (x, y, z)计算腕部位置 → θ₁ atan2(y, x)余弦定理求θ₃ → 两个分支肘上/肘下求θ₂ → 最近解策略选与上一时刻差值最小的解可达性检查不可达则中断并报告↓步骤3有限差分计算速度和加速度ω_j Δθ/Δt中心差分端点用前向/后向α_j Δω/Δt↓步骤4全程校验角度限位、角速度、角加速度逐点校验不可达点报告↓输出4张图表 校验报告关键公式文字翻译位置线性插值p(t) p₀ (p_T - p₀) × (t/T)。匀速直线运动的直接离散化。余弦定理逆解cos(θ₃) (d² - L₁² - L₂²) / (2·L₁·L₂)。d是腕部到基座的距离L₁和L₂是大小臂长度。这个公式来源于三角形三边关系——大小臂和腕部-基座连线构成一个三角形已知三边求内角。中心差分速度ω[i] (θ[i1] - θ[i-1]) / (2Δt)。用前后两个点来估计中间点的导数比前向差分精度更高O(Δt²)但端点只能用前向/后向差分。4. OOP代码实现技术栈严格限定numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。项目结构树welding_cartesian_trajectory_sim/├── run_welding_cartesian_trajectory_sim.py├── requirements.txt├── robot/│ ├── __init__.py│ ├── models.py│ ├── transforms.py│ ├── inverse_kinematics.py│ ├── trajectory_generator.py│ └── visualize_trajectory.py├── tests/│ ├── __init__.py│ └── test_welding_cartesian_trajectory_sim.py└── results/核心源码robot/models.py — 数据模型机器人数据模型连杆参数、关节限位、TCP位姿3-DOF垂直关节臂基座偏航θ₁ 大臂俯仰θ₂ 小臂俯仰θ₃from dataclasses import dataclass, fieldfrom typing import Listimport numpy as npdataclassclass LinkParams:单连杆几何与质量参数name: strlength: float # 连杆长度 (m)mass: float # 连杆质量 (kg)com_x: float # 质心x坐标沿连杆方向(m)com_y: float # 质心y坐标 (m)com_z: float # 质心z坐标 (m)inertia_xx: float 0.0 # 惯性张量Ixx (kg·m²)inertia_yy: float 0.0inertia_zz: float 0.0dataclassclass JointLimit:单关节限位joint_name: strmin_angle: float # 最小角度 (rad)max_angle: float # 最大角度 (rad)max_velocity: float # 最大角速度 (rad/s)max_acceleration: float # 最大角加速度 (rad/s²)dataclassclass JointLimitSet:关节限位集合limits: List[JointLimit] field(default_factorylist)def check_all_positions(self, angles: List[float]) - tuple:校验关节角度是否全部在限位内violations []for i, angle in enumerate(angles):if i len(self.limits):if angle self.limits[i].min_angle or angle self.limits[i].max_angle:violations.append(i)return len(violations) 0, violationsdef check_all_velocities(self, velocities: List[float]) - tuple:校验角速度是否全部在限位内violations []for i, vel in enumerate(velocities):if i len(self.limits):if abs(vel) self.limits[i].max_velocity:violations.append(i)return len(violations) 0, violationsdef check_all_accelerations(self, accelerations: List[float]) - tuple:校验角加速度是否全部在限位内violations []for i, acc in enumerate(accelerations):if i len(self.limits):if abs(acc) self.limits[i].max_acceleration:violations.append(i)return len(violations) 0, violationsdataclassclass TCPPose:TCP位姿位置(x,y,z) 姿态四元数(qw,qx,qy,qz)x: floaty: floatz: floatqw: float 1.0qx: float 0.0qy: float 0.0qz: float 0.0def position(self) - np.ndarray:return np.array([self.x, self.y, self.z])def quaternion(self) - np.ndarray:return np.array([self.qw, self.qx, self.qy, self.qz])robot/transforms.py — 坐标变换与四元数运算坐标变换工具旋转矩阵、四元数、齐次变换矩阵四元数球面线性插值Slerp——本篇姿态恒定时退化为恒等映射为后续姿态渐变场景预留import numpy as npdef euler_to_rotation_matrix(roll: float, pitch: float, yaw: float) - np.ndarray:将ZYX欧拉角转换为3×3旋转矩阵roll绕x轴, pitch绕y轴, yaw绕z轴cr, sr np.cos(roll), np.sin(roll)cp, sp np.cos(pitch), np.sin(pitch)cy, sy np.cos(yaw), np.sin(yaw)R np.array([[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr sy*sr],[sy*cp, sy*sp*sr cy*cr, sy*sp*cr - cy*sr],[-sp, cp*sr, cp*cr]])return Rdef rotation_matrix_to_quaternion(R: np.ndarray) - np.ndarray:将3×3旋转矩阵转换为单位四元数 [qw, qx, qy, qz]使用Shepperd法避免数值不稳定q np.zeros(4)trace np.trace(R)if trace 0:s 0.5 / np.sqrt(trace 1.0)q[0] 0.25 / sq[1] (R[2, 1] - R[1, 2]) * sq[2] (R[0, 2] - R[2, 0]) * sq[3] (R[1, 0] - R[0, 1]) * selse:if R[0, 0] R[1, 1] and R[0, 0] R[2, 2]:s 2.0 * np.sqrt(1.0 R[0, 0] - R[1, 1] - R[2, 2])q[0] (R[2, 1] - R[1, 2]) / sq[1] 0.25 * sq[2] (R[0, 1] R[1, 0]) / sq[3] (R[0, 2] R[2, 0]) / selif R[1, 1] R[2, 2]:s 2.0 * np.sqrt(1.0 R[1, 1] - R[0, 0] - R[2, 2])q[0] (R[0, 2] - R[2, 0]) / sq[1] (R[0, 1] R[1, 0]) / sq[2] 0.25 * sq[3] (R[1, 2] R[2, 1]) / selse:s 2.0 * np.sqrt(1.0 R[2, 2] - R[0, 0] - R[1, 1])q[0] (R[1, 0] - R[0, 1]) / sq[1] (R[0, 2] R[2, 0]) / sq[2] (R[1, 2] R[2, 1]) / sq[3] 0.25 * sreturn q / np.linalg.norm(q)def quaternion_slerp(q0: np.ndarray, q1: np.ndarray, t: float) - np.ndarray:四元数球面线性插值Slerp本篇焊缝姿态恒定q0q1此函数退化为恒等映射返回q0为后续姿态渐变场景船形焊、环缝预留q0, q1: 单位四元数 [qw, qx, qy, qz]t: 插值参数0≤t≤1# 数值保护确保四元数为单位四元数q0 q0 / np.linalg.norm(q0)q1 q1 / np.linalg.norm(q1)dot np.dot(q0, q1)# 如果点积为负取q1的反方向以确保最短路径if dot 0.0:q1 -q1dot -dot# 如果点积接近1使用线性插值避免除零if dot 0.9995:result q0 t * (q1 - q0)return result / np.linalg.norm(result)theta_0 np.arccos(np.clip(dot, -1.0, 1.0))theta theta_0 * ts0 np.sin(theta_0 - theta) / np.sin(theta_0)s1 np.sin(theta) / np.sin(theta_0)return s0 * q0 s1 * q1def quaternion_to_rotation_matrix(q: np.ndarray) - np.ndarray:将单位四元数转换为3×3旋转矩阵qw, qx, qy, qz qR np.array([[1 - 2*(qy**2 qz**2), 2*(qx*qy - qw*qz), 2*(qx*qz qw*qy)],[2*(qx*qy qw*qz), 1 - 2*(qx**2 qz**2), 2*(qy*qz - qw*qx)],[2*(qx*qz - qw*qy), 2*(qy*qz qw*qx), 1 - 2*(qx**2 qy**2)]])return Rrobot/inverse_kinematics.py — 逆运动学求解器3-DOF垂直关节臂逆运动学求解器几何法解析求解θ₁atan2(y,x), θ₂和θ₃由余弦定理求得支持最近解选择策略避免关节跳变不可达点直接返回失败标志不静默填充import numpy as npfrom typing import List, Tuple, Optionalfrom robot.models import JointLimitSetclass InverseKinematicsSolver:3-DOF垂直关节臂逆运动学构型基座偏航θ₁ 大臂俯仰θ₂ 小臂俯仰θ₃焊枪垂直向下TCP在腕部法兰正下方 tcp_offset 处def __init__(self, L1: float, L2: float, tcp_offset: float,joint_limits: JointLimitSet):self.L1 L1self.L2 L2self.tcp_offset tcp_offsetself.limits joint_limitsdef solve(self, tcp_x: float, tcp_y: float, tcp_z: float,prev_angles: Optional[List[float]] None) - Tuple[np.ndarray, bool]:逆运动学求解输入TCP在基座坐标系中的位置 (m)输出(关节角度数组 [θ₁, θ₂, θ₃], 是否成功)不可达时返回 (零数组, False)由调用方决定是否中断# θ₁基座偏航theta1 np.arctan2(tcp_y, tcp_x)# 腕部位置焊枪垂直向下腕部在TCP正上方tcp_offset处wrist_x tcp_xwrist_y tcp_ywrist_z tcp_z self.tcp_offset# 腕部到基座的水平距离和高度r np.sqrt(wrist_x**2 wrist_y**2)h wrist_z# 腕部到基座的直线距离d np.sqrt(r**2 h**2)# 可达性检查外不可达和内奇异if d self.L1 self.L2:return np.array([0.0, 0.0, 0.0]), Falseif d abs(self.L1 - self.L2):return np.array([0.0, 0.0, 0.0]), Falsecos_theta3 (d**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)cos_theta3 np.clip(cos_theta3, -1.0, 1.0)# 两个解分支肘部向上(负角)和肘部向下(正角)theta3_option1 -np.arccos(cos_theta3)theta3_option2 np.arccos(cos_theta3)solutions []for theta3 in [theta3_option1, theta3_option2]:alpha np.arctan2(h, r)beta np.arctan2(self.L2 * np.sin(theta3),self.L1 self.L2 * np.cos(theta3))theta2 alpha - betaangles np.array([theta1, theta2, theta3])solutions.append(angles)# 检查限位并选择最优解valid_solutions []for sol in solutions:is_valid, _ self.limits.check_all_positions(sol.tolist())if is_valid:valid_solutions.append(sol)if len(valid_solutions) 0:return np.array([0.0, 0.0, 0.0]), Falseif prev_angles is None:return valid_solutions[0], True# 最近解策略prev np.array(prev_angles)best_sol valid_solutions[0]best_diff np.sum(np.abs(valid_solutions[0] - prev))for sol in valid_solutions[1:]:diff np.sum(np.abs(sol - prev))if diff best_diff:best_diff diffbest_sol solreturn best_sol, Truerobot/trajectory_generator.py — 笛卡尔空间直线轨迹生成笛卡尔空间直线轨迹规划位置线性插值 逆运动学逐点求解逆解失败时直接中断并报告不可达点import numpy as npfrom typing import List, Dictfrom robot.models import TCPPose, JointLimitSetfrom robot.inverse_kinematics import InverseKinematicsSolverclass CartesianLinearTrajectoryGenerator:笛卡尔空间直线轨迹生成器TCP从起点到终点沿直线运动姿态恒定垂直向下每个采样点通过逆运动学求解关节角度逆解失败直接中断不静默填充def __init__(self, ik_solver: InverseKinematicsSolver,joint_limits: JointLimitSet):self.ik ik_solverself.limits joint_limitsdef generate(self, start_pose: TCPPose, end_pose: TCPPose,total_time: float, n_samples: int) - Dict:if total_time 0:raise ValueError(运动时间必须大于0)if n_samples 2:raise ValueError(采样数至少为2)t_array np.linspace(0, total_time, n_samples)dt total_time / (n_samples - 1)tcp_positions np.zeros((n_samples, 3))joint_angles np.zeros((n_samples, 3))joint_velocities np.zeros((n_samples, 3))joint_accelerations np.zeros((n_samples, 3))ik_success_flags np.zeros(n_samples, dtypebool)unreachable_indices [] # 记录不可达的采样点索引prev_angles Nonestart_pos start_pose.position()for i, t in enumerate(t_array):tau t / total_time# 位置线性插值tcp_positions[i, :] start_pos (end_pose.position() - start_pos) * tau# 逆运动学求解——失败直接中断angles, success self.ik.solve(tcp_positions[i, 0], tcp_positions[i, 1],tcp_positions[i, 2], prev_angles)if not success:unreachable_indices.append(i)# 不可达点不填充假数据标记失败并停止后续计算breakjoint_angles[i, :] anglesik_success_flags[i] Trueprev_angles angles.tolist()# 如果有不可达点截断有效数据到第一个不可达点之前if len(unreachable_indices) 0:first_fail unreachable_indices[0]n_valid first_failelse:n_valid n_samples# 有限差分计算速度和加速度仅对有效数据if n_valid 2:for j in range(3):# 中心差分内部点if n_valid 2:joint_velocities[1:n_valid-1, j] (joint_angles[2:n_valid, j] - joint_angles[0:n_valid-2, j]) / (2 * dt)# 端点前向/后向差分joint_velocities[0, j] (joint_angles[1, j] - joint_angles[0, j]) / dtif n_valid 1:joint_velocities[n_valid-1, j] (joint_angles[n_valid-1, j] - joint_angles[n_valid-2, j]) / dtfor j in range(3):if n_valid 2:joint_accelerations[1:n_valid-1, j] (joint_velocities[2:n_valid, j] - joint_velocities[0:n_valid-2, j]) / (2 * dt)joint_accelerations[0, j] (joint_velocities[1, j] - joint_velocities[0, j]) / dtif n_valid 1:joint_accelerations[n_valid-1, j] (joint_velocities[n_valid-1, j] - joint_velocities[n_valid-2, j]) / dt# 全程限位校验仅对有效数据velocity_violations []acceleration_violations []for i in range(n_valid):vel_list joint_velocities[i, :].tolist()acc_list joint_accelerations[i, :].tolist()_, v_viol self.limits.check_all_velocities(vel_list)_, a_viol self.limits.check_all_accelerations(acc_list)for j in v_viol:velocity_violations.append((i, j, vel_list[j],self.limits.limits[j].max_velocity))for j in a_viol:acceleration_violations.append((i, j, acc_list[j],self.limits.limits[j].max_acceleration))return {time: t_array[:n_valid],tcp_positions: tcp_positions[:n_valid],joint_angles: joint_angles[:n_valid],joint_velocities: joint_velocities[:n_valid],joint_accelerations: joint_accelerations[:n_valid],ik_success_flags: ik_success_flags[:n_valid],unreachable_indices: unreachable_indices,velocity_violations: velocity_violations,acceleration_violations: acceleration_violations,n_valid_samples: n_valid,n_total_samples: n_samples}robot/visualize_trajectory.py — 可视化模块焊接作业笛卡尔轨迹规划可视化4张核心图表TCP三维直线轨迹、关节角度曲线、关节角速度曲线、关节角加速度曲线import numpy as npimport matplotlib.pyplot as pltplt.rcParams[font.sans-serif] [SimHei, DejaVu Sans]plt.rcParams[axes.unicode_minus] Falseclass TrajectoryVisualizer:def __init__(self, results_dirresults):self.results_dir results_dirimport osos.makedirs(results_dir, exist_okTrue)def plot_tcp_3d_trajectory(self, tcp_positions, start_pos, end_pos):① TCP三维直线轨迹验证笛卡尔空间直线运动fig plt.figure(figsize(10, 8))ax fig.add_subplot(111, projection3d)ax.plot(tcp_positions[:, 0], tcp_positions[:, 1], tcp_positions[:, 2],b-, linewidth1.5, labelTCP Trajectory)ax.scatter([start_pos[0]], [start_pos[1]], [start_pos[2]],cgreen, markero, s80, labelStart, zorder5)ax.scatter([end_pos[0]], [end_pos[1]], [end_pos[2]],cred, markers, s80, labelEnd, zorder5)ax.set_xlabel(X (m))ax.set_ylabel(Y (m))ax.set_zlabel(Z (m))ax.set_title(TCP 3D Linear Trajectory in Cartesian Space)ax.legend(fontsize8)ax.grid(True, alpha0.3)plt.tight_layout()path f{self.results_dir}/01_tcp_3d_linear_trajectory.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_joint_angles(self, timestamps, joint_angles):② 关节角度曲线验证逆运动学求解的平滑性fig, ax plt.subplots(figsize(14, 5))joint_names [θ₁ (Base Yaw), θ₂ (Elbow Pitch), θ₃ (Wrist Pitch)]colors [steelblue, orange, green]for j in range(3):ax.plot(timestamps, joint_angles[:, j], -,colorcolors[j], linewidth1.2, labeljoint_names[j])ax.set_xlabel(Time (s))ax.set_ylabel(Joint Angle (rad))ax.set_title(Joint Angle Profiles from Cartesian Linear Trajectory)ax.legend(locupper right, fontsize8)ax.grid(True, alpha0.3)plt.tight_layout()path f{self.results_dir}/02_joint_angle_curves.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_joint_velocities(self, timestamps, joint_velocities, joint_limits):③ 关节角速度曲线限位线验证速度不超限fig, axes plt利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛
《Python机器人基础原理》第二十八篇焊接作业仿真——笛卡尔空间直线轨迹规划生成末端执行器直线运动位姿序列1. 实际工业应用场景周三下午某汽车零部件焊接工段。一台3-DOF垂直关节臂基座偏航θ₁ 大臂俯仰θ₂ 小臂俯仰θ₃搭载焊枪末端执行器负责在汽车排气管法兰盘上沿直线焊缝进行MAG焊接。机器人构型参数大臂长450 mm、质量4.2 kg质心距基座0.22 m小臂长350 mm、质量2.1 kg质心距肘部0.18 m末端焊枪通过腕部法兰刚性连接质量0.8 kgTCP工具中心点位于法兰正下方120 mm处。关节限位θ₁±180°、最大角速度2.8 rad/s、最大角加速度7.0 rad/s²θ₂±130°、最大角速度2.2 rad/s、最大角加速度5.0 rad/s²θ₃±150°、最大角速度3.0 rad/s、最大角加速度8.0 rad/s²。焊缝是一条空间直线段起点在基座坐标系中的位置为 (0.35 m, 0.12 m, 0.30 m)终点为 (0.55 m, 0.12 m, 0.30 m)长度200 mm位于水平面内。要求焊枪TCP沿此直线以恒定线速度 0.05 m/s 匀速运动焊枪姿态保持垂直向下焊枪轴线与重力方向平行TCP始终垂直于工件表面。小赵接到任务在离线仿真中验证这条直线轨迹是否可行——生成TCP的笛卡尔空间直线位姿序列通过逆运动学求解各关节角度随时间的变化输出关节空间的位置/速度/加速度曲线校验全程是否超出关节限位。他先在纸上画了个草图TCP从(0.35, 0.12, 0.30)到(0.55, 0.12, 0.30)沿Y0.12的水平线走200 mm。速度0.05 m/s需要4.0秒。小李路过看了一眼你打算怎么让TCP走直线直接给起点终点关节角度做插值对关节空间五次多项式。那TCP走的是直线吗小赵愣了一下。关节空间里做插值TCP在笛卡尔空间里走的是曲线。你的焊枪火焰轨迹会弯焊缝直接报废。……那怎么办笛卡尔空间直线规划。先在笛卡尔空间把TCP的直线轨迹按时间采样每个采样点通过逆运动学算出对应的关节角度。这样TCP走的是真直线但各关节的速度不会均匀——映射回来之后是非线性的。逆解失败怎么办比如某个点够不到离线校验工具的正确行为是直接报出来——哪一段不可达、为什么。不能偷偷用上一个点的角度填上那样后面的速度差分全是假的。本篇解决的问题给定TCP起点和终点的笛卡尔坐标及恒定姿态在笛卡尔空间生成直线位置序列姿态恒定四元数Slerp退化为恒等映射相关代码为后续姿态渐变场景预留通过逆运动学求解各关节角度输出关节空间轨迹曲线全程校验限位逆解不可达点直接中断并报告。⚠️ 声明本篇全部代码为课程离线仿真运行在普通PC上用于离线焊接轨迹规划校验。不可直接用于真实机器人控制器、焊接电源或产线设备。真实工业现场需结合焊接工艺参数电流、电压、送丝速度、实时逆解、奇异点回避、外部轴同步。代码作者不对直接使用本代码于真实设备造成的任何后果负责。2. 痛点分析课程模块 本篇对应的工程痛点第2章 位姿描述与坐标变换 → 2.3 旋转矩阵与四元数 姿态表示需从旋转矩阵转换为四元数为后续球面线性插值Slerp打基础第3章 连杆参数及运动学求解 → 3.2 逆运动学 笛卡尔空间每个采样点需通过逆运动学求解关节角度解析解存在多解需选最近解第6章 轨迹规划与控制 → 6.5 笛卡尔空间轨迹规划 关节空间插值无法保证TCP走直线→必须在笛卡尔空间规划→逆解映射本篇代码解决 L2 离线笛卡尔轨迹规划校验层。输入TCP起点/终点位姿、运动时间、采样频率。在笛卡尔空间线性插值生成位置序列调用逆运动学求解器逐点计算关节角度输出关节空间轨迹曲线、限位校验报告、不可达点报告。不覆盖焊接工艺参数建模熔池动力学、热变形、实时迭代逆解、奇异点回避策略、力控焊接接触力反馈、姿态渐变焊缝如船形焊、环缝需启用Slerp。3. 核心逻辑讲解物理直觉想象你手里拿着一支笔要在纸上画一条直线。你不会去想手腕转多少度、肘关节弯多少度——你只关心笔尖在纸上的轨迹。这就是笛卡尔空间规划先确定末端TCP要走的路径再反推每个关节该怎么动。但机器人不是人的手臂它有严格的关节限位和速度上限。所以你需要把笛卡尔路径翻译成关节的运动指令。翻译的过程就是逆运动学。关键难点笛卡尔空间的匀速直线运动映射回关节空间后各关节的角速度不是常数。靠近奇异构型时某个关节需要快速旋转来补偿TCP的小位移——这就是奇异点问题。本篇的3-DOF垂直关节臂在完全伸直或完全折叠时会出现奇异代码中通过限位校验和不可达检测来暴露这个问题。位置线性插值 姿态处理TCP位置从起点到终点做线性插值p(t) p₀ (p_T - p₀) · (t/T_total)。每一项含义p₀是起点坐标p_T - p₀是总位移向量t/T_total是归一化时间0到1乘起来就是当前时刻的位置。匀速直线运动的直接结果。关于姿态本篇焊缝要求焊枪始终垂直向下起点和终点的姿态四元数完全相同q₀ q₁。因此四元数球面线性插值Slerp退化为恒等映射——每个采样点输出相同的四元数。代码中transforms.py实现了完整的Slerp含最短路径检测和线性退化保护但本篇不调用它参与逆解。这是有意为之的预留设计当后续场景需要姿态渐变如船形焊、角焊缝、法兰环缝起弧段时Slerp结果将作为逆解的腕部方向约束输入。本篇重点验证位置→逆解→关节轨迹的核心链路。逆运动学几何法将3-DOF垂直关节臂投影到垂直平面。已知TCP的(x, y, z)腕部在TCP正上方tcp_offset处因为焊枪垂直向下所以腕部位置为(x, y, z tcp_offset)。- θ₁基座偏航由腕部在基座平面的投影决定θ₁ atan2(y, x)- 腕部到基座的水平距离 r √(x² y²)高度 h z tcp_offset- 腕部到基座的直线距离 d √(r² h²)- 余弦定理求θ₃cos(θ₃) (d² - L₁² - L₂²) / (2·L₁·L₂)- θ₂ atan2(h, r) - atan2(L₂·sin(θ₃), L₁ L₂·cos(θ₃))可达性检查d L₁ L₂外不可达或 d |L₁ - L₂|内不可达/奇异时该点不可达直接中断。业务逻辑 → 代码映射输入TCP起点位姿 (x0,y0,z0, q0)、终点位姿 (xT,yT,zT, qT)、运动时间 T_total、采样数 N↓步骤1笛卡尔空间位置线性插值p(t) p0 (pT - p0) · (t/T_total)姿态q(t) q0本篇恒定Slerp预留↓步骤2逆运动学求解几何法逐点对每点 (x, y, z)计算腕部位置 → θ₁ atan2(y, x)余弦定理求θ₃ → 两个分支肘上/肘下求θ₂ → 最近解策略选与上一时刻差值最小的解可达性检查不可达则中断并报告↓步骤3有限差分计算速度和加速度ω_j Δθ/Δt中心差分端点用前向/后向α_j Δω/Δt↓步骤4全程校验角度限位、角速度、角加速度逐点校验不可达点报告↓输出4张图表 校验报告关键公式文字翻译位置线性插值p(t) p₀ (p_T - p₀) × (t/T)。匀速直线运动的直接离散化。余弦定理逆解cos(θ₃) (d² - L₁² - L₂²) / (2·L₁·L₂)。d是腕部到基座的距离L₁和L₂是大小臂长度。这个公式来源于三角形三边关系——大小臂和腕部-基座连线构成一个三角形已知三边求内角。中心差分速度ω[i] (θ[i1] - θ[i-1]) / (2Δt)。用前后两个点来估计中间点的导数比前向差分精度更高O(Δt²)但端点只能用前向/后向差分。4. OOP代码实现技术栈严格限定numpy, scipy, matplotlib。不引入pybullet、roboticstoolbox等任何第三方机器人库。项目结构树welding_cartesian_trajectory_sim/├── run_welding_cartesian_trajectory_sim.py├── requirements.txt├── robot/│ ├── __init__.py│ ├── models.py│ ├── transforms.py│ ├── inverse_kinematics.py│ ├── trajectory_generator.py│ └── visualize_trajectory.py├── tests/│ ├── __init__.py│ └── test_welding_cartesian_trajectory_sim.py└── results/核心源码robot/models.py — 数据模型机器人数据模型连杆参数、关节限位、TCP位姿3-DOF垂直关节臂基座偏航θ₁ 大臂俯仰θ₂ 小臂俯仰θ₃from dataclasses import dataclass, fieldfrom typing import Listimport numpy as npdataclassclass LinkParams:单连杆几何与质量参数name: strlength: float # 连杆长度 (m)mass: float # 连杆质量 (kg)com_x: float # 质心x坐标沿连杆方向(m)com_y: float # 质心y坐标 (m)com_z: float # 质心z坐标 (m)inertia_xx: float 0.0 # 惯性张量Ixx (kg·m²)inertia_yy: float 0.0inertia_zz: float 0.0dataclassclass JointLimit:单关节限位joint_name: strmin_angle: float # 最小角度 (rad)max_angle: float # 最大角度 (rad)max_velocity: float # 最大角速度 (rad/s)max_acceleration: float # 最大角加速度 (rad/s²)dataclassclass JointLimitSet:关节限位集合limits: List[JointLimit] field(default_factorylist)def check_all_positions(self, angles: List[float]) - tuple:校验关节角度是否全部在限位内violations []for i, angle in enumerate(angles):if i len(self.limits):if angle self.limits[i].min_angle or angle self.limits[i].max_angle:violations.append(i)return len(violations) 0, violationsdef check_all_velocities(self, velocities: List[float]) - tuple:校验角速度是否全部在限位内violations []for i, vel in enumerate(velocities):if i len(self.limits):if abs(vel) self.limits[i].max_velocity:violations.append(i)return len(violations) 0, violationsdef check_all_accelerations(self, accelerations: List[float]) - tuple:校验角加速度是否全部在限位内violations []for i, acc in enumerate(accelerations):if i len(self.limits):if abs(acc) self.limits[i].max_acceleration:violations.append(i)return len(violations) 0, violationsdataclassclass TCPPose:TCP位姿位置(x,y,z) 姿态四元数(qw,qx,qy,qz)x: floaty: floatz: floatqw: float 1.0qx: float 0.0qy: float 0.0qz: float 0.0def position(self) - np.ndarray:return np.array([self.x, self.y, self.z])def quaternion(self) - np.ndarray:return np.array([self.qw, self.qx, self.qy, self.qz])robot/transforms.py — 坐标变换与四元数运算坐标变换工具旋转矩阵、四元数、齐次变换矩阵四元数球面线性插值Slerp——本篇姿态恒定时退化为恒等映射为后续姿态渐变场景预留import numpy as npdef euler_to_rotation_matrix(roll: float, pitch: float, yaw: float) - np.ndarray:将ZYX欧拉角转换为3×3旋转矩阵roll绕x轴, pitch绕y轴, yaw绕z轴cr, sr np.cos(roll), np.sin(roll)cp, sp np.cos(pitch), np.sin(pitch)cy, sy np.cos(yaw), np.sin(yaw)R np.array([[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr sy*sr],[sy*cp, sy*sp*sr cy*cr, sy*sp*cr - cy*sr],[-sp, cp*sr, cp*cr]])return Rdef rotation_matrix_to_quaternion(R: np.ndarray) - np.ndarray:将3×3旋转矩阵转换为单位四元数 [qw, qx, qy, qz]使用Shepperd法避免数值不稳定q np.zeros(4)trace np.trace(R)if trace 0:s 0.5 / np.sqrt(trace 1.0)q[0] 0.25 / sq[1] (R[2, 1] - R[1, 2]) * sq[2] (R[0, 2] - R[2, 0]) * sq[3] (R[1, 0] - R[0, 1]) * selse:if R[0, 0] R[1, 1] and R[0, 0] R[2, 2]:s 2.0 * np.sqrt(1.0 R[0, 0] - R[1, 1] - R[2, 2])q[0] (R[2, 1] - R[1, 2]) / sq[1] 0.25 * sq[2] (R[0, 1] R[1, 0]) / sq[3] (R[0, 2] R[2, 0]) / selif R[1, 1] R[2, 2]:s 2.0 * np.sqrt(1.0 R[1, 1] - R[0, 0] - R[2, 2])q[0] (R[0, 2] - R[2, 0]) / sq[1] (R[0, 1] R[1, 0]) / sq[2] 0.25 * sq[3] (R[1, 2] R[2, 1]) / selse:s 2.0 * np.sqrt(1.0 R[2, 2] - R[0, 0] - R[1, 1])q[0] (R[1, 0] - R[0, 1]) / sq[1] (R[0, 2] R[2, 0]) / sq[2] (R[1, 2] R[2, 1]) / sq[3] 0.25 * sreturn q / np.linalg.norm(q)def quaternion_slerp(q0: np.ndarray, q1: np.ndarray, t: float) - np.ndarray:四元数球面线性插值Slerp本篇焊缝姿态恒定q0q1此函数退化为恒等映射返回q0为后续姿态渐变场景船形焊、环缝预留q0, q1: 单位四元数 [qw, qx, qy, qz]t: 插值参数0≤t≤1# 数值保护确保四元数为单位四元数q0 q0 / np.linalg.norm(q0)q1 q1 / np.linalg.norm(q1)dot np.dot(q0, q1)# 如果点积为负取q1的反方向以确保最短路径if dot 0.0:q1 -q1dot -dot# 如果点积接近1使用线性插值避免除零if dot 0.9995:result q0 t * (q1 - q0)return result / np.linalg.norm(result)theta_0 np.arccos(np.clip(dot, -1.0, 1.0))theta theta_0 * ts0 np.sin(theta_0 - theta) / np.sin(theta_0)s1 np.sin(theta) / np.sin(theta_0)return s0 * q0 s1 * q1def quaternion_to_rotation_matrix(q: np.ndarray) - np.ndarray:将单位四元数转换为3×3旋转矩阵qw, qx, qy, qz qR np.array([[1 - 2*(qy**2 qz**2), 2*(qx*qy - qw*qz), 2*(qx*qz qw*qy)],[2*(qx*qy qw*qz), 1 - 2*(qx**2 qz**2), 2*(qy*qz - qw*qx)],[2*(qx*qz - qw*qy), 2*(qy*qz qw*qx), 1 - 2*(qx**2 qy**2)]])return Rrobot/inverse_kinematics.py — 逆运动学求解器3-DOF垂直关节臂逆运动学求解器几何法解析求解θ₁atan2(y,x), θ₂和θ₃由余弦定理求得支持最近解选择策略避免关节跳变不可达点直接返回失败标志不静默填充import numpy as npfrom typing import List, Tuple, Optionalfrom robot.models import JointLimitSetclass InverseKinematicsSolver:3-DOF垂直关节臂逆运动学构型基座偏航θ₁ 大臂俯仰θ₂ 小臂俯仰θ₃焊枪垂直向下TCP在腕部法兰正下方 tcp_offset 处def __init__(self, L1: float, L2: float, tcp_offset: float,joint_limits: JointLimitSet):self.L1 L1self.L2 L2self.tcp_offset tcp_offsetself.limits joint_limitsdef solve(self, tcp_x: float, tcp_y: float, tcp_z: float,prev_angles: Optional[List[float]] None) - Tuple[np.ndarray, bool]:逆运动学求解输入TCP在基座坐标系中的位置 (m)输出(关节角度数组 [θ₁, θ₂, θ₃], 是否成功)不可达时返回 (零数组, False)由调用方决定是否中断# θ₁基座偏航theta1 np.arctan2(tcp_y, tcp_x)# 腕部位置焊枪垂直向下腕部在TCP正上方tcp_offset处wrist_x tcp_xwrist_y tcp_ywrist_z tcp_z self.tcp_offset# 腕部到基座的水平距离和高度r np.sqrt(wrist_x**2 wrist_y**2)h wrist_z# 腕部到基座的直线距离d np.sqrt(r**2 h**2)# 可达性检查外不可达和内奇异if d self.L1 self.L2:return np.array([0.0, 0.0, 0.0]), Falseif d abs(self.L1 - self.L2):return np.array([0.0, 0.0, 0.0]), Falsecos_theta3 (d**2 - self.L1**2 - self.L2**2) / (2 * self.L1 * self.L2)cos_theta3 np.clip(cos_theta3, -1.0, 1.0)# 两个解分支肘部向上(负角)和肘部向下(正角)theta3_option1 -np.arccos(cos_theta3)theta3_option2 np.arccos(cos_theta3)solutions []for theta3 in [theta3_option1, theta3_option2]:alpha np.arctan2(h, r)beta np.arctan2(self.L2 * np.sin(theta3),self.L1 self.L2 * np.cos(theta3))theta2 alpha - betaangles np.array([theta1, theta2, theta3])solutions.append(angles)# 检查限位并选择最优解valid_solutions []for sol in solutions:is_valid, _ self.limits.check_all_positions(sol.tolist())if is_valid:valid_solutions.append(sol)if len(valid_solutions) 0:return np.array([0.0, 0.0, 0.0]), Falseif prev_angles is None:return valid_solutions[0], True# 最近解策略prev np.array(prev_angles)best_sol valid_solutions[0]best_diff np.sum(np.abs(valid_solutions[0] - prev))for sol in valid_solutions[1:]:diff np.sum(np.abs(sol - prev))if diff best_diff:best_diff diffbest_sol solreturn best_sol, Truerobot/trajectory_generator.py — 笛卡尔空间直线轨迹生成笛卡尔空间直线轨迹规划位置线性插值 逆运动学逐点求解逆解失败时直接中断并报告不可达点import numpy as npfrom typing import List, Dictfrom robot.models import TCPPose, JointLimitSetfrom robot.inverse_kinematics import InverseKinematicsSolverclass CartesianLinearTrajectoryGenerator:笛卡尔空间直线轨迹生成器TCP从起点到终点沿直线运动姿态恒定垂直向下每个采样点通过逆运动学求解关节角度逆解失败直接中断不静默填充def __init__(self, ik_solver: InverseKinematicsSolver,joint_limits: JointLimitSet):self.ik ik_solverself.limits joint_limitsdef generate(self, start_pose: TCPPose, end_pose: TCPPose,total_time: float, n_samples: int) - Dict:if total_time 0:raise ValueError(运动时间必须大于0)if n_samples 2:raise ValueError(采样数至少为2)t_array np.linspace(0, total_time, n_samples)dt total_time / (n_samples - 1)tcp_positions np.zeros((n_samples, 3))joint_angles np.zeros((n_samples, 3))joint_velocities np.zeros((n_samples, 3))joint_accelerations np.zeros((n_samples, 3))ik_success_flags np.zeros(n_samples, dtypebool)unreachable_indices [] # 记录不可达的采样点索引prev_angles Nonestart_pos start_pose.position()for i, t in enumerate(t_array):tau t / total_time# 位置线性插值tcp_positions[i, :] start_pos (end_pose.position() - start_pos) * tau# 逆运动学求解——失败直接中断angles, success self.ik.solve(tcp_positions[i, 0], tcp_positions[i, 1],tcp_positions[i, 2], prev_angles)if not success:unreachable_indices.append(i)# 不可达点不填充假数据标记失败并停止后续计算breakjoint_angles[i, :] anglesik_success_flags[i] Trueprev_angles angles.tolist()# 如果有不可达点截断有效数据到第一个不可达点之前if len(unreachable_indices) 0:first_fail unreachable_indices[0]n_valid first_failelse:n_valid n_samples# 有限差分计算速度和加速度仅对有效数据if n_valid 2:for j in range(3):# 中心差分内部点if n_valid 2:joint_velocities[1:n_valid-1, j] (joint_angles[2:n_valid, j] - joint_angles[0:n_valid-2, j]) / (2 * dt)# 端点前向/后向差分joint_velocities[0, j] (joint_angles[1, j] - joint_angles[0, j]) / dtif n_valid 1:joint_velocities[n_valid-1, j] (joint_angles[n_valid-1, j] - joint_angles[n_valid-2, j]) / dtfor j in range(3):if n_valid 2:joint_accelerations[1:n_valid-1, j] (joint_velocities[2:n_valid, j] - joint_velocities[0:n_valid-2, j]) / (2 * dt)joint_accelerations[0, j] (joint_velocities[1, j] - joint_velocities[0, j]) / dtif n_valid 1:joint_accelerations[n_valid-1, j] (joint_velocities[n_valid-1, j] - joint_velocities[n_valid-2, j]) / dt# 全程限位校验仅对有效数据velocity_violations []acceleration_violations []for i in range(n_valid):vel_list joint_velocities[i, :].tolist()acc_list joint_accelerations[i, :].tolist()_, v_viol self.limits.check_all_velocities(vel_list)_, a_viol self.limits.check_all_accelerations(acc_list)for j in v_viol:velocity_violations.append((i, j, vel_list[j],self.limits.limits[j].max_velocity))for j in a_viol:acceleration_violations.append((i, j, acc_list[j],self.limits.limits[j].max_acceleration))return {time: t_array[:n_valid],tcp_positions: tcp_positions[:n_valid],joint_angles: joint_angles[:n_valid],joint_velocities: joint_velocities[:n_valid],joint_accelerations: joint_accelerations[:n_valid],ik_success_flags: ik_success_flags[:n_valid],unreachable_indices: unreachable_indices,velocity_violations: velocity_violations,acceleration_violations: acceleration_violations,n_valid_samples: n_valid,n_total_samples: n_samples}robot/visualize_trajectory.py — 可视化模块焊接作业笛卡尔轨迹规划可视化4张核心图表TCP三维直线轨迹、关节角度曲线、关节角速度曲线、关节角加速度曲线import numpy as npimport matplotlib.pyplot as pltplt.rcParams[font.sans-serif] [SimHei, DejaVu Sans]plt.rcParams[axes.unicode_minus] Falseclass TrajectoryVisualizer:def __init__(self, results_dirresults):self.results_dir results_dirimport osos.makedirs(results_dir, exist_okTrue)def plot_tcp_3d_trajectory(self, tcp_positions, start_pos, end_pos):① TCP三维直线轨迹验证笛卡尔空间直线运动fig plt.figure(figsize(10, 8))ax fig.add_subplot(111, projection3d)ax.plot(tcp_positions[:, 0], tcp_positions[:, 1], tcp_positions[:, 2],b-, linewidth1.5, labelTCP Trajectory)ax.scatter([start_pos[0]], [start_pos[1]], [start_pos[2]],cgreen, markero, s80, labelStart, zorder5)ax.scatter([end_pos[0]], [end_pos[1]], [end_pos[2]],cred, markers, s80, labelEnd, zorder5)ax.set_xlabel(X (m))ax.set_ylabel(Y (m))ax.set_zlabel(Z (m))ax.set_title(TCP 3D Linear Trajectory in Cartesian Space)ax.legend(fontsize8)ax.grid(True, alpha0.3)plt.tight_layout()path f{self.results_dir}/01_tcp_3d_linear_trajectory.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_joint_angles(self, timestamps, joint_angles):② 关节角度曲线验证逆运动学求解的平滑性fig, ax plt.subplots(figsize(14, 5))joint_names [θ₁ (Base Yaw), θ₂ (Elbow Pitch), θ₃ (Wrist Pitch)]colors [steelblue, orange, green]for j in range(3):ax.plot(timestamps, joint_angles[:, j], -,colorcolors[j], linewidth1.2, labeljoint_names[j])ax.set_xlabel(Time (s))ax.set_ylabel(Joint Angle (rad))ax.set_title(Joint Angle Profiles from Cartesian Linear Trajectory)ax.legend(locupper right, fontsize8)ax.grid(True, alpha0.3)plt.tight_layout()path f{self.results_dir}/02_joint_angle_curves.pngplt.savefig(path, dpi150)plt.close()return pathdef plot_joint_velocities(self, timestamps, joint_velocities, joint_limits):③ 关节角速度曲线限位线验证速度不超限fig, axes plt利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛
