Home » 3D-LiDAR（Livox：MID-360）を使った自己位置推定と移動軌跡の取得

3D-LiDAR（Livox：MID-360）を使った自己位置推定と移動軌跡の取得

Livox 製 MID-360 を搭載したハンディーマップキットを使って、自己位置推定＆移動軌跡を実装しました。
  • MID-360内蔵IMU（慣性計測ユニット）を使用
  • 取得した点群（地図）と自己周辺の点群から自己位置推定を行い、移動軌跡を取得できます。

  • ハンディーマップキット

  • 自己位置推定＆移動軌跡の取得

目次

3D-LiDAR（Livox：MID-360）を使った自己位置推定と移動軌跡の特徴

事前に取得したポイント クラウド マップでのリアルタイム 3D に対してグローバル ローカリゼーションを実行します。
低周期でのグローバル ローカリゼーション (約 0.5 ～ 0.2 Hz) と FAST-LIO からの高周期のオドメトリを融合することで、システム全体の計算効率が向上します。
走行距離測定の累積誤差を排除します。
初期の位置特定は、RVIZ からの大まかな手動推定、または別のセンサー/アルゴリズムからの姿勢によって指定できます。

ハードウエア構成

環境

  • ROS（Noetic）

セットアップ

Livox_SDK2 をインストール＆セットアップします。

Livox_SDK2（GitHub）

Livox_ros_driver2 をインストール＆セットアップします。

Livox_ros_driver2（GitHub）

pipをインストールします。

$ sudo apt intall python3-pip

numpyをインストールをインストールします。

$ sudo apt intall ros-noetic-ros-numpy

open3dをインストールします。

$ pip install open3d

open3dをインストール終了後、以下のコマンドでテストを実行し、エラーが無ければOK。

$ python3 -c "import open3d"

github に公開されている FAST-LIO-LOCALIZATIONのパッケージをダウンロードします。

$git clone https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION.git
$cd FAST_LIO_LOCALIZATION
$git submodule update --init

このまま、catkin_makeを実行しても、エラーが発生するので、以下を修正する

livox_ros_driver2　用にファイルを変更

  • 修正ファイル
    FAST-LIO/CMakeLists.txt、LIO-Livox/package.xml
    FAST-LIO/src/laserMapping.cpp
    FAST-LIO/src/preprocess.cpp
    FAST-LIO/src/preprocess.h
  • 修正内容
    livox_ros_driver => livox_ros_driver2

コンパイルエラー対策

  • 修正ファイル
    FAST_LIO_LOCALIZATION/include/common_lib.h
  • 修正内容
    fast_lio/Pose6D.h => fast_lio_localization/Pose6D.h

コンパイルの実行

$ catkin_make

scriptディレクトリ以下のPythonファイルをpython3用に変更します。

  • 変換ツールのインストール
$ sudo apt-get install 2to3
  • scrips以下のファイルを変換
$ 2to3 -w ./scripts
  • すべてのファイルの1行目を変更
    #!/usr/bin/env python2 => #!/usr/bin/env python3
  • scripts/global_localization.py
    o3d.registration => o3d.pipelines.registration　に変更します。
  • numpyのバージョンを下げます：1.20.3
$ pip uninstall -y numpy
$ pip install numpy=1.20.3

パラメータの設定

launchファイル（localization_mid360.launch）

そのままでは地図が表示されないので、localization_mid360.launch内のを変更します。
frame_id:=/map => frame_id:=map

fast_lio_localizationパッケージでは、あらかじめ取得した地図データのファイル名を指定します。
<arg name="map" default="1(find fast_lio_localization)/pcd/scans.pcd" />

  • ※地図データ（pcdファイル）は必要に応じて、CloudCompare等を使ってダウンサンプリングしてください。

また、各種パラメータはFAST-LIOと同じになります。 

<launch>
<!-- Launch file for Livox MID-360 LiDAR -->

	<arg name="rviz" default="true" />

	<rosparam command="load" file="$(find fast_lio_localization)/config/mid360.yaml" />

	<param name="feature_extract_enable" type="bool" value="0"/>
	<param name="point_filter_num" type="int" value="10"/>
	<param name="max_iteration" type="int" value="3" />
	<param name="filter_size_surf" type="double" value="0.5" />
	<param name="filter_size_map" type="double" value="0.5" />
	<param name="cube_side_length" type="double" value="1000" />
	<param name="runtime_pos_log_enable" type="bool" value="0" />
	<param name="pcd_save_enable" type="bool" value="0" />

    <node pkg="fast_lio_localization" type="fastlio_mapping" name="laserMapping" output="screen" />
	<arg name="map" default="$(find fast_lio_localization)/pcd/scans.pcd" />

	<!-- loalization-->
    <node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen">
    </node>

	<!-- transform  fusion-->
    <node pkg="fast_lio_localization" type="transform_fusion.py" name="transform_fusion" output="screen" />

	<!-- glbal map-->
    <node pkg="pcl_ros" type="pcd_to_pointcloud" name="map_publishe" output="screen"
		  args="$(arg map) 5 _frame_id:=map cloud_pcd:=/map" />

	<group if="$(arg rviz)">
	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio_localization)/rviz_cfg/localization.rviz" />
	</group>

</launch>

configファイル（mid360.yaml）

configファイルに関しても、各種パラメータはFAST-LIOと同じになります。 

common:
    lid_topic:  "/livox/lidar"
    imu_topic:  "/livox/imu"
    time_sync_en: false                 # ONLY turn on when external time synchronization is really not possible
    time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
                                                     # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0

preprocess:
    lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
    scan_line: 4
    blind: 0.2

mapping:
    acc_cov: 0.1
    gyr_cov: 0.1
    b_acc_cov: 0.0001
    b_gyr_cov: 0.0001
    fov_degree:    360
    det_range:      50.0
    extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
    extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
    extrinsic_R: [ 1, 0, 0,
                　        0, 1, 0,
                          0, 0, 1]

publish:
    scan_publish_en:  true       # false: close all the point cloud output
    dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
    scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame

global_localization.py の編集

必要に応じて、scripts/global_localization.py ファイル内の以下パラメータを編集します。

# only those scan2map-matching with higher fitness than LOCALIZATION_TH will be taken
    　　　　LOCALIZATION_TH = 0.98
    # FOV(rad), modify this according to your LiDAR type
    　　　　FOV = 6.3
    # The farthest distance(meters) within FOV
    　　　　FOV_FAR = 50
  • LOCALIZATION_TH：地図の点群のセンサーの点群の何%が一致したら自己位置があっているかを判断する閾値
  • FOV：水平方向のFOV（ラジアン）
  • FOV_FAR：最大距離（ｍ）

実行方法

  • Terminal-1
$ roslaunch fast_lio_localization localization_mid360.launch
  • Terminal-2
$ roslaunch livox_ros_driver2 msg_MID360.launch

FAST-LIO-LOCALIZATION 自己位置推定 & 移動軌跡

  • ※本動画では、rosbagを再生しています。

rosbagを使用する場合は、以下の手順で実行してください。

  • fast_lio_localizationを実行後、rvizが立ち上がり、しばらくすると読み込んだ地図データ（scan.pcd）が表示されます。
  • 地図が表示された後、rosbagを再生し、LiDARからの点群が表示(1~2秒程度再生)されたら、pauseします。
  • rviz上で「2D Pose Estimate」を使って、初期位置・方向を指定します。
  • 初期位置の指定が上手く実行出来たら、rosbagの再生を再開します。

参考

※注意

本システムはあくまでもデモ用として確認しています。従いまして、不具合などの責任は一切負いかねます。
また、商用利用する場合は、ソースコード作成元へ確認してください。

お問い合わせ