天井ナビゲーション

業界の痛点を直撃⇒天井ナビゲーション

室内移動搬送ロボットは、複雑な環境変化において多くの課題に直面しており、特に高ダイナミックな変化のある環境では、貨物の移動などによる環境特性の変化がSLAMナビゲーションにとって、安定性と正確性を著しく低下させることがあり、自己位置が失い、ナビゲーションが継続できないことがよくあります。例えば、回転率が非常に高い大型倉庫では、頻繁に変動する貨物、設備、そして人が運転するフォークリフトも行き来するため、環境特性の安定性に影響を与えることが大きくなります。多くの企業はSLAMナビゲーション用のLidarを貨物より上に設置することで安定した点群マップの中で自律走行できるように工夫していますが、Lidarの位置を高くすることで、地面の凹凸による少しの振動でナビゲーション精度を大きく影響してしまいます。また、貨物が天井近くまでに積まれた現場では時よりLidarの高さが4m以上高くなることもあります。

(住友倉庫のサイトより引用)

月光の天井ビジョンナビゲーション技術は、幅広い適用性を持ち、このような室内シーンの現場に対応できます。天井ナビゲーション技術を搭載した月光ロボットは、複雑で動的に変化する環境に直面した際に、高効率で正確なトップビジョンによる位置決めナビゲーションを実現し、移動ロボットの作業の信頼性と安定性を大幅に向上させます。

多次元環境認識と迅速な展開
天井の環境特性は比較的安定しており、変化しにくいことを考慮して、月光の天井ナビゲーションアルゴリズムは、360°照射可能なLidarで天井をスキャンをし、三次元マップを構築します。天井にマーカーを追加する必要は一切ありません。AIアルゴリズムを通じて、センサーデータを深く融合し、環境内のさまざまな特徴点を自動的に識別し、バーチャルリアリティのデジタル再現を行います。動的に変化が頻繁な工場や物体の遮蔽が多い倉庫において、天井ナビゲーションシステムは高いロバスト性を持つ環境認識と位置決めサービスを提供し、複雑な環境下でロボットの走行の安定性を確保します。

スマートな特徴抽出と姿勢キャリブレーション
部分的な環境特性があまり明確でない上部シーンに直面しても、深層学習アルゴリズムにて、上部環境特性を抽出し、ロボット自身の姿勢を常時キャリブレーション実施し、常に環境の中での自己位置を特定できます。天井ナビゲーションの位置決めは、位置精度が±1cmに達し、レーザーから天井の距離は最大60メートルに対応しています。このナビゲーション方式は、静的なシーンにうまく適応できるだけでなく、動的に変化する環境にも対応できるため、地図情報のリアルタイム更新と正確性を確保します。工場の作業場や倉庫の棚においても、このシステムは効率的に機能します。

イメージ

ナビゲーション用Lidar
月光ロボットの天井ナビゲーションに使用しているは128ライン3Dライダー、JT128である、

https://www.hesaitech.com/product/jt128/

低消費電力、高信頼性のナビゲーション用の3Dライダーで、360°天井を常時スキャンし、VFOVは187°

アルゴリズム

1：SLAMのフレームワーク

SLAM（Simultaneous Localization and Mapping）アルゴリズムのフレームワークは、フロントエンドのデータ関連とバックエンドの最適化の2つの部分で構成されています。下の図に示されているように、フロントエンドではレーザー点群データに基づいて、スキャンマッチングを使用してフレーム間の位置合わせを行い、ループクロージャ検出によって異なるフレーム間の点群データの関係を取得し、位置推定を継続的に更新し、対応する地図情報を保存します。バックエンドでは、フロントエンドで得たロボットの姿勢と観測制約を維持および最適化し、構築された地図の最適化とロボットのリアルタイムの姿勢を得ます。

月光ロボットではJT128にLOAM（LiDAR odometry and mapping）アルゴリズム(現在最も代表的な3DレーザーSLAMアルゴリズムの1つである）を組み込みました、このアルゴリズムは、SLAMをオドメトリ算法とマッピング算法に分割し、高速で高精度、ロバスト性があり、計算コストが低いという利点があります。また、LOAMの特徴マッチングを最適化し、ループクロージャの検出を追加して、最適化したアルゴリズムは計算リソースの使用を減らしながらも同等またはそれ以上の精度を実現しています。

2：フロントエンド

フロントエンドにはセンサー信号の処理と画像処理が含まれる

・Lidar Toolbox™とNavigation Toolbox™を使用して、2D/3D LiDARの処理とスキャンマッチングを行います。

・3D点群データの処理と点群データのレジストレーションを行います。

・フィーチャーバッグを使用してループクロージャの検出を行います。

・深層学習を使用して物体検出やセマンティックセグメンテーションを行います。

・Automated Driving Toolbox™を使用して、3D LiDARポイントクラウドを含む地図を生成します。

・Sensor Fusion and Tracking Toolbox™を使用して、センサーフュージョンを位置推定や複数目標追跡に使用します。

2-1：スキャンマッチングアルゴリズム

スキャンマッチングアルゴリズムの精度と計算効率は、SLAMアルゴリズムの軌跡推定と地図構築の正確性に直接影響します。これにより、マップの最適化においてロボットの状態の初期値や制約関係が提供されます。スキャンマッチングアルゴリズムは、主に以下の3つのタイプに分類されます：ICP（Iterative Closest Point）に基づくアルゴリズム、幾何学的特徴に基づくアルゴリズム、数学的特徴に基づくアルゴリズムです。月光ロボットはフロントエンドに、深層学習を用いた3DレーザーSLAMアルゴリズムを追加しました。

2-2：ICP（Iterative Closest Point）に基づくマッチングアルゴリズム

ICP（Iterative Closest Point）アルゴリズムは、距離誤差を最小化することを目的としており、反復的な手法を用いて隣接するスキャンフレーム間の点の最適な変換関係を見つけます。クラシックICPアルゴリズムの局所最適解の問題を解決しましたが不十分ではないので、月光ロボットはこのICPアルゴリズムを改良し、点の選択、マッチング、重み付け、誤った関連の削除、またはコスト関数の最小化など、最適化を行いました。PP-ICP（点対面ICP）、PL-ICP（点対線ICP）、GICP（一般化ICP）、NICP（標準ICP）など、3D LiDAR SLAMに適した改良アルゴリズムを開発し適応させました。PP-ICPアルゴリズムでは、誤差コストとして点と面の距離を使用します。一方、PL-ICPアルゴリズムでは、実際の状況により適した点と直線の距離を誤差コストとして利用します。GICP（Generalized ICP）は、ICPアルゴリズムとPL-ICPアルゴリズムを組み合わせ、確率的なフレームワークで点群のレジストレーションを行い、適用性と精度を向上させます。一方、NICP（Normal ICP）は、環境の曲面の法線ベクトルと曲率を考慮して精度を向上させます。IMLS-ICP（Iterative Closest Point with a Non-Stationary Least Squares）は、各点に重みを割り当て、代表的な点を選択してマッチングを行います。また、目標点に基づく局所的な曲面法線を使用して、IMLS法を用いて曲面を再構築します。BLAM（Bayesian Localization and Mapping）は、GICPアルゴリズムを使用して2つの点群データの姿勢変換を初期計算し、現在のフレームに対応する最も近い点を取得し、再度GICPアルゴリズムを使用して正確な姿勢データを取得します。これにより、マップの精度が向上し、ループクロージャ検出が行われます。また、LiTAMINアルゴリズムは、対称的なK-L（Kullback-Leibler）ダイバージェンスを、ICPのコスト関数に導入して、2つの確率分布の差異を反映させています。これにより、点群のレジストレーションの点の数が減少し、処理速度が向上します。また、K-Dツリー（K次元ツリー）を使用してボクセルグリッドマップを構築することで、レジストレーションの精度を保ちながら、アルゴリズムの正確性と頑健性を向上させます。ICPとその派生アルゴリズムは、収束を実現するために多くの反復が必要であり、月光ロボットの天井ナビゲーション技術はこの辺りも巧妙に処理しています。
月光ロボットはナビゲーション用Lidarに従来のICPアルゴリズムを実装し、さらに正面には環境検知用Lidarを装着し、カラーICPアルゴリズムも実装しました、点群データの取得のみならず、カラーSLAMによる環境の識別も加えました。

正面の環境識別と天井の環境識別にてマルチSLAMで環境を綿密にスキャンをし重ねることろを自動合成しながら、天井と目の前の環境で組み合わせた立体的な空間を識別し、精度の高い環境情報を取得しています。

2-3：幾何特徴に基づくマッチングアルゴリズム

LOAM（Lidar Odometry and Mapping）は、局所的な平滑性を利用して、現在のフレームとマッチングフレームのエッジと平面の特徴を抽出し、現在の点群データの点をマップのエッジと平面とマッチングさせるアルゴリズムです。月光ロボットはこの幾何特徴に基づくマッチングアルゴリズムをより良いパフォーマンスを実現するため、LeGO-LOAMの特徴抽出の前に天井を分離する最適化を行い、R-LOAMは参照オブジェクトから抽出された天井の特徴を組み合わせることで、長期的なドリフトを効果的に減少させています。これらのアルゴリズムのソフトウェアシステムの概要図は、下記の図のようになります。ただし、R-LOAMは3Dグリッドと参照オブジェクトの位置合わせの精度要件が高いのと、幾何特徴に基づくマッチングアルゴリズムは、点群変換関係の事前情報が不要で、特定の形状の幾何特徴を反復的に解いて計算するため、演算能力が高いGPUの実装が必要になります。その代わり、リアルタイム性が向上し、高いマッチング精度を持っています。

LeGO-LOAMのアルゴリズムのシステム概要図

Surfel(Surface Elements as Rendering Primitives）は点ベースのレンダリング方法であり、3D形状をポリゴンメッシュではなく、点群を使用してレンダリングします。ただし、GPUの処理が必要です。月光ロボットは、面の特徴を使用して、連続した3Dレーザースキャンとインクリメンタルに構築された多解像度グリッドマップを融合させました。測定の疎な性質やグリッドの離散化における正確性を実現するために、面を確率的にガウス混合モデルに割り当て、アルゴリズムの正確性と効率性を向上させました。SLAMの手法であるElastic-Fusionの処理方法には、SuMaやElastic-LiDAR Fusionがあります。これらの手法は、レーザー点群データの形状を面で近似し、面の特徴を利用してレーザーオドメトリを実現します。

幾何特徴に基づくマッチングアルゴリズムの処理前と処理後の点群データ

2-4：数学的特徴に基づくマッチングアルゴリズム

数学的特征に基づくアルゴリズムは、スキャンデータやデータフレームの姿勢変化を表現するために、さまざまな数学的性質を使用するマッチング手法です。月光ロボットは、正規分布変換（NDT）に基づくマッチングアルゴリズムを使用しています。NDTアルゴリズムは、高速な空間モデリング技術であり、簡潔な3Dポイントクラウドモデルを構築するために良く使用されます。このアルゴリズムは、配准の過程で正規分布を使用してキーフレームを表現し、グローバルな姿勢のセットを得ることができます。大量の点群データに対して、3次元NDT（3-dimension NDT）アルゴリズムでは、ONDT（occupancy NDT）アルゴリズムで多次元正規分布変換の地図表現ができ、低いメモリ消費量で高い精度と高速な処理速度を実現しました。RGC-NDT（regional growth clustering normal distribution transformation）アルゴリズムは、クラスタリングアルゴリズムを使用して、詳細な自然特徴を捕捉します。月光ロボットではこれらのアルゴリズムを最適化をし、従来のNDTアルゴリズムと比較して、マッチングの精度が約2倍向上し、マッチングにかかる時間は1/3以下になりました。

2-5：深層学習を用いた3DレーザーSLAMアルゴリズム

深層学習を用いた方法は、データ駆動型の学習により、手動設計よりもより正確なモデルを得ることができます。SLAMと深層学習の組み合わせは、深層学習を伝統的なSLAMフレームワークの一部または複数のステップに適用することで、SLAMアルゴリズムの効率性、精度、および頑健性を向上させることを目的としています。また、セマンティックSLAMでは、深層学習を使用して意味情報を抽出し、意味地図を構築することで、ロボットの自己理解能力を向上させることができます。後述するバックエンドのアルゴリズムは、線形フィルタリングから非線形最適化に進化してきました。特に、ファクターグラフ最適化とスライディングウィンドウの組み合わせは、リアルタイム性と頑健性の良さから、さまざまな融合位置推定やマッピングシステムで広く利用されています。

フレーム間推定

深層学習を用いたフレーム間推定手法は優れた性能を持っています。大きく分けて2つあります。

(1) 点群レジストレーション方法

GMM(Gaussian mixed model)、3DFeat-Net、FCGF(fully convolutional geometric features)、Siamese-PointNet(Siamese point network)、UGMMReg(unified Gaussian mixture model registration)；

(2) エンドツーエンドの深層学習を用いた点群レジストレーション方法

DeepGMR(Deep Gaussian mixture registration)、3DRegNet(3D registration network)、MLP_GCN(multilayer perceptron-graph convolutional networks)、IPCR(iterative point cloud registration)。

月光ロボットは伝統的な数学理論と深層ニューラルネットワークをエンドツーエンドのフレームワークに組み合わせて、高精度かつ効率的な方法でフロントエンドに深層学習を実装しました。

3：バックエンド

バックエンドでは、2D/3Dの姿勢グラフを使用

・Navigation Toolboxを使用して、2D/3Dの姿勢グラフを生成します。

・ノードとエッジの制約に基づいて姿勢グラフを最適化します。

・Computer Vision Toolboxを使用してバンドル調整を行います

・SLAM地図生成ツールを使用して、占有格子地図を生成

・ループクロージャを探し、修正し、地図を占有格子としてエクスポートしてパスプランニングに使用します。

・Navigation Toolboxを使用して、パスプランニングアルゴリズムを実装しパスプランニングと制御を行う

・制御指令を送信して計画されたパスに従い、障害物を回避します。

・Parallel Computing Toolboxを使用して、計算集約型のプロセスを並列実行し、処理速度を向上させます。

3-1：フィルタリング推定に基づくSLAMアルゴリズム

フィルタリング理論に基づくSLAMアルゴリズムは、ベイズ推定の原理を利用しています。具体的には、カルマンフィルタをベースとしたSLAMアルゴリズムと、パーティクルフィルタをベースとしたSLAMアルゴリズムに分けられます。拡張カルマンフィルタに基づくSLAMアルゴリズムは、非線形の運動モデルと観測モデルを線形化処理してSLAMを実現しています。月光ロボットでは、非線形システムを無追跡変換で処理する無追跡カルマンフィルタ法を加えました、その精度が拡張カルマンフィルタ法よりも高い効果が得ることができました。粒子フィルタの手法は、一定数の重み付きの粒子を使用して事後確率分布を表現し、非線形非ガウスのシステムに適用することができます。また、RBPF（Rao-Blackwellized particle filter）アルゴリズムで、ロボットの位置補正と位置推定の問題をそれぞれ拡張カルマンフィルタと粒子フィルタを使用して解決し、アルゴリズムの計算複雑さを大幅に低減しました。さらにFast-SLAMを用いることで、地図の精度を向上させることができました。センサから得た情報を適応的リサンプリング技術を使用して適切な粒子の種類を維持するためにgmapping手法で、従来の方法よりも1桁少ない粒子数で高精度の地図を生成することが可能となり、ナビゲーション精度が10mmを実現しました。

3-2：非線形最適化を基にしたSLAMアルゴリズム

グラフ最適化アルゴリズムフレームワークは、現在最も広く使用されているSLAMのフレームワークの一つです。グラフ最適化アルゴリズムフレームワークは、精度を保証しながら計算量を大幅に削減するため、重要なノードを維持することで最適化を行います。下図に示されているように、グラフ最適化に基づくアルゴリズムは、最小二乗法、緩和技術、確率的勾配降下法、および多様体最適化の4つに分類されます。複数のセンサーデータを統合してローカルなサブマップを作成し、サブマップマッチングアルゴリズムを使用してグローバルな最適化を行うことができます。2Dおよび3Dの点群マップを構築することができ、高い効率と堅牢性を持っています。

4：ループクロージャ

従来のループクロージャーでは、主にマニュアルによって特徴が選択されますが、精度が低く計算量も多いです。一方、深層学習は大量の学習データと適切なネットワーク構築により、分類に関係のない要素に対して感度が低く、高い識別力を持つ特徴を効率的に抽出できます。月光ロボットでは、深層学習を基にしたループクロージャーのアルゴリズムを改良し、PointNetVLAD（PointNetとNetVLADの組み合わせ）、PointNetを使用して局所特徴を抽出し、NetVLADを使用してグローバル特徴を集約し、3D点群からエンドツーエンドでグローバルディスクリプタを抽出します。DH3D（deep hierarchical 3D）は、元の3Dポイントクラウドから局所特徴を学習し、その特徴を採取機構を使用してグローバル特徴ディスクリプタに集約します。代替点群表現とネットワークアーキテクチャに基づくグローバル点群ディスクリプタの抽出手法であるMinkLoc3Dを使用しています。ピラミッド点群トランスフォーマーネットワーク（PPT-Net）のトランスフォーマーモジュールを使用して局所特徴の識別を強化し、グローバルディスクリプタを生成します。
FastLCD（高速でコンパクトなループクロージャー検出手法）は、元の点群スキャンから直接多モーダル特徴を抽出し、グローバルな総合ディスクリプタにエンコードします。また、回転不変性を持っています。NDT-Transformerは、大規模な点群に対して、軽量化されたNDTユニットのグローバルディスクリプタを学習し、ループクロージャーを検出します。局所3D深度ディスクリプタに基づくループクロージャー検出手法手法で、新しいオーバーラップ度計算方法を使用してループクロージャーを検出し、より高い精度を実現しています。Overlap-Transformerは、軽量なTransformerニューラルネットワークであり、深度画像を使用してグローバルディスクリプタを構築し、ループクロージャーを検出します。画像特徴記述子（JT128内蔵）の驚異的な性能と高い技術により、画像ベースのループクロージャー検出は超高速で、長期的に安定した位置推定システムを構築し、ループクロージャ検出を実現することができました。
閉ループを検出はセマンティックオブジェクトを使用し、セマンティックオブジェクト間の空間関係をエンコードして、グローバルおよびローカルなディスクリプタを構築し、検出を行います。3Dレーザーデータを使用したセマンティック情報とスキャンコンテキストを統合したグローバルディスクリプタSSC（セマンティックスキャンコンテキスト）で3段階の検索手法を使用して閉ループ検出を実現しました。

5：レーダーと慣性計測ユニットの融合

レーダーと慣性計測ユニット（IMU）の融合は、SLAM式ナビゲーション技術において、低い垂直分解能、低い更新速度、運動による歪みなどの問題を克服することができます。レーダーとIMUの融合には、タイトカップリングとルースカップリングの2つの方法があります。キャプリングは、すべての測定データを統合最適化して、複雑な変動する環境で正確な姿勢推定を得る方法ですが、計算量が多く、リアルタイム性に課題があります。ルーズカップリングは、各センサーの測定データを個別に処理し、それを統合する方法で、計算量が少なく、実装が容易ですが、位置推定の精度に制約があります。

月光ロボットのLeGO-LOAMを基にしたLIO-SAM（LiDAR inertial odometry via smoothing and mapping）は、IMUのプレインテグレーション因子、レーザーオドメトリ因子、GPS因子、およびループ検出要素を因子グラフフレームワークに統合し、より高い精度、リアルタイム性、および低いドリフトを得ることができます。固体レーザーレーダーとIMUの組み合わせについて研究し、リアルタイムなLiDAR慣性オドメトリとマッピングに使用される新しいセンサーフュージョン手法であるLiLi-OM（Livox LiDAR odometry and mapping）を環境を検知するセンサーに適応させました。