🔍 机器人感知与传感器¶

from ultralytics import YOLO

# 加载预训练模型
model = YOLO("yolov8n.pt")  # YOLOv8 nano版，适合实时场景

# 推理
results = model("robot_workspace.jpg")

# 提取检测结果用于机器人决策
for r in results:
    for box in r.boxes:
        cls_name = model.names[int(box.cls)]
        x1, y1, x2, y2 = box.xyxy[0].tolist()
        conf = float(box.conf)
        # 计算物体中心（像素坐标）
        cx, cy = (x1 + x2) / 2, (y1 + y2) / 2
        print(f"检测到 {cls_name} (置信度{conf:.2f}) 中心: ({cx:.0f}, {cy:.0f})")

from sam2.build_sam import build_sam2
from sam2.sam2_image_predictor import SAM2ImagePredictor
import numpy as np

# SAM2 分割任意物体（需要模型配置文件 + 权重文件）
sam2_model = build_sam2("sam2.1_hiera_l.yaml", "sam2.1_hiera_large.pt")
predictor = SAM2ImagePredictor(sam2_model)
predictor.set_image(image)

# 给定一个点提示 → 分割出对应物体
masks, scores, logits = predictor.predict(
    point_coords=np.array([[cx, cy]]),  # 检测框中心
    point_labels=np.array([1]),          # 1=前景
    multimask_output=True
)
best_mask = masks[np.argmax(scores)]  # 取最高分的mask

import numpy as np

class PoseEstimator:
    """基于深度图 + 检测框的简化位姿估计"""

    def __init__(self, camera_intrinsics):
        """
        camera_intrinsics: 相机内参矩阵 K (3x3)
        [[fx, 0, cx],
         [0, fy, cy],
         [0,  0,  1]]
        """
        self.K = camera_intrinsics
        self.fx = camera_intrinsics[0, 0]
        self.fy = camera_intrinsics[1, 1]
        self.cx = camera_intrinsics[0, 2]
        self.cy = camera_intrinsics[1, 2]

    def pixel_to_3d(self, u, v, depth):
        """像素坐标 + 深度 → 3D相机坐标"""
        z = depth
        x = (u - self.cx) * z / self.fx
        y = (v - self.cy) * z / self.fy
        return np.array([x, y, z])

    def estimate_grasp_pose(self, mask, depth_image):
        """从分割mask和深度图估计抓取位姿"""
        # 获取mask区域的深度值
        masked_depth = depth_image[mask > 0]
        median_depth = np.median(masked_depth[masked_depth > 0])

        # 计算mask质心
        ys, xs = np.where(mask > 0)
        cu, cv = xs.mean(), ys.mean()

        # 转换为3D坐标
        position = self.pixel_to_3d(cu, cv, median_depth)

        return {
            'position': position,         # [x, y, z] 米
            'approach_direction': np.array([0, 0, -1]),  # 默认从上方接近
            'grasp_width': self._estimate_width(xs, median_depth)
        }

    def _estimate_width(self, xs, depth):
        """根据mask宽度估计物体尺寸"""
        pixel_width = xs.max() - xs.min()
        real_width = pixel_width * depth / self.fx
        return real_width

import open3d as o3d
import numpy as np

def process_point_cloud(depth_image, rgb_image, intrinsics):
    """从深度图和RGB图生成彩色点云"""

    # 创建Open3D相机内参
    pinhole = o3d.camera.PinholeCameraIntrinsic(
        width=640, height=480,
        fx=intrinsics[0, 0], fy=intrinsics[1, 1],
        cx=intrinsics[0, 2], cy=intrinsics[1, 2]
    )

    # 从RGBD图像创建点云
    rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
        o3d.geometry.Image(rgb_image),
        o3d.geometry.Image(depth_image.astype(np.float32)),
        depth_scale=1000.0,  # mm → m
        depth_trunc=2.0,     # 截断距离
        convert_rgb_to_intensity=False
    )
    pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, pinhole)

    # 下采样 + 去噪
    pcd = pcd.voxel_down_sample(voxel_size=0.005)   # 5mm体素下采样
    pcd, inliers = pcd.remove_statistical_outlier(
        nb_neighbors=20, std_ratio=2.0
    )

    # 平面分割（移除桌面）
    plane_model, plane_inliers = pcd.segment_plane(
        distance_threshold=0.01, ransac_n=3, num_iterations=1000
    )
    objects_pcd = pcd.select_by_index(plane_inliers, invert=True)

    # 聚类分割（分离不同物体）
    labels = np.array(objects_pcd.cluster_dbscan(
        eps=0.02, min_points=50
    ))

    return objects_pcd, labels

def extract_object_features(pcd, label, labels):
    """提取单个物体的几何特征"""
    obj_pcd = pcd.select_by_index(np.where(labels == label)[0])

    # 包围盒
    obb = obj_pcd.get_oriented_bounding_box()

    # 中心点
    center = obb.center

    # 尺寸（长宽高）
    extent = obb.extent  # [length, width, height]

    # 法向量估计
    obj_pcd.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.02, max_nn=30)
    )

    return {
        'center': center,
        'size': extent,
        'bbox': obb,
        'num_points': len(obj_pcd.points)
    }

import torch
import torch.nn as nn

class PointNetEncoder(nn.Module):  # 继承nn.Module定义网络层
    """PointNet: 直接处理无序点云的深度学习模型"""

    def __init__(self, in_channels=3, global_feat_dim=1024):
        super().__init__()  # super()调用父类方法
        # 逐点MLP（共享参数）
        self.mlp1 = nn.Sequential(
            nn.Conv1d(in_channels, 64, 1),
            nn.BatchNorm1d(64), nn.ReLU(),
            nn.Conv1d(64, 128, 1),
            nn.BatchNorm1d(128), nn.ReLU(),
            nn.Conv1d(128, global_feat_dim, 1),
            nn.BatchNorm1d(global_feat_dim), nn.ReLU()
        )

    def forward(self, x):
        """
        x: (B, N, 3) 点云坐标
        return: (B, global_feat_dim) 全局特征
        """
        x = x.transpose(1, 2)      # (B, 3, N)
        x = self.mlp1(x)           # (B, 1024, N)
        x = torch.max(x, dim=2)[0] # 全局max pooling → (B, 1024)
        return x

# PointNet的关键创新：
# 1. 对称函数(max pooling)实现点云置换不变性
# 2. 共享MLP（1x1卷积）逐点提取特征
# 3. T-Net学习输入/特征对齐变换

class TactileFeedbackController:
    """基于触觉反馈的力控抓取（PID控制）"""

    def __init__(self, target_force=5.0, kp=0.01, ki=0.001, kd=0.002,
                 dt=0.01, integral_limit=100.0, derivative_filter_N=10):
        self.target_force = target_force  # 目标抓取力(N)
        self.kp = kp                      # 比例增益
        self.ki = ki                      # 积分增益
        self.kd = kd                      # 微分增益
        self.dt = dt                      # 控制周期(秒)
        self.integral_limit = integral_limit  # 积分限幅
        self.N = derivative_filter_N      # 微分滤波系数
        self.error_integral = 0.0
        self.prev_force = 0.0             # 上一次力测量值
        self.prev_derivative = 0.0        # 上一次滤波微分值

    def compute_grip_adjustment(self, current_force):
        """
        PID控制器：根据当前力反馈调整夹爪开合量
        - 比例项: 响应误差
        - 积分项: 消除稳态误差（含抗饱和限幅）
        - 微分项: 对测量值微分 + 一阶低通滤波抑制传感器噪声
        """
        error = self.target_force - current_force

        # 积分项（含抗饱和限幅）
        self.error_integral += error * self.dt
        # 限幅防积分饱和：当积分项超限时停止累积
        self.error_integral = max(-self.integral_limit,
                                  min(self.integral_limit, self.error_integral))

        # 微分项：对测量值微分（避免设定值突变冲击）+ 低通滤波
        raw_deriv = -(current_force - self.prev_force) / self.dt
        alpha = self.N * self.dt / (1.0 + self.N * self.dt)
        filtered_deriv = alpha * raw_deriv + (1 - alpha) * self.prev_derivative

        # PID输出
        adjustment = (self.kp * error
                     + self.ki * self.error_integral
                     + self.kd * filtered_deriv)

        # 更新状态
        self.prev_force = current_force
        self.prev_derivative = filtered_deriv

        return adjustment  # 正值=收紧，负值=松开

    def grasp_with_feedback(self, gripper, tactile_sensor, max_steps=100):
        """闭环力控抓取"""
        for step in range(max_steps):
            force = tactile_sensor.read_force()

            if abs(force - self.target_force) < 0.5:  # 容差0.5N
                print(f"抓取稳定: {force:.1f}N (目标{self.target_force}N)")
                return True

            adj = self.compute_grip_adjustment(force)
            gripper.move_relative(adj)

        return False

import numpy as np

class KalmanFilter:
    """简化版卡尔曼滤波，用于融合IMU和视觉定位"""

    def __init__(self, state_dim, obs_dim):
        self.x = np.zeros(state_dim)              # 状态向量
        self.P = np.eye(state_dim) * 1.0           # 状态协方差
        self.Q = np.eye(state_dim) * 0.01          # 过程噪声
        self.R = np.eye(obs_dim) * 0.1             # 观测噪声
        self.F = np.eye(state_dim)                 # 状态转移矩阵
        self.H = np.eye(obs_dim, state_dim)        # 观测矩阵

    def predict(self, u=None):
        """预测步骤（使用运动模型）"""
        self.x = self.F @ self.x
        if u is not None:
            self.x += u  # 控制输入
        self.P = self.F @ self.P @ self.F.T + self.Q

    def update(self, z):
        """更新步骤（使用传感器观测）"""
        y = z - self.H @ self.x                       # 观测残差
        S = self.H @ self.P @ self.H.T + self.R        # 残差协方差
        K = self.P @ self.H.T @ np.linalg.inv(S)       # 卡尔曼增益
        self.x = self.x + K @ y                         # 状态更新
        I = np.eye(len(self.x))
        self.P = (I - K @ self.H) @ self.P              # 协方差更新

# 使用示例：融合IMU加速度和视觉位置
kf = KalmanFilter(state_dim=6, obs_dim=3)  # [x,y,z,vx,vy,vz] 状态, [x,y,z] 观测

# 设置运动模型（匀速模型, dt=0.01s）
dt = 0.01
kf.F = np.array([
    [1, 0, 0, dt, 0, 0],
    [0, 1, 0, 0, dt, 0],
    [0, 0, 1, 0, 0, dt],
    [0, 0, 0, 1, 0, 0],
    [0, 0, 0, 0, 1, 0],
    [0, 0, 0, 0, 0, 1]
])
kf.H = np.eye(3, 6)  # 只观测位置

# 循环
for imu_accel, visual_position in sensor_stream:
    kf.predict(u=np.concatenate([np.zeros(3), imu_accel * dt]))
    if visual_position is not None:  # 视觉更新频率低于IMU
        kf.update(visual_position)
    fused_pose = kf.x[:3]  # 融合后的位置估计

# 机器人语言指令 → 视觉定位 → 分割
# 例：人说"把红色的杯子拿给我"

from groundingdino.util.inference import load_model, predict
from segment_anything import sam_model_registry, SamPredictor

class OpenVocabPerception:
    """开放词汇感知：理解任意文本描述的物体"""

    def __init__(self):
        self.grounding_model = load_model(
            "GroundingDINO_SwinB_cfg.py",
            "groundingdino_swinb_cogcoor.pth"
        )
        sam = sam_model_registry["vit_h"](
            checkpoint="sam_vit_h.pth"
        )
        self.sam_predictor = SamPredictor(sam)

    def detect_and_segment(self, image, text_prompt,
                           box_threshold=0.3, text_threshold=0.25):
        """
        image: RGB图像 (H, W, 3)
        text_prompt: 自然语言描述, 如 "red cup"
        返回: 检测框列表 + 分割mask列表
        """
        # Step 1: Grounding DINO 定位
        boxes, logits, phrases = predict(
            model=self.grounding_model,
            image=image,
            caption=text_prompt,
            box_threshold=box_threshold,
            text_threshold=text_threshold
        )

        # Step 2: SAM 精细分割
        self.sam_predictor.set_image(image)
        masks = []
        for box in boxes:
            mask, score, _ = self.sam_predictor.predict(
                box=box.numpy(),
                multimask_output=False
            )
            masks.append(mask[0])

        return boxes, masks, phrases

# 使用
perception = OpenVocabPerception()
boxes, masks, labels = perception.detect_and_segment(
    image=camera.capture(),
    text_prompt="red cup . blue bottle . white plate"
)
# → 机器人现在知道每个物体的精确位置和形状