Dead Reckoningによる自己位置推定

はじめに

Turing株式会社 E2E先行開発チームの尾㟢です。私たちの開発しているAIモデルの一つに、画像情報を入力として車両が進む目標経路を出力するものがあります（参考）。Turingでは、目標経路の教師データ作成やそれに追従するための車両制御も独自に開発しています。本記事では、自動運転に欠かせない自己位置推定の技術についてご紹介します。

自動運転の自己位置推定における課題

自己位置推定で一般的な方法は、GNSS（Global Navigation Satellite System）を用いた手法です。近年は、RTK（Real Time Kinematics）という、GNSS測位の高精度化手法が多く用いられています。RTKは、基準局と移動局の二つの受信機を用いてGNSS信号を補正し、誤差を数センチ以下に抑えることができます。

しかし、現実世界では常にGNSSが受信できるとは限りません。トンネルや地下駐車場はイメージしやすいと思いますが、他にも歩道橋の下やビル街（下図参照）でもGNSSが頻繁に受信できません。安全な車両制御のためには、環境によらない常に安定した制御が必須なため、GNSS信号が途切れても自己位置推定ができる仕組みが必要です。

GNSSからの信号が十分に受信できない場所（トンネル、歩道橋下、ビル街）

また、GNSSからの経路情報は、経路追従に必要な情報量に対して冗長でもあります。GNSSはグローバルな空間での自己位置を算出しますが、経路追従にはローカルな空間での自己位置で十分です。これは、私たちが運転するときを考えるとわかりやすいですが、自分が向いている方角が北なのか南なのかわからなくても、自分の進行方向に対してどう進むかがわかれば、道なりに運転することができます。

以上のことから、環境を選ばない自動運転には、GNSSの精度が悪い状況下でもローカルな自己位置推定をする技術が必要です。

Dead Reckoningによる自己位置推定

Turingでは、GNSSに依存しないローカルな自己位置推定の手法を活用し、Dead Reckoningと呼んでいます。ローカルな環境での自己位置は、速度と姿勢を推定し時間積分することで求めることができます。具体的には、車輪速とIMU（Inertial Measurement Unit）から状態推定アルゴリズムによって、速度と姿勢を推定し、自己位置推定を行っています。

Dead Reckoningのアルゴリズム
Dead Reckoningの難しさは、計算に必要な変数に対して、使用できるセンサが少ない点にあります。一般的な状態推定アルゴリズムでは、複数センサから二通りの方法で求めたい値を算出し、その差から観測誤差や積分誤差を修正することができます。しかし、GNSSが使えない場合は、積分によってしか自己位置を求めることができません。そのため、積分による累積誤差をできるだけ減らすため、速度と姿勢の正確な推定が重要となってきます。

Dead Reckoningのアルゴリズム

本来は、速度と姿勢を同時に推定しますが、簡略化のため、速度は既知とし姿勢推定に絞って紹介します。ポイントは、IMUに含まれる重力成分をうまく利用して、姿勢を推定する点です。

まず、IMU の観測値と既知の速度から、車両座標系における重力ベクトル$\mathbf{g}_{\mathrm{meas}}$を算出します。

$$\mathbf{g}_{\mathrm{meas}} = \dot{\mathbf{v}} - \mathbf{a} + \boldsymbol{\omega} \times \mathbf{v}$$

$\mathbf{v}$は車両の質量中心の速度、$\dot{\mathbf{v}}$は速度から求まる車両の加速度を表します。また、$\mathbf{a}$、$\boldsymbol{\omega}$はそれぞれ、IMUの加速度センサおよびジャイロセンサの値です。

次に、この車両座標系での重力ベクトルを、現在の推定姿勢を用いて推定します。

$$\mathbf{g}_{\mathrm{pred}}=R(\boldsymbol{\Theta})^{\top}\mathbf{g}_0$$

ここで、$R(\boldsymbol{\Theta})$は姿勢に対応する回転行列、$\mathbf{g}_0$は基準座標系での重力ベクトルです。

観測値と推定値から２通りで同じ量を求めましたが、これらの差は観測および推定誤差に起因します。そこで、この差を姿勢の時間発展方程式に加え、フィードバックします。

$$\dot{\boldsymbol{\Theta}}=T(\boldsymbol{\Theta})\,\boldsymbol{\omega}-K_{\Theta}\,(\mathbf{g}_{\mathrm{pred}} - \mathbf{g}_{\mathrm{meas}})$$

ここで、$T(\boldsymbol{\Theta})$は角速度を姿勢微分へ変換する行列、$K_{\Theta}$は姿勢補正ゲインで安定性を考慮して設計します。

データ検証

Dead Reckoningの精度を検証してみました。GNSSが使用できるところで、GNSSによる経路とDead Reckoningで求めた経路を比較しました。目視では一見差がわからないくらい、精度よく推定できました。

Dead ReckoningとGNSSの経路を比較
次に、XY方向の座標を時間軸に合わせてプロットしました。単純計算で、X方向は0.65%, Y方向は1.57%の誤差という精度でした。

Dead ReckoningとGNSSのXY方向変位を比較

また、当たり前ですが、GNSSが受信できない場面では、GNSS経路は大きくずれてしまいますが、Dead Reckoningによる経路は全く影響を受けません。

GNSSが受信できない場面で、Dead ReckoningとGNSSの経路を比較

このように、リアルタイム制御には十分な精度でGNSSに頼らずに自己位置推定をすることができました。実際、このDead Reckoningによる自己位置推定は自動運転システムにも組み込まれており、環境に依存せずに走行実験をすることができるようになりました。

おわりに

本記事では、環境に依存しない自動運転に必要な、Dead Reckoningという自己位置推定手法を紹介しました。Turingの目指す自動運転の開発には、機械学習の知識の他にも、制御や物理モデルに関する深い理解が必要です。様々な分野のエキスパート達が集まって、日々思考錯誤しながら開発を行っています。興味がある方は、ぜひ気軽にご連絡ください！