Hcc的BOLG
0%

如何读懂我的智能车开源

发表于 分类于 ， Valine：

一些C++类与对象的基础

        这一部分需要读者自学，不用太过于深入，只需要知道在C++中类如何建立、如何在类中定义函数并且如何在类外调用即可。

下载开源

        访问链接https://github.com/hccc1203/RunACCM2025 按照下图所示操作。

下载开源
图 1 下载开源
下载并解压
图 2 解压

        在vscode中打开RunACCM2025-main文件夹。

打开
图 3 在vscode打开

代码结构

主要文件作用说明

  • main.cpp：线程创建与整体调度入口
  • standard.cpp：核心巡线与决策逻辑
  • general.h：常用工具函数封装
  • cross.cpp / cross.h：十字元素处理模块
  • ring：圆环相关算法实现
            本程序主要使用的是生产者-消费者多线程模型，下面代码展示了整个程序的线程结构，是理解本项目运行机制的关键。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
std::vector<PredictResult> predict_result;

std::thread task_producer(&producer, std::ref(task_factory), std::ref(AI_task_factory), std::ref(config));

std::thread AI_producer(&AIConsumer, std::ref(AI_task_factory), std::ref(predict_result), std::ref(config));

std::thread task_consumer(&consumer, std::ref(task_factory), std::ref(debug_factory), std::ref(predict_result), std::ref(config), std::ref(uart));
if (config.en_show) {
    std::thread debug_data_consumer(&debugDataConsumer, std::ref(debug_factory));
    debug_data_consumer.join();
}
task_producer.join();
AI_producer.join();
task_consumer.join();

        由上述代码可以知道，在不开启显示的情况下我们一共启动了三个线程，分别是task_producer，AI_producer，task_consumer。这三个线程分别有什么用呢，我们可以通过vscode跳转至其执行函数

task_producer

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
bool producer(Factory<TaskData> & task_data, Factory<TaskData> & AI_task_data, Config & config) {
    Capture capture(config);
    while (true) {
        TaskData src;
        if (!capture.getImage(src.img)) 
        {
            printf("noimage");
            continue;
        }
        auto time_now = std::chrono::steady_clock::now();
        src.timestamp = time_now;
        AI_task_data.produce(src);
        task_data.produce(src);
        if(flag)
        {
            exit(0);
        }
    }
    return true;	
}

        不难看出，这个线程主要的任务就是采集图像并生成类型为TaskData的结构体，并将其放入AI_task_data和task_data的队列中去，以便于AI_producer和task_consumer两个线程调用。

AI_producer

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
bool AIConsumer(Factory<TaskData> & AI_task_data, std::vector<PredictResult> & predict_result, Config & config) {
    shared_ptr<Detection> detection = make_shared<Detection>(config.model);
    detection->score = config.score;
    std::mutex lock;
    while (true) {
        TaskData dst;
        AI_task_data.consume(dst);
        detection->inference(dst.img);
        lock.lock();
        predict_result = detection->results;
        Mat img = dst.img.clone();
        // 此代码为开源代码
        if(flag)
        {
            exit(0);
        }
        printf("Detected\n");
        if(config.en_AI_show)
        lock.unlock();
    }
    return true;	
}

        这个线程主要功能就是从AI_task_data拿出一个TaskData类型的结构并用于AI推理，并将推理结果放入predict_result这个全局变量中（本质上是PredictResult结构的有向数组）

task_consumer

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
bool consumer(Factory<TaskData> & task_data, Factory<DebugData> & debug_data, std::vector<PredictResult> & predict_result, Config & config, shared_ptr<Uart> & uart) {
	// 此代码为开源代码
    Standard standard(config);
    bool stop2_flag = false;
    int stop_counter = 0;
    while (true) {
        auto time_start = std::chrono::steady_clock::now();
        TaskData src;
        DebugData debug;
        auto result = predict_result;
        task_data.consume(src);
        if (src.img.empty()) continue;
        // 执行巡线代码
        TaskData dst = standard.run(src.img, predict_result,pitch_angle);
        printf("run_finish!\n");
        
        uart->carControl(dst.speed, dst.pwm,dst.buzzer_enable); // 串口通信控制车辆
        printf("run_finish2!\n");
        
        if(standard.Stop_flag)
        {
            uart->carControl(0,dst.pwm,2);
            stop_counter ++;
            // standard.videoWriter.release();
            if(stop_counter > 100)
            {
                flag = true;
                stop2_flag = false;
            }
        }
        if(flag)
        {
            exit(0);
        }
		// debug
        debug.img = dst.img;
		debug.results = result;
        debug.pwm = dst.pwm;
        debug.speed = dst.speed;
		debug_data.produce(debug);
		auto time_end = std::chrono::steady_clock::now();
		auto t = std::chrono::duration<double, std::milli>(time_end - time_start).count();
		// std::cout << (int)t << "ms" << std::endl;
        signal(SIGINT, callbackSignal);
	}
	return true;
}

        这个线程就是我们智能车的主要线程了，从代码中可以看到这个线程主要从task_data队列中获得一个包含图片、时间等数据的结构体，并将图片和AI推理结果传入Standard大类中的run函数，这个函数也返回一个结构体，其中包含了车辆目标速度、车辆目标打角等信息，再使用串口将这些信息发送至下位机TC264中进行处理。
        那么接下来我们着重介绍一下Standard这个大类。

Standard类run函数解析

        Standard.run函数分为以下几个部分：基础巡线、元素检测、元素处理、中线选择、偏差计算、速度判定、运行控制。接下来我将逐个讲解。

基础巡线部分

基础巡线
图 4 基础巡线部分
        trackRecognition函数主要是对基础赛道进行处理，包括左右手巡线、透视变换、角点检测、直道判断等，fitting函数则是对透视变换后的边线进行向量法平移得到拟合后的中线，这部分暂时由读者自行学习，后续我会出更详细的教程。

元素检测及处理

        此处以十字识别及处理为示例

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
    /* ***************************************************************** */
    /* **************************** 元素检测 **************************** */
    /* ***************************************************************** */
if (scene == Scene::NormalScene  && burger_end_counter > 100  && _elem_order[order_index] == 6)
{
    cross.Cross_Check(is_t_L_pointLeft_find, is_t_L_pointRight_find, imgBinary,t_L_pointLeft,t_L_pointRight,t_L_pointLeft_id,t_L_pointRight_id,t_pointsEdgeLeft_size,t_pointsEdgeRight_size);
    if (cross.flag_cross != Cross::Cross_None)
    {
        scene = Scene::CrossScene;
    }
}

// ........................
// 此处省略了一些代码

    /* ***************************************************************** */
    /* **************************** 元素处理 **************************** */
    /* ***************************************************************** */
if (scene == Scene::CrossScene)
{
    cross.Cross_Run(t_pointsEdgeLeft, t_pointsEdgeRight, imgGray,is_t_L_pointLeft_find, is_t_L_pointRight_find,t_L_pointLeft_id, t_L_pointRight_id,t_pointsEdgeLeft_size,t_pointsEdgeRight_size);
    t_pointsEdgeLeft_size = t_pointsEdgeLeft.size();
    t_pointsEdgeRight_size = t_pointsEdgeRight.size();

    if ((t_pointsEdgeRight_size - t_pointsEdgeLeft_size) > 1)
    {
        trackState = TrackState::TRACK_RIGHT;
    }
    else if ((t_pointsEdgeLeft_size - t_pointsEdgeRight_size) > 1)
    {
        trackState = TrackState::TRACK_LEFT;
    }
    centerCompute(t_pointsEdgeLeft, t_pointsEdgeLeft_size, 0);
    t_left_CenterEdge_size = t_left_CenterEdge.size();

    centerCompute(t_pointsEdgeRight, t_pointsEdgeRight_size, 1);
    t_right_CenterEdge_size = t_right_CenterEdge.size();
    
    if (cross.flag_cross == Cross::Cross_None)
    {
        scene = Scene::NormalScene;
        order_index++;
    }
}

        我们在程序开始时定义了一个名为scene的枚举体，并将其初始化为NormalScene。在Cross.h文件对Cross类的定义中，我们又根据车辆经过十字路口时的过程和状态将其分类，如图六所示

scene
图 5 scene
scene
图 6 CrossState
        我们在处理元素的过程中，主要做以下几件事：先检测scene是否为NormalScene，确保是正常状态下再检测十字；接着跑cross.Cross_Check函数，当检测到十字后，十字类内部的flag_cross将发生变化，不再是Cross::Cross_None，此时判定为进入十字；接着运行cross.Cross_Run函数，对十字进行处理，其中可能要对边线做出改变（如补线操作），所以在十字结束后再运行一次centerCompute函数，拟合最新的中线。

        有细心的读者应该观察到，我们此处还有一个判断_elem_order[order_index] == 6的操作，这是我们团队的小巧思。我们在程序开始时定义了一个_elem_order数组并将order_index初始化为0，这个数组中存放的是整个赛道所有元素的顺序,我们检测完一个元素，order_index就加一，可以做到按顺序检测元素，从而降低算力消耗和误判概率。
        此外，基于这套系统，我们还可以做出针对赛道的分段速度或分段预瞄点。在杭电国赛第一天，我们就是降低了第一个元素前赛道的速度才得以完赛。

1
2
3
4
5
6
7
8
9
10
11
"elem_order":[5,6,6,6,6,5,2,0,6,6,5,5],
"speed_order":[90,90,90,90,90,80,90,90,90,90,90,90,90,90],
"aim_dis_n_order":[0.4,0.4,0.4,0.4,0.4,0.4,0.4,0.4,0.4,0.4,0.4,0.4,0.4,0.4],
"aim_angle_p_order":[0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9,0.9],
"aim_angle_d_order":[0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8],
"obstacle_order_index":[0,0,0,0,0,1,0,0,0,0,0,0],
"record": [
    {
        "#order": " 顺序定义 1.汉堡 2.虚线停车 4.障碍 5.圆环 6.十字 7.桥 0.终止位 8.左1充电 9.左2充电 10.右1充电 11.右2充电"
    }
]

中线选择

        这一部分并没有什么特别的，只是根据前面所选择的trackState来将不同的中线存放到t_CenterEdge这个有向数组中，防止混乱。

偏差计算

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
// 找最近点(起始点中线归一化)
   for (int i = 0; i < t_CenterEdge_size; i++)
   {
       float dx = t_CenterEdge[i].x - cx;
       float dy = t_CenterEdge[i].y - cy;
       float dist = sqrt(dx * dx + dy * dy);
       if (dist < min_dist)
       {
           min_dist = dist;
           begin_id = i;
       }
   }

   begin_id = general.clip(begin_id, 0, t_CenterEdge_size - 1);

   std::vector<POINT> temp_center;

   int temp_center_size;
   printf("begin id%d size %d\n", begin_id, t_CenterEdge_size);
   if ((begin_id >= 0 && t_CenterEdge_size - begin_id >= 3))
   {
       center_effective_flag = true;
       if (begin_id > 0)
       {
           cx = t_CenterEdge[begin_id].x;
           cy = t_CenterEdge[begin_id].y;
       }
       for (int i = begin_id; i < t_CenterEdge_size; i++)
       {
           temp_center.emplace_back(t_CenterEdge[i].x, t_CenterEdge[i].y);
       }

       temp_center_size = temp_center.size();
       t_CenterEdge.clear();
       t_CenterEdge_size = 0;
       // logger("temp:size", temp_center_size);
       resample_points(temp_center, temp_center_size, t_CenterEdge,
                       t_CenterEdge_size, SAMPLE_DIST * pixel_per_meter);


       double min_dis = 1000000;

       for(int i = 1;i < t_CenterEdge_size;i++)
       {
           double dx = t_CenterEdge[i].x - cx;
           double dy = cy - t_CenterEdge[i].y;
           double dn = sqrt(dx*dx + dy*dy);

           double dis = aim_distance_f * pixel_per_meter - dn;
           if(dis < 0)
           {
               dis *= -1;
           }
           if(dis  < min_dis)
           {
               aim_index_far = i;
               min_dis = dis;
           }
       }

       min_dis = 1000000;
       for(int i = 1;i < t_CenterEdge_size;i++)
       {
           double dx = t_CenterEdge[i].x - cx;
           double dy = cy - t_CenterEdge[i].y;
           double dn = sqrt(dx*dx + dy*dy);

           double dis = aim_distance_n * pixel_per_meter - dn;
           if(dis < 0)
           {
               dis *= -1;
           }
           if(dis  < min_dis)
           {
               aim_index_near = i;
               min_dis = dis;
           }
       }
       float dx =
           t_CenterEdge[aim_index_far].x - cx; // rptsn[aim_idx__far][0] - cx;
       float dy = cy - t_CenterEdge[aim_index_far].y +
                  car_length * pixel_per_meter; // cy - rptsn[aim_idx__far][1];
       float dn = sqrt(dx * dx + dy * dy);
       float error_far =
           (-atanf(pixel_per_meter * 2 * car_length * dx / dn / dn) * 180 /
            PI);
       assert(!isnan(error_far));
       printf("far_dx:%f,far_dy %f\n",dx,dy);
       printf("cx %f cy %f\n",cx,cy);
       // 计算近锚点偏差值
       float dx_near =
           t_CenterEdge[aim_index_near].x - cx; // rptsn[aim_idx_near][0] - cx;
       float dy_near =
           cy - t_CenterEdge[aim_index_near].y +
           car_length * pixel_per_meter; // cy - rptsn[aim_idx_near][1];
       float dn_near = sqrt(dx_near * dx_near + dy_near * dy_near);
       float error_near = (-atanf(pixel_per_meter * 2 * car_length * dx_near /
                                  dn_near / dn_near) *
                           180 / PI);
       assert(!isnan(error_near));
       // ...此处省略一些代码
   }

        这段代码的主要思想即为纯跟踪，B站上有许多讲解纯跟踪原理的视频，大家可以自行观看。省略的代码是在每一种场景下远预瞄点和近预瞄点不同的权重。

剩余部分

        剩余部分主要是不同场景下的速度控制以及PD计算出舵机打角值，对于走马观碑和燕过留痕组用处不大，各位读者可以选择性观看。

结语

        希望这篇文章能够帮助你建立一种阅读智能车开源代码的思路：先看结构，再看数据流，最后深入算法细节。后续我也会围绕基础巡线、元素算法、参数设计和调试方法，继续对这套系统进行更细致的讲解，帮助更多同学真正做到“看得懂、改得动、用得稳”。