自动驾驶控制与规划——Project 2: 车辆横向控制

目录

  • 零、任务介绍
  • 一、环境配置
  • 二、算法
  • 三、代码实现
  • 四、效果展示

零、任务介绍

  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计算偏航误差 θ 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δe=de,d=v/k
可得
δ e = tan ⁡ − 1 k e v \delta_e = \tan^{-1} \frac{ke}{v} δe=tan1vke
此处的增益 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)+tan1v(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)+tan1(ks+v(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(θ)+tan1(ks+v(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::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 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-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

运行效果如下:

自动驾驶控制与规划——Project 2: 车辆横向控制

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/bicheng/64322.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Qt6开发自签名证书的https代理服务器

目标&#xff1a;制作一个具备类似Fiddler、Burpsuit、Wireshark的https协议代理抓包功能&#xff0c;但是集成到自己的app内&#xff0c;这样无需修改系统代理设置&#xff0c;使用QWebengineview通过自建的代理服务器&#xff0c;即可实现https包的实时监测、注入等自定义功能…

鸿蒙Next合理使用状态管理总结

在使用鸿蒙Next进行开发时&#xff0c;合理的状态管理对于优化UI性能和提升用户体验至关重要。许多开发者由于对状态管理特性了解不足&#xff0c;常遇到UI不刷新或刷新性能差的问题。本文将从合理使用属性、合理使用ForEach/LazyForEach等方面进行总结&#xff0c;帮助开发者掌…

Windows如何安装Php 7.4

一、进入官网&#xff0c;选择其他版本 https://windows.php.net/download/ 二、配置环境变量 将解压后的php 路径在系统环境变量中配置一下 cmd 后输入 php-v

ensp 静态路由配置

A公司有广州总部、重庆分部和深圳分部3个办公地点&#xff0c;各分部与总部之间使用路由器互联。广州、重庆、深圳的路由器分别为R1、R2、R3&#xff0c;为路由器配置静态路由&#xff0c;使所有计算机能够互相访问&#xff0c;实训拓扑图如图所示 绘制拓扑图 给pc机配置ip地址…

红米Note 9 Pro5G刷LineageOS

LineageOS介绍 LineageOS 是一个基于 Android 的开源操作系统&#xff0c;是面向智能手机和平板电脑等设备的替代性操作系统。它是 CyanogenMod 的继承者&#xff0c;而 CyanogenMod 是曾经非常受欢迎的一个第三方 Android 定制 ROM。 在 2016 年&#xff0c;CyanogenMod 项目因…

ECharts实现数据可视化入门详解

文章目录 ECharts实现数据可视化入门详解一、引言二、基础配置1.1、代码示例 三、动态数据与交互2.1、代码示例 四、高级用法1、多图表组合1.1、在同一容器中绘制多个图表1.2、创建多个容器并分别初始化 ECharts 实例1.3、实现多图联动 五、总结 ECharts实现数据可视化入门详解…

盲盒3.0版h5版-可打包app-新优化版紫色版

整体界面ui美观大气&#xff0c;盲盒项目也是一直比较热门的&#xff0c;各大平台一直自己也有做。 感兴趣的小伙伴可以搭建做自己的项目。盲盒项目的利润率还是很大的。

MacbookPro M1 安装Hive

前提注意⚠️⚠️⚠️ 1&#xff09;在安装Hive前确实需要安装MySQL&#xff0c;因为Hive可以使用MySQL作为元数据存储 2&#xff09;在安装Hive之前&#xff0c;需要先安装Hadoop。Hive是一个构建在Hadoop之上的数据仓库软件&#xff0c;它使用Hadoop的HDFS&#xff08;分布…

Crawl4AI:一个为大型语言模型(LLM)和AI应用设计的网页爬虫和数据提取工具实战

这里写目录标题 一、crawl4AI功能及简介1、简介2、特性 二、项目地址三、环境安装四、大模型申请五、代码示例1.生成markdown2.结构化数据 一、crawl4AI功能及简介 1、简介 Crawl4AI 是一个开源的网页爬虫和数据抓取工具&#xff0c;一个python项目&#xff0c;主要为大型语言…

游戏引擎学习第50天

仓库: https://gitee.com/mrxiao_com/2d_game Minkowski 这个算法有点懵逼 回顾 基本上&#xff0c;现在我们所处的阶段是&#xff0c;回顾最初的代码&#xff0c;我们正在讨论我们希望在引擎中实现的所有功能。我们正在做的版本是初步的、粗略的版本&#xff0c;涵盖我们认…

深度解读:Top14金融顶刊

作者Toby&#xff1a;来源&#xff1a;Python风控模型&#xff0c;Top14金融顶刊 各位同学好&#xff0c;我是Toby老师&#xff0c;今天为大家介绍金融风控领域的顶级学术期刊&#xff0c;用于小论文发布平台参考。 金融风控领域内有许多顶级学术期刊&#xff0c;它们发表高质…

【k8s】在ingress-controlller中Admission Webhook 的作用

介绍1 在 NGINX Ingress Controller 中&#xff0c;Admission Webhook 是一种用于增强 Kubernetes API 请求的机制&#xff0c;它允许你在资源&#xff08;如 Ingress&#xff09;被创建或更新之前对这些请求进行验证或修改。具体来说&#xff0c;Admission Webhook 在 NGINX I…

数据库管理-第271期 Oracle 23ai:用MongoDB的方式来操作JSON二元性(20241214)

数据库管理271期 2024-12-14 数据库管理-第271期 Oracle 23ai&#xff1a;用MongoDB的方式来操作JSON二元性&#xff08;20241214&#xff09;1 初始化数据1.1 创建用户1.2 导入数据1.3 创建JSON关系二元性视图 2 创建ORDS服务2.1 下载JDK172.2 安装ORDS2.3 启用MongoDB API2.4…

计网_虚拟局域网VLAN

2024.12.08&#xff1a;计算机网络虚拟局域网VLAN学习笔记 虚拟局域网VLAN VLAN背景&#xff08;认真看&#xff09;VLAN定义&#xff08;最大的好处是隔离广播域&#xff09;VLAN以太网帧格式的扩展划分虚拟局域网VLAN的方式虚拟局域网的优点 VLAN背景&#xff08;认真看&…

深入探索JavaScript网络编程:AJAX与Axios库的完美结合

深入探索JavaScript网络编程&#xff1a;AJAX与Axios库的完美结合 在现代Web开发中&#xff0c;网络编程是不可或缺的一部分。AJAX&#xff08;Asynchronous JavaScript and XML&#xff09;技术使得网页能够在不重新加载整个页面的情况下与服务器进行数据交换&#xff0c;从而…

使用ENSP实现NAT(2)

一、NAT的类型 二、静态NAT 1.项目拓扑 2.项目实现 路由器AR1配置&#xff1a; 进入系统视图 sys将路由器命名为AR1 sysname AR1关闭信息中心 undo info-center enable 进入g0/0/0接口 int g0/0/0将g0/0/0接口IP地址配置为192.168.10.254/24 ip address 192.168.10.254 24进…

lombok常用注解的功能和效果

官网&#xff1a;https://www.projectlombok.org github源码地址&#xff1a;https://github.com/rzwitserloot/lombok Lombok是一个Java库&#xff0c;它能通过注解的方式自动生成一些Java代码&#xff0c;从而减少代码冗余。以下是一些Lombok常用的注解&#xff1a; Data 功…

Oracle PDB的开启和关闭

[生产环境关闭与开启Oracle PDB] 【运维场景】 在运维Oracle PDB的时候经常要开启和关闭PDB&#xff0c;对关闭和开启PDB的操作要非常熟悉。 【操作方法】 1. PDB的打开与关闭 关闭和开启DB的时候要看DB的警告日志&#xff0c;日志位置&#xff08;在Oracle用户下查看&…

【树莓派4B】MindSpore lite 部署demo

一个demo&#xff0c;mindspore lite 部署在树莓派4B ubuntu22.04中&#xff0c;为后续操作开个门&#xff01; 环境 开发环境&#xff1a;wsl-ubuntu22.04分发版部署环境&#xff1a;树莓派4B&#xff0c;操作系统为ubuntu22.04mindspore lite版本&#xff1a;mindspore-li…

LivePortrait 部署笔记

LivePortrait 开源地址&#xff1a; https://github.com/KwaiVGI/LivePortrait 模型下载&#xff1a; export HF_ENDPOINThttps://hf-mirror.com huggingface-cli download --resume-download KwaiVGI/LivePortrait --local-dir models--KwaiVGI--LivePortrait