Abudoriです。 前回はつくばチャレンジ2024で走行したロボットを紹介しました。 www.abudorilab.com
前々回でSLAMした地図をロボットに取り付けられているセンサデータを使って自己位置推定をします。 自律走行の要な部分なので使えるようになって活用してみてください。
まだSLAMの回を見ていない方はこちらからどうぞ！ www.abudorilab.com www.abudorilab.com www.abudorilab.com
スポンサードリンク
lidar_localization_ros2 とは
rsasaki0109さんのlidar_localization_ros2を使用します。
ROS 2（Humble以降？）で動作するLocalizationパッケージです。
Githubを見てみると、
センサデータの/cloudと3DMapのpcdファイルを読み込んだトピックである/mapを入力すると3次元座標の/pcl_poseを出力するようです。
/pcl_poseはgeometory_msgs/PoseStamped型でこの中身を見てみると、geometry_msgs/Poseが格納されています。このPoseはPointとQuaternionが入っていて、Poseは[x,y,z]、Quaternionは[x,y,z,w]の値となります。
Poseは3次元座標の原点からのX、Y、Zの値がそのまま入っています。このブログで以前取り上げた方法で3DMapを作っていた場合、計測開始直後の場所が原点で、その後に、そこからどれくらい動いたかを示す位置座標として扱うことができそうです。
Quaternionは回転座標です。回転方向の３軸である、roll、pitch、yawをコンピュータが計算しやすいように数学の四元数に変換したものと思ってください。この値がわかるということは、ロボットがどっちの方向に向いているかがわかるということです。
他の値はOptional（使いたければつかってね）なので、一旦気にしなくてもOKです。あとで解説します。
↓格納されている情報を見てみましょう docs.ros.org docs.ros.org
この/pcl_poseが出力されるということは、もうロボットが地図上のどこにいるかがわかったということですので、向かいたい目標値までどのように進むべきかの経路を逆算できることになります。なんだかこれだけで自律走行に近づいた気がしませんか？
このlidar_localization_ros2で実際に自己位置推定をやってみましょう。つくばチャレンジの3DMapとロボットのセンサ情報のROSBAGをGoogleDriveで公開していますので、お手元にROS 2がある方は、実際に試してみましょう（ちょっとだけパワフルなPCが必要かもしれません）。
インストール
今回はHumble環境で使用しました。
colcon_wsにコードをクローンする。
cd your/colcon_ws/src git clone https://github.com/rsasaki0109/lidar_localization_ros2.git colcon build --symlink-install
ビルドエラーになってしまいました。
githubのReadmeを読み進めると、このパッケージ内では、別のROSパッケージを依存しています。
同じ作者のndt_omp_ros2をインストールします。（SLAMの時に利用したGLIMの作者のndt_ompをベースに作られているようです） 同じようにcolcon_wsにcloneしてビルドします。
git clone https://github.com/rsasaki0109/ndt_omp_ros2.git colcon build --symlink-install
ビルドが通れば成功です。Humble、Jazzyで、ndt_omp_ros2を追加でクローンすること以外の作業はせずにビルドすることができました。
スポンサードリンク
使用方法
準備するもの
使い始めるにあたって、自己位置を推定したい3次元地図とセンサデータが必要です。3次元地図はSLAMアルゴリズムを利用して自作するか、Abudoriがつくばチャレンジの実験走行で作成した以下の地図を使って試してみることができます。
3次元地図アルゴリズムは以前紹介したGLIMがおすすめです。使用方法は以下のページをご覧ください。 www.abudorilab.com
つくばチャレンジのコースをロボットに搭載されたVelodyen LiDARのセンサデータを使って、GLIMで作成した地図は以下から手に入ります。Apatch2.0ライセンスでご利用ください。 drive.google.com
センサデータは現地でロボットのセンサをすべて起動する（すべてのセンサのトピックが見られる状態）、または、その状況をrosbagで収録したものを再生させた状態でlidar_lozalization_ros2を起動させると自己位置を返します。
机上でお試しをする場合、Abudoriがつくばチャレンジの現場で採取したセンサデータを公開しています。Apatch2.0でご利用ください。
設定
まずは設定します。lidar_localization_ros2の全体の構造を見てみます。
.
├── CMakeLists.txt
├── LICENSE
├── README.md
├── images
│ └── path.png
├── include
│ └── lidar_localization
│ ├── lidar_localization_component.hpp
│ └── lidar_undistortion.hpp
├── launch
│ └── lidar_localization.launch.py
├── package.xml
├── param
│ └── localization.yaml
├── rviz
│ └── localization.rviz
└── src
├── lidar_localization_component.cpp
└── lidar_localization_node.cpp
注目すべき点は以下のファイルです。自己位置推定で使用するパラメータをそれぞれ設定できます。基本的にはこのファイルの編集のみで操作が完結します。わかりやすいですね。
・launch/pcl_localization_launch.py
・param/localization.yaml
launchファイルの設定
基本的には、入力トピックの名前やTFの設定を行います。
入力するセンサとして、3DLiDAR、IMU、Odometryの三種類が選べます。3DLiDARは必須で、それにOdometryやIMUを付加することができます。どちらもLiDARを補助するもので、綺麗なセンサデータを供給することができれば、自己位置推定の精度を上げることができるでしょう。
3DLiDARの位置を指定します。ロボットフレームの原点である、base_linkに対して3DLiDARがどの位置に設置されているかを指定します。
argumentsのゼロの値をご自身のロボットに合わせて変えましょう。最初から順番に、位置の(x,y,z)、回転のクォータニオンの(x,y,z,w)の順の合計７個の値を入れます。 ダウンロードしたrosbagを使う場合は、このままにします（地図を作ったときも、自己位置推定をするときも3DLiDARがくっついただけの設定になっているので、そのままにしています）。
lidar_tf = launch_ros.actions.Node(
name='lidar_tf',
package='tf2_ros',
executable='static_transform_publisher',
arguments=['0','0','0','0','0','0','1','base_link','velodyne']
)
LiDARのトピックを選択します。今回はVelodyneのVLP-16を使います。rosbagも同様です。デフォルトではトピック名が/velodyne_pointsに指定されていますが、別のセンサを使う場合はここを変更します。
lidar_localization = launch_ros.actions.LifecycleNode(
name='lidar_localization',
namespace='',
package='lidar_localization_ros2',
executable='lidar_localization_node',
parameters=[localization_param_dir],
remappings=[('/cloud','/velodyne_points')],
output='screen')
IMUを使用する場合は同じように取り付け位置のtf情報を変更します。 配布のrosbagは使用しないのでそのままです。
imu_tf = launch_ros.actions.Node(
name='imu_tf',
package='tf2_ros',
executable='static_transform_publisher',
arguments=['0','0','0','0','0','0','1','base_link','imu_link']
)
localization.yamlの設定
こちらは自己位置推定アルゴリズムのパラメータを指定することができます。
まずは、使うセンサの有無を指定できます。 つくばチャレンジ2024では、odometryを使用して、IMUを使用しない設定にし、コース全域で安定した自己位置推定を継続することができました。
配布のrosbagで試す場合は、両方ともfalseにします。odometryもIMUも入っていません（そのため、設定によってはうまく行かない箇所もあります）。
use_odom: false
use_imu: false
次にマッチング手法を設定します。lidar_localization_ros2では、スキャンマッチングの方法を選択することができます。 つくばチャレンジ2024では、NDTマッチングが安定していたので、そちらを選びました。 NDTとNDT_OMPがありますが、OMPはOpenMPを使ったマルチスレッド並列処理が入っているものです。基本的にはOMPの方を選びましょう。
registration_method: "NDT_OMP"
最後にNDTマッチングの解像度を調整します。
まず、NDTマッチングの説明はTier4さんの説明資料がわかりやすいです。こちらの5ページにあるNDボクセルの大きさを指定します。 www.slideshare.net
NDボクセルの大きさを指定します。つくばチャレンジ2024では、2.0に設定したところうまく動作しました。 ただし、使用するセンサや地図の大きさ、精密さによって異なるので、ご注意ください。あくまで、Abudoriがうまくいった値です。
ndt_resolution: 2.0
アルゴリズムに関しては、全くの素人なので解説は他に譲りますが、体感としては、 - 小さくすると、自己位置の軌跡がキレイになる - ただし、場面が変わるとマッチングしなくなる、全く別のところとマッチングするなどが発生する - 大きくすると、自己位置の軌跡が多少ギザギザする - ギザギザするかわりに、マッチングしない致命的なエラーが減る といった印象を受けました。
まずはそのままで実施して、自分が走らせたいコース全周のrosbagでうまくいかない点があれば変更するアプローチがいいと思います。 地図の大きさや、物が煩雑にあるか、見通しの良いところかで設定するパラメータ、失敗するパラメータは異なりますので、試行錯誤してみて設定します。
スポンサードリンク
次回は実際にサンプルrosbagを動かしてみよう
長くなったので区切りますが、次回は配布pcdファイルとrosbagで動かしてみましょう。
冒頭にも説明しましたが、pcdファイルを入力してから、ロボットのセンサデータを入力すると、自己位置が出力されます。 ご自身のロボットの値に置き換えてみて、うまく行くように調整することで、3次元空間で自己位置推定する方法がわかるようになるはずです！
