文章目录

  • 1. Lattice规划介绍
  • 2. 相关代码

1. Lattice规划介绍

Lattice Planner 是一种基于栅格地图的规划算法,通过搜索和优化实现路径规划的目的。Lattice Planner 的核心思想是将路径规划问题转化为一系列离散化的决策问题,通过搜索和优化得到最优路径(多条路径撒点)。

与传统的A*算法不同,Lattice Planner 能够考虑车辆的动力学约束、道路限制和障碍物等因素,生成更加平滑且安全的路径。

在 Autoware 中,Lattice Planner 主要由以下几个模块组成:

lattice_planner_core:实现 Lattice Planner 的核心逻辑,包括路径搜索、代价计算和路径优化等。

lattice_planner_node:将 Lattice Planner 和 ROS 框架相结合,实现与其他模块的数据交互。

lattice_structure:定义了路径的数据结构,包括路径上的点、速度和加速度等信息。

lattice_traj_optimizer:负责对生成的路径进行优化,得到更加平滑和自然的路径。

Lattice主要用于自主停车、避障等功能。

2. 相关代码

Lattice主要有以下节点:lattice_trajectory_gen、lattice_twist_convert、lattice_velocity_set、path_select

waypointTrajectory根据当前位姿、速度、路径点等信息生成预测轨迹:

static union Spline waypointTrajectory(union State veh, union State goal, union Spline curvature, int next_waypoint)
{curvature.success=TRUE;  bool convergence=FALSE;int iteration = 0;union State veh_next;double dt = step_size;veh.v=goal.v;// While loop for computing trajectory parameterswhile(convergence == FALSE && iteration<4){// Set time horizondouble horizon = curvature.s/veh.vdes;ROS_INFO_STREAM("vdes: " << veh.vdes);ROS_INFO_STREAM("horizon: " << horizon);// Run motion modelveh_next = motionModel(veh, goal, curvature, dt, horizon, 0);// Determine convergence criteriaconvergence = checkConvergence(veh_next, goal);// If the motion model doesn't get us to the goal compute new parametersif(convergence==FALSE){// Update parameterscurvature = generateCorrection(veh, veh_next, goal, curvature, dt, horizon);iteration++;// Escape route for poorly conditioned Jacobianif(curvature.success==FALSE){ROS_INFO_STREAM("Init State: sx "<<veh.sx<<" sy " <<veh.sy<<" theta "<<veh.theta<<" kappa "<<veh.kappa<<" v "<<veh.v);ROS_INFO_STREAM("Goal State: sx "<<goal.sx<<" sy " <<goal.sy<<" theta "<<goal.theta<<" kappa "<<goal.kappa<<" v"<<goal.v);break;}}    }if(convergence==FALSE){ROS_INFO_STREAM("Next State: sx "<<veh_next.sx<<" sy " <<veh_next.sy<<" theta "<<veh_next.theta<<" kappa "<<veh_next.kappa<<" v "<<veh_next.v);ROS_INFO_STREAM("Init State: sx "<<veh.sx<<" sy " <<veh.sy<<" theta "<<veh.theta<<" kappa "<<veh.kappa);ROS_INFO_STREAM("Goal State: sx "<<goal.sx<<" sy " <<goal.sy<<" theta "<<goal.theta<<" kappa "<<goal.kappa);curvature.success= FALSE;}else{ROS_INFO_STREAM("Converged in "<<iteration<<" iterations");#ifdef LOG_OUTPUT// Set time horizondouble horizon = curvature.s/v_0;// Run motion model and log data for plottingveh_next = motionModel(veh, goal, curvature, 0.1, horizon, 1);fmm_sx<<"0.0 \n";fmm_sy<<"0.0 \n";#endif}return curvature;
}

lattice_twist_convert订阅车辆状态和规划输出,并发布到twist_cmd命令:

  // Publish the following topics:// Commandsros::Publisher cmd_velocity_publisher = nh.advertise<geometry_msgs::TwistStamped>("twist_raw", 10);// Subscribe to the following topics:// Curvature parameters and state parametersros::Subscriber spline_parameters = nh.subscribe("spline", 1, splineCallback);ros::Subscriber state_parameters = nh.subscribe("state", 1, stateCallback);

lattice_velocity_set可根据不同的路段和情况设置不同的车辆速度。

path_select用来实现换道:

#include <ros/ros.h>
#include "autoware_msgs/Lane.h"
#include <iostream>static ros::Publisher _pub;void callback(const autoware_msgs::Lane &msg)
{_pub.publish(msg);
}int main(int argc, char **argv)
{ros::init(argc, argv, "path_select");ros::NodeHandle nh;ros::Subscriber twist_sub = nh.subscribe("temporal_waypoints", 1, callback);_pub = nh.advertise<autoware_msgs::Lane>("final_waypoints", 1000,true);ros::spin();return 0;
}

以上。

【Autoware规控】Lattice规划节点相关推荐

  1. 【Autoware规控】mpc_follower模型预测控制节点

    文章目录 1. 技术原理 2. 代码实现 1. 技术原理 MPC,即Model Predictive Control(模型预测控制),是一种基于动态模型的控制算法.MPC算法通过建立系统的数学模型,根 ...

  2. 【Autoware规控】PurePursuit纯跟踪控制节点

    PurePursuit轨迹跟踪控制. 文章目录 1. 技术原理 2. 代码实现 3. 算法改进 1. 技术原理 轨迹跟踪模块主要负责控制车辆沿着规划的路径点行驶,即根据车辆当前的速度.位姿及路径点信息 ...

  3. 总规、控规、修规、概念性规划之间的关系

    规划包含:总规.控规.修规.概念性规划 总规.控规.修规.概念性规划之间的关系 各规划关系图: 各种类型的规划: 国民经济和社会发展规划(五年规划): 是全国或者某一地区经济.社会发展的总体纲要,是具 ...

  4. “狂飙”的自动驾驶车,用了哪些规控算法?

    视频中这辆"狂飙"的自动驾驶车,正通过MPC轨迹跟踪控制器使其沿着规划好的轨迹行驶.作者用C++实现了MPC算法,并利用ROS与自动驾驶仿真器CARLA进行联合仿真,测试了算法的有 ...

  5. 万字长文详解特斯拉自动驾驶体系(感知/规控/标注/仿真)

    作者 | 和君 编辑 | 禾隐记 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取 点击进入→自动驾驶之心[全栈算法]技术交流群 汽车革命的上半场是电动化,下半 ...

  6. 上海内推 | 极氪智能科技百万年薪「氪学家」项目招聘规控/感知算法工程师

    合适的工作难找?最新的招聘信息也不知道? AI 求职为大家精选人工智能领域最新鲜的招聘信息,助你先人一步投递,快人一步入职! 极氪智能科技 「氪学家」是极氪智能科技倾斜极氪战略规划与资源,用百万年薪吸 ...

  7. 万字长文解读深度学习算法在自动驾驶规控中的应用

    交流群 | 进"传感器群/滑板底盘群"请加微信号:xsh041388 交流群 | 进"汽车基础软件群"请加微信号:ckc1087 备注信息:群名称 + 真实姓名 ...

  8. 北京内推 | 华为诺亚方舟实验室招聘自动驾驶预测/规控/仿真算法研究员

    合适的工作难找?最新的招聘信息也不知道? AI 求职为大家精选人工智能领域最新鲜的招聘信息,助你先人一步投递,快人一步入职! 华为 华为诺亚方舟实验室(Noah's Ark Lab)是华为的 AI 能 ...

  9. 北京内推 | 华为诺亚方舟实验室招聘自动驾驶预测/规控/仿真实习生

    合适的工作难找?最新的招聘信息也不知道? AI 求职为大家精选人工智能领域最新鲜的招聘信息,助你先人一步投递,快人一步入职! 华为 华为诺亚方舟实验室(Noah's Ark Lab)是华为的 AI 能 ...

最新文章

  1. 字符串的UPDATE和REPLACE部分
  2. 并不对劲的BJOI2019
  3. MYSQL--事务隔离
  4. Visual C++ 基础数据类型的转换
  5. 省选+NOI 第十部分 概率统计与多项式
  6. 前n个正整数相乘的时间复杂度为_初一数学常考的21个知识点,掌握好,轻松110+!...
  7. 直通车测图需要满足哪些数据?
  8. 概率论与数理统计基础概念整理
  9. 计算机毕业设计springboot+vue基本微信小程序的考试系统
  10. jquery表格日历写入html代码,基于jQuery日历插件制作日历
  11. linux内top命令,Linux中的top命令的详细解释
  12. 2021-2022-2 ACM集训队每周程序设计竞赛(1) - 问题 B: 蹩脚两轮车 - 题解
  13. 一维参照物的摄像机定标
  14. 银行刷题记录(招商银行信用卡中心)
  15. 梁权财到徐闻生态工业集聚区调研
  16. 腾讯大数据Hermes爱马仕的系统
  17. 如何获取浏览器的user-agent
  18. Vue | 显示切换(v-if与v-show,display,visibility与opacity)
  19. CSS 盒子模型(border、padding、margin)
  20. Matlab底层算法实现图像反色变换

热门文章

  1. background: #ff0000 url(/i/eg_bg_03.gif) ; 图片 + 颜色
  2. 【Vue3】源码解析
  3. Python 调用Windows内置的语音合成,并生成wav文件
  4. fi sap 凭证冲销 稅_凭证冲销FB08
  5. Zebra 开源框架设计
  6. 杭电CTF 密码学(1)
  7. 【转】人际关系的书,我只推荐这三本,其它的根本就不用看了
  8. python读取docx文件_Python读写docx文件的方法
  9. 神经网络的三种训练方法,三种常见的神经网络
  10. Ubuntu下代替dos2unix命令 (转自两个文件)