Part 4 · 自動運転のしくみ

4.4 自己位置推定と地図

「いまどこにいるか」を知る

読了目安 10分発展前提：4.3 センサとフュージョン（分散の逆数での重み付け）、1.2 条件付き確率とベイズの定理▶ 動画（約5分）

この回で分かること

自己位置推定 に必要な精度と、GNSS・推測航法・地図照合のそれぞれの得意と苦手を説明できる
推測航法の誤差が走った距離とともにたまることを計算し、外からの手がかりで直す必要があることを説明できる
「予測で広がり、観測で縮む」という自己位置推定のくり返し（ベイズの更新・カルマンフィルタ）を、簡単な計算で確かめられる

直感で理解する

人は、見慣れた景色や標識から「いまどこにいるか」を自然に判断しています。自動運転車も、車線の中のどこを走っているか、次の停止線まであと何 m かを知る必要があります。これが 自己位置推定 です。地図の上の自分の位置が分かれば、センサにはまだ見えていない先のカーブや停止線、ODD の境界（4.1）も、地図から前もって知ることができます。

自己位置を知る手がかりは、大きく3つあります。

方法	しくみ	得意	苦手
GNSS（衛星測位）	複数の衛星の電波が届くまでの時間から位置を求める	地球上のどこでも、絶対的な位置が分かる。基準局の補正を使う RTK なら数 cm	ふつうは数 m の誤差。トンネル・地下・ビルの谷間では電波が届かない、建物の反射で誤差が大きくなる
推測航法（IMU・車輪）	加速度・回転・車輪の回転から、動いた量を足し合わせる	外の手がかりなしに、なめらかに素早く求まる	小さな誤差が、走った距離とともにたまっていく
地図照合	LiDAR の点群やカメラで見た車線・標識を、高精度地図と照らし合わせる	車線に対する位置が cm 単位で分かる。トンネルの中でも使える	地図が現実と違う（工事、雪で車線が見えない）と外れる。似た景色を取り違える

高精度地図 は、車線の形・停止線・標識などを数 cm〜十数 cm の精度で記録した地図です。その上に、工事や渋滞のように刻々と変わる情報を重ねて使うこともあります。3つの手がかりを 4.3 のフュージョンで組み合わせ、互いの弱点を補います。

トンネルを通るときの位置の誤差（模式図）。組み合わせても、トンネルの中では推測航法だけになり誤差がたまるが、出口で GNSS が戻るとすぐに小さくなる。トンネルの中でも車線や壁の形を地図と照合できれば、誤差がたまるのを抑えられる。

地図を作りながら、同時にその地図の中の自分の位置を求める方法を SLAM（Simultaneous Localization and Mapping）と呼びます。高精度地図を作るときや、地図のない場所を走るロボットなどで使われます。

数式で確かめる

① どれくらいの精度が必要か

幅 3.5 m の車線を幅 1.8 m の車が走るとき、左右の余裕はそれぞれ (3.5 − 1.8) ÷ 2 ＝ 0.85 m です。この余裕は、自己位置推定だけでなく、車線の認識や操作（ハンドルの制御）のずれとも分け合う必要があります（4.2 の要求の割り当て）。自己位置推定に 0.3 m を割り当て、それを ±3σ（約99.7%）で守ろうとすると、

$$3\sigma \le 0.3\ \text{m} \quad\Longrightarrow\quad \sigma \le 0.1\ \text{m}$$

が必要です。ふつうの GNSS の数 m の誤差では、とても足りません。

② 推測航法の誤差はたまる

推測航法では、向き（方位）の小さな誤差が、そのまま横のずれになってたまっていきます。向きの誤差を δθ（ラジアン）、走った距離を L とすると、横のずれはおよそ

$$e_{\text{横}} \approx L\,\delta\theta$$

です（角度が小さいときの近似。1° ＝ π/180 ≈ 0.0175 ラジアン）。向きがわずか 0.1°（約 0.00175 ラジアン）ずれたまま 1 km のトンネルを走ると、横のずれは 1000 × 0.00175 ≈ 1.7 m と、車線の幅の半分近くになります。前後の向きも同じで、車輪の回転から求めた速度が 1% ずれていると、1 km 走るうちに 10 m ずれ、停止線の位置を大きく見誤ります。推測航法は短い間なら頼りになりますが、外からの手がかり（GNSS や地図照合）で、くり返し直す必要があります。

③ 予測で広がり、観測で縮む

自己位置推定は、2つの手順のくり返しです。推測航法で位置を先に進める「予測」では、誤差が加わって分布が広がります。地図照合や GNSS による「観測」では、4.3 の③と同じく分散の逆数を足し合わせて、分布が縮みます。

$$\text{予測：}\ \sigma^2 \leftarrow \sigma^2 + q, \qquad \text{観測：}\ \frac{1}{\sigma^2} \leftarrow \frac{1}{\sigma^2} + \frac{1}{\sigma_z^2}$$

q は1回の予測で加わる誤差の分散、$\sigma_z$ は観測の誤差です（← は 2.3 で使った「更新する」という記号）。トンネルを出たとき、推測航法の誤差が σ ＝ 1.2 m になっていたとします。そこで地図照合の観測（$\sigma_z = 0.2$ m）が入ると、

$$\frac{1}{\sigma^2} = \frac{1}{1.2^2} + \frac{1}{0.2^2} \approx 0.69 + 25 = 25.7 \quad\Longrightarrow\quad \sigma \approx 0.20\ \text{m}$$

と、1回の観測で、観測とほぼ同じ精度まで戻ります。この「予測 → 観測」をくり返すのが、4.3 で紹介したカルマンフィルタです。1.2 のベイズの定理を時刻ごとにくり返し使っていることにあたり、予測が事前の見込み、観測で更新したものが事後の見込みです。

似た景色が続く場所では、「左の車線にいる」「右の車線にいる」のように、位置の候補が複数に分かれることがあります。これは1つの正規分布では表せないので、たくさんの候補の点（粒子）で確率を表す パーティクルフィルタ なども使われます。

まとめ

自己位置推定は、GNSS（絶対的な位置・トンネルでは使えない）、推測航法（なめらか・誤差がたまる）、地図照合（車線に対して cm 単位・地図の正しさが前提）を組み合わせて行う
車線の中を走るには 10 cm 程度の精度が必要になる。推測航法の横のずれは「距離 × 向きの誤差」でたまり、0.1° でも 1 km で約 1.7 m になる
予測で広がり、観測で縮む、をくり返す（カルマンフィルタ・ベイズの更新）。地図と現実の食い違いや、位置の確からしさを見張ることが安全につながる

確認クイズ

Q1. 車線の幅が 3.6 m、車の幅が 1.8 m のとき、左右の余裕はそれぞれ何 m？

(3.6 − 1.8) ÷ 2 ＝ 0.9 m です。この余裕を、自己位置推定・車線の認識・操作のずれで分け合います。

Q2. 向きが 0.2° ずれたまま 500 m 走ると、横のずれはおよそ何 m？

0.2° は約 0.0035 ラジアンなので、500 × 0.0035 ≈ 1.7 m です。

Q3. 推測航法の誤差が σ ＝ 1.0 m になったところに、σ_z ＝ 0.25 m の地図照合の観測が入りました。合わせた誤差は？

1/σ² ＝ 1/1.0² ＋ 1/0.25² ＝ 1 ＋ 16 ＝ 17 なので、σ ＝ 1/√17 ≈ 0.24 m です。

Q4. 工事で車線が右にずれた区間を、古い高精度地図のまま走ると、何が起こりうる？どう防ぐ？

地図の車線に合わせて走ろうとして、実際の車線や工事の区画にはみ出すおそれがあります。地図照合がうまく合わず、位置を取り違えることもあります。センサで見た車線を優先し、地図との食い違いを見張って機能を制限する、地図をこまめに更新する、工事区間を ODD の外として扱う、などで防ぎます。

もっと知りたい人へ

次の回：4.5 予測と経路計画
関連：4.3 センサとフュージョン（分散の逆数での重み付け）、4.1 自動運転レベルとODD、1.2 条件付き確率とベイズの定理、3.7 不確実性とキャリブレーション
用語集：自己位置推定、高精度地図、推測航法、地図照合、GNSS・RTK、カルマンフィルタ
解説動画：YouTube（約5分）（静止画のスライドと音声で要点を説明。英語版の記事には英語の動画があります）
参考文献
- S. Thrun, W. Burgard, D. Fox, Probabilistic Robotics, MIT Press, 2005.
- J. Levinson, M. Montemerlo, S. Thrun, “Map-Based Precision Vehicle Localization in Urban Environments,” Robotics: Science and Systems, 2007.
- H. Durrant-Whyte, T. Bailey, “Simultaneous Localization and Mapping: Part I,” IEEE Robotics & Automation Magazine, 2006.