#include <pigpio.h>
#include <iostream>
#include <unistd.h>
#include <opencv2/opencv.hpp>
#include <vector>
using namespace cv;
using namespace std;
//
---------- 配置参数
----------
const int BACKGROUND_COLOR = 1; // 1: 红色背景, 2: 蓝色背景
//
---------- 函数声明
----------
void GetLineROI(Mat src, Mat &ROI);
void GetSignROI(Mat src, Mat &ROI);
void GetROI(Mat src, Mat &ROI);
bool detectBackgroundPresence(Mat &img);
int detectABLetter(Mat &img);
double PID(
double error1);
int atc(int angle);
int dong();
int xunji(Mat frame);
void preprocessImg(Mat img, Mat &gray, Mat &canny);
void saveDebugImage(Mat img, string name, int frame_count);
bool detect_A_shape(Mat &roi);
bool detect_B_shape(Mat &roi);
void performAction(int letter);
Mat preprocessForLetter(Mat &roi);
void improveLineDetection(Mat &canny);
//
---------- 全局变量
----------
const int
pwm_pin = 13;
const int servo_pin = 12;
const int
pwm_freq1 = 200;
const int
pwm_freq2 = 50;
const int
pwm_range1 = 40000;
const int
pwm_range2 = 100;
const int init_duty = 10000;
double last_error = 0;
const
double kp = 0.25, kd = 0.06; // 进一步降低PID参数
const
double min_ang = 75, max_ang = 89; // 缩小角度范围
bool debug_save = true;
bool ab_detected = false;
int detected_letter = 0;
int action_cooldown = 0;
//
---------- 保存调试图像
----------
void saveDebugImage(Mat img, string name, int frame_count) {
if (debug_save && frame_count % 100 == 0) { // 进一步减少保存频率
string filename = "/tmp/" + name + "_" + to_string(frame_count) + ".jpg";
imwrite(filename, img);
cout << "Saved debug image: " << filename << endl;
}
}
//
---------- 循迹ROI
----------
void GetROI(Mat src, Mat &ROI) {
int width = src.cols;
int height = src.rows;
// 调整ROI，使用更靠近车辆的区域
Rect rect(Point(width/4, height*3/4), Point(3*width/4, height
-20));
ROI = src(rect).clone();
}
//
---------- 标志检测ROI
----------
void GetSignROI(Mat src, Mat &ROI) {
int width = src.cols;
int height = src.rows;
// 扩大检测区域
Rect rect(Point(width/4, height/3), Point(3*width/4, 2*height/3));
ROI = src(rect).clone();
}
//
---------- 图像预处理（优化版）
----------
void preprocessImg(Mat img, Mat &gray, Mat &canny) {
cvtColor(img, gray, COLOR_BGR2GRAY);
GaussianBlur(gray, gray, Size(7, 7), 1.0, 1.0); // 增加模糊程度
// 使用自适应Canny阈值
double median_val = cv::median(gray);
int lower_thresh = max(0, (int)(0.5 * median_val));
int upper_thresh = min(255, (int)(1.5 * median_val));
Canny(gray, canny, lower_thresh, upper_thresh, 3);
// 改善线条检测
improveLineDetection(canny);
}
//
---------- 改善线条检测
----------
void improveLineDetection(Mat &canny) {
// 使用形态学操作连接断开的边缘
Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
morphologyEx(canny, canny, MORPH_CLOSE, kernel);
// 去除小噪点
vector<vector<Point>> contours;
findContours(canny, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
for (size_t i = 0; i < contours.size(); i++) {
if (contourArea(contours[i]) < 10) {
drawContours(canny, contours, (int)i, Scalar(0),
-1);
}
}
}
//
---------- 字母预处理
----------
Mat preprocessForLetter(Mat &roi) {
Mat gray, binary;
if (roi.channels() == 3) {
cvtColor(roi, gray, COLOR_BGR2GRAY);
} else {
gray = roi.clone();
}
GaussianBlur(gray, gray, Size(5, 5), 1.0);
// 尝试多种二值化方法
Mat binary1, binary2;
adaptiveThreshold(gray, binary1, 255, ADAPTIVE_THRESH_GAUSSIAN_C, THRESH_BINARY, 11, 2);
threshold(gray, binary2, 0, 255, THRESH_BINARY | THRESH_OTSU);
// 选择效果更好的二值化结果
int nonZero1 = countNonZero(binary1);
int nonZero2 = countNonZero(binary2);
binary = (nonZero1 > nonZero2) ? binary1 : binary2;
// 对于深色背景浅色字母的情况，可能需要反转
if (nonZero1 < roi.total() * 0.3 || nonZero2 < roi.total() * 0.3) {
bitwise_not(binary, binary);
}
Mat kernel_open = getStructuringElement(MORPH_RECT, Size(2, 2));
Mat kernel_close = getStructuringElement(MORPH_RECT, Size(6, 6));
morphologyEx(binary, binary, MORPH_OPEN, kernel_open);
morphologyEx(binary, binary, MORPH_CLOSE, kernel_close);
return binary;
}
//
---------- 背景存在检测（优化版）
----------
bool detectBackgroundPresence(Mat &img) {
Mat roi;
GetSignROI(img, roi);
if (roi.empty()) {
return false;
}
Mat hsv;
cvtColor(roi, hsv, COLOR_BGR2HSV);
Mat color_mask;
if (BACKGROUND_COLOR == 1) {
// 扩大红色检测范围
Mat red_mask1, red_mask2, red_mask3;
inRange(hsv, Scalar(0, 30, 30), Scalar(20, 255, 255), red_mask1);
inRange(hsv, Scalar(150, 30, 30), Scalar(180, 255, 255), red_mask2);
// 增加亮度较低的红色检测
inRange(hsv, Scalar(0, 50, 20), Scalar(10, 255, 100), red_mask3);
color_mask = red_mask1 | red_mask2 | red_mask3;
} else {
// 扩大蓝色检测范围
Mat blue_mask1, blue_mask2;
inRange(hsv, Scalar(85, 30, 30), Scalar(145, 255, 255), blue_mask1);
inRange(hsv, Scalar(100, 40, 20), Scalar(140, 255, 100), blue_mask2);
color_mask = blue_mask1 | blue_mask2;
}
// 使用形态学操作填充区域
Mat kernel_dilate = getStructuringElement(MORPH_ELLIPSE, Size(7, 7));
dilate(color_mask, color_mask, kernel_dilate);
Mat kernel_close = getStructuringElement(MORPH_ELLIPSE, Size(15, 15));
morphologyEx(color_mask, color_mask, MORPH_CLOSE, kernel_close);
int color_pixels = countNonZero(color_mask);
int total_pixels = roi.rows * roi.cols;
double color_ratio = (
double)color_pixels / total_pixels;
cout << "Background presence
- Color pixels: " << color_pixels
<< " / " << total_pixels << " (" << color_ratio*100 << "%)" << endl;
saveDebugImage(color_mask, "bg_color_mask", 0);
// 大幅降低阈值
return color_ratio > 0.005; // 从0.02降到0.005
}
//
---------- A/B字母检测
----------
int detectABLetter(Mat &img) {
Mat roi;
GetSignROI(img, roi);
if (roi.empty()) {
cout << "Sign ROI is empty!" << endl;
return 0;
}
saveDebugImage(roi, "sign_roi", 0);
// 直接使用颜色分割尝试找到字母
Mat hsv;
cvtColor(roi, hsv, COLOR_BGR2HSV);
Mat letter_mask;
if (BACKGROUND_COLOR == 1) {
// 红色背景上的字母可能是黑色或白色
// 寻找非红色区域
Mat red_mask;
inRange(hsv, Scalar(0, 30, 30), Scalar(20, 255, 255), red_mask);
inRange(hsv, Scalar(150, 30, 30), Scalar(180, 255, 255), letter_mask);
letter_mask = ~red_mask; // 反转，得到非红色区域
} else {
// 蓝色背景上的字母
Mat blue_mask;
inRange(hsv, Scalar(85, 30, 30), Scalar(145, 255, 255), blue_mask);
letter_mask = ~blue_mask;
}
// 形态学操作
Mat kernel = getStructuringElement(MORPH_RECT, Size(3, 3));
morphologyEx(letter_mask, letter_mask, MORPH_OPEN, kernel);
saveDebugImage(letter_mask, "letter_mask", 0);
vector<vector<Point>> contours;
findContours(letter_mask, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
cout << "Found " << contours.size() << " contours for letter detection" << endl;
sort(contours.begin(), contours.end(),
[](const vector<Point>& a, const vector<Point>& b) {
return contourArea(a) > contourArea(b);
});
if (!contours.empty()) {
double area = contourArea(contours[0]);
Rect rect = boundingRect(contours[0]);
double aspect_ratio = (
double)rect.width / rect.height;
cout << "Main contour
- Area: " << area
<< ", Bounding rect: " << rect.width << "x" << rect.height
<< ", Aspect ratio: " << aspect_ratio << endl;
// 放宽条件
if (area > 200 && area < 50000 && aspect_ratio > 0.3 && aspect_ratio < 3.0) {
Mat letter_roi = letter_mask(rect);
saveDebugImage(letter_roi, "letter_candidate", 0);
cout << "
-> Testing for A/B shape" << endl;
if (detect_A_shape(letter_roi)) {
cout << "*** Detected A ***" << endl;
Rect global_rect(
rect.x + img.cols/4,
rect.y + img.rows/3,
rect.width,
rect.height
);
rectangle(img, global_rect, Scalar(0, 255, 0), 3);
putText(img, "A", Point(global_rect.x, global_rect.y
-10),
FONT_HERSHEY_SIMPLEX, 1.5, Scalar(0, 255, 0), 3);
return 1;
}
else if (detect_B_shape(letter_roi)) {
cout << "*** Detected B ***" << endl;
Rect global_rect(
rect.x + img.cols/4,
rect.y + img.rows/3,
rect.width,
rect.height
);
rectangle(img, global_rect, Scalar(0, 255, 0), 3);
putText(img, "B", Point(global_rect.x, global_rect.y
-10),
FONT_HERSHEY_SIMPLEX, 1.5, Scalar(0, 255, 0), 3);
return 2;
} else {
cout << "
-> Not A or B" << endl;
}
} else {
cout << "
-> Skipped due to area/aspect ratio" << endl;
}
}
return 0;
}
//
---------- A形状检测
----------
bool detect_A_shape(Mat &roi) {
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
findContours(roi.clone(), contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);
if (hierarchy.empty() || contours.empty()) return false;
int valid_hole_count = 0;
// 找到最大的轮廓
int main_contour_idx =
-1;
double max_area = 0;
for (int i = 0; i < contours.size(); i++) {
double area = contourArea(contours[i]);
if (area > max_area) {
max_area = area;
main_contour_idx = i;
}
}
if (main_contour_idx ==
-1) return false;
// 统计孔洞
for (int i = 0; i < hierarchy.size(); i++) {
if (hierarchy[i][3] == main_contour_idx) {
double hole_area = contourArea(contours[i]);
if (hole_area > 10) { // 最小孔洞面积
valid_hole_count++;
}
}
}
cout << " A shape
- Valid holes: " << valid_hole_count << endl;
return (valid_hole_count >= 1); // A至少有一个孔洞
}
//
---------- B形状检测
----------
bool detect_B_shape(Mat &roi) {
vector<vector<Point>> contours;
vector<Vec4i> hierarchy;
findContours(roi.clone(), contours, hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE);
if (hierarchy.empty() || contours.empty()) return false;
int valid_hole_count = 0;
// 找到最大的轮廓
int main_contour_idx =
-1;
double max_area = 0;
for (int i = 0; i < contours.size(); i++) {
double area = contourArea(contours[i]);
if (area > max_area) {
max_area = area;
main_contour_idx = i;
}
}
if (main_contour_idx ==
-1) return false;
// 统计孔洞
for (int i = 0; i < hierarchy.size(); i++) {
if (hierarchy[i][3] == main_contour_idx) {
double hole_area = contourArea(contours[i]);
if (hole_area > 10) { // 最小孔洞面积
valid_hole_count++;
}
}
}
cout << " B shape
- Valid holes: " << valid_hole_count << endl;
return (valid_hole_count >= 2); // B至少有两个孔洞
}
//
---------- PID控制（稳定版）
----------
double PID(
double error) {
// 限制误差范围
double limited_error = max(min(error, 50.0),
-50.0);
double angle = kp * limited_error + kd * (limited_error
- last_error);
last_error = limited_error;
return angle;
}
//
---------- 舵机角度转占空比
----------
int atc(int angle) {
int pulse_width = 500 + (angle / 180.0) * 2000;
int duty = pulse_width / 20000.0 * 100;
return duty;
}
//
---------- 硬件初始化
----------
int dong() {
if (gpioInitialise() < 0) {
cout << "pigpio init error" << endl;
return
-1;
}
gpioSetMode(
pwm_pin, PI_OUTPUT);
gpioSet
PWMfrequency(
pwm_pin,
pwm_freq1);
gpioSet
PWMrange(
pwm_pin,
pwm_range1);
gpioSetMode(servo_pin, PI_OUTPUT);
gpioSet
PWMfrequency(servo_pin,
pwm_freq2);
gpioSet
PWMrange(servo_pin,
pwm_range2);
gpio
PWM(
pwm_pin, init_duty);
gpio
PWM(servo_pin, atc(82));
cout << "pigpio ok!" << endl;
sleep(2);
return 0;
}
//
---------- 巡线控制（稳定版）
----------
int xunji(Mat frame) {
Mat img, gray, canny;
GetROI(frame, img);
if (img.empty()) {
cout << "Line ROI is empty!" << endl;
return 0;
}
preprocessImg(img, gray, canny);
int cols = canny.cols;
int rows = canny.rows;
saveDebugImage(canny, "line_canny", 0);
// 改进的巡线算法
- 使用加权平均
vector<
double> weights;
vector<int> centers;
double total_weight = 0;
double weighted_sum = 0;
// 从底部向上扫描，越靠近底部权重越高
for (int i = rows
-1; i >= max(0, rows
-80); i
--) {
int left_
edge =
-1;
int right_
edge =
-1;
// 寻找左边缘
for (int j = cols/3; j >= 5; j
--) {
if (canny.at<uchar>(i, j) == 255) {
left_
edge = j;
break;
}
}
// 寻找右边缘
for (int j = 2*cols/3; j < cols
-5; j++) {
if (canny.at<uchar>(i, j) == 255) {
right_
edge = j;
break;
}
}
// 如果找到左右边缘，计算中心点
if (left_
edge !=
-1 && right_
edge !=
-1) {
int center = (left_
edge + right_
edge) / 2;
double weight = (
double)(i + 1) / rows; // 底部权重更高
centers.push_back(center);
weights.push_back(weight);
weighted_sum += center * weight;
total_weight += weight;
}
}
if (total_weight > 0 && !centers.empty()) {
double center_avg = weighted_sum / total_weight;
double error = center_avg
- cols / 2.0;
cout << "Line following
- Valid rows: " << centers.size()
<< ", Center avg: " << center_avg << ", Target: " << cols/2
<< ", Error: " << error << endl;
double angle = PID(error);
angle = 82
- angle;
angle = max(min_ang, min(max_ang, angle));
cout << "Calculated angle: " << angle << endl;
gpio
PWM(servo_pin, atc((int)angle));
return 1;
} else {
cout << "Line lost! Only " << centers.size() << " valid rows found." << endl;
// 丢失线路时保持直行，但稍微减速
gpio
PWM(servo_pin, atc(82));
gpio
PWM(
pwm_pin, 8500); // 减速
return 0;
}
}
//
---------- 执行动作函数
----------
void performAction(int letter) {
if (letter == 1) {
cout << "=== Performing A Action: Turn Left ===" << endl;
// 减速
gpio
PWM(
pwm_pin, 8000);
usleep(300000);
// 左转
gpio
PWM(servo_pin, atc(75));
usleep(800000);
// 转回直行并恢复正常速度
gpio
PWM(servo_pin, atc(82));
gpio
PWM(
pwm_pin, 9300);
usleep(400000);
} else if (letter == 2) {
cout << "=== Performing B Action: Turn Right ===" << endl;
// 减速
gpio
PWM(
pwm_pin, 8000);
usleep(300000);
// 右转
gpio
PWM(servo_pin, atc(89));
usleep(800000);
// 转回直行并恢复正常速度
gpio
PWM(servo_pin, atc(82));
gpio
PWM(
pwm_pin, 9300);
usleep(400000);
}
}
//
---------- 主函数
----------
int main() {
cout << "=== STABLE LINE FOLLOWING WITH A/B DETECTION ===" << endl;
cout << "Background Color: " << (BACKGROUND_COLOR == 1 ? "RED" : "BLUE") << endl;
cout << "A: Turn Left, B: Turn Right" << endl;
cout << "=====================================" << endl;
if (dong() < 0) {
cout << "pigpio error !" << endl;
return
-1;
}
VideoCapture cap(0);
if (!cap.isOpened()) {
cout << "cap error" << endl;
gpio
PWM(
pwm_pin, 0);
gpioTerminate();
return
-1;
}
// 设置摄像头参数
cap.set(CAP_PROP_FRAME_WIDTH, 640);
cap.set(CAP_PROP_FRAME_HEIGHT, 480);
cap.set(CAP_PROP_FPS, 30);
cap.set(CAP_PROP_BRIGHTNESS, 50);
cap.set(CAP_PROP_CONTRAST, 50);
cap.set(CAP_PROP_SATURATION, 60); // 提高饱和度有助于颜色检测
Mat frame;
int frame_count = 0;
int detection_interval = 15; // 降低检测频率
int line_lost_count = 0;
const int max_line_lost = 30; // 降低丢失阈值
int sign_detection_count = 0;
const int sign_confirm_threshold = 2;
cout << "Starting stable line following..." << endl;
// 初始前进
gpio
PWM(
pwm_pin, 9300);
try {
while (cap.read(frame)) {
frame_count++;
// 正常巡线
int line_status = xunji(frame);
if (line_status == 0) {
line_lost_count++;
if (line_lost_count > max_line_lost) {
cout << "Line lost for too long, stopping!" << endl;
gpio
PWM(
pwm_pin, 6000);
break;
}
} else {
line_lost_count = 0;
// 恢复速度
gpio
PWM(
pwm_pin, 9300);
}
// 检测标志和字母（降低频率）
if (frame_count % detection_interval == 0 && action_cooldown <= 0) {
bool background_present = detectBackgroundPresence(frame);
if (background_present) {
sign_detection_count++;
cout << "Background detected: " << sign_detection_count << "/" << sign_confirm_threshold << endl;
if (sign_detection_count >= sign_confirm_threshold && !ab_detected) {
int detected = detectABLetter(frame);
if (detected != 0) {
ab_detected = true;
detected_letter = detected;
cout << "=== LETTER DETECTED: " << (detected == 1 ? "A" : "B") << " ===" << endl;
performAction(detected);
action_cooldown = 120; // 冷却时间
sign_detection_count = 0;
}
}
} else {
sign_detection_count = 0;
}
}
// 更新冷却时间
if (action_cooldown > 0) {
action_cooldown
--;
}
// 重置检测状态
if (action_cooldown == 0 && ab_detected) {
ab_detected = false;
detected_letter = 0;
cout << "Reset detection state, ready for next sign" << endl;
}
usleep(20000); // 增加延迟，提高稳定性
}
} catch (const exception& e) {
cout << "Exception occurred: " << e.what() << endl;
}
// 清理
gpio
PWM(
pwm_pin, 0);
gpio
PWM(servo_pin, atc(82));
gpioTerminate();
cap.release();
cout << "Program exited!" << endl;
return 0;
}
本文探讨了传统PWM的Trailing-Edge和Leading-Edge实现及其缺点，并详细介绍了Double-Edge PWM如何通过降低延迟来改进。此外，还讨论了基于环形振荡器的PWM设计和低延迟Double-Edge PWM的实现方法。
最低0.47元/天 解锁文章
扫一扫
9万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
