(57)【要約】【課題】３軸ジャイロセンサを用いることなく、車両の製造コストの上昇抑制およびコンパクト化を図りながら、車両の３軸まわりの角速度等を高精度で測定することができる装置等を提供する。【解決手段】車両姿勢推定装置１００によれば、グローバル座標系における車両１の姿勢を表わす姿勢ベクトルpsti(k)(i=x,y,z)の時間変化態様に基づき、比較的高価でかつサイズが大きい３軸ジャイロセンサを用いることなく、車両１の製造コストの上昇抑制およびコンパクト化を図りながら、車両１の３軸まわりの角速度を表わす角速度ベクトルω(k)が測定される。【選択図】  図１
2009075075.tif 000002
【特許請求の範囲】
【請求項１】
車両の挙動を測定する装置であって、
車両の挙動を表わすパラメータを測定する第１演算処理部と、
前記第１演算処理部による測定結果に基づき、グローバル座標系における前記車両の姿勢を表わす姿勢ベクトルを算出する第２演算処理部と、
前記第２演算処理部により算出された前記姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす角速度ベクトルを算出する第３演算処理部とを備えていることを特徴とする車両挙動測定装置。
【請求項２】
請求項１記載の車両挙動測定装置において、
前記第２演算処理部が、前記グローバル座標系における前記車両座標系の３つの直交軸のそれぞれの姿勢を表わす第ｉ姿勢ベクトル（ｉ＝１，２，３）を前記姿勢ベクトルとして算出し、
前記第３演算処理部が、前回の前記第ｉ姿勢ベクトルを今回の前記第ｉ姿勢ベクトルに一致させるような３つの回転軸ベクトルを合成することにより前記グローバル座標系における回転主軸の向きを示す回転主軸ベクトルを求め、前記回転主軸ベクトルと、前回および今回の前記第ｉ姿勢ベクトルとに基づき、前記グローバル座標系の３つの直交軸まわりの前記車両の角速度を表わす１次角速度ベクトルを算出した上で、前記１次角速度ベクトルと、前記第ｉ姿勢ベクトルとに基づき、前記車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす２次角速度ベクトルを前記角速度ベクトルとして算出することを特徴とする車両挙動測定装置。
【請求項３】
請求項１または２記載の車両挙動測定装置において、
前記第３演算処理部が前記角速度ベクトルの時間変化態様に基づき、前記車両座標系の３つの直交軸のそれぞれまわりの前記車両の角加速度を表わす角加速度ベクトルを算出することを特徴とする車両挙動測定装置。
【請求項４】
車両の挙動を測定する方法であって、
車両の挙動を表わすパラメータを測定する第１演算処理と、
前記第１演算処理による測定結果に基づき、グローバル座標系における前記車両の姿勢を表わす姿勢ベクトルを算出する第２演算処理と、
前記第２演算処理により算出された前記姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす角速度ベクトルを算出する第３演算処理とを実行することを特徴とする車両挙動測定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両の挙動を測定する装置および方法に関する。
【背景技術】
【０００２】
ナビゲーション装置にマップの上に正確な車両の位置を表示させる等のため、車両位置の測定精度向上を図る手法が提案されている。たとえば、車速センサおよび３軸加速度センサの出力に基づいて車両が走行する道路の傾斜角度が算出され、当該算出角度に基づいてジャイロセンサの出力が補正されることにより、道路勾配による影響を解消して車両の位置測定精度の向上が図られている（たとえば、特許文献１参照）。
【特許文献１】特開平９−４２９７９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかし、車両挙動を正確に測定するためには、当該車両のヨー軸のみならず、ロール軸やピッチ軸まわりの角速度や角加速度を測定する必要がある。一方、３軸周りの角速度等を測定するために比較的高価でかつサイズが大きい３軸ジャイロセンサを車両に搭載することは車両の製造コストおよび車両のコンパクト化の観点から好ましくない。
【０００４】
そこで、本発明は、３軸ジャイロセンサを用いることなく、車両の製造コストの上昇抑制およびコンパクト化を図りながら、車両の３軸まわりの角速度等を高精度で測定することができる装置等を提供することを解決課題とする。
【課題を解決するための手段】
【０００５】
第１発明の車両挙動測定装置は、車両の挙動を測定する装置であって、車両の挙動を表わすパラメータを測定する第１演算処理部と、前記第１演算処理部による測定結果に基づき、グローバル座標系における前記車両の姿勢を表わす姿勢ベクトルを算出する第２演算処理部と、前記第２演算処理部により算出された前記姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす角速度ベクトルを算出する第３演算処理部とを備えていることを特徴とする。
【０００６】
第１発明の車両挙動測定装置によれば、グローバル座標系における車両の姿勢を表わす姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの車両の角速度を表わす角速度ベクトルが測定される。したがって、比較的高価かつサイズが大きい３軸ジャイロセンサを用いることなく、車両の製造コストの上昇抑制およびコンパクト化を図りながら、車両の３軸まわりの角速度が高精度で測定されうる。
【０００７】
第２発明の車両挙動測定装置は、第１発明の車両挙動測定装置において、前記第２演算処理部が、前記グローバル座標系における前記車両座標系の３つの直交軸のそれぞれの姿勢を表わす第ｉ姿勢ベクトル（ｉ＝１，２，３）を前記姿勢ベクトルとして算出し、前記第３演算処理部が、前回の前記第ｉ姿勢ベクトルを今回の前記第ｉ姿勢ベクトルに一致させるような３つの回転軸ベクトルを合成することにより前記グローバル座標系における回転主軸の向きを示す回転主軸ベクトルを求め、前記回転主軸ベクトルと、前回および今回の前記第ｉ姿勢ベクトルとに基づき、前記グローバル座標系の３つの直交軸まわりの前記車両の角速度を表わす１次角速度ベクトルを算出した上で、前記１次角速度ベクトルと、前記第ｉ姿勢ベクトルとに基づき、前記車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす２次角速度ベクトルを前記角速度ベクトルとして算出することを特徴とする。
【０００８】
第２発明の車両挙動測定装置によれば、グローバル座標系において前回の車両座標系の姿勢を今回の車両座標系の姿勢に一致させるための回転主軸が考慮されることにより、車両座標系の３つの直交軸のそれぞれまわりの車両の角速度が高精度で測定されうる。
【０００９】
第３発明の車両挙動測定装置は、第１または第２発明の車両挙動測定装置において、前記第３演算処理部が前記角速度ベクトルの時間変化態様に基づき、前記車両座標系の３つの直交軸のそれぞれまわりの前記車両の角加速度を表わす角加速度ベクトルを算出することを特徴とする。
【００１０】
第３発明の車両挙動測定装置によれば、前記のように３軸ジャイロセンサを用いることなく高精度で測定された、車両の３軸まわりの角速度に基づき、車両の３軸まわりの角加速度が高精度で測定されうる。
【００１１】
第４発明の車両挙動測定方法は、車両の挙動を表わすパラメータを測定する第１演算処理と、前記第１演算処理による測定結果に基づき、グローバル座標系における前記車両の姿勢を表わす姿勢ベクトルを算出する第２演算処理と、前記第２演算処理により算出された前記姿勢ベクトルの時間変化態様に基づき、車両座標系の３つの直交軸のそれぞれまわりの前記車両の角速度を表わす角速度ベクトルを算出する第３演算処理とを実行することを特徴とする。
【００１２】
第４発明の車両挙動測定方法によれば、第１発明の装置と同様に、３軸ジャイロセンサを用いることなく、車両の３軸まわりの角速度が高精度で測定されうる。
【発明を実施するための最良の形態】
【００１３】
本発明の車両挙動測定装置および方法の実施形態について図面を用いて説明する。以下、パラメータに用いる添字「ｇ」は当該パラメータがグローバル座標系におけるパラメータであることを表し、添字「ｃ」は当該パラメータが車両座標系におけるパラメータであることを表わす。
【００１４】
まず、第１実施形態の車両挙動制御装置について説明する。図１および図２に示されている車両挙動測定装置１００は車両１に搭載されている車両制御装置１０の一部として構成されている。車両制御装置１０はコンピュータまたはＥＣＵ（電子制御ユニット。ＣＰＵ，ＲＯＭ，ＲＡＭおよびこれらを接続するバスライン等により構成されている。）により構成されている。車両１には速度センサ１０１と、２軸加速度センサ１０２と、１軸ジャイロセンサ１０３とがさらに搭載されている。速度センサ１０１は車両１のロール軸方向（ｘ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(cos2θ(k)+cos2φ(k)) 1/2  ..(103)
【００２４】
そして、第２演算処理部１２０は、第１演算処理部１１０の測定結果に基づく前記算出結果と、第１回転演算子としての第１クォータニオンqt1(k)とに基づき、関係式（１０４）にしたがって１次姿勢ベクトル（クォータニオンによって表現されている。）pst1i(k)(i=x,y,z)を算出する（図３／Ｓ０２４）。
【００２５】
pst1i(k)≡qt1*(k)psti(0)qt1(k)，
pstx(0)＝(1,0,0,0), psty(0)＝(0,1,0,0), pstz(0)＝(0,0,1,0) ..(104)
【００２６】
第１クォータニオンqt1(k)はグローバル座標系のｚg軸を車両座標系のｚc軸に一致させるための、グローバル座標系の回転を表している。この回転は、グローバル座標系のｚg軸と車両座標系のｚc軸とを含む平面の単位法線ベクトルｎ1＝（ｎ1x，ｎ1y，ｎ1z，０）まわりの回転である。第１クォータニオンqt1(k)はグローバル座標系のｚg方向に対する時刻ｋにおける車両座標系のｚc方向の傾斜角度ψ(k)に基づいて関係式（１０５）により表現される。
【００２７】
qt1(k)≡(qt1x(k),qt1y(k),qt1z(k),qt1w(k)),
qt1x(k)＝n1x(k)sin(ψ(k)/2), qt1y(k)＝n1y(k)sin(ψ(k)/2),
qt1z(k)＝n1z(k)sin(ψ(k)/2), qt1w(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)
【００３２】
そして、第２演算処理部１２０は、第１演算処理部１１０の測定結果に基づく１次姿勢ベクトルpst1i(k)等の前記算出結果と、演算子格納部１１２に格納されている第２回転演算子としての第２クォータニオンqt2(k)とに基づき、関係式（１０８）にしたがって２次姿勢ベクトル（クォータニオンによって表されている。）pst2i(k)(i=x,y,z)をグローバル座標系における車両１の姿勢を表わす第ｉ姿勢ベクトルpsti(k)として算出する（図３／Ｓ０２８）。第ｉ姿勢ベクトルpsti(k)は、グローバル座標系における、車両座標系のｉ軸の向きを示す単位ベクトルに相当する。なお、１次姿勢ベクトルpst1i(k)および２次姿勢ベクトルpst2i(k)が逐次算出されるのではなく、第１クォータニオンqt1(k)および第２クォータニオンqt2(k)の合成クォータニオンqt(k)＝qt1(k)qt2(k)にしたがって第ｉ姿勢ベクトルpsti(k)が一度に算定されてもよい。
【００３３】
pst2i(k)≡qt2*(k)pst1i(k)qt2(k)
＝qt2*(k)qt1*(k)psti(0)qt1(k)qt2(k) (i＝x,y,z) ..(108)
【００３４】
第２クォータニオンqt2(k)はグローバル座標系のｘg軸およびｙg軸のそれぞれを車両座標系のｘc軸およびｙc軸のそれぞれに一致させるための、グローバル座標系の回転を表している。この回転は、グローバル座標系のｚg方向の単位ベクトル（０，０，１，０）まわりの回転である。第２クォータニオンqt2(k)はグローバル座標系のｘg方向に対する、時刻ｋにおける車両座標系のｘc方向の傾斜角度ξ(k)に基づいて関係式（１０９）により表現される。
【００３５】
qt2(k)≡(qt2x(k),qt2y(k),qt2z(k),qt2w(k)),
qt2x(k)＝0, qt2y(k)＝0,
qt2z(k)＝sin(ξ(k)/2), qt2w(k)＝cos(ξ(k)/2) ..(109)
【００３６】
さらに、第３演算処理部１３０が第２演算処理部１２０による算出結果に基づき、車両座標系における３つの直交軸のそれぞれまわりの車両１の角速度等を測定する「第３演算処理」を実行する。まず、第３演算処理部１３０は第ｉ姿勢ベクトルpsti(k)の時間変化態様に基づき、グローバル座標系における３つの直交軸のそれぞれまわりの車両１の角速度を表わす１次角速度ベクトルω1(k)を算出する（図４／Ｓ０３２）。具体的には、前回（時刻ｔ＝ｋ−１）の姿勢ベクトルpsti(k-1)を今回（時刻ｔ＝ｋ）の姿勢ベクトルpsti(k)に一致させるためのグローバル座標系における回転主軸の向きを示す回転主軸ベクトルｎ2(k)が関係式（１１０）にしたがって算定される。
【００３７】
n2(k)＝Σi=x,y,z(psti(k-1)×psti(k))/|psti(k-1)×psti(k)| ..(110)
【００３８】
その上で、図７（ａ）に示されている回転主軸ベクトルｎ2(k)まわりの姿勢ベクトルpsti(k)の回転角度Θi(k)に基づき、関係式（１１１）にしたがって１次角速度ベクトルω1(k)が算出される。
【００３９】
ω1(k) ＝(ω1x(k),ω1y(k),ω1z(k),0)
＝ωn(k)(n2x(k),n2y(k),n2z(k),0),
ωn(k)＝Θ(k)/Δt,
Θ(k)＝avg(Θi(k)), max(Θi(k)) または min(Θi(k)),
Θi(k)＝arccos{n2(k)×psti(k-1))・(n2(k)×psti(k))} ..(111)
【００４０】
「avg(Θi(k))」は第ｉ姿勢ベクトルpsti(k)(i=x(=1),y(=2),z(=3))の回転主軸まわりの回転角度Θi(k)のうち２つまたは３つ（ただし「０」を除く。）の平均値を意味する。「max(Θi(k))」は３つの回転角度Θi(k)の最大値を意味する。「min(Θi(k))」は３つの回転角度Θi(k)の最小値（ただし「０」を除く。）を意味する。
【００４１】
さらに、第３演算処理部１３０が１次角速度ベクトルω1(k)と、グローバル座標系における車両１の姿勢を表わす第ｉ姿勢ベクトルpsti(k)とに基づき、関係式（１１２）にしたがって、車両座標系における３つの直交軸のそれぞれまわりの車両１の角速度を表わす２次角速度ベクトルω2(k)を角速度ベクトルω(k)として算出する（図４／Ｓ０３４）。
【００４２】
ω2(k)＝(ω2x(k),ω2y(k),ω2z(k),0)
ω2i(k)＝psti*(k)ω1(k)psti(k)
＝qt2*(k)qt1*(k)ω1(k)qt1(k)qt2(k) ..(112)
【００４３】
角速度ベクトルω(k)の算出に際して用いられている第１クォータニオンqt1(k)および第２クォータニオンqt2(k)の合成クォータニオンqt1(k)qt2(k)は、前記のようにグローバル座標系と車両座標系とを一致させるための、グローバル座標系の回転または座標変換を表している。
【００４４】
また、第３演算処理部１３０は、前回（時刻ｔ＝ｋ−１）の角速度ベクトルω(k-1)および今回（時刻ｔ＝ｋ）の角速度ベクトルω(k)に基づき、式（１１４）にしたがって角加速度ベクトルω’(k)を算出する（図４／Ｓ０３６）。
【００４５】
ω’(k)＝(ω(k)−ω(k-1))/Δt ..(114)
【００４６】
前述の処理が繰り返されることにより、車両１の３つの直交軸、すなわち、ロール軸、ピッチ軸およびヨー軸のそれぞれまわりの角速度ベクトルω(k)および角加速度ベクトルω’(k)が逐次算出または測定される。
【００４７】
続いて、第２実施形態の車両挙動測定装置１００について説明する。第２実施形態の車両挙動装置１００は、第１実施形態の車両挙動装置１００と比較してハードウェアの構成は同様である一方（図１、図２参照）、第２演算処理および第３演算処理の内容が相違している。具体的には、まず第１演算処理部１１０が第１実施形態と同様に第１演算処理を実行する（図３／Ｓ０１２〜Ｓ０１８参照）。
【００４８】
次に第２演算処理部１２０が第１実施形態と同様にグローバル座標系のｚg方向に対する車両座標系のｘc方向、ｙc方向およびｚc方向のそれぞれの傾斜角度θ(k)、φ(k)およびψ(k)を算出する（図３／Ｓ０２１〜Ｓ０２３参照）。また、第２演算処理部１２０は第１クォータニオンqt1(k)に基づき、関係式（２０４）にしたがって１次姿勢ベクトルpst1(k)を算出する（関係式（１０５）、図３／Ｓ０２４参照）。係数ｃiは任意に設定されうるが、ここでは１次姿勢ベクトルpst1(k)が単位ベクトルになるという束縛条件化で設定されている。
【００４９】
pst1(k)≡Σi=x,y,zciqt1*(k)psti(0)qt1(k) ..(204)
【００５０】
そして、第２演算処理部１２０が１次姿勢ベクトルpst1(k)に基づき、第２クォータニオンqt2(k)を用いて、関係式（２０８）にしたがって２次姿勢ベクトルpst2 (k)を姿勢ベクトルpst(k)として算定する（図３／Ｓ０２５〜Ｓ０２８参照）。
【００５１】
pst2i(k)≡qt2*(k)pst1(k)qt2(k)
＝qt2*(k)qt1*(k)pst(0)qt1(k)qt2(k) (i＝x,y,z) ..(208)
【００５２】
さらに、第３演算処理部１３０が姿勢ベクトルpst(k)の回転主軸まわりの角速度を表わす予備角速度ベクトルω0(k)を算出する（図４／Ｓ０３２参照）。具体的には、図７（ｂ）に示されているように２つの異なる時点ｋ−１およびｋにおける姿勢ベクトルpst(k-1)およびpst(k)のなす角度θ(k)の変化率θ(k)／Δｔに基づき、関係式（２１０）にしたがって当該姿勢ベクトルpst(k)の回転主軸まわりの角速度を表わす予備角速度ベクトルω0(k)が算出される。予備角速度ベクトルω0(k)は、図７（ｂ）に示されているように、時刻ｋ−１における姿勢ベクトルpst(k-1)をｘn軸上で正方向に向かう単位とし、２つの異なる時刻における姿勢ベクトルpst(k-1)およびpst(k)を含む平面の法線をｚn軸（回転主軸）とする回転座標系における３次元ベクトルとして定義される。
【００５３】
ω0(k)＝(0,0,θ(k)/Δt,0),
θ(k)＝arccos(pst(k-1)・pst(k+1))
＝arccos(Σi=x,y,zpsti(k-1)・psti(k)) ..(210)
【００５４】
また、第３演算処理部１３０は、予備角速度ベクトルω0(k)と、２つの姿勢ベクトルpst(k-1)およびpst(k)を包含する平面のグローバル座標系における姿勢とに基づき、関係式（２１１）にしたがってグローバル座標系における姿勢ベクトルの３軸まわりの角速度を表わす１次角速度ベクトルω1(k)を算出する（図４／Ｓ０３２参照）。
【００５５】
ω1(k)≡qt3*(k)ω0(k)qt3(k) ..(211)
【００５６】
第３クォータニオンqt3(k)は回転座標系のｚn軸をグローバル座標系のｚg軸に一致させるための、回転座標系のｚn軸の回転を表している。この回転は、回転座標系のｚn軸とグローバル座標系のｚg軸とを含む平面の単位法線ベクトルｎ3＝（ｎ3x，ｎ3y，ｎ3z）まわりの回転である。第３クォータニオンqt3(k)はグローバル座標系のｚg方向に対する時刻ｋにおける車両座標系のｚc方向の傾斜角度η(k)に基づいて関係式（２１２）により表現される。
【００５７】
qt3(k)≡(qt3x(k),qt3y(k),qt3z(k),qt3w(k)),
qt3x(k)＝n3x(k)sin(η(k)/2), qt3y(k)＝n3y(k)sin(η(k)/2),
qt3z(k)＝n3z(k)sin(η(k)/2), qt3w(k)＝cos(η(k)/2) ..(212)
【００５８】
そして、第３演算処理部１３０は第１実施形態と同様に車両座標系における姿勢ベクトルpst(k)の３軸まわりの角速度を表わす角速度ベクトルω(k)および角加速度ベクトルω’(k)を算出する（関係式（１１３）（１１４）、図４／Ｓ０３６，Ｓ０３８参照）。前述の処理が繰り返されることにより、車両１の３軸、すなわち、ロール軸、ピッチ軸およびヨー軸まわりの角速度および角加速度が逐次算出または測定される。
【００５９】
前記機能を発揮する車両挙動測定装置１００によれば、グローバル座標系における車両１の姿勢を表わす第ｉ姿勢ベクトルpsti(k)（第１実施形態）または姿勢ベクトルpst(k)（第２実施形態）の時間変化態様に基づき、比較的高価でかつサイズが大きい３軸ジャイロセンサを用いることなく、車両１の製造コストの上昇抑制およびコンパクト化を図りながら、車両１の３軸まわりの角速度を表わす角速度ベクトルω(k)が測定される（図４／Ｓ０３２，Ｓ０３４，図７（ａ）（ｂ）参照）。また、角速度ベクトルω(k)に基づき、車両１の３軸まわりの角加速度を表わす角加速度ベクトルω’(k)が高精度で測定されうる（図４／Ｓ０３６参照）。
【００６０】
なお、グローバル座標系における車両１の姿勢を表わす姿勢ベクトルおよび角速度ベクトルがクォータニオンに代えて、数学的な等価な３次元のベクトルによって表現されてもよい。また、車両１の姿勢推定に際してクォータニオンに代えて、数学的に等価な行列が姿勢演算子として用いられてもよい。具体的には、第１回転行列Ｑ1(k)および第２回転行列Ｑ2(k)のそれぞれを用いて、関係式（１３２）にしたがって時刻ｋにおける車両１の姿勢ベクトルＰ(k)が推定される（「ｔ」は転置を表わす。）。
【００６１】
P(k)＝(Px(k),Py(k),Pz(k))＝Q2(k)・Q1(k)・P(0),
Pi(k)＝t(pi1(k),p i2(k),pi3(k)),
Px(0)＝t(1,0,0), Py(0)＝t(0,1,0), Pz(k)＝t(0,0,1) ..(132)
【００６２】
第１回転行列Ｑ1(k)はグローバル座標系のｚg軸を、車両座標系のｚc軸に一致させるためのグローバル座標系のｙg軸まわりの回転を表している。第１回転行列Ｑ1(k)は関係式（１３４）により表現される。
【００６３】
Q1(k)≡(Q11(k),Q12(k),Q13(k)),
Q11＝t(1−2(qt1y2+qt1z2),2(qt1xqt1y−qt1zqt1w),2(qt1zqt1x＋qt1wqt1y)),
Q12＝t(2(qt1xqt1y＋qt1zqt1w),1−2(qt1z2+qt1x2),2(qt1yqt1x−qt1wqt1x)),
Q13＝t(2(qt1zqt1x−qt1wqt1y),2(qt1yqt1z＋qt1wqt1x),1−2(qt1y2+qt1z2)) ..(134)
【００６４】
第２回転行列Ｑ2(k)はグローバル座標系のｘg軸およびｙg軸のそれぞれを車両座標系のｘc軸およびｙc軸のそれぞれに一致させるための、グローバル座標系のｚg軸まわりの回転を表している。第２回転行列Ｑ2(k)はグローバル座標系のｘg方向に対する、時刻ｋにおける車両座標系のｘc方向の傾斜角度ξ(k)に基づいて関係式（１３６）により表現される。
【００６５】
Q2(k)≡(Q21(k),Q22(k),Q23(k)),
Q21＝t(cosξ(k),−sinξ(k),0),
Q22＝t(sinξ(k),cosξ(k),0),
Q23＝t(0,0,1) ..(136)
【００６６】
当該実施形態によれば、前記実施形態と同様に比較的高価でかつサイズが大きい３軸ジャイロセンサを用いることなく、車両１の製造コストの上昇抑制およびコンパクト化を図りながら、グローバル座標系における車両１の姿勢を高精度で推定することができる。
【図面の簡単な説明】
【００６７】
【図１】車両挙動測定装置の構成に関する説明図
【図２】車両挙動測定装置の構成に関する説明図
【図３】車両挙動測定方法に関する説明図
【図４】車両挙動測定方法に関する説明図
【図５】グローバル座標系と車両座標系との関係に関する説明図
【図６】ヨーレート補正に関する説明図
【図７】角速度ベクトルの算出方法に関する説明図
【符号の説明】
【００６８】
１‥車両、１０‥車両制御装置、１００‥車両挙動測定装置、１０１‥速度センサ、１０２‥２軸加速度センサ、１０３‥１軸ジャイロセンサ、１１０‥第１演算処理部、１２０‥第２演算処理部、１３０‥第３演算処理部
【図１】
2009075075.tif 000003
【図２】
2009075075.tif 000004
【図３】
2009075075.tif 000005
【図４】
2009075075.tif 000006
【図５】
2009075075.tif 000007
【図６】
2009075075.tif 000008
【図７】
2009075075.tif 000009