网站有收录没权重,汉阳放心的建站企丿,视频上到什么地方可以做网站链接,wordpress 404插件目录 零、任务介绍一、环境配置二、算法三、代码实现四、效果展示 零、任务介绍
补全src/ros-bridge/carla_shenlan_projects/carla_shenlan_stanley_pid_controller/src/stanley_controller.cpp中的TODO部分。
一、环境配置
上一次作业中没有配置docker使用gpu#xff0c;… 目录 零、任务介绍一、环境配置二、算法三、代码实现四、效果展示 零、任务介绍
补全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 --nethost -it --name foxy_gpu --gpus all -e NVIDIA_DRIVER_CAPABILITIESall\-v /home/star:/home/star \-v /tmp/.X11-unix:/tmp/.X11-unix \-v /dev:/dev \-v /dev/dri:/dev/dri \--env DISPLAYunix:1 \--env ROS_DISTROfoxy \fishros2/ros:foxy-desktop二、算法
Stanley控制使用前轮中心作为参考点根据轨迹上距离参考点最近的点计算偏航误差和横向误差。 首先根据参考点的heading和车的heading计算偏航误差 θ e \theta_e θe。然后考虑横向误差由几何关系可得 tan δ e e d , d v / k \begin{aligned} \tan \delta_e \frac{e}{d}, d v/k \end{aligned} tanδede,dv/k 可得 δ e tan − 1 k e v \delta_e \tan^{-1} \frac{ke}{v} δetan−1vke 此处的增益 k k k根据实验调整。结合上述两个误差项可以得到Stanley控制律 δ ( t ) θ e ( t ) tan − 1 k e ( t ) v ( t ) \delta(t) \theta_e(t) \tan^{-1}\frac{ke(t)}{v(t)} δ(t)θe(t)tan−1v(t)ke(t) 观察上述控制律可以发现当车速 v v v较低时即便是比较小的横向误差 e e e也会引起反正切函数的剧烈变化因此在分母上增加一项常数控制律变为 δ ( t ) θ e ( t ) tan − 1 ( k e ( t ) k s v ( t ) ) \delta(t) \theta_e(t) \tan^{-1}\left(\frac{ke(t)}{k_s v(t)} \right) δ(t)θe(t)tan−1(ksv(t)ke(t)) 当车速较快时如果轨迹的偏航角变化较大直接跟踪会导致车辆横向振荡因此可以在 θ e \theta_e θe中加入阻尼即增加PD控制器。综上所述最终的Stanley控制器如下 δ P D ( θ ) tan − 1 ( k e ( t ) k s v ( t ) ) \delta PD(\theta) \tan^{-1}\left(\frac{ke(t)}{k_s v(t)} \right) δPD(θ)tan−1(ksv(t)ke(t))
三、代码实现
此处使用的PD控制器可以参考上一个project中的实现方法自动驾驶控制与规划——Project 1: 车辆纵向控制。为了避免低速行驶时的横向振荡加入参数 k s k_s ks
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::vectorTrajectoryPoint 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 error2.计算cross tracking error3.利用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 trajectoryTrajectoryPoint nearest_pt QueryNearestPointByPosition(vehicle_state.x, vehicle_state.y);// theta_ref_在QueryNearestPointByPosition中已经更新了// get lateral error and heading errordouble 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-port2000 -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运行效果如下 自动驾驶控制与规划——Project 2: 车辆横向控制
