Limelight

Posted on Edited on In

基于limelight 3A的摄像头追踪

\"Whole Car\"

1.拆解任务

开始进行这次任务的同学想必已经完成了基于底盘(odometry
computer)的摄像头锁定，这次的任务与odometry computer
不同的是这次需要使用到limelight摄像头来进行追踪。

相比于只使用底盘来进行锁定，limelight摄像头对于追踪的tag的更换更加方便和灵活，因此这种方式优于底盘驱动的锁定。

所以如何实现这个锁定呢？

我们可以把它拆成几步：
首先，我们需要解决摄像头的问题，需要通过读取摄像头的反馈来确定我们的视觉中心距离摄像头识别到的apriltag有多远（横向）。
第二，我们需要处理这个摄像头的信息过滤，确保它只会锁定/反馈给我们需要的apriltag的数据而不是同时锁定很多个不同的april tag的数据。
第三，我们需要让电机转一个距离，让我们的摄像头能够成功的将apriltag放到视角中心周边。（这个时候需要提到一个死区的概念，即为apriltag距离视角中心的可接受距离，以避免摄像头频繁转动或者抖动）
第四，我们需要考虑当摄像头识别不到目标aprilcode的时候让电机不断旋转来寻找april code 的位置。

好了，其实这个就是这么简单，就让我们开始吧。

2.Limelight的基本代码与使用方式

要完成任务，我们需要了解如何从limelight中获取数据。
这个分为两个部分，第一部分是如何从硬件上（limelight本身）设置过滤器和分辨率以及管道。第二部分则是limelight在AS上的代码实现。

2.0 limelight与电机的链接

固定好limelight摄像头后，使用一根BtoC线连接Limelight的C口与Controll
Hub的任意一个USB口。
\"LimelightConnect\"
对于电机，参考麦轮底盘教程的电机链接与configuration 设置
\"CameraMotorLine1\"
\"CameraMotorLine2\"

2.1 limelight的硬件

现在你可以开始设置limelight的硬件了！
使用一根数据线连接limelight摄像头的C口与你的电脑，随后进入这个http://limelight.local:5801网址，以设置你的limelight。
进入之后，选择一个你喜欢的pipeline进行设置。
你需要注意的几项有：

  • Input: Exposure(曝光度)
    • 这一项最好设置在200左右以达到最优性能。
  • Input: Resolution(分辨率)
    • 这一项最好设置一个能看的分辨率以及尽量高的帧数，用来提高识别到april
      code的概率。
  • Configuration: ID filters(ID过滤器 注意这里是白名单)
    • 这一项是用来过滤你想要锁定的april tag
      ID的，如例子中展示，只允许了ID23的april tag被检测。
  • 最后不要忘记按一下左上角的星星保存pipeline
    \"LimelightWebpage1\"
    \"LimelightWebpage2\"

2.2 limelight在AS上的数据读取

首先我们在AS上调用的话就需要获取摄像头的硬件并为他们进行初始化。这与普通硬件的初始化是一样的。

1
2
DcMotorEx cameraMotor = hardwareMap.get(DcMotorEx.class, "cameraMotor");
Limelight3A limelight = hardwareMap.get(Limelight3A.class, "camera1");

在成功获取硬件道路之后我们可以为其进行更进一步的初始化，包括切换管道，以及开启limelight摄像头。

1
2
limelight.pipelineSwitch(7);
limelight.start();

在为他们完成初始化之后就可以通过limelight
来使用摄像头的功能以及读取摄像头的返回值了。

在官网上，他们为摄像头配备了很多语句以及他们的用途：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
List<FiducialResult> fiducials = result.getFiducialResults();
for (FiducialResult fiducial : fiducials) {
    int id = fiducial.getFiducialId(); // 基准标记的ID号
    double x = detection.getTargetXDegrees(); // 它的位置（左右）
    double y = detection.getTargetYDegrees(); // 它的位置（上下）
    double StrafeDistance_3D = fiducial.getRobotPoseTargetSpace().getY(); 
    telemetry.addData("基准标记 " + id, "距离 " + distance + " 米");
}

fiducial.getRobotPoseTargetSpace(); // 相对于AprilTag坐标系的机器人姿态（最有用）
fiducial.getCameraPoseTargetSpace(); // 相对于AprilTag的相机姿态（有用）
fiducial.getRobotPoseFieldSpace(); // 仅基于此标签的场地坐标系中的机器人姿态（有用）
fiducial.getTargetPoseCameraSpace(); // 相机坐标系中的AprilTag姿态（不太有用）
fiducial.getTargetPoseRobotSpace(); // 机器人坐标系中的AprilTag姿态（不太有用）

官网地址
这是针对april tag的部分的大致代码。
但是实际上我们需要用到的语句只有：

1
fiducial.getTargetXDegrees();

因为我们几乎只需要使用这句话来对我们想要锁定的目标的横向的偏移量进行测量。
在我们的代码中我们把这部分写成了这样：

1
2
3
LLResult result = limelight.getLatestResult();
if (result != null && result.isValid() && !result.getFiducialResults().isEmpty()) {
    for (LLResultTypes.FiducialResult tag : result.getFiducialResults()) {

同时我们对于fiducial ID
的检测和筛选已经在硬件部分完成了，因此不需要再次判断是否为我们想要锁定的april
code。我们可以直接基于这个tx来给电机进行功率输出。

1
double txRaw = tag.getTargetXDegrees();

如果需要驱动telemetry来进行调试，也可以调用

1
2
3
4
telemetry.addData("TagID", tag.getFiducialId());
telemetry.addData("txRaw", txRaw);
telemetry.addData("txFiltered", txFiltered);
telemetry.addData("ty", tag.getTargetYDegrees());

至于为什么fiducals是一个数组，是因为原本limelight是为了同时识别很多个目标做的，因此会得到很多组数据。而我们只需要锁定一个数据并且已经在硬件层面进行过过滤了所以并不需要担心循环中出现其他的tag，直接在循环内执行代码就行。

需要注意的是，这些语句只能针对fiducals数组中的元素才能调用，当针对整个数组时是无法调用的。

还需要注意的是tx给出的返回值是一个角度（角度制）。

3.如何驱动电机

得到来自摄像头的对于april code
的偏移角度之后就可以通过计算一个简化版的PID调试来控制电机的旋转功率来让我们的摄像头对准april
code。
这里需要注意的是我们一定需要设定一个死区double deadband = 7.0;来确保摄像头不会过于敏感。这个死区大概位于5-10度之间是较为合适的。如果没有死区或者死区太小会导致摄像头（电机）的摆动，因此需要死区来限制最小调整范围以确保摆动被尽可能限制。

1
2
3
4
double kP = 0.01; // 减少幅度
double power = kP * txFiltered;
power = Math.max(-0.15, Math.min(0.15, power));
cameraMotor.setPower(power);

接下来，你会发现如果不给摄像头设置一个默认值（当april
code仍然在死区内时），摄像头会一直转动一直到摄像头达到死区的另外一边，并开始反过来转动，循环往复。
这个时候我们就需要给代码加上分类处理，处理当摄像头存在于死区之内的情况。

1
2
cameraMotor.setPower(0);
telemetry.addLine("Aligned (within deadband)");

把这两个合起来长这样：

1
2
3
4
5
6
7
8
9
10
11
double deadband = 7.0;
if (Math.abs(txFiltered) > deadband) {
    double kP = 0.01; // 减少幅度
    double power = kP * txFiltered;
    power = Math.max(-0.15, Math.min(0.15, power));
    cameraMotor.setPower(power);
    telemetry.addData("MotorPower", power);
} else {
    cameraMotor.setPower(0);
    telemetry.addLine("Aligned (within deadband)");
}

这样子就完成了对于tx的大致处理，你的摄像头已经可以大概对准你设定的april
code了。

如果想要更加稳定的摄像头，或者彻底消除初步代码带来的波动，请移步高斯滤波。

当然，你会问另一个问题，”如果我的摄像头没有在一开始就对准april code
或者我的摄像头后面丢失目标了怎么办？”
很好，这就引导我们解决另外一个情况，即为待机状态，或者寻找状态（我更喜欢叫他哨戒状态）。
这部分也很简单，只需要通过调用在limelight摄像头的这些函数就可以实现了：

1
2
3
4
LLResult result = limelight.getLatestResult();
if (result != null && result.isValid() && !result.getFiducialResults().isEmpty()) {
    for (LLResultTypes.FiducialResult tag : result.getFiducialResults()) {
        double txRaw = tag.getTargetXDegrees();

这一部分中的if()即可判断摄像头在这一刻是否检测到了数据，如果有，那么可以进入上面的部分进行追踪，如果没有，就可以进入哨戒状态，通过慢速的，不断的旋转电机来在车体周围寻找目标。

1
2
double lostPower = 0.13; // 用小功率找
cameraMotor.setPower(lostPower);

更优的哨戒模式可以在这里查看。

到这里，恭喜你，你就完成了通过摄像头返回的tx值来驱动电机锁定目标的大致框架。剩下的工作就只剩下了调试和优化你写出来的框架以及接入麦轮底盘代码进行遥控测试。

4.调试和优化

4.1高斯滤波

4.2丢失目标寻找（哨戒模式）优化

很好，相信到这里你的大部分代码已经写完了，现在我们来解决车体在旋转时丢失目标后如何优化寻找效率。当然对于普通的移动丢失目标也适用。
首先，在正常情况下，我们的车体在丢失目标之后都会进入哨戒模式，正向旋转一整圈寻找丢失的目标。有的时候这很管用，如果目标正好在左边。但是如果目标在右面呢？直接把输出功率乘以-1让电机反向旋转吗？
不，这样子会导致摄像头寻找从左边丢失的目标的时间加长，同时还非常不美观，必须舍弃一边来获取另一边的效率。
相信看到这里各位都想到了解决的方式，那就是通过记录最后一次看到目标tag的tx来确定目标是从左边消失的还是从右边消失的，进而来确定电机应该往哪个方向旋转。
这样子处理的话可以大大减少电机的旋转时间，从而减少丢失目标的时间，是一项十分有用的优化。
可以从最开始的只要钻转就会丢失目标进化到全速旋转也不会丢失目标。

实现思路是这样的：
首先设立一个循环外变量，lastTx。
让这个变量等于每次获取到april code
之后的tx值。注意，对于这个变量，tx值的内容并不重要，重要的是tx值的正负。其实完全可以使用bool类型来做变量但是我为了调试方便还是选择了double

1
2
3
4
5
6
7
8
9
private Double lastTx = null; // 初始化
@Override
    public void runOpMode() throws InterruptedException {
        //skip for now
        while (opModeIsActive()) {
            LLResult result = limelight.getLatestResult();
            if (result != null && result.isValid() && !result.getFiducialResults().isEmpty()) {
                for (LLResultTypes.FiducialResult tag : result.getFiducialResults()) {
                    lastTx = txRaw; // 更新最后一次检测到的tx

然后，在进入哨戒模式时，检测这个lastTx,判断其的正负，来确定电机旋转的方向。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
else {
    //根据 lastTx 决定方向
    if (lastTx != null) {
        double lostPower = 0.13; // 用小功率找
        if (lastTx > 0) {
            //最后在右边往左转
            cameraMotor.setPower(lostPower); //控制电机向左去找
        } else {
            //最后在左边往右转
            cameraMotor.setPower(-lostPower); //控制电机向右去找
        }
    } else {
        cameraMotor.setPower(0.15);
    }
}

在找到april
code后，代码退出哨戒模式重新返回锁定模式，再次开始更新lastTx。

5.完整代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
package org.firstinspires.ftc.teamcode;
//在cyz发的limelight111版本的基础上加了lasttx，用来判断丢失时云台转的方向
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.hardware.limelightvision.LLResultTypes;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

import java.util.LinkedList;
import java.util.Queue;

@TeleOp(name = "Limelighttest1112")
public class limelight1112 extends LinearOpMode {

    private Limelight3A limelight;


    private static final int WINDOW_SIZE = 6;  // 高斯窗口增大一点
    private final Queue<Double> txHistory = new LinkedList<>();
    private Double lastTx = null; // 上一次检测到的tx

    int lostcount = 0;

    // 高斯权重
    private final double[] gaussianKernel = {0.06136, 0.24477, 0.38774, 0.24477, 0.06136};
    private double previousTxFiltered = 0;  // 搞一个低通滤波器

    @Override
    public void runOpMode() throws InterruptedException {
        DcMotorEx cameraMotor = hardwareMap.get(DcMotorEx.class, "cameraMotor");


        cameraMotor.setMode(DcMotorEx.RunMode.RUN_WITHOUT_ENCODER);

        limelight = hardwareMap.get(Limelight3A.class, "camera1");

        telemetry.setMsTransmissionInterval(11);

        limelight.pipelineSwitch(7);
        limelight.start();
        DcMotorEx frontLeftMotor = hardwareMap.get(DcMotorEx.class,"leftFrontMotor");
        DcMotorEx backLeftMotor = hardwareMap.get(DcMotorEx.class,"leftBackMotor");
        DcMotorEx frontRightMotor = hardwareMap.get(DcMotorEx.class,"rightFrontMotor");
        DcMotorEx backRightMotor = hardwareMap.get(DcMotorEx.class,"rightBackMotor");

        // Reverse the right side motors. This may be wrong for your setup.
        // If your robot moves backwards when commanded to go forwards,
        // reverse the left side instead.
        // See the note about this earlier on this page.[]\
        frontRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);
        backRightMotor.setDirection(DcMotorSimple.Direction.REVERSE);

        waitForStart();

        int notfound = 0;

        while (opModeIsActive()) {
            LLResult result = limelight.getLatestResult();

            if (result != null && result.isValid() && !result.getFiducialResults().isEmpty()) {
                for (LLResultTypes.FiducialResult tag : result.getFiducialResults()) {
                    double txRaw = tag.getTargetXDegrees();
                    lastTx = txRaw; // 更新最后一次检测到的tx

                    if (txHistory.size() >= WINDOW_SIZE) txHistory.poll();
                    txHistory.add(txRaw);

                    double txFiltered = applyGaussianFilter();
                    txFiltered = applyLowPassFilter(txFiltered);

                    double deadband = 7.0;

                    if (Math.abs(txFiltered) > deadband) {
                        double kP = 0.01; // 减少幅度
                        double power = kP * txFiltered;
                        power = Math.max(-0.15, Math.min(0.15, power));
                        cameraMotor.setPower(power);
                        telemetry.addData("MotorPower", power);
                    } else {
                        cameraMotor.setPower(0);
                        telemetry.addLine("Aligned (within deadband)");
                    }

                    telemetry.addData("TagID", tag.getFiducialId());
                    telemetry.addData("txRaw", txRaw);
                    telemetry.addData("txFiltered", txFiltered);
                    telemetry.addData("ty", tag.getTargetYDegrees());
                }
            } else {
                //根据 lastTx 决定方向
                if (lastTx != null) {
                    double lostPower = 0.13; // 用小功率找
                    if (lastTx > 0) {
                        //最后在右边往左转
                        cameraMotor.setPower(lostPower);
                    } else {
                        //最后在左边往右转
                        cameraMotor.setPower(-lostPower);
                    }
                } else {
                    cameraMotor.setPower(0.15);
                }
            }


            telemetry.update();
            double y = -gamepad1.left_stick_y; // Remember, Y stick value is reversed
            double x = gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
            double rx = gamepad1.right_stick_x;

            // Denominator is the largest motor power (absolute value) or 1
            // This ensures all the powers maintain the same ratio,
            // but only if at least one is out of the range [-1, 1]
            double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
            double frontLeftPower = (y + x + rx) / denominator;
            double backLeftPower = (y - x + rx) / denominator;
            double frontRightPower = (y - x - rx) / denominator;
            double backRightPower = (y + x - rx) / denominator;

            double powerCoefficent= 0.6;

            frontLeftMotor.setPower(frontLeftPower*powerCoefficent);
            backLeftMotor.setPower(backLeftPower*powerCoefficent);
            frontRightMotor.setPower(frontRightPower*powerCoefficent);
            backRightMotor.setPower(backRightPower*powerCoefficent);
        }
    }

    private double applyGaussianFilter() {
        if (txHistory.isEmpty()) return 0.0;

        Double[] values = txHistory.toArray(new Double[0]);
        int size = values.length;
        double sum = 0;

        for (int i = 0; i < size; i++) {
            // 权重对齐到最新值（右端）
            int kernelIndex = gaussianKernel.length - size + i;
            if (kernelIndex < 0) kernelIndex = 0;
            sum += values[i] * gaussianKernel[kernelIndex];
        }

        return sum;
    }
    private double applyLowPassFilter(double currentTxFiltered) {
        double alpha = 0.1;  // 平滑系数，值越小越平滑
        double filtered = alpha * currentTxFiltered + (1 - alpha) * previousTxFiltered;
        previousTxFiltered = filtered;
        return filtered;
    }
}
//作者：檀若之，陈奕卓，忻鹿, 周泊君
//时间：Sep 28, 2025

神秘生物图鉴
\"LimelightWebpage1\"

作者：忻鹿
Sep 28, 2025
:::