机器人视觉：C#上位机 + YOLO 实现目标追踪与路径规划，从动态抓取到自主导航全流程

zhxup606

347人浏览 · 2026-03-03 08:13:05

zhxup606 · 2026-03-03 08:13:05 发布

聚焦工业机器人最常遇到的几个关键环节。这些代码基于 .NET 8/9 + YOLOv11 ONNX + ByteTrack + Hybrid A* + DWA 的典型工业组合，2026年主流做法。

1. 更完整的实时追踪 + 世界坐标转换（含手眼标定）

// 相机内参 + 外参（手眼标定结果，通常通过EasyHandEye或OpenCV标定获得）
private Matrix4x4 cameraToBaseTransform;  // 相机 → 机械臂基坐标变换矩阵
private Matrix3x3 cameraIntrinsic;        // 3x3 内参矩阵
private Vector2 distortionCoeffs;         // 畸变系数（简化版）

private Vector3 PixelToWorld(Vector2 pixel, float depth)
{
    // 像素 → 归一化坐标
    float x = (pixel.X - cameraIntrinsic.M11) / cameraIntrinsic.M00;
    float y = (pixel.Y - cameraIntrinsic.M22) / cameraIntrinsic.M33;

    // 畸变校正（简单径向畸变模型）
    float r2 = x * x + y * y;
    float radial = 1 + distortionCoeffs.X * r2 + distortionCoeffs.Y * r2 * r2;
    x *= radial;
    y *= radial;

    // 深度 → 3D点（相机坐标系）
    Vector3 pointCam = new Vector3(x * depth, y * depth, depth);

    // 相机坐标 → 机械臂基坐标
    Vector4 pointBase = cameraToBaseTransform * new Vector4(pointCam.X, pointCam.Y, pointCam.Z, 1);
    return new Vector3(pointBase.X, pointBase.Y, pointBase.Z);
}

// 使用示例：在追踪回调中转换
private void OnTrackUpdate(TrackedTarget target)
{
    // 假设深度由深度相机或固定距离估算
    float estimatedDepth = 1.2f;  // 实际用深度相机或模型预测
    target.CenterWorld = PixelToWorld(target.CenterPixel, estimatedDepth);

    // 后续用于路径规划或抓取位姿
}

2. Hybrid A* 全局规划（含运动学约束）

public class HybridAStarNode : IEquatable<HybridAStarNode>
{
    public Vector3 Position;     // x,y,theta (弧度)
    public float G;              // 实际代价
    public float H;              // 启发式
    public HybridAStarNode Parent;

    public float F => G + H;

    public bool Equals(HybridAStarNode other) =>
        Math.Abs(Position.X - other.Position.X) < 0.05f &&
        Math.Abs(Position.Y - other.Position.Y) < 0.05f &&
        Math.Abs(AngleDiff(Position.Z, other.Position.Z)) < 0.1f;

    private float AngleDiff(float a, float b) => (a - b + MathF.PI) % (2 * MathF.PI) - MathF.PI;
}

public List<Vector3> HybridAStarPlan(Vector3 start, Vector3 goal, OccupancyGrid grid)
{
    var open = new PriorityQueue<HybridAStarNode, float>();
    var closed = new HashSet<HybridAStarNode>();

    var startNode = new HybridAStarNode { Position = start, G = 0, H = Heuristic(start, goal) };
    open.Enqueue(startNode, startNode.F);

    while (open.Count > 0)
    {
        var current = open.Dequeue();
        if (closed.Contains(current)) continue;
        closed.Add(current);

        if (Distance(current.Position, goal) < 0.3f)
            return ReconstructHybridPath(current);

        // 扩展运动基元（前进、后退、转弯）
        foreach (var motion in GetMotionPrimitives(current.Position))
        {
            if (!IsCollisionFree(motion.End, grid)) continue;

            var neighbor = new HybridAStarNode
            {
                Position = motion.End,
                G = current.G + motion.Cost,
                H = Heuristic(motion.End, goal),
                Parent = current
            };

            if (!closed.Contains(neighbor))
                open.Enqueue(neighbor, neighbor.F);
        }
    }
    return null; // 无解
}

private List<MotionPrimitive> GetMotionPrimitives(Vector3 pose)
{
    // 示例：前进0.5m、转±15°、后退0.3m 等
    return new List<MotionPrimitive>
    {
        new MotionPrimitive(pose + new Vector3(0.5f * MathF.Cos(pose.Z), 0.5f * MathF.Sin(pose.Z), 0), 0.5f),
        // ... 更多基元
    };
}

3. DWA 局部避障（实时速度指令生成）

public class DWA
{
    public static VelocityCommand Compute(
        Pose2D currentPose,
        Vector2 localGoal,
        List<Vector2> obstacles,
        float vMax = 1.2f, float wMax = 2.0f,
        float vRes = 0.1f, float wRes = 0.2f,
        float dt = 0.1f, float predictTime = 3.0f)
    {
        VelocityCommand bestCmd = new();
        float bestScore = float.MinValue;

        for (float v = -vMax; v <= vMax; v += vRes)
        for (float w = -wMax; w <= wMax; w += wRes)
        {
            var traj = SimulateTrajectory(currentPose, v, w, dt, predictTime);

            float headingScore = HeadingScore(traj.End, localGoal);
            float distScore = DistToObstacleScore(traj, obstacles);
            float velScore = Math.Abs(v);  // 鼓励前进

            float score = 0.4f * headingScore + 0.5f * distScore + 0.1f * velScore;

            if (score > bestScore)
            {
                bestScore = score;
                bestCmd = new VelocityCommand { Linear = v, Angular = w };
            }
        }

        return bestCmd;
    }

    private static Trajectory SimulateTrajectory(Pose2D start, float v, float w, float dt, float time)
    {
        var traj = new Trajectory { Poses = new List<Pose2D> { start } };
        var current = start;

        for (float t = dt; t <= time; t += dt)
        {
            current.X += v * MathF.Cos(current.Theta) * dt;
            current.Y += v * MathF.Sin(current.Theta) * dt;
            current.Theta += w * dt;
            traj.Poses.Add(current);
        }
        return traj;
    }
}

4. 机械臂动态抓取位姿生成（含姿态估计）

private Pose6D GenerateGraspPose(TrackedTarget target)
{
    // 假设YOLO输出2D框 + 深度 → 3D中心点
    Vector3 centerWorld = PixelToWorld(target.CenterPixel, target.Depth);

    // 简单抓取姿态：Z轴向下，X轴朝向目标
    Quaternion orientation = Quaternion.LookRotation(Vector3.forward, Vector3.up);

    // 偏移抓取点（避免中心抓取导致碰撞）
    Vector3 graspOffset = new Vector3(0, 0, -0.05f); // 向下5cm
    Vector3 graspPos = centerWorld + orientation * graspOffset;

    return new Pose6D { Position = graspPos, Orientation = orientation };
}

// 下发给机械臂控制器（示例：OPC UA）
private void SendGraspCommand(Pose6D pose)
{
    opcSession.WriteNode("ns=2;s=TargetGraspPose", pose);
}

5. 实时地图更新与障碍物膨胀（激光雷达/深度相机）

private void UpdateOccupancyGrid(LaserScan scan)
{
    foreach (var range in scan.Ranges)
    {
        if (range < scan.RangeMin || range > scan.RangeMax) continue;

        float angle = scan.AngleMin + i * scan.AngleIncrement;
        float x = range * MathF.Cos(angle);
        float y = range * MathF.Sin(angle);

        // 坐标转换到地图坐标系
        var mapPos = WorldToMap(new Vector2(x, y));

        // 膨胀障碍物（安全半径0.5m）
        InflateObstacle(mapPos, radiusCells: 10); // 10格 ≈ 0.5m @ 5cm分辨率
    }
}

private void InflateObstacle(Point center, int radius)
{
    for (int dx = -radius; dx <= radius; dx++)
    for (int dy = -radius; dy <= radius; dy++)
    {
        if (dx * dx + dy * dy <= radius * radius)
        {
            int x = center.X + dx;
            int y = center.Y + dy;
            if (grid.IsValid(x, y))
                grid[x, y] = 100; // 完全占用
        }
    }
}