自动驾驶控制与规划——Project 1: 车辆纵向控制

零、任务介绍
一、环境配置
- [1.1 CARLA的配置](#1.1 CARLA的配置)
- [1.2 Docker Ubuntu 20.04 + ROS2 Foxy的配置](#1.2 Docker Ubuntu 20.04 + ROS2 Foxy的配置)
二、算法
- [2.1 定速巡航](#2.1 定速巡航)
- [2.2 自适应巡航](#2.2 自适应巡航)
- [2.3 离散PID控制](#2.3 离散PID控制)
三、代码实现
- [3.1 代码补全](#3.1 代码补全)
- 3.2仿真验证

零、任务介绍

配置Carla仿真器
配置carla-ros-bridge
补全src\ros-bridge\carla_shenlan_project_1\carla_shenlan_pid_controller\src\pid_controller.cpp中的double PIDController::Control(const double error, const double dt)和void PIDController::Reset()。

一、环境配置

笔者使用的电脑系统配置如下

名称	型号/参数
CPU	13th Gen Intel® Core™ i9-13900H × 20
运行内存	32GB
GPU	NVIDIA GeForce RTX 4060
操作系统	Ubuntu 24.04.1 LTS

1.1 CARLA的配置

从CARLA项目的Release中下载CARLA_0.9.13.tar.gz（该版本支持carla-ros-bridge）下载链接

下载完成后解压到希望安装的位置，在命令行运行CarlaUE4.sh即可启动仿真器。通过-carla-rpc-port=2000可以指定仿真器在主机的2000端口通信。

bash 复制代码

./CarlaUE4.sh -carla-rpc-port=2000

命令行输出如下：

可以看到仿真器的画面：

至此CARLA的配置就完成了，如果ROS和CARLA在同一个机器上运行，还需要配置client，但是笔者由于系统版本，使用下面的Docker配置ROS和client。

1.2 Docker Ubuntu 20.04 + ROS2 Foxy的配置

Ubuntu 24.04 系统python版本为3.12，而carla并没有针对这个python版本的包；Ubuntu 24.04的ROS2版本为jazzy，而carla-ros-bridge对ROS2 foxy适配性比较好，因此笔者选择使用docker配置Ubuntu 20.04 + ROS2 foxy环境。

使用鱼香ROS的一键安装配置docker：小鱼的一键安装

在安装完成后，发现一键安装脚本中的docker的网络配置为bridge模式，而bridge模式只能通过一个特定端口跟主机通信，而这个端口被占用后，CARLA就无法使用这个端口发布数据。

因此笔者使用小鱼的镜像重新创建了一个网络模式为host的容器，命令如下：

bash 复制代码

docker run -d --net=host -it --name foxy \
    -v /home/star:/home/star \
    -v /tmp/.X11-unix:/tmp/.X11-unix \
    -v /dev:/dev \
    -v /dev/dri:/dev/dri \
    --env DISPLAY=unix:1 \
    --env ROS_DISTRO=foxy \
    fishros2/ros:foxy-desktop

启动容器并进入容器

bash 复制代码

docker exec -it foxy /bin/bash

在宿主机允许用户访问x11显示：

bash 复制代码

xhost +

在容器中打开RVIZ2测试显示是否正常

bash 复制代码

rviz2

运行后能看到如下的画面：

小鱼的镜像中没有pip，因此需要先安装pip，执行如下命令

bash 复制代码

sudo apt update
sudo apt upgrade
sudo apt install python3-pip

这样安装的pip版本比较低，运行如下命令升级pip

bash 复制代码

pip install --upgrade pip -i https://mirrors.tuna.tsinghua.edu.cn/pypi/web/simple
sudo apt remove python3-pip
source ~/.bashrc
pip --version

输出为：

bash 复制代码

pip 24.3.1 from /usr/local/lib/python3.8/dist-packages/pip (python 3.8)

安装CARLA client:

bash 复制代码

pip install pygame numpy transforms3d -i https://mirrors.tuna.tsinghua.edu.cn/pypi/web/simple 
pip install carla==0.9.13 -i https://mirrors.tuna.tsinghua.edu.cn/pypi/web/simple

安装其他依赖：

bash 复制代码

sudo apt install ros-foxy-derived-object-msgs

至此环境配置就成功了

二、算法

2.1 定速巡航

定速巡航的输入是设定的速度，上层控制器输出是期望的加速度，底层控制器输出是油门和刹车。

期望纵向加速度为 X ¨ d e s \ddot X_{des} X¨des，使用一阶滞后环节模拟执行过程中的瞬态，可以得到车辆加速度
X ¨ = 1 τ s + 1 X ¨ d e s \ddot X = \frac{1}{\tau s + 1} \ddot X_{des} X¨=τs+11X¨des

纵向速度是纵向加速度的积分
s V ( s ) = 1 τ s + 1 A d e s ( s ) sV(s) = \frac{1}{\tau s + 1} A_{des}(s) sV(s)=τs+11Ades(s)

可以得到被控对象的模型为
P ( s ) = V ( s ) A d e s ( s ) = 1 s ( τ s + 1 ) P(s) = \frac{V(s)}{A_{des}(s)} = \frac{1}{s(\tau s + 1)} P(s)=Ades(s)V(s)=s(τs+1)1

使用PI控制器，误差为实际速度和参考速度的差
u ( t ) = − K p ( V x − V r e f ) − K i ∫ 0 t ( V x − V r e f ) d t u(t) = -K_p(V_x - V_{ref}) - K_i \int_0^t (V_x - V_{ref})dt u(t)=−Kp(Vx−Vref)−Ki∫0t(Vx−Vref)dt

PI控制器的传递函数：
C ( s ) = K p + K i s C(s) = K_p + \frac{K_i}{s} C(s)=Kp+sKi

系统闭环传递函数
V x V d e s = P ( s ) C ( s ) 1 + P ( s ) C ( s ) = K p s + K i τ s 3 + s 2 + K p s + K i \frac{V_x}{V_{des}} = \frac{P(s)C(s)}{1 + P(s)C(s)} = \frac{K_p s + K_i}{\tau s^3 + s^2 + K_p s + K_i} VdesVx=1+P(s)C(s)P(s)C(s)=τs3+s2+Kps+KiKps+Ki

底层控制可以使用车辆模型或者油门标定表实现。

2.2 自适应巡航

自适应巡航考虑有前车的情况，在定速巡航的基础上增加了定时间间隔和定速的切换，分别称为spacing control和speed control。通过设定间隔时间 T g a p T_{gap} Tgap可得安全距离：
D s a f e = D d e f a u l t + V e g o T g a p D_{safe} = D_{default} + V_{ego} T_{gap} Dsafe=Ddefault+VegoTgap

为什么要设定 T g a p T_{gap} Tgap而非直接设置一个固定的距离呢，因为高速低车距易发生碰撞，而低速高车距也不利于车链整体的行驶。

自适应巡航控制器可以用如下公式表示：
D r e l ≤ D s a f e , X ¨ d e s = − K p ( D r e l − D s e t ) − K i ∫ 0 t ( D r e l − D s e t ) d t D r e l > D s a f e , X ¨ d e s = − K p ( V e g o − V s e t ) − K i ∫ 0 t ( V e g o − V s e t ) d t \begin{aligned} D_{rel} \leq D_{safe}, &\ddot X_{des} = -K_p (D_{rel} - D_{set}) - K_i \int_0^t (D_{rel} - D_{set}) dt \\ D_{rel} > D_{safe}, &\ddot X_{des} = -K_p (V_{ego} - V_{set}) - K_i \int_0^t (V_{ego} - V_{set}) dt \end{aligned} Drel≤Dsafe,Drel>Dsafe,X¨des=−Kp(Drel−Dset)−Ki∫0t(Drel−Dset)dtX¨des=−Kp(Vego−Vset)−Ki∫0t(Vego−Vset)dt

2.3 离散PID控制

u ( t ) = K p e ( t ) + K i ∑ t = 0 k e i ⋅ d t + K d e ( k ) − e ( k − 1 ) d t u(t) = K_p e(t) + K_i \sum_{t=0}^k e_i\cdot dt + K_d \frac{e(k) - e(k-1)}{dt} u(t)=Kpe(t)+Kit=0∑kei⋅dt+Kddte(k)−e(k−1)

三、代码实现

3.1 代码补全

cpp 复制代码

double PIDController::Control(const double error, const double dt) {
    assert(dt > 0 && "dt must be positive!!!");   

    // 比例
    proportional_part = kp_ * error;
    // 微分
    if (!first_hit_){
        first_hit_ = true;
        derivative_part = 0.0;
    }
    else{
        derivative_part = kd_ * (error - previous_error_) / dt;
    } 
    // 积分
    integral_ += error * dt;
    integral_part = ki_ * integral_;
    
    // current_output = (proportional_part + integral_part + derivative_part);
    std::cout << "Proportional_part: " << proportional_part << ", Integral_part: " << integral_part << ", Derivative_part: " << derivative_part << std::endl;
    current_output = proportional_part + derivative_part;

    previous_error_ = error;
    previous_output_ = current_output;

    return current_output;
}

cpp 复制代码

void PIDController::Reset() {
    double kp_ = 0.0;
    double ki_ = 0.0;
    double kd_ = 0.0;
    double previous_error_ = 0.0;
    double previous_output_ = 0.0;
    double integral_ = 0.0;
    bool first_hit_ = false;
                
    double proportional_part = 0;
    double integral_part = 0;
    double derivative_part = 0;
    double current_output = 0;
}

3.2仿真验证

在宿主机上启动carla-simulator

bash 复制代码

./CarlaUE4.sh -carla-rpc-port=2000

在容器中编译工作空间

bash 复制代码

colcon build
source install/setup.bash

启动carla-ros-bridge

bash 复制代码

ros2 launch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch.py

启动控制器

bash 复制代码

ros2 run carla_shenlan_pid_controller carla_shenlan_pid_controller_node

启动plot juggler并且监听话题/carla/ego_vehicle/vehicle_status/velocity，/carla/ego_vehicle/target_velocity/velocity，/carla/ego_vehicle/vehicle_control_cmd/throttle

bash 复制代码

ros2 run plotjuggler plotjuggler

最终运行效果

CARLA-ROS2车辆纵向控制仿真

自动驾驶控制与规划——Project 1: 车辆纵向控制

目录

零、任务介绍

一、环境配置

1.1 CARLA的配置

1.2 Docker Ubuntu 20.04 + ROS2 Foxy的配置

二、算法

2.1 定速巡航

2.2 自适应巡航

2.3 离散PID控制

三、代码实现

3.1 代码补全

3.2仿真验证