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

零、任务介绍

课程主页

1. 配置Carla仿真器
2. 配置carla-ros-bridge
3. 补全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端口通信。

./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的容器，命令如下：

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

启动容器并进入容器

docker exec -it foxy /bin/bash

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

xhost +

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

rviz2

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

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

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

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

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

输出为：

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

安装CARLA client:

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 

安装其他依赖：

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

至此环境配置就成功了

二、算法

2.1 定速巡航

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

期望纵向加速度为 ${\ddot{X}}_{des}$，使用一阶滞后环节模拟执行过程中的瞬态，可以得到车辆加速度
$$\ddot{X}=\frac{1}{\tau s+1}{\ddot{X}}_{des}$$
纵向速度是纵向加速度的积分
$$sV(s)=\frac{1}{\tau s+1}{A}_{des}(s)$$
可以得到被控对象的模型为
$$P(s)=\frac{V(s)}{A_{des}(s)}=\frac{1}{s(\tau s+1)}$$
使用PI控制器，误差为实际速度和参考速度的差
$$u(t)=-K_p(V_x-V_{ref})-K_i{\int }_0^t(V_x-V_{ref})dt$$
PI控制器的传递函数：
$$C(s)=K_p+\frac{K_i}{s}$$
系统闭环传递函数
$$\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}$$
底层控制可以使用车辆模型或者油门标定表实现。

2.2 自适应巡航

自适应巡航考虑有前车的情况，在定速巡航的基础上增加了定时间间隔和定速的切换，分别称为spacing control和speed control。通过设定间隔时间$T_{gap}$可得安全距离：
$$D_{safe}=D_{default}+V_{ego}{T}_{gap}$$
为什么要设定$T_{gap}$而非直接设置一个固定的距离呢，因为高速低车距易发生碰撞，而低速高车距也不利于车链整体的行驶。
自适应巡航控制器可以用如下公式表示：
$$\begin{array}{rl}{D}_{rel}\le 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{array}$$

2.3 离散PID控制

$$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}$$

三、代码实现

3.1 代码补全

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;
}

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

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

在容器中编译工作空间

colcon build
source install/setup.bash

启动carla-ros-bridge

ros2 launch carla_ros_bridge carla_ros_bridge_with_example_ego_vehicle.launch.py

启动控制器

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

ros2 run plotjuggler plotjuggler 

最终运行效果