LiDAR SLAMの実装とセンサ融合 ~Lie群からContinuous-Time LIOまで~

ROBOMECH2026で行われたSLAMワークショップの際に使用したスライドです。
Lie群を用いたLiDAR SLAMの実装に関して、短いですがチュートリアル的に解説しています。
また最近行っている研究（縮退環境におけるセンサ融合）に関しても少し説明しています。

Transcript

1. LiDAR SLAMの実装とセンサ融合 ~ Lie群からContinuous-Time LIOまで ~ 名古屋大学 特任准教授 株式会社LOCT 代表取締役 赤井 直紀 @naokiakai ROBOMECH 2026 SLAMワークショップ 1
2. 簡単な自己紹介（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
3.  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
4.  LiDAR-Inertial SLAMを実装するのに必要なLie群とLie代数の知識 ➢ Lie群を使ったスキャンマッチングの実装 ➢ IMUとの融合（時間の都合でかなり簡略化）  最近取り組んでいる話 ➢ 軌跡を連続時間で表現するContinuous-Time LIO（CT-LIO）について ➢ CT-LIOを用いた縮退環境でのセンサ融合（ホイールオドメトリ、反射強度、ドップラー速度） 今日のお話 4
5.  SLAMの基本は3次元空間での姿勢推定、まずは姿勢の表現方法を考える ➢ 位置（Position）は基本的に (x, y, z) ➢ 角度（Orientation）はいくつか存在 ➢ 代表的（直感的）な表現方法はオイラー角 (roll, pitch, yaw) を用いたもの 3次元の姿勢推定問題における状態表現 ➢ ROSユーザーにとって馴染みあるのが四元数 （クォータニオン: Quaternion） ➢ 回転軸uと回転θの4パラメータで回転を表現 SLAMではあまり使われない表現 SLAMで使っても問題ないけど今日は違うお話 5
6.  回転行列を用いた姿勢の表現 ➢ 機体座標系の軸が世界座標系でどの方向を向い ているかを表す3x3の行列 ➢ Special Orthogonal Group in 3 dimensionsとも呼 ばれ、これを省略してSO(3)と呼ぶ 3次元の姿勢推定問題における状態表現 ➢ SO(3)には満たすべき性質がある （すべての3x3の行列がSO(3)になるというわけではない） 本日の主役はこれ 6
7.  今日の話で利用する姿勢の表現方法 ➢ 回転行列Rと位置tでそれぞれ表す 3次元の姿勢推定問題における状態表現 ➢ もしくはSpecial Euclidean Group（SE(3)）で表す（SE(3)にも満たすべき制約がある） LiDAR SLAMでLie群といった場合、おおよそSO(3)とSE(3)のことを指す （省略して と書くこともある） 7
8.  SO(3)やSE(3)の計算は必ず積で行う ➢ 状態ベクトルxがΔx動いた場合はx + Δxだが、SE(3)の和はSE(3)でなくなってしまう（SO(3)も） 計算の際の注意事項 ➢ 変化を適用させるときは行列の積を計算する ➢ 同様に差分を計算するときは逆行列の積を計算する これはダメ 8
9.  SLAMでの座標変換はSO(3)、特にSE(3)を使うと簡単にまとめられる 点の座標変換 LiDAR座標の点 世界座標の点 SO(3)を用いた場合 SE(3)を用いた場合 （この場合pの4要素目は常に1となる） 9
10.  複数の変換が入る場合はSE(3)を使うとすっきりまとまる 点の座標変換 LiDAR to IMU IMU to World １． を用いてLiDAR to IMU ２． を用いてIMU to World 複雑な座標変換も整理しやすい LIOでは基本的にIMUの姿勢を求める 座標変換の把握は必須の知識 10
11.  LiDARが計測した点を世界座標に変換する（先ほどの話） LiDAR座標の点 世界座標の点 世界座標（World）におけるLiDARの姿勢 ➢ 座標変換後、地図点群の中で最近傍点（対応点） を取得して残差（Residual）を定める ➢ 残差の総和を最小にする姿勢を求める（最適化） スキャンマッチングにおける残差 11
12.  表記の厳密性は置いておいて、ひとまず最適化（残差の総和の最小化）の方針を整理 ➢ ガウス・ニュートン法で解くために各残差rに関する状態xのヤコビアンJを計算する 最適化の方針 状態xはRとtを含む（Rが行列なのは今は無視） この場合Jは3x6の行列になる ➢ ヤコビアンと残差を用いて、ヘッセ行列Hと勾配bを求める ➢ 次式を満たすδxを求めて状態を更新する この足し算も正しくないけどまずは無視 SO(3)の制約から外れてしまう 12
13. ➢ 残差rをもう一度確認 ヤコビアンを求めてみる（並進） ➢ 並進tに関する微分は簡単に求まるが、少し丁寧に導出 テイラー展開して2次の微小量を無視 これを満たすJ（ヤコビアン）を求める ➢ これを計算していくと残差rに対する並進tのヤコビアンが求められる 並進に関するヤコビアンは単なる単位行列になる （これは一般的な線形対数の話） 13
14. ➢ 問題は回転行列Rの方 ヤコビアンを求めてみる（回転） これはダメ ➢ ここで「Lie群」と調べるとよく出てくるこの図が出てくる Rに接する3次元のベクトル 空間での変動をδθとする δθの変動を考慮したSO(3)多 様体上での状態がexp(δθ^)R 14
15. ➢ 回転行列Rに、Rの接空間上での微小回転δθが作用した新たな回転行列を以下のように定義 ヤコビアンを求めてみる（回転） 歪対象行列 これがLie代数 実質自由度3の 3次元ベクトル ➢ 指数写像は以下のように定義される（so(3)からSO(3)への写像） Lie代数（ベクトル空間）上でコストを最小にするδθを求めて回転行列を更新していく Lie代数上であれば一般的な微分計算が可能 15
16. ➢ 残差rのδθに関するテイラー展開を考える ➢ このJを満たすものがδθに関するヤコビアンとなる ヤコビアンを求めてみる（回転） 回転に関するヤコビアン 16
17. ➢ 残差rに対する状態x = (δθ t)に関するヤコビアンは以下となる Lie群を用いたガウスニュートン法による最適化  ここまでの議論を基に、Lie群を用いたスキャンマッチングの実装を整理 ➢ これを用いてガウス・ニュートン法により更新量δxを求める ➢ 状態xを更新、ただし回転に関しては指数写像を用いて更新（収束するまで繰り返す） 17
18. ➢ ルーズカップリング（代表例：Direct LIO） IMUとの融合方式  IMUとの融合方法は大別して2種類 ➢ タイトカップリング（代表例：FAST-LIO、LIO-SAM、GLIM） ✓ IMUの計測値を積分して姿勢を予測 ✓ スキャンマッチングにより姿勢を計算 ✓ 2種類の姿勢を融合 ✓ IMU、スキャンマッチング由来の 誤差を同時最小化して姿勢を計算 タイトカップリングに比べて実装が容易だが 精度が低い傾向にある ルーズカップリングに比べて実装は難しいが 精度が高い傾向にある 今日のお話ではタイトカップリングの話を扱います 18
19. ➢ 状態量を以下のように定義する（必ずこうするというわけではない） 状態と残差の定義  IMU由来の残差について考えていく それぞれ速度、IMUの 角速度・加速度バイアス ➢ IMUの計測値Uと2地点間の状態から、IMU由来の誤差を考える 角速度と加速度 19
20. ➢ IMUの計測値を積分して回転行列、速度、位置の更新を行う（IMUプレインテグレーション） 状態と残差の定義 ➢ IMUの計測値から更新量（ΔR, Δt, Δv）を定めることができる（厳密さは割愛してます） この変化量をIMU由来の残差を定める際の観測として利用する 20
21. 状態と残差の定義 ➢ 推定対象とする状態間からも変化量がそれぞれ計算できる ➢ IMUの計測値から作られた変化量と、2状態間から計算される変化量の差分を残差とする ←これはso(3)を対応する3次元ベクトル変換にする演算 この9次元ベクトルがIMU由来の残差 21
22.  LiDAR、IMU由来の残差を同時に最小化する（タイトカップリング） IMU LiDAR Prior（初期値） タイトカップリングの場合、 Priorがないと解が定まらない ➢ それぞれの残差に対するヤコビアンを求める LiDARに対する残差は回転と位置にのみ依存 ヤコビアンは前述のものと同じ IMUに対する残差にはすべての状態が依存する このヤコビアンの導出は極めて煩雑 コスト関数の定義 タイトカップリングの実装では基本的にCeresやGTSAMの様なライブラリを使うことが多い ただし何をどう最小化しているのかということは正確に理解しないといけない 22
23.  ファクターグラフを用いるとかなり直感的に式を記述できる ファクターグラフを用いた表現 スキャンマッチングファクター IMUプレインテグレーションファクター Priorファクター 何に関する残差が定義されているか、 その残差がどの状態に依存しているか、 定式化されている問題が直感的に理解しやすい IMU LiDAR Prior（初期値） 23
24. 最近取り組んでいること ⚫ 軌跡を連続間で表現したLIO ⚫ 縮退対応のためのセンサ融合 24
25. 縮退環境におけるLIOの例（すべてセルフ実装） ENWIDE（LIO+反射強度） M3DGR（LIO+ホイールオドメトリ） Boreas（LIO+ドップラー速度） 25
26. 縮退環境  トンネル、もしくは開けた空間の様に、いくつかの方向に姿勢を拘束できない環境 ➢ ガウスニュートン法的には、更新量を求める際のヘッセ行列がランク落ちしている（数値的に不安定） ランク落ちを防ぐ情報の追加 センサ融合が必要 26
27.  センサ融合では基本的に時間同期が問題になる Continuous-Time LIO（CT-LIO） ➢ IMUとLiDARを同期させようとしても厳密に時間同期するデータは存在しない ➢ 軌跡を連続時間表現（例えばCubic B-Spline曲線）で表現すれば任意の時間の姿勢が計算可能 27
28.  3次のCumulative B-Splineで姿勢を表現することを考える Cumulative B-Splineを用いた軌跡の連続表現 ➢ ある任意の姿勢を計算するにあたり4つの姿勢（制御点：Control Points）を用いる ➢ λは制御点の間隔で決定的に決まる係数 直接的に時刻tの姿勢R(t), t(t)を最適化するのではなく、 制御点を最適化することで時刻tの姿勢を得られるようにする 28
29. 連続時間上の軌跡の残差 ➢ 残差も連続時間上の時刻tに対して定まる ➢ ただし求めたいヤコビアンは制御点として選ばれた姿勢に関するもの ➢ これは連鎖律（Chain Rule）を用いて計算できる（姿勢に関しても同様） 実際の計算、特に回転に関する方は複雑 導出の詳細は私のGitHubで公開してます NaokiAkai/ctlio_ros2 29
30. ガウスニュートン法を用いた最適化 ➢ 一つの残差に対して4つの制御点に関するヤコビアンが求められる ➢ スキャンマッチングの時と同様にヘッセ行列と勾配を求めて更新量を計算（基本的に変わらない） ➢ ただし更新だけ各制御点毎の更新となる 制御点0 制御点1 制御点2 制御点3 30
31. 連続時間表現を用いる利点  連続時間で表現した軌跡を微分して角速度、加速度が得られる ➢ 最適化した制御点群から任意の時間の角速度と加速度が計算できる ➢ 微分により得られた角速度と加速度と、IMUが計測した加速度と角速度を同じ次元で比較できる IMUの計測値 31 IMUのすべての計測値と直接比較をすることができる
32. IMUプレインテグレーションとの比較  性質の異なる残差が得られる プレインテグレーションした 区間で1つの残差が定まる 任意時間で計測されたIMUの 計測値と直接比較できる 計測値の特性が潰れる （安定ではある） 計測値の特性を考慮しやすい 32
33. ホイールオドメトリの融合  IMUとオドメトリの特性を考慮した融合が可能 IMU角速度残差 IMU加速度残差 オドメトリ角速度残差 オドメトリ速度残差 ➢ コスト関数にはオドメトリに由来する残差も追加 IMU LiDAR Prior オドメトリ 外れ値の多いLiDAR、オドメトリ由来の残差にのみロバストカーネルを設けるなどの融合が可能 （区間で積分して移動量として観測を扱うと瞬間的な外れ値を弾くのが難しくなる） 33
34. ENWIDE（LIO+反射強度） Boreas（LIO+ドップラー速度） その他の融合 反射強度 ドップラー速度 4D LiDAR (Aeva Aeries Ⅱ) を使用 カメラは着色のみに利用 36
35. 縮退環境における融合の所感  必ず融合を行うというのが良いというわけではない ➢ そもそもLIO単体で十分な精度が出ることの方が多い ➢ ホイールオドメトリ単体の制度はLIOより圧倒的に低い ➢ ただしLIO単体で推定ができない区間があることは確か ➢ 縮退方向にのみ作用する融合をさせることが重要そう（今検証中） 37
36.  LiDAR-Inertial SLAMを実装するのに必要なLie群とLie代数の知識 ➢ Lie群を使ったスキャンマッチングの実装 ➢ IMUとの融合  最近取り組んでいる話 ➢ 軌跡を連続時間で表現するContinuous-Time LIO（CT-LIO）について ➢ CT-LIOを用いた縮退環境でのセンサ融合について おわりに 38 本日は講演の機会を頂き誠にありがとうございました