基于多传感器融合的循迹打击系统

一、作品概述

本系统设计了一套双核心协同工作的智能打击系统，通过任务解耦实现高效控制：

二、系统硬件架构

2.1 下位机部分（循迹小车）

2.2 上位机部分（云台系统）

三、下位机核心代码解析 (MSPM0)

下位机采用 C 语言开发，核心在于陀螺仪积分算法与状态机循迹逻辑。

3.1 陀螺仪姿态解算

利用 MPU6050 的 Z 轴角速度进行积分，计算偏航角 (Yaw)。增加了角度归一化和最小角度差计算，防止过零点跳变。

C / MSPM0// 计算从 start 到 current 的最小角度差 [-180, 180]
float angle_diff(float start, float current) {
    float diff = current - start;
    while (diff > 180.0f) diff -= 360.0f;
    while (diff < -180.0f) diff += 360.0f;
    return diff;
}

// 陀螺仪积分更新 yaw
void checkout() {
    int16_t accel[3], gyro[3];
    float temperature;
    mpu6050_read(gyro, accel, &temperature);

    int16_t gz_raw = gyro[2];
    // 500°/s 模式下灵敏度 65.5 LSB/(°/s)
    float gz_dps = ((float)gz_raw) / 65.5f;
    float dt = 0.01f; // 10ms 采样间隔

    yaw += gz_dps * dt;

    // 归一化到 [-180, 180]
    while (yaw > 180.0f) yaw -= 360.0f;
    while (yaw < -180.0f) yaw += 360.0f;
}

3.2 精准转弯控制 (tm2)

采用差速驱动实现原地旋转。当陀螺仪检测到转动角度达到 22.0f（根据齿轮比换算为车身 90° 或其他目标角度）时立即停止。

C / MSPM0void tm2() {
    delay_ms(30);
    checkout();
    yaw = 0.0f; // 清零积分，消除累积误差
    float yaw_start = yaw;
    delay_ms(30);
    
    // 差速转弯：左轮正转，右轮反转
    Set_Speed(0, 3000);  
    Set_Speed(1, -3000); 
    
    while (1) {
        checkout();
        float delta = angle_diff(yaw_start, yaw);
        
        if (fabsf(delta) >= 22.0f) { // 达到目标角度
            Set_Speed(0, 0);
            Set_Speed(1, 0);
            break;
        }
        delay_ms(10);
    }
    delay_ms(200); // 等待惯性停止
}

3.3 状态机循迹算法 (tm1)

基于 4 路红外传感器的状态组合，采用分级调速策略。偏离越远，修正速度差越大。

C / MSPM0void tm1() {
    IR_Module_Read();

    // 情况 1 & 2: 直行 (全灭或中间两路亮)
    if((ir_dh2 && ir_dh3 && !ir_dh1 && !ir_dh4) || (!ir_dh1 && !ir_dh2 && !ir_dh3 && !ir_dh4)) {
        Set_Speed(0, BASE_SPEED);
        Set_Speed(1, BASE_SPEED);
    }
    // 情况 3: 偏左 (左内亮) -> 右轮加速
    else if(ir_dh2 && !ir_dh3) {
        Set_Speed(0, BASE_SPEED);
        Set_Speed(1, TURN_SPEED);
    }
    // 情况 4: 偏右 (右内亮) -> 左轮加速
    else if(!ir_dh2 && ir_dh3) {
        Set_Speed(0, TURN_SPEED);
        Set_Speed(1, BASE_SPEED);
    }
    // 情况 5: 极左 (最左亮) -> 大角度右转
    else if(ir_dh4 && !ir_dh1 && !ir_dh2 && !ir_dh3) {
        Set_Speed(0, TURN_SPEED / 2);
        Set_Speed(1, BASE_SPEED * 2);
    }
    // ... 其他情况兜底直行
    else {
        Set_Speed(0, BASE_SPEED);
        Set_Speed(1, BASE_SPEED);
    }
}

四、上位机核心代码解析 (K230)

上位机运行 Python 脚本，利用 K230 的图像处理能力进行靶心识别，并通过查表法控制云台。

4.1 视觉几何解算

基于小孔成像模型，将像素坐标偏移量转换为实际的角度偏移量，并根据靶心在图像中的高度估算距离。

Python / K230def get_angle_offset(cx, cy):
    """计算靶心相对于画面中心的偏移角度"""
    center_x = FRAME_W / 2
    center_y = FRAME_H / 2
    dx = cx - center_x
    dy = cy - center_y
    
    # 利用焦距参数 F_H/F_V 进行反正切计算
    angle_h = math.atan(dx / F_H)
    angle_v = -math.atan(dy / F_V) # Y 轴翻转校正
    
    return math.degrees(angle_h), math.degrees(angle_v)

def estimate_distance(blob_height_pixels):
    """通过靶心高度估算距离 (mm)"""
    if blob_height_pixels == 0:
        return None
    return (REAL_HEIGHT_MM * F_V) / blob_height_pixels

4.2 智能查表控制算法

为了简化调试并提高响应速度，采用查表法替代传统 PID。根据水平和垂直角度的不同区间，返回不同的舵机调整步长。

Python / K230def compute_change_h(angle_h, angle_v):
    """根据角度偏差查表获取水平修正量"""
    ah = abs(angle_h)
    
    # 区域 1: 垂直角度较小 (目标较近或较低)
    if angle_v < 13:
        if angle_h > 0: # 偏右
            if ah > 25: return 11
            elif ah > 10: return 5
            else: return 3
        else: # 偏左
            if ah > 20: return 2
            else: return 3
            
    # 区域 2: 垂直角度较大 (目标较远或较高)
    else:
        if ah > 25: return 13 # 大偏差快速修正
        elif ah > 10: return 5
        else: return 3
    
    return 0

4.3 主循环与靶心筛选

通过密度阈值 (`density`) 筛选出空心的环形靶心，排除实心干扰物。

Python / K230while True:
    img = sensor.snapshot()
    
    if not adjusted:
        # 寻找黑色色块
        blobs = img.find_blobs([black_tape_threshold], 
                               area_threshold=200, 
                               merge=True)
        if blobs:
            # 关键：筛选空心靶心 (密度小于阈值)
            hollow_blobs = [b for b in blobs if b.density() < DENSITY_THRESHOLD]
            
            if hollow_blobs:
                # 选择面积最大的目标
                max_blob = max(hollow_blobs, key=lambda b: b.pixels())
                bx, by = max_blob.cx(), max_blob.cy()
                
                # 解算角度并控制舵机
                angle_h, angle_v = get_angle_offset(bx, by)
                move_servo(angle_h, angle_v, max_blob.h())
                
                adjusted = True
                done_pin.value(1) # 发送完成信号给下位机
    
    time.sleep(0.005)

五、系统联调与测试结果

5.1 成品展示

5.2 问题与解决方案

• 陀螺仪漂移：通过在每次转弯动作前强制清零积分 (`yaw=0`)，只计算相对角度，有效消除了累积误差。
• 红外误判：增加软件滤波逻辑，连续采样三次取中值，避免瞬间干扰导致小车失控。
• 识别速度慢：将 K230 摄像头分辨率降至 640x360，并优化 Blob 查找参数，帧率提升至 100FPS+。
• 舵机抖动：在 PWM 输出时增加死区补偿，并在目标附近减小调整步长，实现平滑锁定。

六、总结

本系统成功实现了双核协同的智能打击任务。MSPM0 保证了底层运动的实时性与稳定性，K230 发挥了强大的边缘计算能力进行视觉处理。两者协同工作，架构清晰，调试方便。

未来改进方向

• 引入PID 算法进一步优化循迹平滑度和云台追踪的细腻度。
• 将四路红外传感器换成八路传感器，检测将更精准
• 移植FreeRTOS到下位机，实现多任务并行处理（如同时处理蓝牙通信）。
• 在 K230 上部署轻量级深度学习模型 (如 YOLO-Fast)，提高复杂背景下的靶心识别率。
• 增加 WiFi/ESP8266 模块，实现手机 APP 远程监控图传画面。