特許 6454857 姿勢検出装置及び姿勢検出方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6454857

(24)【登録日】2018年12月28日

(45)【発行日】2019年1月23日

(54)【発明の名称】姿勢検出装置及び姿勢検出方法

(51)【国際特許分類】

   G01C 19/00 20130101AFI20190110BHJP

   G01C 9/08 20060101ALI20190110BHJP

【ＦＩ】

   G01C19/00 Z

   G01C9/08

【請求項の数】8

【全頁数】9

(21)【出願番号】特願2015-107466(P2015-107466)

(22)【出願日】2015年5月27日

(65)【公開番号】特開2016-223798(P2016-223798A)

(43)【公開日】2016年12月28日

【審査請求日】2017年12月28日

(73)【特許権者】

【識別番号】000203634

【氏名又は名称】多摩川精機株式会社

(74)【代理人】

【識別番号】100110423

【弁理士】

【氏名又は名称】曾我 道治

(74)【代理人】

【識別番号】100111648

【弁理士】

【氏名又は名称】梶並 順

(74)【代理人】

【識別番号】100147500

【弁理士】

【氏名又は名称】田口 雅啓

(74)【代理人】

【識別番号】100166235

【弁理士】

【氏名又は名称】大井 一郎

(74)【代理人】

【識別番号】100179914

【弁理士】

【氏名又は名称】光永 和宏

(74)【代理人】

【識別番号】100179936

【弁理士】

【氏名又は名称】金山 明日香

(72)【発明者】

【氏名】熊谷 秀夫

【審査官】 櫻井 仁

(56)【参考文献】

【文献】 特開平０９−１２６７９５（ＪＰ，Ａ）

【文献】 特開２００１−０３３４７９（ＪＰ，Ａ）

【文献】 特開２００４−１３２７６９（ＪＰ，Ａ）

【文献】 特開平１１−２４８４５５（ＪＰ，Ａ）

【文献】 特開２００８−００２９９２（ＪＰ，Ａ）

【文献】 特開２０１０−１１７２６０（ＪＰ，Ａ）

【文献】 特開２０１１−０６４４８３（ＪＰ，Ａ）

【文献】 特開２０１４−１０８６４６（ＪＰ，Ａ）

【文献】 米国特許第０４３２１６７８（ＵＳ，Ａ）

【文献】 特開２０１２−１４１２１９（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｃ １９／００−１９／７２

Ｇ０１Ｂ ２１／００−２１／３２

Ｇ０１Ｃ ９／０８

(57)【特許請求の範囲】

【請求項1】

移動体に設けられ、前記移動体の姿勢を検出する姿勢検出装置において、
直交する３軸に沿った直線加速度を検知する加速度センサと、
直交する３軸まわりの角速度を検知する角速度センサと、
前記移動体の進行速度データを受け取るように構成されると共に前記加速度センサ及び前記角速度センサから検知データを受け取るように構成され、前記移動体の姿勢を演算する演算部と
を備え、
前記演算部は、
前記進行速度データと前記角速度センサからの検知角速度データとを用いた補正を伴って、前記加速度センサからの検知直線加速度データを用いて直線速度を算出し、
前記直線速度と前記進行速度データとの差異である差異速度を用いて、前記角速度センサからの検知角速度データを補正して、前記移動体の姿勢角を算出する、姿勢検出装置。

【請求項2】

前記演算部は、前記進行速度データと前記検知角速度データとを用いた補正では、前記差異速度と、前記進行速度データ及び前記検知角速度データに基づく前記移動体の遠心加速度とを用いる請求項１に記載の姿勢検出装置。

【請求項3】

前記演算部は、前記進行速度データと前記検知角速度データとを用いた補正では、前記検知直線加速度データから前記遠心加速度に関する要素を除去した加速度を時間積分して得られる速度に対して、前記差異速度に関する要素を除去する請求項２に記載の姿勢検出装置。

【請求項4】

前記演算部は、前記差異速度を用いた補正では、前記角速度センサからの検知角速度データから、前記差異速度の角速度成分に関する要素を除去する請求項１〜３のいずれか一項に記載の姿勢検出装置。

【請求項5】

移動体の姿勢を検出する方法において、
前記移動体の進行速度データと、直交する３軸に沿った前記移動体の直線加速度データと、直交する３軸まわりの前記移動体の角速度データとを取得し、
前記進行速度データと前記角速度データとを用いた補正を伴って、前記直線加速度データを用いて直線速度を算出し、
前記直線速度と前記進行速度データとの差異である差異速度を用いて前記角速度データを補正して、前記移動体の姿勢角を算出する方法。

【請求項6】

前記進行速度データと前記角速度データとを用いた補正では、前記差異速度と、前記進行速度データ及び前記角速度データに基づく前記移動体の遠心加速度とを用いる請求項５に記載の方法。

【請求項7】

前記進行速度データと前記角速度データとを用いた補正では、前記直線加速度から前記遠心加速度に関する要素を除去した加速度を時間積分して得られる速度に対して、前記差異速度に関する要素を除去する請求項６に記載の方法。

【請求項8】

前記差異速度を用いた補正では、前記差異速度の角速度成分に関する要素を、前記角速度データからを除去する請求項５〜７のいずれか一項に記載の方法。

【発明の詳細な説明】

【技術分野】

【0001】

この発明は、移動体の姿勢検出装置及び姿勢検出方法に関する。

【背景技術】

【0002】

例えば、特許文献１には、車両用灯具の光軸位置をオートレベリング制御により調節する車両用灯具の制御装置が記載されている。車両用灯具の光軸位置を制御するために、車両の姿勢が検出される。車両姿勢の検出のために、互いに直交するＸ軸、Ｙ軸及びＺ軸を有する３軸加速度センサが用いられている。センサのＸ軸が車両の前後軸と沿うように、センサのＹ軸が車両の左右軸と沿うように、センサのＺ軸が車両の上下軸と沿うように、車両に取り付けられている。そして、加速度センサは、３軸方向における加速度ベクトルの各成分の数値を出力し、これにより、水平面に対する車両の傾斜角度が検出される。水平面に対する車両の傾斜角度は、水平面に対する路面の傾斜角度である路面角度と、路面に対する車両の傾斜角度である車両姿勢角度との合計角度である。そして、車両用灯具の光軸位置の制御は、車両停止中では、車両が移動して路面角度が変化することが稀であるため、合計角度の変化が車両姿勢角度の変化であると推定して、実施される。また、車両走行中での車両用灯具の光軸位置の制御は、積載荷重や乗車人数が増減して車両姿勢角度が変化することは稀であるため、合計角度の変化を路面角度の変化であると推定して、実施される。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１２−３０７８２号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

特許文献１では、車両走行中では、車両姿勢角度が変化することは稀であるため、合計角度の変化を路面角度の変化であると推定している。しかしながら、走行中の車両には、加減速すること及び旋回することによって、ピッチングモーメントによる車両の前後間での姿勢変化、ローリングモーメントによる車両の左右間での姿勢変化、ヨーモーメントによる車両の旋回方向の姿勢変化が生じる。このため、特許文献１に記載のような推定方法を用いると、車両の姿勢の検出精度が低くなるという問題がある。

【0005】

この発明はこのような問題点を解決するためになされたものであり、車両つまり移動体の姿勢の検出精度の向上を図る姿勢検出装置及び姿勢検出方法を提供することを目的とする。

【課題を解決するための手段】

【0006】

上記の課題を解決するために、この発明に係る姿勢検出装置は、移動体に設けられ、移動体の姿勢を検出する姿勢検出装置において、直交する３軸に沿った直線加速度を検知する加速度センサと、直交する３軸まわりの角速度を検知する角速度センサと、移動体の進行速度データを受け取るように構成されると共に加速度センサ及び角速度センサから検知データを受け取るように構成され、移動体の姿勢を演算する演算部とを備え、演算部は、進行速度データと角速度センサからの検知角速度データとを用いた補正を伴って、加速度センサからの検知直線加速度データを用いて直線速度を算出し、直線速度と進行速度データとの差異である差異速度を用いて、角速度センサからの検知角速度データを補正して、移動体の姿勢角を算出する。

【0007】

演算部は、進行速度データと検知角速度データとを用いた補正では、差異速度と、進行速度データ及び検知角速度データに基づく移動体の遠心加速度とを用いてよい。
演算部は、進行速度データと検知角速度データとを用いた補正では、検知直線加速度データから遠心加速度に関する要素を除去した加速度を時間積分して得られる速度に対して、差異速度に関する要素を除去してよい。
演算部は、差異速度を用いた補正では、角速度センサからの検知角速度データから、差異速度の角速度成分に関する要素を除去してよい。

【0008】

また、この発明に係る姿勢検出方法は、移動体の姿勢を検出する方法において、移動体の進行速度データと、直交する３軸に沿った移動体の直線加速度データと、直交する３軸まわりの移動体の角速度データとを取得し、進行速度データと角速度データとを用いた補正を伴って、直線加速度データを用いて直線速度を算出し、直線速度と進行速度データとの差異である差異速度を用いて角速度データを補正して、移動体の姿勢角を算出する。

【0009】

進行速度データと角速度データとを用いた補正では、差異速度と、進行速度データ及び角速度データに基づく移動体の遠心加速度とを用いてよい。
進行速度データと角速度データとを用いた補正では、直線加速度から遠心加速度に関する要素を除去した加速度を時間積分して得られる速度に対して、差異速度に関する要素を除去してよい。
差異速度を用いた補正では、差異速度の角速度成分に関する要素を、角速度データからを除去してよい。

【発明の効果】

【0010】

この発明に係る姿勢検出装置及び姿勢検出方法によれば、移動体の姿勢の検出精度の向上が可能になる。

【図面の簡単な説明】

【0011】

【図1】本発明の実施の形態に係る姿勢検出装置の概略図である。

【図2】図１の姿勢検出装置の構成を示すブロック図である。

【図3】本発明の実施の形態に係る姿勢検出装置による姿勢角検出動作の流れを示す図である。

【発明を実施するための形態】

【0012】

以下、本発明の実施の形態に係る姿勢検出装置について添付図面に基づいて説明する。なお、本実施の形態では、姿勢検出装置は、移動体としての自動車等の車両に搭載されるものとして、説明する。
図１及び図２をあわせて参照すると、図示しない車両に搭載される姿勢検出装置１００は、基板２と、基板２上に実装されたＣＰＵ回路等によって構成される演算部３と、基板２上に実装された３軸ジャイロセンサ１０と、基板２上に実装された３軸加速度センサ２０とを１つの検出ユニット１として含む。本実施の形態では、検出ユニット１は、基板２を図示しない車両のフロアに平行な状態として、つまり車両が位置する地面に平行な状態として、車両に固定される。ここで、３軸ジャイロセンサ１０は、角速度センサを構成している。
さらに、姿勢検出装置１００は、検出ユニット１の外部に設けられた車速センサ４を含む。車速センサ４は、姿勢検出装置１００のために個別に設けられたものであってもよく、図示しない車両に標準に備えられている車速センサを兼用したものであってもよい。演算部３は、車速センサ４が検知する車両の速度情報を受け取るように構成されている。

【0013】

３軸ジャイロセンサ１０は、Ｘ軸周り角速度検知部１１と、Ｙ軸周り角速度検知部１２と、Ｚ軸周り角速度検知部１３とを備えている。Ｘ軸は、図示しない車両の前後方向に沿い且つ基板２の表面に沿う方向である。Ｙ軸は、図示しない車両の左右方向に沿い且つ基板２の表面に沿う方向である。Ｚ軸は、図示しない車両の上下方向に沿い且つ基板２の表面に垂直な方向である。Ｘ軸、Ｙ軸及びＺ軸は互いに直交する。よって、Ｘ軸周り角速度検知部１１は、Ｘ軸周りの車両のロール角の変化速度であるロール角速度を検知する。Ｙ軸周り角速度検知部１２は、Ｙ軸周りの車両のピッチング角の変化速度であるピッチング角速度を検知する。Ｚ軸周り角速度検知部１３は、Ｚ軸周りの車両のヨー角の変化速度であるヨー角速度を検知する。演算部３は、３軸ジャイロセンサ１０が検知する車両のロール角速度、ピッチング角速度及びヨー角速度の情報を受け取るように構成されている。

【0014】

３軸加速度センサ２０は、Ｘ軸方向加速度検知部２１と、Ｙ軸方向加速度検知部２２と、Ｚ軸方向加速度検知部２３とを備えている。Ｘ軸方向加速度検知部２１は、Ｘ軸に沿った図示しない車両の前後方向の直線加速度を検知する。Ｙ軸方向加速度検知部２２は、Ｙ軸に沿った車両の左右方向の直線加速度を検知する。Ｚ軸方向加速度検知部２３は、Ｚ軸に沿った車両の上下方向の直線加速度を検知する。演算部３は、３軸加速度センサ２０が検知する車両の前後方向直線加速度、左右方向直線加速度及び上下方向直線加速度の情報を受け取るように構成されている。

【0015】

次に、姿勢検出装置１００による車両の姿勢検出動作を説明する。
図２及び図３をあわせて参照すると、演算部３は、３軸加速度センサ２０から連続的に受け取る直線加速度αと、３軸ジャイロセンサ１０から連続的に受け取る角速度ωと、車速センサ４から連続的に受け取る車速νとを用いて、車両の姿勢角を連続的に算出する。

【0016】

車両の姿勢角を連続的に算出する過程において、演算部３は、既に算出した車両の姿勢角θを用いて、３軸加速度センサ２０から受け取る直線加速度αを、水平面に平行な直交する２つの軸と水平面に垂直な１つの軸とによる三次元座標系に、座標変換する。

【0017】

具体的には、直線加速度αは、Ｘ軸、Ｙ軸及びＺ軸成分として、α_Ｘ、α_Ｙ及びα_Ｚを含んでいる。α_Ｘ及びα_Ｙは、基板２に平行な軸成分であり、α_Ｚは、基板２に垂直な軸成分である。
そして、成分α_Ｘ、α_Ｙ及びα_Ｚと、姿勢角θの成分θ_Ｘ、θ_Ｙ及びθ_Ｚと用いて座標変換することによって、直線加速度αは、成分α_Ｘ’、α_Ｙ’及びα_Ｚ’を有する直線加速度α’となる。
なお、成分θ_Ｘ、θ_Ｙ及びθ_Ｚはそれぞれ、Ｘ軸、Ｙ軸及びＺ軸に対する姿勢角の成分である。さらに、成分α_Ｘ’は、Ｘ軸を水平面に投影したものであるＸ’軸の成分であり、成分α_Ｙ’は、Ｘ’軸と同一の水平面上でＸ’軸に垂直なＹ’軸の成分であり、成分α_Ｚ’は、Ｘ’軸及びＹ’軸に垂直である鉛直方向のＺ’軸の成分である。

【0018】

次いで、演算部３は、車両の直線速度Ｖを求める。
このとき、直線加速度α’（α_Ｘ’，α_Ｙ’，α_Ｚ’）に対して、車両の走行の加減速に起因する加減速加速度の影響と、車両の旋回走行及び登坂・降坂走行に起因する遠心加速度の影響とを排除し、各影響を排除した直線加速度を時間積分して直線速度が算出される。さらに、算出された直線速度と車速とを比較し、これらの間の誤差に基づき、算出された直線速度を補正し、この補正後の直線速度を求めるべき直線速度Ｖとする。

【0019】

具体的には、演算部３は、車両の走行の加減速に起因する加減速加速度の影響と、車両の旋回走行及び登坂・降坂走行に起因する遠心加速度の影響とを合わせた影響を、３軸ジャイロセンサ１０から受け取る角速度ωと車速νとを用いて算出される遠心加速度Ｇにより求める。

【0020】

なお、角速度ωは、Ｘ軸周り、Ｙ軸周り及びＺ軸周りそれぞれの角度方向の成分として、ω_Ｘ、ω_Ｙ及びω_Ｚを含む。車速νは、Ｘ軸、Ｙ軸及びＺ軸成分として、ν_Ｘ、ν_Ｙ及びν_Ｚを含むが、ν_Ｙ及びν_Ｚは、０である。遠心加速度Ｇは、Ｘ軸及びＹ軸を含むＸＹ平面、Ｙ軸及びＺ軸を含むＹＺ平面、並びに、Ｚ軸及びＸ軸を含むＺＸ平面それぞれに沿った加速度成分として、Ｇ_ＸＹ、Ｇ_ＹＺ及びＧ_ＺＸを含む。

【0021】

よって、加速度成分Ｇ_ＸＹ、Ｇ_ＹＺ及びＧ_ＺＸは、以下のとおりとなる。
Ｇ_ＸＹ＝（ν_Ｘ^２＋ν_Ｙ^２）^１／２ω_Ｚ＝ν_Ｘω_Ｚ
Ｇ_ＹＺ＝（ν_Ｙ^２＋ν_Ｚ^２）^１／２ω_Ｘ＝０
Ｇ_ＺＸ＝（ν_Ｚ^２＋ν_Ｘ^２）^１／２ω_Ｙ＝ν_Ｘω_Ｙ

【0022】

さらに、演算部３は、遠心加速度Ｇ（Ｇ_ＸＹ，Ｇ_ＹＺ，Ｇ_ＺＸ）を、Ｘ’軸、Ｙ’軸及びＺ’軸からなる座標系に座標変換して、遠心加速度Ｇ’（Ｇ_Ｘ’Ｙ’，Ｇ_Ｙ’Ｚ’，Ｇ_Ｚ’Ｘ’）を求める。Ｇ_Ｘ’Ｙ’、Ｇ_Ｙ’Ｚ’及びＧ_Ｚ’Ｘ’はそれぞれ、Ｘ’Ｙ’平面、Ｙ’Ｚ’平面及びＺ’Ｘ’平面に沿った加速度成分である。
よって、演算部３は、直線速度Ｖを求める際、直線加速度α’（α_Ｘ’，α_Ｙ’，α_Ｚ’）から、遠心加速度Ｇ’（Ｇ_Ｘ’Ｙ’，Ｇ_Ｙ’Ｚ’，Ｇ_Ｚ’Ｘ’）を減じる、つまり遠心加速度補正を行う。

【0023】

遠心加速度補正後の補正直線加速度α’_Ａ（α_ＡＸ’，α_ＡＹ’，α_ＡＺ’）は、以下のように求まる。
α_ＡＸ’＝α_Ｘ’−（Ｇ_Ｘ’Ｙ’^２＋Ｇ_Ｚ’Ｘ’^２）^１／２
α_ＡＹ’＝α_Ｙ’−（Ｇ_Ｘ’Ｙ’^２＋Ｇ_Ｙ’Ｚ’^２）^１／２
α_ＡＺ’＝α_Ｚ’−（Ｇ_Ｙ’Ｚ’^２＋Ｇ_Ｚ’Ｘ’^２）^１／２−ｇ
なお、ｇは、重力加速度である。

【0024】

さらに、演算部３は、補正直線加速度α’_Ａ（α_ＡＸ’，α_ＡＹ’，α_ＡＺ’）の各成分を時間積分して、対象とする直線速度Ｖの補正前直線速度である速度Ｖ１（Ｖ１_Ｘ’，Ｖ１_Ｙ’，Ｖ１_Ｚ’）を算出する。

【0025】

次いで、演算部３は、車速ν（ν_Ｘ，ν_Ｙ，ν_Ｚ）を、Ｘ’軸、Ｙ’軸及びＺ’軸からなる座標系に座標変換して、車速ν’（ν_Ｘ’，ν_Ｙ’，ν_Ｚ’）を求める。
そして、演算部３は、算出対象とする直線速度Ｖよりも前の時点での直線速度に対応する補正前直線速度Ｖ１ａと、当該時点での車速ν’との差異である差異速度Ｖｄａ’（Ｖｄａ_Ｘ’，Ｖｄａ_Ｙ’，Ｖｄａ_Ｚ’）を求める。

【0026】

演算部３は、算出対象とする直線速度Ｖに対応する補正前直線速度Ｖ１に対して、差異速度Ｖｄａを減じて直線速度Ｖを求める補正を行う、つまり補正速度計算を行う。このとき、直線速度Ｖ（Ｖ_Ｘ’，Ｖ_Ｙ’，Ｖ_Ｚ’）は、以下のとおりとなる。
Ｖ_Ｘ’＝Ｖ１_Ｘ’−Ｖｄａ_Ｘ’×Ｋ１
Ｖ_Ｙ’＝Ｖ１_Ｙ’−Ｖｄａ_Ｙ’×Ｋ１
Ｖ_Ｚ’＝Ｖ１_Ｚ’−Ｖｄａ_Ｚ’×Ｋ１
なお、Ｋ１は、定数であり、３軸ジャイロセンサ１０の性能、３軸加速度センサ２０の性能、車速センサ４の性能、検出ユニット１の取り付け位置などの様々な要素の状況に応じて、適宜設定される。
よって、車両の直線速度Ｖが、車両の走行の加減速に起因する加減速加速度の影響と、車両の旋回走行及び登坂・降坂走行に起因する遠心加速度の影響と、算出された直線速度と車速との間の誤差の影響とが排除されて、求められることになる。つまり、直線速度Ｖは、高い精度を有する。

【0027】

また、演算部３は、算出した直線速度Ｖと、この直線速度Ｖの時点に対応する時点での車速ν’との差異である差異速度Ｖｄ’（Ｖｄ_Ｘ’，Ｖｄ_Ｙ’，Ｖｄ_Ｚ’）を求める。差異速度Ｖｄ’（Ｖｄ_Ｘ’，Ｖｄ_Ｙ’，Ｖｄ_Ｚ’）は、以下のとおりとなる。
Ｖｄ_Ｘ’＝Ｖ_Ｘ’−ν_Ｘ’
Ｖｄ_Ｙ’＝Ｖ_Ｙ’−ν_Ｙ’
Ｖｄ_Ｚ’＝Ｖ_Ｚ’−ν_Ｚ’

【0028】

次いで、演算部３は、差異速度Ｖｄ’をＸ軸、Ｙ軸及びＺ軸からなる座標系に座標変換して、差異速度Ｖｄ（Ｖｄ_Ｘ，Ｖｄ_Ｙ，Ｖｄ_Ｚ）を求め、さらに、差異速度ＶｄからＸ軸周り、Ｙ軸周り及びＺ軸周りの角速度成分ωｄ_Ｘ，ωｄ_Ｙ，ωｄ_Ｚからなる差異角速度ωｄを算出する。差異角速度ωｄ（ωｄ_Ｘ，ωｄ_Ｙ，ωｄ_Ｚ）は、以下のとおりとなる。なお、ｒは、定数であり、３軸ジャイロセンサ１０の構成に応じて定められる。
ωｄ_Ｘ＝（Ｖｄ_Ｙ^２＋Ｖｄ_Ｚ^２）^１／２／ｒ
ωｄ_Ｙ＝（Ｖｄ_Ｘ^２＋Ｖｄ_Ｚ^２）^１／２／ｒ
ωｄ_Ｚ＝（Ｖｄ_Ｘ^２＋Ｖｄ_Ｙ^２）^１／２／ｒ

【0029】

次いで、演算部３は、３軸ジャイロセンサ１０から受け取る角速度ω（ω_Ｘ，ω_Ｙ，ω_Ｚ）の各成分から差異角速度ωｄ（ωｄ_Ｘ，ωｄ_Ｙ，ωｄ_Ｚ）を減じる補正角速度計算を行って、補正角速度ωａ（ωａ_Ｘ，ωａ_Ｙ，ωａ_Ｚ）を算出する。補正角速度ωａの各角速度成分は、以下のとおりとなる。
ωａ_Ｘ＝ω_Ｘ−Ｋ２×ωｄ_Ｘ
ωａ_Ｙ＝ω_Ｙ−Ｋ２×ωｄ_Ｙ
ωａ_Ｚ＝ω_Ｚ−Ｋ２×ωｄ_Ｚ
なお、Ｋ２は、定数であり、３軸ジャイロセンサ１０の性能、３軸加速度センサ２０の性能、車速センサ４の性能、検出ユニット１の取り付け位置などの様々な要素の状況に応じて、適宜設定される。
よって、補正角速度ωａ（ωａ_Ｘ，ωａ_Ｙ，ωａ_Ｚ）は、３軸ジャイロセンサ１０と３軸加速度センサ２０とのみを用いて速度を算出する際に生まれる誤差、つまり、３軸ジャイロセンサ１０と３軸加速度センサ２０とのみを用いて算出される結果に生まれる誤差の影響を、角速度ω（ω_Ｘ，ω_Ｙ，ω_Ｚ）に対して排除したものである。

【0030】

さらに、演算部３は、補正角速度ωａ（ωａ_Ｘ，ωａ_Ｙ，ωａ_Ｚ）に対して時間積分を含むクォータニオン積算を実施し、図示しない車両つまり検出ユニット１の姿勢角θのＸ軸、Ｙ軸及びＺ軸に対する方向余弦ｃｏｓθ_Ｘ、ｃｏｓθ_Ｙ及びｃｏｓθ_Ｚを算出する。演算部３は、算出した方向余弦ｃｏｓθ_Ｘ、ｃｏｓθ_Ｙ及びｃｏｓθ_Ｚに基づき、Ｘ軸、Ｙ軸及びＺ軸に対する姿勢角θの各角度成分θ_Ｘ、θ_Ｙ及びθ_Ｚを算出して、姿勢変化量及び姿勢角を求める。

【0031】

上述で説明したように、本発明の実施の形態に係る姿勢検出装置１００は、車両に設けられて車両の姿勢を検出するものであり、直交する３軸に沿った直線加速度を検知する３軸加速度センサ２０と、直交する３軸まわりの角速度を検知する３軸ジャイロセンサ１０と、車両の進行速度データである車速データを受け取るように構成されると共に３軸加速度センサ２０及び３軸ジャイロセンサ１０から検知データを受け取るように構成され且つ車両の姿勢を演算する演算部３とを備える。演算部３は、車速データと３軸ジャイロセンサ１０からの検知角速度データとを用いた補正を伴って、３軸加速度センサ２０からの検知直線加速度データを用いて直線速度を算出し、直線速度と車速データとの差異である差異速度を用いて、３軸ジャイロセンサ１０からの検知角速度データを補正して、車両の姿勢角を算出する。さらに、演算部３は、車速データと検知角速度データとを用いた補正では、差異速度と、車速データ及び検知角速度データに基づく車両の遠心加速度とを用いる。

【0032】

上述の構成において、演算部３は、３軸加速度センサ２０からの検知直線加速度データから直線速度を算出する際に、車速データと３軸ジャイロセンサ１０からの検知角速度データとを用いた補正を行う、つまり、車両の実際の進行速度に関連する要素及び車両の遠心加速度に関連する要素とを用いた補正を行う。これによって、算出した直線速度において、車両の実際の進行速度に対する差異を補正し、車両の遠心加速度に関連する影響を排除することができる。さらに、差異速度を用いて補正することによって、補正した検知角速度データでは、車両の実際の進行速度に対する差異に関連する実際の角速度に対する誤差を排除することができる。よって、車両の姿勢角の高精度な検出が可能になる。

【0033】

また、姿勢検出装置１００において、演算部３は、車速データと検知角速度データとを用いた補正では、検知角速度データから遠心加速度に関する要素を除去した加速度を時間積分して得られる速度に対して、差異速度に関する要素を除去する。これによって、高精度な結果をもたらす補正計算を簡易に実施することができる。
また、姿勢検出装置１００において、演算部３は、差異速度を用いた補正では、検知角速度データから、差異速度の角速度成分に関する要素を除去する。これによって、高精度な結果をもたらす差異速度を用いた補正を、簡易に実施することができる。

【0034】

また、実施の形態に係る姿勢検出装置１００では、演算部３は、補正速度計算実施後の直線速度と車速との差異速度を用いて、補正角速度計算を行っていたが、これに限定されるものでない。演算部３は、補正前直線速度と車速との差異速度を補正角速度計算に用いてもよい。
また、実施の形態に係る姿勢検出装置１００では、演算部３は、補正角速度計算に関して、３軸ジャイロセンサ１０から受け取る角速度ωの各成分から差異角速度ωｄを減じたものに対して時間積分を含むクォータニオン積算を実施していたが、これに限定されるものでない。演算部３は、角速度ωと差異角速度ωｄとに対して個別に時間積分を含むクォータニオン積算を実施した後に、それぞれの姿勢角の方向余弦から車両の姿勢角を算出してもよい。

【0035】

また、実施の形態に係る姿勢検出装置１００では、演算部３は、ＸＹＺ軸による座標系とＸ’Ｙ’Ｚ’軸による座標系との間で、座標変換を実施していたが、座標変換を実施せずにＸＹＺ軸による座標系のみを用いて演算してもよい。
また、実施の形態に係る姿勢検出装置１００では、３軸ジャイロセンサ１０と３軸加速度センサ２０とは、同じのＸ軸、Ｙ軸及びＺ軸を共有していたが、互いのＸ軸、Ｙ軸及びＺ軸が異なっていてもよい。この場合、演算部３は、演算時、３軸ジャイロセンサ１０及び３軸加速度センサ２０のうちの一方に対して、検知データの座標変換をすればよい。
また、実施の形態に係る姿勢検出装置１００は、車両に搭載されていたが、いかなる移動体に搭載されてもよい。

【符号の説明】

【0036】

１ 検知ユニット、３ 演算部、４ 車速センサ、１０ ３軸ジャイロセンサ（角速度センサ）、２０ ３軸加速度センサ、１００ 姿勢検出装置。

【図1】

【図2】

【図3】