車両の位置姿勢測定装置
【課題】車両の位置姿勢を精度良く測定する車両の位置姿勢測定装置を提供する。
【解決手段】前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、ステアリング角(θ)を検出するステアリング角検出手段(１)と、車両の前後左右四輪の車輪速(ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒ)を検出する車輪速検出手段(２−５)と、前記ステアリング角検出手段からの出力に基づいて実舵角(δＦｌ，δＦｒ)を演算する実舵角演算手段(６)と、前記実舵角と前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値(Ｔ†Ｒ，ｒ†Ｒｌ，ｒ†Ｒｒ)を演算するトレッド・車輪半径推定手段(７)と、前記推定値と後輪の前記車輪速に基づき車両の位置(ｘ，ｙ)および姿勢(γ)を演算する車両位置姿勢演算手段(８)と、を備え、任意の慣性座標系に対する車両の位置および姿勢を測定する。
【解決手段】前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、ステアリング角(θ)を検出するステアリング角検出手段(１)と、車両の前後左右四輪の車輪速(ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒ)を検出する車輪速検出手段(２−５)と、前記ステアリング角検出手段からの出力に基づいて実舵角(δＦｌ，δＦｒ)を演算する実舵角演算手段(６)と、前記実舵角と前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値(Ｔ†Ｒ，ｒ†Ｒｌ，ｒ†Ｒｒ)を演算するトレッド・車輪半径推定手段(７)と、前記推定値と後輪の前記車輪速に基づき車両の位置(ｘ，ｙ)および姿勢(γ)を演算する車両位置姿勢演算手段(８)と、を備え、任意の慣性座標系に対する車両の位置および姿勢を測定する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、任意の慣性座標系における車両の位置・姿勢を測定するための車両の位置姿勢測定装置に関するものである。
【背景技術】
【０００２】
従来の車両の位置姿勢測定装置では、車輪径が変化しないと仮定して非操舵左右輪の回転数から求めた移動距離差および平均移動距離と、さらにトレッドも変化しないとする仮定により、車両の位置および姿勢を測定していた(例えば特許文献１、非特許文献１)。
さらに、車輪の回転数の合計および／または二つの車輪の回転数差および／または車輪の所定の回転に対する時間ないし回転状態もしくは回転状態に達するまでの時間に依存した補正係数を導入することにより、カーブ走行中などに生じるトレッドの変化に対応した位置姿勢測定方式も提案されている(例えば特許文献２)。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開昭６１−２７４２１４号公報
【特許文献２】特表昭５９−５０２１５３号公報
【非特許文献】
【０００４】
【非特許文献１】C. Ming Wang著、”Location Estimation and Uncertainty Analysis for Mobile Robots”、IEEE International Conference on Robotics and Automation、pp.1230-1235, 1988(第２節)
【非特許文献２】馬屋原将明 他著、”非線形逐次最小２乗法に基づく耐クロストークノイズキャンセラ”、電子情報通信学会論文誌Ａ、ｐｐ．１６２−１６９、２００２」
【非特許文献３】谷萩隆嗣 著、”カルマンフィルタと適応信号処理”、ｐｐ．４２−５１、コロナ社、２００５年１２月
【発明の概要】
【発明が解決しようとする課題】
【０００５】
このような車両の位置姿勢測定装置では、タイヤ空気圧の左右差などを要因として、非操舵左右輪の有効径に差異がある場合に、左右輪の移動距離演算に誤差が生じ、その結果位置・姿勢の測定精度が悪化するという問題点があった。
【０００６】
この発明は、上記のような問題点を解決するためになされたものであり、車両の位置姿勢を精度良く測定する車両の位置姿勢測定装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
この発明は、前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、ステアリング角を検出するステアリング角検出手段と、車両の前後左右四輪の車輪速を検出する車輪速検出手段と、前記ステアリング角検出手段からの出力に基づいて実舵角を演算する実舵角演算手段と、前記実舵角と前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値を演算するトレッド・車輪半径推定手段と、前記推定値と後輪の前記車輪速に基づき車両の位置および姿勢を演算する車両位置姿勢演算手段と、を備え、任意の慣性座標系に対する車両の位置および姿勢を測定することを特徴とする車両の位置姿勢測定装置等にある。
【発明の効果】
【０００８】
この発明によれば、前記トレッド・車輪半径推定手段を備えたことにより、走行中にリアトレッドが変化した場合だけでなく、タイヤ空気圧の減少などによって車輪径が変化した場合でも、車両の位置および姿勢を高精度に測定できる。
【図面の簡単な説明】
【０００９】
【図１】この発明の実施の形態１による車両の位置姿勢測定装置の構成図である。
【図２】この発明の原理を説明するための説明図である。
【図３】この発明の実施の形態２による車両の位置姿勢測定装置の構成図である。
【発明を実施するための形態】
【００１０】
以下、この発明による車両の位置姿勢測定装置を各実施の形態に従って図面を用いて説明する。なお、各実施の形態においては、前輪を操舵する車両の位置姿勢測定を行う装置の例を示しており、また同一もしくは相当部分は同一符号で示し、重複する説明は省略する。
【００１１】
実施の形態１．
図１はこの発明の実施の形態１による車両の位置姿勢測定装置の構成図である。この車両の位置姿勢測定装置は、ステアリング角θを検出するためのステアリング角センサ１と、車両の各車輪の車輪速を検出するために車輪速センサ２(左前輪)，車輪速センサ３(右前輪)，車輪速センサ４(左後輪)，車輪速センサ５(右後輪)を有している。車輪速センサ２，３，４，５は、それぞれ対応する車輪が所定角度回転する毎に検出パルスを出力する公知の構成の車輪速センサである。
【００１２】
実舵角演算器６は、ステアリング角センサ１により検出されたステアリング角θを予め実験によって求めた関係式(１)に代入し、実舵角δＦｌ，δＦｒを演算する。
【００１３】
【数１】
【００１４】
トレッド・車輪半径推定器７は、実舵角演算器６から出力される実舵角δＦｌ，δＦｒと車輪速センサ２，３，４，５からそれぞれ出力される各パルス信号ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒから、フロントトレッドＴＲ，リアトレッドＴＲ，左前車輪半径ｒＦｌ，右前車輪半径ｒＦｒ，左後車輪半径ｒＲｌ，右後車輪半径ｒＲｒを推定する。図２を参照しながら、その原理を説明する。車両の左前車輪１１，右前車輪１２，左後車輪１３，右後車輪１４の横滑り角が全て０であると仮定すると、
【００１５】
【数２】
【００１６】
が成り立つ。ただし、
ｖＦｌ，ｖＦｒ，ｖＲｌ，ｖＲｒ：それぞれ車輪１１，１２，１３，１４の移動速度
ｖｘ，ｖｙ：それぞれ重心速度のｘ軸成分，ｙ軸成分(慣性座標系は任意)
(ドット)γ：車両の姿勢角(ヨー角：基準座標ｘｙ上の車両方向のｘ軸とのなす角度)γの単位時間当たりの変化量(角速度)
ＬＦ：前輪軸中心(左前車輪１１と右前車輪１２を結ぶ線の中心)と車両重心ｃ.ｇ.との間の長さ
ＬＲ：後輪軸中心(左後車輪１３と右後車輪１４を結ぶ線の中心)と車両重心ｃ.ｇ.との間の長さ
である。例えば、後退駐車時のように、車両が低速で定常運動を行なっていると考えられる場合においては、このような仮定が成立する。
【００１７】
さらに、上式を整理すると、
【００１８】
【数３】
【００１９】
が得られる。Ｌはホイルベースを示す。ところで、各車輪速センサ２，３，４，５からの出力パルス数ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒと各車輪１１，１２，１３，１４の移動速度ｖＦｌ，ｖＦｒ，ｖＲｌ，ｖＲｒとの間には、関係式
【００２０】
【数４】
【００２１】
が成り立つ。ただし、ＮＦｌ，ＮＦｒ，ＮＲｌ，ＮＲｒは各車輪が１回転したときのパルス数であり、Ｔはサンプリング時間、ｎは正の整数、である。したがって、ここで考えている定常運動状態では、操舵速度は十分小さいので、
【００２２】
【数５】
【００２３】
と仮定できるので((ドット)δＦｌ，(ドット)δＦｒはそれぞれ実舵角δＦｌ，δＦｒの単位時間当たりの変化量(実舵角速度))、(９)，(１０)より、
【００２４】
【数６】
【００２５】
が成り立つ。そこで、この関係式(１２)から、車輪半径およびトレッドの走行中の実際の値である真値とメーカが公表しているノミナル値(公称値)との誤差量が満たす関係式を導出し、その関係式に対して非線形逐次最小二乗法、拡張カルマンフィルタなどを用いたパラメータ推定手法を適用することで、実舵角δＦｌ，δＦｒとパルス数ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒから、ノミナル値に誤差補正を加えた車輪半径およびトレッドの推定値を得ることができる。
【００２６】
例えば、車輪半径およびトレッドの真値ｒＦｌ，ｒＦｒ，ｒＲｌ，ｒＲｒ，ＴＦ，ＴＲは、ノミナル値ｒ＊Ｆｌ，ｒ＊Ｆｒ，ｒ＊Ｒｌ，ｒ＊Ｒｒ，Ｔ＊Ｆ，Ｔ＊Ｒとスケールファクタ誤差ΔｒＦｌ，ΔｒＦｒ，ΔｒＲｌ，ΔｒＲｒ，ΔＴＦ，ΔＴＲを用いて、
【００２７】
【数７】
【００２８】
と表現できるので、関係式(１３)を関係式(１２)に代入することにより、誤差量ΔｒＦｌ，ΔｒＦｒ，ΔｒＲｌ，ΔｒＲｒ，ΔＴＦ，ΔＴＲが満たすべき関係式が得られる。なお、非線形逐次最小二乗法および拡張カルマンフィルタについては、それぞれ上記非特許文献２，３などを参照のこと。
【００２９】
車両位置姿勢演算器８は、トレッド・車輪半径推定器７から出力される後輪(左後、右後)車輪半径およびリアトレッドの推定値ｒＲｌ†，ｒＲｒ†，ＴＲ†と、後輪(左後、右後)の車輪速センサ４，５からそれぞれ出力される各パルス信号ＰＲｌ，ＰＲｒから、車両の平面位置および姿勢を演算する。図２を参照しながら、その原理を説明する。車両の運動学的モデルより、後輪軸中心の位置ｘ，ｙは関係式
【００３０】
【数８】
【００３１】
を満たす((ドット)ｘ，(ドット)ｙはｘ，ｙ座標の値の単位時間当たりの変化量)。そこで、関係式(６)，(７)を関係式(１４)に代入して得られる関係式および関係式(６)をサンプリング時間Ｔ(０＜Ｔ＜＜１)で離散近似すると、後輪軸中心の位置ｘ(ｎＴ)，ｙ(ｎＴ)および姿勢角(ヨー角)γ(ｎＴ)が満たすべき差分方程式
【００３２】
【数９】
【００３３】
が得られる(上記非特許文献１参照)。ただし、
【００３４】
【数１０】
【００３５】
である。さらに、ここで考えている定常運動状態では、車両加速度は十分小さいので、
【００３６】
【数１１】
【００３７】
と近似できるので、関係式(１０)より、
【００３８】
【数１２】
【００３９】
が成り立つが、後輪車輪半径およびリアトレッドの真値は未知であるので、それらをトレッド・車輪半径推定器７からの出力値、すなわち後輪車輪半径およびリアトレッドの推定値ｒＲｌ†，ｒＲｒ†，ＴＲ†に置き換えると、関係式(１９)が得られる。
【００４０】
【数１３】
【００４１】
よって、車両運動を記述する慣性座標系の設定に応じて、初期値を適切に設定することにより、任意の慣性座標系における位置ｘ(ｎＴ)，ｙ(ｎＴ)および姿勢角(ヨー角)γ(ｎＴ)は、後輪車輪半径およびリアトレッドの推定値ｒＲｌ†，ｒＲｒ†，ＴＲ†とパルス信号ＰＲｌ，ＰＲｒから、関係式(１５)と(１９)に基づいて演算できる。
【００４２】
以上のような構成によれば、トレッド・車輪半径推定器７がオンラインで車輪半径およびトレッドを推定しつつ、その結果に基づき車両位置姿勢演算器８が車両の位置および姿勢を演算するので、リアトレッドあるいは後輪車輪径が変化しノミナル値と異なるといった場合でも、車両の位置および姿勢を高精度に測定できる。
【００４３】
実施の形態２．
図３はこの発明の実施の形態２による車両の位置姿勢測定装置の構成図である。この装置は、車両の各車輪の車輪速を検出するための車輪速センサ(左前輪)２，車輪速センサ(右前輪)３，車輪速センサ(左後輪)４，車輪速センサ(右後輪)５以外のセンサを必要としない。この構成によれば、図１に示したステアリング角センサ１と実舵角演算器６が不要となるほか、ステアリング機構に基づく検出誤差による車両の位置姿勢測定精度の悪化が生じない。
【００４４】
図３のトレッド・車輪半径推定器７ａの原理を説明する。関係式(１２)よりδＦｌ，δＦｒを消去すると、
【００４５】
【数１４】
【００４６】
が成り立つ。この関係式(２０)から導出できる車輪半径およびトレッドの真値とノミナル値との誤差量が満たす関係式に対して、非線形逐次最小二乗法、拡張カルマンフィルタなどのパラメータ推定手法を適用すると、パルス数ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒのみから、ノミナル値に誤差補正を加えた車輪半径、またはトレッド、または車輪半径およびトレッドの両方のそれぞれ推定値を得ることができる。
【符号の説明】
【００４７】
１ ステアリング角センサ(ステアリング角検出手段)、２，３，４，５ 車輪速センサ(車輪速検出手段)、６ 実舵角演算器(実舵角演算手段)、７，７ａ トレッド・車輪半径推定器(トレッド・車輪半径推定手段)、８ 車両位置姿勢演算器(車両位置姿勢演算手段)、１１，１２，１３，１４ 車輪。
【技術分野】
【０００１】
この発明は、任意の慣性座標系における車両の位置・姿勢を測定するための車両の位置姿勢測定装置に関するものである。
【背景技術】
【０００２】
従来の車両の位置姿勢測定装置では、車輪径が変化しないと仮定して非操舵左右輪の回転数から求めた移動距離差および平均移動距離と、さらにトレッドも変化しないとする仮定により、車両の位置および姿勢を測定していた(例えば特許文献１、非特許文献１)。
さらに、車輪の回転数の合計および／または二つの車輪の回転数差および／または車輪の所定の回転に対する時間ないし回転状態もしくは回転状態に達するまでの時間に依存した補正係数を導入することにより、カーブ走行中などに生じるトレッドの変化に対応した位置姿勢測定方式も提案されている(例えば特許文献２)。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開昭６１−２７４２１４号公報
【特許文献２】特表昭５９−５０２１５３号公報
【非特許文献】
【０００４】
【非特許文献１】C. Ming Wang著、”Location Estimation and Uncertainty Analysis for Mobile Robots”、IEEE International Conference on Robotics and Automation、pp.1230-1235, 1988(第２節)
【非特許文献２】馬屋原将明 他著、”非線形逐次最小２乗法に基づく耐クロストークノイズキャンセラ”、電子情報通信学会論文誌Ａ、ｐｐ．１６２−１６９、２００２」
【非特許文献３】谷萩隆嗣 著、”カルマンフィルタと適応信号処理”、ｐｐ．４２−５１、コロナ社、２００５年１２月
【発明の概要】
【発明が解決しようとする課題】
【０００５】
このような車両の位置姿勢測定装置では、タイヤ空気圧の左右差などを要因として、非操舵左右輪の有効径に差異がある場合に、左右輪の移動距離演算に誤差が生じ、その結果位置・姿勢の測定精度が悪化するという問題点があった。
【０００６】
この発明は、上記のような問題点を解決するためになされたものであり、車両の位置姿勢を精度良く測定する車両の位置姿勢測定装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
この発明は、前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、ステアリング角を検出するステアリング角検出手段と、車両の前後左右四輪の車輪速を検出する車輪速検出手段と、前記ステアリング角検出手段からの出力に基づいて実舵角を演算する実舵角演算手段と、前記実舵角と前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値を演算するトレッド・車輪半径推定手段と、前記推定値と後輪の前記車輪速に基づき車両の位置および姿勢を演算する車両位置姿勢演算手段と、を備え、任意の慣性座標系に対する車両の位置および姿勢を測定することを特徴とする車両の位置姿勢測定装置等にある。
【発明の効果】
【０００８】
この発明によれば、前記トレッド・車輪半径推定手段を備えたことにより、走行中にリアトレッドが変化した場合だけでなく、タイヤ空気圧の減少などによって車輪径が変化した場合でも、車両の位置および姿勢を高精度に測定できる。
【図面の簡単な説明】
【０００９】
【図１】この発明の実施の形態１による車両の位置姿勢測定装置の構成図である。
【図２】この発明の原理を説明するための説明図である。
【図３】この発明の実施の形態２による車両の位置姿勢測定装置の構成図である。
【発明を実施するための形態】
【００１０】
以下、この発明による車両の位置姿勢測定装置を各実施の形態に従って図面を用いて説明する。なお、各実施の形態においては、前輪を操舵する車両の位置姿勢測定を行う装置の例を示しており、また同一もしくは相当部分は同一符号で示し、重複する説明は省略する。
【００１１】
実施の形態１．
図１はこの発明の実施の形態１による車両の位置姿勢測定装置の構成図である。この車両の位置姿勢測定装置は、ステアリング角θを検出するためのステアリング角センサ１と、車両の各車輪の車輪速を検出するために車輪速センサ２(左前輪)，車輪速センサ３(右前輪)，車輪速センサ４(左後輪)，車輪速センサ５(右後輪)を有している。車輪速センサ２，３，４，５は、それぞれ対応する車輪が所定角度回転する毎に検出パルスを出力する公知の構成の車輪速センサである。
【００１２】
実舵角演算器６は、ステアリング角センサ１により検出されたステアリング角θを予め実験によって求めた関係式(１)に代入し、実舵角δＦｌ，δＦｒを演算する。
【００１３】
【数１】
【００１４】
トレッド・車輪半径推定器７は、実舵角演算器６から出力される実舵角δＦｌ，δＦｒと車輪速センサ２，３，４，５からそれぞれ出力される各パルス信号ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒから、フロントトレッドＴＲ，リアトレッドＴＲ，左前車輪半径ｒＦｌ，右前車輪半径ｒＦｒ，左後車輪半径ｒＲｌ，右後車輪半径ｒＲｒを推定する。図２を参照しながら、その原理を説明する。車両の左前車輪１１，右前車輪１２，左後車輪１３，右後車輪１４の横滑り角が全て０であると仮定すると、
【００１５】
【数２】
【００１６】
が成り立つ。ただし、
ｖＦｌ，ｖＦｒ，ｖＲｌ，ｖＲｒ：それぞれ車輪１１，１２，１３，１４の移動速度
ｖｘ，ｖｙ：それぞれ重心速度のｘ軸成分，ｙ軸成分(慣性座標系は任意)
(ドット)γ：車両の姿勢角(ヨー角：基準座標ｘｙ上の車両方向のｘ軸とのなす角度)γの単位時間当たりの変化量(角速度)
ＬＦ：前輪軸中心(左前車輪１１と右前車輪１２を結ぶ線の中心)と車両重心ｃ.ｇ.との間の長さ
ＬＲ：後輪軸中心(左後車輪１３と右後車輪１４を結ぶ線の中心)と車両重心ｃ.ｇ.との間の長さ
である。例えば、後退駐車時のように、車両が低速で定常運動を行なっていると考えられる場合においては、このような仮定が成立する。
【００１７】
さらに、上式を整理すると、
【００１８】
【数３】
【００１９】
が得られる。Ｌはホイルベースを示す。ところで、各車輪速センサ２，３，４，５からの出力パルス数ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒと各車輪１１，１２，１３，１４の移動速度ｖＦｌ，ｖＦｒ，ｖＲｌ，ｖＲｒとの間には、関係式
【００２０】
【数４】
【００２１】
が成り立つ。ただし、ＮＦｌ，ＮＦｒ，ＮＲｌ，ＮＲｒは各車輪が１回転したときのパルス数であり、Ｔはサンプリング時間、ｎは正の整数、である。したがって、ここで考えている定常運動状態では、操舵速度は十分小さいので、
【００２２】
【数５】
【００２３】
と仮定できるので((ドット)δＦｌ，(ドット)δＦｒはそれぞれ実舵角δＦｌ，δＦｒの単位時間当たりの変化量(実舵角速度))、(９)，(１０)より、
【００２４】
【数６】
【００２５】
が成り立つ。そこで、この関係式(１２)から、車輪半径およびトレッドの走行中の実際の値である真値とメーカが公表しているノミナル値(公称値)との誤差量が満たす関係式を導出し、その関係式に対して非線形逐次最小二乗法、拡張カルマンフィルタなどを用いたパラメータ推定手法を適用することで、実舵角δＦｌ，δＦｒとパルス数ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒから、ノミナル値に誤差補正を加えた車輪半径およびトレッドの推定値を得ることができる。
【００２６】
例えば、車輪半径およびトレッドの真値ｒＦｌ，ｒＦｒ，ｒＲｌ，ｒＲｒ，ＴＦ，ＴＲは、ノミナル値ｒ＊Ｆｌ，ｒ＊Ｆｒ，ｒ＊Ｒｌ，ｒ＊Ｒｒ，Ｔ＊Ｆ，Ｔ＊Ｒとスケールファクタ誤差ΔｒＦｌ，ΔｒＦｒ，ΔｒＲｌ，ΔｒＲｒ，ΔＴＦ，ΔＴＲを用いて、
【００２７】
【数７】
【００２８】
と表現できるので、関係式(１３)を関係式(１２)に代入することにより、誤差量ΔｒＦｌ，ΔｒＦｒ，ΔｒＲｌ，ΔｒＲｒ，ΔＴＦ，ΔＴＲが満たすべき関係式が得られる。なお、非線形逐次最小二乗法および拡張カルマンフィルタについては、それぞれ上記非特許文献２，３などを参照のこと。
【００２９】
車両位置姿勢演算器８は、トレッド・車輪半径推定器７から出力される後輪(左後、右後)車輪半径およびリアトレッドの推定値ｒＲｌ†，ｒＲｒ†，ＴＲ†と、後輪(左後、右後)の車輪速センサ４，５からそれぞれ出力される各パルス信号ＰＲｌ，ＰＲｒから、車両の平面位置および姿勢を演算する。図２を参照しながら、その原理を説明する。車両の運動学的モデルより、後輪軸中心の位置ｘ，ｙは関係式
【００３０】
【数８】
【００３１】
を満たす((ドット)ｘ，(ドット)ｙはｘ，ｙ座標の値の単位時間当たりの変化量)。そこで、関係式(６)，(７)を関係式(１４)に代入して得られる関係式および関係式(６)をサンプリング時間Ｔ(０＜Ｔ＜＜１)で離散近似すると、後輪軸中心の位置ｘ(ｎＴ)，ｙ(ｎＴ)および姿勢角(ヨー角)γ(ｎＴ)が満たすべき差分方程式
【００３２】
【数９】
【００３３】
が得られる(上記非特許文献１参照)。ただし、
【００３４】
【数１０】
【００３５】
である。さらに、ここで考えている定常運動状態では、車両加速度は十分小さいので、
【００３６】
【数１１】
【００３７】
と近似できるので、関係式(１０)より、
【００３８】
【数１２】
【００３９】
が成り立つが、後輪車輪半径およびリアトレッドの真値は未知であるので、それらをトレッド・車輪半径推定器７からの出力値、すなわち後輪車輪半径およびリアトレッドの推定値ｒＲｌ†，ｒＲｒ†，ＴＲ†に置き換えると、関係式(１９)が得られる。
【００４０】
【数１３】
【００４１】
よって、車両運動を記述する慣性座標系の設定に応じて、初期値を適切に設定することにより、任意の慣性座標系における位置ｘ(ｎＴ)，ｙ(ｎＴ)および姿勢角(ヨー角)γ(ｎＴ)は、後輪車輪半径およびリアトレッドの推定値ｒＲｌ†，ｒＲｒ†，ＴＲ†とパルス信号ＰＲｌ，ＰＲｒから、関係式(１５)と(１９)に基づいて演算できる。
【００４２】
以上のような構成によれば、トレッド・車輪半径推定器７がオンラインで車輪半径およびトレッドを推定しつつ、その結果に基づき車両位置姿勢演算器８が車両の位置および姿勢を演算するので、リアトレッドあるいは後輪車輪径が変化しノミナル値と異なるといった場合でも、車両の位置および姿勢を高精度に測定できる。
【００４３】
実施の形態２．
図３はこの発明の実施の形態２による車両の位置姿勢測定装置の構成図である。この装置は、車両の各車輪の車輪速を検出するための車輪速センサ(左前輪)２，車輪速センサ(右前輪)３，車輪速センサ(左後輪)４，車輪速センサ(右後輪)５以外のセンサを必要としない。この構成によれば、図１に示したステアリング角センサ１と実舵角演算器６が不要となるほか、ステアリング機構に基づく検出誤差による車両の位置姿勢測定精度の悪化が生じない。
【００４４】
図３のトレッド・車輪半径推定器７ａの原理を説明する。関係式(１２)よりδＦｌ，δＦｒを消去すると、
【００４５】
【数１４】
【００４６】
が成り立つ。この関係式(２０)から導出できる車輪半径およびトレッドの真値とノミナル値との誤差量が満たす関係式に対して、非線形逐次最小二乗法、拡張カルマンフィルタなどのパラメータ推定手法を適用すると、パルス数ＰＦｌ，ＰＦｒ，ＰＲｌ，ＰＲｒのみから、ノミナル値に誤差補正を加えた車輪半径、またはトレッド、または車輪半径およびトレッドの両方のそれぞれ推定値を得ることができる。
【符号の説明】
【００４７】
１ ステアリング角センサ(ステアリング角検出手段)、２，３，４，５ 車輪速センサ(車輪速検出手段)、６ 実舵角演算器(実舵角演算手段)、７，７ａ トレッド・車輪半径推定器(トレッド・車輪半径推定手段)、８ 車両位置姿勢演算器(車両位置姿勢演算手段)、１１，１２，１３，１４ 車輪。
【特許請求の範囲】
【請求項１】
前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、
ステアリング角を検出するステアリング角検出手段と、
車両の前後左右四輪の車輪速を検出する車輪速検出手段と、
前記ステアリング角検出手段からの出力に基づいて実舵角を演算する実舵角演算手段と、
前記実舵角と前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値を演算するトレッド・車輪半径推定手段と、
前記推定値と後輪の前記車輪速に基づき車両の位置および姿勢を演算する車両位置姿勢演算手段と、
を備え、任意の慣性座標系に対する車両の位置および姿勢を測定することを特徴とする車両の位置姿勢測定装置。
【請求項２】
前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、
車両の前後左右四輪の車輪速を検出する車輪速検出手段と、
前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値を演算するトレッド・車輪半径推定手段と、
前記推定値と後輪の前記車輪速に基づき車両の位置および姿勢を演算する車両位置姿勢演算手段と、
を備え、任意の慣性座標系に対する車両の位置および姿勢を測定することを特徴とする車両の位置姿勢測定装置。
【請求項１】
前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、
ステアリング角を検出するステアリング角検出手段と、
車両の前後左右四輪の車輪速を検出する車輪速検出手段と、
前記ステアリング角検出手段からの出力に基づいて実舵角を演算する実舵角演算手段と、
前記実舵角と前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値を演算するトレッド・車輪半径推定手段と、
前記推定値と後輪の前記車輪速に基づき車両の位置および姿勢を演算する車両位置姿勢演算手段と、
を備え、任意の慣性座標系に対する車両の位置および姿勢を測定することを特徴とする車両の位置姿勢測定装置。
【請求項２】
前輪を操舵する車両の位置および姿勢を検出するための車両の位置姿勢測定装置であって、
車両の前後左右四輪の車輪速を検出する車輪速検出手段と、
前記車輪速に基づきリアトレッドおよび後輪車輪半径のそれぞれの推定値を演算するトレッド・車輪半径推定手段と、
前記推定値と後輪の前記車輪速に基づき車両の位置および姿勢を演算する車両位置姿勢演算手段と、
を備え、任意の慣性座標系に対する車両の位置および姿勢を測定することを特徴とする車両の位置姿勢測定装置。
【図１】
【図２】
【図３】
【図２】
【図３】
【公開番号】特開２０１２−２５５６８６（Ｐ２０１２−２５５６８６Ａ）
【公開日】平成２４年１２月２７日（２０１２．１２．２７）
【国際特許分類】
【出願番号】特願２０１１−１２８０３６（Ｐ２０１１−１２８０３６）
【出願日】平成２３年６月８日（２０１１．６．８）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２４年１２月２７日（２０１２．１２．２７）
【国際特許分類】
【出願日】平成２３年６月８日（２０１１．６．８）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]