JPWO2018139621A1 - 慣性計測方法と慣性計測装置及び慣性計測プログラム - Google Patents

Abstract

様々な誤差要因を効果的にキャンセルすることができ、非ＧＰＳ環境下でも高精度な検出結果が得られる移動体の慣性計測方法及び慣性計測装置である。角速度信号ωのフィルタリング処理S1s，S1uを行う。加速度信号及び角速度信号から概略の初期姿勢角を算出した後、これらを微調整するアライメント処理S2を行う。角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理S3を行う。角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理S4s，S4uを行う。加速度信号から重力加速度及び加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する位置移動量算出処理S5s，S5uを行う。

Description

本発明は、角速度センサや加速度センサ等を使用して移動体の姿勢・位置を計測する慣性計測方法と慣性計測装置及び慣性計測プログラムに関する。

従来、マルチコプタ、ロボット、自動車等の移動体に角速度センサや加速度センサを取り付け、これらの出力信号を積分して移動体の姿勢や位置等を検出する慣性計測方法が種々の用途に用いられている。この慣性計測方法において、各センサの出力信号を単純積分すると、出力信号に含まれる白色雑音が累積されて、出力値に重畳する誤差の値がランダムウォークし（ゼロ点がランダムにドリフトし）、高精度に検出することができないため、従来から、ランダムウォークを抑制するための様々な技術が提案されている。

従来、例えば特許文献１に開示されているように、移動体の角速度を検出する精角速度センサと、移動体の姿勢角を検出する粗姿勢角検出手段と、粗姿勢角検出手段及び精角速度センサの出力信号に基づいて移動体の姿勢角を推定する姿勢角推定器とを備えた移動体姿勢角検出装置があった。姿勢角推定器は、粗姿勢角信号と精加速度信号に動的カルマンフィルタを適用することにより、移動体の姿勢角及び精角速度センサのドリフト値を推定し、ドリフトをキャンセルした姿勢角推定値を出力する。粗姿勢角検出手段は、例えばＧＰＳ単体又は複数のＧＰＳからなるＤＧＰＳ（ディファレンシャル測位方式）を用いて構成される。

特開２０００−２９２１７０号公報

特許文献１の移動体姿勢角検出装置は、メインの検出手段である精角速度センサに加え、第二の検出手段である粗姿勢角検出手段を別個に設ける必要があるので、装置の構成が複雑になる。また、ＤＧＰＳを用いて粗姿勢角検出手段を構成した場合、測位衛星からの電波が届かない非ＧＰＳ環境下では高精度な検出ができなくなるので、工場や倉庫の中、橋梁の下、トンネルの中等で使用される移動体には適用することができない。さらに、ＤＧＰＳを用いたこの移動体姿勢角検出装置でも、白色雑音による影響を十分に除去することはできず、姿勢角の検出精度には限界があった。

また、慣性計測における誤差は、上述したランダムウォーク以外に、信号に重畳しているノイズによる誤差、積分を行う際に、初期のバイアスに起因する積分値の誤差等があり、その他、加速度センサと角速度センサによって検出される重力加速度と地球角速度（地球の自転や公転によって発生する角速度）も誤差となる。例えば、マルチコプタ等の慣性計測を行う場合、図２３、図２４に示すように、空中で移動体１２が傾いたり揺動したりすると加速度センサで検出される重力加速度の成分が変化するので、出力信号から重力加速度の成分をキャンセルする（補正する）のは容易ではない。地球角速度の成分についても同様である
本発明は、上記背景技術に鑑みて成されたものであり、様々な誤差要因を効果的にキャンセルすることができ、非ＧＰＳ環境下でも高精度な検出結果が得られる移動体の慣性計測方法と慣性計測装置及び慣性計測プログラムを提供することを目的とする。

本発明は、移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測方法であって、
前記角速度信号に重畳しているノイズ成分を低減するフィルタリング処理と、前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整するアライメント処理と、前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理と、前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理と、前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する位置移動量算出処理とで構成され、
前記移動体が静止している時に前記フィルタリング処理、前記アライメント処理及び前記角速度・加速度バイアス算出処理を順番に実行し、その後、前記フィルタリング処理、前記姿勢角算出処理及び前記位置移動量算出処理を順番に繰り返し実行し、
前記移動体が静止している時の前記フィルタリング処理、前記アライメント処理、前記移動体が静止している時に前記姿勢角算出処理、及び前記移動体が静止している時に前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行い、
前記移動体が運動している時の前記フィルタリング処理、前記移動体が運動している時の前記姿勢角算出処理、及び前記移動体が運動している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行う慣性計測方法である。

この慣性計測方法は、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記アライメント処理では、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出し、前記角速度・加速度バイアス算出処理では、地球角速度をゼロと仮定して前記角速度バイアスを算出し、前記姿勢角算出処理では、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出するようにしてもよい。

本発明は、移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測装置であって、
前記角速度信号に重畳しているノイズ成分を低減する処理を行うフィルタリング部と、前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整する処理を行うアライメント部と、前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する処理を行う角速度・加速度バイアス算出部と、前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する処理を行う姿勢角算出部と、前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する処理を行う位置移動量算出部とで構成され、
前記移動体が静止している時に前記フィルタリング部、前記アライメント部及び前記角速度・加速度バイアス算出部が前記の各処理を順番に実行し、その後、前記フィルタリング部、前記姿勢角算出部及び前記位置移動量算出部が前記の各処理を順番に繰り返し実行し、
前記移動体が静止している時に前記フィルタリング部が行う処理、前記アライメント部が行う処理、前記移動体が静止している時に前記姿勢角算出部が行う処理、及び前記移動体が静止している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行われ、
前記移動体が運動している時に前記フィルタリング部が行う処理、前記移動体が運動している時に前記姿勢角算出部が行う処理、及び前記移動体が運動している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行われる慣性計測装置である。

この慣性計測装置は、前記アライメント部は、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出する処理を行い、前記角速度・加速度バイアス算出部は、地球角速度をゼロと仮定して前記角速度バイアスを算出する処理を行い、前記姿勢角算出部は、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出するようにしてもよい。

本発明は、前記慣性計測方法を実行させるための各処理の実行用プログラムにより構成された慣性計測プログラムである。

本発明の慣性計測方法及び慣性計測装置は、静止している時に独特なアライメント処理と角速度・加速度バイアス算出処理とを行い、さらに姿勢角及び位置移動量を算出する時に、重力加速度及び地球角速度の成分をキャンセルする処理を行うので、重力加速度及び地球角速度に起因する誤差を小さくすることができる。

さらに、本発明は、各処理で推定を行うとき、静止状態推定フィルタと運動状態推定フィルタを使い分けているという点にも特徴がある。慣性計測の誤差としては、（ａ）信号に重畳しているノイズによる誤差、（ｂ）ノイズに起因する積分値のランダムウォーク、（ｃ）バイアスに起因する積分値の誤差の増大、（ｄ）ノイズに起因する積分値の誤差等があり、これらを１種類の推定フィルタですべて抑制することは難しい。しかし、本発明のように、静止時は（ａ）、（ｂ）、（ｃ）を抑制する静止状態推定フィルタを使用して推定を行い、運動時は（ａ）、（ｄ）を抑制する運動状態推定フィルタを使用して推定を行うことにより、慣性計測の誤差を効果的に抑制し、姿勢角及び位置移動量を高精度に計測することができる。

本発明により、初期の方位角の検出用に電子コンパスの方位信号を利用し、地球角速度を無視することによって、比較的安価な角速度センサを選択することが可能になり、装置全体のコストダウンを図ることができる。

さらに、本発明の慣性計測装置は、汎用のコンピュータに本発明の慣性計測プログラムを用いて容易に構成することができ、しかも、特許文献１の移動体姿勢角検出装置のように、ランダムウォークを抑制するために第二の検出手段を付設する必要もない。従って、装置を非常にシンプルに構成することができる。

本発明の慣性計測装置の第一の実施形態のシステム構成図である。 第一の実施形態の慣性計測装置が行う慣性計測方法（本発明の慣性計測方法の第一の実施形態）を示すフローチャートである。 図２の中のアライメント処理の内容を示すフローチャートである。 アライメント処理で行う初期姿勢角算出ステップの内容を示すフローチャートである。 アライメント処理で行う第１の初期姿勢角調整ステップの内容を示すフローチャートである。 アライメント処理で行う第２の初期姿勢角調整ステップの内容を示すフローチャートである。 アライメント処理で行う第３の初期姿勢角調整ステップの内容を示すフローチャートである。 図２の中の角速度・加速度バイアス算出処理の内容を示すスローチャートである。 図２の中の姿勢角算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。 図２の中の位置移動量算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。 静止状態推定フィルタと運動状態推定フィルタの設定方法の違いを説明するタイミングチャート（ａ）、（ｂ）である。 静止状態推定フィルタの効果を確認するために行ったシミュレーションの条件を示す図（ａ）、比較例のシミュレーションの条件を示す図（ｂ）である。 図１２（ａ）、（ｂ）の条件で行ったシミュレーション結果を示す波形（ａ）、（ｂ）である。 静止状態推定フィルタの効果を確認するために行ったシミュレーションの条件を示す図（ａ）、比較例のシミュレーションの条件を示す図（ｂ）である。 図１４（ａ）、（ｂ）の条件で行ったシミュレーション結果を示す波形（ａ）、（ｂ）である。 運動状態推定フィルタの効果を確認するために行ったシミュレーションの条件を示す図（ａ）、比較例のシミュレーションの条件を示す図（ｂ）である。 図１６（ａ）、（ｂ）の条件で行ったシミュレーション結果を示す波形（ａ）、（ｂ）である。 本発明の慣性計測装置の第二の実施形態のシステム構成図である。 第二の実施形態の慣性計測装置が行うアライメント処理の内容を示すフローチャートである。 図１９の中の初期姿勢角算出ステップの内容を示すフローチャートである。 第二の実施形態の慣性計測装置が行う角速度・加速度バイアス算出処理の内容を示すフローチャートである。 第二の実施形態の慣性計測装置が行う姿勢角算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。 地理座標系と機体座標系を示す図である。 移動体が傾いた時の重力加速度の成分の変化を示す図（ａ）、（ｂ）である。

以下、本発明の慣性計測装置と慣性計測装置及び慣性計測プログラムの第一の実施形態について、図１〜図１７に基づいて説明する。この実施形態の慣性計測装置１０は、図１に示すように、移動体１２の機体１２ａに取り付けた角速度センサ１４が出力する角速度信号ω^B及び加速度センサ１６が出力する加速度信号ａ^Bに基づいて、移動体１２の姿勢角φ（ロール），θ（ピッチ），ψ（ヨー）及び位置移動量ｒ^Gを導出する装置であり、この実施形態の慣性計測方法は、この慣性計測方法を実行させるための各処理の実行用プログラムにより構成された慣性計測プログラムを、慣性計測装置１０にインストールして実行される。なお、□^B，□^Gという表記の記号はベクトルであり、「B」は各軸成分を機体座標系で表したものであることを示し、「G」は各軸成分を地理座標系で表したものであることを示す。

慣性計測装置１０は、フィルタリング部１８、アライメント部２０、角速度・加速度バイアス算出部２２、姿勢角算出部２４、位置移動量算出部２６とで構成されている。まず、各ブロックの機能を簡単に説明する。

フィルタリング部１８は、角速度信号ω^Bに重畳しているノイズ成分を低減する処理（フィルタリング処理S1s，S1u）を行う。

アライメント部２０は、加速度信号ａ^B及び角速度信号ω^Bから概略の初期姿勢角φ0，θ0，ψ0を算出した後、加速度信号ａ^Bから重力加速度ｇp^Bを差し引いた加速度ah^Bを積分して求まる速度Vh^Bの絶対値が一定以下になり、且つ角速度信号ω^Bから地球角速度ωp^Bを差し引いた角速度αh^Bを積分して求まる角度の絶対値が一定以下の値になるように、初期姿勢角φ0，θ0，ψ0を微調整する処理（アライメント処理S2）を行う。

角速度・加速度バイアス算出部２２は、角速度信号ω^Bから地球角速度ωp^Bを差し引いた角速度バイアスωb^Bを算出するとともに、加速度信号ａ^Bから重力加速度ｇp^Bを差し引いた加速度バイアスａb^Bを算出する処理（角速度・加速度バイアス算出処理S3）を行う。

姿勢角算出部２４は、角速度信号ω^Bから地球角速度ωp^B及び角速度バイアスωb^Bを差し引いた補正角速度ωh^Bを算出し、これを積分して姿勢角φ，θ，ψを算出する処理（姿勢角算出処理S4s，S4u）を行う。

位置移動量算出部２６は、加速度信号ａ^Bから重力加速度ｇp^B及び加速度バイアスａb^Bを差し引いた補正加速度ａh^Gを算出し、これを２階積分して位置移動量ｒ^Gを算出する処理（位置移動量算出処理S5s，S5u）を行う。

慣性計測装置１０は、図２に示すように、移動体１２が静止している時に、フィルタリング処理S1s、アライメント処理S2、角速度・加速度バイアス算出処理S3を順番に実行し、その後、フィルタリング処理S1s又はS1u、姿勢角算出処理S4s又はS4u、位置移動量算出処理S5s又はS5uを順番に繰り返し実行する。

移動体１２がマルチコプタの場合、「静止」とは、例えば、地面に着地してスタンバイしている状態等のことであり、飛行を開始すると「運動」の状態になる。静止状態か運動状態かの判断は、慣性計測装置１０が角速度信号ω^Bと加速度信号ａ^Bを適宜の方法で分析することによって行う。

以下、各処理の詳細な内容について順番に説明する。図２に示すように、まず静止状態か運動状態かの判断を行い、静止状態と判断した場合、フィルタリング処理S1sを行う。フィルタリングは、角速度信号ω^Bに重畳しているノイズ成分を低減する処理を、静止状態推定フィルタＡs1を使用した推定によって行う。静止状態推定フィルタＡs1は、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。

フィルタリング処理S1sが終了した後、静止状態でアライメントが未終了の場合は、アライメント処理S2を行う。アライメント処理S2は、初期姿勢角算出ステップS21と３つの初期姿勢角調整ステップS22．S23，S24を行う。図３に示すように、アライメント処理S2を開始し、初期姿勢角φ0，θ0，ψ0が未算出の場合、初期姿勢角算出ステップS21を行う。

初期姿勢角算出ステップS21では、図４に示すように、まず、ステップS211で、加速度信号ａ^Bとフィルタリング処理後の角速度信号ω^Bを取得し、規定時間における各信号の平均値と分散を計算する。そして、規定時間の経過後、ステップS212に進み、ステップS211で算出した加速度信号ａ^Bの平均値が真の重力加速度のｇp^Bと等しいと仮定し、さらに角速度信号ω^Bの平均値が真の地球角速度ωp^Bと等しいと仮定して、初期姿勢角φ0，θ0，ψ0を算出する（ステップS212）。この仮定は、アライメント処理S2は静止時の加速度信号ａ^B及び角速度信号ω^Bに基づいて行うことが前提であり、上記の仮定により、概略の初期姿勢角φ0，θ0，ψ0を、図４の中に記載したシンプルな式で簡単に求めることができる。

初期姿勢角φ0，θ0，ψ0を算出した後、初期姿勢角θ0が未調整の場合は、第１の初期姿勢角調整ステップS22に進む。第１の初期姿勢角調整ステップS22は、初期姿勢角θ0を微調整するステップである。図５に示すように、まずステップS221に進み、地理座標系の重力加速度ｇp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇp^Bに変換する。座標変換行列Ｃ(G-B)は公知の行列であり、右辺のCφ，Cθ，Cψは各々cosφ，cosθ，cosψであり、Sφ，Sθ，Sψは各々sinφ，sinθ，sinψである。

ステップS221を行った後はステップS222に進み、加速度信号ａ^BのＸ軸成分a(x)から重力加速度ｇp^BのＸ軸成分gp(x)を差し引いて、重力加速度の影響をキャンセルした補正加速度ah(x)を算出する。そして、補正加速度ah(x)に静止状態推定フィルタＡs1を適用し、積分値であるＸ軸方向の速度vh(x)を推定する（ステップS223）。この静止状態推定フィルタＡs1は、フィルタリング処理S1sで使用したのと同じものである。

速度vh(x)を推定すると、規定時間における速度vh(x)の平均値を計算する（ステップS224）。そして、速度vh(x)の絶対値が一定以上に大きい場合はステップS225に進み、初期姿勢角θ0を直前の値から変更（増減）させる。初期姿勢角θ0を変更してステップS221〜S224を繰り返し実行すると、やがて速度vh(x)の絶対値が一定以下（≒0）になるので、S226に進み、その時のθ0を調整後の初期姿勢角θ0とする。これでθ0の調整が終了する。

初期姿勢角θ0の調整が終了した後、初期姿勢角φ0が未調整の場合は、図３に示すように、第２の初期姿勢角調整ステップS23に進む。第２の初期姿勢角調整ステップS23は、初期姿勢角φ0を微調整するステップである。図６に示すように、まずステップS231に進み、地理座標系の重力加速度ｇp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇp^Bに変換する。ステップS231は、先に説明したステップS221と同様の処理である。

ステップS231を行った後はステップS232に進み、加速度信号ａ^BのＹ軸成分a(y)から重力加速度ｇp^BのＹ軸成分gp(y)を差し引いて、重力加速度の影響をキャンセルした補正加速度ah(y)を算出する。そして、補正加速度ah(y)に静止状態推定フィルタＡs1を適用し、積分値であるＹ軸方向の速度vh(y)を推定する（ステップS233）。この静止状態推定フィルタＡs1も、フィルタリング処理S1sで使用したのと同じものである。

速度vh(y)を推定すると、規定時間における速度vh(y)の平均値を計算する（ステップS234）。そして、速度vh(y)の絶対値が一定以上に大きい場合はステップS235に進み、初期姿勢角φ0を直前の値から変更（増減）させる。初期姿勢角φ0を変更してステップS231〜S234を繰り返し実行すると、やがて速度vh(y)の絶対値が一定以下（≒0）になるので、S236に進み、その時のφ0を調整後の初期姿勢角φ0とする。これでφ0の調整が終了する。

初期姿勢角φ0の調整が終了した後、初期姿勢角ψ0が未調整の場合は、図３に示すように、第３の初期姿勢角調整ステップS24に進む。第３の初期姿勢角調整ステップS24は、初期姿勢角ψ0を微調整するステップである。図７に示すように、まずステップS241に進み、地理座標系の地球角速度ωp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωp^Bに変換する。

ステップS241を行った後はステップS242に進み、角速度信号ω^BのＸ軸成分ω(x)から地球角速度ωp^BのＸ軸成分ωp(x)を差し引いて、地球角速度の影響をキャンセルした補正角速度ωh(x)を算出する。そして、補正角速度ωh(x)に静止状態推定フィルタＡs1を適用し、積分値であるＸ軸方向の角度αh(x)を推定する（ステップS243）。この静止状態推定フィルタＡs1も、フィルタリング処理S1sで使用したのと同じものである。

角度αh(x)を推定すると、規定時間における角度αh(x)の平均値を計算する（ステップS244）。そして、角度αh(y)の絶対値が一定以上に大きい場合はステップS245に進み、初期姿勢角ψ0を直前の値から変更（増減）させる。初期姿勢角ψ0を変更してステップS241〜S244を繰り返し実行すると、やがて角度αh(x)の絶対値が一定以下（≒0）になるので、S246に進み、その時のψ0を調整後の初期姿勢角ψ0とする。これでψ0の調整が終了し、アライメント処理S2が終了する。

アライメント処理S2が終了した後、静止状態で角速度・加速度バイアスωb^B，ａb^B未算出の場合、角速度・加速度バイアス算出処理S3を行う。角速度・加速度バイアス算出処理S3は、図８に示すように、角速度バイアスωb^Bが未算出の場合、まずステップS31に進み、地理座標系の地球角速度ωp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωp^Bに変換する。そして、ステップS32に進み、角速度信号ω^Bから地球角速度ωp^Bを差し引いて、角速度誤差Δω^Bを算出する。

角速度誤差Δω^Bを算出すると、規定時間における角速度誤差Δω^Bの平均値を計算し、その結果を角速度バイアスωb^Bとし（ステップS34）、これで角速度バイアスωb^Bの算出が終了する。

角速度バイアスωb^Bの算出が終了した後、加速度バイアスａb^Bが未算出の場合はステップS35に進み、地理座標系の重力加速度ｇp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇp^Bに変換する。そして、ステップS36に進み、加速度信号ａ^Bから重力加速度ωp^Bを差し引いて、加速度誤差Δａ^Bを算出する。

加速度誤差Δａ^Bを算出すると、規定時間における加速度誤差Δａ^Bの平均値を計算し、その結果を加速度バイアスａb^Bとする（ステップS34）。これで加速度バイアスａb^Bの算出が終了し、角速度・加速度バイアス算出処理S3が終了する。

角速度・加速度バイアス算出処理S3が終了した後、静止状態の時は、図２に示すように、フィルタリング処理S1sを経て、姿勢角算出処理S4sを行う。姿勢角算出処理S4sは、図９（ａ）に示すように、まずステップS41sに進み、地理座標系の地球角速度ωp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωp^Bに変換する。そして、ステップS42sに進み、角速度信号ω^Bから地球角速度ωp^Bと角速度バイアスωb^Bを差し引いて、地球角速度等の誤差要因をキャンセルした補正角速度ωh^Bを算出する。

補正角速度ωh^Bを算出すると、ステップS43sに進み、機体座標系の補正角速度ωh^Bを、式（２）に示す座標変換行列Ｒ(B-A)を用いて姿勢角速度φ（ドット），θ（ドット），ψ（ドット）に変換する。座標変換行列Ｒ(B-A)は公知の行列である。

ステップS43sを行った後はステップS44sに進み、姿勢角速度φ（ドット），θ（ドット），ψ（ドット）にそれぞれ静止状態推定フィルタＡs1を適用し、積分値である姿勢角φ，θ，ψを推定する。この静止状態推定フィルタＡs1は、フィルタリング処理S1sで使用したのと同じものである。そして、座標変換行列Ｃ(G-B)を更新して姿勢角算出処理S4sが終了する（ステップS45s）。

姿勢角算出処理S4sが終了した後、静止状態の時は、図２に示すように、位置移動量算出処理S5sを行う。位置移動量算出処理S5sは、図１０（ａ）に示すように、まずステップS51sに進み、地理座標系の重力加速度ｇp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇp^Bに変換する。そして、ステップS52sに進み、加速度信号ａ^Bから重力加速度ｇp^Bと加速度バイアスａb^Bを差し引いて、重力加速度等の誤差要因をキャンセルした補正加速度ａh^Bを算出する。

補正加速度ａh^Bを算出すると、ステップS53sに進み、機体座標系の補正加速度ａh^Bを、式（１）に示す座標変換行列Ｃ(G-B)を用いて地理座標系の加速度ａh^Gに変換する。

ステップS53sを行った後はステップS54sに進み、加速度ａh^Gの各軸成分に静止状態推定フィルタＡs2を適用し、２階積分値である位置移動量ｒ^Gを推定する。静止状態推定フィルタＡs2は、先の静止状態推定フィルタＡs1と同様に、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。これで姿勢角算出処理S5sが終了する。

角速度・加速度バイアス算出処理S3が終了した後、運動状態の時は、図２に示すようにフィルタリング処理S1uを経て、姿勢角算出処理S4uを行う。フィルタリング処理S1uは、上記のフィルタリング処理S1sとは異なり、角速度信号ω^Bに重畳しているノイズ成分を低減する処理を、運動状態推定フィルタＡu1を使用した推定によって行う。運動状態推定フィルタＡu1は、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。

姿勢角算出処理S4uは、図９（ｂ）に示すように、ステップS41u〜S45uを順に行う。この中のステップS41u〜S43u，S45uは静止時のステップS41s〜S43s，S45sと同じ内容であり、異なるのはステップS44u（下線部）である。

ステップS44uでは、ステップS43uの処理結果である姿勢角速度φ（ドット），θ（ドット），ψ（ドット）に対して運動状態推定フィルタＡu1を適用し、積分値である姿勢角φ，θ，ψを推定する。この運動状態推定フィルタＡu1は、フィルタリング処理S1uで使用したのと同じものである。そして、座標変換行列Ｃ(G-B)を更新し、姿勢角算出処理S4uが終了する（ステップS45u）。

姿勢角算出処理S4uが終了した後、運動状態の時は、図２に示すように、位置移動量算出処理S5uを行う。位置移動量算出処理S5uは、図１０（ｂ）に示すように、ステップS51u〜S54uを順に行う。この中のステップS51u〜S53uは静止時のステップS51s〜S53sと同じ内容であり、異なるのはステップS54u（下線部）である。

ステップS54uでは、ステップS53uの処理結果である加速度ａh^Gの各軸成分に対して運動状態推定フィルタＡu2を適用し、２階積分値である位置移動量ｒ^Gを推定する。この運動状態推定フィルタＡu2は、先の運動状態推定フィルタＡu1と同様に、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。これで姿勢角算出処理S5uが終了する。

このように、慣性計測装置１０は、移動体１２が静止している時に、フィルタリング処理S1s、アライメント処理S2、角速度・加速度バイアス算出処理S3を順番に実行し、その後、フィルタリング処理S1s又はS1u、姿勢角算出処理S4s又はS4u、位置移動量算出処理S5s又はS5uを順番に繰り返し実行することによって、角速度信号ω^B及び加速度信号ａ^Bに基づいて、移動体１２の姿勢角φ（ロール），θ（ピッチ），ψ（ヨー）及び位置移動量ｒ^Gを導出する。

次に、４つの状態推定フィルタＡs1，Ａs2，Ａu1，Ａu2の中の、静止状態推定フィルタＡs1と運動状態推定フィルタＡu1について説明する。

静止状態推定フィルタＡs1は、静止時のアライメント処理S2、姿勢角算出処理S4sで１階積分を行う際に使用されるフィルタであり、静止時のフィルタリング処理S1sでも使用されている。静止状態推定フィルタＡs1には、推定を行うための演算式として次の式（３）、（４）が設定されている。

式（３）、（４）では、状態変数をｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。ｖはａの積分値であり、例えばａを加速度とすればｖが速度になり、ａを角速度とすればｖが角度になる。そして、式（３）の中の４行４列の行列が、静止時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値のランダムウォーク、及び積分を行う際の、バイアス起因の積分値の誤差の増大を抑制するように設定されている。

運動状態推定フィルタＡs1は、運動時の姿勢角算出処理S4uで１階積分を行う際に使用されるフィルタであり、運動時のフィルタリング処理S1uでも使用されている。運動状態推定フィルタＡu1には、推定を行うための演算式として、上記の式（４）と次の式（５）とが設定されている。

式（４）、（５）では、状態変数をｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。そして、式（５）の中の４行４列の行列は、運動時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されている。

式（３）と式（５）を比較して分かるように、各行列の内容が異なるのは第２行である。静止状態推定フィルタＡs1の第２行は、図１１（ａ）に示すように、タイミングk-1の後のしばらくの間（例えば、タイミンブk-1とタイミングkの中間点までの間）、積分値ｖが一定に保持されるという考え方で設定されている。一方、運動状態推定フィルタＡu1の第２行は、図１１（ｂ）に示すように、タイミングk-1からタイミングkまでの間、積分値ｖが一定の傾きで変化するという考え方で設定されている。この第２行の設定方法の違いにより、静止状態推定フィルタＡs1は静止状態に適した推定を行うことができ、運動状態推定フィルタＡu1は、運動状態に適した推定を行うことができる。

発明者は、静止状態推定フィルタＡs1の動作を確認するため、シミュレーションによる試験を行った。第１の試験は、図１２（ａ）に示すように、信号ａを白色雑音（統計的に制御されたノイズ発生手段が発生させた平均値０、分散１の信号）とし、静止状態推定フィルタＡs1を適用して信号ａと積分値ｖの推定を行った。また、比較例として、図１２（ｂ）に示すように、一般的な積分器２８を用いて信号ａの積分値ｖを計算した。

第１の試験の結果は、図１３（ａ）、（ｂ）に示す通りである。静止状態推定フィルタＡs1の推定ａを見ると、白色雑音（＝信号ａ）が低減していることが分かる。また、積分器２８の計算ｖ（比較例）を見ると、時間とともにゼロ点がランダムにドリフトしており、上述した積分を行う際の、ノイズ起因の積分値のランダムウォークが発生していることが分かる。これに対して、静止状態推定フィルタＡs1の推定ｖを見ると、ゼロ点がほとんどドリフトせず、ランダムウォークが効果的に抑制されている。

また、静止状態推定フィルタＡs1の動作を確認するため、第２の試験を行った。第２の試験は、図１４（ａ）に示すように、信号ａをバイアス（信号レベル１）に上記の白色雑音を重畳させたものとし、静止状態推定フィルタＡs1を適用して信号ａと積分値ｖの推定を行った。また、比較例として、図１４（ｂ）に示すように、一般的な積分器２８を用いて信号ａの積分値ｖを計算した。

第２の試験の結果は、図１５（ａ）、（ｂ）に示す通りである。静止状態推定フィルタＡs1の推定ａを見ると、信号ａに含まれる白色雑音が低減していることが分かる。また、積分器２８の計算ｖ（比較例）を見ると、時間とともにゼロ点が右肩上がりにドリフトしており、上述した積分を行う際の、バイアス起因の積分値の誤差の増大が発生していることが分かる。これに対して、静止状態推定フィルタＡs1の推定ｖを見ると、ゼロ点がほとんどドリフトせず、バイアス起因の誤差の増大が効果的に抑制されている。

さらに、運動状態静止状態推定フィルタＡu1の動作を確認するため、第３の試験を行った。第３の試験は、図１６（ａ）に示すように、信号ａを理想正弦波（振幅１、周波数４Hz）に上記の白色雑音を重畳させたものとし、運動状態推定フィルタＡu1を適用して信号ａと積分値ｖの推定を行った。また、比較例として、図１６（ｂ）に示すように、一般的な積分器２８を用いて信号ａの積分値ｖを計算した。

第３の試験の結果は、図１７（ａ）、（ｂ）に示す通りである。運動状態推定フィルタＡu1の推定ａを見ると、信号ａに含まれる白色雑音が低減していることが分かる。また、積分器２８の計算ｖ（比較例）を見ると、ノイズ起因による積分値の誤差が見られるが、運動状態推定フィルタＡu1の推定ｖを見ると、この積分によるノイズ起因の誤差が効果的に抑制されている。

次に、４つの状態推定フィルタＡs1，Ａs2，Ａu1，Ａu2の中の、静止状態推定フィルタＡs2と運動状態推定フィルタＡu2について説明する。

静止状態推定フィルタＡs2は、静止時の位置移動量算出処理S5sで２階積分を行う際に使用されるフィルタである。静止状態推定フィルタＡs2には、推定を行うための演算式として次の式（６）、（７）が設定されている。

式（６）、（７）では、状態変数をｒ(k)、ｒ(k-1)、ｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。ｖはaの積分値であり、ｒはｖの積分値である。つまり、ａを加速度とすれば、ｖが速度でｒが位置移動量になる。そして、式（６）の中の６行６列の行列が、静止時の信号に重畳しているノイズを低減し、且つノイズ起因の積分値のランダムウォーク及びバイアス起因の積分値の誤差の増大を抑制するように設定されている。

運動状態推定フィルタＡu2は、運動時の位置移動量算出S5uで２階積分を行う際に使用されるフィルタである。運動状態推定フィルタＡu2には、推定を行うための演算式として、上記の式（７）と次の式（８）とが設定されている。

式（７）、（８）では、状態変数をｒ(k)、ｒ(k-1)、ｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。そして、式（８）の中の６行６列の行列は、運動時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されている。

式（６）と式（８）を比較して分かるように、各行列の内容が異なるのは第２行と第４行である。第４行の違いは、図１１（ａ）、（ｂ）を用いて先に説明した通りである。すなわち、静止状態推定フィルタＡs2の第４行は、タイミングk-1の後のしばらくの間、積分値ｖが一定に保持されるという考え方で設定され、運動状態推定フィルタＡu2の第４行は、図１１（ｂ）に示すように、タイミングk-1からタイミングkまでの間、積分値ｖが一定の傾きで変化するという考え方で設定されている。

第２行の考え方も、第４行の考え方と同じである。すなわち、静止状態推定フィルタＡs2の第２行は、タイミングk-1の後のしばらくの間、積分値ｒが一定に保持されるという考え方で設定され、運動状態推定フィルタＡu2の第２行は、タイミングk-1からタイミングkまでの間、積分値ｒが一定の傾きで変化するという考え方で設定されている。

この第２行及び第４行の設定方法の違いにより、静止状態推定フィルタＡs2は、静止状態に適した推定を行うことができ、運動状態推定フィルタＡu2は、運動状態に適した推定を行うことができる。

以上説明したように、この実施形態の慣性計測方法及びその慣性計測プログラムがインストールされた慣性計測装置１０は、静止している時にアライメント処理S2と角速度・加速度バイアス算出処理S3を行い、姿勢角及び位置移動量を算出する時に、重力加速度及び地球角速度の成分をキャンセルする処理を行うので、重力加速度及び地球角速度に起因する誤差を小さくすることができる。

さらに、各処理で推定を行うとき、静止状態推フィルタＡs1，Ａs2と運動状態推定フィルタＡu1，Ａs2とを使い分けている。従って、慣性計測で問題になる（ａ）信号に重畳しているノイズによる誤差、（ｂ）ノイズ起因の積分値のランダムウォーク、（ｃ）バイアス起因の積分値の誤差の増大、（ｄ）ノイズ起因の積分値の誤差等に対し、静止時は静止状態推定フィルタＡs1，Ａs2を使用した推定を行って（ａ）、（ｂ）、（ｃ）を効果的に抑制し、運動時は運動状態推定フィルタＡu1，Ａu2を使用した推定を行って（ａ）、（ｄ）を効果的に抑制することができ、姿勢角φ，θ，ψ及び位置移動量ｒ^Gを高精度に計測することができる。

慣性計測装置１０は、汎用のコンピュータで容易に構成することができ、しかも、特許文献１の移動体姿勢角検出装置のように、ランダムウォークを抑制するために第二の検出手段を付設する必要もない。従って、装置を非常にシンプルに構成することができる。

次に、以下、本発明の慣性計測装置及び慣性計測方法の第二の実施形態について、図１８〜図２１に基づいて説明する。この実施形態の慣性計測装置３０（及び慣性計測方法）は、上記の慣性計測装置１０（及び慣性計測方法）の構成の一部を変更したものであり、ここでは、慣性計測装置１０と同様の構成は同一の符号を付して説明を省略する。

第一の実施形態の慣性計測装置１０は、必要なセンサ素子の種類が少ない（角速度センサ１４及び加速度センサ１６の２種類だけ）という特徴があるが、上記のアライメントを効果的に行うには、地球角速度を精度よく検出できる高性能な角速度センサ１４を使用する必要があり、装置が高価になる可能性がある。これに対して、第二の実施形態の慣性計測装置３０は、図１８に示すように、初期姿勢角ψ0の算出に電子コンパス３２の方位信号Ｈを利用することにより、安価な角速度センサ１４が使用可能になり、装置全体のコストを抑えることができるという特徴がある。

慣性計測装置３０が行う慣性計測方法の全体の流れは、慣性計測装置１０の流れ（図２）と同様であるが、アライメント処理S2、加速度・角速度バイアス算出処理S3及び姿勢角速度算出処理S4s，S4uの内容に少し異なる部分があるので、異なる部分について順番に説明する。

慣性計測装置３０が行うアライメント処理S2は、図１９に示すように、３つの初期姿勢角φ0，θ0，ψ0のうち、φ0，θ0についての処理は慣性計測装置１０と同じであるが、初期姿勢角ψ0は、ステップS62に示すように、電子コンパス３２の方位信号Ｈに基づいて算出し、調整は行わない。従って、初期姿勢角算出ステップS61では、図２０に示すように、初期姿勢角φ0，θ0だけを算出し(ステップS611)、これを調整する（ステップS612）という処理になっている。安価な角速度センサは、微小な地球角速度を高精度に検出することが難しいが、電子コンパス３２の方位信号Ｈを利用することによって、初期姿勢角ψ0を算出することができ、アライメント処理を行うことができる。地球角速度よりも高速の角速度の検出は、安価な角速度センサでも可能なので、角速度センサ１４のコストダウンを図ることができる。

その他、角速度・加速度バイアス算出処理S3では、図２１のステップS71，S72に示すように、角速度誤差Δω^Bを算出する時、地球角速度ωp^Bをゼロと仮定して行う。同様に、姿勢角算出処理S4s，S4uでは、図２２のステップS81s，S82sとステップS81u，S82uに示すように、角速度誤差Δω^Bを算出する時、地球角速度ωp^Bをゼロと仮定して行う。

慣性計測装置３０及びその慣性計測方法によれば、慣性計測装置１０の場合と同様の効果を得ることができ、必要なセンサの種類は増えるものの、装置全体のコストダウンを図ることができる。

なお、本発明の慣性計測方法と慣性計測装置と慣性計測装置及び慣性計測プログラムは、上記実施形態に限定されるものではない。例えば、静止状態推定フィルタの設定は、上記の静止状態推定フィルタＡs1（式（３）と（４））やＡs2（式（６）と（７））の設定に限定されるものではなく、上述した効果が得られる範囲で、式の内容は変更してもよい。また、上記実施形態では、静止状態推定フィルタＡs1を複数の処理で兼用しているが、個々の処理内容に応じて設定を変えたものを使用してもよい。

運動状態推定フィルタの設定は、上記の運動状態推定フィルタＡu1（式（４）と（５））やＡu2（式（７）と（８））の設定に限定されるものではなく、上述した効果が得られる範囲で、式の内容は変更してもよい。また、上記実施形態では、運動状態推定フィルタＡu1を複数の処理で兼用しているが、個々の処理内容に応じて設定を変えたものを使用してもよい。

また、本発明の慣性計測方法及び慣性計測装置は、空中を飛行するマルチコプタ等の慣性計測に好適であるが、マルチコプタ以外の移動体にも適用できることは言うまでもなく、上記の優れた効果を得ることができる。

１０，３０ 慣性計測装置
１２ 移動体
１２ａ 機体
１４ 角速度センサ
１６ 加速度センサ
１８ フィルタリング部
２０ アライメント部
２２ 加速度・角速度バイアス算出部
２４ 姿勢角算出部
２６ 位置移動量算出部
２８ 積分器
Ａs1，Ａs2 静止状態推定フィルタ
Ａu1，Ａu2 運動状態推定フィルタ
Ｓ1s，Ｓ1u フィルタリング処理
Ｓ2 アライメント処理
Ｓ3 角速度・加速度バイアス算出処理
Ｓ4s，Ｓ4u 姿勢角算出処理
Ｓ5s，Ｓ5u 位置移動量算出処理

Claims (5)

1. 移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測方法であって、
   前記角速度信号に重畳しているノイズ成分を低減するフィルタリング処理と、
   前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整するアライメント処理と、
   前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理と、
   前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理と、
   前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する位置移動量算出処理とで構成され、
   前記移動体が静止している時に前記フィルタリング処理、前記アライメント処理及び前記角速度・加速度バイアス算出処理を順番に実行し、その後、前記フィルタリング処理、前記姿勢角算出処理及び前記位置移動量算出処理を順番に繰り返し実行し、
   前記移動体が静止している時の前記フィルタリング処理、前記アライメント処理、前記移動体が静止している時に前記姿勢角算出処理、及び前記移動体が静止している時に前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行い、
   前記移動体が運動している時の前記フィルタリング処理、前記移動体が運動している時の前記姿勢角算出処理、及び前記移動体が運動している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行うことを特徴とする慣性計測方法。
2. 前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記アライメント処理では、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出し、前記角速度・加速度バイアス算出処理では、地球角速度をゼロと仮定して前記角速度バイアスを算出し、前記姿勢角算出処理では、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出する請求項１記載の慣性計測方法
3. 移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測装置であって、
   前記角速度信号に重畳しているノイズ成分を低減する処理を行うフィルタリング部と、
   前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整する処理を行うアライメント部と、
   前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する処理を行う角速度・加速度バイアス算出部と、
   前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する処理を行う姿勢角算出部と、
   前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する処理を行う位置移動量算出部とで構成され、
   前記移動体が静止している時に前記フィルタリング部、前記アライメント部及び前記角速度・加速度バイアス算出部が前記の各処理を順番に実行し、その後、前記フィルタリング部、前記姿勢角算出部及び前記位置移動量算出部が前記の各処理を順番に繰り返し実行し、
   前記移動体が静止している時に前記フィルタリング部が行う処理、前記アライメント部が行う処理、前記移動体が静止している時に前記姿勢角算出部が行う処理、及び前記移動体が静止している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行われ、
   前記移動体が運動している時に前記フィルタリング部が行う処理、前記移動体が運動している時に前記姿勢角算出部が行う処理、及び前記移動体が運動している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行われることを特徴とする慣性計測装置。
4. 前記アライメント部は、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出する処理を行い、前記角速度・加速度バイアス算出部は、地球角速度をゼロと仮定して前記角速度バイアスを算出する処理を行い、前記姿勢角算出部は、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出する処理を行う請求項３記載の慣性計測装置。
5. 請求項１又は２記載の慣性計測方法を実行させるための前記各処理の実行用プログラムにより構成された慣性計測プログラム。

Images