JP2015170133A - 軌跡推定装置、軌跡推定方法およびプログラム - Google Patents

Abstract

【課題】移動体の移動軌跡を精度よく推定する。【解決手段】車輪速受付部は、移動体の左右の前輪の車輪速の計測値を受付ける。操舵角受付部は、移動体の進行方向を変更する操舵角の計測値を受付ける。記憶部は、左右の前輪と左右の前輪より移動体における後部に設けられた左右の後輪との移動体の前後方向の距離を記憶している。また、記憶部は、左右の前輪の左右方向の間隔、および操舵角と操舵角による移動体の回転半径とに応じた定数を記憶している。軌跡推定部は、左右の前輪の車輪速の計測値、操舵角の計測値、前後方向の距離、左右方向の間隔、および定数に基づき、以下のように推定を行う。すなわち、軌跡推定部は、左右の後輪の回転中心を通る直線上の点を中心とする円上における左右の後輪の回転中心の中点の旋回量と、中点の並進量とを推定する。【選択図】図１

Description

本発明は、軌跡推定装置、軌跡推定方法およびプログラムに関する。

昨今、車両のような、前輪を操舵輪とし、左右前輪、左右後輪の４輪を持つ移動体について、車輪速情報を用いて自車の運動あるいは移動軌跡を推定する技術が提案されている。例えば、自動駐車を行うための移動軌跡のずれを防止する技術や、車両の走行条件が変化しても、予測走行軌跡と実際の走行軌跡とを一致させるように修正する技術などが知られている（例えば、特許文献１〜２参照）。移動体の移動軌跡を、車輪速情報を用いて、幾何学的に推定する技術も提案されている（例えば、特許文献３〜５参照）。

特開２００１−１３８９４１号公報 特開２００２−３６２３９０号公報 特開２０１１−８３８５号公報 特開平７−８１６０９号公報 特開平３−９０４８２号公報

ところで、昨今の車両は、前輪駆動の場合が多く、このため、前輪の車輪速が計測されることが多い。この場合、操舵角を考慮した移動体の移動を推定するには、２つの後輪の中点の移動を推定することが好ましい。

しかしながら、上記のような従来技術では、前輪駆動の車両の移動が、精度よく推定できないことがある。これは、例えば、以下のような理由による。すなわち、車両が円運動するとした場合の車両の向きが適切に考慮されていない、後輪の車輪速、または前後輪の車輪速を用いた式による推定が行われており、前輪の車輪速の計測値を当てはめることができない、などの理由である。

なお、かかる問題は、車両に限らず、車輪を有する他の移動体においても生ずるものである。

ひとつの側面によれば、本発明の目的は、移動体の移動軌跡を精度よく推定することである。

ひとつの態様である軌跡推定装置は、車輪速受付部と、操舵角受付部と、記憶部と、軌跡推定部とを有する。車輪速受付部は、移動体の左右の前輪の車輪速の計測値を受付ける。操舵角受付部は、移動体の進行方向を変更する操舵角の計測値を受付ける。記憶部は、左右の前輪と左右の前輪より移動体における後部に設けられた左右の後輪との移動体の前後方向の距離を記憶している。また、記憶部は、左右の前輪の左右方向の間隔、および操舵角と操舵角による移動体の回転半径とに応じた定数を記憶している。軌跡推定部は、左右の前輪の車輪速の計測値、操舵角の計測値、前後方向の距離、左右方向の間隔、および定数に基づき、以下のように推定を行う。すなわち、軌跡推定部は、左右の後輪の回転中心を通る直線上の点を中心とする円上における左右の後輪の回転中心の中点の旋回量と、中点の並進量とを推定する。

１つの実施形態によれば、移動体の移動軌跡を精度よく推定することが可能になる。

第１の実施の形態による軌跡推定装置の機能的な構成の一例を示す図である。 第１の実施の形態による軌跡推定装置のハードウエア構成の一例を示す図である。 第１の実施の形態によるパラメータテーブルの一例を示す図である。 第１の実施の形態による幾何学的モデルにおける円運動について説明する図である。 第１の実施の形態による操舵機構モデルの一例を示す図である。 第１の実施の形態による前輪方向角と曲率との関係を示す図である。 第１の実施の形態による前輪方向角と回転半径との関係を示す図である。 第１の実施の形態による車両の移動方向と旋回量との関係を説明する図である。 第１の実施の形態による軌跡推定装置の動作を示すフローチャートである。 第２の実施の形態による軌跡推定装置の機能的な構成の一例を示すブロック図である。 第２の実施の形態による閾値テーブルの一例を示す図である。 第２の実施の形態による積算計測値テーブルの一例を示す図である。 第２の実施の形態による前時刻軌跡テーブルの一例を示す図である。 誤差蓄積の影響を説明する図である。 第２の実施の形態による軌跡推定の手法を説明する図である。 第２の実施の形態による軌跡推定の手法を説明する図である。 第２の実施の形態による基準時選択による積算誤差の低減を説明する図である。 第２の実施の形態による軌跡推定装置の主要な処理を示すフローチャートである。 第２の実施の形態による軌跡推定装置の車輪速積算処理を示すフローチャートである。 第２の実施の形態による動き判定処理を示すフローチャートである。 第２の実施の形態による操舵角算出処理を示すフローチャートである。 第３の実施の形態による軌跡推定装置の機能的な構成の一例を示すブロック図である。 第３の実施の形態による閾値テーブルの一例を示す図である。 第３の実施の形態による軌跡推定の手法を説明する図である。 第３の実施の形態による軌跡推定の手法を説明する図である。 第３の実施の形態による軌跡推定装置の主要な処理を示すフローチャートである。 第４の実施の形態による軌跡推定装置の構成の一例を示す図である。 標準的なコンピュータのハードウエア構成の一例を示す図である。

（第１の実施の形態）
以下、図面を参照しながら、第１の実施の形態による軌跡推定装置１について説明する。図１は、第１の実施の形態による軌跡推定装置１の機能的な構成の一例を示す図である。図２は、第１の実施の形態による軌跡推定装置１のハードウエア構成の一例を示す図である。軌跡推定装置１は、左右の前輪、および左右の後輪を有する移動体の移動軌跡を推定する装置である。軌跡推定装置１は、左右の前輪夫々の車輪速の計測値と、前輪の操舵角の計測値とに基づき、移動体の軌跡を推定する。ここで、前輪とは、移動体の前部に設けられた車輪をいう。また、後輪とは、移動体の前輪より後部に設けられた車輪をいう。左右とは、移動体の前方向に対する左右をいう。操舵角とは、移動体の前後方向に対する前輪の向きを変えるためのハンドルの切り角である。

軌跡推定装置１は、左右の前輪の車輪速、操舵角、前輪と後輪との前後方向の距離、前輪の左右方向の間隔、および操舵角と操舵角による移動体の回転半径とに応じた定数に基づき、軌跡を推定する。このとき、軌跡推定装置１は、左右の後輪の回転中心を通る直線上の点を中心とする円上における左右の後輪の回転中心の中点の旋回量と、中点の並進量とを推定する。このようにして、移動体の軌跡が推定される。

なお、移動体として、２つの前輪および２つの後輪を有する車両を例にして説明する。また、車両において、左右の前輪の夫々の車輪速（以下、夫々左車輪速、右車輪速ともいう）、および操舵角が計測されることとする。軌跡推定装置１は、例えば、マイクロコンピュータなど、小型の情報処理装置などとすることができ、車両に搭載されるようにしてもよい。

図２に示すように、軌跡推定装置１は、Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ（ＣＰＵ）２１、メモリ２３、車輪速取得Ｉｎｔｅｒｆａｃｅ（Ｉ／Ｆ）２５、操舵角取得Ｉ／Ｆ２７、出力Ｉ／Ｆ２９を有し、互いにバス３１で接続されている。

ＣＰＵ２１は、軌跡推定装置１の動作を制御する演算処理装置である。ＣＰＵ２１は、例えばメモリ２３に予め記憶された制御プログラムを読み込んで実行することにより、軌跡推定装置１としての制御処理を行う。メモリ２３は、例えば、読出し専用記憶装置、随時書き込み及び読み出し可能な記憶装置などである。車輪速取得Ｉ／Ｆ２５は、車輪速センサ３３から左右前輪の夫々の車輪速を受付ける際の管理を行うインタフェース装置である。操舵角取得Ｉ／Ｆ２７は、操舵角センサ３５から、車両の前輪の方向を変化させるハンドルの切り角を受付ける際の管理を行うインタフェース装置である。出力Ｉ／Ｆ２９は、軌跡の推定結果を出力する際の管理を行うインタフェース装置である。

なお、車輪速センサ３３は、車両に備えられ、左右前輪の夫々の車輪速の計測値を出力する装置である。車輪速とは、例えば車輪の単位時間当たりの回転移動距離である。操舵角センサ３５は、車両に備えられ、車両の前輪の方向を変化させるハンドルの操舵角の計測値を出力する装置である。

図１に示すように、軌跡推定装置１は、演算処理部３および記憶部５を有している。演算処理部３は、車輪速受付部１１、操舵角受付部１３、軌跡推定部１５の機能を有している。記憶部５は、パラメータＤａｔａ Ｂａｃｅ（ＤＢ）１９を格納している。

演算処理部３の前述の各機能は、例えば、ＣＰＵ２１が、制御プログラムを読み込んで実行することにより実現される。車輪速受付部１１は、車輪速センサ３３からの左車輪速、右車輪速の計測値を受付ける。操舵角受付部１３は、操舵角センサ３５からの操舵角の計測値を受付ける。

軌跡推定部１５は、左車輪速、右車輪速、操舵角の計測値に基づき、車両の軌跡を推定する。このとき、軌跡推定部１５は、パラメータＤＢ１９に格納されたパラメータを用いて推定を行う。

図３は、パラメータテーブル１１０の一例を示す図である。パラメータテーブル１１０は、車輪左右間隔の二分の一を示す、車輪左右間隔値Ｔ、車輪前後間隔Ｌ（以下、ホイールベースＬという）、定数μを有している。車輪左右間隔値Ｔは、車両の前輪同士の間隔の二分の一であり、例えば、メートル（ｍ）を単位とする実数である。ホイールベースＬは、前輪と後輪との車両の前後方向の距離であり、例えば、メートル（ｍ）を単位とする実数である。定数μは、車両の設計値により予め決められた定数であり、例えば、ラジアンを単位とする実数である。定数μは、例えば、予め実際のハンドル切り角と、その切り角での車両の回転半径との関係から求めることができる。あるいは、後述する操舵機構モデルにおける設定値から求めることができる。

以下、図４から図８を参照しながら、軌跡推定装置１において用いられる、軌跡推定の幾何学的モデルについて説明する。図４は、幾何学的モデルにおける円運動について説明する図である。図４に示すように、軌跡推定装置１は、車両５０の移動の軌跡を、回転中心Ｍに対する円運動として推定する。車両５０は、左前輪５２、右前輪５４、左後輪５６、右後輪５８を備えている。図４において、ｘ軸とｙ軸とは互いに直交する軸である。ｘ軸は、左後輪５６の回転中心と右後輪５８の回転中心とを通る軸とすることができる。ｙ軸は、左後輪５６の回転中心と右後輪５８の回転中心との中点Ｏ１を通り、車両５０の前後方向に平行な軸である。ここで、ｘ軸とｙ軸との交点を原点Ｐ０という。

以下、左後輪５６の回転中心と右後輪５８の回転中心とを繋ぐ軸を後軸という。車両５０における前輪と後輪との前後方向の距離は、ホイールベースＬである。車輪左右間隔値Ｔは、左前輪５２と右前輪５４との間隔の２分の１である。本幾何学的モデルでは、車両５０の軌跡の推定は、中点Ｏ１の軌跡として推定される。よって、中点Ｏ１は、以下、推定点Ｏ１ということとする。

図４の例では、車両５０の推定点Ｏ１が、ｘｙ軸の交点（原点Ｐ０）から、回転中心Ｍに対して旋回角ωとなる位置Ｐ１まで回転移動を行った状態を示している。回転半径ＲＲＣは、推定点Ｏ１の円運動の半径である。推定点Ｏ１が図４のｘｙ座標系の原点Ｐ０にある状態を移動前、位置Ｐ１にある状態を移動後という。左前輪速ＶＦＬは、例えば単位時間の移動前から移動後までの左前輪５２の移動軌跡として示される。右前輪速ＶＦＲは、例えば単位時間の移動前から移動後までの右前輪５４の移動軌跡として示される。車速ＶＲＣは、移動前から移動後までの推定点Ｏ１の移動軌跡として示される。並進量ＴＲＣは、移動前から移動後までの推定点Ｏ１の並進量である。進行角βは、並進量ＴＲＣの方向がｙ軸となす角である。なお、移動前から移動後まで、左前輪５２、右前輪５４の車両５０の前後方向となす角は一定であるとする。

＜推定点Ｏ１での車速ＶＲＣの推定＞
図４において、幾何学的関係から、左前輪速ＶＦＬ、右前輪速ＶＦＲは、式１で表される。
ＶＦＬ＝ＲＦＬ×ω＝ｓｑｒｔ（（ＲＲＣ＋Ｔ）＾２＋Ｌ＾２）×ω
ＶＦＲ＝ＲＦＲ×ω＝ｓｑｒｔ（（ＲＲＣ‐Ｔ）＾２＋Ｌ＾２）×ω
・・・（式１）
なお、「ｓｑｒｔ（Ｆ）」は、Ｆの平方根を表す。このとき、左前輪５２および右前輪５４の各車輪速の二乗平均平方根を、二乗平均車輪速ＶＲＭＳとすると、下記式２で表される。
ＶＲＭＳ＝ｓｑｒｔ（（ＶＦＬ＾２＋ＶＦＲ＾２）／２）
＝ｓｑｒｔ（（（（ＲＲＣ＋Ｔ）＾２＋Ｌ＾２）×ω＾２
＋（（ＲＲＣ‐Ｔ）＾２＋Ｌ＾２）×ω＾２）／２）
＝ω×ｓｑｒｔ（ＲＲＣ＾２＋Ｌ＾２＋Ｔ＾２）
・・・（式２）

式２を、旋回角ωについて解くと、下記式３のようになる。
ω＝ＶＲＭＳ／ｓｑｒｔ（ＲＲＣ＾２＋Ｌ＾２＋Ｔ＾２）・・・（式３）
ただし、二乗平均車輪速ＶＲＭＳは常に正の値をとるので、符号を表すパラメータとして、下記式４のパラメータｋを導入する。
ｋ＝１ ：（（ＶＦＬ＋ＶＦＲ）/２≧０）
ｋ＝−１：（（ＶＦＬ＋ＶＦＲ）/２＜０）
・・・（式４）

式３および式４より、旋回角ωを下記式５で再定義する。
ω＝ｋ×ＶＲＭＳ／ｓｑｒｔ（ＲＲＣ＾２＋Ｌ＾２＋Ｔ＾２）・・・（式５）
よって、後輪軸中央の推定点Ｏ１での車速ＶＲＣは、下記式６で表される。
ＶＲＣ＝ＲＲＣ×ω
＝ＲＲＣ×ｋ×ＶＲＭＳ／ｓｑｒｔ（ＲＲＣ＾２＋Ｌ＾２＋Ｔ＾２）
・・・（式６）

一般に、回転半径ＲＲＣでの記述は直進時に回転半径が無限大となり計算しづらいため、回転半径の逆数である曲率ｃを導入する。ここで、ｃは、下記式７で定義する。
ｃ＝１／ＲＲＣ （ＲＲＣ＝１／ｃ）・・・（式７）

式６と式７とにより、下記式８が得られる。
ＶＲＣ＝（１／ｃ）×ｋ×ＶＲＭＳ／ｓｑｒｔ（（１／ｃ）＾２＋Ｌ＾２＋Ｔ＾２）
＝ｋ×ＶＲＭＳ／ｓｑｒｔ（（Ｌ＾２＋Ｔ＾２）×ｃ＾２＋１）
＝ｋ×ｓｑｒｔ（（ＶＦＬ＾２＋ＶＦＲ＾２）
／（２×（（Ｌ＾２＋Ｔ＾２）×ｃ＾２＋１）））・・・（式８）
上記式８により、左前輪速ＶＦＬおよび右前輪速ＶＦＲから、推定点Ｏ１における幾何学的に正しい車速ＶＲＣが推定される。

＜前輪方向角γと回転半径ＲＲＣおよび曲率ｃの推定＞
次に、図５および図６を参照しながら、前輪方向角γと曲率ｃとの関係について説明する。図５は、操舵機構モデルの一例を示す図である。図６は、前輪方向角γと曲率ｃとの関係を示す図である。

図５に示すように、本実施の形態による操舵機構モデルにおいては、第一次近似として、平行リンク機構としてモデル化する。ここで、前輪方向角γとは、車両５０の前後方向と左前輪５２または右前輪５４とのなす角度である。

図５に示すように、実際の車両の操舵機構を第一次近似として平行リンク機構としてモデル化する。実際の車両では、例えば、ステアリングホイールから伸びたステアリング軸でピニオンギアが回され、これをラックギアで受けてタイロッドを左右に変位させる。このタイロッドが、ナックルアームを介して車両ハブに接続されているものとする。本実施の形態による操舵機構モデルでは、ナックルアーム６６、６８とラック（タイロッド６４）が９０度に位置する平行リンクとして近似することとする。

図５において、本操舵機構モデルでは、左前輪５２と右前輪５４とは互いに平行であり、車両５０の前後方向となす角が互いに同一になっているものとする。ステアリング軸６０は、不図示のハンドルから伸びている軸であり、ハンドルの操作に応じて、例えば操舵角φ度回転する。ピニオンギア６２は、ステアリング軸６０の回転をラックアンドピニオン機構により直線変位に変換する。ナックルアーム６６の一端は、左前輪５２に接続され、他端は、タイロッド６４に直角に接続されている。ナックルアーム６８の一端は、右前輪５４に接続され、他端は、タイロッド６４に直角に接続されている。ナックルアーム６６、ナックルアーム６８の長さは、夫々ナックルアーム長Ａである。

変位量Ｕは、タイロッド６４の左右方向の変位であり、ハンドル操作によりステアリング軸６０が操舵角φ回転した際に、ピニオンギア６２により変換されることにより生ずる変位量である。前輪方向角γは、タイロッド６４の変位が変位量Ｕである際に、ナックルアーム６６、ナックルアーム６８を介して変化する左前輪５２および右前輪５４の角度である。

図５に示した操舵機構モデルにおける幾何学的な関係より、下記式９が成り立つ。
ｔａｎ（γ）＝Ｕ／ｓｑｒｔ（Ａ＾２−Ｕ＾２）・・・（式９）
ここで、変位量Ｕは操舵角φに比例した量となり、比例係数をｐとして、式１０で表される。
Ｕ＝ｐ×φ・・・（式１０）
また、前輪方向角γと回転半径ＲＲＣとの幾何学的な関係より、下記式１１が成り立つ。
ＲＲＣ＝Ｌ／ｔａｎ（γ）・・・（式１１）
ここで先に習い曲率ｃを導入すると、式１２が成り立つ。
ｃ＝１／ＲＲＣ
＝ｔａｎ（γ）／Ｌ
＝Ｕ／（Ｌ×ｓｑｒｔ（Ａ＾２−Ｕ＾２））
＝（ｐ×φ）／（Ｌ×ｓｑｒｔ（Ａ＾２−（ｐ×φ）＾２））
＝φ／（Ｌ×ｓｑｒｔ（μ＾２−φ＾２））
・・・（式１２）
ただし、定数μは、下記式１３で表される。
μ＝Ａ／ｐ・・・（式１３）

図６は、図５の操舵機構モデルにおける、操舵角φと曲率ｃとの関係を示す図である。すなわち、図６は、式１２の関係を示している。図６に示すように、式１２では、操舵角φに対して曲率ｃを単なる比例関係とした場合とは異なる関係となる。

図７は、前輪方向角γと、回転半径ＲＲＣとの関係を示す図である。図７において、説明の都合上、前輪８０と後輪８２とが示されている。前輪８０は、左前輪５２と右前輪５４との中点に配置された車輪である。後輪８２は、推定点Ｏ１に配置された車輪である。方向８９は、車両５０の進行方向を示す。方向８９は、車両５０の前後方向を示す軸８６と前輪方向角γの角度をなしている。

このとき、後輪軌跡９０は、車両５０が回転中心Ｍに対する円運動をする際の、後輪８２の回転運動の軌跡である。前輪軌跡９２は、同様の場合の、前輪８０の軌跡を示す。このように、前輪８０と後輪８２とは、異なる半径の円運動をしている。このため、式１２は、前輪の中点の軌跡を示す場合には適用できない。すなわち、上記式１２で表される曲率ｃは、左前輪５２と右前輪５４との車輪速を用いて表された、推定点Ｏ１の軌跡を示す式であることが分かる。これによって、上記式１２は、推定点Ｏ１における、幾何学的に矛盾が少なく精度の高い、操舵角φと曲率ｃとの関係となる。

＜低速移動時の旋回モデルでの移動方向の定式化＞
次に、図８を参照しながら、車両５０の移動方向と旋回量との関係を説明する。図８は、車両５０の移動方向と旋回量との関係を説明する図である。図８において、推定点Ｏ１は、ｘ軸とｙ軸とで表される座標系の原点Ｐ０（移動開始時刻ｔ１）から、位置Ｐ１（移動後の時刻ｔ２）まで移動することが示されている。このとき、回転中心Ｍは、ｘ軸上にある。また、移動前後の車両５０の旋回角は旋回角ωで表される。軸１０５、軸１０７は、移動後の位置Ｐ１を原点とする直交座標系を示す。また、車両５０の走行速度は十分に低速の場合を考える。したがって、駆動輪と操舵輪はすべりが無く、転がりによって走行し、その間の移動は円運動と近似することができるとする。このとき、上述のように、車両５０の円運動は、推定点Ｏ１の位置の移動として表される。

推定点Ｏ１が、車両５０の円運動の開始点である原点Ｐ０にあるとき、図８に示した幾何学的な関係より、下記式が成り立つ。

距離（Ｐ０〜Ｍ）＝距離（Ｐ１〜Ｍ）・・・（式１４）
すなわち、三角形Ｐ０−Ｐ１−Ｍは、移動体の旋回量ωを頂角とする二等辺三角形である。よって、角Ｐ０−Ｐ１−Ｍ＝角Ｐ１−Ｐ０−Ｍ＝ξとなり、下記式１５が成り立つ。
ξ＝（１８０度−ω）／２＝９０度−ω／２・・・（式１５）

式１５より、車両５０の進行方向とｙ軸とのなす角である進行角βは、下記式１６で表される。
β＝９０度−ξ・・・（式１６）
＝９０度−（９０度−ω／２）
＝ω／２・・・（式１６）

すなわち、移動体の旋回量ωが分かるとき、移動体の物理的な制約によって、時刻ｔ１での移動体の進行方向βは決定できる。また、原点Ｐ０と原点Ｐ１を結ぶ円弧は時刻ｔ１から時刻ｔ２での移動軌跡で、その道のり距離は、推定点Ｏ１の車速ＶＲＣとすることができる。ここで、原点Ｐ０から原点Ｐ１への弦を並進量ＴＲＣとすると、並進量ＴＲＣは推定点Ｏ１の並進移動成分となる。図８における幾何学的な関係より、下記式１７が成り立つ。
ＴＲＣ＝（２×ＶＲＣ×ｓｉｎ（ω／２））／ω・・・（式１７）

よって、以下のように幾何学的に正しい並進移動量を推定することができる。並進移動量のｘ成分を移動量ｔｘ、ｙ成分を移動量ｔｙとすると、下記式１８のようになる。
ｔｘ＝ＴＲＣ×ｓｉｎ（β）＝ＴＲＣ×ｓｉｎ（ω／２）
ｔｙ＝ＴＲＣ×ｃｏｓ（β）＝ＴＲＣ×ｃｏｓ（ω／２）
・・・（式１８）

以下、図９を参照しながら、軌跡推定装置１の軌跡推定処理について説明する。図９は、軌跡推定装置１の動作の一例を示すフローチャートである。図９に示すように、軌跡推定装置１において、車輪速受付部１１は、左右の前輪夫々の車輪速の計測値を受付ける。操舵角受付部１３は、前輪の操舵角の計測値を受付ける（Ｓ１２１）。軌跡推定部１５は、左右前輪夫々の車輪速の計測値と、前輪の操舵角の計測値とを用いて、上述の式１〜式１８に示したように、移動体の軌跡を推定する。このとき、軌跡推定装置１は、左右の前輪の車輪速、操舵角、前輪と後輪との前後方向の距離、前輪の左右方向の間隔、および操舵角と操舵角による移動体の回転半径とに応じた定数に基づき、軌跡を推定する（Ｓ１２２）。

このように、軌跡推定装置１は、左右の後輪の回転中心の中点の移動を、円運動として推定する。この円運動は、２つの後輪の回転中心を通る直線上の点を中心とする円運動である。軌跡は、移動体の旋回量と並進量として推定される。軌跡推定装置１は、システムの停止が検出されるまでＳ１２１〜Ｓ１２２の処理を繰り返し（Ｓ１２３：ＮＯ）、検出されると（Ｓ１２３：ＹＥＳ）、処理を終了する。

以上のように、本実施の形態による軌跡推定装置１によれば、運動が円運動で記述できる短時間での移動について、車両５０の軌跡を、旋回量ωと並進移動量（ｔｘ、ｔｙ）により精度良く推定できる。

このとき、軌跡推定装置１は、前輪の車輪速と、操舵角との測定値を用いる。また、軌跡推定装置１は、予め決められた車輪左右間隔値Ｔ、ホイールベースＬ、定数μを用いる。これにより、車両５０が円運動するとしたとき、後輪軸の中点である推定点Ｏ１の移動として軌跡を高精度に推定可能である。このとき、車両５０の並進の方向は、車両５０の向きとは異なる方向として推定される。例えば、式５、式８、式１２、式１７、式１８により、旋回角ω、並進量ＴＲＣ、並進移動量（ｔｘ、ｔｙ）が算出される。

操舵角と車両の回転半径の実測値との比例関係は、常に成立してはいないが、操舵角モデルにより、操舵角と車両の回転半径との関係において実測値に近い値を用いることが可能となり、軌跡推定の精度をさらに向上させることが可能となっている。

上記のような軌跡推定装置１により、前輪を操舵輪とし、左右前後輪の４輪を有する移動体について、左右前輪の車輪速と操舵角を入力として、低速時の移動軌跡を旋回量と並進量で推定することができる。この軌跡推定装置により、例えば、駐車・発車時の自動操縦や、車載カメラ映像を用いた移動ステレオによる周囲物位置の計測による周囲物との接触警報発生などに利用できる。

（第２の実施の形態）
以下、第２の実施の形態による軌跡推定装置１５０について説明する。第２の実施の形態において、軌跡推定装置１と同様の構成および動作については、同一符号を付し、重複説明を省略する。軌跡推定装置１５０のハードウエア構成の一例は、軌跡推定装置１の例と同様である。

図１０は、第２の実施の形態による軌跡推定装置１５０の機能的な構成の一例を示すブロック図である。軌跡推定装置１５０は、所定時間毎の測定時に測定された、車輪速及び操舵角の測定値を受付ける。軌跡推定装置１５０は、測定時の中から基準時を設定し、設定された基準時から最新の測定時までの車両５０の軌跡を推定し、推定された軌跡に基づき、１つ前の測定時から最新の測定時までの軌跡を推定する。また、軌跡推定装置１５０は、基準時から最新の測定時までの車両５０の軌跡に基づき、基準時を更新する。基準時とは、車輪速を積算するための開始時および平均操舵角を算出するための開始時であって、軌跡を推定する際の移動前の位置に対応する時刻である。最新の測定時とは、車輪速受付部１１、または操舵角受付部１３から最新に入力された測定値が測定された時刻であるが、便宜上、最新に入力が受付けられた時刻としてもよい。ここで、時刻は絶対的な時刻のみでなく、適宜定められる相対的な時刻とすることができる。

図１０に示すように、第２の実施の形態による軌跡推定装置１５０は、演算処理部１５１、記憶部１６０を有している。演算処理部１５１は、車輪速受付部１１、車輪速積算部１２、操舵角受付部１３、操舵角推定部１４、軌跡推定部１５、基準時更新部１５３、および軌跡算出部１５５を有している。記憶部１６０は、パラメータＤＢ１９、閾値ＤＢ１６２、積算計測値ＤＢ１６４、前時刻軌跡ＤＢ１６６を有している。

車輪速積算部１２は、車輪速受付部１１が受付けた左前輪５２と右前輪５４との夫々の車輪速を基準時から所定時間毎に積算し、積算計測値ＤＢ１６４に記憶させる。操舵角推定部１４は、操舵角受付部１３が受付けた操舵角を、基準時から積算し、算出された積算操舵角、および積算回数を積算計測値ＤＢ１６４に記憶させる。

このとき、車輪速受付部１１は、所定時間毎に計測される左右輪夫々の車輪速を受付ける。車輪速積算部１２は、積算計測値ＤＢ１６４に保存される前計測時までの左右の車輪速積算値を読み出し、最新に車輪速受付部１１から入力された車輪速を加算し、その加算値を積算計測値ＤＢ１６４に書き戻す。また、車輪速積算部１２は、軌跡推定部１５に車輪速の積算値を出力する。さらに、車輪速積算部１２は、最新の計測時での車輪速の大きさから車両５０が停止しているか否かを判定し、動きの有無を操舵角推定部１４に出力する。

操舵角受付部１３は、所定時間毎に計測される操舵角の測定値を受付ける。操舵角推定部１４は、車両５０が停止していないと判定されている場合には、積算計測値ＤＢ１６４に保存される前計測時までの操舵角積算値を読み出し、操舵角受付部１３から最新に入力された操舵角を加算する。操舵角推定部１４は、車両５０が停止していないと判定されている場合には、積算計測値ＤＢ１６４から積算回数を読み出して１を加算し、操舵角積算値と積算回数とを積算計測値ＤＢ１６４に書き戻す。

軌跡推定部１５は、積算計測値ＤＢ１６４に記憶された車輪速の積算値、および積算操舵角と積算回数とに応じた平均操舵角に基づき、車両５０の軌跡を推定する。軌跡推定部１５の処理は、第１の実施の形態における軌跡推定装置１での車輪速、操舵角に代えて、車輪速の積算値、操舵角の平均値を用いる処理となる。

基準時更新部１５３は、軌跡推定部１５の推定結果に基づき、基準時を更新すると判別された場合には、基準時を更新する。基準時の更新に関する判別の詳細については後述する。

軌跡算出部１５５は、軌跡推定部１５が推定した軌跡、および前時刻軌跡ＤＢ１６６に記憶された軌跡に基づき、車両５０の軌跡を算出する。算出した旋回角ω、並進量を、前時刻軌跡ＤＢ１６６に記憶させる。

閾値ＤＢ１６２は、軌跡推定部１５の処理、基準時更新部１５３の処理、軌跡算出部１５５の処理に用いるための閾値を記憶したデータベースである。図１１は、閾値テーブル１８２の一例を示す図である。閾値テーブル１８２は、閾値ＤＢ１６２のデータ構造の一例である。図１１に示すように、閾値テーブル１８２は、動き判定閾値ＴＨ１、動き回数閾値ＴＨ２、旋回判定閾値ＴＨω、基準時更新閾値ＴＨＫを有している。

動き判定閾値ＴＨ１は、動き有無を判定するための車輪速に対する閾値で、例えばキロメートル毎時を単位とする実数値である。動き回数閾値ＴＨ２は、動きの有無を判定する際の、後述する停止フラグＳＴＯＰ＿Ｆのチャタリング防止のためのヒステリシス用の閾値で、停止と判断された回数と対応する整数値である。旋回判定閾値ＴＨωは、旋回の有無を判定するための閾値であり、例えば「度」を単位とする実数値である。基準時更新閾値ＴＨＫは、基準時を更新するか否かを判別するための閾値であり、例えばセンチメートルを単位とする実数値である。

図１２は、積算計測値テーブル１８４の一例を示す図である。積算計測値テーブル１８４は、積算計測値ＤＢ１６４のデータ構造の一例である。図１２に示すように、積算計測値テーブル１８４は、積算左車輪速ＣＵＭＶＬ、積算右車輪速ＣＵＭＶＲ、停止カウンタＳＣＯＵＮＴ、停止フラグＳＴＯＰ＿Ｆ、積算操舵角量ＣＵＭφ、積算回数Ｎφを有している。積算左車輪速ＣＵＭＶＬは、所定時間毎に計測された左車輪速を、基準時から積算した値であり、例えばメートル毎秒を単位とする実数値である。積算右車輪速ＣＵＭＶＲは、所定時間毎に計測された右車輪速を基準時から積算した値であり、例えばメートル毎秒を単位とする実数値である。停止カウンタＳＣＯＵＮＴは、動き判定を行う際に用いるカウント数であり、整数値である。停止フラグＳＴＯＰ＿Ｆは、停止しているか否かを示す判別値であり、例えば、停止している場合に「１」、停止していない場合に「０」等とすることができる。積算操舵角量ＣＵＭφは、所定時間毎に計測される操舵角φを積算した値であり、例えば「度」を単位とする実数値である。積算回数Ｎφは、操舵角φの積算回数であり、整数値である。

図１３は、前時刻軌跡テーブル１８６の一例を示す図である。前時刻軌跡テーブル１８６は、前時刻軌跡ＤＢ１６６のデータ構造の一例である。図１３に示すように、前時刻軌跡テーブル１８６は、基準時から最新の計測時の一つ前の計測時までの前時刻旋回量αｐ、前時刻ｘ並進量ｑｘｐ、前時刻ｙ並進量ｑｙｐを有している。前時刻旋回量αｐは、例えば、角度を単位とする実数値である。前時刻ｘ並進量ｑｘｐ、前時刻ｙ並進量ｑｙｐは、例えばメートルを単位とする実数値である。

図１４は、誤差蓄積の影響を説明する図である。図１４では、路面１７０において、車両５０が移動していることを示している。軌跡１７２は、車両５０の実際の移動を示している。推定軌跡１７４は、例えば、計測時毎に基準時を更新したとして、軌跡推定装置１５０により推定される所定時間毎の軌跡を連続して示している。図１４の例では、計測回を重ねる毎に、実際の軌跡１７２と推定軌跡１７４との誤差が広がり、例えば誤差１７６を持つに至ることを示している。これは、車輪速、操舵角等を、所定時間毎にティジタル値として計測していること、および、車両５０の軌跡を円運動で近似していることに起因する誤差と考えられる。このように、積算を行うことによる誤差をなくすため、本実施の形態においては、基準時を設定し、基準時からある程度の距離までの軌跡を推定するようにしている。

図１５、図１６は、本実施の形態の軌跡推定の手法を説明する図である。図１５に示すように、計測時推定位置２００−１、２００−２、・・・（まとめて計測時推定位置２００ともいう）は、所定時間毎の車輪速の計測時における車両５０の推定位置を示している。軌跡２０４は、計測時推定位置２００をつないだ曲線であり、車両５０の推定された移動軌跡である。このとき、基準時推定位置２０２−ｋ１、２０２−ｋ２、・・・は、基準時における車両５０の推定位置を示している。基準時更新閾値ＴＨＫ１は、基準時を更新するための閾値となる距離である。本実施の形態においては、基準時からの推定並進距離が、基準時更新閾値ＴＨＫ１以上になった場合、最新の計測時を新たな基準時とする。また、各計測時の推定位置は、最新の基準時から最新の計測時までの積算車輪速、平均操舵角に基づき推定される。各計測時に対応して出力される推定位置の算出方法は後述する。

図１６に示すように、まず、計測時推定位置２０８−１が基準時推定位置２０９−ｋ１となっている。すなわち、基準時は、計測時推定位置２０８−１と対応する計測時となっている。次に、軌跡推定部１５は、基準時から、計測時推定位置２０８−２に対応する計測時までの積算車輪速および平均操舵角に基づき、車両５０の計測時推定位置２０８−１〜計測時推定位置２０８―２の間の軌跡を推定する。並進量２１０−１は、計測時推定位置２０８−１〜計測時推定位置２０８―２の間の推定された並進量である。並進量２１０−１が、基準時更新閾値ＴＨＫ１に満たない場合には、次の計測時に、基準時からの積算車輪速及び平均操舵角に基づき、車両５０の軌跡が推定され、計測時推定位置２０８−３が求められる。図１６例では、計測時推定位置２０８−５において推定された並進量２１０−４が、基準時更新閾値ＴＨＫ１以上になったことが示されている。このとき、基準時推定位置２０９−ｋ２に対応する計測時が、更新された基準時となる。

図１７は、本実施の形態による基準時選択による積算誤差の低減を説明する図である。図１７に示すように、例えば、車輪速、操舵角の入力は、計測時推定位置２００−１、２００−２、・・・に対応する時刻において行われる。基準時は、直前の基準時からの推定並進量が基準時更新閾値ＴＨＫ１以上になった場合に更新される。軌跡の推定は、基準時から計測時までの車輪速、操舵角の測定結果に基づき行う。出力する推定結果は、基準時から前計測時までの推定結果と、基準時から最新の計測時までの推定結果との差分として算出される。

以下、図１８〜図２１を参照しながら、第２の実施の形態による軌跡推定装置１５０の動作について説明する。図１８は、軌跡推定装置１５０の主要な処理を示すフローチャートである。図１８に示すように、軌跡推定装置１５０において、車輪速積算部１２は、車輪速受付部１１で受付けられた車輪速の計測値を積算し、積算計測値ＤＢ１６４に格納する。操舵角推定部１４は、車輪速積算部１２で車両５０が動いていると判定された場合に、操舵角受付部１３で受付けた操舵角の計測値を積算して、平均操舵角を算出すると共に、積算値を積算計測値ＤＢ１６４に格納する（Ｓ２２１）。Ｓ２２１の処理の詳細については、後述する。

軌跡推定部１５は、車輪速積算部１２で算出された積算車輪速、および操舵角推定部１４で算出された平均操舵角に基づき、第１の実施の形態と同様に旋回量、並進量を推定する（Ｓ２２２）。このＳ２２２の処理の詳細については、後述する。

基準時更新部１５３は、推定された並進量が、基準時更新閾値ＴＨＫ１以上である場合に（Ｓ２２３：ＹＥＳ）、基準時を更新する（Ｓ２２４）。基準時更新部１５３は、推定された並進量が、基準時更新閾値ＴＨＫ１未満である場合には（Ｓ２２３：ＮＯ）、Ｓ２２５に処理を進める。

軌跡算出部１５５は、前時刻軌跡テーブル１８６に格納された情報と、推定された旋回角ω、並進量ｄｘ、ｄｙに基づき、前時刻からの旋回量および並進量を算出する（Ｓ２２５）。演算処理部１５１は、システム終了のための操作等があったか否かを判別し、ある場合には（Ｓ２２６：ＮＯ）、Ｓ２２１から処理を繰り返す。ない場合には（Ｓ２２６：ＹＥＳ）、演算処理部１５１は、軌跡推定処理を終了する。

図１９は、軌跡推定装置１５０の車輪速積算処理を示すフローチャートである。図１９の処理は、図１８におけるＳ２２１の詳細を示す処理である。図１９に示すように、車輪速積算部１２は、車輪速受付部１１が受付けた計測時刻ｔにおける左前輪速ＶＦＬ（ｔ）、右前輪速ＶＦＲ（ｔ）、積算左車輪速ＣＵＭＶＬ、積算右車輪速ＣＵＬＶＲについて、以下の演算を行う（Ｓ２３１）。
ＣＵＭＶＬ＝ＣＵＭＶＬ＋ＶＦＬ（ｔ）
ＣＵＭＶＲ＝ＣＵＭＶＲ＋ＶＦＲ（ｔ）
・・・（式１９）

また、車輪速積算部１２は、算出した積算左車輪速ＣＵＭＶＬ、積算右車輪速ＣＵＬＶＲを、積算計測値テーブル１８４に書き戻すことにより積算計測値テーブル１８４を更新すると共に、軌跡推定部１５に出力する。車輪速積算部１２は、処理を、図２０の動き判定に進める。

図２０は、動き判定処理を示すフローチャートである。ここで、車輪速積算部１２は、停止カウンタＳＣＯＵＮＴ、停止フラグＳＴＯＰ＿Ｆ共に初期値を「０」としておく。車輪速積算部１２は、最新の計測時の左右の車輪速ＶＦＬ（ｔ）、ＶＦＲ（ｔ）のうちの最大の値が、動き判定閾値ＴＨ１未満の場合には（Ｓ２４１：ＹＥＳ）、停止カウンタＳＣＯＵＮＴに「１」を加え、積算計測値テーブル１８４を更新する（Ｓ２４２）。車輪速積算部１２は、停止カウンタＳＣＯＵＮＴが動き回数閾値ＴＨ２より大きい場合には（Ｓ２４３：ＹＥＳ）、停止フラグＳＴＯＰ＿Ｆ＝１とし（Ｓ２４４）、操舵角積算処理へ処理を進める。車輪速積算部１２は、停止カウンタＳＣＯＵＮＴが動き回数閾値ＴＨ２以下の場合には（Ｓ２４３：ＮＯ）、操舵角積算処理へ処理を進める。

車輪速積算部１２は、Ｓ２４１で、最新の計測時の左右の車輪速ＶＦＬ（ｔ）、ＶＦＲ（ｔ）のうちの最大の値が、動き判定閾値ＴＨ１以上の場合には（Ｓ２４１：ＮＯ）、Ｓ２４５へ処理を進める。Ｓ２４５では、車輪速積算部１２は、停止カウンタＳＣＯＵＮＴ＝０、停止フラグＳＴＯＰ＿Ｆ＝０として積算計測値テーブル１８４を更新し（Ｓ２４５）、操舵角積算処理へ処理を進める。このように動き判定では、停止フラグＳＴＯＰ＿Ｆが判定結果として、積算計測値テーブル１８４に出力される。

図２１は、操舵角算出処理を示すフローチャートである。ここで、積算操舵角量ＣＵＭφの初期値、および積算回数Ｎφの初期値は「０」である。操舵角推定部１４は、車両５０に取り付けられたステアリングの操舵角センサ３５から、例えば所定時間毎に操舵角受付部１３が受付ける操舵角に基づき、操舵角を推定する。操舵角推定部１４は、積算計測値テーブル１８４から停止フラグＳＴＯＰ＿Ｆを読み出し、「１」である場合には（Ｓ２５１：ＹＥＳ）、図１８の処理に処理を戻す。

停止フラグが「０」である場合には（Ｓ２５１：ＮＯ）、操舵角推定部１４は、積算操舵角量ＣＵＭφに、計測時刻ｔでの操舵角φ（ｔ）を加算するとともに、積算回数Ｎφに「１」を加算し、積算計測値テーブル１８４を更新する（Ｓ２５２）。すなわち、操舵角推定部１４は、車両５０が動いているときのみ、積算計測値テーブル１８４に保存される積算操舵角量ＣＵＭφを読み出し、入力された操舵角φ（ｔ）を加算することにより、車両５０が動いている間の平均操舵角を求める。

操舵角推定部１４は、平均操舵角φａ＝ＣＵＭφ／Ｎφとする（Ｓ２５３）。なお、Ｎφ＝０の場合に、φａ＝φ（ｔ）としてもよい。このようにして、操舵角推定部１４は、車両５０が動いているときのみ操舵角を積算して平均し、平均操舵角φａを算出し、軌跡推定部１５に出力する。

ここで、軌跡推定部１５によるＳ２２２の処理の詳細について説明する。現在の基準時から最新の計測時までの積算車輪速、平均操舵角を、左積算車輪速ＣＵＭＶＬ、右積算車輪速ＣＵＭＶＲ、平均操舵角φａとする。このとき、車両５０の旋回の曲率ｃは、式１２において、操舵角φを平均操舵角φａに置き換えることにより、下記式２０で与えられる。
ｃ＝φａ／（Ｌ×ｓｑｒｔ（μ＾２−φａ＾２））・・・（式２０）

また、積算車輪速の二乗平均ルートをＶＲＭＳとすると、積算左車輪速ＣＵＭＶＬ、積算右車輪速ＣＵＭＶＲを用いて、下記式２１で与えられる。
ＶＲＭＳ＝ｓｑｒｔ（（ＣＵＭＶＬ＾２＋ＣＵＭＶＲ＾２）／２）・・・（式２１）
これを用いて、現在の基準時から最新の計測時までの車両５０の推定点Ｏ１の車速ＶＲＣは、下記式２２で与えられる。
ＶＲＣ＝ｋ×ＶＲＭＳ／ｓｑｒｔ（ｃ＾２×（Ｌ＾２＋Ｔ＾２）＋１）・・・（式２２）
ただし、ｋは移動速度の符号を表すためのパラメータで、下記式２３で定義される。
ｋ＝ １ （（ＣＵＭＶＬ＋ＣＵＭＶＲ）/２＞＝０）
＝−１ （（ＣＵＭＶＬ＋ＣＵＭＶＲ）/２＜ ０）
・・・（式２３）

車両５０の旋回角ωは、下記式２４で算出する。
ω＝ｃ×ＶＲＣ・・・（式２４）
軌跡推定部１５は、式２２、式２４に基づき、現在の基準時から直前の計測時までの車両５０の旋回量αと並進移動量（ｑｘ、ｑｙ）を算出する。具体的には、下記式２５の関係が成り立つ。
α＝ω・・・（式２５）

また、車速ＶＲＣは現在の基準時から最新の計測時までの車両５０の運動を円運動とするときの弧の長さを表す。この弧に対応する弦の長さを並進量ＴＲＣとするとき、式２６が成り立つ。
ＴＲＣ＝（２×ＶＲＣ×ｓｉｎ（ω／２））／ω （｜ω｜≧旋回判定閾値ＴＨω）
＝ＶＲＣ （｜ω｜＜旋回判定閾値ＴＨω）
・・・（式２６）
ただし、旋回判定閾値ＴＨωは、旋回角ωが０に非常に近いか否かを判定するための閾値である。この並進量ＴＲＣを用いて、車両５０の並進移動量（ｑｘ、ｑｙ）は、下記式２７で与えられる。
ｑｘ ＝ＴＲＣ×ｓｉｎ（ω／２）
ｑｙ ＝ＴＲＣ×ｃｏｓ（ω／２）
・・・（式２７）
軌跡推定部１５は、これら旋回量α、並進移動量（ｑｘ、ｑｙ）を軌跡算出部１５５に出力すると共に、並進量ＴＲＣを基準時更新部１５３に出力する。

以下、軌跡算出部１５５の処理についてさらに説明する。軌跡算出部１５５は、基準時から最新の計測時までの積算左車輪速ＣＵＭＶＬ、積算右車輪速ＣＵＭＶＲ、平均操舵角φａに基づき、旋回量α、並進移動量（ｑｘ、ｑｙ）を算出し、軌跡推定装置１５０外部に出力する。

＜１時刻間旋回量・並進量の算出＞
図１３に示すように、前時刻軌跡テーブル１８６には、前時刻旋回量αｐ、前時刻ｘ並進量ｑｘｐ、前時刻ｙ並進量ｑｙｐが保存されている。前時刻とは、最新の計測時の一つ前の計測時である。このとき、前時刻から最新の計測時までの旋回量α１、並進移動量（ｑｘ１、ｑｙ１）は、下記式２８で与えられる。
α１＝α―αｐ
ｑｘ１＝ｃｏｓ（αｐ）×（ｑｘ−ｑｘｐ）−ｓｉｎ（αｐ）×（ｑｙ−ｑｙｐ）
ｑｙ１＝ｓｉｎ（αｐ）×（ｑｘ−ｑｘｐ）＋ｃｏｓ（αｐ）×（ｑｙ−ｑｙｐ）
・・・（式２８）
軌跡算出部１５５は、旋回量α１、並進移動量（ｑｘ１、ｑｙ１）を、軌跡推定装置１５０外部に出力する。また、軌跡算出部１５５は、次の計測時での処理に備え、旋回量α、並進移動量（ｑｘ、ｑｙ）を、前時刻軌跡テーブル１８６における旋回量αｐ、並進移動量（ｑｘｐ、ｑｙｐ）として保存する。

以下、基準時更新部１５３の処理についてさらに説明する。基準時更新部１５３は、軌跡推定部１５で推定された並進量ＴＲＣを入力として、基準時更新閾値ＴＨＫ１との比較の結果に応じて、基準時を更新する。

基準時の更新は、以下のように行われる。すなわち、下記式２９が満たされる場合に、基準時を最新の計測時に更新する。
｜ＴＲＣ｜＞ＴＨＫ１・・・（式２９）
式２９が満たされる場合、基準時からの車両５０の移動量が所定量を超えることになる。

基準時更新部１５３が、基準時を更新するとき、積算計測値ＤＢ１６４において、下記の設定が行われる。
ＣＵＭＶＬ＝０
ＣＵＭＶＲ＝０
ＣＵＭφ＝０
Ｎφ＝０
・・・（式３０）
前時刻軌跡ＤＢ１６６については、基準時更新の際に、前時刻軌跡テーブル１８６において、下記の式３１のように設定が行われる。
αｐ＝０
ｑｘｐ＝０
ｑｙｐ＝０
・・・（式３１）

以上説明したように、第２の実施の形態による軌跡推定装置１５０によれば、短時間での移動でしか成立しない円運動で移動体の軌跡を推定し、短時間で推定した旋回量・並進量を積算することにより、移動体の任意の運動について移動軌跡を推定することができる。軌跡推定装置１５０では、軌跡を推定する基準となる基準時が設けられる。この基準時は、毎計測時に更新されるのではなく、所定の条件を満たした場合に更新される。所定の条件が満たされるまで、基準時から所定時間毎に計測される車輪速および操舵角に基づき、基準時から最新の計測時までの軌跡が算出される。このとき、車輪速は、基準時から最新の計測時までの積算値が用いられる。操舵角は、基準時から最新の計測時までの平均が用いられる。算出された軌跡における車両５０の並進量が基準時更新閾値ＴＨＫ１を超えると、基準時は、最新の計測時に更新される。また、軌跡推定装置１５０は、基準時から最新の計測時までの軌跡と、基準時から、最新の一つ前の計測時までの軌跡との差分に基づく旋回量と並進量とを、軌跡として出力する。

以上のように、軌跡推定装置１５０では、基準時を時系列的に計測時毎に更新するのではなく、一定条件内では固定し、計測時だけを時系列的に更新していく。そして、基準時から最新の計測時までの推定並進量が所定の基準時更新閾値ＴＨＫ１を超えた段階で、旋回量と並進量とを推定すると共に、最新の計測時を新たな基準時として更新する。このように、軌跡推定の並進距離を所定距離より長くする構成を用いることで、旋回角や並進量を積算することによる誤差蓄積が発生することが防止される。これにより、単一の円運動あるいは直進運動として軌跡が記述できる範囲では、基準時を更新せず、運動の種類が変化したタイミングで基準時を更新することができる。よって、単一運動の範囲はその間の長い距離での車輪速の積算値を評価することになるため、積算した車輪速に含まれる誤差を小さくでき、旋回・並進の推定値の精度を向上できる。このように、例えば、計測値を離散的に取得し、軌跡を円運動で近似して算出された軌跡を足し合わせることで生じる誤差を低減することが可能になり、精度よく移動体の軌跡を推定することができる。

なお、旋回量と並進量は各計測時に推定して、１つ前の計測時からの旋回量と並進量に変換して外部に出力する。１つ前の計測時までの軌跡と、最新の計測時までの軌跡との偏差を出力することで、基準時からの推定値に基づき軌跡が推定されるため、積算誤差は含まれない値とすることができる。

これに加えて、車両５０の移動速度が低速の場合は、所定時間での移動量が小さいため、結果として推定される移動量の誤差も大きくなることが懸念されるが、長い距離での推定を行うことにより、軌跡推定装置１５０では、誤差を低下させることができる。よって、軌跡推定装置１５０による軌跡推定方法は、車両５０の速度が低速でも利用可能である。また、車両５０に時系列で運動変化を伴う移動があっても、旋回量と並進量とを精度良く推定できる。

（第３の実施の形態）
以下、第３の実施の形態による軌跡推定装置２５０について説明する。第３の実施の形態において、軌跡推定装置１または軌跡推定装置１５０と同様の構成および動作については、同一符号を付し、重複説明を省略する。軌跡推定装置２５０のハードウエア構成の一例は、軌跡推定装置１の例と同様である。

図２２は、第３の実施の形態による軌跡推定装置２５０の機能的な構成の一例を示すブロック図である。軌跡推定装置２５０は、所定時間毎の測定時に測定された、車輪速及び操舵角の測定値を受付ける。軌跡推定装置２５０は、第２の実施の形態による軌跡推定装置１５０と同様に、測定時の中から基準時を設定し、最新の基準時から最新の測定時までの軌跡を推定し、推定された軌跡に基づき、１つ前の測定時から最新の測定時までの軌跡を推定する。また、軌跡推定装置２５０は、基準時からの車両５０の軌跡に基づき、基準時を更新する。軌跡推定装置２５０は、第２の実施の形態による軌跡推定装置１５０とは、基準時の更新方法が異なっている。

軌跡推定装置２５０は、演算処理部２５１、記憶部２６０を有している。演算処理部２５１は、第２の実施の形態による軌跡推定装置１５０における演算処理部１５１と比較すると、基準時更新部１５３に代えて基準時更新部２５３を有しており、累積移動量推定部２５５をさらに有する。

累積移動量推定部２５５は、軌跡推定部１５で推定された軌跡から、積算車輪速を逆算し、基準時更新部２５３に出力する。基準時更新部２５３は、軌跡推定部１５の推定結果と累積移動量推定部２５５での算出結果に基づき、基準時を更新すると判別された場合には、基準時を更新する。基準時の更新に関する判別の詳細については後述する。

記憶部２６０は、パラメータＤＢ１９、積算計測値ＤＢ１６４、前時刻軌跡ＤＢ１６６に加え、閾値ＤＢ２６２を有している。図２３は、閾値テーブル２７５の一例を示す図である。閾値テーブル２７５は、閾値ＤＢ２６２に格納されるデータのデータ構造の一例である。閾値テーブル２７５は、第２の実施の形態において説明した閾値テーブル１８２の各パラメータに加え、積算車輪速差閾値ＴＨ２を有している。積算車輪速差閾値ＴＨ２は、基準値を未更新とする最大車輪速差量を示し、例えばセンチメートルを単位とする実数値である。

図２４、図２５は、本実施の形態の軌跡推定の手法を説明する図である。図２４に示すように、計測時推定位置２００−１、２００−２、・・・（まとめて計測時推定位置２００ともいう）は、所定時間毎の車輪速の計測時における車両５０の推定位置を示している。軌跡２０４は、計測時推定位置２００をつないだ曲線であり、車両５０の推定された移動軌跡である。このとき、基準時推定位置２７０−ｋ１、２７０−ｋ２、・・・は、基準時における車両５０の推定位置を示している。本実施の形態においては、基準時からの推定並進距離から逆算した車輪速と、積算車輪速とが、積算車輪速差閾値ＴＨ２を超えた場合、最新の計測時を新たな基準時とする。また、各計測時の推定位置は、最新の基準時から最新の計測時までの積算車輪速、平均操舵角に基づき推定される。各計測時に対応して出力される推定位置の算出方法は、第２の実施の形態と同様である。

図２５は、本実施の形態による基準時選択による積算誤差の低減を説明する図である。図２５に示すように、例えば、車輪速、操舵角の入力は、計測時推定位置２００−１、２００−２、・・・に対応する時刻において行われる。基準時は、例えば、直前の基準時刻に対応する基準時位置２７０−ｋ１から計測時推定位置２００−ｎ１までの推定並進量から逆算した車輪速と積算計測値テーブル１８４に格納された積算車輪速との差が積算車輪速差閾値ＴＨ２以上になった場合に更新される。軌跡の推定は、基準時から計測時までの車輪速、操舵角の測定結果に基づき行う。出力する推定結果は、基準時から前計測時までの推定結果と、基準時から最新の計測時までの推定結果とに基づき算出する。

以下、図２６を参照しながら、第３の実施の形態による軌跡推定装置２５０の動作について説明する。図２６は、軌跡推定装置２５０の主要な処理を示すフローチャートである。図２６に示すように、軌跡推定装置２５０において、車輪速積算部１２は、車輪速受付部１１で受付けられた車輪速の計測値を積算し、積算計測値ＤＢ１６４に格納する。操舵角推定部１４は、車輪速積算部１２で車両５０が動いていると判定された場合に、操舵角受付部１３で受付けた操舵角の計測値を積算して、平均操舵角を算出し、積算値を積算計測値ＤＢ１６４に格納する（Ｓ２８１）。Ｓ２８１の処理は、第２の実施の形態におけるＳ２２１の処理と同様である。

軌跡推定部１５は、例えば積算計測値テーブル１８４のように積算計測値ＤＢ１６４に格納された積算車輪速、および算出される平均操舵角に基づき、第１および第２の実施の形態と同様に旋回量、並進量を推定する（Ｓ２８２）。このＳ２８２の処理の詳細については、第２の実施の形態におけるＳ２２２の処理と同様である。

累積移動量推定部２５５は、Ｓ２８２で推定された旋回量、並進量から、左前輪および右前輪の車輪速積算値を逆算する（Ｓ２８３）。この処理の詳細については後述する。

基準時更新部２５３は、推定された並進量が、基準時更新閾値ＴＨＫ１以上である場合、または、算出された車輪速積算値と、計測による車輪速積算値との差の絶対値が積算車輪速差閾値ＴＨ２より大きい場合に（Ｓ２８４：ＹＥＳ）、基準時を更新する（Ｓ２８５）。基準時更新部２５３は、推定された並進量が、基準時更新閾値ＴＨＫ１未満であり、かつ算出された車輪速積算値と、計測による車輪速積算値との差の絶対値が積算車輪速差閾値ＴＨ２以下の場合には（Ｓ２８４：ＮＯ）、Ｓ２８６に処理を進める。

軌跡算出部１５５は、前時刻軌跡テーブル１８６と、推定された旋回角α、並進移動量（ｄｘ、ｄｙ）に基づき、前時刻からの旋回量および並進量を算出する（Ｓ２８６）。演算処理部２５１は、システム終了のための操作等があったか否かを判別し、ない場合には（Ｓ２８７：ＮＯ）、Ｓ２８１から処理を繰り返す。あった場合には（Ｓ２２６：ＹＥＳ）、演算処理部２５１は、軌跡推定処理を終了する。

ここで、Ｓ２８３、Ｓ２８４の処理の詳細について説明する。軌跡推定装置２５０は、軌跡推定装置１５０と同様に、車輪速の積算および操舵角の積算を行い、軌跡推定部１５は、直前の基準時から最新の計測時までの車両５０の旋回量α、並進移動量（ｑｘ、ｑｙ）を算出する。軌跡推定部１５は、これら旋回量α、並進移動量（ｑｘ、ｑｙ）を軌跡算出部１５５に出力すると共に、旋回量α、車速ＶＲＣ、並進量ＴＲＣを累積移動量推定部２５５に出力する。

第３の実施の形態において、累積移動量推定部２５５は、基準時から最新の計測時までの旋回量αと並進移動量（ｑｘ、ｑｙ）、並進量ＴＲＣを入力として、左前輪５２および右前輪５４でのそれぞれの累積車輪速を算出する。累積移動量推定部２５５は、算出した累積車輪速を、積算計測値テーブル１８４に格納された積算左車輪速ＣＵＭＶＬ、積算右車輪速ＣＵＭＶＲと比較し、その差と並進移動量（ｑｘ、ｑｙ）を基準時更新部２５３に出力する。

＜左右前輪の累積車輪速の算出＞
基準時から最新の計測時までの推定された車両５０の旋回量αと車速ＶＲＣ、曲率ｃについて、回転運動の基本式から、下記式３２が成り立つ。
α＝ｃ×ＶＲＣ・・・（式３２）
よって、推定点Ｏ１での回転半径ＲＲＣは、下記式３３で表される
ＲＲＣ＝１／ｃ＝ＶＲＣ／α・・・（式３３）

左前輪５２、右前輪５４それぞれの回転半径である左半径ＲＦＬ、右半径ＲＦＲは、下記式３４で与えられる。
ＲＦＬ＝ｓｑｒｔ（（ＲＲＣ＋Ｔ）＾２＋Ｌ＾２）
ＲＦＲ＝ｓｑｒｔ（（ＲＲＣ−Ｔ）＾２＋Ｌ＾２）
・・・（式３４）

したがって、左前輪５２、右前輪５４それぞれの車輪速の推定左車輪速ＥＶＬ、推定右車輪速ＥＶＲは、下記式３４のようになる。
ＥＶＬ＝ＲＦＬ×α
＝ｓｑｒｔ（（ＲＲＣ＋Ｔ）＾２＋Ｌ＾２）×α
＝ｓｑｒｔ（（ＶＲＣ／α＋Ｔ）＾２＋Ｌ＾２）×α
＝ｋ×ｓｑｒｔ（（ＶＲＣ＋α×Ｔ）＾２＋（α×Ｌ）＾２）
ＥＶＲ＝ＲＦＲ×α
＝ｓｑｒｔ（（ＲＲＣ−Ｔ）＾２＋Ｌ＾２）×α
＝ｓｑｒｔ（（ＶＲＣ／α−Ｔ）＾２＋Ｌ＾２）×α
＝ｋ×ｓｑｒｔ（（ＶＲＣ−α×Ｔ）＾２＋（α×Ｌ）＾２）
・・・（式３５）
ただし、ｋは移動速度の符号を表すためのパラメータで、以下で定義される。
ｋ＝ １ （ＶＲＣ≧０）
＝−１ （ＶＲＣ＜０）
・・・（式３６）

＜車輪速積算値と推定車輪速積算値の差分＞
積算計測値テーブル１８４に保存される、計測値に基づく積算左車輪速ＣＵＭＶＬ、積算右車輪速ＣＵＭＶＲと、推定された推定左車輪速ＥＶＬ、推定右車輪速ＥＶＲとの差分値ＤＩＦＦを、式３７により定義する。
ＤＩＦＦ＝ＭＩＮ（｜ＣＵＭＶＬ−ＥＶＬ｜、｜ＣＵＭＶＲ−ＥＶＲ｜）
・・・（式３７）
累積移動量推定部２５５は、差分値ＤＩＦＦを基準時更新部２５３に出力する。

＜基準時更新判定＞
基準時更新部２５３は、推定された並進量ＴＲＣと差分値ＤＩＦＦを入力として、基準時の更新の必要性を判定し、必要な場合に基準時を更新する。基準時更新部２５３は、下記式３８が満たされる場合に基準時を更新する。
ＴＲＣ＞ＴＨＫ１ または ｜ＤＩＦＦ｜＞ＴＨＫ２・・・（式３８）
基準時更新部２５３が基準時を更新した場合、第２の実施の形態と同様、積算計測値テーブル１８４において、式３０、式３１の設定が行われる。

以上説明したように、第３の実施の形態による軌跡推定装置２５０によれば、第２の実施の形態による軌跡推定装置１５０と同様の効果を得ることができる。さらに、差分値ＤＩＦＦを導入して基準値更新の判定を行うので、並進量ＴＲＣが少なくても運動の種類が変化した場合などにも、精度よく対応して軌跡の推定を行うことが可能である。すなわち、推定した並進量から積算車輪速を逆算し、計測値に基づく積算車輪速と比較することで、車両５０の運動が円運動で記述できない場合を判別することができる。よって、車両５０の運動が円運動で記述できなくなった段階で、基準時を更新し、積算誤差の低減をより図ることができる。なお、基準時更新部２５３は、基準時更新閾値ＴＨＫ１を用いず、差分値ＤＩＦＦのみによる判別を行うようにしてもよい。

（変形例）
以下、第１から第３の実施の形態の変形例について説明する。本変形例は、第１から第３の実施の形態における軌跡推定部１５の処理の変形例である。本変形例において、第１から第３の実施の形態による軌跡推定装置１、軌跡推定装置１５０、または軌跡推定装置２５０と同様の構成および動作については、同一符号を付し、重複説明を省略する。本変形例に用いられる軌跡推定装置のハードウエア構成の一例は、軌跡推定装置１の例と同様である。

以下、変形例による軌跡推定部１５の処理について説明する。本変形例において軌跡推定部１５は、車輪速と操舵角、または車輪速積算値と平均操舵角とに基づき、軌跡を推定する。以下、第２または第３の実施の形態においての処理と同様、車輪速積算値と平均操舵角とに基づく処理を例にして説明するが、同様の方法を第１の実施の形態に適用することも可能である。

本変形例においては、第２の実施の形態の式２０、式３３、および式３４が用いられる。理解のため、以下に記載する。
ｃ＝φａ／（Ｌ×ｓｑｒｔ（μ＾２−φａ＾２））・・・（式２０）
ＲＲＣ＝１／ｃ・・・（式３３）
ＲＦＬ＝ｓｑｒｔ（（ＲＲＣ＋Ｔ）＾２＋Ｌ＾２）
ＲＦＲ＝ｓｑｒｔ（（ＲＲＣ−Ｔ）＾２＋Ｌ＾２）
・・・（式３４）
ここで、推定される円運動での角速度をωとすると、左前輪５２、右前輪５４の左前輪速ＶＦＬ、右前輪速ＶＦＲは、下記式３９で与えられる。
ＶＦＬ＝ＲＦＬ×ω
ＶＦＲ＝ＲＦＲ×ω
・・・（式３９）

式３９を行列表現で表すと、下記式４０で表される。

式４０を擬似逆行列を用いて最小二乗法で解くとすると、式４１を解くことになる。

式４１を解くと、下記式４２となる。
ω＝（ＲＦＬ×ＶＦＬ＋ＲＦＲ×ＶＦＲ）／（ＲＦＬ＾２＋ＲＦＲ＾２）
＝（ｓｑｒｔ（（ＲＲＣ＋Ｔ）＾２＋Ｌ＾２）×ＶＦＬ
＋ｓｑｒｔ（（ＲＲＣ−Ｔ）＾２＋Ｌ＾２）×ＶＦＲ）
／（２×（ＲＲＣ＾２＋Ｌ＾２＋Ｔ＾２））
＝（ｓｑｒｔ（（１/ｃ＋Ｔ）＾２＋Ｌ＾２）×ＶＦＬ
＋ｓｑｒｔ（（１/ｃ−Ｔ）＾２＋Ｌ＾２）×ＶＦＲ）
／（２×（（１/ｃ）＾２＋Ｌ＾２＋Ｔ＾２））
＝ｃ×（ｓｑｒｔ（（１＋ｃ×Ｔ）＾２＋ｃ×Ｌ＾２）×ＶＦＬ
＋ｓｑｒｔ（（１−ｃ×Ｔ）＾２＋ｃ×Ｌ＾２）×ＶＦＲ）
／（２×（（ｃ＾２×（Ｌ＾２＋Ｔ＾２）＋１））
・・・（式４２）
よって、推定点Ｏ１での車両５０の車速ＶＲＣは、下記式４３で表される。
ＶＲＣ＝ＲＲＣ×ω
＝ω／ｃ
＝（ｓｑｒｔ（（１＋ｃ×Ｔ）＾２＋ｃ×Ｌ＾２）×ＶＦＬ
＋ｓｑｒｔ（（１−ｃ×Ｔ）＾２＋ｃ×Ｌ＾２）×ＶＦＲ）
／（２×（（ｃ＾２×（Ｌ＾２＋Ｔ＾２）＋１））
・・・（式４３）

第２の実施の形態または第３の実施の形態において、左前輪速ＶＦＬ、右前輪速ＶＦＲは、積算左車輪速ＣＵＭＶＬ、積算右車輪速ＣＵＭＶＲで与えられる。よって、式４３は、式４４のように変形される。
ＶＲＣ＝（ｓｑｒｔ（（１＋ｃ×Ｔ）＾２＋ｃ×Ｌ＾２）×ＣＵＭＶＬ
＋ｓｑｒｔ（（１−ｃ×Ｔ）＾２＋ｃ×Ｌ＾２）×ＣＵＭＶＲ）
／（２×（（ｃ＾２×（Ｌ＾２＋Ｔ＾２）＋１））
・・・（式４４）

次に、上記車速ＶＲＣ、旋回角ωから、基準時から最新の計測時までの車両５０の旋回量αと並進移動量（ｑｘ、ｑｙ）を算出する。具体的には第２の実施の形態における式２６、式２７により求められる。

以上のように算出された、旋回量α、並進移動量（ｑｘ、ｑｙ）が、軌跡算出部１５５に出力される。第３の実施の形態に適用する場合には、さらに、旋回量α、車速ＶＲＣ、並進量ＴＲＣが、累積移動量推定部２５５に出力される。なお、第１の実施の形態による軌跡推定装置１に本変形例を適用する場合には、式２０、式３３、式３４に代えて、式１、式７、式１２を用いればよい。

以上説明したように、本変形例は、第１から第３の実施の形態の変形例とすることができる。本変形例によれば、例えば、実測された左前輪速ＶＦＬと右前輪速ＶＦＲとの間に誤差による数値のばらつきなどがあっても、最小二乗法を用いているため、精度よく軌跡の推定を行うことができる。

（第４の実施の形態）
以下、第４の実施の形態による軌跡推定装置４５０について説明する。第４の実施の形態において、軌跡推定装置１、軌跡推定装置１５０、または軌跡推定装置２５０と同様の構成および動作については、同一符号を付し、重複説明を省略する。

第４の実施の形態による軌跡推定装置４５０は、通信網４３０を介して検出装置４１０と接続される。図２７は、第４の実施の形態による軌跡推定装置４５０および検出装置４１０の構成を示す図である。

図２７に示すように、車両４００に検出装置４１０が搭載されている。検出装置４１０は、車両４００に設けられた車輪速センサ４０２、操舵角センサ４０４から、夫々左前輪速ＶＦＬ、右前輪速ＶＦＲ、積算操舵角量ＣＵＭφを検出し、通信網４３０を介して軌跡推定装置４５０に送信する。軌跡推定装置４５０は、検出装置４１０からの計測値を受信し、車両４００の軌跡を推定する。

検出装置４１０は、ＣＰＵ４１２、メモリ４１４、車輪速取得Ｉ／Ｆ４１６、操舵角取得Ｉ／Ｆ４１８、出力Ｉ／Ｆ４２０、送受信装置４２２を有する。ＣＰＵ４１２は、検出装置４１０の動作を制御する演算処理装置である。ＣＰＵ４１２は、例えばメモリ４１４に予め記憶された制御プログラムを読み込んで実行することにより、検出装置４１０としての制御処理を行う。メモリ４１４は、例えば、読出し専用記憶装置、随時書き込み及び読み出し可能な記憶装置などである。車輪速取得Ｉ／Ｆ４１６は、車輪速センサ４０２から左右前輪の夫々の車輪速を受付ける際の管理を行うインタフェース装置である。操舵角取得Ｉ／Ｆ４１８は、操舵角センサ４０４から、車両の前輪の方向を変化させるハンドルの切り角を受付ける際の管理を行うインタフェース装置である。出力Ｉ／Ｆ４２０は、軌跡の推定結果を出力する際の管理を行うインタフェース装置である。送受信装置４２２は、通信網４３０を介して軌跡推定装置４５０との間の通信を行う送受信装置である。

軌跡推定装置４５０は、ＣＰＵ４５２、メモリ４５４、入力装置４５６、出力装置４５８、送受信装置４６０を有している。ＣＰＵ４５２は、軌跡推定装置４５０の動作を制御する演算処理装置である。ＣＰＵ４５２は、例えばメモリ４５４に予め記憶された制御プログラムを読み込んで実行することにより、軌跡推定装置４５０としての制御処理を行う。メモリ４５４は、例えば、読出し専用記憶装置、随時書き込み及び読み出し可能な記憶装置などである。入力装置４５６は、検出装置４１０からの車輪速、操舵角等を受付ける。出力装置４５８は、結果を出力する。送受信装置４６０は、通信網４３０を介して検出装置４１０との間の通信を行う。軌跡推定装置４５０は、上記に説明した第１から第４の実施の形態または変形例による軌跡推定装置１、軌跡推定装置１５０、軌跡推定装置２５０のいずれでもよい。

上記のような構成により、車両４００で計測した計測値に基づき通信網４３０を介して軌跡推定装置４５０で、車両４００の軌跡の推定を行うことができる。軌跡推定装置４５０として、第１から第３および変形例のいずれかを採用することにより、同様の効果を得ることができる。

ここで、上記第１から第４の実施の形態および変形例による軌跡推定方法の動作をコンピュータに行わせるために共通に適用されるコンピュータの例について説明する。図２８は、標準的なコンピュータのハードウエア構成の一例を示すブロック図である。図２８に示すように、コンピュータ５００は、Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ（ＣＰＵ）５０２、メモリ５０４、入力装置５０６、出力装置５０８、外部記憶装置５１２、媒体駆動装置５１４、ネットワーク接続装置等がバス５１０を介して接続されている。

ＣＰＵ５０２は、コンピュータ５００全体の動作を制御する演算処理装置である。メモリ５０４は、コンピュータ５００の動作を制御するプログラムを予め記憶したり、プログラムを実行する際に必要に応じて作業領域として使用したりするための記憶部である。メモリ５０４は、例えばＲａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ（ＲＡＭ）、Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ（ＲＯＭ）等である。入力装置５０６は、コンピュータの使用者により操作されると、その操作内容に対応付けられている使用者からの各種情報の入力を取得し、取得した入力情報をＣＰＵ５０２に送付する装置であり、例えばキーボード装置、マウス装置などである。出力装置５０８は、コンピュータ５００による処理結果を出力する装置であり、表示装置などが含まれる。例えば表示装置は、ＣＰＵ５０２により送付される表示データに応じてテキストや画像を表示する。

外部記憶装置５１２は、例えば、ハードディスクなどの記憶装置であり、ＣＰＵ５０２により実行される各種制御プログラムや、取得したデータ等を記憶しておく装置である。媒体駆動装置５１４は、可搬記録媒体５１６に書き込みおよび読み出しを行うための装置である。ＣＰＵ５０２は、可搬記録媒体５１６に記録されている所定の制御プログラムを、媒体駆動装置５１４を介して読み出して実行することによって、各種の制御処理を行うようにすることもできる。可搬記録媒体５１６は、例えばＣｏｍｐａｃｔ Ｄｉｓｃ（ＣＤ）−ＲＯＭ、Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ（ＤＶＤ）、Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ（ＵＳＢ）メモリ等である。ネットワーク接続装置５１８は、有線または無線により外部との間で行われる各種データの授受の管理を行うインタフェース装置である。バス５１０は、上記各装置等を互いに接続し、データのやり取りを行う通信経路である。

上記第１から第４の実施の形態による軌跡推定方法をコンピュータに実行させるプログラムは、例えば外部記憶装置５１２に記憶させる。ＣＰＵ５０２は、外部記憶装置５１２からプログラムを読み出し、コンピュータ５００に軌跡推定の動作を行なわせる。このとき、まず、軌跡推定の処理をＣＰＵ５０２に行わせるための制御プログラムを作成して外部記憶装置５１２に記憶させておく。そして、入力装置５０６から所定の指示をＣＰＵ５０２に与えて、この制御プログラムを外部記憶装置５１２から読み出させて実行させるようにする。また、このプログラムは、可搬記録媒体５１６に記憶するようにしてもよい。

なお、本発明は、以上に述べた実施の形態に限定されるものではなく、本発明の要旨を逸脱しない範囲内で種々の構成または実施形態を採ることができる。

以上の実施形態に関し、さらに以下の付記を開示する。
（付記１）
移動体の左右の前輪の車輪速の計測値を受付ける車輪速受付部と、
前記移動体の進行方向を変更する操舵角の計測値を受付ける操舵角受付部と、
前記左右の前輪と前記左右の前輪より前記移動体における後部に設けられた左右の後輪との前記移動体の前後方向の距離、前記左右の前輪の左右方向の間隔、および前記操舵角と前記操舵角による前記移動体の回転半径とに応じた定数を記憶した記憶部と、
前記左右の前輪の車輪速の計測値、前記操舵角の計測値、前記前後方向の距離、前記左右方向の間隔、および前記定数に基づき、前記左右の後輪の回転中心を通る直線上の点を中心とする円上における前記左右の後輪の回転中心の中点の旋回量と、前記中点の並進量とを推定する軌跡推定部と、
を有することを特徴とする軌跡推定装置。
（付記２）
軌跡推定部は、前記左右の前輪のうちの左前輪は、前記左前輪の角度を変更する第１の軸に接続され、右前輪は、前記右前輪の角度を変更する第２の軸に接続され、前記第１の軸と前記第２の軸とは、第３の軸にそれぞれ直角に接続され、前記操舵角は、前記第３の軸の前記左右方向の変位に変換されるとして、前記円の回転半径を算出することにより、前記旋回量および前記並進量を推定する
ことを特徴とする付記１に記載の軌跡推定装置。
（付記３）
前記旋回量および前記並進量を推定する基準とする基準時から前記基準時が更新されるまで、所定時間毎の計測時に計測された前記左右の前輪の車輪速をそれぞれ積算する車輪速積算部と、
前記基準時から最新の前記計測時までの平均操舵角を推定する操舵角推定部と、
前記基準時を更新する基準時更新部と、
をさらに有し、
積算された前記左右の前輪の車輪速と前記平均操舵角とに基づき前記所定時間毎の計測時に応じた前記旋回量および前記並進量を算出する軌跡算出部と、
をさらに有し、
前記軌跡推定部は、前記基準時から前記最新の計測時までの旋回量および並進量を、それぞれ積算された前記左右の車輪速、および前記平均操舵角に基づき推定し、
前記基準時更新部は、推定された前記並進量が、前記旋回量または前記並進量の誤差の増大を抑制する予め定められた条件に当てはまる場合は、前記基準時を前記最新の計測時に更新し、
軌跡算出部は、推定された前記旋回量および前記並進量に基づき、前記最新の計測時に応じた前記移動体の旋回量および並進量を算出する、
ことを特徴とする付記１または付記２に記載の軌跡推定装置。
（付記４）
前記条件は、推定された前記基準時から前記最新の計測時までの前記移動体の並進量が基準時更新閾値以上であることであり、
軌跡算出部は、推定された前記旋回量および前記並進量に基づき、前記最新の計測時の一つ前の計測時から前記最新の計測時までの前記移動体の旋回量および並進量を前記最新の計測時に応じた旋回量および並進量として算出する、
ことを特徴とする付記３に記載の軌跡推定装置。
（付記５）
推定された前記基準時から前記最新の計測時までの前記移動体の旋回量および並進量から、前記左右の前輪の左車輪速および右車輪速を逆算し、前記車輪速積算部により積算された左車輪速または右車輪速と比較する累積移動量推定部
をさらに有し、
前記条件は、逆算された前記左右の前輪の左車輪速または右車輪速と、前記車輪速積算部により積算された左車輪速または右車輪速との差が第２の所定量以内であることであり、
軌跡算出部は、推定された前記旋回量および前記並進量に基づき、前記最新の計測時の一つ前の計測時から前記最新の計測時までの前記移動体の旋回量および並進量を前記最新の計測時に応じた旋回量および並進量軌跡として算出する、
ことを特徴とする付記３または付記４に記載の軌跡推定装置。
（付記６）
前記軌跡推定部は、前記左右の前輪の車輪速の計測値、前記操舵角の計測値、前記前後方向の距離、前記左右方向の間隔、および前記定数に基づき、最小二乗法を用いて、前記左右の後輪の回転中心を通る直線上の点を中心とする円上における前記左右の後輪の回転中心の中点の旋回量と、前記中点の並進量とを推定することを特徴とする付記１から付記５のいずれかに記載の軌跡推定装置。
（付記７）
コンピュータによって実行される軌跡推定方法であって、
移動体の左右の前輪の車輪速の計測値、および前記移動体の進行方向を変更する操舵角の計測値を受付け、
前記左右の前輪と前記左右の前輪より前記移動体における後部に設けられた左右の後輪との前記移動体の前後方向の距離、前記左右の前輪の左右方向の間隔、および前記操舵角と前記操舵角による前記移動体の回転半径とに応じた定数に基づき、前記左右の後輪の回転中心を通る直線上の点を中心とする円上における前記左右の後輪の回転中心の中点の旋回量と、前記中点の並進量とを推定する、
ことを特徴とする軌跡推定方法。
（付記８）
前記旋回量および前記並進量を推定する処理においては、前記左右の前輪のうちの左前輪は、前記左前輪の角度を変更する第１の軸に接続され、右前輪は、前記右前輪の角度を変更する第２の軸に接続され、前記第１の軸と前記第２の軸とは、第３の軸にそれぞれ直角に接続され、前記操舵角は、前記第３の軸の前記左右方向の変位に変換されるとして、前記円の回転半径を算出する
ことを特徴とする付記７に記載の軌跡推定方法。
（付記９）
前記旋回量および前記並進量を推定する基準とする基準時から前記基準時が更新されるまで、所定時間毎の計測時に計測された前記左右の前輪の車輪速をそれぞれ積算し、
前記基準時から最新の前記計測時までの平均操舵角を推定し、
前記基準時から前記最新の計測時までの旋回量および並進量を、それぞれ積算された前記左右の車輪速、および前記平均操舵角に基づき推定し、
推定された前記並進量が、前記旋回量または前記並進量の誤差の増大を抑制する予め定められた条件に当てはまる場合は、前記基準時を前記最新の計測時に更新し
推定された前記旋回量および前記並進量に基づき、前記最新の計測時に応じた前記移動体の旋回量および並進量を算出する、
ことを特徴とする付記７または付記８に記載の軌跡推定方法。
（付記１０）
前記条件は、推定された前記基準時から前記最新の計測時までの前記移動体の並進量が基準時更新閾値以上であることであり、
推定された前記旋回量および前記並進量に基づき、前記最新の計測時の一つ前の計測時から前記最新の計測時までの前記移動体の旋回量および並進量を前記最新の計測時に応じた旋回量および並進量として算出する、
ことを特徴とする付記９に記載の軌跡推定方法。
（付記１１）
さらに、推定された前記基準時から前記最新の計測時までの前記移動体の旋回量および並進量から、前記左右の前輪の左車輪速および右車輪速を逆算し、
前記条件は、逆算された前記左右の前輪の左車輪速または右車輪速と、前記車輪速積算部により積算された左車輪速または右車輪速との差が第２の所定量以内であることであり、
推定された前記旋回量および前記並進量に基づき、前記最新の計測時の一つ前の計測時から前記最新の計測時までの前記移動体の旋回量および並進量を前記最新の計測時に応じた旋回量および前記並進量として算出する、
ことを特徴とする付記９または付記１０に記載の軌跡推定方法。
（付記１２）
前記左右の前輪の車輪速の計測値、前記操舵角の計測値、前記前後方向の距離、前記左右方向の間隔、および前記定数に基づき、最小二乗法を用いて、前記左右の後輪の回転中心を通る直線上の点を中心とする円上における前記左右の後輪の回転中心の中点の旋回量と、前記中点の並進量とを推定する
ことを特徴とする付記７から付記１１のいずれかに記載の軌跡推定方法。
（付記１３）
移動体の左右の前輪の車輪速の計測値、および前記移動体の進行方向を変更する操舵角の計測値を受付け、
前記左右の前輪と前記左右の前輪より前記移動体における後部に設けられた左右の後輪との前記移動体の前後方向の距離、前記左右の前輪の左右方向の間隔、および前記操舵角と前記操舵角による前記移動体の回転半径とに応じた定数に基づき、前記左右の後輪の回転中心を通る直線上の点を中心とする円上における前記左右の後輪の回転中心の中点の旋回量と、前記中点の並進量とを推定する、
処理をコンピュータに実行させるプログラム。
（付記１４）
前記旋回量および前記並進量を推定する処理においては、前記左右の前輪のうちの左前輪は、前記左前輪の角度を変更する第１の軸に接続され、右前輪は、前記右前輪の角度を変更する第２の軸に接続され、前記第１の軸と前記第２の軸とは、第３の軸にそれぞれ直角に接続され、前記操舵角は、前記第３の軸の前記左右方向の変位に変換されるとして、前記円の回転半径を算出する
ことを特徴とする付記１３に記載のプログラム。
（付記１５）
前記旋回量および前記並進量を推定する基準とする基準時から前記基準時が更新されるまで、所定時間毎の計測時に計測された前記左右の前輪の車輪速をそれぞれ積算し、
前記基準時から最新の前記計測時までの平均操舵角を推定し、
前記基準時から前記最新の計測時までの旋回量および並進量を、それぞれ積算された前記左右の車輪速、および前記平均操舵角に基づき推定し、
推定された前記並進量が、前記旋回量または前記並進量の誤差の増大を抑制する予め定められた条件に当てはまる場合は、前記基準時を前記最新の計測時に更新し
推定された前記旋回量および前記並進量に基づき、前記最新の計測時に応じた前記移動体の旋回量および並進量を算出する、
ことを特徴とする付記１３または付記１４に記載のプログラム。

１ 軌跡推定装置
３ 演算処理部
５ 記憶部
１１ 車輪速受付部
１２ 車輪速積算部
１３ 操舵角受付部
１４ 操舵角推定部
１５ 軌跡推定部
１９ パラメータＤＢ
２１ ＣＰＵ
２３ メモリ
２５ 車輪速取得Ｉ／Ｆ
２７ 操舵角取得Ｉ／Ｆ
２９ 出力Ｉ／Ｆ
３１ バス
３３ 車輪速センサ
３５ 操舵角センサ
４０ パラメータテーブル
５０ 車両
５２ 左前輪
５４ 右前輪
５６ 左後輪
５８ 右後輪
６０ ステアリング軸
６２ ピニオンギア
６４ タイロッド
６６ ナックルアーム
６８ ナックルアーム
ＲＲＣ 回転半径
ＶＦＬ 左前輪速
ＶＦＲ 右前輪速
ＶＲＣ 車速
ＴＲＣ 並進量
Ｌ 車輪前後間隔
Ｍ 回転中心
Ｔ 車輪左右間隔値
ＶＲＭＳ 二乗平均車輪速

Claims (8)

1. 移動体の左右の前輪の車輪速の計測値を受付ける車輪速受付部と、
   前記移動体の進行方向を変更する操舵角の計測値を受付ける操舵角受付部と、
   前記左右の前輪と前記左右の前輪より前記移動体における後部に設けられた左右の後輪との前記移動体の前後方向の距離、前記左右の前輪の左右方向の間隔、および前記操舵角と前記操舵角による前記移動体の回転半径とに応じた定数を記憶した記憶部と、
   前記左右の前輪の車輪速の計測値、前記操舵角の計測値、前記前後方向の距離、前記左右方向の間隔、および前記定数に基づき、前記左右の後輪の回転中心を通る直線上の点を中心とする円上における前記左右の後輪の回転中心の中点の旋回量と、前記中点の並進量とを推定する軌跡推定部と、
   を有することを特徴とする軌跡推定装置。
2. 軌跡推定部は、前記左右の前輪のうちの左前輪は、前記左前輪の角度を変更する第１の軸に接続され、右前輪は、前記右前輪の角度を変更する第２の軸に接続され、前記第１の軸と前記第２の軸とは、第３の軸にそれぞれ直角に接続され、前記操舵角は、前記第３の軸の前記左右方向の変位に変換されるとして、前記円の回転半径を算出することにより、前記旋回量および前記並進量を推定する
   ことを特徴とする請求項１に記載の軌跡推定装置。
3. 前記旋回量および前記並進量を推定する基準とする基準時から前記基準時が更新されるまで、所定時間毎の計測時に計測された前記左右の前輪の車輪速をそれぞれ積算する車輪速積算部と、
   前記基準時から最新の前記計測時までの平均操舵角を推定する操舵角推定部と、
   前記基準時を更新する基準時更新部と、
   をさらに有し、
   積算された前記左右の前輪の車輪速と前記平均操舵角とに基づき前記所定時間毎の計測時に応じた前記旋回量および前記並進量を算出する軌跡算出部と、
   をさらに有し、
   前記軌跡推定部は、前記基準時から前記最新の計測時までの旋回量および並進量を、それぞれ積算された前記左右の車輪速、および前記平均操舵角に基づき推定し、
   前記基準時更新部は、推定された前記並進量が、前記旋回量または前記並進量の誤差の増大を抑制する予め定められた条件に当てはまる場合は、前記基準時を前記最新の計測時に更新し、
   軌跡算出部は、推定された前記旋回量および前記並進量に基づき、前記最新の計測時に応じた前記移動体の旋回量および並進量を算出する、
   ことを特徴とする請求項１または請求項２に記載の軌跡推定装置。
4. 前記条件は、推定された前記基準時から前記最新の計測時までの前記移動体の並進量が基準時更新閾値以上であることであり、
   軌跡算出部は、推定された前記旋回量および前記並進量に基づき、前記最新の計測時の一つ前の計測時から前記最新の計測時までの前記移動体の旋回量および並進量を前記最新の計測時に応じた旋回量および並進量として算出する、
   ことを特徴とする請求項３に記載の軌跡推定装置。
5. 推定された前記基準時から前記最新の計測時までの前記移動体の旋回量および並進量から、前記左右の前輪の左車輪速および右車輪速を逆算し、前記車輪速積算部により積算された左車輪速または右車輪速と比較する累積移動量推定部
   をさらに有し、
   前記条件は、逆算された前記左右の前輪の左車輪速または右車輪速と、前記車輪速積算部により積算された左車輪速または右車輪速との差が第２の所定量以内であることであり、
   軌跡算出部は、推定された前記旋回量および前記並進量に基づき、前記最新の計測時の一つ前の計測時から前記最新の計測時までの前記移動体の旋回量および並進量を前記最新の計測時に応じた旋回量および並進量軌跡として算出する、
   ことを特徴とする請求項３または請求項４に記載の軌跡推定装置。
6. 前記軌跡推定部は、前記左右の前輪の車輪速の計測値、前記操舵角の計測値、前記前後方向の距離、前記左右方向の間隔、および前記定数に基づき、最小二乗法を用いて、前記左右の後輪の回転中心を通る直線上の点を中心とする円上における前記左右の後輪の回転中心の中点の旋回量と、前記中点の並進量とを推定することを特徴とする請求項１から請求項５のいずれかに記載の軌跡推定装置。
7. コンピュータによって実行される軌跡推定方法であって、
   移動体の左右の前輪の車輪速の計測値、および前記移動体の進行方向を変更する操舵角の計測値を受付け、
   前記左右の前輪と前記左右の前輪より前記移動体における後部に設けられた左右の後輪との前記移動体の前後方向の距離、前記左右の前輪の左右方向の間隔、および前記操舵角と前記操舵角による前記移動体の回転半径とに応じた定数に基づき、前記左右の後輪の回転中心を通る直線上の点を中心とする円上における前記左右の後輪の回転中心の中点の旋回量と、前記中点の並進量とを推定する、
   ことを特徴とする軌跡推定方法。
8. 移動体の左右の前輪の車輪速の計測値、および前記移動体の進行方向を変更する操舵角の計測値を受付け、
   前記左右の前輪と前記左右の前輪より前記移動体における後部に設けられた左右の後輪との前記移動体の前後方向の距離、前記左右の前輪の左右方向の間隔、および前記操舵角と前記操舵角による前記移動体の回転半径とに応じた定数に基づき、前記左右の後輪の回転中心を通る直線上の点を中心とする円上における前記左右の後輪の回転中心の中点の旋回量と、前記中点の並進量とを推定する、
   処理をコンピュータに実行させるプログラム。

Images