基于视觉反馈的实时闭环控制系统

┌────────────────────────────────────────────────────────────┐
│                   视觉-运动闭环控制系统                      │
├─────────────┬─────────────┬─────────────┬─────────────┤
│  视觉感知层  │  状态估计层  │  运动规划层  │  运动控制层  │
│  (Vision)   │ (Estimation)│ (Planning)  │ (Control)   │
└──────┬──────┴──────┬──────┴──────┬──────┴──────┬──────┘
       │             │             │             │
   结构光相机     滤波与状态     轨迹生成器    差速控制器
       │             │             │             │
   客户检测       误差计算       速度规划       电机驱动
       │             │             │             │
   位姿估计       预测更新      避撞检查       执行反馈
└────────────────────────────────────────────────────────────┘

输入：结构光相机点云数据
输出：客户中心点位置、法线方向、边界框

# 特征检测伪代码
def detect_toilet(point_cloud):
    # 1. 基于先验尺寸的边界框检测
    # 客户典型尺寸：长40-50cm，宽35-40cm，高70-75cm
    clusters = euclidean_clustering(point_cloud, 
                                    min_size=1000, 
                                    max_size=5000)

    # 2. 几何特征匹配
    for cluster in clusters:
        # 检查是否为长方体形状（客户水箱+底座）
        bounding_box = get_oriented_bounding_box(cluster)

        # 检查尺寸是否符合客户先验
        if validate_toilet_dimensions(bounding_box):
            # 3. 检测客户圈（圆环/椭圆特征）
            toilet_seat = detect_circular_features(cluster)

            # 4. 计算客户正面方向（水箱在背面）
            front_direction = determine_front_direction(bounding_box)

            return ToiletPose(center, front_direction, bounding_box)

X = [x, y, θ, v, ω]ᵀ
其中：
- (x,y)：底盘在局部坐标系中的位置
- θ：底盘朝向（相对于初始朝向）
- v：线速度
- ω：角速度

Z = [d, φ, α]ᵀ
其中：
- d：相机到客户中心的距离
- φ：客户中心在图像中的水平偏角
- α：客户朝向与相机朝向的夹角

class ToiletTrackingEKF:
    def predict(self, dt, v_cmd, ω_cmd):
        """运动模型预测"""
        # 差速运动模型
        if ω_cmd != 0:
            x_new = x + (v_cmd/ω_cmd)*(sin(θ + ω_cmd*dt) - sin(θ))
            y_new = y + (v_cmd/ω_cmd)*(cos(θ) - cos(θ + ω_cmd*dt))
            θ_new = θ + ω_cmd*dt
        else:
            x_new = x + v_cmd*dt*cos(θ)
            y_new = y + v_cmd*dt*sin(θ)
            θ_new = θ

    def update(self, z_vision):
        """视觉观测更新"""
        # 观测方程：相机看到的客户相对位姿
        H = compute_jacobian()  # 观测矩阵
        K = P * Hᵀ * (H * P * Hᵀ + R)⁻¹  # 卡尔曼增益
        X = X + K * (z_vision - h(X))  # 状态更新

def compute_potential_field(toilet_pose, robot_pose):
    # 1. 目标吸引力（客户位置）
    F_att = k_att * (goal_position - robot_position)

    # 2. 朝向吸引力（对齐客户正面）
    F_orientation = k_ori * (toilet_front_direction - robot_direction)

    # 3. 距离势场（保持0.5米距离）
    distance = norm(robot_position - toilet_position)
    if distance < 0.5:
        F_distance = k_dist * (0.5 - distance) * unit_vector_away
    else:
        F_distance = k_dist * (distance - 0.5) * unit_vector_toward

    # 4. 平滑约束（防止突变）
    F_smooth = k_smooth * (previous_velocity - current_velocity)

    return F_total = F_att + F_orientation + F_distance + F_smooth

def compute_velocity_commands(F_total, current_state):
    """
    将势场力转换为速度指令
    """
    # 线速度：与前进方向力分量成正比
    v = min(v_max, k_v * dot(F_total, robot_front_direction))

    # 角速度：与侧向力分量成正比
    ω = k_ω * cross(F_total, robot_front_direction)

    # 距离调节：接近目标时减速
    if distance_to_goal < 1.0:
        v = v * (distance_to_goal / 1.0)

    # 朝向调节：最终对正阶段
    if distance_to_goal < 0.7:
        angle_error = angle_between(robot_front, toilet_front)
        ω = ω + k_align * angle_error

    return v, ω

class DifferentialController:
    def __init__(self):
        self.v_pid = PID(kp=0.5, ki=0.01, kd=0.1)
        self.ω_pid = PID(kp=1.0, ki=0.05, kd=0.2)

    def execute(self, v_target, ω_target):
        # 当前状态反馈
        v_current, ω_current = get_wheel_encoders()

        # PID控制
        v_cmd = self.v_pid.update(v_target, v_current)
        ω_cmd = self.ω_pid.update(ω_target, ω_current)

        # 转换为轮速
        wheel_left = (v_cmd - (ω_cmd * wheelbase/2)) / wheel_radius
        wheel_right = (v_cmd + (ω_cmd * wheelbase/2)) / wheel_radius

        # 发送给电机驱动器
        set_wheel_velocities(wheel_left, wheel_right)

策略：快速转向客户方向
v = 0.3 m/s * (distance/1.0)  # 按比例减速
ω = k_ω * φ  # φ为水平偏角

策略：同时调整位置和朝向
v = 0.2 * (distance - 0.5)  # 向0.5米收敛
ω = k1 * φ + k2 * α  # 综合偏角和对齐角

策略：微调朝向，速度归零
v = 0.1 * (distance - 0.5)
ω = k_align * (α + β)  # α:朝向误差，β:位置补偿角
停止条件：|distance-0.5|<0.02m且|α|<2°

def smooth_control_loop():
    # 1. 视觉数据低通滤波
    toilet_pose = low_pass_filter(raw_vision_pose, α=0.3)

    # 2. 死区处理（避免在目标附近振荡）
    if abs(distance - 0.5) < 0.02 and abs(angle_error) < 2:
        return 0, 0  # 停止

    # 3. 速度限制
    v_cmd = apply_rate_limit(v_cmd, max_accel=0.5)
    ω_cmd = apply_rate_limit(ω_cmd, max_α_accel=1.0)

    # 4. 前瞻控制（基于运动模型预测）
    predicted_pose = motion_model_predict(current_pose, v_cmd, ω_cmd, dt=0.1)
    if check_collision(predicted_pose, toilet_pose):
        adjust_commands_to_avoid()

# 视觉处理参数
VOXEL_SIZE = 0.01  # 降采样体素大小
MIN_TOILET_POINTS = 1000  # 最小点云数量

# 控制参数
K_ATT = 1.0  # 吸引力增益
K_ORI = 0.8  # 朝向对齐增益
K_DIST = 1.2  # 距离保持增益

# PID参数
V_PID = [0.5, 0.01, 0.1]  # [Kp, Ki, Kd] 线速度
W_PID = [1.0, 0.05, 0.2]   # [Kp, Ki, Kd] 角速度

# 运动限制
V_MAX = 0.5  # m/s
W_MAX = 1.0  # rad/s
MAX_ACCEL = 0.5  # m/s²

def adaptive_parameter_tuning(distance, angle_error):
    """
    根据距离和误差自适应调整控制参数
    """
    # 距离越近，控制越精细
    distance_factor = min(1.0, distance / 0.5)

    # 动态调整增益
    k_v = 0.3 + 0.7 * distance_factor  # 远距离增益大
    k_ω = 0.5 + 1.5 * (1 - distance_factor)  # 近距离转向灵敏

    # 根据误差调整响应速度
    if abs(angle_error) > 30:  # 大误差时快速响应
        k_ω *= 2.0
        k_v *= 0.5  # 大角度偏差时减速