(57)【要約】【課題】比較的安価な２軸の加速度センサを用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、車両の姿勢を高精度で推定することができる装置等を提供する。【解決手段】車両姿勢推定装置１００によれば、車両１の挙動を表すパラメータとして当該車両の速度と、２軸（ロール軸およびピッチ軸）方向の加速度と、１軸（ヨー軸）まわりの角速度とが測定される。また、当該測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるためのグローバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるためのグローバル座標系の回転を表す第２演算子との合成演算子にしたがってグローバル座標系における車両の姿勢が推定される。【選択図】  図１
2009053039.tif 000002
【特許請求の範囲】
【請求項１】
車両の姿勢を推定する装置であって、
前記車両の速度と、加速度のロール軸方向成分およびピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とを測定する第１処理部と、
該第１処理部による測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるための該グローmmバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるための、該グローバル座標系の回転を表す第２演算子との合成演算子にしたがって前記車両の姿勢を算出する第２処理部とを備えていることを特徴とする車両姿勢推定装置。
【請求項２】
請求項１記載の車両姿勢推定装置において、
前記第２処理部が、前記第１処理部による測定結果のうち前記車両の速度と、加速度のロール軸方向成分と、前記車両のヨー軸まわりの角速度とに基づき、前記第１演算子にしたがってグローバル座標系のｚ軸に対する車両座標系のヨー軸の姿勢を第１姿勢として算出し、前記第１姿勢と、前記第１処理部による測定結果のうち前記車両の速度と、加速度のピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とに基づき、前記第２演算子にしたがって、ｚ軸とヨー軸とが一致している状態でグローバル座標系のｘ軸およびｙ軸のそれぞれに対する車両座標系のロール軸およびピッチ軸のそれぞれの姿勢を第２姿勢として算出することを特徴とする車両姿勢推定装置。
【請求項３】
請求項１または２記載の車両姿勢推定装置において、
前記演算処理部が、前記第１演算子および前記第２演算子としてのクォータニオンにしたがって前記車両の姿勢を算出することを特徴とする車両姿勢推定装置。
【請求項４】
車両の姿勢を推定する方法であって、
前記車両の速度と、加速度のロール軸方向成分およびピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とを測定する第１処理と、
該第１処理における測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるための該グローバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるための、該グローバル座標系の回転を表す第２演算子との合成演算子にしたがって前記車両の姿勢を算出する第２処理とを実行することを特徴とする車両姿勢推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両の姿勢を推定する装置等に関する。
【背景技術】
【０００２】
ナビゲーション装置にマップの上に正確な車両の位置を表示させる等のため、車両位置の測定精度向上を図る手法が提案されている。たとえば、車速センサおよび３軸加速度センサの出力に基づいて車両が走行する道路の傾斜角度が算出され、当該算出角度に基づいてジャイロセンサの出力が補正されることにより、道路勾配による影響を解消して車両の位置測定精度の向上が図られている（たとえば、特許文献１参照）。
【特許文献１】特開平９−４２９７９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかし、前記従来技術では車両の速度と、３軸方向の加速度と、１軸まわりの角速度との測定結果に基づいて車両の姿勢が求められているが、高価な３軸方向の加速度センサを用いており、コストの観点から好ましくない。
【０００４】
そこで、本発明は、比較的安価な２軸の加速度センサを用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、車両の姿勢を高精度で推定することができる装置等を提供することを解決課題とする。
【課題を解決するための手段】
【０００５】
第１発明の車両姿勢推定装置は、前記車両の速度と、加速度のロール軸方向成分およびピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とを測定する第１処理部と、該第１処理部による測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるための該グローバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるための、該グローバル座標系の回転を表す第２演算子との合成演算子にしたがって前記車両の姿勢を算出する第２処理部とを備えていることを特徴とする。
【０００６】
第１発明の車両姿勢推定装置によれば、車両の挙動を表すパラメータとして当該車両の速度と、２軸（ロール軸およびピッチ軸）方向の加速度と、１軸（ヨー軸）まわりの角速度とが測定される。すなわち、１軸（ヨー軸）方向の加速度の測定が不要となる分だけ測定対象の数を前記従来技術よりも減らすことができる。また、当該測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるためのグローバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるためのグローバル座標系の回転を表す第２演算子との合成演算子にしたがってグローバル座標系における車両の生成が推定される。したがって、比較的安価な２軸の加速度センサを用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、グローバル座標系における車両の姿勢を高精度で推定することができる。
【０００７】
第２発明の車両姿勢推定装置は、第１発明の車両姿勢推定装置において、前記第２処理部が、前記第１処理部による測定結果のうち前記車両の速度と、加速度のロール軸方向成分と、前記車両のヨー軸まわりの角速度とに基づき、前記第１演算子にしたがってグローバル座標系のｚ軸に対する車両座標系のヨー軸の姿勢を第１姿勢として算出し、前記第１姿勢と、前記第１処理部による測定結果のうち前記車両の速度と、加速度のピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とに基づき、前記第２演算子にしたがって、ｚ軸とヨー軸とが一致している状態でグローバル座標系のｘ軸およびｙ軸のそれぞれに対する車両座標系のロール軸およびピッチ軸のそれぞれの姿勢を第２姿勢として算出することを特徴とする。
【０００８】
第２発明の車両姿勢推定装置によれば、第１姿勢が算出された後、第１姿勢に基づいて第２姿勢がグローバル座標系に対する車両の姿勢として算出されうる。
【０００９】
第３発明の車両姿勢推定装置は、第１または第２発明の車両姿勢推定装置において、前記第２処理部が、前記第１演算子および前記第２演算子としてのクォータニオンにしたがって前記車両の姿勢を算出することを特徴とする。
【００１０】
第３発明の車両姿勢推定装置によれば、クォータニオンの採用によって第２処理量を低下させながら車両の姿勢が高精度で算出されうる。
【００１１】
第４発明の車両姿勢推定方法は、前記車両の加速度のロール軸方向成分およびピッチ軸方向成分と、ヨー軸まわりの角速度とを測定する第１処理と、該第１処理における測定結果に基づき、グローバル座標系のｚ軸を車両座標系のヨー軸に一致させるための第１演算子と、該グローバル座標系のｚ軸と該車両座標系のヨー軸とが一致した状態で、該グローバル座標系のｘ軸およびｙ軸のそれぞれを該車両座標系のロール軸およびピッチ軸のそれぞれに一致させるための第２演算子との合成演算子にしたがって前記車両の姿勢を算出する第２処理とを実行することを特徴とする。
【００１２】
第４発明の方法によれば、測定対象となる車両挙動を表すパラメータの数を前記従来技術よりも減らしながら、車両の姿勢を高精度で推定することができる。
【発明を実施するための最良の形態】
【００１３】
本発明の車両姿勢推定装置および方法の実施形態について図面を用いて説明する。
【００１４】
図１および図２に示されている車両１には車両制御装置１０と、速度センサ１０１と、２軸加速度センサ１０２と、１軸ジャイロセンサ１０３とが搭載されている。車両制御装置１０は車両姿勢推定装置１００を備えている。車両姿勢推定装置１００は第１処理部１１０と、演算子格納部１１２と、第２処理部１２０とを備えている。
【００１５】
以下、パラメータに用いる添字「ｇ」は当該パラメータがグローバル座標系におけるパラメータであることを表し、添字「ｃ」は当該パラメータが車両座標系におけるパラメータであることを表す。
【００１６】
速度センサ１０１は車両１のロール軸方向（ｘc方向）の速度ｖxc［ｋ］に応じた信号を出力する。２軸加速度センサ１０２は車両１の加速度αのロール軸方向成分αxc［ｋ］およびピッチ軸方向成分（ｙc方向成分）αyc［ｋ］に応じた信号を出力する。１軸ジャイロセンサ１０３は車両１のヨー軸（ｚc軸）まわりの角速度ωzc［ｋ］に応じた信号を出力する。
【００１７】
第１処理部１２１は速度センサ１０１、２軸加速度センサ１０２および１軸ジャイロセンサ１０３のそれぞれからの出力信号に基づき、車両１のロール軸方向の速度ｖxc［ｋ］と、加速度αのロール軸方向成分αxc［ｋ］およびピッチ軸方向成分αyc［ｋ］と、ヨー軸まわりの角速度ωzc［ｋ］とを測定する。第２処理部１２０は第１処理部１１０による測定結果に基づき、演算子格納部１１２に格納されている第１演算子および第２演算子にしたがってグローバル座標系における車両１の姿勢を推定する。
【００１８】
前記構成の車両姿勢推定装置による車両１の姿勢推定方法について説明する。
【００１９】
まず、第１処理部１１０が車両１の挙動を表すパラメータ測定する「第１処理」を実行する。具体的には、第１処理部１１０は車速センサ１０１からの出力に基づき、時刻ｋにおける車両１のロール軸方向の速度ｖxc［ｋ］を測定する（図３／Ｓ０１２）。第１処理部１１０は２軸加速度センサ１０２からの出力信号に基づき、時刻ｋにおける車両１の加速度αのロール軸方向成分αxc［ｋ］を測定し（図３／Ｓ０１４）、かつ、時刻ｋにおける車両１の加速度αのｙ方向成分αyc［ｋ］を測定する（図３／Ｓ０１６）。第１処理部１１０は１軸ジャイロセンサ１０３からの出力信号に基づき、時刻ｋにおける車両１のヨー軸周りの角速度ωzc［ｋ］を測定する（図３／Ｓ０１８）。
【００２０】
続いて、第２処理部１２０は第１処理部１１０による測定結果に基づき、グローバル座標系における車両１の姿勢を推定する「第２処理」を実行する。
【００２１】
具体的には、第２処理部１２０がグローバル座標系のｚg方向に対する時刻ｋにおける車両座標系のｘc方向の傾斜角度θ［ｋ］を算出する（図３／Ｓ０２１）。当該傾斜角度θ［ｋ］は、時刻ｋにおける車両１の加速度のロール軸方向成分αxc［ｋ］と、時刻ｋにおける車両１のロール軸方向の速度ｖxc［ｋ］の時間変化率ｖxc’［ｋ］と、転心からジャイロセンサ１０３までのｙ方向の距離ｌyと、時刻ｋにおけるヨー軸まわりの角速度ωzc［ｋ］の時間変化率ωzc’［ｋ］とに基づき、関係式（１）にしたがって算出される。関係式（１）は、図４（ａ）に示されているように車両１の加速度のｘc方向成分αxc［ｋ］は、重力、遠心力、加減速による慣性力および転心力のそれぞれのｘc方向成分の和に等しいことを表している（ここでは、遠心力のｘc方向成分は０である）。
【００２２】
【数１】
2009053039.tif 000003
【００２３】
また、第２処理部１２０はグローバル座標系のｚg方向に対する時刻ｋにおける車両座標系のｙc方向の傾斜角度φ［ｋ］を算出する（図３／Ｓ０２２）。当該傾斜角度φ［ｋ］は、時刻ｋにおける車両１の加速度のｙ方向成分αyc［ｋ］と、時刻ｋにおける車両１の速度のｘ方向成分ｖyc［ｋ］と、転心からヨーレートセンサ１１３までのｘ方向の距離ｌxと、時刻ｋにおけるヨーレートωzc［ｋ］と、その時間変化率ωzc’［ｋ］とに基づき、関係式（２）にしたがって算出される。関係式（２）は、図４（ｂ）に示されているように車両１の加速度のｙc方向成分αxc［ｋ］は、重力、遠心力、加減速による慣性力および転心力のそれぞれのｙc方向成分の和に等しいことを表している（ここでは、加減速による慣性力のｙc方向成分は０である）。
【００２４】
【数２】
2009053039.tif 000004
【００２５】
さらに、第２処理部１２０がグローバル座標系のｚg方向に対する時刻ｋにおける車両座標系のｚc方向の傾斜角度ψ［ｋ］を算出する（図３／Ｓ０２３）。当該傾斜角度ψ［ｋ］は、図４（ｃ）に示されている幾何学的関係に基づいて算出される。具体的には、ｚc方向の傾斜角度ψ［ｋ］は、グローバル座標系のｚg方向に対する時刻ｋにおける車両座標系のｘc方向およびｙc方向のそれぞれの傾斜角度θ［ｋ］およびφ［ｋ］に基づき、式（３）にしたがって算出される。
【００２６】
【数３】
2009053039.tif 000005
【００２７】
そして、第２処理部１２０は、第１処理部１１０の測定結果に基づく前記算出結果と、演算子格納部１１２に格納されている第１演算子としての第１クォータニオンｑｔ1［ｋ］とに基づき、式（４）にしたがって車両１の第１姿勢ｐｔ1i［ｋ］（ｉ＝ｘc，ｙc，ｚc）を算出する（図３／Ｓ０２４）。
【００２８】
【数４】
2009053039.tif 000006
【００２９】
第１クォータニオンｑｔ1［ｋ］はグローバル座標系のｚg軸を車両座標系のｚc軸に一致させるための、グローバル座標系のｚg軸の回転を表している。この回転は、グローバル座標系のｚg軸と、車両座標系のｚc軸とを含む平面の単位法線ベクトルｎ＝（ｎx，ｎy，ｎz）まわりの回転である。第１クォータニオンｑｔ1［ｋ］はグローバル座標系のｚg方向に対する時刻ｋにおける車両座標系のｚc方向の傾斜角度ψ［ｋ］に基づいて式（５）のように表現される。
【００３０】
【数５】
2009053039.tif 000007
【００３１】
さらに、第２処理部１２がグローバル座標系における車両１のｚg軸まわりのヨーレートω［ｋ］を算出する（図３／Ｓ０２５）。図５に示されているように車両座標系のヨー軸またはｚc方向が、グローバル座標系のｚg方向から角度ψ［ｋ］だけ傾斜していると、ヨーレートセンサ１１３の感度がｃｏｓψ［ｋ］だけ低下する。そこで、ヨーレートセンサ１１３の出力に基づくｚc軸まわりのヨーレートωcz［ｋ］が、グローバル座標系のｚg方向に対する車両座標系のｚc方向の傾斜角度ψ［ｋ］に基づき、関係式（６）にしたがって補正されることにより、グローバル座標系における車両１のｚg軸まわりの（本来の）ヨーレートω［ｋ］が算出される。
【００３２】
【数６】
2009053039.tif 000008
【００３３】
また、第２処理部１２０がグローバル座標系のｘg方向に対する、時刻ｋにおける車両座標系のｘc方向の傾斜角度ξ［ｋ］を算出する（図３／Ｓ０２６）。当該傾斜角度ξ［ｋ］は、先の時刻ｋ−１における当該傾斜角度ξ［ｋ−１］（ξ［０］＝０）と、時刻ｋにおける車両１の角速度ω［ｋ］とに基づき、式（７）にしたがって算出される。
【００３４】
【数７】
2009053039.tif 000009
【００３５】
そして、第２処理部１２０は、第１処理部１１０の測定結果に基づく第１姿勢等の前記算出結果と、演算子格納部１１２に格納されている第２演算子としての第２クォータニオンｑｔ2［ｋ］とに基づき、式（８）にしたがって車両１の第２姿勢ｐｔ2i［ｋ］（ｉ＝ｘc，ｙc，ｚc）を算出する（図３／Ｓ０２８）。
【００３６】
【数８】
2009053039.tif 000010
【００３７】
第２クォータニオンｑｔ2［ｋ］はグローバル座標系のｘg軸およびｙg軸のそれぞれを車両座標系のｘc軸およびｙc軸のそれぞれに一致させるための、グローバル座標系の回転を表している。この回転は、グローバル座標系のｚg方向の単位ベクトル（０，０，１）まわりの回転である。第２クォータニオンｑｔ2［ｋ］はグローバル座標系のｘg方向に対する、時刻ｋにおける車両座標系のｘc方向の傾斜角度ξ［ｋ］に基づいて式（９）により表現される。
【００３８】
【数９】
2009053039.tif 000011
【００３９】
そして、第２姿勢ｐｔ2i［ｋ］の算出結果がグローバル座標系における、時刻ｋにおける車両１の姿勢として推定される。
【００４０】
前記機能を発揮する車両姿勢推定装置によれば、車両１の挙動を表すパラメータとして当該車両１の速度ｖxc［ｋ］と、２軸方向の加速度（αxc［ｋ］およびαyc［ｋ］）と、１軸（ヨー軸）まわりの角速度ωzc［ｋ］とが測定される。すなわち、１軸（ヨー軸）方向の加速度αzc［ｋ］の測定が不要となる分だけ測定対象の数を前記従来技術よりも減らすことができる（図３／Ｓ０１２，Ｓ０１４，Ｓ０１６，Ｓ０１８参照）。また、当該測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるためのグローバル座標系の回転を表す第１クォータニオンｑｔ1［ｋ］と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるためのグローバル座標系の回転を表す第２クォータニオンｑｔ2［ｋ］との合成演算子にしたがってグローバル座標系における車両の生成が推定される（図３／Ｓ０２１〜Ｓ０２８参照）。したがって、３軸加速度センサよりも比較的安価な２軸加速度センサ１０２を用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、グローバル座標系における車両１の姿勢を高精度で推定することができる。
【００４１】
なお、前記実施形態では第１姿勢ｐｔ1i［ｋ］が算出され、第１姿勢ｐｔ1i［ｋ］に基づいて第２姿勢ｐｔ2i［ｋ］が算出されたが（図３／Ｓ０２４，Ｓ０２７参照）、第１クォータニオンｑｔ1［ｋ］および第２クォータニオンｑｔ2［ｋ］の合成クォータニオンｑｔ［ｋ］＝ｑｔ1［ｋ］・ｑｔ2［ｋ］にしたがって、
クォータニオンに代えて行列が演算子として用いられて車両１の姿勢が推定されてもよい。具体的には、第１回転行列Ｑ2［ｋ］および第２回転行列Ｑ2［ｋ］のそれぞれを用いて、式（１０）にしたがって時刻ｋにおける車両１の姿勢Ｐ［ｋ］が推定される。
【００４２】
【数１０】
2009053039.tif 000012
【００４３】
第１回転行列Ｑ1［ｋ］はグローバル座標系のｚg軸を、車両座標系のｚc軸に一致させるためのグローバル座標系のｙg軸まわりの回転を表している。第１回転行列Ｑ1［ｋ］は式（１１）により表現される。
【００４４】
【数１１】
2009053039.tif 000013
【００４５】
第２回転行列Ｑ2［ｋ］はグローバル座標系のｘg軸およびｙg軸のそれぞれを車両座標系のｘc軸およびｙc軸のそれぞれに一致させるための、グローバル座標系のｚg軸まわりの回転を表している。第２回転行列Ｑ2［ｋ］はグローバル座標系のｘg方向に対する、時刻ｋにおける車両座標系のｘc方向の傾斜角度ξ［ｋ］に基づいて式（１２）により表現される。
【００４６】
【数１２】
2009053039.tif 000014
【００４７】
当該実施形態によれば、前記実施形態と同様に３軸加速度センサよりも比較的安価な２軸加速度センサ１０２を用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、グローバル座標系における車両１の姿勢を高精度で推定することができる。
【図面の簡単な説明】
【００４８】
【図１】車両姿勢推定装置の構成に関する説明図
【図２】車両姿勢推定装置の構成に関する説明図
【図３】車両姿勢推定方法に関する説明図
【図４】グローバル座標系と車両座標系との関係に関する説明図
【図５】ヨーレート補正に関する説明図
【符号の説明】
【００４９】
１‥車両、１０‥車両制御装置、１００‥車両姿勢推定装置、１０１‥２軸加速度センサ、１０２‥１軸ジャイロセンサ、１１０‥第１処理部、１１２‥演算子格納部、１２０‥第２処理部
【図１】
2009053039.tif 000015
【図２】
2009053039.tif 000016
【図３】
2009053039.tif 000017
【図４】
2009053039.tif 000018
【図５】
2009053039.tif 000019