单物体最优抓取轨迹生成

基于 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, imgsz=640, verbose=False)
    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 = (x1+x2)/2, (y1+y2)/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 = f"XYZ:{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"点位{idx+1}: {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，直接仿真并执行轨迹
多物体优先级排序，依次生成抓取路径