序言：
感谢十八届全国大学生智能汽车竞赛-室外赛的主办方和组委会让我们有了参加这次比赛的机会！当我们在参加5g室外专项赛时，我们发现很多的组又很多的同学都是第一次参加智能车竞赛对于智能车的了解还不是很深，对于代码如何编写需要哪些模块也不是很熟悉，所以我们打算开源本方案，为未来的智能车参赛者提供一丝丝帮助！如果代码方面的问题可以私信我，若能够提供帮助，一定竭尽全力！
后续项目的源码会在github更新，同时我们打算更新GPS和陀螺仪利用惯导技术来完成比赛，尽情期待！
项目简介
1.使用c++编程实现整体代码
2.纯竞速赛道可以将实现车体机械结构的最快速度巡线
3.可以将官方的摄像头帧率跑满
1.相机矫正程序
官方规定使用的摄像头太过于抽象，如果不对相机的图片进行矫正那么对于赛道的巡线效果会非常差。相机矫正这在csdn上已经有很多教程，我的建议还是使用matlab去标定摄像头这样的数据更加标准。
cv::Mat undistort(const cv::Mat &frame)
{
double k1 = -0.340032906324299;
double k2 = 0.101344757327394;
double p1 = 0.0;
double p2 = 0.0;
double k3 = 0.0;
cv::Mat K = (cv::Mat_<double>(3, 3) << 162.063205442089, 0.0, 154.707845362265,
0.0, 162.326264903804, 129.914361509615,
0.0, 0.0, 1.0);
cv::Mat D = (cv::Mat_<double>(1, 5) << k1, k2, p1, p2, k3);
cv::Mat mapx, mapy;
cv::Mat undistortedFrame;
cv::initUndistortRectifyMap(K, D, cv::Mat(), K, frame.size(), CV_32FC1, mapx, mapy);
cv::remap(frame, undistortedFrame, mapx, mapy, cv::INTER_LINEAR);
return undistortedFrame;
}
2.赛道线提取
本方案采用sobel算子，霍夫线段检测等方法对赛道线进行提取，我们通过对图片的剪裁提高了程序的运行速度，同时我们也使用了sobel算子的一些增强方式已便于更好的适应赛道环境，无论赛道线的清晰程度，只要是在人眼可分辨的范围内，都可以进行赛道线段提取。
Mat ImagePreprocessing(const cv::Mat &frame_a)
{
int width = 320;
int height = 240;
binaryImage_1 = cv::Mat::zeros(height, width, CV_8U);
//--------这一段代码中包含了canny算子和一些图像处理的方案-------
// cv::Mat grayImage;
// cv::cvtColor(frame, grayImage, cv::COLOR_BGR2GRAY);
// cv::Mat edgeImage;
// cv::Canny(grayImage, edgeImage, 50, 150);
// vector<Vec4i> lines;
// HoughLinesP(edgeImage, lines, 1, CV_PI / 180, 180, 100, 10);
// for (size_t i = 0; i < lines.size(); i++) {
// Vec4i l = lines[i];
// line(binaryImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(255), 4);
// }
// // GaussianBlur(frame, img, cv::Size(7, 7), 0);
// cvtColor(img, hsv_image, cv::COLOR_BGR2HSV);
// // Scalar lower_white(10, 43, 46);
// // Scalar upper_white(180, 255, 255);
// // inRange(hsv_image, lower_white, upper_white, white_mask);
// // bitwise_not(white_mask, final_img);
// // cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(1, 1));
// // cv::morphologyEx(final_img, delite_frame, cv::MORPH_OPEN, kernel);
// Mat img_edges;
// Canny(frame, img_edges, 100, 150);
// cv::Mat blurred;
// cv::Mat edges;
// cv::GaussianBlur(frame_a, blurred, cv::Size(5, 5), 0);
// int roiYStart = 100;
// int roiYEnd = 155;
// // 创建感兴趣区域（ROI）
// cv::Mat roiImage = blurred(cv::Range(roiYStart, roiYEnd), cv::Range(0, frame_a.cols));
// // Canny边缘检测
// cv::Canny(roiImage, edges, 100, 150);
// // 创建全黑图像
// cv::Mat blackImage = cv::Mat::zeros(frame_a.size(), CV_8UC1);
// // 将Canny边缘叠加到全黑图像的感兴趣区域中
// edges.copyTo(blackImage(cv::Range(roiYStart, roiYEnd), cv::Range(0, frame_a.cols)));
// imshow("canny1",edges);
// imshow("canny2",blackImage);
//------------------------------------------------------
//-------------------赛道条件好的情况--------------------
// int kernelSize = 5;
// double sigma = 1.0;
// cv::Mat blurredImage;
// cv::GaussianBlur(frame_a, blurredImage, cv::Size(kernelSize, kernelSize), sigma);
// cv::Mat grad_x, grad_y;
// cv::Mat sobel_x,sobel_y;
// cv::Mat abs_grad_x, abs_grad_y;
// int xWeight = 1;
// int yWeight = 1;
// cv::Sobel(blurredImage, grad_x, CV_16S, 1, 0, 3, xWeight);
// cv::Sobel(blurredImage, grad_y, CV_16S, 0, 1, 3, yWeight);
// cv::convertScaleAbs(grad_x, abs_grad_x);
// cv::convertScaleAbs(grad_y, abs_grad_y);
// cv::Mat edges;
// cv::addWeighted(abs_grad_x, 0.5, abs_grad_y,0.5, 0, edges);
// imshow("edges",edges);
// cv::threshold(edges, binaryImage, 0, 255, cv::THRESH_BINARY + cv::THRESH_OTSU);
// imshow("binaryImage",binaryImage);
//---------------------赛道条件差的情况------------------
// Sobel边缘检测
Mat originalImage;
cvtColor(frame, originalImage, cv::COLOR_BGR2GRAY);
// float alpha = 0.2; // 调整这个值以控制对比度增强的强度
// Mat enhancedImage = customEqualizeHist(originalImage, alpha);
Mat enhancedImage = originalImage;
// Sobel边缘检测
Mat sobelx, sobely;
Sobel(enhancedImage, sobelx, CV_64F, 1, 0, 3);
Sobel(enhancedImage, sobely, CV_64F, 0, 1, 3);
Mat gradientMagnitude = abs(sobelx) + abs(sobely);
convertScaleAbs(gradientMagnitude, gradientMagnitude);
// 调整阈值
Mat binaryImage12 = Mat::zeros(enhancedImage.size(), CV_8U);
// threshold(gradientMagnitude, binaryImage12, 50, 255, THRESH_BINARY);
cv::threshold(gradientMagnitude, binaryImage12, 0, 255, cv::THRESH_BINARY + cv::THRESH_OTSU);
cv::Mat kernel1 = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::dilate(binaryImage12, binaryImage, kernel1, cv::Point(-1, -1), 1); // 这个地方也要修改
// cv::dilate(binaryImage, binaryImage, kernel1, cv::Point(-1, -1), 1);
int x_roi = 1;
int y_roi = 109;
int width_roi = 318;
int height_roi = 46;
cv::Rect roi(x_roi, y_roi, width_roi, height_roi);
cv::Mat croppedObject = binaryImage(roi);
vector<Vec4i> lines;
HoughLinesP(croppedObject, lines, 1, CV_PI / 180, 25, 15, 10);
for (size_t i = 0; i < lines.size(); i++)
{
Vec4i l = lines[i];
double angle = atan2(l[3] - l[1], l[2] - l[0]) * 180.0 / CV_PI;
double length = sqrt(pow(l[3] - l[1], 2) + pow(l[2] - l[0], 2));
double aspect_ratio = length / abs(l[3] - l[1]);
if (abs(angle) > 15)
{
Vec4i l = lines[i];
l[0] += x_roi;
l[1] += y_roi;
l[2] += x_roi;
l[3] += y_roi;
line(binaryImage_1, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(255), 2, LINE_AA);
}
}
return binaryImage_1;
}
3.斑马线识别
本方案采用室内智能车的斑马线识别方案，选取了图像中的ROI区域，用来提高斑马线的识别率和减少对其的误判！
int crossroad(Mat frame)
{
flag_cross = 0;
int height = frame.rows;
int width = frame.cols;
Mat hsv;
cvtColor(frame, hsv, COLOR_BGR2HSV);
Scalar lower_white = Scalar(0, 0, 221);
Scalar upper_white = Scalar(180, 30, 255);
Mat mask1;
inRange(hsv, lower_white, upper_white, mask1);
Mat kernel = getStructuringElement(MORPH_RECT, Size(5, 5));
dilate(mask1, mask1, kernel);
erode(mask1, mask1, kernel);
Mat src(mask1, Rect(100, 85, 120, 60));
int cout1 = 0, cout2 = 0, flag = 0;
for (int i = 0; i < src.rows; i++)
{
if (cout1 < 10)
{
flag = 0;
}
cout1 = 0;
for (int j = 10; j < src.cols - 10; j++)
{
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)
{
cout1++;
}
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)
{
cout1++;
}
if (cout1 >= 10)
{
cout2++;
flag++;
if (flag >= 3)
{
cout << "斑马线" << endl;
flag_cross = 1;
}
break;
}
}
}
cout << "flag_cross" << flag_cross << endl;
return flag_cross;
}
4.黄线识别
本方案使用了两种方式对于黄线进行检测分别为HSV颜色分割和Canny边缘检测，同时本方案配备了对黄线色域阈值调整提取程序，可以对不同光照条件下的黄线实现识别
HSV版本
int yellow_hsv(Mat img, bool visual_flag)
{
Mat cropped_image, canvas, image;
Scalar lowerb = Scalar(3, 0, 0);
Scalar upperb = Scalar(40, 100, 255);
int yellow_num = 0;
int height = img.rows;
int width = img.cols;
int half_height = int(height / 2);
int per_height = int(height / 20);
int area_1 = int(0.002775 * height * width);
int area_2 = int(0.025 * height * width);
image = img.clone();
cropped_image = image(Rect(0, half_height - per_height, width, per_height * 3));
if (visual_flag == true)
{
canvas = cropped_image.clone();
}
cvtColor(cropped_image, cropped_image, COLOR_BGR2HSV);
morphologyEx(cropped_image, cropped_image, MORPH_OPEN, getStructuringElement(MORPH_RECT, Size(3, 3)));
inRange(cropped_image, lowerb, upperb, cropped_image);
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
findContours(cropped_image, contours, hierarchy, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
for (size_t i = 0; i < contours.size(); i++)
{
double area = contourArea(contours[i]);
if (area > area_1 && area < area_2)
{
Rect rect = boundingRect(contours[i]);
double aspect_ratio = rect.width / rect.height;
if (aspect_ratio > 10)
{
if (visual_flag == true)
{
rectangle(canvas, rect, Scalar(255, 0, 0), 2, LINE_AA);
printf("x: %d, y: %d, width: %d, height: %d, aspect_ratio: %f\n", rect.x, rect.y, rect.width,
rect.height, aspect_ratio);
}
yellow_num += 1;
}
}
}
if (visual_flag == true)
{
imshow("Image_hsv", canvas);
waitKey(0);
}
return yellow_num;
}
Canny版本
int yellow_edge(Mat img, bool visual_flag)
{
Mat cropped_image, canvas, image;
int yellow_num = 0;
int height = img.rows;
int width = img.cols;
int half_height = int(height / 2);
int per_height = int(height / 20);
image = img.clone();
cropped_image = image(Rect(0, half_height - per_height, width, per_height * 3));
if (visual_flag == true)
{
canvas = cropped_image.clone();
}
cvtColor(cropped_image, cropped_image, COLOR_BGR2GRAY);
Canny(cropped_image, cropped_image, 50, 150, 3);
vector<Vec4i> lines;
HoughLinesP(cropped_image, lines, 1, CV_PI / 180, 150, 125, 10);
if (lines.size() == 0)
{
printf("No yellow edge detected!\n");
return 0;
}
else
{
for (size_t i = 0; i < lines.size(); i++)
{
Vec4i l = lines[i];
double angle = atan2(l[3] - l[1], l[2] - l[0]) * 180.0 / CV_PI;
double length = sqrt(pow(l[3] - l[1], 2) + pow(l[2] - l[0], 2));
double aspect_ratio = length / abs(l[3] - l[1]);
if (abs(angle) < 5 && aspect_ratio > 5)
{
if (visual_flag == true)
{
line(canvas, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(255, 0, 0), 2, LINE_AA);
printf("x: %d, y: %d, x1: %d, y1: %d, angle: %f, length: %f, aspect_ratio: %f\n", l[0], l[1], l[2],
l[3], angle, length, aspect_ratio);
}
yellow_num += 1;
}
}
}
if (visual_flag == true)
{
imshow("Image_edge", canvas);
waitKey(0);
}
return yellow_num;
}
阈值调整程序
队伍中的大佬最近在考试，不理我，这个等寒假再更新！！！
5.蓝色挡板识别
蓝色挡板分为两部分：1.检测到蓝色挡板，2.蓝色挡板移开
1.检测到蓝色挡板
void blue_card_find(void)
{
vector<vector<Point>> contours;
vector<Vec4i> hierarcy;
findContours(mask, contours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
if (contours.size() > 0)
{
sort(contours.begin(), contours.end(), Contour_Area);
vector<vector<Point>> newContours;
for (const vector<Point> &contour : contours)
{
Point2f center;
float radius;
minEnclosingCircle(contour, center, radius);
if (center.y > 90 && center.y < 160)
{
newContours.push_back(contour);
}
}
contours = newContours;
if (contours.size() > 0)
{
if (contourArea(contours[0]) > 500)
{
cout << "find biggest blue" << endl;
Point2f center;
float radius;
minEnclosingCircle(contours[0], center, radius);
circle(frame, center, static_cast<int>(radius), Scalar(0, 255, 0), 2);
find_first = 1;
}
else
{
cout << "not found blue" << endl;
}
}
}
else
{
cout << "not found blue" << endl;
}
}
2.蓝色挡板移开
void blue_card_remove(void)
{
cout << "entry move blue process" << endl;
vector<vector<Point>> contours;
vector<Vec4i> hierarcy;
findContours(mask, contours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
if (contours.size() > 0)
{
sort(contours.begin(), contours.end(), Contour_Area);
vector<vector<Point>> newContours;
for (const vector<Point> &contour : contours)
{
Point2f center;
float radius;
minEnclosingCircle(contour, center, radius);
if (center.y > 90 && center.y < 160)
{
newContours.push_back(contour);
}
}
contours = newContours;
if (contours.size() == 0)
{
begin_sign = 0;
cout << "move" << endl;
sleep(2);
}
}
else
{
begin_sign = 0;
cout << "蓝色挡板移开" << endl;
sleep(2);
}
}
6.锥桶避障方案
锥桶避障实际上就是对锥桶的颜色进行识别，再利用锥桶中心位置对图像进行补线，从而实现动态路径规划，这一部分代码分为三部分对于1.锥桶颜色进行识别，2.根据锥桶位置对锥桶进行判断是否补线，3.根据锥桶位置对补线方案进行替换，4.对图像补线后进行再次巡线
补线程序（锥桶主程序）
void blue_vertebral_model(void)
{
contours_all = blue_vertebral_barrel_find_all(mask);
if (contours_all.size() != 0)
{
Point2f center;
float radius;
minEnclosingCircle(contours_all[0], center, radius);
heighest = center.y;
if (try_patching_line == 2)
{
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),
8);
cout << "center.x:" << center.x << endl;
cout << "center.y:" << center.y << endl;
cout << "1" << endl;
}
else if (try_patching_line == 1)
{
if (count_change != 2)
{
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),
8);
}
else
{
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),
8);
}
cout << "center.x:" << center.x << endl;
cout << "center.y:" << center.y << endl;
cout << "2" << endl;
}
}
}
锥桶的判断程序和补线方式转换程序
vector<vector<Point>> blue_vertebral_barrel_find_all(Mat mask)
{
vector<vector<Point>> contours;
vector<Vec4i> hierarcy;
Point2f center;
float radius;
findContours(mask, contours, hierarcy, RETR_EXTERNAL, CHAIN_APPROX_NONE);
if (contours.size() > 0)
{
vector<vector<Point>> newContours;
for (const vector<Point> &contour : contours)
{
Point2f center;
float radius;
minEnclosingCircle(contour, center, radius);
if (center.y > 108 && center.y < 153)
{
newContours.push_back(contour);
}
}
contours = newContours;
}
if (contours.size() > 0)
{
vector<vector<Point>> newContours2;
for (const vector<Point> &contour : contours)
{
if (contourArea(contour) > 10)
{
newContours2.push_back(contour);
}
}
contours = newContours2;
}
if (contours.size() > 0)
{
vector<vector<Point>> newContours5;
for (const vector<Point> &contour : contours)
{
Point2f center;
float radius;
minEnclosingCircle(contour, center, radius);
center.x = (int)center.x;
center.y = (int)center.y;
if (center.x > left_line[center.y - 108].x && center.x < right_line[center.y - 108].x)
{
cout << "过滤后的点：center.x" << center.x << endl;
cout << "center.y " << center.y << endl;
cout << "left_line " << left_line[center.y - 108].x << endl;
cout << "right_line " << right_line[center.y - 108].x << endl;
cv::circle(frame, Point(center.x, center.y), 10, cv::Scalar(0, 0, 0), -1);
newContours5.push_back(contour);
}
}
contours = newContours5;
}
if (contours.size() > 0)
{
sort(contours.begin(), contours.end(), Contour_Area);
now_blue_max = (int)contourArea(contours[0]);
}
else
{
now_blue_max = 0;
}
vector<vector<Point>> newContours4;
newContours4 = contours;
if (contours.size() > 0)
{
vector<vector<Point>> newContours3;
for (const vector<Point> &contour : contours)
{
if (contourArea(contour) < 140)
{
newContours3.push_back(contour);
}
}
contours = newContours3;
}
// cout << "now_blue_max" << now_blue_max <<endl;
// cout << "contours.size()" << contours.size() <<endl;
if (contours.size() == 0 && newContours4.size() != 0)
{
if (last_blue == 0)
{
if (try_patching_line == 1)
{
try_patching_line = 2;
}
else if (try_patching_line == 2)
{
try_patching_line = 1;
}
cout << "--------------------------------补线方式转换------------------------------" << endl;
number1 = 0;
count_change++;
}
}
if (now_blue_max > 140)
{
last_blue = 1;
}
else
{
last_blue = 0;
}
return contours;
}
7.控制代码
本方案采用的控制代码与室内智能车差不多，但是建议在锥桶避障时采用多套PID，同时可以考虑使用赛道中线方差实现变加速控制（团队做了后面没用因为确实没啥用！），这里给一些示例代码可以供参考！
1.控制程序
double average(vector<int> vec)
{
if (vec.size() < 1)
return -1;
double sum = 0;
for (int i = 0; i < vec.size(); i++)
{
sum += vec[i];
}
return (double)sum / vec.size();
}
double sigma(vector<int> vec)
{
if (vec.size() < 1)
return 0;
double aver = average(vec);
double sigma = 0;
for (int i = 0; i < vec.size(); i++)
{
sigma += (vec[i] - aver) * (vec[i] - aver);
}
sigma /= (double)vec.size();
return sigma;
}
void motor_servo_contral(int flag_yellow_cond, int cross)
{
if (flag_yellow_cond != 0 && flag_yellow_finish == 0 && flag_cross_finish == 1)
{
flag_yellow_finish = 1;
gpioPWM(12, 730);
gpioPWM(13, 10000);
usleep(250000);
gpioPWM(13, 9800);
gpioPWM(13, 8800);
usleep(250000);
_exit(0);
}
else if (cross == 1 && flag_cross_finish == 0)
{
flag_cross_finish = 1;
gpioPWM(13, 9800);
gpioPWM(13, 8900);
usleep(550000);
gpioPWM(12, 730);
gpioPWM(13, 10000);
sleep(3);
}
else if (contours_all.size() != 0 && count_change < 3 && number1 >= 7 && flag_cross_finish == 1)
{
if (try_patching_line == 2 && count_change < 3 && count_change >= 1)
{
float servo_pwm_chayan = servo_pd_blue(160);
servo_pwm_now = servo_pwm_chayan;
}
else if (try_patching_line == 1 && count_change >= 1 && count_change < 3)
{
float servo_pwm_chayan = servo_pd_blue(160);
servo_pwm_now = servo_pwm_chayan;
}
else
{
servo_pwm_now = servo_pd_blue(160);
}
cout << "bin_imagepwm" << servo_pwm_now << endl;
if (times4 == 0)
{
times4 = 1;
// gpioPWM(13, 9800);
// gpioPWM(13, 8700);
gpioPWM(13, 10000);
// usleep(50000);
}
// gpioPWM(13, 10000);//11000
// gpioPWM(13, 10500);
gpioPWM(12, servo_pwm_now);
}
else
{
if (count_change < 1 || count_change > 2)
{
cout << 1 << endl;
if (count_change > 2 && flag_cross_finish == 1)
{
servo_pwm_now = servo_pd(160);
cout << "pwm" << servo_pwm_now << endl;
gpioPWM(13, 13000);
gpioPWM(12, servo_pwm_now);
}
else if (count_change < 1 && flag_cross_finish == 0)
{
servo_pwm_now = servo_pd(160);
cout << "pwm" << servo_pwm_now << endl;
gpioPWM(13, 12000);
gpioPWM(12, servo_pwm_now);
}
else
{
cout << "after" << endl;
servo_pwm_now = servo_pd_after(160);
cout << "pwm" << servo_pwm_now << endl;
gpioPWM(13, 11700);
gpioPWM(12, servo_pwm_now);
}
}
else
{
cout << 2 << endl;
servo_pwm_now = servo_pd_left(160);
cout << "pwm" << servo_pwm_now << endl;
gpioPWM(13, 11400);
gpioPWM(12, servo_pwm_now);
}
}
// ----------------------------------变加速控制-------------------------------
// uint8_t controlLow = 0; // 速度控制下限
// uint8_t controlMid = 5;
// uint8_t controlHigh = 10; // 速度控制上限
// float servo_pwm_now = servo_pd(320);
// if (mid.size() > 20)
// {
// vector<POINT> centerV;
// int filt = mid.size() / 5;
// for (int i = filt; i < mid.size() - filt; i++)
// {
// centerV.push_back(mid[i]);
// }
// sigmaCenter = sigma(centerV);
// }
// else
// sigmaCenter = 1000;
// if (abs(sigmaCenter) < 100.0)
// {
// counterShift++;
// if (counterShift > controlHigh)
// counterShift = controlHigh;
// }
// else
// {
// counterShift--;
// if (counterShift < controlLow)
// counterShift = controlLow;
// }
// if (counterShift > controlMid)
// {
// motorSpeed = speedhigh;
// cout << "高" << endl;
// }
// else
// {
// motorSpeed = speedlow;
// cout << "低" << endl;
// }
// gpioPWM(13, motorSpeed);
// gpioPWM(12, servo_pwm_now);
// ------------------------------------纯竞速---------------------------------
// if (flag_yellow_cond != 0 && flag_yellow_finish == 0 && flag_cross_finish == 1)
// {
// flag_yellow_finish = 1;
// gpioPWM(12, 730);
// gpioPWM(13, 10000);
// usleep(250000);
// gpioPWM(13, 9800);
// gpioPWM(13, 8800);
// usleep(250000);
// cout << "停止" << endl;
// _exit(0);
// }
// else if (cross == 1 && flag_cross_finish == 0)
// {
// flag_cross_finish = 1;
// cout << "11" << endl;
// gpioPWM(13, 9800);
// gpioPWM(13, 8900);
// usleep(550000); //
// gpioPWM(12, 730);
// gpioPWM(13, 10000);
// sleep(3);
// }
// else if (contours_all.size() != 0 && count_change < 3 && number1 >= 7)
// {
// if (try_patching_line == 2 && count_change < 3 && count_change >= 1)
// {
// float servo_pwm_chayan = servo_pd_blue(160);
// servo_pwm_now = servo_pwm_chayan;
// }
// else if (try_patching_line == 1 && count_change >= 1 && count_change < 3)
// {
// float servo_pwm_chayan = servo_pd_blue(160);
// servo_pwm_now = servo_pwm_chayan;
// }
// else
// {
// servo_pwm_now = servo_pd_blue(160);
// }
// if (times4 == 0)
// {
// times4 = 1;
// gpioPWM(13, 9800);
// gpioPWM(13, 8700);
// usleep(100000);
// }
// gpioPWM(13, 10000);//11000
// gpioPWM(12, servo_pwm_now);
// }
// else
// {
// if (flag_cross_finish == 0)
// {
// servo_pwm_now = servo_pd(160);
// gpioPWM(13, 12000);
// gpioPWM(12, servo_pwm_now);
// }
// else
// {
// servo_pwm_now = servo_pd(160);
// gpioPWM(13, 12500);
// gpioPWM(12, servo_pwm_now);
// }
// }
}
2.PID控制程序
本方案直接使用pwm波数值进行舵机控制，如果您使用了逆透视的方案来做巡线，那么我更加建议使用角度来做pwm波控制。
float servo_pd(int target)
{
int size = int(mid.size());
int pidx = int((mid[23].x + mid[20].x + mid[25].x) / 3);
error_first = target - pidx;
servo_pwm_diff = kp * error_first + kd * (error_first - last_error);
// cout << "servo_pwm_diff:" << servo_pwm_diff << endl;
last_error = error_first;
servo_pwm = 720 + servo_pwm_diff;
if (servo_pwm > 900)
{
servo_pwm = 900;
}
else if (servo_pwm < 600)
{
servo_pwm = 600;
}
return servo_pwm;
}
8.附属模块代码程序
1.拍照程序（python版）
import cv2
import os
import numpy as np
def undistort(frame):
k1, k2, p1, p2, k3 = -0.340032906324299, 0.101344757327394,0.0,0.0, 0.0
k = np.array([
[162.063205442089, 0, 154.707845362265],
[0, 162.326264903804,129.914361509615],
[0, 0, 1]
])
# 畸变系数
d = np.array([
k1, k2, p1, p2, k3
])
height, weight = frame.shape[:2]
mapx, mapy = cv2.initUndistortRectifyMap(k, d, None, k, (weight, height), 5)
return cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320) #设置宽度
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240) #设置长度
if not cap.isOpened():
print("无法打开摄像头")
else:
while True:
ret, frame = cap.read()
if not ret:
print("无法捕捉")
break
frame = undistort(frame)
cv2.imshow('Press "q" to Capture and Quit', frame)
key = cv2.waitKey(1)
# 如果用户按下 "q"，拍照并保存，然后退出程序?
if key & 0xFF == ord('q'):
photo_path = os.path.join("test_photo", '40.jpg')
cv2.imwrite(photo_path, frame)
print(f"已保存照片为 {photo_path}")
break
cap.release()
cv2.destroyAllWindows()
2.蓝色挡板测试程序（python版）
import cv2
import numpy as np
# 使用比较函数对轮廓进行排序
def contour_area(contour):
return cv2.contourArea(contour)
def process_frame(frame):
change_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
lower_bound = np.array([100, 43, 46], dtype=np.uint8)
upper_bound = np.array([124, 255, 255], dtype=np.uint8)
mask = cv2.inRange(change_frame, lower_bound, upper_bound)
kernel = np.ones((5, 5), np.uint8)
mask = cv2.dilate(mask, kernel, iterations=1)
mask = cv2.erode(mask, kernel, iterations=1)
return mask
cap = cv2.VideoCapture(0)
find_first = 0
begin_sign = 0
while True:
ret, frame = cap.read()
processed_frame = process_frame(frame)
contours, hierarchy = cv2.findContours(processed_frame, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
if begin_sign == 0:
if find_first == 0:
if len(contours) > 0:
contours = sorted(contours, key=cv2.contourArea, reverse=True)
new_contours = []
for contour in contours:
(x, y), radius = cv2.minEnclosingCircle(contour)
center = (int(x), int(y))
# 去除掉上面和下面的噪点
if center[1] > 180 and center[1] < 320:
new_contours.append(contour)
# 将新的列表赋值给 contours
contours = new_contours
if(len(contours) > 0):
# 检查列表中第一个轮廓的面积是否大于 1300
if cv2.contourArea(contours[0]) > 1300:
print("找到了最大的蓝色挡板")
(x, y), radius = cv2.minEnclosingCircle(contours[0])
center = (int(x), int(y))
radius = int(radius)
cv2.circle(frame, center, radius, (0, 255, 0), 2)
find_first = 1
else:
print("没找到蓝色挡板")
else:
print("进入移开挡板的程序")
if len(contours) > 0:
contours = sorted(contours, key=cv2.contourArea, reverse=True)
new_contours = []
for contour in contours:
(x, y), radius = cv2.minEnclosingCircle(contour)
center = (int(x), int(y))
# 去除掉上面和下面的噪点
if center[1] > 180 and center[1] < 320:
new_contours.append(contour)
contours = new_contours
if(len(contours) > 0):
if cv2.contourArea(contours[0]) < 300:
begin_sign = 1
print("挡板移开")
else:
begin_sign = 1
print("挡板移开")
cv2.imshow('Original Frame', frame)
cv2.imshow('Processed Frame', processed_frame)
# 检测按键，如果按下Esc键则退出循环
if cv2.waitKey(1) == 27:
break
cap.release()
cv2.destroyAllWindows()
3.锥桶轮廓面积测试程序（python版）
import cv2
import numpy as np
def find_blue_contours(image_path):
# 读取图像
image = cv2.imread(image_path)
# 将图像转换为HSV颜色空间
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
# 定义蓝色的HSV范围
lower_blue = np.array([100, 43, 46])
upper_blue = np.array([124, 255, 255])
# 创建一个蓝色掩码
mask = cv2.inRange(hsv, lower_blue, upper_blue)
# 执行形态学操作，可以根据实际情况调整参数
kernel = np.ones((5, 5), np.uint8)
mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)
mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel)
# 寻找轮廓
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
# 仅保留y轴在210-320范围内的轮廓
filtered_contours = [contour for contour in contours if 95 <= cv2.boundingRect(contour)[1] <= 165]
# 在图像上画出轮廓，并写出大小
for i, contour in enumerate(filtered_contours):
area = cv2.contourArea(contour)
cv2.drawContours(image, [contour], 0, (0, 255, 0), 2)
cv2.putText(image, f"Contour {i + 1}: {area:.2f}", (10, 30 * (i + 1)), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)
# 显示图像
cv2.imshow("Blue Contours", image)
cv2.waitKey(0)
cv2.destroyAllWindows()
if __name__ == "__main__":
image_path = "/home/pi/test_photo/blue_zhuitong8.jpg" # 替换为你的图像文件路径
find_blue_contours(image_path)
4.相机矫正程序（python版）
# 摄像头畸变矫正函数，输入待矫正的图形变量
import cv2
import os
import numpy as np
def undistort(frame):
k1, k2, p1, p2, k3 = -0.287246515012261, 0.066176222325459, 0.005615032474715,0.003425003902561, 0.0
# 相机坐标系到像素坐标系的转换矩阵
k = np.array([
[3.111337497474041e+02, -2.333471935388314, 2.915941445374422e+02],
[0, 3.109853062871910e+02, 2.473500696130221e+02],
[0, 0, 1]
])
# 畸变系数
d = np.array([
k1, k2, p1, p2, k3
])
height, weight = frame.shape[:2]
mapx, mapy = cv2.initUndistortRectifyMap(k, d, None, k, (weight, height), 5)
# 返回矫正好的图形变量
return cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)
# 打开摄像头
cap = cv2.VideoCapture(0) # 0表示默认摄像头，如果有多个摄像头，可以尝试不同的数字来选择不同的摄像头
if not cap.isOpened():
print("无法打开摄像头.")
else:
while True:
# 捕捉一帧
ret, frame = cap.read()
if not ret:
print("无法捕捉帧.")
break
frame = undistort(frame)
# 显示当前帧
cv2.imshow('Press "q" to Capture and Quit', frame)
# 等待用户按下键盘
key = cv2.waitKey(1)
# # 如果用户按下 "q" 键，拍照并保存，然后退出程序
# if key & 0xFF == ord('q'):
# # 构造完整的文件路径
# photo_path = os.path.join("test_photo", '30.jpg')
# cv2.imwrite(photo_path, frame)
# print(f"已保存照片为 {photo_path}")
# break
# 释放摄像头
cap.release()
# 关闭所有窗口
cv2.destroyAllWindows()
5.Canny边缘检测阈值调整程序（c++版本）
#include <opencv2/opencv.hpp>
using namespace cv;
// 回调函数
void onTrackbarChange(int, void *)
{
// 什么也不做
}
int main()
{
// 读取图片
Mat img_original = imread("/home/pi/test_photo/1.jpg");
// 创建窗口
namedWindow("Canny");
// 创建两个滑动条，分别控制threshold1和threshold2
int threshold1 = 50;
int threshold2 = 100;
createTrackbar("threshold1", "Canny", &threshold1, 400, onTrackbarChange);
createTrackbar("threshold2", "Canny", &threshold2, 400, onTrackbarChange);
while (true)
{
// Canny边缘检测
Mat img_edges;
Canny(img_original, img_edges, threshold1, threshold2);
// 显示图片
imshow("original", img_original);
imshow("Canny", img_edges);
// 检测键盘输入，如果按下 'q' 键，退出循环
if (waitKey(1) == 'q')
{
break;
}
}
destroyAllWindows();
return 0;
}
6.HoughLinesP阈值调整程序（c++版本）
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
Mat g_mask1Image, g_midImage, g_mask1GrayImage;
cv::Mat sobelX8U;
cv::Mat blurredImage;
cv::Mat sobelX;
cv::Mat binaryImage;
cv::Mat croppedObject;
int g_HoughLinesThreshold = 150;
int g_minLineLength = 100;
void on_HoughLines(int, void *);
Mat customEqualizeHist(const Mat &inputImage, float alpha)
{
Mat enhancedImage;
equalizeHist(inputImage, enhancedImage);
// 减弱对比度增强的效果
return alpha * enhancedImage + (1 - alpha) * inputImage;
}
int main()
{
g_mask1Image = imread("/home/pi/test_photo/1.jpg", cv::IMREAD_GRAYSCALE);
if (!g_mask1Image.data)
{
return -1;
}
imshow("g_mask1Image", g_mask1Image);
// float alpha = 0.5; // 调整这个值以控制对比度增强的强度
// Mat enhancedImage = customEqualizeHist(g_mask1Image, alpha);
Mat enhancedImage = g_mask1Image;
// Sobel边缘检测
Mat sobelx, sobely;
Sobel(enhancedImage, sobelx, CV_64F, 1, 0, 3);
Sobel(enhancedImage, sobely, CV_64F, 0, 1, 3);
Mat gradientMagnitude = abs(sobelx) + abs(sobely);
convertScaleAbs(gradientMagnitude, gradientMagnitude);
// 调整阈值
Mat binaryImage12 = Mat::zeros(enhancedImage.size(), CV_8U);
// threshold(gradientMagnitude, binaryImage12, 50, 255, THRESH_BINARY);
cv::threshold(gradientMagnitude, binaryImage12, 0, 255, cv::THRESH_BINARY + cv::THRESH_OTSU);
imshow("binaryImage", binaryImage12);
imshow("gradientMagnitude", gradientMagnitude);
cv::Mat kernel1 = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
cv::dilate(binaryImage12, binaryImage, kernel1, cv::Point(-1, -1), 1);
// imshow("1",binaryImage);
int x_roi = 1;
int y_roi = 109;
int width_roi = 318;
int height_roi = 45;
cv::Rect roi(x_roi, y_roi, width_roi, height_roi);
croppedObject = binaryImage(roi);
namedWindow("HoughLines", WINDOW_AUTOSIZE);
createTrackbar(" g_HoughLinesThreshold", "HoughLines", &g_HoughLinesThreshold, 150, on_HoughLines);
createTrackbar("g_minLineLength", "HoughLines", &g_minLineLength, 100, on_HoughLines);
on_HoughLines(0, 0);
while (char(waitKey(1)) != 'q')
{
}
waitKey(0);
return 0;
}
void on_HoughLines(int, void *)
{
cvtColor(croppedObject, g_mask1GrayImage, COLOR_GRAY2BGR);
vector<Vec4i> lines
HoughLinesP(croppedObject, lines, 1, CV_PI / 180, g_HoughLinesThreshold, g_minLineLength, 10);
for (size_t i = 0; i < lines.size(); i++)
{
Vec4i l = lines[i];
line(g_mask1GrayImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(186, 88, 255), 1, LINE_AA);
}
imshow("HoughLines", g_mask1GrayImage);
}
尾言
感谢湘潭大学电工电子实验室为我们提供的参赛机会，感谢向礼丹老师和电工电子实验室的每个指导老师，感谢十七届参加室外赛的朱嘉明和崔全学长，衷心祝愿各位未来参加智能车比赛的车友能够赛出佳绩！在智能车竞赛中再创辉煌！
本文转载自: https://blog.csdn.net/m0_64688675/article/details/134916040
版权归原作者 zy（舔狗版） 所有， 如有侵权，请联系我们删除。
版权归原作者 zy（舔狗版） 所有， 如有侵权，请联系我们删除。