自己写的代码，可能有问题，参数是随便给的，自己调也不麻烦
#include <opencv2/opencv.hpp>
#include
using namespace cv;
const int img_center_col = 320;
const int total_rows = 480;
int valid_rows = 0;
float valid_ratio = 1.0f;
const float valid_row_thresh = 0.7f; // 有效行阈值
// 输入：二值图 binImg（黑白图）
// 输出：speed —— 最终速度
float getSpeedByCurvatureAndWhite(Mat &binImg,char cishu)
{
float factor = 1.0f;
// 1. 统计白点数量
int whiteCount = countNonZero(binImg);
// 归一化到 0~1
float totalPixel = binImg.rows * binImg.cols;
float whiteRatio = (float)whiteCount / totalPixel;
whiteRatio = fmin(fmax(whiteRatio, 0.0f), 1.0f); // 限制0~1
//2. 计算曲率
float curvature = calcCurvature(binImg);
// 取车正前方最近的行（最下方行）计算偏移
int row = binImg.rows - 10;
float center_col = getCenterAtRow(binImg, row); // 找中心
// 计算偏移量（归一化到0~1）
float offset = fabs(center_col - img_center_col) / (binImg.cols / 2.0f);//img_center_col:中心点坐标
offset = fmin(offset, 1.0f); // 限制最大偏移为1
valid_rows = 0; //有效行数
int check_rows = 0; // 统计实际检查的行数
// 统计有效行（能找到中心的行）
for (int y = 20; y < total_rows - 20; y += 5) { // 每隔5行统计，减少计算
check_rows ++;
if (getCenterAtRow(binImg, y) >= 0) {
valid_rows++;
}
}
if (check_rows > 0)
{ // 防止除0崩溃
valid_ratio = (float)valid_rows / check_rows;
}
else
{
valid_ratio = 0.3f; // 没检查到行，强制减速
}
if(valid_ratio < valid_row_thresh)
{
valid_ratio = valid_ratio / valid_row_thresh;
}
else
valid_ratio = 1;
//3. 函数算速度
const float v_max = 80.0f; // 直道最大速度
const float v_min = 25.0f; // 最小速度
const float k_curve = 0.9f; // 曲率强度
const float k_white = 0.7f; // 白点抑制强度
switch (cishu) {
case 1: // 一次曲线（线性）
{
factor = 1.0f - k_curve * curvature - k_white * whiteRatio;
break;
}
case 2: // 二次曲线（平缓）
{
float curveFactor = 1.0f - k_curve * curvature * curvature;
float whiteFactor = 1.0f - k_white * whiteRatio * whiteRatio;
factor = curveFactor * whiteFactor * offset * valid_ratio;
break;
}
case 3: // 三次曲线（急弯减速最大）
{
float curveFactor = 1.0f - k_curve * pow(curvature, 3);
float whiteFactor = 1.0f - k_white * pow(whiteRatio, 3);
factor = curveFactor * whiteFactor * offset * valid_ratio;
break;
}
default: // 传错值时默认用二次曲线
{
float curveFactor = 1.0f - k_curve * curvature * curvature;
float whiteFactor = 1.0f - k_white * whiteRatio * whiteRatio;
factor = curveFactor * whiteFactor * offset * valid_ratio;
break;
}
}
// 限幅，防止速度为负
factor = fmax(factor, 0.0f);
// 最终速度
return v_min + (v_max - v_min) * factor;
}
// 从二值图计算赛道曲率（0~1）
float calcCurvature(Mat &binImg)
{
int h = binImg.rows;
int w = binImg.cols;
// 取三行：下、中、上
int y1 = h - 20; // 近
int y2 = h / 2; // 中
int y3 = 20; // 远
// 找每行中心
float cx1 = getCenterAtRow(binImg, y1);
float cx2 = getCenterAtRow(binImg, y2);
float cx3 = getCenterAtRow(binImg, y3);
if (cx1 < 0 || cx2 < 0 || cx3 < 0)
return 1.0; // 丢线 → 曲率最大，速度最慢
// 计算偏差 → 代表弯曲程度
float err1 = fabs(cx2 - cx1);
float err2 = fabs(cx3 - cx2);
float totalErr = err1 + err2;
// 归一化曲率 0~1
float curvature = totalErr / (w / 2.0f);
curvature = fmin(curvature, 1.0f);
return curvature;
}
float getCenterAtRow(Mat &binImg, int y)
{
if (y < 0 || y >= binImg.rows) return -1; // 行号越界返回-1
uchar *ptr = binImg.ptr(y);
int w = binImg.cols;
int left = -1, right = -1;
// 找左边缘
for (int x = 0; x < w; x++) {
if (ptr[x] == 255) {
left = x;
break;
}
}
// 找右边缘
for (int x = w - 1; x >= 0; x--) {
if (ptr[x] == 255) {
right = x;
break;
}
}
// 没找到边缘返回-1，否则返回中心点
return (left == -1 || right == -1) ? -1 : (left + right) / 2.0f;
}
