2023世界智能驾驶挑战赛（WIDC）仿真赛-感知决策控制组金奖（冠军）方案和代码分享

前言

2023世界智能驾驶挑战赛（WIDC）元宇宙虚拟仿真赛今天公布成绩，我们队有幸获得了元宇宙虚拟仿真赛-感知决策控制组金奖（第一名），运气不错哈哈，给大家分享一下我们的代码思路和调试经验。
先来看一下比赛需要完成的任务和我们的排名。

决赛的排名：

一、整体方案介绍

从上面的图片可以看到，我们的思路是分模块去调试的：感知模块使用单目相机测距，并用卡尔曼滤波对距离估计进行修正，决策模块使用的是有限状态机：三个状态机之间的切换、控制模块使用最常用的PID，下面会详细介绍各个模块调试的流程。

二、感知模块调试

由于赛题本身只有单目相机和车道线传感器，所以测距只能使用单目相机。

2.1 单目相机测距

单目相机测距的原理是使用单目相机拍的照片中物体的大小来进行估算距离的，上图中已经展示出来了，相机的摄像头和拍物体的照片构成一个相似三角形，这里其实就是小孔成像的原理。计算公式如下：

代码如下：

2.2 数据后处理（卡尔曼滤波）

由于单目相机测距存在一定误差，而且经过调试发现在距离小于7m的时候，距离会变成‘inf’，也就是此时单目相机测距已经失去作用了，需要解决这样的问题，所以需要用卡尔曼滤波进行处理，方案如下。
保留距离的历史数据，如果距离从上一时刻到当前时刻从有值变成了‘inf’，那就把当前的距离估计值暂且等于pre_distance-speed*dt，然后再用卡尔曼滤波对当前的距离估计值进行滤波，使距离估计更准确。

三、决策模块调试

3.1 决策模块思路

决策模块使用三个状态机进行切换，speedup是在交通流中行驶，overtake是需要进行超车时候的状态机，Finalspeedup是最后阶段驶出交通流后将速度提升至最高速行驶的状态机。
规划模块就是一个简单的距离比较，比如我在中间车道，我想超车是从左边还是右边呢，那就看左边车道车和右边的车哪个距离更近，走距离较远的那个车道进行超车。

四、控制模块调试

4.1 控制模块调试

我们采用位置式PID，在纵向控制的时候分为四个阶段：加速阶段、稳定控速阶段、减速第一阶段、减速第二阶段；PID的原理比较简单，这里不再赘述，不懂的同学先去看看PID控制的原理哦。

横向控制就是取车辆前方3-4m一个前视点，然后把它当做目标点去跟踪，计算出转角即可达到横向控制的目的。

总结

2023世界智能驾驶挑战赛（WIDC）元宇宙虚拟仿真赛感知决策控制组的比赛持续了一个多月，从四月份的中下旬开始备赛，前前后后大概一个月的时间，辛苦了一个月拿到了金奖，还是很开心的，欢迎大家改进我们的方案，期待改进出更好的方案。