JP5865325B2 - 移動体位置算出装置及び方法 - Google Patents

Description

この発明は、右車輪及び左車輪を有する移動体に搭載された移動体位置算出装置、及び当該装置を用いる移動体位置算出方法に関する。

従来から、車輪を有する移動体を自動運転により所望の位置に移動させるための支援装置が開発されている。例えば、特許文献１では、車両の右車輪及び左車輪に一対の車輪センサを設け、該一対の車輪センサからの出力パルスの計数値に基づき車両の現在位置を算出する駐車支援装置が提案されている。

特開２０１０−２６９７０７号公報（段落［０１０５］〜［０１０９］等）

ところで、種々の因子により、各車輪センサからのパルスの出力タイミングのずれ（換言すれば、位相ずれ）が生じる場合がある。例えば、計数の時間範囲がきわめて小さい場合又はきわめて小さい車速で直進する場合、時間の経過に伴って計数値が振動し、計数値の差分値が「１」と「−１」の間で交互に揺らぐ現象が発生し易い。

このため、採用する算出方法によっては、例えば「直進」と認識されずに、左右のいずれか一方向に僅かながら「旋回」するとして誤って認識される場合がある。その結果、車両の現在位置の算出精度が低下するという懸念があった。

本発明は、上記した課題を解決するためになされたものであり、一対の車輪センサからの出力タイミングが異なる場合であっても、移動体の現在位置の算出精度を向上可能な移動体位置算出装置及び方法を提供することを目的とする。

本発明に係る移動体位置算出装置は、右車輪及び左車輪を有する移動体に搭載された装置であって、前記右車輪の回転量に比例する数の第１パルス及び前記左車輪の回転量に比例する数の第２パルスを出力するパルス出力手段と、前記パルス出力手段からの前記第１パルスの出力数である第１出力値及び前記第２パルスの出力数である第２出力値を、所定の時間範囲内にて計数するパルス計数手段と、前記パルス計数手段により計数された前記第１出力値及び前記第２出力値に基づいて前記移動体の現在位置を算出する位置算出手段とを備え、前記位置算出手段は、少なくとも、前記第１出力値から前記第２出力値を引いた差分値の絶対値が１である場合、前記第１出力値及び／又は前記第２出力値に対して１より小さい正値を加減算することで、前記差分値の絶対値を１よりも小さくする補正を行った後に前記現在位置を算出する。

このように、少なくとも、第１出力値から第２出力値を引いた差分値の絶対値が１である場合、前記第１出力値及び／又は前記第２出力値に対して１より小さい正値を加減算することで、前記差分値の絶対値を１よりも小さくする補正を行った後に現在位置を算出するようにしたので、出力タイミングに起因する差分値の振動幅が計数分解能（すなわち、１）よりも小さくなり、算出の際にこの振動の影響を受け難くなる。これにより、一対の車輪センサからの出力タイミングが異なる場合であっても、移動体の現在位置の算出精度を向上できる。

また、前記位置算出手段は、前記第１出力値及び前記第２出力値を中間値にて等しくする前記補正を行った後に前記現在位置を算出することが好ましい。これにより、出力タイミングに起因する差分値の振動を遮断可能であり、算出の際にこの振動の影響を受けることがなくなる。

また、前記位置算出手段は、前記補正の後の計数タイミングでの前記第１出力値及び前記第２出力値に対して、前記補正による加算量又は減算量を相殺する調整を行った後に前記現在位置を算出することが好ましい。補正による加算量又は減算量を相殺する調整を行うことで、算出精度が統計的・巨視的に維持される。

また、前記位置算出手段は、前記補正の後に前記第１パルスが初めて出力された計数タイミングでの前記第１出力値に対して前記加算量を相殺する調整を行った後、又は、前記補正の後に前記第２パルスが初めて出力された計数タイミングでの前記第２出力値に対して前記加算量を相殺する調整を行った後に前記現在位置を算出することが好ましい。これにより、調整された第１出力値又は第２出力値の符号が正から負に変化することを防止可能であり、算出精度が突発的に低下することを抑制できる。

また、前記位置算出手段は、更に、前回の計数タイミングでの前記差分値と今回の計数タイミングでの前記差分値との積が−１である場合、前記補正を行った後に前記現在位置を算出することが好ましい。この積が−１である場合に移動体が直進している可能性が高いので、算出の際に上記した振動の影響を受け易くなっている。すなわち、算出精度の向上効果が一層顕著に現われる。

また、前記位置算出手段は、更に、前記第１出力値及び前記第２出力値がいずれも閾値を下回る場合、前記補正を行った後に前記現在位置を算出することが好ましい。第１出力値及び第２出力値がいずれも閾値を下回る場合に移動体が低速で移動している可能性が高いので、算出の際に上記した振動の影響を受け易くなっている。すなわち、算出精度の向上効果が一層顕著に現われる。

また、前記位置算出手段は、前記第１出力値及び前記第２出力値の平均値を前記中間値として、前記現在位置を算出することが好ましい。これにより、第１出力値及び／又は第２出力値に加減算する値が常に一定（すなわち、０．５）となり、演算処理の簡略化・高速化を図ることができる。

本発明に係る移動体位置算出方法は、右車輪及び左車輪を有する移動体に搭載された装置を用いる方法であって、前記右車輪の回転量に比例する数の第１パルス及び前記左車輪の回転量に比例する数の第２パルスを出力するパルス出力ステップと、前記第１パルスの出力数である第１出力値及び前記第２パルスの出力数である第２出力値を、所定の時間範囲内にて計数するパルス計数ステップと、計数された前記第１出力値及び前記第２出力値に基づいて前記移動体の現在位置を算出する位置算出ステップとを備え、前記位置算出ステップでは、少なくとも、前記第１出力値から前記第２出力値を引いた差分値の絶対値が１である場合、前記第１出力値及び／又は前記第２出力値に対して１より小さい正値を加減算することで、前記差分値の絶対値を１よりも小さくする補正を行った後に前記現在位置を算出する。

本発明に係る移動体位置算出装置及び方法によれば、少なくとも、第１出力値から第２出力値を引いた差分値の絶対値が１である場合、前記第１出力値及び／又は前記第２出力値に対して１より小さい正値を加減算することで、前記差分値の絶対値を１よりも小さくする補正を行った後に現在位置を算出するようにしたので、出力タイミングに起因する差分値の振動幅が計数分解能（すなわち、１）よりも小さくなり、算出の際にこの振動の影響を受け難くなる。これにより、一対の車輪センサからの出力タイミングが異なる場合であっても、移動体の現在位置の算出精度を向上できる。

この実施形態に係る駐車支援装置が搭載された車両の全体構成図である。 図１に示す駐車支援装置の機能ブロック図である。 駐車スペースを探索する車両の位置関係を示す概略平面図である。 駐車座標系及び駐車経路の決定結果を示す概略説明図である。 図５Ａは、車両の位置及び傾きの算出方法を示す説明図である。図５Ｂは、車両の特徴点の位置の算出方法を示す説明図である。 図６Ａ及び図６Ｂは、パルス出力手段からの出力波形の時間特性を示すグラフである。 図６Ａ及び図６Ｂとは別の出力波形及び今回出力値の差分値の時間特性を示す概略説明図である。 図２に示す移動体位置算出装置の動作説明に供されるフローチャートである。 今回出力値を補正出力値に変換するためのテーブルデータの一例を示す図である。 図１０Ａ及び図１０Ｂは、図９に示すテーブルデータを用いた補正結果を示す概略説明図である。 図８のステップＳ３における詳細フローチャートである。 車両の切り返し動作が行われた場合にて好適なテーブルデータの一例を示す図である。

以下、本発明に係る移動体位置算出装置及び方法について、これを実施する駐車支援装置との関係において好適な実施形態を挙げ、添付の図面を参照しながら詳細に説明する。

［駐車支援装置１０が搭載された車両１２の全体構成］
図１は、この実施形態に係る駐車支援装置１０が搭載された車両１２の全体構成図である。四輪車である車両１２は、右前輪１３Ｒ、左前輪１３Ｌ、右後輪１４Ｒ、及び左後輪１４Ｌを有する。

車両１２は、ステアリングホイール１６と、ステアリングホイール１６が取り付けられたステアリングシャフト１８と、ステアリングシャフト１８を回転駆動する電動モータを有するステアリングアクチュエータ２０と、ステアリングシャフト１８の回転角度（操舵角）を検出する舵角センサ２２と、各種制御を実行する３つの電子制御ユニット（以下、ＥＣＵ２４、２５、２６）と、車両１２の駐車支援に関する各種制御を実行する電子制御ユニット（以下、駐車支援ＥＣＵ２７）とを備える。

ＥＣＵ２４、２５、２６及び駐車支援ＥＣＵ２７、中央処理装置（ＣＰＵ）及びメモリを備えるコンピュータである。ＥＣＵ２４〜２６は、駐車支援ＥＣＵ２７にそれぞれ接続されており、車両１２を目標駐車位置Ｐｅ（図４）に誘導するための各信号（以下、誘導信号）を駐車支援ＥＣＵ２７から取得する。

ＥＣＵ２４は、電動パワーステアリング（Electric Power Steering）に関する各種制御を司るユニットである。ＥＣＵ２４には、ステアリングホイール１６を作動させるステアリングアクチュエータ２０が接続されている。

ＥＣＵ２５は、電動パーキングブレーキ（Electric Parking Break）に関する各種制御を司るユニットである。ＥＣＵ２５には、図示しないブレーキを作動させるブレーキアクチュエータ２８が接続されている。

ＥＣＵ２６は、電動パワーシフト（Electric Power Shift）に関する各種制御を司るユニットである。ＥＣＵ２６には、自動変速機のシフトレバー（図示しない）を変更するシフトアクチュエータ２９が接続されている。また、駐車支援ＥＣＵ２７には、シフトレバーの位置（以下、シフト位置）を検出するシフト位置センサ３０と、車両１２の速度を検出する車速センサ３１とが更に接続されている。

右後輪１４Ｒの近傍には、該右後輪１４Ｒの回転量を検出する右車輪センサ３２Ｒが設けられている。左後輪１４Ｌの近傍には、該左後輪１４Ｌの回転量を検出する左車輪センサ３２Ｌが設けられている。右車輪センサ３２Ｒ及び左車輪センサ３２Ｌは、例えば、右後輪１４Ｒ及び左後輪１４Ｌの回転量を光学的に検出するロータリエンコーダで構成されている。

車両１２の前部中央及び後部中央には、該車両１２の前方及び後方の画像を表す撮像信号を取得するカメラ３４、３５がそれぞれ配置されている。また、車両１２の右側部及び左側部には、該車両１２の右側方及び左側方の画像を表す撮像信号を取得するカメラ３６Ｒ、３６Ｌがそれぞれ配置されている。駐車支援ＥＣＵ２７には、各カメラ３４、３５、３６Ｒ、３６Ｌからの撮像信号が逐次供給される。

車両１２の車室内には、報知等のための音声を発するスピーカ３８と、カメラ３４等により撮像された画像、地図情報等を表示するタッチパネルディスプレイ４０が配置されている。

駐車支援ＥＣＵ２７は、上記した各種センサから供給される検知信号に基づいて、車両１２の周囲の状況を認識し、ＥＣＵ２４及びステアリングアクチュエータ２０を介してステアリングホイール１６の制御を行う。これと併せて又はこれとは別に、駐車支援ＥＣＵ２７は、ＥＣＵ２６及びシフトアクチュエータ２９を介した車両１２の前進／後退の切り換え制御、ＥＣＵ２５及びブレーキアクチュエータ２８を介した車両１２の停止制御を行う。駐車支援ＥＣＵ２７は、これらの制御を介して、車両１２を自動操舵によって所望の駐車スペースに駐車させる自動駐車支援制御を実現する。

［駐車支援装置１０の機能ブロック図］
図２は、図１に示す駐車支援装置１０の機能ブロック図である。駐車支援ＥＣＵ２７は、駐車位置・経路決定部５０、パルス計数部５２（パルス計数手段）、位置算出部５４（位置算出手段）及び駐車制御部５６として機能する。

駐車位置・経路決定部５０は、センサ群５８から供給されたセンサ信号に基づいて車両１２の外界の状態を検知した後、車両１２の駐車スペース（目標駐車位置Ｐｅ）を決定する。そして、駐車位置・経路決定部５０は、支援開始位置Ｐｓ（車両１２の現在位置）から目標駐車位置Ｐｅまでの経路（以下、駐車経路８０；図４）を決定する。なお、センサ群５８は、カメラ３４、３５、３６Ｒ、３６Ｌ（図１）の全部又は一部であってもよいし、これ以外の検出手段（例えば、超音波センサ、レーダ）が含まれてもよい。

パルス計数部５２は、パルス出力手段３２としての右車輪センサ３２Ｒ及び左車輪センサ３２Ｌからの各パルスの出力数を所定の時間範囲にて計数する。これにより、パルス計数部５２は、右車輪センサ３２Ｒから出力されたパルス（以下、第１パルス）を計数した今回出力値Ｎ１（第１出力値）、及び左車輪センサ３２Ｌから出力されたパルス（以下、第２パルス）を計数した今回出力値Ｎ２（第２出力値）を取得する。ここで、右車輪センサ３２Ｒは右後輪１４Ｒ（図１）の回転量に比例する数の第１パルスを出力し、左車輪センサ３２Ｌは左後輪１４Ｌ（図１）の回転量に比例する数の第２パルスを出力する点に留意する。

位置算出部５４は、パルス計数部５２により計数された今回出力値Ｎ１、Ｎ２に基づいて所定の時間範囲での変位量を求め、前回算出された位置にこの変位量を加算することで、車両１２の現在位置を算出する。ここで、「現在位置」とは、車両１２の現時点での位置・姿勢を特定する各種情報を意味し、例えば、基準位置４４（図１）の座標及び傾きであっても、車両１２を外接する四角形の頂点の各座標であってもよい。図１の例では、基準位置４４は、右後輪１４Ｒ及び左後輪１４Ｌを連結する車軸４２の中点に相当する。

駐車制御部５６は、駐車位置・経路決定部５０により決定された駐車経路８０、及び位置算出部５４により算出された現在位置に基づいて、車両１２を誘導・制御するための誘導信号を生成し、ＥＣＵ２４〜２６に向けて出力する。

なお、移動体位置算出装置６０は、移動体としての車両１２に搭載され、該車両１２の位置を算出する装置である。本図例では、移動体位置算出装置６０は、パルス出力手段３２、パルス計数部５２及び位置算出部５４から構成される。

［駐車支援装置１０の基本的動作］
続いて、駐車支援装置１０の基本的動作について、図３及び図４を参照しながら説明する。

図３に示すように、運転者は、車両１２を矢印方向に直進させながら、道路７０の左側方にある駐車スペースを探索する場合を想定する。本図から理解されるように、車両１２の左方には、別の車両（以下、他車７１、７２、７３、７４）が既に駐車している。

その後、運転者は、他車７２、７３の間のスペース７６に駐車可能であると判断した場合、車両１２を一旦停止させた後、タッチパネルディスプレイ４０（図１）を介した所定のタッチ操作（例えば、［開始］アイコンのタッチ）を実行する。そうすると、駐車支援ＥＣＵ２７は、車両１２がスペース７６に駐車する支援動作を開始する。先ず、駐車位置・経路決定部５０の動作により、駐車座標系及び駐車経路８０がそれぞれ決定される。

図４に示すように、ｘ軸−ｙ軸の駐車座標系は、スペース７６の一部である目標エリア７８によって定義される。具体的には、ｘ軸は目標エリア７８の短辺方向に平行すると共に、ｙ軸は目標エリア７８の長辺方向に平行する。そして、車両１２の先頭位置に相当する短辺の中点が、駐車座標系の原点Ｏとして設定されている。

また、基準位置４４（図１）の軌跡である駐車経路８０は、車両１２の移動に伴って他車７１〜７４に干渉しない条件下に決定される。本図例では、スペース７６に対して車両１２の先頭部を旋回させて停止し、１回の切り返しのみで目標エリア７８に駐車する駐車経路８０を示している。この駐車経路８０は、右方向へ旋回しながら前進する第１経路８１と、左方向に旋回しながら後退する第２経路８２とから構成される。

第１経路８１上での走行動作は、駐車支援を開始する位置である支援開始位置Ｐｓ、右方向への転舵を開始する位置である中間位置Ｐｍ１、該右方向への転舵を終了する位置である中間位置Ｐｍ２、及び前進から後退に切り返す位置であって、左回りの後退旋回を開始する位置である中間位置Ｐｍ３で特徴付けられる。第２経路８２上での走行動作は、左方向に旋回しつつ後退する位置であって、真っ直ぐの状態での後退を開始する位置である中間位置Ｐｍ４、及び駐車支援を終了する位置である目標駐車位置Ｐｅで特徴付けられる。このように、上記した各位置の座標、及び隣接する位置間での旋回条件（具体的には半径及び中心点）を与えることで、駐車経路８０に沿った走行スケジュールを決定できる。

そして、駐車制御部５６は、決定された走行スケジュールに従って車両１２を誘導・制御するための誘導信号を生成する。ここで、駐車制御部５６は、位置算出部５４により逐次算出された位置に基づき、駐車座標系での車両１２の現在位置を認識しながら走行スケジュールを順次実行する。

そして、走行スケジュールがすべて完了した場合、駐車制御部５６による誘導動作が終了する。このようにして、駐車支援装置１０の支援を受けることで、自動的又は半自動的に、車両１２を目標エリア７８に駐車させることができる。

［位置算出方法の概要及びその問題点］
上記した誘導動作を実行するためには、車両１２の現在位置を精度よく把握することが肝要である。そこで、車両１２の位置情報の算出方法について、図５Ａ〜図６Ｂを参照しながら説明する。

図５Ａは、車両１２の位置（ｘ，ｙ）及び傾きθの算出方法を示す説明図である。時間（ｔ−Δｔ）における基準位置４４の座標及び傾きが（ｘｏ，ｙｏ）、θｏであったとする。この場合、時間Δｔが経過した後（時間ｔ）における車両１２の傾きθは、次の（１）式で算出される。なお、ｋは正の係数、Ｌｂは後輪のトレッドサイズである。
θ＝θｏ＋ｋ・Δｔ（Ｎ１−Ｎ２）／Ｌｂ ‥（１）

また、時間（ｔ−Δｔ）〜ｔにおける変位量（Δｘ，Δｙ）は、例えば、この時間範囲では旋回の中心点が変化しないと仮定した上で、幾何学的考察により算出される。そして、車両１２の位置（ｘ，ｙ）は、ｘ＝ｘｏ＋Δｘ、ｙ＝ｙｏ＋Δｙにより算出される。

図５Ｂは、車両１２の特徴点９１、９２、９３、９４の位置の算出方法を示す説明図である。時間ｔにおける基準位置４４の座標及び傾きが（ｘ，ｙ）、θであったとする。ここで、車両１２の外接四角形の各頂点を特徴点９１〜９４とすると、これらのｘ座標（順に、ｘ１、ｘ２、ｘ３、ｘ４）はそれぞれ、次の（２）〜（５）式で算出される。
ｘ１＝ｘ＋（ＷＢ＋ＦＯ）ｃｏｓθ＋０．５・ＶＷ・ｓｉｎθ ‥（２）
ｘ２＝ｘ＋（ＷＢ＋ＦＯ）ｃｏｓθ−０．５・ＶＷ・ｓｉｎθ ‥（３）
ｘ３＝ｘ−ＲＯ・ｃｏｓθ＋０．５・ＶＷ・ｓｉｎθ ‥（４）
ｘ４＝ｘ−ＲＯ・ｃｏｓθ−０．５・ＶＷ・ｓｉｎθ ‥（５）

なお、ＷＢはホイールベースの長さ、ＦＯはフロントオーバーハングの長さ、ＲＯはリアオーバーハングの長さ、ＶＷは車両１２の全幅をそれぞれ示す。

図６Ａ及び図６Ｂは、パルス出力手段３２からの出力波形の時間特性を示すグラフである。グラフの横軸は時間（単位は［ｍｓ］）であり、グラフの縦軸は第１パルス（上段）及び第２パルス（下段）の出力、具体的には電圧値又は電流値である。なお、破線状の縦線は、計数の各時間範囲における開始時点及び終了時点を模式的に示す線である。

図６Ａは、車両１２が相対的に大きい車速にて直進する場合のグラフに相当する。車両１２が直進する際、同一の時間範囲にて、右後輪１４Ｒの回転量と左後輪１４Ｌの回転量の間に偏差が生じない。よって、本図に示すように、各時間範囲における第１パルスの出力数が第２パルスの出力数に等しいか実質的に等しい状態（Ｎ１＝Ｎ２）が継続する。

図６Ｂは、車両１２が相対的に小さい車速にて左方向に旋回する場合のグラフに相当する。車両１２が旋回する際、同一の時間範囲にて、右後輪１４Ｒの回転量と左後輪１４Ｌの回転量の間に偏差が生じる。よって、本図に示すように、各時間範囲における第１パルスの出力数が第２パルスの出力数よりも大きい状態（Ｎ１＞Ｎ２）が継続する。

図７は、図６Ａ及び図６Ｂとは別の出力波形及び今回出力値Ｎ１、Ｎ２の差分値の時間特性を示す概略説明図である。本図は、車両１２がきわめて小さい車速にて直進する場合のグラフに相当し、２回分の計数につき１つのパルスが出力される場合を想定する。

車速が一定値である場合、右車輪センサ３２Ｒから出力される第１パルスの発生間隔（Ｔｃ）は、左車輪センサ３２Ｌから出力される第２パルスの発生間隔（Ｔｃ）に略一致する。ところが、種々の因子により、第１パルス及び第２パルスの出力タイミング（換言すれば、位相）が異なる場合がある。本図例では、第２パルスの出力タイミングは、第１パルスと比べてＴｐ（＜Ｔｃ）だけ遅延している。

本図に示すように、今回出力値Ｎ１、Ｎ２は、（Ｎ１，Ｎ２）＝（１，０）→（０，１）→（１，０）→（０，１）→‥、と遷移し、時間の経過に伴って各値が振動する。換言すれば、差分値（Ｎ１−Ｎ２）が「１」と「−１」の間で交互に揺らぐことになる。このように、今回出力値Ｎ１、Ｎ２は、統計的・巨視的には略同じ値で推移するが、「０」を保った状態は維持されない。

このため、採用する算出手法によっては、車両１２が直進する状態であっても「直進」と認識されずに、左右のいずれか一方向に僅かながら「旋回」するとして誤って認識される場合がある。その結果、車両１２の位置及び／又は角度の算出精度が低下し、特に車速が小さい場合にその影響度が顕在化することがある。以下、この不都合を解決するための位置算出方法を新たに提案する。

［移動体位置算出装置６０の動作］
続いて、図２に示す移動体位置算出装置６０の動作について、図８のフローチャートを参照しながら詳細に説明する。

ステップＳ１において、パルス計数部５２は、右車輪センサ３２Ｒからの第１出力数を所定の時間範囲内にて計数し、今回出力値Ｎ１として取得する。また、パルス計数部５２は、左車輪センサ３２Ｌからの第２出力数を上記した時間範囲内にて計数し、今回出力値Ｎ２として取得する。ここでは、時間範囲として一定値を用いているが、計数のタイミング毎に変更してもよい。

ステップＳ２において、位置算出部５４は、前回の計数タイミングで計数された第１出力値（以下、前回出力値Ｍ１）、第２出力値（以下、前回出力値Ｍ２）を、図示しないバッファメモリからそれぞれ読み出す。

ステップＳ３において、位置算出部５４は、ステップＳ２で読み出した前回出力値Ｍ１、Ｍ２を用いて、ステップＳ１で取得された今回出力値Ｎ１、Ｎ２を補正する。その結果、補正された第１出力値（以下、補正出力値Ｎｃ１）及び第２出力値（以下、補正出力値Ｎｃ２）が得られる。

ステップＳ４において、位置算出部５４は、ステップＳ１で取得された今回出力値Ｎ１、Ｎ２を上述のバッファメモリに格納することで、前回出力値Ｍ１、Ｍ２をそれぞれ更新する。

ステップＳ５において、位置算出部５４は、ステップＳ３で取得された補正出力値Ｎｃ１、Ｎｃ２を用いて、駐車座標系における車両１２の現在位置を算出する。

ステップＳ６において、位置算出部５４は、ステップＳ５で算出された基準位置４４の座標（ｘ，ｙ）及び傾きθを示す信号を駐車制御部５６に向けて出力する。

［今回出力値Ｎ１、Ｎ２の補正方法］
続いて、図８のステップＳ３における補正方法の具体例について、図９〜図１２を参照しながら詳細に説明する。

＜第１の補正方法＞
第１の補正方法は、固定の変換テーブルを用いる方法に関する。以下、図９〜図１０Ｂを参照しながら詳細に説明する。

図９は、今回出力値Ｎ１、Ｎ２を補正出力値Ｎｃ１、Ｎｃ２に変換するためのテーブルデータの一例を示す図である。本図は、４つの変数（Ｍ１、Ｍ２、Ｎ１、Ｎ２）を入力値とし、２つの変数（Ｎｃ１、Ｎｃ２）を出力値とするテーブルを示している。例えば、（Ｍ１，Ｍ２）＝（１，０）且つ（Ｎ１，Ｎ２）＝（１，０）を満たすとき、テーブルＩＤ「Ｃ１」を参照することで、（Ｎｃ１，Ｎｃ２）＝（１，０）が得られる。また、（Ｍ１，Ｍ２）＝（１，０）且つ（Ｎ１，Ｎ２）＝（０，１）を満たすとき、テーブルＩＤ「Ｃ２」を参照することで、（Ｎｃ１，Ｎｃ２）＝（０．５，０．５）が得られる。

なお、このテーブルデータは、前回出力値Ｍ１（Ｍ２）及び今回出力値Ｎ１（Ｎ２）の平均値を、補正出力値Ｎｃ１（Ｎｃ２）とする演算子に相当する。より詳細には、Ｎｃ１＝（Ｎ１＋Ｍ１）／２、Ｎｃ２＝（Ｎ２＋Ｍ２）／２、を満たすように設計されている。

図１０Ａ及び図１０Ｂは、図９に示すテーブルデータを用いた補正結果を示す概略説明図である。グラフの横軸及び縦軸の定義は図６Ａと同一であるので、その説明を省略する。

図１０Ａは、図７と同様に、車両１２の直進走行下に、２回分の計数タイミングにつき１つのパルスが出力される場合を想定する。第１パルス及び第２パルスの出力波形は図７と同一である。この今回出力値Ｎ１、Ｎ２の時系列に対して上記した変換テーブルを作用することで、補正出力値Ｎｃ１、Ｎｃ２の時系列に変換される。

本図に示すように、テーブルデータは、（ｉｎｉｔ）→（Ｃ２）→（Ｃ４）→（Ｃ２）→（Ｃ４）→‥のＩＤの順で参照される。なお、（ｉｎｉｔ）は、変換テーブルを参照せずに予め設定された初期条件に相当する。

その結果、補正出力値Ｎｃ１、Ｎｃ２は、（Ｎｃ１，Ｎｃ２）＝（０．５，０．５）→（０．５，０．５）→（０．５，０．５）→（０．５，０．５）→‥、と遷移し、時間の経過にかかわらず各値が一定になる。換言すれば、差分値（Ｎｃ１−Ｎｃ２）が「０」に保たれる。

なお、ハッチングを付した矩形は、今回出力値Ｎ１、Ｎ２に対する加算量又は減算量を模式的に示す仮想パルスである。各時間軸から上方に延びる仮想パルスは加算量（正値）であり、各時間軸から下方に延びる仮想パルスは減算量（負値）である。この仮想パルスは、第１パルス及び第２パルスの単位計数値（１）の半分（０．５）に相当する。このように、補正の後の計数タイミング（本図例では、次の計数タイミング）での今回出力値Ｎ１、Ｎ２に対して、この補正による加算量又は減算量を相殺する調整を順次行っている。

図１０Ｂは、車両１２の直進走行下に、３回分の計数タイミングにつき２つのパルスが出力される場合を想定する。この今回出力値Ｎ１、Ｎ２の時系列に対して上記した変換テーブルを作用することで、補正出力値Ｎｃ１、Ｎｃ２の時系列に変換される。

本図に示すように、テーブルデータは、（ｉｎｉｔ）→（Ｃ７）→（Ｃ２）→（Ｃ６）→（Ｃ７）→‥のＩＤの順で参照される。その結果、補正出力値Ｎｃ１、Ｎｃ２は、（Ｎｃ１，Ｎｃ２）＝（０．５，０．５）→（０．５，０．５）→（０．５，０．５）→（０．５，０．５）→‥、と遷移し、時間の経過にかかわらず各値が一定になる。換言すれば、差分値（Ｎｃ１−Ｎｃ２）が「０」に保たれる。

このようにして、位置算出部５４は、今回出力値Ｎ１、Ｎ２に対して１よりも小さい正値（ここでは、０、５）を加減算することで、｜Ｎ１−Ｎ２｜＜１、より好ましくは｜Ｎ１−Ｎ２｜＝０にとなるように補正する。なお、図９に示すテーブルデータの各値に対して、任意の自然数であるオフセット値を加算することで、Ｎ１、Ｎ２＞１の場合でも本手法をそのまま適用できる。

＜第２の補正方法＞
第２の補正方法は、条件分岐により補正要否を判定する方法に関する。以下、図１１のフローチャートを参照しながら詳細に説明する。

ステップＳ３１において、位置算出部５４は、｜Ｎ１−Ｎ２｜＝１を満たすか否かを判定する。満たすと判定された場合、次のステップ（Ｓ３２）に進む。

ステップＳ３２において、位置算出部５４は、（Ｍ１−Ｍ２）（Ｎ１−Ｎ２）＝−１を満たすか否かを判定する。満たすと判定された場合、次のステップ（Ｓ３３）に進む。

ステップＳ３３において、位置算出部５４は、（Ｎ１＜Ｔｈ）且つ（Ｎ２＜Ｔｈ）を満たすか否かを判定する。満たすと判定された場合、次のステップ（Ｓ３４）に進む。閾値Ｔｈは２以上である任意の値であり、例えば、Ｔｈ＝２に設定することができる。

ステップＳ３４において、位置算出部５４は、所定の演算式に従って今回出力値Ｎ１、Ｎ２を補正する。より詳細には、Ｎ１＜Ｎ２を満たす場合、補正出力値Ｎｃ１、Ｎｃ２はそれぞれ、Ｎｃ１＝Ｎ１＋ε（０＜ε＜１）、Ｎｃ２＝Ｎ２＋ε−１とする。逆に、Ｎ１＞Ｎ２を満たす場合、Ｎｃ１＝Ｎ１＋ε−１、Ｎｃ２＝Ｎ２＋εとする。この結果、Ｎｃ１＝Ｎｃ２が成り立つ。このように、位置算出部５４は、今回出力値Ｎ１、Ｎ２を中間値にて等しくする補正を行う。

一方、上述したステップＳ３１、Ｓ３２、Ｓ３３にて少なくとも１つの条件を満たさないと判定された場合（ステップＳ３１〜Ｓ３３：ＮＯ）、ステップＳ３５に進む。

ステップＳ３５において、位置算出部５４は、補正出力値Ｎｃ１に今回出力値Ｎ１の値を代入すると共に、補正出力値Ｎｃ２に今回出力値Ｎ２の値を代入する。つまり、今回出力値Ｎ１、Ｎ２を補正することなくそのまま使用する。

ステップＳ３６において、位置算出部５４は、ステップＳ３４又はＳ３５で得られた補正出力値Ｎｃ１、Ｎｃ２を調整する。ここで、「調整」とは、ステップＳ３４の補正の後の計数タイミングでのＮ１、Ｎ２に対して加算量（＋ε）又は減算量（−ε）を相殺することを意味する。

例えば、補正の後に第１パルスが初めて出力された計数タイミングにて、今回出力値Ｎ１（実際は、Ｎｃ１）に対してεを減算することで加算量（＋ε）を相殺する。また、補正の後に第２パルスが初めて出力された計数タイミングにて、今回出力値Ｎ２（実際は、Ｎｃ２）に対してεを減算することで加算量（＋ε）を相殺する。

例えば、補正の後に第１パルスの出力数が初めて０であった計数タイミングにて、今回出力値Ｎ１（実際は、Ｎｃ１）に対してεを加算することで減算量（−ε）を相殺する。また、補正の後に第２パルスの出力数が初めて０であった計数タイミングにて、今回出力値Ｎ２（実際は、Ｎｃ２）に対してεを加算することで減算量（−ε）を相殺する。

以上のようにして、位置算出部５４は、所定の補正条件を満たす場合に今回出力値Ｎ１、Ｎ２を補正することで、補正出力値Ｎｃ１、Ｎｃ２を得る（図８のステップＳ３）。例えば、ε＝０．５とし、図７に示す出力波形に対して本補正処理を実行することで、第１の補正方法と同一の結果（図１０Ａ及び図１０Ｂ）が得られる。

ところで、図７例よりも更に小さい車速で直進する場合、或いは、車輪センサ３２Ｒ、３２Ｌにおける回転量の検出分解能が低い場合、例えば、（Ｎ１，Ｎ２）＝（１，０）→（０，０）→（０，１）→（０，０）→‥、と遷移することがある。そこで、位置算出部５４は、前回出力値Ｍ１、Ｍ２のみならず、その直近に少なくとも１回計数された出力値を記憶・参照してもよい。具体的には、前回出力値Ｍ１、Ｍ２が（Ｍ１，Ｍ２）＝（０，０）であった場合、前回出力値Ｍ１、Ｍ２に代わって前々回出力値を用いることで、出力タイミングに起因する差分値を小さくする効果が得られる。これにより、上記した場合であっても、車両１２の現在位置の算出精度を維持できる。

＜切り返し動作時の改良例＞
続いて、車両１２の切り返し動作が行われた場合にて好適な演算例について、図１２を参照しながら説明する。図４例では、車両１２が中間位置Ｐｍ３に到達した場合を想定する。

図１２は、今回出力値Ｎ１、Ｎ２を補正出力値Ｎｃ１、Ｎｃ２に変換するためのテーブルデータの別の一例を示す図である。本図は、４つの変数（Ｍ１、Ｍ２、Ｎ１、Ｎ２）を入力値とし、２つの変数（Ｎｃ１、Ｎｃ２）を出力値とするテーブルを示している。このテーブルデータは、Ｎｃ１＝（Ｎ１−Ｍ１）／２、Ｎｃ２＝（Ｎ２−Ｍ２）／２、を満たすように設計されている。例えば、（Ｍ１，Ｍ２）＝（１，０）且つ（Ｎ１，Ｎ２）＝（０，１）を満たすとき、テーブルＩＤ「Ｃ２」を参照することで、（Ｎｃ１，Ｎｃ２）＝（−０．５，０．５）が得られる。

このように、車両１２の進行方向が反転された場合であっても、前回出力値Ｍ１、Ｍ２を破棄せずに、しかも符号を反転させてそのまま利用することで、位置の算出精度が向上する。

［この実施形態の効果］
以上のように、右後輪１４Ｒ及び左後輪１４Ｌを有する車両１２に搭載された移動体位置算出装置６０は、右後輪１４Ｒの回転量に比例する数の第１パルス及び左後輪１４Ｌの回転量に比例する数の第２パルスを出力するパルス出力手段３２と、第１パルスの出力数である今回出力値Ｎ１及び第２パルスの出力数である今回出力値Ｎ２を、所定の時間範囲内にて計数するパルス計数部５２と、今回出力値Ｎ１、Ｎ２に基づいて車両１２の現在位置を算出する位置算出部５４を備える。

そして、位置算出部５４は、少なくとも、今回出力値Ｎ１、Ｎ２の差分値の絶対値｜Ｎ１−Ｎ２｜が１である場合、今回出力値Ｎ１、Ｎ２に対して１より小さい正値を加減算することで、｜Ｎ１−Ｎ２｜を１よりも小さくする補正を行った後に現在位置を算出する。

このように構成しているので、出力タイミングに起因する差分値の振動幅が計数分解能（すなわち、１）よりも小さくなり、算出の際にこの振動の影響を受け難くなる。これにより、パルス出力手段３２からの出力タイミングが異なる場合であっても、車両１２の現在位置の算出精度を向上できる。

また、位置算出部５４は、今回出力値Ｎ１、Ｎ２を中間値にて等しくする補正を行った後に現在位置を算出してもよい。これにより、出力タイミングに起因する差分値の振動を遮断可能であり、算出の際にこの振動の影響を受けることがなくなる。

また、位置算出部５４は、補正の後の計数タイミングでの今回出力値Ｎ１、Ｎ２に対して、当該補正による加算量又は減算量を相殺する調整を行った後に現在位置を算出してもよい。補正による加算量又は減算量を相殺する調整を行うことで、算出精度が統計的・巨視的に維持される。

また、位置算出部５４は、補正の後に第１パルス（又は第２パルス）が初めて出力された計数タイミングでの今回出力値Ｎ１、Ｎ２に対して加算量を相殺する調整を行った後に現在位置を算出してもよい。これにより、調整された今回出力値Ｎ１、Ｎ２の符号が正から負に変化することを防止可能であり、算出精度が突発的に低下することを抑制できる。

また、位置算出部５４は、更に、前回の計数タイミングでの差分値（Ｍ１−Ｍ２）と今回の計数タイミングでの差分値（Ｎ１−Ｎ２）との積が−１である場合、前記補正を行った後に前記現在位置を算出することが好ましい。この積が−１である場合に移動体が直進している可能性が高いので、算出の際に上記した振動の影響を受け易くなっている。すなわち、算出精度の向上効果が一層顕著に現われる。

また、位置算出部５４は、更に、今回出力値Ｎ１、Ｎ２がいずれも閾値Ｔｈを下回る場合、当該補正を行った後に現在位置を算出してもよい。今回出力値Ｎ１、Ｎ２がいずれも閾値Ｔｈを下回る場合に車両１２が低速で移動している可能性が高いので、算出の際に上記した振動の影響を受け易くなっている。すなわち、算出精度の向上効果が一層顕著に現われる。

また、位置算出部５４は、今回出力値Ｎ１、Ｎ２の平均値を中間値として、現在位置を算出してもよい。これにより、今回出力値Ｎ１、Ｎ２に加減算する値が常に一定（０．５）となり、演算処理の簡略化・高速化を図ることができる。

［補足］
なお、この発明は、上述した実施形態に限定されるものではなく、この発明の主旨を逸脱しない範囲で自由に変更できることは勿論である。

この実施形態では、右後輪１４Ｒ及び左後輪１４Ｌの回転量に基づいて車両１２の位置を算出しているが、右前輪１３Ｒ及び左前輪１３Ｌの回転量を用いても同様の作用効果が生じることは言うまでもない。

この実施形態では、車両１２として四輪車を例に挙げて説明したが、二輪車を含む車両であってもよく、車両以外であっても右車輪及び左車輪を有する移動体であれば種類は問わない。

１０…駐車支援装置 １２…車両
１４Ｒ…右後輪 １４Ｌ…左後輪
１６…ステアリングホイール １８…ステアリングシャフト
２０…ステアリングアクチュエータ ２２…舵角センサ
２４、２５、２６…ＥＣＵ ２７…駐車支援ＥＣＵ
２８…ブレーキアクチュエータ ２９…シフトアクチュエータ
３０…シフト位置センサ ３１…車速センサ
３２…パルス出力手段 ３２Ｒ、３２Ｌ…車輪センサ
４０…タッチパネルディスプレイ ４４…基準位置
５０…駐車位置・経路決定部 ５２…パルス計数部
５４…位置算出部 ５６…駐車制御部
６０…移動体位置算出装置 ７０…道路
７１〜７４…他車 ７６…スペース
７８…目標エリア ８０…駐車経路
８１…第１経路 ８２…第２経路
Ｐｅ…目標駐車位置 Ｐｍ１〜Ｐｍ４…中間位置
Ｐｓ…支援開始位置

Claims (8)

1. 右車輪及び左車輪を有する移動体に搭載された移動体位置算出装置であって、
   前記右車輪の回転量に比例する数の第１パルス及び前記左車輪の回転量に比例する数の第２パルスを出力するパルス出力手段と、
   前記パルス出力手段からの前記第１パルスの出力数である第１出力値及び前記第２パルスの出力数である第２出力値を、所定の時間範囲内にて計数するパルス計数手段と、
   前記パルス計数手段により計数された前記第１出力値及び前記第２出力値に基づいて前記移動体の現在位置を算出する位置算出手段と
   を備え、
   前記位置算出手段は、少なくとも、前記第１出力値から前記第２出力値を引いた差分値の絶対値が１である場合、前記第１出力値及び／又は前記第２出力値に対して１より小さい正値を加減算することで、前記差分値の絶対値を１よりも小さくする補正を行った後に前記現在位置を算出する
   ことを特徴とする移動体位置算出装置。
2. 請求項１記載の移動体位置算出装置において、
   前記位置算出手段は、前記第１出力値及び前記第２出力値を中間値にて等しくする前記補正を行った後に前記現在位置を算出する
   ことを特徴とする移動体位置算出装置。
3. 請求項１又は２に記載の移動体位置算出装置において、
   前記位置算出手段は、前記補正の後の計数タイミングでの前記第１出力値及び前記第２出力値に対して、前記補正による加算量又は減算量を相殺する調整を行った後に前記現在位置を算出することを特徴とする移動体位置算出装置。
4. 請求項３記載の移動体位置算出装置において、
   前記位置算出手段は、前記補正の後に前記第１パルスが初めて出力された計数タイミングでの前記第１出力値に対して前記加算量を相殺する調整を行った後、又は、前記補正の後に前記第２パルスが初めて出力された計数タイミングでの前記第２出力値に対して前記加算量を相殺する調整を行った後に前記現在位置を算出することを特徴とする移動体位置算出装置。
5. 請求項１〜４のいずれか１項に記載の移動体位置算出装置において、
   前記位置算出手段は、更に、前回の計数タイミングでの前記差分値と今回の計数タイミングでの前記差分値との積が−１である場合、前記補正を行った後に前記現在位置を算出することを特徴とする移動体位置算出装置。
6. 請求項１〜４のいずれか１項に記載の移動体位置算出装置において、
   前記位置算出手段は、更に、前記第１出力値及び前記第２出力値がいずれも閾値を下回る場合、前記補正を行った後に前記現在位置を算出することを特徴とする移動体位置算出装置。
7. 請求項２記載の移動体位置算出装置において、
   前記位置算出手段は、前記第１出力値及び前記第２出力値の平均値を前記中間値として、前記現在位置を算出することを特徴とする移動体位置算出装置。
8. 右車輪及び左車輪を有する移動体に搭載された装置を用いる移動体位置算出方法であって、
   前記右車輪の回転量に比例する数の第１パルス及び前記左車輪の回転量に比例する数の第２パルスを出力するパルス出力ステップと、
   前記第１パルスの出力数である第１出力値及び前記第２パルスの出力数である第２出力値を、所定の時間範囲内にて計数するパルス計数ステップと、
   計数された前記第１出力値及び前記第２出力値に基づいて前記移動体の現在位置を算出する位置算出ステップと
   を備え、
   前記位置算出ステップでは、少なくとも、前記第１出力値から前記第２出力値を引いた差分値の絶対値が１である場合、前記第１出力値及び／又は前記第２出力値に対して１より小さい正値を加減算することで、前記差分値の絶対値を１よりも小さくする補正を行った後に前記現在位置を算出する
   ことを特徴とする移動体位置算出方法。

Images