特開 2023-138046 設置角算出装置、設置角算出方法、及び設置角算出プログラム

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

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

(11)【公開番号】P2023138046

(43)【公開日】2023-09-29

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

(51)【国際特許分類】

   G01P 21/00 20060101AFI20230922BHJP

【ＦＩ】

G01P21/00

【審査請求】未請求

【請求項の数】9

【出願形態】ＯＬ

(21)【出願番号】P 2022044536

(22)【出願日】2022-03-18

(71)【出願人】

【識別番号】000003609

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

(71)【出願人】

【識別番号】000004260

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

(74)【代理人】

【識別番号】110001519

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

(72)【発明者】

【氏名】鈴木 徳祥

(72)【発明者】

【氏名】牧戸 知史

(72)【発明者】

【氏名】武藤 勝彦

(72)【発明者】

【氏名】宮島 朗

(72)【発明者】

【氏名】宝地 卓

(57)【要約】

【課題】路面の傾斜に関わらず、慣性計測装置のロール、ピッチ、及びヨーの各々の方向における設置角を精度よく算出する。
【解決手段】ＩＭＵの出力に基づいて当該ＩＭＵのピッチ方向のピッチ姿勢角及びロール方向のロール姿勢角を推定する姿勢角推定部４０と、推定したピッチ姿勢角及びロール姿勢角を用いて算出した当該ＩＭＵのピッチ方向のピッチ設置角及びヨー方向のヨー設置角に基づいて、当該ＩＭＵの出力を、当該ＩＭＵのＸ軸を車体のＸ軸に一致させた場合の状態に補正する補正部４４と、補正した当該ＩＭＵの出力に基づいて算出した重力加速度成分から水平面に対する当該ＩＭＵのＹ軸の角度である第１ロール角を算出し、水平面に対する車体のＹ軸の角度である第２ロール角を算出する車体ロール角算出部４６と、第１ロール角と第２ロール角とを比較して当該ＩＭＵのロール方向の設置角を算出するロール設置角算出部４８と、を含む。
【選択図】図３

【特許請求の範囲】

【請求項1】

慣性計測装置の出力に基づいて水平面に対する前記慣性計測装置のピッチ方向のピッチ姿勢角及びロール方向のロール姿勢角を推定する姿勢角推定部と、
前記ピッチ姿勢角及び前記ロール姿勢角を用いて補正した前記慣性計測装置の出力に基づいて前記慣性計測装置のピッチ方向のピッチ設置角及びヨー方向のヨー設置角の各々を算出するピッチ・ヨー設置角算出部と、
前記水平面に対する車体のＹ軸の角度である車体ロール角を算出する車体ロール角算出部と、
前記ピッチ・ヨー設置角算出部で算出した前記慣性計測装置のピッチ設置角及びヨー設置角を用いて補正した前記慣性計測装置の出力から算出した前記水平面に対する前記慣性計測装置のＹ軸の角度である慣性計測装置ロール角と、前記車体ロール角と、を比較して前記慣性計測装置のロール方向の設置角を算出するロール設置角算出部と、
を含む設置角算出装置。

【請求項2】

前記車体のＹ軸方向に平行して、所定以上の距離を隔てて各々が同じ高さに設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナを含み、
前記車体ロール角算出部は、前記第１アンテナ、及び前記第２アンテナの各々で取得した測位情報により、前記第１アンテナと前記第２アンテナと間の相対測位演算を行うことで、前記車体ロール角を算出する請求項１に記載の設置角算出装置。

【請求項3】

前記車体の前方及び後方のいずれかの画像を取得する撮像装置を含み、
前記車体ロール角算出部は、前記撮像装置で取得した画像から抽出した建造物の鉛直方向のエッジの集合が示すベクトルと、前記画像の傾きを示す前記画像のフレームの縦方向に平行なベクトルとのなす角を前記車体ロール角として算出する請求項１に記載の設置角算出装置。

【請求項4】

前記姿勢角推定部は、前記慣性計測装置の加速度出力に基づき、前記水平面に対する前記慣性計測装置の前記ピッチ姿勢角及び前記ロール姿勢角を推定する請求項１～３のいずれか１項に記載の設置角算出装置。

【請求項5】

ロール設置角算出部は、前記ピッチ設置角及び前記ヨー設置角に基づいて、前記慣性計測装置の出力を、前記慣性計測装置のＸ軸を車体のＸ軸に一致させた場合の状態に補正する請求項１～４のいずれか１項に記載の設置角算出装置。

【請求項6】

前記車体の旋回状態及び加減速に基づいて前記車体の運動状態を判定する運動状態判定部をさらに備え、
前記ピッチ・ヨー設置角算出部は、前記運動状態判定部が前記車体が直進運動かつ加減速大と判定した場合に、前記慣性計測装置の加速度出力から、前記ピッチ姿勢角及び前記ロール姿勢角を含む重力加速度成分を控除して得た並進加速度成分のＺ軸方向の成分を、前記並進加速度成分のＸ軸方向の成分で除算して得た商の逆正接として前記ピッチ設置角を算出すると共に、前記並進加速度成分のＹ軸方向の成分を、前記並進加速度成分のＸ軸方向の成分の２乗と、前記並進加速度成分のＺ軸方向の成分の２乗との和の平方根で除算して得た商の逆正接として前記ヨー設置角を算出する請求項５に記載の設置角算出装置。

【請求項7】

前記ロール設置角算出部は、前記補正した前記慣性計測装置の出力に基づいて算出した重力加速度成分のＹ軸方向の成分が前記慣性計測装置ロール角と重力加速度との積に近似することに基づいて前記慣性計測装置ロール角を算出する請求項１～６のいずれか１項に記載の設置角算出装置。

【請求項8】

慣性計測装置の出力に基づいて水平面に対する前記慣性計測装置のピッチ方向のピッチ姿勢角及びロール方向のロール姿勢角を推定する工程と、
前記ピッチ姿勢角及び前記ロール姿勢角を用いて補正した前記慣性計測装置の出力に基づいて前記慣性計測装置のピッチ方向のピッチ設置角及びヨー方向のヨー設置角の各々を算出する工程と、
前記水平面に対する車体のＹ軸の角度である車体ロール角を算出する工程と、
前記ピッチ設置角及び前記ヨー設置角を用いて補正した前記慣性計測装置の出力から算出した前記水平面に対する前記慣性計測装置のＹ軸の角度である慣性計測装置ロール角と、前記車体ロール角と、を比較して前記慣性計測装置のロール方向の設置角を算出する工程と
を含む設置角算出方法。

【請求項9】

コンピュータを、
慣性計測装置の出力に基づいて水平面に対する前記慣性計測装置のピッチ方向のピッチ姿勢角及びロール方向のロール姿勢角を推定する姿勢角推定部、前記ピッチ姿勢角及び前記ロール姿勢角を用いて補正した前記慣性計測装置の出力に基づいて前記慣性計測装置のピッチ方向のピッチ設置角及びヨー方向のヨー設置角の各々を算出するピッチ・ヨー設置角算出部、前記水平面に対する車体のＹ軸の角度である車体ロール角を算出する車体ロール角算出部と、前記ピッチ・ヨー設置角算出部で算出した前記慣性計測装置のピッチ設置角及びヨー設置角を用いて補正した前記慣性計測装置の出力から算出した前記水平面に対する前記慣性計測装置のＹ軸の角度である慣性計測装置ロール角と、前記車体ロール角と、を比較して前記慣性計測装置のロール方向の設置角を算出するロール設置角算出部として機能させる、設置角算出プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、ＩＭＵ（Inertial Measurement Unit）等の慣性計測装置の設置角算出装置、設置角算出方法、及び設置角算出プログラムに関する。

【背景技術】

【0002】

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

【0003】

特許文献１には、車載装置の設置角度が変更された場合において車載装置による処理の精度を維持する取付け角度検出装置の発明が開示されている。特許文献１に記載の角度検出装置は、例えば車両が停止している場合、又は当該車両が一定速で走行している場合等の、当該車両の進行方向の加速度成分がゼロの場合における加速度センサの出力を積算し、平均化させた値を算出することにより、ピッチ姿勢角を算出する。

【0004】

また、特許文献２には、実走行状態において、電子機器の筐体の設置角度を算出することのできる設置角度算出装置の発明が開示されている。特許文献２に記載の角度算出装置は、車両が水平面上を走行していることを前提として、筐体の設置角度の候補を算出している。

【先行技術文献】

【特許文献】

【0005】

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

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

【発明の概要】

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

【0006】

特許文献１及び特許文献２の各々に記載の技術は、いずれも水平面を走行していることを前提として推定を行っているので、勾配のある路面走行時は、設置角度の推定誤差が発生するという問題がある。

【0007】

図１４（Ａ）は車体２００が水平面を走行している場合、図１４（Ｂ）は水平面６４に対して２％（１．１５度）程度の横断勾配を有する斜面６２を車体２００が走行している場合、を各々示した説明図である。図１４（Ａ）、（Ｂ）において、車体２００の水平面進行方向をＸ軸、車体２００の中心部から水平面横方向をＹ軸、鉛直方向をＺ軸としている。横断勾配αは、車体２００のＹ軸方向における、水平面６４に対する斜面６２の傾斜である。また、ロール角φはＸ軸回りの、ピッチ角θはＹ軸回りの、ヨー角ψはＺ軸回りの各々の角度である。

【0008】

一般の路面は水はけ等のために、完全な水平面となる場合は稀であり、ほとんどの場合、図１４（Ｂ）に示したような横断勾配αが存在する。横断勾配αは、車体２００がＸ軸方向に走行するのであれば、車体２００の進行方向に対する設置角度に相当するヨー角ψ及びピッチ角θの算出において無視できるので、水平面での走行を前提とした特許文献１及び特許文献２の各々に開示された装置でも車体２００の進行方向に対する設置角度に相当するヨー角ψ及びピッチ角θは算出できる。しかしながら、図１４（Ｂ）に示したように、ロール角φは横断勾配αと同じ方向の角度を示すので、車体２００に対してではなく水平面６４を基準とした姿勢角を求める特許文献１及び特許文献２の各々に記載の技術では、ロール方向の設置角は算出できない。

【0009】

これは、車体座標系のＸ軸方向が水平面でなくても車両運動状態から基準とする軸方向が求められるのに対し、車体座標系のＹ軸方向は車体のロール角φで変化する場合に基準方向を定めるための情報が存在しないためである。従って、車体２００に対する慣性計測装置のロール設置角は、水平面に対するロール角だけでは求められず、車体２００のロール角の情報が必要となる。

【0010】

特許文献１では、水平面６４を前提とすると、車体座標系のＹ軸方向が水平方向（車体のロール角が０）になるため、水平面に対するロール角を慣性計測装置の設置角と見なしていた。しかし、車体のロール角が０でない場合、この前提が崩れるため、慣性計測装置のロール設置角を求めることはできない。また、特許文献２に記載の技術では、ロール角についての記載が存在しない。

【0011】

本発明は、上記事情を鑑みて成されたものであり、路面の傾斜に関わらず、慣性計測装置のロール、ピッチ、及びヨーの各々の方向における設置角を精度よく算出することができる設置角算出装置、設置角算出方法、及び設置角算出プログラムを提供することを目的とする。

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

【0012】

上記目的を達成するために、請求項１に係る設置角算出装置は、慣性計測装置の出力に基づいて水平面に対する前記慣性計測装置のピッチ方向のピッチ姿勢角及びロール方向のロール姿勢角を推定する姿勢角推定部と、前記ピッチ姿勢角及び前記ロール姿勢角を用いて補正した前記慣性計測装置の出力に基づいて前記慣性計測装置のピッチ方向のピッチ設置角及びヨー方向のヨー設置角の各々を算出するピッチ・ヨー設置角算出部と、前記水平面に対する車体のＹ軸の角度である車体ロール角を算出する車体ロール角算出部と、前記ピッチ・ヨー設置角算出部で算出した前記慣性計測装置のピッチ設置角及びヨー設置角を用いて補正した前記慣性計測装置の出力から算出した前記水平面に対する前記慣性計測装置のＹ軸の角度である慣性計測装置ロール角と、前記車体ロール角と、を比較して前記慣性計測装置のロール方向の設置角を算出するロール設置角算出部と、を含む。

【0013】

請求項２に係る設置角算出装置は、前記車体のＹ軸方向に平行して、所定以上の距離を隔てて各々が同じ高さに設置され、衛星からの測位情報を各々取得可能な第１アンテナ、及び第２アンテナを含み、前記車体ロール角算出部は、前記第１アンテナ、及び前記第２アンテナの各々で取得した測位情報により、前記第１アンテナと前記第２アンテナと間の相対測位演算を行うことで、前記車体ロール角を算出する。

【0014】

請求項３に係る設置角算出装置は、前記車体の前方及び後方のいずれかの画像を取得する撮像装置を含み、前記車体ロール角算出部は、前記撮像装置で取得した画像から抽出した建造物の鉛直方向のエッジの集合が示すベクトルと、前記画像の傾きを示す前記画像のフレームの縦方向に平行なベクトルとのなす角を前記車体ロール角として算出する。

【0015】

請求項４に係る設置角算出装置は、前記姿勢角推定部は、前記慣性計測装置の加速度出力に基づき、前記水平面に対する前記慣性計測装置の前記ピッチ姿勢角及び前記ロール姿勢角を推定する。

【0016】

請求項５に係る設置角算出装置は、ロール設置角算出部は、前記ピッチ設置角及び前記ヨー設置角に基づいて、前記慣性計測装置の出力を、前記慣性計測装置のＸ軸を車体のＸ軸に一致させた場合の状態に補正する。

【0017】

請求項６に係る設置角算出装置は、前記車体の旋回状態及び加減速に基づいて前記車体の運動状態を判定する運動状態判定部をさらに備え、前記ピッチ・ヨー設置角算出部は、前記運動状態判定部が前記車体が直進運動かつ加減速大と判定した場合に、前記慣性計測装置の加速度出力から、前記ピッチ姿勢角及び前記ロール姿勢角を含む重力加速度成分を控除して得た並進加速度成分のＺ軸方向の成分を、前記並進加速度成分のＸ軸方向の成分で除算して得た商の逆正接として前記ピッチ設置角を算出すると共に、前記並進加速度成分のＹ軸方向の成分を、前記並進加速度成分のＸ軸方向の成分の２乗と、前記並進加速度成分のＺ軸方向の成分の２乗との和の平方根で除算して得た商の逆正接として前記ヨー設置角を算出する。

【0018】

請求項７に係る設置角算出装置は、前記ロール設置角算出部は、前記補正した前記慣性計測装置の出力に基づいて算出した重力加速度成分のＹ軸方向の成分が前記慣性計測装置ロール角と重力加速度との積に近似することに基づいて前記慣性計測装置ロール角を算出する。

【0019】

請求項８に係る設置角算出方法は、慣性計測装置の出力に基づいて水平面に対する前記慣性計測装置のピッチ方向のピッチ姿勢角及びロール方向のロール姿勢角を推定する工程と、前記ピッチ姿勢角及び前記ロール姿勢角を用いて補正した前記慣性計測装置の出力に基づいて前記慣性計測装置のピッチ方向のピッチ設置角及びヨー方向のヨー設置角の各々を算出する工程と、前記水平面に対する車体のＹ軸の角度である車体ロール角を算出する工程と、前記ピッチ設置角及び前記ヨー設置角を用いて補正した前記慣性計測装置の出力から算出した前記水平面に対する前記慣性計測装置のＹ軸の角度である慣性計測装置ロール角と、前記車体ロール角と、を比較して前記慣性計測装置のロール方向の設置角を算出する。

【0020】

請求項９に係る設置角算出プログラムは、コンピュータを、慣性計測装置の出力に基づいて水平面に対する前記慣性計測装置のピッチ方向のピッチ姿勢角及びロール方向のロール姿勢角を推定する姿勢角推定部、前記ピッチ姿勢角及び前記ロール姿勢角を用いて補正した前記慣性計測装置の出力に基づいて前記慣性計測装置のピッチ方向のピッチ設置角及びヨー方向のヨー設置角の各々を算出するピッチ・ヨー設置角算出部、前記水平面に対する車体のＹ軸の角度である車体ロール角を算出する車体ロール角算出部と、前記ピッチ・ヨー設置角算出部で算出した前記慣性計測装置のピッチ設置角及びヨー設置角を用いて補正した前記慣性計測装置の出力から算出した前記水平面に対する前記慣性計測装置のＹ軸の角度である慣性計測装置ロール角と、前記車体ロール角と、を比較して前記慣性計測装置のロール方向の設置角を算出するロール設置角算出部として機能させる。

【発明の効果】

【0021】

本発明の設置角算出装置、設置角算出方法、及び設置角算出プログラムによれば、路面の傾斜に関わらず、慣性計測装置のロール、ピッチ、及びヨーの各々の方向における設置角を精度よく算出することができる。

【図面の簡単な説明】

【0022】

【図1】本発明の第１実施形態に係る設置角算出装置の構成の一例を示したブロック図である。

【図2】（Ａ）は本発明の第１実施形態における第１ＧＮＳＳアンテナ及び第２ＧＮＳＳアンテナの配置の一例を示した概略図であり、（Ｂ）は第１ＧＮＳＳアンテナ及び第２ＧＮＳＳアンテナの一般的な配置の一例を示した概略図である。

【図3】演算装置の具体的な構成の一例を示すブロック図である。

【図4】本発明の第１実施形態に係る設置角算出装置の処理の流れの概略を示したブロック図である。

【図5】本発明の第１実施形態に係る設置角算出装置の処理の一例を示したフローチャートである。

【図6】図５のステップＳ１００におけるピッチ・ヨー方向設置角推定の処理の一例を示したフローチャートである。

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

【図8】並進加速度成分の向きと水平面との関係を示した説明図である。

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

【図10】（Ａ）は、車体のヨー方向設置角にずれがある場合を、（Ｂ）は、車体のヨー方向設置角にずれがない場合を各々示した概略図である。

【図11】相対測位による第２ロール角の算出の説明図である。

【図12】本発明の第２実施形態において、撮像装置で取得した車体前方の画像の一例である。

【図13】（Ａ）は、設置角算出装置の最小限の構成の一例を、（Ｂ）は、本発明の第１実施形態に係る設置角算出装置の最小限の構成の一例を、（Ｃ）は、本発明の第２実施形態に係る設置角算出装置の最小限の構成の一例を、各々示した概略図である。

【図14】（Ａ）は車体が水平面を走行している場合、（Ｂ）は水平面に対して２％（１．１５度）程度の横断勾配を有する斜面を車体２００が走行している場合、を各々示した説明図である。

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

【0023】

［第１実施形態］
以下、図面を参照して本発明の第１実施形態を詳細に説明する。図１は、本実施形態に係る設置角算出装置１０の構成の一例を示したブロック図である。設置角算出装置１０は、演算装置１４の演算に必要なデータ及び演算装置１４による演算結果を記憶する記憶装置１８と、撮像装置２２が取得した画像情報からエッジを抽出する画像情報処理部２０と、画像情報処理部２０が抽出したエッジ、車速センサ２４が検出した車体２００の前後速度、３軸のジャイロと３方向の加速度計によって、３次元の角速度と加速度とが検出可能なＩＭＵ２６が検出した車体２００の方位角の角速度及び加速度、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々で衛星から受信したＧＮＳＳ（Global Navigation Satellite System：全地球航法衛星システム）の情報、並びに地図情報データベース３４に格納された地図情報が各々入力される入力装置１２と、入力装置１２から入力された入力データ及び記憶装置１８に記憶されたデータに基づいてＩＭＵ２６の設置角度の算出及び車体２００の自己位置の推定の演算を行なうコンピュータ等で構成された演算装置１４と、車体２００の外界との無線通信が可能なＶ２Ｘ通信部３６と、演算装置１４による演算結果等を表示可能なＣＲＴ又はＬＣＤ等で構成された表示装置１６と、で構成されている。

【0024】

本実施形態に係る撮像装置２２は車載カメラ等であり、一例として、撮影により取得した車両周辺の画像情報を解析して走路の白線、及び建造物等をエッジとして検出する。ＩＭＵ２６は、走行時の車両の挙動を示す３軸の角速度（ピッチレート、ロールレート、ヨーレート）と３軸の加速度（前後加速度、横加速度、上下加速度）とが検出可能な慣性計測装置である。

【0025】

図２（Ａ）は本実施形態における第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の配置の一例を示した概略図であり、図２（Ｂ）は第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の一般的な配置の一例を示した概略図である。

【0026】

図２（Ａ）に示したように、本実施形態では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々を、車体２００の横方向であるＹ軸方向に配置している。本実施形態では、車体２００の方位角の情報と、ロール角の情報とを取得するために、図２（Ａ）に示したように第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０を車体２００の横方向に配置する。

【0027】

本実施形態では、第１ＧＮＳＳアンテナ２８と第２ＧＮＳＳアンテナ３０との各々を車体２００のＹ軸方向に平行して、所定以上の距離を隔てて同じ高さに設置する。車体形状は多くの場合、左右対称のため、２つのＧＮＳＳアンテナを同じ高さに設置することは容易である。横方向に２つのＧＮＳＳアンテナを配置することにより、２つのＧＮＳＳアンテナ間の相対測位演算を行い、車体２００の座標Ｙ軸の水平面に対する角度である第２ロール角を算出する。第１ＧＮＳＳアンテナ２８と第２ＧＮＳＳアンテナ３０との間の所定以上の距離は、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の基線長を可能な限り長くするために、例えば、車体２００のルーフの横幅に近い値である。

【0028】

図２（Ｂ）に示した第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の一般的な配置は、第１ＧＮＳＳアンテナ２８と第２ＧＮＳＳアンテナ３０との距離である基線長を可能な限り長くするためのものである。第１ＧＮＳＳアンテナ２８と第２ＧＮＳＳアンテナ３０との基線長を長くすることにより、車体２００の方位角の算出精度を高めることができる。しかしながら、車体２００のロール角の推定には適さない。第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０で取得した情報に基づく第２ロール角の算出の詳細については後述する。

【0029】

図３は、演算装置１４の具体的な構成の一例を示すブロック図である。演算装置１４は、一種のコンピュータであり、ＣＰＵ（Central Processing Unit）１４Ｂ、ＲＯＭ（Read Only Memory）１４Ａ、ＲＡＭ（Random Access Memory）１４Ｃ、及び入出力ポート１４Ｄを備える。

【0030】

演算装置１４では、ＣＰＵ１４Ｂ、ＲＯＭ１４Ａ、ＲＡＭ１４Ｃ、及び入出力ポート１４Ｄがアドレスバス、データバス、及び制御バス等の各種バスを介して互いに接続されている。入出力ポート１４Ｄには、各種の入出力機器として、入力装置１２、ハードディスク（ＨＤＤ）等である記憶装置１８、表示装置１６、及びＶ２Ｘ通信部３６等が各々接続されている。

【0031】

記憶装置１８には、設置角算出のための設置角算出プログラムがインストールされている。本実施形態では、ＣＰＵ１４Ｂが設置角算出プログラムを実行することにより、設置角算出を実行する。また、ＣＰＵ１４Ｂは、設置角算出プログラムによる処理結果を表示装置１６に表示させる。なお、本実施形態の設置角算出プログラムを演算装置１４にインストールするには、幾つかの方法があるが、例えば、設置角算出プログラムをセットアッププログラムと共にＣＤ－ＲＯＭやＤＶＤ等に記憶しておき、入出力装置であるディスクドライブ等にディスクをセットし、ＣＰＵ１４Ｂに対してセットアッププログラムを実行することにより記憶装置１８に設置角算出プログラムをインストールする。または、公衆電話回線又はネットワークを介して演算装置１４と接続される他の情報処理機器と通信することで、記憶装置１８に設置角算出プログラムをインストールするようにしてもよい。

【0032】

次に、演算装置１４のＣＰＵ１４Ｂが設置角算出プログラムを実行することで実現される各種機能について説明する。設置角算出プログラムは、ＩＭＵ２６が出力した情報に基づいて水平面に対するピッチ姿勢角及びロール姿勢角を推定する姿勢角推定機能、車体２００の旋回状態及び加減速に基づいて車体２００の運動状態を判定する運動状態判定機能、ピッチ姿勢角及びヨー姿勢角を用いてＩＭＵ２６の座標軸と車体２００の座標軸とのピッチ方向、及びヨー方向における各々のずれを、ピッチ設置角、及びヨー設置角として算出し、算出したピッチ設置角、及びヨー設置角に基づいて、ＩＭＵ２６の出力を、ＩＭＵ２６のＸ軸を車体２００のＸ軸に一致させた場合の状態に補正する補正機能、Ｘ軸を補正したＩＭＵ２６で検出した情報に基づく重力加速度成分から水平面に対するＩＭＵ２６のＹ軸の角度である第１ロール角を算出すると共に、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々で衛星から取得した情報に基づいて車体２００のＹ軸の角度である第２ロール角を算出する車体ロール角算出機能、並びに第１ロール角と第２ロール角とを比較してＩＭＵ２６のロール設置角を算出するロール設置角算出機能として機能させる。ＣＰＵ１４Ｂは、設置角算出プログラムを実行することにより、姿勢角推定部４０、運動状態判定部４２、補正部４４、車体ロール角算出部４６及びロール設置角算出部４８として機能する。

【0033】

図４は、本実施形態に係る設置角算出装置１０の処理の流れの概略を示したブロック図である。ブロックＢ１００では、ジャイロセンサであるＩＭＵ２６がヨー方向の角速度であるヨーレートを検出して出力する。

【0034】

ブロックＢ１０２では、ジャイロセンサであるＩＭＵ２６が、ピッチ方向の角速度であるピッチレート、及びロール方向の角速度であるロールレートの各々を検出して出力する。

【0035】

ブロックＢ１０４では、ＩＭＵ２６に含まれる加速度センサが前後加速度Ｇ_ｘ、横速度Ｇ_y、及び上下加速度Ｇ_ｚを検出して出力する。

【0036】

ブロックＢ１０６では、ＩＭＵ２６が出力したピッチレート、ロールレート、前後加速度Ｇ_ｘ、横速度Ｇ_y、及び上下加速度Ｇ_ｚに基づいて、水平面に対するピッチ姿勢角及びロール姿勢角を算出する。

【0037】

ブロックＢ１０８では、算出されたピッチ姿勢角及びロール姿勢角に基づき、ブロックＢ１０４で出力した前後加速度Ｇ_ｘ、横速度Ｇ_y、及び上下加速度Ｇ_ｚから重力加速度成分、及びゼロ点誤差成分の影響を補正して、補正後の並進加速度成分を出力する。

【0038】

ブロックＢ１１０では、車体２００が直進かつ加減速大の状態であることを判定する。判定の条件は、例えば、ヨーレートが閾値以下、かつ、前後加速度Ｇ_ｘが閾値以上であるか否かである。ブロックＢ１１０では、ブロックＢ１１０Ａにおいてヨーレートが閾値以下か否かを判定し、ブロックＢ１１０Ｂにおいて前後加速度Ｇ_ｘが閾値以上であるか否かを判定する。ブロックＢ１１０Ａにおいてヨーレートが閾値以下であると判定した場合、車体２００は直進状態であり、ブロックＢ１１０Ｂにおいて前後加速度Ｇ_ｘが閾値以上であると判定した場合、車体２００は加減速大の状態である。

【0039】

ブロックＢ１１２では、ブロックＢ１１０で車体２００が直進状態かつ加減速大にある場合に、ブロックＢ１０８で出力した並進加速度成分に基づいて、ピッチ方向の設置角、及びヨー方向の設置角の各々を算出して出力する。

【0040】

ブロックＢ１１４では、ブロックＢ１１２で出力したピッチ方向及びヨー方向の各々の設置角に従ってＩＭＵ２６のＸ軸を車体２００のＸ軸に一致させることにより、ピッチ方向の設置角、及びヨー方向の設置角の各々を補正する。実際には、ＩＭＵ２６の搭載位置を変更するわけではないので、ＩＭＵ２６の座標軸の補正という表現には語弊があるが、本実施形態では、ＩＭＵ２６の出力を、ピッチ方向の設置角、及びヨー方向の設置角等で補正することを、便宜上、ＩＭＵ２６の座標軸の補正と称する。

【0041】

ブロックＢ１１６では、ＩＭＵ２６のＸ軸補正後の重力加速度成分に基づき、水平面に対するＩＭＵ２６のＹ軸の角度である第１ロール角を算出する。

【0042】

ブロックＢ１１８では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々で、衛星から測位に必要な情報を取得する。

【0043】

ブロックＢ１２０では、車体２００の横方向に、所定以上の距離を隔てて同じ高さに設置した第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々による測位結果から車体２００のＹ軸の水平面に対する角度である第２ロール角を算出する。

【0044】

ブロックＢ１２２では、第１ロール角と第２ロール角とを比較することにより、車体の座標軸に対するＩＭＵ２６のロール方向の設置角を算出して、出力する。

【0045】

図５は、本実施形態に係る設置角算出装置１０の処理の一例を示したフローチャートである。ステップＳ１００では、ＩＭＵ２６のピッチ方向及びヨー方向の各々の設置角を推定する。

【0046】

図６は、図５のステップＳ１００におけるピッチ・ヨー方向設置角推定の処理の一例を示したフローチャートである。ステップＳ２００では、ＩＭＵ２６の出力に基づき、水平面に対するピッチ姿勢角及びロール姿勢角を算出すると共に、算出したピッチ姿勢角及びロール姿勢角に基づき、ＩＭＵ２６の出力から重力加速度成分、及びゼロ点誤差成分の影響を補正して、補正後の加速度センサ値を求める重力加速度成分による補正処理を行う。

【0047】

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

【0048】

加速度センサの出力Ｇ_x、Ｇ_y、Ｇ_zの各々は、下記の式（１）に示したように、［１］～［３］の３つの成分の和で表される。式（１）において、［１］は、加速度センサの姿勢角による重力加速度成分を、［２］は、回転（旋回）運動の遠心力に対応する成分を、［３］は、並進運動の加速度成分を、各々示している。

・・・（１）

【0049】

上記の式（１）において、θはピッチ姿勢角、φはロール姿勢角、Ｕは前後速度、Ｖは左右速度、Ｗは上下速度、Ｐはロール方向の角速度であるロールレート、Ｑはピッチ方向の角速度であるピッチレート、Ｒはヨー方向の角速度であるヨーレートを、各々示している。

【0050】

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

【0051】

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

【0052】

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

【0053】

重力加速度成分の除去は、姿勢角（ピッチ姿勢角・ロール姿勢角）を図４のブロックＢ１０６に示したように推定しておき、推定ピッチ姿勢角θ＾、推定ロール姿勢角φ＾から下記の式（２）で求められる重力加速度成分を、加速度センサの出力から控除すればよい（推定ピッチ姿勢角θ＾、及び推定ロール姿勢角φ＾の記号は、式ではθ及びφの上に＾付きで表される）。姿勢角の推定は、相補フィルタやカルマンフィルタを用いて推定してもよい。ＩＭＵ２６の出力を観測値として、当該観測値に対する予測値を推定することで、姿勢角推定精度が改善できる。

・・・（２）

【0054】

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

【0055】

上述のように、水平面基準のピッチ姿勢角及びロール姿勢角を推定する場合、加速度センサの出力Ｇ_ｘ、Ｇ_ｙ、Ｇ_ｚにゼロ点誤差が含まれていると、停車時であってもＧ_ｘ ^２＋Ｇ_ｙ ^２＋Ｇ_ｚ ^２＝ｇ^２が成立しない場合がある。かかる場合は、Ｇ_ｘ ^２＋Ｇ_ｙ ^２＋Ｇ_ｚ ^２＝ｇ^２が成立するように、例えば、Ｇ_ｚを予め補正した後、姿勢角推定、及び重力加速度成分の補正を行う。

【0056】

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

【0057】

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

・・・（３－１）

【0058】

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

・・・（３－２）

【0059】

ピッチ姿勢角θが分かっていれば、下記の式（３－３）のように、加速度センサの出力Ｇ_xから重力加速度成分Ｇ_gxを控除することで、車体２００の並進加速度成分である車両加速度Ｇ_vxが求められる。

・・・（３－３）

【0060】

次に、Ｇ_ｘにゼロ点誤差がある場合について説明する。停車時にピッチ姿勢角θの時、Ｇ_ｘに表れる重力成分にゼロ点誤差Ｚ_ｘが加わるため、重力加速度成分Ｇ’_ｇｘは、下記の式（４－１）で表される。

・・・（４－１）

【0061】

推定ピッチ姿勢角θ＾は、ゼロ点誤差Ｚ_xを含んで推定される。この段階で正しいピッチ姿勢角θは不明である。

【0062】

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

・・・（４－２）

【0063】

下記の式（４－３）のように、加速度センサの出力Ｇ_xから、推定ピッチ姿勢角θ＾に基づく重力加速度成分Ｇ’_ｇｘ（＝－ｇ・ｓｉｎθ＾）を控除すると、車体２００の並進加速度成分である車両加速度Ｇ_vxが求められる。

・・・（４－３）

【0064】

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

【0065】

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

【0066】

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

・・・（５－１）

【0067】

ピッチ姿勢角θ、ロール姿勢角φの状態で、ＩＭＵ２６のＹ軸方向に車両運動による加速度Ｇ_Ｖｙが加わる時、ＩＭＵ２６で検出される値は、下記の式（５－２）で表される。

・・・（５－２）

【0068】

ピッチ姿勢角θ、及びロール姿勢角φが分かっていれば、下記の式（５－３）に示したように、加速度センサの出力であるＧ_yから重力加速度成分Ｇ_gyを控除することで、車体２００の並進加速度成分である車両加速度Ｇ_vyが求められる。

・・・（５－３）

【0069】

次に、Ｇ_ｙにゼロ点誤差がある場合について説明する。停車時にピッチ姿勢角θ、ロール姿勢角φの時、Ｇ_ｙに表れる重力成分にゼロ点誤差Ｚ_ｙが加わるため、重力加速度成分Ｇ’_ｇyは、下記の式（６－１）で表される。

・・・（６－１）

【0070】

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

【0071】

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

・・・（６－２）

【0072】

下記の式（６－３）に示したように、推定ロール角φ＾、推定ピッチ姿勢角θ＾を用いて、加速度センサで検出される値であるＧ’_yから重力加速度成分Ｇ’_gyを控除すると、車体２００の並進加速度成分である車両加速度Ｇ_vyが得られる。

・・・（６－３）

【0073】

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

【0074】

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

【0075】

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

・・・（７－１）

【0076】

ピッチ姿勢角θ、ロール姿勢角φの状態で、ＩＭＵ２６のＺ軸方向に車両運動による加速度Ｇ_Ｖｚが加わる時、ＩＭＵ２６で検出される値であるＧ_zは、下記の式（７－２）で表される。

・・・（７－２）

【0077】

ピッチ姿勢角θ、及びロール姿勢角φが分かっていれば、下記の式（７－３）に示したように、ＩＭＵ２６で検出される値であるＧ_zから重力加速度成分Ｇ_gzを控除することで、車体２００の並進加速度成分である車両加速度Ｇ_vzが得られる。

・・・（７－３）

【0078】

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

・・・（８－１）

【0079】

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

【0080】

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

【0081】

ピッチ姿勢角θ、ロール姿勢角φの状態で、ＩＭＵ２６のＺ軸方向に車両運動による加速度Ｇ_Ｖｚが加わる時、ＩＭＵ２６で検出される値は、下記の式（８－２）となる。

・・・（８－２）

【0082】

下記の式（８－３）に示したように、推定ロール姿勢角φ＾、推定ピッチ姿勢角θ＾を用いて、加速度センサで検出される値であるＧ’_zから重力加速度成分Ｇ’_gzを控除すると、車体２００の並進加速度成分である車両加速度Ｇ_vzが得られる。

・・・（８－３）

【0083】

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

【0084】

以上説明した手順により、図６のステップＳ２００における重力加速度成分の補正を行った後、ステップＳ２０２では、車体２００が直進かつ加減速大の状態であるか否かを判定する。ステップＳ２０２では、ＩＭＵ２６で検出したヨーレートが所定のヨーレート閾値以下の場合に車体２００が直進状態であると判定する。また、ステップＳ２０２では、前後加速度Ｇxが所定の前後加速度閾値以上の場合に、車体２００が加減速大の状態であると判定する。車体２００が直進かつ加減速大の状態である場合には、手順をステップＳ２０４に移行する。加減速大の状態でない場合には、手順をステップＳ２００に戻して処理を繰り返す。所定のヨーレート閾値、及び所定の前後加速度閾値の各々は、設計段階でのシミュレーション、又は実車を用いた試験等を通じて具体的に決定する。

【0085】

ステップＳ２０４では、補正後の加速度センサ値のセンサ比を出力する。センサ比により、ピッチ方向設置角θ_Ｅの正接、及びヨー方向設置角ψ_Ｅの正接を各々算出する。

【0086】

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

【0087】

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

・・・（９）

【0088】

傾斜路面上を車体が並進加速度Ｇ_Ｖで直進走行する場合、ＩＭＵ２６の出力は、式（１）の重力加速度成分を第１項とし、ピッチ方向設置角θ_Ｅに対応した加速度成分を第２項とした、下記の式（１０）のようになる。

・・・（１０）

【0089】

従って、式（１０）から式（９）で示した重力加速度成分を減算することにより、以下の式（１１）のようなピッチ方向設置角θ_Ｅに対応した加速度成分のみを抽出できる。

・・・（１１）

【0090】

式（１１）におけるＧ^* _zを、同じく式（１１）におけるＧ^* _xで除算して得た商は、補正後の加速度センサ値の比であり、以下の式（１２）のように、ピッチ方向設置角の正接となる。そして、ピッチ方向設置角の正接の逆正接（ａｒｃｔａｎ）は、ピッチ方向設置角θ_Ｅとなる。

・・・（１２）

【0091】

なお、ＩＭＵ２６の出力にノイズ成分が混在している場合、並進加速度Ｇ_Ｖが大きい値であるほど、当該ノイズの影響は低下する。本実施形態では、ステップＳ２０２で車体の加減速が大である場合に加速度センサ値の比の算出を限定することにより、精度良くピッチ方向設置角θ_Ｅの正接を算出することができる。

【0092】

図１０（Ａ）は、車体２００のヨー方向設置角にずれがある場合を、図１０（Ｂ）は、車体２００のヨー方向設置角にずれがない場合を各々示した概略図である。図１０（Ｂ）に示したように、ヨー方向設置角にずれがない場合、車体の座標軸＝ＩＭＵ座標軸となる。車両が直進状態で加減速を行う場合、車体前後加速度を並進加速度Ｇ_ｖとすると、ＩＭＵ２６の出力は、下記の式（１３）で表される。

・・・（１３）

【0093】

図１０（Ａ）に示したように、ヨー方向設置角にずれがある場合、ＩＭＵ２６の設置角が車体のＸ軸とずれていると、加速度センサの出力は、下記の式（１４）で表される。

・・・（１４）

【0094】

式（１４）の２行目の式の左辺を、式（１４）の１行目の左辺で除算して得た商は、補正後の加速度センサ値の比であり、以下の式（１５）のように、ヨー方向設置角の正接となる。そして、ヨー方向設置角の正接の逆正接は、ヨー方向設置角ψ_Ｅとなる。

・・・（１５）

【0095】

ステップＳ２０６では、ピッチ方向設置角の正接を示す加速度センサ比、及びヨー方向設置角の正接を示す加速度センサ比の各々のサンプル数が所定値に達したか否かを判定すする。センサ比にはノイズ成分の影響があるため、本実施形態では、例えば、十分なサンプル数でセンサ比の値を平均化する。十分なサンプル数で平均化することにより、センサ比及びその逆正接は、ほぼ一定値になる。平均化は逆正接を算出する前でも後でもよい。

【0096】

ステップＳ２０６で、サンプル数が所定値に達した場合は手順をステップＳ２０８に移行し、サンプル数が所定値に達していない場合は手順をステップＳ２００に移行してセンサ比の算出を継続する。

【0097】

ステップＳ２０８では、ヨー方向設置角、及びピッチ方向設置角を算出して処理をリターンする。前述のように、ヨー方向設置角、及びピッチ方向設置角は、センサ比の逆正接である。

【0098】

以上で、図５のステップＳ１００におけるピッチ・ヨー方向におけるＩＭＵ２６の設置角の推定は終了し、手順は、図５のステップＳ１０２に移行する。ステップＳ１０２では、ピッチ・ヨー方向におけるＩＭＵ２６の座標軸を補正する。

【0099】

上記で求めた設置角に基づきＩＭＵ２６の座標軸を補正するには、ＩＭＵ２６のＹ軸を回転軸としてＺＸ平面上でピッチ設置角θ_Eだけ回転し、その後、ＩＭＵ２６のＺ軸を回転軸としてＸＹ平面上でヨー設置角ψ_Eだけ回転する。その結果、ＩＭＵ２６のＸ軸方向が車体２００のＸ軸方向に一致する。実際には、ＩＭＵ２６の搭載位置を変更するわけではないが、本実施形態では、ＩＭＵ２６の出力を、ピッチ設置角θ_E、ヨー設置角ψ_E及び後述するロール設置角φ_Ｅで補正することを、便宜上、ＩＭＵ２６の座標軸の補正と称する。

【0100】

ステップＳ１０４では、設置角によるＩＭＵ２６の座標軸の補正後の加速度センサ出力を求め、重力加速度成分に基づき、水平面に対する補正後のＩＭＵ２６のＹ軸方向の角度、すなわち、ロール方向の角度である第１ロール角を算出する。

【0101】

第１ロール角φ'₁は、以下のようにして算出する。前述のように、Ｙ軸方向の重力加速度成分は、下記の式（１６）で表される。式（１６）中のφ₁は、補正後のロール姿勢角である。

・・・（１６）

【0102】

第１ロール角φ'₁は、水平面に対する補正後のＩＭＵ２６のＹ軸方向の角度なので、式（１６）に示したＹ軸方向の重力加速度成分は、下記の式（１７）のようになる。

・・・（１７）

【0103】

一般的な走行環境では、ロール姿勢角の絶対値は小さいため、ｓｉｎφ'₁≒φ'₁と近似することができるので、式（１６）及び式（１７）から下記の式（１８）が得られる。

・・・（１８）

【0104】

補正後のロール姿勢角φ₁は、以下のようにして算出する。Ｚ軸方向の重力加速度成分は、下記の式（１９）で表されるので、上述の式（１６）で示したＧ_gyを、式（１９）で示したＧ_gzで除算して得た商は、以下の式（２０）のように、補正後のロール姿勢角φ₁の正接となる。そして、補正後のロール姿勢角φ₁の正接の逆正接は、補正後のロール姿勢角φ₁となる。式（１６）で示したＧ_gy、式（１８）右辺に示したｃｏｓθ、及び式（１９）で示したＧ_gzの各々は、一例として、Ｘ軸を補正したＩＭＵ２６の出力に基づいて推定したピッチ姿勢角θとロール姿勢角と重力加速度ｇとを用いて算出する。

・・・（１９）

・・・（２０）

【0105】

ステップＳ１０６では、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の、２つのアンテナ間の相対測位演算を行うことで、車体２００の座標Ｙ軸の水平面に対する角度である第２ロール角φ₂を算出する。本実施形態では、一例として、ＲＴＫ－ＧＮＳＳ（Real Time Kinematic GNSS）等の位置推定の誤差をセンチメートル単位にまで抑制できる演算手法により、第２ロール角φ₂を算出する。

【0106】

具体的には、図１１に示したように、ＲＴＫ－ＧＮＳＳ等によって算出された、第１ＧＮＳＳアンテナ２８及び第２ＧＮＳＳアンテナ３０の各々の座標Ｚ軸方向の位置の偏差をδ、第1ＧＮＳＳアンテナ２８と第２ＧＮＳＳアンテナ３０との間の距離をσとすると、δ/σは、下記の式（２１）に示すように、第２ロール角φ₂の正接となる。そして、第２ロール角φ₂の正接の逆正接は、第２ロール角φ₂となる。

・・・（２１）

【0107】

ステップＳ１０８では、ステップＳ１０４で算出した第１ロール角φ'₁と、ステップＳ１０６で算出した第２ロール角φ₂とを比較することで、車体２００の座標軸に対するロール設置角φ_Ｅを算出して処理を終了する。しかしながら、本実施形態では、第１ロール角φ'₁と、ステップＳ１０６で算出した第２ロール角φ₂との比較に限定されない。例えば、補正後のロール姿勢角φ₁と、φ₂/ｃｏｓθとを比較してもよい。

【0108】

以上説明したように、本実施形態によれば、ＩＭＵ２６の座標軸をピッチ方向・ヨー方向だけでなくロール方向も、車体２００の座標軸に合わせることが可能となる。その結果、ＩＭＵ２６の設置角のずれに起因する車両の推測航法の誤差を低減することができ、車両の姿勢角、運動及び走行軌跡をより精度よく推定することができる。

【0109】

［第２実施形態］
続いて本発明の第２実施形態について説明する。本実施形態に係る設置角算出装置の構成は、第１実施形態の設置角算出装置１０と同様なので、第１実施形態と同一の構成については同一の符号を付して詳細な説明は省略する。

【0110】

前述のように、第１実施形態では、図５のステップＳ１０６において、ＧＮＳＳによる相対測位で第２ロール角φ₂を算出した。本実施形態では、車載カメラである撮像装置２２で取得した画像から第２ロール角φ₂を算出する。なお、本実施形態では、撮像装置２２の座標軸は、車体２００の座標軸と一致していることを前提とする。

【0111】

図１２は、撮像装置２２で取得した車体２００前方の画像の一例である。一般に、建物の壁面等の人口建造物のエッジ、又は電信柱、若しくは標識等の支柱部分等は、路面の傾斜にかかわらず鉛直に設置される場合が多い。従って、車体２００の前方、又は後方を捉えた撮像装置２２の画像からエッジを検出すると、鉛直方向のエッジを数多く検出できる。また、鉛直方向のエッジは全て同じ向きを示す。

【0112】

本実施形態では、図１２に示したような画像から鉛直方向に近いエッジを抽出し、抽出したエッジから統計的に外れ値を除去し、平均化することでエッジの鉛直方向を算出する。そして、鉛直方向に対する画像の傾きから車体２００の第２ロール角φ₂を算出する。

【0113】

図１２において、エッジの鉛直方向をベクトル７０、画像の傾きを示す、画像のフレームの縦方向に平行なベクトルをベクトル７２とすると、第２ロール角φ₂の余弦は、下記の式（２２）で表される。そして、第２ロール角φ₂の余弦の逆余弦（ａｒｃｃｏｓ）は第２ロール角φ₂となる。

・・・（２２）

【0114】

上述のように、画像に基づいて第２ロール角φ₂を算出した後は、第１実施形態と同様に、図５のステップＳ１０４で算出した第１ロール角φ₁と、画像に基づいて算出した第２ロール角φ₂とを比較することで、車体２００の座標軸に対するロール設置角φ_Ｅを算出して処理を終了する。

【0115】

以上説明したように、本実施形態によれば、撮像装置２２で取得した画像から算出した第２ロール角φ₂を用いて、ＩＭＵ２６の、車体２００の座標軸に対するロール設置角φ_Ｅを算出する。ＧＮＳＳは、衛星との通信状態によっては、測位に係る情報を適切に取得できない場合があるが、本実施形態では、衛星との通信状態にかかわりなくＩＭＵ２６の、車体２００の座標軸に対するロール設置角φ_Ｅを算出できる。

【0116】

なお、本発明は、上述した実施形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。例えば、撮像装置２２で取得した画像は、車体２００前方の画像のみならず、車体２００後方の画像でもよい。

【0117】

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

【0118】

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

【0119】

また、第１実施形態、及び第２実施形態では、設置角算出装置は図１及び図３に示した構成としたが、これに限定されない。図１３（Ａ）は、設置角算出装置の最小限の構成の一例であり、演算装置１４ＡにはＩＭＵ２６、第１ロール角を算出するためのＧＮＳＳ測距部６０、及び第２ロール角を算出するための第２ロール角算出部６２が接続されている。

【0120】

図１３（Ｂ）は、第１実施形態に係る設置角算出装置の最小限の構成の一例であり、演算装置１４ＡにはＩＭＵ２６、第１ＧＮＳＳ測距部７０、及び第２ＧＮＳＳ測距部７２が接続されている。

【0121】

図１３（Ｃ）は、第２実施形態に係る設置角算出装置の最小限の構成の一例であり、演算装置１４ＡにはＩＭＵ２６、第１ロール角等を算出するためのＧＮＳＳ測距部６０、及び撮像装置２２が接続されている。

【0122】

図１３（Ａ）～（Ｃ）に示したように、設置角算出装置は、ＩＭＵ２６と演算装置とを核にした最小限の構成でもＩＭＵ２６の設置角を算出することができる。

【0123】

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

【0124】

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

【0125】

なお、特許請求の範囲に記載の「慣性計測装置ロール角」は、明細書の発明の詳細な説明に記載の「第１ロール角」に、同「車体ロール角」は、明細書の発明の詳細な説明に記載の「第２ロール角」に、各々該当する。

【0126】

（付記項１）
メモリと、
前記メモリに接続された少なくとも１つのプロセッサと、
を含み、
前記プロセッサは、
慣性計測装置の出力に基づいて水平面に対する前記慣性計測装置のピッチ方向のピッチ姿勢角及びロール方向のロール姿勢角を推定し、前記ピッチ姿勢角及び前記ロール姿勢角を用いて補正した前記慣性計測装置の出力に基づいて前記慣性計測装置のピッチ方向のピッチ設置角及びヨー方向のヨー設置角の各々を算出し、前記水平面に対する車体のＹ軸の角度である車体ロール角を算出し、前記ピッチ設置角及び前記ヨー設置角を用いて補正した前記慣性計測装置の出力から算出した前記水平面に対する前記慣性計測装置のＹ軸の角度である慣性計測装置ロール角と、前記車体ロール角と、を比較して前記慣性計測装置のロール方向の設置角を算出する、
ように構成されている設置角算出装置。

【符号の説明】

【0127】

１０ 設置角算出装置
１２ 入力装置
１４ 演算装置
１４Ａ ＲＯＭ
１４Ｂ ＣＰＵ
１４Ｃ ＲＡＭ
１４Ｄ 入出力ポート
１６ 表示装置
１８ 記憶装置
２０ 画像情報処理部
２２ 撮像装置
２６ ＩＭＵ
２８ 第１ＧＮＳＳアンテナ
３０ 第２ＧＮＳＳアンテナ
３４ 地図情報データベース
４０ 姿勢角推定部
４２ 運動状態判定部
４４ 補正部
４６ 車体ロール角算出部
４８ ロール設置角算出部
７０、７２ ベクトル
２００ 車体

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】