单物体最优抓取轨迹生成

基于 3D 位姿规划直线平滑抓取轨迹包含趋近 - 抓取 - 复位三段最优运动路径适配机械臂点位运动核心规划逻辑基准位机械臂初始安全待机点趋近段直线匀速靠近物体上方预备抓取点抓取段垂直下落至物体抓取中心位姿抬升段抓取后回升至安全高度复位段返回初始待机位置完整可运行代码python运行from ultralytics import YOLO import cv2 import numpy as np import math # 模型与相机参数 model YOLO(yolo26n.pt) FOCAL_LENGTH 600 REAL_WIDTH 0.5 # 机械臂基准点位(相机坐标系单位m) HOME_X, HOME_Y, HOME_Z 0.0, -0.2, 0.8 # 初始待机位 PRE_Z_OFFSET 0.15 # 抓取预备高度落差 LIFT_HEIGHT 0.2 # 抓取抬升高度 # 摄像头初始化 cap cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) cv2.namedWindow(最优抓取轨迹规划, cv2.WINDOW_NORMAL) cv2.resizeWindow(最优抓取轨迹规划, 1280, 720) # 轨迹点位存储 trajectory_points [] def gen_optimal_grasp_traj(obj_x, obj_y, obj_z, grasp_angle): 生成最优抓取轨迹点位列表 traj [] # 1. 初始待机点 traj.append([HOME_X, HOME_Y, HOME_Z, grasp_angle]) # 2. 物体上方预备抓取点 pre_x, pre_y, pre_z obj_x, obj_y, obj_z PRE_Z_OFFSET traj.append([pre_x, pre_y, pre_z, grasp_angle]) # 3. 物体中心抓取点 traj.append([obj_x, obj_y, obj_z, grasp_angle]) # 4. 抓取后抬升安全点 lift_z obj_z LIFT_HEIGHT traj.append([obj_x, obj_y, lift_z, grasp_angle]) # 5. 返回初始位 traj.append([HOME_X, HOME_Y, HOME_Z, grasp_angle]) return traj while True: ret, frame cap.read() if not ret: break results model(frame, imgsz640, verboseFalse) img results[0].plot() trajectory_points.clear() for box in results[0].boxes: x1, y1, x2, y2 box.xyxy[0].cpu().numpy() cls_name model.names[int(box.cls[0])] box_w x2 - x1 box_h y2 - y1 # 计算3D坐标 obj_z (REAL_WIDTH * FOCAL_LENGTH) / box_w cx, cy (x1x2)/2, (y1y2)/2 obj_x (cx - frame.shape[1]//2) * obj_z / FOCAL_LENGTH obj_y (cy - frame.shape[0]//2) * obj_z / FOCAL_LENGTH # 计算抓取姿态角 grasp_angle 0.0 if box_w box_h else 90.0 # 生成最优抓取轨迹 grasp_traj gen_optimal_grasp_traj(obj_x, obj_y, obj_z, grasp_angle) trajectory_points grasp_traj # 绘制抓取中心点 cv2.circle(img, (int(cx), int(cy)), 8, (0,0,255), -1) # 显示坐标姿态 info fXYZ:{obj_x:.2f} {obj_y:.2f} {obj_z:.2f}m Angle:{grasp_angle}deg cv2.putText(img, info, (int(x1), int(y1)-15), cv2.FONT_HERSHEY_SIMPLEX, 1, (0,255,0), 2) # 打印轨迹点位可下发机械臂 if trajectory_points: print(最优抓取轨迹点位(X,Y,Z,角度)) for idx, pos in enumerate(trajectory_points): print(f点位{idx1}: {pos}) cv2.imshow(最优抓取轨迹规划, img) if cv2.waitKey(1) 0xFF ord(q): break cap.release() cv2.destroyAllWindows()运行指令bash运行python3 yolo3d.py轨迹特性最优设计避障安全先到物体上空再下落规避周边障碍物运动平顺直线插值路径速度波动小、抖动低动作闭环抓取完成自动复位可连续作业姿态适配根据物体长宽自动匹配夹持角度轨迹输出说明终端打印 5 组轨迹点位格式[X,Y,Z,抓取角度]可直接对接运动控制器下发执行进阶扩展方向增加轨迹插补生成密集中间点位实现顺滑运动加入碰撞检测修正危险轨迹路径对接 ROS/MoveIt直接仿真并执行轨迹多物体优先级排序依次生成抓取路径