JP6901584B2 - 移動体の姿勢センサ装置 - Google Patents

Description

本発明は、自動車などの移動体の姿勢を検出する移動体の姿勢センサ装置に関する。

移動体の一例である自動車のナビゲーション、安全支援および自動運転といったアプリケーションでは、自動車の姿勢を精度良く検知することが、自動車の位置や走行状態の認識に重要である。ここで、自動車の姿勢とは、自動車の方位角（ヨー）と姿勢角（ピッチおよびロール）を指す。

移動体の姿勢をＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）で検知する場合、方位角は少なくとも１つのアンテナの地図平面上測位結果の変化から算出し、姿勢角は複数のアンテナの高度差から算出する。従って、例えば船舶のように大きい移動体においては、ＧＮＳＳから方位角・姿勢角をどちらも得ることができる（特許文献１）。

特開２００２−５４９４６号公報

しかしながら、特許文献１に記載の複数のＧＮＳＳアンテナで姿勢角を得る方法を、船舶のような大きな移動体ではなく、比較的に小さな、例えば、自動車に適用した場合、自動車の全長・全幅の制約上、複数のアンテナ間の互いの距離を船舶のようには長く取ることができない。このため、ピッチ角やロール角変化によるアンテナ間の高度差は相対的に小さいものとなり、姿勢角を精度良く得ることは困難である。

角速度センサと加速度センサを用いて、自動車等の姿勢を検知する方法も考えられる。しかし、角速度センサにはゼロ点のドリフトがあるため、角度を得るための時間積分において角度オフセット誤差が生じるという課題がある。そこで、移動体の静止を監視し、移動体の静止時に加速度センサで絶対自動車姿勢を検知し、角速度センサ由来のオフセット誤差補正を行うことが考えられる。

しかしながら、角速度センサ由来のオフセット誤差補正を行う手法では、自動車が長期間連続して走行状態にある場合、角度オフセット誤差の補正を実施することができない。

このため、カメラを利用して、カメラが取得する画像の特徴点の変化から、自動車の方位角変動（ヨー回転）を計算し、これによってヨーレートセンサによる角度オフセット誤差の補正を行うことが考えられる。

しかしながら、カメラを用いて自動車の姿勢角を検知する場合、自動車の姿勢角（ピッチ角とロール角）変動は大きくとも±１５度程度であるため、姿勢角の変化に伴って生じるカメラ画像中の特徴量変化が小さく、姿勢角を精度良く取得できない。

本発明は、上記課題に鑑みてなされたものであり、その目的は、角速度センサ由来の角度オフセット誤差の補正を、移動体の走行中に実行可能であり、高精度の移動体の姿勢を検出することができる移動体の姿勢センサ装置を実現することである。

上記目的を達成するため、本発明は次のように構成される。

移動体の姿勢センサ装置において、移動体の互いに直交する３軸の角速度を検出する角速度センサと、上記移動体の方位角を検出する方位角センサと、上記角速度センサが検出した角速度から方位角の変化を計算し、この計算した方位角の変化と、上記方位角センサが検出した方位角の変化との変化差を計算し、計算した上記変化差が閾値以下または最小となるように、上記角速度センサが検出した少なくとも１軸のオフセット誤差を計算し、上記移動体の姿勢を計算する計算式から、上記オフセット誤差を差し引いて、上記移動体の姿勢を計算するコントローラと、を備える。

本発明によれば、角速度センサ由来の角度オフセット誤差の補正を、移動体の走行中に実行可能であり、高精度の移動体の姿勢を検出することができる移動体の姿勢センサ装置を実現することができる。

実施例１における姿勢センサ装置が適用される、角速度センサを含む自動車のロケータシステムおよび運転支援システムの概略構成図である。 角速度センサおよび加速度センサが自動車に実装される形態例を示す図である。 ３次元空間上の自動車の姿勢を、２次元地図上の姿勢に投影した図である。 実施例１におけるオイラー角のヨー角から、２次元地図上での自動車の位置を計算する信号処理のブロック図を示す図である。 実施例１における、ロケータ用ＭＣＵで実行される角速度センサのゼロ点ドリフトによる角度オフセット誤差を補正するフローチャートである。 実施例１における数値探索手順のステップＳ５３をより具体的に示したフローチャートである。 実施例１における、３軸の角速度センサのヨー、ピッチ、ロール軸を自動車のヨー、ピッチ、ロール軸に対して１０度以上、各軸をずらした状態で搭載した角速度センサの状態を示す図である。 実施例１における自動車に複数の３軸角速度センサを搭載した状態を示す図である。 実施例１において、ロケータ用ＭＣＵで実行される、自動車の位置計算と、ＧＮＳＳによる自動車の位置計測結果を選択的に利用する手順の例を示すフローチャートである。 実施例１におけるマップマッチングの説明のための図である。 実施例１におけるロケータ用ＭＣＵで実行されるＧＮＳＳの信頼度に応じて、ＧＮＳＳによって得られる方位角の重みと、角速度センサによって得られる方位角の重みとを変化させる処理手順を示すフローチャートである。 実施例２を構成する角速度センサを含む自動車のロケータシステムおよび運転支援システムの構成図である。 実施例２における方位角センサであるカメラによって自動車の方位角を得る手順の一例を示す図である。 実施例２におけるレーダデバイスによって自動車の方位角を得る手順の一例を示す図である。 実施例２における自動車外に設置されたカメラよって、自動車の方位角を得る手順の一例を示す図である。 実施例２における測定対象の自動車とは別の自動車に設置されたカメラによって、測定対象の自動車の方位角を得る手順の一例を示す図である。

以下の実施形態においては、便宜上その必要があるときは、複数のセクションまたは実施の形態に分割して説明するが、特に明示した場合を除き、それらは互いに無関係なものではなく、一方は他方の一部または全部の変形例、詳細、補足説明等の関係にある。また、以下の実施の形態において、要素の数等（個数、数値、量、範囲等を含む）に言及する場合、特に明示した場合および原理的に明らかに特定の数に限定される場合等を除き、その特定の数に限定されるものではなく、特定の数以上でも以下でもよい。

さらに、以下の実施形態において、その構成要素（要素ステップ等も含む）は、特に明示した場合および原理的に明らかに必須であると考えられる場合等を除き、必ずしも必須のものではない。

同様に、以下の実施形態において、構成要素等の形状、位置関係等に言及するときは、特に明示した場合および原理的に明らかにそうでないと考えられる場合等を除き、実質的にその形状等に近似または類似するもの等を含むものとする。このことは、上記数値および範囲についても同様である。

（実施例１）
以下に示す実施例は、本発明を移動体の一例である自動車に適用した場合の例である。

図１は、本発明の実施例１における姿勢センサ装置が適用される、角速度センサを含む自動車のロケータ（位置測定）システムおよび運転支援システムの概略構成図である。

図１において、ロケータシステムでは、３軸（ヨー、ピッチ、ロール）の角速度を検出する角速度センサ１０１と、３軸の加速度センサ１０２と、方位角を検出する方位角センサとしてのＧＮＳＳセンサ（全地球航法衛星システムのセンサ）１０３と、移動体の速度を検出する速度センサ１０４とがロケータ用ＭＣＵ（コントローラ（Ｍｉｃｒｏ Ｃｏｎｔｒｏｌｌｅｒ Ｕｎｉｔ））１０５に接続される。また、地図データ記憶部１０６もロケータ用ＭＣＵ１０５に接続される。

ここで、角速度センサ１０１および加速度センサ１０２には、いわゆるＭＥＭＳ（Ｍｉｃｒｏｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ）型のセンサを用いることで、コストや設置スペース上の利点が得られる。

ロケータ用ＭＣＵ１０５から出力される自動車の位置情報は運転支援システム用ＭＣＵ１０７に送られ、例えばパワートレイン・ブレーキシステム１０８、操舵デバイス１０９の制御に用いられる。また、通信デバイス１１０を通じて外部へ自動車の情報を通知するテレメトリシステムを構成することや、ドライバー向けＧＵＩ（Ｇｒａｐｈｉｃａｌ Ｕｓｅｒ Ｉｎｔｅｒｆａｃｅ）１１１、すなわちナビゲーション画面等への位置情報表示を行うことも可能である。

図２は角速度センサ１０１および加速度センサ１０２が自動車に実装される形態例を示す図である。なお、図２は、説明のため、パッケージのフタ部分を除去して示している。

角速度センサ１０１および加速度センサ１０２は、ロケータ用ＭＣＵ１０５等のその他の要素から独立したパッケージ２０１に実装されることが望ましい。例えば、プリント基板２０２上に角速度センサ１０１、加速度センサ１０２が樹脂モールドパッケージ２０３によってまとめて実装され、通信ＩＣ２０４も同じ基板に実装される。ここで、プリント基板２０２はロケータ用ＭＣＵ１０５や運転支援システムのＭＣＵ１０７が実装されるプリント基板とは異なるものである。したがって、外部との通信は通信ＩＣ２０４およびコネクタ２０５を通じて行われる。

ここでは一例としてＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ Ａｒｅａ Ｎｅｔｗｏｒｋ）を用いる。このような構成とすることは、角速度センサ１０１、加速度センサ１０２の自動車への設置自由度を増す効果がある。すなわち、角速度センサ１０１を、ロケータ用ＭＣＵ１０５や運転支援システムのＭＣＵ１０７が実装されるプリント基板とは、別の基板に配置し、角速度センサ１０１を振動や温度変化の少ない場所へ取り付ける。これにより、外乱に対してロバストなシステムを構成できる。

ロケータ用ＭＣＵ１０５では、次式（１）に示すヨー、ピッチ、ロールの順の軸回転で定義されるオイラー角の定義に従って、角速度センサ１０１の出力によりセンサの姿勢、すなわち自動車の姿勢を示すオイラー角を計算する（ただし、オイラー角の回転順序や軸の選択は前記のものに限定されない）。

式（１）において、ｐ（ｋ）は、ｋの時点におけるヨー角速度センサの出力、ｑ（ｋ）は同ピッチ角速度センサの出力、ｒ（ｋ）は同ロール角速度センサの出力である。また、Ｔｓはサンプリング間隔であり、Ｙａｗ（ｋ）、Ｐｉｔｃｈ（ｋ）、Ｒｏｌｌ（ｋ）は、それぞれｋの時点におけるオイラー角の各角度である。

オイラー角の初期値（ｋ＝０）は、加速度センサ１０２の出力より得る。Ｐｉｔｃｈ（０）は次式（２）で得られ、Ｒｏｌｌ（０）は次式（３）で得ることができる。

ここで、式（２）および式（３）において、ｇｘ、ｇｙ、ｇｚは加速度センサ１０２のＸ、Ｙ、Ｚ軸の出力である（自動車の進行方向をＸ軸正、進行方向から見て右手方向をＹ軸正、地球中心方向をＺ軸正にとる）。

なお、加速度センサ１０２のみでは絶対方位角に相当するＹａｗ（０）を得ることはできない。従って、加速度センサ１０２および角速度センサ１０１から得るヨー角は、ある時点のヨー角からの相対的な角度変化を示すものとなる。以下、「方位角」とは方位角の変動量（相対方位角）を示す。

なお、加速度センサ１０２で、より精度の良いオイラー角初期値を得るため、例えば加速度センサ１０２に自動車の駐車中も電源を供給し、自動車の駐車時間にわたって傾き計算を行った平均を用いることも考えられる。

ロケータ用ＭＣＵ１０５では、自動車の２次元（南北−東西面とする）地図上での位置を計算する。ここで、位置とは、測定開始時点からの変位量（相対位置）であって、地球上の絶対座標ではないが、ＧＮＳＳセンサ１０３や地図データ記憶部１０６に記憶された地図データへのマッピングによって初期位置を与えられれば、絶対座標を得ることができる。

本実施例１で２次元地図上での位置を計算するためにオイラー角を用いる理由を述べる。

図３は、３次元空間上の自動車の姿勢を、２次元地図上の姿勢に投影した図である。図３において、自動車は、３次元空間３０１に存在する道路３０２を走行する。このため、道路３０２に傾斜があると、自動車の軸３０３もこの傾斜に沿って傾く。このとき、自動車に取り付けられたセンサもこの自動車の軸３０３に沿って傾くので、自動車の道路３０２に沿った旋回でヨー角速度センサが検知するのは、２次元地図３０４上の道３０５に存在する自動車の軸３０６に沿った回転ではなく、３次元図空間３０１に存在する自動車の軸３０３に沿った回転となる。

一方、オイラー角で自動車の姿勢を表現した場合、自動車が坂道を走行していても、オイラー角のヨー角は２次元空間３０４上の自動車の軸３０６のヨー角となる。このため、オイラー角のヨー角と、自動車の速度センサ１０４の時間積分を組み合わせることで、２次元地図上の自動車の変位を精度良く得ることができる。

以上の理由より、ロケータ用ＭＣＵ１０５では、自動車の２次元地図上での位置を計算するため、３軸の角速度センサを用いてオイラー角を計算する。

図４は、実施例１におけるオイラー角のヨー角から、２次元地図上での自動車の位置を計算する信号処理のブロック図を示す図であり、ＭＣＵ１０５内の機能ブロックを説明する図である。

図４において、オイラー角のヨー角４０２は、ｓｉｎ計算器４０３、ｃｏｓ計算器４０４で、それぞれｓｉｎ成分およびｃｏｓ成分に分解される。そして、ｓｉｎ計算器４０３およびｃｏｓ計算器４０４からのｓｉｎ成分およびｃｏｓ成分は、速度センサ１０４の出力と乗算器４０８ｓおよび４０８ｃで掛け算される。

さらに、乗算器４０８ｓ及び４０８ｃからの出力は、積分器４０９ｓおよび４０９ｃで、サンプリング間隔Ｔｓ４０６の時間積分が行われる。これにより、移動体である自動車の変位が計算される。つまり、移動体の方位角の変化と、速度センサ１０４が検出した速度の時間積分から得られる移動距離により、移動体の変位を計算する測位機能を有している。

ここで、速度センサ１０４が車輪速センサや対地ドップラーレーダ速度センサ等である場合は、得られる速度は３次元空間３０１上の自動車の軸３０３に沿った変位速度である。そこで、オイラー角のピッチ角４０１のｃｏｓ分を、ｃｏｓ計算器（余弦計算器）４０５を経て乗算器４０８ｓ及び４０８ｃで、ｓｉｎ計算器（正弦計算器）４０３およびｃｏｓ計算器（余弦計算器）４０４からのｓｉｎ成分およびｃｏｓ成分と掛けることで、速度センサ１０４が検出した速度を補正し、２次元地図上の速度に換算することができる。

この手順によって、２次元地図上の自動車の位置計算をさらに高精度に得ることができる。特に、前述のように、オイラー角の定義をヨー、ピッチ、ロールの順で定義しておくと、ピッチ角がロール角回転前に定義されることから、ピッチ角がロール角の影響を受けにくく、位置計算の精度向上に有効である。

図５は、実施例１における、ロケータ用ＭＣＵ１０５で実行される角速度センサ１０１のゼロ点ドリフトによる角度オフセット誤差を補正する手順について示すフローチャートである。ここでは、角速度センサ１０１を用いて計算するオイラー角のヨー、ピッチ、ロール角には角速度センサ１０１のゼロ点ドリフトによる角度オフセット誤差（ａ、ｂ、ｃ）がそれぞれ時刻ｔ１の時点で乗っているとする。この時点ではオフセット誤差の値はわからないため、上記式（１）に示すオイラー角計算の初期値は、まずｔ１の時点でのオイラー角を用いることが、最尤の値である。次に、ｔ１からｔ２までの角速度センサ１０１の結果から、ｔ２の時点までのオイラー角を計算する（ステップＳ５１）。ここで得たオイラー角のうち方位角に相当する角度であるヨー角をθ１（ｔ）とする。

ステップＳ５１に並行して、時刻ｔ１からｔ２について、ＧＮＳＳセンサ１０３による２次元地図上の方位角を得る（ステップＳ５２）。これをθ２（ｔ）とする。

もし、ステップＳ５１で得た角速度センサ１０１によるオイラー角（ヨー、ピッチ、ロール）にゼロ点ドリフトによるオフセット誤差（ａ、ｂ、ｃ）がなければ、ステップＳ５１で得たθ１（ｔ）と、ステップＳ５２で得たＧＮＳＳセンサ１０３によるθ２（ｔ）は、いずれも２次元地図上の方位角を示しているので一致する。

しかし、実際には、本発明が対象としている課題の通り、角速度センサ１０１によるオイラー角のヨー、ピッチ、ロール角に角度オフセット誤差（ａ、ｂ、ｃ）が乗っているので、これらは一致しない。もし、ｔ１の時点での角度オフセット誤差（ａ、ｂ、ｃ）がわかっていれば、これをあらかじめ差し引いた上で、ステップＳ５１の計算を行えば、θ１（ｔ）とθ２（ｔ）は一致する（時点ｔ１からｔ２にかけてのオフセット誤差変動は無視できるほど小さいとする）。

そこで、ステップＳ５３では、時刻ｔ１〜ｔ２でθ１（ｔ）＝θ２（ｔ）、すなわち、θ１（ｔ）とθ２（ｔ）との差の二乗和が最小となるようなオイラー角の角度オフセット量（ａ、ｂ、ｃ）を数値探索する。

図６は実施例１における数値探索手順のステップＳ５３をより具体的に示したフローチャートである。図６を用いて数値探索手順を詳細に述べる。

角速度センサ１０１のゼロ点ドリフトによるオフセット誤差推定値の初期値（ａ_ｎ、ｂ_ｎ、ｃ_ｎ）を設ける（ステップＳ６１）。ここではｎ＝１である。次に、オイラー角計算のｔ１時点の初期値から（ａ_ｎ、ｂ_ｎ、ｃ_ｎ）をそれぞれ差し引いた上で、式（１）に従って、ｔ１からｔ２までの角速度センサ１０１の結果を用いてオイラー角を計算する（ステップＳ６２）。この方位角（ヨー角）をθ１（ｔ）とする。

次に、ステップＳ６２で新たに得たθ１（ｔ）とＧＮＳＳセンサ１０３から得たθ２（ｔ）との差の二乗和が所定の閾値よりも小さいならば（または最小値であれば）（ステップＳ６３）、計算を終了する。ステップＳ６３において、上記θ２（ｔ）との差（角速度センサ１０１が検出した方位角の変化と、方位角センサ１０３が検出した方位角の変化との変化差）の二乗和が所定の閾値よりも小さく無い場合は（または最小値でなければ）、オフセット誤差（ａ_ｎ、ｂ_ｎ、ｃ_ｎ）の推定値を（ａ_ｎ＋１、ｂ_ｎ＋１、ｃ_ｎ＋１）に変化させる（ステップＳ６４）。

ここで、（ａ_ｎ、ｂ_ｎ、ｃ_ｎ）の変化量については、例えば最急降下法等の非線形計画法の解法によって得る。数値探索の方法はここに示した手順に限定されない。また、ヨー角は相対的な変化であるので、方位角の角度オフセット誤差ａはＧＮＳＳセンサ１０３によるｔ１時点の方位角を適用し、姿勢角に関する角度オフセット誤差であるｂとｃのみを探索してもよい。探索する変数が２つになり、探索の高速化が期待できる。

次に、ステップＳ６５に進み、前ステップＳ６４にて得た角度オフセット誤差の推定値（ａ_ｎ＋１、ｂ_ｎ＋１、ｃ_ｎ＋１）を用いてオイラー角を再度ｔ１からｔ２まで計算する。ここで得たオイラー角は、自動車の運動として矛盾のない制約条件を満たしているかどうかをステップＳ６６で検証する（後述する）。ここで制約条件を満たしていない場合はステップＳ６４に戻る。ステップＳ６４では角度オフセット誤差推定値の変化量を改めて変更し、ステップＳ６５へと処理を進める。ステップＳ６６の条件を満たしている場合は、ステップＳ６３に戻り、以降、ステップＳ６３の条件を満たすまでこのループを繰り返す。

ステップＳ６３の条件を満たした後は、図５に戻り、ステップＳ５４において、ｔ１の時点での絶対姿勢角を、上の手順で推定した角度オフセット誤差の推定値（ａ_ｎ＋１、ｂ_ｎ＋１、ｃ_ｎ＋１）を差し引いた値に更新する（補正する）。また、時点ｔ１〜ｔ２までのオイラー角もこれに従った値に更新する（補正する）。

以上の手順によって、３軸の角速度センサ１０１のゼロ点ドリフトによって生じるオイラー角（ヨー、ピッチ、ロール）のオフセット誤差は、ＧＮＳＳセンサ１０３によって得られる方位角だけで補正される。この補正は式（１）に示すようにオイラー角の計算においてヨー、ピッチ、ロールが互いに関係していることによって得られるものである。

図７は、実施例１における、３軸の角速度センサ１０１のヨー、ピッチ、ロール軸を、自動車７０１のヨー、ピッチ、ロール軸に対して１０度以上、各軸をずらした状態で搭載した角速度センサ７０３の状態を示す図である。もし、３軸の角速度センサ７０３のヨー、ピッチ、ロール軸７０４と、自動車７０１のヨー、ピッチ、ロール軸７０２とを一致させて角速度センサ７０３を設置した場合、自動車７０１が地図平面に対して傾きのない道路を走っている間、角速度センサ７０３のピッチとロールがオイラー角ヨーに無関係となり、オフセット誤差探索の収束性が悪くなる。

そこで、３軸の角速度センサ７０３のヨー、ピッチ、ロール軸は、自動車７０１のヨー、ピッチ、ロール軸に対して、例えば１０度以上、少なくとも一つの軸をずらす。好ましくは、各軸をずらした状態で搭載することが望ましい。

さらに、前述の通り、自動車としての運動に対して矛盾する結果に収束しないよう、制約条件（図６のステップＳ６６）を課すことが収束性向上のために好適である。例えば、自動車が通常の運転で生じるロール角、ピッチ角は±１５度程度であると考えられるので、ロール角、ピッチ角の値域にこのような制約を加えることで、角度オフセット誤差探索の収束性や安定性を向上することができる。

地図データ記憶部１０６などから自動車の走行中の道路の傾きなどの走行環境が既知である場合は、走行環境に応じて上記制約を変えても良い。車種や乗員数によっても上記制約範囲には違いが生じる。加速度センサ１０２や操舵角など自動車の運動に関するセンサの出力も、制約条件としての情報になりうる。

なお、角速度センサ１０１のヨー、ピッチ、ロール軸は、自動車のヨー、ピッチ、ロール軸に対して１０度以上各軸をずらした状態で搭載することが望ましいと記載したが、２度以上各軸をずらすことにより、オフセット誤差探索の収束性の悪化を抑制することが可能である。

図８は、実施例１における自動車に複数の３軸角速度センサを搭載した状態を示す図である。図８に示すように、自動車８０１に複数の３軸角速度センサ８０２を搭載した場合、それぞれに生じる角度オフセット誤差は独立である。このため、複数の３軸角速度センサ８０２を自動車８０１に搭載し、それぞれについて角度オフセット誤差（ａ、ｂ、ｃ）を求め、複数の３軸角速度センサ８０２から得られる複数の自動車の姿勢を取得する構成とすることも考えられる。つまり、複数の３軸角速度センサ８０２から得られる角速度センサ１０１のオフセット誤差除去後の移動体のヨー、ピッチ、ロールを足し合わせ、または比較し、移動体の姿勢の検出精度および信頼性を向上することができる。また、それぞれの３軸角速度センサ８０２によって得られる自動車８０１の姿勢を比較し、矛盾があればいずれかに収束不良があったことを検出できる。

なお、図８に示した例は、３軸角速度センサ８０２が２つの場合の例であるが、３つ以上の３軸角速度センサを一つの自動車に搭載することも可能である。

以上示した手順によって、角速度センサ１０１（以降７０３、８０２も含むこととする）によって計算される自動車のオイラー角を、ＧＮＳＳセンサ１０３によって、常時オフセット誤差のない状態に維持しておけば、自動車がトンネルに入ったり、ビル群によってＧＮＳＳセンサ１０３の衛星からの電波が遮断されたりする状況に陥っても、その間は角速度センサ１０１による２次元地図上での方位角計算を精度良く実施できるので、ロケータ用ＭＣＵ１０５は、自動車の位置計測を継続できる。

より具体的には、ＧＮＳＳセンサ１０３の信頼度に従って、角速度センサ１０１によって計算される自動車のオイラー角から得る自動車の位置計算と、ＧＮＳＳセンサ１０３による自動車の位置計測結果を選択的に利用する。この手順について図９を参照して説明する。

図９は、実施例１において、ロケータ用ＭＣＵ１０５で実行される、自動車の位置計算と、ＧＮＳＳセンサ１０３による自動車の位置計測結果を選択的に利用する手順の例を示すフローチャートである。

図９において、ロケータ用ＭＣＵ１０５は、ＧＮＳＳセンサ１０３の信頼度を定期的に確認する（ステップＳ９１）。これは例えば、ＧＮＳＳセンサ１０３の捕捉衛星数や受信電波強度から判断できる。

ステップＳ９１において、ＧＮＳＳセンサ１０３の信頼度が高いと判断した時は、上に示した角速度センサ１０１によって計算されるオイラー角のオフセット誤差更新を行う（ステップＳ９２）。続いて、ＧＮＳＳセンサ１０３で得る方位角より、図５に示した信号処理を経て自動車の位置を計算する（ステップＳ９３）。

一方、ステップＳ９１で、ＧＮＳＳセンサ１０３の信頼度が低いと判断した時は、角速度センサ１０１により得るオイラー角のヨー角（方位角）を用いて、自動車の位置を計算する（ステップＳ９４）。

以上のように、角速度センサ１０１によらない方位角の測定システム（上述した例ではＧＮＳＳセンサ１０３）の信頼度に応じて、オフセット誤差の補正可否を判断することで、角度オフセット誤差の補正精度を保つことができる。

また、運転支援システム向けのＭＣＵ１０７など上位のシステムへ与える方位角や位置情報の精度も維持する効果をもたらす。

ＧＮＳＳセンサ１０３による信頼度が低い時は、軌跡の地図へのマッチング（いわゆるマップマッチング）を利用することも可能である。ＧＮＳＳセンサ１０３の信頼度が低い時、短期的な方位角の計測結果の精度は低いものとなる。しかし、地図データ記憶部１０６に記憶された地図データよって、走行している道路に沿って走行した際に発生する方位角の変動は予測できる。

図１０は、実施例１におけるマップマッチングの説明のための図であり、９０度のカーブを含む道路１００１と、この道路１００１を走行した自動車の軌跡１００２とを示す図である。

一般に、カーブ地点での自動車軌跡のように、移動自由度の多い道路において、短期的な周期で自動車軌跡を予測することは難しい。一方で、図１０に示した例では、カーブ前（図の下方部分）に直線があり、カーブ後にも直線がある。従って、このカーブ前とカーブ後における直線の間の長期的な周期で発生する方位角変化は、自動車が道路１００１に沿って走る限りは９０度になることが高い確度で予想可能である。

従って、このような地図データで得られる制約を用いて自動車の方位角を推定し、ＧＮＳＳセンサ１０３によって得る方位角と合わせて、角速度センサ１０１による方位角の角度オフセット誤差推定値を得ることも可能である。

上位のシステムに与える方位角や位置情報は、上に示したようにＧＮＳＳセンサ１０３と角速度センサ１０１によるものを完全に切り替える方法の他に、ＧＮＳＳセンサ１０３によって得られるものと、角速度センサ１０１により得られるものの両方を用いる方法も考えられる。

図１１は、実施例１におけるロケータ用ＭＣＵ１０５で実行される、ＧＮＳＳセンサ１０３の信頼度に応じて、ＧＮＳＳセンサ１０３によって得られる方位角の重みと、角速度センサ１０１によって得られる方位角の重みとを変化させる処理手順を示すフローチャートである。

図１１において、ステップＳ１１１でＧＮＳＳセンサ１０３の信頼度が閾値以上かどうか確認し、ＧＮＳＳセンサ１０３の信頼度が高い場合は、ＧＮＳＳセンサ１０３で得る方位角の重みを増加して角度センサ１０１の重みを減少する（ステップＳ１１２）。ステップＳ１１１で、ＧＮＳＳセンサ１０３の信頼度が閾値未満の場合は、角速度センサ１０１によりオイラー角で得る方位角の重みを増加し、ＧＮＳＳセンサ１０３で得る方位角の重みを減少する（ステップＳ１１３）。

そして、ステップＳ１１４で両者の重みを考慮して自動車の位置を計算し、ステップＳ１１１に戻る。

このような構成は、時入力システムの切り替え時、つまりＧＮＳＳセンサ１０３と角速度センサ１０１との切り替えに発生する変位の飛びを防ぐなどの利点がある。

さらに、カルマンフィルタによって両システムからの入力を常時取り入れ、方位角ないし位置を計算するシステムも考えられる。この時、例えばシステムの状態変数としては角速度センサ１０１によるオイラー角（ヨー、ピッチ、ロール）、角速度センサ１０１によるオイラー角のオフセット誤差（ヨー、ピッチ、ロールそれぞれについて）、角速度センサ１０１各軸周りの角速度（ヨー、ピッチ、ロール）、および角速度センサ１０１各軸周りの角速度に乗っているバイアス誤差（ヨー、ピッチ、ロールそれぞれについて）である。システムの観測変数としてはバイアス誤差を含むオイラー角（ヨー、ピッチ、ロール）と、ＧＮＳＳセンサ１０３によるバイアス誤差のない方位角と、角速度センサ１０１のバイアス誤差を含む３軸出力となる。

この場合、逐一オフセット誤差のないオイラー角を得ることが可能になるため、応答性向上などの利点がある。また、カルマンフィルタを適用する場合は、ＧＮＳＳセンサ１０３や、角速度センサ１０１によって得られる方位角の信頼度に基づき、カルマンフィルタのパラメータである観測ノイズを信頼度に基づいて動的に変化させることも考えられる。

さらに、角速度センサ１０１によるオイラー角のヨーと、ＧＮＳＳセンサ１０３によって得る方位角の差をカルマンフィルタの観測変数とする構成も考えられる。これはいわゆる相補フィルタと呼ばれる構成である。

以上に説明したように、実施例１によれば、角速度センサ１０１のゼロ点ドリフトによる角度オフセット誤差補正が、ＧＮＳＳセンサ１０３から得られる方位角だけを用いて実行することができる。ＧＮＳＳセンサ１０３から得られる方位角は、自動車の走行中であっても取得可能である。

従って、自動車のような姿勢角を正確に測りにくい測定対象においても、自動車の停止を必要とせず、角速度センサ１０１で姿勢角を精度良く得ることができるので、２次元地図上での位置計算を高精度に実行できる。

つまり、角速度センサ由来の角度オフセット誤差の補正を、移動体の走行中に実行可能であり、高精度の移動体の姿勢を検出することができる移動体の姿勢センサ装置を実現することができる。

（実施例２）
次に、本発明の実施例２について説明する。

図１２は、実施例２を構成する角速度センサ１０１を含む自動車のロケータシステムおよび運転支援システムの構成図である。

図１２に示した実施例２と実施例１と同一の符号は、同一の要素を示し、これらの要素の説明は省略する。図１２に示した実施例では、ＧＮＳＳセンサ１０３が方位角センサ１１２に置き換わっており、その点が実施例１との違いである。

ここで、方位角センサ１１２は、カメラ（撮像部）であり、搭載された自動車の方位角を測定する。

図１３は、実施例２における方位角センサ１１２であるカメラ１３０２によって自動車１３０１の方位角を得る手順の一例を示す図である。

図１３において、自動車１３０１に搭載されたカメラ１３０２は、白線１３０３の映像を取得し、白線１３０３上の複数の点（例えば、１３０４ａ、１３０４ｂ、・・・）での接線（１３０５ａ、１３０５ｂ、・・・）におけるベクトルの変化量から、自動車１３０１の方位角を計算する。ここで、カメラ１３０２はステレオカメラ（２台のカメラ）として構成すれば、白線１３０３上の点の、カメラ１３０２からの距離を視差によって計算できるので好適である。

以上に示すように、自動車１３０１に搭載されたカメラ１３０２によっても、自動車１３０１の方位角を測定可能である。

従って、実施例１の図５に示した手順で、ＧＮＳＳセンサ１０３で得ていた方位角を、カメラ１３０２で得たものに置き換えることによって、角速度センサ１０１によりオイラー角で得る姿勢のオフセット誤差を補正できる。

また、図９に示したＧＮＳＳセンサ１０３と角速度センサ１０１との選択的利用については、ＧＮＳＳセンサ１０３をそのままカメラ１３０２に置き換えられる。ここで、カメラ１３０２の信頼度とは、白線検知の確からしさや、環境の明るさ等である。

ＧＮＳＳセンサ１０３の代わりにカメラ１３０２を用いることで、トンネルやビル群に囲まれた道路のように、ＧＮＳＳセンサ１０３では電波が遮断される環境においても、角速度センサ１０１によらず方位角を得ることができる。

一方、カメラ１３０１は天候変化による影響を受けやすく、また、白線のない道路での方位角検知精度が劣化するため、ＧＮＳＳセンサ１０３とカメラ１３０１との両者を組み合わせ、相補的に自動車１３０１の方位角を検知する構成とすることが好適である。

なお、ここで方位角センサ１１２は、電波、音波、レーザ等の反射信号を利用するレーダデバイスとすることも可能である。レーダで検出した検出物との相対変位によって、搭載された自動車の方位角を測定する。図１４は、実施例２におけるレーダデバイス１４０２ａによって自動車１４０１の方位角を得る手順の一例を示す図である。ここで、レーダデバイス１４０２ａは、一例としては７７ＧＨｚを中心周波数とする無線レーダがある。しかし、超音波や、レーザなど、レーダの媒体は問わない。例えば、本実施例２では機械的にレーザの指向性を変化させて高解像度に周辺の環境を認識するいわゆるＬｉＤＡＲもレーダの一種である。

自動車１４０１に搭載されたレーダデバイス１４０２ａは、無線信号１４０２ｂを発信し、検出物１４０３で反射した無線信号を受信する。レーダデバイス１４０２ａは、ここでは一例として周波数が連続的に変化するＦＭ−ＣＷ（Ｆｒｅｑｕｅｎｃｙ Ｍｏｄｕｌａｔｅｄ Ｃｏｎｔｉｎｕｏｕｓ Ｗａｖｅ）変調を送信しているものとする。ここで、ＦＭ−ＣＷ発振信号と受信信号をミキシングし、フーリエ変換によって両信号の周波数差や周波数の時間変化を見ることで、外界の検出物１４０３までの距離や、相対速度を得ることができる。また、アレーアンテナや機械的に指向性を変化させるアンテナを具備することで、対象物の方向を検知できる。

上記原理によって、自動車１４０１が変位した際に、検出物１４０３との相対距離および角度の変化から、自動車１４０１の方位角を得ることができる。

ここで検出物１４０３は、自動車１４０１の方位角を得る目的のために道路に設けられたものであっても良いし、信号機等の道路のインフラストラクチャーや、前後を走る車などでも良い。

以上に説明したように、自動車１４０１に搭載されたレーダデバイス１４０２ａによっても、自動車１４０１の方位角を測定可能である。従って、図５に示した手順で、ＧＮＳＳセンサ１０３で得ていた方位角をレーダデバイス１４０２ａで得たものに置き換えれば、角速度センサ１０１によりオイラー角で得る姿勢のオフセット誤差を補正できる。

また、図９に示したＧＮＳＳセンサ１０３は、そのままレーダデバイス１４０２ａに置き換えて、角速度センサ１０１と選択的に利用することができる。ここでレーダデバイス１４０２ａの信頼度とは、受信信号強度や、フーリエ変換等の信号処理を経て得る検出物１４０３の位置・方向環境の確からしさである。

ＧＮＳＳセンサ１０３の代わりにレーダデバイス１４０２ａを用いることで、トンネルやビル群に囲まれた道路のようにＧＮＳＳセンサ１０３では電波が遮断される環境においても、角速度センサ１０１によらず方位角を得ることができる。また、レーダデバイス１４０２ａは一般に天候変化や周辺の明るさによる影響も受けにくい。

一方、レーダデバイス１４０２ａは、一般に距離、角度とも分解能は相対的に粗い。

従って、ＧＮＳＳセンサ１０３およびレーダデバイス１４０２ａを組み合わせ、相補的に自動車の１４０１方位角を検知する構成とすることが好適である。

さらに、方位角センサ１１２は、自動車外に設置されたカメラでもよい。自動車外に設置されたカメラで検出した自動車の姿勢変化によって、対象の自動車の方位角を測定する。

図１５は、実施例２における自動車１５０１外に設置されたカメラ１５０２によって、自動車１５０１の方位角を得る手順の一例を示す図である。ここで、カメラ１５０２は、道路のインフラストラクチャーとして設置され、自動車１５０１をカメラ１５０２によって連続的に撮影し、その時間間隔毎の変化量から方位角の変化を得る。ここで得た方位角の変化量は、通信デバイス１５０３によって、自動車１５０１に通知する。ここで、自動車１５０１側にも通信デバイス１５０４が具備されている。

方位角の変化量を通知するにあたっては、自動車１５０１とは異なる自動車への送信を防ぐために、いわゆるブロードキャストではなく、１対１の通信を確立した上で通知することが望ましい。また、インフラストラクチャーとしてのカメラ１５０２が悪意ある第３者のものでないことを示すために、電子証明書を通信の確立時に送付しても良い。

また、自動車１５０１には、姿勢変化を認識させるため、マーカー１５０５を具備していても良い。マーカー１５０５は、画像認識で検出しやすい幾何学パターンや、自動車１５０１の外観を損ねない不可視光線などが考えられる。

カメラ１５０２で撮影した自動車１５０１の画像から方位角を得る方法としては、様々な方法がある。一例としては、測定対象の自動車１５０１の３次元モデル（マーカー含む）を任意の視点から２次元画像に射影した画像と、カメラ１５０２が撮影した自動車１５０１の画像について、マーカー１５０５の位置関係が最も一致するような３次元モデルの視点を求め、この視点から方位角を得る方法が考えられる。

以上説明したように、自動車１５０１外に搭載されたカメラ１５０２によっても、自動車１５０１の方位角を測定可能である。一方、カメラ１５０２を自動車１５０１外に設置した場合、ここまで示してきたような常時の方位角計測は不可能である。このため、図５に示した手順は、自動車１５０１がカメラ１５０２の撮影可能な範囲にあるときに限って、実行する（ここで、図５のＧＮＳＳセンサ１０３は自動車１５０１外に設置したカメラ１５０２と置き換えられる）。

また、自動車１５０１外に設置したカメラ１５０２によって、自動車１５０１の方位角に加えて姿勢角を検知できるように構成しても良い。この場合、図５のステップＳ５３は行われない。また、図５のステップＳ５４の代わりにカメラ１５０２で取得した方位角と姿勢角とを、取得時点の自動車１５０１の姿勢として直ちに更新することとする。

また、方位角センサ１１２は、測定対象の自動車１５０１とは別の自動車に設置されたカメラでも良い。すなわち、測定対象の自動車１５０１とは別の自動車に設置されたカメラで検出した測定対象の自動車１５０１の姿勢変化によって、対象の自動車１５０１の方位角を測定することも可能である。

図１６は、実施例２における測定対象の自動車１６０３とは別の自動車１６０１に設置されたカメラ１６０２によって、測定対象の自動車１６０３の方位角を得る手順の一例を示す図である。

測定対象の自動車１６０３には、マーカー１６０４が設置され、測定対象の自動車１６０３とは別の自動車１６０１は、カメラ１６０２を備える。

カメラ１６０２で撮影された測定対象の自動車１６０３およびマーカー１６０４は、例えば道路脇にインフラとして設置された基準マーカー１６０５や、道路上に設置された基準マーカー１６０６を基準として、図１４に示した方法等によって、測定対象の自動車１６０３の方位角および姿勢角を計算する。

計算した姿勢角及び方位角は、通信デバイス１６０７を通じて測定対象の自動車１６０３に送信される。車対車の車々間通信であっても良いが、基地局を介した通信でも良い。測定対象の自動車１６０３は、通信デバイス１６０８を通じてこれを受信し、角速度センサ１０１によりオイラー角で得る方位角および姿勢角のオフセット誤差を補正する。

測定対象の自動車１６０３もカメラ１６０９を備えており、測定対象の自動車１６０３の前方にある自動車の測定をできるように構成され、測定対象とは別の自動車１６０１にも、マーカー１６１０が搭載され、後方の自動車からの計測に対応している。

このように車々間で互いに角速度センサ１０１によりオイラー角で得る方位角および姿勢角のオフセット誤差を補正できる構成とすることで、図１５に示したようなインフラストラクチャーとしてのカメラを設置するコストが削減できる。

以上の説明のように、実施例２によれば、実施例１のＧＮＳＳセンサ１０３に代えて、カメラ等の方位角センサ１１２を用いることにより、実施例１により得られる効果の他に、トンネルやビル群に囲まれた道路のようにＧＮＳＳセンサ１０３では電波が遮断される環境においても、角速度センサ１０１によらず方位角を得ることができるという効果を得ることができる。

上述した実施例の他の実施例としては、実施例１又は実施例２の移動体の姿勢センサ装置を備える自動車がある。

なお、上述した例は、本発明を自動車に適用した場合の例であるが、自動車に限らず、小型の移動体であれば、適用可能である。例えば、電動二輪車にも本発明は適用可能である。

また、上述したオフセット誤差の計算は、３軸すべてはなく、少なくとも１軸のオフセットを計算し、それに基づいて、移動体の姿勢角を補正するように構成してもよい。

１０１、７０３、８０２・・・角速度センサ、１０２・・・加速度センサ、１０３・・・ＧＮＳＳセンサ、１０４・・・速度センサ、１０５・・・ロケータ用ＭＣＵ、１０６・・・地図データ記憶部、１０７・・・運転支援システム用ＭＣＵ、１１２・・・方位角センサ、４０３・・・ｓｉｎ計算器、４０４、４０５・・・ｃｏｓ計算器、４０８ｃ、４０８ｓ・・・乗算器、４０９ｃ、４０９ｓ・・・積分器、７０１、８０１、１３０１、１４０１、１５０１、１６０１、１６０３・・・自動車、１３０２、１５０２、１６０２、１６０９・・・カメラ、１４０２ａ・・・レーダデバイス、１４０３・・・外界の検出物、１５０３、１５０４・・・通信デバイス、１６０７、１６０８・・・通信デバイス、１６０５，１６０６、１６１０・・・マーカー

Claims (13)

1. 移動体の互いに直交する３軸の角速度を検出する角速度センサと、
   上記移動体の方位角を検出する方位角センサと、
   上記角速度センサが検出した角速度から方位角の変化を計算し、この計算した方位角の変化と、上記方位角センサが検出した方位角の変化との変化差を計算し、計算した上記変化差が閾値以下または最小となるように、上記角速度センサが検出した少なくとも１軸のオフセット誤差を計算し、上記移動体の姿勢を計算する計算式から、上記オフセット誤差を差し引いて、上記移動体の姿勢を計算するコントローラと、
   を備えることを特徴とする移動体の姿勢センサ装置。
2. 請求項１に記載の移動体の姿勢センサ装置において、
   上記コントローラは、上記角速度センサが検出した角速度から計算する方位角に制約条件を設定して、上記オフセット誤差を計算することを特徴とする移動体の姿勢センサ装置。
3. 請求項１に記載の移動体の姿勢センサ装置において、
   上記移動体の速度を検出する速度センサを備え、
   上記コントローラは、上記オフセット誤差を補正した上記移動体の方位角の変化と、上記速度センサが検出した速度の時間積分から得られる移動距離により、上記移動体の変位を計算することを特徴とする移動体の姿勢センサ装置。
4. 請求項３に記載の移動体の姿勢センサ装置において、
   上記コントローラは、上記オフセット誤差を補正した上移動体のピッチ角に応じて、上記速度センサが検出した速度を補正し、上記移動体の変位を計算することを特徴とする移動体の姿勢センサ装置。
5. 請求項３に記載の移動体の姿勢センサ装置において、
   上記コントローラは、上記方位角センサの信頼度に基づいて、上記角速度センサが検出した角速度および上記方位角センサが検出した方位角に重み付けを行い、重み付けを行った上記角速度及び方位角を用いて、上記移動体の方位角の変化を計算することを特徴とする移動体の姿勢センサ装置。
6. 請求項１に記載の移動体の姿勢センサ装置において、
   上記角速度センサは、上記コントローラとは別の基板上に配置されることを特徴とする移動体の姿勢センサ装置。
7. 請求項１に記載の移動体の姿勢センサ装置において、
   上記移動体の３軸の方位角は、オイラー角であり、上記移動体のヨー、ピッチ、ロールの順の回転で定義されることを特徴とする移動体の姿勢センサ装置。
8. 請求項７に記載の移動体の姿勢センサ装置において、
   上記３軸の角のヨー、ピッチ、ロール検出軸は、上記移動体のヨー、ピッチ、ロール軸に対し、少なくともいずれか１つの軸に１０度以上の差がある状態で設置されることを特徴とする移動体の姿勢センサ装置。
9. 請求項１に記載の移動体の姿勢センサ装置において、
   上記角速度センサは複数であり、上記コントローラは、複数の上記角速度センサのオフセット誤差除去後の移動体のヨー、ピッチ、ロールを互いに足し合わせ、または比較することを特徴とする移動体の姿勢センサ検出装置。
10. 請求項１から９のうちのいずれか一項に記載の移動体の姿勢センサ装置において、
    上記方位角センサは、全地球航法衛星システムのセンサであることを特徴とする移動体の姿勢センサ装置。
11. 請求項１から９のうちのいずれか一項に記載の移動体の姿勢センサ装置において、
    上記移動体は自動車であり、上記方位角センサは、カメラであることを特徴とする移動体の姿勢センサ装置。
12. 請求項１から９のうちのいずれか一項に記載の移動体の姿勢センサ装置において、
    上記移動体は自動車であり、上記方位角センサは、電波、音波、レーザ等の反射信号を利用するレーダであることを特徴とする移動体の姿勢センサ装置。
13. 請求項１から１２のうちのいずれか一項に記載の移動体の姿勢制御装置を備えることを特徴とする自動車。

Images