特開 2022-188571 設置角算出装置、及び設置角算出プログラム

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022188571

(43)【公開日】2022-12-21

(54)【発明の名称】設置角算出装置、及び設置角算出プログラム

(51)【国際特許分類】

   G01C 21/12 20060101AFI20221214BHJP

   G01C 21/26 20060101ALI20221214BHJP

【ＦＩ】

G01C21/12

G01C21/26 A

【審査請求】未請求

【請求項の数】7

【出願形態】ＯＬ

(21)【出願番号】P 2021096710

(22)【出願日】2021-06-09

(71)【出願人】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(71)【出願人】

【識別番号】000004260

【氏名又は名称】株式会社デンソー

(74)【代理人】

【識別番号】110001519

【氏名又は名称】弁理士法人太陽国際特許事務所

(72)【発明者】

【氏名】鈴木 徳祥

(72)【発明者】

【氏名】宝地 卓

【テーマコード（参考）】

2F129

【Ｆターム（参考）】

2F129AA03

2F129BB23

2F129BB24

2F129BB25

2F129BB26

(57)【要約】

【課題】路面の傾斜に関わらず、車両直進方向に対する設置角を精度よく算出することができる。
【解決手段】加速度センサを用いて車両直進方向に対するピッチ方向設置角を算出する設置角算出装置は、直進かつ加減速が所定の値より大きい加減速大の状態であることを判定する運動状態判定手段、重力加速度方向に対するピッチ姿勢角及びロール姿勢角を少なくとも含む姿勢角を算出する姿勢角算出手段、算出された前記姿勢角に基づき、加速度センサの出力から重力加速度成分を少なくとも含む成分を補正して、補正後の加速度センサ値を求める補正手段、及び直進かつ前記加減速大と判定された時に、補正後の加速度センサ値を用いて路面が傾斜している場合も車両直進方向に対するピッチ方向設置角を算出する設置角推定手段、を含む。
【選択図】図１

【特許請求の範囲】

【請求項1】

加速度センサを用いて車両直進方向に対するピッチ方向設置角を算出する設置角算出装置において、
直進かつ加減速が所定の値より大きい加減速大の状態であることを判定する運動状態判定手段、
重力加速度方向に対するピッチ姿勢角及びロール姿勢角を少なくとも含む姿勢角を算出する姿勢角算出手段、
算出された前記姿勢角に基づき、加速度センサの出力から重力加速度成分を少なくとも含む成分を補正して、補正後の加速度センサ値を求める補正手段、及び
直進かつ前記加減速大と判定された時に、補正後の加速度センサ値を用いて路面が傾斜している場合も車両直進方向に対するピッチ方向設置角を算出する設置角推定手段、
を含む設置角算出装置。

【請求項2】

前記補正手段は、前記成分として重力加速度成分、及びゼロ点誤差成分を差し引くように補正する請求項１に記載の設置角算出装置。

【請求項3】

前記加速度センサとして、上下加速度センサ、及び前後加速度センサの２軸を備え、
直進かつ加減速状態であることを判定するため、ヨーレートと前記加速度センサの出力とを用い、
直進かつ加減速大と判定された時に、前記成分を補正後の上下加速度と前後加速度との比から車両直進方向に対するピッチ方向設置角を算出する、請求項１又は請求項２に記載の設置角算出装置。

【請求項4】

前記加速度センサとして、上下加速度センサ、前後加速度センサ、及び左右加速度センサの３軸を備え、
補正後の加速度センサ値を用いて、上下加速度と前後加速度の比から車両直進方向に対するピッチ方向設置角を、左右加速度と前後加速度及び上下加速度の二乗和の平方根との比から車両直進方向に対するヨー方向設置角を算出する、請求項１から請求項３の何れか１項に記載の設置角算出装置。

【請求項5】

前記直進の判定に、ヨーレートの閾値による判定、ヨーレートと車速の閾値による判定、及び舵角の閾値による判定の何れかを組み合わせて用いる請求項１から請求項４の何れか１項に記載の設置角算出装置。

【請求項6】

前記加減速大の判定に、前後加速度の閾値による判定、車速の平均変化量の閾値による判定、加速度を用いた二乗和の平方根の閾値による判定、及び操作状態による判定の何れかを組み合わせて用いる請求項１から請求項５の何れか１項に記載の設置角算出装置。

【請求項7】

加速度センサを用いて車両直進方向に対するピッチ方向設置角を算出する設置角算出の処理において、
直進かつ加減速が所定の値より大きい加減速大の状態であることを判定し、
重力加速度方向に対するピッチ姿勢角及びロール姿勢角を少なくとも含む姿勢角を算出し、
算出された前記姿勢角に基づき、加速度センサの出力から重力加速度成分を少なくとも含む成分を補正して、補正後の加速度センサ値を求め、
直進かつ前記加減速大と判定された時に、補正後の加速度センサ値を用いて路面が傾斜している場合も車両直進方向に対するピッチ方向設置角を算出する、
処理をコンピュータに実行させる設置角算出プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、設置角算出装置、及び設置角算出プログラムに関する。

【背景技術】

【0002】

車両に取り付けられた装置で計測したセンサ値は、車両に設置する位置や座標軸が想定とずれているために発生する誤差（取り付け角度、取り付け位置）が含まれるため、このような誤差を含むセンサ値を補正する技術がある。

【0003】

例えば、車載装置の取付け角度が変更された場合において車載装置による処理の精度を維持する取付け角度検出装置を提供する技術がある（特許文献１参照）。この技術では、例えば車両が停止している時や一定速で走行している時、すなわち進行方向の加速度成分がゼロの時における加速度センサの出力を積算し、平均化させた値を算出することにより、ピッチ姿勢角を算出する。

【0004】

また、実走行状態において、電子機器の筐体の取付け角度を算出することのできる取付け角度算出装置を提供する技術がある（特許文献２参照）。この技術では、車両が水平面上を加速度Ａで走行していることを前提として、筐体の取付け角度の候補φ’を算出している。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特許４７３９３７８号

【特許文献2】特開２００７－１０７９５１号公報

【発明の概要】

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

【0006】

特許文献１及び特許文献２の従来技術は、いずれも水平面を走行していることを前提として推定を行っている。そのため、勾配のある路面走行時は、取付け角推定誤差が発生するという問題がある。一般の路面は完全な水平面となる場合は少なく、ほとんどの場合、勾配があるため、誤差要因となる。

【0007】

本発明は、上記事情を鑑みて成されたものであり、路面の傾斜に関わらず、車両直進方向に対する設置角を精度よく算出することができる設置角算出装置、及び設置角算出プログラムを提供することを目的とする。

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

【0008】

上記目的を達成するために、本発明に係る設置角算出装置は、加速度センサを用いて車両直進方向に対するピッチ方向設置角を算出する設置角算出装置であって、直進かつ加減速が所定の値より大きい加減速大の状態であることを判定する運動状態判定手段、重力加速度方向に対するピッチ姿勢角及びロール姿勢角を少なくとも含む姿勢角を算出する姿勢角算出手段、算出された前記姿勢角に基づき、加速度センサの出力から重力加速度成分を少なくとも含む成分を補正して、補正後の加速度センサ値を求める補正手段、及び直進かつ前記加減速大と判定された時に、補正後の加速度センサ値を用いて路面が傾斜している場合も車両直進方向に対するピッチ方向設置角を算出する設置角推定手段、を含んで構成されている。

【発明の効果】

【0009】

本発明の設置角算出装置、及び設置角算出プログラムによれば、路面の傾斜に関わらず、車両直進方向に対する設置角を精度よく算出することができる。

【図面の簡単な説明】

【0010】

【図1】第１実施形態の設置角算出装置の構成を示すブロック図である。

【図2】設置角算出装置のハードウェア構成を示すブロック図である。

【図3】設置角算出装置による設置角算出処理の流れを示すフローチャートである。

【図4】本発明の実施形態の手法で推定するピッチ方向設置角を説明するための図である。

【図5】交差点等における停車のための減速区間の重力加速度への影響を想定した場合において、加速度センサの出力と、姿勢角の影響との関係の一例を表したグラフである。

【図6】Ｇ_ｘの向きと水平面との関係を示す図である。

【図7】第２実施形態の設置角算出装置の構成を示すブロック図である。

【図8】車体のヨー方向設置角にずれがない場合とある場合とを示した図である。

【図9】第１実施形態の構成に追加して、姿勢角推定に車速、及びヨーレートを用いる場合の構成例である。

【図10】第２実施形態の構成に追加して、姿勢角推定に車速、及びヨーレートを用いる場合の構成例である。

【図11】検証において、重力加速度成分の補正後の加速度センサ値である。

【図12】検証において、ヨー方向設置角を推定した場合の例である。

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

【0011】

以下、図面を参照して本発明の実施形態を詳細に説明する。

【0012】

本発明の実施形態について説明する前に、従来技術の問題点の詳細について説明する。

【0013】

特許文献１の技術は、水平面を走行していることを前提としており、路面勾配等による車両姿勢角（ピッチ姿勢角及びロール姿勢角）変化の影響を考慮していない。停車、又は一定速走行時の重力加速度成分と前後加速度センサとを比較して取付けピッチ姿勢角を求めているが、水平面に対するピッチ姿勢角を求めて、これを取付け角としている。また参照加速度と記されている車両の加速度は水平方向加速度であることが前提となっている。このことから、勾配のある路面走行時は、取付け角推定誤差が発生するという問題がある。一般の路面は完全な水平面となる場合は少なく、ほとんどの場合、勾配があるため、誤差要因となる。

【0014】

特許文献２の技術は、水平面ではない場合を含む実走行状態を想定しているが、水平面を走行している（＝カーブの走行角度と地図角度とが一致する）と判定された場合のみ取付け角度の候補を蓄積している。このことから、結果的に、水平面を走行していることを前提として推定している。そのため、水平面を前途した推定では、路面勾配の影響で推定誤差が発生するという問題がある。

【0015】

また、上記、特許文献１及び特許文献２の従来技術では、いずれもゼロ点誤差が考慮されていないという問題がある。加速度センサのゼロ点誤差やジャイロのゼロ点誤差といった、センサ誤差の影響については考慮されていないため、センサ誤差の影響により、取付け角の推定に誤差が発生する問題がある。以上が従来技術の問題点における課題であり、本発明の実施形態の手法によって解決する課題である。

【0016】

本発明の実施形態の概要を説明する。車体に慣性センサ（ＩＭＵ：Inertial Measurement Unit）を設置する場合、組付け誤差等により、車体の座標軸とＩＭＵの座標軸との間に設置角のずれが発生する。本発明の実施形態の手法により、車体に搭載した慣性センサの設置角を推定及び補正することで、車両の姿勢及び運動をより精度よく推定することが可能になる。また、路面が傾斜している場合でも、車両直進方向に対する設置角を精度よく求めることができる。

【0017】

設置角として、前後加速度Ｇ_ｘの軸が車両直進時の進行方向に一致していれば、車両が直進加減速を行う際の加速度の影響は、前後加速度Ｇ_ｘのみに表れ、左右加速度Ｇ_ｙ及び上下加速度Ｇ_ｚには表れない。左右加速度Ｇ_ｙ及び上下加速度Ｇ_ｚには重力加速度成分とゼロ点誤差のみが表れる。ピッチ方向の設置角のずれがある場合は、直進加減速時の加速度成分がＧ_ｚ側に一定比率で出力される。ヨー方向の設置角のずれがある場合は、同様にＧ_ｙ側に一定比率で出力される。

【0018】

上記出力を、直進時かつ加減速時の加速度が大きい場合のみを判定及び抽出して平均化することで、雑音の影響を低減し、推定精度を向上する。また、姿勢角に基づいて重力加速度成分とゼロ点誤差の影響を差し引くことで、車両の走行による加減速の影響だけを抽出し、推定精度を向上する。

【0019】

以下、第１実施形態の設置角算出装置の基本的な構成及び作用を説明した後、詳細な原理を説明する。

【0020】

（第１実施形態）
図１は、第１実施形態の設置角算出装置１０の構成を示すブロック図である。図１に示すように、設置角算出装置１０は、ジャイロセンサ９０と、加速度センサ９２と、運動状態判定手段１１０と、姿勢角算出手段１１２と、補正手段１１４と、設置角推定手段１１６とを含んで構成されている。運動状態判定手段１１０には、直進旋回判定部１２０と、加減速大判定部１２２とからなる。なお、姿勢角、補正後の加速度センサ値、及びピッチ方向設置角θ_Ｅの算出の式については、後述する原理の説明で述べる。

【0021】

慣性センサとして、ジャイロセンサ９０と、加速度センサ９２とを車両に設置する。ジャイロセンサ９０は、ヨーレートを出力する。加速度センサ９２は、上下加速度センサ、及び前後加速度センサの２軸を備え、センサ値として前後加速度Ｇ_ｘ、及び上下加速度Ｇ_ｚを出力する。なお、ここで出力されるセンサ値は補正前のセンサ値である。

【0022】

運動状態判定手段１１０は、直進かつ加減速大の状態であることを判定する。判定の条件は、例えば、ヨーレートが閾値以下、かつ、前後加速度Ｇ_ｘが閾値以上の場合に、加減速大の状態であると判定することとする。直進旋回判定部１２０でヨーレートの閾値以下であるかを判定し、加減速大判定部１２２で前後加速度Ｇ_ｘが閾値以上であるかを判定する。

【0023】

姿勢角算出手段１１２は、加速度センサ９２の出力を受信し、姿勢角として、重力加速度方向に対するピッチ姿勢角及びロール姿勢角を算出する。

【0024】

なお、ジャイロセンサ９０からピッチレート、ロールレートを姿勢角算出手段１１２へ出力し、相補フィルタによる姿勢角推定をしてもよい。相補フィルタによる姿勢角推定は、平均的には加速度センサ９２の出力に基づいて姿勢角推定を行い、車両運動により姿勢角が変動する場合はジャイロセンサ９０の出力が姿勢角推定の主体となる。そのため、加速度センサ９２だけの場合に比べて姿勢角推定精度が改善できる。また、カルマンフィルタを用いて姿勢角を推定しても良い。

【0025】

補正手段１１４は、算出されたピッチ姿勢角及びロール姿勢角に基づき、加速度センサ９２の出力から重力加速度成分、及びゼロ点誤差成分の影響を補正して、補正後の加速度センサ値（Ｇ^＊ｘ及びＧ^＊ｚ）を求める。

【0026】

設置角推定手段１１６は、直進かつ加減速大と判定された時に、補正後の加速度センサ値を用いて路面が傾斜している場合も車両直進方向に対するピッチ方向設置角θ_Ｅを算出する。ここでは、補正後の上下加速度と前後加速度との比から車両直進方向に対するピッチ方向設置角θ_Ｅを算出する。

【0027】

図２は、設置角算出装置１０のハードウェア構成を示すブロック図である。なお、ハードウェア構成は図２の例に限定されるものではない。

【0028】

図２に示すように、設置角算出装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１３、ストレージ１４、入力部１５、表示部１６及び通信インタフェース（Ｉ／Ｆ）１７を有する。各構成は、バス１９を介して相互に通信可能に接続されている。

【0029】

ＣＰＵ１１は、中央演算処理ユニットであり、各種プログラムを実行したり、各部を制御したりする。すなわち、ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４からプログラムを読み出し、ＲＡＭ１３を作業領域としてプログラムを実行する。ＣＰＵ１１は、ＲＯＭ１２又はストレージ１４に記憶されているプログラムに従って、上記各構成の制御及び各種の演算処理を行う。本実施形態では、ＲＯＭ１２又はストレージ１４には、設置角算出プログラムが格納されている。

【0030】

ＲＯＭ１２は、各種プログラム及び各種データを格納する。ＲＡＭ１３は、作業領域として一時的にプログラム又はデータを記憶する。ストレージ１４は、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）又はＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）等の記憶装置により構成され、オペレーティングシステムを含む各種プログラム、及び各種データを格納する。

【0031】

入力部１５は、マウス等のポインティングデバイス、及びキーボードを含み、各種の入力を行うために使用される。

【0032】

表示部１６は、例えば、液晶ディスプレイであり、各種の情報を表示する。表示部１６は、タッチパネル方式を採用して、入力部１５として機能してもよい。

【0033】

通信インタフェース１７は、端末等の他の機器と通信するためのインタフェースであり、例えば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。

【0034】

次に、本発明の実施形態に係る設置角算出装置１０の作用について説明する。

【0035】

図３は、設置角算出装置１０による設置角算出処理の流れを示すフローチャートである。ＣＰＵ１１がＲＯＭ１２又はストレージ１４から設置角算出プログラムを読み出して、ＲＡＭ１３に展開して実行することにより、設置角算出処理が行なわれる。なお、設置角算出処理は、ジャイロセンサ９０、及び加速度センサ９２の出力を元に行われる。

【0036】

ステップＳ１００では、ＣＰＵ１１は、姿勢角算出手段１１２として、姿勢角として、重力加速度方向に対するピッチ姿勢角及びロール姿勢角を算出する。

【0037】

ステップＳ１０２では、ＣＰＵ１１は、補正手段１１４として、算出されたピッチ姿勢角及びロール姿勢角に基づき、加速度センサ９２の出力から重力加速度成分、及びゼロ点誤差成分の影響を補正して、補正後の加速度センサ値（Ｇ^＊ｘ及びＧ^＊ｚ）を求める。

【0038】

ステップＳ１０４では、ＣＰＵ１１は、運動状態判定手段１１０として、直進かつ加減速が閾値より大きい加減速大の状態であるか否かを判定する。加減速大の状態である場合には、ステップＳ１０６へ移行する。加減速大の状態でない場合には、ステップＳ１００に戻って処理を繰り返す。

【0039】

ステップＳ１０６では、ＣＰＵ１１は、補正手段１１４として、補正後の加速度センサ値のセンサ比（Ｇ^＊ｚ/Ｇ^＊ｘ）を出力する。センサ比により、ピッチ方向設置角θ_Ｅの正接が求められる（式については後述する）。

【0040】

ステップＳ１０８では、ＣＰＵ１１は、設置角推定手段１１６として、センサ比を用いて、ピッチ方向設置角θ_Ｅを算出して出力し、処理を終了する。ピッチ方向設置角θ_Ｅは、センサ比（Ｇ^＊ｚ/Ｇ^＊ｘ）の逆正接（ａｒｃｔａｎ）により算出できる。

【0041】

センサ比（Ｇ^＊ｚ/Ｇ^＊ｘ）は雑音成分の影響があるため、十分なサンプル数（長時間）で平均化しても良い。十分なサンプル数で平均化することにより、センサ比（Ｇ^＊ｚ/Ｇ^＊ｘ）及びその逆正接は、ほぼ一定値になる。平均化は逆正接を取る前でも後でも良い。以上が設置角算出装置１０の作用の説明である。

【0042】

次に、本発明の実施形態の手法の原理について説明する。

【0043】

図４は、本発明の実施形態の手法で推定するピッチ方向設置角を説明するための図である。ピッチ方向設置角θ_Ｅ、水平面に対するＩＭＵのピッチ姿勢角θ_ｇ、路面勾配θ_Ｒについて、図４の場合、θ_ｇ＝θ_Ｒ＋θ_Ｅとなる。本発明の実施形態では、θ_Ｅのピッチ方向設置角を推定する。これに対して、従来技術では水平面を走行する際のピッチ姿勢角θ_ｇをピッチ方向の取付け角としている。水平面走行時は、θ_Ｒ＝０となるためである。

【0044】

ここで図４に示したピッチ方向設置角θ_ｇと車両直進方向とにずれがある場合を想定して、補正の処理について説明する。［１］車両が傾斜している路面で停車している場合、加速度センサ９２からは、水平面に対するピッチ姿勢角θ_ｇに対応した以下（１）式の重力加速度成分が出力される。ロール姿勢角は０とする。

【数1】

・・・（１）

【0045】

［２］傾斜路面上を車両が走行加速度Ｇ_Ｖで直進走行する場合、加速度センサ９２の出力は、以下（２）式のように、ピッチ方向設置角θ_Ｅに対応した加速度成分＜１＞、（１）式の重力加速度成分＜２＞、の和となる。

【数2】

・・・（２）

【0046】

［３］補正手段１１４で、以下（３）式のように、ピッチ姿勢角θ_ｇに基づいて重力加速度成分を差し引いて補正することで、補正後の加速度センサ値は＜１＞の加速度成分のみを取り出せる。

【数3】

・・・（３）

【0047】

［４］以下（４）式のように、補正後の加速度センサ値の比を取ることで、ピッチ方向設置角の正接が求められる。

【数4】

・・・（４）

【0048】

なお、加速度センサ９２の出力に雑音成分が加わっている場合、走行加速度Ｇ_Ｖが大きい値であるほど、雑音の影響を受けにくくなる。そのため、走行加速度Ｇ_Ｖがある閾値以上の時に上記計算を行うことで、精度良くピッチ方向設置角θ_Ｅを算出することができる。

【0049】

次に、重力加速度成分の補正の例を説明する。図５は、交差点等における停車のための減速区間の重力加速度への影響を想定した場合において、加速度センサ９２の出力と、姿勢角の影響との関係の一例を表したグラフである。図５の上段のグラフが車両の減速による出力、下段のグラフが車両の姿勢角による重力加速度成分による出力を示している。両者の和が加速度センサ９２の出力になる。そのため補正では、姿勢角による重力加速度成分の影響を差し引く必要がある。直進又は加減速時の車両の姿勢角変化は、主に「加減速による慣性と路面縦勾配による車体のピッチ角変動」、及び「路面横勾配による車体のロール角変動」が考えられる。重力加速度成分の補正では、図５の下段の成分（姿勢角に基づく成分）を差し引いて補正することによって、ピッチ角方向の設置角誤差をより精度よく推定できる。なお、姿勢角推定において加速度センサ９２の出力に含まれるゼロ点誤差については後述する。

【0050】

加速度センサ９２の出力は、次の［１］～［３］の３つの成分の和として得られる。［１］（加速度センサ９２の）姿勢角による重力加速度成分、［２］回転（旋回）運動の遠心力に対応する成分、［３］並進運動の加速度成分、である。以下（５）式は、３つの成分の和による重力加速度成分の出力を示す。

【数5】

・・・（５）

【0051】

θはピッチ姿勢角、φはロール姿勢角、Ｕは前後速度、Ｖは左右速度、Ｗは上下速度、Ｐはロール角速度、Ｑはピッチ角速度、Ｒはヨー角速度である。

【0052】

車両の運動を考えた場合、停車時および定常走行（一定速度で直進）時は［１］姿勢角重力成分のみとなるため、加速度センサ９２の出力から容易に姿勢角を求めることができる。直進走行中は［２］回転（旋回）運動に対応する成分は無視して良い。直進かつ加減速時は、［３］の成分は車両の進行方向に沿って加速度Ｇ_Ｖが発生する。

【0053】

直進かつ加減速時において、車両直進方向に対して慣性センサの設置角にずれがある場合、上記［３］の成分である車両の前後加速度Ｇ_Ｖの成分の一部が、設置角のずれによりＧ_ｙ，Ｇ_ｚにも出力され、その分Ｇ_ｘの出力も変化する。この性質を利用して、設置角を推定する。

【0054】

補正手段１１４の重力加速度成分の補正では、上記［１］の姿勢角による重力加速度成分を除去することにより、上記［３］の成分だけを取り出すことで、設置角の推定精度を向上させることができる。なお、姿勢角推定に誤差があると、重力加速度成分が精度よく除去できない問題がある。姿勢角推定の誤差要因としては、加速度センサ９２のゼロ点誤差がある。

【0055】

重力加速度成分の除去は、姿勢角（ピッチ姿勢角・ロール姿勢角）を推定しておき、推定ピッチ姿勢角θ＾、推定ロール姿勢角φ＾から下記（６）式で求められる重力加速度成分を差し引けば良い（推定ピッチ姿勢角θ＾、及び推定ロール姿勢角φ＾の記号は、式ではθ及びφの上に＾付きで表される）。

【数6】

・・・（６）

【0056】

この時に、入力される推定姿勢角（θ＾，φ＾）に加速度センサ９２のゼロ点誤差の影響が含まれている場合であっても、重力加速度成分と共にゼロ点誤差の影響が差し引かれることを次に説明する。

【0057】

姿勢角推定と加速度ゼロ点誤差の関係について、以下、Ｇ_ｘ、Ｇ_ｙ、Ｇ_ｚのそれぞれのゼロ点誤差について説明する。なお、姿勢角推定では、重力加速度方向基準のピッチ姿勢角及びロール姿勢角を推定する。この時、加速度センサ９２の出力Ｇ_ｘ、Ｇ_ｙ、Ｇ_ｚにゼロ点誤差が含まれていると、停車時であってもＧ_ｘ ^２＋Ｇ_ｙ ^２＋Ｇ_ｚ ^２＝ｇ^２が成立しない場合がある。この場合は、Ｇ_ｘ ^２＋Ｇ_ｙ ^２＋Ｇ_ｚ ^２＝ｇ^２が成立するように、例えば、Ｇ_ｚを補正しておき、姿勢角推定、及び重力加速度成分の補正を行えば良い。

【0058】

Ｇ_ｘについて、ゼロ点誤差がない場合と、ある場合とを比較して説明する。なお、Ｇ_ｘの向きと水平面との関係を図６に示す。

【0059】

Ｇ_ｘにゼロ点誤差がない場合について説明する。停車時にピッチ姿勢角θの時、Ｇ_ｘに表れる重力成分は、以下（７－１）式となる。ここからピッチ姿勢角θを推定することができる。

【数7】

・・・（７－１）

【0060】

ピッチ姿勢角θの状態で、Ｇ_ｘ軸方向に車両運動による加速度Ｇ_Ｖｘが加わる時、加速度センサ９２で検出される値は、以下（７－２）式となる。

【数8】

・・・（７－２）

【0061】

ピッチ姿勢角θが分かっていれば、以下（７－３）式のように、重力加速度成分を差し引くことで、車両加速度が求められる。

【数9】

・・・（７－３）

【0062】

次に、Ｇ_ｘにゼロ点誤差がある場合について説明する。停車時にピッチ姿勢角θの時、Ｇ_ｘに表れる重力成分にゼロ点誤差Ｚ_ｘが加わるため、以下、（８－１）式となる。

【数10】

・・・（８－１）

【0063】

推定ピッチ姿勢角θ＾は、ゼロ点誤差の影響で正しいピッチ姿勢角θからは誤差を持って推定される。この時、正しいピッチ姿勢角θは不明である。

【0064】

ピッチ姿勢角θの状態で、Ｇ_ｘ軸方向に車両運動による加速度Ｇ_Ｖｘが加わる時、加速度センサ９２で検出される値は、以下（８－２）式で表される。

【数11】

・・・（８－２）

【0065】

以下（８－３）式のように、推定ピッチ姿勢角θ＾に基づく重力加速度成分Ｇ’_ｇｘ（＝－ｇ・ｓｉｎθ＾）を差し引くと、車両加速度が求められる。

【数12】

・・・（８－３）

【0066】

（７－３）式と（８－３）式を比べると求められる車両加速度は同じであることから、加速度センサ９２の前後加速度Ｇ_ｘにゼロ点誤差Ｚ_ｘが含まれていても、ゼロ点誤差がない時と同様に、車両加速度が正しく求められることが分かる。以上から、推定ピッチ姿勢角θ＾に基づく重力加速度成分にはＧ_ｘのゼロ点誤差の影響が含まれており、Ｇ_ｘの検出値から両者を合わせて差し引くことができることが分かる。この時、ゼロ点誤差の値は未知のままで求める必要がない。よって、Ｇ_ｘのゼロ点誤差は補正が不要である。

【0067】

Ｇ_ｘと同様にして、加速度センサ９２の出力Ｇ_ｙにゼロ点誤差が含まれていても、車両加速度を正しく求めることができる。Ｇ_ｙを考える際には、推定ピッチ姿勢角、推定ロール姿勢角に基づく重力加速度成分を差し引くことになる。以下、Ｇ_ｙについて、ゼロ点誤差がない場合と、ある場合とを比較して説明する。

【0068】

Ｇ_ｙにゼロ点誤差がない場合について説明する。停車時にピッチ姿勢角θ、ロール姿勢角φの時、Ｇ_ｙに表れる重力成分は、以下（９－１）式となる。ここからロール角φを推定することができる。なお、ピッチ姿勢角θは上記Ｇ_ｘの過程で推定済みとする。

【数13】

・・・（９－１）

【0069】

ピッチ姿勢角θ、ロール姿勢角φの状態で、Ｇ_ｙ軸方向に車両運動による加速度Ｇ_Ｖｙが加わる時、加速度センサ９２で検出される値は、以下（９－２）式となる。

【数14】

・・・（９－２）

【0070】

ピッチ姿勢角θ、及びロール姿勢角φが分かっていれば、以下（９－３）式のように、重力加速度成分を差し引くことで、車両加速度が求められる。

【数15】

・・・（９－３）

【0071】

次に、Ｇ_ｙにゼロ点誤差がある場合について説明する。停車時にピッチ姿勢角θ、ロール姿勢角φの時、Ｇ_ｙに表れる重力成分にゼロ点誤差Ｚ_ｙが加わるため、以下、（１０－１）式となる。

【数16】

・・・（１０－１）

【0072】

推定ロール角φ＾は、推定ピッチ姿勢角θ＾を用いて、推定できるが、Ｇ_ｘ、Ｇ_ｙのゼロ点誤差の影響が含まれる。

【0073】

ピッチ姿勢角θ、ロール姿勢角φの状態で、Ｇ_ｙ軸方向に車両運動による加速度Ｇ_Ｖｙが加わる時、加速度センサ９２で検出される値は、以下（１０－２）式で表される。

【数17】

・・・（１０－２）

【0074】

以下（１０－３）式のように、推定ロール角φ＾、推定ピッチ姿勢角θ＾を用いて重力加速度成分を差し引くと、車両加速度が求められる。

【数18】

・・・（１０－３）

【0075】

（９－３）式と（１０－３）式を比べると、求められる車両加速度は同じであることがわかる。

【0076】

同様にして、加速度センサ９２の出力Ｇ_ｚにゼロ点誤差が含まれていても、車両加速度を正しく求めることができる。Ｇ_ｚを考える際には、推定ピッチ姿勢角、推定ロール姿勢角に基づく重力加速度成分と共に、Ｇ_ｚのゼロ点誤差の補正を行った上で差し引くことになる。以下、Ｇ_ｚについて、ゼロ点誤差がない場合と、ある場合とを比較して説明する。

【0077】

Ｇ_ｚにゼロ点誤差がない場合について説明する。停車時にピッチ姿勢角θ、ロール姿勢角φの時、Ｇ_ｚに表れる重力成分は、以下（１１－１）式となる。上記Ｇ_ｘ及びＧ_ｙの過程で求めたロール姿勢角φ、ピッチ姿勢角θはこの式を満たす。

【数19】

・・・（１１－１）

【0078】

ピッチ姿勢角θ、ロール姿勢角φの状態で、Ｇ_ｚ軸方向に車両運動による加速度Ｇ_Ｖｚが加わる時、加速度センサ９２で検出される値は、以下（１１－２）式となる。

【数20】

・・・（１１－２）

【0079】

ピッチ姿勢角θ、及びロール姿勢角φが分かっていれば、以下（１１－３）式のように、重力加速度成分を差し引くことで、車両加速度が求められる。

【数21】

・・・（１１－３）

【0080】

次に、Ｇ_ｚにゼロ点誤差がある場合について説明する。停車時にピッチ姿勢角θ、ロール姿勢角φの時、Ｇ_ｚに表れる重力成分にゼロ点誤差Ｚ_ｚが加わるため、以下、（１２－１）式となる。

【数22】

・・・（１２－１）

【0081】

ここで、推定ロール姿勢角φ＾、推定ピッチ姿勢角θ＾を上式に代入すると、Ｇ_ｘ、Ｇ_ｙのゼロ点誤差の影響が含まれているため、上式を満たさない場合がある。上式を満たさない場合について説明する。

【0082】

ゼロ点誤差を含んだ加速度センサ９２の出力Ｇ’_ｘ、Ｇ’_ｙ、Ｇ’_ｚを用いて求めた推定姿勢角（推定ロール姿勢角φ＾、推定ピッチ姿勢角θ＾）は、真値に対して誤差を持つ。この時、各ゼロ点誤差Ｚ_ｘ、Ｚ_ｙ、Ｚ_ｚは独立に発生するため、停車時であってもＧ_ｘ ^２＋Ｇ_ｙ ^２＋Ｇ_ｚ ^２＝ｇ^２が成立しない場合がある。この場合、推定姿勢角を用いてゼロ点誤差を差し引くには、Ｇ_ｘ ^２＋Ｇ_ｙ ^２＋Ｇ_ｚ ^２＝ｇ^２が成立するように、例えば、Ｇ_ｚのゼロ点を修正しておいてから、推定姿勢角を求め、重力加速度成分の補正を行えば良い（なお、ゼロ点を正しく補正しておく必要はない）。

【0083】

ピッチ姿勢角θ、ロール姿勢角φの状態で、Ｇ_ｚ軸方向に車両運動による加速度Ｇ_Ｖｚが加わる時、加速度センサ９２で検出される値は、以下（１２－２）式となる。

【数23】

・・・（１２－２）

【0084】

以下（１２－３）式のように、推定ロール姿勢角φ＾、推定ピッチ姿勢角θ＾を用いて重力加速度成分を差し引くと、車両加速度が求められる。

【数24】

・・・（１２－３）

【0085】

（１１－３）式と（１２－３）式を比べると、求められる車両加速度は同じであることがわかる。

【0086】

なお、姿勢角推定は、相補フィルタやカルマンフィルタを用いて推定しても良い。ジャイロセンサ９０、加速度センサ９２の出力を追加して推定することで、姿勢角推定精度が改善できる。

【0087】

以上説明したように、第１実施形態に係る設置角算出装置１０によれば、路面の傾斜に関わらず、車両直進方向に対する設置角を精度よく算出することができる。

【0088】

（第２実施形態）
第２実施形態は、ピッチ方向の設置角推定と同時に、ヨー方向の設置角推定を行う態様である。

【0089】

図７は、第２実施形態の設置角算出装置２１０の構成を示すブロック図である。図７に示すように、ピッチ方向及びヨー方向の設置角推定には、Ｇ_ｘ、Ｇ_ｚに加え、Ｇ_ｙも使用する点が第１実施形態と異なっている。また、ジャイロセンサ９０からピッチレート、ロールレートを出力して相補フィルタで姿勢角推定する。なお、作用の流れについては第１実施形態の図３と同様であるため、異なる部分についてのみ説明する。

【0090】

加速度センサ２９２は、上下加速度センサ、前後加速度センサ、及び左右加速度センサの３軸を備え、センサ値として前後加速度Ｇ_ｘ、左右加速度Ｇ_ｙ、及び上下加速度Ｇ_ｚを出力する。

【0091】

推定条件は、第１実施形態と同様に、直進かつ加減速が大きい場合とする。また、推定ピッチ姿勢角・推定ロール姿勢角を用いて、重力加速度成分を差し引いておく必要がある。

【0092】

補正手段２１４は、算出されたピッチ姿勢角及びロール姿勢角に基づき、加速度センサ２９２の出力から重力加速度成分、及びゼロ点誤差成分の影響を補正して、補正後の加速度センサ値（Ｇ^＊ｘ、Ｇ^＊ｙ、及びＧ^＊ｚ）を求める。

【0093】

推定姿勢角でＧ_ｘ、Ｇ_ｙ、Ｇ_ｚから重力加速度成分と同時にゼロ点誤差を差し引くためには、停止時、及び一定速度直進時の加速度センサ２９２の出力が、Ｇ_ｘ ^２＋Ｇ_ｙ ^２＋Ｇ_ｚ ^２＝ｇ^２が成立するように、ゼロ点誤差を修正しておく。例えば、Ｇ_ｘ、Ｇ_ｙのゼロ点誤差を求めていない場合であっても、Ｇ_ｚのゼロ点誤差を上式になるようにＧ_ｘ、Ｇ_ｙに依存する形でＧ_ｚのゼロ点を修正すれば良い。

【0094】

設置角推定手段２１６は、補正後の加速度センサ値を用いて、上下加速度Ｇ_ｚと前後加速度Ｇ_ｘの比から車両直進方向に対するピッチ方向設置角θ_Ｅを、左右加速度Ｇ_ｙと、前後加速度Ｇ_ｘ及び上下加速度Ｇ_ｚの二乗和の平方根との比から車両直進方向に対するヨー方向設置角ψ_Ｅを算出する。

【0095】

ピッチ方向設置角θ_Ｅは、ａｒｃｔａｎ（＜Ｇ^＊ _ｚ／Ｇ^＊ _ｘ＞）で求められる。ヨー方向設置角ψ_Ｅは、ａｒｃｔａｎ（＜Ｇ^＊ _ｙ／ｓｑｒｔ（Ｇ^＊ _ｘ ^２＋Ｇ^＊ _ｚ ^２）＞）で求める。図３のステップＳ１０６では、センサ比の出力としてＧ^＊ _ｙ／ｓｑｒｔ（Ｇ^＊ _ｘ ^２＋Ｇ^＊ _ｚ ^２）を出力すればよい。

【0096】

ヨー方向設置角ψ_Ｅの算出の原理について説明する。図８は、車体のヨー方向設置角にずれがない場合とある場合とを示した図である。ヨー方向設置角ずれがない場合、車体の座標軸＝ＩＭＵ座標軸となる。車両が直進状態で加減速を行う場合、車体前後加速度をＧｖとすると、加速度センサ２９２の出力は、以下（１３）式で表される。

【数25】

・・・（１３）

【0097】

一方、ヨー方向設置角ずれがある場合、加速度センサ２９２の設置角が車体のＸ軸とずれていると、加速度センサ２９２の出力は、以下（１４）式で表される。

【数26】

・・・（１４）

【0098】

車両が直進状態で加減速を行っている間、上記のＧ_ｙ√Ｇ_ｘ ^２／Ｇ_ｚ ^２を平均化し、逆正接（ａｒｃｔａｎ）を取ることで、ヨー方向設置角ψ_Ｅを求められる。この時、Ｇ_Ｖの値は不要である。

【0099】

以上説明したように、第２実施形態に係る設置角算出装置２１０によれば、路面の傾斜に関わらず、車両直進方向に対する設置角を精度よく算出することができる。

【0100】

上述した各実施形態についての説明の補足として、姿勢角推定のバリエーションについて整理する。

【0101】

姿勢角は、加速度センサ（９２、２９２）の出力に基づいて推定する。この時に、ゼロ点誤差を含んだ状態であっても、ゼロ点誤差を何らかの方法で推定・補正した後でも、どちらでも良い。ゼロ点誤差がある場合は、推定された姿勢角に誤差が発生するが、この誤差は、重力加速度成分と合わせて差し引かれるため、ジャイロセンサ９０のヨー方向設置角推定には影響を与えない。

【0102】

姿勢角推定の基本態様としては、姿勢角を、前後加速度Ｇ_ｘ、左右加速度Ｇ_ｙを用いて推定する。停車時もしくは一定速度で直進時であれば、加速度センサの出力は姿勢角による重力加速度成分のみのため、ピッチ姿勢角・ロール姿勢角を推定することができる。ジャイロセンサ９０によるヨー方向設置角推定は、直進かつ加減速が大きい場合に行うため、加速・減速による車体のピッチ角変動の影響を受けるが、通常の市街地等の走行時であれば、発進加速時・停止減速時で逆方向にピッチ角が変動するため、両者を合わせて平均化することで、ピッチ角変動の影響を低減することができる。

【0103】

相補フィルタにより姿勢角推定する場合を説明する。この場合、姿勢角を、前後加速度Ｇ_ｘ、左右加速度Ｇ_ｙ、ピッチレートジャイロ、及びロールレートジャイロを用いて推定する。相補フィルタでは、ジャイロ（角速度）の積算を用いて速い姿勢角変化（高周波）成分を推定し、加速度センサ（９２、２９２）の出力で遅い姿勢角変化（低周波）成分を推定し、両者を組み合わせて求める。そのため、車両の運動により姿勢角が変動する場合の追従性が良くなるため、加速度センサ（９２、２９２）の出力だけ用いる場合に比べて姿勢角推定精度が改善される。加速度センサ（９２、２９２）のゼロ点誤差による姿勢角推定誤差はそのまま残るため、ジャイロセンサ９０のヨー方向設置角推定の際に、推定姿勢角を用いて重力加速度成分を差し引くとゼロ点誤差の影響も除去できる。

【0104】

また、姿勢角推定に、車速センサ９４からの出力、及びヨーレートを用いるようにしてもよい。図９及び図１０は、姿勢角推定に車速、及びヨーレートを用いる場合の構成例である。また、図９は第１実施形態に追加した場合の構成、図１０は第２実施形態に追加した場合の構成である。

【0105】

加速度センサによる姿勢角推定に、車速、ヨーレートジャイロを追加して推定する場合、下記の（１５）式に基づいて推定することで、停車時、及び一定速度直進時に加えて、直進加減速時、定常円旋回時にも姿勢角を求めることが可能になる。

【数27】

・・・（１５）

【0106】

姿勢角推定手段（１１２、２１２）における推定アルゴリズムの詳細は問わない。加速度センサ（９２、２９２）の出力Ｇ_ｘ、Ｇ_ｙのゼロ点誤差は、補正されていても、いなくても良い。基本的には重力加速度に対する姿勢角（ピッチ姿勢角・ロール姿勢角）は加速度センサの出力で求める。

【0107】

その他の姿勢角推定のバリエーションとしては、Ｇ_ｘ、Ｇ_ｙ、Ｇ_ｚを用いるもの、更にセンサを組み合わせるもの、カルマンフィルタを用いるもの等、任意の既存の姿勢角推定手法であっても、Ｇ_ｘ、Ｇ_ｙのゼロ点誤差が姿勢角の推定結果に同様の影響を与えるものであれば、いずれを使用しても良い。Ｇ_ｚのゼロ点誤差をＧ_ｘ、Ｇ_ｙに従属するように決める。

【0108】

加速度センサ２９２で３軸を用いる場合のゼロ点誤差は、Ｇ_ｘ、Ｇ_ｙのゼロ点誤差を推定姿勢角で重力加速度成分と合わせて差し引くためには、停止状態でＧ_ｚのゼロ点誤差を、下記（１６）式の姿勢角の推定について、（１７）式を満たすように事前に補正しておく。（１７）式が成立していればＧ_ｘ、Ｇ_ｙ、Ｇ_ｚを事前に調整しても良い。

【数28】

・・・（１６）

【数29】

・・・（１７）
（実測データでの検証例）
市街地での通常走行において、信号等による発進・停止を含む実車走行データを使用して検証を行った。検証では、ピッチ方向設置角がほぼ０で、ヨー方向設置角を推定した。図１１は、重力加速度成分の補正後の加速度センサ値である。図１２は、ヨー方向設置角を推定した場合の例である。ジャイロセンサ９０のヨー方向設置角はほぼ０となるように設置してあるため、設置角推定結果が０ｄｅｇ付近となれば良い。データ全体で約３０分の走行で平均化を行った。直進・加減速が大きい場合の判定基準として、ヨーレート＜２ｄｅｇ／ｓ、ａｂｓ（Ｇｘ）＞１００ｍＧを用いた。検証の結果、平均値は－０．２１９７５ｄｅｇ、標準偏差は３．９ｄｅｇとなり、設置角推定結果が０ｄｅｇ付近となったことを確認できた。

【0109】

なお、本発明は、上述した実施形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

【0110】

例えば、上述した実施形態では、車両運動状態の判定をヨーレートと前後加速度Ｇ_ｘのみを用いていたがこれに限定されるものではない。例えば、他のセンサも使用できる場合は、次の何れかの判定を組み合わせても良い。直進判定については、［１－１］直進判定について、ヨーレートがある閾値以下、［１－２］（ヨーレート×車速）がある閾値以下（旋回で発生する横加速度で判定）、［１－３］舵角がある閾値以下、の判定手法が挙げられる。加減速大の判定については、［２－１］Ｇ_ｘの絶対値がある閾値以上、［２－２］車速の平均変化量がある閾値以上、［２－３］ｓｑｒｔ（Ｇ_ｘ ^２＋Ｇ_ｙ ^２）がある閾値以上（ヨー方向設置角が大きくずれている場合）、［２－４］ｓｑｒｔ（Ｇ_ｘ ^２＋Ｇ_ｙ ^２＋Ｇ_ｚ ^２）がある閾値以上（ピッチ方向設置角も大きくずれている場合）、［２－５］アクセル開度がある閾値以上、ブレーキ圧もしくは踏力がある閾値以上、等の操作状態から判定、の判定手法が挙げられる。

【0111】

車両運動状態判定のその他のバリエーションとして、直進かつ加減速の大きい場合をより具体的に限定すると、停車状態から発進するための発進加速がある閾値以上、又は定速直進状態から停車するための減速度がある閾値以上、の場面において、推定するようにしても良い。また、例えば自動運転システムの場合、車両の制御情報でどのような走行をするかが分かるため、「直進かつ加減速大」という設置角推定に適した走行条件を車両制御情報から判断するようにしても良い。

【0112】

なお、上記各実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した設置角算出処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、設置角算出処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

【0113】

また、上記各実施形態では、設置角算出処理のプログラムがＲＯＭまたはストレージに予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ－ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の非一時的（ｎｏｎ－ｔｒａｎｓｉｔｏｒｙ）記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

【符号の説明】

【0114】

１０、２１０ 設置角算出装置
９０ ジャイロセンサ
９２、２９２ 加速度センサ
９４ 車速センサ
１１０ 運動状態判定手段
１１２ 姿勢角算出手段
１１４、２１４ 補正手段
１１６、２１６ 設置角推定手段
１２０ 直進旋回判定部
１２２ 加減速大判定部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】