Share
Embed
LiDAR SLAMの実装とセンサ融合 ~Lie群からContinuous-Time LIOまで~
ROBOMECH2026で行われたSLAMワークショップの際に使用したスライドです。
Lie群を用いたLiDAR SLAMの実装に関して、短いですがチュートリアル的に解説しています。
また最近行っている研究(縮退環境におけるセンサ融合)に関しても少し説明しています。
Naoki Akai
July 03, 2026
Transcript
36 slides
-
LiDAR SLAMの実装とセンサ融合 ~ Lie群からContinuous-Time LIOまで ~ 名古屋大学 特任准教授 株式会社LOCT 代表取締役
赤井 直紀 @naokiakai ROBOMECH 2026 SLAMワークショップ 1 -
簡単な自己紹介(SLAMとの関わり) 2011年から研究を開始、つくばチャレンジに参加(宇都宮大学) ➢ 最初にLiDAR(北陽電機社製Top-URG)を用いた障害物回避を実装 ➢ 次に自己位置推定Monte Calor localization(MCL)を実装 (当時の地図構築技術は適当、オドメトリベースでループは地図切替で対応)
(2011年から14年までメインで参加したすべての課題を達成) 2016年博士取得、その後より自己位置推定に関する研究を実施 ➢ MCLをベースに色々な確率モデルの研究を実施 ➢ 2023年、JFRでReliable Monte Carlo Localization for Mobile Robotsという論文を出版 (タイトルはもちろんD. FoxとS. Thrunの有名な論文からインスパイアされている) つくばチャレンジ2013の写真 (横塚さんも写ってます) 2 -
2022年に会社を作る ➢ 23年に会社経由でFAST-LIO*の再現実装ができないかと声をかけられる ➢ LiDAR-Inertial Odometry(LIO)を追いかけるも数式で挫折 ➢ Lie群を使ったLIO、SLAMの基礎・実装方法を一から学び始める ➢
2025年にほぼ依存ライブラリ無しのLiDAR SLAM**を公開 FAST-LIOの論文より* * W. Xu and F. Zhang. “FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter,” RA-L, 2021. ** plain_slam_ros2: https://github.com/NaokiAkai/plain_slam_ros2 一般の人よりSLAMは理解しているが、 ずっとSLAMの研究をしていた者ではない 簡単な自己紹介(SLAMとの関わり) 今日はここ数年で蓄積した知見を基に チュートリアル的な話を 3 -
LiDAR-Inertial SLAMを実装するのに必要なLie群とLie代数の知識 ➢ Lie群を使ったスキャンマッチングの実装 ➢ IMUとの融合(時間の都合でかなり簡略化) 最近取り組んでいる話 ➢
軌跡を連続時間で表現するContinuous-Time LIO(CT-LIO)について ➢ CT-LIOを用いた縮退環境でのセンサ融合(ホイールオドメトリ、反射強度、ドップラー速度) 今日のお話 4 -
SLAMの基本は3次元空間での姿勢推定、まずは姿勢の表現方法を考える ➢ 位置(Position)は基本的に (x, y, z) ➢ 角度(Orientation)はいくつか存在 ➢
代表的(直感的)な表現方法はオイラー角 (roll, pitch, yaw) を用いたもの 3次元の姿勢推定問題における状態表現 ➢ ROSユーザーにとって馴染みあるのが四元数 (クォータニオン: Quaternion) ➢ 回転軸uと回転θの4パラメータで回転を表現 SLAMではあまり使われない表現 SLAMで使っても問題ないけど今日は違うお話 5 -
回転行列を用いた姿勢の表現 ➢ 機体座標系の軸が世界座標系でどの方向を向い ているかを表す3x3の行列 ➢ Special Orthogonal Group in
3 dimensionsとも呼 ばれ、これを省略してSO(3)と呼ぶ 3次元の姿勢推定問題における状態表現 ➢ SO(3)には満たすべき性質がある (すべての3x3の行列がSO(3)になるというわけではない) 本日の主役はこれ 6 -
今日の話で利用する姿勢の表現方法 ➢ 回転行列Rと位置tでそれぞれ表す 3次元の姿勢推定問題における状態表現 ➢ もしくはSpecial Euclidean Group(SE(3))で表す(SE(3)にも満たすべき制約がある) LiDAR
SLAMでLie群といった場合、おおよそSO(3)とSE(3)のことを指す (省略して と書くこともある) 7 -
SO(3)やSE(3)の計算は必ず積で行う ➢ 状態ベクトルxがΔx動いた場合はx + Δxだが、SE(3)の和はSE(3)でなくなってしまう(SO(3)も) 計算の際の注意事項 ➢ 変化を適用させるときは行列の積を計算する ➢
同様に差分を計算するときは逆行列の積を計算する これはダメ 8 -
複数の変換が入る場合はSE(3)を使うとすっきりまとまる 点の座標変換 LiDAR to IMU IMU to World 1.
を用いてLiDAR to IMU 2. を用いてIMU to World 複雑な座標変換も整理しやすい LIOでは基本的にIMUの姿勢を求める 座標変換の把握は必須の知識 10 -
表記の厳密性は置いておいて、ひとまず最適化(残差の総和の最小化)の方針を整理 ➢ ガウス・ニュートン法で解くために各残差rに関する状態xのヤコビアンJを計算する 最適化の方針 状態xはRとtを含む(Rが行列なのは今は無視) この場合Jは3x6の行列になる ➢ ヤコビアンと残差を用いて、ヘッセ行列Hと勾配bを求める ➢
次式を満たすδxを求めて状態を更新する この足し算も正しくないけどまずは無視 SO(3)の制約から外れてしまう 12 -
➢ 残差rに対する状態x = (δθ t)に関するヤコビアンは以下となる Lie群を用いたガウスニュートン法による最適化 ここまでの議論を基に、Lie群を用いたスキャンマッチングの実装を整理 ➢ これを用いてガウス・ニュートン法により更新量δxを求める
➢ 状態xを更新、ただし回転に関しては指数写像を用いて更新(収束するまで繰り返す) 17 -
➢ ルーズカップリング(代表例:Direct LIO) IMUとの融合方式 IMUとの融合方法は大別して2種類 ➢ タイトカップリング(代表例:FAST-LIO、LIO-SAM、GLIM) ✓ IMUの計測値を積分して姿勢を予測
✓ スキャンマッチングにより姿勢を計算 ✓ 2種類の姿勢を融合 ✓ IMU、スキャンマッチング由来の 誤差を同時最小化して姿勢を計算 タイトカップリングに比べて実装が容易だが 精度が低い傾向にある ルーズカップリングに比べて実装は難しいが 精度が高い傾向にある 今日のお話ではタイトカップリングの話を扱います 18 -
LiDAR、IMU由来の残差を同時に最小化する(タイトカップリング) IMU LiDAR Prior(初期値) タイトカップリングの場合、 Priorがないと解が定まらない ➢ それぞれの残差に対するヤコビアンを求める LiDARに対する残差は回転と位置にのみ依存
ヤコビアンは前述のものと同じ IMUに対する残差にはすべての状態が依存する このヤコビアンの導出は極めて煩雑 コスト関数の定義 タイトカップリングの実装では基本的にCeresやGTSAMの様なライブラリを使うことが多い ただし何をどう最小化しているのかということは正確に理解しないといけない 22 -
3次のCumulative B-Splineで姿勢を表現することを考える Cumulative B-Splineを用いた軌跡の連続表現 ➢ ある任意の姿勢を計算するにあたり4つの姿勢(制御点:Control Points)を用いる ➢ λは制御点の間隔で決定的に決まる係数
直接的に時刻tの姿勢R(t), t(t)を最適化するのではなく、 制御点を最適化することで時刻tの姿勢を得られるようにする 28 -
ホイールオドメトリの融合 IMUとオドメトリの特性を考慮した融合が可能 IMU角速度残差 IMU加速度残差 オドメトリ角速度残差 オドメトリ速度残差 ➢ コスト関数にはオドメトリに由来する残差も追加 IMU
LiDAR Prior オドメトリ 外れ値の多いLiDAR、オドメトリ由来の残差にのみロバストカーネルを設けるなどの融合が可能 (区間で積分して移動量として観測を扱うと瞬間的な外れ値を弾くのが難しくなる) 33 -
縮退環境における融合の所感 必ず融合を行うというのが良いというわけではない ➢ そもそもLIO単体で十分な精度が出ることの方が多い ➢ ホイールオドメトリ単体の制度はLIOより圧倒的に低い ➢ ただしLIO単体で推定ができない区間があることは確か ➢
縮退方向にのみ作用する融合をさせることが重要そう(今検証中) 37 -
LiDAR-Inertial SLAMを実装するのに必要なLie群とLie代数の知識 ➢ Lie群を使ったスキャンマッチングの実装 ➢ IMUとの融合 最近取り組んでいる話 ➢
軌跡を連続時間で表現するContinuous-Time LIO(CT-LIO)について ➢ CT-LIOを用いた縮退環境でのセンサ融合について おわりに 38 本日は講演の機会を頂き誠にありがとうございました