JP6575685B2

JP6575685B2 - 自己位置推定方法及び自己位置推定装置

Info

Publication number: JP6575685B2
Application number: JP2018530244A
Authority: JP
Inventors: 卓也南里; 千加夫土谷; 泰仁佐野; 博幸 ▲高▼野
Original assignee: Nissan Motor Co Ltd
Current assignee: Nissan Motor Co Ltd
Priority date: 2016-07-26
Filing date: 2016-07-26
Publication date: 2019-09-18
Anticipated expiration: 2036-07-26
Also published as: JPWO2018020588A1; BR112019001508A2; CN109477724A; BR112019001508B1; KR20190031544A; RU2715500C1; CN109477724B; EP3492870A1; EP3492870B1; CA3031723C; US10625746B2; US20190263420A1; MX2019000759A; CA3031723A1; EP3492870A4; WO2018020588A1

Description

本発明は、自己位置推定方法及び自己位置推定装置に関する。

既知の物標と移動体との間の相対位置を検出して移動体の位置を推定する技術として、特許文献１に記載の技術が知られている。
特許文献１に記載のロボットは、移動可能な領域を点集合データで示す点環境地図と、ロボットに搭載したレーザーレンジセンサの検出結果を点集合データで示す周囲環境情報との位置ずれ量に基づき、ロボットの自己位置の推定結果を補正する。

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

物標と移動体との間の相対位置を検出するセンサが移動体に搭載されていると、移動体の挙動変化に従う姿勢の変化によってセンサの向きが変わり、相対位置の検出結果に誤差が発生する。
本発明は、移動体に搭載したセンサにより検出した物標との相対位置に、移動体の挙動変化に伴う誤差が生じても、このような誤差による移動体の位置推定精度の低下を抑制することを目的とする。

本発明の一態様に係る自己位置推定方法では、移動体の周囲に存在する物標の相対位置を検出し、移動体の移動量に基づき相対位置を補正して物標位置データとして蓄積する。蓄積した物標位置データのうち、移動体の挙動変化量が閾値未満である期間中に検出した相対位置の物標位置データと地図情報とを照合して移動体の現在位置を推定する。

本発明の一態様によれば、移動体に搭載したセンサにより検出した物標との相対位置に、移動体の挙動変化に伴う誤差が生じても、このような誤差による移動体の位置推定精度の低下を抑制できる。
本発明の目的及び利点は、特許請求の範囲に示した要素及びその組合せを用いて具現化され達成される。前述の一般的な記述及び以下の詳細な記述の両方は、単なる例示及び説明であり、特許請求の範囲のように本発明を限定するものでないと解するべきである。

実施形態の自己位置推定装置を搭載した車両の概略構成の一例のブロック図である。自己位置推定回路の概略構成の一例のブロック図である。車両のピッチにより発生する物標の相対位置の測定誤差の説明図である。車両のロールにより発生する物標の相対位置の測定誤差の説明図である。選択済物標位置データの説明図である。物標位置データと地図情報との照合による自己位置の推定方法の一例の説明図である。第１実施形態の自己位置推定方法の一例を示すフローチャートである。第２実施形態の自己位置推定方法の一例を示すフローチャートである。第３実施形態の自己位置推定回路の概略構成の一例のブロック図である。第３実施形態の自己位置推定方法の一例を示すフローチャートである。

以下、本発明の実施形態について、図面を参照しつつ説明する。
（第１実施形態）
（構成）
図１を参照する。以下、移動体の一例として車両の現在位置を推定する場合について説明するが、本発明は車両に限らず様々な移動体の現在位置の推定に広く適用することができる。
車両１には、自己位置推定装置２と運転支援システム３が搭載される。自己位置推定装置２は、撮像装置１０と、距離測定装置１１と、車輪速センサ１２と、操舵角センサ１３と、ジャイロセンサ１４と、加速度センサ１５と、自己位置推定回路１６を備える。

撮像装置１０は、車両１の車室内などに取り付けられ、例えば車両１の前方領域を撮像する。撮像装置１０は、例えば広角カメラであってよい。撮像装置１０は、車両１の前方領域の撮像画像を自己位置推定回路１６へ出力する。
距離測定装置１１は、車両１の車室外などに取り付けられ、車両１の前方領域に電磁波を照射しその反射波を検出する。距離測定装置１１は、例えばレーザレンジファインダであってよい。また、距離測定装置１１の取り付け位置は、例えば車両１のボンネット、バンパー、ナンバープレート、ヘッドライト、又はサイドミラーの周辺であってよい。距離測定装置１１は、測定結果を自己位置推定回路１６へ出力する。

車輪速センサ１２は、車両１の車輪が１回転する毎に予め設定した数の車輪速パルスを発生させる。車輪速センサ１２は、車輪速パルスを自己位置推定回路１６へ出力する。
操舵角センサ１３は、例えば、車両１のステアリングホイールを回転可能に支持するステアリングコラムに設けられる。操舵角センサ１３は、操舵操作子であるステアリングホイールの現在の回転角度（操舵操作量）である現在操舵角を検出する。操舵角センサ１３は、検出した現在操舵角を自己位置推定回路１６へ出力する。

ジャイロセンサ１４は、車両１に発生するヨーレート、ピッチ方向の変位量、及びロール方向の変位量を検出する。ジャイロセンサ１４は、検出したヨーレート、ピッチ方向の変位量、及びロール方向の変位量を自己位置推定回路１６へ出力する。
加速度センサ１５は、車両１に発生する車幅方向の加減速度である横Ｇ、及び前後方向の加減速度を検出する。加速度センサ１５は、検出した横Ｇ及び前後方向の加減速度を自己位置推定回路１６へ出力する。

自己位置推定回路１６は、ＣＰＵ（Central Processing Unit）等のプロセッサ、記憶装置及び周辺部品を含む電子回路装置である。
自己位置推定回路１６は、撮像装置１０、距離測定装置１１、車輪速センサ１２、操舵角センサ１３、ジャイロセンサ１４、及び加速度センサ１５から受信した信号と、既知の物標の地図上の位置を示す地図情報に基づいて、車両１の地図上の現在位置を推定する。以下、車両１の地図上の現在位置を「自己位置」と表記することがある。自己位置推定回路１６は、自己位置を示す自己位置信号を運転支援システム３へ出力する。

運転支援システム３は、自己位置推定回路１６から受信した自己位置信号が示す自己位置を用いて、運転者による車両１の運転に対する運転支援を行う。
運転支援の一例は、例えば運転者に対する警報等の情報提供であってよい。運転支援システム３は、車両１の自己位置に応じて運転者に提示する警報の種類及び強度の少なくとも一方を制御してよい。
運転支援の一例は、車両１の制動制御、加速制御及び操舵制御の少なくとも１つを含む車両１の走行状態の制御であってもよい。例えば運転支援システム３は、車両１の自己位置に応じて車両１に制動力及び駆動力のいずれを発生させるのかを決定してよい。

次に、自己位置推定回路１６の構成を説明する。図２を参照する。自己位置推定回路１６は、物標位置検出部２０と、移動量推定部２１と、挙動検出部２２と、物標位置蓄積部２３と、記憶装置２４と、選択部２５と、位置推定部２６と、地図情報取得部２７を備える。
自己位置推定回路１６が備えるプロセッサは、記憶装置２４に格納されたコンピュータプログラムを実行することにより、物標位置検出部２０、移動量推定部２１、挙動検出部２２、物標位置蓄積部２３、選択部２５、位置推定部２６及び地図情報取得部２７の機能を実現する。

物標位置検出部２０は、撮像装置１０が生成した車両１の前方領域の撮像画像を受信する。また、物標位置検出部２０は、距離測定装置１１の測定結果を受信する。
物標位置検出部２０は、車両１の前方領域の撮像画像と距離測定装置１１の測定結果に基づいて、車両１の周囲に存在する物標を検出する。例えば物標位置検出部２０は、車両１の前方に存在する物標を検出する。
さらに、物標位置検出部２０は、車両１に対する物標の相対位置を検出する。物標位置検出部２０は、検出した相対位置を示す相対位置信号を物標位置蓄積部２３へ出力する。
ここで物標とは、例えば車両１が走行する走行路面上の線（車線区分線等）や、路肩の縁石、ガードレール等であってよい。

移動量推定部２１は、車輪速センサ１２、操舵角センサ１３及びジャイロセンサ１４から、それぞれ車輪速パルス、現在操舵角及びヨーレートを受信する。移動量推定部２１は、車輪速センサ１２、操舵角センサ１３及びジャイロセンサ１４から受信したこれらの信号に基づいて、前回の処理周期で車両１の自己位置を推定してから現在までの車両１の移動量ΔＰを推定する。移動量推定部２１は、推定した移動量ΔＰを示す移動量信号を物標位置蓄積部２３へ出力する。

挙動検出部２２は、車両１に発生するヨーレート、ピッチ方向の変位量（すなわちピッチ方向の回転量）、及びロール方向の変位量（すなわちロール方向の回転量）をジャイロセンサ１４から受信する。挙動検出部２２は、横Ｇ及び前後方向の加減速度を加速度センサ１５から受信する。
挙動検出部２２は、ジャイロセンサ１４及び加速度センサ１５から受信したこれらの信号に基づいて、車両１の基準静止姿勢からのピッチ方向及びロール方向の変位量、ヨーレート、横Ｇ、並びに前後方向の加減速度の少なくとも１つを、車両１の挙動変化量として検出する。挙動検出部２２は、操舵角センサ１３が検出した操舵制御量に基づいてヨーレートもしくは横Ｇを推定してもよい。

物標位置蓄積部２３は、物標位置検出部２０から相対位置信号を受信し、移動量推定部２１から移動量信号を受信する。
物標位置蓄積部２３は、相対位置信号が示す車両１の周囲の物標の相対位置を記憶装置２４に蓄積する。
また、物標位置蓄積部２３は、過去に蓄積した物標の相対位置を、現在までの経過時間と移動量信号が示す移動量ΔＰを用いて、車両１の現在位置に対する相対位置へ補正する。すなわち、物標位置蓄積部２３は、現在までの経過時間に車両が移動した移動量ΔＰだけ車両１の移動方向と逆方向に相対位置を移動させる。

物標位置蓄積部２３は、補正した相対位置である物標位置のデータ（以下「物標位置データ」と記載する場合がある）を記憶装置２４に蓄積する。このとき挙動検出部２２は、物標位置データの相対位置の検出時に検出した挙動変化量を、物標位置データに付加して記憶装置２４に記憶する。
既に物標位置データが記憶装置２４に蓄積されている場合には、物標位置蓄積部２３は、移動量信号が示す移動量ΔＰを用いて蓄積された物標位置データを更新する。すなわち物標位置蓄積部２３は、蓄積された物標位置データの相対位置を、移動量ΔＰ分だけ車両１の移動方向と逆方向に移動させる。その後、物標位置蓄積部２３は、移動量ΔＰ分だけ相対移動させた相対位置を、蓄積していた物標位置データに上書きする。

選択部２５は、記憶装置２４に蓄積されている物標位置データから、車両１の自己位置の推定に用いる物標位置データを選択する。自己位置の推定に用いるために選択される物標位置データを、以下「選択済物標位置データ」と記載する場合がある。
選択部２５は、例えば車両１の現在位置の周囲の物標の物標位置データを選択してよい。例えば、車両１の現在位置から約２０ｍ以内の物標の物標位置データを選択する。車両１の現在位置の周囲の物標の物標位置データは、移動量ΔＰを用いた補正による誤差の蓄積が少ないので位置精度が高い傾向にある。例えば、道路境界であるレーンや縁石の位置データは走路内の横位置の精度が高い。

また、車両１の挙動変化量が大きい場合には、物標の相対位置を検出する物標検出センサである撮像装置１０や距離測定装置１１の向きが、車両１の姿勢変化により変化してしまい、センサが検出した物標の相対位置に誤差が生じる。
図３を参照する。上段は、車両１の姿勢が所定の基準静止姿勢にある状態を示す。例えば基準静止姿勢は、車両１の車体水平方向と路面とが平行になる姿勢であってよい。下段は、基準静止姿勢からピッチ方向に車両１が回転している状態を示す。

車両１が基準静止姿勢にある場合、車両１から距離Ｌ１だけ離れた前方の物標は、車体水平方向より角度θ下方の方向に検出される。
車両１がピッチ方向に回転していると（すなわちピッチ方向に変位していると）、車両１から距離Ｌ２だけ離れた物標が、車体水平方向より同一角度θ下方の方向に検出される。すなわち、車両１がピッチ方向に回転していることにより、車両１から距離Ｌ２だけ離れた物標との相対距離を誤って距離Ｌ１と検出することになる。このため測定誤差（Ｌ１−Ｌ２）が発生する。

ピッチ方向の変位量に関しては、加減速などにより車両１のピッチ方向の変位が変化し、停止線などの物標の検出位置が変化してしまうことがある。例えば、ブレーキによる減速でピッチ方向に１度程度の姿勢変化があった場合に、距離５ｍ先の停止線までの距離を約３０ｃｍ短く誤算出してしまう。そのような物標位置データに基づき自己位置を推定すると、正確な位置より停止線へ３０ｃｍ近く自己位置を誤算出してしまうため、交差点の右左折時などで所望の走行経路から外れてしまうおそれがある。

このため選択部２５は、物標位置データに付加されて挙動変化量として記憶装置２４に記憶されているピッチ方向の変位量を参照し、ピッチ方向の変位量が所定の閾値未満である物標位置データを選択済物標位置データとして選択する。すなわち選択部２５は、ピッチ方向の変位量が閾値未満である期間中に検出した相対位置の物標位置データを、選択済物標位置データとして選択する。
なお、ピッチ方向の変化は、車両１が加減速した際に発生する。したがって、挙動変化量として記憶装置２４に記憶されている加減速度が所定の閾値未満である物標位置データを選択済物標位置データとして選択してもよい。

図４を参照する。上段は、車両１の姿勢が所定の基準静止姿勢にある状態を示す。下段は、基準静止姿勢からロール方向に車両１が回転している状態を示す。
車両１が基準静止姿勢にある場合、車両１から距離Ｌ１だけ離れた側方の物標は、車体水平方向より角度θ下方の方向に検出される。
車両１がロール方向に回転していると（すなわちロール方向に変位していると）、車両１から距離Ｌ２だけ離れた物標が、車体水平方向より同一角度θ下方の方向に検出される。すなわち、車両１がロール方向に回転していることにより、車両１から距離Ｌ２だけ離れた物標との相対距離を誤って距離Ｌ１と検出することになる。このため測定誤差（Ｌ１−Ｌ２）が発生する。

ロール方向の変位量に関しては、カントなどの影響によりロール方向の変位が変化し、車両１の側方にある白線や縁石等の物標の位置を誤算出してしまうことがある。例えば、カント２％程度の道路においてロール方向に１度程度の姿勢変化があった場合に、距離５ｍ横の白線までの距離が約３０ｃｍ短く誤算出してしまう。そのような物標位置データに基づき自己位置を推定すると、横方向に３０ｃｍずれて自己位置を誤算出してしまうため、縁石などに乗り上げてしまう可能性もある。

このため選択部２５は、物標位置データに付加されて挙動変化量として記憶装置２４に記憶されているロール方向の変位量を参照し、ロール方向の変位量が所定の閾値未満である物標位置データを選択済物標位置データとして選択する。すなわち選択部２５は、ロール方向の変位量が閾値未満である期間中に検出した相対位置の物標位置データを、選択済物標位置データとして選択する。
なお、ロール方向の変位は、車両１の旋回時に発生する。したがって、挙動変化量として記憶装置２４に記憶されているヨーレート又は横Ｇが所定の閾値未満である物標位置データを選択済物標位置データとして選択してもよい。

このようにして選択部２５は、記憶装置２４に蓄積した物標位置データのうち、挙動変化量が閾値未満である期間中に検出した相対位置の物標位置データを、選択済物標位置データとして選択する。
挙動変化量が所定の閾値未満である物標位置データを選択することにより、車両１の挙動変化による誤差の少ない物標位置データが、車両１の自己位置の推定に用いる選択済物標位置データとして選択される。言い換えれば、挙動変化量が所定の閾値以上である物標位置データを選択しないことにより、車両１の挙動変化による誤差の大きな物標位置データが、車両１の自己位置の推定に用いる選択済物標位置データから除外される。なお、挙動変化量が閾値未満である期間中に検出した相対位置の物標位置データを、選択済物標位置データとしてすべて選択する必要はなく、地図情報取得部２７が取得した地図情報と照合して、車両１の自己位置を推定できるために必要な物標位置データのみ選択するようにしてもよい。

図５を参照する。円形のプロットは物標位置データが示す物標の位置を、三角形のプロットは地図情報が示す物標の地図上の位置を模式的に示す。
挙動変化量が所定の閾値未満の物標位置データを選択することで、車両１の挙動変化による誤差が小さく地図上の位置に良好にマッチングする物標位置データを選択し、挙動変化による誤差が大きく地図上の位置にマッチングしにくいデータを除外できる。

図２を参照する。位置推定部２６は、選択済物標位置データを、地図情報取得部２７が取得した地図情報と照合することにより、車両１の自己位置を推定する。
地図情報取得部２７は、地図データと、地図データ上に存在する物標の地図上の位置を示す地図情報を取得する。例えば、地図情報取得部２７は、カーナビゲーションシステムや地図データベース等である。なお、地図情報取得部２７は、無線通信（路車間通信、または、車車間通信でも可）等の通信システムを介して外部から地図情報を取得してもよい。この場合、地図情報取得部２７は、定期的に最新の地図情報を入手して、保有する地図情報を更新してもよい。また、地図情報取得部２７は、車両１が実際に走行した走路で検出した物標の位置情報を、地図情報として蓄積してもよい。

位置推定部２６は、例えば以下のようなデータ照合処理によって、選択済物標位置データと地図情報とを照合して車両１の自己位置を推定してよい。
図６を参照する。参照符号Ｓ_ｉは選択済物標位置データを示す。インデックスｉは１〜Ｎの整数であり、Ｎは選択済物標位置データの個数である。
位置推定部２６は、前回の処理周期で推定した自己位置を移動量ΔＰで補正して車両１の仮位置を決定する。

位置推定部２６は、車両１の地図上の位置が仮位置であると仮定して、選択済物標位置データＳ_ｉが示す物標の相対位置を地図上の絶対位置に変換する。位置推定部２６は、選択済物標位置データＳ_ｉの絶対位置に最も近い地図情報中の物標の位置情報Ｍ_ｊを選択する。図６の例では、位置情報Ｍ_ｘが選択済物標位置データＳ_１に最も近く、位置情報Ｍ_ｙが選択済物標位置データＳ_２に最も近く、位置情報Ｍ_ｚが選択済物標位置データＳ_３に最も近い。
位置推定部２６は、選択済物標位置データＳ_ｉとこのデータに最も近い位置情報Ｍ_ｊとの距離Ｄ_ｉｊを算出し、以下の式（１）を用いて距離Ｄ_ｉｊの平均Ｓを算出する。

位置推定部２６は、数値解析により、平均Ｓが最小となる車両１の位置及び姿勢を算出して、車両１の自己位置の推定値として決定する。位置推定部２６は、自己位置の推定値を運転支援システム３へ出力する。

（動作）
次に、第１実施形態に係る自己位置推定装置２の動作について説明する。図７を参照する。
ステップＳ１において撮像装置１０、距離測定装置１１、及び物標位置検出部２０は、車両１の周囲に存在する物標の車両１に対する相対位置を検出する。物標位置検出部２０は、検出した相対位置を示す相対位置信号を物標位置蓄積部２３へ出力する。
ステップＳ２において移動量推定部２１は、前回の処理周期で車両１の自己位置を推定してから現在までの車両１の移動量ΔＰを推定する。

ステップＳ３においてジャイロセンサ１４、加速度センサ１５、及び挙動検出部２２は、車両１の挙動変化量を検出する。
ステップＳ４において物標位置蓄積部２３は、相対位置信号が示す車両１の周囲の物標の相対位置を記憶装置２４に蓄積する。また物標位置蓄積部２３は、過去に蓄積した物標の相対位置を、現在までの経過時間と移動量信号が示す移動量ΔＰを用いて車両１の現在位置に対する相対位置へ補正し、物標位置データとして記憶装置２４に蓄積する。このとき挙動検出部２２は、物標位置データの相対位置の検出時に検出した挙動変化量を、物標位置データに付加して記憶装置２４に記憶する。

ステップＳ５において選択部２５は、記憶装置２４に蓄積した物標位置データのうち、挙動変化量が閾値未満である期間中に検出した相対位置の物標位置データを、選択済物標位置データとして選択する。
ステップＳ６において位置推定部２６は、選択済物標位置データと地図情報とを照合して車両１の自己位置を推定する。
ステップＳ７において運転支援システム３は、位置推定部２６が推定した車両１の自己位置を用いて、運転者による車両１の運転に対する運転支援を実施する。

（第１実施形態の効果）
（１）物標検出センサとしての撮像装置１０及び距離測定装置１１と物標位置検出部２０とは、車両１の周囲に存在する物標の車両１に対する相対位置を検出する。移動量推定部２１は、車両１の移動量を推定する。物標位置蓄積部２３は、車両１の移動量に基づき相対位置を補正して物標位置データとして蓄積する。挙動センサとしてのジャイロセンサ１４及び加速度センサ１５と挙動検出部２２とは、車両１の挙動変化量を検出する。選択部２５は、蓄積した物標位置データのうち、挙動変化量が閾値未満である期間中に検出した相対位置の物標位置データを選択する。位置推定部２６は、選択した物標位置データと物標の地図上の位置を示す地図情報とを照合することで車両１の現在位置を推定する。

挙動変化量が閾値未満である期間に検出した相対位置の物標位置データを選択して車両１の位置推定に用いるので、移動体の挙動変化による姿勢変化のために物標位置データに誤差が発生しても、誤差が大きい物標位置データを位置推定から除外できる。このため、移動体の挙動変化により物標位置データに発生する誤差による車両１の位置推定精度の低下を抑制できる。

（２）選択部２５は、車両１の現在位置の周囲の物標の物標位置データを選択し、位置推定部２６は、選択した物標位置データと地図情報と照合する。車両１の現在位置の周囲の物標の物標位置データは、移動量ΔＰを用いた補正による誤差の蓄積が少ないので位置精度が高い傾向にある。車両１の現在位置の周囲の物標の物標位置データを選択して車両１の位置推定に用いることにより、車両１の位置推定精度を向上することができる。

（３）挙動変化量には、車両１のピッチ方向の回転量を含んでよい。すなわち、選択部２５は、車両１のピッチ方向の回転量が閾値未満である期間中に検出した相対位置の物標位置データを、位置推定に用いる物標位置データとして選択してよい。このため、車両１のピッチ方向の回転により物標位置データに発生する誤差による車両１の位置推定精度の低下を抑制できる。

（４）挙動変化量には、車両１のロール方向の回転量を含んでよい。すなわち、選択部２５は、車両１のロール方向の回転量が閾値未満である期間中に検出した相対位置の物標位置データを、位置推定に用いる物標位置データとして選択してよい。このため、車両１のロール方向の回転により物標位置データに発生する誤差による車両１の位置推定精度の低下を抑制できる。

（５）挙動変化量には、車両１の加減速度を含んでよい。すなわち、選択部２５は、車両１の加減速度が閾値未満である期間中に検出した相対位置の物標位置データを、位置推定に用いる物標位置データとして選択してよい。車両１の加減速度は、前後方向の加減速度であってもよく横Ｇであってもよい。
車両１のピッチ方向の回転は前後方向の加減速度によって生じ、ロール方向の回転は横方向の加減速度によって生じる。
車両１の加減速度が閾値未満である期間中に検出した相対位置の物標位置データを用いることにより、加減速度によりピッチ方向又はロール方向に生じた回転により物標位置データに発生する誤差による車両１の位置推定精度の低下を抑制できる。

（変形例）
（１）選択部２５は、挙動変化量が閾値以上である期間に検出した相対位置の物標位置データを記憶装置２４から削除してもよい。すなわち、選択部２５は、挙動変化量が閾値未満である期間に検出した相対位置の物標位置データを選択して記憶装置２４に残してよい。位置推定部２６は、記憶装置２４に残っている物標位置データと物標の地図上の位置を示す地図情報とを照合することで車両１の現在位置を推定してよい。
挙動変化量が閾値以上である期間に検出した相対位置の物標位置データを記憶装置２４から削除することにより、記憶装置２４の記憶領域を有効に活用することができる。

（２）選択部２５は、車両１の挙動変化が一時的な変化であり自己位置の推定精度に影響しない場合には、挙動変化量が所定の閾値以上である期間の物標位置データであっても選択済物標位置データから除外しなくてもよい。
例えば、挙動変化量が閾値以上である期間が所定長以上継続しない場合には、選択部２５は、挙動変化量が閾値未満である期間に加え、挙動変化量が閾値以上である期間中に検出した相対位置の物標位置データも、選択済物標位置データとして選択する。
一方で、選択部２５は、挙動変化量が閾値以上である期間が所定長以上継続した場合に、挙動変化量が閾値未満である期間（すなわち挙動変化量が閾値以上である期間以外の期間）中に検出した相対位置の物標位置データを選択済物標位置データとして選択する。
例えば、選択部２５は、閾値以上の挙動変化量を所定回数連続して検出した場合に、挙動変化量が閾値以上である期間が所定長以上継続すると判断してよい。

また、選択部２５は、閾値以上の挙動変化量が継続した後、挙動変化量が閾値未満の期間が所定長以上継続しない場合には、挙動変化量が閾値未満の状態が所定長以上継続しない期間中に検出した相対位置の物標位置データを選択済物標位置データから除外する。
例えば選択部２５は、閾値以上の挙動変化量を所定回数連続して検出した期間の開始時点から、閾値未満の挙動変化量を所定回数連続して検出した期間の開始時点までの期間中に検出した相対位置の物標位置データを選択済物標位置データから除外する。
このように、挙動変化量が閾値以上である期間が所定長以上継続した場合に、この期間以外の期間に検出した相対位置の物標位置データを選択するので、自己位置の推定精度に影響を与える挙動変化が発生した期間の物標位置データを適切に除外できる。

（第２実施形態）
続いて、第２実施形態の自己位置推定装置２を説明する。
渋滞などで車両が低速で加減速を繰返した場合や、交差点などで車両が旋回をした場合には、車輪速パルスの精度が悪化して車両１の進行方向の移動量ΔＰの測定精度が低下する。このような移動量ΔＰを用いて記憶装置２４に蓄積した物標位置データを補正すると、物標位置データに誤差が蓄積する。車両１に大きなヨーレートや横Ｇが発生した場合も同様である。

このため、閾値以上の挙動変化量が発生した場合に、選択部２５はそれ以前に検出した相対位置の物標位置データを選択済物標位置データから除外し、挙動変化量が閾値未満になった後に検出した相対位置の物標位置データを選択済物標位置データとして選択する。すなわち選択部２５は、挙動変化量が閾値未満である状態が現在まで継続する期間中に検出した相対位置の物標位置データを選択済物標位置データとして選択する。なお、挙動変化量が閾値未満である状態が現在まで継続する期間中に検出した相対位置の物標位置データを選択済物標位置データとしてすべて選択する必要はなく、地図情報取得部２７が取得した地図情報と照合して、車両１の自己位置を推定できるために必要な物標位置データのみ選択するようにしてもよい。

図８を参照する。
ステップＳ１０及びＳ１１の処理は、図７のステップＳ１及びＳ２の処理と同様である。
ステップＳ１２において物標位置蓄積部２３は、相対位置信号が示す車両１の周囲の物標の相対位置を記憶装置２４に蓄積する。また物標位置蓄積部２３は、過去に蓄積した物標の相対位置を、現在までの経過時間と移動量信号が示す移動量ΔＰを用いて車両１の現在位置に対する相対位置へ補正し、物標位置データとして記憶装置２４に蓄積する。
ステップＳ１３の処理は、図７のステップＳ３の処理と同様である。

ステップＳ１４において選択部２５は、挙動変化量が閾値以上になったか否かを判断する。挙動変化量が閾値以上になった場合（ステップＳ１４：Ｙ）に処理はステップＳ１５へ進む。挙動変化量が閾値以上にならない場合（ステップＳ１４：Ｎ）に処理はステップＳ１７へ進む。
ステップＳ１５において選択部２５は、挙動変化量が閾値未満になったか否かを判断する。挙動変化量が閾値未満になった場合（ステップＳ１５：Ｙ）に処理はステップＳ１６へ進む。挙動変化量が閾値未満にならない場合（ステップＳ１５：Ｎ）に処理はステップＳ１７へ進む。

ステップＳ１６において選択部２５は、挙動変化量が閾値未満になる前に検出した相対位置の物標位置データを記憶装置２４から削除する。すなわち、選択部２５は、挙動変化量が閾値未満である状態が現在まで継続する期間中に検出した相対位置の物標位置データを選択して記憶装置２４に残す。
ステップＳ１７において位置推定部２６は、記憶装置２４に残っている物標位置データと地図情報とを照合することで車両１の現在位置を推定する。
ステップＳ１８の処理は、図７のステップＳ７の処理と同様である。

（第２実施形態の効果）
（１）選択部２５は、挙動変化量が閾値未満である状態が現在まで継続する期間中に検出した相対位置の物標位置データを選択済物標位置データとして選択する。位置推定部２６は、記憶装置２４に残っている選択済物標位置データと物標の地図上の位置を示す地図情報とを照合することで車両１の現在位置を推定する。このため、車両１の挙動変化による誤差を含んだ移動量ΔＰで補正され精度が低下した物標位置データを選択済物標位置データから除外できる。このため、挙動変化により移動量ΔＰの測定誤差が発生しても、自己位置の推定精度の低下を抑制することができる。

（２）挙動変化量には、車両１の加減速度を含んでよい。すなわち、選択部２５は、車両１の加減速度が閾値未満である状態が現在まで継続する期間中に検出した相対位置の物標位置データを選択済物標位置データとして選択してよい。車両１の加減速度は、前後方向の加減速度、横Ｇ、ヨーレートであってよい。
このため、車両１に生じた加減速度によって移動量ΔＰの測定誤差が発生しても、自己位置の推定精度の低下を抑制することができる。

（第３実施形態）
続いて、第３実施形態の自己位置推定装置２を説明する。
挙動変化量が閾値未満である状態が継続している間に検出した相対位置の物標位置データを用いて自己推定を行うことで、物標との相対位置や移動量ΔＰに生じる測定誤差を抑制して車両１の自己位置を推定できる。
したがって、ある第１期間で挙動変化量が閾値以上になった場合、第１期間以前の挙動変化量が閾値未満であった第２期間中と、第１期間以後に挙動変化量が閾値未満になる第３期間中では、それぞれ車両１の自己位置を高い精度で検出することができる。このため、第２期間中に推定した車両１の自己位置と第３期間中に推定した車両１の自己位置との間の相対位置を高い精度で算出することができる。

したがって、第２期間に検出した相対位置の物標位置データが、挙動変化量が閾値以上である第１期間に誤差を含んだ移動量ΔＰで補正されて精度が低下しても、第２期間で推定した自己位置と第３期間で推定した自己位置との間の相対位置を用いて補正できる。
第３実施形態の自己位置推定回路１６は、第２期間中に推定した自己位置と第３期間中に推定した自己位置との間の相対位置を用いて第２期間に検出した相対位置の物標位置データを補正する。

図９を参照する。自己位置推定回路１６は、補正部２８を備える。自己位置推定回路１６が備えるプロセッサは、記憶装置２４に格納されたコンピュータプログラムを実行することにより、補正部２８の機能を実現する。
位置推定部２６は、第１期間以前の挙動変化量が閾値未満であった第２期間において、第２期間中に検出した相対位置の物標位置データと地図情報とを照合することにより第１期間以前の車両１の第１位置を推定する。位置推定部２６は、第１位置を運転支援システム３及び補正部２８に出力する。
補正部２８は、第２期間において推定した車両１の第１位置の情報を、第２期間中に検出した相対位置の物標位置データに付加して記憶装置２４に記憶する。

位置推定部２６は、第１期間以後の挙動変化量が閾値未満であった第３期間において、第３期間中に検出した相対位置の物標位置データと地図情報とを照合することにより第１期間以後の車両１の第２位置を推定する。位置推定部２６は、第２位置を補正部２８に出力する。
補正部２８は、第１位置と第２位置との相対位置に基づいて、第２期間中に検出した相対位置の物標位置データを補正する。

第２期間中に検出した相対位置の物標位置データが補正された後、位置推定部２６は、補正された物標位置データ及び第３期間中に検出した相対位置の物標位置データと地図情報とを照合することにより、第１期間以後の車両１の第２位置を推定する。なお、補正された物標位置データ及び第３期間中に検出した相対位置の物標位置データのすべての物標位置データと地図情報とを照合する必要はなく、車両１の自己位置を推定できるために必要な物標位置データのみと地図情報とを照合するようにしてもよい。
位置推定部２６は、物標位置データの補正後に推定した第２位置を運転支援システム３に出力する。位置推定部２６は、物標位置データの補正後に推定した第２位置の情報を、第３期間中に検出した相対位置の物標位置データに付加して記憶装置２４に記憶する。

図１０を参照する。ステップＳ２０〜Ｓ２３の処理は、図８のステップＳ１０〜Ｓ１３の処理と同様である。
ステップＳ２４において選択部２５は、挙動変化量が閾値以上になったか否かを判断する。挙動変化量が閾値以上になった場合（ステップＳ２４：Ｙ）に処理はステップＳ２５へ進む。挙動変化量が閾値以上にならない場合（ステップＳ２４：Ｎ）に処理はステップＳ３２へ進む。
ステップＳ２５において選択部２５は、挙動変化量が閾値未満になったか否かを判断する。挙動変化量が閾値未満になった場合（ステップＳ２５：Ｙ）に処理はステップＳ２６へ進む。挙動変化量が閾値未満にならない場合（ステップＳ２５：Ｎ）に処理はステップＳ３２へ進む。

ステップＳ２６において選択部２５は、挙動変化量が閾値以上であった第１期間に検出した相対位置の物標位置データを記憶装置２４から削除する。
ステップＳ２７において選択部２５は、第１期間以後に挙動変化量が閾値未満になった第３期間に検出した相対位置の物標位置データを選択する。
ステップＳ２８において位置推定部２６は、ステップＳ２７で選択した物標位置データと地図情報とを照合して車両１の第２位置を推定する。

ステップＳ２９において補正部２８は、第１期間以前の挙動変化量が閾値未満になった第２期間の物標位置データに付加して記憶された、第２期間に推定した車両１の第１位置の情報を記憶装置２４から読み出す。補正部２８は、第１位置と第２位置との相対位置に基づいて、第２期間中に検出した相対位置の物標位置データを補正する。
ステップＳ３０において位置推定部２６は、記憶装置２４に残っている物標位置データ（すなわちステップＳ２９で補正した物標位置データ及び第３期間に検出した相対位置の物標位置データ）と地図情報とを照合することで第１期間以後の第２位置を推定する。補正部２８は、ステップＳ３０において推定した車両１の第２位置の情報を、第３期間中に検出した相対位置の物標位置データに付加して記憶装置２４に記憶する。
ステップＳ３１の処理は、図８のステップＳ１８の処理と同様である。
ステップＳ３２の処理は、図８のステップＳ１７の処理と同様である。ステップＳ３２の後、処理はステップＳ３１へ進む。

（第３実施形態の効果）
位置推定部２６は、挙動変化量が閾値以上である第１期間以前の期間であって挙動変化量が閾値未満である第２期間中に検出した相対位置の物標位置データと地図情報とを照合することにより第１期間以前の車両１の第１位置を推定する。また、位置推定部２６は、第１期間以後の期間であって挙動変化量が閾値未満である第３期間中に検出した相対位置の物標位置データと地図情報とを照合することにより第１期間以後の車両１の第２位置を推定する。補正部２８は、第１位置と第２位置との相対位置に基づいて、第２期間中に検出した相対位置の物標位置データを補正する。位置推定部２６は、補正した物標位置データ及び第３期間中に検出した相対位置の物標位置データと地図情報とを照合することにより車両１の自己位置を推定する。
これにより、挙動変化量が閾値以上である第１期間以前に検出した相対位置の物標位置データを再度利用することができるので、自己位置推定の精度を向上できる。

ここに記載されている全ての例及び条件的な用語は、読者が、本発明と技術の進展のために発明者により与えられる概念とを理解する際の助けとなるように、教育的な目的を意図したものであり、具体的に記載されている上記の例及び条件、並びに本発明の優位性及び劣等性を示すことに関する本明細書における例の構成に限定されることなく解釈されるべきものである。本発明の実施例は詳細に説明されているが、本発明の精神及び範囲から外れることなく、様々な変更、置換及び修正をこれに加えることが可能であると解すべきである。

１…車両，２…自己位置推定装置，３…運転支援システム，１０…撮像装置，１１…距離測定装置，１２…車輪速センサ，１３…操舵角センサ，１４…ジャイロセンサ，１５…加速度センサ，１６…自己位置推定回路，２０…物標位置検出部，２１…移動量推定部，２２…挙動検出部，２３…物標位置蓄積部，２４…記憶装置，２５…選択部，２６…位置推定部，２７…地図情報取得部，２８…補正部

Claims

移動体の周囲に存在する物標の前記移動体に対する相対位置を検出し、
前記移動体の移動量を推定し、
前記移動体の移動量に基づき前記相対位置を補正して物標位置データとして蓄積し、
前記移動体の挙動変化量を検出し、
蓄積した前記物標位置データのうち、前記挙動変化量が閾値未満である期間中に検出した前記相対位置の物標位置データを選択し、
選択した前記物標位置データと前記物標の地図上の位置を示す地図情報とを照合することにより前記移動体の現在位置を推定する、
ことを特徴とする自己位置推定方法。
前記挙動変化量が前記閾値以上である期間が所定長以上継続した場合に、前記挙動変化量が前記閾値以上である期間以外の期間中に検出した前記相対位置の物標位置データを選択して前記地図情報と照合し、
前記挙動変化量が前記閾値以上である期間が前記所定長以上継続しない場合に、前記挙動変化量が前記閾値以上である期間中に検出した前記相対位置の物標位置データを、前記地図情報と照合する物標位置データに含める、
ことを特徴とする請求項１に記載の自己位置推定方法。
前記移動体の現在位置の周囲の物標の物標位置データを選択して前記地図情報と照合することを特徴とする請求項１又は２に記載の自己位置推定方法。
前記挙動変化量は、前記移動体のピッチ方向の回転量を含むことを特徴とする請求項１〜３のいずれか一項に記載の自己位置推定方法。
前記挙動変化量は、前記移動体のロール方向の回転量を含むことを特徴とする請求項１〜４のいずれか一項に記載の自己位置推定方法。
前記挙動変化量は、前記移動体の加減速度を含むことを特徴とする請求項１〜５のいずれか一項に記載の自己位置推定方法。
前記挙動変化量が前記閾値未満である状態が現在まで継続する期間中に検出した前記相対位置の物標位置データを選択して、前記地図情報と照合することを特徴とする請求項１〜６のいずれか一項に記載の自己位置推定方法。
前記挙動変化量が前記閾値以上である第１期間以前の期間であって前記挙動変化量が前記閾値未満である第２期間中に検出した前記相対位置の物標位置データと前記地図情報とを照合することにより前記第１期間以前の前記移動体の第１位置を推定し、
前記第１期間以後の期間であって前記挙動変化量が前記閾値未満である第３期間中に検出した前記相対位置の物標位置データと前記地図情報とを照合することにより前記第１期間以後の前記移動体の第２位置を推定し、
前記第１位置と前記第２位置との相対位置に基づいて、前記第２期間中に検出した前記相対位置の物標位置データを補正する、
ことを特徴とする請求項１〜６のいずれか一項に記載の自己位置推定方法。
移動体の周囲に存在する物標の前記移動体に対する相対位置を検出する物標検出センサと、
前記移動体の車輪速を検出する車輪速センサと、
前記移動体の挙動変化量を検出する挙動センサと、
少なくとも前記車輪速センサの検出結果に応じて前記移動体の移動量を推定し、前記物標検出センサが検出した前記相対位置を前記移動量に基づき補正して物標位置データとして記憶装置に蓄積し、蓄積した前記物標位置データのうち、前記挙動変化量が閾値未満である期間中に検出した前記相対位置の物標位置データを選択し、選択した前記物標位置データと前記物標の地図上の位置を示す地図情報とを照合することにより前記移動体の現在位置を推定する自己位置推定回路と、
を備えることを特徴とする自己位置推定装置。