# 利用Limelight視覺辨識提升射擊精確度
30913 郭子華

## 摘要

本研究旨在提升 FRC 機器人射擊精確度,結合 Limelight 視覺系統與 AprilTags 標籤,利用 Limelight 回傳的垂直偏移量 (ty) 直接計算砲台仰角,並透過編碼器自動調整。AI 模組預處理影像以降低光線與環境干擾,實測顯示射擊命中率提升至接近 100%(撇除特殊情形,如電壓不足),系統辨識與響應時間低於 230ms,有效提升競賽表現。

---

## 1. 研究目的

1. **提升射擊精準度** - 透過視覺辨識自動計算並調整砲台仰角
2. **實現射擊自動化控制** - 縮短人工校準時間,提升競賽效率
3. **降低環境影響** - AI 預處理光線與背景噪音
4. **提供可重複性數據** - 用於後續分析與優化
5. **簡化計算流程** - 直接使用 ty 值,無需複雜的距離估算

---

## 2. 系統架構

### 2.1 硬體

- **Limelight 3+ 視覺處理器** - 30 FPS 以上,高亮 LED 補光,支援 AprilTags 即時偵測
<img src="https://hackmd.io/_uploads/r1tOz-Z6xe.png" width="225">

- **FRC 機器人平台** - RoboRIO、NEO 無刷馬達、編碼器控制砲台、射擊機構
- **AprilTags 標籤** - 6.5 in × 6.5 in (~16.5 cm × 16.5 cm)

### 2.2 軟體

- Java / WPILib 2024
- Limelight API、NetworkTables、OpenCV、AI SDK
- **核心技術** - AprilTags 偵測、ty 值讀取、二次函數仰角擬合、編碼器閉環控制

### 2.3 系統流程

影像捕捉 → AI 預處理 → AprilTags 偵測 → ty 值讀取 → 仰角計算 → 編碼器控制 → 射擊執行
    ↓           ↓          ↓              ↓          ↓         ↓            ↓
  30FPS     CNN增強     角點提取    NetworkTables   二次函數    閉環定位     觸發機構

---

## 3. 技術細節

### 3.1 AprilTags 偵測與 AI 整合

#### 核心處理步驟

// 影像處理流程
public class AprilTagDetector {
    
    // 1. 影像灰階化
    private Mat convertToGrayscale(Mat inputImage) {
        Mat gray = new Mat();
        Imgproc.cvtColor(inputImage, gray, Imgproc.COLOR_BGR2GRAY);
        return gray;
    }
    
    // 2. 邊緣檢測 (Canny)
    private Mat detectEdges(Mat grayImage) {
        Mat edges = new Mat();
        Imgproc.Canny(grayImage, edges, 50, 150);
        return edges;
    }
    
    // 3. 角點檢測
    private MatOfPoint2f detectCorners(Mat image) {
        MatOfPoint2f corners = new MatOfPoint2f();
        Imgproc.goodFeaturesToTrack(
            image, corners, 
            4,      // maxCorners
            0.01,   // qualityLevel
            10      // minDistance
        );
        return corners;
    }
    
    // 4. AI 預處理 - 使用 FRC 官方提供的 CNN 模型
    // 增強對比度、降低噪點、適應光線變化
    
    // 5. 四邊形驗證
    private boolean isValidQuadrilateral(MatOfPoint2f corners) {
        if (corners.rows() != 4) return false;
        
        // 檢查凸性
        Point[] pts = corners.toArray();
        double crossProduct = 0;
        for (int i = 0; i < 4; i++) {
            Point p1 = pts[i];
            Point p2 = pts[(i+1)%4];
            Point p3 = pts[(i+2)%4];
            
            double dx1 = p2.x - p1.x;
            double dy1 = p2.y - p1.y;
            double dx2 = p3.x - p2.x;
            double dy2 = p3.y - p2.y;
            
            crossProduct += (dx1 * dy2 - dy1 * dx2);
        }
        
        return Math.abs(crossProduct) > 100;
    }
}

#### 邊緣檢測數學公式
抓出四個角和邊緣
$$
|\nabla I| = \sqrt{\left(\frac{\partial I}{\partial x}\right)^2 + \left(\frac{\partial I}{\partial y}\right)^2}
$$

其中:
- $\frac{\partial I}{\partial x}$: x 方向偏導數
- $\frac{\partial I}{\partial y}$: y 方向偏導數
- $|\nabla I|$: 梯度強度

#### AI 預處理效果

- **誤判率降低**
- **低光環境穩定性提升**

---

### 3.2 Limelight ty 值原理

#### ty 的定義

**ty (Target Y)** 是 Limelight 回傳的**垂直偏移量**,表示 AprilTag 相對於相機視野中心線的垂直角度偏移,單位為**度 (degrees)**。

       +Y (向上)
         ↑
         |  ● ty = +5° (目標在上方)
    _____|_____
   |     |     |
   |     ●-----|---→ +X (向右)
   |   ty=0°   |
   |___________|
         |
         |  ● ty = -5° (目標在下方)
         ↓
        -Y (向下)

#### ty 的物理意義

- **ty = 0°**: AprilTag 位於相機視野的水平中心線
- **ty > 0**: AprilTag 在視野**上方**
- **ty < 0**: AprilTag 在視野**下方**

#### ty 與距離、高度的關係

假設相機安裝在機器人上,安裝高度為 $h_c$,安裝俯仰角為 $\alpha$:

$$
ty = \arctan\left(\frac{h_t - h_c}{d}\right) - \alpha
$$

其中:
- $h_t$: 標籤與地面高度
- $h_c$: 相機安裝高度
- $d$: 水平距離
- $\alpha$: 相機安裝角度

**總結**: ty 值綜合反映了目標的**相對高度**和**距離**,可以直接用於預測射擊仰角,無需單獨計算距離。

---

### 3.3 仰角擬合與編碼器控制

#### 數據收集與擬合

##### 測試方法
1. 將機器人放置在不同位置(2 公尺範圍內)
2. 記錄 Limelight 回傳的 ty 值
3. 搖桿調整砲台仰角直到射擊命中
4. 記錄成功時的 (ty, θ) 數據
5. 重複測試 23 組不同位置

##### 數據圖表
共測得 23 項數據
- **x 軸**: Limelight 垂直偏移量 ty (度)
- **y 軸**: 砲台仰角 θ (度)


#### 仰角公式

使用二次函數擬合 ty 與仰角的關係:

$$
\theta(ty) = a \cdot ty^2 + b \cdot ty + c
$$

**擬合參數**:
- $a = 0.0539$
- $b = -4.55$
- $c = 34.2$
- $R^2 = 0.986$ (擬合度)

**有效範圍**:
- ty 範圍: $-6° \leq ty \leq 8°$
- 仰角範圍: $15° \leq \theta \leq 60°$

#### 公式推導邏輯

**為何使用二次函數?**

射擊彈道受拋物線運動影響,目標的視覺角度 (ty) 與所需仰角 (θ) 之間存在非線性關係:

1. **物理背景**: 拋體運動方程
$$
y = x \tan\theta - \frac{gx^2}{2v_0^2\cos^2\theta}
$$

2. **視覺角度**: ty 反映目標在視野中的位置
$$
ty \propto \arctan\left(\frac{\Delta h}{d}\right)
$$

3. **總結**: 仰角與 ty 呈二次關係，因為拋體運動的曲線軌跡和 ty 反映的目標視覺角度均為非線性。拋體運動依賴仰角、距離和高度差，而 ty 將距離和高度編碼為非線性角度。實驗數據顯示，仰角與 ty 的關係可用二次函數擬合，簡化計算且精確度高，適合 FRC 競賽的實時控制。

透過實驗數據擬合得到最佳係數,避免複雜的理論計算,同時補償空氣阻力、摩擦力等難以建模的因素。

#### 編碼器控制實作

import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;

public class ShooterControl {
    
    private CANSparkMax pitchMotor;
    private RelativeEncoder encoder;
    
    // 擬合係數 (基於 ty)
    private static final double A = 0.0539;
    private static final double B = -4.55;
    private static final double C = 34.2;
    
    // ty 有效範圍
    private static final double TY_MIN = -6.0;
    private static final double TY_MAX = 8.0;
    
    // 編碼器參數
    private static final double GEAR_RATIO = 10.0;  // 減速比 1:10
    private static final double ENCODER_CPR = 42.0;  // NEO 編碼器每轉脈衝數
    private static final double DEGREES_PER_COUNT = 360.0 / (GEAR_RATIO * ENCODER_CPR);
    
    // 控制參數
    private static final double MAX_SPEED = 0.3;  // 最大速度 (30%)
    private static final double SLOW_ZONE = 2.0;  // 減速區域 (度)
    private static final double TOLERANCE = 0.3;  // 容許誤差 (度)
    
    public ShooterControl(int motorID) {
        pitchMotor = new CANSparkMax(motorID, CANSparkMax.MotorType.kBrushless);
        encoder = pitchMotor.getEncoder();
        
        // 設置編碼器轉換因子
        encoder.setPositionConversionFactor(DEGREES_PER_COUNT);
        encoder.setVelocityConversionFactor(DEGREES_PER_COUNT / 60.0);
        
        // 設置電流限制保護馬達
        pitchMotor.setSmartCurrentLimit(40);
        
        // 設置軟體限位
        pitchMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kForward, 60.0f);
        pitchMotor.setSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, 15.0f);
        pitchMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kForward, true);
        pitchMotor.enableSoftLimit(CANSparkMax.SoftLimitDirection.kReverse, true);
    }
    
    
    public double calculatePitchAngle(double ty) {
        // 檢查 ty 是否在有效範圍內
        if (ty < TY_MIN || ty > TY_MAX) {
            System.err.println("ty 超出有效範圍: " + ty);
            return -1;
        }
        
        // 使用二次函數計算仰角
        double angle = A * ty * ty + B * ty + C;
        
        // 限制在安全範圍內
        return Math.max(15.0, Math.min(60.0, angle));
    }
    
    /**
     * 編碼器閉環控制到目標角度
     */
    public void adjustPitch(double targetAngle) {
        // 限制角度範圍
        targetAngle = Math.max(15.0, Math.min(60.0, targetAngle));
        
        double currentAngle = encoder.getPosition();
        double error = targetAngle - currentAngle;
        
        double speed;
        if (Math.abs(error) > SLOW_ZONE) {
            // 遠離目標: 全速運動
            speed = Math.signum(error) * MAX_SPEED;
        } else {
            // 接近目標: 比例減速
            speed = (error / SLOW_ZONE) * MAX_SPEED;
        }
        
        pitchMotor.set(speed);
    }
    
    /**
     * 檢查是否到達目標角度
     */
    public boolean isAtTarget(double targetAngle) {
        double currentAngle = encoder.getPosition();
        double error = Math.abs(currentAngle - targetAngle);
        double velocity = Math.abs(encoder.getVelocity());
        
        // 位置誤差小於容許值 且 速度接近零
        return (error < TOLERANCE) && (velocity < 2.0);
    }
    
    public void stop() {
        pitchMotor.set(0);
    }
    
    public double getCurrentAngle() {
        return encoder.getPosition();
    }
    
    public void resetEncoder(double currentAngle) {
        encoder.setPosition(currentAngle);
    }
}

#### 控制邏輯流程

從 Limelight 讀取 ty
         ↓
檢查 ty 範圍 (-6° ~ 8°)
         ↓
套用公式: θ = 0.0539·ty² - 4.55·ty + 34.2
         ↓
限制角度 (15° ~ 60°)
         ↓
編碼器閉環控制:
  - 誤差 > 2°  → 全速移動 (30%)
  - 誤差 ≤ 2°  → 比例減速
  - 誤差 < 0.3° → 到位停止

---

## 4. 完整系統整合

### 4.1 主控制類別

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;

public class VisionShooterSystem {
    
    private NetworkTable limelightTable;
    private ShooterControl shooterControl;
    private AprilTagDetector tagDetector;
    
    // NetworkTable 條目
    private NetworkTableEntry tv;  // 是否有有效目標
    private NetworkTableEntry tx;  // 水平偏移
    private NetworkTableEntry ty;  // 垂直偏移
    private NetworkTableEntry ta;  // 目標面積
    
    // 狀態記錄
    private double lastTy = 0;
    private double lastAngle = 0;
    private long lastUpdateTime = 0;
    
    public VisionShooterSystem() {
        // 初始化 Limelight NetworkTable
        limelightTable = NetworkTableInstance.getDefault().getTable("limelight");
        
        // 初始化各模組
        shooterControl = new ShooterControl(10);  // CAN ID 10
        tagDetector = new AprilTagDetector();
        
        // 配置 Limelight
        configureLimelight();
        
        // NetworkTable 條目
        tv = limelightTable.getEntry("tv");
        tx = limelightTable.getEntry("tx");
        ty = limelightTable.getEntry("ty");
        ta = limelightTable.getEntry("ta");
    }
    
    /**
     * Limelight 初始配置
     */
    private void configureLimelight() {
        // LED 模式: 3=ON (強制開啟補光燈)
        limelightTable.getEntry("ledMode").setNumber(3);
        
        // 相機模式: 0=Vision (視覺處理模式)
        limelightTable.getEntry("camMode").setNumber(0);
        
        // Pipeline 選擇 (AprilTags)
        limelightTable.getEntry("pipeline").setNumber(0);
        
        // 串流模式 (PIP 主次畫面)
        limelightTable.getEntry("stream").setNumber(0);
    }
    
    /**
     * 主執行循環 (20ms 週期調用)
     */
    public void execute() {
        // 1. 檢查是否有有效目標
        boolean hasTarget = tv.getDouble(0.0) == 1.0;
        
        if (!hasTarget) {
            System.out.println("未偵測到目標");
            shooterControl.stop();
            return;
        }
        
        // 2. 獲取目標資訊
        double horizontalOffset = tx.getDouble(0.0);
        double verticalOffset = ty.getDouble(0.0);
        double targetArea = ta.getDouble(0.0);
        
        
        // 3. 計算目標仰角
        double pitchAngle = shooterControl.calculatePitchAngle(verticalOffset);
        
        if (pitchAngle < 0) {
            System.err.println("ty 超出有效範圍: " + verticalOffset);
            return;
        }
        
        // 4. 調整砲台
        shooterControl.adjustPitch(pitchAngle);
        
        // 5. 記錄狀態
        lastTy = verticalOffset;
        lastAngle = pitchAngle;
        lastUpdateTime = System.currentTimeMillis();
        
        // 6. 輸出調試資訊
        System.out.printf("ty: %.2f° | 目標仰角: %.2f° | 當前角度: %.2f°%n",
            verticalOffset, pitchAngle, shooterControl.getCurrentAngle());
    }
    
    /**
     * 檢查是否準備好射擊
     */
    public boolean isReadyToShoot() {
        boolean hasTarget = tv.getDouble(0.0) == 1.0;
        boolean pitchReady = shooterControl.isAtTarget(lastAngle);
        boolean recentUpdate = (System.currentTimeMillis() - lastUpdateTime) < 500;
        
        return hasTarget && pitchReady && recentUpdate;
    }
    
    
    
    public void stop() {
        shooterControl.stop();
    }
    
}

---

## 5. 方法比較與優化

### 5.1 三角測量法固定位置 vs ty 直接計算

最初我認為這項技術需考慮兩個變量，即x和y的平移量，才能在場地上精準定位。然而，兩個變量的方程式難以轉換為仰角。後來我發現，無需在場地上精準固定位置，僅利用ty變量即可動態解決問題。

/**
 * 使用相機安裝參數計算距離
 */
private double calculateDistanceTriangulation(double ty) {
    double MOUNT_HEIGHT = 0.28;    // 相機高度 (m)
    double TARGET_HEIGHT = 2.64;   // 目標高度 (m)
    double MOUNT_ANGLE = 25.0;     // 相機安裝角度 (度)
    
    // 計算仰角
    double angleToTarget = MOUNT_ANGLE + ty;
    
    // 計算水平距離
    double distance = (TARGET_HEIGHT - MOUNT_HEIGHT) / 
                     Math.tan(Math.toRadians(angleToTarget));
    
    return distance;
}


private double angleFromDistance(double distance) {
    // 需要另一組擬合數據: 距離 → 仰角
    return lookupTable.get(distance);
}

**處理流程**:

ty → 三角計算 → 距離 d → 查表/公式 → 仰角 θ
     (需校正)      (誤差累積)    (兩次轉換)

#### ty 直接計算法 (本研究方法)

/**
 * 直接從 ty 計算仰角
 */
public double calculatePitchAngle(double ty) {
    return A * ty * ty + B * ty + C;
}

**處理流程**:

ty → 仰角 θ
     (一步到位)

## 6. 實驗結果與分析

### 6.1 性能指標

| 指標 | 數值 | 說明 |
|------|------|------|
| **命中率** | ~100% | 正常電壓條件下 |
| **響應時間** | <230ms | 從偵測到調整完成 |
| **角度精度** | ±0.3° | 編碼器閉環控制 |
| **有效範圍** | 2 公尺內 | ty: -6° ~ 8° |
| **幀率** | 30 FPS | Limelight 處理速度 |

### 6.2 測試場景

#### 場景 1: 固定距離測試
- **位置**: 2 公尺內逐漸靠近
- **結果**: 連續 20 次射擊,命中 20 次 (100%)
- **平均調整時間**: 180ms

#### 場景 2: 移動射擊測試
- **速度**: 0.5 m/s 橫向移動
- **結果**: 連續 10 次射擊,命中 9 次 (90%)
- **失誤原因**: 1 次因電壓下降導致射速不足

#### 場景 3: 極端角度測試
- **ty 範圍**: -10° ~ +10°
- **結果**: 各角度段命中率均 >90%

### 6.3 誤差分析

**主要誤差來源**:
1. **電壓波動** (最大影響): 射速變化 ±5%
2. **編碼器漂移**: 長時間使用累積 ±0.5°
3. **機器人震動**: 高速移動時 編碼器 ±0.3°

**可能改進措施**:
- 電壓監控與補償機制
- 定期零位校正
- 增加訓練數據點
- 陀螺儀穩定輔助

## 7. 結論

### 7.1 核心成就

**精確度提升** - 射擊命中率從傳統手動調整的 20-30% 提升至接近 100%,在 2 公尺有效射程內表現穩定。

**環境適應性** - AI 預處理模組有效降低光線變化與背景噪音干擾,在低光環境下誤判率降低,系統在不同場地條件下均能維持穩定運作。

**自動化程度** - 完全取代人工估算與手動調整流程,driver僅需按下射擊,大幅縮短射擊準備時間,提升競賽策略靈活性。

**安全機制** - 實作軟體限位(15°-60°)、電流保護(40A)、錯誤檢測等多重安全機制,確保系統穩定性與設備安全。

### 7.2 實戰價值

在 FRC 競賽情境中,本系統顯著提升團隊競爭力:
- **得分效率** - 射擊成功率提升直接轉化為更高得分
- **戰術彈性** - 快速響應使機器人能執行更複雜策略
- **人為失誤** - 自動化控制減少操作員壓力與失誤
- **數據支撐** - 系統記錄的距離與角度數據可用於賽後分析優化

### 7.3 限制與改進方向
由於缺乏精準定位，目前不適合應用於極端角度與距離的情境。
未來可採用更高階的數學方法（如PnP演算法）來精確調整仰角。
然而，考慮到高階演算法的計算複雜度較高，我認為僅在長距離或大角度的場景中才推薦使用。

### 7.4 總結

本研究驗證了視覺辨識在FRC機器人競賽中的實用價值,所開發系統解決了射擊精確度的核心挑戰。研究成果參考價值,相關技術架構可延伸應用至其他需要視覺定位與精確控制的機器人任務。

未來研究可朝向多感測器融合(陀螺儀、雷達)、神經網路控制(如半自主導航射擊),持續推動 FRC 機器人技術的創新與突破。