JP2012255686A - 車両の位置姿勢測定装置 - Google Patents

Abstract

【課題】車両の位置姿勢を精度良く測定する車両の位置姿勢測定装置を提供する。
【解決手段】前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、ステアリング角(θ)を検出するステアリング角検出手段(１)と、車両の前後左右四輪の車輪速(Ｐ_Ｆｌ，Ｐ_Ｆｒ，Ｐ_Ｒｌ，Ｐ_Ｒｒ)を検出する車輪速検出手段(２−５)と、前記ステアリング角検出手段からの出力に基づいて実舵角(δ_Ｆｌ，δ_Ｆｒ)を演算する実舵角演算手段(６)と、前記実舵角と前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値(Ｔ^† _Ｒ，ｒ^† _Ｒｌ，ｒ^† _Ｒｒ)を演算するトレッド・車輪半径推定手段(７)と、前記推定値と後輪の前記車輪速に基づき車両の位置(ｘ，ｙ)および姿勢(γ)を演算する車両位置姿勢演算手段(８)と、を備え、任意の慣性座標系に対する車両の位置および姿勢を測定する。
【選択図】図１

Description

この発明は、任意の慣性座標系における車両の位置・姿勢を測定するための車両の位置姿勢測定装置に関するものである。

従来の車両の位置姿勢測定装置では、車輪径が変化しないと仮定して非操舵左右輪の回転数から求めた移動距離差および平均移動距離と、さらにトレッドも変化しないとする仮定により、車両の位置および姿勢を測定していた(例えば特許文献１、非特許文献１)。
さらに、車輪の回転数の合計および／または二つの車輪の回転数差および／または車輪の所定の回転に対する時間ないし回転状態もしくは回転状態に達するまでの時間に依存した補正係数を導入することにより、カーブ走行中などに生じるトレッドの変化に対応した位置姿勢測定方式も提案されている(例えば特許文献２)。

特開昭６１−２７４２１４号公報 特表昭５９−５０２１５３号公報

C. Ming Wang著、"Location Estimation and Uncertainty Analysis for Mobile Robots"、IEEE International Conference on Robotics and Automation、pp.1230-1235, 1988(第２節) 馬屋原将明 他著、"非線形逐次最小２乗法に基づく耐クロストークノイズキャンセラ"、電子情報通信学会論文誌Ａ、ｐｐ．１６２−１６９、２００２」 谷萩隆嗣 著、"カルマンフィルタと適応信号処理"、ｐｐ．４２−５１、コロナ社、２００５年１２月

このような車両の位置姿勢測定装置では、タイヤ空気圧の左右差などを要因として、非操舵左右輪の有効径に差異がある場合に、左右輪の移動距離演算に誤差が生じ、その結果位置・姿勢の測定精度が悪化するという問題点があった。

この発明は、上記のような問題点を解決するためになされたものであり、車両の位置姿勢を精度良く測定する車両の位置姿勢測定装置を提供することを目的とする。

この発明は、前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、ステアリング角を検出するステアリング角検出手段と、車両の前後左右四輪の車輪速を検出する車輪速検出手段と、前記ステアリング角検出手段からの出力に基づいて実舵角を演算する実舵角演算手段と、前記実舵角と前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値を演算するトレッド・車輪半径推定手段と、前記推定値と後輪の前記車輪速に基づき車両の位置および姿勢を演算する車両位置姿勢演算手段と、を備え、任意の慣性座標系に対する車両の位置および姿勢を測定することを特徴とする車両の位置姿勢測定装置等にある。

この発明によれば、前記トレッド・車輪半径推定手段を備えたことにより、走行中にリアトレッドが変化した場合だけでなく、タイヤ空気圧の減少などによって車輪径が変化した場合でも、車両の位置および姿勢を高精度に測定できる。

この発明の実施の形態１による車両の位置姿勢測定装置の構成図である。 この発明の原理を説明するための説明図である。 この発明の実施の形態２による車両の位置姿勢測定装置の構成図である。

以下、この発明による車両の位置姿勢測定装置を各実施の形態に従って図面を用いて説明する。なお、各実施の形態においては、前輪を操舵する車両の位置姿勢測定を行う装置の例を示しており、また同一もしくは相当部分は同一符号で示し、重複する説明は省略する。

実施の形態１．
図１はこの発明の実施の形態１による車両の位置姿勢測定装置の構成図である。この車両の位置姿勢測定装置は、ステアリング角θを検出するためのステアリング角センサ１と、車両の各車輪の車輪速を検出するために車輪速センサ２(左前輪)，車輪速センサ３(右前輪)，車輪速センサ４(左後輪)，車輪速センサ５(右後輪)を有している。車輪速センサ２，３，４，５は、それぞれ対応する車輪が所定角度回転する毎に検出パルスを出力する公知の構成の車輪速センサである。

実舵角演算器６は、ステアリング角センサ１により検出されたステアリング角θを予め実験によって求めた関係式(１)に代入し、実舵角δ_Ｆｌ，δ_Ｆｒを演算する。

トレッド・車輪半径推定器７は、実舵角演算器６から出力される実舵角δ_Ｆｌ，δ_Ｆｒと車輪速センサ２，３，４，５からそれぞれ出力される各パルス信号Ｐ_Ｆｌ，Ｐ_Ｆｒ，Ｐ_Ｒｌ，Ｐ_Ｒｒから、フロントトレッドＴ_Ｒ，リアトレッドＴ_Ｒ，左前車輪半径ｒ_Ｆｌ，右前車輪半径ｒ_Ｆｒ，左後車輪半径ｒ_Ｒｌ，右後車輪半径ｒ_Ｒｒを推定する。図２を参照しながら、その原理を説明する。車両の左前車輪１１，右前車輪１２，左後車輪１３，右後車輪１４の横滑り角が全て０であると仮定すると、

が成り立つ。ただし、
ｖ_Ｆｌ，ｖ_Ｆｒ，ｖ_Ｒｌ，ｖ_Ｒｒ：それぞれ車輪１１，１２，１３，１４の移動速度
ｖ_ｘ，ｖ_ｙ：それぞれ重心速度のｘ軸成分，ｙ軸成分(慣性座標系は任意)
(ドット)γ：車両の姿勢角(ヨー角：基準座標ｘｙ上の車両方向のｘ軸とのなす角度)γの単位時間当たりの変化量(角速度)
Ｌ_Ｆ：前輪軸中心(左前車輪１１と右前車輪１２を結ぶ線の中心)と車両重心ｃ.ｇ.との間の長さ
Ｌ_Ｒ：後輪軸中心(左後車輪１３と右後車輪１４を結ぶ線の中心)と車両重心ｃ.ｇ.との間の長さ
である。例えば、後退駐車時のように、車両が低速で定常運動を行なっていると考えられる場合においては、このような仮定が成立する。

さらに、上式を整理すると、

が得られる。Ｌはホイルベースを示す。ところで、各車輪速センサ２，３，４，５からの出力パルス数Ｐ_Ｆｌ，Ｐ_Ｆｒ，Ｐ_Ｒｌ，Ｐ_Ｒｒと各車輪１１，１２，１３，１４の移動速度ｖ_Ｆｌ，ｖ_Ｆｒ，ｖ_Ｒｌ，ｖ_Ｒｒとの間には、関係式

が成り立つ。ただし、Ｎ_Ｆｌ，Ｎ_Ｆｒ，Ｎ_Ｒｌ，Ｎ_Ｒｒは各車輪が１回転したときのパルス数であり、Ｔはサンプリング時間、ｎは正の整数、である。したがって、ここで考えている定常運動状態では、操舵速度は十分小さいので、

と仮定できるので((ドット)δ_Ｆｌ，(ドット)δ_Ｆｒはそれぞれ実舵角δ_Ｆｌ，δ_Ｆｒの単位時間当たりの変化量(実舵角速度))、(９)，(１０)より、

が成り立つ。そこで、この関係式(１２)から、車輪半径およびトレッドの走行中の実際の値である真値とメーカが公表しているノミナル値(公称値)との誤差量が満たす関係式を導出し、その関係式に対して非線形逐次最小二乗法、拡張カルマンフィルタなどを用いたパラメータ推定手法を適用することで、実舵角δ_Ｆｌ，δ_Ｆｒとパルス数Ｐ_Ｆｌ，Ｐ_Ｆｒ，Ｐ_Ｒｌ，Ｐ_Ｒｒから、ノミナル値に誤差補正を加えた車輪半径およびトレッドの推定値を得ることができる。

例えば、車輪半径およびトレッドの真値ｒ_Ｆｌ，ｒ_Ｆｒ，ｒ_Ｒｌ，ｒ_Ｒｒ，Ｔ_Ｆ，Ｔ_Ｒは、ノミナル値ｒ^＊ _Ｆｌ，ｒ^＊ _Ｆｒ，ｒ^＊ _Ｒｌ，ｒ^＊ _Ｒｒ，Ｔ^＊ _Ｆ，Ｔ^＊ _Ｒとスケールファクタ誤差Δｒ_Ｆｌ，Δｒ_Ｆｒ，Δｒ_Ｒｌ，Δｒ_Ｒｒ，ΔＴ_Ｆ，ΔＴ_Ｒを用いて、

と表現できるので、関係式(１３)を関係式(１２)に代入することにより、誤差量Δｒ_Ｆｌ，Δｒ_Ｆｒ，Δｒ_Ｒｌ，Δｒ_Ｒｒ，ΔＴ_Ｆ，ΔＴ_Ｒが満たすべき関係式が得られる。なお、非線形逐次最小二乗法および拡張カルマンフィルタについては、それぞれ上記非特許文献２，３などを参照のこと。

車両位置姿勢演算器８は、トレッド・車輪半径推定器７から出力される後輪(左後、右後)車輪半径およびリアトレッドの推定値ｒ_Ｒｌ ^†，ｒ_Ｒｒ ^†，Ｔ_Ｒ ^†と、後輪(左後、右後)の車輪速センサ４，５からそれぞれ出力される各パルス信号Ｐ_Ｒｌ，Ｐ_Ｒｒから、車両の平面位置および姿勢を演算する。図２を参照しながら、その原理を説明する。車両の運動学的モデルより、後輪軸中心の位置ｘ，ｙは関係式

を満たす((ドット)ｘ，(ドット)ｙはｘ，ｙ座標の値の単位時間当たりの変化量)。そこで、関係式(６)，(７)を関係式(１４)に代入して得られる関係式および関係式(６)をサンプリング時間Ｔ(０＜Ｔ＜＜１)で離散近似すると、後輪軸中心の位置ｘ(ｎＴ)，ｙ(ｎＴ)および姿勢角(ヨー角)γ(ｎＴ)が満たすべき差分方程式

が得られる(上記非特許文献１参照)。ただし、

である。さらに、ここで考えている定常運動状態では、車両加速度は十分小さいので、

と近似できるので、関係式(１０)より、

が成り立つが、後輪車輪半径およびリアトレッドの真値は未知であるので、それらをトレッド・車輪半径推定器７からの出力値、すなわち後輪車輪半径およびリアトレッドの推定値ｒ_Ｒｌ ^†，ｒ_Ｒｒ ^†，Ｔ_Ｒ ^†に置き換えると、関係式(１９)が得られる。

よって、車両運動を記述する慣性座標系の設定に応じて、初期値を適切に設定することにより、任意の慣性座標系における位置ｘ(ｎＴ)，ｙ(ｎＴ)および姿勢角(ヨー角)γ(ｎＴ)は、後輪車輪半径およびリアトレッドの推定値ｒ_Ｒｌ ^†，ｒ_Ｒｒ ^†，Ｔ_Ｒ ^†とパルス信号Ｐ_Ｒｌ，Ｐ_Ｒｒから、関係式(１５)と(１９)に基づいて演算できる。

以上のような構成によれば、トレッド・車輪半径推定器７がオンラインで車輪半径およびトレッドを推定しつつ、その結果に基づき車両位置姿勢演算器８が車両の位置および姿勢を演算するので、リアトレッドあるいは後輪車輪径が変化しノミナル値と異なるといった場合でも、車両の位置および姿勢を高精度に測定できる。

実施の形態２．
図３はこの発明の実施の形態２による車両の位置姿勢測定装置の構成図である。この装置は、車両の各車輪の車輪速を検出するための車輪速センサ(左前輪)２，車輪速センサ(右前輪)３，車輪速センサ(左後輪)４，車輪速センサ(右後輪)５以外のセンサを必要としない。この構成によれば、図１に示したステアリング角センサ１と実舵角演算器６が不要となるほか、ステアリング機構に基づく検出誤差による車両の位置姿勢測定精度の悪化が生じない。

図３のトレッド・車輪半径推定器７ａの原理を説明する。関係式(１２)よりδ_Ｆｌ，δ_Ｆｒを消去すると、

が成り立つ。この関係式(２０)から導出できる車輪半径およびトレッドの真値とノミナル値との誤差量が満たす関係式に対して、非線形逐次最小二乗法、拡張カルマンフィルタなどのパラメータ推定手法を適用すると、パルス数Ｐ_Ｆｌ，Ｐ_Ｆｒ，Ｐ_Ｒｌ，Ｐ_Ｒｒのみから、ノミナル値に誤差補正を加えた車輪半径、またはトレッド、または車輪半径およびトレッドの両方のそれぞれ推定値を得ることができる。

１ ステアリング角センサ(ステアリング角検出手段)、２，３，４，５ 車輪速センサ(車輪速検出手段)、６ 実舵角演算器(実舵角演算手段)、７，７ａ トレッド・車輪半径推定器(トレッド・車輪半径推定手段)、８ 車両位置姿勢演算器(車両位置姿勢演算手段)、１１，１２，１３，１４ 車輪。

Claims (2)

1. 前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、
   ステアリング角を検出するステアリング角検出手段と、
   車両の前後左右四輪の車輪速を検出する車輪速検出手段と、
   前記ステアリング角検出手段からの出力に基づいて実舵角を演算する実舵角演算手段と、
   前記実舵角と前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値を演算するトレッド・車輪半径推定手段と、
   前記推定値と後輪の前記車輪速に基づき車両の位置および姿勢を演算する車両位置姿勢演算手段と、
   を備え、任意の慣性座標系に対する車両の位置および姿勢を測定することを特徴とする車両の位置姿勢測定装置。
2. 前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、
   車両の前後左右四輪の車輪速を検出する車輪速検出手段と、
   前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値を演算するトレッド・車輪半径推定手段と、
   前記推定値と後輪の前記車輪速に基づき車両の位置および姿勢を演算する車両位置姿勢演算手段と、
   を備え、任意の慣性座標系に対する車両の位置および姿勢を測定することを特徴とする車両の位置姿勢測定装置。

Images