无人机建图+障碍识别+路径规划

简介

闲来无事，本着月更博主的优良美德，这次终于可以给大家更新了。这几天做个双目视觉避障，技术难度不算特别高，里面用到的一些算法都是非常好理解的。项目主要包括建图+避障+路径规划三大部分，其中路径规划都有对应的神经网络模型，基础点的有蚁群、退火、成长优化等等，这些算法各有特色，所以本项目所使用的并不一定就是最好的，只是相对比较快的。至于其他的功能可以从视觉惯性上学习。

建图

在无人机上挂一枚4k摄像头，使用H616芯片作为视觉处理CPU，同时利用抽帧法获取摄像机数据以降低CPU负担，下面将详细讲解。

Step：抽帧法

先介绍一下H616的参数，他是一个四核CPU，拥有1.5G主频64位指令集。这里使用的是香橙派zero3，所以还带有一个OpenGL。

虽然这个配置可以显示4k图像，但是在处理上还需要加强，摄像头为4k/30fps，并且建图无人机速度也没有快过摄像头，所以我们可以在视频按照一定间隔取出一帧进行图像拼接。

void Capture2Img(cv::VideoCapture& capture, cv::Mat& result, int fps) {
    capture.read(result);
    int i = 0;
    cv::Mat frame;
    while (capture.read(frame)){
        if (i % fps == 0) ImgTreat(result, frame, result);
        std::cout << "FPS tick in " << i << std::endl;
        i++;
    }
}

这里使用的是通过VideoCapture进行参数传递，fps为抽帧间隔。

获取到图像后，我们就需要进行图像拼接，这里我们可以利用SLAM建图的思想完成：

计算ORB特征点
将特征点进行匹配
将匹配的特征点进行汉明距离上的筛选
保存最佳的筛选特征点
利用特征点计算单应矩阵
拼接图像并去黑边

这里使用到了单应矩阵，他的主要作用就是在一段连续变化的图片中，描述图片的变化过程，如果搭配视觉惯性算法可以清楚的表输出摄像机的位姿和视角等信息，不过这里并不需要，仅仅需要将变化后的图像进行变换，计算得到和原图像比例相当的图像然后进行拼接即可。

这里放出代码，感兴趣可以阅读，数学原理不做赘述：

void ImgTreat(cv::Mat& baseImg, cv::Mat& addImg, cv::Mat& result) {
    // 复制两张图片，避免对原图进行修改
    cv::Mat img1 = baseImg.clone();
    cv::Mat img2 = addImg.clone();
    // 灰度
    cv::Mat img1Gray, img2Gray;
    cv::cvtColor(img1, img1Gray, cv::COLOR_BGR2GRAY);
    cv::cvtColor(img2, img2Gray, cv::COLOR_BGR2GRAY);
    // 创建ORB特征匹配，并创建特征点列表
    cv::Ptr<cv::ORB> orb = cv::ORB::create();
    std::vector<cv::KeyPoint> kp1, kp2;
    cv::Mat desp1, desp2;
    // 计算特征点
    orb->detectAndCompute(img1Gray, cv::Mat(), kp1, desp1);
    orb->detectAndCompute(img2Gray, cv::Mat(), kp2, desp2);
    // 使用汉明距离进行筛选
    cv::BFMatcher matcher(cv::NORM_HAMMING);
    std::vector<cv::DMatch> matches;
    matcher.match(desp1, desp2, matches);
    // 筛选出距离在常数范围的点，过滤掉相似度过低的点
    double min_dist = 10000, max_dist = 0;
    for (int i = 0; i < desp1.rows; i++) {
        double dist = matches[i].distance;
        if (dist < min_dist) min_dist = dist;
        if (dist > max_dist) max_dist = dist;
    }
    // 使用经验公式获取最佳配对点
    std::vector<cv::DMatch> goodMatches;
    for (int i = 0; i < desp1.rows; i++) {
        // 28是调参效果最好的，一般是30，与倍数2均为调试得到
        if (matches[i].distance <= max(2 * min_dist, 28.0)) {
            goodMatches.push_back(matches[i]);
        }
    }
    // 获取最佳匹配点的坐标
    std::vector<cv::Point2f> pts1, pts2;
    for (size_t i = 0; i < goodMatches.size(); i++) {
        pts1.push_back(kp1[goodMatches[i].queryIdx].pt);
        pts2.push_back(kp2[goodMatches[i].trainIdx].pt);
    }

    // 计算单应矩阵
    cv::Mat H = cv::findHomography(pts2, pts1, cv::RANSAC);
    cv::Mat imgWrap;
    // cv::Size内容为图像继续拼接的位置，可以搭配陀螺仪的方向使用。
    cv::warpPerspective(img2, imgWrap, H, cv::Size(img1.cols + img2.cols, img1.rows));
    // 清除黑边
    KillBlackEdge(imgWrap);
    cv::imshow("imgWrap", imgWrap);
    cv::waitKey(0);
    result = imgWrap.clone();
}

最后我们来到了黑边删除算法，只需确定有RGB颜色的四条边即可：

void KillBlackEdge(cv::Mat& img){
    int left = 0, right = img.cols - 1, top = 0, bottom = img.rows - 1;
    for (int i = 0; i < img.cols; i++)
        for (int j = 0; j < img.rows; j++)
            if (img.at<cv::Vec3b>(j, i)[0] != 0 || img.at<cv::Vec3b>(j, i)[1] != 0 || img.at<cv::Vec3b>(j, i)[2] != 0){
                left = i;
                goto left;
            }
    left:
    for (int i = img.cols - 1; i >= 0; i--)
        for (int j = 0; j < img.rows; j++)
            if (img.at<cv::Vec3b>(j, i)[0] != 0 || img.at<cv::Vec3b>(j, i)[1] != 0 || img.at<cv::Vec3b>(j, i)[2] != 0){
                right = i;
                goto right;
            }
    right:
    for (int i = 0; i < img.rows; i++)
        for (int j = 0; j < img.cols; j++)
            if (img.at<cv::Vec3b>(i, j)[0] != 0 || img.at<cv::Vec3b>(i, j)[1] != 0 || img.at<cv::Vec3b>(i, j)[2] != 0){
                top = i;
                goto top;
            }
    top:
    for (int i = img.rows - 1; i >= 0; i--)
        for (int j = 0; j < img.cols; j++)
            if (img.at<cv::Vec3b>(i, j)[0] != 0 || img.at<cv::Vec3b>(i, j)[1] != 0 || img.at<cv::Vec3b>(i, j)[2] != 0){
                bottom = i;
                goto bottom;
            }
    bottom:
    cv::Mat img2 = img(cv::Rect(left, top, right - left, bottom - top));
    img = img2.clone();
}

双目避障

视觉避障最大的难点是精度和像素，双目视觉因为要同时处理两幅图像，所以对CPU的要求比较大，这里采用了RK3588的板子，也就是香橙派4b，这个CPU最高可以处理8k/30fps，有8个CPU核心和NPU用于神经计算加速，所以这里还需要用到神经网络进行判断。

在我之前的一期博客里面详细介绍了双目视觉与双目测距，这里给出链接：双目视觉测距

一些具体的原理，这里就不做赘述了，我们将标定好的参数保存在一个结构体里面：

typedef struct Camera{
    Mat CameraLeft;
    Mat CameraRight;

    Mat leftdistortion;
    Mat rightdistortion;

    Mat R;
    Mat t;

    double f;
    int baseLine;
} Camera_t;

const Camera_t camera = {
        (Mat_<double>(3,3)<<370.483442574848, -2.47527624986299, 275.906787436161,
                0, 366.502425941927, 174.672892269533,
                0, 0, 1),
        (Mat_<double>(3,3)<<377.468308869075, -3.56547821844709, 273.99047261022,
                0, 375.08647620698, 167.305790413573,
                0, 0, 1),
        (Mat_<double>(1,5)<<-0.063144218, 0.004560032, 0.001733565, -0.010034782, 0),
        (Mat_<double>(1,5)<<-0.048945539, -0.031878549, 0.002019302, -0.009916148, 0),
        (Mat_<double>(3,3)<<0.99999034158838, 0.0012791509071335, 0.00420481901070739,
                -0.00128177438515139, 0.999998985528464, 0.000621286140687432,
                -0.00420402002630774, -0.000626669769352302, 0.999990966709509),
        (Mat_<double>(3,1)<<-58.2696017484474, 0.40143041946323, 8.31746279031737),
        2.4,61
};

然后我们利用畸变系数进行畸变消除：

void undistortion_math(const Camera_t& Camera, Mat img, Mat* result, int flag){
    if (flag == 0)
        for (int i = 0; i < img.rows; i++)
            for (int j = 0; j < img.cols; j++){
                double x = (j - Camera.CameraLeft.at<double>(0,2)) / Camera.CameraLeft.at<double>(0,0);
                double y = (i - Camera.CameraLeft.at<double>(1,2)) / Camera.CameraLeft.at<double>(1,1);
                double r = sqrt(x * x + y * y);
                double x_distortion = x * (1 + Camera.leftdistortion.at<double>(0,0) * r * r +
                        Camera.leftdistortion.at<double>(0,1) * r * r * r * r +
                        Camera.leftdistortion.at<double>(0,4) * r * r * r * r * r * r) +
                                2 * Camera.leftdistortion.at<double>(0,2) * x * y +
                                Camera.leftdistortion.at<double>(0,3) * (r * r + 2 * x * x);
                double y_distortion = y * (1 + Camera.leftdistortion.at<double>(0,0) * r * r +
                        Camera.leftdistortion.at<double>(0,1) * r * r * r * r +
                        Camera.leftdistortion.at<double>(0,4) * r * r * r * r * r * r) +
                                Camera.leftdistortion.at<double>(0,2) * (r * r + 2 * y * y) +
                                        2 * Camera.leftdistortion.at<double>(0,3) * x * y;
                int x_ = x_distortion * Camera.CameraLeft.at<double>(0,0) + Camera.CameraLeft.at<double>(0,2);
                int y_ = y_distortion * Camera.CameraLeft.at<double>(1,1) + Camera.CameraLeft.at<double>(1,2);
                if (x_ >= 0 && x_ < img.cols && y_ >= 0 && y_ < img.rows)
                    result->at<uchar>(y_, x_) = img.at<uchar>(i, j);
            }
    else
        for (int i = 0; i < img.rows; i++)
            for (int j = 0; j < img.cols; j++){
                double x = (j - Camera.CameraRight.at<double>(0,2)) / Camera.CameraRight.at<double>(0,0);
                double y = (i - Camera.CameraRight.at<double>(1,2)) / Camera.CameraRight.at<double>(1,1);
                double r = sqrt(x * x + y * y);
                double x_distortion = x * (1 + Camera.rightdistortion.at<double>(0,0) * r * r + Camera.rightdistortion.at<double>(0,1) * r * r * r * r + Camera.rightdistortion.at<double>(0,4) * r * r * r * r * r * r) + 2 * Camera.rightdistortion.at<double>(0,2) * x * y + Camera.rightdistortion.at<double>(0,3) * (r * r + 2 * x * x);
                double y_distortion = y * (1 + Camera.rightdistortion.at<double>(0,0) * r * r + Camera.rightdistortion.at<double>(0,1) * r * r * r * r + Camera.rightdistortion.at<double>(0,4) * r * r * r * r * r * r) + Camera.rightdistortion.at<double>(0,2) * (r * r + 2 * y * y) + 2 * Camera.rightdistortion.at<double>(0,3) * x * y;
                int x_ = x_distortion * Camera.CameraRight.at<double>(0,0) + Camera.CameraRight.at<double>(0,2);
                int y_ = y_distortion * Camera.CameraRight.at<double>(1,1) + Camera.CameraRight.at<double>(1,2);
                if (x_ >= 0 && x_ < img.cols && y_ >= 0 && y_ < img.rows)
                    result->at<uchar>(y_, x_) = img.at<uchar>(i, j);
            }
}

最后我们利用内联函数的多线程完成SGBM算法：

inline void SGBM_Sub(Camera_t camera, Mat leftEye, Mat rightEye, vector<NearPoint_t> &list){
    mutex mtx;
    mtx.lock();
    Ptr<StereoSGBM> sgbm = StereoSGBM::create(0,16,3);
    sgbm->setPreFilterCap(63);
    sgbm->setBlockSize(3);
    sgbm->setP1(8*3*3);
    sgbm->setP2(32*3*3);
    sgbm->setMinDisparity(0);
    sgbm->setNumDisparities(16);
    sgbm->setUniquenessRatio(10);
    sgbm->setSpeckleWindowSize(100);
    sgbm->setSpeckleRange(32);
    sgbm->setDisp12MaxDiff(1);
    Mat disp, disp8;
    sgbm->compute(leftEye, rightEye, disp);
    normalize(disp, disp8, 0, 255, NORM_MINMAX, CV_8U);

    // 获取距离近点的角度和距离,且邻近区域不计算
    double bolck_size = 1;
    int mixtures = 300;
    double min_distance = 10;
    for (int i = 0; i < disp8.rows; i += mixtures * bolck_size)
        for (int j = 0; j < disp8.cols; j += bolck_size)
            if (disp8.at<uchar>(i, j) > 0) {
                double angle = atan((j - disp8.cols / 2) / camera.f);
                double distance = camera.baseLine * camera.f / (j - disp8.cols / 2);
                if (distance > min_distance && distance < 1000) {
                    cout << "angle: " << angle << " distance: " << distance << endl;
                    list.push_back(new NearPoint(distance, angle));
                }
            }

    mtx.unlock();
}

void SGBM(Camera_t camera, Mat leftEye, Mat rightEye, vector<NearPoint_t> &list){
    thread sgbmThread(SGBM_Sub, camera, leftEye, rightEye, ref(list));
    sgbmThread.join();
}

最后我们就得到了双目摄像机所确定的深度图，并在vector中保存返回。

最后我们利用对应的极坐标点画出雷达图：

void ObstactMap(vector<NearPoint_t> list, Mat* result){
    // 根据list画出平面点云, RGB图像
    // 设置点RGB值
    for (int i = 0; i < 1000; i++) result->at<Vec3b>(797, i) = Vec3b(50,205,50);
    for (int i = 0; i < 800; i++) result->at<Vec3b>(i, 499) = Vec3b(50,205,50);


    int pointsize = 5;
    for (auto i : list) {
        int x = i->rou * cos(i->theta) * 10;
        int y = i->rou * sin(i->theta) * 8;
        for (int j = -pointsize; j < pointsize; j++)
            for (int k = -pointsize; k < pointsize; k++)
                if (x + j >= 0 && x + j < 1000 && y + k >= 0 && y + k < 800)
                    result->at<uchar>(y + k, x + j) = 255;
    }
}

避障算法

因为我们不需要使用神经网络，所以我们就抛开强化学习等学习模型不谈，只用数学方法去求解这样的最优路径，缺点也很明显，那就是因为数据量被压缩了，所以变成了二维避障方法，这个算法是从成长优化算法（GO）演化过来的。

无人机的路径规划方法也要考虑最短路径的问题，所以我们需要假设障碍物是一个点，我们需要绕开这个点，并且还要和点保持一定距离，所以我们就需要让无人机沿着某一个圆的切点进行移动，

因为在本项目中使用到的都是极坐标，所以我们可以很方便的求出这样的向量。下面给出部分代码：

void Sort(double *list,int begin, int end){
    // 快速排序
    if (begin >= end) return;
    int i = begin, j = end;
    double key = list[begin];
    while (i < j){
        while (i < j && list[j] >= key) j--;
        if (i < j) list[i++] = list[j];
        while (i < j && list[i] <= key) i++;
        if (i < j) list[j--] = list[i];
    }
    list[i] = key;
    Sort(list, begin, i - 1);
    Sort(list, i + 1, end);
}

void Sort(double *list, int size){
    Sort(list, 0, size - 1);
}

void Cartesian2Polar(vector<NearPoint_t>& list, Polar_t polar){
    double aim_theta, aim_distance = 0;
    double theta[list.size()];
    for (int i = 0; i < list.size(); i++) theta[i] = list[i]->theta;
    Sort(theta, list.size());
    double max = 0;
    for (int i = 0; i < list.size() - 1; i++)
        if (theta[i + 1] - theta[i] > max){
            max = theta[i + 1] - theta[i];
            aim_theta = (theta[i + 1] + theta[i]) / 2;
            aim_distance = (list[i + 1]->rou + list[i]->rou) / 2;
        }
    polar->theta = aim_theta;
    polar->distance = aim_distance;
}