零、任务介绍

1. 补全src/ros-bridge/carla_shenlan_projects/carla_shenlan_stanley_pid_controller/src/stanley_controller.cpp中的TODO部分。

一、环境配置

上一次作业中没有配置docker使用gpu，后续可能有GPU计算的需求，因此重新运行一个带有GPU的容器。docker使用GPU的配置教程可以参考：在docker容器中使用nvidia显卡渲染rviz2界面。运行容器的命令如下：

docker run -d --net=host -it --name foxy_gpu --gpus all -e NVIDIA_DRIVER_CAPABILITIES=all\
    -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

二、算法

Stanley控制使用前轮中心作为参考点，根据轨迹上距离参考点最近的点计算偏航误差和横向误差。

首先根据参考点的heading和车的heading计算偏航误差 ${\theta }_e$。然后考虑横向误差，由几何关系可得
$$\begin{array}{r}\tan {\delta }_e=\frac{e}{d},d=v/k\end{array}$$
可得
$${\delta }_e={\tan }^{-1}\frac{ke}{v}$$
此处的增益$k$根据实验调整。结合上述两个误差项可以得到Stanley控制律
$$\delta (t)={\theta }_e(t)+{\tan }^{-1}\frac{ke(t)}{v(t)}$$
观察上述控制律可以发现，当车速$v$较低时，即便是比较小的横向误差$e$也会引起反正切函数的剧烈变化，因此在分母上增加一项常数，控制律变为
$$\delta (t)={\theta }_e(t)+{\tan }^{-1}\left(\frac{ke(t)}{k_s+v(t)}\right)$$
当车速较快时，如果轨迹的偏航角变化较大，直接跟踪会导致车辆横向振荡，因此可以在${\theta }_e$中加入阻尼，即增加PD控制器。综上所述，最终的Stanley控制器如下
$$\delta =PD(\theta )+{\tan }^{-1}\left(\frac{ke(t)}{k_s+v(t)}\right)$$

三、代码实现

此处使用的PD控制器可以参考上一个project中的实现方法自动驾驶控制与规划——Project 1: 车辆纵向控制。为了避免低速行驶时的横向振荡，加入参数$k_s$

class StanleyController {
public:
  StanleyController(){};
  ~StanleyController(){};

  void LoadControlConf();
  void ComputeControlCmd(const VehicleState &vehicle_state,
                         const TrajectoryData &planning_published_trajectory,
                         ControlCmd &cmd);
  void ComputeLateralErrors(const double x, const double y, const double theta,
                            double &e_y, double &e_theta);
  TrajectoryPoint QueryNearestPointByPosition(const double x, const double y);

protected:
  std::vector<TrajectoryPoint> trajectory_points_;
  double k_y_ = 0.0;
  double k_s_ = 0.0;	// 低速行驶时v小，较小的e也会导致atan振荡
  double u_min_ = 0.0;
  double u_max_ = 100.0;

  double theta_ref_;
  double theta_0_;
};
} // namespace control
} // namespace shenlan

这里的参数可以根据实验效果进行调整

void StanleyController::LoadControlConf() {
    k_y_ = 0.5;
    k_s_ = 0.5;
}

控制器整体的流程是：1.计算heading error；2.计算cross tracking error；3.利用Stanley控制器计算控制指令。需要注意对输出进行限幅。

void StanleyController::ComputeControlCmd(const VehicleState &vehicle_state, const TrajectoryData &planning_published_trajectory, ControlCmd &cmd) {
    trajectory_points_ = planning_published_trajectory.trajectory_points;
    // find the closest point on the reference trajectory
    TrajectoryPoint nearest_pt = QueryNearestPointByPosition(vehicle_state.x, vehicle_state.y);
    // theta_ref_在QueryNearestPointByPosition中已经更新了

    // get lateral error and heading error
    double e_y = 0.0;
    double e_theta = 0.0;

    ComputeLateralErrors(vehicle_state.x - nearest_pt.x, vehicle_state.y - nearest_pt.y, vehicle_state.heading, e_y, e_theta);

    double e_theta_pd = e_theta_pid_controller.Control(e_theta, 0.01);
    cmd.steer_target = e_theta_pd + atan2(k_y_ * e_y, vehicle_state.velocity + k_s_);

    // 输出限幅
    if (cmd.steer_target > 1.0) {
        cmd.steer_target = 1.0;
    } else if (cmd.steer_target < -1.0) {
        cmd.steer_target = -1.0;
    }
}

在计算误差时需要注意，横向误差是带有方向的，以车辆朝向为参考，左正右负。偏航误差在计算时超过$[-\pi ,\pi )$的需要重新标准化到$[-\pi ,\pi )$中。

void StanleyController::ComputeLateralErrors(const double x, const double y, const double theta, double &e_y, double &e_theta) {
    // 车头方向的单位矢量 (cos(theta), sin(theta))
    // 横向误差以车辆朝向为参考，左正右负
    e_y = cos(theta) * y - sin(theta) * x;

    e_theta = theta - theta_ref_;
    if (e_theta <= -M_PI) {
        e_theta += 2 * M_PI;
    } else if (e_theta >= M_PI) {
        e_theta -= 2 * M_PI;
    }
    std::cout << "theta: " << theta << " theta_ref_: " << theta_ref_ << std::endl;
    std::cout << "e_theta: " << e_theta << std::endl;
}

四、效果展示

在宿主机启动carla仿真器

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

在docker容器中启动carla-ros-bridge

ros2 launch carla_shenlan_bridge_ego_vis carla_bridge_ego_vehicle.launch.py

启动控制节点

ros2 run carla_shenlan_stanley_pid_controller carla_shenlan_stanley_pid_controller_node

运行效果如下：