视觉引导降落

光学处理

这里我用一段视觉引导着陆的算法来举例，在嵌入式平台或者机载计算机平台上，我们的算力并不是非常的强大，所以我们需要使用一些简单的占用率比较小的算法来完成，这就不得不使用opencv进行图像处理了。

这个课题的目的是视觉引导无人机进行着陆，无人机的机臂下会捆绑四个红色灯点，视觉部分在地面上扫描无人机所在位置，然后获取坐标并引导移动到中心部位着陆，同时还需要计算机体的旋转角度。

这个题目的难度主要集中在如何搜索无人机的四个红色灯点，我们大可以把环境假设的极端一些，也就是在阳光下的红色灯点。

1、色彩增强

为了确保在一些强光环境下导致相机过曝，我们必须把相机的曝光值调节的较低，这样可以过滤掉白天极亮的环境，也可以保留四个高亮的红色灯点。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
Camera(int id) : cap(id) {
    if (!cap.isOpened()) {
        cerr << "Error: Camera not opened" << endl;
        exit(1);
    }

    // 设置相机分辨率
    cap.set(CAP_PROP_FRAME_WIDTH, 1920);
    cap.set(CAP_PROP_FRAME_HEIGHT, 1080);
    // 设置相机曝光
    cap.set(CAP_PROP_AUTO_EXPOSURE, 0.25);
    // 设置帧数
    cap.set(CAP_PROP_FPS, 30);

    // 设置相机光心
    camera_center_x = cap.get(CAP_PROP_FRAME_WIDTH) / 2;
    camera_center_y = cap.get(CAP_PROP_FRAME_HEIGHT) / 2;
}

这里为了方便，我把整个图像处理环节写作了Camera类，同时也可以设置相机的内参用于之后的姿态恢复。

一些私有属性

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
private:
    // 相对世界坐标
    double x{}, y{};
    // 相对世界旋转角
    double angle{};
    // 焦距逼近距离
    double distance{};
    // 焦距
    double f{};
    // 识别到的目标宽和高
    double width{}, height{};

    // 相机光心
    double camera_center_x{}, camera_center_y{};

    // 相机内参
    Mat params;
    // 畸变系数
    double k1, k2, p1, p2, k3;

    // 相机捕获
    VideoCapture cap;
    // 捕获帧
    Mat frame;

接下来需要给低曝光图像的色彩进行增强，也就是对RGB进行放大。这里可以直接使用opencv的方法：

1
2
Mat superframe;
frame.convertTo(superframe, -1, 0.4, 0.6);

其中的frame是private属性，是不断对相机捕获的照片帧。这个方法的原型为：

1
void convertTo( OutputArray m, int rtype, double alpha=1, double beta=0 ) const;

其中rtype为输出矩阵的位深度（即最大值比如8位深度就是255，即R最大取255，如果小于0则和原来一样，位深度决定了色彩的丰富度和精细度）。alpha为尺度变化，也就是把图像的RGB值都乘以alpha。beta为偏移量，即在最终结果加beta。也可以写作：

2、色彩提取

这里我需要把除了红色以外的所有颜色给降低影响，我们可以通过差值滤波，这里需要知道：

对于相机而言，CMOS上的一个像素需要同时保存RGB三种颜色信息，我们单纯提取单一色通道可能会夹杂着其他通道带来的影响。所以我们需要把红色通道依次减去蓝色绿色，将两个差加到一起就可以去除掉除了红色以外所有颜色的影响了

代码可以通过absdiff进行运算即：

1
2
3
4
5
6
7
// 差值滤波(红色就是红色通道，也是蓝色和绿色的差值)
Mat blue = channels[0];
Mat green = channels[1];
Mat diff1, diff2;
absdiff(red, blue, diff1);
absdiff(red, green, diff2);
red = diff1 + diff2;

这样输出的图像就会变成：

原图

处理结果

可以看到白色直接被完全的削弱了。一旦我点个红灯就会发现：

3

这个清晰度已经达到了可以准确识别的效果，但是因为外面还存在一些色散，我们需要去除色散并二值化，这里需要用到高斯模糊和闭运算。

1
2
3
4
5
6
7
8
9
// 2. 高斯模糊
GaussianBlur(red, red, Size(5, 5), 0);

// 3. 二值化
threshold(red, red, 100, 125, THRESH_BINARY);

// 闭运算色散滤波(闭运算即腐蚀+膨胀, 开运算即膨胀+腐蚀)
Mat kernel = getStructuringElement(MORPH_RECT, Size(5, 5));
morphologyEx(red, red, MORPH_CLOSE, kernel);

所谓的闭运算就是腐蚀后膨胀，开运算就是先膨胀后腐蚀。闭运算可以把色散去除掉，开运算可以保留更多光学信息。对于我们的要求，我们希望把无关的色彩信息与色散去除掉，我们必须使用闭运算去除掉散射的光线。通过此法就可以得到一个相对规矩的图像：

4

3、优化劣化

我们发现红色区域就是圆形，只不过边边角角太丑了，我们可以简单的膨胀一下，但是这里有更好的办法即：图形增益算法。假设我们需要增益的是圆形我们就需要进行边缘检测，我们就需要给定一个拟合度，当拟合度过低是把当前的边缘给掩盖住。

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
// 增益圆形
void enhanceCircles(cv::Mat& input, cv::Mat& UPend, double threshold) {
    Mat binaryImage = input.clone();
    cv::Mat output = cv::Mat::zeros(binaryImage.size(), CV_8UC1);
    std::vector<std::vector<cv::Point>> contours;
    cv::findContours(binaryImage, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);

    for (const auto& contour : contours) {
        cv::Point2f center;
        float radius;
        cv::minEnclosingCircle(contour, center, radius);

        double areaContour = cv::contourArea(contour);
        double areaCircle = CV_PI * radius * radius;

        // 判断轮廓是否接近圆形
        if (std::abs(areaContour - areaCircle) < areaCircle * threshold) {
            cv::circle(output, center, static_cast<int>(radius), cv::Scalar(255), -1);
        }
    }

    // 执行模糊操作来劣化非圆形区域
    cv::GaussianBlur(output, output, cv::Size(0, 0), 5);
    UPend = output.clone();
    cv::bitwise_or(output, binaryImage, binaryImage);
}

这个方法通过模糊劣化非圆形区域得到较为真实的圆形区域，判断轮廓仅需判断边界与半径的距离是否超过了阈值比例即可。看看此法的输出效果：

1
2
3
4
// 圆形增益UPend
Mat UPend;
enhanceCircles(red,UPend, 0.7);
_imgshow("UPend", UPend);

4、边缘识别

5

此法把原本图形的杂七杂八部分全部去除掉，保留了人工绘制的区域。这样我们再去寻找圆形区域就变得即为简单了。

1
2
3
4
5
6
// 5. UPend中找出圆形轮廓
vector<vector<Point>> filter_contours;
vector<Vec4i> filter_hierarchy;
findContours(UPend, filter_contours, filter_hierarchy, RETR_TREE, CHAIN_APPROX_TC89_KCOS);
// 绘制轮廓
drawContours(frame, filter_contours, -1, Scalar(0, 255, 0), 2);

这样绘制出几个圆形就可以显示一下了。

因为我们需要通过四个灯得到飞机的位置和姿态，所以我们把四个圆形的圆心当作四个顶点计算几何中心，并以四个点为参考系计算相对姿态：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
// 6、计算轮廓中心点并绘制
vector<Point> centers;
for (const auto& contour : filter_contours) {
    Moments m = moments(contour);
    Point center(m.m10 / m.m00, m.m01 / m.m00);
    centers.push_back(center);
}

// 轮廓中心数目(过多证明引入其他不是的点，直接剔除。过少则可能是没看清，需要变焦处理)
if (centers.size() > 4) {
    NoOpetion;
}

// 没看清(<4)和没看见(>0)是两个概念
else if (centers.size() < 4 && !centers.empty()){
    // 数码变焦
}else{
    // 正好四个点
}

我们可以看一下经过上述算法处理后的效果：

5

我们可以看到绘制的都非常精确，中心点找的也非常好。假设我们正好找到四个点的话，我们就需要进行中心的绘制（也就是中心点紫色的位置，因为代码已经写好了，所以我就没有取消中心点的绘制）

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
// 姿态恢复算法
poseRecovery(centers);
// 7、算可以组成正方形的四个点, 正方形的各个边长要近似相等，最大不能超过100
vector<Point> square_centers;
for (const auto& center1 : centers) {
    for (const auto& center2 : centers) {
        for (const auto& center3 : centers) {
            for (const auto& center4 : centers) {
                double d1 = norm(center1 - center2);
                double d2 = norm(center2 - center3);
                double d3 = norm(center3 - center4);
                double d4 = norm(center4 - center1);

                if (d1 > 100 || d2 > 100 || d3 > 100 || d4 > 100) continue;

                double d5 = norm(center1 - center3);
                double d6 = norm(center2 - center4);

                if (std::abs(d1 - d2) < 10 && std::abs(d2 - d3) < 10 && std::abs(d3 - d4) < 10 &&
                    std::abs(d4 - d1) < 10 && std::abs(d5 - d6) < 10) {
                    square_centers.push_back(center1);
                    square_centers.push_back(center2);
                    square_centers.push_back(center3);
                    square_centers.push_back(center4);
                }
            }
        }
    }
}

// 矩形的宽和高
// 判断点是否存在
if (square_centers.size() < 4) NoOpetion;
this->width = norm(square_centers[0] - square_centers[1]);
this->height = norm(square_centers[1] - square_centers[2]);


// 8、计算正方形中心点
Point square_center;
for (const auto& center : square_centers) square_center += center;
if (square_centers.empty()) NoOpetion;

square_center.x /= square_centers.size();
square_center.y /= square_centers.size();
// 计算中心坐标
this->x = square_center.x - this->camera_center_x;
this->y = square_center.y  - this->camera_center_y;
// 旋转角度
this->angle = atan2(this->y, this->x) * 180 / CV_PI;

// 10、绘制正方形的中心点
circle(frame, square_center, 15, Scalar(255, 0, 255), -1);

一旦正好检测到四个点，并且对于这个焦段在5m的相机来讲，飞机肯定在很近很近的位置了，所以我们必须要确保飞机的姿态正常，我们需要计算一下四个点组成的形状是否合规，是否存在异常情况。

这里用的姿态恢复是PnP约束，也就是简单的三角约束：

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
// 位姿恢复
void poseRecovery(std::vector<Point>& specialPoint){
    if (this->params.empty()) {
        cerr << "Error: Camera parameters not set" << endl;
        return;
    }

    if (this->k1 == 0 && this->k2 == 0 && this->p1 == 0 && this->p2 == 0 && this->k3 == 0) {
        cerr << "Error: Distortion coefficients not set" << endl;
        return;
    }
    // 1、将特殊点转换为相机坐标系
    std::vector<Point2f> specialPointCamera;
    cv::undistortPoints(specialPoint, specialPointCamera, params, Mat::eye(3, 3, CV_64F), Mat::zeros(5, 1, CV_64F));
    // 2、计算特殊点的世界坐标
    std::vector<Point3f> specialPointWorld;
    for (const auto& point : specialPointCamera) {
        double x = point.x;
        double y = point.y;
        double z = f;
        specialPointWorld.emplace_back(x, y, z);
    }
    // 3、求解位姿
    Mat rvec, tvec;
    cv::solvePnP(specialPointWorld, specialPointCamera, params, Mat::zeros(5, 1, CV_64F), rvec, tvec);
    // 4、将旋转向量转换为旋转矩阵
    Mat R;
    cv::Rodrigues(rvec, R);
    // 5、将旋转矩阵转换为欧拉角
    Mat euler;
    cv::Rodrigues(R, euler);
    // 6、计算相对于相机的位姿
    Mat relativePose = Mat::eye(4, 4, CV_64F);
    R.copyTo(relativePose(Rect(0, 0, 3, 3)));
    tvec.copyTo(relativePose(Rect(3, 0, 1, 3)));
}

6、数码变焦

通过计算投影光线与成像平面组成的三角形，利用相机内参求解三角形边长就可以恢复相机的旋转矩阵。将平移矩阵当作计算出的边长就可以了。此法运行后就得到了上面的图片。对于超过4个点的情况我们直接不做处理就好。对于0到4个点的情况，我们只需要进行权重数码缩放。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
// 数码变焦
Mat scaleImageAtPosition(Mat treatIMG, int x, int y, double f) {
    Mat inputImage = treatIMG.clone();
    Mat outputImage;
    // 计算缩放后的中心区域尺寸
    int center_width = (int)(inputImage.cols / f);
    int center_height = (int)(inputImage.rows / f);
    // 计算ROI的左上角坐标
    int roi_x = std::max(0, x - center_width / 2);
    int roi_y = std::max(0, y - center_height / 2);
    // 确保ROI不会超出图像边界
    if (roi_x + center_width > inputImage.cols) roi_x = inputImage.cols - center_width;
    if (roi_y + center_height > inputImage.rows) roi_y = inputImage.rows - center_height;

    // 定义ROI并提取区域
    cv::Rect roi(roi_x, roi_y, center_width, center_height);
    cv::Mat roiImage = inputImage(roi);
    // 调整大小
    cv::resize(roiImage, outputImage, cv::Size(), f, f);
    return outputImage;
}

数码变焦就是给定缩放中心点，给定缩放倍数，框选一个ROI区域进行方法即可。这样可以保证图像尺寸不变，此法劣于光学变焦的点在于会损失像素信息，优点就是总体的场视角不会改变。

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
// 点数在0-4之间
// 1、计算变焦中心点
Point center;
for (const auto& c : centers) center += c;
center.x /= centers.size();
center.y /= centers.size();
// 2、变焦(把目前识别到的点加起来取个平均值，计算得到的坐标当作缩放中心，原因很简单，特殊点不会单独出现，既然疑似就重点观察)
Mat zoomed = scaleImageAtPosition(red, center.x, center.y, ZOOM);
Mat scaleFrame = scaleImageAtPosition(frame, center.x, center.y, ZOOM);
// 3、识别圆形轮廓
Mat NPend;
enhanceCircles(zoomed,NPend, 0.5);
_imgshow("NPend", NPend);

vector<vector<Point>> scale_contours;
vector<Vec4i> scale_hierarchy;
findContours(NPend, scale_contours, scale_hierarchy, RETR_TREE, CHAIN_APPROX_TC89_KCOS);

// 4、计算轮廓中心点并绘制
vector<Point> new_centers;
for (const auto& contour : scale_contours) {
    Moments m2 = moments(contour);
    Point new_center(m2.m10 / m2.m00, m2.m01 / m2.m00);
    new_centers.push_back(new_center);
    // circle(scaleFrame, new_center, 5, Scalar(0, 255, 0), -1);
}

// 判断尺寸
if (new_centers.size() != 4) NoOpetion;
// 5、计算正方形中心点
Point square_center;
for (const auto& center : new_centers) square_center += center;
square_center.x /= new_centers.size();
square_center.y /= new_centers.size();
// 绘制缩放后的中心点
circle(scaleFrame, square_center, 15, Scalar(255, 0, 255), -1);
_imgshow("scaleFrame", scaleFrame);

// 6、差分中心点
Point relatice_center = relativePosition(square_center, Point(zoomed.cols / 2, zoomed.rows / 2), center, ZOOM);
// 计算中心点
this->x = relatice_center.x - this->camera_center_x;
this->y = relatice_center.y  - this->camera_center_y;
// 旋转角度
this->angle = atan2(this->y, this->x) * 180 / CV_PI;
// 绘制实际中心点
circle(frame, relatice_center, 15, Scalar(255, 0, 255), -1);
return;

这里存在了一个缩放后目标恢复到原坐标系的过程，这个很简单，高中生都可以推出：

1
2
3
4
5
6
7
8
9
10
11
// aim为识别到的中心点，change_center为变焦后的中心点，normal_center为变焦后的中心点,change_normal_center为变焦前的正常中心点，f为缩放系数
Point relativePosition(Point aim, Point change_center, Point change_normal_center, double f) {
    double dx = (change_center.x - aim.x) * (1 - 1 / f);
    double dy = (change_center.y - aim.y) * (1 - 1 / f);

    Point relative;
    relative.x = change_normal_center.x + dx;
    relative.y = change_normal_center.y + dy;

    return relative;
}

也就是把缩放后的偏差缩放回去，毕竟虽然坐标系变了，但是相对关系没变。这样我们就得到了一个权重数码变焦算法。

7、结束

通过这个办法就可以实现一个10m内的10寸无人机引导降落。如果在外太空还可以进行相对姿态解算进行空间站对接。

技术
#C++ #计算机视觉 #Slam
视觉引导降落
https://blog.minloha.cn/posts/111521cf2bd5972024101537.html
作者
Minloha
发布于
2024年10月15日
更新于
2024年11月20日
许可协议