つくばチャレンジ選択課題Cにおける、3D LiDARのみを用いた経路封鎖看板の検出アルゴリズム
この記事では、3D LiDARの反射強度情報と点群処理を用いた経路封鎖看板の検出方法について解説します。本手法は、「つくばチャレンジ」で使用される経路封鎖看板を対象としています。
背景と課題
つくばチャレンジの選択課題Cでは、経路を封鎖する看板を正しく認識して回避することが求められます。（以下、イメージ）従来はカメラによる画像認識が主流ですが、照明条件や天候の変化に弱いという課題があります。
そこで本記事では、LiDARのみを用いて看板を検出する方法を紹介します。LiDARの持つ「反射強度」を利用することで、環境に依存しない安定した検出が可能になります。
看板の特徴
つくばチャレンジのルールでは看板は以下のルールのもと設置されます。
- 歩道の両端に看板が設置されている
- 看板の両端に再帰反射テープが貼られている
再帰反射テープは、入射光を光源に向かって反射させる性質を持ちます。そのため、LiDARでスキャンすると再帰反射テープの部分の反射強度が高いことを確認できます。
検出方法の流れ
本手法の大まかな流れは以下の通りです。
1. 反射強度に基づく点群の抽出
ロボット前方の点群のうち、反射強度が高い点を取り出します。
2. DBSCANクラスタリング
点群をクラスタリングして、看板候補を分離します。
ハイパーパラメータ（eps, min_samples）は現場環境に応じて調整が必要です。
3. 初期位置合わせ
テンプレート点群と候補点群をおおまかに位置合わせします。
4. ICPアルゴリズムによる精密位置合わせ
Iterative Closest Point (ICP) を用いて、テンプレートと候補の点群を高精度に合わせます。
5. Fitness Scoreによる類似度判定
ICPの結果得られる Fitness Score を用いて、看板かどうかを判定します。
手順の詳細
1. 反射強度に基づく点群の抽出
LiDAR から得られる点群のうち、ロボット前方で距離が近く、反射強度が高い点だけを取り出します。
-
距離の条件：0.5 m 以上 4.0 m 以下
→ 近すぎる点はセンサーの死角になりやすく、遠すぎる点は密度が低いため除外します。 -
角度の条件：10° から 170° の範囲
→ ロボットの真正面付近だけに絞ることで、不要な側方や後方の点を取り除きます。 -
反射強度の条件：130 以上
→ 看板は高反射テープが貼られているため、強度が大きい点を抽出すると候補が浮かび上がります。
以下は処理の例になります。
2. DBSCANクラスタリング
次に、抽出した点群を DBSCAN でクラスタリングします。本手法では以下のパラメータを設定しました。
- パラメータ：
eps = 0.7 m （近傍点として扱う距離のしきい値）
min_samples = 3 （クラスタを形成するために必要な最小点数）
まず、eps を 0.7 m に設定した理由について説明します。看板には長さ約 0.7 m の反射テープが貼られており、さらに看板の両サイドのテープの間隔はおよそ 0.3 m です。eps を 0.7 m にすることで、両サイドのテープが 1 つのクラスタとして認識されることを意図しています。
一方で、min_samples の値はシルエットスコアを用いて決定しました。具体的には、eps = 0.7 に固定した上で min_samples を 3 から 8 の範囲で変動させ、そのときのシルエットスコアを比較しました。その結果、min_samples = 3 のときに最もスコアが高かったため、本手法では 3 を採用しています。以下が、eps = 0.7 に固定し、min_samples を 3 から 8 の範囲で変動させたときの、シルエットスコアの値になります。
上記のパラメータを用い、クラスタリングした結果を以下に示します。色ごとにクラスが異なり、ノイズ、人、看板が、それぞれ異なるクラスに分類できていることが確認できます。
3. 初期位置合わせ
得られたクラスタを看板のテンプレート点群と比較する前に、粗い位置合わせを行います。閉路看板のテンプレート点群と、候補として抽出されたクラスタ点群を比較するために、両者の重心計算、回転変換、および並進変換を行い、初期位置合わせを行います。
3.1 点群の定義
まず、閉路看板のテンプレート点群および候補クラスタ点群を以下のように定義します。
各点は三次元座標 $(x, y, z)$ を持ちます。テンプレート点群の点数を $N$、クラスタ点群の点数を $M$ とします。
$$
\mathbf{P}_{\text{template},i} = [x_i, y_i, z_i], \quad i = 1, 2, \dots, N
$$
$$
\mathbf{P}_{\text{clustered},j} = [x_j, y_j, z_j], \quad j = 1, 2, \dots, M
$$
3.2 重心の計算
両者の点群の重心を、それぞれの点の座標の平均値をとることで計算します。
テンプレート点群の重心を $\mathbf{C}_{\text{template}}$、クラスタ点群の重心を $\mathbf{C}_{\text{clustered}}$ とします。
$$
\mathbf{C}_{\text{template}} = \frac{1}{N} \sum_{i=1}^{N} [x_i, y_i, z_i]
$$
$$
\mathbf{C}_{\text{clustered}} = \frac{1}{M} \sum_{j=1}^{M} [x_j, y_j, z_j]
$$
3.3 重心間ベクトルの計算
両者の重心の差を計算し、$\Delta x, \Delta y$ を以下のように定義します。
$$
\Delta x = \frac{1}{M} \sum_{j=1}^{M} x_j - \frac{1}{N} \sum_{i=1}^{N} x_i
$$
$$
\Delta y = \frac{1}{M} \sum_{j=1}^{M} y_j - \frac{1}{N} \sum_{i=1}^{N} y_i
$$
3.4 回転角度の計算
この差ベクトルを用いて、Z軸周りの回転角 $\theta$ を計算します。
$$
\theta = \arctan2(\Delta y, \Delta x)
$$
3.5 回転変換の適用
テンプレート点群を回転させるため、以下の回転行列 $\mathbf{R}$ を用います。
回転後のテンプレート点群を $\mathbf{P}_{\text{template},i}'$ とします。
$$
\mathbf{R} = \begin{bmatrix}
\cos \theta & -\sin \theta & 0 \\
\sin \theta & \cos \theta & 0 \\
0 & 0 & 1
\end{bmatrix}
$$
$$
\mathbf{P}_{\text{template},i}' = \mathbf{R} \cdot \mathbf{P}_{\text{template},i}
$$
3.6 並進変換の適用
回転後のテンプレート点群の重心 $\mathbf{C}_{\text{template}}'$ を再計算し、それをクラスタ点群の重心 $\mathbf{C}_{\text{clustered}}$ に一致させるように並進を行います。
並進後のテンプレート点群を $\mathbf{P}_{\text{template},i}''$ とします。
$$
\mathbf{C}_{\text{template}}' = \frac{1}{N} \sum_{i=1}^{N} \mathbf{P}_{\text{template},i}'
$$
$$
\mathbf{P}_{\text{template},i}'' = \mathbf{P}_{\text{template},i}' + (\mathbf{C}_{\text{clustered}} - \mathbf{C}_{\text{template}}')
$$
以上の計算により、テンプレート点群をクラスタ点群に対して重心合わせと回転合わせを行い、初期位置合わせを実現します。この初期位置合わせにより、後続の ICP (Iterative Closest Point) アルゴリズムを適用する準備が整いました。
4. ICPアルゴリズムによる精密位置合わせ
粗く揃えた後、ICP アルゴリズムで高精度に位置合わせを行います。
ICP は以下の操作を繰り返します：
- 各点の最近傍点をテンプレート内から探す
- それらの対応点に基づいて回転・並進の変換行列を計算
- 点群を更新して再計算
5. Fitness Scoreによる類似度判定
ICP の結果から、看板との一致度を Fitness Score で評価します。
- Fitness Score = 一致した点の割合（0～1 の範囲）
- 本実装では 0.8 以上 を看板と判定する基準に設定
例えば、
- クラスタ点群 100 点中、80 点以上がテンプレートと一致 → Fitness = 0.8 以上 → 看板と判定
- クラスタ点群 50 点中、30 点しか一致しない → Fitness = 0.6 → 看板ではない
となります。
以上が、処理の一連の流れになります。
参考
- 実装例は以下にあります
- つくばチャレンジ 2024, https://tsukubachallenge.jp/2024/