特開 2024-41548 自律航法システム

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

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

(11)【公開番号】P2024041548

(43)【公開日】2024-03-27

(54)【発明の名称】自律航法システム

(51)【国際特許分類】

   G01C 21/28 20060101AFI20240319BHJP

【ＦＩ】

G01C21/28

【審査請求】未請求

【請求項の数】9

【出願形態】ＯＬ

(21)【出願番号】P 2022146423

(22)【出願日】2022-09-14

(71)【出願人】

【識別番号】000010098

【氏名又は名称】アルプスアルパイン株式会社

(74)【代理人】

【識別番号】100099748

【弁理士】

【氏名又は名称】佐藤 克志

(74)【代理人】

【識別番号】100103171

【弁理士】

【氏名又は名称】雨貝 正彦

(74)【代理人】

【識別番号】100105784

【弁理士】

【氏名又は名称】橘 和之

(74)【代理人】

【識別番号】100098497

【弁理士】

【氏名又は名称】片寄 恭三

(72)【発明者】

【氏名】清野 裕之

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

2F129

【Ｆターム（参考）】

2F129AA03

2F129BB02

2F129BB22

2F129BB26

2F129BB38

2F129BB66

(57)【要約】

【課題】慣性センサの取付け角のヨー角が大きい場合にも慣性センサの取付け角の推定を良好に行う「自律航法システム」を提供する。
を提供する。
【解決手段】センサ取付角検出部１３において６軸慣性センサ１１のおおよその取付角のヨー角A3を検出し、センサ出力変換部１５において、６軸慣性センサ１１の座標系のXs,Ys、Zsの３軸の加速度出力（ax,ay,az)と角速度出力（ωx,ωy,ωz)を、検出した取付角A3が属する角度範囲に応じて、ヨー角が±45以内の座標系のXs',Y's、Zsの３軸の加速度（ax',ay',az)と角速度（ωx',ωy',ωz)に変換し、カルマンフィルタ１６に入力する。
【選択図】図１

【特許請求の範囲】

【請求項1】

移動体に搭載される自律航法システムであって、
慣性センサと、
前記慣性センサの前記移動体に対する取付角のヨー角を検出するヨー角検出手段と、
前記慣性センサの出力を変換し出力する変換手段と、
前記変換手段の出力を用いて前記移動体の状態を推定するカルマンフィルタとを有し、
前記カルマンフィルタが推定する状態は、前記慣性センサの取付角を含み、
前記変換手段は、前記ヨー角検出手段が検出したヨー角に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±９０°以内の仮想の慣性センサの出力に変換し出力することを特徴とする自律航法システム。

【請求項2】

移動体に搭載される自律航法システムであって、
慣性センサと、
前記慣性センサの前記移動体に対する取付角のヨー角が属する角度範囲をヨー角角度範囲として検出するヨー角角度範囲検出手段と、
前記慣性センサの出力を変換し出力する変換手段と、
前記変換手段の出力を用いて前記移動体の状態を推定するカルマンフィルタとを有し、
前記カルマンフィルタが推定する状態は、前記慣性センサの取付角を含み、
前記変換手段は、前記ヨー角角度範囲検出手段が検出したヨー角角度範囲に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±９０°以内の仮想の慣性センサの出力に変換し出力することを特徴とする自律航法システム。

【請求項3】

請求項２記載の自律航法システムであって、
前記ヨー角角度範囲検出手段は、-45°と+45°の間の角度範囲、+45°と+135°の間の角度範囲、+135°と+180°の間の角度範囲、-45°と-135°の間の角度範囲、-135°と-180°の間の角度範囲のうちの、前記慣性センサの前記移動体に対する取付角のヨー角が属する角度範囲を前記ヨー角角度範囲として検出し、
前記変換手段は、前記ヨー角角度範囲検出手段が検出したヨー角角度範囲に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±４５°以内の仮想の慣性センサの出力に変換し出力することを特徴とする自律航法システム。

【請求項4】

請求項３記載の自律航法システムであって、
前記慣性センサは、X、Y、Zの直交する３軸方向の加速度（ax,ay,az)を出力する慣性センサであり、
前記変換手段は、前記変換として加速度（ax,ay,az)の加速度（ax',ay',az')への変換を行うと共に、当該加速度（ax',ay',az')への変換を、
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、
（ax',ay',az')=（ax,ay,az)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、
（ax',ay',az')=（-ay,ax,az)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、
（ax',ay',az')=（-ax,-ay,az)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、
（ax',ay',az')=（ay,-ax,az)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、
（ax',ay',az')=（-ax,-ay,az)
とすることにより行うことを特徴とする自律航法システム。

【請求項5】

請求項３記載の自律航法システムであって、
前記慣性センサは、X、Y、Zの直交する３軸まわりの角速度（ωx,ωy,ωz)を出力する慣性センサであり、
前記変換手段は、前記変換として角速度（ωx,ωy,ωz)の角速度（ωx',ωy',ωz')への変換を行うと共に、当該角速度（ωx',ωy',ωz')への変換を、
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、
（ωx',ωy',ωz')=（ωx,ωy,ωz)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、
（ωx',ωy',ωz')=（-ωy,ωx,ωz)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、
（ωx',ωy',ωz')=（-ωx,-ωy,ωz)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、
（ωx',ωy',ωz')=（ωy,-ωx,ωz)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、
（ωx',ωy',ωz')=（-ωx,-ωy,ωz)
とすることにより行うことを特徴とする自律航法システム。

【請求項6】

請求項４記載の自律航法システムであって、
前記慣性センサは、加速度（ax,ay,az)に加え、前記X、Y、Zの３軸まわりの角速度（ωx,ωy,ωz)を出力し、
前記変換手段は、前記変換として、さらに、角速度（ωx,ωy,ωz)の角速度（ωx',ωy',ωz')への変換を行うと共に、当該角速度（ωx',ωy',ωz')への変換を、
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、
（ωx',ωy',ωz')=（ωx,ωy,ωz)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、
（ωx',ωy',ωz')=（-ωy,ωx,ωz)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、
（ωx',ωy',ωz')=（-ωx,-ωy,ωz)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、
（ωx',ωy',ωz')=（ωy,-ωx,ωz)
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、
（ωx',ωy',ωz')=（-ωx,-ωy,ωz)
とすることにより行うことを特徴とする自律航法システム。

【請求項7】

請求項２、３、４、５または６記載の自律航法システムであって、
前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のヨー角を検出し、
当該自律航法システムは、
前記ヨー角角度範囲検出手段が検出したヨー角とヨー角角度範囲とに基づいて、前記仮想の慣性センサのヨー角を算定し、前記カルマンフィルタが推定する取付角のヨー角の初期値として当該カルマンフィルタに設定する初期値設定手段を有することを特徴とする自律航法システム。

【請求項8】

請求項７記載の自律航法システムであって、
前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のロール角とピッチ角を検出し、
前記初期値設定手段は、前記ヨー角角度範囲検出手段が検出したロール角とピッチ角とヨー角角度範囲とに基づいて、前記仮想の慣性センサのピッチ角を算定し、前記カルマンフィルタが推定する取付角のピッチ角の初期値として当該カルマンフィルタに設定することを特徴とする自律航法システム。

【請求項9】

請求項３、４、５または６記載の自律航法システムであって、
前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のロール角A1とピッチ角A2とヨー角A3とを検出し、
当該自律航法システムは、
前記カルマンフィルタが推定する取付角のピッチ角の初期値SA2とヨー角の初期値SA3を設定する初期値設定手段を有し、
当該初期値設定手段は、
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、
SA2=A2
SA3=A3
ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、
SA2=A1
SA3=A3-90°
ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、
SA2=-A2
SA3=A3-180°
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、
SA2=-A1
SA3=A3+90°
ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、
SA2=-A2
SA3=A3+180°
により初期値SA2とヨー角の初期値SA3を算定することを特徴とする自律航法システム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、カルマンフィルタを用いた自律航法システムに関するものである。

【背景技術】

【0002】

カルマンフィルタを用いた自律航法システムとしては、自動車に搭載された自律航法システムが知られている（たとえば、特許文献１）。
この自律航法システムでは、車速センサの出力と、３軸方向の加速度を検出する加速度センサの出力と、３軸周りの角速度を検出するジャイロの出力と、ＧＰＳ受信機の出力を反復拡張カルマンフィルタで処理して、自動車の三次元位置ベクトル等の状態を推定している。

【0003】

また、本発明に関する技術として、Xs軸方向の加速度を検出する加速度センサの出力と、Xs軸と垂直なZs軸周りの角速度を検出するジャイロの出力と、ＧＰＳ受信機の出力を用いて、当該加速度センサやジャイロを含む装置の自動車への取付け角を検出する技術が知られている（たとえば、特許文献２）。

【0004】

ここで、この技術では、自動車の前方方向に向かう軸をXm、自動車の上方方向に向かう軸をZmとして、Xm-Zm座標系に対する、Xs-Zs座標系のピッチ角θpitch、ヨー角θyow、ロール角θrollを、自動車への取付け角として、次のように求めている。
すなわち、車両の進行方向の加速度成分がゼロの時における加速度センサの出力する加速度Asを積算し、平均化させた値Aaveを算出する。そして、式１に示すように、値Aaveを重力加速度gで除算しsin(θpitch)を求め、sin(θpitch)の値よりピッチ角θpitchを得る。

【0005】

【数1】

次に、式２に示すように、加速度Asからg×sin(θpitch)を減算し値Asxとする。そして、ＧＰＳ受信機の出力に基づいて自動車のX軸方向の加速度Arを求め、式３に示すように、加速度Arの余弦値Ar×cos(θpitch)を除算することで、cos(θyaw)を求め、cos(θyaw)の値よりヨー角θyawを得る。

【0006】

【数2】

また、ＧＰＳ受信機の出力に基づいて自動車のZ軸周りの角速度ωrを求めると共に、式４に示すように、角速度ωrとジャイロの出力する角速度ωsとの比をとることで、ジャイロの感度Sgを求める。そして、式５に示すように、感度Sgをcos(θpitch)で除算することでcos(θroll)を求め、cos(θroll)の値よりロール角θrollを得る。

【0007】

【数3】

【先行技術文献】

【特許文献】

【0008】

【特許文献1】特許第５１６４６４５号公報

【特許文献2】特開２００４-２３９６１３号公報

【発明の概要】

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

【0009】

３軸方向の加速度を検出する加速度センサと３軸周りの角速度を検出するジャイロとを備えた６軸慣性センサと、カルマンフィルタとを用いて自律航法システムを構築する場合、自動車の三次元位置ベクトルの算出に関わる６軸慣性センサの自動車への取付け角も状態として推定する必要がある。

【0010】

しかし、６軸慣性センサの自動車への取付け角が、初期値として設定した取付け角の標準値から乖離していると、取付け角の推定値が収束するまでに長時間を要したり、取付け角の推定値が誤った値に収束したりすることがあった。
また、６軸慣性センサの自動車への取付け角のヨー角が±90°を超えると、カルマンフィルタの推定能力の許容範囲を超えてしまい、取付け角の推定値が収束せずに発散してしまうことがあった。

【0011】

そして、このことが、慣性センサとカルマンフィルタを用いた自律航法システムにおいて、三次元位置ベクトル等の自動車の状態を適正に算出することの妨げとなっていた。
そこで、本発明は、移動体に搭載される、慣性センサとカルマンフィルタを用いた自律航法システムにおいて、慣性センサの移動体への取付け角によらずに、移動体の状態を適正に算出することを課題とする。

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

【0012】

前記課題達成のために、本発明は、移動体に搭載される自律航法システムに、慣性センサと、前記慣性センサの前記移動体に対する取付角のヨー角を検出するヨー角検出手段と、前記慣性センサの出力を変換し出力する変換手段と、前記変換手段の出力を用いて前記移動体の状態を推定するカルマンフィルタとを備えたものである。但し、前記カルマンフィルタが推定する状態は、前記慣性センサの取付角を含み、前記変換手段は、前記ヨー角検出手段が検出したヨー角に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±９０°以内の仮想の慣性センサの出力に変換し出力する。

【0013】

また、前記課題達成のために、本発明は、移動体に搭載される自律航法システムに、慣性センサと、前記慣性センサの前記移動体に対する取付角のヨー角が属する角度範囲をヨー角角度範囲として検出するヨー角角度範囲検出手段と、前記慣性センサの出力を変換し出力する変換手段と、前記変換手段の出力を用いて前記移動体の状態を推定するカルマンフィルタとを備えたものである。但し、前記カルマンフィルタが推定する状態は、前記慣性センサの取付角を含み、前記変換手段は、前記ヨー角角度範囲検出手段が検出したヨー角角度範囲に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±９０°以内の仮想の慣性センサの出力に変換し出力する。

【0014】

ここで、この自律航法システムは、前記ヨー角角度範囲検出手段において、-45°と+45°の間の角度範囲、+45°と+135°の間の角度範囲、+135°と+180°の間の角度範囲、-45°と-135°の間の角度範囲、-135°と-180°の間の角度範囲のうちの、前記慣性センサの前記移動体に対する取付角のヨー角が属する角度範囲を前記ヨー角角度範囲として検出し、前記変換手段は、前記ヨー角角度範囲検出手段が検出したヨー角角度範囲に応じて、前記慣性センサの出力を、前記移動体に対する取付角のヨー角が少なくとも±４５°以内の仮想の慣性センサの出力に変換し出力するように構成してもよい。

【0015】

また、このように構成した場合、前記慣性センサを、X、Y、Zの直交する３軸方向の加速度（ax,ay,az)を出力する慣性センサとし、前記変換手段において、前記変換として加速度（ax,ay,az)の加速度（ax',ay',az')への変換を行うと共に、当該加速度（ax',ay',az')への変換を、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、（ax',ay',az')=（ax,ay,az)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、（ax',ay',az')=（-ay,ax,az)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、（ax',ay',az')=（-ax,-ay,az)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、（ax',ay',az')=（ay,-ax,az)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、（ax',ay',az')=（-ax,-ay,az)とすることにより行うようにしてもよい。

【0016】

また、このように構成した場合、前記慣性センサを、X、Y、Zの直交する３軸まわりの角速度（ωx,ωy,ωz)を出力する慣性センサとし、前記変換手段において、前記変換として角速度（ωx,ωy,ωz)の角速度（ωx',ωy',ωz')への変換を行うと共に、当該角速度（ωx',ωy',ωz')への変換を、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、（ωx',ωy',ωz')=（ωx,ωy,ωz)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、（ωx',ωy',ωz')=（-ωy,ωx,ωz)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、（ωx',ωy',ωz')=（-ωx,-ωy,ωz)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、（ωx',ωy',ωz')=（ωy,-ωx,ωz)、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、（ωx',ωy',ωz')=（-ωx,-ωy,ωz)とすることにより行ってもよい。

【0017】

また、このように構成した場合、前記慣性センサを、加速度（ax,ay,az)と角速度（ωx,ωy,ωz)を出力するものとし、前記変換手段において、前記変換として、上述した加速度（ax,ay,az)の加速度（ax',ay',az')への変換と角速度（ωx,ωy,ωz)の角速度（ωx',ωy',ωz')への変換を行うようにしてもよい。

【0018】

このように、カルマンフィルタにおいて、変換手段で慣性センサの出力を変換して取付角のヨー角が±９０°以内や±４５°以内の仮想の慣性センサの出力を生成し、慣性センサの出力に代えてカルマンフィルタに入力することにより、慣性センサの移動体への取付け角によらずに、カルマンフィルタにおいて、取付け角の推定が発散してしまうことが抑止されると共に、取付け角の推定を正しい値に速やかに収束できることが期待できる。
ここで、以上のヨー角角度範囲検出手段を備えた自律航法システムは、前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のヨー角を検出するように構成すると共に、当該自律航法システムに、前記ヨー角角度範囲検出手段が検出したヨー角とヨー角角度範囲とに基づいて、前記仮想の慣性センサのヨー角を算定し、前記カルマンフィルタが推定する取付角のヨー角の初期値として当該カルマンフィルタに設定する初期値設定手段を備えてもよい。

【0019】

また、この場合には、さらに、前記ヨー角角度範囲検出手段において、前記慣性センサの前記移動体に対する取付角のロール角とピッチ角を検出し、前記初期値設定手段において、前記ヨー角角度範囲検出手段が検出したロール角とピッチ角とヨー角角度範囲とに基づいて、前記仮想の慣性センサのピッチ角を算定し、前記カルマンフィルタが推定する取付角のピッチ角の初期値として当該カルマンフィルタに設定してもよい。

【0020】

また、ヨー角角度範囲検出手段において、-45°と+45°の間の角度範囲、+45°と+135°の間の角度範囲、+135°と+180°の間の角度範囲、-45°と-135°の間の角度範囲、-135°と-180°の間の角度範囲のうちからヨー角角度範囲を検出する自律航法システムは、前記ヨー角角度範囲検出手段は、前記慣性センサの前記移動体に対する取付角のロール角A1とピッチ角A2とヨー角A3とを検出するように構成すると共に、当該自律航法システムに、前記カルマンフィルタが推定する取付角のピッチ角の初期値SA2とヨー角の初期値SA3を設定する初期値設定手段を備えてもよい。この場合、当該初期値設定手段は、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と+45°の間の角度範囲のとき、SA2=A2、SA3=A3、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+45°と+135°の間の角度範囲のとき、SA2=A1、SA3=A3-90°、ヨー角角度範囲検出手段が検出したヨー角角度範囲が+135°と+180°の間の角度範囲のとき、SA2=-A2、SA3=A3-180°、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-45°と-135°の間の角度範囲のとき、SA2=-A1、SA3=A3+90°、ヨー角角度範囲検出手段が検出したヨー角角度範囲が-135°と-180°の間の角度範囲のとき、SA2=-A2、SA3=A3+180°により初期値SA2とヨー角の初期値SA3を算定する。

【0021】

これらのように、カルマンフィルタで推定する取り付け角の初期値を設定することにより、６軸慣性センサの取り付け角の推定が、発散することなく速やかに収束することが期待できる。

【発明の効果】

【0022】

以上のように、本発明によれば、移動体に搭載される、慣性センサとカルマンフィルタを用いた自律航法システムにおいて、慣性センサの移動体への取付け角によらずに、移動体の状態を適正に算出することができる。

【図面の簡単な説明】

【0023】

【図1】本発明の実施形態に係る自律航法システムの構成を示すブロック図である。

【図2】本発明の実施形態においてカルマンフィルタが推定する６軸慣性センサの自動車への取付け角のヨー角を示す図である。

【図3】本発明の実施形態における６軸慣性センサの座標系から仮想６軸慣性センサの座標系の変換例を示す図である。

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

【0024】

以下、本発明の実施形態について説明する。
図１に、本実施形態に係る自律航法システムの構成を示す。
自律航法システム１は、自動車に搭載された、自動車の位置ベクトル等の自動車の状態の算出するシステムであり、自律航法システム１が算出した自動車の位置ベクトルなどの自動車の状態は、同じく自動車に搭載されている自動運転システムやナビゲーションシステム等である状態利用システム２によって利用される。

【0025】

図示するように、自律航法システム１は、６軸慣性センサ１１、衛星測位を行うＧＮＳＳ受信機１２、センサ取付角検出部１３、制御部１４、センサ出力変換部１５、カルマンフィルタ１６を備えている。
また、６軸慣性センサ１１は、６軸慣性センサ１１に対して固定された直交座標系であるセンサ座標系のXs、Ys、Zsの３軸方向の各加速度ax、ay、azを検出する加速度センサ１１１と、Xs、Ys、Zsの３軸周りの各角速度をωx、ωy、ωz検出するジャイロ１１２を備えている。
また、カルマンフィルタ１６は、予測ステップ実行部１６１とフィルタリングステップ実行部１６２を備えている。

【0026】

センサ出力変換部１５は、制御部１４から設定された加速度変換関数Taに従って、加速度センサ１１１が出力する加速度ax、ay、azを、加速度ax'、ay'、az'に変換してカルマンフィルタ１６に出力すると共に、制御部１４から設定された角速度変換関数Tωに従って、ジャイロ１１２が出力する角速度ωx、ωy、ωzを、角速度ωx'、ωy'、ωz'に変換してカルマンフィルタ１６に出力する。
次に、カルマンフィルタ１６は、式６に示す状態方程式と式７に示す観測方程式に則って、各時点kの自動車の状態X_kを推定する。

【0027】

【数4】

推定される自動車の状態X_kには、自動車の三次元位置ベクトル（三次元移動距離）、６軸慣性センサ１１の三次元速度ベクトル、６軸慣性センサ１１のロール、ピッチ、ヨーの姿勢角、６軸慣性センサ１１の自動車への取り付け角のピッチ角とヨー角、加速度センサ１１１とジャイロ１１２の感度やバイアス等が含まれる。
式６においてu_kはセンサ出力変換部１５から入力する加速度ax'、ay'、az'、角速度ωx'、ωy'、ωz'を含む入力ベクトル、q_kは入力ベクトルの誤差を表す。また、ｆは、誤差q_kが無かったとした場合に、前時点k-1の状態X_k-1と入力ベクトルu_kから、状態X_kを導出する非線形関数である。

【0028】

式７において、Z_kは、信号処理部を介してカルマンフィルタ１６に入力するＧＮＳＳ受信器の測定値やセンサ出力変換部１５から入力する角速度ωx'、ωy'、ωz'等を含む測定値、r_kは測定値の誤差を表す。また、hは、誤差r_kが無かったとした場合に、状態X_kから測定値Z_kを導出する非線形関数である。
カルマンフィルタ１６の予測ステップ実行部１６１とフィルタリングステップ実行部１６２は、たとえば、以下の処理を行う。
すなわち、カルマンフィルタ１６の予測ステップ実行部１６１は、時点kにおいて、式８に従って状態X_k+1の事前推定値X^- _k+1を算定する。ここで、式８中の、X⁺ _kは、フィルタリングステップ実行部１６２において前時点kで算定された事後推定値である。

【0029】

【数5】

また、予測ステップ実行部１６１は、式９に従って、事前誤差共分散P^- _k+1を算定する。式９中において、Φ_k、Γ_kはそれぞれ、非線形関数fのヤコビアンで定義した行列、Φ^T _k、Γ^T _kはそれぞれ、Φ_k、Γ_kの転置行列、P⁺ _kは、フィルタリングステップ実行部１６２で前時点kにおいて算定された事後誤差共分散であり、Q_kは、誤差q_kを表す共分散行列である。

【0030】

【数6】

次に、フィルタリングステップ実行部１６２は、時点kにおいて、測定値が得られた場合、式１０、式１１、式１２をiを０から１ずつ進めながら、１回もしくは複数回である所定回数、または、所定の停止条件が成立するまで繰り返すフィルタリングステップを実行する。

【0031】

【数7】

式１０は、カルマンゲインK_k,iの算出式であり、式中P^- _kは、前時点k-1において予測ステップで算定された事前誤差共分散である。また、x⁺ _k,0は、前時点k-1において予測ステップで算定された事前推定値x^- _kとする。
また、H_kは、非線形関数hのヤコビアンで定義した行列、H^T _kは、H_kの転置行列、R_kは、誤差r_kを表す共分散行列である。

【0032】

次に、式１１は、事後推定値X⁺ _k,i+1を算定する式であり、式１２は事後誤差共分散P⁺ _k,i+1を算定する式であり、式中のIは単位行列である。
そして、フィルタリングステップ実行部１６２は、式１０、式１１、式１２の繰り返しをｎ回で停止したならば、最終的に得られている事後推定値X⁺ _k,ｎと、事後誤差共分散P⁺ _k,nを、事後推定値X⁺ _kと、事後誤差共分散P⁺ _kとする。
そして、事後推定値X⁺ _kを、自動車の状態X_k+1の推定値として、必要な場合には、事後誤差共分散P⁺ _k,nと共に状態利用システム２に出力する。
なお、時点kにおいて、測定値が得られず、フィルタリングステップを実行できなかった場合は、事前推定値X^- _kを、自動車の状態X_kの推定値として、必要な場合には、事後誤差共分散P^- _k,nと共に状態利用システム２に出力する。

【0033】

さて、このようなカルマンフィルタ１６の予測ステップ実行部１６１の処理は、事前推定値X^- _kの初期値X^- ₀、事前誤差共分散P^- _kの初期値P^- ₀を設定した後の時点k=1から開始される。
また、フィルタリングステップ実行部１６２の処理は、予測ステップ実行部１６１によって、最初の事前推定値x^- ₁と事前誤差共分散P^- ₁が算定された時点k=1から開始される。
事前推定値X^- _kの初期値X^- ₀に含める６軸慣性センサ１１の自動車への取り付け角のピッチ角の初期値SA2とヨー角の初期値SA3の設定は制御部１４が行う。その他の初期値は、期待値等を設定する。

【0034】

以下、制御部１４が行う、上述したセンサ出力変換部１５への加速度変換関数Taと角速度変換関数Tωの設定と、カルマンフィルタ１６への事前推定値X^- _kの初期値X^- ₀の一部としての、６軸慣性センサ１１の取り付け角のピッチ角の初期値SA2とヨー角の初期値SA3の設定について説明する。

【0035】

まず、加速度変換関数Taと角速度変換関数Tωの設定について説明する。
センサ取付角検出部１３は、自律航法システム１の初期設定時に、６軸慣性センサ１１の自動車への、おおよその取り付け角（ロール角A1、ピッチ角A2、ヨー角A3）を検出する。
ここで、自動車に対して固定された直交座標系である車両座標系が予め定義されており、車両座標系の３軸は、自動車の前方向に向かうXm軸、自動車の右方向に向かうYm軸、自動車の上方向に向かうZm軸である。

【0036】

そして、６軸慣性センサ１１の自動車への取付角のロール角A1は、Xm軸回りのセンサ座標系の車両座標系に対する傾きであり、ピッチ角A2はYm軸回りのセンサ座標系の車両座標系に対する傾きであり、ヨー角A3は図２ａ、ｂに示すようにZm軸回りのセンサ座標系の車両座標系に対する傾きである。ここで、図２ａ、ｂにおいて、ヨー角A3は、時計回り方向を正方向とする。

【0037】

さて、センサ取付角検出部１３の、このような６軸慣性センサ１１の自動車への、おおよそ取り付け角（ロール角A1、ピッチ角A2、ヨー角A3）の検出は、たとえば、上述した特許文献２の技術を適用し、加速度センサ１１１の出力と、ジャイロ１１２の出力と、ＧＮＳＳ受信機１２の出力から前掲した式１-式５によって推定することができる。

【0038】

ただし、センサ取付角検出部１３の６軸慣性センサ１１の自動車への、おおよそ取り付け角（ロール角A1、ピッチ角A2、ヨー角A3）の検出は、他の手法により行ってもよいし、オペレータから設定されたロール角A1、ピッチ角A2、ヨー角A3を、そのままロール角A1、ピッチ角A2、ヨー角A3として推定するものであってもよい。
いずれの場合も、ロール角A1、ピッチ角A2、ヨー角A3の推定の精度は、最低でも、各角度が、-45°から+45度、+45°から+135°、-45°から-135°、+135°から+180°-135°から-180°のいずれに属するかを判別できる精度とする。

【0039】

次に、制御部１４は、センサ取付角検出部１３が検出したヨー角A3が属する角度範囲に応じて、センサ出力変換部１５において加速度センサ１１１が出力する加速度ax、ay、azを、加速度ax'、ay'、az'に変換する加速度変換関数Taと、センサ出力変換部１５において、ジャイロ１１２が出力する角速度ωx、ωy、ωzを、角速度ωx'、ωy'、ωz'に変換する角速度変換関数Tωを次のように設定する。

【0040】

-45°＜A3＜+45°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（ax,ay,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（ωx,ωy,ωz)
+45°≦A3＜+135°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（-ay,ax,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（-ωy,ωx,ωz)
+135°≦A3＜+180°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（-ax,-ay,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（-ωx,-ωy,ωz)
-45°≧A3＞-135°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（ay,-ax,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（ωy,-ωx,ωz)
-135°≧A3≧-180°のとき、
（ax',ay',az')=Ta（ax,ay,az)=（-ax,-ay,az)
（ωx',ωy',ωz')=Tω（ωx,ωy,ωz)=（-ωx,-ωy,ωz)

【0041】

このように加速度変換関数Taと角速度変換関数Tωを設定することにより、加速度センサ１１１が出力する加速度ax、ay、azは、図５ａ-ｅにヨー角A3が属する角度範囲毎に示すXs'軸方向の加速度ax'とYs'軸方向の加速度を表すay'と、Zs軸方向の加速度を表すaz'に変換され、ジャイロ１１２が出力する角速度ωx、ωy、ωzは、Xs'軸回りの角速度を表すωx'とYs'軸回りの角速度を表すωy'とZs軸回りの角速度を表すωz'とに変換される。

【0042】

すなわち、６軸慣性センサ１１の出力は、Xs'軸、Ys'軸、Zs軸を直交３軸とする仮想のセンサ座標系を持つ仮想６軸慣性センサの出力に変換される。
また、この仮想６軸慣性センサの自動車への取り付け角のヨー角A3'は、Xs'軸とys'軸が、図示するように、Xs軸とys軸をヨー方向にヨー角A3が属する角度範囲に応じて、

【0043】

-45°＜A3＜+45°のとき0°、
+45°≦A3＜+135°のとき-90°、
+135°≦A3＜+180°のとき-180°
-45°≧A3＞-135°のとき+90°、
-135°≧A3≧-180°のとき+180°
回転したものとなるので、

【0044】

-45°＜A3＜+45°のときA3'=A3、
+45°≦A3＜+135°のときA3'=A3-90°、
+135°≦A3＜+180°のときA3'=A3-180°
-45°≧A3＞-135°のときA3'=A3+90°、
-135°≧A3≧-180°のときA3'=A3+180°

【0045】

となり、ヨー角A3によらずに、ヨー角A3'は図中に灰色で示した±45°の範囲内に収まることとなる。
よって、カルマンフィルタ１６において、６軸慣性センサ１１の出力に代えて、センサ出力変換部１５の変換によって生成した仮想６軸慣性センサの出力を用いることは、自動車への取り付け角のヨー角が±45°の範囲内のA3'である６軸慣性センサの出力を用いることと等価である。
よって、６軸慣性センサ１１の自動車への取付け角によらずに、取付け角の推定が発散してしまうことが抑止されると共に、取付け角の推定を正しい値に速やかに収束できることが期待できる。

【0046】

次に、制御部１４が行う６軸慣性センサ１１の取り付け角のピッチ角の初期値SA2とヨー角の初期値SA3の設定について説明する。
ここで、カルマンフィルタ１６は、センサ取付角検出部１３が検出したヨー角A3が属する角度範囲に応じて定まる角度θだけ、Xs軸、Ys軸、Zs軸を直交３軸とする６軸慣性センサ１１の座標系をZs軸周りに回転させた、Xs'軸、Ys'軸、Zs'軸を直交３軸とする仮想６軸慣性センサ座標系の車両座標系に対する取り付け角のピッチ角とヨー角を推定する。
そこで、制御部１４は、カルマンフィルタ１６への、取り付け角のピッチ角の初期値SA2、ヨー角の初期値SA3を、センサ取付角検出部１３が検出したピッチ角A2とヨー角A3と、ヨー角A3が属する角度範囲に応じて次のように設定する。
すなわち、
-45°＜A3＜+45°のときSA2=A2、θ=0°、SA3=A3+θ、
+135°≦A3＜+180°のときSA2=A2、θ=-180°、SA3=A3+θ
-135°≧A3≧-180°のときSA2=A2、θ=+180°、SA3=A3+θ
とする。

【0047】

また、センサ取付角検出部１３が検出したヨー角A3が属する角度範囲が次の範囲にあり、センサ座標系のZs軸が車両座標系のZm軸に近似していることが期待できる場合には、取り付け角のピッチ角の初期値SA2、ヨー角の初期値SA3を、センサ取付角検出部１３が検出したロール角A1とヨー角A3と、ヨー角A3が属する角度範囲に応じて次のように設定する。
+45°≦A3＜+135°のときSA2=A1、θ=-90°、SA3=A3+θ
-45°≧A3≧-135°のときSA2=-A1、θ=+90°、SA3=A3+θ

【0048】

制御部１４において、以上のように取り付け角のピッチ角の初期値SA2とヨー角の初期値SA3を設定することにより、６軸慣性センサ１１の取り付け角のピッチ角とヨー角の推定が、発散することなく速やかに収束することが期待できる。
なお、以上の実施形態では仮想６軸慣性センサのヨー角A3'を±45°の範囲内に収めたが、ヨー角A3'を収める範囲は必ずしも±45°の範囲でなくても良く、±90°未満の任意の範囲としてもよい。

【符号の説明】

【0049】

１…自律航法システム、２…状態利用システム、１１…６軸慣性センサ、１２…ＧＮＳＳ受信機、１３…センサ取付角検出部、１４…制御部、１５…センサ出力変換部、１６…カルマンフィルタ、１１１…加速度センサ、１１２…ジャイロ、１６１…予測ステップ実行部、１６２…フィルタリングステップ実行部。

【図1】

【図2】

【図3】