autoware lidar_localizer包下的ndt_matching节点的学习

最新推荐文章于 2025-06-27 16:35:16 发布

JZJHfc

最新推荐文章于 2025-06-27 16:35:16 发布

阅读量4.6k

点赞数 5

CC 4.0 BY-SA版权

文章标签：定位

本文链接：https://blog.youkuaiyun.com/JZJHfc/article/details/105025328

autoware lidar_localizer包下的ndt_matching节点的学习

激光雷达定位

自身定位对于自动驾驶汽车来说非常重要，常见的机器人定位方法有视觉slam方法(即时定位与地图构建)，根据激光雷达实时扫描到的点云图构建地图，然后进行定位的方法。但是在自动驾驶具有高速移动，高安全性的要求下，对该方法的延时要求较高。而另一种定位方法位ndt(正态分布变换)地图匹配方法，则是在已有的高精度地图的基础上，将扫描到的点云地图进行高维变换，找出匹配度最高的转换关系，得到定位位置。autoware采用的是ndt地图匹配定位算法，其中能够选择是否需要gnss进行协同定位。

ndt算法:

先根据参考数据（reference scan）来构建多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。

将参考点云（reference scan）所占的空间划分成体素，并计算每个网格的多维正态分布参数
将点云投票到各个格子
计算格子的正态分布PDF参数
将第二幅scan的每个点按转移矩阵T的变换
第二幅scan的点落于reference的哪个格子，计算响应的概率分布函数
求所有点的最优值

ndt_matching

lidar_localizer节点的launch文件如图所示，其中主要查看ndt_matching.launch文件，该文件，描述了启动节点，以及相应的参数配置。
在这里插入图片描述
launch文件主要启动lidar_localizer功能包下的ndt_matching节点文件。参数设置主要包括method_type，use_gnss，use_odom，use_imu，imu_upside_down等。

<launch>
  <arg name="method_type" default="0" /> <!-- pcl_generic=0, pcl_anh=1, pcl_anh_gpu=2, pcl_openmp=3 -->
  <arg name="use_gnss" default="1" />
  <arg name="use_odom" default="false" />
  <arg name="use_imu" default="false" />
  <arg name="imu_upside_down" default="false" />
  <arg name="imu_topic" default="/imu_raw" />
  <arg name="queue_size" default="1" />
  <arg name="offset" default="linear" />
  <arg name="get_height" default="false" />
  <arg name="use_local_transform" default="false" />
  <arg name="sync" default="false" />
  <arg name="output_log_data" default="false" />

  <node pkg="lidar_localizer" type="ndt_matching" name="ndt_matching" output="log">
    <param name="method_type" value="$(arg method_type)" />
    <param name="use_gnss" value="$(arg use_gnss)" />
    <param name="use_odom" value="$(arg use_odom)" />
    <param name="use_imu" value="$(arg use_imu)" />
    <param name="imu_upside_down" value="$(arg imu_upside_down)" />
    <param name="imu_topic" value="$(arg imu_topic)" />
    <param name="queue_size" value="$(arg queue_size)" />
    <param name="offset" value="$(arg offset)" />
    <param name="get_height" value="$(arg get_height)" />
    <param name="use_local_transform" value="$(arg use_local_transform)" />
    <param name="output_log_data" value="$(arg output_log_data)" />
    <remap from="/points_raw" to="/sync_drivers/points_raw" if="$(arg sync)" />
  </node>
</launch>

主要分析ndt_matching.cpp节点。首先看main函数，初始化并命名节点ndt_matching，然后初始化线程互斥锁，为后续多线程操作做准备。接着初始化共有句柄nh，以及私有句柄private_nh。

int main(int argc, char** argv)
{
   
   
  ros::init(argc, argv, "ndt_matching");
  pthread_mutex_init(&mutex, NULL);

  ros::NodeHandle nh;
  ros::NodeHandle private_nh("~");
  node_status_publisher_ptr_ = std::make_shared<autoware_health_checker::NodeStatusPublisher>(nh,private_nh);//声明节点状态的智能指针
  node_status_publisher_ptr_->ENABLE();
  node_status_publisher_ptr_->NODE_ACTIVATE();

然后设置了日志文件的名称

private_nh.getParam("output_log_data", _output_log_data);
  if(_output_log_data)
  {
   
   
    char buffer[80];
    std::time_t now = std::time(NULL);
    std::tm* pnow = std::localtime(&now);
    std::strftime(buffer, 80, "%Y%m%d_%H%M%S", pnow);
    std::string directory_name = "/tmp/Autoware/log/ndt_matching";
    filename = directory_name + "/" + std::string(buffer) + ".csv";
    boost::filesystem::create_directories(boost::filesystem::path(directory_name));
    ofs.open(filename.c_str(), std::ios::app);
  }

tl_btol表示点之间的平移关系，rot_x_btol描述的是描述关于x轴的旋转关系，_tf_roll为绕x轴的旋转角度，rot_y_btol，rot_z_btol同样表示y轴预z轴的旋转角度。tf_btol为位姿变换关系静态变换矩阵，由坐标*旋转来表示。

  Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation 平移关系
  Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation 描述关于x轴的旋转关系，_tf_roll为绕x轴的旋转角度
  Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
  Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
  tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();

发布车辆，惯导，例程及ndt预测位置。

predict_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose", 10);//发布预测位资的消息
  predict_pose_imu_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu", 10);
  predict_pose_odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_odom", 10);
  predict_pose_imu_odom_pub = nh.advertise<geometry_msgs::PoseStamped>("/predict_pose_imu_odom", 10);
  ndt_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/ndt_pose", 10);

通过回调函数订阅参数，初始位置，过滤点集等消息。

  ros::Subscriber param_sub = nh.subscribe("config/ndt", 10, param_callback);
  ros::Subscriber gnss_sub = nh.subscribe("gnss_pose", 10, gnss_callback);
  //  ros::Subscriber map_sub = nh.subscribe("points_map", 1, map_callback);
  ros::Subscriber initialpose_sub = nh.subscribe("initialpose", 10, initialpose_callback);
  ros::Subscriber points_sub = nh.subscribe("filtered_points", _queue_size, points_callback);
  ros::Subscriber odom_sub = nh.subscribe("/vehicle/odom", _queue_size * 10, odom_callback);
  ros::Subscriber imu_sub = nh.subscribe(_imu_topic.c_str(), _queue_size * 10, imu_callback);

然后看convertPoseIntoRelativeCoordinate这个函数，它返回的是位置，主要是将位置坐标转换到参考坐标系。首先声明存储目标姿态的四元数。四元数是ros里用来表示3D刚体物体姿态。无人汽车在空间中的位资主要有自身的坐标位置(x,y,z)以及绕着参考坐标系坐标轴的旋转角度。通常描述旋转量有欧拉角，还有四元数，而欧拉角表示的是刚体绕着坐标轴的旋转角度，欧拉角的优点具有直观性，但是采用欧拉角会使得描述刚体处于万向锁的情况，所以欧拉角适用于验证。而四元数类似于复数，其具有一个实部和三个虚部，四元数可以表示空间中任意旋转。

通过调用setRPY函数能够按照固定轴xyz来设置旋转角度，并且存储表示目标位置的向量，然后作为参数传入表示目标位资的target_tf对象的构造函数。

static pose convertPoseIntoRelativeCoordinate(const pose &target_pose, const pose &reference_pose)//将位置转换到相对坐标系
{
   
   
    tf::Quaternion target_q;//目标的四元数
    target_q.setRPY(target_pose.roll, target_pose.pitch, target_pose.yaw);//按照固定轴来设置旋转角度xyz轴下
    tf::Vector3 target_v(target_pose.x, target_pose.y, target_pose.z);//目标位置向量
    tf::Transform target_tf(target_q, target_v);//声明存放目标位资转换信息(平移+旋转)的对象，并初始化构造函数。

    tf::Quaternion reference_q;//参照四元数
    reference_q.setRPY(reference_pose.roll, reference_pose.pitch, reference_pose.yaw);//设置xyz绕轴转动角度
    tf::Vector3 reference_v(reference_pose.x, reference_pose.y, reference_pose.z);//设置参考位置向量
    tf::Transform reference_tf(reference_q, reference_v);

    tf::Transform trans_target_tf = reference_tf.inverse() * target_tf;//inverse返回变换的逆矩阵如

    pose trans_target_pose;//训练目标的坐标
    trans_target_pose.x = trans_target_tf.getOrigin().getX();//返回x值
    trans_target_pose.y = trans_target_tf.getOrigin().getY();
    trans_target_pose.z = trans_target_tf.getOrigin().getZ();
    tf::Matrix3x3 tmp_m(trans_target_tf.getRotation());
    tmp_m.getRPY(trans_target_pose.roll, trans_target_pose.pitch, trans_target_pose.yaw);

    return trans_target_pose;
}//返回转换的目标位置

参数订阅者param_sub通过config/ndt话题，当接收到配置信息的时候调用回调函数param_callback来进行参数配置，其中主要配置的有初始化位置。以及是否使用gnss时参数配置有所不同。


 static void param_callback(const autoware_config_msgs::ConfigNDT::ConstPtr& input)//参数回调函数完成参数的初始化
{
   
   
  if (_use_gnss != input->init_pos_gnss)
  {
   
   
    init_pos_set = 0;
  }
  else if (_use_gnss == 0 &&
           (initial_pose.x != input->x || initial_pose.y != input->y || initial_pose.z != input->z ||
            initial_pose.roll != input->roll || initial_pose.pitch != input->pitch || initial_pose.yaw != input->yaw))
  {
   
   
    init_pos_set = 0;
  }

  _use_gnss = input->init_pos_gnss;

  // Setting parameters

位姿初始化以及速度的初始化。

 localizer_pose.x = initial_pose.x;
    localizer_pose.y = initial_pose.y;
    localizer_pose.z = initial_pose.z;
    localizer_pose.roll = initial_pose.roll;
    localizer_pose.pitch = initial_pose.pitch;
    localizer_pose.yaw = initial_pose.yaw;

    previous_pose.x = initial_pose.x;
    previous_pose.y = initial_pose.y;
    previous_pose.z = initial_pose.z;
    previous_pose.roll = initial_pose.roll;
    previous_pose.pitch = initial_pose.pitch;
    previous_pose.yaw = initial_pose.yaw;

    current_pose.x = initial_pose.x;
    current_pose.y = initial_pose.y;
    current_pose.z = initial_pose.z;
    current_pose.roll = initial_pose.roll;
    current_pose.pitch = initial_pose.pitch;
    current_pose.yaw = initial_pose.yaw;

    current_velocity = 0;
    current_velocity_x = 0;
    current_velocity_y = 0;
    current_velocity_z = 0;
    angular_velocity = 0;

    current_pose_imu.x = 0;
    current_pose_imu.y = 0;
    current_pose_imu.z = 0;
    current_pose_imu.roll = 0