既知の点群マップに基づく NDT のレーザー SLAM 位置決め

概要

L4 レベルの自動運転システムの場合、測位モジュールは通常、GNSS、IMU、車輪速度計 (車両シャーシ)、カメラおよび LIDAR オドメトリ測定を統合し、フィルタリング アルゴリズム (EKF、UKF など) を使用して、スムーズなセンチメートルレベルの絶対測位を取得します。中でも、高精度点群地図とライダーに基づく登録測位（ライダーオドメトリ）は、その精度が高く信頼性が高いため、通常、融合測位全体の中で大きな比重を占めており、自動運転測位の比較的重要な部分となっている。システム「絶対位置」データの信頼できるソース。

このデモは、インターネット上のオープンソース アルゴリズム フレームワークと専門家の経験共有に基づいて、ROS システムの下でマッピング アルゴリズム (SC-LEGO-LOAM) + 点群マッチング アルゴリズム (NDT) に基づく測位機能を実装します。

点群マップ

自動運転車の LiDAR 測位は、通常、事前にオフラインで構築された高精度の点群マップに依存します。その理由は次のとおりです。

• レベル4以上の自動運転システムでは位置精度と安定性に対する要求が非常に高く、絶対誤差を20cm以内に抑える必要がある。
• Pure SLAM は現時点では自動運転の測位精度と信頼性の要件を満たすことができず、つまり、現在の研究では自動運転車のオンライン マッピングと測位を実現することは困難です (問題には、閉ループ最適化、グローバル最適化、誤差累積補正などが含まれます)。 ）
• 高精度地図メーカーの完全な生産プロセスには、大量の計算能力と労働力が必要であり、理想的な点群地図や意味地図を生成することはできますが、オフラインでの生産 (時間と人員) が必要です。
• ライダー測位は、高精度地図を使用して比較的簡単に実現でき、IMU と車輪速計を統合したこの測位方法の精度と信頼性は、基本的に自動車両測位のニーズを満たします。

したがって、上記の客観的な理由に基づいて、現在の L4 およびほとんどの L3 自動運転システム測位モジュールは依然として事前に構築された高精度マップに基づいており、この登録に使用されるセンサーはカメラ (Mobileeye など) であり、ほとんどのメーカーは依然として使用していますLIDAR 登録のアイデア。

点群マップとは、LIDAR登録のためにあらかじめ構築される「測位用のマップ」です。

点群マップを構築するために、それぞれ LEGO-LOAM アルゴリズムと SC-LEGO-LOAM アルゴリズムを使用しました。より大きな領域を持つ点群マップの場合は、明らかにスキャン コンテキスト メソッドを使用して、点群マップ上でループ クロージャ検出とループ クロージャ効果を実行します。姿勢マップの最適化後 より良いため、ここでは、点群のマッチングと位置決めの次のステップとして、SC-LEGO-LOAM によって構築された点群マップを使用します。

構築されたマップをマップ フォルダーに配置します。プロジェクト内の map_loader ノードは、主にマップをロードするために使用されます。

MapLoader::MapLoader(ros::NodeHandle &nh){
    
    
    std::string pcd_file_path, map_topic;
    nh.param<std::string>("pcd_path", pcd_file_path, "");
    nh.param<std::string>("map_topic", map_topic, "point_map");

    init_tf_params(nh);

    pc_map_pub_ = nh.advertise<sensor_msgs::PointCloud2>(map_topic, 10, true);

    file_list_.push_back(pcd_file_path);

    auto pc_msg = CreatePcd();
    
    auto out_msg = TransformMap(pc_msg);

    if (out_msg.width != 0) {
    
    
		out_msg.header.frame_id = "map";
		pc_map_pub_.publish(out_msg);
	}

}

コンストラクタでは、pcdファイルのパスとマップトピックを読み込み、マップの変換パラメータを初期化します（マップを変換する必要がない場合、値は0に設定されます）

void MapLoader::init_tf_params(ros::NodeHandle &nh){
    
    
    nh.param<float>("x", tf_x_, 0.0);
    nh.param<float>("y", tf_y_, 0.0);
    nh.param<float>("z", tf_z_, 0.0);
    nh.param<float>("roll", tf_roll_, 0.0);
    nh.param<float>("pitch", tf_pitch_, 0.0);
    nh.param<float>("yaw", tf_yaw_, 0.0);
    ROS_INFO_STREAM("x" << tf_x_ <<"y: "<<tf_y_<<"z: "<<tf_z_<<"roll: "
                        <<tf_roll_<<" pitch: "<< tf_pitch_<<"yaw: "<<tf_yaw_);
}

関数 CreatePcd() は pcd をロードするために使用され、TransformMap() はマップの移動と回転に使用され、Eigen と pcl::transformPointCloud() を使用して点群変換を実装します。

sensor_msgs::PointCloud2 MapLoader::TransformMap(sensor_msgs::PointCloud2 & in){
    
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(in, *in_pc);

    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_pc_ptr(new pcl::PointCloud<pcl::PointXYZ>);

    Eigen::Translation3f tl_m2w(tf_x_, tf_y_, tf_z_);                 // tl: translation
    Eigen::AngleAxisf rot_x_m2w(tf_roll_, Eigen::Vector3f::UnitX());  // rot: rotation
    Eigen::AngleAxisf rot_y_m2w(tf_pitch_, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rot_z_m2w(tf_yaw_, Eigen::Vector3f::UnitZ());
    Eigen::Matrix4f tf_m2w = (tl_m2w * rot_z_m2w * rot_y_m2w * rot_x_m2w).matrix();

    pcl::transformPointCloud(*in_pc, *transformed_pc_ptr, tf_m2w);

    SaveMap(transformed_pc_ptr);
    
    sensor_msgs::PointCloud2 output_msg;
    pcl::toROSMsg(*transformed_pc_ptr, output_msg);
    return output_msg;
}

入力点群のダウンサンプリング

NDT アルゴリズムによって最適化される目的関数は、主に入力点群とターゲット点群の確率分布の類似性です。この登録アルゴリズムの計算量は、次の 2 つの要因と正の相関があります。

• 入力点群内の点の密度
• 初期姿勢推定におけるバイアス

入力点群ポイントの密度が高いほど、NDT レジストレーションに必要な計算の複雑さは大きくなり、初期姿勢推定が悪くなる
(真の姿勢から離れるほど) ため、対応する計算の複雑さも大きくなります。初期姿勢が低すぎる場合は、 NDTも収束できないかもしれない。
自動運転 LiDAR 測位にはリアルタイム性に対する要求が高く、点群の登録にかかる時間は短いほど良いため、入力点群をダウンサンプリングして NDT 登録の速度を向上させることができます。この記事では VoxelGrid を使用します入力点群をダウンサンプリングします。このサンプリング方法により、入力点群の密度が減少します。コードはプロジェクトの voxel_grid_filter.cpp にあります。メイン コードは次のとおりです:

static void scan_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
    
    
  pcl::PointCloud<pcl::PointXYZ> scan;
  pcl::fromROSMsg(*input, scan);

  if(measurement_range != MAX_MEASUREMENT_RANGE){
    
    
    scan = removePointsByRange(scan, 0, measurement_range);
  }

  pcl::PointCloud<pcl::PointXYZ>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZ>(scan));
  pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZ>());

  sensor_msgs::PointCloud2 filtered_msg;

  // if voxel_leaf_size < 0.1 voxel_grid_filter cannot down sample (It is specification in PCL)
  if (voxel_leaf_size >= 0.1)
  {
    
    
    // Downsampling the velodyne scan using VoxelGrid filter
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_filter;
    voxel_grid_filter.setLeafSize(voxel_leaf_size, voxel_leaf_size, voxel_leaf_size);
    voxel_grid_filter.setInputCloud(scan_ptr);
    voxel_grid_filter.filter(*filtered_scan_ptr);
    pcl::toROSMsg(*filtered_scan_ptr, filtered_msg);
  }
  else
  {
    
    
    pcl::toROSMsg(*scan_ptr, filtered_msg);
  }

  filtered_msg.header = input->header;
  std::cout <<"frame.id"<<filtered_msg.header.frame_id << std::endl ;
  filtered_points_pub.publish(filtered_msg);

}

点群を取得した後、まず点群をインターセプトします。測位のために MAX_MEASUREMENT_RANGE 距離内の点のみを保持します (この記事では MAX_MEASUREMENT_RANGE = 120 メートル)。VoxelGrid ダウンサンプリングの主なパラメーターは voxel_leaf_size で、ダウンサンプリングの選択を設定します。辺の長さ立方体の単位 (メートル単位)。このような立方体には 1 つのポイントのみが保持されます。このパラメータは、points_downsample.launch ファイルで構成できます。

<arg name="leaf_size" default="3.0" />

上に示したように、この記事ではリーフ サイズ 3 メートルを使用します。このパラメータは、実際に使用する LIDAR ポイントの密度に応じて決定できます。リアルタイムの登録を追求しますが、測位精度をあまり犠牲にしたくありません。したがって、入力点群のダウンサンプリングの程度は、リアルタイム パフォーマンスと測位精度のバランスを取る必要があります。経験によれば、16 ライン LIDAR が使用されていると推測される場合は、点群のリーフ サイズを制御する方が適切です。レーザーを使用する場合、32 ライン以上のレーダーの場合、リーフ サイズは 2 ～ 3m に設定できます。
ダウンサンプリングされた点群は、/filtered_points後続の NDT 登録と位置決めのためにトピックに出力されます。

NDT を使用して自動運転車の高精度測位を実現

コードのこの部分は主に次のように実装されていますndt.cpp。

初期姿勢の取得。
登録と位置決めに事前構築されたマップを使用するすべてのメソッドは、初期姿勢を提供する必要があります。産業上の実践では、この初期姿勢は通常、GNSS を通じて取得されます。この記事では、このステップを簡略化し、Rviz で手動で指定します。 . 初期姿勢。Rviz で設定された初期姿勢は、通常、デフォルトで/initialpose topic先頭に。NdtLocalizerコンストラクターで、トピックをリッスンするサブスクライバーを作成します。

initial_pose_sub_ = nh_.subscribe("initialpose", 100, &NdtLocalizer::callback_init_pose, this);

初期姿勢 ( geometry_msgs::PoseWithCovarianceStamped) が送信されると、次のコールバックが実行されますcallback_init_pose。

void NdtLocalizer::callback_init_pose(
  const geometry_msgs::PoseWithCovarianceStamped::ConstPtr & initial_pose_msg_ptr)
{
    
    
  if (initial_pose_msg_ptr->header.frame_id == map_frame_) {
    
    
    initial_pose_cov_msg_ = *initial_pose_msg_ptr;
  } else {
    
    
    // get TF from pose_frame to map_frame
    geometry_msgs::TransformStamped::Ptr TF_pose_to_map_ptr(new geometry_msgs::TransformStamped);
    get_transform(map_frame_, initial_pose_msg_ptr->header.frame_id, TF_pose_to_map_ptr);

    // transform pose_frame to map_frame
    geometry_msgs::PoseWithCovarianceStamped::Ptr mapTF_initial_pose_msg_ptr(
      new geometry_msgs::PoseWithCovarianceStamped);
    tf2::doTransform(*initial_pose_msg_ptr, *mapTF_initial_pose_msg_ptr, *TF_pose_to_map_ptr);
    // mapTF_initial_pose_msg_ptr->header.stamp = initial_pose_msg_ptr->header.stamp;
    initial_pose_cov_msg_ = *mapTF_initial_pose_msg_ptr;
  }
  // if click the initpose again, re init！
  init_pose = false;
}

NDT 登録では、主に次の 4 つの座標系間の変化に焦点を当てます。

• ワールド座標系 (frame_id = world)
• マップ座標系 (frame_id = マップ)
• 車両ベース座標系 (frame_id = Base_link)
• LIDAR 座標系 (この記事では Frame_id = ouster、フレーム ID は使用する LIDAR によって異なります)

このプロジェクトでは、static_tf.launch2 つの固定変換を使用してワールドをマップに公開し、base_link に追放します。

<node pkg="tf2_ros" type="static_transform_publisher" name="localizer_to_base_link" args="0 0 1.753999 0.03 0.01 0.01 base_link rslidar"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 map world" />

注: ここでは、問題を単純化するために、地図と世界が同じ座標系にあると仮定します。特定の自動運転システムの開発では、世界横メルカトル図法 (UTM) 投影法と経度および緯度に従って連携する必要があります。 WGS84 座標系 現在のマップとワールド座標系の間の平行移動関係と、East North Up (ENU) 座標系での回転量を取得します。
localizer_to_base_linkつまり、LIDAR とベース リンク間の変換関係は LIDAR の外部パラメータの 1 つであり、静的な変換でもあります。

初期姿勢の取得に戻ります Rviz 上で手動で指定した初期姿勢を取得したら、まず座標系を統一します 姿勢がマップ座標系の場合は保存し、他の座標系の場合はまず統一しますマップ座標系に変換し、get_transform次のように定義される関数を通じて変換関係を取得します。

bool NdtLocalizer::get_transform(
  const std::string & target_frame, const std::string & source_frame,
  const geometry_msgs::TransformStamped::Ptr & transform_stamped_ptr)
{
    
    
  if (target_frame == source_frame) {
    
    
    transform_stamped_ptr->header.stamp = ros::Time::now();
    transform_stamped_ptr->header.frame_id = target_frame;
    transform_stamped_ptr->child_frame_id = source_frame;
    transform_stamped_ptr->transform.translation.x = 0.0;
    transform_stamped_ptr->transform.translation.y = 0.0;
    transform_stamped_ptr->transform.translation.z = 0.0;
    transform_stamped_ptr->transform.rotation.x = 0.0;
    transform_stamped_ptr->transform.rotation.y = 0.0;
    transform_stamped_ptr->transform.rotation.z = 0.0;
    transform_stamped_ptr->transform.rotation.w = 1.0;
    return true;
  }

  try {
    
    
    *transform_stamped_ptr =
      tf2_buffer_.lookupTransform(target_frame, source_frame, ros::Time(0), ros::Duration(1.0));
  } catch (tf2::TransformException & ex) {
    
    
    ROS_WARN("%s", ex.what());
    ROS_ERROR("Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str());

    transform_stamped_ptr->header.stamp = ros::Time::now();
    transform_stamped_ptr->header.frame_id = target_frame;
    transform_stamped_ptr->child_frame_id = source_frame;
    transform_stamped_ptr->transform.translation.x = 0.0;
    transform_stamped_ptr->transform.translation.y = 0.0;
    transform_stamped_ptr->transform.translation.z = 0.0;
    transform_stamped_ptr->transform.rotation.x = 0.0;
    transform_stamped_ptr->transform.rotation.y = 0.0;
    transform_stamped_ptr->transform.rotation.z = 0.0;
    transform_stamped_ptr->transform.rotation.w = 1.0;
    return false;
  }
  return true;
}

マップの tf を取得したら、tf2::doTransform によって初期姿勢を直接マップ座標系に変換します。(デモのこの部分は後で追加されます)

マップの初期化

NDT 登録のターゲット点群は、事前に SC-LEGO-LOAM を使用して構築した点群マップです。mapLoader ノードによって送信される点群マップ メッセージをリッスンする Subscriber を作成し、次のコールバックを実行します。

map_points_sub_ = nh_.subscribe("points_map", 1, &NdtLocalizer::callback_pointsmap, this);

void NdtLocalizer::callback_pointsmap(
  const sensor_msgs::PointCloud2::ConstPtr & map_points_msg_ptr)
{
    
    
  const auto trans_epsilon = ndt_.getTransformationEpsilon();
  const auto step_size = ndt_.getStepSize();
  const auto resolution = ndt_.getResolution();
  const auto max_iterations = ndt_.getMaximumIterations();

  pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_new;

  ndt_new.setTransformationEpsilon(trans_epsilon);
  ndt_new.setStepSize(step_size);
  ndt_new.setResolution(resolution);
  ndt_new.setMaximumIterations(max_iterations);

  pcl::PointCloud<pcl::PointXYZ>::Ptr map_points_ptr(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg(*map_points_msg_ptr, *map_points_ptr);
  ndt_new.setInputTarget(map_points_ptr);
  // create Thread
  // detach
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  ndt_new.align(*output_cloud, Eigen::Matrix4f::Identity());

  // swap
  ndt_map_mtx_.lock();
  ndt_ = ndt_new;
  ndt_map_mtx_.unlock();
}

最も重要なステップはndt_new.setInputTarget(map_points_ptr)、点群マップを取得した後、ndt のターゲット点群を点群マップに設定することです。これは、ここでの NDT アルゴリズムの基本パラメータでもあります。

ndt_new.setTransformationEpsilon(trans_epsilon); 検索の最小変更量;
ndt_new.setStepSize(step_size); 検索のステップ サイズ
ndt_new.setResolution(resolution); ターゲット点群の ND ボクセルのサイズ (メートル単位)
ndt_new .setMinimumIterations(max_iterations) ;ニュートン法を使用して最適化された反復の数

NDT登録の位置付け

点群の登録と位置決めは、主に次のコールバックで実装されます。

void NdtLocalizer::callback_pointcloud(
  const sensor_msgs::PointCloud2::ConstPtr & sensor_points_sensorTF_msg_ptr)

このコールバックは、ダウンサンプリングされた点群をリッスンし、まず PointCloud2 メッセージを解析して pcl の PointCloud 構造にします。

const std::string sensor_frame = sensor_points_sensorTF_msg_ptr->header.frame_id;
const auto sensor_ros_time = sensor_points_sensorTF_msg_ptr->header.stamp;

boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_sensorTF_ptr(
new pcl::PointCloud<pcl::PointXYZ>);

点群は LIDAR 座標系にあるため、データは Base_link 座標系に投影されます。

geometry_msgs::TransformStamped::Ptr TF_base_to_sensor_ptr(new geometry_msgs::TransformStamped);
get_transform(base_frame_, sensor_frame, TF_base_to_sensor_ptr);
  //const auto exe_start_time = std::chrono::system_clock::now();
const Eigen::Affine3d base_to_sensor_affine = tf2::transformToEigen(*TF_base_to_sensor_ptr);
const Eigen::Matrix4f base_to_sensor_matrix = base_to_sensor_affine.matrix().cast<float>();

boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> sensor_points_baselinkTF_ptr(
new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(
*sensor_points_sensorTF_ptr, *sensor_points_baselinkTF_ptr, base_to_sensor_matrix);

NDT の入力点群として設定します。

 ndt_.setInputSource(sensor_points_baselinkTF_ptr);

  if (ndt_.getInputTarget() == nullptr) {
    
    
    ROS_WARN_STREAM_THROTTLE(1, "No MAP!");
    return;
  }

最後に、この登録の初期姿勢推定を設定する必要もあります。これは、次の 2 つの状況に分ける必要があります。

Eigen::Matrix4f initial_pose_matrix;
  if (!init_pose){
    
    
    Eigen::Affine3d initial_pose_affine;
    tf2::fromMsg(initial_pose_cov_msg_.pose.pose, initial_pose_affine);
    initial_pose_matrix = initial_pose_affine.matrix().cast<float>();
    // for the first time, we don't know the pre_trans, so just use the init_trans, 
    // which means, the delta trans for the second time is 0
    pre_trans = initial_pose_matrix;
    init_pose = true;
  }else
  {
    
    
    // use predicted pose as init guess (currently we only impl linear model)
    initial_pose_matrix = pre_trans * delta_trans;
  }

最初の登録の場合は、Rviz で手動で指定した初期姿勢が使用されます。それ以外の場合は、線形モデル (均一角速度) によって予測された初期推定値が使用されます。pcl によって実装された NDT では、初期姿勢推定が Eigen::Matrix4f (つまり、標準の同次変換行列) で表される必要があるため、上記のコードでは、最初の登録である場合、Pose を Eigen に変換する必要があります。 :Matrix4f とtf2::fromMsg()関数を使用して完了します。非プライマリ登録の場合、私たちのアイデアは、最後の NDT の位置決め結果 (変換行列pre_trans) + 線形変換量 (変換行列delta_trans) を使用することです。線形代数では、ベクトルAB が最後の測位の変換 (つまり、最後の測位の Base_link からマップ原点への変換、つまりpre_trans)を記述するために使用され、 BC が現在の位置決めから前の位置決めへの変換を表す場合、測位（すなわちdelta_trans）、次に現在の測位AC
AC=AB * BCとして表すことができる
ため、現在位置の初期姿勢推定値は pre_trans * delta_trans として表すことができます。

次に、この初期推定値を設定し、登録に ndt を使用します。

ndt_.align(*output_cloud, initial_pose_matrix);

位置決め変換行列を取得しました ( base_link からマップへの変換)result_pose_matrix

const Eigen::Matrix4f result_pose_matrix = ndt_.getFinalTransformation();

これを Pose msg に変換し、TF として公開して、この位置決めを完了します。

// publish
  geometry_msgs::PoseStamped result_pose_stamped_msg;
  result_pose_stamped_msg.header.stamp = sensor_ros_time;
  result_pose_stamped_msg.header.frame_id = map_frame_;
  result_pose_stamped_msg.pose = result_pose_msg;

  if (is_converged) {
    
    
    ndt_pose_pub_.publish(result_pose_stamped_msg);
  }

  // publish tf(map frame to base frame)
     publish_tf(map_frame_, base_frame_, result_pose_stamped_msg);

さらに、初期姿勢推定のために次の delta_trans を計算する必要もあります。

// calculate the delta tf from pre_trans to current_trans
  delta_trans = pre_trans.inverse() * result_pose_matrix;
  
  pre_trans = result_pose_matrix;

delta_trans は、現在の変換と前の変換の差 (平行移動と回転の差) です。最後に、現在の変換は、次の初期姿勢推定のために pre_trans として保存されます。この時点で、NDT 登録プロセスは終了します。

練習する

ターミナルを開く

source devel/setup.bash
roslaunch ndt_localizer ndt_localizer.launch

リアルタイム出力マッチング時間は基本的に約8msで、自動運転登録と測位のリアルタイム要件を満たしており、マッチング反復回数と線形変換行列がリアルタイムで出力されます。

rostopic echo /ndt_pose を通じて、トピックによって出力された位置と角度の情報をリアルタイムで表示できます。