2020.2.12 修士論文発表会 M19INO03

先進自動車研究所 令和元年度報告資料

コーストレース制御向上に寄与する
リアルタイムな自車位置推定アルゴリズムの研究

神奈川工科大学
工学研究科 機械システム工学専攻
井上・小宮研究室

1884007 關野 修
Kanagawa Institute of Technology Inoue-Komiya Lab.

1

研究背景

全方位スキャンLiDAR

高精度3D地図

超高精度IMU※1と測量用GNSS※2を用いた複合航法システム+スキャンマッチング•

LiDARが百万円弱と高価

Fig.1 Automatic driving technology•

高精度3D地図が必要

research vehicle•

推定軌跡に連続性が無い•

ロバスト性に課題

デッドレコニング•

複合航法システムが数千万円と非常に高価※1 IMU: Inertial Measurement Unit
※2 GNSS: Global Navigation Satellites System
Kanagawa Institute of Technology Inoue-Komiya Lab.

2

研究背景

全方位スキャンLiDAR

高精度3D地図超高精度IMU※1と測量用GNSS※2を用いた複合航法システム+スキャンマッチング

Fig.1 Automatic driving technology
research vehicle•

LiDARが百万円弱と高価•

非常に高価なIMU
高精度3D地図が必要•

推定軌跡に連続性が無い•

ロバスト性に課題•

光ファイバージャイロ(Fiber Optic Gyro)•

リングレーザージャイロ(Ring Laser Gyro)

デッドレコニング•

複合航法システムが数千万円と非常に高価+

測量用GNSS

Kanagawa Institute of Technology Inoue-Komiya Lab.

3

研究背景
本研究
全方位スキャンLiDAR

高精度3D地図

超高精度IMU※1と測量用GNSS※2を用いた複合航法システム

MEMS IMU+低コスト化

GNSS receiver Wheel speed sensor
数万円

数万円

車両標準装備廉価なセンサ構成で、ロバストかつ高精度で車両制御にスキャンマッチング

デッドレコニング

適用可能なリアルタイム性のある自車位置推定Fig.1 Automatic driving technology
research vehicle•

LiDARが百万円弱と高価•

非常に高価なIMU
高精度3D地図が必要•

推定軌跡に連続性が無い•

ロバスト性に課題•光ファイバージャイロ(Fiber Optic Gyro)•

リングレーザージャイロ(Ring Laser Gyro)•

複合航法システムが数千万円と非常に高価+

測量用GNSS※MEMS: Micro Electro Mechanical Systems
Kanagawa Institute of Technology Inoue-Komiya Lab.

4

廉価なセンサによるデッドレコニングの課題• 車輪速センサから取得した車速は誤差を含む• 絶対的な方位角が不明• 温度ドリフトによる誤差を含む(停車中にヨーレイトの平均値を計算し、補正するのが一般的だが走行中の補正ができない)
Fig.2 MEMS IMU

上記の要因により積分誤差が発生してしまい、高精度な自車位置推定を期待できない吉田智章他：3Dスキャナとジャイロを用いた屋外ナビゲーションプラットホーム，計測自動制御学会論文集，Vol.47，No.10，p.493-500 (2011)

Kanagawa Institute of Technology Inoue-Komiya Lab.

5

廉価なセンサによるデッドレコニングの課題

目標精度• 車輪速センサから取得した車速は誤差を含む• 絶対的な方位角が不明• 温度ドリフトによる誤差を含む(停車中にヨーレイトの平均値を計算し、補正するのが一般的だが走行中の補正ができない)
100m走行で車両直交方向
Fig.2 MEMS IMU

0.6m以内の相対位置推定精度

Fig.3 Target accuracy

上記の要因により積分誤差が発生してしまい、高精度な自車位置推定を期待できない吉田智章他：3Dスキャナとジャイロを用いた屋外ナビゲーションプラットホーム，計測自動制御学会論文集，Vol.47，No.10，p.493-500 (2011)

Kanagawa Institute of Technology Inoue-Komiya Lab.

6

提案手法
MEMS IMU、車輪速センサに加えGNSS受信機を組み合わせ高精度化

GNSS Dopplerについて

Fig.4 GNSS Doppler

東方向速度 𝑉𝐸
北方向速度 𝑉𝑁
上方向速度 𝑉𝑈

Kojima et al．：Precise Localization using Tightly Coupled Integration based on Trajectory Estimated from GPS Doppler，Proc．of AVEC 2010，168，p.1-6 (2010)

Kanagawa Institute of Technology Inoue-Komiya Lab.

7

提案手法
車輪速スケールファクタ推定•

車輪速センサから取得した車速は誤差を含む→Dopplerから得られた車速を利用し、スケールファクタを推定し補正

方位角推定
•

絶対的な方位角が不明→Dopplerから得られた方位角を利用し、絶対的な方位角を推定

ヨーレイトオフセット推定(停止時・走行時)•

温度ドリフトによる誤差を含む→停車時は平均値、走行時は推定した方位角を利用し推定積分誤差を低減し、自車位置推定精度を向上

Kanagawa Institute of Technology Inoue-Komiya Lab.

8

提案手法
車輪速スケールファクタ推定
𝐷𝑜𝑝𝑝𝑙𝑒𝑟による推定速度 𝑉𝐷 =SF=
GNSS受信時かつ𝑉𝐸2 + 𝑉𝑁2 + 𝑉𝑈2

VD

VCAN

一定車速以上車輪速センサから

Doppler

取得した車速車速=
GPS DopplerからCAN算出した車速

車輪速
リングバッファー

Kanagawa Institute of Technology Inoue-Komiya Lab.

処理前
スケール
ファクタ

中央値

SF

補正車速 𝑉ത = VCAN ・𝑆𝐹
9

提案手法
方位角推定
※ヨーレイトオフセット推定開始後は、補正されたヨーレイトを積分

𝐷𝑜𝑝𝑝𝑙𝑒𝑟による推定方位角 𝜃𝐷= tan−1

𝑉𝐸
𝑉𝑁

ヨーレイトの積分値(ヨー角)GPS Dopplerから算出した方位角
リングバッファー

Process 1

Kanagawa Institute of Technology Inoue-Komiya Lab.

Process 210

提案手法
方位角推定
残差の最大値がしきい値を超えていたらバッファーから該当箇所を削除する

残差の絶対値
暫定方位角

Process 3Process 4

Process 5

Doppler
方位角

しきい値を超えた値がもうない→暫定方位角=方位角未推定時→前回推定した方位角+ヨーレイト積分値=方位角
Kanagawa Institute of Technology Inoue-Komiya Lab.

ヨー角

Process 1へ

11

提案手法
ヨーレイトオフセット推定(停車中)=停止時平均
ヨーレイトオフセット推定(走行中)最小二乗法で一次式に近似

方位角推定値

IMUヨーレイト
積分値
リングバッファー

Process 1

Process2

Process3

傾き × (−1)= 走行時ヨーレイトオフセット
Kanagawa Institute of Technology Inoue-Komiya Lab.

12

提案手法
ヨーレイトオフセット推定(停車中)=停止時平均
ヨーレイトオフセット推定(走行中)最小二乗法で一次式に近似

方位角推定値

IMUヨーレイト
積分値
リングバッファー

Process 1

Process2

Process3

傾き × (−1)= 走行時ヨーレイトオフセット
Kanagawa Institute of Technology Inoue-Komiya Lab.

13

提案手法
ヨーレイトオフセット推定(停車中)=停止時平均ポイント
方位角推定にはヨーレイトオフセット誤差推定が必要
ヨーレイトオフセット推定(走行中)最小二乗法で一次式に近似

方位角推定値

IMUヨーレイト
積分値
リングバッファー

方位角推定

ヨーレイトオフセット推定

Process 1

Process2

Process3

ヨーレイトオフセット推定には方位角が必要

推定を繰り返すことで精度向上
傾き × (−1)= 走行時ヨーレイトオフセット
Kanagawa Institute of Technology Inoue-Komiya Lab.

14

自車位置推定性能評価試験
評価方法
コース:
→神奈川県厚木市 市街地路
リファレンス:→測量用 RTK-GNSS
提案手法、従来手法用センサ:→MEMS IMU

Fig.5 RTK-GNSS→汎用GNSS受信機→車輪速センサ(車両CAN)
10m走行毎に100m走行後の誤差を比較
Fig.6 Evaluation route2D誤差
進行方向誤差Fig.7 Evaluation methods

直交方向誤差※従来手法:車輪速センサ生値+IMUヨーレイト(停止時平均値で温度ドリフト補正)
Kanagawa Institute of Technology Inoue-Komiya Lab.

15

自車位置推定性能評価試験

Fig.8 2D誤差

Fig.9 進行方向誤差

Fig.10 直交方向誤差※縦軸に示す%はリファレンスと比較し、横軸に示す誤差(m)以内の推定を何%達成しているかを示す
Kanagawa Institute of Technology Inoue-Komiya Lab.

16

車両制御評価試験
評価方法
LiDARと3D高精度地図データを用いたNDTスキャンマッチング(Normal Distributions Transform ScanMatching)による自車位置推定結果と本アルゴリズムによる自車位置推定結果それぞれを用いて車両制御を行い評価コース:→神奈川工科大学構内路 および 厚木中
央自動車学校3D地図データ:→移動式高精度3次元計測システムであるMMS(Mobile Mapping System)で作成した3D地図データ
NDTスキャンマッチングおよび経路追従(Pure pursuit):→自動運転向けオープンソースソフト
ウェアAutowareを使用

Kanagawa Institute of Technology Inoue-Komiya Lab.

Fig.11 MMS

Fig.12 Automatic driving technology
research vehicle

17車両制御評価試験

システム:
コース詳細:

一周約193m

Fig.14 Course in Kanagawa Institute of Technology
NDTスキャンマッ
チング

センサ

位置推定

一周約272m
経路追従

車両逆モデル本アルゴリズム
位置推定

Fig.13 System configuration※本アルゴリズムでは、絶対位置は推定できないため、スタート位置で停車中のNDTスキャンマッチングの位置を初期位置として計算

Kanagawa Institute of Technology Inoue-Komiya Lab.

Fig.15 Course in Atsugi Chuoh Driving School18

車両制御評価試験
位置推定結果比較結果

Fig.16 Vehicle position estimation expansion when going straight※縦軸横軸に示す座標は平面直角座標系の系番号9を基準にした絶対位置を表す
Kanagawa Institute of Technology Inoue-Komiya Lab.

19

車両制御評価試験
ハンドル角指令値、計測値比較結果
赤線→ハンドル角計測値
青線→ハンドル角指令値

本アルゴリズムでの制御時

Fig.17 Steering command value by IMU&GNSS algorithm, actual value

Kanagawa Institute of Technology Inoue-Komiya Lab.

NDTスキャンマッチングでの制御時

Fig.18 Steering command value by NDT scan matching, actual value

20

結言MEMS IMU
数万円

GNSS receiver Wheel speed sensor
数万円

車両標準装備廉価なセンサ構成で、ロバストかつ高精度で車両制御に適用可能なリアルタイム性のある自車位置推定

100m走行で0.6m以内の高周期かつ連続性の高い位置推定

相対位置推定精度を95%達成

によりなめらかな車両制御の実現

さらなる高精度化→GNSSドップラによるタイヤ横滑り角の推定
絶対位置推定→緯度経度情報やNDTスキャンマッチングなどとの統合Kanagawa Institute of Technology Inoue-Komiya Lab.

21