より安全な自動運転へ向けた死角手前での減速

死角手前での減速機能によって向上する自動運転の安全性

こんにちは、ティアフォーでPlanning/Controlの開発をしている田中です。今回は、交通ルールによって速度を決定するモジュールの一部である死角手前での減速機能を紹介し、この機能が自動運転に与えるインパクトについてお話します。

なお、ティアフォーでは、「自動運転の民主化」をともに実現していく様々なエンジニア・リサーチャーを募集しています。もしご興味があればカジュアル面談も可能ですので以下のページからコンタクトいただければと思います。

tier4.jp

背景・課題

通常走行時、Autowareは自車の制限速度にしたがって走行しますが、走行中のレーンに人や車が飛び出してきたときは衝突しないように緊急停止します。閉鎖空間や立ち入り禁止区域など、人が入れないような場所では飛び出しが起こりえないので考慮する必要はありませんが、公道では交差点でないところでも飛び出しは起こりえます。上限速度で走行していた場合は、万が一の飛び出しに対応できない可能性もあります。したがって、緊急停止時にきちんと停止できるかどうかは非常に重要な問題となります。そこで、死角手前から緩やかに減速する機能を開発しました。

想定シナリオ

自車が走行しているレーン付近に停車中の車両がある場合、その車両の裏が死角となって歩行者が飛び出してくる可能性があるため、停車車両が作る死角地点の十分手前から適切な減速を行います。

衝突前に緊急停止可能な上限速度を \(v_\mathit{ebs}\) とします。\(v_\mathit{ebs}\) は、フルブレーキの減速度 \(a_\mathit{ebs}\) と歩行者が衝突地点にたどり着くまでの時間 \(\frac{d_\mathit{lateral}}{v_\mathit{obstacle}}\) 、システム遅延 \(t_\mathit{safe}\) を用いて以下のように求められます。

\begin{align} v_\mathit{ebs}=a_\mathit{ebs} \cdot \left(\frac{d_\mathit{lateral}}{v_\mathit{obstacle}}-t_\mathit{safe}\right) \end{align}

なお、ここでは死角から出てくる歩行者は \(v_\mathit{obstacle}\) の速度で等速直線運動をするものと仮定しています。

次に、現在地点から死角地点まで一定のブレーキ量で安全に減速するための目標速度の上限を \(v_\mathit{pbs}\) とすると、\(v_\mathit{pbs}\) は現在の自車速度 \(v_0\)、死角までの縦方向距離 \(d_\mathit{longitudinal}\) 、死角手前での減速として許容できるブレーキ量 \(a_\mathit{pbs}\) を用いて、等加速度直線運動の関係式から以下のように求められます。

\begin{align} v_\mathit{pbs}= \sqrt{v_0^2 - a_\mathit{pbs} \cdot d_\mathit{longitudinal}} \end{align}

理論上目標速度に向かって減速しきることが好ましいのですが、誤検出によって死角と判定されたものが急にできたときなどすべての死角に対応できないこと、急減速時の車内の人の安全担保、そして後続車からの衝突回避を考慮して上記の上限速度を用いています。

最後に、走行時に交通の妨げとならないための最低速度定数 \(v_\mathit{min}\) を後続車との車間距離 \(d_\mathit{follow}\)、制限速度 \(v_\mathit{limit}\)、後続車が減速を認識するのにかかる時間 \(s_\mathit{safe}\) を用いて以下のように求めます。

\begin{align} v_\mathit{min}=v_\mathit{limit}-\frac{d_\mathit{follow}}{s_\mathit{safe}} \end{align}

私有地や狭い道路であれば高速で走行する車両は少ないのですが、車の通行量が多い国道や大通りなどではある程度速度を出して走らないと交通の妨げになったり、後続車から追突されてしまうことが考えられます。

これら \(v_\mathit{ebs}\), \(v_\mathit{pbs}\), \(v_\mathit{min}\) を用いて、死角手前での減速計画は以下のようになります。

死角手前での減速フロー

最後に、死角手前での減速フローを載せておきます。

まとめ

このように、死角手前であらかじめ減速しておくことで予期せぬ事故を回避できます。今後もより安全な自動運転へ向けた減速計画について検討していきたいと考えています。

参考文献

この機能の実装には以下の文献を参考にしました。

• Euro NCAP. European New Car Assessment Programme (Euro NCAP)—Test Protocol—AEB VRU systems, Version 3.0.4. Technical Report, 2021. https://cdn.euroncap.com/media/62795/euro-ncap-aeb-vru-test-protocol-v304.pdf
• Yohei Fujinami, Pongsathorn Raksincharoensak, Dirk Ulbricht, and Rolf Adomat. Risk Predictive Driver Assistance System for Collision Avoidance in Intersection Right Turns. Journal of Robotics and Mechatronics, Vol.30, No.1, pp.15-23, 2018. https://doi.org/10.20965/jrm.2018.p0015

Autowareにおける3次元物体検出アルゴリズムの再検討【サーベイ編】

ティアフォーのSensing/Perceptionチームで開発を行っている村松です。Autowareの動物体検出アルゴリズムのうち一部を再検討し、Autowareに組み込むまでについて紹介します。今回はそのサーベイ編として、調査した概要や手法についてお話します。

なお、ティアフォーでは、「自動運転の民主化」をともに実現していく様々なエンジニア・リサーチャーを募集しています。もしご興味があればカジュアル面談も可能ですので以下のページからコンタクトいただければと思います。

tier4.jp

自動運転における3次元物体検出について

3次元物体検出とは、3次元空間での物体のクラス（種類）・位置・大きさ・向きなどを推定する技術です。自動運転において、事故なく目的地まで移動するためには、他車両や歩行者などがどこにどの大きさで存在するかという周辺環境の認識が必須となります。より正確に物体検出を行うことで、自車両がどのように動くか判断することができます。

*1より引用

自動運転車両にはLiDAR（Light Detection and Ranging）やカメラ・レーダーなどのセンサーが搭載されています。その中でも、多くの3次元物体検出手法でLiDARが採用されています。LiDARは、レーザー光を照射し、反射して戻ってくるまでの時間を計測することで空間の位置情報が取得できます。この空間の位置情報が3次元物体検出には重要な特徴量となります。

一方で、単眼画像やステレオ画像からの3次元物体検出も数多く研究されています。しかし、一般的なカメラ画像には深度の情報がないため、RGB（Red-Green-Blue color model）のみから深度を推定する必要があり、特に単眼画像からの推定はとても難しいタスクです。また、カメラのキャリブレーションがかなり正確でないと画像から空間へ投影した際にずれが生じるという難しさもあります。そして、一番大きな問題として学習したカメラ画像のキャリブレーションのパラメータも含めて学習してしまうために、カメラの取り付け位置を変更すると学習したモデルは全く使えなくなってしまうことがあります。オープンソースであるAutowareとしては、センサーの取り付け位置が完全に固定されてしまうことは致命的な問題です。

また、自動運転システムに3次元物体検出手法を組み込む際の計算リソースが限られていることも考慮する必要があります。点群を比較的効率的に処理できるVoxelNet*2と呼ばれる手法があります。VoxelNetについて簡単に説明すると、空間をVoxel化し、点群をCNNs（Convolutional Neural Networks）で処理できる特徴量に落とし込むことで効率的な処理を可能にする手法です。しかし、このVoxelNetには3D CNNsが使われているため、とても大きな計算リソースを必要とします。この問題を解決するためにSECOND*3とPointPillars*4と呼ばれる2つの手法があり、点群をVoxel特徴量に落とし込む3次元物体検出手法のほとんどはこのどちらかをベースとしています。

2つの手法の大まかな特徴は以下のようになります。

SECOND

• 3D CNNsを独自のSparse 3D CNNsに置き換えることで高速化
• 検出精度がよい

PointPillars

• 3D CNNsを除去することで高速化
• SECONDと比較し検出精度は落ちるが軽量で高速

今回、実行時間とAutowareへの組み込みのためのC++への移植性を考慮し、上記2つのうちPointPillarsをベースとする手法を候補にしました。

手法紹介

今回、3次元物体検出を調査した論文として、CenterPoint*5とAFDet*6と呼ばれる2つの手法を紹介します。PointPillarsについては、以前の記事で紹介されているためそちらを参照ください。

tech.tier4.jp

Anchor-free Detection

今回紹介するCenterPointとAFDetはAnchor-freeと呼ばれる手法です。従来のPointPillarsなどの手法では、Anchorと呼ばれるBounding Boxの基準となるもので検出範囲内にマッチング検索をし、Intersection over Union (IoU)がしきい値を超えた物体を学習するという手法でした。Anchor-basedの手法では、検索方法やしきい値を適切に決定しなければ、効率よく物体を学習することができません。そのため、より多くの物体がIoUのしきい値を超えるようにヒューリスティックにパラメータ探索をしなくてはなりません（下図a）。

一方で、Anchor-freeの手法では、Anchorを使わずに物体の中心をヒートマップとして学習するため、様々な形状の物体をとりこぼすことがありません（下図b）。

*7より引用

一般的な画像からの物体検出では、上図のように回転のない長方形として物体の位置を示します。しかし、自動運転の3次元物体検出では、物体の向きも考慮する必要があります。また、歩行者や自転車などの小さい物体やトラックやバスなどの長さのある物体なども多く、向きを含めたBounding BoxでIoUがしきい値を超えるようなAnchorを設定するには多くのパラメータやチューニングが必要となります（下図参照）。

*8より引用

Center-based 3D Object Detection and Tracking (CenterPoint)

CenterPointは、The Conference on Computer Vision and Pattern Recognition (CVPR) 2021で採択された論文で、画像から物体検出を行うCenterNet *9を3次元物体検出に応用した手法です。CenterNetの著者も共著者の一人となっており、基本的な考え方については同じものとなっています。

CenterPointの概要図 *10より引用

CenterPointの概要について紹介します。CenterPointは3D Backbone、First Stage、Second Stageと主に3つの処理で構成されています。

3D Backboneは点群を入力とし、Bird's-eye-viewの疑似画像特徴量へと変換します。この処理はPointPillarsやSECONDで採用されている手法をそのまま用いているため詳しく知りたい方はそれぞれの論文を参照していただければと思います。

First Stageと呼ばれる処理では、疑似画像特徴量から各物体のパラメータを推定します。まず、ヒートマップのピーク位置から物体の大まかな位置を推定します。次に、各ネットワークの出力からそのピーク位置に対応するオフセットの推定値を用いることで、物体のより詳細な位置を推定します。そして、高さ・サイズ・向きをそれぞれ推定することで、3D Bounding Boxの位置・サイズ・向きが推定できます。

ネットワークから出力する各推定値 *11より引用

Second Stageでは、First Stageで推定された3次元物体の中心と四辺の中心の位置に対応する特徴量を疑似画像から抽出し、MLP（Multilayer Perceptron）を適用することでScoreと3D Bounding Boxの調節を行います（CenterPointの概要図のd）。

他の3次元物体検出手法と異なる点として、3次元トラッキングを行うために物体の速度推定も行っています。現在のフレームの物体位置と速度から過去フレームの位置を逆算することで、過去の位置に物体が存在した場合に同一物体だと割り当てることができます。

CenterPointは今年のWaymo Open Dataset ChallengeのReal-time 3D Detection分野で2位を獲得しました。そのレポートについてはこちらのリンクにあります。今回の記事の内容は、このコンペティションの前に調査したものなので、コンペティションで使われた手法等の詳しい内容はレポートを参照していただければと思います。

AFDet: Anchor Free One Stage 3D Object Detection

AFDetは昨年のWaymo Open Dataset Challengeの3D Detectionと今年のReal-time 3D Detectionで1位を獲得した手法です。CenterPointとほぼ同じタイミングでarXivに公開されましたが、基本的な考え方はCenterPointのFirst Stageまでの手法と同じです。CenterPointと大きく異なる点は3つあります。

1つ目は、学習時に物体の中心を推定するためのヒートマップの生成方法です。CenterPointではガウス分布を用いていましたが、AFDetでは2点間のユークリッド距離の逆数を用いています。

2つ目は、学習時のオフセットのlossの計算方法です。CenterPointでは物体の中心のグリッドに対応する値のみをlossとして計算しますが、AFDetでは物体の中心に対応する値だけでなく、その周辺のグリッドの値まで含めてlossの計算をします。この方法は、物体のヒートマップのピークが物体の中心とずれていた場合でも、オフセットで物体の中心を正しく推定できるようにすることが目的です。

3つ目は、物体の向きの推定方法です。CenterPointでは向きθをcos(θ)、sin(θ)に分解することで向きベクトルとして学習していますが、AFDetではMulti-binと呼ばれる手法を用いています。この手法は複雑なため簡単に説明すると、θを2つの範囲に区切り、どちらの範囲に入るかの分類とcos(θ)、sin(θ)のregressionを活用することでθを推定する手法です。詳しい内容は論文を参照していただければと思います。

実験

ここまでCenterPointとAFDetについて紹介してきましたが、AFDetは実装が公開されていなかったため、今回はCenterPointのみを検証することにしました。比較対象は、Autowareに実装済みであるPointPillarsです。PointPillarsはAnchor-based、CenterPointはAnchor-freeであることがポイントです。

データセットは規模や画像のBounding Boxのアノテーションも含まれることを考慮してWaymo Open Dataset *12を採用しました。実験にはhttps://github.com/open-mmlab/OpenPCDetのリポジトリを活用しました。

学習条件:

• データセット: trainデータの4分の1
• 検出範囲: -75m ~ 75m四方
• Voxelサイズ: 0.32m x 0.32m x 10m
• Optimizer: Adam
• Scheduler: OneCycle

評価

評価条件:

• データセット: validationデータの4分の1
• 評価方法: nuScenes形式

一般的な画像からの物体検出の評価方法は、主にIoUをしきい値として検出結果にTrue/Falseを割り当てることで計算できるAverage Precision（AP）が使われています。3次元物体検出でも同様にこのAPが使われていますが、このTrue/Falseの割り当て方法では位置や向きのズレをほとんど許容できません。今回は、このズレを許容し物体が検出されていることを優先的に判断するために、nuScenes*13の評価方法である物体の中心間距離をしきい値としてTrue/Falseを割り当ててAPを計算する評価方法を採用しました。nuScenesと同様に中心間距離0.5mをしきい値として用いています。

evaluation

考察

車と自転車はCenterPoint、歩行者はPointPillarsの方が高精度という結果になりました。したがって、この結果から総合的に判断して、Autowareでは既存の動物体検出アルゴリズムと入れ替える形でCenterPointを採用することにしました。

PointPillarsの自転車検出性能がCenterPointより著しく低い理由として、自転車のサンプル数が他のクラスと比べて圧倒的に少ないこと、自転車は小さくて細長い形状のため、Anchorマッチングのしきい値を超えるようなサンプルが少なくなったことが考えられます。学習に使用したデータは、車が約480万サンプル、歩行者が約220万サンプルに対して自転車が約5万サンプルと他のクラスの1~2%にとどまります。

このサンプル数が少ないケースでの問題を解決するために、ground-truth sampling augmentationと呼ばれる手法が多くの論文で採用されています。このground-truth sampling augmentationとは、物体のアノテーションとその物体に対応する点群をデータベースとして保持し、学習フレームにデータベースから物体を追加することで学習するサンプル数を増やすという手法です（下図）。しかし、このフレームに追加されるサンプルは、アノテーションの値をそのまま用いているため学習フレームの幾何的特徴を無視しています。壁や地面にめり込んだ物体や空中に浮いている物体など、現実に存在しない物体も生成し学習してしまいます。これはデータセットの学習フレーム間で似たようなシーンが多い場合には問題ありませんが、立体的な交通環境が多い日本のデータにはそのまま採用できないaugmentationの手法だと考えているため、今回は採用を見送りました。この問題は、地図データを活用することでより現実のデータと整合性のとれるような配置に修正することで解決できると考えています。

 ground-truth samplingなし

ground-truth samplingあり

まとめ

今回は、3次元物体検出の手法としてCenterPointを採用したところまで紹介しました。次回以降、実際にティアフォーが実証実験を行っている日本の道路環境において物体検出できるよう工夫した方法などについて紹介する予定です。

*1:Ke Chen, et al. "MVLidarNet: Real-Time Multi-Class Scene Understanding for Autonomous Driving Using Multiple Views." 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020.

*2:Yin Zhou and Oncel Tuzel. "VoxelNet: End-to-End Learning for Point Cloud Based 3D Object Detection." Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, 2018.

*3:Yan Yan, Yuxing Mao, and Bo Li. "SECOND: Sparsely Embedded Convolutional Detection." Sensors 18.10 (2018): 3337.

*4:Alex H. Lang, et al. "PointPillars: Fast Encoders for Object Detection from Point Clouds." Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 2019.

*5:Tianwei Yin, Xingyi Zhou, and Philipp Krähenbühl. "Center-based 3D Object Detection and Tracking." Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), 2021.

*6:Runzhou Ge, et al. "AFDet: Anchor Free One Stage 3D Object Detection." arXiv preprint arXiv:2006.12671, 2020.

*7:Xingyi Zhou, Dequan Wang, and Philipp Krähenbühl. "Objects as Points." arXiv preprint arXiv:1904.07850, 2019.

*8:前掲、5に同じ 

*9:前掲、7に同じ

*10:前掲、5に同じ

*11:前掲、6に同じ

*12:https://waymo.com/open/

*13:https://www.nuscenes.org/

カルマンフィルターが自動運転の自己位置推定で使われるまで

はじめまして、ティアフォー技術本部 Planning / Controlチームで開発を行っている堀部と申します。

今回は状態推定の王道技術「カルマンフィルター」が実際に自動運転で用いられるまでの道のりやノウハウなどを書いていこうと思います。

みなさんはカルマンフィルターという言葉を聞いたことがありますでしょうか。

カルマンフィルターとは「状態推定」と呼ばれる技術の一種であり、自動運転においては現在の走行状態、例えば車速や自分の位置を知るために用いられます。

非常に有名な手法で、簡単に使えて性能も高く、状態推定と言えばまずカルマンフィルターと言われるほど不動の地位を確立しており、幅広いアプリケーションで利用されています。

使い勝手に定評のあるカルマンフィルターですが、実際に自動運転のシステムとして実用レベルで動かすためには多くの地道な作業が必要になります。

この記事では、カルマンフィルターが実際に自動運転で使われるまでに何が行われているのか、自動運転を支える基礎技術の裏側をご紹介したいと思います。

目次は以下のようになっています。

カルマンフィルターって？

カルマンフィルター（KF：Kalman Filter）の説明は詳しい文献がたくさんありますので、ここは概念的な説明に限定して説明します。

KFの主な目的は、複数の情報を適切に統合し、より精度の高い推定値を計算すること です。ここで言う「複数の情報」とは、「複数の観測値」とそれらの「信頼度」、「対象の運動モデル」、現在の「推定値の信頼度」などです。1960年にアルゴリズムが提唱されてから、長いこと工学分野の第一線で用いられてきました。*1

実際に、自動運転における自己位置推定での使われ方を例に見ていきましょう。

自動運転における観測値とは、車輪エンコーダーから取得できる車速や、IMUの加速度・角速度情報、GPSから得られる位置情報などになります。

これらの情報は単体でも利用可能ですが、例えばGPSの位置情報は絶えずノイズが乗っています。一方で、人間の知識として「車両は真横に移動しない」と言った車両の運動に関する情報を持っています。これらの情報を「車速センサ」や「車両運動モデル」を用いて統合することによって、より精度の高い車両位置を推定することができるのです。 

この複数情報の統合に数学的な最適性を取り入れたものがKFとなります。

 

Autowareでどのようにカルマンフィルターが使われているのか

我々の開発する自動運転OS「Autoware」においても、自己位置推定や移動物体のトラッキングなどにKFが使われています。今回は自己位置推定を例に深堀りしていきます。

自己位置推定（Localization）とはその名の通り、自分の位置を推定する技術です。下図に、AutowareにおけるSensing/Localizationモジュールの構成図を示しました。右側のLocalizationブロックのPose Twist Fusion FilterにKFが実装されており、Pose EstimatorとTwist Estimatorで計算された自車両のPose（位置・姿勢情報）とTwist（速度・加速度情報）を統合して最終的な推定値を計算することが目的となります。

 なお、AutowareではPose EstimatorとしてNDT scan matchingと呼ばれる手法を用いており、事前に用意した点群地図（下図：白色）と、自動運転車に搭載されたLiDARセンサーの観測点群データ（下図：赤や黄緑色）を比較して、パズルのように点群が最もうまく当てはまる場所を探し出して自己位置を計算しています。

このNDT scan matching単体でもかなり正確な自己位置を計算することができますが、LiDARのセンサーノイズや外環境の変化により絶えず数センチ程度のノイズが乗っています。ここにKFを導入することにより、推定精度をさらに向上させることが可能となります。

NDT scan matchingについての詳しい情報が知りたい方は過去の記事で述べていますので、こちらをご覧ください。前後編、二部構成の大作となっております。

tech.tier4.jp

自動運転の自己位置推定でカルマンフィルターが使われるまで

さて、ここからはいよいよ本題の実装編に入っていきます。

いざ実際にカルマンフィルターを実装すると、特定環境で上手くノイズを除去できなかったり、推定値が一方向にズレる、といった問題が出てきます。

これらの問題の原因は、カルマンフィルターが想定している前提条件と実環境との差異を基準に議論することができます。

例えばカルマンフィルターは「センサーデータのノイズは白色ガウス分布である」といった仮定に基づいて最適化を行います。白色ガウス分布とは非常に理想的な形状をしているノイズなのですが、実環境でノイズがこのような理想形状をしていることは稀であり、自動運転においても例外ではありません。

センサーの遅延を例に見てみましょう。どんなセンサーにも遅延は存在しますが、通常のカルマンフィルターではセンサーデータに遅延が含まれることは想定していません。従って、特別な対処をせずにカルマンフィルターを適用すると、この遅延は白色ガウスノイズであるという仮定の下で処理されます。この仮定は実際のノイズ特性とは大きくかけ離れたものであり、推定精度の劣化に繋がります。 

カルマンフィルターの想定する仮定を乱す要因は他にもあり、例えば

• 運動モデルの非線形性
• 外れ値の存在
• ノイズの多峰性

などです。これらの原因を見定め、１つずつ適切な対処をしていくと、仮定と現実の乖離が徐々に小さくなり、最終的な推定精度が向上していきます。

以下では代表的な問題と、その対応について解説します。

車両運動の非線形性（非線形拡張）

まずは非線形拡張についてです。ここはご存知の方も多いかと思います。

本来KFは線形モデルに対して理論が構築されたものでした。一方で車両の運動は非線形の数式で表されます。以下の式を見てみましょう。

この式は車両が速度v、角速度ωで走行しているときの運動を簡易的に表現したものであり、上の二行の式によって「車両は自分が向いている方向に進む」ことを表しています。ここで問題になるのがsinとcosの存在です。彼らの存在によって式は非線形となってしまい、通常のKFでは対処できません。

この非線形への対処はすでに多くの方法が提案されています。有名なものではExtended Kalman Filter (EKF)、Unscented Kalman Filter (UKF)、Particle Filter (PF) などがあり、対象となる非線形性の強さなどに応じて利用するアルゴリズムを使い分けます。

これらのアルゴリズムは非線形性への対応度と調整・計算コストのトレードオフとなっています。例えば、最も柔軟に非線形に対応できるParticle Filterと呼ばれるアルゴリズムは、一方で非常に計算コストが高く、出力結果にランダム性が生じるといった課題もあり、モデルの非線形特性を適切に理解した上でアルゴリズムを選定する必要があります。

Autowareでは現在はEKFと呼ばれる手法を用いて非線形モデルに対応しています。

センサーに遅延がある（遅延補償）

センサーには必ず遅延が存在します。例えばティアフォーで利用している回転式のLiDARは100msかけて360度のスキャンを行うため、平均50msの遅延が存在します。また、KFの前段のNDTによる計算処理も数10msの時間を要します。

数10msと言われると誤差のようにも聞こえますが、高速で移動する自動運転においてこの遅延は致命的であり、例えば時速60kmで走行中に自己位置推定に100msの遅延が生じたとすると、それだけで約1.7mの誤差を生み出します。

AutowareではAugmented State Kalman Filterと呼ばれる手法を用いて遅延補償を行っています。これはKFの状態を時間方向に拡張し、「過去の状態」を陽に考慮することによって遅延に対応する手法であり、これによって遅延を含んだ情報を、拡張された次元の一部が遅延無しで観測されたとみなすことができます。

 詳しくはこちらの書籍 *2 にまとめられています。

（こちらは1979年の論文集なのですが、遅延よりも先にデジタル処理による離散化の影響と補償法について述べられています。当時は計算機の処理が遅く、KFの処理自体に時間がかかっていたのでしょう。こういった時代背景も踏まえて論文を読むといろいろな発見があって面白いですね。）  

センサー間の時刻同期（センサー同期）

さて、KFを使って遅延に対処する方法が分かりました。しかし、肝心の遅延時間はどのように測定するのでしょうか。

センサーが観測をした真の時間を知る必要があるのですが、ここでもいくつか問題が生じます。それが時刻同期やタイミング同期です。

時刻同期はイメージしやすいと思います。センサーが観測した時刻を知るためには、センサーの時刻ソースと自動運転システムの時刻ソースを揃えたり、観測〜データ受信までの遅延を事前計測しておく必要があります（センサ側でタイムスタンプが得られる場合は前者、出来ない場合は後者）。自動運転には多くの計算機が搭載されており、それぞれの時刻を全て同期させる必要があります。

タイミング同期は、複数センサーの観測タイミングを制御する仕組みです。Autowareでは複数のLiDARデータから得られた情報を結合し、一つの大きなデータとして処理します。このときそれぞれのLiDARが異なるタイミングでスキャンを実行していては、空間的なズレが生じてしまい適切な結合が出来ません。そこで、全てのLiDARの動作タイミングを同期し、仮想的に一つの大きなLiDARが回転しているように振る舞わせることによってこの問題に対処しています。

また、このセンサー同期には他にもいくつかの目的があり、LiDAR同士の干渉問題の解決や、動物体検出におけるLiDAR-カメラフュージョンの性能向上にも一役買っています（下図参照）。

これはこれで話が広がる部分ですが、また次の機会に。

取得した点群が歪んでいる（歪み補正）

時間方向の点群の歪みも問題になります。

LiDARの「一定時間かけてデータを取得する」という特性上、データ取得の開始・終了時点で車両位置が違う、という問題が出てきます。このため、LiDARの生データをそのまま「特定の時刻の情報」として扱うことはできません。

これはカーブ走行時などに特に顕著に現れ、低速走行においても大きな測定誤差を生じます。

この問題を解決するため、AutowareではLiDARの取得点群一つ一つに時刻を埋め込み、その時の車速・角速度情報を用いて点群に時刻補正を掛けるという対応を行っています。

これによって自車の移動に依存しない安定した点群を取得することが可能となり、自己位置推定の精度が大幅に向上します。

モデルに誤差がある（バイアス誤差モデル）

次は車両モデルの誤差についての話です。例として、LiDARセンサーの取り付け角度がズレている場合を考えてみます。

NDT scan matchingはセンサーによって得られた点群によって自己位置を推定しますが、あくまでも推定できるのはセンサーの位置姿勢であり、センサーと車両の位置関係は事前に与える必要があります。

この位置関係が間違っているとどうなるでしょうか。

例えば下図のように、実際の車両は右に向かって走行していますが、センサーが誤って斜め上の方向に向かって取り付けられており、車両の向きを間違って推定してしまったとします。すると、KFが推定する位置と実際の位置に乖離が生じます。これはKFから見るとノイズの一種なのですが、常に特定方向に偏ったノイズが与えられてしまい、これは事前に説明した白色ガウスノイズの仮定から逸れてしまいます。結果、常に推定結果が横に逸れた状態で計算されます。

これは適応オブザーバー（外乱オブザーバー）と呼ばれる方法を用いて解決されます。

適応オブザーバーとは、KFのモデルに「センサーの取り付け角度がズレる可能性があるよ」という情報を付加することによって、走行中にこのズレを自動で推定する手法です。 

実際には、このセンサーの取り付け誤差は走行前のセンサーキャリブレーションによって取り除かれます。

一方で、どうしても事前に取り除けない誤差、例えば温度変化によるヨーレートセンサーのバイアス項や、空気圧変化によるタイヤ径の変化、路面環境の変化などは、このように動的な補償によって対処する必要があります。

周期の異なるセンサーの統合（滑らかな補間）

複数のセンサー情報を統合するKFの技術は、センサーフュージョンと呼ばれることもあります。ここで問題になるのがセンサーごとの周期の違いです。

例えば、IMUのような内界センサーは数100~数1000Hzで情報を出力することができる一方、NDT scan matchingのような外界センサーに依存した手法は（現在は）10~20Hzでしか情報を出力できません。これらのセンサー周期の違いを考慮せずに統合してしまうと、低周期のセンサー情報がKFに統合されたときに大きく自己位置が変化する、という現象が発生します（下図：左側）。

この「大きな自己位置の変化」自体はKFとしては正常な動作であり、その瞬間にもっとも信頼度の高い位置を出力しているだけに過ぎません。それに対して、この自己位置を車両制御で用いる際には、自己位置のズレがステアの振動に直結するため問題が生じます。

AutowareのKFでは、自己位置の変動に大きな影響を及ぼす情報が得られた場合は、その入力を時間方向に分散させて更新を掛けることによって、滑らかな自己位置の遷移を行っています（下図：右側）。

これは、遅延補償機能との合わせ技であり、遅れて更新を掛けても適切に処理可能であるという前提に基づいています。 

急に自己位置が変な場所へ飛んでしまう（外れ値除去）

続いて、センサーの外れ値対応についてです。

例えばGPSのキッドナップ問題と呼ばれる有名な課題があり、GPS信号のマルチパスなどによって、急にGPSの位置が大きくズレることがあります。NDTも同じく、アルゴリズムが収束しなかった場合などは推定値が大きく真値から外れます。

KFはガウスノイズを仮定しているという性質上、外れ値に大きく引きずられるという特徴があり、これらの外れ値は別途対応する必要があります。

Autowareではこの外れ値判定に2つのゲートを設けて対応しています。

まず、NDT側でイテレーション回数などの内部情報を元に外れ値・アルゴリズムの失敗を検出し、計算結果がおかしい場合は結果を出力しないという方針を取っています。これは基本的にワンショットの情報（これまでの時系列の統計情報を含まない情報）で判断されます。

その後段で、KFによって時系列情報を用いた外れ値判定（マハラノビスゲート）を適用します。KFでは推定結果の信頼度（誤差共分散）を内部情報として蓄えており、この情報を元に、KFへの入力がどれだけ確からしいのかを計算することができます。

例えば、これまでの入力の信頼度が低く十分な推定ができていない場合に、実は自己位置の場所が大きく異なっていましたと言われたら、その値は考慮すべきです。一方で、すでに十分な推定精度が得られている場合は、たとえ外部情報が自信満々に値を出してきたとしても、それがあり得ない確率であればその入力を棄却する必要があります。この外れ値の計算にはマハラノビス距離と呼ばれる、入力ベクトルの距離を共分散行列で正規化した値が用いられており、確率論ベースで情報の統合・棄却を判定します。

今の推定精度はどれくらいなのか（共分散の検討）

上節で、共分散行列を元に推定値の信頼度を計算するという話をしました。この「信頼度が計算できる」というKFの機能は非常に有用であり、自動運転の安全性に大きく影響します。

例えば、自己位置の推定精度が悪い場合はレーンからはみ出てしまう可能性があるため、速度を落として走行したり、停止することが求められます。

しかし、この信頼度を正確に計算するためには、KFで統合される信号のノイズ情報が正しく設定されている必要があり、「運動モデルがどの程度正しいのか」「どのくらいのズレがあるのか」「観測値の誤差はどのようなノイズ特性を持つのか」などを一つ一つ検証する必要があります。

このノイズ情報の設定はなかなかに骨の折れる作業で、"How To NOT Make the Extended Kalman Filter Fail" *3という論文で取り扱われるほど多くのエンジニアを悩ませてきた問題です。

例えばIMUのヨーレートセンサーのノイズです。まずは真値と比較を行い、ノイズ情報（二次モーメント）を計算します。このときのノイズ特性が、どれだけKFの仮定（白色ガウス）とズレているのか、が大きなポイントとなります。ヨーレートセンサーは一般的にバイアスノイズの影響が大きいため、まずはこのノイズを除去する必要があります。

その他のセンサーや運動モデルにおいても、どうやったら白色ガウス分布からのズレを除去できるのか、除去できない場合はワーストケースを考慮すると共分散はどう設定すべきなのか、などを検討する必要があります。

ヘッセ行列から計算されたNDTの誤差共分散楕円の可視化。カーブ時に楕円が大きくなっているのが分かる。

緊急時の対応

ここまでKFの拡張や精度、外れ値対応などについて述べてきました。しかし、自動運転において「外れ値が来たので無視します」という処理だけでは安全性を担保することができません。

ここでAutowareのLocalizationモジュールを再見すると、Localizer Diagnosticsというモジュールが存在していることが分かります。

このモジュールはAutowareの自己位置推定の機能群を常に監視しており、外れ値が計算された、位置推定の精度が落ちた、などの情報を統合して現在の自己位置推定の状態を判断します。

この診断情報は別のSystemモジュールへ送られ、状態に応じて適切な指示が車両に送られます。例えば、LiDARに依存した自己位置推定アルゴリズムが何らかの異常によって再起不能になった場合でも、内界センサー（車速センサーやIMU）による自己位置推定によってしばらくは走行が可能であり、自己位置の推定誤差が閾値を超過する前に適切な駐車場所を見つけ、路肩に停車するといった動作が可能となります。

最後に

最後まで読んでいただきありがとうございました。

このブログで述べた機能は、いくつか開発途中のものもありますが、大半はOSS（Open Source Software）として利用可能となっています。興味があればぜひ御一見ください。

github.com

いろいろと機能の説明をしてきましたが、自動運転技術は発展途上であり、まだまだ開発が必要な領域が残っています。

このブログを読んで、自動運転って面白そう...！と思った方がいましたら、ぜひ一緒に開発をしましょう！下記リンクからお問い合わせをお待ちしています。

tier4.jp

*1:Kalman, R. E. (March 1, 1960). "A New Approach to Linear Filtering and Prediction Problems." ASME. J. Basic Eng. March 1960; 82(1): 35–45.

*2:Anderson, B. D. O., & Moore, J. B. (1979). Optimal Filtering. Englewood Cliffs, NJ: Prentice-Hall.

*3:René Schneider and Christos Georgakis. "How To NOT Make the Extended Kalman Filter Fail." Industrial & Engineering Chemistry Research 2013 52 (9), 3354–3362.

Visual-Inertial Odometryが自動運転に与えるインパクトと応用への課題

　こんにちは、ティアフォーでVisual SLAMの研究開発をしている石田です。今回はVisual-Inertial Odometryという、カメラとIMU(慣性計測装置)を用いた経路推定手法を紹介し、これを自動運転に応用できた場合のインパクトと、応用までに乗り越えなければならない課題についてお話します。

走行経路の推定結果

なお、ティアフォーでは、「自動運転の民主化」をともに実現していく様々なエンジニア・リサーチャーを募集しています。もしご興味があればカジュアル面談も可能ですので以下のページからコンタクトいただければと思います。

tier4.jp

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

　自己位置推定とは、名前のとおり車両やセンサーデバイスなどが地図の中でどこにいるのかを推定するための技術であり、自動運転において欠かせない要素のひとつです。自分がどこを走っているか把握できなければ迷子になってしまいますし、自分が走っている場所の先に何があるか把握することも難しくなってしまいます。正確な自己位置推定が実現できれば、車両のスムーズな制御につなげることができて、さらに信号機や標識、停止線などの場所をあらかじめ地図に埋め込んでおき、車両がこれらに近づいたことを正確に検知できれば、安全かつ確実な制御を行うことができます。

自己位置推定の難しさ

　自己位置推定の課題は大きく分けて3つあります。1つ目は精度、2つ目はロバスト性、そして3つ目はこれらを小さい計算量で達成することです。
　公道を走る車両の自己位置推定は少なくとも数十センチの精度が要求されます。この精度を達成できないと、走行レーンをはみ出したり他車にぶつかったりしてしまいます。
　自動車は様々な環境や場所を走るため、自動運転車に搭載される自己位置推定システムも正確な動作が必要となります。市街地も森も田園も橋の上も安全に走れる必要がありますし、雨や雪が降っていても正確に動作する必要があります。このように様々な場所や環境で動作する能力は「ロバスト性」と呼ばれ、精度と同様に自己位置推定手法における重要な指標とされています。
　計算量を抑えることも重要な課題のひとつです。自分の位置を計算するのに1秒かかっていたら、その間に車が動いてしまい、周囲の物体にぶつかってしまいます。しかし、計算を高速化させたいからといって車に巨大なコンピュータを積むと、車両の価格が上がったり、膨大なエネルギーを消費したり、コンピュータの周囲が高温になってしまいます。このため、自動運転のための自己位置推定手法は小さいコンピュータでも高速に動作しなければなりません。
　高精度で、ロバストで、ライトな自己位置推定を実現する。これが我々の仕事です。

自己位置推定を実現するためのセンサー

　自己位置推定を実現するためのセンサーには様々なものが存在します。皆さんにとって最も身近なものは、おそらくスマートフォンやカーナビに搭載されているGPSでしょう。しかし、皆さんもご存知のようにGPSは屋内などではうまく動作できないうえ、自動運転に使えるほどの精度が常に実現できるわけではありません。
　現在のティアフォーの自動運転車は主にLiDARというセンサーを用いています。LiDARはレーザー光によって周囲の形状を把握するセンサーで、LiDARによって得られた車両の周囲の形状と、あらかじめ作成しておいた点群地図を照合することで、車両の位置を高い精度で推定することができます。しかし、LiDARにも弱点があります。LiDARを用いた自己位置推定手法は主に周囲の環境の構造情報を頼りにしているため、特徴的な構造が観測しづらい場所、たとえば田園などではうまく動作できません。また、性能のよいLiDARは非常に高価で数千万円もしてしまうため、これをそのまま自動運転車に用いると、車両も同様に高価になり自動運転車の普及の妨げになってしまいます。したがって、自動運転の実用化および普及促進のためには、LiDARだけに頼るのではなく複数の安価なセンサーを組み合わせることが不可欠です。
　ここで今回私が紹介するVisual-Inertial Odometryの出番です。Visual-Inertial Odometry(VIO)とは、カメラとIMU(慣性計測装置)を使って移動経路を求める手法です。
　カメラはスマートフォンにも搭載されていますし、昨今のリモートワークでお世話になっている方も多いでしょう。もしかしたら毎日使っている方もいるかもしれません。IMUもカメラと同様に多くのスマートフォンに搭載されているのですが、こちらは馴染みのない方もいらっしゃるかと思います。IMUは人間における三半規管のようなもので、加速度と回転速度を計測することができるセンサーです。皆さんは車やバスに乗っているとき、目を閉じても車が加速しているのか減速しているのか、右に曲がっているのか左に曲がっているのかを、大まかに感じ取ることができると思います。IMUもこれと似ていて、物体の加速度や回転速度を計測することができ、この情報をもとにしてセンサーがどの方向にどれぐらい動いたかを大まかに計算することができます。これらのセンサーから得られる情報を組み合わせて移動経路を推定するのがVisual-Inertial Odometryです。

VIOに用いられるセンサーの一例。画像中央にある小型の横長のものがカメラとIMUが一体になったもの。画像で示されているのは検証用のものであり、実際の制御に用いられているものではない。画像左上に見えている筒状のものがLiDARであり、こちらは形状ベースの位置推定に利用される。

　カメラやIMUはほとんどのスマートフォンに搭載されている非常にありふれたセンサーです。自動運転車に利用するためにはより高信頼なものが必要になるものの、それでも数万円から数十万円で調達することができます。VIOはLiDARベースの手法と比べるとまだまだ精度やロバスト性の点で劣るため、いますぐLiDARに置き換えられるというものではありません。しかし、カメラやIMUから得られた情報を組み合わせれば、同じ性能の自己位置推定をより安価なLiDARで実現できる可能性があります。結果として、車両全体の価格を押し下げることができ、自動運転の迅速な普及に貢献することができます。

Visual-Inertial Odometryの具体的な手法

　VIOはすでに多くの研究がなされており、様々な手法が存在します。今回紹介するのは、現在私が性能検証を行っている、VINS-FusionというVIOの手法です。VINS-Fusionを選定した理由として、VIOの手法の中では精度が高いこと、他のセンサーと組み合わせやすいこと、計算量を抑える仕組みが導入されていることが挙げられます。ここでは、VINS-Fusionの手法の特徴を大まかに解説し、安心安全な自動運転を実現するために必要な研究と今後の開発についてお話します。

手法の特徴

　VINS-Fusionは、センサーの観測値を用いてあるエラー値を算出し、それをできるだけ小さくするようなセンサーの姿勢を求めることで、センサーの移動経路を推定します。たとえば、ある静止した物体を見ているとき、カメラが近くにいるならその物体は大きく見えますし、カメラが遠くにいるなら小さく見えるはずです。カメラが遠ざかっているのにその物体がだんだん大きく見えるようになるのはおかしいですよね。IMUについても同様で、IMUに対して加速度が前にかかっているなら、IMUは前に進んでいるはずです。加速度が前にかかっているのに、IMUが後ろに進んでいるなら、これもやはりおかしいです。この「おかしさ」を数値にし、それができるだけ小さくなるようなセンサー姿勢を求めるのがVINS-Fusionのアプローチです。物体が徐々に大きく見えるようになっているとき、「カメラが物体に近づいている」と推論すればこの「おかしさ」の数値は減っていきます。同様に、IMUが前向きの加速度を検知しているとき、「IMUは前に進んでいる」と推論すればやはりこの「おかしさ」の数値は減っていきます。このように、物理現象とつじつまが合うように「おかしさ」の数値を設計し、それが減るようなセンサーの姿勢を求めるのがVINS-Fusionの戦略です。

計算量を減らすための工夫

1. IMU積分値近似
　VINS-Fusionのアプローチは、先ほどの「おかしさ」を徐々に減らしながらセンサーの姿勢を調節するため、一般に計算コストが高いとされています。この問題を解決するため、VINS-Fusionにはこの「おかしさ」を高速に計算するためのIMU積分値近似という仕組みが備わっています。
　IMUで観測された加速度や回転速度はしばしば実際の値よりも少し大きく、あるいは小さく出力されます。これはIMUの特性上どうしても起きてしまうもので、VIOの開発者たちはがんばってこのズレを補正することで、実際の値にできるだけ近い観測値を得ようとします。
　観測値と実際の値のズレは先ほどの「おかしさ」を計算するときに問題になります。「おかしさ」を計算するためにはIMUの観測値を積分して、IMUが進んだ距離を計算する必要があります。たとえば、「今はIMUから1.0という加速度が観測されているから、1秒間に8.0cm進んでいるはずだ。でも姿勢推定システムは1秒間に6.0cm進んでいると考えているから、おかしさは2.0cmだな！」というふうに考えるわけです。しかし、観測値のズレを補正するということは、距離計算の入力値(加速度や回転速度、先ほどの例だと1.0という数字)が変わることに相当するので、観測値の補正を行うたびに進んだ距離の計算をやり直さなければならなくなってしまいます。「さっきは加速度1.0で計算したけど本当は1.2なの？じゃあ今何cm進んでいるんだろう...」という計算を何度も何度も繰り返さなければならなくなってしまうのです。冒頭で述べたように、VIOは小さい計算コストで高速に動作することが求められるので、この計算に時間を取られてしまうと実用化の大きな支障になります。そこでVINS-Fusionは、この計算を近似的に行うことで処理の高速化を図っています。たとえば、「加速度を+0.1補正したら進む距離が0.4cm増える。ならば、加速度を+0.2補正したら進む距離は0.8cm増えるだろう」というように、非常に大まかに、しかし高速に計算を行います。これにより、姿勢推定全体を高速化することができます。

2. キーフレーム選択
　動画撮影用のカメラは1秒間に何枚ものフレームを撮影することができます。たとえばiPhoneのビデオ機能は1秒間に30フレーム撮影しているそうです。では1秒間に30回撮影されるフレームそれぞれについて、位置や姿勢を推定する必要があるでしょうか。カメラが高速に移動していれば、たしかにそうかもしれません。では、静止している場合はどうでしょうか。静止しているなら、1秒間に30回も姿勢推定を行う必要はありませんね。VINS-Fusionをはじめ、多くのVIOには、キーフレーム選択と呼ばれる「大きく動いたときにだけフレームの姿勢を推定する」という機能が備わっています。このように、必要なときのみ姿勢推定を行うことで実行速度を上げることができます。また、余った計算コストを必要な部分に振り分けることで、精度も高められるというわけです。

これから

　最後に、VIOの今後の課題と将来性についてお話して、記事の締めくくりとします。

他のセンサーとの組み合わせ

　VIOはまだまだ発展途上の技術であり、精度やロバスト性はLiDARベースの自己位置推定手法におよびません。しかし、VIOをLiDARベースの手法と組み合わせることで、お互いの弱点を補いあい、強みを活かすことができます。たとえば、VIOはカメラ画像を入力とするため、対向車のヘッドライトのような大きな照度変化に影響されやすいという弱点があります。一方でLiDARはレーザー光で周囲の形状を把握するセンサーなので、明るさの変化の影響をほとんど受けないという特性があります。したがって、VIOとLiDARを組み合わせることで、様々な環境で安定して動作する自己位置推定システムを作ることができます。現在私はこういったセンサーの組み合わせに取り組み始めているところです。

動けないときに「動けない！」と言ってくれる自己位置推定

　霧や大雨の中では、人間でさえ安全な運転が難しいと感じることがあり、これは皆さんも経験したことがあるかもしれません。同様に、自己位置推定の手法も環境によっては確実に動作できないことが考えられます。こういったときに無理に自己位置推定を行って危険な走行をするのではなく、「動かない！」と言って停止することも自動運転車の大事な機能です。VIOはまだまだ普通の環境でしっかり動くことを目指す段階なので、このような停止判断機能はもう少し先の話になりますが、高信頼な自動運転を実現するためにはいずれ取り組まなければならない課題です。

誰もが安心して乗れる車を実現するために

　Visual-Inertial OdometryはAR(Augmented Reality、拡張現実)などではすでに実用化されているものの、自動運転に応用するためにはまだまだ多くのハードルを乗り越えなければなりません。様々な環境で正確かつ高速に、しかも高信頼に動作する自己位置推定手法を開発するのは、非常にチャレンジングでワクワクするものです。もしこのような仕事にご興味ある方がいましたら、改めてにはなりますが、以下のページからご応募ください！！

tier4.jp

読み解くNDT Scan Matchingの計算 [後編]

こんにちは、ティアフォーエンジニアの村上です。

今回は、 読み解くNDT Scan Matchingの計算 [前編] - Tier IV Tech Blog の続きとして、自動運転位置推定のターニングポイントとなった、Scan Matchingによる高精度自己位置推定技術の華、NDT Scan Matchingを読み解いてみようと思います。

まだの方は、前編もあわせてぜひご覧ください。

tech.tier4.jp

なお、ティアフォーでは、「自動運転の民主化」をともに実現していく様々なエンジニア・リサーチャーを募集しています。もしご興味があればカジュアル面談も可能ですので以下のページからコンタクトいただければと思います。

tier4.jp

Scan Matchingのスコア計算へ

復習になりますが、LiDAR点(ここではSourceと呼びます)とMap点(ここではTargetと呼びます)との点と点のScan Matchingをそのまま考えると、その整合度合いを計算するために、O(N) * O(M) [where N = Source点数, M = Target点数]の計算量が必要となります。ここで、Target点群をいくつかの格子に分割後、"格子中の点群の正規分布"、つまり雰囲気のようなものへ変換してしまい、Sourceの各点がその分布上その位置に存在する確率を「スコア値」として考えてしまう発想に至ります。もはや分布となってしまったTargetは(当然分割格子数は実計算の多寡に影響するとはいえ)、定数扱いとなり、計算量はぐっと減少し、O(N)となります。また、雰囲気との比較というのは、局所最適解に陥りやすいマッチング処理において、ロバスト性を高める点にも一役買っています。

このTarget点群の正規分布化(Normal Distribution Transform)を用いた、Scan Matching処理こそがNDT Scan Matchingであり、今日の自動運転の高精度自己位置推定において大活躍しています。

前編において解説した、近傍Voxelの取得によって、Source各点に対するTarget中近傍Voxel(の"雰囲気")が取得されました。本編では、その取得された近傍に対して、Source点がたしかにそこにあるという"たしからしさ" = スコア値計算の部分を見ていきます。

OpenCL accelerated NDT Scan Matcher

前編でもOpenCLを用いた再実装を行いましたが、今回は前編の成果物もまとめ、NDT Scan MatcherをOpenCLで実装し、他のデバイス(もちろんCPUもです！)へ容易にオフロードできるパッケージを作成しました。以降コードリーディングの際にも折に触れて登場します。

github.com

OpenCL版のREADMEはこちらをご覧ください。

前項で述べた通り、NDT Scan Matchingにおいては、その内部の処理は、Source各点に対して、各点並列でスコアを計算していきます。最終的にはそのSource各点のスコア値は合算されますが、この処理は並列化が比較的容易なアルゴリズムとなっており、AutowareではOpenMPによる並列化実装が採用されています。

OpenCLでの並列化も、基本的にはOpenMPと同じ戦法をとっており、さらに幅広いデバイス(Embedded GPU, GPGPU, CPU, Accelerator...)への適用を念頭にしたチューニングがなされています。

NDT Scan Matcher外観

それではソースコードを読みこんでいきますが、前編では省略した、NDT Scan Matcherの外観から読んでいきたいと思います。

NDT Scan Matcherは、LiDARからの点群を受けとった時点から処理がはじまります。NDT Scan Matcherに限ったことではありませんが、Autowareのソースコードリーディングにおいては、まずそのnodeが購読するtopicを知り、そのtopicを処理するcallbackを見つける所が最初の一歩です。

LiDAR点に対しては、void NDTScanMatcher::callbackSensorPoints がまさにそのcallbackにあたります。

さて、いきなり色々な処理があり面くらう所ですが、NDTにおいてその処理の大部分を占めるのは、俗に “align” 処理と呼ばれるものであり、一例として経過時間を以下に添付しますが、実際の所、計算機パフォーマンス的にはalign処理さえ抑えれば良いということがわかります。

1align_time: 21.448ms (align処理が占めた時間、大体exe_time - 2.0ms程度) 2exe_time: 23.242ms (callbackSensorPoints全体の時間)

そして、そのalign処理を呼び出しているのが ndt_scan_matcher_core.cpp L.371 の箇所です。

上記で登場する ndt_ptr_ という変数は、NDT Scan Matcherの各実装を示しており、node起動時のパラメーターによってどの実装が使用されるか決まります。今回のOpenCL実装は、NormalDistributionTransformOCLクラスによって実装がなされている…かと思いきや、その実装はさらに、別の ndt_ptr_ (class名としてはndt_ocl::NormalDistributionsTransform) のalign関数、実際には親クラス (pcl::Registration) のalign関数を呼び出しており、この中で、その子クラスで実装される関数である、ndt_ocl::NormalDistributionsTransform::computeTransformationを呼び出しています。ここまでの相関関係を図示しておくと以下の通りになりますが、これは実装の形式の話であり、最終的に重要なものは、自己位置計算の実装である、computeTransformationです。

クラス/関数相関図

このcomputeTransformation関数の中では、マッチングスコアがよくなる方向へ移動、スコア計算の流れを何度かイテレーションし、スコア(正確にはスコアを元に計算した値)が事前設定した数値条件 (transformation_epsilon_) を満足したら、その時点での変換行列を最終的な結果として終了します。このようにalign処理は、移動とスコア計算のイテレーションという一連の流れのことであるとソースコードからわかります。

これ以降の処理追跡では、重要な部分を動的な観測から"あたり"をつけつつ、行っていきます。

さらに深くへ

ソースコード上では様々な処理が存在しますが、複雑な記述だからといって重要度が高い、あるいは処理負荷が高いといったのはよくある誤謬です。

今回はperfの結果可視化ツールのFlameGraphと、ValgrindのツールであるCallgrindの2つの結果を、ソースコードリーディングの座右に置きたいと思います。

github.com

ndt scan matcher FlameGraph

ndt scan matcher Call Graph

Call Graphにおいて上から3番目の緑のBOXが callbackSensorPoints であり、先程のコードリーディングで登場しました。ここを起点に下に目を移していきます。最初の枝分かれで、 init の方は 72:1 で呼び出しの頻度が低いため、左に降りていきましょう。すると computeDerivatives があり、これがFlameGraphにも登場している、処理割合が大きい関数です。

最終的に2つの関数にたどりつきます。それは updateDerivatives と radiusSearch でありますが、後者は前編で解説をした部分ですので、ここで updateDerivatives をもう少し掘り下げて見てみましょう。

ndt scan matcher Call Graph (detailed)

computeDerivatives は updateDerivatives および computePointDerivatives から成ることがわかります。

以上をまとめると、NDT align処理の主関数は下記のたった3つであり、これにより重点的に見ていく場所が明らかになりました。

1. radiusSearch
2. updateDerivatives
3. computePointDerivatives

computeDerivativesの中では、まずSource各点に対するTarget内の近傍探索、点の雰囲気とのマッチングスコア計算、そしてスコアがよくなる方向を上記の関数を用いて計算しています。

ここからはOpenCL実装の話ですが、上記の通り"Source各点に対する並列化"が容易であるため、1, 2, 3をSource各点に対して呼び出すカーネル関数 computeDerivatives を用意しています。

github.com

なお、Autowareが採用している OpenMP版 でも同様の並列化が、omp_parallelを用いて実装されています。

(補足) 地図のVoxel分割と地図KD木構築実装

OpenCL版で大きく異なるのは、むしろ地図のVoxel分割(およびそのKD木の構築)の部分であり、概念としては前編の通りですが、今一度package化に際しての実装を説明しておきます。

OpenCL版NDT Scan Matcherではndt_ocl::NormalDistributionsTransform::setInputTarget関数内のinitから、target_cells_.filter()関数(なお、target_cells_の型は、ndt_ocl::VoxelGridCovariance<PointTarget>) 、さらにPCLの関数を経由した後、最終的に voxel_grid_covariance_ocl_impl のこの箇所で木を構築しています。

OpenMPとの違いとしては、FLANN Libraryを経由せず、そのメモリ配置を voxel_grid_covariance_ocl_impl 内で任意に制御できる点にあります。この点は組み込み環境の制限されたメモリ環境においては必須となります。

そして最終的に作成された木メモリは、SVM (Shared Virtual Memory) argumentとしてkernelに渡され、デバイス側の近傍探索に使用されます。

スコア計算部の"命令実行的性質"

計算量といってしまうと、それはランダウ表記で示される、入力数に対するチューリングマシン上での操作手数の関係(スケーラビリティ)という抽象性を持つものになってしまいますが、ここではより具体的に、特定の命令セット命令を持つ(load/store型)計算機上における、命令の実行性質をみたいと思います。

computePointDerivatives

2つの行列演算といくつかの行列のエレメント生成から成ります。

• j_ang * x4 (<8, 4> * <4, 1>)
• h_ang * x4 (<16, 4> * <4, 1>)

いずれもかなり小規模な行列であるため、行列演算自体のカーネル化やSIMD packedな演算化は行っていません。実際にカーネルコードを、RISC-Vのコンパイラ(RV32if)でコンパイルした結果における、命令ヒストグラムを見てみます。

大体がflw(FPRメモリロード) → fmadd.s(乗算加算複合) → fsw(FPRメモリストア)の系統であり、またspill outも極端でないため、fsw << flwとなっていることがわかります。ちなみにfmv.w.xはGPR → FPR移動命令だったり、retは(仮想命令)関数リターン(jalr x0)だったりしますが、本筋とは関係ありません。ちなみに乗算加算複合命令はgccでは-O3でないと出現しないようです。

updateDerivatives

多数の小規模行列演算と、スカラー値演算、およびexponent演算が使用されているのが特徴的です(スカラー値演算も多数ありますが、以下では割愛しています)。

• e_x_cov_x<scalar> = exp(-gauss_d2<scalar> * x_trans4<1, 4>.dot(x_trans4<1, 4> * c_inv4<4, 4>) * 0.5f

• c_inv4_x_point_gradient4<4, 6> = c_inv4<4, 4> * point_gradient4<4, 6>

• x_trans4_dot_c_inv4_x_point_gradient4<6, 1> = x_trans4<1, 4> * c_inv4_x_point_gradient4<4, 6>

• x_trans4_x_c_inv4<1, 4> = x_trans4<1, 4> * c_inv4<4, 4>

• point_gradient4_colj_dot_c_inv4_x_point_gradient4_col_i<6, 6> = point_gradient4.transpose()<6, 4> * c_inv4_x_point_gradient4<4, 6>

• for i in 0 ... 5

• x_trans4_dot_c_inv4_x_ext_point_hessian_4ij.noalias()<6, 1> = x_trans4_x_c_inv4<1, 4> * point_hessian_.block<4, 6>(i * 4, 0)

最終的に、このupdateDerivativesの返却値がscoreとなり、これをinput点群数で割り正規化したものが、Transform Probabilityとして、推定のもっともらしさを表す数値として使用されることになります。

こちらも、RISC-Vコンパイラでのコンパイル結果とその命令分布を見てみましょう。

こちらはソースコードでも依存関係のある行列演算であるため、flwやfswよりもかなりfmul.s, fmadd.sの比重が大きいこととちょうど符合しています。

OpenCL実装について

紹介したOpenCL実装とOpenMP実装について、実際に(README記載の)サンプルを動作させて比較してみます。今回は、OpenCLもCPUにoffloadする形となっています。

Align TimeとTransform Probabilityの時間推移

結論としては、平均的にOpenCLの方がAlign Timeの時間は大きく、分散が小さいです。これは、OpenMPでは処理のロードバランシング等の最適化が行われており、応答性に関してはより優れた結果になっていると思われます。例えば、GPGPUやAccelerator等のCPU外のdeviceにもoffloadすることが可能な点がOpenCL版の魅力ではありますが、ナイーブな実装もあってCPUは少し苦手といった所のようです。

Transform Probabilityとしては同程度の推移です。

最後にFlameGraphを取得してみると、libgomp (OpenMP) libraryの代わりに、intel::internal::arena (Threading Building Block)が使用されていることがわかります。

ndt_ocl packageのREADMEでは、Intel環境におけるOpenCL 2.x環境の構築の例も紹介していますので、実行する際にぜひご参照ください。

リソース管理厳格化

OpenCLの実装にあたっては、使用する各種のリソースにも制限を意図的に加えています(いずれも設定が可能です)。

実際問題として車載環境のような組み込みによった環境では、動的で潤沢なメモリ確保ができない場合の方が多いですし、何より実行毎に予想できない挙動であることは、一つのリスクとなります。

重要なことは、平均でも最小でもなく、最大値を予め設定できるということです。

具体的には、NDT Scan Matchingにおいて、その処理量に影響を与える要素は3つしかなく、以下の3要素の最大値を予め定めさえすれば、処理時間を予測できるということになります。

1. LiDAR点数
2. 発見近傍Voxel数
3. スコア値計算の最大繰り返し数

OpenMP実装では、3のmax_iterationのみの制限でありますが、OpenCL実装では全てに以下のような制限をかけることができます。

• LIMIT_NUM (MAX_NEIGHBOR_VOXELS)
  • Radius Searchが発見する近傍Voxcelの限界数、これを超過すると単純に近傍として追加されず、Scan Matchingが不安定となる恐れがあります。
• MAX_PCL_INPUT_NUM (MAX_NUM_OF_LIDAR_POINTS)
  • LiDARからの入力点群数の最大値を設定する。

おわりに

前編/後編と2回にわたり、NDT Scan Matchingの計算面の調査を行い、その結果を一緒に眺めてきましたが、いかがでしたでしょうか。

途中から解析や、再実装の話に始終してしまいましたが、腰をすえてソースコードを追ってみる時、ただやみくもに読むよりも定量的に姿を捉えながら全体を理解していく、そんな一例をご紹介できたかと思います。

魔法のような自動運転のアルゴリズムも、実装となってしまえばただの計算です。Autowareはオープンなソフトウェアです。実際に読み、触れ、動作させることで理解ができます。臆することなく挑戦していきましょう！

安全への取り組み：③自動運転車のサイバーセキュリティについて

　こんにちは、ティアフォーのSafety Engineerの須永です。

ティアフォーの安全への取り組みを数回にわたって紹介していくシリーズ。時間がたってしまいましたが今回は第3回目になります。

　1回目は法律・ガイドラインについてでした。

tech.tier4.jp

　2回目は安心・安全についてでした。

　3回目の今回は自動車のサイバーセキュリティについての取り組みを説明していこうと思います。

　なお、ティアフォーでは、「自動運転の民主化」をともに実現していくサイバーセキュリティエンジニアを絶賛大募集しています。自動車および自動運転のサイバーセキュリティは最先端、かつ広域にわたる知識が必要になってきます。我こそは、という方が居ましたら、気軽にカジュアル面談からでも対応しますので是非応募ください。

自動車のサイバーセキュリティと安全

　自動車のサイバーセキュリティと安全にはどのような関係があるのでしょうか？

2015年に行われたBlack Hat USA 2015という大規模なセキュリティカンファレンスで、ホワイトハッカーであるCharlie Miller氏とChris Valasek氏がFiat Chrysler Automobiles（FCA）社のJeep Cherokeeという車両のハッキングに成功したことを報告し、140万台がリコールという話になりました。このハッキングは、はじめはWi-Fi経由でオーディオ音量を調整したりGPSで車両をトラッキングするなど、いわゆる車両内のマルチメディアシステムを掌握していました。さらにハッキングを進めると、マルチメディアシステムとは独立した車両の運転制御用のCANバスにアクセスできるまでになり、結果として外部から車両の運転制御までできるようになりました。つまり、車両を遠隔操作できることが分かったのです。ホワイトハッカーが実験的に行ったハッキングではあったものの、もし悪意のあるハッカーが遠隔操作で車両をコントロールすれば、故意に事故を起こすこともできるわけです。

youtu.be

自動車へのサイバー攻撃の現状

　2010年から2019年の10年間に公開された世界の車両サイバー攻撃インシデントの総件数は把握されている件数のみでも400件余り、2019年は単年で150件相当とのデータがあり、把握できていないものも含めるとその数倍はある可能性があります。このように、車両のサイバー攻撃は急増してきています。(参照：自動車へのサイバー攻撃の新常識と対応の考察)
　サイバー攻撃対象は、キーエントリシステムやテレマティクスや自動車メーカーのサーバ、カーシェアリングや自動車警報サービスなどのモバイルアプリケーションサーバといった自動車窃盗目的が多く、ホワイトハッカーの研究目的の攻撃から、悪意あるハッカーの攻撃に移行しているのは確実なようです。もし悪意のあるハッカーがJeep Cherokeeの事例のように遠隔操作で車両をコントロールすれば、故意に事故を起こすこともできるわけです。自動車のハッキングは最悪人命に関わることから、車両のサイバーセキュリティ侵害は安全侵害の一つになりえるとし、世界中が事態を重大にとらえている状況です。

自動車サイバーセキュリティのルール化

　Jeep Cherokeeのハッキングと時を同じくして、自動車業界はCASE(Connected・Autonomous・Shared & Services・Electric)時代といわれる100年に一度の変革期を迎えました。これらの背景を受けて、自動車業界や自動車のサイバーセキュリティルールを定めようとする国際的な動きが本格化したのです。

　主に国際間での自動車のルールを取り決める国連のワーキンググループWP29(自動車基準調和世界フォーラム)および国際規格を制定するISOでルール化が動きはじめます。これらの活動は国際協調するために整合するのに時間がかかり、WP29では2020年6月にUN-R-155(車両型式・サイバーセキュリティマネジメントシステム)・UN-R156(車両型式・ソフトウェアマネジメントシステム)として、またISOでは2021年の5月にFDIS としてISO/SAE 21434が正式な承認を迎える一歩手前まで来ている状況です。

　日本は自動車のサイバーセキュリティに関して各国を先導するように、WP29では共同議長または副議長を務めてきました。その活動の中で国土交通省は、2020年8月に自動車の特定改造等に関わる許可制度を発表しており対応が必要になっています。

　自動運転もまだ完全に完成されたソフトウェアが存在しないことから、適宜ソフトウェアをアップデートして機能をどんどん追加・改良していく段階です。市販車に搭載された場合でもアップデートしていけるようにOTA(Over-The-Air)が行われることが想定されています。また、自動運転システムになると車両だけでなくそれを取り巻くサブシステムとしてクラウドを利用します。また、先の説明でも行ったようにサイバーセキュリティ侵害がそのまま安全侵害に直結する可能性も出てきています。

　自動運転を見据えた規格では、SaFAD(Safety First for Automated Driving)が標準規格化されたISO/TR 4804や民間規格であるUL 4600もあります。これらの中でも、システムのライフサイクルの中でサイバーセキュリティの確保および適宜見直しを行ってアップデートしていくことが求められています。

自動車のサイバーセキュリティルールの概要

今まで説明してきたルールでは概ね考え方の連携ができており、サイバーセキュリティの守り方は2種類に大別されます。

1.　サイバーセキュリティ・ソフトウェアアップデートを運用できる組織体制

2.　サイバーセキュリティ・ソフトウェアアップデートを安全に行うためのリスクアセスメント

　1の運用できる組織体制とは、いわゆるCSMS(Cyber Security Management System)のことで、サイバーセキュリティに関わるリスクマネジメントを効果的・効率的に実施するための仕組みになります。サイバーセキュリティを完全にすることはできないため、一般的にはスイスチーズモデルのように何層かの対策を打つのが一般的ですが、深刻な脆弱性問題が出た時にすぐに対処できる組織体制が必要、かつ準備しておくことが必要ということになります。

　2のリスクアセスメントとは、人はミスをすることを前提に、設計手法(プロセス)の中でミスを見つけてつぶし、設計の変更があった場合はその影響を見つけて設計に反映させていくものです。この設計プロセスの中でミスを抑える手法は、安全規格であるISO 26262のV字プロセスと同じ手法になるため、V字を使用した開発プロセスという意味で兄弟的存在になります。また、製品の企画段階から廃棄までの製品ライフサイクルの中で活動していく部分に関しては同じく安全規格のISO/PAS 21448 (SOTIF) と兄弟的な存在になってきます。これからの自動車・自動運転車のルール・規格としてはこの3つの規格を総合・統合した形で見ていき、サイバーセキュリティを含んだ安全を確保していくことが業界の暗黙のルールになっています。

自動車・自動運転システムのサイバーセキュリティ規格と安全規格

自動車のサイバーセキュリティ開発プロセス

　安全とサイバーセキュリティは、同じような開発プロセスでルール化されていますが、当然全く同じというわけではないので、その違いについて簡単に説明していきます。

　まず、自動車の安全設計プロセスとして有名なISO 26262は、自動車でも特に車両の電気電子機器の機能安全(壊れても安全・壊れたら分かる)設計を行うもので、V字プロセスとして開発(設計・検証)を進めるものになります。対象のシステムの構成をアイテム定義し、定義したアイテムの潜在的・危険因子を特定するためのハザード分析を行い、壊れたらどのような危害を加えてしまうか危険度や被害度・回避可能性などのリスク評価を行います。そのリスク評価に対して安全目標を立ててリスクを許容可能なレベルまで落とせるように安全要件を作成し、設計・実装していきます。設計・実装したものが意図したとおり、効果あるものか確認・検証していきます。　

　併せて、重要な安全設計プロセスとしてのISO/PAS 21448では、システムとして未知で不安全である事象を減らし、既知で安全である事象を増やすことを続けていきます。これは車両・システムが破棄されるまで継続していき、安全性をどんどん上げていく取り組みになります。

自動車の安全開発プロセス

　同じように、自動車のサイバーセキュリティ設計プロセスについて、ISO/SAE 21434の概要をなぞる形で説明します。この規格は車両の電気電子機器の開発、生産、運用、保守を含めたサイバーセキュリティのリスク管理のためのエンジニアリング要件になっています。対象となるシステムの構成でアイテムおよび守るべき資産を定義し、定義したアイテム・資産のサイバーセキュリティ侵害による影響を脅威分析として実施し、攻撃の経路および影響からリスク評価を行います。そのリスク評価に対してサイバーセキュリティ目標を立てて許容可能なリスクレベルまで落とせるように要件を作成し、設計・実装していきます。設計・実装したものが意図したとおり、効果あるものか確認・検証していきます。さらにサイバーセキュリティポリシーを設定し、その製品に脆弱性があった場合は対策・対応およびインシデントの管理を車両が廃棄されるまで継続していきます。これらのサイバーセキュリティインシデントは、PSIRT(Product Security Incident Response Team)という組織横断的なチームによって管理していきます。

自動車のサイバーセキュリティ開発プロセス

安全分析・サイバーセキュリティ分析およびリスクアセスメントの差分

　開発領域では似たようなプロセスであるものの、安全・サイバーセキュリティ設計では大きく違うところがあります。

1.　サイバーセキュリティでは資産の特定がある

　サイバーセキュリティで侵害があると、影響があるステークホルダーを明らかにし、資産をCIA (秘匿性、完全性、可用性：Confidentiality、Integrity、Availability)の3要素に分類します。 サイバーセキュリティ侵害による影響は、無視できるものから深刻なものまで測定され、SFOP(安全性、財務、運用、プライバシー：Safety、Financial、Operational、Privacy)に分類します。安全は万人に影響がありますが、サイバーセキュリティは非常に範囲が広いので守るべき人や物に対して領域の線引きをする必要があるということを示しています。

2.　分析方法

　安全分析ではHARA(Hazard Analysis and Risk Assessment)を実施し安全性の評価をしていきます。この分析にはFTA(Fault Tree Analysis)・FMEA(Failure Mode and Effects Analysis)・FMEDA(Failure Modes, Effects, and Diagnostics Analysis)・HAZOP(HAZard and OPerability studies)・STAMP(Systems-Theoretic Accident Model and Processes) /STPA(STAMP based Process Analysis)などの多くの安全分析手法が使用されます。

　一方、サイバーセキュリティ分析ではTARA(Threat Analysis and Risk Assessment)を実施しサイバーセキュリティ評価をしていきます。ハザードが脅威(Threat)に置き換わったものになります。この分析手法にも様々な種類がありますが、DFD(Data Flow Diagram)によってデータの流れを可視化し、STRIDE法や5W法およびAttack Treeでサイバーセキュリティリスクを洗い出し、CVSS(Common Vulnerability Scoring System)で脆弱性を評価、対策を検討し、要件化を通して設計に反映していきます。

安全分析・リスクアセス・要求プロセス

サイバーセキュリティ分析・リスクアセス・要求プロセス

　車両のサイバーセキュリティは一般のコンピュータに比べると分析結果や知見など、まだまだなところがあります。また、自動運転システム自体巨大なシステムになっているのでサイバーセキュリティは安全と同様、膨大な分析と対策検討が必要になってきます。

　ティアフォーでは自動運転システムを搭載したPoC車両を開発しています。車両を改造してのシステムになるので、できる範囲からサイバーセキュリティ分析およびその対策を入れてはいますが、まだまだ発展途上という状況です。

サイバーセキュリティ分析抜粋

おわりに

　以上、簡単ではありますが、自動車のサイバーセキュリティに関する内容を説明させて頂きました。簡単に表現しすぎているところもあるのでルールの詳細はご確認ください。自動運転のサイバーセキュリティへの対応はソフトウェアに関する部分だけでなく、ハードウェアやシステム、さらに組織体制まで見る必要があり、安全と同様、もしくはそれ以上に大変なものです。したがって自動運転車での対応はそれだけ必要かつチャレンジングになっています。

　最後に、改めてになりますが、ティアフォーで自動運転システムのサイバーセキュリティ構築にチャレンジしたい方がおりましたら、以下のページよりコンタクトいただければと思います。カジュアルな面談も大歓迎ですので気軽にアクセスください！

自動運転の地図、ベクターマップについて

こんにちは、ティアフォーでベクターマップの作成ツールの開発をしている浦本と申します。今回は、Autowareで使われている地図の中でも、特にベクターマップとその作成ツールについてお話しようと思います。

なお、ティアフォーでは、「自動運転の民主化」をともに実現していく様々なエンジニア・リサーチャーを募集しています。もしご興味があればカジュアル面談も可能ですので以下のページからコンタクトいただければと思います。

tier4.jp

自動運転の地図

点群地図とベクターマップについて

Autowareで使われる地図には、自己位置推定のための点群地図と、その地図上の走行可能領域や信号・標識などを定義したベクターマップがあります。

Autowareは、道路や周囲の環境の形状を持っている点群地図と、走行中に取得したスキャンデータが最もマッチする重ね合わせから、車両の位置や向きを推定しています。この仕組みをスキャンマッチングといいます。スキャンマッチングについての詳細はこちらをご覧ください。

tech.tier4.jp

ベクターマップには、走行可能領域や信号・標識以外にも、速度制限、優先道路などの交通ルールや注意が必要なエリアのような情報を入れることができ、これらの情報を適切に設定することで、車両がその場所に適した走行をできるようにしています。

Autowareは現在、このベクターマップのフォーマットとしてLanelet2を採用しており、このフォーマットをAutoware用に一部拡張して使っています（これ以降、この記事ではLanelet2はAutoware用に拡張したものを指します）。ティアフォーで開発中のベクターマップ作成ツール、Vector Map Builder (VMB)もこのAutoware拡張のLanelet2の地図作成が可能です。以下の画像はこのツール上で地図を表示している例です。白い点群地図と色がついているベクターマップを重ねています。

Lanelet2について

ここでは、Lanelet2の詳細について説明したいと思います。

Lanelet2はOpenStreetMap（OSM）形式を拡張したもので、拡張子は.osm、ファイルに出力する際にはXML形式となります。Lanelet2は以下の基本オブジェクトで構成されますが、出力時にはOSMに合わせてnode、way、relationの3種類に分類されます。各基本オブジェクトの説明とファイルに出力した際のサンプルは以下のとおりです。なお、画像はVMBでの表示例です。

Point

• 一番基本となるオブジェクトで、座標や信号の色情報などを持ちます。
• OSMのnodeに分類されます。z座標はeleというタグで表されます。
• ローカル座標を使いたい場合は、local_x、local_yというタグを設定することもできます。
• lat/lonは緯度経度、local_x、local_y、eleの単位はメートルです。

<node id="1" lat="35.0000" lon="139.0000">
  <tag k="ele" v="19.000"/>
</node>

 

LineString

• Pointのリストを順番に線で結んだオブジェクト。LineString2つでLaneletを構成したり、信号機や一時停止線に使われます。
• OSMのwayに分類されます。
• 構成要素のPointをndタグで持ちます。
• 一時停止線はtypeをstop_line、点線はsubtypeをdashedにすることで表します。Laneletの境界線のLineStringをdashedにすると隣接レーンに車線変更が可能になるなど、タグの付け方を変えることで様々な設定が可能です。詳細はこちらをご覧ください。

  github.com

<way id="1">
 <nd ref="1"/>
 <nd ref="2"/>
 <nd ref="3"/>
 <nd ref="4"/>
 <tag k="type" v="line_thin"/>
 <tag k="subtype" v="solid"/>
</way>

 

Polygon

• Pointで囲まれた領域を表します。障害物検知領域を表すDetectionAreaなどで使われます。
• OSMのwayに分類され、area=yesというタグをつけます。

<way id="1">
 <nd ref="1"/>
 <nd ref="2"/>
 <nd ref="3"/>
 <nd ref="4"/>
 <tag k="type" v="detection_area"/>
 <tag k="area" v="yes"/>
</way>

 

Lanelet

• 左右にLineStringを1つずつ持ち、その間の領域を表します。向きを持っており、車線や横断歩道などに使われます。
• OSMのrelationに分類され、type=laneletというタグをつけます。
• 左右のLineStringをmemberとして持ちます。
• その車線で守る必要のある交通ルール（RegulatoryElement）がある場合は、それもmemberとして持ちます。
• 制限速度や通行対象（車や歩行者など）、一方通行かどうかなどの情報を持てます。

<relation id="1">
 <member type="way" role="left" ref="1"/>
 <member type="way" role="right" ref="2"/>
 <member type="relation" role="regulatory_element" ref="2"/>
 <tag k="type" v="lanelet"/>
 <tag k="subtype" v="road"/>
 <tag k="speed_limit" v="50"/>
 <tag k="location" v="urban"/>
 <tag k="participant:vehicle" v="yes"/>
 <tag k="one_way" v="yes"/>
</relation>

 

Area

• LineStringで囲まれた領域を表します。車両や人の通行領域を表す場合によく使われますが、Laneletと違って向きは持ちません。
• OSMのrelationに分類され、type=multipolygonというタグをつけます。
• 構成要素のLineStringをmemberとして時計回りの順番に持ちます。

<relation id="1">
 <member type="way" role="outer" ref="1"/>
 <member type="way" role="outer" ref="2"/>
 <tag k="type" v="multipolygon"/>
</relation>

 

※Polygonとの見た目の違いは、周囲がLineStringで囲まれているかどうかです。

RegulatoryElement

• 一時停止、信号、優先道路などの種類があり、それらがどの車線や一時停止線、信号機と紐付いているのかという情報を持っています。オブジェクト間の紐付けとルール情報を持つだけなので地図上に表示されるものではなく、画像はありません。
• OSMのrelationに分類されます。type=regulatory_elementというタグをつけます。
• subtypeでルールの種類を指定します（例：traffic_lightは信号、right_of_wayは優先道路）。

<relation id="1">
 <member type="way" role="refers" ref="1"/>
 <member type="way" role="light_bulbs" ref="2"/>
 <member type="way" role="ref_line" ref="3"/>
 <tag k="type" v="regulatory_element"/>
 <tag k="subtype" v="traffic_light"/>
</relation>

RegulatoryElementはルールの種類によってmemberの数や種類も変わってきます。例としてサンプルにも記載している信号を説明します。信号のmemberは3つで、refers、ref_line、light_bulbsがあります。それぞれtypeがwayとなっているので、LineStringがPolygonを表すのですが、信号に紐付くのは全てLineStringです。

まずはrefersですが、これは信号の外形を表します。ファイルに出力すると以下のwayのようになるのですが、ndの2と3は画像の丸（Point）の部分で信号機の幅を表し、heightタグで高さを表します。

<way id="1">
 <nd ref="2"/>
 <nd ref="3"/>
 <tag k="type" v="traffic_light"/>
 <tag k="height" v="0.450000"/>
</way>

 次にlight_bulbsは、以下のwayのようになり、traffic_light_idは信号機の外形のLineStringのidを表しています。ndの4から6はPointで、以下のidが4のnodeのような形でそれぞれcolorタグで信号の色情報を持ちます（画像の信号の場合はred、yellow、greenの３つ）。

<way id="2">
 <nd ref="4"/>
 <nd ref="5"/>
 <nd ref="6"/>
 <tag k="type" v="light_bulbs"/>
 <tag k="traffic_light_id" v="1"/>
</way>
<node id="4" lat="35.0000" lon="139.0000">
 <tag k="ele" v="19.546"/>
 <tag k="color" v="red"/>
</node> 

最後にref_lineですが、信号のref_lineは信号が赤だった時に停止する停止線を表すため、以下のwayのように、stop_lineというtypeを設定します。

<way id="3">
 <nd ref="7"/>
 <nd ref="8"/>
 <tag k="type" v="stop_line"/>
</way>

このようなオブジェクトを組み合わせて、ベクターマップを作っています。

Vector Map Builderの紹介

これまで説明してきたようなベクターマップを作成するためのツールとして、ティアフォーではVMBを開発しています。ブラウザで動作するツールで、TypeScript、React、Three.jsを主に使っています。

リンクはこちらになりますが、最初にティアフォーアカウントを作成いただいてからログインが可能となります。

tools.tier4.jp

ここでは、地図を作りやすくするために実装しているVMBの機能を一部紹介します。

1つ目は、点群地図の表示範囲指定機能です。この機能を使うケースとして、例えば屋内のベクターマップを作成するときに、点群に屋根が入ってしまっていてそのままでは屋内の形状が見にくいというものがあります。そういう場合は、この機能で不要な屋根部分の点群をカットし、見たい範囲の点群だけを見えるようにすることで、地図作成がしやすくなります。他にも、下の画像のように木が邪魔で車線が見づらい場合にも使えたり、上下の高さを指定できるので、ビルのような複数階を含む点群があったときには、見たい階だけを残して作業することも可能です。

   
2つ目は、地図のリアルタイムチェック機能です。車線や交通ルールが増えて地図が複雑になると、作成・編集時にミスが起こりやすくなり、チェックも大変になります。ミスが残ってしまっていても、地図作成後はシミュレータで動作チェックを実施するため、想定外の挙動をする場合はそこでわかるのですが、複数箇所が間違っているとVMBに戻って該当箇所を修正→走行チェックという作業を何度か繰り返さなければいけない場合があります。

VMBではそのような作業を少しでも減らすため、フォーマットとして間違っている点に加え、ティアフォーのこれまでの地図作成経験からリストアップしたチェック項目を作成作業中にチェックするようにしています。問題のある設定をすると、その設定を行った直後に問題のあるオブジェクトとその詳細を表示し、修正を促します。また、修正をしないまま地図の保存を行った場合は、問題点が残っていることを表示し、問題点に気づかずに保存してしまうことをできるだけ避けられるようにしています。

画像は設定している制限速度を超えたLaneletが存在する場合の例です。何が問題なのかという説明とIDを表示しており、IDをクリックすることで該当オブジェクトの設定が確認できるようになっています。

 

チェックすべき項目は、現在も実証実験等から問題を吸い上げたりしながら増やしており、今以上に充実させていく予定です。 

まとめ

今回は、Autowareで使われている地図の中でも、特にベクターマップについてお話しました。自動運転車がどのような地図を使って走行しているのか、イメージできたでしょうか。

ベクターマップは精度が求められるので作成時は細かな作業が多く、持たせる情報も多いことから設定とチェックに時間がかかります。これらの問題を解消し、より安全な地図をより早く作ることができるツール・仕組みの開発をこれからも進めていきます。