JP2009075075A - 車両挙動測定装置、車両挙動測定方法 - Google Patents

Abstract

【課題】３軸ジャイロセンサを用いることなく、車両の製造コストの上昇抑制およびコンパクト化を図りながら、車両の３軸まわりの角速度等を高精度で測定することができる装置等を提供する。
【解決手段】車両姿勢推定装置１００によれば、グローバル座標系における車両１の姿勢を表わす姿勢ベクトルpst_i(k)(i=x,y,z)の時間変化態様に基づき、比較的高価でかつサイズが大きい３軸ジャイロセンサを用いることなく、車両１の製造コストの上昇抑制およびコンパクト化を図りながら、車両１の３軸まわりの角速度を表わす角速度ベクトルω(k)が測定される。
【選択図】 図１

Description

本発明は、車両の挙動を測定する装置および方法に関する。

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

しかし、車両挙動を正確に測定するためには、当該車両のヨー軸のみならず、ロール軸やピッチ軸まわりの角速度や角加速度を測定する必要がある。一方、３軸周りの角速度等を測定するために比較的高価でかつサイズが大きい３軸ジャイロセンサを車両に搭載することは車両の製造コストおよび車両のコンパクト化の観点から好ましくない。

そこで、本発明は、３軸ジャイロセンサを用いることなく、車両の製造コストの上昇抑制およびコンパクト化を図りながら、車両の３軸まわりの角速度等を高精度で測定することができる装置等を提供することを解決課題とする。

第１発明の車両挙動測定装置は、車両の挙動を測定する装置であって、車両の挙動を表わすパラメータを測定する第１演算処理部と、前記第１演算処理部による測定結果に基づき、グローバル座標系における前記車両の姿勢を表わす姿勢ベクトルを算出する第２演算処理部と、前記第２演算処理部により算出された前記姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす角速度ベクトルを算出する第３演算処理部とを備えていることを特徴とする。

第１発明の車両挙動測定装置によれば、グローバル座標系における車両の姿勢を表わす姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの車両の角速度を表わす角速度ベクトルが測定される。したがって、比較的高価かつサイズが大きい３軸ジャイロセンサを用いることなく、車両の製造コストの上昇抑制およびコンパクト化を図りながら、車両の３軸まわりの角速度が高精度で測定されうる。

第２発明の車両挙動測定装置は、第１発明の車両挙動測定装置において、前記第２演算処理部が、前記グローバル座標系における前記車両座標系の３つの直交軸のそれぞれの姿勢を表わす第ｉ姿勢ベクトル（ｉ＝１，２，３）を前記姿勢ベクトルとして算出し、前記第３演算処理部が、前回の前記第ｉ姿勢ベクトルを今回の前記第ｉ姿勢ベクトルに一致させるような３つの回転軸ベクトルを合成することにより前記グローバル座標系における回転主軸の向きを示す回転主軸ベクトルを求め、前記回転主軸ベクトルと、前回および今回の前記第ｉ姿勢ベクトルとに基づき、前記グローバル座標系の３つの直交軸まわりの前記車両の角速度を表わす１次角速度ベクトルを算出した上で、前記１次角速度ベクトルと、前記第ｉ姿勢ベクトルとに基づき、前記車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす２次角速度ベクトルを前記角速度ベクトルとして算出することを特徴とする。

第２発明の車両挙動測定装置によれば、グローバル座標系において前回の車両座標系の姿勢を今回の車両座標系の姿勢に一致させるための回転主軸が考慮されることにより、車両座標系の３つの直交軸のそれぞれまわりの車両の角速度が高精度で測定されうる。

第３発明の車両挙動測定装置は、第１または第２発明の車両挙動測定装置において、前記第３演算処理部が前記角速度ベクトルの時間変化態様に基づき、前記車両座標系の３つの直交軸のそれぞれまわりの前記車両の角加速度を表わす角加速度ベクトルを算出することを特徴とする。

第３発明の車両挙動測定装置によれば、前記のように３軸ジャイロセンサを用いることなく高精度で測定された、車両の３軸まわりの角速度に基づき、車両の３軸まわりの角加速度が高精度で測定されうる。

第４発明の車両挙動測定方法は、車両の挙動を表わすパラメータを測定する第１演算処理と、前記第１演算処理による測定結果に基づき、グローバル座標系における前記車両の姿勢を表わす姿勢ベクトルを算出する第２演算処理と、前記第２演算処理により算出された前記姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす角速度ベクトルを算出する第３演算処理とを実行することを特徴とする。

第４発明の車両挙動測定方法によれば、第１発明の装置と同様に、３軸ジャイロセンサを用いることなく、車両の３軸まわりの角速度が高精度で測定されうる。

本発明の車両挙動測定装置および方法の実施形態について図面を用いて説明する。以下、パラメータに用いる添字「ｇ」は当該パラメータがグローバル座標系におけるパラメータであることを表し、添字「ｃ」は当該パラメータが車両座標系におけるパラメータであることを表わす。

まず、第１実施形態の車両挙動制御装置について説明する。図１および図２に示されている車両挙動測定装置１００は車両１に搭載されている車両制御装置１０の一部として構成されている。車両制御装置１０はコンピュータまたはＥＣＵ（電子制御ユニット。ＣＰＵ，ＲＯＭ，ＲＡＭおよびこれらを接続するバスライン等により構成されている。）により構成されている。車両１には速度センサ１０１と、２軸加速度センサ１０２と、１軸ジャイロセンサ１０３とがさらに搭載されている。速度センサ１０１は車両１のロール軸方向（ｘ_c方向）の速度ｖ_xc(k)に応じた信号を出力する。２軸加速度センサ１０２は車両１の加速度αのロール軸方向成分α_xc(k)およびピッチ軸方向成分（ｙ_c方向成分）α_yc(k)に応じた信号を出力する。１軸ジャイロセンサ１０３は車両１のヨー軸（ｚ_c軸）まわりの角速度ω_zc(k)に応じた信号を出力する。

車両制御装置１０は車両挙動測定装置１００による測定結果に基づき、車両１の挙動を制御する。車両挙動測定装置１００は第１演算処理部１１０と、第２演算処理部１２０と、第３演算処理部１３０とを備えている。第１演算処理部１１０は速度センサ１０１、２軸加速度センサ１０２および１軸ジャイロセンサ１０３のそれぞれからの出力信号に基づき、時刻ｋにおける車両１のロール軸方向の速度ｖ_xc(k)と、加速度αのロール軸方向成分α_xc(k)およびピッチ軸方向成分α_yc(k)と、ヨー軸まわりの角速度ω_zc(k)とを測定する。第２演算処理部１２０は第１演算処理部１２０による測定結果に基づき、姿勢演算子にしたがってグローバル座標系における車両１の姿勢を表わす姿勢ベクトルを算出する。第３演算処理部１３０は第２演算処理部１２０により算出された姿勢ベクトルと、この姿勢ベクトルの時間変化とに基づき、車両１の３軸、すなわちロール軸、ピッチ軸およびヨー軸まわりの角速度および角加速度を算出する。各演算処理部は、演算回路、または、メモリおよびこのメモリからプログラムを読み出して、このプログラムにしたがって担当する演算処理を実行する演算処理装置（ＣＰＵ）により構成されている。第１演算処理部１１０による測定結果、第２演算処理部１２０による算出結果および第３演算処理部１３０による算出結果のそれぞれはメモリ等の記憶装置に格納される。

前記構成の第１実施形態の車両挙動測定装置１００による車両１の挙動測定方法について説明する。

まず、第１演算処理部１１０が車両１の挙動を表わすパラメータ測定する「第１演算処理」を実行する。具体的には、第１演算処理部１１０は車速センサ１０１からの出力に基づき、車両１のロール軸方向の速度ｖ_zc(k)（ｋは時刻を表わす。以下同じ。）を測定する（図３／Ｓ０１２）。第１演算処理部１１０は２軸加速度センサ１０２からの出力信号に基づき、車両１の加速度αのロール軸方向成分α_xc(k)を測定し（図３／Ｓ０１４）、かつ、車両１の加速度αのｙ方向成分α_yc(k)を測定する（図３／Ｓ０１６）。第１演算処理部１１０は１軸ジャイロセンサ１０３からの出力信号に基づき、車両１のヨー軸周りの角速度ω_zc(k)を測定する（図３／Ｓ０１８）。

また、第２演算処理部１２０が第１演算処理部１１０による測定結果に基づき、グローバル座標系における車両１の姿勢を表わす姿勢ベクトルを算出する「第２演算処理」を実行する。具体的には、第２演算処理部１２０がグローバル座標系のｚ_g方向に対する車両座標系のｘ_c方向の傾斜角度θ(k)を算出する（図３／Ｓ０２１）。当該傾斜角度θ(k)は、車両１の加速度のロール軸方向成分α_xc(k)と、車両１のロール軸方向の速度ｖ_xc(k)の時間変化率ｖ_xc’(k)と、転心からジャイロセンサ１０３までのｙ方向の距離ｌ_yと、ヨー軸まわりの角速度ω_zc(k)の時間変化率（角加速度）ω_zc’(k)とに基づき、関係式（１０１）にしたがって算出される。関係式（１０１）は、図５（ａ）に示されているように車両１の加速度のｘ_c方向成分α_xc(k)は、重力、遠心力、加減速による慣性力および転心力のそれぞれのｘ_c方向成分の和に等しいことを表している（ここでは、遠心力のｘ_c方向成分は０である）。

θ(k)＝π/2−arcsin[(α_xc(k)−ｖ_xc’(k)−ｌ_yω_xc’(k))/ｇ］,
ｖ_xc’(k)＝(ｖ_xc(k)−ｖ_xc(k-1))/Δｔ，
ω_zc’(k)＝(ω_zc(k)−ω_zc(k-1))/Δｔ ..(101)

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

φ(k)＝π/2−arcsin[(α_yc(k)−ｖ_xc(k)ω_zc(k)−ｌ_xω_zc’(k))/ｇ] ..(102)

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

ψ(k)＝arcsin(cos²θ(k)+cos²φ(k)) ^1/2 ..(103)

そして、第２演算処理部１２０は、第１演算処理部１１０の測定結果に基づく前記算出結果と、第１回転演算子としての第１クォータニオンqt₁(k)とに基づき、関係式（１０４）にしたがって１次姿勢ベクトル（クォータニオンによって表現されている。）pst_1i(k)(i=x,y,z)を算出する（図３／Ｓ０２４）。

pst_1i(k)≡qt₁ ^*(k)pst_i(0)qt₁(k)，
pst_x(0)＝(1,0,0,0), pst_y(0)＝(0,1,0,0), pst_z(0)＝(0,0,1,0) ..(104)

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

qt₁(k)≡(qt_1x(k),qt_1y(k),qt_1z(k),qt_1w(k)),
qt_1x(k)＝n_1x(k)sin(ψ(k)/2), qt_1y(k)＝n_1y(k)sin(ψ(k)/2),
qt_1z(k)＝n_1z(k)sin(ψ(k)/2), qt_1w(k)＝cos(ψ(k)/2) ..(105)

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

ω_zg(k)＝ω_zc(k)/cosψ(k) ..(106)

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

ξ(k)＝ξ(k-1)＋ω_gz(k)Δt ..(107)

そして、第２演算処理部１２０は、第１演算処理部１１０の測定結果に基づく１次姿勢ベクトルpst_1i(k)等の前記算出結果と、演算子格納部１１２に格納されている第２回転演算子としての第２クォータニオンqt₂(k)とに基づき、関係式（１０８）にしたがって２次姿勢ベクトル（クォータニオンによって表されている。）pst_2i(k)(i=x,y,z)をグローバル座標系における車両１の姿勢を表わす第ｉ姿勢ベクトルpst_i(k)として算出する（図３／Ｓ０２８）。第ｉ姿勢ベクトルpst_i(k)は、グローバル座標系における、車両座標系のｉ軸の向きを示す単位ベクトルに相当する。なお、１次姿勢ベクトルpst_1i(k)および２次姿勢ベクトルpst_2i(k)が逐次算出されるのではなく、第１クォータニオンqt₁(k)および第２クォータニオンqt₂(k)の合成クォータニオンqt(k)＝qt₁(k)qt₂(k)にしたがって第ｉ姿勢ベクトルpst_i(k)が一度に算定されてもよい。

pst_2i(k)≡qt₂ ^*(k)pst_1i(k)qt₂(k)
＝qt₂ ^*(k)qt₁ ^*(k)pst_i(0)qt₁(k)qt₂(k) (i＝x,y,z) ..(108)

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

qt₂(k)≡(qt_2x(k),qt_2y(k),qt_2z(k),qt_2w(k)),
qt_2x(k)＝0, qt_2y(k)＝0,
qt_2z(k)＝sin(ξ(k)/2), qt_2w(k)＝cos(ξ(k)/2) ..(109)

さらに、第３演算処理部１３０が第２演算処理部１２０による算出結果に基づき、車両座標系における３つの直交軸のそれぞれまわりの車両１の角速度等を測定する「第３演算処理」を実行する。まず、第３演算処理部１３０は第ｉ姿勢ベクトルpst_i(k)の時間変化態様に基づき、グローバル座標系における３つの直交軸のそれぞれまわりの車両１の角速度を表わす１次角速度ベクトルω₁(k)を算出する（図４／Ｓ０３２）。具体的には、前回（時刻ｔ＝ｋ−１）の姿勢ベクトルpst_i(k-1)を今回（時刻ｔ＝ｋ）の姿勢ベクトルpst_i(k)に一致させるためのグローバル座標系における回転主軸の向きを示す回転主軸ベクトルｎ₂(k)が関係式（１１０）にしたがって算定される。

n₂(k)＝Σ_i=x,y,z(pst_i(k-1)×pst_i(k))/|pst_i(k-1)×pst_i(k)| ..(110)

その上で、図７（ａ）に示されている回転主軸ベクトルｎ₂(k)まわりの姿勢ベクトルpst_i(k)の回転角度Θ_i(k)に基づき、関係式（１１１）にしたがって１次角速度ベクトルω₁(k)が算出される。

ω₁(k) ＝(ω_1x(k),ω_1y(k),ω_1z(k),0)
＝ω_n(k)(n_2x(k),n_2y(k),n_2z(k),0),
ω_n(k)＝Θ(k)/Δt,
Θ(k)＝avg(Θ_i(k)), max(Θ_i(k)) または min(Θ_i(k)),
Θ_i(k)＝arccos{n₂(k)×pst_i(k-1))・(n₂(k)×pst_i(k))} ..(111)

「avg(Θ_i(k))」は第ｉ姿勢ベクトルpst_i(k)(i=x(=1),y(=2),z(=3))の回転主軸まわりの回転角度Θ_i(k)のうち２つまたは３つ（ただし「０」を除く。）の平均値を意味する。「max(Θ_i(k))」は３つの回転角度Θ_i(k)の最大値を意味する。「min(Θ_i(k))」は３つの回転角度Θ_i(k)の最小値（ただし「０」を除く。）を意味する。

さらに、第３演算処理部１３０が１次角速度ベクトルω₁(k)と、グローバル座標系における車両１の姿勢を表わす第ｉ姿勢ベクトルpst_i(k)とに基づき、関係式（１１２）にしたがって、車両座標系における３つの直交軸のそれぞれまわりの車両１の角速度を表わす２次角速度ベクトルω₂(k)を角速度ベクトルω(k)として算出する（図４／Ｓ０３４）。

ω₂(k)＝(ω_2x(k),ω_2y(k),ω_2z(k),0)
ω_2i(k)＝pst_i ^*(k)ω₁(k)pst_i(k)
＝qt₂ ^*(k)qt₁ ^*(k)ω₁(k)qt₁(k)qt₂(k) ..(112)

角速度ベクトルω(k)の算出に際して用いられている第１クォータニオンqt₁(k)および第２クォータニオンqt₂(k)の合成クォータニオンqt₁(k)qt₂(k)は、前記のようにグローバル座標系と車両座標系とを一致させるための、グローバル座標系の回転または座標変換を表している。

また、第３演算処理部１３０は、前回（時刻ｔ＝ｋ−１）の角速度ベクトルω(k-1)および今回（時刻ｔ＝ｋ）の角速度ベクトルω(k)に基づき、式（１１４）にしたがって角加速度ベクトルω’(k)を算出する（図４／Ｓ０３６）。

ω’(k)＝(ω(k)−ω(k-1))/Δt ..(114)

前述の処理が繰り返されることにより、車両１の３つの直交軸、すなわち、ロール軸、ピッチ軸およびヨー軸のそれぞれまわりの角速度ベクトルω(k)および角加速度ベクトルω’(k)が逐次算出または測定される。

続いて、第２実施形態の車両挙動測定装置１００について説明する。第２実施形態の車両挙動装置１００は、第１実施形態の車両挙動装置１００と比較してハードウェアの構成は同様である一方（図１、図２参照）、第２演算処理および第３演算処理の内容が相違している。具体的には、まず第１演算処理部１１０が第１実施形態と同様に第１演算処理を実行する（図３／Ｓ０１２〜Ｓ０１８参照）。

次に第２演算処理部１２０が第１実施形態と同様にグローバル座標系のｚ_g方向に対する車両座標系のｘ_c方向、ｙ_c方向およびｚ_c方向のそれぞれの傾斜角度θ(k)、φ(k)およびψ(k)を算出する（図３／Ｓ０２１〜Ｓ０２３参照）。また、第２演算処理部１２０は第１クォータニオンqt₁(k)に基づき、関係式（２０４）にしたがって１次姿勢ベクトルpst₁(k)を算出する（関係式（１０５）、図３／Ｓ０２４参照）。係数ｃ_iは任意に設定されうるが、ここでは１次姿勢ベクトルpst₁(k)が単位ベクトルになるという束縛条件化で設定されている。

pst₁(k)≡Σ_i=x,y,zc_iqt₁ ^*(k)pst_i(0)qt₁(k) ..(204)

そして、第２演算処理部１２０が１次姿勢ベクトルpst₁(k)に基づき、第２クォータニオンqt₂(k)を用いて、関係式（２０８）にしたがって２次姿勢ベクトルpst₂ (k)を姿勢ベクトルpst(k)として算定する（図３／Ｓ０２５〜Ｓ０２８参照）。

pst_2i(k)≡qt₂ ^*(k)pst₁(k)qt₂(k)
＝qt₂ ^*(k)qt₁ ^*(k)pst(0)qt₁(k)qt₂(k) (i＝x,y,z) ..(208)

さらに、第３演算処理部１３０が姿勢ベクトルpst(k)の回転主軸まわりの角速度を表わす予備角速度ベクトルω₀(k)を算出する（図４／Ｓ０３２参照）。具体的には、図７（ｂ）に示されているように２つの異なる時点ｋ−１およびｋにおける姿勢ベクトルpst(k-1)およびpst(k)のなす角度θ(k)の変化率θ(k)／Δｔに基づき、関係式（２１０）にしたがって当該姿勢ベクトルpst(k)の回転主軸まわりの角速度を表わす予備角速度ベクトルω₀(k)が算出される。予備角速度ベクトルω₀(k)は、図７（ｂ）に示されているように、時刻ｋ−１における姿勢ベクトルpst(k-1)をｘ_n軸上で正方向に向かう単位とし、２つの異なる時刻における姿勢ベクトルpst(k-1)およびpst(k)を含む平面の法線をｚ_n軸（回転主軸）とする回転座標系における３次元ベクトルとして定義される。

ω₀(k)＝(0,0,θ(k)/Δt,0),
θ(k)＝arccos(pst(k-1)・pst(k+1))
＝arccos(Σ_i=x,y,zpst_i(k-1)・pst_i(k)) ..(210)

また、第３演算処理部１３０は、予備角速度ベクトルω₀(k)と、２つの姿勢ベクトルpst(k-1)およびpst(k)を包含する平面のグローバル座標系における姿勢とに基づき、関係式（２１１）にしたがってグローバル座標系における姿勢ベクトルの３軸まわりの角速度を表わす１次角速度ベクトルω₁(k)を算出する（図４／Ｓ０３２参照）。

ω₁(k)≡qt₃ ^*(k)ω₀(k)qt₃(k) ..(211)

第３クォータニオンqt₃(k)は回転座標系のｚ_n軸をグローバル座標系のｚ_g軸に一致させるための、回転座標系のｚ_n軸の回転を表している。この回転は、回転座標系のｚ_n軸とグローバル座標系のｚ_g軸とを含む平面の単位法線ベクトルｎ₃＝（ｎ_3x，ｎ_3y，ｎ_3z）まわりの回転である。第３クォータニオンqt₃(k)はグローバル座標系のｚ_g方向に対する時刻ｋにおける車両座標系のｚ_c方向の傾斜角度η(k)に基づいて関係式（２１２）により表現される。

qt₃(k)≡(qt_3x(k),qt_3y(k),qt_3z(k),qt_3w(k)),
qt_3x(k)＝n_3x(k)sin(η(k)/2), qt_3y(k)＝n_3y(k)sin(η(k)/2),
qt_3z(k)＝n_3z(k)sin(η(k)/2), qt_3w(k)＝cos(η(k)/2) ..(212)

そして、第３演算処理部１３０は第１実施形態と同様に車両座標系における姿勢ベクトルpst(k)の３軸まわりの角速度を表わす角速度ベクトルω(k)および角加速度ベクトルω’(k)を算出する（関係式（１１３）（１１４）、図４／Ｓ０３６，Ｓ０３８参照）。前述の処理が繰り返されることにより、車両１の３軸、すなわち、ロール軸、ピッチ軸およびヨー軸まわりの角速度および角加速度が逐次算出または測定される。

前記機能を発揮する車両挙動測定装置１００によれば、グローバル座標系における車両１の姿勢を表わす第ｉ姿勢ベクトルpst_i(k)（第１実施形態）または姿勢ベクトルpst(k)（第２実施形態）の時間変化態様に基づき、比較的高価でかつサイズが大きい３軸ジャイロセンサを用いることなく、車両１の製造コストの上昇抑制およびコンパクト化を図りながら、車両１の３軸まわりの角速度を表わす角速度ベクトルω(k)が測定される（図４／Ｓ０３２，Ｓ０３４，図７（ａ）（ｂ）参照）。また、角速度ベクトルω(k)に基づき、車両１の３軸まわりの角加速度を表わす角加速度ベクトルω’(k)が高精度で測定されうる（図４／Ｓ０３６参照）。

なお、グローバル座標系における車両１の姿勢を表わす姿勢ベクトルおよび角速度ベクトルがクォータニオンに代えて、数学的な等価な３次元のベクトルによって表現されてもよい。また、車両１の姿勢推定に際してクォータニオンに代えて、数学的に等価な行列が姿勢演算子として用いられてもよい。具体的には、第１回転行列Ｑ₁(k)および第２回転行列Ｑ₂(k)のそれぞれを用いて、関係式（１３２）にしたがって時刻ｋにおける車両１の姿勢ベクトルＰ(k)が推定される（「ｔ」は転置を表わす。）。

P(k)＝(P_x(k),P_y(k),P_z(k))＝Q₂(k)・Q₁(k)・P(0),
P_i(k)＝^t(p_i1(k),p _i2(k),p_i3(k)),
P_x(0)＝^t(1,0,0), P_y(0)＝^t(0,1,0), P_z(k)＝^t(0,0,1) ..(132)

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

Q₁(k)≡(Q₁₁(k),Q₁₂(k),Q₁₃(k)),
Q₁₁＝^t(1−2(qt_1y ²+qt_1z ²),2(qt_1xqt_1y−qt_1zqt_1w),2(qt_1zqt_1x＋qt_1wqt_1y)),
Q₁₂＝^t(2(qt_1xqt_1y＋qt_1zqt_1w),1−2(qt_1z ²+qt_1x ²),2(qt_1yqt_1x−qt_1wqt_1x)),
Q₁₃＝^t(2(qt_1zqt_1x−qt_1wqt_1y),2(qt_1yqt_1z＋qt_1wqt_1x),1−2(qt_1y ²+qt_1z ²)) ..(134)

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

Q₂(k)≡(Q₂₁(k),Q₂₂(k),Q₂₃(k)),
Q₂₁＝^t(cosξ(k),−sinξ(k),0),
Q₂₂＝^t(sinξ(k),cosξ(k),0),
Q₂₃＝^t(0,0,1) ..(136)

当該実施形態によれば、前記実施形態と同様に比較的高価でかつサイズが大きい３軸ジャイロセンサを用いることなく、車両１の製造コストの上昇抑制およびコンパクト化を図りながら、グローバル座標系における車両１の姿勢を高精度で推定することができる。

車両挙動測定装置の構成に関する説明図 車両挙動測定装置の構成に関する説明図 車両挙動測定方法に関する説明図 車両挙動測定方法に関する説明図 グローバル座標系と車両座標系との関係に関する説明図 ヨーレート補正に関する説明図 角速度ベクトルの算出方法に関する説明図

符号の説明

１‥車両、１０‥車両制御装置、１００‥車両挙動測定装置、１０１‥速度センサ、１０２‥２軸加速度センサ、１０３‥１軸ジャイロセンサ、１１０‥第１演算処理部、１２０‥第２演算処理部、１３０‥第３演算処理部

Claims (4)

1. 車両の挙動を測定する装置であって、
   車両の挙動を表わすパラメータを測定する第１演算処理部と、
   前記第１演算処理部による測定結果に基づき、グローバル座標系における前記車両の姿勢を表わす姿勢ベクトルを算出する第２演算処理部と、
   前記第２演算処理部により算出された前記姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす角速度ベクトルを算出する第３演算処理部とを備えていることを特徴とする車両挙動測定装置。
2. 請求項１記載の車両挙動測定装置において、
   前記第２演算処理部が、前記グローバル座標系における前記車両座標系の３つの直交軸のそれぞれの姿勢を表わす第ｉ姿勢ベクトル（ｉ＝１，２，３）を前記姿勢ベクトルとして算出し、
   前記第３演算処理部が、前回の前記第ｉ姿勢ベクトルを今回の前記第ｉ姿勢ベクトルに一致させるような３つの回転軸ベクトルを合成することにより前記グローバル座標系における回転主軸の向きを示す回転主軸ベクトルを求め、前記回転主軸ベクトルと、前回および今回の前記第ｉ姿勢ベクトルとに基づき、前記グローバル座標系の３つの直交軸まわりの前記車両の角速度を表わす１次角速度ベクトルを算出した上で、前記１次角速度ベクトルと、前記第ｉ姿勢ベクトルとに基づき、前記車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす２次角速度ベクトルを前記角速度ベクトルとして算出することを特徴とする車両挙動測定装置。
3. 請求項１または２記載の車両挙動測定装置において、
   前記第３演算処理部が前記角速度ベクトルの時間変化態様に基づき、前記車両座標系の３つの直交軸のそれぞれまわりの前記車両の角加速度を表わす角加速度ベクトルを算出することを特徴とする車両挙動測定装置。
4. 車両の挙動を測定する方法であって、
   車両の挙動を表わすパラメータを測定する第１演算処理と、
   前記第１演算処理による測定結果に基づき、グローバル座標系における前記車両の姿勢を表わす姿勢ベクトルを算出する第２演算処理と、
   前記第２演算処理により算出された前記姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす角速度ベクトルを算出する第３演算処理とを実行することを特徴とする車両挙動測定方法。

Images