【Autoware规控】Lattice规划节点
文章目录
- 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规划节点相关推荐
- 【Autoware规控】mpc_follower模型预测控制节点
文章目录 1. 技术原理 2. 代码实现 1. 技术原理 MPC,即Model Predictive Control(模型预测控制),是一种基于动态模型的控制算法.MPC算法通过建立系统的数学模型,根 ...
- 【Autoware规控】PurePursuit纯跟踪控制节点
PurePursuit轨迹跟踪控制. 文章目录 1. 技术原理 2. 代码实现 3. 算法改进 1. 技术原理 轨迹跟踪模块主要负责控制车辆沿着规划的路径点行驶,即根据车辆当前的速度.位姿及路径点信息 ...
- 总规、控规、修规、概念性规划之间的关系
规划包含:总规.控规.修规.概念性规划 总规.控规.修规.概念性规划之间的关系 各规划关系图: 各种类型的规划: 国民经济和社会发展规划(五年规划): 是全国或者某一地区经济.社会发展的总体纲要,是具 ...
- “狂飙”的自动驾驶车,用了哪些规控算法?
视频中这辆"狂飙"的自动驾驶车,正通过MPC轨迹跟踪控制器使其沿着规划好的轨迹行驶.作者用C++实现了MPC算法,并利用ROS与自动驾驶仿真器CARLA进行联合仿真,测试了算法的有 ...
- 万字长文详解特斯拉自动驾驶体系(感知/规控/标注/仿真)
作者 | 和君 编辑 | 禾隐记 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取 点击进入→自动驾驶之心[全栈算法]技术交流群 汽车革命的上半场是电动化,下半 ...
- 上海内推 | 极氪智能科技百万年薪「氪学家」项目招聘规控/感知算法工程师
合适的工作难找?最新的招聘信息也不知道? AI 求职为大家精选人工智能领域最新鲜的招聘信息,助你先人一步投递,快人一步入职! 极氪智能科技 「氪学家」是极氪智能科技倾斜极氪战略规划与资源,用百万年薪吸 ...
- 万字长文解读深度学习算法在自动驾驶规控中的应用
交流群 | 进"传感器群/滑板底盘群"请加微信号:xsh041388 交流群 | 进"汽车基础软件群"请加微信号:ckc1087 备注信息:群名称 + 真实姓名 ...
- 北京内推 | 华为诺亚方舟实验室招聘自动驾驶预测/规控/仿真算法研究员
合适的工作难找?最新的招聘信息也不知道? AI 求职为大家精选人工智能领域最新鲜的招聘信息,助你先人一步投递,快人一步入职! 华为 华为诺亚方舟实验室(Noah's Ark Lab)是华为的 AI 能 ...
- 北京内推 | 华为诺亚方舟实验室招聘自动驾驶预测/规控/仿真实习生
合适的工作难找?最新的招聘信息也不知道? AI 求职为大家精选人工智能领域最新鲜的招聘信息,助你先人一步投递,快人一步入职! 华为 华为诺亚方舟实验室(Noah's Ark Lab)是华为的 AI 能 ...
最新文章
- SpringBoot02_构建rest工程完成第一个controller类
- 几个预编译指令的用法
- java实现c语言的函数_C语言实现返回字符串函数的四种方法
- 【转】SVN各种颜色状态
- C#LeetCode刷题,走进Google,走近人生
- 1. 根据输出的数据,对各个阶维度的反推+2.tf中生成根据指定的shape,tensor的各个阶的维度判断
- 去哪儿-08-city-search
- php发送163邮件554,PHPMailer配置163邮箱发送邮件
- list工具类 ListUtils.java
- LeetCode 127. 单词接龙(广度优先遍历)
- 网站跨站点脚本,Sql注入等攻击的处理
- 深入浅出统计学 笔记 总结 学习心得
- 苹果CEO库克亚洲之行总结 未来挑战远大于机遇
- H5 网络版坦克大战
- Linux的开源操作系统
- 6、淘宝双11数据分析与预测
- 关于用transmac黑苹果制作引导盘无法识别
- 阿里IOT用AMQP在服务端订阅消息,踩坑
- python 实现雪容融
- 2023寒假模拟赛1题解