日本機械学会 Dynamics and Design Conference 2026 ／ OS9-2-1-01

差分進化法による低解像度3D-LiDARを用いた
オンライン3D-SLAMの安定化

Stabilization of Online 3D SLAM using Differential Evolution for Low-Resolution 3D LiDAR

井上一道　埼玉大学教育学部技術・情報講座 2026.8.30 – 9.2 長良川国際会議場

情報・ロボット研究室（Information and Robotics Laboratory）　lab.ir-laboratory.com

→ またはクリックで進む

本発表の構成

発表の流れと要点

導入SLAMとは
1研究背景・課題設定
2提案手法
3実験・検証
4まとめ

1章｜課題

低解像度化により既存手法の点群地図が帯状に崩れる比較図 — 低解像度化で既存手法の地図が崩れる

上半球視野では真下の路面が見えず，壁は鉛直でZの手がかりに乏しい．走るほど高さ誤差が大きくなり，地図が崩壊する．

2章｜提案

Z拘束の有無による推定Zの推移の比較グラフ — Z拘束あり（青）となし（橙）のZの推移

逐次推定ではZ=0に固定して誤差を溜めず，ループ閉じ込みのときだけ6DoFへ解放する．

3章｜結果

1.42キロメートルの走行で構築した3次元点群地図のアイソメ図 — 1.42 km走行で構築した3次元地図

Z変動幅は 6.88 → 1.22 m．XY・回転誤差も同時に改善した．

SLAMとは｜ Simultaneous Localization and Mapping

SLAM — 地図を作りながら，同時に自分の位置を推定する技術

実際のJT16走行ログから取り出した2つのスキャンをスキャンA（青緑）とスキャンB（橙）で色分けした，変換前と変換後の比較図．変換前（左）は素朴に重ねただけで同じ壁が2本にズレて見えるが，変換後（右）は相対姿勢で位置合わせした結果，壁が1本に重なる．①同じ壁の重なりを検出，②その重なりから相対姿勢を推定して自己位置とする，③スキャンBだけが観測した新しい領域を地図に追加＝地図が成長，の3段階を注記．クリックで拡大表示できる — 実データ（JT16，走行ログ）：変換前（左，同じ壁がズレる）と変換後（右，相対姿勢で位置合わせ）．スキャンを1枚ずつ順に重ね合わせて姿勢を求めるこの処理を**逐次スキャンマッチング**と呼ぶ．

自律移動ロボットが，あらかじめ地図のない環境で動作するために必要な基盤技術．
本研究は，このSLAMを2D-LiDARではなく3D-LiDAR，かつ低解像度なセンサーで実現する取り組み．

ドラッグ：回転／ホイール：ズーム

3D-LiDAR（実データ／レイヤーを順に走査）

SLAM実行中の画面．走行に従って地図が育ち，同時に自己位置（軌跡）が伸びる．

1 ｜研究背景

屋外自律移動ロボットとLiDARコストの課題

自律移動ロボットの活動範囲は，屋外を含む人との共存環境へ拡大している
屋外3D-SLAMには高垂直分解能のマルチチャンネル3D-LiDARが広く用いられる
例：Velodyne HDL-64E（64ch，垂直視野26.8°，角度分解能0.4°/ch）
一方で，高解像度LiDARはセンサーコスト・システム複雑さの点で負担が大きい
垂直チャンネル数・水平分解能を抑えた低解像度3D-LiDARによる実現可能性を探る

高解像度3D-LiDAR（Velodyne HDL-64E）による1フレーム分の密な点群をアイソメ風に描画したもの — 参考：高解像度3D-LiDAR（Velodyne HDL-64E，64ch）の1フレーム分の点群，115,384点（KITTIデータセット，Geiger et al. 2013，本研究のデータではない）

1 ｜課題設定

上半球視野センサーでは，Z方向の推定が不安定になる

MULLSによるKITTI07点群地図のXYビュー比較．左のベースライン（64層・地面あり）では建物や通路の形状が明確に再現されるが，右の地面除去+16層+水平0.6度間引き条件では点群が軌跡に沿った帯状のノイズと化し地図構造を失っている．クリックで拡大表示できる — 低解像度化した場合の地図（MULLS／KITTI07）．左：64層，右：JT16相当まで間引き．**クリックで拡大**

JT16（16ch）の垂直視野は上半球のみ（0°〜40°）．地上高1.65 mに設置し，路面を直接観測しない構成とした．
スキャン点群にZ方向の幾何学的特徴が乏しく，逐次スキャンマッチングでZ推定が不安定になる．

2 ｜提案手法

提案手法の骨子：頑健な探索 × 段階的なZ拘束

① ロバストな位置合わせ

差分進化法（DE）でスキャンを対応づける

点群が疎で手がかりが少なくても，広い範囲を探索するDEなら誤った対応（局所解）に陥りにくい．

② 段階的なZ拘束

普段はZ固定，ループ閉じ込み時だけ解放する

逐次推定ではZ＝0に固定してドリフトを防ぎ，同じ場所に戻ってきたと分かる「ループ閉じ込み」の瞬間だけ，Zも含めて柔軟に補正する．

この2つの工夫により，安価で低解像度な3D-LiDARでも安定した自己位置推定・地図構築を実現する．

ここで出てきた「差分進化法」と「ループ閉じ込み」は，このあとのスライドで順に説明する．

2 ｜提案手法（補足①）

差分進化法（DE）について

DEの1世代

初期化：姿勢の事前推定値を中心とした限られた範囲 W に，候補（個体）をランダムに配置．
変異：2個体の差 (x_r2 − x_r3) を F 倍し，別の個体 x_i に加えて新しい候補 v_i を作る．
交叉：v_i と元の x_i の成分を混ぜる．
選択：スキャンとの合致が良い方を次世代に残す．

候補どうしの差を使うため，集団が散らばっている序盤は大きく，収束するにつれ小さく動く．探索の粗さが自動で切り替わり，疎な点群でも局所解に捕まりにくい．

2 ｜提案手法（補足②）

ループ閉じ込みの概要

逐次スキャンマッチングは1枚ずつの誤差が積もるため，長く走ると始点へ戻っても位置が一致しない．
現在のスキャンを過去のスキャンと照合し，「ここは前に通った場所だ」と判定できた対応をループと呼ぶ．

その対応を拘束として加え，経路全体をつじつまが合うように調整し直す．これがループ閉じ込み．
逐次推定では戻せない誤差を一括で戻せる，数少ない機会になる．

2 ｜提案手法の核心

フロントエンドでZ=0，バックエンドで6DoF探索

システム構成図．Z拘束に関わる2箇所を強調．構成は付録1，フロントエンドの式は付録7〜10，バックエンドの式は付録11〜14．

スキャンマッチング自体は6DoFで実行し，Z=0拘束は後処理として適用．路面の傾斜・段差に柔軟対応．

PGO後はキーフレームのZが非ゼロになり得るが，その姿勢を基点に逐次Z=0拘束が継続する．

3 ｜実験設定

実験概要とロボット仕様

実験に使用した移動ロボットの外観．上部にJT16 3D-LiDARを地上高1.65mに搭載 — 実験に使用した移動ロボットの外観（JT16を地上高1.65 mに搭載）

項目	仕様
Robot	L750×W527 mm，70 kg
3D-LiDAR	JT16，16ch，垂直視野 0°–40°，最大100 m
Controller	MVC01（ジャイロオドメトリ + IMU）
PC	MacBook Pro 16（M5 Max, 128 GB）

実験条件

2026年7月4日16時30分頃，曇天下の埼玉大学構内．舗装路面，建物外周を周回するコース．人・車両通行のある環境．

3 ｜実験結果

提案手法によるオンライン3次元地図構築

埼玉大学構内を1.42キロメートル周回した際に構築された3次元点群地図と推定軌跡．色は高さZを表す．クリックで拡大表示できる — 再構築された3次元点群地図と推定軌跡（1.42 kmの走行，Z=0拘束あり）．点の色は高さ（Z）を表す．

同じ3次元点群地図を斜め上から見たアイソメ図．建物や並木の高さ構造と，赤い推定軌跡が立体的に見える．クリックで拡大表示できる — 同じ地図のアイソメ図（斜め上から）

Google Earthの3D表示で見た埼玉大学構内．オレンジの線が走行コースで，教育学部C棟・H棟や図書館，総合体育館などの建物の外周を周回している．クリックで拡大表示できる — 実環境（Google Earth）．オレンジが走行コース

1423.8 m ／ 360 ／ 59 本

走行距離／キーフレーム数／検出ループエッジ数

3 ｜ Z=0拘束の検証

Zドリフトの抑制効果

Z拘束あり（提案手法） Z拘束なし（比較対象）

全経路1.42キロメートルにおける走行距離に対するZ推定値の推移．Z拘束なしでは最大5.32メートルまでドリフトし，Z拘束ありでは変動が1.22メートル幅に収まる — 全経路（1.42 km）における走行距離に対するZ推定値の推移の比較

6.88 m

Z変動幅（拘束なし／最小−1.56, 最大5.32 m）

1.22 m

Z変動幅（拘束あり）

3 ｜ Z=0拘束の検証

同じ場所と判定したキーフレーム間に残る高さの差

Z拘束ありとなしのループエッジ分布の比較図．エッジの色は対応キーフレーム間のZ座標差を表し，拘束ありでは平均0.17メートル，拘束なしでは平均0.99メートル．クリックで拡大表示できる — 検出されたループエッジ（色は対応キーフレーム間のΔZ）．拘束あり：59本，ΔZ平均0.17 m．拘束なし：53本，ΔZ平均0.99 m．同じ場所と判定しながら1 m近い高さの差が残る．

※ SLAMの推定結果に依存しない独立ICPによる閉合誤差の評価は，
予稿原稿5節および付録8を参照．

4 ｜まとめ

本発表のまとめ

フロントエンドは6DoF DE + Z=0後処理でZドリフトを逐次抑制し，バックエンドは6DoF PGOでループ閉じ込み時に柔軟な姿勢補正を行う，段階的な拘束設計を提案した．
埼玉大学構内での1.42 km走行実験により，有効性を定量的に確認した．

6.88 → 1.22 m

Z変動幅

0.99 → 0.17 m

ループΔZ平均

付録15 参照

2.16 → 0.012 m

ICP・Z誤差

4.89 → 2.10°

ICP・回転誤差

今後の課題

ループ対応点ΔZの要因分析（推定誤差と環境要因の切り分け）．
傾斜路面環境への適用拡張．

謝辞

本研究に使用したロボットコントローラMVC01は，オリエンタルモーター株式会社より提供を受けた．また，実験に使用した移動ロボットの部品の一部は，公益社団法人スズキ財団の一般科学技術研究助成金による支援を受けた．ここに記して謝意を表する．

Appendix

付録

以降は質疑応答用の補足資料です

1システム概要（フロントエンド／バックエンド並列構成）
2関連研究（差分進化法SLAM，LiDAR慣性オドメトリ）
3低解像度化で地図構造が崩壊する
4走行コース（実環境との対応）
5低解像度化による既存手法の破綻（軌跡・Z）
6スキャンマッチング（DEとマルチスケール評価）
7評価関数①　点の分類（スキャンラインの曲率）

8評価関数②　ボクセルへの当てはまり（NDT型）
9評価関数③　マルチスケールボクセル評価
10局所精細化（点対平面ICPとHuber重み）
11ポーズグラフ最適化（エッジごとの重み付け）
12何を最小化しているのか（目的関数）
13エッジの情報行列
14ロバスト化（MADによる棄却とDCS）
15独立ICPによる閉合誤差の評価

付録1 ｜システム概要

フロントエンド／バックエンド並列構成

文献（友納，2016）を参考にRustでリアルタイム実装．両スレッドはチャンネルを通じて非同期に連携する．

付録2 ｜関連研究

差分進化法SLAMとLiDAR慣性オドメトリ

Moreno et al., 2009

ELF（Evolutive Localization Filter）

差分進化法（DE）を占有グリッドSLAMに適用．
対象は2D屋内環境．
3D点群スキャンマッチングへの適用は未検証．

Xu & Zhang, 2021

FAST-LIO

密結合カルマンフィルタによる高速LiDAR慣性オドメトリ．
100〜250 Hzの高周波IMUを前提とした運動補償．
本研究は30 Hz程度の低周波IMUを想定し，厳密な運動補償は行わない．

本研究

低解像度3D-LiDAR × DE

DEを低解像度3D点群の逐次スキャンマッチングへ拡張．
フロントエンド・バックエンド双方にDEを適用．
ジャイロ統合オドメトリの増分をDEの初期推定値として付与．

付録3 ｜課題設定（地図構造の崩壊）

低解像度化で地図構造が崩壊

64層・地面あり

16層・地面除去

ベースライン（64層・地面あり）では，建物・通路の形状が点群地図として明確に再現される．
JT16相当のスパース性（地面除去+16層+水平0.6°）に間引くと，点群は軌跡に沿った帯状のノイズと化し，地図としての構造を失う．

付録4 ｜走行コース

走行コース — 実際のキャンパスとの対応

この図の見かた

オレンジの線が実際に走行した1.42 kmの経路．教育学部C棟・H棟，図書館，総合体育館などの外周を周回する．
前スライドの点群地図と同じ範囲．建物の並びや中庭の抜けが対応して見える．

Google Earth で開く ↗

付録5 ｜課題設定（軌跡・Zの定量的破綻）

低解像度化による既存手法の破綻

MULLSにKITTI07データを地面点除去・16層化・水平0.6度間引きで低解像度化して入力すると，本来150メートル四方程度のループ軌跡が-1500メートルまで暴走し，Z推定も-1000メートルまで発散する図．ベースライン（64層・地面あり）はほぼ平坦なまま — MULLS（原著コード）／KITTI07に地面点除去・16層化・水平0.6°間引きを施し，JT16相当のスパース性を模擬（10Hz条件）．左：上面軌跡，右：走行距離に対するZ．

993.56 m

Z変動幅（間引き後）

2029.79 m

始点終点ギャップ（間引き後）

3.62 m

Z変動幅（ベースライン，64層・地面あり）

付録6 ｜スキャンマッチング

1スキャンを合わせるまでの流れ

step 1 ｜前処理

傾き補正と点の分類

スキャン始端・終端のIMU姿勢（roll/pitch）を線形補間して傾きを補正．
スキャンラインの曲率でエッジ点・平面点に分類．

→ 付録7

step 2 ｜大域探索

差分進化法（DE）

6DoFの姿勢変化量を最適化変数として解く．
初期値は前フレーム姿勢に，ジャイロ統合ホイールオドメトリのXYA増分を合成した値．
評価関数はマルチスケールボクセルマップ．

→ 付録8・9

step 3 ｜局所精細化

点対平面ICP

DEの解を初期値に，Gauss-Newton法で4反復．
Huber重みで外れ値の影響を抑える．

→ 付録10

大域探索で当たりを付け，局所精細化で詰める二段構え．フロントエンドの逐次処理とバックエンドのループ検出の双方で同じDEを使う．

付録7 ｜評価関数

点の分類

チャンネルごとの点列で，前後 $N$ 点との差の総和から曲率 $c$ を求める（LOAMと同様）．

c = \frac{1}{2 N ∥ p_{i} ∥} ∥ \sum_{j = - N, j \neq 0}^{N} (p_{i} - p_{i + j}) ∥

edge

エッジ点

$c > 0.05$ ．建物の角や柱など，方向が急に変わる場所．

planar

平面点

$c < 0.01$ ．壁や路面など，滑らかに続く場所．

other

通常点

上記以外．マッチングには使うが，特徴点としては扱わない．

前後 $N = 5$ 点を使用．分母の $∥ p_{i} ∥$ により，遠方の点ほど点間隔が広がる影響を正規化している．

付録8 ｜評価関数

ボクセルへの当てはまり — NDT型の個別コスト

地図はボクセルマップとして保持し，各ボクセルに点群の重心 $μ$ と共分散行列 $Σ$ を記録する．候補姿勢で変換したスキャン点 $p$ のコストは次式で定義する．

s (p) = 1 - \exp (- \frac{1}{2} {(p - μ)}^{⊤} Σ^{- 1} (p - μ))

NDT（Normal Distributions Transform）と同等の枠組み．ボクセル内の分布形状（平面状・線状）が反映されるため，点が少なくても面としての当てはまりを評価できる．値が低いほど良い適合．

s_{f} (p) = α_{f} \cdot s (p), α_{f} = {\begin{cases} 0.7 & フィーチャ点マッチ \\ 1.0 & それ以外 \end{cases}

feature bonus

同種の特徴どうしを優先する

スキャン点がエッジ点・平面点で，対応ボクセルも同種の特徴を持つ場合にコストを0.7倍する．対応ボクセルが無い場合は $s_{f} = 1.0$ （最大コスト）．

付録9 ｜評価関数

マルチスケールボクセル評価

c = {\begin{cases} 0.8 s_{0} + 0.2 s_{1} & L0・L1 両方ヒット \\ s_{0} & L0 のみヒット \\ 0.1 + 0.9 s_{1} & L1 のみヒット \\ 0.3 + 0.7 s_{2} & L2 のみヒット \\ 1.0 & いずれもミス \end{cases}

3 layers

3段階の解像度

L0：基本解像度 $r_{vox}$
L1： $3 r_{vox}$ 　／　L2： $9 r_{vox}$

L0を優先して細かく合わせつつ，点が無い領域ではL1・L2へ段階的にフォールバックする．粗い層ほど下駄（0.1・0.3）を履かせ，細かい層で合ったほうが必ず有利になるようにしてある．

低解像度LiDARでは1フレームの点が疎で，単一解像度だと「どのボクセルにも当たらない」点が増え，評価関数が平坦になって探索が効かなくなる．Multi-Layered NDT の考え方に基づく．

付録10 ｜局所精細化

点対平面ICPとHuber重み

DEの大域探索の結果を初期値とし，Gauss-Newton法で精密化する．姿勢による変換を $T (p_{i}) = R p_{i} + t$ とする．

r_{i} = {\begin{cases} n_{i}^{⊤} (T (p_{i}) - μ_{i}) & 平面性 > 0.1（点対平面） \\ ∥ T (p_{i}) - μ_{i} ∥ & それ以外（点対重心） \end{cases}

E (T) = \sum_{i} ρ_{H} (r_{i}), ρ_{H} (r) = {\begin{cases} \frac{1}{2} r^{2} & (| r | \leq k) \\ k (| r | - \frac{1}{2} k) & (| r | > k) \end{cases}

Huber重みにより，残差の大きい対応（動く物体・誤対応）が二乗で効かなくなり，外れ値の影響が抑えられる． $k = 0.1$ m，Gauss-Newton更新を4イテレーション．

付録11 ｜ポーズグラフ最適化

エッジごとに信頼性を変える

一周して最初の通路へ戻ってくると，離れた時刻のキーフレーム同士が空間的に近づく．そこに張るのがループエッジで，線の太さが最適化での重み（情報行列の大きさ）．破線は最適化に入る前に捨てたエッジ．

→ 付録12　目的関数　／　→ 付録13　重みの決め方　／　→ 付録14　ロバスト化の二段階

付録12 ｜ポーズグラフ最適化

ポーズグラフ最適化の目的関数

全キーフレームの姿勢 $X = {x_{1}, \dots, x_{N}}$ を，全エッジの誤差の重み付き二乗和が最小になるように解き直す．

\hat{X} = \underset{X}{\arg \min} (\sum_{(i, j) \in E_{odo}} e_{ij}^{⊤} Ω_{ij}^{odo} e_{ij} + \sum_{(i, j) \in E_{loop}} s_{ij} e_{ij}^{⊤} Ω_{ij}^{loop} e_{ij})

terms

各項の意味

$e_{ij}$ ：エッジ誤差．観測した相対姿勢と，推定姿勢から計算される相対姿勢との食い違い（6次元）．エッジの主張どおりの位置関係なら0になる．
$Ω_{ij}^{odo}$ ・ $Ω_{ij}^{loop}$ ：そのエッジの重み（情報行列）．種類ごとに与え方が違う．

edge set

2種類のエッジを同時に釣り合わせる

和を分けて書いたが，同じ目的関数を1回で解く．ループエッジだけを信じて強引に閉じるのではなく，オドメトリの拘束と折り合う姿勢に落ち着く．
$s_{ij}$ はDCSによる減衰で，ループエッジにだけ掛かる（オドメトリエッジは $s_{ij} = 1$ ）．
解法は，疎行列のCholesky分解に基づくLevenberg-Marquardt法．

→ 付録13　重み $Ω$ の決め方　／　→ 付録14　誤検出の棄却と，減衰 $s_{ij}$ の決め方

付録13 ｜ポーズグラフ最適化

エッジの情報行列

エッジ誤差 $e_{ij}$ の重み付き二乗和 $\sum_{ij} e_{ij}^{⊤} Ω_{ij}^{'} e_{ij}$ を最小化する．重み $Ω$ はエッジの種類ごとに与える．

Ω^{odo} = diag (\frac{1}{{σ_{t} (∥ t ∥)}^{2}}, Ω_{y}, Ω_{z}, Ω_{roll}, Ω_{pitch}, \frac{1}{{σ_{r} (∥ t ∥)}^{2}})

オドメトリエッジ：動いた量 $∥ t ∥$ に応じて $σ_{t} = σ_{t, 0} + k_{t} ∥ t ∥$ と線形に信頼度を下げる．横滑り（Y）・鉛直（Z）・Roll・Pitch は，非ホロノミック拘束と平面走行の仮定から常に高信頼度の固定値にする．

Ω^{loop} = ω_{0} \max (\sqrt{c_{s} (s) c_{m} (m)}, c_{\min}) I_{6}

ループエッジ：マッチングスコア $s$ と対応点数 $m$ から求めた信頼度で等方的に重み付ける． $c_{\min}$ は下限値で，情報行列が過度に小さくなるのを防ぐ．

付録14 ｜ポーズグラフ最適化

ロバスト化

stage 1 ｜ before PGO

MADによる外れ値の棄却

s > median (s) + 4.685 MAD

全ループエッジのスコア分布から中央値と中央絶対偏差を求め，この条件を満たすエッジをPGO前に捨てる．係数4.685はTukey biweightの定数．中央値ベースなので，外れ値自体に引きずられにくい．

stage 2 ｜ inside PGO

DCS（動的共分散スケーリング）

Ω_{ij}^{'} = s_{ij} Ω_{ij}, s_{ij} = \min (\frac{2 ϕ}{ϕ + χ_{ij}^{2}}, 1)

残差 $χ_{ij}^{2} = e_{ij}^{⊤} Ω_{ij} e_{ij}$ が大きいエッジほど $s_{ij}$ が小さくなり，寄与が連続的に抑えられる．ループエッジにのみ適用（オドメトリエッジは $s_{ij} = 1$ ）．

棄却しきれなかった誤検出が残っても，DCSが残差に応じて自動的に効きを弱める．最終的な重み $Ω_{ij}^{'}$ を使い，疎行列のCholesky分解に基づくLevenberg-Marquardt法で姿勢グラフ全体を最適化する．

付録15 ｜独立ICPによる閉合誤差

始点↔終点での位置合わせ評価

条件	ΔZ [m]	XY誤差 [m]	回転誤差 [deg]
Z拘束あり	0.012	0.493	2.10
Z拘束なし	2.16	1.39	4.89

走行の始点・終点で取得した生スキャン同士を点対平面ICPで直接照合し，閉合誤差を求めた．オンライン推定の結果を一切使わないため，SLAM内部の自己評価とは独立した指標になる．全経路1.42 km．

interpretation

Z以外の誤差も同時に改善する

Z拘束はZのみを制約する操作であるにもかかわらず，XY誤差は 1.39 → 0.493 m，回転誤差は 4.89 → 2.10 deg と改善している．
高さが不確かなまま逐次推定を続けると，マッチングは誤った高さのもとで水平方向・姿勢角を合わせにいくため，誤差がZ以外へ波及する．
Z方向の推定不安定性は，Zだけの問題にとどまらず姿勢推定全体を悪化させていたことを示唆する．

差分進化法による低解像度3D-LiDARを用いたオンライン3D-SLAMの安定化