基于结构光的视觉伺服系统实现方案

┌────────────────────────────────────────────────────────┐
│                   视觉伺服控制系统架构                   │
├─────────────┬──────────────┬────────────┬──────────────┤
│  视觉感知层  │  状态估计层   │  控制决策层  │  执行控制层   │
├─────────────┼──────────────┼────────────┼──────────────┤
│ 结构光相机   │ 设备位姿估计  │ PID控制器   │ 差速底盘      │
│ 点云处理     │ 机器人状态    │ 轨迹规划    │ 电机驱动      │
│ 设备识别     │ 坐标变换      │ 避障逻辑    │              │
└─────────────┴──────────────┴────────────┴──────────────┘

# 简化版设备识别算法（基于结构光点云特征）
class DeviceDetector:
    def __init__(self):
        # 设备的基本几何特征（简化假设）
        self.device_height_range = (0.4, 0.5)  # 设备高度范围(m)
        self.device_width_range = (0.35, 0.45)  # 设备宽度范围(m)

    def detect_device(self, point_cloud):
        """
        输入：结构光相机点云数据(N,3)
        输出：设备在相机坐标系中的位置和尺寸
        """
        # 步骤1：地面分割（最简单方法）
        # 假设地面是平面，取最低点作为地面参考
        ground_z = np.min(point_cloud[:, 2])
        ground_threshold = ground_z + 0.05  # 地面以上5cm

        # 步骤2：提取地面以上的物体
        above_ground = point_cloud[point_cloud[:, 2] > ground_threshold]

        if len(above_ground) == 0:
            return None

        # 步骤3：聚类（使用最简化的连通域分析）
        # 寻找最大簇（假设设备是视野中最大的物体）
        clusters = self.simple_clustering(above_ground)

        if not clusters:
            return None

        # 取最大的簇作为设备
        main_cluster = max(clusters, key=lambda x: len(x))

        # 步骤4：计算边界框
        min_pt = np.min(main_cluster, axis=0)
        max_pt = np.max(main_cluster, axis=0)

        # 计算中心点（相机坐标系下）
        center = (min_pt + max_pt) / 2

        # 计算尺寸
        size = max_pt - min_pt

        return {
            'center': center,  # [x, y, z]相机坐标系
            'size': size,      # [width, height, depth]
            'bbox': (min_pt, max_pt)
        }

    def simple_clustering(self, points, voxel_size=0.02):
        """最简单的聚类算法"""
        clusters = []
        visited = np.zeros(len(points), dtype=bool)

        for i in range(len(points)):
            if not visited[i]:
                # 种子点
                cluster = []
                stack = [i]

                while stack:
                    idx = stack.pop()
                    if not visited[idx]:
                        visited[idx] = True
                        cluster.append(points[idx])

                        # 查找邻居（简单欧氏距离）
                        for j in range(len(points)):
                            if not visited[j]:
                                dist = np.linalg.norm(points[idx] - points[j])
                                if dist < voxel_size * 2:
                                    stack.append(j)

                if len(cluster) > 10:  # 过滤掉小噪声
                    clusters.append(np.array(cluster))

        return clusters

#相机坐标系到机器人坐标系
class PoseEstimator:
    def __init__(self):
        # 相机安装参数（需测量或标定）
        # 假设相机安装在机器人正前方，高度0.3m，朝前
        self.camera_offset = np.array([0, 0, 0.3])  # 相机相对于机器人中心的偏移
        # 相机安装角度（俯仰角可能略微向下）
        self.camera_pitch = -0.1  # 弧度，略微向下看

    def camera_to_robot(self, camera_pos):
        """
        将相机坐标系下的点转换到机器人坐标系
        相机坐标系：X右，Y下，Z前
        机器人坐标系：X前，Y左，Z上
        """
        # 坐标轴变换
        robot_pos = np.array([
            camera_pos[2],  # 相机Z轴 -> 机器人X轴（前方）
            -camera_pos[0], # 相机X轴 -> 机器人Y轴（左方，取负）
            -camera_pos[1]  # 相机Y轴 -> 机器人Z轴（上方，取负）
        ])

        # 考虑相机安装角度（简单旋转）
        if self.camera_pitch != 0:
            # 绕Y轴旋转（俯仰）
            R = np.array([
                [np.cos(self.camera_pitch), 0, np.sin(self.camera_pitch)],
                [0, 1, 0],
                [-np.sin(self.camera_pitch), 0, np.cos(self.camera_pitch)]
            ])
            robot_pos = R @ robot_pos

        # 加上相机安装位置偏移
        robot_pos += self.camera_offset

        return robot_pos

    def calculate_relative_pose(self, device_center_cam):
        """
        计算机器人相对于设备的位姿
        返回：距离，角度偏差
        """
        # 转换到机器人坐标系
        device_center_robot = self.camera_to_robot(device_center_cam)

        # 计算水平距离（忽略高度）
        distance = np.sqrt(device_center_robot[0]**2 + device_center_robot[1]**2)

        # 计算角度偏差（机器人需要转动的角度）
        # atan2(y, x) 计算与正前方的夹角
        angle_error = np.arctan2(device_center_robot[1], device_center_robot[0])

        return {
            'distance': distance,
            'angle_error': angle_error,
            'device_pos_robot': device_center_robot
        }

#双PID控制器
#PID1：控制线速度v，输入误差为x_b - 0.5，输出为v
#PID2：控制角速度w，输入误差为y_b - 0，输出为w
class DeviceAlignmentController:
    def __init__(self):
        # 目标状态
        self.target_distance = 0.5  # 目标距离0.5m
        self.target_angle = 0.0     # 目标角度0度（正对）

        # 距离PID参数
        self.kp_dist = 0.8   # 比例系数
        self.ki_dist = 0.01  # 积分系数
        self.kd_dist = 0.1   # 微分系数
        self.dist_integral = 0
        self.prev_dist_error = 0

        # 角度PID参数
        self.kp_angle = 1.5   # 比例系数
        self.ki_angle = 0.02  # 积分系数
        self.kd_angle = 0.15  # 微分系数
        self.angle_integral = 0
        self.prev_angle_error = 0

        # 安全限制
        self.max_linear_speed = 0.3   # 最大线速度 m/s
        self.max_angular_speed = 0.5  # 最大角速度 rad/s
        self.min_safe_distance = 0.3  # 最小安全距离

    def compute_control(self, current_pose, dt=0.1):
        """
        基于当前位姿计算控制命令
        current_pose: {'distance': d, 'angle_error': theta}
        dt: 控制周期（秒）
        """
        # 1. 计算误差
        dist_error = current_pose['distance'] - self.target_distance
        angle_error = current_pose['angle_error']

        # 安全检查
        if current_pose['distance'] < self.min_safe_distance:
            # 太近了，先后退
            return self.emergency_stop()

        # 2. 距离PID控制（线速度）
        # 比例项
        p_dist = self.kp_dist * dist_error

        # 积分项（防止积分饱和）
        self.dist_integral += dist_error * dt
        self.dist_integral = np.clip(self.dist_integral, -1, 1)
        i_dist = self.ki_dist * self.dist_integral

        # 微分项
        d_dist = self.kd_dist * (dist_error - self.prev_dist_error) / dt
        self.prev_dist_error = dist_error

        # 计算线速度
        linear_vel = p_dist + i_dist + d_dist

        # 3. 角度PID控制（角速度）
        # 比例项
        p_angle = self.kp_angle * angle_error

        # 积分项
        self.angle_integral += angle_error * dt
        self.angle_integral = np.clip(self.angle_integral, -0.5, 0.5)
        i_angle = self.ki_angle * self.angle_integral

        # 微分项
        d_angle = self.kd_angle * (angle_error - self.prev_angle_error) / dt
        self.prev_angle_error = angle_error

        # 计算角速度
        angular_vel = p_angle + i_angle + d_angle

        # 4. 应用限幅
        linear_vel = np.clip(linear_vel, -self.max_linear_speed, self.max_linear_speed)
        angular_vel = np.clip(angular_vel, -self.max_angular_speed, self.max_angular_speed)

        # 5. 决策逻辑：当角度偏差大时，优先转向
        if abs(angle_error) > 0.3:  # 角度偏差大于17度
            # 主要转向，低速前进或停止
            linear_vel *= 0.2

        # 6. 接近目标时的减速
        if abs(dist_error) < 0.1:  # 距离目标10cm以内
            linear_vel *= 0.5

        if abs(angle_error) < 0.05:  # 角度接近
            angular_vel *= 0.3

        return {
            'linear_velocity': linear_vel,
            'angular_velocity': angular_vel,
            'distance_error': dist_error,
            'angle_error': angle_error
        }

    def emergency_stop(self):
        """紧急停止"""
        return {
            'linear_velocity': -0.1,  # 缓慢后退
            'angular_velocity': 0,
            'distance_error': 0,
            'angle_error': 0
        }
class AlignmentStateMachine:
    def __init__(self):
        self.state = "SEARCHING"  # 状态: SEARCHING, APPROACHING, ALIGNING, FINISHED
        self.stable_counter = 0

    def update_state(self, pose_data, control_output):
        """
        根据当前状态和误差更新状态
        """
        dist_error = abs(pose_data['distance'] - 0.5)
        angle_error = abs(pose_data['angle_error'])

        # 状态转移逻辑
        if self.state == "SEARCHING":
            if pose_data['distance'] > 0:
                self.state = "APPROACHING"

        elif self.state == "APPROACHING":
            if dist_error < 0.1 and angle_error < 0.1:
                self.stable_counter += 1
                if self.stable_counter > 5:  # 稳定5个周期
                    self.state = "FINISHED"
            else:
                self.stable_counter = 0

        elif self.state == "FINISHED":
            # 停止运动
            pass

        return self.state

#差速底盘控制
class DifferentialDriveController:
    def __init__(self):
        # 机器人参数
        self.wheel_radius = 0.05  # 轮子半径(m)
        self.wheel_base = 0.3     # 轮间距(m)

    def velocity_to_wheel_speeds(self, linear_vel, angular_vel):
        """
        将线速度和角速度转换为左右轮速度
        差速运动学公式：
        v_left = linear_vel - (angular_vel * wheel_base / 2)
        v_right = linear_vel + (angular_vel * wheel_base / 2)
        """
        v_left = linear_vel - (angular_vel * self.wheel_base / 2)
        v_right = linear_vel + (angular_vel * self.wheel_base / 2)

        # 转换为电机转速（RPM）
        # 线速度 m/s -> 轮子角速度 rad/s -> RPM
        wheel_rpm_left = (v_left / self.wheel_radius) * (60 / (2 * np.pi))
        wheel_rpm_right = (v_right / self.wheel_radius) * (60 / (2 * np.pi))

        return {
            'left_motor_rpm': wheel_rpm_left,
            'right_motor_rpm': wheel_rpm_right
        }

class DeviceAlignmentRobot:
    def __init__(self):
        # 初始化各模块
        self.detector = DeviceDetector()
        self.pose_estimator = PoseEstimator()
        self.controller = DeviceAlignmentController()
        self.drive_controller = DifferentialDriveController()
        self.state_machine = AlignmentStateMachine()

        # 运行参数
        self.control_rate = 10  # Hz
        self.running = True

    def run(self):
        """主控制循环"""
        import time

        while self.running:
            start_time = time.time()

            # 1. 获取点云数据（模拟接口）
            point_cloud = self.get_point_cloud()

            # 2. 检测设备
            device_data = self.detector.detect_device(point_cloud)

            if device_data is None:
                # 没检测到设备，原地缓慢旋转搜索
                self.search_mode()
                continue

            # 3. 计算相对位姿
            pose_data = self.pose_estimator.calculate_relative_pose(
                device_data['center']
            )

            # 4. 计算控制命令
            control_cmd = self.controller.compute_control(pose_data, dt=1/self.control_rate)

            # 5. 更新状态机
            state = self.state_machine.update_state(pose_data, control_cmd)

            # 6. 根据状态调整控制
            if state == "FINISHED":
                control_cmd['linear_velocity'] = 0
                control_cmd['angular_velocity'] = 0
                print("任务完成！已正对设备")
                self.running = False

            # 7. 转换为电机指令并执行
            motor_cmd = self.drive_controller.velocity_to_wheel_speeds(
                control_cmd['linear_velocity'],
                control_cmd['angular_velocity']
            )

            # 8. 发送控制指令（实际硬件接口）
            self.send_motor_command(motor_cmd)

            # 9. 打印调试信息
            self.print_debug_info(pose_data, control_cmd, state)

            # 10. 保持固定控制频率
            elapsed = time.time() - start_time
            sleep_time = max(0, 1/self.control_rate - elapsed)
            time.sleep(sleep_time)

    def search_mode(self):
        """搜索模式：原地缓慢旋转"""
        motor_cmd = self.drive_controller.velocity_to_wheel_speeds(
            0,  # 线速度为0
            0.2  # 缓慢旋转
        )
        self.send_motor_command(motor_cmd)

    def get_point_cloud(self):
        """获取点云数据（需要根据实际相机SDK实现）"""
        # 这里应该是实际相机接口调用
        # 返回形状为(N,3)的numpy数组
        pass

    def send_motor_command(self, motor_cmd):
        """发送电机指令（需要根据实际硬件实现）"""
        # 这里应该是实际电机控制接口
        pass

    def print_debug_info(self, pose, control, state):
        """打印调试信息"""
        print(f"状态: {state}")
        print(f"距离: {pose['distance']:.3f}m, 角度偏差: {np.degrees(pose['angle_error']):.1f}°")
        print(f"控制: v={control['linear_velocity']:.3f}m/s, ω={control['angular_velocity']:.3f}rad/s")
        print("-" * 50)