特殊環境ロボット向け：反射強度を使ったIntensity LiDAR速度推定

1. はじめに

自律移動ロボットにおいて，進行方向速度推定は非常に重要な技術です．一般的な自律移動ロボットの制御では，ローカルパスプランニングにて計算した目標速度と実速度を一致させるようなモータ指示値を計算します．目標速度と実速度の誤差に基づいてPID制御等にてモータのトルクや回転速度を計算することにより，ローカルパスプランニングに沿った挙動を実現します．目標速度と実速度が乖離してしまうと，ロボットと障害物がぶつかってしまったり，ロボットが進入禁止領域まで走行してしまうといったことも生じてしまいます．

一般的な自律移動ロボットは車輪速にスケールファクタと呼ばれる係数を乗じて速度を推定します．ロボットの車輪が路面をグリップしていれば，スケールファクタ補正のみで高精度な速度を推定できます．しかしながら，以下のような環境下では車輪速を用いて速度を推定することが困難となります．

• 路面$\mu$が極めて低く，車輪と路面間に過大なスリップが生じやすい環境

• 車輪速センサ自体の性能が悪く，車輪速から実速度を推定できない状況

• 土砂や水たまり等の影響により，車輪と路面間の滑りが動的に変動する環境

不整地のような特殊環境を走行する自律移動ロボットでは走行環境の影響で車輪速を取得できないことは珍しくありません．車輪速を取得できなければ，別のセンサから速度を推定して速度制御する必要があります．また，車輪速が得られる走行環境においても車輪速センサの故障やセンサとECU間の通信異常により，一時的に車輪速センサの情報を得られない場合もあります．不整地を走行するロボットは，用途上の理由で壊れにくいことが求められます．人間が入れないような特殊環境を自律移動ロボットにて調査する場合，ロボットが走行不能となってしまうと回収することができません．したがって，車輪速センサ異常時にも走行継続するための速度推定手法が必要となります．

以上の背景のもと，本記事では3D LiDARの反射強度情報を用いたIntensity LiDARオドメトリという手法を用いてロボットの実速度を推定する手法について解説します．昨今の自律移動ロボットは走行環境のマッピングや高度な空間認識のために3D LiDARを搭載する事例が増えています．Livox社のmid360等の安価な3D LiDARが普及したことにより，展示会等で3D LiDARを搭載したロボットを見かけることも珍しくはありません．「LiDARって何？」という方は既に過去の有益な解説記事が多数存在するため，そちらをご覧ください．例えば以下の記事が参考となります．

一般的なLiDARを用いた推定では，フレーム間の点群の幾何学的な形状に基づくスキャンマッチングにより相対位置と速度を推定できます．一方，本記事ではLiDARから得られる外界の幾何学形状ではなく，反射強度と呼ばれる物性に依存する信号の強さ情報を用いた推定手法について解説します．照明の影響を受けにくく，形状とは異なる情報源として反射強度のテクスチャを利用できます．自己位置推定分野では，例えば下記の論文にて反射強度情報を利用することで優れた性能を実現できることが示されています．

Guadagnino, T., Chen, X., Sodano, M., Behley, J., Grisetti, G., & Stachniss, C. (2022). Fast sparse LiDAR odometry using self-supervised feature selection on intensity images. IEEE Robotics and Automation Letters, 7(3), 7597-7604.

Pfreundschuh, P., Oleynikova, H., Cadena, C., Siegwart, R., & Andersson, O. (2024, May). Coin-lio: Complementary intensity-augmented lidar inertial odometry. In 2024 IEEE International Conference on Robotics and Automation (ICRA) (pp. 1730-1737). IEEE.

これらの手法を参考にして自律移動ロボットの速度推定向けに拡張できれば特殊環境に対しても稼働可能となることが期待できます．本記事の手法はLiDARの反射強度情報を利用して速度を推定することにより，一般的なLiDARのスキャンマッチング手法やカメラを用いた推定手法と異なるメリットを有しています．そのため、他手法と併用することで特殊環境でも速度推定を継続できる可能性があります．

本記事では，手法の概要を説明し，本手法における注意事項について述べます．ROS 2のサンプルソフトでのデモ結果を説明します．今回の記事を見た方にとって少しでも参考になる内容が含まれていれば幸いです．

2. 問題設定

本記事の問題設定に関して述べます． 下記の図が自律移動ロボットの座標系です．黒線の座標系が地図座標系であり，青線の座標系が車両座標系です．問題設定を簡単にするため，LiDARを基準としたセンサ座標系とロボットの車両座標系も同一とみなします．ここでは，車速を車体の進行方向速度とみなします．本システムでは車載されたLiDARのセンサ出力として，車両座標系における周辺物体の形状と反射強度を点群情報として取得できるとします．点群の点$p$は

p=(x,y,z,I)

として取得できます．$I$は点群の各点に付属する反射強度の値となります．以上の問題設定のもと，LiDARの点群情報を用いて車両座標系の速度を推定することを目指します．

車両の問題設定

3.速度推定手法

本記事の手法は，LiDARの点群情報を反射強度画像と呼ばれる画像に変換します．反射強度画像に伝統的な画像認識による特徴量抽出を施すことにより，移動量を推定して速度を求めます．すなわち，LiDARと伝統的な画像認識を組み合わせた方法と言えます．本章にてLiDARを用いた反射強度に基づく速度推定手法について説明します．

反射強度画像の生成

LiDARにて取得した点群に前処理を施します．具体的には外れ値除去フィルターを用いて点群のノイズを除去します．LiDARの固有特性として有効な測定距離が存在するのであれば，有効距離外の点群を除去するための距離フィルターも適用します．点群処理用のC++ライブラリのpcl等に実装されている一般的な手法で外れ値除去すれば問題ありません．

次に，前処理済の点群を反射強度画像に変換します．点群の各点$p=(x,y,z,I)$について，距離$r$と方位角$\theta$と仰角$\phi$を

r = sqrt(x^2 + y^2 + z^2)

\theta = atan2(y, x)

\phi = atan2(z, sqrt(x^2 + y^2))

と計算します．以上の値に基づき下記の図のように点群の各点群を画像座標系 $(u,v)$ に投影します．

点群の反射強度画像への変換

画像の各ピクセルの画素値には各点の反射強度$I$を設定します．ただし，反射強度は距離や入射角などの影響を受けやすいため，反射強度の値をそのまま用いて反射強度画像を作成すると速度推定精度が劣化してしまいます．したがって，反射強度画像作成時に反射強度も補正します．反射強度の前処理手法はLiDARの特性に依存しますが，例えば距離依存の粗い補正として

I_{r}=clip(Ir^{\alpha})

を施します．ここで $\alpha$ は補正係数，$clip()$ は強度の上下限クリップです．距離の大きさに基づいて反射強度を補正することにより，距離の悪影響を抑制します．もし，同一画素に複数点が入る場合は，最近傍点（最小$r$） の点を採用します．作成した画像に対して正規化処理を施し，0〜255の8bit画像に変換します．

反射強度画像が作成できたら画像前処理を施します．一般的な画像認識と同じく，画像の特徴点抽出に適したテクスチャとなるように前処理することで速度推定精度向上が見込めます．例えば，OpenCVに実装されている局所コントラスト強調や平滑化を適用すれば，特徴点検出に適した反射強度画像となります．

以上の手順により反射強度画像を作成できます．以下が点群の反射強度画像への変換例です．

LiDAR点群

反射強度画像

特徴点抽出

反射強度画像を用いてフレーム間の速度を推定します．ここでは，特徴点マッチングと呼ばれる手法を用いてフレーム間の速度を推定します．反射強度画像から特定のパターンや形状，テクスチャを抽出し，画像内の物体や構造を識別するための特徴点を検出する手法です．現時刻の反射強度画像と過去の反射強度画像間での対応点を導出することにより，移動量を推定できます．特徴点に関しては既に様々な記事で解説がなされています．例えば，以下の記事をご参照ください．

特徴点抽出とマッチングの方法として，SIFTやAKAZEと呼ばれる特徴点抽出に基づく方法が存在します．例えば，以下の記事のようにSIFTにフレーム間の特徴点をマッチングできます．

特徴点をマッチングできれば，「過去の画像に移っていたものが現時刻の画像のどこにあるか」がわかります．すなわち，反射強度画像上で特徴点をマッチングできれば1フレーム前にLiDARで検出した物体が現時刻の画像座標系$(u,v)$上でどの程度移動したかを推定できます．こちらに関しても，OpenCVに存在する特徴抽出や特徴マッチングの一般的な方法を適用すれば問題ありません．一般的な特徴点抽出とマッチングにより，1フレーム間の反射強度画像の対応点のペアを作成します．

移動量推定

特徴点の対応点ペア$(u_{prev},v_{prev}) (u_{curr},v_{curr})$ が得られたら，それぞれのフレームの特徴点に対応したLiDARの3次元点群に基づく3D画像を参照し，1フレーム前の3次元点群$P_{prev}[i]$と現在のフレームの3次元点群$P_{curr}[i]$の対応点ペアのデータ列を生成します．本手順により，LiDARの過去のフレームに存在していた3次元点群上の地物が現在のフレームのどこに存在するかがわかります．したがって，3次元点群の対応点ペア間の座標変換行列を推定すれば，車体の1フレーム間の移動量を推定できます．つまり，対応点ペアのデータ列を用いて

P_{\mathrm{curr}} \approx R\,P_{\mathrm{prev}} + t

を満たす剛体変換 $(R,t)$ を推定します．ここで $R$ は回転行列であるため、単純な線形最小二乗ではなく対応点間の二乗誤差を最小化する剛体変換推定として求解します．しかしながら，実際の対応点ペアのデータ列にはLiDARのノイズや2D特徴点の誤マッチングに起因する誤った対応点ペアが存在しえます．誤対応が混ざるため，RANSACで外れ値を除去してから最小二乗法にて座標変換行列を推定します．RANSACとは，外れ値除いて回帰分析モデルを学習させたい時に使用する手法となります．詳細は例えば下記の記事をご参照ください．

以上の手順によりフレーム間の移動量を推定できます．移動量の時間変化量を計算することにより，進行方向速度を計算できます．

後処理

フレーム間の移動量から換算した速度推定値は外れ値や過大なノイズを有しています．マッチングや投影誤差により速度推定値の波形が不連続に変化し，車速推定値にはずれ値が生じることがあります．したがって，後処理として高周波数のはずれ値やノイズを信号処理フィルタで除去します．

ここでは，以下の図のように①メディアンフィルタ，②変化率制限フィルタ，③上下限速度制限フィルタ，の3つのフィルタを推定値に対して施す．

後処理のブロック線図

ここでは，フレーム間の移動量から得た車速推定値に対してメディアンフィルタ

V_{m} [m]=                                                                
median(V_L [m],V_L [m-1],⋯,V_L [m-l])

を施します．$median()$がメディアンフィルタの関数であり，$l$がウインドウサイズです．メディアンフィルタはウインドウサイズ分の時系列データをソートして中央値を出力します．メディアンフィルタを施すことではずれ値を除去できます．1サンプル分のはずれ値であれば，ウインドウサイズが小さくてもはずれ値を除去できます．はずれ値が連続するケースではウインドウサイズを大きくすることではずれ値除去できます．ただし，ウインドウサイズを大きくしてしまうと，信号通過前後で過大な遅延が生じてしまいます． 以上の点から提案手法では連続したはずれ値を抑制するため，メディアンフィルタの出力に変化率制限フィルタ

V_R[m] =
\begin{cases}
V_m[m], & |a_L[m]| < a_{\mathrm{lim}} \\
V_m[m-1] + \operatorname{sgn}(a_L[m])\, a_{\mathrm{lim}}\, \Delta t, & |a_L[m]| \ge a_{\mathrm{lim}}
\end{cases}

a_L[m] = \frac{V_m[m]-V_m[m-1]}{\Delta t}

を施します．$a_{lim}$が最大加速度となります．ここでは，メディアンフィルタのウインドウサイズをなるべく小さめに設定し，メディアンフィルタで除去しきれないはずれ値の影響を変化率制限フィルタで抑制します．本手法によりメディアンフィルタの遅延をなるべく抑えつつ，連続したはずれ値による推定誤差を低減できます．変化率制限フィルタ適用後，上下限速度制限フィルタ

V_O[m] =
\begin{cases}
V_R[m], & |V_R[m]| < V_{\mathrm{max}} \\
\operatorname{sgn}(V_R[m])V_{max}\, , & |V_R[m]| > V_{\mathrm{max}}
\end{cases}

を適用します．$v_{max}$が最大速度となります．速度推定値が最大値以下となるように飽和処理を施すことにより，何らかの理由で生じたドリフトや異常値を抑制します．

以上の手順によりLiDARの点群を用いて速度を推定できます．下記の図が本手法における推定処理のブロック線図となります．また，推定の手順は以下のようにまとめることができます．

(1) LiDARの点群を2次元画像に投影し，各画素に反射強度を設定することで反射強度画像を作成

(2) 反射強度画像上で特徴点を検出し，フレーム間でマッチングして 2D対応点ペアを作成

(3) 2D対応点が指す画素に対応した3D画像に相当するLiDARの3次元点群を参照して，3次元の対応点ペアのデータ列を作成

(4) 3D対応点ペアを用いてRANSACと最小二乗法により，フレーム間の移動量を推定

(5) 移動量から換算した速度推定値に対して，後処理として信号処理フィルタを適用して外れ値や異常値を除去

補足・注意事項

ここでは，著者が思いつく限りの補足・注意事項に関して説明をしていきます．

ビジュアルオドメトリとの関係

ここまで読んだ読者の中にコンピュータビジョンに馴染みのある方がいれば，
「LiDARの3次元点群から反射強度画像を作成するところを除くと，伝統的なビジュアルオドメトリによる推定と同じでは？」と思われるかもしれません．ビジュアルオドメトリとは，カメラ画像を用いた自己位置推定手法の一つであり，古くから研究され体系化されています．

D. Scaramuzza, F. Fraundorfer, "Visual Odometry: Part I - The First 30 Years and Fundamentals," IEEE Robotics and Automation Magazine, Volume 18, issue 4, 2011.

D. Nister, O. Naroditsky, and J. Bergen, “Visual odometry,” in Proc. Int. Conf. Computer Vision and Pattern Recognition, 2004, pp. 652-659.

結論から言うと，本手法の中核である特徴点抽出・特徴点マッチング・外れ値除去・移動量推定は，特徴点ベースのビジュアルオドメトリと同じ方法で構成できます．違いは 「画像から3D対応点を得るための深度（距離）情報をどこから得るか」 にあります．

一般的な単眼ビジュアルオドメトリでは，2D特徴点の対応から相対的な運動は推定できます．しかしながら，メートルスケールの深度やスケールは追加情報がないと一意に定まりません．視点幾何による三角測量は可能ですが，スケール不定になりやすいです．ステレオカメラやRGB-Dを用いる場合，視差や深度センサにより深度を推定できます。また，高精度化のために学習ベースの深度推定を併用するケースもあります．ただし，キャリブレーション誤差・照明条件・テクスチャ不足などにより精度が大きく低下する場合があります．

本手法は反射強度画像の変換元がLiDARの3次元点群であり，各画素$((u,v)$ に対応する距離を同時に参照できます．言い換えると，LiDAR点群が深度（距離）情報の役割を兼ねる ため，2D特徴点の対応から3D対応点を比較的素直に構成できます．したがって本手法は，「LiDAR由来の反射強度画像を用いた，特徴点ベースのメートルスケールなオドメトリ」と整理できます。

他手法に対するメリット・デメリット

外界センサを用いて速度推定する手法として，上述のビジュアルオドメトリや一般的なLiDAR点群スキャンマッチング手法が挙げられます．一般的なLiDAR点群スキャンマッチング手法では，フレーム間の幾何学的形状を比較することで移動量推定と速度推定ができます．ここでは，他手法と本記事の手法のメリット・デメリットについて解説していきます．

一般的なLiDAR点群スキャンマッチングと本記事の手法との大きな差異は，本記事の手法が幾何学的な形状だけに依存した方法でなく，画像ベースの推定方法となる点です．したがって，以下の点がメリット・デメリットとなります．

メリット

• 点群の幾何学的形状だけに依存せず反射強度のテクスチャも用いて推定するため，トンネルや廊下のように形状が単純・反復しやすい環境でも，条件次第では劣化しにくいことがある
• 疎な特徴点である画像を用いた処理となるため，実装・計算量の設計次第では点群を用いた最適化より軽量化できる余地がある．特にCPUのみの実装では軽量化しやすい．
• 反射強度の局所パターンが安定して得られる環境では，部分的な遮蔽があっても“有効な対応点だけで”移動量推定を継続しやすい．

デメリット

• 反射強度に十分なテクスチャがない環境では特徴点が抽出できず推定が不安定になる．
• 反射強度が距離・入射角・センサ特性・自動露光的な内部処理等の影響を受けるため，補正や正規化の設計次第で環境依存の誤差が出やすい．
• 雨・霧・粉塵・水しぶき等で点群ノイズが増えると，画像投影とマッチングが崩れやすくなる．

また，一般的なビジュアルオドメトリと本記事の手法との大きな差異は，反射強度画像の変換元がLiDARの3次元点群となる点です．したがって，以下の点がメリット・デメリットとなります．

メリット

• 反射強度画像に基づくため，暗所・逆光・フリッカ等の照明変化の影響を受けにくい．
• LiDARの距離情報によりメートルスケールを直接得られるため，ビジュアルオドメトリで問題になりがちなスケール不定やスケールドリフトを避けやすい．
• 深度推定をしなくても3D対応点を構成できるため，構成がシンプルになりやすいです．

デメリット

• LiDARの角度分解能や点密度がカメラ画像より粗くなりやすいため，遠距離や細い構造物では特徴点が取りにくい．
• 反射強度が材質・濡れ・汚れ等で変動し，同一物体でも時系列で見え方が変わることがある．
• 3D-LiDARが必要となるため，センサコスト/消費電力/設置制約の面では，一般にカメラより重くなりがちです．

以上の点から，本手法はスキャンマッチングやビジュアルオドメトリの上位互換というわけではありません．反射強度テクスチャが有効な環境では有利になる一方で，テクスチャが乏しい環境や点群ノイズが大きい状況では不利になります．したがって，各手法の得意・不得意を理解したうえで用途に応じて使い分けることが重要です．計算資源に余裕があれば，スキャンマッチングやIMU等と併用し，状況に応じて重み付けやフォールバックを行うことで，特殊環境に対してよりロバストな速度推定が実現できると考えられます．

ROS 2での実験

それでは本記事の方法で推定できるかを試してみます．本記事の手法は反射強度を有する3次元点群を取得可能なLiDARであれば利用可能です．HesaiやVelodyneのLiDARであればgithub上のROS 2 driverを使ってタイムスタンプ付きの点群を取得可能です．より安価なLiDARで推定したければ，livox社のmid360の利用をお勧めします．mid360はROS 2 driverにやや癖がありますが，最近は有志の方が手順などを整備してくれているので昔より導入が容易となっています．

ただし，本記事の手法はLiDARの点群から反射強度画像を作成するという特性上，ある程度の点群密度が求められます．点群がスパースになるほど反射強度画像に含まれる特徴量が乏しくなるため，使用環境に応じてLiDARを厳選することをお勧めします．

本記事では，Velodyne VLP-32CのROS2bagデータを用いて速度推定を試みます．ROS 2 Jazzy環境でのサンプルコードを作成し，LiDARの点群を受信するたびに速度を推定するアプリを作成しました．コードはgithub上に公開しています．

今回は下記のミニPCにROS 2のソフトを実装し，ROS2bagデータを用いて速度推定性能をテストしました．下記の図が速度推定の結果となります．

速度推定結果

速度推定値は大まかに真値と一致しています．停止時や極低速時にわずかに誤差が生じていますが，後処理のフィルタで除去できない高周波数かつ低ゲインのノイズだと考えられます．本記事の手法はLiDARのみを用いた方法となります．LiDARと時刻同期したIMUを用いて極低速時や停止時の速度を補正するような運用に変更すれば，もう少し精度を改善できると考えられます．しかしながら，32ラインの比較的スパースな点群でも速度を推定できており，他の推定手法と併用すれば十分に有効活用できると考えられます．

まとめ

本記事では，車輪速が使いにくい環境でも進行方向速度を推定する手段として，3D LiDARの反射強度から反射強度画像を作り，特徴点マッチングでフレーム間の移動量を推定する手法（Intensity LiDARオドメトリ）を紹介しました．点群の幾何形状だけに依存せず，反射強度テクスチャを手がかりとして利用できる点が特徴です．また，推定値に対してメディアンフィルタや変化率制限などの後処理を入れることで，外れ値やノイズの影響を抑えられることも示しました．

ROS 2の実験では，VLP-32CのROS2bagデータでも速度の傾向が真値と概ね一致し，反射強度画像ベースの推定が一定の条件下で有効に機能することを確認できました．一方で，反射強度のテクスチャが乏しい環境や，濡れ・粉塵などで強度の再現性が落ちる状況では不安定になり得るため，本手法単独で万能というわけではありません．用途や環境に応じて，他の推定手法と組み合わせて使うことが重要です．

今後はIMUを組み合わせたよりロバストな方法も作成していきます．また，mid360を用いた事例等も作成していきますので，また機会があれば記事として公開できればと思います．