JP7055484B2

JP7055484B2 - ＩＭＵに基づくｕｗｂ測位異常値処理方法

Info

Publication number: JP7055484B2
Application number: JP2020554405A
Authority: JP
Inventors: 仁全魯; 偉孟; 仕康連; 佳文薛
Original assignee: Guangdong University of Technology
Current assignee: Guangdong University of Technology
Priority date: 2019-10-29
Filing date: 2020-09-18
Publication date: 2022-04-18
Anticipated expiration: 2040-09-18
Also published as: WO2021082790A1; JP2022500617A; CN110926460A; CN110926460B

Description

発明は、ｕｗｂ測位の技術分野に関し、特にＩＭＵに基づくｕｗｂ測位異常値処理方法に関する。

Ｕｗｂ測位技術は、１ＧＨｚ以上の周波数帯域幅を用いた無線キャリア通信技術であり、正弦波キャリアではなく、ナノ秒スケールの非正弦波ナローパルスを用いてデータを伝送するため、占めるスペクトル範囲が広く、無線通信を使用するにもかかわらず、そのデータ伝送速度が数百メガビット／ｓ以上に達する。ｕｗｂは高速データ伝送、低消費電力、高安全性、高測位精度の特徴があるため、さまざまな分野の室内測位に広く使用されており、本特許のｕｗｂの応用背景は、室内でドローンに高精度の測位データを提供することである。室内環境の複雑さ及び各ｕｗｂハードウェアの違いのため、ｕｗｂ距離測定入力値の生データにはジャンプ、ロスが生じ、実際のデータが損失されてしまい、この場合、ｕｗｂ測位に連続的かつ正確な測定データ値を提供するために、このような異常データ値を処理・充填する必要がある。

現在、異常値の常の処理方式としては、異常値を含むデータを削除し、異常値を欠損値として、欠損値処理方法で処理し、平均値で補正するか、処理しないなどがあるが、以上のような異常値処理方法では、ｕｗｂ測位データの処理過程に一定の欠陥があり、たとえば、平均値で補正する方法では、ドローンが大きな加速度で飛行する場合、平均値で異常値を代えると、明らかなデータの歪みが発生し、一方、ほかの処理方法のいずれも、後続のアルゴリズムによる推定に誤差をもたらす。

本発明の目的は、従来技術の欠陥を解決し、ＩＭＵに基づくｕｗｂ測位異常値処理方法を提供することであり、４つのｕｗｂ基地局でｕｗｂ測位を行う例では、従来の生データ前処理過程における異常値の判別が不正確であること、及び異常値補正の精度などの問題を解決し、測定ハードウェアデータの正確性を向上させる。本発明は、ハードウェア機器を追加する必要がなく、複雑な環境でデータを測定するときのｕｗｂセンサの安定性を高め、また、後続のｕｗｂ測位解に有効で正確な測定出力データを提供し、ｕｗｂ測位の安定性、効率及びリアルタイム性を高める。

上記目的を達成させるために、本発明による技術案は以下のとおりである。
ＩＭＵに基づくｕｗｂ測位異常値処理方法であって、
ドローンのＩＭＵデータを取得するステップＳ１と、
加速度計とジャイロスコープを組み合わせて、ドローンのオイラー角を計算するステップＳ２と、
ステップＳ２で得られたドローンのオイラー角から、Ｘ、Ｙ、Ｚ軸の加速度及び△ｔ時間内のＸ、Ｙ、Ｚ軸上の変位を計算し、ドローンの変位値Ｑを推定するステップＳ３と、
ステップＳ３で得られた変位値Ｑ及びｕｗｂセンサの距離データから、ｕｗｂ測定値について異常検出を行い、ｕｗｂ測定値が異常である場合、ステップＳ５に入るステップＳ４と、
異常なｕｗｂ測定値に対してデータ融合を行い、ｕｗｂ測定値を補正するステップＳ５と、を含む。

さらに、前記ステップＳ２では、具体的には、
オイラー角及びジャイロスコープのドリフトを推定し、つまり、（式

であり、ｂ_ｇ，ｋはジャイロスコープのドリフト量、θ、φはそれぞれピッチ角、ロール角である。）のようにし、

であり、

離散化して、

を得て、単純化には、Ｘ_ｋ＝Φ_ｋ－１Ｘ_ｋ－１＋Γ_ｋ－１ｗ_ｋ－１とし、
ここで、ｋ－１時刻でのシステム遷移行列は、

であり、
ｋ－１時刻でのシステムノイズ行列は、

であり、
ｋ－１時刻での状態ベクトルは、

であり、
ｋ－１時刻でのシステムノイズは、

であり、
観測モデルは、

であり、
ここで、

であり、
Ｋ時刻での観測行列は、

であり、
前記のように、単純化したプロセスモデルと観測モデルは、

になり、
プロセスモデルと観測モデルを得た後、初期化

（ここで、Ｅ（Ｘ_０）は零時刻での数学的期待値、Ｅ［（Ｘ_０－Ｅ（Ｘ_０））（Ｘ_０－Ｅ（Ｘ_０））^Ｔ］は零時刻の誤差共分散の計算を示す。）を行い、
状態推定予測Ｘ_{ｋ｜ｋ－１}＝Φ_ｋ－１Ｘ_ｋ－１を行い、
誤差共分散予測：
Ｐ_{ｋ｜ｋ－１}＝Φ_ｋ－１Ｐ_{ｋ－１｜ｋ－１}Φ_ｋ－１ ^Ｔ＋Γ_ｋ－１Ｑ_ｋ－１Γ_ｋ－１ ^Ｔ（Ｑ_ｋ－１はシステムノイズ共分散行列を示す。）を行い、
カルマンゲイン行列は、

（ここで、Ｈ_ｋ＝［Ｏ_{３×３－ｇ}Ｉ］。Ｒ^Ｔは測定ノイズ共分散行列である。）であり、
状態推定更新：
Ｘ_ｋ｜ｋ＝Ｘ_{ｋ｜ｋ－１}＋Ｋ_ｋ（Ｚ－Ｈ_ｋＸ_{ｋ｜ｋ－１}）
（ここで、Ｚは観測データである。）を行い、
誤差共分散更新：
Ｐ_ｋ｜ｋ＝（Ｉ－Ｋ_ｋＨ_ｋ）Ｐ_{ｋ｜ｋ－１}
（Ｉは単位行列である。）を行い、状態ベクトルＸ_ｋから正確なオイラー角を得る。
さらに、前記ステップＳ３では、Ｘ、Ｙ、Ｚ軸の加速度の計算は、具体的には、

（ここで、ｘ_ｋ（ｉ，ｊ）はｘ_ｋの第ｉ行、第ｊ列のデータを表す。）から、ピッチ角θとロール角φを求め、ドローンの飛行速度が非常に小さく、すなわちその瞬時加速度を無視できるため、測定した加速度がｇ重力加速度に近く、測定した加速度の値ａ_ｂから、

さらに、前記ステップＳ３では、△ｔ時間内のＸ、Ｙ、Ｚ軸上の変位の計算は、式

であり、
ここで、ｖ_ｔは現在速度であり、ｖ_ｔ＝（Ｐ_ｔ－Ｐ_ｔ－１）／Δｔは三次元ベクトルであり、Ｐ_ｔはｔ時刻のｕｗｂ測位システムの位置ソリューションによる位置であり、前時刻の平均速度を現在時間の速度とし、また、

がΔｔ時間内に零に近いので、
Δｓ＝ｖ_ｔΔｔ＋ａｔΔｔであり、であり、
式中、ａはＩＭＵソリューションにより得られる正確な加速度のナビゲーション座標系での加速度であり、ｔはシステムの動作時間であり、上記式よりｔ＋Δｔ時刻の推定変位ｓ_ｔ＋Δｓを得て、
ここで、

であり、Δ_ｓ１、Δ_ｓ２、Δ_ｓ３はそれぞれサンプリング時間Δｔ内のナビゲーション座標系でのＸ、Ｙ、Ｚ軸の変位である。

さらに、前記ステップＳ３では、ドローンの変位値Ｑの計算式は、

であり、
ｘ、ｙ、ｚは前回測定した測位値であり、（ａ_＊１、ａ_＊２、ａ_＊３）は＊番目の基地局の位置値である。

さらに、前記ステップＳ４では、異常値を検出して判断する際に、式は

はそれぞれｕｗｂの現在の測定距離値と前の時刻の測定値であり、ＱはΔｔ時間内の変位値であり、ｍｅａｎはｎ個の測定データの標準偏差値であり、ｋは比重パラメータであり、具体的な環境パラメータに応じて調整することができ、式

を測定異常値とする。

さらに、前記ステップＳ５ｕｗｂでは、測定値を補正する式は、

であり、
ここで、ｓｔｄはｎ個の測定データの平均値である。

従来技術に比べて、本技術案の原理及び利点は以下のとおりである。
１．本技術案では、ドローンに備えるＩＭＵセンサと組み合わせてドローンのΔｔ時間内の変位を予測することにより、ｕｗｂで測定された距離値の変動範囲を効果的に予測し、データが異常であるか否かを正確に判別できる。
２．本技術案では、異常値データが判断され、且つドローンのΔｔ時間内の変位が推定されることにより、異常値をより正確に補正し、生データ値を効果的に回復できる。

本発明の実施例又は従来技術の技術案をより明瞭に説明するために、以下、実施例又は従来技術の説明に必要なサービスを簡単に説明するが、勿論、以下の説明における図面は、本発明のいくつかの実施例に過ぎず、当業者であれば、創造的な努力を必要とせずに、これら図面に基づいてほかの図面を得ることができる。

本発明のＩＭＵに基づくｕｗｂ測位異常値処理方法の原理のフローチャートである。

以下、特定の実施例にて本発明をさらに説明する。

図１に示すように、本実施例に記載のＩＭＵに基づくｕｗｂ測位異常値処理方法は、ステップＳ１～ステップＳ５を含む。

Ｓ１、ドローンのＩＭＵデータを取得する。

Ｓ２、加速度計とジャイロスコープを組み合わせて、ドローンのオイラー角を計算し、具体的には、
オイラー角及びジャイロスコープのドリフトを推定し、つまり、

であり、

はジャイロスコープのノイズであり、
離散化して、

（式中、Ｔ_ｓはサンプリング時間である。）を得て、
単純化して状態変数を分離し、

を得て、
単純化には、Ｘ_ｋ＝Φ_ｋ－１Ｘ_ｋ－１＋Γ_ｋ－１ｗ_ｋ－１とし、
ここで、ｋ－１時刻でのシステム遷移行列は、

であり、
ｋ－１時刻でのシステムノイズ行列は、

であり、
ｋ－１時刻での状態ベクトルは、

であり、
ｋ－１時刻でのシステムノイズは、

であり、
観測モデルは、

であり、
ここで、

であり、
Ｋ時刻での観測行列は、

になり、
プロセスモデルと観測モデルを得た後、初期化

（ここで、Ｈ_ｋ＝［Ｏ_{３×３－ｇ}Ｉ］。Ｒ^Ｔは測定ノイズ共分散行列である。）であり、
状態推定更新：
Ｘ_ｋ｜ｋ＝Ｘ_{ｋ｜ｋ－１}＋Ｋ_ｋ（Ｚ－Ｈ_ｋＸ_{ｋ｜ｋ－１}）
（ここで、Ｚは観測データである。）を行い、
誤差共分散更新：
Ｐ_ｋ｜ｋ＝（Ｉ－Ｋ_ｋＨ_ｋ）Ｐ_{ｋ｜ｋ－１}
（Ｉは単位行列である。）を行い、状態ベクトルＸ_ｋから正確なオイラー角を得る。

Ｓ３、ステップＳ２で得られたドローンのオイラー角から、Ｘ、Ｙ、Ｚ軸の加速度を計算し、具体的には、

（ここで、ｘｋ（ｉ，ｊ）はｘｋの第ｉ行、第ｊ列のデータを表す。）から、ピッチ角θとロール角φを求め、ドローンの飛行速度が非常に小さく、すなわちその瞬時加速度を無視できるため、測定した加速度がｇ重力加速度に近く、測定した加速度の値ａｂから、

△ｔ時間内のＸ、Ｙ、Ｚ軸上の変位を計算し、具体的には、式

であり、
ここで、ｖｔは現在速度であり、ｖｔ＝（Ｐｔ－Ｐｔ－１）／Δｔは三次元ベクトルであり、Ｐｔはｔ時刻のｕｗｂ測位システムの位置ソリューションによる位置であり、前時刻の平均速度を現在時間の速度とし、また、

はΔｔ時間内に零に近いので、
Δｓ＝ｖｔΔｔ＋ａｔΔｔであり、
式中、ａはＩＭＵソリューションにより得られる正確な加速度のナビゲーション座標系での加速度であり、ｔはシステムの動作時間であり、上記式よりｔ＋Δｔ時刻の推定変位ｓｔ＋Δｓを得て、
ここで、

であり、Δｓ１、Δｓ２、Δｓ３はそれぞれサンプリング時間Δｔ内のナビゲーション座標系でのＸ、Ｙ、Ｚ軸の変位である。
ドローンの変位値Ｑを以下の式により推定し、

であり、
ｘ、ｙ、ｚは前回測定した測位値であり、（ａ＊１、ａ＊２、ａ＊３）は＊番目の基地局の位置値である。

Ｓ４、ステップＳ３で得られた変位値Ｑ及びｕｗｂセンサの距離データから、ｕｗｂ測定値について異常検出を行い、ｕｗｂ測定値が異常である場合、ステップＳ５に入り、異常値を検出して判断する際に、式は

を測定異常値とする。

Ｓ５、異常なｕｗｂ測定値に対してデータ融合を行い、ｕｗｂ測定値を補正し、測定値

４つのｕｗｂ基地局がｕｗｂ測位を行う本実施例では、従来の生データの前処理過程における異常値の判別が不正確であること、及び異常値補正の精度などの問題を解決し、測定ハードウェアデータの正確性を向上させる。さらに、本実施例ハードウェア機器を追加する必要がなく、複雑な環境でデータを測定するときのｕｗｂセンサの安定性を高め、また、後続のｕｗｂ測位解に有効で正確な測定出力データを提供し、ｕｗｂ測位の安定性、効率及びリアルタイム性を高める。

以上の実施例は、本発明の好適な実施例に過ぎず、これらにより本発明の実施範囲を制限するものではなく、このため、本発明の形態、原理に基づいて行われる変化はすべて本発明の特許範囲に含まれるものとする。

Claims

ＩＭＵに基づくｕｗｂ測位異常値処理方法であって、
ドローンのＩＭＵデータを取得するステップＳ１と、
加速度計とジャイロスコープを組み合わせて、ドローンのピッチ角とロール角を計算するステップＳ２と、
ステップＳ２で得られたドローンのピッチ角とロール角から、Ｘ、Ｙ、Ｚ軸の加速度及び△ｔ時間内のＸ、Ｙ、Ｚ軸上の変位を計算し、ドローンの変位値Ｑを推定するステップＳ３と、
ステップＳ３で得られた変位値Ｑ及びｕｗｂセンサのｕｗｂ測定値を用いて、前記ｕｗｂ測定値について異常検出を行い、前記ｕｗｂ測定値が異常である場合、ステップＳ５に入るステップＳ４と、
異常な前記ｕｗｂ測定値について計算し、前記ｕｗｂ測定値を補正するステップＳ５と、を含み、
前記ステップＳ２では、具体的には、
オイラー角及びジャイロスコープのドリフトを推定し、つまり、

であり、ｂｇ，ｋはジャイロスコープのドリフト量、θ、φはそれぞれピッチ角、ロール角である。）のようにし、

のため（ωｂはドローンの角速度、Ｒは測地座標系に対するドローン座標系の回転行列である。）、第３列を抽出し、つまり、

であり、

ここで、ドリフト誤差ｂｇを減算し、次にジャイロスコープ測定ホワイトノイズｗｇを減算し、ドリフトノイズモデリングにはホワイトガウスノイズとすることができ、

はジャイロスコープのノイズであり、離散化して、

（式中、Ｔｓはサンプリング時間である。）を得て、
単純化して状態変数を分離し、

を得て、
単純化には、Ｘｋ＝Φｋ－１Ｘｋ－１＋Γｋ－１ｗｋ－１とし、
ここで、ｋ－１時刻でのシステム遷移行列は、

であり、ｋ－１時刻での状態ベクトルは、

であり、観測モデルは、

であり、ここで、

であり、Ｋ時刻での観測行列は、

であり、前記のように、単純化したプロセスモデルと観測モデルは、

になり、プロセスモデルと観測モデルを得た後、初期化

（ここで、Ｅ（Ｘ０）は零時刻での数学的期待値、Ｅ［（Ｘ０－Ｅ（Ｘ０））（Ｘ０－Ｅ（Ｘ０））Ｔ］は零時刻の誤差共分散の計算を示す。）を行い、
状態推定予測Ｘｋ｜ｋ－１＝Φｋ－１Ｘｋ－１を行い、
誤差共分散予測：
Ｐｋ｜ｋ－１＝Φｋ－１Ｐｋ－１｜ｋ－１Φｋ－１Ｔ＋Γｋ－１Ｑｋ－１Γｋ－１Ｔ（Ｑｋ－１はシステムノイズ共分散行列を示す。）を行い、
カルマンゲイン行列は、

（ここで、Ｈｋ＝［Ｏ３×３－ｇＩ］。ＲＴは測定ノイズ共分散行列である。）であり、
状態推定更新：
Ｘｋ｜ｋ＝Ｘｋ｜ｋ－１＋Ｋｋ（Ｚ－ＨｋＸｋ｜ｋ－１）
（ここで、Ｚは観測データである。）を行い、
誤差共分散更新：
Ｐｋ｜ｋ＝（Ｉ－ＫｋＨｋ）Ｐｋ｜ｋ－１
（Ｉは単位行列である。）を行い、状態ベクトルＸｋから正確なオイラー角を得る、ことを特徴とするＩＭＵに基づくｕｗｂ測位異常値処理方法。
前記ステップＳ３では、Ｘ、Ｙ、Ｚ軸の加速度の計算は、具体的には、

（ここで、ｘｋ（ｉ，ｊ）はｘｋの第ｉ行、第ｊ列のデータを表す。）から、ピッチ角θとロール角φを求め、ドローンの飛行速度が非常に小さく、すなわちその瞬時加速度を無視できるため、測定した加速度がｇ重力加速度に近く、測定した加速度の値ａｂから、

ことを特徴とする請求項１に記載のＩＭＵに基づくｕｗｂ測位異常値処理方法。
前記ステップＳ３では、△ｔ時間内のＸ、Ｙ、Ｚ軸上の変位の計算は、式

であり、ここで、ｖｔは現在速度であり、ｖｔ＝（Ｐｔ－Ｐｔ－１）／Δｔは三次元ベクトルであり、Ｐｔはｔ時刻のｕｗｂ測位システムの位置ソリューションによる位置であり、前時刻の平均速度を現在時間の速度とし、また、

はΔｔ時間内に零に近いので、
Δｓ＝ｖｔΔｔ＋ａｔΔｔであり、
式中、ａはＩＭＵソリューションにより得られる正確な加速度のナビゲーション座標系での加速度であり、ｔはシステムの動作時間であり、上記式よりｔ＋Δｔ時刻の推定変位ｓｔ＋Δｓを得て、
ここで、

であり、Δｓ１、Δｓ２、Δｓ３はそれぞれサンプリング時間Δｔ内のナビゲーション座標系でのＸ、Ｙ、Ｚ軸の変位である、ことを特徴とする請求項２に記載のＩＭＵに基づくｕｗｂ測位異常値処理方法。
前記ステップＳ３では、ドローンの変位値Ｑの計算式は、

であり、
ｘ、ｙ、ｚは前回測定した測位値であり、（ａ＊１、ａ＊２、ａ＊３）は＊番目の基地局の位置値である、ことを特徴とする請求項３に記載のＩＭＵに基づくｕｗｂ測位異常値処理方法。
前記ステップＳ４では、異常値を検出して判断する際に、式は

はそれぞれ現在の前記ｕｗｂ測定値と前の時刻の前記ｕｗｂ測定値であり、ＱはΔｔ時間内の変位値であり、ｍｅａｎはｎ個の前記ｕｗｂ測定値の標準偏差値であり、ｋは比重パラメータであり、具体的な環境パラメータに応じて調整することができ、式

を測定異常値とする、ことを特徴とする請求項４に記載のＩＭＵに基づくｕｗｂ測位異常値処理方法。
前記ステップＳ５では、前記ｕｗｂ測定値を補正する式は、

であり、
ここで、ｓｔｄはｎ個の前記ｕｗｂ測定値の平均値である、ことを特徴とする請求項５に記載のＩＭＵに基づくｕｗｂ測位異常値処理方法。