lattice planner在LGSVL中的仿真实践

本文详细介绍了基于深蓝学院规划与控制课程的项目代码中的路径规划节点(PathPlanningNode)。主要内容包括main函数对PathPlanningNode的初始化过程,初始化函数的具体实现细节如参数获取、路网文件加载、控制器初始化等,以及关键函数loadRoadmap和GetWayPoints的功能。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >


本项目主要基于深蓝学院规划与控制课程的项目代码,在此基础上做出了一些改进,此帖用于对项目代码做一些梳理。

一 项目代码组成

在这里插入图片描述

一. main函数

主要对PathPlanningNode进行初始化

#include <iostream>
#include "path_planning_node.h"
int main(int argc, char** argv) {
  ros::init(argc, argv, "path_planning");
  PathPlanningNode planning_node;
  ROS_INFO("Start Demo!");
  if (!planning_node.init()) {
    std::cout << "fail to init planning node" << std::endl;
    return -1;
  }
  ros::spin();
  return 0;
}

二. pathplanningNode初始化函数

bool PathPlanningNode::init() {
  std::string vehicle_odom_topic;
  std::string vehicle_cmd_topic;
  std::string roadmap_path;
  std::string path_vis_topic;
  std::string frame_id;
  std::string imu_topic;
  std::string local_traj_topic = "local_traj";
  double speed_P, speed_I, speed_D, target_speed, vis_frequency;
  pnh_.getParam("vehicle_odom_topic",
                vehicle_odom_topic);  //读取车辆定位的topic名
  pnh_.getParam("vehicle_cmd_topic",
                vehicle_cmd_topic);             //读取车辆控制的topic名
  pnh_.getParam("roadmap_path", roadmap_path);  //读取路网文件名
  pnh_.getParam("map_path", map_path_);
  pnh_.getParam("path_vis_topic", path_vis_topic);  //读取可视化路网名
  pnh_.getParam("target_speed", target_speed);      //读取目标速度
  pnh_.getParam("goal_tolerance", goalTolerance_);  //读取到终点的容忍距离
  pnh_.getParam("speed_P", speed_P);                //读取PID参数
  pnh_.getParam("speed_I", speed_I);
  pnh_.getParam("speed_D", speed_D);
  pnh_.getParam("control_frequency", controlFrequency_);  //读取控制的频率
  pnh_.getParam("vis_frequency", vis_frequency);  //读取路网显示的频率
  pnh_.getParam("frame_id", frame_id);            //读取全局坐标系名
  pnh_.getParam("c_speed", c_speed_);  //读取Frenet规划器,初始目标速度
  pnh_.getParam("c_d", c_d_);  //读取Frenet规划器,初始横向偏差
  pnh_.getParam("c_d_d", c_d_d_);  //读取Frenet规划器,初始横向速度偏差
  pnh_.getParam("c_d_dd", c_d_dd_);  //读取Frenet规划器,初始横向加速度偏差
  pnh_.getParam("s0", s0_);  //读取Frenet规划器,初始纵向距离
  pnh_.getParam("imu_topic", imu_topic);

  frame_id_ = frame_id;

  //加载路网文件
  if (!loadRoadmap(roadmap_path, target_speed)) return false;
  GetWayPoints();  //在参考路径的基础上进行踩点
  speedPidControllerPtr_ = std::shared_ptr<PIDController>(
      new PIDController(speed_P, speed_I, speed_D));
  lqrController_ = std::shared_ptr<LqrController>(new LqrController());
  lqrController_->LoadControlConf();
  lqrController_->Init();
  // 可视化
  visualization_ = VisualizationPtr(new Visualization(nh_));

  // 加载参考线
  if (use_reference_line_) {
    LoadReferenceLine();
  }

  // 构建相对平滑的Frenet曲线坐标系,一个中间暂时方案
  csp_obj_ = new Spline2D(wx_, wy_);

  // 全局路径可视化
  PlotGlobalPath();

  //  Update Obstacle
  // 添加虚拟障碍物
  UpdateStaticObstacle();

  roadmapMarkerPtr_ =
      std::shared_ptr<RosVizTools>(new RosVizTools(nh_, path_vis_topic));

  localTrajectoryMarkerPtr_ =
      std::shared_ptr<RosVizTools>(new RosVizTools(nh_, local_traj_topic));

  VehiclePoseSub_ = nh_.subscribe(vehicle_odom_topic, 10,
                                  &PathPlanningNode::odomCallback, this);
  ImuSub_ = nh_.subscribe(imu_topic, 10, &PathPlanningNode::IMUCallback, this);
  controlPub_ =
      nh_.advertise<lgsvl_msgs::VehicleControlData>(vehicle_cmd_topic, 1000);

  visTimer_ = nh_.createTimer(ros::Duration(1 / vis_frequency),
                              &PathPlanningNode::visTimerLoop,
                              this);  //注册可视化线程

  plannerTimer_ = nh_.createTimer(ros::Duration(1 / plannerFrequency_),
                                  &PathPlanningNode::plannerTimerLoop,
                                  this);  //注册规划线程

  controlTimer_ = nh_.createTimer(ros::Duration(1 / controlFrequency_),
                                  &PathPlanningNode::controlTimerLoop,
                                  this);  //注册控制线程

  addRoadmapMarker(planningPublishedTrajectory_.trajectory_points, frame_id);

  goalPoint_ =
      planningPublishedTrajectory_.trajectory_points.back();  //确定目标点

  ROS_INFO("planner node and lqr_control_node init finish!");
  return true;
}
2.1 loadRoadmap函数
bool PathPlanningNode::loadRoadmap(const std::string &roadmap_path,const double target_speed)

主要的作用是将参考线数据读入,并根据参考线中的x,y坐标计算出每个路径点的航向角,相对于路径起点的累计历程s,路径中每个离散点的曲率,曲率一阶导。

2.1.1 ComputePathProfile函数
bool ReferenceLine::ComputePathProfile(std::vector<double>* headings,std::vector<double>* accumulated_s,std::vector<double>* kappas,std::vector<double>* dkappas)

这个函数对曲率的计算可以参考下面链接里的公式:链接: link

2.2 GetWayPoints()函数
GetWayPoints()

这个函数主要从之前的参考线离散点中每隔10米挑出一个路点,不知道这个有啥用。

### Lattice Planner in Robotics or Path Planning Technology Lattice planners are widely used within the field of robotics, especially for vehicles that require precise control over both longitudinal and lateral movements. A lattice planner generates a set of feasible trajectories by precomputing a library of maneuvers based on kinematic models of the vehicle[^1]. These maneuvers can then be combined to form complex paths while ensuring dynamic feasibility. The core idea behind lattice-based methods is to discretize the continuous space into discrete segments called lattices. Each node represents a state (position, orientation), and edges represent transitions between states through predefined motion primitives. This approach allows efficient search algorithms like Dijkstra's algorithm or A* to find optimal solutions quickly without violating physical constraints imposed by the robotic system[^2]. For implementation purposes, consider an example where one might want to implement such a planner: ```cpp // Define Motion Primitive Structure struct MotionPrimitive { double dx; // Change in X position double dy; // Change in Y position double dtheta; // Change in heading angle }; std::vector<MotionPrimitive> CreateMotionPrimitives() { std::vector<MotionPrimitive> motions; // Add some basic forward/backward moves here... motions.push_back({0.5, 0.0, 0}); // Move Forward motions.push_back({-0.5, 0.0, 0}); // Move Backward // Add turning actions... motions.push_back({0.25, 0.25, M_PI/8});// Turn Left slightly moving forward motions.push_back({0.25,-0.25,-M_PI/8});// Turn Right slightly moving forward return motions; } ``` This code snippet demonstrates how simple motion primitives could look when implemented as part of a larger lattice planning framework. The actual complexity would depend heavily upon specific application requirements including but not limited to environmental conditions, sensor accuracy, computational resources available etc. In practice, integrating these components requires careful consideration of several factors: - **Vehicle Dynamics**: Accurate modeling ensures generated plans respect real-world limitations. - **Environment Representation**: Efficient data structures facilitate rapid updates during operation. - **Search Algorithm Selection**: Balancing optimality against computation time remains crucial.
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值