JP2009053039A

JP2009053039A - 車両姿勢推定装置、車両姿勢推定方法

Info

Publication number: JP2009053039A
Application number: JP2007220029A
Authority: JP
Inventors: Kazunori Kanai; 一憲金井; Yukinobu Nakamura; 之信中村
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2007-08-27
Filing date: 2007-08-27
Publication date: 2009-03-12
Also published as: US20090062980A1

Abstract

【課題】比較的安価な２軸の加速度センサを用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、車両の姿勢を高精度で推定することができる装置等を提供する。
【解決手段】車両姿勢推定装置１００によれば、車両１の挙動を表すパラメータとして当該車両の速度と、２軸（ロール軸およびピッチ軸）方向の加速度と、１軸（ヨー軸）まわりの角速度とが測定される。また、当該測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるためのグローバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるためのグローバル座標系の回転を表す第２演算子との合成演算子にしたがってグローバル座標系における車両の姿勢が推定される。
【選択図】図１

Description

本発明は、車両の姿勢を推定する装置等に関する。

ナビゲーション装置にマップの上に正確な車両の位置を表示させる等のため、車両位置の測定精度向上を図る手法が提案されている。たとえば、車速センサおよび３軸加速度センサの出力に基づいて車両が走行する道路の傾斜角度が算出され、当該算出角度に基づいてジャイロセンサの出力が補正されることにより、道路勾配による影響を解消して車両の位置測定精度の向上が図られている（たとえば、特許文献１参照）。
特開平９−４２９７９号公報

しかし、前記従来技術では車両の速度と、３軸方向の加速度と、１軸まわりの角速度との測定結果に基づいて車両の姿勢が求められているが、高価な３軸方向の加速度センサを用いており、コストの観点から好ましくない。

そこで、本発明は、比較的安価な２軸の加速度センサを用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、車両の姿勢を高精度で推定することができる装置等を提供することを解決課題とする。

第１発明の車両姿勢推定装置は、前記車両の速度と、加速度のロール軸方向成分およびピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とを測定する第１処理部と、該第１処理部による測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるための該グローバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるための、該グローバル座標系の回転を表す第２演算子との合成演算子にしたがって前記車両の姿勢を算出する第２処理部とを備えていることを特徴とする。

第１発明の車両姿勢推定装置によれば、車両の挙動を表すパラメータとして当該車両の速度と、２軸（ロール軸およびピッチ軸）方向の加速度と、１軸（ヨー軸）まわりの角速度とが測定される。すなわち、１軸（ヨー軸）方向の加速度の測定が不要となる分だけ測定対象の数を前記従来技術よりも減らすことができる。また、当該測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるためのグローバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるためのグローバル座標系の回転を表す第２演算子との合成演算子にしたがってグローバル座標系における車両の生成が推定される。したがって、比較的安価な２軸の加速度センサを用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、グローバル座標系における車両の姿勢を高精度で推定することができる。

第２発明の車両姿勢推定装置は、第１発明の車両姿勢推定装置において、前記第２処理部が、前記第１処理部による測定結果のうち前記車両の速度と、加速度のロール軸方向成分と、前記車両のヨー軸まわりの角速度とに基づき、前記第１演算子にしたがってグローバル座標系のｚ軸に対する車両座標系のヨー軸の姿勢を第１姿勢として算出し、前記第１姿勢と、前記第１処理部による測定結果のうち前記車両の速度と、加速度のピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とに基づき、前記第２演算子にしたがって、ｚ軸とヨー軸とが一致している状態でグローバル座標系のｘ軸およびｙ軸のそれぞれに対する車両座標系のロール軸およびピッチ軸のそれぞれの姿勢を第２姿勢として算出することを特徴とする。

第２発明の車両姿勢推定装置によれば、第１姿勢が算出された後、第１姿勢に基づいて第２姿勢がグローバル座標系に対する車両の姿勢として算出されうる。

第３発明の車両姿勢推定装置は、第１または第２発明の車両姿勢推定装置において、前記第２処理部が、前記第１演算子および前記第２演算子としてのクォータニオンにしたがって前記車両の姿勢を算出することを特徴とする。

第３発明の車両姿勢推定装置によれば、クォータニオンの採用によって第２処理量を低下させながら車両の姿勢が高精度で算出されうる。

第４発明の車両姿勢推定方法は、前記車両の加速度のロール軸方向成分およびピッチ軸方向成分と、ヨー軸まわりの角速度とを測定する第１処理と、該第１処理における測定結果に基づき、グローバル座標系のｚ軸を車両座標系のヨー軸に一致させるための第１演算子と、該グローバル座標系のｚ軸と該車両座標系のヨー軸とが一致した状態で、該グローバル座標系のｘ軸およびｙ軸のそれぞれを該車両座標系のロール軸およびピッチ軸のそれぞれに一致させるための第２演算子との合成演算子にしたがって前記車両の姿勢を算出する第２処理とを実行することを特徴とする。

第４発明の方法によれば、測定対象となる車両挙動を表すパラメータの数を前記従来技術よりも減らしながら、車両の姿勢を高精度で推定することができる。

本発明の車両姿勢推定装置および方法の実施形態について図面を用いて説明する。

図１および図２に示されている車両１には車両制御装置１０と、速度センサ１０１と、２軸加速度センサ１０２と、１軸ジャイロセンサ１０３とが搭載されている。車両制御装置１０は車両姿勢推定装置１００を備えている。車両姿勢推定装置１００は第１処理部１１０と、演算子格納部１１２と、第２処理部１２０とを備えている。

以下、パラメータに用いる添字「ｇ」は当該パラメータがグローバル座標系におけるパラメータであることを表し、添字「ｃ」は当該パラメータが車両座標系におけるパラメータであることを表す。

速度センサ１０１は車両１のロール軸方向（ｘ_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方向成分は０である）。

また、第２処理部１２０はグローバル座標系のｚ_g方向に対する時刻ｋにおける車両座標系のｙ_c方向の傾斜角度φ［ｋ］を算出する（図３／Ｓ０２２）。当該傾斜角度φ［ｋ］は、時刻ｋにおける車両１の加速度のｙ方向成分α_yc［ｋ］と、時刻ｋにおける車両１の速度のｘ方向成分ｖ_yc［ｋ］と、転心からヨーレートセンサ１１３までのｘ方向の距離ｌ_xと、時刻ｋにおけるヨーレートω_zc［ｋ］と、その時間変化率ω_zc’［ｋ］とに基づき、関係式（２）にしたがって算出される。関係式（２）は、図４（ｂ）に示されているように車両１の加速度のｙ_c方向成分α_xc［ｋ］は、重力、遠心力、加減速による慣性力および転心力のそれぞれのｙ_c方向成分の和に等しいことを表している（ここでは、加減速による慣性力のｙ_c方向成分は０である）。

さらに、第２処理部１２０がグローバル座標系のｚ_g方向に対する時刻ｋにおける車両座標系のｚ_c方向の傾斜角度ψ［ｋ］を算出する（図３／Ｓ０２３）。当該傾斜角度ψ［ｋ］は、図４（ｃ）に示されている幾何学的関係に基づいて算出される。具体的には、ｚ_c方向の傾斜角度ψ［ｋ］は、グローバル座標系のｚ_g方向に対する時刻ｋにおける車両座標系のｘ_c方向およびｙ_c方向のそれぞれの傾斜角度θ［ｋ］およびφ［ｋ］に基づき、式（３）にしたがって算出される。

そして、第２処理部１２０は、第１処理部１１０の測定結果に基づく前記算出結果と、演算子格納部１１２に格納されている第１演算子としての第１クォータニオンｑｔ₁［ｋ］とに基づき、式（４）にしたがって車両１の第１姿勢ｐｔ_1i［ｋ］（ｉ＝ｘ_c，ｙ_c，ｚ_c）を算出する（図３／Ｓ０２４）。

第１クォータニオンｑｔ₁［ｋ］はグローバル座標系のｚ_g軸を車両座標系のｚ_c軸に一致させるための、グローバル座標系のｚ_g軸の回転を表している。この回転は、グローバル座標系のｚ_g軸と、車両座標系のｚ_c軸とを含む平面の単位法線ベクトルｎ＝（ｎ_x，ｎ_y，ｎ_z）まわりの回転である。第１クォータニオンｑｔ₁［ｋ］はグローバル座標系のｚ_g方向に対する時刻ｋにおける車両座標系のｚ_c方向の傾斜角度ψ［ｋ］に基づいて式（５）のように表現される。

さらに、第２処理部１２がグローバル座標系における車両１のｚ_g軸まわりのヨーレートω［ｋ］を算出する（図３／Ｓ０２５）。図５に示されているように車両座標系のヨー軸またはｚ_c方向が、グローバル座標系のｚ_g方向から角度ψ［ｋ］だけ傾斜していると、ヨーレートセンサ１１３の感度がｃｏｓψ［ｋ］だけ低下する。そこで、ヨーレートセンサ１１３の出力に基づくｚ_c軸まわりのヨーレートω_cz［ｋ］が、グローバル座標系のｚ_g方向に対する車両座標系のｚ_c方向の傾斜角度ψ［ｋ］に基づき、関係式（６）にしたがって補正されることにより、グローバル座標系における車両１のｚ_g軸まわりの（本来の）ヨーレートω［ｋ］が算出される。

また、第２処理部１２０がグローバル座標系のｘ_g方向に対する、時刻ｋにおける車両座標系のｘ_c方向の傾斜角度ξ［ｋ］を算出する（図３／Ｓ０２６）。当該傾斜角度ξ［ｋ］は、先の時刻ｋ−１における当該傾斜角度ξ［ｋ−１］（ξ［０］＝０）と、時刻ｋにおける車両１の角速度ω［ｋ］とに基づき、式（７）にしたがって算出される。

そして、第２処理部１２０は、第１処理部１１０の測定結果に基づく第１姿勢等の前記算出結果と、演算子格納部１１２に格納されている第２演算子としての第２クォータニオンｑｔ₂［ｋ］とに基づき、式（８）にしたがって車両１の第２姿勢ｐｔ_2i［ｋ］（ｉ＝ｘ_c，ｙ_c，ｚ_c）を算出する（図３／Ｓ０２８）。

第２クォータニオンｑｔ₂［ｋ］はグローバル座標系のｘ_g軸およびｙ_g軸のそれぞれを車両座標系のｘ_c軸およびｙ_c軸のそれぞれに一致させるための、グローバル座標系の回転を表している。この回転は、グローバル座標系のｚ_g方向の単位ベクトル（０，０，１）まわりの回転である。第２クォータニオンｑｔ₂［ｋ］はグローバル座標系のｘ_g方向に対する、時刻ｋにおける車両座標系のｘ_c方向の傾斜角度ξ［ｋ］に基づいて式（９）により表現される。

そして、第２姿勢ｐｔ_2i［ｋ］の算出結果がグローバル座標系における、時刻ｋにおける車両１の姿勢として推定される。

前記機能を発揮する車両姿勢推定装置によれば、車両１の挙動を表すパラメータとして当該車両１の速度ｖ_xc［ｋ］と、２軸方向の加速度（α_xc［ｋ］およびα_yc［ｋ］）と、１軸（ヨー軸）まわりの角速度ω_zc［ｋ］とが測定される。すなわち、１軸（ヨー軸）方向の加速度α_zc［ｋ］の測定が不要となる分だけ測定対象の数を前記従来技術よりも減らすことができる（図３／Ｓ０１２，Ｓ０１４，Ｓ０１６，Ｓ０１８参照）。また、当該測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるためのグローバル座標系の回転を表す第１クォータニオンｑｔ₁［ｋ］と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるためのグローバル座標系の回転を表す第２クォータニオンｑｔ₂［ｋ］との合成演算子にしたがってグローバル座標系における車両の生成が推定される（図３／Ｓ０２１〜Ｓ０２８参照）。したがって、３軸加速度センサよりも比較的安価な２軸加速度センサ１０２を用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、グローバル座標系における車両１の姿勢を高精度で推定することができる。

なお、前記実施形態では第１姿勢ｐｔ_1i［ｋ］が算出され、第１姿勢ｐｔ_1i［ｋ］に基づいて第２姿勢ｐｔ_2i［ｋ］が算出されたが（図３／Ｓ０２４，Ｓ０２７参照）、第１クォータニオンｑｔ₁［ｋ］および第２クォータニオンｑｔ₂［ｋ］の合成クォータニオンｑｔ［ｋ］＝ｑｔ₁［ｋ］・ｑｔ₂［ｋ］にしたがって、
クォータニオンに代えて行列が演算子として用いられて車両１の姿勢が推定されてもよい。具体的には、第１回転行列Ｑ₂［ｋ］および第２回転行列Ｑ₂［ｋ］のそれぞれを用いて、式（１０）にしたがって時刻ｋにおける車両１の姿勢Ｐ［ｋ］が推定される。

第１回転行列Ｑ₁［ｋ］はグローバル座標系のｚ_g軸を、車両座標系のｚ_c軸に一致させるためのグローバル座標系のｙ_g軸まわりの回転を表している。第１回転行列Ｑ₁［ｋ］は式（１１）により表現される。

第２回転行列Ｑ₂［ｋ］はグローバル座標系のｘ_g軸およびｙ_g軸のそれぞれを車両座標系のｘ_c軸およびｙ_c軸のそれぞれに一致させるための、グローバル座標系のｚ_g軸まわりの回転を表している。第２回転行列Ｑ₂［ｋ］はグローバル座標系のｘ_g方向に対する、時刻ｋにおける車両座標系のｘ_c方向の傾斜角度ξ［ｋ］に基づいて式（１２）により表現される。

当該実施形態によれば、前記実施形態と同様に３軸加速度センサよりも比較的安価な２軸加速度センサ１０２を用いて測定対象となる車両挙動を表すパラメータの数を減らしながら、グローバル座標系における車両１の姿勢を高精度で推定することができる。

車両姿勢推定装置の構成に関する説明図車両姿勢推定装置の構成に関する説明図車両姿勢推定方法に関する説明図グローバル座標系と車両座標系との関係に関する説明図ヨーレート補正に関する説明図

符号の説明

１‥車両、１０‥車両制御装置、１００‥車両姿勢推定装置、１０１‥２軸加速度センサ、１０２‥１軸ジャイロセンサ、１１０‥第１処理部、１１２‥演算子格納部、１２０‥第２処理部

Claims

車両の姿勢を推定する装置であって、
前記車両の速度と、加速度のロール軸方向成分およびピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とを測定する第１処理部と、
該第１処理部による測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるための該グローmmバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるための、該グローバル座標系の回転を表す第２演算子との合成演算子にしたがって前記車両の姿勢を算出する第２処理部とを備えていることを特徴とする車両姿勢推定装置。
請求項１記載の車両姿勢推定装置において、
前記第２処理部が、前記第１処理部による測定結果のうち前記車両の速度と、加速度のロール軸方向成分と、前記車両のヨー軸まわりの角速度とに基づき、前記第１演算子にしたがってグローバル座標系のｚ軸に対する車両座標系のヨー軸の姿勢を第１姿勢として算出し、前記第１姿勢と、前記第１処理部による測定結果のうち前記車両の速度と、加速度のピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とに基づき、前記第２演算子にしたがって、ｚ軸とヨー軸とが一致している状態でグローバル座標系のｘ軸およびｙ軸のそれぞれに対する車両座標系のロール軸およびピッチ軸のそれぞれの姿勢を第２姿勢として算出することを特徴とする車両姿勢推定装置。
請求項１または２記載の車両姿勢推定装置において、
前記演算処理部が、前記第１演算子および前記第２演算子としてのクォータニオンにしたがって前記車両の姿勢を算出することを特徴とする車両姿勢推定装置。
車両の姿勢を推定する方法であって、
前記車両の速度と、加速度のロール軸方向成分およびピッチ軸方向成分と、前記車両のヨー軸まわりの角速度とを測定する第１処理と、
該第１処理における測定結果に基づき、グローバル座標系のｚ軸と車両座標系のヨー軸とを一致させるための該グローバル座標系の回転を表す第１演算子と、グローバル座標系のｘ軸およびｙ軸のそれぞれと車両座標系のロール軸およびピッチ軸のそれぞれとを一致させるための、該グローバル座標系の回転を表す第２演算子との合成演算子にしたがって前記車両の姿勢を算出する第２処理とを実行することを特徴とする車両姿勢推定方法。