import numpy as np
import open3d as o3d
from scipy.spatial.transform import Rotation as R
from enum import Enum
import trimesh # 新增：碰撞检测依赖
# -------------------------- 1. 新增：工业场景适配枚举 --------------------------
class WorkpieceMaterial(Enum):
"""工件材质（决定焊接参数）"""
CARBON_STEEL = "碳钢"
STAINLESS_STEEL = "不锈钢"
ALUMINUM = "铝合金"
class RobotBrand(Enum):
"""机器人品牌（适配不同协议）"""
FANUC = "发那科"
ABB = "ABB"
KUKA = "库卡"
# -------------------------- 2. 修正：3D视觉感知（增加抗干扰处理） --------------------------
class PointCloudProcessor:
def __init__(self, pcd_file_path: str, workpiece_material: WorkpieceMaterial):
self.pcd = self.load_point_cloud(pcd_file_path)
self.filtered_pcd = None
self.weld_seam_points = None
self.material = workpiece_material # 新增：材质适配去噪参数
# （保留加载点云方法）
def load_point_cloud(self, file_path: str) -> o3d.geometry.PointCloud:
pcd = o3d.io.read_point_cloud(file_path)
if not pcd.has_points():
raise ValueError("点云文件加载失败，无有效点数据")
return pcd
# 修正：根据材质调整去噪参数，增加形态学滤波
def preprocess_point_cloud(self, voxel_size: float = 0.002) -> None:
# 1. 体素下采样
downsampled = self.pcd.voxel_down_sample(voxel_size=voxel_size)
# 2. 材质适配统计滤波（铝合金反射强，需宽松阈值；碳钢锈蚀多，需严格阈值）
if self.material == WorkpieceMaterial.ALUMINUM:
self.filtered_pcd, _ = downsampled.remove_statistical_outlier(nb_neighbors=30, std_ratio=2.5)
elif self.material == WorkpieceMaterial.CARBON_STEEL:
self.filtered_pcd, _ = downsampled.remove_statistical_outlier(nb_neighbors=15, std_ratio=1.5)
else:
self.filtered_pcd, _ = downsampled.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
# 3. 新增：形态学滤波（去除表面小噪声点）
self.filtered_pcd = self._morphological_filter(self.filtered_pcd)
# 4. 平面分割（分离工件与背景）
plane_model, inliers = self.filtered_pcd.segment_plane(
distance_threshold=0.005, ransac_n=3, num_iterations=1000
)
self.filtered_pcd = self.filtered_pcd.select_by_index(inliers, invert=True)
@staticmethod
def _morphological_filter(pcd: o3d.geometry.PointCloud) -> o3d.geometry.PointCloud:
"""形态学滤波：去除孤立小簇点"""
points = np.asarray(pcd.points)
from sklearn.cluster import DBSCAN
clustering = DBSCAN(eps=0.008, min_samples=8).fit(points)
# 保留点数最多的簇（工件主体）
main_cluster_idx = np.argmax(np.bincount(clustering.labels_[clustering.labels_ != -1]))
main_cluster_points = points[clustering.labels_ == main_cluster_idx]
pcd_out = o3d.geometry.PointCloud()
pcd_out.points = o3d.utility.Vector3dVector(main_cluster_points)
return pcd_out
# （保留焊缝提取方法，无核心修正）
def extract_weld_seam(self, threshold: float = 0.02) -> None:
if self.filtered_pcd is None:
raise RuntimeError("请先执行点云预处理")
mesh, _ = o3d.geometry.TriangleMesh.create_from_point_cloud_alpha_shape(
self.filtered_pcd, alpha=0.03
)
vertices = np.asarray(mesh.vertices)
curvatures = self.calculate_curvature(mesh)
seam_candidates = vertices[curvatures > threshold]
from sklearn.cluster import DBSCAN
clustering = DBSCAN(eps=0.01, min_samples=5).fit(seam_candidates)
main_seam_idx = np.argmax(np.bincount(clustering.labels_[clustering.labels_ != -1]))
self.weld_seam_points = seam_candidates[clustering.labels_ == main_seam_idx]
start_point = self.weld_seam_points[0]
distances = np.linalg.norm(self.weld_seam_points - start_point, axis=1)
self.weld_seam_points = self.weld_seam_points[np.argsort(distances)]
@staticmethod
def calculate_curvature(mesh: o3d.geometry.TriangleMesh) -> np.ndarray:
normals = np.asarray(mesh.vertex_normals)
vertices = np.asarray(mesh.vertices)
curvatures = np.zeros(len(vertices))
for i in range(len(vertices)):
adj_vertices = []
for triangle in mesh.triangles:
if i in triangle:
adj_vertices.extend([v for v in triangle if v != i])
adj_vertices = list(set(adj_vertices))
if len(adj_vertices) < 3:
curvatures[i] = 0
continue
adj_normals = normals[adj_vertices]
curvatures[i] = np.var(adj_normals)
return curvatures
# -------------------------- 3. 修正：路径规划（增加碰撞检测） --------------------------
class WeldPathPlanner:
def __init__(self, seam_points: np.ndarray, obstacle_meshes: list = None):
self.seam_points = seam_points
self.planned_path = None
self.tool_orientation = None
self.obstacle_meshes = obstacle_meshes # 新增：障碍物三维模型（夹具、周边设备）
self.tool_mesh = self._create_tool_mesh() # 新增：焊枪模型（用于碰撞检测）
def _create_tool_mesh(self) -> trimesh.Trimesh:
"""创建焊枪简化模型（圆柱+球体，模拟实际尺寸）"""
gun_body = trimesh.creation.cylinder(
radius=0.015, height=0.3, transform=trimesh.transformations.translation_matrix([0, 0, -0.15])
)
gun_tip = trimesh.creation.icosahedron(radius=0.005, subdivisions=1)
gun_tip.apply_translation([0, 0, 0.15])
return trimesh.util.concatenate([gun_body, gun_tip])
# （保留路径平滑方法，无核心修正）
def generate_smooth_path(self) -> None:
from scipy.interpolate import make_interp_spline
t = np.linspace(0, 1, len(self.seam_points))
spline_x = make_interp_spline(t, self.seam_points[:, 0], k=3)
spline_y = make_interp_spline(t, self.seam_points[:, 1], k=3)
spline_z = make_interp_spline(t, self.seam_points[:, 2], k=3)
t_smooth = np.linspace(0, 1, int(len(self.seam_points) * 5))
x_smooth = spline_x(t_smooth)
y_smooth = spline_y(t_smooth)
z_smooth = spline_z(t_smooth)
self.planned_path = np.column_stack((x_smooth, y_smooth, z_smooth))
self._downsample_path(0.005)
def _downsample_path(self, resolution: float) -> None:
if len(self.planned_path) < 2:
return
filtered_path = [self.planned_path[0]]
for point in self.planned_path[1:]:
if np.linalg.norm(point - filtered_path[-1]) >= resolution:
filtered_path.append(point)
self.planned_path = np.array(filtered_path)
# （保留姿态计算方法，无核心修正）
def calculate_tool_orientation(self) -> None:
self.tool_orientation = []
for i in range(len(self.planned_path)):
if i == 0:
tangent = self.planned_path[1] - self.planned_path[0]
elif i == len(self.planned_path) - 1:
tangent = self.planned_path[-1] - self.planned_path[-2]
else:
tangent = self.planned_path[i+1] - self.planned_path[i-1]
tangent = tangent / np.linalg.norm(tangent)
up_vector = np.array([0, 1, 0])
x_axis = np.cross(up_vector, tangent)
x_axis = x_axis / np.linalg.norm(x_axis)
y_axis = np.cross(tangent, x_axis)
y_axis = y_axis / np.linalg.norm(y_axis)
rot_matrix = np.column_stack((x_axis, y_axis, tangent))
quat = R.from_matrix(rot_matrix).as_quat()
self.tool_orientation.append(quat)
self.tool_orientation = np.array(self.tool_orientation)
# 新增：碰撞检测与路径修正
def check_collision(self, safety_margin: float = 0.02) -> bool:
"""检查路径是否与障碍物碰撞，返回是否安全"""
if self.obstacle_meshes is None or len(self.obstacle_meshes) == 0:
print("警告：未传入障碍物模型，跳过碰撞检测（实际应用中严禁省略）")
return True
if self.planned_path is None or self.tool_orientation is None:
raise RuntimeError("请先生成路径和姿态")
for point, quat in zip(self.planned_path, self.tool_orientation):
# 计算焊枪在当前路径点的姿态矩阵
rot = R.from_quat(quat).as_matrix()
transform = np.eye(4)
transform[:3, :3] = rot
transform[:3, 3] = point
# 应用变换到焊枪模型
tool_transformed = self.tool_mesh.copy()
tool_transformed.apply_transform(transform)
# 检查与所有障碍物的碰撞
for obstacle in self.obstacle_meshes:
# 扩大障碍物边界（加入安全余量）
obstacle_expanded = obstacle.copy()
obstacle_expanded.apply_scale(1 + safety_margin / np.mean(obstacle.extents))
if trimesh.collision.collision_pair(tool_transformed, obstacle_expanded)[0]:
return False # 碰撞发生
return True
def adjust_path_for_collision(self, step: float = 0.005) -> None:
"""碰撞路径修正：沿法向量偏移路径点（简单避障逻辑）"""
if self.check_collision():
return
print("检测到碰撞，开始修正路径...")
# 计算焊缝的法向量（用于偏移方向）
seam_normals = self._calculate_seam_normals()
for i in range(len(self.planned_path)):
# 沿法向量偏移路径点
self.planned_path[i] += seam_normals[i] * step
# 重新检查碰撞，直到安全或达到最大偏移次数
max_attempts = 10
attempts = 0
while not self.check_collision() and attempts < max_attempts:
self.planned_path[i] += seam_normals[i] * step
attempts += 1
if attempts >= max_attempts:
raise RuntimeError(f"路径点 {i} 无法避障，建议调整夹具位置")
def _calculate_seam_normals(self) -> np.ndarray:
"""计算焊缝的法向量（基于相邻路径点）"""
normals = np.zeros_like(self.planned_path)
for i in range(len(self.planned_path)):
if i == 0:
vec1 = self.planned_path[1] - self.planned_path[0]
vec2 = self.planned_path[2] - self.planned_path[0]
elif i == len(self.planned_path) - 1:
vec1 = self.planned_path[-1] - self.planned_path[-2]
vec2 = self.planned_path[-1] - self.planned_path[-3]
else:
vec1 = self.planned_path[i] - self.planned_path[i-1]
vec2 = self.planned_path[i+1] - self.planned_path[i]
normal = np.cross(vec1, vec2)
normal = normal / np.linalg.norm(normal)
normals[i] = normal
return normals
# -------------------------- 4. 修正：程序生成（适配多品牌+材质参数匹配） --------------------------
class WeldProgramGenerator:
def __init__(
self,
path: np.ndarray,
orientation: np.ndarray,
robot_brand: RobotBrand,
workpiece_material: WorkpieceMaterial,
plate_thickness: float = 5.0 # 新增：板厚（mm），决定焊接参数
):
self.path = path
self.orientation = orientation
self.robot_brand = robot_brand
self.material = workpiece_material
self.plate_thickness = plate_thickness
# 新增：根据材质和板厚匹配焊接参数（工业级默认值，可校准）
self.weld_params = self._match_weld_params()
self.program = []
def _match_weld_params(self) -> dict:
"""根据材质和板厚匹配焊接参数（MIG工艺为例）"""
params = {}
if self.material == WorkpieceMaterial.CARBON_STEEL:
if self.plate_thickness <= 3:
params = {"current": 120-150, "voltage": 18-20, "speed": 0.05-0.08}
elif 3 < self.plate_thickness <= 8:
params = {"current": 150-190, "voltage": 20-24, "speed": 0.04-0.06}
else:
params = {"current": 190-230, "voltage": 24-28, "speed": 0.03-0.05}
elif self.material == WorkpieceMaterial.STAINLESS_STEEL:
if self.plate_thickness <= 3:
params = {"current": 100-130, "voltage": 17-19, "speed": 0.04-0.07}
elif 3 < self.plate_thickness <= 8:
params = {"current": 130-170, "voltage": 19-22, "speed": 0.03-0.05}
else:
params = {"current": 170-210, "voltage": 22-26, "speed": 0.02-0.04}
elif self.material == WorkpieceMaterial.ALUMINUM:
if self.plate_thickness <= 3:
params = {"current": 140-180, "voltage": 20-22, "speed": 0.06-0.09}
elif 3 < self.plate_thickness <= 8:
params = {"current": 180-220, "voltage": 22-25, "speed": 0.05-0.07}
else:
params = {"current": 220-260, "voltage": 25-29, "speed": 0.04-0.06}
return params
# 修正：支持多品牌机器人程序生成
def generate_program(self, program_name: str = "WELD_PROG01") -> str:
if self.robot_brand == RobotBrand.FANUC:
return self._generate_fanuc_program(program_name)
elif self.robot_brand == RobotBrand.ABB:
return self._generate_abb_program(program_name)
elif self.robot_brand == RobotBrand.KUKA:
return self._generate_kuka_program(program_name)
else:
raise ValueError(f"不支持的机器人品牌：{self.robot_brand.value}")
def _generate_fanuc_program(self, program_name: str) -> str:
self.program.append(f"/PROG {program_name}")
self.program.append("/ATTR")
self.program.append("OWNER = MNEDITOR;")
self.program.append(f"COMMENT = \"{self.material.value} {self.plate_thickness}mm 自动焊接程序\";")
self.program.append("PROTECT = READ_WRITE;")
self.program.append("/MN")
# 焊接参数初始化（适配材质和板厚）
self.program.append(f" CALL WELD_INIT({self.weld_params['current']}, {self.weld_params['voltage']});")
self.program.append(f" SPEED {self.weld_params['speed']};")
self.program.append(" ACC 1.0;")
# 运动指令
for i, (point, quat) in enumerate(zip(self.path, self.orientation)):
x, y, z = point
qx, qy, qz, qw = quat
if i == 0:
self.program.append(f" JOG P[{i+1}] = ({x:.3f}, {y:.3f}, {z+0.05:.3f}, {qx:.6f}, {qy:.6f}, {qz:.6f}, {qw:.6f});")
self.program.append(f" LIN P[{i+2}] = ({x:.3f}, {y:.3f}, {z:.3f}, {qx:.6f}, {qy:.6f}, {qz:.6f}, {qw:.6f}) SPEED 0.1;")
self.program.append(" CALL WELD_START();")
else:
self.program.append(f" LIN P[{i+2}] = ({x:.3f}, {y:.3f}, {z:.3f}, {qx:.6f}, {qy:.6f}, {qz:.6f}, {qw:.6f});")
# 程序尾
self.program.append(" CALL WELD_STOP();")
self.program.append(f" LIN P[{len(self.path)+2}] = ({self.path[-1,0]:.3f}, {self.path[-1,1]:.3f}, {self.path[-1,2]+0.05:.3f}, {self.orientation[-1,0]:.6f}, {self.orientation[-1,1]:.6f}, {self.orientation[-1,2]:.6f}, {self.orientation[-1,3]:.6f}) SPEED 0.2;")
self.program.append(" JOG P[0] = (0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0);")
self.program.append("/END")
program_str = "\n".join(self.program)
with open(f"{program_name}_FANUC.tp", "w", encoding="utf-8") as f:
f.write(program_str)
return program_str
def _generate_abb_program(self, program_name: str) -> str:
"""生成 ABB 机器人 RAPID 程序"""
self.program.append(f"MODULE {program_name}")
self.program.append(f"! {self.material.value} {self.plate_thickness}mm 自动焊接程序")
self.program.append("PROC main()")
# 焊接参数初始化
self.program.append(f" WELD_INIT {self.weld_params['current']}, {self.weld_params['voltage']};")
self.program.append(f" speeddata v1:={self.weld_params['speed']}, 1.0, 1.0, 1.0;")
self.program.append(" accdata a1:=1.0, 1.0, 1.0, 1.0;")
# 运动指令
for i, (point, quat) in enumerate(zip(self.path, self.orientation)):
x, y, z = point
qx, qy, qz, qw = quat
# ABB 姿态用 Euler 角（ZYX 顺序）
euler = R.from_quat(quat).as_euler('zyx', degrees=True)
rx, ry, rz = euler
if i == 0:
self.program.append(f" MoveJ [[{x:.3f},{y:.3f},{z+0.05:.3f}], [{rx:.2f},{ry:.2f},{rz:.2f}]], v1, a1, tool0;")
self.program.append(f" MoveL [[{x:.3f},{y:.3f},{z:.3f}], [{rx:.2f},{ry:.2f},{rz:.2f}]], v1, a1, tool0;")
self.program.append(" WELD_START;")
else:
self.program.append(f" MoveL [[{x:.3f},{y:.3f},{z:.3f}], [{rx:.2f},{ry:.2f},{rz:.2f}]], v1, a1, tool0;")
# 程序尾
self.program.append(" WELD_STOP;")
self.program.append(f" MoveL [[{self.path[-1,0]:.3f},{self.path[-1,1]:.3f},{self.path[-1,2]+0.05:.3f}], [{rx:.2f},{ry:.2f},{rz:.2f}]], v1, a1, tool0;")
self.program.append(" MoveJ [[0.0,0.0,0.5], [0.0,0.0,0.0]], v1, a1, tool0;")
self.program.append("ENDPROC")
self.program.append("ENDMODULE")
program_str = "\n".join(self.program)
with open(f"{program_name}_ABB.mod", "w", encoding="utf-8") as f:
f.write(program_str)
return program_str
def _generate_kuka_program(self, program_name: str) -> str:
"""生成 KUKA 机器人 KRL 程序"""
self.program.append(f"DEF {program_name}()")
self.program.append(f"! {self.material.value} {self.plate_thickness}mm 自动焊接程序")
# 焊接参数初始化
self.program.append(f" WELD_INIT({self.weld_params['current']}, {self.weld_params['voltage']});")
self.program.append(f" SPEED = {self.weld_params['speed']};")
self.program.append(" ACC = 1.0;")
# 运动指令
for i, (point, quat) in enumerate(zip(self.path, self.orientation)):
x, y, z = point
qx, qy, qz, qw = quat
# KUKA 姿态用四元数（w,x,y,z 顺序）
if i == 0:
self.program.append(f" PTP {{X {x:.3f}, Y {y:.3f}, Z {z+0.05:.3f}, A {qx:.6f}, B {qy:.6f}, C {qz:.6f}, S {qw:.6f}}} ;")
self.program.append(f" LIN {{X {x:.3f}, Y {y:.3f}, Z {z:.3f}, A {qx:.6f}, B {qy:.6f}, C {qz:.6f}, S {qw:.6f}}} SPEED {self.weld_params['speed']};")
self.program.append(" WELD_START();")
else:
self.program.append(f" LIN {{X {x:.3f}, Y {y:.3f}, Z {z:.3f}, A {qx:.6f}, B {qy:.6f}, C {qz:.6f}, S {qw:.6f}}} SPEED {self.weld_params['speed']};")
# 程序尾
self.program.append(" WELD_STOP();")
self.program.append(f" LIN {{X {self.path[-1,0]:.3f}, Y {self.path[-1,1]:.3f}, Z {self.path[-1,2]+0.05:.3f}, A {qx:.6f}, B {qy:.6f}, C {qz:.6f}, S {qw:.6f}}} SPEED 0.2;")
self.program.append(" PTP {{X 0.0, Y 0.0, Z 0.5, A 0.0, B 0.0, C 0.0, S 1.0}} ;")
self.program.append("END")
program_str = "\n".join(self.program)
with open(f"{program_name}_KUKA.src", "w", encoding="utf-8") as f:
f.write(program_str)
return program_str
# -------------------------- 5. 主函数：整合工业级流程 --------------------------
def main():
# 1. 加载障碍物模型（实际应用中需导入夹具、设备的 STL 文件）
obstacle_meshes = []
try:
fixture_mesh = trimesh.load("fixture.stl") # 夹具模型
obstacle_meshes.append(fixture_mesh)
print("成功加载障碍物模型")
except:
print("警告：未找到障碍物模型文件，碰撞检测功能受限")
# 2. 初始化参数（工业场景实际配置）
workpiece_material = WorkpieceMaterial.CARBON_STEEL # 工件材质
plate_thickness = 6.0 # 板厚（mm）
robot_brand = RobotBrand.FANUC # 机器人品牌
pcd_file_path = "weld_workpiece.pcd" # 工件点云文件
# 3. 3D 视觉感知（抗干扰处理）
pcd_processor = PointCloudProcessor(pcd_file_path, workpiece_material)
pcd_processor.preprocess_point_cloud()
pcd_processor.extract_weld_seam()
if pcd_processor.weld_seam_points is None or len(pcd_processor.weld_seam_points) < 10:
raise RuntimeError("焊缝提取失败，点云质量或参数设置不合理")
# 4. 路径规划（碰撞检测+修正）
path_planner = WeldPathPlanner(pcd_processor.weld_seam_points, obstacle_meshes)
path_planner.generate_smooth_path()
path_planner.calculate_tool_orientation()
if not path_planner.check_collision():
path_planner.adjust_path_for_collision()
if not path_planner.check_collision():
raise RuntimeError("路径避障失败，请优化夹具布局或焊接顺序")
# 5. 生成机器人程序（适配品牌+材质参数）
program_generator = WeldProgramGenerator(
path=path_planner.planned_path,
orientation=path_planner.tool_orientation,
robot_brand=robot_brand,
workpiece_material=workpiece_material,
plate_thickness=plate_thickness
)
program_generator.generate_program("WELD_CARBON_STEEL_6MM")
print("工业级焊接程序生成完成")
if __name__ == "__main__":
main()帮我检查这段代码
最新发布
本文介绍了R语言中如何利用segment.curvature参数分析曲线曲率，曲率值接近0时，曲线角度增大。通过示例代码展示了使用curvature包计算曲率并观察其对角度影响的过程。
订阅专栏 解锁全文
扫一扫
473
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
