はじめに
End-to-End自動運転開発チーム (以降E2Eチーム) の塩塚です。本記事では、画像認識結果とベクターマップをマッチングすることで自己位置推定を行う手法について紹介します。
アイディアとしては
- 高架下などGNSSが不安定な場所では自己位置がズレる。
- その結果、本来は適切に車線中央を走行しているのに、自己位置上は白線の上を走行しているような状態になる。
- 車両にとりつけられたカメラの画像を見れば車線中央を走行していることは明らかなので、その情報を使って自己位置を修正する。
といった感じです。
結果はこちらです。以下の場面ではGNSSによる自己位置が車線一本分もずれていますが、それを補正できています。
- 左上（補正前）と右上（補正後）の画像を見ると、画像認識結果（赤）とベクターマップ（青）が合うように自己位置が補正されていることが分かります。
- 左下はマッチングの様子です。（パッと目では何をやっているのか分かりにくいですがデバッグ用に重宝したのでそのまま掲載しています。）
- 右下は補正前の自己位置（青）とマップマッチングで補正された自己位置（緑）です。
それでは記事の残りは手法解説になります。
マップマッチング
マップマッチングは大きく2つの技術によって成立します。カメラ画像を使ったマップ要素の抽出と、抽出したマップ要素とベクターマップのマッチング部分です。
1. マップ要素の抽出
ここでは主に2つのことを行います。
- カメラ画像からマップ要素を抽出する。
- 画像座標系のマップ要素を、車両中心の相対座標に変換する。これはベクターマップと同じ座標系にするためです。
1.1. カメラ画像からマップ要素の抽出
カメラ画像に対してセグメンテーションを行うことで、白線や横断歩道、走行可能エリアなどを抽出します。
今回は、yolopv2 というモデルを使用しました。yolopv2 は白線や走行可能エリアをセグメンテーションするモデルです。以下が検出結果です。白線や横断歩道のエッジを検出できます。
yolopv2の検出結果
ただし、しばしば実際の線よりも太く検出される傾向があったため、skelton 処理を行って輪郭を先鋭化しています。
yolopv2の検出結果にskelton処理を行なったもの
1.2. マップ要素を相対座標に変換
画像から検出したマップ要素は、画像座標のピクセル座標を持っています。これを、車両を原点とした相対座標系に変換します。画像座標から相対座標に変換するには、ピクセル座標に加えて深度情報が必要です。私達のデータ収集車両には LiDAR が設置されているので、LiDAR の深度情報を利用します。
1.2.1. 画像空間に LiDAR 点群の投影
左図が相対座標系の LiDAR 点群で (rawの状態)、右図がそれを画像空間に射影した時の LiDAR 点群です。
1.2.2. 画像空間でマップ要素と LiDAR 点群の対応付け
画像から検出したマップ要素と LiDAR 点群を同じ画像空間に投影し、各点の対応付けを行います。これにより、画像空間のマップ要素に深度情報を付与することができます。
1.2.3. 深度情報を使いマップ要素を相対座標系に変換
左図は右図のような画像座標上のマップ要素を相対座標系に変換したものです。上から見たような座標系になります。
ここまでで、カメラ画像から検出したマップ要素をベクターマップとマッチングするための準備が整いました。
2. 抽出したマップ要素とベクターマップのマッチング
抽出したマップ要素とベクターマップをマッチングします。
2.1. ベクターマップの抽出
ベクターマップは世界座標系で作られています。そこで、自己位置をもとに相対座標系でのベクターマップを切り出します。左図が世界座標系でのマップで、右図が相対座標系でのマップです。右図の原点が車両の自己位置になります。
以下は、画像から抽出したマップ要素（赤）とベクターマップ（青）の比較です。左図が相対座標系での比較で、右の画像が画像座標での比較です。
マップ要素を見ると自車両が白線と白線の間にいることが分かります。一方、誤った自己位置をもとに抽出したベクターマップ上では自車両は白線の上にいることになっています。このズレを補正することがマップマッチングの目的です。
2.2. マッチング
マッチングには small_gicp を使用させていただいています。pythonからも呼び出せるようになっておりとても使いやすいです。
以下のような形で使用できます。ただし、今回は回転方向の補正は行わないように一部ソースコードを変更し、ビルドしなおして使っています。
import small_gicp
def get_matching_result(map_points: np.ndarray, seg_points: np.ndarray) -> np.ndarray:
""" matchingを行い、補正のための変換行列を返す
Args:
map_points (np.ndarray): ベクターマップ
seg_points (np.ndarray): 抽出したマップ要素
Returns:
np.ndarray: 補正情報が入った変換行列
"""
result = small_gicp.align(
target_points=map_points,
source_points=seg_points,
)
return result.T_target_source
2.3. マッチング結果による補正
以下のように変換できます。ただし、今回は回転方向の変換は行なっていません。
def correct_pose(pose_trans: np.ndarray, T_target_source: np.ndarray) -> np.ndarray:
"""
与えられた変換行列を用いて、自己位置を補正するための新しい位置を計算する
Args:
pose_trans (np.ndarray): 補正前の自己位置
T_target_source (np.ndarray): 補正に使用する変換行列
Returns:
np.ndarray: 補正後の位置
"""
corrected_pos = T_target_source[:3, :3].T @ pose_trans - T_target_source[:3, 3:4]
return corrected_pos
ここまでの操作によって、マップマッチングによって自己位置が補正できます。
処理の流れは以上です。処理結果は最初に貼った動画の通りです。
まとめ
今回は基本的なアイディアを中心に説明しましたが、他にも重要だった仕組みはいくつかあります。
- 時系列情報の利用
- マルチカメラの利用
- 車両の速度情報を使うことでマッチング誤差による影響の抑制
- 変換行列の初期値を工夫し、大きく自己位置がずれていても探索可能にした
- 多段階のマッチングによる安定かつ高い精度のマッチング
マップマッチングでやっていること自体はシンプルです。ただし、画像座標系と相対座標系と世界座標系を自由に変換するための知見やキャリブレーション技術は重要です。今回は座標変換のやりかたには触れませんでしたが以下の記事で少し触れていますので興味がある方はそちらも参照ください。
Turingでは高層ビルや高架下などが多数存在する都市部を対象に自動運転の開発を行っています。このような場所ではGNSSだけでは自己位置が不安定になりますが、マップマッチングような方法を組み合わせることで都市部の自己位置推定を向上させることができます。
E2Eチームが目指しているものは都内を30分以上走行可能な自動運転AIですが、そのためにさまざまな技術開発が行われています。積極採用中ですのでご興味ある方はぜひ！
Discussion