JP6384604B2 - 自己位置推定装置及び自己位置推定方法 - Google Patents

Description

本発明は、移動体の自己位置を推定する自己位置推定装置及びその方法に関する。

移動体に搭載されたカメラやレーザレンジファインダを用いて移動体の周囲をセンシングし、その結果と地図とをマッチングして、地図上の位置や姿勢角を求める自己位置推定技術が開発されており、従来では特許文献１が開示されている。

特許文献１の自己位置推定技術では、車輪速パルスやジャイロ等で算出した移動量とセンシングした結果の過去の履歴とを蓄積して連結し、この結果と地図が一致するように調整して移動体の位置と姿勢角を推定している。また、特許文献１の自己位置推定技術では、常に一定距離のセンシング結果の履歴を用いることによって、移動体の移動速度の影響を排除し、自己位置の推定を安定して行えるようにしている。

特開２００８−２５０９０６号公報

しかしながら、上述した従来の自己位置推定技術では、一定距離のセンシング結果の履歴を用いていたので、移動量の検出誤差が大きくなる状況では、センシング結果と地図との間の乖離が大きくなって自己位置の推定が困難になるという問題点があった。

そこで、本発明は、上述した実情に鑑みて提案されたものであり、移動量の検出誤差が大きくなる状況であっても自己位置を高精度、且つ安定して推定することのできる自己位置推定装置及びその方法を提供することを目的とする。

上述した課題を解決するために、本発明の一態様に係る自己位置推定装置及びその方法は、移動体の周囲に存在する物標の物標位置を検出して移動体の移動量を検出する。そして、検出した物標位置を移動量だけ移動させて物標位置データとして蓄積し、地図上に存在する物標の物標位置を含む地図情報を取得する。この後、移動体の現在位置までの移動履歴に基づいて設定された所定範囲の物標位置データと地図情報に含まれる物標位置とを照合して移動体の自己位置を推定する。

図１は、本発明の第１実施形態に係る自己位置推定装置を備えた自己位置推定システムの構成を示すブロック図である。 図２は、レーザレンジファインダとカメラの車両への搭載位置を示す図である。 図３は、本発明の第１実施形態に係る自己位置推定装置による自己位置推定処理の処理手順を示すフローチャートである。 図４は、本発明の第１実施形態に係る自己位置推定装置で採用された座標系を説明するための図である。 図５は、本発明の第１実施形態に係る自己位置推定装置のレーザレンジファインダによる検出方法を説明するための図である。 図６は、本発明の第１実施形態に係る自己位置推定装置のカメラによる白線の検出方法を説明するための図である。 図７は、本発明の第１実施形態に係る自己位置推定装置による物標の検出結果を示す図である。 図８は、本発明の第１実施形態に係る自己位置推定装置による移動量の検出誤差の推定方法を説明するための図である。 図９は、本発明の第１実施形態に係る自己位置推定装置による抽出範囲の設定方法を説明するための図である。 図１０は、本発明の第１実施形態に係る自己位置推定装置によって取得されたリンク・ノード情報を説明するための図である。 図１１は、本発明の第１実施形態の変形例５に係る自己位置推定装置による抽出範囲の設定方法を説明するための図である。 図１２は、従来の自己位置推定技術を説明するための図である。 図１３は、従来の自己位置推定技術を説明するための図である。 図１４は、従来の自己位置推定技術を説明するための図である。 図１５は、本発明の第１実施形態に係る自己位置推定装置による抽出範囲の設定方法を説明するための図である。 図１６は、本発明の第２実施形態に係る自己位置推定装置による自己位置推定処理の処理手順を示すフローチャートである。

以下、本発明を適用した第１及び第２実施形態について図面を参照して説明する。

［第１実施形態］
［自己位置推定システムの構成］
図１は、本実施形態に係る自己位置推定装置を備えた自己位置推定システムの構成を示すブロック図である。図１に示すように、本実施形態に係る自己位置推定システムは、ＥＣＵ１と、カメラ２と、三次元地図データベース３と、車両センサ群４と、レーザレンジファインダ５とを備えている。

ここで、ＥＣＵ１は、ＲＯＭ、ＲＡＭ、演算回路等によって構成された電子制御ユニットであり、ＲＯＭには本実施形態に係る自己位置推定装置１０を実現するプログラムが格納されている。尚、ＥＣＵ１は他の制御に用いるＥＣＵと兼用してもよい。

カメラ２（２ａ、２ｂ）は、ＣＣＤ等の固体撮像素子を用いたものであり、例えば、図２に示すように車両の左右のドアミラーに設置され、車両の下方の路面を撮像可能となる方向に向けられている。撮像された画像はＥＣＵ１へ送信される。

三次元地図データベース３は、地図上に存在する物標の物標位置を含む地図情報を記憶した記憶手段であり、例えば道路表示を含む周囲環境の三次元位置情報を記憶している。地図情報に記録されている物標（landmark）は、地図に登録されているものであり、区画線、停止線、横断歩道、路面マーク等の路面標示の他に、縁石等の路面上の構造物や建物等が含まれる。白線等の各地図情報は、エッジの集合体として定義されている。エッジが長い直線の場合には、例えば１ｍ毎に区切られるため、極端に長いエッジは存在しない。直線の場合には、各エッジは、直線の両端点を示す３次元位置情報を持っている。曲線の場合には、各エッジは、曲線の両端点と中央点を示す３次元位置情報を持っている。また、三次元地図データベース３は一般的なナビゲーションシステムが有するノード・リンク情報を記憶しているので、目的地までの経路案内や過去の走行経路を記録する処理をＥＣＵ１によって行うことができる。

車両センサ群４は、ＧＰＳ受信機４１と、アクセルセンサ４２と、ステアリングセンサ４３と、ブレーキセンサ４４と、車速センサ４５と、加速度センサ４６と、車輪速センサ４７と、ヨーレートセンサ４８とを備えている。車両センサ群４は、ＥＣＵ１に接続され、各センサ４１〜４８によって検出された各種の検出値をＥＣＵ１に供給する。ＥＣＵ１は、車両センサ群４の出力値を用いることによって、車両の概位置を算出したり、単位時間に車両が進んだ移動量を示すオドメトリを算出する。

レーザレンジファインダ５（５ａ、５ｂ）は、図２に示すように車体の左右方向をスキャンできるように取り付けられている。レーザレンジファインダ５は、真下方向からそれぞれ水平方向までの９０［ｄｅｇ］の走査範囲で０．１［ｄｅｇ］刻みに９００本×２＝１８００本のレーザを進行方向と垂直に照射する。これにより、路面や障害物までの距離εｉ［ｍ］（ｉ=１〜１８００）と反射波の強度δｉ（ｉ=１〜１８００、０≦δｉ≦２５５）を検出することができる。

自己位置推定装置１０は、自己位置推定処理を実行する制御部として機能し、車両の周囲に存在する物標の物標位置と三次元地図データベース３に格納された地図情報とをマッチングすることによって、車両の位置及び姿勢角を推定する。自己位置推定装置１０は、ＲＯＭに格納された自己位置推定用のプログラムを実行することにより、物標位置検出部１２、移動量検出部１４、物標位置蓄積部１６、地図情報取得部１８及び自己位置推定部２０として動作する。

物標位置検出部１２は、レーザレンジファインダ５のスキャン結果から車両の周囲に存在する物標の物標位置を検出する。また、物標位置検出部１２は、カメラ２の画像から物標位置を検出してもよいし、カメラ２とレーザレンジファインダ５の両方を用いて物標位置を検出してもよい。

移動量検出部１４は、車両に搭載された車両センサ群４からの情報に基づいて車両の移動量であるオドメトリを検出する。

物標位置蓄積部１６は、以前に実行された各制御周期において物標位置検出部１２で検出された物標位置を、移動量検出部１４で検出された移動量の分だけ移動させて物標位置データとして蓄積する。

地図情報取得部１８は、三次元地図データベース３から地図情報を取得しており、取得する地図情報には、地図上に存在する物標の物標位置が含まれている。

自己位置推定部２０は、本実施形態では、車両の走行履歴（移動体の移動履歴に相当）の過去の移動量変化に基づいて移動量検出部１４で検出された移動量の検出誤差を推定する。そして、自己位置推定部２０は、車両の走行履歴の過去の移動量変化から推定した移動量の検出誤差に基づいて、物標位置蓄積部１６から物標位置データを抽出するときの所定範囲（後述の抽出範囲Ａ）を設定し、物標位置蓄積部１６に蓄積された物標位置データのうち車両の走行履歴に基づいて設定された所定範囲（抽出範囲Ａ）に含まれる物標位置データを抽出し、抽出された物標位置データと地図情報に含まれる物標位置とを照合して車両の自己位置を推定する。

尚、本実施形態では車両に適用した場合について説明するが、航空機や船舶等の移動体にも適用可能である。航空機や船舶に適用する場合には、周囲環境として道路標示に関する情報の代わりに地形や建造物とマッチングすることによって移動体の自己位置を推定することができる。

［自己位置推定処理の手順］
次に、本実施形態に係る自己位置推定処理の手順を図３のフローチャートを参照して説明する。尚、本実施形態では、車両の自己位置を推定する際に、図４に示す２つの座標系を用いている。すなわち、地図情報の原点を中心とした絶対座標系と、車両の後輪車軸中心を原点とした相対空間座標系である。絶対座標系は、地図情報の原点を原点Ｏとして、東西方向をＸ軸、南北方向をＹ軸、鉛直上方をＺ軸としている。この絶対座標系において、車両が向いている方位角（ヨー角）θ［ｒａｄ］は、東方向（Ｘ軸方向）を０としたときの反時計回りの角度で表される。また、相対空間座標系は、車両の後輪車軸中心を原点ｏとして、車両の前後方向をｘ軸、車幅方向をｙ軸、鉛直上方をｚ軸とする。

尚、本実施形態では、推定する車両の自己位置として、東西方向（Ｘ軸方向）の位置（Ｘ座標［ｍ］）、南北方向（Ｙ軸方向）の位置（Ｙ座標［ｍ］）、姿勢角情報として車両の方位角θ（ヨー角［ｒａｄ］）の合計３自由度の位置と姿勢角を推定する。ただし、３自由度の位置と姿勢角だけでなく、６自由度の空間における位置と姿勢角を求めることも可能である。また、以下に説明する自己位置推定処理は、例えば１００ｍｓｅｃ程度の間隔で連続的に行われる。

図３に示すように、まずステップＳ１０において、物標位置検出部１２は、車両の周囲に存在する物標の物標位置を検出する。具体的に、物標位置検出部１２は、レーザレンジファインダ５のスキャン結果を取得し、区画線や停止線等の路面標示や縁石、建物等の構造物の物標位置を抽出する。ここで抽出される物標位置は、相対空間座標系の車両に対する相対平面位置座標（ｘｊ（ｔ）、ｙｊ（ｔ））[ｍ]である。尚、ｘｊ、ｙｊのｊは、抽出された路面標示や縁石の数と等しい。また、ｔは現在の時刻（周期）を表している。

路面標示の相対平面位置座標の求め方は、照射されたレーザの反射波の強度δｉが閾値δｔｈ以上になると、レーザが路面標示に照射されているものと判断して、レーザが照射された点の自車に対する距離εｉと角度から相対平面位置座標を求める。このようにして求められる理由は、路面標示部分には反射材が多く含まれているので、路面のアスファルトと比較してより強くレーザを反射するからである。また、閾値δｔｈは予め実験等で求めておき、ＥＣＵ１に記憶させておけばよく、本実施形態では閾値δｔｈに１２０を設定する。

また、縁石や建物等の構造物の物標位置の求め方については、各レーザが照射された場所までの距離εｉと角度から各照射点の相対平面位置座標（ｘｊ（ｔ）、ｙｊ（ｔ））を算出して求める。具体的には、例えば図５（ａ）に示す状況において、レーザレンジファインダ５からレーザを照射すると、レーザは縁石５１を含む路面にライン５３に示すように照射される。この照射されたレーザの点群を検出すると、図５（ｂ）に示すように大きく変化する箇所５５が検出される。したがって、検出された点群にＨｏｕｇｈ変換等の線分検出手法を用いて、点群の傾きが大きく変化する場所を抽出し、その場所の相対平面位置座標（ｘｊ（ｔ）、ｙｊ（ｔ））を算出する。路面上の構造物が建物の場合でも、点群の傾きが大きく変化する場所を抽出すれば、同様に相対平面位置座標（ｘｊ（ｔ）、ｙｊ（ｔ））を求めることができる。

また、ステップＳ１０において、物標位置検出部１２は、カメラ２の画像を取得し、カメラ２の画像から区画線等の道路標示の相対平面位置座標（ｘｊ（ｔ）、ｙｊ（ｔ））を抽出してもよい。カメラ２の画像から道路標示の相対平面位置座標を抽出する方法については、図６を参照して説明する。図６（ａ）は、車両の左下向きを撮像しているカメラ２ａの画像６０ａと、車両の右下向きを撮像しているカメラ２ｂの画像６０ｂを示している。これらの画像６０ａ、６０ｂにおいて、物標位置検出部１２は、領域６１ａ、６１ｂに対して白線の検出を行う。例えば、領域６１ａ、６１ｂに対して２値化処理を行って輝度値が高い範囲６３を抽出し、図６（ｂ）に示すように輝度値が高い部分６５を検出する。そして、図６（ｃ）に示すように、輝度値が高い部分６５の重心位置６７を算出し、カメラ２の内部パラメータ（カメラモデル）と外部パラメータ（カメラ２の車両取付け位置）から、重心位置６７の相対平面位置座標（ｘｊ（ｔ）、ｙｊ（ｔ））を求める。

尚、車両のエンジンが始動した直後、或いは車両が電気自動車である場合には駆動用の電源がＯＮになった直後は、レーザレンジファインダ５のスキャン結果が蓄積されていない。そのため、本実施形態では車両のエンジンがＯＦＦ、或いは車両が電気自動車である場合には駆動用の電源がＯＦＦになったときに、現在位置から後述する抽出範囲Ａ[ｍ]の範囲内のスキャン結果を抽出しておき、ＥＣＵ１のメモリまたは記憶媒体に記録しておく。そして、車両のエンジンが始動、或いは車両が電気自動車である場合には駆動用の電源がＯＮになった瞬間に、記録されているスキャン結果を読み込むようにする。

また、ステップＳ１０で抽出された区画線や縁石、建物等の相対平面位置座標（ｘｊ（ｔ）、ｙｊ（ｔ））は、後述する処理を実行することにより、現在の時刻ｔにおける相対平面位置座標として、車両のエンジンがＯＦＦになるまで記録され続ける。また、車両が電気自動車である場合には、駆動用の電源がＯＦＦになるまで記録され続ける。

次に、ステップＳ２０において、移動量検出部１４は、車両センサ群４から得られるセンサ情報に基づいて、１周期前から現在の時刻ｔまでの車両の移動量であるオドメトリを検出する。このオドメトリは、車両が単位時間に進んだ移動量である。例えば、本実施形態では、ヨー角の移動量について、ヨーレートセンサ４８から取得したヨーレートγ［ｒａｄ／ｓ］にＥＣＵ１の演算周期１００ｍｓｅｃを乗じてヨー角の変化量Δθ（ｔ）［ｒａｄ］を求める。また、並進方向の移動量は車速センサ４５から取得した車速Ｖ［ｍ／ｓ］にＥＣＵ１の演算周期１００ｍｓｅｃを乗じて並進方向の移動量ΔＬ（ｔ）［ｍ］を求める。さらに、オドメトリを算出するにあたって、車両の各車輪のタイヤパラメータを計測し、２輪モデル等を用いることによって、車体の横滑り角や車体の横すべり量を推定して算出してもよい。

ステップＳ３０において、物標位置蓄積部１６は、物標位置検出部１２で検出された物標位置を、移動量検出部１４で検出された移動量だけ移動させて物標位置データとして蓄積する。具体的に説明すると、以前の各周期においてステップＳ１０で取得した区画線や縁石等の物標の相対平面位置座標（ｘｊ（ｔ）、ｙｊ（ｔ））を、ステップＳ２０で取得したオドメトリの分だけ移動させる。言い換えると、レーザレンジファインダ５、或いはカメラ２の何れか一方または両方で過去に取得した区画線や縁石等の物標位置を、オドメトリ情報を用いて、現在の車両の後輪車軸中心を原点とした相対空間座標系に変換する。

これにより、例えば、図７（ｂ）に示すようにレーザレンジファインダ５のスキャン結果から物標の位置を特定することができる。図７（ｂ）は、図７（ａ）に示す上空から見た走路を、車両が矢印７１の経路で走行した場合に蓄積された物標位置データを示したものである。すなわち、図７（ｂ）は、ステップＳ１０においてレーザレンジファインダ５で検出された物標の位置情報を、ステップＳ２０で検出された移動量だけ移動させて蓄積した結果を表示した一例である。図７（ｂ）には、路面の反射強度が高い点、例えば区画線や停止線、横断歩道の位置とともに、レーザの検出結果が大きく変化する縁石の位置が表示されている。

ステップＳ４０において、自己位置推定部２０は、車両の走行履歴の過去の移動量変化に基づいて車両の移動量の検出誤差を推定し、推定した移動量の検出誤差に基づいて、物標位置蓄積部１６から物標位置データを抽出するときの所定範囲（抽出範囲Ａ）を設定する。特に、本実施形態では、車両の走行履歴の過去の移動量変化から移動量の検出誤差を推定しており、走行履歴の過去の移動量変化の具体例として、車両の現在位置と所定時間前の位置、または所定距離遡った位置における過去の移動量変化から移動量の検出誤差を推定する。

自己位置推定部２０は、図８に示すように、走行履歴から一周期前のステップＳ５０で算出された車両の自己位置８０（Ｘ（ｔ），Ｙ（ｔ），θ（ｔ））と、現在より時間Ｔ［ｓ］だけ前の車両の自己位置８２（Ｘ（ｔ−Ｔ），Ｙ（ｔ−Ｔ），θ（ｔ−Ｔ））とを比較し、移動量変化を得る。ただし、ここでいう自己位置とは、絶対座標系の東西方向の位置（Ｘ座標［ｍ］）、南北方向の位置（Ｙ座標［ｍ］）、姿勢角情報として東方向を０とした時の反時計回りの方位角θ（ヨー角［ｒａｄ］）である。

そして、図８では、走行履歴から時間Ｔ［ｓ］だけ前の車両の自己位置８２が、現在の自己位置８０の車両の向きから見て車幅方向に絶対値Δｙ［ｍ］だけずれた過去の移動量変化がある。この車幅方向の過去の移動量変化であるずれ量の絶対値Δｙ［ｍ］が閾値ｙｔｈ［ｍ］以上である場合には、自己位置推定部２０は、移動量の検出誤差が大きいと推定できる。一方、車幅方向の過去の移動量変化であるずれ量の絶対値Δｙ［ｍ］が閾値ｙｔｈ［ｍ］より小さい場合には、自己位置推定部２０は、移動量の検出誤差が小さいと推定できる。

図８に示すように、車両が車幅方向に移動したということは、オドメトリの誤差が蓄積しやすい移動量変化の大きい旋回や車線変更、右左折、曲線路の走行を行ったと推定できるので、移動量の検出誤差が大きいと推定することができる。

そして、自己位置推定部２０は、走行履歴の過去の移動量変化から移動量の検出誤差が小さいと推定される場合には、後述するステップＳ５０で物標位置データを抽出するときの抽出範囲Ａ［ｍ］を大きくして、例えば２００ｍに設定する。一方、自己位置推定部２０は、走行履歴の過去の移動量変化から移動量の検出誤差が大きいと推定される場合には、抽出範囲Ａ［ｍ］を小さくして、例えば１００ｍに設定する。また、図９に示すように、走行履歴から、過去の移動量変化であるずれ量Δｙが大きくなるのに応じて、抽出範囲Ａを連続的に小さくなるように変化させてもよい。すなわち、走行履歴の過去の移動量変化から推定される移動量の検出誤差が大きくなるのに応じて、抽出範囲Ａが小さくなるように設定する。このように走行履歴の過去の移動量変化から移動量の検出誤差が大きいと推定される場合には、抽出範囲Ａを小さくすることによって、オドメトリの誤差の蓄積が少なくなるようにする。

尚、閾値ｙｔｈは、例えば５０ｍに設定すればよい。また、抽出範囲Ａの減少幅は、ステップＳ５０のマッチング状態を予め実験やシミュレーションによって検証し、最適な値を設定するものであり、２００ｍから１００ｍに減少させる以外に他の値に設定してもよい。

また、走行履歴として、車幅方向の移動量だけではなく車両が通過した交差点の数を用いて、過去の移動量変化を推定し、移動量の検出誤差の大きさを推定してもよい。この場合、まず抽出範囲Ａを２００ｍに設定しておき、三次元地図データベース３に登録されたリンク・ノード情報から、走行履歴として抽出範囲Ａの中で交差点を通過した回数をカウントする。

そして、走行履歴として交差点を通過した回数が３回未満の場合には、過去の移動量変化が小さいので、移動量の検出誤差は小さいと推定して抽出範囲Ａを２００ｍのままに設定する。一方、走行履歴として交差点を通過した回数が３回以上の場合には、過去の移動量変化が大きいので、移動量の検出誤差が大きくなると推定して、抽出範囲Ａを１００ｍに設定する。また、この他にも、抽出範囲Ａの中の地図上の最大勾配を探索し、例えば走行履歴として最大勾配が４％以上の区間が含まれている場合には、過去の移動量変化が大きいので、移動量の検出誤差が大きくなると推定して、抽出範囲Ａを１００ｍに設定してもよい。

このように、走行履歴としては、車両の現在位置と所定時間前の位置に加えて、車両の走行経路上の地図情報を参照して、過去の移動量変化を推定し、移動量の検出誤差を推定してもよい。これにより、右左折や停止、発進を伴う交差点や急勾配の区間等の移動量変化が大きく、オドメトリの誤差が蓄積しやすいポイントが走行経路に含まれる場合には、抽出範囲Ａを小さくしてオドメトリの誤差の蓄積が少なくなるようにできる。

ステップＳ５０において、自己位置推定部２０は、物標位置蓄積部１６に蓄積された物標位置データのうちステップＳ４０で設定された抽出範囲Ａに含まれる物標位置データを抽出する。そして、抽出された物標位置データと地図情報に含まれる物標位置とを照合して車両の自己位置を推定する。

具体的に、自己位置推定部２０は、ステップＳ３０で蓄積された区画線や縁石等の物標位置データの中からステップＳ４０で設定された抽出範囲Ａの範囲内にある物標位置データを抽出する。このとき、物標位置データは、ステップＳ２０で算出された移動量ΔＬ（ｔ）を現在の時刻ｔから遡って積算し、この積算値が抽出範囲Ａを超えるまで抽出される。

そして、抽出した物標位置データと三次元地図データベース３に格納されている地図情報の物標位置とをマッチングして、車両の絶対座標系における自己位置を推定する。すなわち、車両の東西方向の位置（Ｘ座標）と、南北方向の位置（Ｙ座標）と、方位角（ヨー角θ）とからなる合計３自由度の位置と姿勢角を推定する。こうして自己位置が推定されると、本実施形態に係る自己位置推定処理は終了する。

尚、ステップＳ５０におけるマッチングには、ＩＣＰ（Iterative Closest Point）アルゴリズムを用いる。このとき、三次元地図データベース３の地図情報に含まれる物標位置のうち、例えば区画線は、その両端の端点を評価点としてマッチングする。また、車両（カメラ２）に近いほど物標位置データはオドメトリの誤差の影響を受けないので、車両の近傍については直線補完して評価点の数を増加させ、車両の遠方については評価点の数を減少させる。

［変形例１］
本実施形態の変形例１としては、ステップＳ４０において車両の走行履歴から移動量の検出誤差を推定する際に、走行履歴の具体例として、車両の右左折回数が多いほど、過去の移動量変化が大きいので、移動量の検出誤差が大きいと推定し、抽出範囲Ａを狭くする。この場合、まず抽出範囲Ａを２００ｍに設定しておき、三次元地図データベース３に登録されたリンク・ノード情報から、抽出範囲Ａの範囲内で車両が交差点で右左折した回数をカウントする。

尚、リンク・ノード情報は、図１０に示すように、分岐したリンク（矢印）とノード（丸印）が記録されており、各リンクには直進、右折、左折、合流、分岐を判断できるような属性情報が付与されている。したがって、車両がどのリンクを通過したかを検出して、このリンク・ノード情報を参照することによって、車両の右左折情報を得ることができる。

そして、走行履歴として、交差点で右左折した回数が２回未満の場合には、過去の移動量変化が小さいので、移動量の検出誤差は小さいと推定して抽出範囲Ａを大きく、例えば２００ｍのままに設定する。一方、走行履歴として、交差点で右左折した回数が２回以上の場合には、過去の移動量変化が大きいので、移動量の検出誤差が大きいと推定して、抽出範囲Ａを小さく、例えば１００ｍに設定する。このとき、右左折の回数に応じて抽出範囲Ａを連続的に変化させてもよい。

このように、オドメトリの誤差が蓄積しやすい右左折を行った場合には、抽出範囲Ａを小さくしてオドメトリの誤差の蓄積が少なくなるようにする。

［変形例２］
変形例２としては、ステップＳ４０の走行履歴の具体例として、車両の車線変更の回数が多いほど、過去の移動量変化が大きいので、移動量の検出誤差が大きいと推定し、抽出範囲Ａを狭くする。この場合、まず抽出範囲Ａを２００ｍに設定しておき、三次元地図データベース３に登録されたリンク・ノード情報から、抽出範囲Ａの範囲内で車両が車線変更した回数をカウントする。図１０に示すように、リンク・ノード情報には、複数の車線がある道路について車線毎に個別にリンク情報が設定されているので、リンク・ノード情報を参照すれば、車線変更の回数をカウントすることができる。

そして、走行履歴として、車線変更の回数が０回の場合には、過去の移動量変化が小さいので、移動量の検出誤差は小さいと推定して抽出範囲Ａを大きく、例えば２００ｍのままに設定する。一方、走行履歴として、車線変更の回数が１回以上である場合には、過去の移動量変化が大きいので、移動量の検出誤差が大きいと推定して、抽出範囲Ａを小さく、例えば１００ｍに設定する。

このように、オドメトリの誤差が蓄積しやすい車線変更をした場合には、抽出範囲Ａを小さくしてオドメトリの誤差の蓄積が少なくなるようにする。

［変形例３］
変形例３としては、ステップＳ４０の走行履歴の具体例として、車両の分流または合流の回数が多いほど、過去の移動量変化が大きいので、移動量の検出誤差が大きいと推定し、抽出範囲Ａを小さくする。この場合、まず抽出範囲Ａを２００ｍに設定しておき、三次元地図データベース３に登録されたリンク・ノード情報から、抽出範囲Ａの範囲内で車両が分流または合流した回数をカウントする。分流または合流の判断については、図１０に示すように、リンク・ノード情報の各リンクに付与された属性情報を参照すればよい。図１０では合流、分岐のリンクは図示されていないが、各リンクには直進、右折、左折、合流、分岐を判断できるような属性情報が付与されているので、この属性情報を参照すれば、車両が分流または合流したかを判断することができる。

そして、走行履歴として、分流または合流の回数が０回の場合には、過去の移動量変化が小さいので、移動量の検出誤差は小さいと推定して抽出範囲Ａを大きく、例えば２００ｍのままに設定する。一方、走行履歴として、分流または合流の回数が１回以上である場合には、過去の移動量変化が大きいので、移動量の検出誤差が大きいと推定して、抽出範囲Ａを小さく、例えば１８０ｍに設定する。

このように、オドメトリの誤差が蓄積しやすい分流または合流をした場合には、抽出範囲Ａを小さくしてオドメトリの誤差の蓄積が少なくなるようにする。

［変形例４］
変形例４としては、ステップＳ４０の走行履歴の具体例として、車両が走行したカーブの曲率半径が小さいほど、過去の移動量変化が大きいので、移動量の検出誤差が大きいと推定し、抽出範囲Ａを狭くする。この場合、まず抽出範囲Ａを２００ｍに設定しておき、三次元地図データベース３に登録されたリンク・ノード情報から、抽出範囲Ａの範囲内で車両が走行したカーブの曲率半径を検出する。曲率半径は、リンク・ノード情報の各リンクに記録されているので、車両が走行したリンクを特定すれば、曲率半径を検出することができる。

そして、走行履歴として、車両が走行したカーブの曲率半径が５０ｍより大きい場合には、過去の移動量変化が小さいので、移動量の検出誤差は小さいと推定して抽出範囲Ａを大きく、例えば２００ｍのままに設定する。一方、走行履歴として、車両が走行したカーブの曲率半径が５０ｍ以下である場合には、過去の移動量変化が大きいので、移動量の検出誤差が大きいと推定して、抽出範囲Ａを小さく、例えば１００ｍに設定する。このとき、曲率半径に応じて抽出範囲Ａを連続的に変化させてもよい。

このように、オドメトリの誤差が蓄積しやすい曲率半径の小さいカーブを車両が走行した場合には、抽出範囲Ａを小さくしてオドメトリの誤差の蓄積が少なくなるようにする。

［変形例５］
変形例５として、上述した実施形態と変形例１〜４の複数の走行履歴の形態を用いて過去の移動量変化を推定して、移動量の検出誤差を推定し、抽出範囲Ａを設定してもよい。この場合、図１１に示すように、走行履歴の過去の移動量変化であるずれ量Δｙや右左折回数等の過去の移動量変化が大きいと想定される誤差要因毎にそれぞれ抽出範囲Ａの減少量ΔＡを設定しておき、各誤差要因において検出誤差が大きいと推定された場合には、各減少量ΔＡをそれぞれ抽出範囲Ａから減少させるようにする。例えば、図１１では、走行履歴でずれ量Δｙが５０ｍ以上、交差点通過回数が３回以上、右左折回数が２回以上の場合にはそれぞれ減少量ΔＡが２０ｍに設定され、分流または合流、車線変更の回数が１回以上である場合には減少量ΔＡが１０ｍに設定されている。また、走行履歴で車両が走行したカーブの曲率半径が５０ｍ以下である場合には減少量ΔＡが５０ｍに設定されている。そして、移動量の検出誤差が大きいと推定された誤差要因が複数あった場合には、これらの減少量ΔＡの和を、予め設定された２００ｍから減少させて抽出範囲Ａを設定する。例えば、走行履歴で右左折回数が２回以上で車線変更が１回あった場合には、それぞれの減少量２０ｍと１０ｍの和となる３０ｍを２００ｍから減少させて抽出範囲Ａを１７０ｍに設定する。このとき、減少量ΔＡの和が大きくなりすぎて抽出範囲Ａが小さくなりすぎると、ステップＳ５０で実行されるマッチングが難しくなるので、抽出範囲Ａの最小値を例えば１００ｍに設定しておくようにする。また、減少幅ΔＡは、ステップＳ５０のマッチング状態を予め実験やシミュレーションによって検証し、最適な値が設定されるようにする。

［第１実施形態の効果］
次に、本実施形態に係る自己位置推定装置による効果を説明する。まず、従来の自己位置推定技術について説明すると、従来では移動量とセンシング結果の履歴とを蓄積して連結することで自己位置を推定していた。例えば、図１２（ａ）は、車両の左右の下向き方向を撮像した魚眼カメラの画像を俯瞰変換したものである。図１２（ａ）に示すように現在の俯瞰画像１２１は小さいが、図１２（ｂ）に示すように、現在の俯瞰画像１２１に過去の移動量とセンシング結果（俯瞰画像）の履歴を蓄積して連結した部分１２３を追加すると、過去の一定区間の俯瞰画像を得ることができる。したがって、駐車車両等の障害物があっても過去のセンシング結果を用いて自己位置を推定することが可能になる。

このとき、従来の自己位置推定技術では、常に一定距離のセンシング結果の履歴を用いることによって、移動体の移動速度の影響を排除し、自己位置の推定を安定して行えるようにしていた。これは、一定時間のセンシング結果の履歴を用いた場合、移動体が極低速、あるいは停止していると、現在のセンシング結果のみを利用しているのと同じことになってしまうからである。また、移動速度が高速になると、センシング結果の履歴を連結して得られる領域が広くなり、地図情報と一致するように調整するための演算負荷が大きくなるからである。

しかしながら、上述した従来の自己位置推定技術では、一定距離のセンシング結果の履歴を用いていたので、移動量の検出誤差が大きくなるような状況では、センシング結果と地図情報との間の乖離が大きくなって自己位置の推定が困難になるという問題点があった。

例えば、図１３に示すように車両の走行履歴が一定速で直進だった場合１３１と、加減速を含む右左折を繰り返した場合１３３で、同じ距離のセンシング結果を連結した場合について比較する。

図１４は、この比較結果を示す図であり、図中の黒丸は車両に搭載されたレーザレンジファインダによって時々刻々検出した白線エッジの位置を示しており、白丸は従来の自己位置推定技術で過去のセンシング結果を連結した結果を示している。

図１４（ａ）に示すように、車両の走行履歴が一定速で直進だった場合では、車両の過去の移動量変化が小さいため移動量の検出誤差は小さくなり、検出した白線エッジ（黒丸）と連結したセンシング結果（白丸）との間の乖離は小さくなる。

これに対して、図１４（ｂ）に示すように、車両の走行履歴で、車両が加減速を含む右左折を繰り返した場合では、過去の移動量変化が大きく、その結果、移動量の検出誤差が大きくなるので、現在から過去にさかのぼるにしたがって、検出した白線エッジ（黒丸）と連結したセンシング結果（白丸）との間の乖離が大きくなる。このような場合には、移動量の検出に用いている車両モデルをより厳密なモデルにすることによって、移動量の検出誤差を低減する対策が考えられる。しかし、車両を例にすると、乗員や搭載燃料の変化による車体の質量変化や路面の摩擦係数の変化まで厳密にモデル化することは難しく、上記の問題を解決することは困難であった。

そこで、本実施形態に係る自己位置推定装置では、車両の走行履歴から移動量の検出誤差を推定し、推定した検出誤差が大きくなるのに応じて、蓄積された物標位置データから抽出するデータの範囲を小さくする。例えば、図１５に示すように、移動量の検出誤差が小さい直進の場合には抽出範囲１５１を広くし、検出誤差が大きい右左折を繰り返す場合には抽出範囲１５３を狭くする。

このように、本実施形態に係る自己位置推定装置では、走行履歴に基づいて物標位置データの所定範囲（抽出範囲Ａ）を設定し、所定範囲（抽出範囲Ａ）から抽出された物標位置データと地図情報に含まれる物標位置とを照合して車両の自己位置を推定する。これにより、走行履歴で車両が右左折を繰り返すような場合は、過去の移動量変化が大きいので、検出誤差が大きいとして、所定範囲（抽出範囲Ａ）を狭くして地図情報との乖離が十分小さい範囲で、物標位置データと地図情報の物標位置とを照合することができる。また、走行履歴で車両が定速で直進するような場合は、過去の移動量変化が小さいので、検出誤差が小さいとして、所定範囲（抽出範囲Ａ）を広くして、より多くの物標位置データと地図情報の物標位置とを照合することができる。したがって、走行履歴に基づいて、移動量の検出誤差が小さい状況だけでなく、検出誤差が大きくなるような状況であっても、自己位置を高精度、且つ安定して推定することができる。

また、本実施形態に係る自己位置推定装置では、走行履歴で車両の右左折回数が多いほど、所定範囲を狭くする。交差点での右左折は、車両の旋回だけでなく、その前後に加減速も伴い、過去の移動量変化が大きいので、車体の前後方向における移動量の検出誤差が大きくなる。したがって、移動量の検出誤差が蓄積しやすい右左折回数が多い場合には、所定範囲（抽出範囲Ａ）を小さくすることで、移動量の検出誤差の蓄積を少なくすることができ、これによって自己位置を高精度、且つ安定して推定することができる。

さらに、本実施形態に係る自己位置推定装置では、走行履歴で車両の車線変更の回数が多いほど、所定範囲を狭くする。車両では車線変更の際には、過去の移動量変化が大きく、非線形な車体の横滑り運動が発生するので、高精度に移動量を推定することが難しくなり、移動量の検出誤差が大きくなる。したがって、移動量の検出誤差が蓄積しやすい車線変更の回数が多い場合には、所定範囲（抽出範囲Ａ）を小さくすることで、移動量の検出誤差の蓄積を少なくすることができ、これによって自己位置を高精度、且つ安定して推定することができる。

また、本実施形態に係る自己位置推定装置では、走行履歴で車両の分流または合流の回数が多いほど、所定範囲を狭くする。車両の分流または合流では車線の変更や旋回といった過去の移動量変化が大きく、移動量の検出誤差が拡大するような挙動が生じる。したがって、移動量の検出誤差が蓄積しやすい分流または合流の回数が多い場合には、所定範囲（抽出範囲Ａ）を小さくすることで、移動量の検出誤差の蓄積を少なくすることができ、これによって自己位置を高精度、且つ安定して推定することができる。

さらに、本実施形態に係る自己位置推定装置では、走行履歴で車両が走行したカーブの曲率半径が小さいほど、所定範囲を狭くする。高車速で急なカーブを走行すると、車線変更と同じく、過去の移動量変化が大きく、非線形な車体の横滑り運動が発生するので、高精度に移動量を推定することが難しくなる。したがって、移動量の検出誤差が蓄積しやすい曲率半径の小さいカーブを走行した場合には、所定範囲（抽出範囲Ａ）を小さくすることで、移動量の検出誤差の蓄積を少なくすることができ、これによって自己位置を高精度、且つ安定して推定することができる。

［第２実施形態］
次に、本発明の第２実施形態に係る自己位置推定装置について図面を参照して説明する。尚、本実施形態に係る自己位置推定システムの構成は第１実施形態と同一なので、詳細な説明は省略する。

［自己位置推定処理の手順］
本実施形態に係る自己位置推定処理の手順を図１６のフローチャートを参照して説明する。第１実施形態では、ステップＳ４０において車両の走行履歴から移動量の検出誤差を推定していた。しかし、本実施形態では、ステップＳ１４０において走行履歴の中の車両挙動に注目して、所定範囲（抽出範囲Ａ）を設定することが、第１実施形態と相違している。尚、ステップＳ１０〜Ｓ３０とステップＳ５０は、図３の第１実施形態と同一であるため、詳細な説明は省略する。

ステップＳ１４０において、自己位置推定部２０は、走行履歴から車両の移動量の検出誤差を推定し、推定した移動量の検出誤差に基づいて、物標位置蓄積部１６から物標位置データを抽出するときの所定範囲（抽出範囲Ａ）を設定する。特に、本実施形態では、走行履歴の中の過去の移動量変化である車両挙動から移動量の検出誤差を推定しており、車両挙動の具体例として、車両の旋回量から移動量の検出誤差を推定する。

自己位置推定部２０は、走行履歴から一周期前のステップＳ５０で算出された車両の姿勢角θ（ｔ）と、現在より時間Ｔ［ｓ］だけ前の車両の姿勢角θ（ｔ−Ｔ）との差を計算して移動量変化である旋回量ｄθ［ｒａｄ］を求める。そして、この過去の移動量変化である旋回量ｄθの絶対値が閾値ｄθｔｈ以上である場合には、自己位置推定部２０は、移動量の検出誤差が大きいと推定できる。一方、過去の移動量変化である旋回量ｄθの絶対値が閾値ｄθｔｈより小さい場合には、自己位置推定部２０は、移動量の検出誤差が小さいと推定できる。尚、閾値ｄθｔｈは、例えば１．７４５[ｒａｄ]≒１００［ｄｅｇ］に設定すればよい。

このように車両の移動量変化である旋回量が大きいということは、オドメトリの誤差が蓄積しやすい旋回や車線変更、曲線路の走行を行ったと推定できるので、移動量の検出誤差が大きいと推定することができる。

そして、自己位置推定部２０は、走行履歴の過去の移動量変化から移動量の検出誤差が小さいと推定される場合には、ステップＳ５０で物標位置データを抽出するときの抽出範囲Ａ［ｍ］を大きくして、例えば２００ｍに設定する。一方、自己位置推定部２０は、走行履歴の過去の移動量変化から移動量の検出誤差が大きいと推定される場合には、抽出範囲Ａ［ｍ］を小さくして、例えば１００ｍに設定する。また、走行履歴の過去の移動量変化である旋回量が大きくなるのに応じて、抽出範囲Ａを連続的に小さくなるように変化させてもよい。すなわち、走行履歴の過去の移動量変化から推定される移動量の検出誤差が大きくなるのに応じて、抽出範囲Ａが小さくなるように設定する。

このように、車両の旋回量が大きくてオドメトリの誤差が蓄積しやすい場合には、抽出範囲Ａを小さくして、オドメトリの誤差の蓄積が少なくなるようにする。

また、旋回量ｄθではなく、走行履歴の現在の時刻ｔから時間Ｔ［ｓ］だけ前までの時々刻々のヨー角の変化量Δθ（ｔ）の絶対値を積分して旋回量ｄθを求めてもよい。この場合、スラローム走行等で姿勢角が見かけ上は元に戻っていても、実際の旋回量は大きくなっていることを検出することができる。

また、旋回量の積分値ではなく、抽出範囲Ａにおける旋回速度（ヨーレート）の絶対値の最大値γabs［ｒａｄ／ｓ］を検出し、このγabsが閾値γｔｈ［ｒａｄ／ｓ］以上である場合に移動量の検出誤差が大きいと推定してもよい。閾値γｔｈは、例えば０．３９[ｒａｄ／ｓ]≒２０[ｄｅｇ／ｓ]に設定すればよい。

［変形例６］
また、変形例６としては、ステップＳ１４０の車両挙動の具体例として、車両の車速変化が大きいほど、移動量の検出誤差が大きいと推定し、抽出範囲Ａを狭くする。この場合、まず抽出範囲Ａを２００ｍに設定しておき、車両の前後方向の加速度を計測する加速度センサ４６の計測値α[ｍ／ｓ^２]の絶対値の最大値αａｂｓ[ｍ／ｓ^２]を検出する。

そして、走行履歴として、このαａｂｓが閾値αｔｈ[ｍ／ｓ^２]より小さい場合には移動量の検出誤差は小さいと推定して抽出範囲Ａを大きく、例えば２００ｍのままに設定する。一方、走行履歴として、αａｂｓが閾値αｔｈ以上である場合には移動量の検出誤差が大きくなると推定して、抽出範囲Ａを小さく、例えば１００ｍに設定する。閾値αｔｈは、例えば０．２[ｍ／ｓ^２]に設定すればよい。このとき、αａｂｓに応じて抽出範囲Ａを連続的に変化させてもよい。

このように、車両が、オドメトリの誤差が蓄積しやすい大きな車速変化をした場合には、抽出範囲Ａを小さくしてオドメトリの誤差の蓄積が少なくなるようにする。

尚、加速度センサ４６に多軸センサを用い、車両の車幅方向、上下方向の加速度を計測して、その合成成分で判断するようにしてもよい。さらに、ステップＳ１４０では、車両挙動から、例えば車両が交差点を右左折した、車線変更を行った等を判定し、第１実施形態で説明した方法で抽出範囲Ａを設定してもよい。

［第２実施形態の効果］
以上詳細に説明したように、本実施形態に係る自己位置推定装置では、走行履歴の過去の移動量変化である車両の旋回量が大きいほど、移動量の検出誤差が大きいと推定し、所定範囲（抽出範囲Ａ）を小さくする。車両が旋回すると、旋回方向だけでなくタイヤのすべりによる車幅方向の移動量の検出誤差が大きくなる。したがって、走行履歴に基づいて、移動量の検出誤差が蓄積しやすい車両の旋回量が大きい場合には、所定範囲（抽出範囲Ａ）を小さくすることで、移動量の検出誤差の蓄積を少なくすることができ、これによって自己位置を高精度、且つ安定して推定することができる。

また、本実施形態に係る自己位置推定装置では、走行履歴の車両の車速変化が大きいほど、移動量の検出誤差が大きいと推定し、所定範囲（抽出範囲Ａ）を小さくする。車両に加減速が発生した場合には、過去の移動量変化が大きいので、車体の前後方向の移動量の検出誤差が大きくなる。したがって、走行履歴に基づいて、移動量の検出誤差が蓄積しやすい車速変化が大きい場合には、所定範囲（抽出範囲Ａ）を小さくすることで、移動量の検出誤差の蓄積を少なくすることができ、これによって自己位置を高精度、且つ安定して推定することができる。

なお、上述の実施形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計などに応じて種々の変更が可能であることは勿論である。

特に、上述した実施形態では車両を例にして説明したが、少なくとも１台のカメラまたはレーザレンジファインダの何れか一方または両方と、オドメトリを計測するためのセンサを搭載した移動体であれば、航空機や船舶等にも本発明は適用可能である。また、上述した実施形態では車両の３自由度の位置及び姿勢角を求めているが、６自由度での位置及び姿勢角を推定することも可能である。

１ ＥＣＵ
２、２ａ、２ｂ カメラ
３ 三次元地図データベース
４ 車両センサ群
５、５ａ、５ｂ レーザレンジファインダ
１０ 自己位置推定装置
１２ 物標位置検出部
１４ 移動量検出部
１６ 物標位置蓄積部
１８ 地図情報取得部
２０ 自己位置推定部
４１ ＧＰＳ受信機
４２ アクセルセンサ
４３ ステアリングセンサ
４４ ブレーキセンサ
４５ 車速センサ
４６ 加速度センサ
４７ 車輪速センサ
４８ ヨーレートセンサ

Claims (9)

1. 移動体の自己位置を推定する自己位置推定装置であって、
   前記移動体の周囲に存在する物標の物標位置を検出する物標位置検出部と、
   前記移動体の移動量を検出する移動量検出部と、
   前記物標位置検出部で検出された物標位置を、前記移動量検出部で検出された移動量だけ移動させて物標位置データとして蓄積する物標位置蓄積部と、
   地図上に存在する物標の物標位置を含む地図情報を取得する地図情報取得部と、
   前記移動体の現在位置までの移動履歴の誤差要因に基づいて設定された所定範囲の前記物標位置データと前記地図情報に含まれる物標位置とを照合して前記移動体の自己位置を推定する自己位置推定部と
   を備えたことを特徴とする自己位置推定装置。
2. 前記自己位置推定部は、前記移動履歴の前記移動体の過去の移動量変化が大きいほど、前記所定範囲を小さくすることを特徴とする請求項１に記載の自己位置推定装置。
3. 前記移動体は、車両であり、
   前記自己位置推定部は、前記移動履歴で前記車両の右左折回数が多いほど、前記所定範囲を小さくすることを特徴とする請求項１または２に記載の自己位置推定装置。
4. 前記移動体は、車両であり、
   前記自己位置推定部は、前記移動履歴で前記車両の車線変更の回数が多いほど、前記所定範囲を小さくすることを特徴とする請求項１〜３のいずれか１項に記載の自己位置推定装置。
5. 前記移動体は、車両であり、
   前記自己位置推定部は、前記移動履歴で前記車両の分流または合流の回数が多いほど、前記所定範囲を小さくすることを特徴とする請求項１〜４のいずれか１項に記載の自己位置推定装置。
6. 前記移動体は、車両であり、
   前記自己位置推定部は、前記移動履歴で前記車両が走行したカーブの曲率半径が小さいほど、前記所定範囲を小さくすることを特徴とする請求項１〜５のいずれか１項に記載の自己位置推定装置。
7. 前記自己位置推定部は、前記移動履歴で前記移動体の旋回量が大きいほど、前記所定範囲を小さくすることを特徴とする請求項１〜６のいずれか１項に記載の自己位置推定装置。
8. 前記自己位置推定部は、前記移動履歴で前記移動体の移動速度変化が大きいほど、前記所定範囲を小さくすることを特徴とする請求項１〜７のいずれか１項に記載の自己位置推定装置。
9. 移動体の自己位置を推定する自己位置推定方法であって、
   前記移動体に搭載された制御部が、前記移動体の周囲に存在する物標の物標位置を検出し、
   前記制御部が、前記移動体の移動量を検出し、
   前記制御部が、検出された前記物標位置を、検出された前記移動量だけ移動させて物標位置データとして蓄積し、
   前記制御部が、地図上に存在する物標の物標位置を含む地図情報を取得し、
   前記制御部が、前記移動体の現在位置までの移動履歴の誤差要因に基づいて設定された所定範囲の前記物標位置データと前記地図情報に含まれる物標位置とを照合して前記移動体の自己位置を推定することを特徴とする自己位置推定方法。

Images