每日算法 — 使用java实现抓娃娃：机械臂运动规划与碰撞检测算法

引言

抓娃娃机是游乐场和商场中备受欢迎的娱乐设备，玩家通过操控机械臂移动、下爪、抓取，试图将心仪的玩偶从玻璃箱中取出。这一过程看似简单，实则涉及精密的运动规划与碰撞检测：机械臂如何在三维空间中避开箱体和障碍物到达目标正上方？爪子下降时如何确保不与玩偶或箱体发生碰撞？本文将用Java实现一个简化版二维抓娃娃机模拟器，重点讲解关节型机械臂逆运动学求解与圆形碰撞检测两大核心算法。

核心算法原理

1. 关节型机械臂逆运动学（IK）

机械臂由多个连杆和旋转关节组成。逆运动学的目标是：给定末端执行器（爪子）的目标位置 (tx, ty)，求解各关节的旋转角度。

对于两关节机械臂（臂长分别为 L1 和 L2）：
– 设基座到目标的距离为 D = sqrt(tx² + ty²)
– 使用余弦定理求解第二关节角度 θ2：
cos(θ2) = (D² - L1² - L2²) / (2 * L1 * L2)
θ2 = arccos(cos(θ2))
– 再求解第一关节角度 θ1：
θ1 = atan2(ty, tx) - atan2(L2*sin(θ2), L1 + L2*cos(θ2))

2. 圆形碰撞检测

在二维投影中，爪子和玩偶均可近似为圆形。碰撞检测即判断两圆圆心距离是否小于半径之和：

碰撞 = sqrt((x1-x2)² + (y1-y2)²) < (r1 + r2)

若检测到碰撞，则禁止该方向的运动，或触发抓取逻辑。

Java完整实现

import java.util.ArrayList;
import java.util.List;

/**
 * 简化版抓娃娃机模拟器
 * 核心算法：关节型机械臂逆运动学 + 圆形碰撞检测
 */
public class ClawMachineSimulator {

    // 机械臂参数
    private static final double ARM_L1 = 80;   // 第一臂长
    private static final double ARM_L2 = 60;   // 第二臂长
    private static final double CLAW_SIZE = 8; // 爪子半径

    // 玻璃箱边界
    private static final double BOX_LEFT = 50;
    private static final double BOX_RIGHT = 250;
    private static final double BOX_TOP = 80;
    private static final double BOX_BOTTOM = 220;

    // 机械臂状态
    private double baseX = 150, baseY = 30; // 基座位置
    private double theta1 = Math.toRadians(90); // 第一关节角度
    private double theta2 = Math.toRadians(0);  // 第二关节角度
    private double clawOpen = 1.0; // 爪子开合度 (0=闭合, 1=张开)
    private boolean isGrabbing = false;

    // 关节坐标
    private double joint1X, joint1Y; // 第一关节
    private double joint2X, joint2Y; // 末端/爪子中心

    // 玩偶列表
    static class Doll {
        double x, y, radius;
        boolean isGrabbed;
        String name;

        Doll(double x, double y, double radius, String name) {
            this.x = x; this.y = y;
            this.radius = radius;
            this.name = name;
            this.isGrabbed = false;
        }
    }

    private final List<Doll> dolls;

    public ClawMachineSimulator() {
        this.dolls = new ArrayList<>();
        initDolls();
        updateForwardKinematics();
    }

    private void initDolls() {
        dolls.add(new Doll(100, 180, 12, "小熊"));
        dolls.add(new Doll(140, 190, 10, "兔子"));
        dolls.add(new Doll(180, 170, 14, "企鹅"));
        dolls.add(new Doll(120, 150, 9, "猫咪"));
        dolls.add(new Doll(200, 185, 11, "狗狗"));
    }

    /**
     * 核心算法1：正向运动学
     * 根据关节角度计算各关节坐标
     */
    public void updateForwardKinematics() {
        joint1X = baseX + ARM_L1 * Math.cos(theta1);
        joint1Y = baseY + ARM_L1 * Math.sin(theta1);

        joint2X = joint1X + ARM_L2 * Math.cos(theta1 + theta2);
        joint2Y = joint1Y + ARM_L2 * Math.sin(theta1 + theta2);
    }

    /**
     * 核心算法2：逆运动学求解
     * 给定目标位置，求解关节角度
     * @param targetX 目标x坐标
     * @param targetY 目标y坐标
     * @return 是否可达
     */
    public boolean solveInverseKinematics(double targetX, double targetY) {
        double dx = targetX - baseX;
        double dy = targetY - baseY;
        double D = Math.sqrt(dx * dx + dy * dy);

        // 检查可达性
        if (D > ARM_L1 + ARM_L2 || D < Math.abs(ARM_L1 - ARM_L2)) {
            return false; // 目标超出工作空间
        }

        // 余弦定理求解 theta2
        double cosTheta2 = (D * D - ARM_L1 * ARM_L1 - ARM_L2 * ARM_L2)
                         / (2 * ARM_L1 * ARM_L2);
        // 限制在 [-1, 1] 范围内避免浮点误差
        cosTheta2 = Math.max(-1.0, Math.min(1.0, cosTheta2));

        double theta2New = Math.acos(cosTheta2);
        // 选择肘部向下的解（更自然）
        // theta2New = -theta2New; // 可切换为另一种构型

        // 求解 theta1
        double alpha = Math.atan2(dy, dx);
        double beta = Math.atan2(ARM_L2 * Math.sin(theta2New),
                                 ARM_L1 + ARM_L2 * Math.cos(theta2New));
        double theta1New = alpha - beta;

        // 关节角度限制（避免机械结构冲突）
        if (theta1New < Math.toRadians(10) || theta1New > Math.toRadians(170)) {
            return false;
        }

        this.theta1 = theta1New;
        this.theta2 = theta2New;
        updateForwardKinematics();
        return true;
    }

    /**
     * 核心算法3：圆形碰撞检测
     * @return 碰撞的玩偶，无碰撞返回null
     */
    public Doll checkCollision() {
        for (Doll doll : dolls) {
            double dx = joint2X - doll.x;
            double dy = joint2Y - doll.y;
            double dist = Math.sqrt(dx * dx + dy * dy);
            if (dist < CLAW_SIZE + doll.radius) {
                return doll;
            }
        }
        return null;
    }

    /**
     * 检测爪子是否与箱体边界碰撞
     */
    public boolean checkBoxCollision() {
        return joint2X - CLAW_SIZE < BOX_LEFT ||
               joint2X + CLAW_SIZE > BOX_RIGHT ||
               joint2Y - CLAW_SIZE < BOX_TOP ||
               joint2Y + CLAW_SIZE > BOX_BOTTOM;
    }

    /**
     * 移动爪子到目标位置（带碰撞检测的安全移动）
     */
    public boolean moveClawTo(double targetX, double targetY) {
        // 目标限制在箱体内
        targetX = Math.max(BOX_LEFT + CLAW_SIZE,
                  Math.min(BOX_RIGHT - CLAW_SIZE, targetX));
        targetY = Math.max(BOX_TOP + CLAW_SIZE,
                  Math.min(BOX_BOTTOM - CLAW_SIZE, targetY));

        if (!solveInverseKinematics(targetX, targetY)) {
            return false; // 目标不可达
        }

        // 检测与箱体碰撞
        if (checkBoxCollision()) {
            return false;
        }

        return true;
    }

    /**
     * 执行抓取操作
     */
    public boolean grab() {
        clawOpen = 0.0; // 闭合爪子
        Doll hit = checkCollision();
        if (hit != null) {
            hit.isGrabbed = true;
            isGrabbing = true;
            return true;
        }
        return false;
    }

    /**
     * 执行释放操作
     */
    public void release() {
        clawOpen = 1.0; // 张开爪子
        for (Doll doll : dolls) {
            if (doll.isGrabbed) {
                doll.isGrabbed = false;
                // 释放后玩偶落回底部
                doll.y = Math.min(doll.y + 30, BOX_BOTTOM - doll.radius);
            }
        }
        isGrabbing = false;
    }

    /**
     * AI自动抓取策略
     */
    public void aiAutoGrab() {
        System.out.println("=== AI自动抓取 ===");
        for (Doll doll : dolls) {
            if (doll.isGrabbed) continue;

            System.out.printf("尝试抓取: %s (位置: %.0f, %.0f)%n", doll.name, doll.x, doll.y);

            // 步骤1：移动到玩偶正上方
            double approachX = doll.x;
            double approachY = doll.y - doll.radius - 5;
            if (!moveClawTo(approachX, approachY)) {
                System.out.println("  正上方不可达，尝试偏移...");
                // 尝试多个偏移位置
                boolean success = false;
                for (double offset = -20; offset <= 20; offset += 10) {
                    if (moveClawTo(approachX + offset, approachY)) {
                        success = true;
                        break;
                    }
                }
                if (!success) {
                    System.out.println("  无法到达该玩偶附近，跳过");
                    continue;
                }
            }

            // 步骤2：下降
            System.out.println("  下降中...");
            for (double y = approachY; y <= doll.y; y += 3) {
                if (!moveClawTo(joint2X, y)) {
                    System.out.println("  下降路径被阻挡");
                    break;
                }
                render();
                sleep(100);
            }

            // 步骤3：抓取
            if (grab()) {
                System.out.println("  ✓ 抓取成功！");
            } else {
                System.out.println("  ✗ 抓取失败，未命中");
                release();
                continue;
            }

            // 步骤4：提起并移出
            System.out.println("  提起并移出...");
            for (double y = joint2Y; y >= BOX_TOP - 20; y -= 5) {
                moveClawTo(joint2X, y);
                // 同步移动被抓取的玩偶
                for (Doll d : dolls) {
                    if (d.isGrabbed) {
                        d.x = joint2X;
                        d.y = joint2Y;
                    }
                }
                render();
                sleep(100);
            }

            // 步骤5：释放（成功取出）
            release();
            System.out.println("  ✓ " + doll.name + " 成功取出！");
            break; // 每次只抓一个
        }
    }

    private void sleep(long ms) {
        try { Thread.sleep(ms); } catch (InterruptedException e) {}
    }

    /**
     * 控制台可视化
     */
    public void render() {
        int W = 60, H = 24;
        char[][] canvas = new char[H][W];
        for (char[] row : canvas) java.util.Arrays.fill(row, ' ');

        // 绘制箱体
        int bl = (int)(BOX_LEFT / 5), br = (int)(BOX_RIGHT / 5);
        int bt = (int)(BOX_TOP / 10), bb = (int)(BOX_BOTTOM / 10);
        for (int x = bl; x <= br; x++) {
            canvas[bt][x] = '─'; canvas[bb][x] = '─';
        }
        for (int y = bt; y <= bb; y++) {
            canvas[y][bl] = '│'; canvas[y][br] = '│';
        }
        canvas[bt][bl] = '┌'; canvas[bt][br] = '┐';
        canvas[bb][bl] = '└'; canvas[bb][br] = '┘';

        // 绘制玩偶
        for (Doll doll : dolls) {
            int dx = (int)(doll.x / 5);
            int dy = (int)(doll.y / 10);
            if (dx >= 0 && dx < W && dy >= 0 && dy < H) {
                canvas[dy][dx] = doll.isGrabbed ? '*' : 'o';
            }
        }

        // 绘制机械臂
        int bx = (int)(baseX / 5), by = (int)(baseY / 10);
        int j1x = (int)(joint1X / 5), j1y = (int)(joint1Y / 10);
        int j2x = (int)(joint2X / 5), j2y = (int)(joint2Y / 10);

        drawLine(canvas, bx, by, j1x, j1y, '=');
        drawLine(canvas, j1x, j1y, j2x, j2y, '-');
        if (j2x >= 0 && j2x < W && j2y >= 0 && j2y < H) {
            canvas[j2y][j2x] = clawOpen > 0.5 ? 'C' : 'c'; // C=张开, c=闭合
        }

        System.out.print("\033[H\033[2J");
        for (char[] row : canvas) System.out.println(new String(row));
        System.out.printf("关节角度: θ1=%.1f° θ2=%.1f° | 爪子: %s%n",
                Math.toDegrees(theta1), Math.toDegrees(theta2),
                clawOpen > 0.5 ? "张开" : "闭合");
    }

    private void drawLine(char[][] canvas, int x1, int y1, int x2, int y2, char ch) {
        int steps = Math.max(Math.abs(x2-x1), Math.abs(y2-y1));
        if (steps == 0) return;
        for (int i = 0; i <= steps; i++) {
            int x = x1 + (x2-x1) * i / steps;
            int y = y1 + (y2-y1) * i / steps;
            if (x >= 0 && x < canvas[0].length && y >= 0 && y < canvas.length) {
                canvas[y][x] = ch;
            }
        }
    }

    public static void main(String[] args) {
        ClawMachineSimulator sim = new ClawMachineSimulator();
        System.out.println("=== 抓娃娃机模拟器 ===");
        System.out.println("o = 玩偶, * = 被抓取的玩偶, C/c = 爪子, =/- = 机械臂");
        sim.render();
        sim.sleep(1000);

        sim.aiAutoGrab();

        System.out.println("\n模拟结束！");
        System.out.println("成功抓取的玩偶：");
        for (Doll d : sim.dolls) {
            System.out.printf("  %s: %s%n", d.name, d.isGrabbed ? "已抓取（等待释放）" : "仍在箱中");
        }
    }
}

算法复杂度分析

总结

本文通过Java实现了一个简化版抓娃娃机模拟器，重点实现了两个核心算法：

1. 关节型机械臂逆运动学（IK）：利用余弦定理和三角函数关系，将二维目标位置反向求解为两个关节的旋转角度。通过可达性检查和工作空间限制，确保机械臂运动在安全范围内。

2. 圆形碰撞检测：在二维投影中将爪子和玩偶建模为圆形，通过圆心距与半径和的比较实现高效碰撞判定。结合箱体边界检测，构建了完整的安全运动约束系统。

这两个算法是工业机器人控制和游戏物理交互的基础。逆运动学的数学思想广泛应用于机械臂轨迹规划、角色动画骨骼系统和三维建模中；圆形碰撞检测则是2D游戏物理引擎中最基础的碰撞原语。理解这些原理，对掌握计算机图形学和机器人学都有重要价值。