智能网联车竞赛代码

因为有时不在线，周末才会慢慢更新教程。

 

现在是一个ACC控制器的代码教学

1. SimOne 相关头文件

#include "SimOneNetAPI.h"
#include "SSD/SimPoint3D.h"
#include "util/UtilMath.h"
#include "utilStartSimOneNode.h"

• SimOneNetAPI.h：
  • 它是 SimOne 仿真平台的网络 API 头文件。这个头文件里定义了和仿真环境进行网络通信的函数、数据结构等，例如向仿真环境发送控制指令，或者从仿真环境接收传感器数据。
• SSD/SimPoint3D.h：
  • 此头文件定义了三维空间点的数据结构与相关操作。SSD 是命名空间或者模块名，SimPoint3D 是一个类或者结构体，用来表示三维空间中的一个点，包含 x、y、z 坐标，并且提供了点的运算方法，像点的加法、减法、距离计算等。
• util/UtilMath.h：
  • 包含了一些通用的数学工具函数。util 一般是通用工具模块的命名，其中包含的函数有三角函数计算、向量运算、矩阵运算、距离计算等，这些函数在自动驾驶仿真里处理传感器数据、路径规划等方面会经常用到。
• PID.hpp：
  • PID 指的是比例 - 积分 - 微分控制器，是自动控制领域常用的控制算法。这个头文件定义了 PID 控制器的类或者结构体，以及相关的计算方法，用于实现对车辆速度、转向等的控制。
• ../HDMap/include/SampleGetNearMostLane.h：
  • 从文件名可知，它和高精度地图（HDMap）有关。这个头文件提供了一个函数，用于获取距离指定点最近的车道信息.
• ../HDMap/include/SampleGetLaneST.h：
  • 同样和高精度地图相关，这个头文件提供了一个函数，用于将车辆的位置转换到车道的 s-t 坐标系下。s 代表沿着车道中心线的纵向距离，t 代表垂直于车道中心线的横向距离，这种转换在路径规划和车辆控制中很常用。
• <memory>：
  • 该头文件提供了智能指针相关的功能，像 std::unique_ptr、std::shared_ptr 和 std::weak_ptr。智能指针能够自动管理动态分配的内存，避免内存泄漏，在现代 C++ 编程中被广泛使用。
• <limits>：
  • 这个头文件定义了各种数值类型的极限值，例如 std::numeric_limits<int>::max() 可以获取 int 类型的最大值。在代码里，可能会用这些极限值来初始化变量或者进行边界检查。
  •  
  • <iostream>：
    • 它提供了标准输入输出流的功能，包含 std::cin（标准输入）、std::cout（标准输出）和 std::cerr（标准错误输出）。在代码中，通常会用 std::cout 输出调试信息或者程序运行状态。
• 当障碍物在主车辆后方且距离小于安全距离时：
  • 运用 PID 控制器计算控制命令，目标是让主车辆与障碍物的距离保持在安全距离范围内。
  • 根据 PID 控制器的输出调整油门和刹车。若输出为正，则加大油门；若输出为负，则加大刹车。
• 计算预计碰撞时间，当预计碰撞时间小于设定的阈值时，触发自动紧急制动（AEB）状态：
  • 进入 AEB 状态后，将油门设为 0，只进行刹车操作。
  • 根据预计碰撞时间的紧急程度，调整刹车力度。 

    #include "PID.hpp"
     #include "../HDMap/include/SampleGetNearMostLane.h" 
    #include "../HDMap/include/SampleGetLaneST.h"

     标准库函数
  •  
•  
   

#include <memory>
#include <limits>
#include <iostream>

 

 

2. 全局变量定义

cpp

bool inAEBState = false;
bool isSimOneInitialized = false;

• inAEBState：表示是否处于自动紧急制动（AEB）状态。
• isSimOneInitialized：表示 SimOne 仿真环境是否已经初始化完成。

3. 仿真环境初始化

cpp

StartSimOne::WaitSimOneIsOk(true);
SimOneSM::SetDriverName(0, "ACC");

// load HdMap
int timeout = 20;
while (true) {
    if (SimOneSM::LoadHDMap(timeout)) {
        std::cout << "HDMap Information Loaded" << std::endl;
        break;
    }
    std::cout << "HDMap Information Loading..." << std::endl;
}

 

 

• StartSimOne::WaitSimOneIsOk(true)：等待 SimOne 仿真环境准备就绪。
• SimOneSM::SetDriverName(0, "ACC")：设置 0 号车辆的驾驶员名称为 "ACC"。
• 循环尝试加载高精度地图（HDMap），直到加载成功。

4. 主循环

cpp

while (true) {
    int frame = SimOneAPI::Wait(); 

    if (SimOneAPI::GetCaseRunStatus() == SimOne_Case_Status::SimOne_Case_Status_Stop) {
        break;
    }

    std::unique_ptr<SimOne_Data_Gps> pGps = std::make_unique<SimOne_Data_Gps>();
    if (!SimOneSM::GetGps(0, pGps.get())) {
        std::cout << "Fetch GPS failed" << std::endl;
    }

    std::unique_ptr<SimOne_Data_Obstacle> pObstacle = std::make_unique<SimOne_Data_Obstacle>();
    if (!SimOneAPI::GetSimOneGroundTruth(pObstacle.get())) {
        std::cout << "Fetch obstacle failed" << std::endl;
    }

 

• SimOneAPI::Wait()：等待下一帧仿真数据。
• 当仿真案例状态为停止时，跳出主循环。
• 获取 0 号车辆的 GPS 信息和障碍物信息，如果获取失败则输出错误信息。

5. 车辆和障碍物信息处理

cpp

if (SimOneAPI::GetCaseRunStatus() == SimOne_Case_Status::SimOne_Case_Status_Running && pObstacle->timestamp > 0 && pGps->timestamp > 0) {
    if (!isSimOneInitialized) {
        std::cout << "SimOne Initialized!" << std::endl;
        isSimOneInitialized = true;
    }

    SSD::SimPoint3D mainVehiclePos(pGps->posX, pGps->posY, pGps->posZ);
    double mainVehicleSpeed = UtilMath::calculateSpeed(pGps->velX, pGps->velY, pGps->velZ);

    double minDistance = std::numeric_limits<double>::max();
    int potentialObstacleIndex = pObstacle->obstacleSize;
    SSD::SimString mainVehicleLaneId = SampleGetNearMostLane(mainVehiclePos);
    SSD::SimString potentialObstacleLaneId = "";
    for (size_t i = 0; i < pObstacle->obstacleSize; ++i) {
        SSD::SimPoint3D obstaclePos(pObstacle->obstacle[i].posX, pObstacle->obstacle[i].posY, pObstacle->obstacle[i].posZ);
        SSD::SimString obstacleLaneId = SampleGetNearMostLane(obstaclePos);
        if (mainVehicleLaneId == obstacleLaneId) {
            double obstacleDistance = UtilMath::planarDistance(mainVehiclePos, obstaclePos);

            if (obstacleDistance < minDistance) {
                minDistance = obstacleDistance;
                potentialObstacleIndex = (int)i;
                potentialObstacleLaneId = obstacleLaneId;
            }
        }
    }

    auto& potentialObstacle = pObstacle->obstacle[potentialObstacleIndex];
    double obstacleSpeed = UtilMath::calculateSpeed(potentialObstacle.velX, potentialObstacle.velY, potentialObstacle.velZ);

    SSD::SimPoint3D potentialObstaclePos(potentialObstacle.posX, potentialObstacle.posY, potentialObstacle.posZ);
    double sObstalce = 0.;
    double tObstacle = 0.;

    double sMainVehicle = 0.;
    double tMainVehicle = 0.;

    bool isObstalceBehind = false;
    if (!potentialObstacleLaneId.Empty()) {

        SampleGetLaneST(potentialObstacleLaneId, potentialObstaclePos, sObstalce, tObstacle);
        SampleGetLaneST(mainVehicleLaneId, mainVehiclePos, sMainVehicle, tMainVehicle);

        isObstalceBehind = !(sMainVehicle >= sObstalce);
    }

 

• 当仿真处于运行状态且障碍物和 GPS 数据有效时，进行以下操作：
  • 输出 SimOne 初始化完成信息。
  • 计算主车辆的位置和速度。
  • 找出与主车辆在同车道且距离最近的障碍物。
  • 计算障碍物和主车辆在车道上的 s 和 t 坐标。
  • 判断障碍物是否在主车辆后方。

6. 车辆控制

cpp

std::unique_ptr<SimOne_Data_Control> pControl = std::make_unique<SimOne_Data_Control>();

// Control mainVehicle with SimOneDriver
SimOneSM::GetDriverControl(0, pControl.get());

std::cout << "Command Form SimOneDriver(throttle, brake, steering)" << pControl->throttle << "," <<
    pControl->brake << "," << pControl->steering << std::endl;

// Control mainVehicle without SimOneDriver
/*pControl->throttle = 0.5f;
pControl->brake = 0.f;
pControl->steering = 0.f;
pControl->handbrake = false;
pControl->isManualGear = false;
pControl->gear = static_cast<EGearMode>(1);*/

if (isObstalceBehind) {
    double defaultDistance = 10.0;
    double safeDistance = 30.0;
    double timeToCollision = std::abs((minDistance - defaultDistance)) / (obstacleSpeed - mainVehicleSpeed);
    double defaultTimeToCollisionGeneral = 3.6;
    double defaultTimeToCollisionUrgent = 1.8;

    if (minDistance < safeDistance) {

        PID PidControl(0.3, 1.8 * minDistance / 20., 0.);

        float command = -(float)(PidControl.pidControl(safeDistance, minDistance) / (0.5f * 0.05f * 0.05f));

        pControl->throttle = command > 0 ? command : 0.f;
        pControl->brake = command > 0 ? 0.f : -command;

        inAEBState = false;
        if (-timeToCollision < defaultTimeToCollisionGeneral && timeToCollision < 0) {
            inAEBState = true;
            pControl->brake = (float)(mainVehicleSpeed * 3.6 * 1.0 + 0.20);
            if (-timeToCollision < defaultTimeToCollisionUrgent) {
                pControl->brake = (float)(mainVehicleSpeed * 3.6 * 1.2 + 0.20);
            }
        }
        if (inAEBState) {
            pControl->throttle = 0.f;
        }
    }				
}
SimOneSM::SetDrive(0, pControl.get());

 

• 获取 SimOneDriver 对主车辆的控制命令并输出。
• 当障碍物在主车辆后方且距离小于安全距离时：
  • 使用 PID 控制器计算控制命令。
  • 根据计算结果调整油门和刹车。
  • 当碰撞时间小于设定阈值时，进入 AEB 状态并加大刹车力度。
• 将最终的控制命令发送给 SimOne 仿真环境。

7. 进入下一帧

cpp

SimOneAPI::NextFrame(frame);

下面解释一下主要代码

1. 前提条件判断

cpp

if (isObstalceBehind) {

 

首先检查障碍物是否在主车辆后方。只有当障碍物在主车辆后方时，才会执行后续的速度控制和紧急制动逻辑。

2. 常量定义

cpp

double defaultDistance = 10.0;
double safeDistance = 30.0;
double timeToCollision = std::abs((minDistance - defaultDistance)) / (obstacleSpeed - mainVehicleSpeed);
double defaultTimeToCollisionGeneral = 3.6;
double defaultTimeToCollisionUrgent = 1.8;

 

• defaultDistance：设定的一个默认距离，用于计算预计碰撞时间。
• safeDistance：安全距离，当主车辆与障碍物的距离小于此值时，会触发速度控制逻辑。
• timeToCollision：计算预计碰撞时间，公式为 |当前距离 - 默认距离| / (障碍物速度 - 主车辆速度)。取绝对值是为了确保时间为正数。
• defaultTimeToCollisionGeneral：一般情况下的预计碰撞时间阈值，当预计碰撞时间小于此值时，会触发自动紧急制动（AEB）状态。
• defaultTimeToCollisionUrgent：紧急情况下的预计碰撞时间阈值，当预计碰撞时间小于此值时，会加大刹车力度。

3. 安全距离判断

cpp

if (minDistance < safeDistance) {

 

检查主车辆与障碍物的距离是否小于安全距离。如果小于安全距离，则执行后续的速度控制逻辑。

4. PID 控制器计算

cpp

PID PidControl(0.3, 1.8 * minDistance / 20., 0.);
float command = -(float)(PidControl.pidControl(safeDistance, minDistance) / (0.5f * 0.05f * 0.05f));

 

• PID PidControl(0.3, 1.8 * minDistance / 20., 0.);：创建一个 PID 控制器实例，三个参数分别是比例系数（0.3）、积分系数（1.8 * minDistance / 20.）和微分系数（0）。积分系数根据当前距离动态调整。
• PidControl.pidControl(safeDistance, minDistance)：调用 PID 控制器的 pidControl 方法，计算控制命令，目标值是安全距离，当前值是实际距离。
• command：将 PID 控制器的输出除以一个固定系数（0.5f * 0.05f * 0.05f），并取负，得到最终的控制命令。

5. 油门和刹车控制

cpp

pControl->throttle = command > 0 ? command : 0.f;
pControl->brake = command > 0 ? 0.f : -command;

 

• 如果控制命令 command 大于 0，则将其作为油门值，刹车值设为 0。
• 如果控制命令 command 小于等于 0，则将其绝对值作为刹车值，油门值设为 0。

6. 自动紧急制动（AEB）逻辑

cpp

inAEBState = false;
if (-timeToCollision < defaultTimeToCollisionGeneral && timeToCollision < 0) {
    inAEBState = true;
    pControl->brake = (float)(mainVehicleSpeed * 3.6 * 1.0 + 0.20);
    if (-timeToCollision < defaultTimeToCollisionUrgent) {
        pControl->brake = (float)(mainVehicleSpeed * 3.6 * 1.2 + 0.20);
    }
}

 

• 首先将 AEB 状态 inAEBState 设为 false。
• 当预计碰撞时间满足 (-timeToCollision < defaultTimeToCollisionGeneral && timeToCollision < 0) 时，进入 AEB 状态，将 inAEBState 设为 true，并根据主车辆速度计算刹车值。
• 如果预计碰撞时间进一步小于紧急阈值 defaultTimeToCollisionUrgent，则加大刹车力度。

7. 油门限制

cpp

if (inAEBState) {
    pControl->throttle = 0.f;
}

当处于 AEB 状态时，将油门值设为 0，确保车辆只进行刹车操作。

 

 

这些代码只是一小部分，如果需要完整代码可以私信。