JP6907952B2 - 自己位置補正装置及び自己位置補正方法 - Google Patents

Description

本開示は、自己位置補正装置及び自己位置法制方法に関する。

特許文献１には、地物を利用して、自己位置を補正する技術が記載されている。この技術では、デッドレコニングにより車両の自己位置を測位し、測位した際の特定の方向をカメラで撮像し、自己位置を起点として撮像した画像に写る特定の地物の方位角を算出する。また、データベースから特定の地物の位置を取得して、測位した自己位置を起点とし、データベース上の特定の地物の方位角を算出する。そして、これらの方位角の差分に基づいて、測位部が測位した自己位置の誤差を推定し、測位した自己位置を誤差に基づいて補正する。

特開２００８−２４９６３９号公報

しかし、自己位置の前後方向の誤差を、より精度よく補正可能な技術が望まれていた。

本開示は、上述の課題を解決するためになされたものであり、以下の形態として実現することが可能である。

本開示の一形態によれば、移動装置（１０）において取得された前記移動装置の走行情報を用いて計算された、前記移動装置が走行する走路の特徴である走路特徴量を逐次取得する第１取得部（１１０）と；前記走路特徴量と走路の位置とが対応付けられた地図情報（２１０）を用いて、前記移動装置において測位された前記移動装置の自己位置に対応する前記走路特徴量を逐次取得する第２取得部（１２０）と；前記第２取得部により取得された前記走路特徴量から得られる第２走行軌跡（Ｆ２,ｆ２）を、前記第１取得部により取得された前記走路特徴量から得られる第１走行軌跡（Ｆ１,ｆ１）に近づけるための、前記第２走行軌跡のずらし量（α）を算出する算出部（１３０）と；前記ずらし量を用いて、前記移動装置において測位された前記自己位置を補正する補正部（１４０）と、を備える。前記算出部は、前記第１走行軌跡における前記走路特徴量と、前記第２走行軌跡をずらした後の軌跡における前記走路特徴量との差の積分値が最も小さくなるずらし量を、前記補正部が使用する前記ずらし量として算出する。前記補正部は、前記ずらし量を変化させた場合における前記差の変化が小さいほど、前記自己位置を補正する量を、前記差が最も小さくなる前記ずらし量よりも少なくする。

この形態の自己位置補正装置によれば、自己位置測位部の測位結果に対応する走路特徴量から得られる第２走行軌跡を、移動装置の走行情報を用いて計算される走路特徴量から得られる第１走行軌跡に重ね合わせるための、第２走行軌跡のずらし量を用いて、移動装置の自己位置が補正される。そのため、自己位置の補正に、走路特徴量の連続的な変化が用いられるので、移動装置の前後方向における自己位置のずれを、精度よく補正することができる。

自己位置補正装置が搭載された車両の概略構成を示すブロック図。 自己位置補正処理のフローチャート。 第１走行軌跡と第２走行軌跡とずらし量を示す図。 車両の実際の位置と自己位置測位部により測位される自己位置とを示すイメージ図。 ずらし量と、走路特徴量の差としての面積と、の関係を示す図。 第１走行軌跡と第２走行軌跡とずらし量を示す図。 ずらし量と、走路特徴量の差としての面積と、の関係を示す図。

・第１実施形態
図１及び図２に基づき、第１実施形態の自己位置補正装置５０の構成を説明する。自己位置補正装置５０は、走路を移動する機能を有する移動装置に搭載される。本実施形態では、移動装置は、車両１０である。車両１０は、自己位置補正装置５０に加え、走行情報取得部２０と、自己位置測位部３０と、を備える。車両１０は、自己位置補正装置５０に接続され、自己位置補正装置５０の出力結果を用いるナビゲーションシステムや、自動運転システムを備えるが、ナビゲーションシステム、自動運転システムについては図示及び詳細な説明は省略する。

走行情報取得部２０は、車両１０の走行情報を取得するセンサ群である。走行情報取得部２０は、車両１０の周囲を計測するセンサ群、車両１０の走行状態を計測するセンサ群により構成されている。センサ群は、撮像カメラ、ミリ波センサ、ＬｉＤＡＲ（Light Detection And Ranging又はLaser Imaging Detection And Ranging）、車速センサ、ヨーレートセンサ、加速度センサ、ジャイロセンサ等であり、走行情報は、これらのセンサ群の検出結果である。本第１実施形態では、走行情報取得部２０は、少なくとも、撮像カメラを含む。撮像カメラは、単眼カメラであり、車両１０の前方を撮像する。撮像カメラは、ステレオカメラであってもよい。走行情報取得部２０は、取得した走行情報を自己位置補正装置５０に入力する。

自己位置測位部３０は、車両１０の自己位置を測位して、自己位置補正装置５０に入力する。自己位置は、車両１０の緯度・経度である。自己位置測位部３０は、ＧＮＳＳ（Global Navigation Satellite System）受信機として構成されており、ＧＮＳＳを構成する人工衛星から受信する電波を用いて、車両１０の自己位置を検出する。

自己位置補正装置５０は、ＣＰＵ１００、メモリ２００及び信号の送受信を行うインタフェース６０を搭載したＥＣＵ（Electronic Control Unit）により構成されている。メモリ２００には、自己位置補正プログラムと、地図上の座標位置である経度・緯度と走路特徴量が対応付けられた地図情報２１０と、が記憶されている。自己位置補正装置５０は、ＣＰＵ１００がメモリ２００に記憶されたプログラムを展開して実行することにより、走路特徴量計算部１０５、第１取得部１１０、第２取得部１２０、算出部１３０及び補正部１４０として機能する。自己位置補正装置５０は、自己位置補正機能がオンにされている間、図２に示される自己位置補正処理を繰り返す。以下、自己位置補正装置５０の各部の機能及び自己位置補正処理について説明する。

走路特徴量計算部１０５は、走行情報を用いて走路特徴量を計算する。走路特徴量とは、車両１０が走行する走路の曲率や勾配であり、本実施形態では走路の曲率である。走路特徴量計算部１０５は、例えば、車両１０の進行方向において車両１０の前部から所定距離の位置での曲率を、撮像カメラの撮像結果にあらわれる道路上の区画白線の位置を用いて計算する。走路特徴量計算部１０５は、例えば、特開２０１３−１９６３４１号公報に記載の方法を用いて曲率を計算してもよいし、特開２０１７−２０６１１９号公報に記載の方法を用いて曲率を計算してもよい。

第１取得部１１０は、走路特徴量計算部１０５により計算された走路特徴量を逐次取得する（ステップＳ１０）。以下、第１取得部１１０が取得する走路特徴量を、「第１走路特徴量」とも呼ぶ。第１取得部１１０は、取得した第１走路特徴量を、メモリ２００に逐次記憶する。

第２取得部１２０は、自己位置測位部３０により測位された自己位置に対応する走路特徴量を、地図情報２１０から取得する（ステップＳ２０）。第２取得部１２０が取得する走路特徴量を、「第２走路特徴量」とも呼ぶ。第２取得部１２０は、取得した第２走路特徴量を、メモリ２００に逐次記憶する。なお、自己位置補正装置５０は、第１走路特徴量の取得（ステップＳ１０）と、第２走路特徴量の取得（ステップＳ２０）とを、並行して実行する。

本実施形態では、第１取得部１１０は、第１走路特徴量を、走行情報取得部２０が走行情報を取得した際の車両１０の走行距離と対応付けて、メモリ２００に記憶する。第２取得部１２０は、第２走路特徴量を、自己位置測位部３０が自己位置を測位した際の車両１０の走行距離と対応付けて、メモリ２００に記憶する。すなわち、メモリ２００には、車両１０の第１走路特徴量に基づく第１走行軌跡Ｆ１と、第２走路特徴量に基づく第２走行軌跡Ｆ２と、が逐次記憶される。他の実施形態では、第１走路特徴量は、走行情報取得部２０が走行情報を取得した際の時刻に対応付けて記憶されてもよく、第２走路特徴量は、自己位置測位部３０が自己位置を測位した際の時刻に対応付けて記憶されてもよい。

算出部１３０は、メモリ２００を参照して、現在時刻から予め定められた過去の時刻までの第１走行軌跡Ｆ１と、第２走行軌跡Ｆ２とを取得する。算出部１３０は、取得した第２走行軌跡Ｆ２を第１走行軌跡Ｆ１に重ね合わせるための、第２走行軌跡Ｆ２のずらし量を算出する（ステップＳ３０）。図３には、横軸ｘを走行距離、縦軸ｙを曲率として、同一期間における第１走行軌跡Ｆ１と第２走行軌跡Ｆ２とが示されている。図４には、車両１０の実際の位置Ｓ１と、自己位置測位部３０により測位された自己位置Ｓ２と、車両１０が走行した走路Ｔと、が示されている。自己位置測位部３０により測位される自己位置Ｓ２は、図４に示すように、車両１０の実際の位置Ｓ１からずれる場合がある。このずれは、例えば、車両１０が走路特徴量変化の大きい走路を走行する場合や、ＧＮＳＳ信号の受信性が悪い走路を走行する場合に、発現し得る。そのため、図３に示すように、第１走行軌跡Ｆ１を表す関数（ｙ＝Ｆ１（ｘ））と第２走行軌跡Ｆ２を表す関数（ｙ＝Ｆ２（ｘ））とが一致しない場合がある。本実施形態では、算出部１３０は、関数ｙ＝Ｆ１（ｘ）と、関数ｙ＝Ｆ２（ｘ）−αで囲まれる部分の面積が小さくなるように、関数ｙ＝Ｆ２（ｘ）をずらすことで、第２走行軌跡Ｆ２を第１走行軌跡Ｆ１に重ね合わせる。算出部１３０は、ずらし量αと、関数ｙ＝Ｆ１（ｘ）と関数ｙ＝Ｆ２（ｘ）−αで囲まれる部分の面積Ｓと、の関係を算出する（図５）。面積Ｓは、ずらし量αだけずらされた第２走行軌跡Ｆ２と、第１走行軌跡Ｆ１と、の現在時刻から予め定められた過去の時刻までにおける走路特徴量の差の積分値を示す。面積Ｓが小さくなることは、ずらし量αだけずらされた第２走行軌跡Ｆ２と、第１走行軌跡Ｆ１と、の現在時刻から予め定められた過去の時刻までにおける、走路特徴量の差が小さくなることでもある。

補正部１４０は、算出されたずらし量αを用いて、自己位置測位部３０の測位結果を補正する（ステップＳ４０）。本実施形態では、補正部１４０は、図５に示す面積Ｓが最も小さくなるずらし量αを、車両１０の前後方向誤差とみなし、ずらし量αを補正量として用いる。補正部１４０は、自己位置Ｓ２を、ずらし量αだけ車両１０の前後方向と同じ方向にずらすことで、車両１０の自己位置Ｓ２を補正する。補正部１４０は、自己位置Ｓ２が測位された際の車両１０の進行方向を車両１０の前方向、後退方向を車両１０の後方向とする。補正部１４０は、ずらし量αが正の値である場合には、自己位置Ｓ２を、車両１０の前方向にαに対応する緯度・経度分ずらす。補正部１４０は、ずらし量αが負の値である場合には、自己位置Ｓ２を、車両１０の後方向にαに対応する緯度・経度分ずらす。ＣＰＵ１００は、補正した自己位置をインタフェース６０を介して出力する。車両１０のナビゲーション装置に関するＥＣＵや、車両１０の自動運転を実行するＥＣＵは、ＣＰＵ１００から出力された自己位置を用いて、車両１０のナビゲーションや、自動運転を実行する。

この形態によれば、自己位置測位部３０の測位結果に対応する走路特徴量から得られる第２走行軌跡Ｆ２を、車両１０の走行情報を用いて計算される走路特徴量から得られる第１走行軌跡Ｆ１に重ね合わせるための、第２走行軌跡Ｆ２のずらし量αを用いて、車両１０の自己位置Ｓ２が補正される。そのため、自己位置Ｓ２の補正に、走路特徴量の連続的な変化が用いられるので、車両１０の前後方向における自己位置Ｓ２のずれを、精度よく補正することができる。

この形態によれば、走路特徴量は曲率であるため、曲率が連続的に変化する走路、例えば、カーブの出入り口付近における車両１０の前後方向における自己位置Ｓ２のずれを、精度よく補正することができる。

この形態によれば、第１走行軌跡Ｆ１を表す関数ｙ＝Ｆ１（ｘ）と、第２走行軌跡Ｆ２を表す関数ｙ＝Ｆ２（ｘ）を走行距離に対してαだけずらした関数ｙ＝Ｆ２（ｘ）−αで囲まれる部分の面積が、最小になるずらし量αの値を用いて、車両１０の自己位置Ｓ２が補正される。そのため、現在時刻から予め定められた過去の時刻までの、走路特徴量の差が最も小さくなるように、自己位置Ｓ２が補正されるので、車両１０の前後方向における自己位置Ｓ２のずれを、精度よく補正することができる。

・第２実施形態
図６には、第１走行軌跡ｆ１と第２走行軌跡ｆ２の他の例が示されている。図７には、ずらし量αと、関数ｙ＝ｆ１（ｘ）と関数ｙ＝ｆ２（ｘ）−αで囲まれる部分の面積Ｓと、の関係が示されている。図６の走行軌跡ｆ１、ｆ２は、図３に示す走行軌跡Ｆ１、Ｆ２に比べて、曲率が小さい走路を走行した場合の走行軌跡である。走路特徴量の変化が小さい走路を走行する場合は、走路特徴量の変化が大きい走路を走行する場合と比べて、ずらし量αの変化に対する面積Ｓの変化が小さい（図５、図７）。

第２実施形態では、補正部１４０は、第２走行軌跡ｆ２のずらし量αを変化させた場合における、面積Ｓの変化が小さいほど、自己位置Ｓ２を補正する量を、面積Ｓが最も小さくなるずらし量αよりも少なくする。メモリ２００には、ずらし量αに対する面積Ｓの変化量と、ずらし量αに乗算する係数と、の関係が記憶されており、補正部１４０は、面積Ｓの変化量に対応する係数を取得して、ずらし量αに係数を乗算した値を、自己位置Ｓ２の前後方向の補正量とする。係数の最大値は、１．０である。このように、補正部１４０は、ずらし量αを変化させた場合における、第１走路特徴量と第２走路特徴量との差の変化に応じて、自己位置Ｓ２の前後方向の補正量を決定する。第２実施形態の自己位置補正装置５０の各部が実行する機能のその他の点については、第１実施形態と同様であるため、説明を省略する。

この形態によれば、補正部１４０は、ずらし量αを変化させた場合における、第１走路特徴量と第２走路特徴量との差の変化に応じて、自己位置Ｓ２の前後方向の補正量を決定するので、自己位置Ｓ２の前後方向の補正量が大きくなりすぎることを抑制することができる。そのため、車両１０の前後方向における自己位置Ｓ２のずれを、より精度よく補正することができる。

・第３実施形態
第３実施形態では、走路特徴量は、走路Ｔの勾配である。走路特徴量計算部１０５は、ジャイロセンサ、加速度センサや車速センサの検出結果から勾配を計算する。例えば、走路特徴量計算部１０５は、車両１０のピッチ方向（上下方向）にも感度をもつように構成された３Ｄジャイロセンサから、車両１０の角速度を得て回転運動量を求めることで、車両１０の走行する走路Ｔの勾配を検出してもよい。あるいは、加速度センサが検出する加速度から、車速センサにより検出された車速を微分したものを減じて、重力加速度の車両進行方向成分を得る。ここで重力加速度が既知であるので、勾配角の正弦が得られ、その逆関数をとれば、道路勾配角を得られる。第１取得部１１０は、走路特徴量計算部１０５から勾配を取得して、メモリ２００に逐次記憶する。また、第３実施形態では、地図情報２１０には、地図上の座標位置と走路特徴量としての勾配と、が対応付けて記憶されており、第２取得部１２０は、自己位置測位部３０の測位結果に対応する勾配を地図情報２１０から取得して、メモリ２００に逐次記憶する。第３実施形態の自己位置補正装置５０の各部が実行する機能のその他の点については、第１実施形態と同様であるため、説明を省略する。

この形態によれば、走路の勾配を用いて自己位置Ｓ２を補正することができるので、勾配が連続的に変化する走路における車両１０の前後方向における自己位置Ｓ２のずれを、精度よく補正することができる。

・他の実施形態１
算出部１３０は、現在時刻から予め定められた過去の時刻までの第１走行軌跡Ｆ１、ｆ１、第２走行軌跡Ｆ２、ｆ２を取得し、第２走行軌跡Ｆ２、ｆ２のピーク値を示す走行距離を、第１走行軌跡Ｆ１、ｆ１のピーク値を示す走行距離に一致させた場合の、第２走行軌跡Ｆ２、ｆ２のずらし量を、自己位置Ｓ２を補正するためのずらし量αとして算出してもよい。この形態によれば、第２走行軌跡Ｆ２、ｆ２の走路特徴量の変化率が大きな箇所を、第１走行軌跡Ｆ１、ｆ１の走路特徴量の変化率が大きな箇所に一致させることにより、車両１０の前後方向における自己位置Ｓ２のずれを、精度よく補正することができる。

・他の実施形態２
車両１０は、自己位置測位部３０としてデッドレコニングセンサを備えていてもよい。第２取得部１２０は、デッドレコニングセンサを用いて測位された自己位置Ｓ２に対応する走路特徴量を地図情報２１０から取得してもよい。

・他の実施形態３
上記各実施形態は適宜組み合わせられてもよい。第１実施形態と第３実施形態とを組み合わせる場合には、例えば、曲率と勾配のうち、第２走行軌跡Ｆ２、ｆ２のずらし量αに対する面積Ｓの変化の大きい方のずらし量αを用いて、車両１０の前後方向のずれを補正してもよい。

・他の実施形態４
上記実施形態において、地図情報２１０は、車両１０の外部に設けられたサーバに保存されていてもよい。自己位置補正装置５０は、自己位置測位部３０の測位結果に対応する走路特徴量を、サーバから受信可能なように構成されていてもよい。

・他の実施形態５
上記実施形態において、自己位置補正装置５０の各部の実行する機能は、車両１０の外部に設けられたサーバにより行われてもよい。車両１０は、走行情報取得部２０による検出結果や、自己位置測位部３０による測位結果を外部サーバに送信し、サーバは、これらの結果を取得して演算することにより、自己位置Ｓ２を補正してもよい。

・他の実施形態６
上記形態では、移動装置として車両１０を挙げたが、自己位置補正装置５０による自己位置補正の対象となるのは、路面を移動する移動装置であれば、他の輸送用機器（例えば、二輪車）でもよいし、ロボットでもよい。ロボットの場合、例えば、自動車のように車輪で路面に接地してもよいし、２足歩行するタイプでもよい。

本開示は、自己位置補正装置５０以外の種々の形態で実現することも可能である。例えば、自己位置補正方法、かかる方法を実現するためのコンピュータプログラム、かかるコンピュータプログラムを記憶した記憶媒体、自己位置補正装置５０を搭載した車両１０等の形態で実現することができる。また、上記実施形態において、ソフトウエアによって実現された機能及び処理の一部又は全部は、ハードウエアによって実現されてもよい。また、ハードウエアによって実現された機能及び処理の一部又は全部は、ソフトウエアによって実現されてもよい。ハードウエアとしては、例えば、集積回路、ディスクリート回路、又は、それらの回路を組み合わせた回路モジュールなど、各種回路を用いてもよい。

１０ 車両、２０ 走行情報取得部、３０ 自己位置測位部、５０ 自己位置補正装置、６０ インタフェース、１０５ 走路特徴量計算部、１１０ 第１取得部、１２０ 第２取得部、１３０ 算出部、１４０ 補正部、２１０ 地図情報、Ｆ１,ｆ１ 第１走行軌跡、Ｆ２,ｆ２ 第２走行軌跡

Claims (5)

1. 移動装置（１０）において取得された前記移動装置の走行情報を用いて計算された、前記移動装置が走行する走路の特徴量である走路特徴量を逐次取得する第１取得部（１１０）と、
   前記走路特徴量と走路の位置とが対応付けられた地図情報（２１０）を用いて、前記移動装置において測位された前記移動装置の自己位置に対応する前記走路特徴量を逐次取得する第２取得部（１２０）と、
   前記第２取得部により取得された前記走路特徴量から得られる第２走行軌跡（Ｆ２,ｆ２）を、前記第１取得部により取得された前記走路特徴量から得られる第１走行軌跡（Ｆ１,ｆ１）に近づけるための、前記第２走行軌跡のずらし量（α）を算出する算出部（１３０）と、
   前記ずらし量を用いて、前記移動装置において測位された前記自己位置を補正する補正部（１４０）と、を備え、
   前記算出部は、前記第１走行軌跡における前記走路特徴量と、前記第２走行軌跡をずらした後の軌跡における前記走路特徴量との差の積分値が最も小さくなるずらし量を、前記補正部が使用する前記ずらし量として算出し、
   前記補正部は、前記ずらし量を変化させた場合における前記差の変化が小さいほど、前記自己位置を補正する量を、前記差が最も小さくなる前記ずらし量よりも少なくする、
   自己位置補正装置。
2. 請求項１に記載の自己位置補正装置であって、
   前記走路特徴量は前記走路の曲率又は勾配である、自己位置補正装置。
3. 請求項１に記載の自己位置補正装置であって、
   前記補正部は、前記積分値の変化量に対する１．０以下の係数を前記ずらし量に乗算した値を用いて、前記移動装置において測位された前記自己位置を補正する、自己位置補正装置。
4. 請求項１から３のいずれか一項に記載の自己位置補正装置であって、
   前記第１走行軌跡および前記第２走行軌跡は、走行距離と前記走路特徴量との関係を示す関数であり、
   前記ずらし量は、前記移動装置の進行方向または後退方向の走行距離に対応する、自己位置補正装置。
5. 移動装置の走行情報を用いて計算された、前記移動装置が走行する走路の特徴量である走路特徴量を、逐次取得する工程（Ｓ１０）と、
   前記走路特徴量と走路の位置とが対応付けられた地図情報を用いて、前記移動装置において測位された前記移動装置の自己位置に対応する前記走路特徴量を逐次取得する工程（Ｓ２０）と、
   前記地図情報を用いて取得された前記走路特徴量から得られる第２走行軌跡を、前記走行情報を用いて計算された前記走路特徴量から得られる第１走行軌跡に近づけるための、前記第２走行軌跡のずらし量を算出する工程（Ｓ３０）と、
   前記ずらし量を用いて、前記移動装置において測位された前記自己位置を補正する工程（Ｓ４０）と、を備え、
   前記第２走行軌跡のずらし量を算出する工程において、前記第１走行軌跡における前記走路特徴量と、前記第２走行軌跡をずらした後の軌跡における前記走路特徴量との差の積分値が最も小さくなるずらし量を、前記ずらし量として算出し、
   前記自己位置を補正する工程において、前記ずらし量を変化させた場合における前記差の変化が小さいほど、前記自己位置を補正する量を、前記差が最も小さくなる前記ずらし量よりも少なくする、
   自己位置補正方法。

Images