JP7576501B2 - 慣性航法装置の誤差補正システム及び慣性航法装置の誤差補正方法 - Google Patents

Description

本発明は、飛行体に搭載された慣性航法装置の誤差補正システム及び慣性航法装置の誤差補正方法に関する。

無人航空機において自ら制御して自律的に飛行する方法としては、ＧＰＳ受信機を搭載し、複数のＧＰＳ衛星から受信したＧＰＳ信号に基づいて自らの位置を特定するなどして飛行する方法が知られている。
しかし、ＧＰＳによる方法ではジャミングや周りの建築物等の影響を受けやすく、それらの影響を受けない、あるいは受けにくい方法として、慣性航法装置を用いる方法が知られている（特許文献１、２等参照）。

一般的に、慣性航法装置では、加速度センサや角速度センサが測定した加速度や角速度を積分することで自身の位置（緯度、経度、高度）や姿勢（ピッチ、ロール、ヨー）を算出する。
そして、無人航空機の制御部が、算出された位置や姿勢に基づいて自動操縦を行うように構成される。

特開２０２０－１４７１１１号公報 特開２０１９－１１５０１２号公報

しかしながら、加速度センサや角速度センサの測定値には、ランダムに発生するランダム誤差のほかにバイアス誤差、すなわち回転や加速がない場合にセンサが生成する信号（誤差）も含まれており、このバイアス誤差が慣性航法装置による位置や姿勢の算出の主要な誤差要因の１つになる。
そして、これは、無人航空機の場合だけでなく、有人の飛行体に搭載された慣性航法装置においても同様に問題になり得る。

本発明は、上記の点を鑑みてなされたものであり、飛行体に搭載された慣性航法装置で生じるバイアス誤差等の誤差を適切に補正することが可能な慣性航法装置の誤差補正システム及び慣性航法装置の誤差補正方法を提供することを目的とする。

前記の問題を解決するために、請求項１に記載の発明は、
搭載した慣性航法装置が算出する位置と姿勢に基づいて自律的に飛行する複数の飛行体における慣性航法装置の誤差補正システムであって、
前記各飛行体はそれぞれ、前記慣性航法装置と、無線通信装置と、自機から見た他機の座標を算出するための座標算出装置と、誤差補正装置と、を搭載しており、
前記誤差補正装置は、
前記座標算出装置が算出した前記自機から見た他機の座標に基づいて自機データに基づく当該他機の方位角及び仰角を算出し、
前記無線通信装置が他機から受信した、当該他機の前記慣性航法装置が算出した当該他機の位置の情報と、自機の前記慣性航法装置が算出した自機の位置の情報に基づいて他機データに基づく当該他機の方位角及び仰角を算出し、
前記自機データに基づく当該他機の方位角及び仰角と、前記他機データに基づく当該他機の方位角及び仰角との差である測角残差に基づいて自機の前記慣性航法装置で生じる誤差を補正することを特徴とする。

請求項２に記載の発明は、請求項１に記載の慣性航法装置の誤差補正システムにおいて、前記誤差補正装置は、前記座標算出装置が算出した前記自機から見た他機の座標と、自機の前記慣性航法装置が算出した自機の前記姿勢の情報に基づいて前記自機データに基づく他機の方位角及び仰角を算出することを特徴とする。

請求項３に記載の発明は、請求項１又は請求項２に記載の慣性航法装置の誤差補正システムにおいて、前記誤差補正装置は、前記自機の前記慣性航法装置が算出した自機の位置から前記他機の慣性航法装置が算出した当該他機の位置に向かうベクトルの方位角及び仰角を前記他機データに基づく当該他機の方位角及び仰角として算出することを特徴とする。

請求項４に記載の発明は、請求項１から請求項３のいずれか一項に記載の慣性航法装置の誤差補正システムにおいて、前記誤差補正装置は、ある他機から受信した、当該他機の前記慣性航法装置が算出した当該他機の位置の情報が信用できないと判断した場合は、当該他機を含む他機に対して、当該他機の位置の情報が信用できない旨を表す信号を発信することを特徴とする。

請求項５に記載の発明は、請求項１から請求項４のいずれか一項に記載の慣性航法装置の誤差補正システムにおいて、前記誤差補正装置は、自機からの距離が遠い他機ほど重みが大きくなるように前記測角残差を重み付けして、前記測角残差に基づいて自機の前記慣性航法装置で生じる誤差を補正することを特徴とする。

請求項６に記載の発明は、請求項１から請求項５のいずれか一項に記載の慣性航法装置の誤差補正システムにおいて、前記誤差補正装置は、ある他機について算出した前記測角残差を立体角表現した場合の立体角が閾値以下である場合、当該他機の識別情報を含む精度確定信号を発信することを特徴とする。

請求項７に記載の発明は、
搭載した慣性航法装置が算出する位置と姿勢に基づいて自律的に飛行する複数の飛行体における慣性航法装置の誤差補正方法であって、
前記飛行体が自機から見た他機の座標を算出する座標算出工程と、
前記座標算出工程で算出した前記自機から見た他機の座標に基づいて自機データに基づく当該他機の方位角及び仰角を算出する自機データに基づく他機の方位角等算出工程と、
無線通信装置を介して当該他機から受信した、当該他機の前記慣性航法装置が算出した当該他機の位置の情報と、自機の前記慣性航法装置が算出した自機の位置の情報に基づいて他機データに基づく当該他機の方位角及び仰角を算出する他機データに基づく他機の方位角等算出工程と、
前記自機データの基づく当該他機の方位角及び仰角と、前記他機データに基づく当該他機の方位角及び仰角との差である測角残差に基づいて自機の前記慣性航法装置で生じる誤差を補正する誤差補正工程と、
を含むことを特徴とする。

本発明によれば、飛行体に搭載された慣性航法装置で生じるバイアス誤差等の誤差を適切に補正することが可能となる。

本実施形態に係る慣性航法装置の誤差補正システムが自律的に飛行する複数の飛行体で構成されることを表す図である。 各飛行体における慣性航法装置の誤差補正システムに関する構成を表すブロック図である。 画像中の他機の画像座標を座標変換して自機から見た当該他機の座標を算出することを説明する図である。 本実施形態に係る慣性航法装置の誤差補正方法の各工程を表すフローチャートである。 自機から見た他機の座標と自機の姿勢に基づいて自機データに基づく他機の方位角及び仰角を算出することを説明する図である。 他機の慣性航法装置が算出した当該他機の位置と自機の慣性航法装置が算出した自機の位置に基づいて他機データに基づく当該他機の方位角及び仰角を算出することを説明する図である。 自機と他機との距離が遠いほど位置の誤差に対応する、自機から見た他機の方位角や仰角の誤差が小さくなることを説明する図である。

以下、本発明に係る慣性航法装置の誤差補正システム及び慣性航法装置の誤差補正方法の実施の形態について、図面を参照して説明する。

［慣性航法装置の誤差補正システム］
本実施形態に係る慣性航法装置の誤差補正システム１では、図１に示すように、後述する慣性航法装置や無線通信装置等を搭載して自律的に飛行する複数の飛行体２が、互いに無線通信するなどしながら、自らに搭載された慣性航法装置で生じる誤差をそれぞれ補正していくものである。
その際、飛行体２は無人航空機であってもよく、有人機であってもよい。

また、複数の飛行体２は、例えば、互いに協同して行動する僚機同士であってもよく、あるいは、例えば、偶然に近くを飛行する状態になった無関係の機体同士であってもよい。
また、複数の飛行体２は、自機２ａと他機２ｂの２機のみであってもよく、あるいは、３機以上であってもよい。

以下、本実施形態に係る慣性航法装置の誤差補正システム１について具体的に説明する。
図２は、各飛行体における慣性航法装置の誤差補正システムに関する構成を表すブロック図である。

［飛行体ごとの構成］
各飛行体２はそれぞれ、慣性航法装置１０と、無線通信装置１１と、座標算出装置１２と、誤差補正装置１３と、を搭載しており、搭載した慣性航法装置１０が算出する位置（緯度、経度、高度）と姿勢（ピッチ、ロール、ヨー）に基づいて自律的に飛行するように構成されている。
また、本実施形態では、各飛行体２にはランプ１４が取り付けられており、ランプ１４を所定の明滅パターンで明滅させることで、ランプ１４の明滅パターンにより自身のＩＤ（識別情報）を他の飛行体２（以下、他機という。）に通知するようになっている。

慣性航法装置１０は、加速度センサ１０Ａと角速度センサ１０Ｂとを備えており、加速度センサ１０Ａや角速度センサ１０Ｂが出力した加速度や角速度を積分することで飛行体２の位置と姿勢を算出し、算出した位置や姿勢の情報を飛行体２の制御部２０に送信する。
また、慣性航法装置１０は、算出した位置や姿勢の情報のうち、位置（緯度、経度、高度）の情報に、自身が搭載されている飛行体２のＩＤを付与して、無線通信装置１１を介して全ての他機に発信するようになっている。

ここで、全ての他機とは、無線通信装置１１を介して自機と通信可能な飛行体２をいう。
また、無線通信装置１１は、他機から発信された、当該他機の慣性航法装置１０が算出し、当該他機のＩＤが付与された当該他機の位置（緯度、経度、高度）の情報を受信すると、当該他機のＩＤが付与された当該他機の位置の情報を座標算出装置１２と誤差補正装置１３に送信するようになっている。

［自機から見た他機の座標の算出］
座標算出装置１２は、自機（すなわち当該座標算出装置１２が搭載されている飛行体２）から見た他機の座標（ａｚ，ｅｌ）を算出するための装置である。
以下、本実施形態での座標算出装置１２における自機から見た他機の座標（ａｚ，ｅｌ）の算出の仕方について説明する。

座標算出装置１２は、カメラ１２Ａを備えている。ここでは、カメラ１２Ａは任意の方向あるいは所定の範囲内で向きを変えることができるように構成されているものとする。また、カメラ１２Ａとして全方位カメラ等を用いることも可能である。
また、本実施形態では、カメラ１２Ａで動画撮影を行う場合について説明するが、静止画を撮影するように構成してもよい。なお、カメラ１２Ａで静止画を撮影する場合、カメラ１２Ａで撮影した画像に撮影されている他機のＩＤを取得するための機構が新たに設けられる。

座標算出装置１２は、カメラ１２Ａで撮影した動画を解析して、画像（すなわち動画を構成する１コマずつの各画像）中に撮影されている他機のランプ１４の明滅パターンを割り出し、画像中に撮影されている当該他機のＩＤを割り出す。
そして、無線通信装置１１が動画中に撮影されている他機から発信された位置の情報（すなわち割り出した当該ＩＤが付与された位置の情報）を受信して座標算出装置１２に送信すると、座標算出装置１２は、図３に示すように、そのタイミングで撮影された画像（動画を構成する１コマの画像）中の当該他機２ｂの画像座標（ｕ，ｖ）を割り出す。

そして、座標算出装置１２は、上記のタイミングでのカメラ１２Ａの向きと当該他機２ｂの画像座標（ｕ，ｖ）に基づいて、自機２ａから見た当該他機２ｂの座標（ａｚ，ｅｌ）を算出する。
すなわち、座標算出装置１２は、図３に示すように、画像中の当該他機２ｂの画像座標（ｕ，ｖ）を座標変換して、自機２ａの前方（ＦＷＤ）、右方（ＲＨ）、下方（ＤＷＮ）を座標軸とする直交座標系における当該他機２ｂの前方（ＦＷＤ）軸に対する方位角（ａｚ）と仰角（ｅｌ）を算出するようになっている。

なお、上記では、座標算出装置１２がカメラ１２Ａで撮影された画像を解析して自機２ａから見た他機２ｂの座標（ａｚ，ｅｌ）を算出する場合について説明するが、本発明はこの場合に限定されず、例えばレーダー等で得られた情報を用いて自機２ａから見た他機２ｂの座標（ａｚ，ｅｌ）を算出するように構成することも可能である。
座標算出装置１２は、以上の処理を全ての他機２ｂについて行い、自機２ａから見た当該他機２ｂの座標（ａｚ，ｅｌ）を算出すると、算出した自機２ａから見た当該他機２ｂの座標（ａｚ，ｅｌ）に割り出した当該他機２ｂのＩＤを付与してそれぞれ誤差補正装置１３に送信する。

［慣性航法装置で生じる誤差の補正］
次に、本実施形態での誤差補正装置１３における自機２ａの慣性航法装置１０で生じる誤差の補正の仕方について説明する。なお、以下で説明する誤差補正装置１３の処理の一部を座標算出装置１２等で行うように構成することも可能である。また、座標算出装置１２や誤差補正装置１３等を１つの装置として構成することも可能である。
また、本実施形態に係る慣性航法装置の誤差補正方法についてもあわせて説明する。

慣性航法装置の誤差補正方法には、図４に示す各工程が含まれており、その流れに従って自機２ａの慣性航法装置１０で生じる誤差の補正の仕方について説明する。
座標算出装置１２で自機２ａから見た他機２ｂの座標（ａｚ，ｅｌ）を算出する座標算出工程（ステップＳ１）については上記の通りである。

［自機データに基づく他機の方位角及び仰角の算出］
誤差補正装置１３は、座標算出装置１２が算出した自機２ａから見た他機２ｂの座標（ａｚ，ｅｌ）に基づいて自機データに基づく当該他機２ｂの方位角及び仰角を算出する（自機データに基づく他機の方位角等算出工程（ステップＳ２））。
この工程では、誤差補正装置１３は、座標算出装置１２から算出した自機２ａから見た他機２ｂの座標（ａｚ，ｅｌ）の情報が送信されてくると、画像中に撮影されている他機２ｂから位置の情報を受信したタイミングで自機２ａの慣性航法装置１０が算出した自機２ａの姿勢（ピッチ、ロール、ヨー）の情報を入手する。

そして、誤差補正装置１３は、図５に示すように、座標算出装置１２が算出した自機２ａから見た他機２ｂの座標（ａｚ，ｅｌ）と、自機２ａの慣性航法装置１０が算出した自機２ａの姿勢（ピッチｐ、ロールｒ、ヨーｙ）の情報に基づいて、当該他機２ｂの方位角ＡＺ１（真北基準。真北は図５におけるＮ軸参照）と仰角ＥＬ１（水平面基準）を算出するようになっている。
しかし、この場合の「他機２ｂの方位角ＡＺ１や仰角ＥＬ１」は、あくまで自機２ａの慣性航法装置１０が算出した自機２ａの姿勢（ピッチｐ、ロールｒ、ヨーｙ）に基づいて算出されたものであり、誤差が含まれている可能性があるため、「自機データに基づく他機の方位角ＡＺ１及び仰角ＥＬ１」という。

［他機データに基づく当該他機の方位角及び仰角の算出］
一方、誤差補正装置１３は、無線通信装置１１が当該他機２ｂから受信した、当該他機２ｂの慣性航法装置１０が算出した当該他機２ｂの位置（緯度、経度、高度）の情報と、無線通信装置１１が当該他機２ｂの位置の情報を受信したタイミングで自機２ａの慣性航法装置１０が算出した自機２ａの位置（緯度、経度、高度）の情報に基づいて、他機データに基づく当該他機２ｂの方位角及び仰角を算出する（他機データに基づく他機の方位角等算出工程（ステップＳ３））。

この工程では、誤差補正装置１３は、図６に示すように、自機２ａの慣性航法装置１０が算出した自機２ａの位置（緯度ＬＡＴａ、経度ＬＯＮａ、高度ＡＬＴａ）から当該他機２ｂの慣性航法装置１０が算出した当該他機２ｂの位置（緯度ＬＡＴｂ、経度ＬＯＮｂ、高度ＡＬＴｂ）に向かうベクトルＶの方位角ＡＺ２（真北基準）と仰角ＥＬ２（水平面基準）を当該他機２ｂの方位角ＡＺ２及び仰角ＥＬ２として算出するようになっている。
この場合の「他機２ｂの方位角ＡＺ２や仰角ＥＬ２」は、あくまで他機２ｂの慣性航法装置１０が算出した、誤差が含まれている可能性がある他機２ｂの位置（緯度、経度、高度）に基づいて算出されたものであるため、上記の「自機データに基づく他機の方位角ＡＺ１及び仰角ＥＬ１」と区別して「他機データに基づく他機の方位角ＡＺ２及び仰角ＥＬ２」という。

なお、ステップＳ２（自機データに基づく他機の方位角及び仰角の算出）とステップＳ３（他機データに基づく他機の方位角及び仰角の算出）は処理の順番が逆でもよく、あるいは併行して行われてもよい。

［他機データに基づく当該他機の方位角及び仰角の算出］
続いて、誤差補正装置１３は、ステップＳ２で算出した自機データに基づく他機２ｂの方位角ＡＺ１及び仰角ＥＬ１とステップＳ３で算出した他機データに基づく当該他機２ｂの方位角ＡＺ２及び仰角ＥＬ２との差である測角残差（ΔＡＺ，ΔＥＬ）を算出し、算出した測角残差（ΔＡＺ，ΔＥＬ）に基づいて自機２ａの慣性航法装置１０で生じる誤差（本実施形態では慣性航法装置１０の加速度センサ１０Ａと角速度センサ１０Ｂの各バイアス誤差）を補正する（誤差補正工程（ステップＳ４））。

具体的には、誤差補正装置１３は、
ΔＡＺ＝ＡＺ１－ＡＺ２ …（１）
ΔＥＬ＝ＥＬ１－ＥＬ２ …（２）
を計算して測角残差（ΔＡＺ，ΔＥＬ）を算出し、算出した測角残差（ΔＡＺ，ΔＥＬ）に基づいて慣性航法装置１０の加速度センサ１０Ａと角速度センサ１０Ｂの各センサバイアス値をそれぞれ算出する。

そして、本実施形態では、誤差補正装置１３は、慣性航法装置１０の加速度センサ１０Ａが出力する加速度に算出した加速度センサ１０Ａのセンサバイアス値を加算し、角速度センサ１０Ｂが出力する角速度に算出した角速度センサ１０Ｂのセンサバイアス値を加算することで、慣性航法装置１０の加速度センサ１０Ａと角速度センサ１０Ｂの各バイアス誤差をそれぞれ補正する。
そして、慣性航法装置１０は、補正された加速度や角速度を積分することで自機２ａの位置（緯度、経度、高度）や姿勢（ピッチ、ロール、ヨー）を算出する。

本実施形態に係る慣性航法装置の誤差補正システム１や慣性航法装置の誤差補正方法では、以上のステップＳ１～Ｓ４の各工程を繰り返し行い、さらに、以上の処理を全ての他機２ｂについて行って、慣性航法装置１０の加速度センサ１０Ａや角速度センサ１０Ｂが出力する加速度や角速度を修正することで、慣性航法装置１０で生じるバイアス誤差等の誤差を補正するようになっている。

［効果］
以上のように、本実施形態に係る慣性航法装置の誤差補正システム１や慣性航法装置の誤差補正方法によれば、誤差補正装置１３が、座標算出装置１２が算出した自機２ａから見た他機２ｂの座標（ａｚ，ｅｌ）に基づいて算出した自機データに基づく当該他機２ｂの方位角ＡＺ１及び仰角ＥＬ１と、自機２ａと当該他機２ｂの各慣性航法装置１０がそれぞれ算出した自機２ａと当該他機２ｂの各位置に基づいて算出した他機データに基づく当該他機２ｂの方位角ＡＺ２及び仰角ＥＬ２との差である測角残差（ΔＡＺ，ΔＥＬ）に基づいて自機２ａの慣性航法装置１０で生じる誤差を補正するように構成した。

そのため、飛行体２に搭載された慣性航法装置１０で発生する誤差（慣性航法装置１０の加速度センサ１０Ａや角速度センサ１０Ｂで発生するバイアス誤差等）を適切に補正することが可能となる。
そのため、ＧＰＳによる方法ではジャミングや周りの建築物等の影響を受けやすく自機２ａの位置（緯度、経度、高度）を特定しにくい状況、またはＧＰＳ信号を受信できない状況においても、慣性航法装置１０を用いて自機２ａの位置（緯度、経度、高度）や姿勢（ピッチ、ロール、ヨー）を算出して自律的に飛行することが可能となる。

［誤差補正の精度を向上させるための処理等について］
なお、上記の実施形態では、全ての他機２ｂ（すなわち無線通信装置１１を介して自機２ａと通信可能な飛行体２）について算出した全ての測角残差（ΔＡＺ，ΔＥＬ）を平等に扱って（すなわち例えばそれらの平均値を算出するなどして）自機２ａの慣性航法装置１０で生じる誤差（慣性航法装置１０の加速度センサ１０Ａや角速度センサ１０Ｂで発生するバイアス誤差等）を補正する場合について説明した。

しかし、他機２ｂの慣性航法装置１０が故障していたり、あるいは妨害等の目的で他機２ｂ（あるいはそれになりすました他者）が故意に誤った情報を発信するなどして誤差補正の精度が低下してしまう場合があり得る。
そこで、以下では、誤差補正の精度を向上させるために上記のフローに追加し得るいくつかの処理等について説明する。

［疑義信号や使用判定について］
前述したように、各飛行体２（自機２ａ、他機２ｂ）から、自らに搭載された慣性航法装置１０が算出した当該自らの位置（緯度、経度、高度）の情報）が発信されるが、例えば、以下のような場合、当該他機２ｂｉから発信される位置の情報は信用できないと考えられる。

（Ａ）ある他機２ｂｉについて繰り返し算出される測角残差（ΔＡＺｉ，ΔＥＬｉ）の変動（標準偏差σｉ）は小さいが、測角残差（ΔＡＺｉ，ΔＥＬｉ）自体の大きさが大きい場合。
具体的には、例えば、当該他機２ｂｉについて繰り返し算出される測角残差（ΔＡＺｉ，ΔＥＬｉ）の標準偏差σｉは所定の閾値未満でありばらつきが少ないが、ΔＡＺｉ、ΔＥＬｉの絶対値の一方又は両方が所定の閾値以上に大きい場合。

すなわち、当該他機２ｂｉについて算出される測角残差（ΔＡＺｉ，ΔＥＬｉ）は毎回同じような値になるが、測角残差（ΔＡＺｉ，ΔＥＬｉ）の大きさが大きい状態が継続する場合である。
これは、当該他機２ｂｉの慣性航法装置１０が故障しているか、あるいは当該他機２ｂｉ（又は他者）が故意に誤った情報を発信していると考えられる。

（Ｂ）ある他機２ｂｉについて繰り返し算出される測角残差（ΔＡＺｉ，ΔＥＬｉ）の変動（標準偏差σｉ）が大きい場合。すなわち、当該他機２ｂｉについて毎回算出される測角残差（ΔＡＺｉ，ΔＥＬｉ）が所定の閾値以上でありばらつきが大きい場合である。
これは、当該他機２ｂｉの慣性航法装置１０が故障していると考えられる。

そこで、本実施形態では、上記のような場合、すなわち、自機２ａの誤差補正装置１３は、ある他機２ｂｉから受信した、当該他機２ｂｉの慣性航法装置１０が算出した当該他機２ｂの位置（緯度、経度、高度）の情報に基づいて算出した測角残差（ΔＡＺｉ，ΔＥＬｉ）が例えば上記の（Ａ）や（Ｂ）の条件を満たすなどして信用できないと判断した場合は、当該他機２ｂｉを含む他機２ｂに対して、当該他機２ｂｉの位置の情報が信用できない旨を表す信号を発信するように構成されている。

以下、この信号を疑義信号という。
疑義信号には、例えば、発信する情報が信用できないと判断した対象の他機２ｂｉのＩＤと、当該疑義信号を発信する自機２ａのＩＤと、疑義の種類（たとえば上記の（Ａ）や（Ｂ）など）が含まれる。

そして、ある他機２ｂｉについて自機２ａや他の他機２ｂが疑義信号を発信した場合、自機２ａの誤差補正装置１３は、当該他機２ｂｉについて算出した測角残差（ΔＡＺｉ，ΔＥＬｉ）を自機２ａの慣性航法装置１０で生じる誤差の補正には使用しないように使用判定を行うように構成することができる。
このように構成すれば、他機２ｂの故障した慣性航法装置１０が算出した当該他機２ｂの位置の情報等や、妨害等の目的で故意に誤った情報を発信している他機２ｂの位置の情報等を、自機２ａの慣性航法装置１０で生じる誤差の補正に用いないようにすることが可能となり、誤差補正の精度を向上させることが可能となる。

一方、他機２ｂが自身以外の多くの他機（自機２ａを含む。）に対して疑義信号を発信するような場合は、当該他機２ｂの慣性航法装置１０自体が故障している可能性がある。
あるいは、当該他機２ｂ（又は他者）がシステムを撹乱する等の目的をもって故意に疑義信号を多発している可能性もある。

そのため、ある他機２ｂが、例えば通信可能な全ての飛行体２のうち所定割合（例えば半分）以上の他の他機２ｂ（自機２ａを含む。）に対して疑義信号を発信している場合、自機２ａの誤差補正装置１３は、当該他機２ｂが発信する疑義信号を無視するように構成することが可能である。
この場合、自機２ａの誤差補正装置１３は、疑義信号を多発する当該他機２ｂ以外の他機２ｂについて算出した測角残差（ΔＡＺ，ΔＥＬ）を自機２ａの慣性航法装置１０で生じる誤差の補正に使用するように使用判定を行うように構成することが可能である。

なお、この場合、疑義信号を多発する当該他機２ｂ以外の他機２ｂ（すなわち慣性航法装置１０が故障しておらずシステムを撹乱する等の目的を有していないと思われる他機２ｂ）が発信する疑義信号の対象の他機２ｂについては、自機２ａの誤差補正装置１３は、その他機２ｂについて算出した測角残差（ΔＡＺ，ΔＥＬ）は自機２ａの慣性航法装置１０で生じる誤差の補正には使用しないように使用判定を行う。
また、ある他機２ｂについて算出した測角残差（ΔＡＺ，ΔＥＬ）を自機２ａの慣性航法装置１０で生じる誤差の補正に使用するか否かの使用判定に、他の基準を加味するように構成することも可能である。

［重み付けについて］
また、全ての他機２ｂ（すなわち無線通信装置１１を介して自機２ａと通信可能な飛行体２）について算出した各測角残差（ΔＡＺ，ΔＥＬ）をそれぞれ重み付けして、自機２ａの慣性航法装置１０で生じる誤差の補正に用いるように構成することも可能である。
以下、重み付けを行う際に考慮し得るいくつかの要素について説明する。

［自機と他機との距離について］
図７に示すように、例えば２機の他機２ｂの慣性航法装置１０が算出する位置（緯度、経度、高度）の誤差Δ（図７ではイメージ的に上下方向の矢印で示されている。）が同程度である場合、自機２ａと他機２ｂとの距離Ｌが遠いほど、位置の誤差Δに対応する、自機２ａから見た他機２ｂの方位角や仰角の誤差δは小さくなる。
すなわち、自機２ａから他機２ｂまでの距離Ｌが近い場合に比べて、自機２ａから他機２ｂまでの距離Ｌが遠い方が、当該他機２ｂの慣性航法装置１０が算出する位置に多少大きな誤差Δがあっても、自機２ａから見た当該他機２ｂの方位角や仰角に与える影響が小さいため、距離Ｌが遠い他機２ｂについて算出した測角残差（ΔＡＺ，ΔＥＬ）の方が、距離Ｌが近い他機２ｂについて算出した測角残差（ΔＡＺ，ΔＥＬ）よりも精度が高い。

そのため、自機２ａの誤差補正装置１３は、自機２ａからの距離Ｌｊが遠い他機２ｂｊほど重みが大きくなるように測角残差（ΔＡＺｊ，ΔＥＬｊ）を重み付けして、上記のように測角残差に基づいて自機２ａの慣性航法装置１０で生じる誤差を補正するように構成することが可能である。
この場合の重みｗｊ、すなわち自機と他機との距離Ｌｊに関する重みｗｊ（Ｌｊ）は、例えば、
ｗｊ（Ｌｊ）＝exp(－α／Ｌｊ) …（３）
の形とすることができる（αは定数）。

［有効測角目標数について］
また、本実施形態では、自機２ａの誤差補正装置１３がある他機２ｂについて算出した測角残差（ΔＡＺ，ΔＥＬ）を立体角表現した場合の立体角Ωが設定された閾値以下であり小さい場合、少なくとも当該他機２ｂに対しては自機２ａの慣性航法装置１０が算出する位置や姿勢の精度が確定したとして、当該他機２ｂのＩＤを含む精度確定信号を発信するようになっている。
精度確定信号には、精度が確定した対象としての当該他機２ｂのＩＤのほか、少なくとも当該精度確定信号を発信した自機２ａのＩＤも含まれる。なお、上記の立体角Ωについて設定される閾値を可変とすることも可能であり、例えば、閾値を、自機２ａが当該他機２ｂを初めて認識した時点では大きな値に設定し、誤差の補正処理を繰り返すごとに徐々に小さくしていくように構成することも可能である。

このように構成する場合、ある他機２ｂｊのＩＤを含む精度確定信号の数Ｎｊ（以下、有効測角目標数Ｎｊという。）が多いほど（すなわちある他機２ｂｊを精度が確定した対象として挙げる飛行体２の数Ｎｊが多いほど）、当該他機２ｂｊは多くの飛行体２と位置や姿勢の精度が確定していると考えられる。
そして、当該他機２ｂｊについて算出した測角残差（ΔＡＺｊ，ΔＥＬｊ）を自機２ａの慣性航法装置１０で生じる誤差の補正に用いる際の重み付けを重くするように構成すれば、誤差補正の精度を向上させることができる。

そのため、自機２ａの誤差補正装置１３は、ある他機２ｂｊのＩＤを含む精度確定信号の数Ｎｊ（すなわち有効測角目標数Ｎｊ）が多いほど当該他機２ｂｊについて算出した測角残差（ΔＡＺｊ，ΔＥＬｊ）の重みｗｊが大きくなるように重み付けして、測角残差に基づいて自機２ａの慣性航法装置１０で生じる誤差を補正するように構成することが可能である。
この場合の重みｗｊ、すなわち有効測角目標数Ｎｊに関する重みｗｊ（Ｎｊ）は、例えば、
ｗｊ（Ｎｊ）＝exp(－β／Ｎｊ) …（４）
の形とすることができる（βは定数）。

［測角残差の変動の安定性について］
また、上記の疑義信号の所で説明したように、ある他機２ｂｉについて算出される測角残差（ΔＡＺｉ，ΔＥＬｉ）の変動（標準偏差σｉ）は小さいが、測角残差（ΔＡＺｉ，ΔＥＬｉ）自体の大きさが大きい場合（上記（Ａ）参照）は、当該他機２ｂｉの慣性航法装置１０の故障や当該他機２ｂｉ（又は他者）が故意に誤った情報を発信している可能性がある。
しかし、上記のように、ある他機２ｂｊについて算出した測角残差（ΔＡＺｊ，ΔＥＬｊ）を立体角表現した場合の立体角Ωｊが閾値以下であり小さく、発信された精度確定信号の対象となっているような他機２ｂｊについては、当該他機２ｂｊの慣性航法装置１０の故障や当該他機２ｂｊ（又は他者）が故意に誤った情報を発信している可能性はなく、あるいはその可能性は非常に小さい。

そして、そのような場合には、当該他機２ｂｊについて算出される測角残差（ΔＡＺｊ，ΔＥＬｊ）の変動が小さい方が（すなわち測角残差の変更が安定している方が）、変動が大きい場合よりも測角残差の信頼性が高く、そのような測角残差を自機２ａの慣性航法装置１０で生じる誤差の補正に用いる際の重み付けを重くするように構成すれば、誤差補正の精度を向上させることができる。

そのため、自機２ａの誤差補正装置１３は、発信された精度確定信号の対象となっている他機２ｂｊについて算出した測角残差（ΔＡＺｊ，ΔＥＬｊ）の変動（標準偏差σｊ）が小さいほど当該他機２ｂｊについて算出した測角残差（ΔＡＺｉ，ΔＥＬｉ）の重みｗｊが大きくなるように重み付けして、測角残差に基づいて自機２ａの慣性航法装置１０で生じる誤差を補正するように構成することが可能である。
この場合の重みｗｊ、すなわち測角残差の標準偏差σｊに関する重みｗｊ（σｊ）は、例えば、
ｗｊ（σｊ）＝exp(－γ・σｊ) …（５）
の形とすることができる（γは定数）。

そして、本実施形態では、上記の式（３）～（５）を考慮して、自機２ａの誤差補正装置１３は、ある他機２ｂｊについて算出した測角残差（ΔＡＺｊ，ΔＥＬｊ）の重みｗｊを以下の式（６）に従って算出するようになっている。

本実施形態では、このように、他機２ｂについて算出した測角残差（ΔＡＺｊ，ΔＥＬｊ）のうち、信頼性が低い測角残差（ΔＡＺｊ，ΔＥＬｊ）は重みｗｊを小さくし、信頼性が高い測角残差（ΔＡＺｊ，ΔＥＬｊ）ほど重みｗｊを大きくするように重み付けして測角残差に基づいて自機２ａの慣性航法装置１０で生じる誤差を補正するように構成することで、互いに精度確定信号の対象となる他機２ｂを増やしていくことが可能となり、慣性航法装置１０を搭載して自律的に飛行する複数の飛行体２（自機２ａを含む。）が全体的に慣性航法装置１０で発生する誤差を小さくしていくことが可能となる。

なお、本発明が上記の実施形態等に限定されず、本発明の趣旨を逸脱しない限り、適宜変更可能であることは言うまでもない。

１ 慣性航法装置の誤差補正システム
２ 飛行体
２ａ 自機
２ｂ 他機
１０ 慣性航法装置
１１ 無線通信装置
１２ 座標算出装置
１３ 誤差補正装置
ＡＬＴ 高度（位置）
（ａｚ，ｅｌ） 自機から見た他機の座標
ＡＺ１、ＥＬ１ 自機データに基づく他機の方位角及び仰角
ＡＺ２、ＥＬ２ 他機データに基づく他機の方位角及び仰角
ＬＡＴ 緯度（位置）
Ｌｊ 自機からの距離
ＬＯＮ 経度（位置）
Ｎｊ 有効測角目標数（精度確定信号の数）
ｐ ピッチ（姿勢）
ｒ ロール（姿勢）
Ｖ ベクトル
ｗｊ 重み
ｙ ヨー（姿勢）
（ΔＡＺ，ΔＥＬ） 測角残差
σｊ 測角残差の標準偏差（測角残差の変動）
Ω 立体角

Claims (7)

1. 搭載した慣性航法装置が算出する位置と姿勢に基づいて自律的に飛行する複数の飛行体における慣性航法装置の誤差補正システムであって、
   前記各飛行体はそれぞれ、前記慣性航法装置と、無線通信装置と、自機から見た他機の座標を算出するための座標算出装置と、誤差補正装置と、を搭載しており、
   前記誤差補正装置は、
   前記座標算出装置が算出した前記自機から見た他機の座標に基づいて自機データに基づく当該他機の方位角及び仰角を算出し、
   前記無線通信装置が他機から受信した、当該他機の前記慣性航法装置が算出した当該他機の位置の情報と、自機の前記慣性航法装置が算出した自機の位置の情報に基づいて他機データに基づく当該他機の方位角及び仰角を算出し、
   前記自機データに基づく当該他機の方位角及び仰角と、前記他機データに基づく当該他機の方位角及び仰角との差である測角残差に基づいて自機の前記慣性航法装置で生じる誤差を補正することを特徴とする慣性航法装置の誤差補正システム。
2. 前記誤差補正装置は、前記座標算出装置が算出した前記自機から見た他機の座標と、自機の前記慣性航法装置が算出した自機の前記姿勢の情報に基づいて前記自機データに基づく他機の方位角及び仰角を算出することを特徴とする請求項１に記載の慣性航法装置の誤差補正システム。
3. 前記誤差補正装置は、前記自機の前記慣性航法装置が算出した自機の位置から前記他機の慣性航法装置が算出した当該他機の位置に向かうベクトルの方位角及び仰角を前記他機データに基づく当該他機の方位角及び仰角として算出することを特徴とする請求項１又は請求項２に記載の慣性航法装置の誤差補正システム。
4. 前記誤差補正装置は、ある他機から受信した、当該他機の前記慣性航法装置が算出した当該他機の位置の情報が信用できないと判断した場合は、当該他機を含む他機に対して、当該他機の位置の情報が信用できない旨を表す信号を発信することを特徴とする請求項１から請求項３のいずれか一項に記載の慣性航法装置の誤差補正システム。
5. 前記誤差補正装置は、自機からの距離が遠い他機ほど重みが大きくなるように前記測角残差を重み付けして、前記測角残差に基づいて自機の前記慣性航法装置で生じる誤差を補正することを特徴とする請求項１から請求項４のいずれか一項に記載の慣性航法装置の誤差補正システム。
6. 前記誤差補正装置は、ある他機について算出した前記測角残差を立体角表現した場合の立体角が閾値以下である場合、当該他機の識別情報を含む精度確定信号を発信することを特徴とする請求項１から請求項５のいずれか一項に記載の慣性航法装置の誤差補正システム。
7. 搭載した慣性航法装置が算出する位置と姿勢に基づいて自律的に飛行する複数の飛行体における慣性航法装置の誤差補正方法であって、
   前記飛行体が自機から見た他機の座標を算出する座標算出工程と、
   前記座標算出工程で算出した前記自機から見た他機の座標に基づいて自機データに基づく当該他機の方位角及び仰角を算出する自機データに基づく他機の方位角等算出工程と、
   無線通信装置を介して当該他機から受信した、当該他機の前記慣性航法装置が算出した当該他機の位置の情報と、自機の前記慣性航法装置が算出した自機の位置の情報に基づいて他機データに基づく当該他機の方位角及び仰角を算出する他機データに基づく他機の方位角等算出工程と、
   前記自機データの基づく当該他機の方位角及び仰角と、前記他機データに基づく当該他機の方位角及び仰角との差である測角残差に基づいて自機の前記慣性航法装置で生じる誤差を補正する誤差補正工程と、
   を含むことを特徴とする慣性航法装置の誤差補正方法。

Images