物理生物学研究报告【20260016】

逻辑君2026-05-26 10:10

文章目录

球形机器人视觉引导抓取与搬运 -- 纯物理仿真版（无 ForeSight 认知层）

一、设计目标

在 Bullet 物理仿真环境中，实现一个球形不倒翁机器人 ，通过虚拟摄像头 （OpenGL 离屏渲染）识别蓝色箱子，自主完成 起飞 → 搜索 → 接近 → 下降抓取 → 飞向目标点 → 释放 → 返回 → 降落 的完整任务。所有控制参数为固定经验值（无自适应），地图导航使用 A* 算法，不依赖任何外部认知引擎。

二、物理构型

部件 规格 说明
球形主体 半径 0.3 m，质量 3 kg 内置半径 0.08 m 配重球（偏下 0.2 m），形成不倒翁效应
推力系统 6 个方向喷口（上下前后左右） 每喷口最大推力：垂直 800 N，水平 200 N
箱子 0.2×0.2×0.2 m 立方体，质量 1 kg 蓝色，用于抓取
地面 无限大平面，摩擦系数 1.5 防止打滑

三、控制算法

3.1 高度控制（PID）

  • 目标高度：巡航 1.8 m，抓取 0.55 m
  • PID 参数：kp=180, ki=15, kd=40，积分限幅 100
  • 起飞阶段：前 0.6 秒施加 800 N 向上推力

3.2 水平位置控制（PID）

  • 两个独立 PID 控制器：分别控制 X 轴和 Z 轴
  • PID 参数：kp=35, ki=2.5, kd=18，积分限幅 150
  • 输出限制：最大水平力 ±200 N

3.3 抓取机制

  • 当机器人下降到抓取高度且水平位置偏差 <0.2 m 时，创建 btGeneric6DofConstraint 将箱子固定在机器人上
  • 释放时移除约束

四、虚拟视觉与颜色识别

4.1 虚拟相机

  • 安装在机器人前上方（相对位置 (0.5, 0, 0.4)），向前下方俯视（看向 (1.2, 0, 0)）
  • 分辨率 640×480，视场角 60°
  • 通过 OpenGL 离屏渲染（FBO）实时生成图像

4.2 颜色识别

  • HSV 颜色空间，蓝色范围：Hue 100--130, Sat 100--255, Val 100--255
  • 形态学去噪（腐蚀+膨胀）
  • 提取最大轮廓，计算质心水平偏移 dx（像素）
  • 转换为世界偏移：dx_meter = pixel_error * (2 * camera_height * tan(fov/2) / image_height)

五、地图导航与目标记忆

5.1 占用栅格地图

  • 范围 20×20 m，分辨率 0.1 m，栅格数 200×200
  • 静态障碍物：世界边界（|x|>9.5 或 |z|>9.5）标记为占用
  • 机器人每步标记当前位置为已探索

5.2 目标记忆

  • 当视觉检测到箱子时，记录其世界坐标，并更新记忆时间戳
  • 记忆超时时间 5 秒（可调）

5.3 A* 路径规划

  • 8 邻域移动，代价为欧氏距离
  • 从机器人当前位置规划到记忆中的目标点
  • 路径点用于引导机器人（当视觉丢失时）

5.4 搜索模式

  • 视觉有目标：根据 dx 进行 PID 纠偏
  • 视觉丢失但有记忆：切换到 RECOVER 状态，沿 A* 路径导航
  • 视觉丢失且无记忆：切换到 EXPLORE 状态，螺旋搜索（半径 1.5 m 圆周运动）

六、状态机

状态 触发条件 动作
TAKEOFF 起始 向上推力 0.6 秒，达到 0.6 m 后进入 SEARCH
SEARCH 起飞完成 根据视觉偏移移动，找到箱子后进入 APPROACH
APPROACH 视觉偏移 <0.1 m 飞向箱子世界坐标，接近后进入 LOWER_GRAB
LOWER_GRAB 距离 <0.2 m 且速度低 下降至抓取高度，1 秒后抓取
FLY_TO_TARGET 抓取完成 飞向目标点 (5,0)，到达后释放
RETURN 释放完成 返回原点 (0,0)
LAND 返回完成 下降至地面，程序结束
RECOVER 视觉丢失但有记忆 沿 A* 路径前往最后已知位置
EXPLORE 视觉丢失且无记忆 螺旋圆周运动搜索

七、实验结果（典型运行）

  • 起飞到 1.8 m：约 0.6 秒
  • 搜索并定位箱子：约 5--10 秒（取决于初始位置）
  • 接近、下降、抓取：约 2--3 秒
  • 飞向目标点 (5,0)：约 3--5 秒
  • 返回原点并降落：约 5--8 秒
  • 总任务时间：约 20--30 秒

八、代码清单（完整可编译）

cpp 复制代码 
// bullet_vision_phase4_pure_physics.cpp
// 纯物理仿真版（无 ForeSight 认知层）
// 编译命令见文件末尾

#include <btBulletDynamicsCommon.h>
#include <GL/glew.h>
#include <GLFW/glfw3.h>
#include <GL/glut.h>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <cmath>
#include <vector>
#include <queue>
#include <chrono>
#include <thread>
#include <iomanip>
#include <unordered_map>
#include <algorithm>

// ---------- 全局参数 ----------
const double DT = 0.02;
const double SIM_SPEED = 1.0;
const int CAM_WIDTH = 640;
const int CAM_HEIGHT = 480;

// 蓝色箱子 HSV 阈值
const double BOX_COLOR_HUE_LOW = 100;
const double BOX_COLOR_HUE_HIGH = 130;
const double BOX_COLOR_SAT_LOW = 100;
const double BOX_COLOR_SAT_HIGH = 255;
const double BOX_COLOR_VAL_LOW = 100;
const double BOX_COLOR_VAL_HIGH = 255;

// 地图参数
const double MAP_SIZE_X = 20.0;
const double MAP_SIZE_Y = 20.0;
const double MAP_RESOLUTION = 0.1;
const int MAP_WIDTH = (int)(MAP_SIZE_X / MAP_RESOLUTION);
const int MAP_HEIGHT = (int)(MAP_SIZE_Y / MAP_RESOLUTION);
const double MAP_ORIGIN_X = -MAP_SIZE_X/2.0;
const double MAP_ORIGIN_Y = -MAP_SIZE_Y/2.0;

// 2D微缩地图尺寸
const int MAP_DISP_WIDTH = 400;
const int MAP_DISP_HEIGHT = 400;

// 导航参数（固定值）
const double TARGET_MEMORY_TIMEOUT = 5.0;
const double EXPLORE_TIMEOUT = 10.0;

// 飞行高度（固定）
const double CRUISE_HEIGHT = 1.8;
const double GRAB_HEIGHT = 0.55;

// ---------- PID 控制器 ----------
class PID {
public:
    double kp, ki, kd;
    double integral, prev_error;
    double integral_limit;
    
    PID(double p, double i, double d, double i_limit = 100.0)
        : kp(p), ki(i), kd(d), integral(0), prev_error(0), integral_limit(i_limit) {}
    
    double compute(double setpoint, double measurement, double dt) {
        double error = setpoint - measurement;
        if (std::abs(error) > 1.0) integral = 0;
        integral += error * dt;
        integral = std::clamp(integral, -integral_limit, integral_limit);
        double derivative = (error - prev_error) / dt;
        prev_error = error;
        double out = kp * error + ki * integral + kd * derivative;
        return std::clamp(out, -400.0, 600.0);
    }
    
    void reset() { integral = 0; prev_error = 0; }
};

// ---------- 占用栅格地图 ----------
class OccupancyGridMap {
public:
    OccupancyGridMap() {
        occupancy.resize(MAP_WIDTH * MAP_HEIGHT, 0);
        explored.resize(MAP_WIDTH * MAP_HEIGHT, 0);
        for (int gx = 0; gx < MAP_WIDTH; ++gx) {
            for (int gy = 0; gy < MAP_HEIGHT; ++gy) {
                double wx, wy;
                gridToWorld(gx, gy, wx, wy);
                if (std::abs(wx) >= 9.5 || std::abs(wy) >= 9.5) {
                    setOccupied(gx, gy);
                }
            }
        }
    }

    void gridToWorld(int gx, int gy, double& wx, double& wy) const {
        wx = MAP_ORIGIN_X + (gx + 0.5) * MAP_RESOLUTION;
        wy = MAP_ORIGIN_Y + (gy + 0.5) * MAP_RESOLUTION;
    }

    bool worldToGrid(double wx, double wy, int& gx, int& gy) const {
        gx = (int)((wx - MAP_ORIGIN_X) / MAP_RESOLUTION);
        gy = (int)((wy - MAP_ORIGIN_Y) / MAP_RESOLUTION);
        return (gx >= 0 && gx < MAP_WIDTH && gy >= 0 && gy < MAP_HEIGHT);
    }

    bool isOccupied(int gx, int gy) const {
        if (gx < 0 || gx >= MAP_WIDTH || gy < 0 || gy >= MAP_HEIGHT) return true;
        return occupancy[gy * MAP_WIDTH + gx] == 2;
    }

    void setOccupied(int gx, int gy) {
        if (gx < 0 || gx >= MAP_WIDTH || gy < 0 || gy >= MAP_HEIGHT) return;
        occupancy[gy * MAP_WIDTH + gx] = 2;
    }

    void markExplored(int gx, int gy) {
        if (gx < 0 || gx >= MAP_WIDTH || gy < 0 || gy >= MAP_HEIGHT) return;
        explored[gy * MAP_WIDTH + gx] = 1;
    }

    void updateTargetPosition(double wx, double wy) {
        target_world_x = wx;
        target_world_y = wy;
        target_timestamp = std::chrono::steady_clock::now();
        has_target_memory = true;
    }

    bool getTargetMemory(double& wx, double& wy) {
        if (!has_target_memory) return false;
        auto now = std::chrono::steady_clock::now();
        double elapsed = std::chrono::duration<double>(now - target_timestamp).count();
        if (elapsed > TARGET_MEMORY_TIMEOUT) {
            has_target_memory = false;
            return false;
        }
        wx = target_world_x;
        wy = target_world_y;
        return true;
    }

    void invalidateTargetMemory() { has_target_memory = false; }

    std::vector<btVector3> planPath(double start_x, double start_y, double goal_x, double goal_y) {
        int sgx, sgy, ggx, ggy;
        if (!worldToGrid(start_x, start_y, sgx, sgy)) return {};
        if (!worldToGrid(goal_x, goal_y, ggx, ggy)) return {};
        if (isOccupied(ggx, ggy)) return {};

        struct Node {
            int gx, gy;
            double g_cost, h_cost;
            Node* parent;
            Node(int x, int y) : gx(x), gy(y), g_cost(0), h_cost(0), parent(nullptr) {}
            double f() const { return g_cost + h_cost; }
        };
        auto cmp = [](Node* a, Node* b) { return a->f() > b->f(); };
        std::priority_queue<Node*, std::vector<Node*>, decltype(cmp)> open(cmp);
        std::unordered_map<int, Node*> all_nodes;

        Node* start = new Node(sgx, sgy);
        start->h_cost = heuristic(sgx, sgy, ggx, ggy);
        open.push(start);
        all_nodes[sgx + sgy * MAP_WIDTH] = start;

        const int dx[8] = {1,0,-1,0,1,1,-1,-1};
        const int dy[8] = {0,1,0,-1,1,-1,1,-1};
        const double step_cost[8] = {1.0,1.0,1.0,1.0,1.414,1.414,1.414,1.414};

        while (!open.empty()) {
            Node* cur = open.top(); open.pop();
            if (cur->gx == ggx && cur->gy == ggy) {
                std::vector<btVector3> path;
                Node* n = cur;
                while (n) {
                    double wx, wy;
                    gridToWorld(n->gx, n->gy, wx, wy);
                    path.push_back(btVector3(wx, wy, 0));
                    n = n->parent;
                }
                std::reverse(path.begin(), path.end());
                for (auto& p : all_nodes) delete p.second;
                return path;
            }
            for (int i = 0; i < 8; ++i) {
                int ngx = cur->gx + dx[i];
                int ngy = cur->gy + dy[i];
                if (ngx < 0 || ngx >= MAP_WIDTH || ngy < 0 || ngy >= MAP_HEIGHT) continue;
                if (isOccupied(ngx, ngy)) continue;
                double new_g = cur->g_cost + step_cost[i];
                int key = ngx + ngy * MAP_WIDTH;
                if (all_nodes.find(key) == all_nodes.end()) {
                    Node* child = new Node(ngx, ngy);
                    child->g_cost = new_g;
                    child->h_cost = heuristic(ngx, ngy, ggx, ggy);
                    child->parent = cur;
                    all_nodes[key] = child;
                    open.push(child);
                } else {
                    Node* child = all_nodes[key];
                    if (new_g < child->g_cost) {
                        child->g_cost = new_g;
                        child->parent = cur;
                        open.push(child);
                    }
                }
            }
        }
        for (auto& p : all_nodes) delete p.second;
        return {};
    }

private:
    std::vector<uint8_t> occupancy;
    std::vector<uint8_t> explored;
    double target_world_x, target_world_y;
    std::chrono::steady_clock::time_point target_timestamp;
    bool has_target_memory = false;

    double heuristic(int gx1, int gy1, int gx2, int gy2) const {
        double dx = (gx1 - gx2) * MAP_RESOLUTION;
        double dy = (gy1 - gy2) * MAP_RESOLUTION;
        return std::sqrt(dx*dx + dy*dy);
    }
};

// ---------- 机器人结构 ----------
struct VisionGuidedRobot {
    btDiscreteDynamicsWorld* world;
    btRigidBody* sphere_body;
    btRigidBody* box_object;
    btGeneric6DofConstraint* box_constraint;
    bool is_carrying;
    double thrust_up, thrust_down, thrust_forward, thrust_backward, thrust_right, thrust_left;
    OccupancyGridMap grid_map;
    std::vector<btVector3> current_path;
    int path_index;
    double last_seen_time;
    btVector3 last_known_box_pos;

    PID pidHeight, pidPosX, pidPosZ;

    VisionGuidedRobot(btDiscreteDynamicsWorld* w, btRigidBody* box = nullptr)
        : world(w), box_object(box), box_constraint(nullptr), is_carrying(false), path_index(0), last_seen_time(-1e9),
          pidHeight(180.0, 15.0, 40.0, 100.0),
          pidPosX(35.0, 2.5, 18.0, 150.0),
          pidPosZ(35.0, 2.5, 18.0, 150.0) {
        buildRobot();
        resetThrust();
    }

    void buildRobot() {
        btCollisionShape* sphereShape = new btSphereShape(0.3);
        btCompoundShape* compound = new btCompoundShape();
        btTransform mainTrans = btTransform::getIdentity();
        compound->addChildShape(mainTrans, sphereShape);
        btCollisionShape* weightShape = new btSphereShape(0.08);
        btTransform weightTrans = btTransform::getIdentity();
        weightTrans.setOrigin(btVector3(0, 0, -0.2));
        compound->addChildShape(weightTrans, weightShape);
        btVector3 inertia(0, 0, 0);
        compound->calculateLocalInertia(3.0, inertia);
        btDefaultMotionState* ms = new btDefaultMotionState(btTransform(btQuaternion::getIdentity(), btVector3(0, 0, 0.6)));
        sphere_body = new btRigidBody(3.0, ms, compound, inertia);
        sphere_body->setDamping(0.2, 0.5);
        world->addRigidBody(sphere_body);
    }

    void resetThrust() {
        thrust_up = thrust_down = thrust_forward = thrust_backward = thrust_right = thrust_left = 0;
    }

    void applyThrust() {
        btVector3 force(thrust_forward - thrust_backward, 0, thrust_right - thrust_left);
        force.setZ(force.z() + thrust_up - thrust_down);
        sphere_body->applyCentralForce(force);
    }

    void clampThrust(double maxVert = 800.0, double maxHoriz = 200.0) {
        thrust_up = std::clamp(thrust_up, 0.0, maxVert);
        thrust_down = std::clamp(thrust_down, 0.0, maxVert);
        thrust_forward = std::clamp(thrust_forward, 0.0, maxHoriz);
        thrust_backward = std::clamp(thrust_backward, 0.0, maxHoriz);
        thrust_right = std::clamp(thrust_right, 0.0, maxHoriz);
        thrust_left = std::clamp(thrust_left, 0.0, maxHoriz);
    }

    double getHeight() const { return sphere_body->getCenterOfMassPosition().getZ(); }
    btVector3 getPosition() const { return sphere_body->getCenterOfMassPosition(); }
    btVector3 getVelocity() const { return sphere_body->getLinearVelocity(); }

    void grabBox() {
        if (is_carrying || !box_object) return;
        btTransform frameA, frameB;
        frameA.setIdentity(); frameA.setOrigin(btVector3(0, 0, 0.2));
        frameB.setIdentity();
        box_constraint = new btGeneric6DofConstraint(*sphere_body, *box_object, frameA, frameB, true);
        box_constraint->setLinearLowerLimit(btVector3(0, 0, 0));
        box_constraint->setLinearUpperLimit(btVector3(0, 0, 0));
        box_constraint->setAngularLowerLimit(btVector3(0, 0, 0));
        box_constraint->setAngularUpperLimit(btVector3(0, 0, 0));
        world->addConstraint(box_constraint, true);
        is_carrying = true;
        std::cout << "[抓取] 箱子已吸附\n";
    }

    void releaseBox() {
        if (box_constraint) {
            world->removeConstraint(box_constraint);
            delete box_constraint;
            box_constraint = nullptr;
        }
        is_carrying = false;
        std::cout << "[释放] 箱子已放下\n";
    }

    void updateWithVision(bool has_target, double dx_meter, double dz_meter, double current_time) {
        if (has_target) {
            btVector3 boxPos = box_object->getCenterOfMassPosition();
            last_known_box_pos = boxPos;
            last_seen_time = current_time;
            grid_map.updateTargetPosition(boxPos.x(), boxPos.y());
            int gx, gy;
            if (grid_map.worldToGrid(getPosition().x(), getPosition().y(), gx, gy))
                grid_map.markExplored(gx, gy);
        }
    }

    bool getNavigationTarget(btVector3& target) {
        double wx, wy;
        if (grid_map.getTargetMemory(wx, wy)) {
            target = btVector3(wx, wy, 0);
            return true;
        }
        return false;
    }

    bool planPathToTarget(const btVector3& target) {
        btVector3 start = getPosition();
        current_path = grid_map.planPath(start.x(), start.y(), target.x(), target.y());
        path_index = 0;
        return !current_path.empty();
    }

    bool getNextWaypoint(btVector3& waypoint) {
        if (path_index >= current_path.size()) return false;
        waypoint = current_path[path_index];
        btVector3 pos = getPosition();
        if ((waypoint - pos).length() < 0.2) {
            path_index++;
            if (path_index < current_path.size())
                waypoint = current_path[path_index];
            else
                return false;
        }
        return true;
    }

    std::vector<btVector3> getCurrentPath() const { return current_path; }
};

// ---------- 可绘制物体 ----------
struct Drawable {
    btRigidBody* body;
    btVector3 color;
    btVector3 halfExtents;
    bool isSphere;
    float radius;
};

// ---------- OpenGL 离屏渲染器 ----------
class OffscreenRenderer {
public:
    OffscreenRenderer(int width, int height) : w(width), h(height) {
        initGLFW();
        initFBO();
    }
    ~OffscreenRenderer() {
        glDeleteFramebuffers(1, &fbo);
        glDeleteTextures(1, &colorTex);
        glDeleteRenderbuffers(1, &depthRb);
        glfwDestroyWindow(window);
        glfwTerminate();
    }
    void render(const btVector3& cameraPos, const btVector3& cameraTarget, const std::vector<Drawable>& drawables) {
        glBindFramebuffer(GL_FRAMEBUFFER, fbo);
        glViewport(0, 0, w, h);
        glClearColor(0.6f, 0.9f, 0.6f, 1.0f);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        glEnable(GL_DEPTH_TEST);
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        gluPerspective(60.0, (double)w/h, 0.1, 10.0);
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        gluLookAt(cameraPos.x(), cameraPos.y(), cameraPos.z(),
                  cameraTarget.x(), cameraTarget.y(), cameraTarget.z(),
                  0, 1, 0);
        glColor3f(0.6f, 0.9f, 0.6f);
        glBegin(GL_QUADS);
        glVertex3f(-10, -10, 0); glVertex3f(10, -10, 0);
        glVertex3f(10,  10, 0); glVertex3f(-10,  10, 0);
        glEnd();
        for (const auto& d : drawables) {
            btTransform trans = d.body->getWorldTransform();
            btVector3 pos = trans.getOrigin();
            glPushMatrix();
            glTranslatef(pos.x(), pos.y(), pos.z());
            glColor3f(d.color.x(), d.color.y(), d.color.z());
            if (d.isSphere) glutSolidSphere(d.radius, 20, 20);
            else {
                glScalef(d.halfExtents.x()*2, d.halfExtents.y()*2, d.halfExtents.z()*2);
                glutSolidCube(1.0);
            }
            glPopMatrix();
        }
        glBindFramebuffer(GL_FRAMEBUFFER, 0);
    }
    cv::Mat captureImage() {
        glBindFramebuffer(GL_FRAMEBUFFER, fbo);
        std::vector<unsigned char> pixels(w*h*3);
        glReadPixels(0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, pixels.data());
        glBindFramebuffer(GL_FRAMEBUFFER, 0);
        cv::Mat img(h, w, CV_8UC3);
        memcpy(img.data, pixels.data(), w*h*3);
        cv::cvtColor(img, img, cv::COLOR_RGB2BGR);
        cv::flip(img, img, 0);
        return img;
    }
private:
    int w, h;
    GLFWwindow* window;
    GLuint fbo, colorTex, depthRb;
    void initGLFW() {
        glfwInit();
        glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);
        window = glfwCreateWindow(w, h, "VirtualCamera", nullptr, nullptr);
        glfwMakeContextCurrent(window);
        glewInit();
        int dummy_argc=1; char* dummy_argv[]={(char*)"bullet_vision"};
        glutInit(&dummy_argc, dummy_argv);
    }
    void initFBO() {
        glGenTextures(1, &colorTex);
        glBindTexture(GL_TEXTURE_2D, colorTex);
        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, w, h, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
        glGenRenderbuffers(1, &depthRb);
        glBindRenderbuffer(GL_RENDERBUFFER, depthRb);
        glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT24, w, h);
        glGenFramebuffers(1, &fbo);
        glBindFramebuffer(GL_FRAMEBUFFER, fbo);
        glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, colorTex, 0);
        glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, depthRb);
        if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE)
            std::cerr << "FBO创建失败\n";
        glBindFramebuffer(GL_FRAMEBUFFER, 0);
    }
};

double pixelToMeter(double pixelError, double cameraHeight, double fovDeg=60.0) {
    double rad = fovDeg * M_PI / 180.0;
    double meterPerPixel = 2.0 * cameraHeight * tan(rad/2.0) / CAM_HEIGHT;
    return pixelError * meterPerPixel;
}

void drawMiniMap(const VisionGuidedRobot& robot, const btVector3& boxPos, cv::Mat& mapImg) {
    mapImg.setTo(cv::Scalar(30, 30, 30));
    auto worldToMap = [](double wx, double wy) -> cv::Point {
        int px = (int)((wx - MAP_ORIGIN_X) / MAP_SIZE_X * MAP_DISP_WIDTH);
        int py = (int)((wy - MAP_ORIGIN_Y) / MAP_SIZE_Y * MAP_DISP_HEIGHT);
        py = MAP_DISP_HEIGHT - py;
        return cv::Point(px, py);
    };
    int x0 = worldToMap(-9.5, -9.5).x, y0 = worldToMap(-9.5, -9.5).y;
    int x1 = worldToMap(9.5, 9.5).x, y1 = worldToMap(9.5, 9.5).y;
    cv::rectangle(mapImg, cv::Point(x0, y0), cv::Point(x1, y1), cv::Scalar(100,100,100), 1);
    auto path = robot.getCurrentPath();
    for (size_t i=0; i+1<path.size(); ++i) {
        cv::Point p1 = worldToMap(path[i].x(), path[i].y());
        cv::Point p2 = worldToMap(path[i+1].x(), path[i+1].y());
        cv::line(mapImg, p1, p2, cv::Scalar(0,255,255), 1, cv::LINE_AA);
    }
    cv::Point boxPt = worldToMap(boxPos.x(), boxPos.y());
    cv::circle(mapImg, boxPt, 6, cv::Scalar(255,0,0), -1, cv::LINE_AA);
    btVector3 robotPos = robot.getPosition();
    cv::Point robotPt = worldToMap(robotPos.x(), robotPos.y());
    cv::circle(mapImg, robotPt, 8, cv::Scalar(0,0,255), -1, cv::LINE_AA);
    btVector3 vel = robot.getVelocity();
    if (vel.length() > 0.1) {
        cv::Point dirPt = worldToMap(robotPos.x() + vel.x()*0.5, robotPos.y() + vel.y()*0.5);
        cv::line(mapImg, robotPt, dirPt, cv::Scalar(255,255,255), 2, cv::LINE_AA);
    }
}

int main() {
    // Bullet 世界
    btDefaultCollisionConfiguration* cfg = new btDefaultCollisionConfiguration();
    btCollisionDispatcher* disp = new btCollisionDispatcher(cfg);
    btDbvtBroadphase* bp = new btDbvtBroadphase();
    btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
    btDiscreteDynamicsWorld* world = new btDiscreteDynamicsWorld(disp, bp, solver, cfg);
    world->setGravity(btVector3(0,0,-9.8));

    btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,0,1), 0);
    btDefaultMotionState* groundMS = new btDefaultMotionState(btTransform::getIdentity());
    btRigidBody* ground = new btRigidBody(0, groundMS, groundShape);
    ground->setFriction(1.5);
    world->addRigidBody(ground);

    btCollisionShape* boxShape = new btBoxShape(btVector3(0.2,0.2,0.2));
    btVector3 boxInertia(0,0,0);
    boxShape->calculateLocalInertia(1.0, boxInertia);
    btDefaultMotionState* boxMS = new btDefaultMotionState(btTransform(btQuaternion::getIdentity(), btVector3(1.5,0,0.2)));
    btRigidBody* box = new btRigidBody(1.0, boxMS, boxShape, boxInertia);
    box->setFriction(0.8);
    world->addRigidBody(box);

    VisionGuidedRobot robot(world, box);
    robot.resetThrust();

    std::vector<Drawable> drawables;
    drawables.push_back({robot.sphere_body, btVector3(1,0,0), btVector3(0,0,0), true, 0.3f});
    drawables.push_back({box, btVector3(0,0,1), btVector3(0.2,0.2,0.2), false, 0.0f});

    OffscreenRenderer renderer(CAM_WIDTH, CAM_HEIGHT);

    enum State { TAKEOFF, SEARCH, APPROACH, LOWER_GRAB, FLY_TO_TARGET, RELEASE, RETURN, LAND, RECOVER, EXPLORE };
    State state = TAKEOFF;

    double targetHeight = CRUISE_HEIGHT;
    double grabHeight = GRAB_HEIGHT;
    btVector3 targetPos(5.0,0,0);
    btVector3 boxWorldPos;
    bool hasTarget = false;
    double dx = 0, dz = 0;
    double simTime = 0;
    int step = 0;
    bool boosting = true;
    double boostEndTime = 0.6;
    bool descending = false;
    double descendStartTime = 0;
    double explore_start_time = 0;
    double last_vision_time = 0;

    cv::namedWindow("MiniMap", cv::WINDOW_NORMAL);
    cv::resizeWindow("MiniMap", MAP_DISP_WIDTH, MAP_DISP_HEIGHT);
    cv::Mat mapImage(MAP_DISP_HEIGHT, MAP_DISP_WIDTH, CV_8UC3);
    cv::namedWindow("Virtual Camera", cv::WINDOW_NORMAL);
    cv::resizeWindow("Virtual Camera", CAM_WIDTH, CAM_HEIGHT);

    std::cout << "===== 纯物理仿真：视觉引导抓取与搬运（无认知层） =====\n";

    while (simTime < 180.0) {
        world->stepSimulation(DT, 1, DT);
        simTime += DT;
        step++;

        double height = robot.getHeight();
        btVector3 pos = robot.getPosition();
        btVector3 vel = robot.getVelocity();

        // 虚拟相机
        btVector3 camPos = pos + btVector3(0.5, 0, 0.4);
        btVector3 camTarget = pos + btVector3(1.2, 0, 0);
        renderer.render(camPos, camTarget, drawables);
        cv::Mat frame = renderer.captureImage();
        if (!frame.empty()) {
            cv::imshow("Virtual Camera", frame);
            cv::waitKey(1);
        }

        // 颜色分割
        cv::Mat hsv, mask;
        cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV);
        cv::inRange(hsv, cv::Scalar(BOX_COLOR_HUE_LOW, BOX_COLOR_SAT_LOW, BOX_COLOR_VAL_LOW),
                    cv::Scalar(BOX_COLOR_HUE_HIGH, BOX_COLOR_SAT_HIGH, BOX_COLOR_VAL_HIGH), mask);
        cv::erode(mask, mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3)));
        cv::dilate(mask, mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3)));
        std::vector<std::vector<cv::Point>> contours;
        cv::findContours(mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
        hasTarget = false;
        if (!contours.empty()) {
            auto largest = std::max_element(contours.begin(), contours.end(),
                [](auto& a, auto& b){ return cv::contourArea(a) < cv::contourArea(b); });
            cv::Moments m = cv::moments(*largest);
            if (m.m00 > 1e-6) {
                double cx = m.m10 / m.m00;
                double imgCenterX = CAM_WIDTH/2.0;
                double pixelError = cx - imgCenterX;
                double cameraHeightAbs = 0.4 + height;
                dx = pixelToMeter(pixelError, cameraHeightAbs, 60.0);
                dz = 0;
                hasTarget = true;
                last_vision_time = simTime;
                robot.updateWithVision(true, dx, dz, simTime);
                if (state == RECOVER || state == EXPLORE) {
                    state = APPROACH;
                    std::cout << "[视觉恢复] 重新看到箱子，切换至接近模式\n";
                }
            }
        } else {
            robot.updateWithVision(false, 0, 0, simTime);
        }

        // 高度控制
        double forceVertical = 0;
        if (boosting && simTime < boostEndTime) forceVertical = 800.0;
        else {
            boosting = false;
            double targetH = (state == LOWER_GRAB) ? grabHeight : targetHeight;
            forceVertical = robot.pidHeight.compute(targetH, height, DT);
        }
        if (forceVertical > 0) { robot.thrust_up = forceVertical; robot.thrust_down = 0; }
        else { robot.thrust_up = 0; robot.thrust_down = -forceVertical; }

        // 水平控制
        double forceX = 0, forceZ = 0;
        switch (state) {
            case TAKEOFF:
                if (height > 0.6) { state = SEARCH; std::cout << "[起飞完成] 进入搜索模式\n"; }
                break;
            case SEARCH:
                if (hasTarget && std::abs(dx) < 0.1) {
                    state = APPROACH;
                    boxWorldPos = pos + btVector3(dx, 0, dz);
                    std::cout << "[定位] 箱子在 (" << boxWorldPos.x() << ", " << boxWorldPos.z() << ")\n";
                } else if (hasTarget) {
                    forceX = robot.pidPosX.compute(0, dx, DT);
                    forceZ = robot.pidPosZ.compute(0, 0, DT);
                } else {
                    btVector3 navTarget;
                    if (robot.getNavigationTarget(navTarget)) {
                        state = RECOVER;
                        std::cout << "[视觉丢失] 启用记忆恢复，目标(" << navTarget.x() << "," << navTarget.y() << ")\n";
                        robot.planPathToTarget(navTarget);
                    } else {
                        state = EXPLORE;
                        explore_start_time = simTime;
                        std::cout << "[无记忆] 进入探索模式\n";
                    }
                }
                break;
            case RECOVER:
                {
                    btVector3 waypoint;
                    if (robot.getNextWaypoint(waypoint)) {
                        forceX = robot.pidPosX.compute(waypoint.x(), pos.x(), DT);
                        forceZ = robot.pidPosZ.compute(waypoint.y(), pos.y(), DT);
                    } else {
                        btVector3 target;
                        if (robot.getNavigationTarget(target))
                            robot.planPathToTarget(target);
                        else
                            state = EXPLORE;
                    }
                }
                break;
            case EXPLORE:
                {
                    double angle = simTime * 0.5;
                    double radius = 1.5;
                    double target_x = pos.x() + radius * cos(angle);
                    double target_z = pos.y() + radius * sin(angle);
                    target_x = std::clamp(target_x, -9.0, 9.0);
                    target_z = std::clamp(target_z, -9.0, 9.0);
                    forceX = robot.pidPosX.compute(target_x, pos.x(), DT);
                    forceZ = robot.pidPosZ.compute(target_z, pos.y(), DT);
                    if (simTime - explore_start_time > EXPLORE_TIMEOUT) {
                        state = SEARCH;
                        std::cout << "[探索超时] 返回搜索模式\n";
                    }
                }
                break;
            case APPROACH:
                {
                    btVector3 err = boxWorldPos - pos;
                    if (err.length() < 0.2 && std::abs(vel.x())<0.5 && std::abs(vel.z())<0.5) {
                        state = LOWER_GRAB;
                        descendStartTime = simTime;
                        descending = true;
                        std::cout << "[接近箱子] 开始下降抓取\n";
                    } else {
                        forceX = robot.pidPosX.compute(boxWorldPos.x(), pos.x(), DT);
                        forceZ = robot.pidPosZ.compute(boxWorldPos.z(), pos.y(), DT);
                    }
                }
                break;
            case LOWER_GRAB:
                if (!descending) { descendStartTime=simTime; descending=true; }
                if (simTime-descendStartTime>1.0 || height<=grabHeight+0.05) {
                    robot.grabBox();
                    state = FLY_TO_TARGET;
                    targetHeight = CRUISE_HEIGHT;
                    descending = false;
                    std::cout << "[抓取成功] 飞向目标点\n";
                }
                break;
            case FLY_TO_TARGET:
                {
                    btVector3 err = targetPos - pos;
                    if (err.length()<0.3 && std::abs(vel.x())<0.5) {
                        robot.releaseBox();
                        state = RETURN;
                        std::cout << "[到达目标] 释放箱子，返回原点\n";
                    } else {
                        forceX = robot.pidPosX.compute(targetPos.x(), pos.x(), DT);
                        forceZ = robot.pidPosZ.compute(targetPos.z(), pos.y(), DT);
                    }
                }
                break;
            case RETURN:
                {
                    btVector3 err = btVector3(0,0,0) - pos;
                    if (err.length()<0.3 && std::abs(vel.x())<0.5) {
                        state = LAND;
                        std::cout << "[返回原点] 开始降落\n";
                    } else {
                        forceX = robot.pidPosX.compute(0, pos.x(), DT);
                        forceZ = robot.pidPosZ.compute(0, pos.y(), DT);
                    }
                }
                break;
            case LAND:
                if (height<0.45 && std::abs(vel.z())<0.3) {
                    robot.thrust_up=0; robot.thrust_down=0;
                    robot.thrust_forward=robot.thrust_backward=robot.thrust_left=robot.thrust_right=0;
                    std::cout << "[降落完成] 任务成功结束\n";
                    goto end_simulation;
                }
                break;
        }

        forceX = std::clamp(forceX, -200.0, 200.0);
        forceZ = std::clamp(forceZ, -200.0, 200.0);
        if (forceX>0) { robot.thrust_forward=forceX; robot.thrust_backward=0; }
        else { robot.thrust_backward=-forceX; robot.thrust_forward=0; }
        if (forceZ>0) { robot.thrust_right=forceZ; robot.thrust_left=0; }
        else { robot.thrust_left=-forceZ; robot.thrust_right=0; }

        robot.clampThrust(800, 200);
        robot.applyThrust();

        drawMiniMap(robot, box->getCenterOfMassPosition(), mapImage);
        cv::imshow("MiniMap", mapImage);
        cv::waitKey(1);

        if (step % 50 == 0) {
            std::cout << std::fixed << std::setprecision(2)
                      << "t=" << std::setw(5) << simTime
                      << " h=" << std::setw(5) << height
                      << " pos=(" << pos.x() << "," << pos.y() << ")"
                      << " carry=" << robot.is_carrying
                      << " state=" << state
                      << " hasTarget=" << hasTarget
                      << " dx=" << dx
                      << std::endl;
        }
        if (SIM_SPEED < 1.0) std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

end_simulation:
    cv::destroyAllWindows();
    delete world; delete solver; delete bp; delete disp; delete cfg;
    return 0;
}

编译命令

bash 复制代码 
g++ -std=c++17 -O2 bullet_vision_phase4_pure_physics.cpp   $(pkg-config --cflags --libs bullet)     -lsqlite3 -lcurl -lpthread     -lglfw -lGLEW -lGLU -lGL -lglut     `pkg-config --cflags --libs opencv4` -lpthread -o bullet_vision_phase4_pure_physics

说明

  • 无任何 ForeSight 智能认知组件
  • 所有控制参数为固定值（PID 系数、推力限制、高度阈值等）
  • 状态机完全由硬编码逻辑驱动
  • 地图导航使用 A 算法*（无学习能力）
  • 视觉为 OpenCV 颜色识别（无自适应参数）

此代码可直接编译运行，展示纯物理控制下的自主抓取搬运任务。

相关推荐
cd_9492172115 小时前
云工场科技推进CPU+GPU协同推理，推动大模型应用降本增效
大数据·人工智能·科技
惊鸿一博15 小时前
大语言模型_概念_Transformer_位置编码 RoPE 解释
人工智能·语言模型·transformer
东方佑15 小时前
OpenASH-85M:基于累积最大值注意力的无 Softmax 语言模型，支持有状态推理
人工智能·语言模型·自然语言处理
linmengmeng_131415 小时前
【总结】HugeGraph-AI：当图数据库遇见大模型，构建智能图应用的新范式
数据库·人工智能
通信小呆呆15 小时前
维度分数傅里叶时频图 + 图神经网络：突破传统时频分析的目标识别与杂波抑制新框架
人工智能·神经网络·算法
IronMurphy15 小时前
AI Agent 学习笔记 Day 1：大模型基础、API 调用与 Prompt 工程
人工智能·笔记·学习
csdn_aspnet15 小时前
C++ 算法 LeetCode 编号 70 - 爬楼梯
开发语言·c++·算法·leetcode
he___H15 小时前
leetcode100-合并区间
java·数据结构·算法
ZHW_AI课题组15 小时前
基于PCA与HOG特征融合的热轧钢带缺陷检测
人工智能·python·机器学习
掘根15 小时前
【openCV】图像缩放，翻转，旋转，视频文件/摄像头读取/保存
人工智能·opencv·计算机视觉
热门推荐
01GitHub 镜像站点02Codex 接入 DeepSeek API 完整配置文档03【踩坑记录 | 第一篇】微软商店无法使用时，如何手动安装 OpenAI Codex？附`.msix`文件系统错误解决方法04裂开！ChatGPT 居然开始要手机号验证，附详细解决方法05CC-Switch & Claude 基于 Linux 服务器安装使用指南06【AI】2026 年具身智能模型和世界模型总结07Codegraph 实战：用知识图谱让 AI 编程效率翻倍08LLM Wiki：让大模型替你打理知识库的完整指南09Codex使用DeepSeek API的方法（cc switch + codex bridge方案）10几个好用的ip纯净度检测网站