【Autoware规控】Lattice规划节点

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 parameters
    while(convergence == FALSE && iteration<4)
    {
        // Set time horizon
        double horizon = curvature.s/veh.vdes;
        ROS_INFO_STREAM("vdes: " << veh.vdes);
        ROS_INFO_STREAM("horizon: " << horizon);

        // Run motion model
        veh_next = motionModel(veh, goal, curvature, dt, horizon, 0);
        
        // Determine convergence criteria
        convergence = checkConvergence(veh_next, goal);

        // If the motion model doesn't get us to the goal compute new parameters
        if(convergence==FALSE)
        {
            // Update parameters
            curvature = generateCorrection(veh, veh_next, goal, curvature, dt, horizon);
            iteration++;

            // Escape route for poorly conditioned Jacobian
            if(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 horizon
         double horizon = curvature.s/v_0;
        // Run motion model and log data for plotting
        veh_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:
  // Commands
  ros::Publisher cmd_velocity_publisher = nh.advertise<geometry_msgs::TwistStamped>("twist_raw", 10);

  // Subscribe to the following topics:
  // Curvature parameters and state parameters
  ros::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;
}

以上。