バナー

3次元LiDARとIMUを用いたSLAM/位置推定


センサ・フュージョンによる高精度追跡

高精度な位置推定の鍵は数学と信号処理技術

図1　3次元LiDARによる自己位置推定とマップ生成のようす．画像クリックで動画を見る．または記事を読む．［提供・著］赤井 直紀（名古屋大学/株式会社LOCT）
詳細：［受講無料，簡単登録］LiDAR×RTK×IMUフュージョン！自動運転＆SLAMロボット開発 セッション3

3次元LiDARとIMU，そして数学で，高速で高精度な地図生成＆自己位置推定

3D LiDAR（Light Detection and Ranging）は，自己位置推定とマップ生成（SLAM，Simultaneous Localization and Mappin）や自己位置推定（Localization）において，極めて高精度な情報を出力する注目のセンサです．

LiDARを使って周囲の環境を点群として捉えたのち，信号処理によってそれらの点群間の一致（スキャン・マッチング）を通じてロボットや車両の位置を特定します．このとき，複数の点群の間で対応する点どうしの距離を最小化する変換を求める最適化問題を解く必要があります．

この問題を解くときに，リー群とリー代数という数学的な枠組みを利用すると，姿勢角（ヨー，ピッチ，ロール）の取り扱いが容易になります．

リー群とリー代数の役割

スキャン・マッチングの最適化では，物体の回転を正確に表現するため，通常のオイラ角ではなくリー群とリー代数が用いられます．

オイラ角では，特定の角度で「ジンバル・ロック」と呼ばれる表現上の問題が発生しますが，リー群を用いることでこれを回避することができ，より正確に位置と向きを求めることが可能です．

ジンバル・ロック（Gimbal Lock）は，オイラ角で物体の姿勢を表現する際に発生する問題です．オイラ角は3つの回転角度（ヨー，ピッチ，ロール）で物体の向きを指定しますが，特定の角度において2つの軸が重なり，回転の自由度が失われる現象を「ジンバル・ロック」といいます．

これにより，物体の姿勢が予期しない制約を受け，スムーズな回転が困難になります．3D計測や制御システムにおいて，この問題は不安定な動作を引き起こす原因となるため，ジンバルロックの回避にはクォータニオンやリー群・リー代数を使った表現が利用されることが多いです．

IMUフュージョンで高精度追跡

高速移動や回転を伴うシナリオでは，LiDAR単体の計測頻度（通常，10Hz程度）では物体の位置を正確に追跡するのは難しく，IMU（慣性測定装置）とのデータ・フュージョンが必要です．IMUは加速度や角速度を連続的に取得するため，LiDARの点群データと補完しあい，高速で変動する状況でも精度の高い位置推定が可能になります．

この技術はLiDAR-Inertial Odometry（LIO）と呼ばれ，自己位置推定の高精度化に重要な役割を果たしています．

LIOの実装:拡張カルマン・フィルタ（EKF）によるアプローチ

LIOの実装では，LiDARのスキャン・マッチング結果とIMUのデータを統合し，拡張カルマン・フィルタ（EKF）を使用して推定状態を更新します．

このアプローチでは，LiDARとIMUの誤差（バイアスや速度）を補正しながら自己位置を正確に追跡できます．EKFを利用することで，予測と観測の両面から位置と速度の更新が行われ，迅速かつ安定した自己位置推定が実現されます．

LiDARを用いたSLAMでは，点群どうしのマッチング（スキャン・マッチング）が基本的な手法であり，この問題は最適化問題として解決できます．しかし，SLAMや位置推定では姿勢角が関わるため，微分計算が煩雑になり，さらにオイラ角を使うとジンバルロックの問題が発生します．リー群とリー代数を用いることで，これらの問題が簡単に解決され，スキャン・マッチングを実装できます．

LiDARの計測周期は低いため，高速に移動する物体の位置を追跡するのが難しいという課題があり，この問題に対して，LiDARとIMUの融合による方法が提案されています．〈著：ZEPマガジン〉

動画を見る，または記事を読む

著者紹介

  • 出身地：栃木県芳賀郡益子町
  • 仕事：2016年，宇都宮大学大学院博士後期課程修了，博士(工学)．その後名古屋大学未来社会創造機構で特任助教，工学研究科で助教を経験し，現在未来社会創造機構の特任准教授．2022年に株式会社LOCTを設立し，代表取締役を務める．
  • 趣味：バレーボール，サウナ，トレーニング 執筆活動：2022年コロナ社から書籍出版，他Qiitaでいくつかの解説記事を執筆

著書

  1. Reliable Monte Carlo localization for mobile robots, Journal of Field Robotics, vol. 40, no. 3, pp. 595-613, 2023.
  2. Detection of localization failures using Markov random fields with fully connected latent variables for safe LiDAR-based Automated driving, IEEE Transactions on Intelligent Transportation Systems, vol. 23, no. 10, pp. 17130-17142, 2022.
  3. Mobile robot localization considering uncertainty of depth regression from camera images, IEEE Robotics and Automation Letters, vol. 7, no. 2, pp. 1431-1438, 2022.
  4. LiDARを用いた高度自己位置推定システム- 移動ロボットのための自己位置推定の高性能化とその実装例 -, 2022年，コロナ社．

参考文献

  1. ［VOD/Pi400 KIT］SLAMロボット＆ラズパイ付き！ROSプログラミング超入門，ZEPエンジニアリング株式会社．
  3. ［VOD/Pi3A KIT］ラズパイ・キットで学ぶLinux I/Oボードの作り方・探し方・動かし方，ZEPエンジニアリング株式会社．
  4. ［VOD/Pi KIT］ラズベリー・パイで学ぶLinux＆Pythonプログラミング超入門，ZEPエンジニアリング株式会社．
  5. ［VOD/PiZero KIT］Python×ラズパイで初めての量子コンピュータ，ZEPエンジニアリング株式会社．
  6. ［VOD/Pi KIT］ラズパイ×Pythonで動かして学ぶモータ制御入門，ZEPエンジニアリング株式会社．