SLAMのアルゴリズムを図解｜スキャンマッチング・IMU融合・ループクロージャで点群が地図になるまで
点群が地図になるまで
— ICP・IMU融合・ループクロージャ
動画「SLAMは何をしているのか｜LiDARとIMUで点群が地図になるまでを可視化」。レーザーが点になるところから、スキャンマッチング、IMUとの融合、ドリフト、ループクローズで誤差が消える瞬間までを、実際に計算した結果の映像で解説しています。
ハンディSLAM（ハンドヘルド3Dスキャナ）は、歩くだけで現場を点群にします。では、その中では実際にどんな計算が行われているのか。本記事では、SLAMのアルゴリズムを「測る・重ねる・支える・ずれる・直す」の順に分解し、スキャンマッチング（ICP）、LiDARとIMUの融合、ドリフト、ポーズグラフ最適化によるループクロージャまでを図解します。
- SLAMは、LiDARが測る「距離と方向」だけからスキャンマッチング（ICPなど）で移動量を求め、IMUで補いながら、地図と自分の位置を同時に推定する技術です。
- 1回ごとの推定誤差は小さくても積み上がり（ドリフト）、同じ場所への再訪を手がかりにしたループクロージャとポーズグラフ最適化で経路全体を一括補正します。
- 仕組みを知ると、計測ルートの組み方・機種の選び方・点群の精度確認を、根拠を持って判断できます。
動画内の位置誤差・方位誤差・最大誤差は、シミュレーション上で実際に計算した値です（演出用の固定値ではありません）。また本記事の図のうち「本記事で計算した例」と記したものは、説明のために本記事用に別途計算したもので、動画とは条件が異なります。いずれも特定機種の性能値ではありません。
1. 機器が測れるのは「距離と方向」だけ
SLAMスキャナのLiDARは、レーザーを照射して反射が戻るまでの時間から、対象までの距離を測ります。記録されるのは「どの向きに（方向）」「どれだけ離れているか（距離）」という2つの値だけ。これを毎秒数十万点という速さで繰り返すことで、周囲の形が点の集まりとして浮かび上がります。
ここで大事なのは、この値がすべてセンサから見た値だという点です。どの点にも「センサがどこで測ったか」は含まれていません。GNSS（衛星測位）が届かない屋内やトンネルでも計測できるのは、そもそも位置を外から受け取らず、形の情報だけから位置を割り出す仕組みだからです。
2. 1スキャンだけでは地図にならない理由
LiDARが一瞬で取得する1回分の点群をスキャンと呼びます。1スキャンの原点はいつもセンサ自身です。人が歩けばセンサも動くので、0.1秒後のスキャンは、同じ部屋を少しずれた位置・向きから見たものになります。
もしセンサの位置と向き（あわせて姿勢、英語でポーズ）が分からないまま2つのスキャンを重ねると、同じ壁が別々の場所に描かれてしまいます。地図を作るには、各スキャンを「どこから・どちらを向いて測ったか」に合わせて置き直す必要があるのです。実際の3次元計測では、姿勢は位置3成分（X・Y・Z）と向き3成分（ロール・ピッチ・ヨー）の計6自由度で表されます。
3. スキャンマッチング（ICP）の反復計算
そこでSLAMは、直前のスキャンと今のスキャンの形を比べることで、センサがどれだけ動いたかを求めます。これがスキャンマッチングです。代表的な手法のひとつがICP（Iterative Closest Point）で、名前のとおり「最も近い点どうしを対応づける」処理を反復します。
- 対応点を探す：今のスキャンの各点について、直前のスキャンの中で最も近い点を探す
- 変換を計算する：対応点どうしのズレ（二乗距離の合計）が最小になる回転と並進を求める（特異値分解＝SVDで解析的に計算できます）
- 適用する：求めた回転・並進で今のスキャンを動かす
- 繰り返す：ズレ（残差）がほとんど変わらなくなるまで1〜3を繰り返す
最初は対応づけが間違っていても、動かすたびに正しい相手に近づくため、残差は反復ごとに縮んでいきます。収束したときの回転と並進こそが、その0.1秒間にセンサが動いた量です。
ICPは「初期値」に弱い
ICPは近い点どうしを対応づけるため、初期のズレが大きすぎると誤った位置に収束する（局所解に陥る）ことがあります。実機のSLAMでは、後述するIMUの予測値を初期値として与えることで、この弱点を補っています。なお、ICPのほかにも、点群を正規分布の集まりとして扱うNDT、エッジや平面といった特徴だけを使う方式など、スキャンマッチングには複数の手法があります。
4. LiDARとIMUの融合 ─ 予測と補正
LiDARは形を正確に捉えますが、1周分のスキャンが揃うのは代表的な構成で毎秒10回ほど。その間に人が歩いたり機器を振ったりすると、0.1秒の間の動きは分かりません。そこで使われるのがIMU（慣性計測装置）です。IMUは角速度と加速度を、その約20倍にあたる毎秒200回程度で計測します。
| センサ | 計測するもの | 更新の速さ（代表例） | 得意なこと | 苦手なこと |
|---|---|---|---|---|
| LiDAR | 周囲の形（距離と方向） | 毎秒約10回 | 形状が正確／長時間ずれにくい | 速い動きの間を埋められない |
| IMU | 角速度・加速度 | 毎秒約200回 | 速い姿勢変化に即応 | 積分するとすぐにずれる |
両者の関係は「予測と補正」の往復です。IMUの値を積分して次の瞬間の姿勢を予測し、LiDARのスキャンマッチングが届いたらその結果で予測を補正する。これを毎秒何回も繰り返すことで、歩行中の揺れや急な向き変えにも追従できます。IMUの情報は、1周のスキャン中に機器が動いたことで生じる点群のゆがみ（モーションディストーション）の補正にも使われます。
5. ドリフト ─ 数ミリの誤差が軌跡をねじる
スキャンマッチングとIMUの融合で求まる移動量は非常に正確ですが、1回ごとの誤差は決してゼロにはなりません。そしてSLAMは「1つ前の自分からどれだけ動いたか」を延々と足し算していく方式なので、数ミリの誤差でも積み重なるほど軌跡はねじれていきます。これがドリフト（累積誤差）です。
特に効くのが向き（方位）の誤差です。向きがわずか1°ずれると、そこから1m進むごとに約1.7cmずつ横にずれていきます。距離が長くなるほど、この「てこ」の効果で位置の誤差が膨らむのです。動画のシミュレーションでは、61.5mの歩行で位置誤差1.67m、方位誤差10.3°に達しました。
点群で「同じ壁が二重に写る」現象は、このドリフトによって同じ壁を違う場所に置いてしまった結果です。ドリフトの現場での現れ方や、観測時間との関係はハンディSLAM徹底解説｜ドリフト・クローズドループ・GCPまでで詳しく解説しています。
ドリフトの出にくさは機種でも変わります。IMUの品質、LiDARの視野と点数、カメラ（Visual SLAM）を併用するかどうかで、同じ歩き方でも結果に差が出ます。主要9機種の精度・稼働時間・価格帯を一覧で比較できます。
9機種の比較表を見る6. ループクロージャとポーズグラフ最適化
ドリフトを解消する鍵がループクロージャ（ループ閉合）です。計測中に出発点など以前通った場所へ戻ると、SLAMは今のスキャンと過去のスキャンが一致することに気づきます。「ここは前に来た場所だ」という発見が、軌跡全体を直す強力な手がかりになります。
ポーズグラフ ─ 姿勢を「点」、拘束を「ばね」として考える
ループクロージャの計算は、ポーズグラフという形で整理されます。各時点の姿勢をノード（点）、スキャンマッチングで求めた「隣どうしの相対的な動き」をエッジ（ばね）とみなし、さらに再訪で見つかった「出発点と同じ場所」という拘束を1本のばねとして追加します。
そのうえで、すべてのばねの伸び縮み（誤差）の二乗和が最小になるように、すべての姿勢を一括で置き直します。これがポーズグラフ最適化です。未知数が多く非線形な問題なので、ガウス・ニュートン法などで繰り返し解きます。その結果、最後に溜まっていた誤差は1か所に押し付けられるのではなく、経路全体に少しずつ配分されます。
動画のシミュレーションでは、同じくSE(2)（平面上の位置と向き）のポーズグラフをガウス・ニュートン法で解き、最大誤差1.669mが0.047mまで一気に整いました。この瞬間はショート動画でも紹介しています。
ショート動画「ズレた点群を一瞬で直す方法｜ループクローズ」（35秒）
「ループを閉じれば誤差はゼロになる」わけではありません。ループクロージャが直せるのは、2回の訪問の間に溜まったズレです。ループの効き目は輪の形ではなく「輪が挟み込んだ経路の長さ」で決まり、閉じても残る縮尺の誤差もあります。直線的な現場での具体的な歩き方は基準点なし・RTKなしでSLAMはどこまで正確かと、動画ループクロージャとは？｜直線現場でハンディSLAMの誤差を消す4つの歩き方で解説しています。
7. 地図と位置を同時に解くということ
ここまでの流れを振り返ると、SLAMが抱える根本的な難しさが見えてきます。位置を知るには地図（前のスキャン）が要り、地図を作るには位置が要る。この鶏と卵の関係を、測りながら同時に解き続けるのがSLAM（Simultaneous Localization and Mapping：自己位置推定と地図構築の同時実行）です。
| ステップ | やっていること | うまくいかないと起きること |
|---|---|---|
| 1. 測る | 方向と距離を毎秒数十万点 | 反射の弱い面・ガラスで点が欠ける |
| 2〜3. 重ねる | スキャンマッチングで移動量を推定 | 特徴の乏しい長い廊下・トンネルで滑る |
| 4. 支える | IMUで予測、LiDARで補正 | 急な動きで点群がゆがむ |
| 5. ずれる | 小さな誤差が積み上がる | 壁が二重に写る・始点と終点が合わない |
| 6. 直す | ループクロージャ＋ポーズグラフ最適化 | ループがないと誤差は残ったまま |
アルゴリズムを知ると、現場での注意点の理由も見えてきます。「特徴の少ない場所は苦手」「急に振り回さない」「出発点に戻って終える」――どれもSLAMの計算の仕組みから来ているのです。
8. LiDAR SLAMとVisual SLAMの違い
本記事で扱ったのは、レーザーの距離情報を使うLiDAR SLAMです。これに対し、カメラ画像の特徴点から動きを推定する方式をVisual SLAMと呼びます。どちらも「形を合わせて動きを求め、ループで直す」という骨組みは同じですが、得意な環境が異なります。
| 方式 | 使う情報 | 得意なこと | 苦手なこと |
|---|---|---|---|
| LiDAR SLAM | レーザーで測った距離と方向 | 暗所でも計測できる／寸法が正確 | 特徴の乏しい長い直線空間 |
| Visual SLAM | カメラ画像の特徴点 | 色・模様の情報を使える／センサが安価 | 暗所・逆光・模様のない面 |
近年のハンドヘルドSLAMスキャナーには、LiDAR・IMUに加えてカメラやGNSS（RTK）を組み合わせ、互いの弱点を補う機種が増えています。たとえばLiGrip O2は、LiDAR・Visual SLAM・IMU・RTKを融合するMLF-SLAMを採用しています。カメラ併用の有無が点群品質にどう影響するかはハンディSLAM徹底解説でも取り上げています。
9. 仕組みを現場で活かす ─ 機材選定と点群の処理
SLAMのアルゴリズムを理解すると、「どの機種を選ぶか」「取った点群をどう確かめるか」の判断軸がはっきりします。ドリフトに効くのはIMUとセンサ融合の質、ループクロージャを活かすのは計測ルートの設計、そして最終的な品質は点群の確認と処理で決まります。
機材を選ぶ ─ APEXが取り扱うハンドヘルドSLAMスキャナー
ほかのメーカー・機種も含めて比較したい場合は、ハンドヘルドSLAMスキャナー比較｜全9機種で精度・稼働時間・価格帯を横断的に確認できます。
点群を確かめる・共有する ─ SIMPLE-POINT
計測した点群にドリフトが残っていないか、壁が二重になっていないかは、3Dで見て断面を切るのがいちばん確実です。APEXの点群処理クラウドSIMPLE-POINTなら、ソフトをインストールせずにブラウザだけで確認できます。
14日間無料トライアル- LAS/LAZの点群をアップロードするだけで、ブラウザ上で3D表示・断面表示
- 外れ値（ノイズ）の除去、林冠下の地表面抽出をワンクリックで
- 同じ現場の2時期のスキャンを比較し、切土・盛土の体積変化を集計
- 距離・面積・体積の計測、LAS/LAZ・DXFでのエクスポート
- 登録不要で見られる期限付きの共有リンクで、発注者や現場とすぐ共有
用語集
| 用語 | 意味 |
|---|---|
| SLAM | Simultaneous Localization and Mapping。自己位置推定と地図構築を同時に行う技術 |
| LiDAR | レーザーを照射し、反射までの時間から距離を測るセンサ |
| IMU | 慣性計測装置。角速度と加速度を高頻度で測り、姿勢の変化を捉える |
| 姿勢（ポーズ） | センサの位置と向き。3次元では位置3＋向き3の6自由度 |
| スキャンマッチング | 2つのスキャンの形を合わせ、その間の相対的な動きを求める処理 |
| ICP | Iterative Closest Point。最近傍点の対応づけと変換推定を反復するスキャンマッチング手法 |
| NDT | Normal Distributions Transform。点群を正規分布の集まりとして扱うスキャンマッチング手法 |
| ドリフト | 推定の小さな誤差が積み上がり、軌跡が徐々にずれていく現象（累積誤差） |
| ループクロージャ | 以前通った場所への再訪を検出し、その一致を拘束として誤差を補正すること |
| ポーズグラフ最適化 | 姿勢をノード、相対拘束をエッジとし、誤差の二乗和が最小になるよう全姿勢を一括で解く処理 |
よくある質問
Q. SLAMとは何の略ですか？
Simultaneous Localization and Mapping の略で、「自己位置推定と地図構築の同時実行」を意味します。GNSSに頼らず、センサで取得した周囲の形から自分の位置と地図を同時に求める技術です。
Q. スキャンマッチングとICPの違いは？
スキャンマッチングは「2つのスキャンの形を合わせて相対的な動きを求める処理」の総称で、ICPはその代表的な手法のひとつです。ほかにNDTや、エッジ・平面などの特徴を使う方式があります。
Q. なぜLiDARだけでなくIMUも必要なのですか？
LiDARの更新は代表的な構成で毎秒10回程度のため、その間の速い動きを捉えられません。IMUは毎秒200回程度で姿勢の変化を測り、スキャンマッチングの初期値やスキャン中のゆがみ補正に使われます。逆にIMUだけでは積分誤差ですぐにずれるため、両者を組み合わせます。
Q. ドリフトはどのくらい発生しますか？
機種・環境・歩き方で大きく変わります。本記事の動画のシミュレーションでは61.5mで位置誤差1.67mとなりましたが、これはループクロージャなしで誤差の性質を見せるための条件です。実務での目安は基準点なし・RTKなしでSLAMはどこまで正確かを参照してください。
Q. ループクロージャを使えば基準点（GCP）は要りませんか？
ループクロージャが整えるのは点群内部の形（相対精度）です。公共座標に載せる絶対精度が必要な場合は、基準点やGNSS／RTKとの結合が別途必要です。違いは点群の相対精度と絶対精度の違いで解説しています。
Q. SLAMで取得した点群は、どうやって確認・共有すればよいですか？
3D表示で全体を見回し、壁や床の断面を切って二重になっていないかを確認するのが基本です。SIMPLE-POINTなら、LAS/LAZをブラウザにアップロードするだけで3D表示・断面・ノイズ除去ができ、期限付きリンクで関係者と共有できます。
Q. ハンディSLAMはどの機種を選べばよいですか？
見るべき軸は測距距離・相対精度・稼働時間、そしてセンサ融合の構成（カメラやRTKの有無）です。成果物に必要な精度から逆算するのが基本で、主要9機種の比較表で横断的に確認できます。実際の現場で試したい場合はデモ機の貸出もご相談ください。
SLAMは、①方向と距離だけを測り、②スキャンマッチングで重ね、③IMUとの融合で支え、④避けられないドリフトを、⑤ループクロージャとポーズグラフ最適化で直す――この繰り返しで地図と位置を同時に求めています。仕組みを理解しておくと、計測ルートの組み方や機種選定の判断が、根拠を持ってできるようになります。
APEXでは、ハンドヘルドSLAMスキャナーの販売・デモ機の貸出から、現場条件に合わせた計測計画、点群の解析・精度検証までを一貫して支援しています。
執筆・監修：APEX株式会社 3次元計測チーム
ハンドヘルドSLAMスキャナー・UAVレーザーの販売と計測、点群処理クラウド「SIMPLE-POINT」の開発・運用を行っています。本記事の図のうちICP・ドリフト・ポーズグラフの計算例は、解説のために当社で実装・計算したものです。
参考文献
- P. J. Besl and N. D. McKay, “A Method for Registration of 3-D Shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, pp. 239–256, 1992.（ICPの原論文）
- G. Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, “A Tutorial on Graph-Based SLAM,” IEEE Intelligent Transportation Systems Magazine, vol. 2, no. 4, pp. 31–43, 2010.（ポーズグラフ最適化の解説）
- 国土地理院「LidarSLAM技術を用いた公共測量マニュアル」（令和4年6月公表／令和5年9月改正）。解説はLidarSLAM技術を用いた公共測量マニュアル解説を参照。
関連記事
機材の導入・デモ機のご相談
「この現場でSLAMはどこまで使えるか」「どの機種を選ぶべきか」という段階からご相談いただけます。デモ機の貸出、実測ベースの精度検証レポートのご請求も承っています。
導入・デモを相談する先に9機種の比較表を見る →点群をブラウザで確かめる
SIMPLE-POINTなら、LAS/LAZをアップロードするだけで3D表示・断面・ノイズ除去・地表面抽出・共有まで。14日間の無料トライアルで、お手元の点群をすぐに試せます。
SIMPLE-POINTに無料登録機能の詳細を見る →本記事および動画に記載の数値は、シミュレーションまたは説明用の計算による値であり、特定機器の性能を保証するものではありません。センサの更新頻度（IMU 200Hz／LiDAR 10Hz）は代表的な構成例で、機種によって異なります。実際の到達精度は現場条件・計測設計・検証方法により変動します。