第十八届天下大学生智能汽车比赛室外赛5g室外专项赛（湘大队）开源 ...

序言：

        感谢十八届天下大学生智能汽车比赛-室外赛的主理方和组委会让我们有了参加这次比赛的机会！当我们在参加5g室外专项赛时，我们发现很多的组又很多的同砚都是第一次参加智能车比赛对于智能车的了解还不是很深，对于代码如何编写需要哪些模块也不是很熟悉，所以我们计划开源本方案，为未来的智能车参赛者提供一丝丝帮助！如果代码方面的问题可以私信我，若可以或许提供帮助，一定竭尽全力！
        github仓库：https://github.com/zzyt6/Smartcar_5g.git
项目简介

        1.利用c++编程实现团体代码
        2.纯竞速赛道可以将实现车体机械结构的最快速度巡线
        3.可以将官方的摄像头帧率跑满
1.相机改正程序

        官方规定利用的摄像头太过于抽象，如果不对相机的图片进行改正那么对于赛道的巡线结果会非常差。相机改正这在csdn上已经有很多教程，我的建议还是利用matlab去标定摄像头这样的数据更加尺度。

1. cv::Mat undistort(const cv::Mat &frame)
2. {
3.     double k1 = -0.340032906324299;
4.     double k2 = 0.101344757327394;
5.     double p1 = 0.0;
6.     double p2 = 0.0;
7.     double k3 = 0.0;
9.     cv::Mat K = (cv::Mat_<double>(3, 3) << 162.063205442089, 0.0, 154.707845362265,
10.                  0.0, 162.326264903804, 129.914361509615,
11.                  0.0, 0.0, 1.0);
13.     cv::Mat D = (cv::Mat_<double>(1, 5) << k1, k2, p1, p2, k3);
14.     cv::Mat mapx, mapy;
15.     cv::Mat undistortedFrame;
17.     cv::initUndistortRectifyMap(K, D, cv::Mat(), K, frame.size(), CV_32FC1, mapx, mapy);
18.     cv::remap(frame, undistortedFrame, mapx, mapy, cv::INTER_LINEAR);
20.     return undistortedFrame;
21. }

复制代码

2.赛道线提取

                本方案采用sobel算子，霍夫线段检测等方法对赛道线进行提取，我们通过对图片的剪裁进步了程序的运行速度，同时我们也利用了sobel算子的一些增强方式已便于更好的顺应赛道环境，无论赛道线的清楚水平，只要是在人眼可分辨的范围内，都可以进行赛道线段提取。

1. Mat ImagePreprocessing(const cv::Mat &frame_a)
2. {
3.     int width = 320;
4.     int height = 240;
6.     binaryImage_1 = cv::Mat::zeros(height, width, CV_8U);
7.     //--------这一段代码中包含了canny算子和一些图像处理的方案-------
8.     // cv::Mat grayImage;
9.     // cv::cvtColor(frame, grayImage, cv::COLOR_BGR2GRAY);
10.     // cv::Mat edgeImage;
11.     // cv::Canny(grayImage, edgeImage, 50, 150);
12.     // vector<Vec4i> lines;
13.     // HoughLinesP(edgeImage, lines, 1, CV_PI / 180, 180, 100, 10);
15.     // for (size_t i = 0; i < lines.size(); i++) {
16.     //     Vec4i l = lines[i];
17.     //     line(binaryImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(255), 4);
18.     // }
19.     // // GaussianBlur(frame, img, cv::Size(7, 7), 0);
20.     // cvtColor(img, hsv_image, cv::COLOR_BGR2HSV);
21.     // // Scalar lower_white(10, 43, 46);
22.     // // Scalar upper_white(180, 255, 255);
23.     // // inRange(hsv_image, lower_white, upper_white, white_mask);
24.     // // bitwise_not(white_mask, final_img);
25.     // // cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(1, 1));
27.     // // cv::morphologyEx(final_img, delite_frame, cv::MORPH_OPEN, kernel);
28.     // Mat img_edges;
29.     // Canny(frame, img_edges, 100, 150);
30.     // cv::Mat blurred;
31.     // cv::Mat edges;
32.     // cv::GaussianBlur(frame_a, blurred, cv::Size(5, 5), 0);
33.     // int roiYStart = 100;
34.     // int roiYEnd = 155;
35.     // // 创建感兴趣区域（ROI）
36.     // cv::Mat roiImage = blurred(cv::Range(roiYStart, roiYEnd), cv::Range(0, frame_a.cols));
37.     // // Canny边缘检测
38.     // cv::Canny(roiImage, edges, 100, 150);
40.     // // 创建全黑图像
41.     // cv::Mat blackImage = cv::Mat::zeros(frame_a.size(), CV_8UC1);
43.     // // 将Canny边缘叠加到全黑图像的感兴趣区域中
44.     // edges.copyTo(blackImage(cv::Range(roiYStart, roiYEnd), cv::Range(0, frame_a.cols)));
45.     // imshow("canny1",edges);
46.     // imshow("canny2",blackImage);
47.     //------------------------------------------------------
48.     //-------------------赛道条件好的情况--------------------
49.     // int kernelSize = 5;
50.     // double sigma = 1.0;
51.     // cv::Mat blurredImage;
52.     // cv::GaussianBlur(frame_a, blurredImage, cv::Size(kernelSize, kernelSize), sigma);
53.     // cv::Mat grad_x, grad_y;
54.     // cv::Mat sobel_x,sobel_y;
55.     // cv::Mat abs_grad_x, abs_grad_y;
56.     // int xWeight = 1;
57.     // int yWeight = 1;
58.     // cv::Sobel(blurredImage, grad_x, CV_16S, 1, 0, 3, xWeight);
59.     // cv::Sobel(blurredImage, grad_y, CV_16S, 0, 1, 3, yWeight);
60.     // cv::convertScaleAbs(grad_x, abs_grad_x);
61.     // cv::convertScaleAbs(grad_y, abs_grad_y);
62.     // cv::Mat edges;
63.     // cv::addWeighted(abs_grad_x, 0.5, abs_grad_y,0.5, 0, edges);
64.     // imshow("edges",edges);
65.     // cv::threshold(edges, binaryImage, 0, 255, cv::THRESH_BINARY + cv::THRESH_OTSU);
66.     // imshow("binaryImage",binaryImage);
67.     //---------------------赛道条件差的情况------------------
68.     // Sobel边缘检测
69.     Mat originalImage;
70.     cvtColor(frame, originalImage, cv::COLOR_BGR2GRAY);
71.     // float alpha = 0.2;  // 调整这个值以控制对比度增强的强度
72.     // Mat enhancedImage = customEqualizeHist(originalImage, alpha);
73.     Mat enhancedImage = originalImage;
74.     // Sobel边缘检测
75.     Mat sobelx, sobely;
76.     Sobel(enhancedImage, sobelx, CV_64F, 1, 0, 3);
77.     Sobel(enhancedImage, sobely, CV_64F, 0, 1, 3);
79.     Mat gradientMagnitude = abs(sobelx) + abs(sobely);
80.     convertScaleAbs(gradientMagnitude, gradientMagnitude);
82.     // 调整阈值
83.     Mat binaryImage12 = Mat::zeros(enhancedImage.size(), CV_8U);
84.     // threshold(gradientMagnitude, binaryImage12, 50, 255, THRESH_BINARY);
85.     cv::threshold(gradientMagnitude, binaryImage12, 0, 255, cv::THRESH_BINARY + cv::THRESH_OTSU);
86.     cv::Mat kernel1 = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
87.     cv::dilate(binaryImage12, binaryImage, kernel1, cv::Point(-1, -1), 1); // 这个地方也要修改
88.     // cv::dilate(binaryImage, binaryImage, kernel1, cv::Point(-1, -1), 1);
89.     int x_roi = 1;
90.     int y_roi = 109;
91.     int width_roi = 318;
92.     int height_roi = 46;
93.     cv::Rect roi(x_roi, y_roi, width_roi, height_roi);
95.     cv::Mat croppedObject = binaryImage(roi);
96.     vector<Vec4i> lines;
97.     HoughLinesP(croppedObject, lines, 1, CV_PI / 180, 25, 15, 10);
98.     for (size_t i = 0; i < lines.size(); i++)
99.     {
100.         Vec4i l = lines[i];
101.         double angle = atan2(l[3] - l[1], l[2] - l[0]) * 180.0 / CV_PI;
102.         double length = sqrt(pow(l[3] - l[1], 2) + pow(l[2] - l[0], 2));
103.         double aspect_ratio = length / abs(l[3] - l[1]);
104.         if (abs(angle) > 15)
105.         {
106.             Vec4i l = lines[i];
107.             l[0] += x_roi;
108.             l[1] += y_roi;
109.             l[2] += x_roi;
110.             l[3] += y_roi;
111.             line(binaryImage_1, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(255), 2, LINE_AA);
112.         }
113.     }
114.     return binaryImage_1;
115. }

复制代码

3.斑马线辨认

        本方案采用室内智能车的斑马线辨认方案，选取了图像中的ROI区域，用来进步斑马线的辨认率和减少对其的误判！

1. int crossroad(Mat frame)
2. {
3.     flag_cross = 0;
4.     int height = frame.rows;
5.     int width = frame.cols;
6.     Mat hsv;
7.     cvtColor(frame, hsv, COLOR_BGR2HSV);
9.     Scalar lower_white = Scalar(0, 0, 221);
10.     Scalar upper_white = Scalar(180, 30, 255);
12.     Mat mask1;
13.     inRange(hsv, lower_white, upper_white, mask1);
14.     Mat kernel = getStructuringElement(MORPH_RECT, Size(5, 5));
15.     dilate(mask1, mask1, kernel);
16.     erode(mask1, mask1, kernel);
17.     Mat src(mask1, Rect(100, 85, 120, 60));
18.     int cout1 = 0, cout2 = 0, flag = 0;
19.     for (int i = 0; i < src.rows; i++)
20.     {
21.         if (cout1 < 10)
22.         {
23.             flag = 0;
24.         }
25.         cout1 = 0;
26.         for (int j = 10; j < src.cols - 10; j++)
27.         {
28.             if (src.at<char>(i, j - 2) == 0 && src.at<uchar>(i, j) == 0 && src.at<uchar>(i, j - 1) == 0 && src.at<uchar>(i, j + 1) == 255 && src.at<uchar>(i, j + 2) == 255)
29.             {
30.                 cout1++;
31.             }
32.             else if (src.at<uchar>(i, j - 2) == 255 && src.at<uchar>(i, j) == 255 && src.at<uchar>(i, j - 1) == 255 && src.at<uchar>(i, j + 1) == 0 && src.at<uchar>(i, j + 2) == 0)
33.             {
34.                 cout1++;
35.             }
36.             if (cout1 >= 10)
37.             {
38.                 cout2++;
39.                 flag++;
40.                 if (flag >= 3)
41.                 {
42.                     cout << "斑马线" << endl;
43.                     flag_cross = 1;
44.                 }
45.                 break;
46.             }
47.         }
48.     }
49.     cout << "flag_cross" << flag_cross << endl;
50.     return flag_cross;
51. }

复制代码

4.黄线辨认

        本方案利用了两种方式对于黄线进行检测分别为HSV颜色分割和Canny边缘检测，同时本方案配备了对黄线色域阈值调整提取程序，可以对不同光照条件下的黄线实现辨认
HSV版本

1. int yellow_hsv(Mat img, bool visual_flag)
2. {
3.     Mat cropped_image, canvas, image;
4.     Scalar lowerb = Scalar(3, 0, 0);
5.     Scalar upperb = Scalar(40, 100, 255);
6.     int yellow_num = 0;
7.     int height = img.rows;
8.     int width = img.cols;
9.     int half_height = int(height / 2);
10.     int per_height = int(height / 20);
11.     int area_1 = int(0.002775 * height * width);
12.     int area_2 = int(0.025 * height * width);
13.     image = img.clone();
14.     cropped_image = image(Rect(0, half_height - per_height, width, per_height * 3));
15.     if (visual_flag == true)
16.     {
17.         canvas = cropped_image.clone();
18.     }
19.     cvtColor(cropped_image, cropped_image, COLOR_BGR2HSV);
20.     morphologyEx(cropped_image, cropped_image, MORPH_OPEN, getStructuringElement(MORPH_RECT, Size(3, 3)));
21.     inRange(cropped_image, lowerb, upperb, cropped_image);
22.     vector<vector<Point>> contours;
23.     vector<Vec4i> hierarchy;
24.     findContours(cropped_image, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
25.     for (size_t i = 0; i < contours.size(); i++)
26.     {
27.         double area = contourArea(contours[i]);
28.         if (area > area_1 && area < area_2)
29.         {
30.             Rect rect = boundingRect(contours[i]);
31.             double aspect_ratio = rect.width / rect.height;
32.             if (aspect_ratio > 10)
33.             {
34.                 if (visual_flag == true)
35.                 {
36.                     rectangle(canvas, rect, Scalar(255, 0, 0), 2, LINE_AA);
37.                     printf("x: %d, y: %d, width: %d, height: %d, aspect_ratio: %f\n", rect.x, rect.y, rect.width,
38.                            rect.height, aspect_ratio);
39.                 }
40.                 yellow_num += 1;
41.             }
42.         }
43.     }
44.     if (visual_flag == true)
45.     {
46.         imshow("Image_hsv", canvas);
47.         waitKey(0);
48.     }
49.     return yellow_num;
50. }

复制代码

Canny版本

1. int yellow_edge(Mat img, bool visual_flag)
2. {
3.     Mat cropped_image, canvas, image;
4.     int yellow_num = 0;
5.     int height = img.rows;
6.     int width = img.cols;
7.     int half_height = int(height / 2);
8.     int per_height = int(height / 20);
9.     image = img.clone();
10.     cropped_image = image(Rect(0, half_height - per_height, width, per_height * 3));
11.     if (visual_flag == true)
12.     {
13.         canvas = cropped_image.clone();
14.     }
15.     cvtColor(cropped_image, cropped_image, COLOR_BGR2GRAY);
16.     Canny(cropped_image, cropped_image, 50, 150, 3);
17.     vector<Vec4i> lines;
18.     HoughLinesP(cropped_image, lines, 1, CV_PI / 180, 150, 125, 10);
19.     if (lines.size() == 0)
20.     {
21.         printf("No yellow edge detected!\n");
22.         return 0;
23.     }
24.     else
25.     {
26.         for (size_t i = 0; i < lines.size(); i++)
27.         {
28.             Vec4i l = lines[i];
29.             double angle = atan2(l[3] - l[1], l[2] - l[0]) * 180.0 / CV_PI;
30.             double length = sqrt(pow(l[3] - l[1], 2) + pow(l[2] - l[0], 2));
31.             double aspect_ratio = length / abs(l[3] - l[1]);
32.             if (abs(angle) < 5 && aspect_ratio > 5)
33.             {
34.                 if (visual_flag == true)
35.                 {
36.                     line(canvas, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(255, 0, 0), 2, LINE_AA);
37.                     printf("x: %d, y: %d, x1: %d, y1: %d, angle: %f, length: %f, aspect_ratio: %f\n", l[0], l[1], l[2],
38.                            l[3], angle, length, aspect_ratio);
39.                 }
40.                 yellow_num += 1;
41.             }
42.         }
43.     }
44.     if (visual_flag == true)
45.     {
46.         imshow("Image_edge", canvas);
47.         waitKey(0);
48.     }
49.     return yellow_num;
50. }

复制代码

阈值调整程序

        队伍中的大佬最近在测验，不理我，这个等寒假再更新！！！
5.蓝色挡板辨认

        蓝色挡板分为两部门：1.检测到蓝色挡板，2.蓝色挡板移开
1.检测到蓝色挡板

1. void blue_card_find(void)
2. {
3.     vector<vector<Point>> contours;
4.     vector<Vec4i> hierarcy;
5.     findContours(mask, contours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
6.     if (contours.size() > 0)
7.     {
8.         sort(contours.begin(), contours.end(), Contour_Area);
9.         vector<vector<Point>> newContours;
10.         for (const vector<Point> &contour : contours)
11.         {
12.             Point2f center;
13.             float radius;
14.             minEnclosingCircle(contour, center, radius);
15.             if (center.y > 90 && center.y < 160)
16.             {
17.                 newContours.push_back(contour);
18.             }
19.         }
21.         contours = newContours;
23.         if (contours.size() > 0)
24.         {
25.             if (contourArea(contours[0]) > 500)
26.             {
27.                 cout << "find biggest blue" << endl;
28.                 Point2f center;
29.                 float radius;
30.                 minEnclosingCircle(contours[0], center, radius);
31.                 circle(frame, center, static_cast<int>(radius), Scalar(0, 255, 0), 2);
32.                 find_first = 1;
33.             }
34.             else
35.             {
36.                 cout << "not found blue" << endl;
37.             }
38.         }
39.     }
40.     else
41.     {
42.         cout << "not found blue" << endl;
43.     }
44. }

复制代码

2.蓝色挡板移开

1. void blue_card_remove(void)
2. {
3.     cout << "entry move blue process" << endl;
4.     vector<vector<Point>> contours;
5.     vector<Vec4i> hierarcy;
6.     findContours(mask, contours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
7.     if (contours.size() > 0)
8.     {
9.         sort(contours.begin(), contours.end(), Contour_Area);
10.         vector<vector<Point>> newContours;
11.         for (const vector<Point> &contour : contours)
12.         {
13.             Point2f center;
14.             float radius;
15.             minEnclosingCircle(contour, center, radius);
16.             if (center.y > 90 && center.y < 160)
17.             {
18.                 newContours.push_back(contour);
19.             }
20.         }
22.         contours = newContours;
24.         if (contours.size() == 0)
25.         {
26.             begin_sign = 0;
27.             cout << "move" << endl;
28.             sleep(2);
29.         }
30.     }
31.     else
32.     {
33.         begin_sign = 0;
34.         cout << "蓝色挡板移开" << endl;
35.         sleep(2);
36.     }
37. }

复制代码

6.锥桶避障方案

        锥桶避障现实上就是对锥桶的颜色进行辨认，再利用锥桶中心位置对图像进行补线，从而实现动态路径规划，这一部门代码分为三部门对于1.锥桶颜色进行辨认，2.根据锥桶位置对锥桶进行判断是否补线，3.根据锥桶位置对补线方案进行替换，4.对图像补线后进行再次巡线
补线程序（锥桶主程序）

1. void blue_vertebral_model(void)
2. {
3.     contours_all = blue_vertebral_barrel_find_all(mask);
4.     if (contours_all.size() != 0)
5.     {
6.         Point2f center;
7.         float radius;
8.         minEnclosingCircle(contours_all[0], center, radius);
9.         heighest = center.y;
10.         if (try_patching_line == 2)
11.         {
12.             bin_image2 = drawWhiteLine(bin_image2, Point(int(center.x), int(center.y)), Point(int((right_line[0].x + right_line[1].x + right_line[2].x) / 3), 155),
13.                                        8);
14.             cout << "center.x:" << center.x << endl;
15.             cout << "center.y:" << center.y << endl;
16.             cout << "1" << endl;
17.         }
18.         else if (try_patching_line == 1)
19.         {
20.             if (count_change != 2)
21.             {
22.                 bin_image2 = drawWhiteLine(bin_image2, Point(int(center.x), int(center.y)), Point(int((left_line[0].x + left_line[1].x + left_line[2].x) / 3), 155),
23.                                            8);
24.             }
25.             else
26.             {
27.                 bin_image2 = drawWhiteLine(bin_image2, Point(int(center.x - 20), int(center.y)), Point(int((left_line[0].x + left_line[1].x + left_line[2].x) / 3), 155),
28.                                            8);
29.             }
30.             cout << "center.x:" << center.x << endl;
31.             cout << "center.y:" << center.y << endl;
32.             cout << "2" << endl;
33.         }
34.     }
35. }

复制代码

锥桶的判断程序和补线方式转换程序

1. vector<vector<Point>> blue_vertebral_barrel_find_all(Mat mask)
2. {
3.     vector<vector<Point>> contours;
4.     vector<Vec4i> hierarcy;
5.     Point2f center;
6.     float radius;
7.     findContours(mask, contours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
8.     if (contours.size() > 0)
9.     {
10.         vector<vector<Point>> newContours;
11.         for (const vector<Point> &contour : contours)
12.         {
13.             Point2f center;
14.             float radius;
15.             minEnclosingCircle(contour, center, radius);
16.             if (center.y > 108 && center.y < 153)
17.             {
18.                 newContours.push_back(contour);
19.             }
20.         }
21.         contours = newContours;
22.     }
23.     if (contours.size() > 0)
24.     {
25.         vector<vector<Point>> newContours2;
26.         for (const vector<Point> &contour : contours)
27.         {
28.             if (contourArea(contour) > 10)
29.             {
30.                 newContours2.push_back(contour);
31.             }
32.         }
33.         contours = newContours2;
34.     }
35.     if (contours.size() > 0)
36.     {
37.         vector<vector<Point>> newContours5;
38.         for (const vector<Point> &contour : contours)
39.         {
40.             Point2f center;
41.             float radius;
42.             minEnclosingCircle(contour, center, radius);
43.             center.x = (int)center.x;
44.             center.y = (int)center.y;
45.             if (center.x > left_line[center.y - 108].x && center.x < right_line[center.y - 108].x)
46.             {
47.                 cout << "过滤后的点：center.x" << center.x << endl;
48.                 cout << "center.y " << center.y << endl;
49.                 cout << "left_line " << left_line[center.y - 108].x << endl;
50.                 cout << "right_line " << right_line[center.y - 108].x << endl;
51.                 cv::circle(frame, Point(center.x, center.y), 10, cv::Scalar(0, 0, 0), -1);
52.                 newContours5.push_back(contour);
53.             }
54.         }
55.         contours = newContours5;
56.     }
57.     if (contours.size() > 0)
58.     {
59.         sort(contours.begin(), contours.end(), Contour_Area);
60.         now_blue_max = (int)contourArea(contours[0]);
61.     }
62.     else
63.     {
64.         now_blue_max = 0;
65.     }
66.     vector<vector<Point>> newContours4;
67.     newContours4 = contours;
68.     if (contours.size() > 0)
69.     {
70.         vector<vector<Point>> newContours3;
71.         for (const vector<Point> &contour : contours)
72.         {
73.             if (contourArea(contour) < 140)
74.             {
75.                 newContours3.push_back(contour);
76.             }
77.         }
78.         contours = newContours3;
79.     }
80.     // cout << "now_blue_max" << now_blue_max <<endl;
81.     // cout << "contours.size()" << contours.size() <<endl;
82.     if (contours.size() == 0 && newContours4.size() != 0)
83.     {
84.         if (last_blue == 0)
85.         {
86.             if (try_patching_line == 1)
87.             {
88.                 try_patching_line = 2;
89.             }
90.             else if (try_patching_line == 2)
91.             {
92.                 try_patching_line = 1;
93.             }
94.             cout << "--------------------------------补线方式转换------------------------------" << endl;
95.             number1 = 0;
96.             count_change++;
97.         }
98.     }
99.     if (now_blue_max > 140)
100.     {
101.         last_blue = 1;
102.     }
103.     else
104.     {
105.         last_blue = 0;
106.     }
107.     return contours;
108. }

复制代码

7.控制代码

        本方案采用的控制代码与室内智能车差不多，但是建议在锥桶避障时采用多套PID，同时可以思量利用赛道中线方差实现变加速控制（团队做了背面没用因为确实没啥用！），这里给一些示例代码可以供参考！
1.控制程序

1. double average(vector<int> vec)
2. {
3.     if (vec.size() < 1)
4.         return -1;
6.     double sum = 0;
7.     for (int i = 0; i < vec.size(); i++)
8.     {
9.         sum += vec[i];
10.     }
12.     return (double)sum / vec.size();
13. }
14. double sigma(vector<int> vec)
15. {
16.     if (vec.size() < 1)
17.         return 0;
19.     double aver = average(vec);
20.     double sigma = 0;
21.     for (int i = 0; i < vec.size(); i++)
22.     {
23.         sigma += (vec[i] - aver) * (vec[i] - aver);
24.     }
25.     sigma /= (double)vec.size();
26.     return sigma;
27. }
28. void motor_servo_contral(int flag_yellow_cond, int cross)
29. {
30.     if (flag_yellow_cond != 0 && flag_yellow_finish == 0 && flag_cross_finish == 1)
31.     {
32.         flag_yellow_finish = 1;
33.         gpioPWM(12, 730);
34.         gpioPWM(13, 10000);
35.         usleep(250000);
36.         gpioPWM(13, 9800);
37.         gpioPWM(13, 8800);
38.         usleep(250000);
39.         _exit(0);
40.     }
41.     else if (cross == 1 && flag_cross_finish == 0)
42.     {
43.         flag_cross_finish = 1;
44.         gpioPWM(13, 9800);
45.         gpioPWM(13, 8900);
46.         usleep(550000);
47.         gpioPWM(12, 730);
48.         gpioPWM(13, 10000);
49.         sleep(3);
50.     }
51.     else if (contours_all.size() != 0 && count_change < 3 && number1 >= 7 && flag_cross_finish == 1)
52.     {
53.         if (try_patching_line == 2 && count_change < 3 && count_change >= 1)
54.         {
55.             float servo_pwm_chayan = servo_pd_blue(160);
56.             servo_pwm_now = servo_pwm_chayan;
57.         }
58.         else if (try_patching_line == 1 && count_change >= 1 && count_change < 3)
59.         {
60.             float servo_pwm_chayan = servo_pd_blue(160);
61.             servo_pwm_now = servo_pwm_chayan;
62.         }
63.         else
64.         {
65.             servo_pwm_now = servo_pd_blue(160);
66.         }
67.         cout << "bin_imagepwm" << servo_pwm_now << endl;
68.         if (times4 == 0)
69.         {
70.             times4 = 1;
71.             // gpioPWM(13, 9800);
72.             // gpioPWM(13, 8700);
73.             gpioPWM(13, 10000);
74.             // usleep(50000);
75.         }
76.         // gpioPWM(13, 10000);//11000
77.         // gpioPWM(13, 10500);
78.         gpioPWM(12, servo_pwm_now);
79.     }
80.     else
81.     {
82.         if (count_change < 1 || count_change > 2)
83.         {
84.             cout << 1 << endl;
85.             if (count_change > 2 && flag_cross_finish == 1)
86.             {
87.                 servo_pwm_now = servo_pd(160);
88.                 cout << "pwm" << servo_pwm_now << endl;
89.                 gpioPWM(13, 13000);
90.                 gpioPWM(12, servo_pwm_now);
91.             }
92.             else if (count_change < 1 && flag_cross_finish == 0)
93.             {
94.                 servo_pwm_now = servo_pd(160);
95.                 cout << "pwm" << servo_pwm_now << endl;
96.                 gpioPWM(13, 12000);
97.                 gpioPWM(12, servo_pwm_now);
98.             }
99.             else
100.             {
101.                 cout << "after" << endl;
102.                 servo_pwm_now = servo_pd_after(160);
103.                 cout << "pwm" << servo_pwm_now << endl;
104.                 gpioPWM(13, 11700);
105.                 gpioPWM(12, servo_pwm_now);
106.             }
107.         }
108.         else
109.         {
110.             cout << 2 << endl;
111.             servo_pwm_now = servo_pd_left(160);
112.             cout << "pwm" << servo_pwm_now << endl;
113.             gpioPWM(13, 11400);
114.             gpioPWM(12, servo_pwm_now);
115.         }
116.     }
117.     // ----------------------------------变加速控制-------------------------------
118.     // uint8_t controlLow = 0;   // 速度控制下限
119.     // uint8_t controlMid = 5;   
120.     // uint8_t controlHigh = 10; // 速度控制上限
121.     // float servo_pwm_now = servo_pd(320);
122.     // if (mid.size() > 20)
123.     // {
124.     //     vector<POINT> centerV;
125.     //     int filt = mid.size() / 5;
126.     //     for (int i = filt; i < mid.size() - filt; i++)
127.     //     {
128.     //         centerV.push_back(mid[i]);
129.     //     }
130.     //     sigmaCenter = sigma(centerV);
131.     // }
132.     // else
133.     //     sigmaCenter = 1000;
134.     // if (abs(sigmaCenter) < 100.0)
135.     // {
136.     //     counterShift++;
137.     //     if (counterShift > controlHigh)
138.     //         counterShift = controlHigh;
139.     // }
140.     // else
141.     // {
142.     //     counterShift--;
143.     //     if (counterShift < controlLow)
144.     //         counterShift = controlLow;
145.     // }
147.     // if (counterShift > controlMid)
148.     // {
149.     //     motorSpeed = speedhigh;
150.     //     cout << "高" << endl;
151.     // }
152.     // else
153.     // {
154.     //     motorSpeed = speedlow;
155.     //     cout << "低" << endl;
156.     // }
157.     // gpioPWM(13, motorSpeed);
158.     // gpioPWM(12, servo_pwm_now);
159.     // ------------------------------------纯竞速---------------------------------
160.     // if (flag_yellow_cond != 0 && flag_yellow_finish == 0 && flag_cross_finish == 1)
161.     // {
162.     //     flag_yellow_finish = 1;
163.     //     gpioPWM(12, 730);
164.     //     gpioPWM(13, 10000);
165.     //     usleep(250000);
166.     //     gpioPWM(13, 9800);
167.     //     gpioPWM(13, 8800);
168.     //     usleep(250000);
169.     //     cout << "停止" << endl;
170.     //     _exit(0);
171.     // }
172.     // else if (cross == 1 && flag_cross_finish == 0)
173.     // {
174.     //     flag_cross_finish = 1;
175.     //     cout << "11" << endl;
176.     //     gpioPWM(13, 9800);
177.     //     gpioPWM(13, 8900);
178.     //     usleep(550000); //
179.     //     gpioPWM(12, 730);
180.     //     gpioPWM(13, 10000);
181.     //     sleep(3);
182.     // }
183.     // else if (contours_all.size() != 0 && count_change < 3 && number1 >= 7)
184.     // {
185.     //     if (try_patching_line == 2 && count_change < 3 && count_change >= 1)
186.     //     {
187.     //         float servo_pwm_chayan = servo_pd_blue(160);
188.     //         servo_pwm_now = servo_pwm_chayan;
189.     //     }
190.     //     else if (try_patching_line == 1 && count_change >= 1 && count_change < 3)
191.     //     {
192.     //         float servo_pwm_chayan = servo_pd_blue(160);
193.     //         servo_pwm_now = servo_pwm_chayan;
194.     //     }
195.     //     else
196.     //     {
197.     //         servo_pwm_now = servo_pd_blue(160);
198.     //     }
199.     //     if (times4 == 0)
200.     //     {
201.     //         times4 = 1;
202.     //         gpioPWM(13, 9800);
203.     //         gpioPWM(13, 8700);
204.     //         usleep(100000);
205.     //     }
206.     //     gpioPWM(13, 10000);//11000
207.     //     gpioPWM(12, servo_pwm_now);
208.     // }
209.     // else
210.     // {
211.     //     if (flag_cross_finish == 0)
212.     //     {
213.     //         servo_pwm_now = servo_pd(160);
214.     //         gpioPWM(13, 12000);
215.     //         gpioPWM(12, servo_pwm_now);
217.     //     }
218.     //     else
219.     //     {
220.     //         servo_pwm_now = servo_pd(160);
221.     //         gpioPWM(13, 12500);
222.     //         gpioPWM(12, servo_pwm_now);
224.     //     }
225.     // }
226. }

复制代码

2.PID控制程序

        本方案直接利用pwm波数值进行舵机控制，如果您利用了逆透视的方案来做巡线，那么我更加建议利用角度来做pwm波控制。

1. float servo_pd(int target)
2. {
3.     int size = int(mid.size());
4.     int pidx = int((mid[23].x + mid[20].x + mid[25].x) / 3);
5.     error_first = target - pidx;
6.     servo_pwm_diff = kp * error_first + kd * (error_first - last_error);
7.     // cout << "servo_pwm_diff:" << servo_pwm_diff << endl;
8.     last_error = error_first;
9.     servo_pwm = 720 + servo_pwm_diff;
10.     if (servo_pwm > 900)
11.     {
12.         servo_pwm = 900;
13.     }
14.     else if (servo_pwm < 600)
15.     {
16.         servo_pwm = 600;
17.     }
18.     return servo_pwm;
19. }

复制代码

8.附属模块代码程序

1.拍照程序（python版）

1. import cv2
2. import os
3. import numpy as np
7. def undistort(frame):
8.     k1, k2, p1, p2, k3 = -0.340032906324299, 0.101344757327394,0.0,0.0, 0.0
10.     k = np.array([
11.         [162.063205442089, 0, 154.707845362265],
12.         [0, 162.326264903804,129.914361509615],
13.         [0, 0, 1]
14.     ])
15.     # 畸变系数
16.     d = np.array([
17.         k1, k2, p1, p2, k3
18.     ])
19.     height, weight = frame.shape[:2]
20.     mapx, mapy = cv2.initUndistortRectifyMap(k, d, None, k, (weight, height), 5)
22.     return cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)
24. cap = cv2.VideoCapture(0)
25. cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)  #设置宽度
26. cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)  #设置长度
27. if not cap.isOpened():
28.     print("无法打开摄像头")
29. else:
30.     while True:
31.         ret, frame = cap.read()
33.         if not ret:
34.             print("无法捕捉")
35.             break
36.         frame = undistort(frame)
37.         cv2.imshow('Press "q" to Capture and Quit', frame)
39.         key = cv2.waitKey(1)
41.         # 如果用户按下 "q"，拍照并保存，然后退出程序?
42.         if key & 0xFF == ord('q'):
43.             photo_path = os.path.join("test_photo", '40.jpg')
44.             cv2.imwrite(photo_path, frame)
45.             print(f"已保存照片为 {photo_path}")
46.             break
48.     cap.release()
50. cv2.destroyAllWindows()

复制代码

2.蓝色挡板测试程序（python版）

1. import cv2
2. import numpy as np
4. # 使用比较函数对轮廓进行排序
5. def contour_area(contour):
6.     return cv2.contourArea(contour)
8. def process_frame(frame):
9.     change_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
10.     lower_bound = np.array([100, 43, 46], dtype=np.uint8)
11.     upper_bound = np.array([124, 255, 255], dtype=np.uint8)
12.     mask = cv2.inRange(change_frame, lower_bound, upper_bound)
13.     kernel = np.ones((5, 5), np.uint8)
14.     mask = cv2.dilate(mask, kernel, iterations=1)
15.     mask = cv2.erode(mask, kernel, iterations=1)
16.     return mask
20. cap = cv2.VideoCapture(0)
21. find_first = 0
22. begin_sign = 0
23. while True:
24.     ret, frame = cap.read()
25.     processed_frame = process_frame(frame)
26.     contours, hierarchy = cv2.findContours(processed_frame, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
27.     if begin_sign == 0:
28.         if find_first == 0:
29.             if len(contours) > 0:
30.                 contours = sorted(contours, key=cv2.contourArea, reverse=True)
31.                 new_contours = []
32.                 for contour in contours:
33.                     (x, y), radius = cv2.minEnclosingCircle(contour)
34.                     center = (int(x), int(y))
35.                     # 去除掉上面和下面的噪点
36.                     if center[1] > 180 and center[1] < 320:
37.                         new_contours.append(contour)
39.                 # 将新的列表赋值给 contours
40.                 contours = new_contours
41.                 if(len(contours) > 0):
43.                 # 检查列表中第一个轮廓的面积是否大于 1300
44.                     if cv2.contourArea(contours[0]) > 1300:
45.                         print("找到了最大的蓝色挡板")
46.                         (x, y), radius = cv2.minEnclosingCircle(contours[0])
47.                         center = (int(x), int(y))
48.                         radius = int(radius)
49.                         cv2.circle(frame, center, radius, (0, 255, 0), 2)
50.                         find_first = 1
51.                     else:
52.                         print("没找到蓝色挡板")
53.         else:
54.             print("进入移开挡板的程序")
55.             if len(contours) > 0:
56.                 contours = sorted(contours, key=cv2.contourArea, reverse=True)
57.                 new_contours = []
58.                 for contour in contours:
59.                     (x, y), radius = cv2.minEnclosingCircle(contour)
60.                     center = (int(x), int(y))
61.                     # 去除掉上面和下面的噪点
62.                     if center[1] > 180 and center[1] < 320:
63.                         new_contours.append(contour)
65.                 contours = new_contours
66.                 if(len(contours) > 0):
67.                     if cv2.contourArea(contours[0]) < 300:
68.                         begin_sign = 1
69.                         print("挡板移开")
70.                 else:
71.                     begin_sign = 1
72.                     print("挡板移开")
73.     cv2.imshow('Original Frame', frame)
74.     cv2.imshow('Processed Frame', processed_frame)
76.     # 检测按键，如果按下Esc键则退出循环
77.     if cv2.waitKey(1) == 27:
78.         break
80. cap.release()
81. cv2.destroyAllWindows()

复制代码

3.锥桶轮廓面积测试程序（python版）

1. import cv2
2. import numpy as np
4. def find_blue_contours(image_path):
5.     # 读取图像
6.     image = cv2.imread(image_path)
8.     # 将图像转换为HSV颜色空间
9.     hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
11.     # 定义蓝色的HSV范围
12.     lower_blue = np.array([100, 43, 46])
13.     upper_blue = np.array([124, 255, 255])
15.     # 创建一个蓝色掩码
16.     mask = cv2.inRange(hsv, lower_blue, upper_blue)
18.     # 执行形态学操作，可以根据实际情况调整参数
19.     kernel = np.ones((5, 5), np.uint8)
20.     mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
21.     mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
23.     # 寻找轮廓
24.     contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
26.     # 仅保留y轴在210-320范围内的轮廓
27.     filtered_contours = [contour for contour in contours if 95 <= cv2.boundingRect(contour)[1] <= 165]
29.     # 在图像上画出轮廓，并写出大小
30.     for i, contour in enumerate(filtered_contours):
31.         area = cv2.contourArea(contour)
32.         cv2.drawContours(image, [contour], 0, (0, 255, 0), 2)
33.         cv2.putText(image, f"Contour {i + 1}: {area:.2f}", (10, 30 * (i + 1)), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
35.     # 显示图像
36.     cv2.imshow("Blue Contours", image)
37.     cv2.waitKey(0)
38.     cv2.destroyAllWindows()
40. if __name__ == "__main__":
41.     image_path = "/home/pi/test_photo/blue_zhuitong8.jpg"  # 替换为你的图像文件路径
42.     find_blue_contours(image_path)

复制代码

4.相机改正程序（python版）

1. # 摄像头畸变矫正函数，输入待矫正的图形变量
2. import cv2
3. import os
4. import numpy as np
6. def undistort(frame):
7.     k1, k2, p1, p2, k3 = -0.287246515012261, 0.066176222325459, 0.005615032474715,0.003425003902561, 0.0
9.     # 相机坐标系到像素坐标系的转换矩阵
10.     k = np.array([
11.         [3.111337497474041e+02, -2.333471935388314, 2.915941445374422e+02],
12.         [0, 3.109853062871910e+02, 2.473500696130221e+02],
13.         [0, 0, 1]
14.     ])
15.     # 畸变系数
16.     d = np.array([
17.         k1, k2, p1, p2, k3
18.     ])
19.     height, weight = frame.shape[:2]
20.     mapx, mapy = cv2.initUndistortRectifyMap(k, d, None, k, (weight, height), 5)
22.     # 返回矫正好的图形变量
23.     return cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)
24. # 打开摄像头
25. cap = cv2.VideoCapture(0)  # 0表示默认摄像头，如果有多个摄像头，可以尝试不同的数字来选择不同的摄像头
27. if not cap.isOpened():
28.     print("无法打开摄像头.")
29. else:
30.     while True:
31.         # 捕捉一帧
32.         ret, frame = cap.read()
34.         if not ret:
35.             print("无法捕捉帧.")
36.             break
37.         frame = undistort(frame)
38.         # 显示当前帧
39.         cv2.imshow('Press "q" to Capture and Quit', frame)
41.         # 等待用户按下键盘
42.         key = cv2.waitKey(1)
44.         # # 如果用户按下 "q" 键，拍照并保存，然后退出程序
45.         # if key & 0xFF == ord('q'):
46.         #     # 构造完整的文件路径
47.         #     photo_path = os.path.join("test_photo", '30.jpg')
48.         #     cv2.imwrite(photo_path, frame)
49.         #     print(f"已保存照片为 {photo_path}")
50.         #     break
52.     # 释放摄像头
53.     cap.release()
55. # 关闭所有窗口
56. cv2.destroyAllWindows()

复制代码

5.Canny边缘检测阈值调整程序（c++版本）

1. #include <opencv2/opencv.hpp>
3. using namespace cv;
5. // 回调函数
6. void onTrackbarChange(int, void *)
7. {
8.     // 什么也不做
9. }
11. int main()
12. {
13.     // 读取图片
14.     Mat img_original = imread("/home/pi/test_photo/1.jpg");
16.     // 创建窗口
17.     namedWindow("Canny");
19.     // 创建两个滑动条，分别控制threshold1和threshold2
20.     int threshold1 = 50;
21.     int threshold2 = 100;
23.     createTrackbar("threshold1", "Canny", &threshold1, 400, onTrackbarChange);
24.     createTrackbar("threshold2", "Canny", &threshold2, 400, onTrackbarChange);
26.     while (true)
27.     {
28.         // Canny边缘检测
29.         Mat img_edges;
30.         Canny(img_original, img_edges, threshold1, threshold2);
32.         // 显示图片
33.         imshow("original", img_original);
34.         imshow("Canny", img_edges);
36.         // 检测键盘输入，如果按下 'q' 键，退出循环
37.         if (waitKey(1) == 'q')
38.         {
39.             break;
40.         }
41.     }
43.     destroyAllWindows();
45.     return 0;
46. }

复制代码

6.HoughLinesP阈值调整程序（c++版本）

1. #include <opencv2/opencv.hpp>
2. using namespace cv;
3. using namespace std;
5. Mat g_mask1Image, g_midImage, g_mask1GrayImage;
6. cv::Mat sobelX8U;
7. cv::Mat blurredImage;
8. cv::Mat sobelX;
9. cv::Mat binaryImage;
10. cv::Mat croppedObject;
11. int g_HoughLinesThreshold = 150;
12. int g_minLineLength = 100;
14. void on_HoughLines(int, void *);
15. Mat customEqualizeHist(const Mat &inputImage, float alpha)
16. {
17.     Mat enhancedImage;
18.     equalizeHist(inputImage, enhancedImage);
20.     // 减弱对比度增强的效果
21.     return alpha * enhancedImage + (1 - alpha) * inputImage;
22. }
23. int main()
24. {
25.     g_mask1Image = imread("/home/pi/test_photo/1.jpg", cv::IMREAD_GRAYSCALE);
26.     if (!g_mask1Image.data)
27.     {
28.         return -1;
29.     }
31.     imshow("g_mask1Image", g_mask1Image);
32.     // float alpha = 0.5;  // 调整这个值以控制对比度增强的强度
33.     // Mat enhancedImage = customEqualizeHist(g_mask1Image, alpha);
34.     Mat enhancedImage = g_mask1Image;
35.     // Sobel边缘检测
36.     Mat sobelx, sobely;
37.     Sobel(enhancedImage, sobelx, CV_64F, 1, 0, 3);
38.     Sobel(enhancedImage, sobely, CV_64F, 0, 1, 3);
40.     Mat gradientMagnitude = abs(sobelx) + abs(sobely);
41.     convertScaleAbs(gradientMagnitude, gradientMagnitude);
43.     // 调整阈值
44.     Mat binaryImage12 = Mat::zeros(enhancedImage.size(), CV_8U);
45.     // threshold(gradientMagnitude, binaryImage12, 50, 255, THRESH_BINARY);
46.     cv::threshold(gradientMagnitude, binaryImage12, 0, 255, cv::THRESH_BINARY + cv::THRESH_OTSU);
47.     imshow("binaryImage", binaryImage12);
48.     imshow("gradientMagnitude", gradientMagnitude);
49.     cv::Mat kernel1 = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
50.     cv::dilate(binaryImage12, binaryImage, kernel1, cv::Point(-1, -1), 1);
51.     // imshow("1",binaryImage);
52.     int x_roi = 1;
53.     int y_roi = 109;
54.     int width_roi = 318;
55.     int height_roi = 45;
56.     cv::Rect roi(x_roi, y_roi, width_roi, height_roi);
58.     croppedObject = binaryImage(roi);
59.     namedWindow("HoughLines", WINDOW_AUTOSIZE);
60.     createTrackbar(" g_HoughLinesThreshold", "HoughLines", &g_HoughLinesThreshold, 150, on_HoughLines);
61.     createTrackbar("g_minLineLength", "HoughLines", &g_minLineLength, 100, on_HoughLines);
63.     on_HoughLines(0, 0);
65.     while (char(waitKey(1)) != 'q')
66.     {
67.     }
69.     waitKey(0);
70.     return 0;
71. }
73. void on_HoughLines(int, void *)
74. {
75.     cvtColor(croppedObject, g_mask1GrayImage, COLOR_GRAY2BGR);
77.     vector<Vec4i> lines
78.         HoughLinesP(croppedObject, lines, 1, CV_PI / 180, g_HoughLinesThreshold, g_minLineLength, 10);
80.     for (size_t i = 0; i < lines.size(); i++)
81.     {
82.         Vec4i l = lines[i];
83.         line(g_mask1GrayImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(186, 88, 255), 1, LINE_AA);
84.     }
86.     imshow("HoughLines", g_mask1GrayImage);
87. }

复制代码

尾言

        感谢湘潭大学电工电子实行室为我们提供的参赛机会，感谢向礼丹老师和电工电子实行室的每个指导老师，感谢十七届参加室外赛的朱嘉明和崔全学长，衷心祝愿各位未来参加智能车比赛的车友可以或许赛出佳绩！在智能车比赛中再创辉煌！



免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。