【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;
}

以上。