JP2020008420A - 移動体の位置及び姿勢を推定する装置とそのプログラム、移動体の位置及び姿勢を推定するシステムとその方法 - Google Patents

Abstract

【課題】低コストの移動体で得られる観測データを用いて移動体の位置及び姿勢を高精度に推定することが可能なシステムを提供する。【解決手段】ＵＡＶ１に設置された３以上の受信機の各々が、複数の衛星７から受信した信号に基づいて、これらの衛星７から受信機までの距離に関する情報を含んだ観測データを生成する。情報処理装置５は、この観測データと、複数の衛星７の位置に関する位置データとに基づいて、１以上の受信機が衛星７からの信号を受信する推定受信位置を算出し、推定受信位置とＵＡＶ１の推定姿勢とに基づいて、ＵＡＶ１における測距装置２０の推定位置を算出する。測距装置２０は、受信機において衛星７からの信号を受信するタイミング同期して対象物にレーザー光を照射し、対象物との距離を測定する。【選択図】図１

Description

本開示は、移動体の位置及び姿勢を推定する装置とそのプログラム、移動体の位置及び姿勢を推定するシステムとその方法に係り、例えば、測量に使用されるドローンなどの移動体の位置及び姿勢を推定する装置に関するものである。

一般に航空測量では、航空機に搭載したカメラやラインセンサによって地上を撮影し、その画像から地図が作成される（例えば、下記の特許文献１を参照）。また、近年では、ドローンなどのＵＡＶ（unmanned aerial vehicle：無人航空機）を使用した航空測量も実用化されている。

特開平１０−１５３４２６号公報

ＵＡＶを使用して地上を撮影した画像から正確な地図を作成するためには、一般に、ＧＣＰ（ground control point）と呼ばれるマーカを予め地上に設置しておき、画像に含まれるＧＣＰの位置情報を利用して地図を補正する必要がある。そのため、ＧＣＰの設置に手間がかかるという問題や、ＧＣＰを設置できない環境には対応できないという問題がある。

他方、近年では、ＵＡＶにレーザースキャナを搭載し、ＵＡＶから地上までの距離を測定する方法が実用化されている。この方法では、ＧＣＰを設置しなくても測量を行うことが可能である一方で、ＵＡＶの位置と姿勢を高い精度で推定することが要求される。高精度のＧＮＳＳ（global navigation satellite system：全地球測位システム）受信機やＩＭＵ（Inertial measurement unit：慣性計測装置）を用いることにより、位置と姿勢の推定精度を高めることは可能である。しかしながら、そのような高精度の機器を用いた場合、製造コストが高くなるという問題がある。また、ＧＮＳＳ受信機とＩＭＵをそれぞれＵＡＶに搭載して測定を行う場合、これらの装置では測定原理が異なり、所望の性能を発揮するために配慮すべき点が異なることから、様々な測定環境において両者の性能要求を満たすことが難しいという問題がある。しかも、ＧＮＳＳ受信機とＩＭＵとでは測定データの性質が異なることから、データ処理が複雑になるという問題もある。

そこで、本開示は、低コストの移動体で得られる観測データを用いて移動体の位置及び姿勢を高精度に推定することが可能な装置とそのプログラムを提供すること、並びに、低コストの移動体の位置及び姿勢を高精度に推定することが可能なシステムとその方法を提供することを目的とする。

本開示の第１の態様は、移動体の位置及び姿勢を推定する装置に関する。この装置は、移動体に設置されたＮ個（Ｎは３以上の整数を示す。）の受信機の各々において複数の衛星から受信した信号に基づいて生成された観測データと、複数の衛星の位置に関する位置データとに基づいて、移動体の姿勢を推定する姿勢推定部と、観測データと位置データとに基づいて、移動体の位置を推定する位置推定部とを有する。観測データは、複数の衛星と受信機において衛星からの信号を受信する受信位置との距離に関する情報を含んでいる。位置推定部は、位置データ及び観測データに基づいて、１以上の受信機が衛星からの信号を受信する推定受信位置を算出し、姿勢推定部において推定された移動体の姿勢と推定受信位置とに基づいて、移動体における基準点の推定位置を算出する。

上記の装置によれば、移動体に設置されたＮ個（３以上）の受信機の各々において複数の衛星から受信した信号に基づいて生成された観測データであって、複数の衛星から受信機の受信位置までの距離に関する情報が含まれた観測データと、複数の衛星の位置に関する位置データとに基づいて、移動体の姿勢が推定されるとともに、移動体の位置が推定される。これにより、ＩＭＵを移動体に設置せずとも、Ｎ個の受信機によって移動体の姿勢を高精度に推定することが可能になる。また、２周波ＧＮＳＳ受信機などの高精度の受信機を移動体に設置せずとも、移動体の位置を高精度に推定することが可能になる。従って、低コストの移動体で得られる観測データを用いて移動体の位置及び姿勢を高精度に推定することが可能になる。

また、上記の装置によれば、位置データ及び観測データに基づいて、移動体に設置された１以上の受信機の推定受信位置が算出され、この算出された推定受信位置と、姿勢推定部において推定された移動体の姿勢とに基づいて、移動体における基準点の推定位置が算出される。従って、基準点にレーザースキャナなどの測定装置を配置させることにより、推定された姿勢と位置に基づく精度の高い測定を行うことが可能になる。

好適に、位置推定部は、位置データ及び観測データに基づいて、各受信機の推定受信位置を算出し、各受信機の受信位置と各受信機の推定受信位置とのずれに関する判定基準に基づいて、各受信機の推定受信位置が適切か否か判定し、当該判定において適切と判定した受信機の推定受信位置と、姿勢推定部において推定された移動体の姿勢とに基づいて、基準点の推定位置を算出してよい。

この構成によれば、位置データ及び観測データに基づいて、各受信機の推定受信位置が算出され、各受信機の推定受信位置が適切か否かが、所定の判定基準に基づいて判定される。この判定基準は、移動体において各受信機が衛星からの信号を受信する受信位置と、算出された各受信機の推定受信位置とのずれに関するものであり、移動体における受信機の受信位置から大きくずれた推定受信位置は不適切と判定されるように設定される。基準点の推定位置は、この判定において適切と判定された受信機の推定受信位置と、姿勢推定部において推定された移動体の姿勢とに基づいて算出される。従って、移動体における受信機の受信位置に対して大きくずれた不適切な推定受信位置が、基準点の推定位置の算出に用いられなくなるため、基準点の推定位置の精度が向上する。

好適に、位置推定部は、各受信機の推定受信位置について適否を判定する場合、１つの受信機の推定受信位置を基準位置として他の受信機の推定受信位置がそれぞれ適切か否か判定する判定処理を、各受信機について行い、各判定処理では、姿勢推定部において推定された移動体の姿勢と基準位置とに基づいて、受信機の受信位置となるべき目標位置を他の受信機の各々について算出し、目標位置と推定受信位置との距離が所定の範囲内に含まれるか否かを他の受信機の各々について判定し、各受信機について判定処理を行った結果に基づいて、各受信機の推定受信位置が適切か否か判定してよい。

この構成によれば、各受信機の推定受信位置について適否を判定する場合、各受信機について判定処理が行われる。各判定処理では、１つの受信機の推定受信位置が基準位置とされ、他の受信機の推定受信位置がそれぞれ適切か否か判定される。すなわち、姿勢推定部において推定された移動体の姿勢と基準位置とに基づいて、他の受信機の目標位置がそれぞれ算出され、目標位置と推定受信位置との距離が所定の範囲内に含まれるか否かが他の受信機の各々について判定される。そして、各受信機について判定処理が行われると、それらの判定処理の結果に基づいて、各受信機の推定受信位置がそれぞれ適切か否か判定される。従って、移動体における受信機の受信位置に対して大きくずれた不適切な推定受信位置が効果的に判別される。

好適に、位置推定部は、２以上の受信機の推定受信位置が適切であると判定した場合、２以上の受信機の推定受信位置について算出される２以上の基準点の推定位置を平均化してよい。

この構成によれば、適切であると判定された２以上の受信機の推定受信位置ついて算出される２以上の推定位置を平均化したものとして、基準点の推定位置が得られるため、基準点の推定位置の精度が向上する。

好適に、位置推定部は、受信機の推定受信位置を算出する場合、衛星から伝搬する信号の搬送波位相に関する整数アンビギュイティが整数値として解かれた高精度の推定受信位置、又は、整数アンビギュイティが非整数値として解かれた低精度の推定受信位置を算出し、基準点の推定位置を算出する場合、当該算出に使用する推定受信位置から低精度の推定受信位置を除外してよい。

この構成によれば、基準点の推定位置を算出する場合、当該算出に使用する推定受信位置から低精度の推定受信位置が除外される。これにより、基準点の推定位置の算出に低精度の推定受信位置が用いられなくなるため、基準点の推定位置の精度が向上する。

好適に、位置推定部は、各衛星からの受信信号の信号対雑音比に関する情報を含んだ観測データに基づいて、同一の衛星からの同一時刻の受信信号に関する信号対雑音比がＮ個の受信機においてばらつく度合いを表す評価値を、複数の衛星についてそれぞれ算出し、各衛星について算出した評価値に基づいて、各衛星からの受信信号が正常か否かを判定し、受信機の推定受信位置を算出する場合、正常であると判定された衛星からの受信信号に基づく観測データを使用してよい。

この構成によれば、衛星ごとに評価値が算出され、各衛星について算出された評価値に基づいて、各衛星からの受信信号が正常か否か判定される。この評価値は、同一の衛星からの同一時刻の受信信号に関する信号対雑音比がＮ個の受信機においてばらつく度合いを表す。マルチパスによる受信信号のフェージングが生じている場合、Ｎ個の受信機においてフェージングによる受信信号の変動が異なることから、Ｎ個の受信機における同一時刻の信号対雑音比のばらつき度合いが大きくなる。従って、信号対雑音比のばらつき度合いを表す評価値に基づいて、マルチパスによる受信信号の変動が生じているか否かを判定することが可能になる。この判定により正常であると判定された衛星からの受信信号に基づく観測データを用いて、各受信機の推定受信位置が算出されるため、推定受信位置の算出結果がマルチパスの影響を受け難くなる。

好適に、２つの受信機の受信位置により規定されるベクトルを基線ベクトルと呼び、移動体の姿勢が所定の基準姿勢の場合における基線ベクトルを基準ベクトルと呼ぶ場合に、姿勢推定部は、観測データ及び位置データに基づいて、２組以上の受信機のペアにおける２以上の基線ベクトルをそれぞれ観測ベクトルとして算出し、算出した２以上の観測ベクトルと、当該２以上の観測ベクトルに対応する２以上の基準ベクトルとに基づいて、移動体の姿勢を推定してよい。

この構成によれば、移動体の姿勢を基準姿勢に対して変化させると、２以上の基線ベクトルは、これに対応する２以上の基準ベクトルに対して変化を生じる。２組以上の受信機のペアについて算出された２以上の観測ベクトルと、これに対応する２以上の基準ベクトルとの違いは、基準姿勢に対する移動体の姿勢の違いを表す。従って、当該２以上の観測ベクトルと当該２以上の基準ベクトルとに基づいて移動体の姿勢を推定することが可能となる。

好適に、姿勢推定部は、Ｎ個の受信機の少なくとも一部の受信機のペアについて観測ベクトルを算出し、算出した観測ベクトルの各々について、観測ベクトルの長さと当該観測ベクトルに対応する基準ベクトルの長さとの誤差が所定の範囲に含まれるか否か判定し、移動体の姿勢を推定する場合、誤差が所定の範囲に含まれると判定した２以上の観測ベクトルと、当該２以上の観測ベクトルに対応する２以上の基準ベクトルとに基づいて当該姿勢を推定してよい。

この構成によれば、算出された各観測ベクトルについて、観測ベクトルの長さと当該観測ベクトルに対応する基準ベクトルの長さとの誤差が所定の範囲に含まれるか否か判定される。そして、移動体の姿勢を推定する場合には、当該誤差が所定の範囲に含まれると判定された２以上の観測ベクトルと、当該２以上の観測ベクトルに対応する２以上の基準ベクトルとに基づいて当該姿勢の推定が行われる。従って、観測ベクトルの長さと当該観測ベクトルに対応する基準ベクトルの長さとの誤差が大きい観測ベクトルが、姿勢の推定に使用されなくなるため、姿勢の推定の精度が向上する。

好適に、姿勢推定部は、２以上の基準ベクトルと２以上の観測ベクトルとに基づいて移動体の姿勢を推定する場合、一対一に対応する２以上の基準ベクトルと２以上の観測ベクトルとの間の変換を規定する変換行列を、目的関数が最小となるように算出してよい。目的関数は、対応関係を持った基準ベクトル及び観測ベクトルのペア毎に得られるベクトル誤差量を、全てのペアについて足し合わせた和に応じた関数であってよい。ベクトル誤差量は、ペアにおける一方のベクトルを変換行列により変換して得られるベクトルと他方のベクトルとの差に応じた値を持ってよい。

この構成によれば、一対一に対応する２以上の基準ベクトルと２以上の観測ベクトルとの間の変換を規定する変換行列が、目的関数を最小とするように算出される。目的関数は、対応関係を持った基準ベクトル及び観測ベクトルのペア毎に得られるベクトル誤差量を、全てのペアについて足し合わせた和に応じた関数であるため、目的関数を最小とする変換行列によって、各ペアのベクトル誤差量が全体的に小さくなる。ベクトル誤差量は、ペアにおける一方のベクトルを変換行列により変換して得られるベクトルと他方のベクトルとの差に応じた値を持つため、各ベクトル誤差量が小さいことは、変換行列による基準ベクトルと観測ベクトルとの間の変換誤差が小さいことを意味する。従って、目的関数を最小とするように変換行列が算出されることにより、基準姿勢に対する姿勢の違いを正しく表した変換行列が得られる。

好適に、姿勢推定部は、２つの受信機の受信位置に対応する観測ベクトルを算出する場合、衛星から伝搬する信号の搬送波位相に関する整数アンビギュイティが整数値として解かれた高精度の観測ベクトル、又は、整数アンビギュイティが非整数値として解かれた低精度の観測ベクトルを算出し、移動体の姿勢を推定する場合、２以上の高精度の観測ベクトルと、当該２以上の高精度の観測ベクトルに対応する２以上の基準ベクトルとに基づいて当該姿勢を推定してよい。

この構成によれば、移動体の姿勢を推定する場合、２以上の高精度の観測ベクトルと、当該２以上の高精度の観測ベクトルに対応する２以上の基準ベクトルとに基づいて当該姿勢の推定が行われる。これにより、低精度の観測ベクトルが移動体の姿勢の推定に用いられなくなるため、姿勢の推定の精度が向上する。

好適に、姿勢推定部は、１組の受信機のペアについて、互いに逆を向く２つの観測ベクトルを算出してよい。

この構成によれば、１組の受信機のペアにおいて、互いに逆を向く２つの観測ベクトルでは、整数アンビギュイティの解の結果（整数値又は非整数値）が一致しない可能性がある。従って、１組の受信機のペアについて２つの観測ベクトルを算出することにより、高精度の観測ベクトルを取得可能な機会が多くなる。

本開示の第２の態様は、上記第１の態様に係る装置における位置推定部及び姿勢推定部としてコンピュータを機能させるためのプログラムに関する。

本開示の第３の態様は、移動体の位置及び姿勢を推定するシステムに関する。このシステムは、移動体に設置され、複数の衛星から放送される信号をそれぞれ受信し、受信した信号に基づいて複数の衛星からの距離に関する情報を含んだ観測データをそれぞれ生成する３以上の受信機と、上記第１の態様に係る装置とを有する。

好適に、第３の態様に係るシステムは、移動体の基準点に位置しており、Ｎ個の受信機における衛星からの信号の受信と同期して、基準点から対象物までの距離を測定する測距装置を有してよい。
この構成によれば、移動体の位置及び姿勢を推定した結果と、測距装置における距離の測定結果とに基づいて、対象物の精密な３次元データが得られる。

本開示の第４の態様は、移動体の位置及び姿勢を推定する方法に関する。この方法は、移動体に設置されたＮ個（Ｎは３以上の整数を示す。）の受信機の各々において、複数の衛星から受信した信号に基づいて、複数の衛星から受信機までの距離に関する情報を含んだ観測データを生成することと、観測データと、複数の衛星の位置に関する位置データとに基づいて、移動体の姿勢を推定することと、観測データと位置データとに基づいて、移動体の位置を推定することとを有する。移動体の位置を推定することは、位置データ及び観測データに基づいて、１以上の受信機が衛星からの信号を受信する推定受信位置を算出することと、推定された移動体の姿勢と推定受信位置とに基づいて、移動体における基準点の推定位置を算出することとを含む。

本開示によれば、低コストの移動体で得られる観測データを用いて移動体の位置及び姿勢を高精度に推定することが可能な装置とそのプログラムを提供できる。また、本開示によれば、低コストの移動体の位置及び姿勢を高精度に推定することが可能なシステムとその方法を提供できる。

図１は、本実施形態に係るシステムの構成の一例を示す図である。 図２Ａ〜図２Ｂは、ＵＡＶの一例を示す図である。 図３は、ＵＡＶに搭載された情報収集装置の構成の一例を示す図である。 図４は、情報処理装置の構成の一例を示す図である。 図５は、ＵＡＶに搭載された情報収集装置により情報を収集して３次元地図を作成する動作を説明するためのフローチャートである。 図６は、姿勢推定処理を説明するための第１フローチャートである。 図７は、姿勢推定処理を説明するための第２フローチャートである。 図８Ａ〜図８Ｂは、６つの受信位置における３０個の基線ベクトル（観測ベクトル、基準ベクトル）を図解した図である。 図９は、位置推定処理を説明するための第１フローチャートである。 図１０は、位置推定処理を説明するための第２フローチャートである。 図１１は、位置推定処理を説明するための第３フローチャートである。 図１２は、目標位置に対する推定受信位置のずれの図解した図である。 図１３は、推定受信位置の適否を判定した結果の一例を示す図である。 図１４は、ＳＮＲのばらつきに基づいて利用可能な衛星を選択する処理の一例を説明するためのフローチャートである。

図１は、本実施形態に係るシステムの構成の一例を示す図である。本実施形態に係るシステムは、移動体としてのＵＡＶ（unmanned aerial vehicle）１において、複数の衛星７から放送される測位用の信号を周期的に受信し、ＵＡＶ１の位置及び姿勢を推定するとともに、信号の受信タイミングに同期して、ＵＡＶ１から地表９までの距離を測定する。このシステムは、ＵＡＶ１の位置と姿勢の推定結果、並びに、ＵＡＶ１から地表９までの距離と方向の測定結果を収集することにより、地表９の３次元の地図を作成する。

図１の例に示すシステムは、情報処理装置５を有する。情報処理装置５は、上空を飛行するＵＡＶ１において取得される観測データ及び測距データと、地上に設置された地上基準局３において取得される観測データとを入力し、これらのデータを処理することによって、地表９の３次元の地図を作成する。

ＵＡＶ１及び地上基準局３において取得される観測データは、複数の衛星７から放送される測位用の信号に基づいて生成されるデータであり、複数の衛星７からの距離（衛星７からアンテナまでの距離）に関する情報を含む。後述するように、ＵＡＶ１は複数の受信機を有するため、ＵＡＶ１では複数の受信機の観測データが取得される。また、ＵＡＶ１において取得される測距データは、ＵＡＶ１から地表９までの距離の測定値と、その距離を測定する際のＵＡＶ１から見た測定方向の情報を含む。測距データは、例えば図１に示すように地表９に向けて照射されたレーザー光の反射光に基づいて得られる距離の測定値と、レーザー光の照射方向に関する情報を含む。ＵＡＶ１及び地上基準局３において取得される観測データと、ＵＡＶ１において取得される測距データは、それぞれ所定の周期（例えば１秒の周期）でほぼ同じ時刻に取得される。

図２Ａ〜図２Ｂは、ＵＡＶ１の一例を示す図であり、図２Ａは平面図、図２Ｂは正面図を示す。図２Ａ〜図２Ｂに示すＵＡＶ１は、プロペラ型のドローン２４と、ドローン２４に連結された情報収集装置１０とを有する。ドローン２４は、本体部２５と、本体部２５を通る仮想中心線ＶＬからそれぞれ離れる方向に伸びた６つの腕部２７−１〜２７−６（以下、区別せずに「腕部２７」と記す場合がある。）と、腕部２７−１〜２７−６の一端に設けられた６つのプロペラ２６−１〜２６−６（以下、区別せずに「プロペラ２６」と記す場合がある。）とを有する。図２Ａに示すように仮想中心線ＶＬと平行な方向から見た場合、６つのプロペラ２６は、仮想中心線ＶＬを中心として環状に等間隔に配置されている。

情報収集装置１０は、それぞれ衛星７からの測位用の信号を受信する６つのアンテナ１９−１〜１９−６（以下、区別せずに「アンテナ１９」と記す場合がある。）と、６つのアンテナ１９が固定されたフレーム１１とを有する。図２Ａに示すように仮想中心線ＶＬと平行な方向から見た場合、６つのアンテナ１９は、仮想中心線ＶＬを中心として環状に等間隔に配置されている。

フレーム１１は、本体部１２と、６つの腕部１７−１〜１７−６（以下、区別せずに「腕部１７」と記す場合がある。）を有する。仮想中心線ＶＬは本体部１２を通っており、６つの腕部１７は仮想中心線ＶＬからそれぞれ離れる方向に伸びている。アンテナ１９は、本体部１２から離れた腕部１７の一端に固定されている。図２Ｂの例において、腕部１７は本体部１２から水平方向に伸び、途中で水平方向から上方へＬ字状に折れ曲がり、上側へ伸びた腕部１７の末端に円盤状のアンテナ１９が固定されている。６つのアンテナは、仮想中心線ＶＬに対して垂直な共通の仮想平面ＶＰ上に位置する。

図２Ａに示すように仮想中心線ＶＬと平行な方向から見た場合、腕部１７は、隣接する２つの腕部２７がなす角をほぼ２等分する方向に伸びている。腕部１７の一端に固定された６つのアンテナ１９は、ドローン２４のプロペラ２６よりも仮想中心線ＶＬから離れており、かつ、プロペラ２６よりも上側に位置する。情報収集装置１０の本体部１２は、ドローン２４の本体部２５の下面に連結されている。情報収集装置１０は、ドローン２４により吊り下げられた状態でドローン２４とともに飛行する。情報収集装置１０の本体部１２の下面には、測距装置２０が取り付けられており、測距装置２０から地表９に向かって測距用のレーザー光が照射される。

図３は、ＵＡＶ１に搭載された情報収集装置１０の構成の一例を示す図である。図３に示す情報収集装置１０は、位置と姿勢の推定に用いられる６つの受信機１８−１〜１８−３（以下、区別せずに「受信機１８」と記す場合がある。）と、測距装置２０と、測距装置２０の測定タイミングの設定に用いられる受信機１８Ａと、制御装置２１とを有する。

受信機１８は、衛星７から放送される信号をアンテナ１９において受信する。受信機１８は、複数の衛星７において放送される測位用の信号を受信し、その受信した信号に基づいて、複数の衛星７とアンテナ１９において測位用の信号が受信される受信位置との距離に関する情報を含んだ観測データを生成する。例えば観測データは、複数の衛星７の各々から放送された信号の搬送波位相に関する情報を含む。受信機１８は、衛星７において精密に管理されるシステム時計に同期したタイミングで、衛星７からの信号を周期的に（例えば１秒の周期で）受信し、観測データを生成する。

受信機１８Ａは、衛星７から放送される信号をアンテナ１９Ａにおいて受信する。受信機１８Ａは、衛星７からの受信信号に基づいて、上述のシステム時計に同期した周期的な受信タイミングを知らせる信号を測距装置２０に出力する。

測距装置２０は、ＵＡＶ１における位置の推定の基準点に位置しており、基準点から対象物までの距離を測定する。測距装置２０は、例えばレーザースキャナであり、地表９の一点に照射したレーザー光に対する反射光の位相や時間差などに基づいて、地表の一点と基準点との距離を計測する。測距装置２０は、レーザー光によって地表９を走査することにより、地表９の多数の位置について距離を計測する。測距装置２０は、受信機１８から出力される受信タイミングの信号に基づき、６個の受信機１８における衛星７からの信号の受信と同期したタイミングで距離の計測を行う。測距装置２０は、距離の測定値と測定方向（レーザー光の照射方向）に関する情報が含まれた測距データを生成する。

制御装置２１は、受信機１８−１〜１８−６において生成される観測データと、測距装置２０において生成される測距データとを記録する。図３の例において、制御装置２１は、処理部２２と記憶部２３を含む。処理部２２は、６つの受信機１８で得られた観測データと測距装置２０で得られた測距データとを、同じ時刻に得られたデータ同士で関連付けて記憶部２３に記録する。これにより、記憶部２３には、同じ時刻に得られた観測データ及び測距データのセットが時刻順に蓄積される。

図４は、情報処理装置５の構成の一例を示す図である。図４に示す情報処理装置５は、インターフェース部５１と、表示部５２と、処理部５３と、記憶部５４を有する。

インターフェース部５１は、ユーザの操作に応じた情報を処理部５３に入力するためのユーザーインターフェース機器（キーボード、マウス、タッチパッド、タッチパネルなど）を含む。またインターフェース部５１は、外部の機器と処理部５３との間で情報をやり取りするための通信インターフェースや、ＵＳＢなどの汎用入出力インターフェース、記録媒体の読み取り装置などを含む。

表示部５２は、処理部５３の制御に従って映像を表示する装置であり、ディスプレイ装置（液晶ディスプレイ、有機ＥＬディスプレイなど）を含む。

処理部５３は、種々の情報処理を行う装置であり、例えば記憶部５４に記憶されるプログラム５４１の命令コードに従って処理を実行するコンピュータを含む。処理部５３は、少なくとも一部の処理を専用のハードウェアによって実行してもよい。

図４の例において、処理部５３は、姿勢推定部５３１と、位置推定部５３２と、３次元地図作成部５３３とを有する。処理部５３は、情報収集装置１０の記憶部２３（図３）に蓄積されたデータ（観測データ、測距データ）をインターフェース部５１において入力し、これらのユニット（姿勢推定部５３１、位置推定部５３２、３次元地図作成部５３３）での処理に使用する。

姿勢推定部５３１は、ＵＡＶ１に設置された６つ受信機１８の各々において複数の衛星７から受信した信号に基づいて生成された観測データと、複数の衛星７の位置に関する位置データとに基づいて、ＵＡＶ１の姿勢を推定する。位置データは、それぞれ所定の軌道を周回する各衛星７の各時刻での位置に関する情報を含んだデータであり、公開された既知の情報に基づいて取得される。

位置推定部５３２は、上述した観測データと位置データとに基づいて、ＵＡＶ１における基準点の位置を推定する。すなわち位置推定部５３２は、位置データ及び観測データに基づいて、１以上の受信機１８がアンテナ１９において衛星７からの信号を受信する位置を推定した「推定受信位置ＰＥ」を算出する。位置推定部５３２は姿勢推定部５３１において推定されたＵＡＶ１の姿勢と、算出した１以上の推定受信位置ＰＥとに基づいて、ＵＡＶ１における基準点の推定位置ＰＸを算出する。

３次元地図作成部５３３は、姿勢推定部５３１において推定されたＵＡＶ１の姿勢と、位置推定部５３２において推定された基準点の推定位置ＰＸと、測距装置２０における距離の測定値と、測距装置２０におけるレーザー照射方向の情報とを時刻ごとに取得し、取得した時刻ごとのデータに基づいて、地表９の各位置の３次元座標を算出する。

記憶部５４は、処理部５３のコンピュータによって実行されるプログラム５４１や、処理部５３の処理の過程で一時的に保存されるデータ、処理部５３の処理に利用される定数などを記憶する。記憶部５４は、たとえばＲＯＭやＲＡＭ、フラッシュメモリ、ハードディスク、磁気記録媒体など、１以上の任意の記憶装置を含む。

次に、上述した構成を有するシステムの動作について説明する。

（全体の動作）
図５は、ＵＡＶ１に搭載された情報収集装置１０により情報を収集して３次元地図を作成する動作を説明するためのフローチャートである。
まず、ＵＡＶ１の各受信機１８において衛星７からの信号を受信する受信位置（各アンテナ１９における信号の受信位置）が計測される（ＳＴ１００）。各受信機１８の受信位置は、基準点（測距装置２０のレーザー出射位置）に対する相対的な位置として精密に計測される。各受信機１８の受信位置は、推定姿勢を算出する際の基準として用いられる他、観測データに基づいて算出された推定位置の適否を判定するために用いられる。

次に、ＵＡＶ１の飛行が実施され、ＵＡＶ１に搭載された情報収集装置１０によって情報が収集される（ＳＴ１０５）。すなわち、情報収集装置１０は、６つの受信機１８における信号の受信と測距装置２０における距離の測定とを同じタイミングで周期的に実施する。情報収集装置１０は、同じタイミングで得られた観測データ及び測距データのセットを、時系列のデータとして蓄積する。

また、ＵＡＶ１の情報収集装置１０における情報の収集と並行して、地上基準局３（図１）における衛星７の信号の受信も実施される。地上基準局３は、公共の機関によって設置されたものでもよいし、ユーザが自ら設置したものでもよい。地上基準局３は、予め精密に位置が測定された場所で各衛星７からの信号を受信し、各衛星７までの距離に関する情報を含んだ観測データを生成する。

情報収集装置１０において所望の情報が収集されると、情報収集装置１０から収集された情報（観測データ及び測距データ）が回収され、情報処理装置５に入力される。また、地上基準局３で得られた観測データや、各衛星７の各時刻での位置を示す位置データも情報処理装置５に入力される（ＳＴ１２０）。

情報処理装置５の姿勢推定部５３１は、情報収集装置１０から収集された６つの受信機１８の観測データと、各衛星７の位置データとに基づいて、各時刻におけるＵＡＶ１の推定姿勢を算出する（ＳＴ１１５）。推定姿勢を算出する処理の詳細については、後ほどで図６〜図８を参照して説明する。

次に、情報処理装置５の位置推定部５３２は、情報収集装置１０から収集された６つの受信機１８の観測データと、各衛星７の位置データと、地上基準局３で得られた観測データと、既に算出されたＵＡＶ１の推定姿勢とに基づいて、ＵＡＶ１における基準点の推定位置ＰＸを算出する（ＳＴ１２０）。基準点の推定位置ＰＸを算出する処理については、後ほど図９〜図１４を参照して説明する。

情報処理装置５の３次元地図作成部５３３は、同一の時刻におけるＵＡＶ１の推定姿勢、基準点の推定位置ＰＸ、及び、測距データ（距離の測定値とレーザー光の照射方向）に基づいて、地表９の一点の３次元座標を算出する。各時刻について算出した地表９の３次元座標を集めることにより、地表９の一定の範囲における３次元データ（３次元地図）が得られる（ＳＴ１２５）。

（姿勢推定処理）
図６〜図７は、情報収集装置１０の姿勢推定部５３１における姿勢推定処理を説明するためのフローチャートである。
まず姿勢推定部５３１は、ステップＳＴ１００（図５）において計測された各受信機１８におけるアンテナ１９の受信位置に基づいて、基準ベクトルＶＡ１〜ＶＡ１５、ＶＢ１〜ＶＢ１５を算出する（ＳＴ２００）。

図８Ａ〜図８Ｂは、６つの受信位置における３０個の基線ベクトル（観測ベクトル、基準ベクトル）を図解した図である。ここで、「基線ベクトル」とは、２つの受信位置（受信機１８のアンテナ１９において衛星７からの信号が受信される位置）により規定されるベクトルである。基線ベクトルは、２つの受信位置の一方を始点、他方を終点したベクトルであり、１組の受信機１８のペアに対して逆向きの方向を持った２つの基線ベクトルが存在する。６つの受信機１８において、受信機１８のペアは１５組存在するため、基線ベクトルは全部で３０個存在する。

「基準ベクトル」とは、ＵＡＶ１の姿勢が所定の基準姿勢の場合における基線ベクトルであり、図８Ａ〜図８Ｂでは「ＶＡｉ」又は「ＶＢｉ」の符号で表されている。ただし、「ｉ」は１から１５までの整数を示す。同一の数字を付された基準ベクトルＶＡｉと基準ベクトルＶＢｉは、１組の受信機１８のペアに対して２つ存在する基線ベクトルであり、互いに逆向きの方向を持つ。以下、基準ベクトルＶＡｉ及びＶＢｉを区別せずに「基準ベクトルＶ」と記す場合がある。

例えば姿勢推定部５３１は、ステップＳＴ１００（図５）において計測された各アンテナ１９の受信位置の座標（基準点を原点とする座標）を、地球固定座標系（ＥＣＥＦ座標系など）の座標へ変換することにより、地球固定座標系に対するＵＡＶ１の基準姿勢を定める。なお、ステップＳＴ１００（図５）において計測された各アンテナ１９の受信位置の座標を地球固定座標系の座標とみなす場合、ステップＳＴ２００の処理は省略してもよい。基準ベクトルＶＡ１〜ＶＡ１５、ＶＢ１〜ＶＢ１５は、地球固定座標系の座標で表された各アンテナ１９の受信位置によってそれぞれ特定される。

次に姿勢推定部５３１は、姿勢の推定の対象となる各受信機１８の観測データを時刻順に選択するとともに、同じ時刻における各衛星７の位置データを取得する（ＳＴ２０５）。そして姿勢推定部５３１は、これらの観測データ及び位置データに基づいて、観測ベクトルＷＡ１〜ＷＡ１５、ＷＢ１〜ＷＢ１５を算出する（ＳＴ２１０）。

「観測ベクトル」とは、観測データ及び位置データに基づいて推定される基線ベクトルであり、図８Ａ〜図８Ｂでは「ＷＡｉ」又は「ＷＢｉ」の符号で表されている。同一の数字を付された観測ベクトルＷＡｉと観測ベクトルＷＢｉは、１組の受信機１８のペアに対して２つ存在する基線ベクトルであり、互いに逆向きの方向を持つ。以下、観測ベクトルＷＡｉ及びＷＢｉを区別せずに「観測ベクトルＷ」と記す場合がある。

例えば姿勢推定部５３１は、２つの受信機１８の受信位置によって規定される基線ベクトルを観測ベクトルＷとして算出する場合、衛星７から受信した信号の搬送波位相を用いた干渉測位の手法を用いる。この場合、姿勢推定部５３１は、一方の受信機１８において受信される信号の搬送波位相と、他方の受信機１８において受信される信号の搬送波位相とに基づいて、一方の受信機１８の受信位置と他方の受信機１８の受信位置との相対関係を観測ベクトルＷとして算出する。干渉測位の手法を用いることにより、受信機１８として汎用的な１周波ＧＮＳＳ受信機を用いた場合でも、２つの受信位置の相対的な位置関係を高精度に推定することが可能となる。

干渉測位の手法によって観測ベクトルＷを算出する場合、姿勢推定部５３１は、２つの受信機において受信された信号の搬送波位相に関する整数アンビキュイティ（例えば搬送波位相二重差の整数アンビギュイティ）を求める演算を行い、整数アンビキュイティが整数値として解かれた高精度の観測ベクトルＷ、又は、整数アンビギュイティが非整数値として解かれた低精度の観測ベクトルＷを算出する。以下、整数アンビキュイティが整数値として解かれた演算結果を「ＦＩＸ解」、整数アンビキュイティが非整数値として解かれた演算結果を「ＦＬＯＡＴ解」と呼ぶ場合がある。

姿勢推定部５３１は、算出した３０個の観測ベクトルＷＡ１〜ＷＡ１５、ＷＢ１〜ＷＢ１５の中から、ＦＩＸ解の観測ベクトルＷのみを抽出する（ＳＴ２１５）。

位置推定部５３２は、互いに逆向きの２つの観測ベクトルＷＡｉ及びＷＢｉのうち、少なくとも一方がＦＩＸ解となる整数値ｉが２個以上あるか否か判定する（ＳＴ２２０）。言い換えると、位置推定部５３２は、２組以上の受信機１８のペアにおける２以上のＦＩＸ解の観測ベクトルＷが算出されたか否かを判定する。

２組以上の受信機１８のペアにおける２以上のＦＩＸ解の観測ベクトルＷが算出された場合（ＳＴ２２０のＹｅｓ）、位置推定部５３２は、算出されたＦＩＸ解の観測ベクトルＷの各々について、観測ベクトルＷの長さと当該観測ベクトルＷに対応する基準ベクトルＶの長さとの誤差が所定の範囲に含まれるか否かを判定する（ＳＴ２２５）。

例えば姿勢推定部５３１は、ＦＩＸ解の観測ベクトルＷｉとこれに対応する基準ベクトルＶｉとに基づいて、誤差Ｅｉ＝｜｜Ｗｉ｜−｜Ｖｉ｜｜を算出する。姿勢推定部５３１は、各観測ベクトルＷｉについて算出した誤差Ｅｉが所定のしきい値Ｅｔｈより小さいか否か判定する。

位置推定部５３２は、誤差Ｅｉの判定結果に基づいて、ＦＩＸ解の観測ベクトルＷｉの中から、誤差Ｅｉがしきい値Ｅｔｈより小さい観測ベクトルＷｉを抽出し（ＳＴ２３０）、この抽出した観測ベクトルＷｉが２以上あるか否か判定する（ＳＴ２３５）。

誤差Ｅｉ＜Ｅｔｈを満たす観測ベクトルＷｉが２以上抽出された場合（ＳＴ２３０のＹｅｓ）、姿勢推定部５３１は、抽出された２以上の観測ベクトルＷｉとこれに対応する２以上の基準ベクトルＶｉとに基づいて、ＵＡＶ１の姿勢を推定する（ＳＴ２４０）。

具体的には、姿勢推定部５３１は、一対一に対応する２以上の基準ベクトルＶｉと２以上の観測ベクトルＷｉとの間の変換を規定する変換行列Ａを、所定の目的関数Ｌ（Ａ）が最小となるように算出する。

観測ベクトルＷｉと基準ベクトルＶｉとは、変換行列Ａによって次式のように表される。

目的関数Ｌ（Ａ）は、例えば次式のように表される。

式（２−１）における｜Ｗｉ−Ａ＊Ｖｉ｜^２の項は、基準ベクトルＶｉを変換行列Ａによって変換したベクトルと観測ベクトルＷｉとの差（ベクトル誤差）に応じた値を持つ。目的関数Ｌ（Ａ）は、この｜Ｗｉ−Ａ＊Ｖｉ｜^２に重み係数αｉを乗じたものを、ステップＳＴ２３０で抽出された全ての観測ベクトルＷｉについて足し合わせた和に応じた関数である。

重み係数αｉは、基準ベクトルＶｉの長さに比例した値を持ち、その総和は式（２−２）に示すように１となる。基準ベクトルＶｉが長くなるほど、アンテナ１９の設置位置の誤差に対する姿勢の誤差は小さくなる。そのため、基準ベクトルＶｉが長いほど｜Ｗｉ−Ａ＊Ｖｉ｜^２に乗じる重み係数αｉを大きくすることによって、姿勢の誤差の小さい変換行列Ａが算出され易くなる。姿勢推定部５３１は、ステップＳＴ４０において変換行列Ａを算出すると、ステップＳＴ２５０へ移行する。

なお、姿勢推定部５３１は、ＦＩＸ解の観測ベクトルＷが１以下の場合や、ＦＩＸ解の観測ベクトルＷが１組の受信機１８のペアにおける逆向きの２つの観測ベクトルＷのみである場合（ＳＴ２２０のＮｏ）、ステップＳＴ４０における変換行列Ａの計算を行わずにステップＳＴ２５０へ移行する。また姿勢推定部５３１は、誤差Ｅｉ＜Ｅｔｈを満たす観測ベクトルＷｉが１以下の場合も、ステップＳＴ２４０をパスしてステップＳＴ２５０に移行する。

姿勢推定部５３１は、ステップＳＴ２５０に移行すると、全ての時刻について変換行列Ａの算出処理が済んだか確認し、処理していない時刻が残っている場合は、時刻を次に進めて（ＳＴ２５５）ステップＳＴ２０５以降の処理を繰り返す。全ての時刻について処理が済んだ場合、姿勢推定部５３１はステップＳＴ２６０に移行する。

姿勢推定部５３１は、ステップＳＴ２６０において、変換行列Ａが未算出であった時刻を特定し、その時刻の前後における変換行列Ａの算出結果に基づいて、当該時刻の変換行列Ａを算出する。例えば姿勢推定部５３１は、前後の時刻において算出した２つの変換行列Ａから、球面線形補間法などを用いて、中間の時刻における変換行列Ａを算出する。

（位置推定処理）
図９〜図１１は、情報収集装置１０の位置推定部５３２における姿勢推定処理を説明するためのフローチャートである。
位置推定部５３２は、位置の推定の対象となる各受信機１８の観測データを時刻順に選択するとともに、姿勢推定処理で使用した各衛星７の位置データに加えて、同じ時刻における地上基準局３の観測データも取得する（ＳＴ３００）。

位置の推定を行うにあたり、まず位置推定部５３２は、６つの受信機１８で同一時刻に受信された同一の衛星７からの受信信号について、ＳＮＲ（信号対雑音比）のばらつき度合いを評価し、その評価結果に基づいて、利用可能な衛星７の観測データを選択する（ＳＴ３０５）。ＳＮＲのばらつき度合いの評価に関する処理については、後ほど図１４を参照して説明する。

次に位置推定部５３２は、ステップＳＴ３０５で選択した各衛星７の観測データと、地上基準局３で得られた各衛星７の観測データと、各衛星７の位置データとに基づいて、６個の受信機１８が各衛星７からの信号を受信すると推定される受信位置（推定受信位置ＰＥ）を算出する。以下、受信機１８−ｊ（ｊは１から６までの整数を示す。）の推定受信位置を「ＰＥｊ」と記す。

位置推定部５３２は、地上基準局３における各衛星７からの信号の受信位置と、受信機１８−ｊの推定受信位置ＰＥｊとの相対的な位置関係を、干渉測位によって算出する。地上基準局３の受信位置は既知であるため、位置推定部５３２は、干渉測位により受信機１８−ｊの推定受信位置ＰＥｊを算出することができる。この場合、位置推定部５３２は、姿勢推定処理における観測ベクトルＷと同様に、地上基準局３の観測データの搬送波位相と受信機１８−ｊの観測データの搬送波位相とに関連する整数アンビキュイティ（例えば搬送波位相二重差の整数アンビキュイティ）を求める演算を行い、ＦＩＸ解又はＦＬＯＡＴ解の推定受信位置ＰＥｊを算出する。

位置推定部５３２は、６つ受信機１８について算出した６つの推定受信位置ＰＥｊから、ＦＩＸ解の推定受信位置ＰＥｊのみを抽出する（ＳＴ３１５）。

ＦＩＸ解の推定受信位置ＰＥｊが２以上ある場合（ＳＴ３２０のＹｅｓ）、位置推定部５３２は、次に述べるステップＳＴ３２５〜ＳＴ３３５の判定処理を、ＦＩＸ解の各推定受信位置ＰＥｊについて行う。ＦＩＸ解の推定受信位置ＰＥｊが２未満の場合（ＳＴ３２０のＮｏ）、位置推定部５３２は、後述するステップＳＴ３６５へ移行する。

位置推定部５３２は、ステップＳＴ３２５〜ＳＴ３３５の判定処理において、複数の受信機１８の受信位置（ステップＳＴ１００で計測された既知の受信位置）と、複数の推定受信位置ＰＥｊとのずれに関する判定基準に基づいて、ＦＩＸ解の推定受信位置ＰＥｊがそれぞれ適切か否か判定する。

具体的には、位置推定部５３２は、ＦＩＸ解の推定受信位置ＰＥｊが算出された２以上の受信機１８を順番に選択する（ＳＴ３２５）。そして、位置推定部５３２は、選択した１つの受信機１８−ｊの推定受信位置ＰＥｊを基準位置ＰＳｊとした場合に、他の受信機１８−ｋ（ｋ≠ｊ）の受信位置となるべき目標位置ＰＴｊｋを算出する。位置推定部５３２は、既に姿勢推定部５３１で推定されたＵＡＶ１の姿勢（変換行列Ａ）と基準位置ＰＳｊとに基づいて、目標位置ＰＴｊｋを算出する（ＳＴ３３０）。各受信機１８の受信位置同士の相対的な位置関係は、ＵＡＶ１の位置や姿勢が変化しても一定であるため、１つの受信機１８−ｊの推定受信位置ＰＥｊ（基準位置ＰＳｊ）とＵＡＶ１の姿勢が分かれば、他の受信機１８−ｋの目標位置ＰＴｊｋを算出することができる。位置推定部５３２は、他の受信機１８−ｋの各々について、推定受信位置ＰＥｋと目標位置ＰＴｊｋとの距離Ｄｊｋが所定の範囲に含まれるか判定する（ＳＴ３３５）。

図１２は、目標位置ＰＴｊｋに対する推定受信位置ＰＥｋのずれを図解した図である。この図の例では、受信機１８−１の推定受信位置ＰＥ１を基準位置ＰＳ１とした場合における、他の受信機１８−２〜１８−６の推定受信位置ＰＥ２〜ＰＥ６と目標位置ＰＴ１２〜ＰＴ１６との距離Ｄ１２〜Ｄ１６が図解されている。基準位置ＰＳｊに対する目標位置ＰＴｊｋは、図１２に示すように、基準位置ＰＳｊを始点とした基準ベクトルＶを変換行列Ａによって変換することより求めることができる。

位置推定部５３２は、ステップＳＴ３２５〜ＳＴ３３５の判定処理を、ＦＩＸ解が得られた全ての受信機１８について行う（ＳＴ３４０、ＳＴ３４５）。

位置推定部５３２は、ＦＩＸ解が得られた全ての受信機１８について判定処理（ＳＴ３２５〜ＳＴ３３５）の結果が得られると、これらの判定結果に基づいて、ＦＩＸ解の推定受信位置ＰＥｊがそれぞれ適切か否か判定する（ＳＴ３５０）。

図１３は、ステップＳＴ３２５〜ＳＴ３３５の判定処理によって推定受信位置ＰＥｊの適否を判定した結果の一例を示す図である。「〇」は距離Ｄｊｋが所定の範囲に含まれる場合を示し、「×」は距離Ｄｊｋが所定の範囲から逸脱している場合を示す。図１３の例において、受信機１８−４の推定受信位置ＰＥ４はＦＬＯＡＴ解であるため、ステップＳＴ３２５〜ＳＴ３３５の判定処理が実施されていない。位置推定部５３２は、例えば、距離Ｄｊｋが所定の範囲に含まれないと判定された判定処理が過半数を超える推定受信位置ＰＥｊを、適切でないと判定する。例えば図１３の例において、推定受信位置ＰＥ６は全判定処理で「×」のため、位置推定部５３２は推定受信位置ＰＥ６を適切でないと判定する。

なお、位置推定部５３２は、基準位置ＰＳｊの判定処理において過半数を超える推定受信位置ＰＥｋの距離Ｄｊｋが所定の範囲に含まれないと判定された場合、基準位置ＰＳｊである推定受信位置ＰＥｊを適切でないと判定してもよい。

位置推定部５３２は、ステップＳＴ３５０において適切であると判定した推定受信位置ＰＥｊから、推定姿勢（変換行列Ａ）に基づいて、基準点の推定位置ＰＸｊをそれぞれ算出する（ＳＴ３５５）。基準点に対する各受信機１８−ｊの受信位置は既に測定されているため（ＳＴ１００：図５）、受信機１８−ｊの推定受信位置ＰＥｊと推定姿勢（変換行列Ａ）が分かれば、基準点の推定位置ＰＸｊを求めることができる。

位置推定部５３２は、ステップＳＴ３５０において２以上の推定受信位置ＰＥｊが適切であると判定した場合、この２以上の推定受信位置ＰＥｊについて算出した２以上の推定位置ＰＸｊを平均化することにより、最終的な推定位置ＰＸを算出する（ＳＴ３６０）。ステップＳＴ３６０において推定位置ＰＸを算出すると、位置推定部５３２はステップＳＴ３８０へ移行する。

なお位置推定部５３２は、ステップＳＴ３１０において算出されたＦＩＸ解の推定受信位置ＰＥｊが１つのみであった場合（ＳＴ３２０のＮｏ、ＳＴ３６５のＹｅｓ）、１つのＦＩＸ解の推定受信位置ＰＥｊと推定姿勢（変換行列Ａ）とに基づいて、基準点の推定位置ＰＸを算出する（ＳＴ３７０）。また位置推定部５３２は、ステップＳＴ３１０においてＦＩＸ解の推定受信位置ＰＥｊが算出されなかった場合（ＳＴ３２０のＮｏ、ＳＴ３６５のＮｏ）、基準点の推定位置ＰＸを未算出とする（ＳＴ３７５）。ステップＳＴ３７０、ＳＴ３７５の後、位置推定部５３２はステップＳＴ３８０へ移行する。

位置推定部５３２は、ステップＳＴ３８０に移行すると、全ての時刻について推定位置ＰＸの算出処理が済んだか確認し、処理していない時刻が残っている場合は、時刻を次に進めて（ＳＴ３８５）ステップＳＴ３００以降の処理を繰り返す。全ての時刻について処理が済んだ場合、位置推定部５３２はステップＳＴ３９０に移行する。

位置推定部５３２は、ステップＳＴ３９０において、推定位置ＰＸが未算出であった時刻を特定し、その時刻の前後における推定位置ＰＸの算出結果に基づいて、当該時刻の推定位置ＰＸを算出する。例えば位置推定部５３２は、前後の時刻において算出した２つの推定位置ＰＸから、球面線形補間法などを用いて、中間の時刻における推定位置ＰＸを算出する。

（ＳＮＲのばらつきに基づく判定処理）
図１４は、ＳＮＲのばらつきに基づいて利用可能な衛星を選択する処理（ＳＴ３０５：図９）の一例を説明するためのフローチャートである。
位置推定部５３２は、信号を受信可能な複数の衛星７から順番に１つの衛星７を選択する（ＳＴ４００）。位置推定部５３２は、選択した１つの衛星７からの受信信号について６つの受信機１８により計測された６つのＳＮＲを取得し、これらのばらつきの度合いを示す評価値を算出する（ＳＴ４０５）。例えば位置推定部５３２は、６つのＳＮＲの標準偏差を評価値として算出する。位置推定部５３２は、算出した評価値が所定のしきい値を超えるか否か判定し（ＳＴ４１０）、評価値がしきい値を超える場合、この衛星の観測データを利用不可とすることを決定する（ＳＴ４１５）。位置推定部５３２は、ステップＳＴ４０５〜ＳＴ４１５の判定処理を全ての衛星７について行う（ＳＴ４２０）。

（まとめ）
本実施形態によれば、次に述べるような効果が得られる。

（１） ＵＡＶ１に設置された６つの受信機１８の各々において、複数の衛星７から受信した信号に基づいて、観測データが生成される。観測データには、複数の衛星７から受信機１８の受信位置までの距離に関する情報が含まれる。この観測データと、複数の衛星７の位置に関する位置データとに基づいて、ＵＡＶ１の姿勢が推定されるとともに、ＵＡＶ１の位置が推定される。これにより、ＩＭＵをＵＡＶ１に設置せずとも、６つの受信機１８によってＵＡＶ１の姿勢を高精度に推定することが可能になる。また、２周波ＧＮＳＳ受信機などの高精度の受信機をＵＡＶ１に設置せずとも、ＵＡＶ１の位置を高精度に推定することが可能になる。従って、低コストのＵＡＶ１で得られる観測データを用いてＵＡＶ１の位置及び姿勢を高精度に推定できる。

（２） 位置データ及び観測データに基づいて、ＵＡＶ１に設置された１以上の受信機１８の推定受信位置ＰＥｊが算出され、この算出された推定受信位置ＰＥｊと、姿勢推定部５３１において推定されたＵＡＶ１の姿勢とに基づいて、ＵＡＶ１における基準点の推定位置ＰＸが算出される。従って、基準点にレーザースキャナなどの測距装置２０や他の測定装置を配置させることにより、推定された姿勢と位置に基づく精度の高い測定を行うことが可能になる。

（３） 位置データ及び観測データに基づいて、各受信機１８の推定受信位置ＰＥｊが算出され、各受信機１８の推定受信位置ＰＥｊの適否が所定の判定基準に基づいて判定される。この判定基準は、ＵＡＶ１において各受信機１８が衛星からの信号を受信する受信位置と、算出された各受信機の推定受信位置ＰＥｊとのずれに関するものであり、ＵＡＶ１における受信機１８の受信位置から大きくずれた推定受信位置ＰＥｊは不適切と判定されるように設定される。基準点の推定位置ＰＸは、この判定において適切と判定された受信機１８の推定受信位置ＰＥｊと、姿勢推定部５３１において推定されたＵＡＶ１の姿勢とに基づいて算出される。従って、ＵＡＶ１における受信機１８の受信位置に対して大きくずれた不適切な推定受信位置ＰＥｊが、基準点の推定位置ＰＸの算出に用いられなくなるため、基準点の推定位置ＰＸの精度を向上させることができる。

（４） 各受信機の推定受信位置について適否を判定する場合、各受信機１８について判定処理（ＳＴ３２５〜ＳＴ３３５）が行われる。各判定処理では、１つの受信機１８−ｊの推定受信位置ＰＥｊが基準位置ＰＳｊとされ、他の受信機１８−ｋ（ｋ≠ｊ）の推定受信位置ＰＥｋがそれぞれ適切か否か判定される。すなわち、姿勢推定部５３１において推定されたＵＡＶ１の姿勢（変換行列Ａ）と基準位置ＰＳｊとに基づいて、他の受信機１８−ｋの目標位置ＰＴｊｋがそれぞれ算出され、目標位置ＰＴｊｋと推定受信位置ＰＥｋとの距離Ｄｊｋが所定の範囲内に含まれるか否かの判定が、他の受信機１８−ｋの各々について行われる。このようにして、各受信機１８についての判定処理（ＳＴ３２５〜ＳＴ３３５）が行われると、それらの判定処理の結果に基づいて、各受信機１８の推定受信位置ＰＥｊがそれぞれ適切か否か判定される。従って、ＵＡＶ１における受信機１８の受信位置に対して大きくずれた不適切な推定受信位置ＰＥｊを効果的に判別できる。
（５） ２以上の受信機１８の推定受信位置ＰＥｊが判定処理（ＳＴ３２５〜ＳＴ３３５）で適切であると判定された場合、この２以上の受信機１８の推定受信位置ＰＥｊついて算出される２以上の基準点の推定位置ＰＸｊを平均化したものとして、基準点の推定位置ＰＸが得られる。そのため、基準点の推定位置ＰＸの精度を向上させることができる。

（６） 基準点の推定位置を算出する場合、当該算出に使用する推定受信位置から低精度の推定受信位置ＰＥｊ（整数アンビキュイティが非整数値として解かれた推定受信位置ＰＥｊ）が除外される。これにより、基準点の推定位置ＰＸの算出に低精度の推定受信位置ＰＥｊが用いられなくなるため、基準点の推定位置ＰＸの精度を向上させることができる。

（７） 同一の衛星７からの同一時刻の受信信号に関するＳＮＲがＮ個の受信機においてばらつく度合いを表す評価値（標準偏差など）が、衛星７ごとに算出される。そして、各衛星７について算出された評価値に基づいて、各衛星７からの受信信号が正常か否か判定される。マルチパスによる受信信号のフェージングが生じている場合、６つの受信機１８においてフェージングによる受信信号の変動が異なることから、６つの受信機における同一時刻のＳＮＲのばらつき度合いが大きくなる。従って、ＳＮＲのばらつき度合いを表す評価値に基づいて、マルチパスによる受信信号の変動が生じているか否かを判定することが可能になる。この判定により正常であると判定された衛星７からの受信信号に基づく観測データを用いて、各受信機１８の推定受信位置ＰＥｊが算出されるため、推定受信位置ＰＥｊの算出結果がマルチパスの影響を受け難くなる。

（８） ＵＡＶ１の姿勢を基準姿勢に対して変化させると、２以上の基線ベクトルは、これに対応する２以上の基準ベクトルＶに対して変化を生じる。２組以上の受信機１８のペアについて算出された２以上の観測ベクトルＷと、これに対応する２以上の基準ベクトルＶとの違いは、基準姿勢に対するＵＡＶ１の姿勢の違いを表す。従って、当該２以上の観測ベクトルＷと当該２以上の基準ベクトルＶとに基づいて、ＵＡＶ１の姿勢を正しく推定することが可能となる。

（９） 算出された各観測ベクトルＷｉについて、観測ベクトルＷｉの長さと当該観測ベクトルＷｉに対応する基準ベクトルＶｉの長さとの誤差Ｅｉが所定の範囲（Ｅｉ＜Ｅｔｈ）に含まれるか否か判定される。そして、ＵＡＶ１の姿勢を推定する場合には、当該誤差Ｅｉが所定の範囲に含まれると判定された２以上の観測ベクトルＷｉと、当該２以上の観測ベクトルＷｉに対応する２以上の基準ベクトルＶｉとに基づいて当該姿勢の推定が行われる。従って、観測ベクトルＷｉの長さと当該観測ベクトルＷｉに対応する基準ベクトルＶｉの長さとの誤差が大きい観測ベクトルＷｉが、姿勢の推定に使用されなくなるため、姿勢の推定の精度を向上させることができる。

（１０） 一対一に対応する２以上の基準ベクトルＶｉと２以上の観測ベクトルＷｉとの間の変換を規定する変換行列Ａが、目的関数Ｌ（Ａ）を最小とするように算出される。式（２−１）に示す目的関数Ｌ（Ａ）は、対応関係を持った基準ベクトルＶｉ及び観測ベクトルＷｉのペア毎に得られるベクトル誤差量（｜Ｗｉ−Ａ＊Ｖｉ｜^２）を、全てのペアについて足し合わせた和に応じた関数であるため、目的関数Ｌ（Ａ）を最小とする変換行列Ａによって、各ペアのベクトル誤差量が全体的に小さくなる。ベクトル誤差量は、基準ベクトルＶｉ及び観測ベクトルＷｉのペアにおける一方のベクトルを変換行列Ａにより変換して得られるベクトルと他方のベクトルとの差に応じた値を持つため、各ベクトル誤差量が小さいことは、変換行列Ａによる基準ベクトルＶｉと観測ベクトルＷｉとの間の変換誤差が小さいことを意味する。従って、目的関数Ｌ（Ａ）を最小とするように変換行列Ａが算出されることにより、基準姿勢に対する姿勢の違いを正しく表した変換行列Ａを得ることができる。

（１１） ＵＡＶ１の姿勢を推定する場合、２以上のＦＩＸ解の観測ベクトルＷｉと、当該２以上のＦＩＸ解の観測ベクトルＷｉに対応する２以上の基準ベクトルＶｉとに基づいて当該姿勢の推定が行われる。これにより、精度の低いＦＬＯＡＴ解の観測ベクトルＷｉがＵＡＶ１の姿勢の推定に用いられなくなるため、姿勢の推定の精度を向上させることができる。

（１２） １組の受信機１８のペアにおいて、互いに逆を向く２つの観測ベクトルＷｉでは、整数アンビギュイティの解の結果（整数値又は非整数値）が一致しない可能性がある。従って、１組の受信機１８のペアについて２つの観測ベクトルＷｉを算出することにより、ＦＩＸ解の観測ベクトルを取得可能な機会（ＦＩＸ率）が多くなる。

（１３） ６つの受信機１８における衛星７からの信号の受信と同期して、基準点から対象物までの距離を測定する測距装置２０がＵＡＶ１の基準点に配置されているため、ＵＡＶ１の位置及び姿勢を推定した結果と、測距装置２０における距離の測定結果とに基づいて、対象物の精密な３次元データを得ることができる。

以上、本実施形態について説明したが、本開示は上述した実施形態に限定されるものではなく、種々のバリエーションを含んでいる。

上述した実施形態においてＵＡＶ１（情報収集装置１０）に設置される受信機１８の個数（６個）は一例であり、姿勢の推定が可能となる３以上の個数であればよい。

また、上述した実施形態では、情報収集装置１０がＵＡＶ１に搭載される例を挙げているが、本開示における移動体はＵＡＶに限定されるものではなく、例えば地上を走行する車両や、海上を航行する船舶でもよい。また、移動体は無人機に限定されるものではなく、人を乗せて移動する乗り物でもよい。

上述した実施形態では、上空を飛行するＵＡＶ１がレーザースキャナ等の測距装置２０を備えており、測距装置２０の測定結果と位置及び姿勢の推定結果とを利用して３次元の地図を作成する例を挙げたが、本開示はこの例に限定されない。本開示の他の例では、測距装置２０の代わりに地表を撮影するカメラを搭載してもよい。また、移動体の位置及び姿勢の推定結果は測量以外の種々の測定にも利用可能であり、測定以外の用途（例えば、移動体の位置及び姿勢を自動的に記録して管理する用途、方位を精密に推定する用途など）にも利用可能である。

１…ＵＡＶ、１０…情報収集装置、１１…フレーム、１２…本体部、１７−１〜１７−６…腕部、１８−１〜１８−６，１８Ａ…受信機、１９−１〜１９−６，１９Ａ…アンテナ、２０…測距装置、２１…制御装置、２２…処理部、２３…記憶部、２４…ドローン、２５…本体部、２６−１〜２６−６…プロペラ、２７−１〜２７−６…腕部、３…地上基準局、５…情報処理装置、５１…インターフェース部、５２…表示部、５３…処理部、５３１…姿勢推定部、５３２…位置推定部、５３３…３次元地図作成部、５４…記憶部、５４１…プログラム、７…衛星、９…地表、Ｖ，ＶＡ１〜ＶＡ１５，ＶＢ１〜ＶＢ１５…基準ベクトル、Ｗ，ＷＡ１〜ＷＡ１５，ＷＢ１〜ＷＢ１５…観測ベクトル、ＰＳ…基準位置、ＰＴ…目標位置、ＰＥ…推定受信位置

本開示の第４の態様は、移動体の位置及び姿勢を推定する方法に関する。この方法は、移動体に設置されたＮ個（Ｎは３以上の整数を示す。）の受信機の各々において、複数の衛星から受信した信号に基づいて、複数の衛星から受信機までの距離に関する情報を含んだ観測データを生成することと、観測データと、複数の衛星の位置に関する位置データとに基づいて、移動体の姿勢を推定することと、観測データと位置データとに基づいて、移動体の位置を推定することとを有する。移動体の位置を推定することは、位置データ及び観測データに基づいて、各受信機が衛星からの信号を受信する推定受信位置を算出することと、各受信機の受信位置と各受信機の推定受信位置とのずれに関する判定基準に基づいて、各受信機の推定受信位置が適切か否か判定することと、当該判定において適切と判定した受信機の推定受信位置と、推定された移動体の姿勢とに基づいて、移動体における基準点の推定位置を算出することとを含む。

Claims (15)

1. 移動体の位置及び姿勢を推定する装置であって、
   前記移動体に設置されたＮ個（Ｎは３以上の整数を示す。）の受信機の各々において複数の衛星から受信した信号に基づいて生成された観測データと、前記複数の衛星の位置に関する位置データとに基づいて、前記移動体の姿勢を推定する姿勢推定部と、
   前記観測データと前記位置データとに基づいて、前記移動体の位置を推定する位置推定部とを有し、
   前記観測データは、前記複数の衛星と前記受信機において前記衛星からの信号を受信する受信位置との距離に関する情報を含んでおり、
   前記位置推定部は、
   前記位置データ及び前記観測データに基づいて、１以上の前記受信機が前記衛星からの信号を受信する推定受信位置を算出し、
   前記姿勢推定部において推定された前記移動体の姿勢と前記推定受信位置とに基づいて、前記移動体における基準点の推定位置を算出する、
   移動体の位置及び姿勢を推定する装置。
2. 前記位置推定部は、
   前記位置データ及び前記観測データに基づいて、各前記受信機の前記推定受信位置を算出し、
   各前記受信機の前記受信位置と各前記受信機の前記推定受信位置とのずれに関する判定基準に基づいて、各前記受信機の前記推定受信位置が適切か否か判定し、
   当該判定において適切と判定した前記受信機の前記推定受信位置と、前記姿勢推定部において推定された前記移動体の姿勢とに基づいて、前記基準点の推定位置を算出する、
   請求項１に記載の移動体の位置及び姿勢を推定する装置。
3. 前記位置推定部は、各前記受信機の前記推定受信位置について適否を判定する場合、
   １つの前記受信機の前記推定受信位置を基準位置として他の前記受信機の前記推定受信位置がそれぞれ適切か否か判定する判定処理を、各前記受信機について行い、
   各前記判定処理では、前記姿勢推定部において推定された前記移動体の姿勢と前記基準位置とに基づいて、前記受信機の前記受信位置となるべき目標位置を他の前記受信機の各々について算出し、前記目標位置と前記推定受信位置との距離が所定の範囲内に含まれるか否かを他の前記受信機の各々について判定し、
   各前記受信機について前記判定処理を行った結果に基づいて、各前記受信機の前記推定受信位置が適切か否か判定する、
   請求項２に記載の移動体の位置及び姿勢を推定する装置。
4. 前記位置推定部は、２以上の前記受信機の推定受信位置が適切であると判定した場合、前記２以上の受信機の推定受信位置について算出される２以上の前記基準点の推定位置を平均化する、
   請求項２又は３に記載の移動体の位置及び姿勢を推定する装置。
5. 前記位置推定部は、
   前記受信機の前記推定受信位置を算出する場合、前記衛星から伝搬する信号の搬送波位相に関する整数アンビギュイティが整数値として解かれた高精度の前記推定受信位置、又は、前記整数アンビギュイティが非整数値として解かれた低精度の前記推定受信位置を算出し、
   前記基準点の推定位置を算出する場合、当該算出に使用する前記推定受信位置から前記低精度の推定受信位置を除外する、
   請求項２〜４の何れか一項に記載の移動体の位置及び姿勢を推定する装置。
6. 前記位置推定部は、
   各前記衛星からの受信信号の信号対雑音比に関する情報を含んだ前記観測データに基づいて、同一の前記衛星からの同一時刻の受信信号に関する前記信号対雑音比が前記Ｎ個の受信機においてばらつく度合いを表す評価値を、前記複数の衛星についてそれぞれ算出し、
   各前記衛星について算出した前記評価値に基づいて、各前記衛星からの受信信号が正常か否かを判定し、
   前記推定受信位置を算出する場合、正常であると判定された前記衛星からの受信信号に基づく前記観測データを使用する、
   請求項１〜５の何れか一項に記載の移動体の位置及び姿勢を推定する装置。
7. ２つの前記受信機の前記受信位置により規定されるベクトルを基線ベクトルと呼び、
   前記移動体の姿勢が所定の基準姿勢の場合における前記基線ベクトルを基準ベクトルと呼ぶ場合に、
   前記姿勢推定部は、
   前記観測データ及び前記位置データに基づいて、２組以上の前記受信機のペアにおける２以上の前記基線ベクトルをそれぞれ観測ベクトルとして算出し、
   算出した前記２以上の前記観測ベクトルと、当該２以上の観測ベクトルに対応する２以上の前記基準ベクトルとに基づいて、前記移動体の姿勢を推定する、
   請求項１〜６の何れか一項に記載の移動体の位置及び姿勢を推定する装置。
8. 前記姿勢推定部は、
   前記Ｎ個の受信機の少なくとも一部の前記受信機のペアについて前記観測ベクトルを算出し、
   算出した前記観測ベクトルの各々について、前記観測ベクトルの長さと当該観測ベクトルに対応する前記基準ベクトルの長さとの誤差が所定の範囲に含まれるか否か判定し、
   前記移動体の姿勢を推定する場合、前記誤差が前記所定の範囲に含まれると判定した２以上の前記観測ベクトルと、当該２以上の観測ベクトルに対応する２以上の前記基準ベクトルとに基づいて当該姿勢を推定する、
   請求項７に記載の移動体の位置及び姿勢を推定する装置。
9. 前記姿勢推定部は、２以上の前記基準ベクトルと２以上の前記観測ベクトルとに基づいて前記移動体の姿勢を推定する場合、
   一対一に対応する前記２以上の基準ベクトルと前記２以上の観測ベクトルとの間の変換を規定する変換行列を、目的関数が最小となるように算出し、
   前記目的関数は、対応関係を持った前記基準ベクトル及び前記観測ベクトルのペア毎に得られるベクトル誤差量を、全ての前記ペアについて足し合わせた和に応じた関数であり、
   前記ベクトル誤差量は、前記ペアにおける一方のベクトルを前記変換行列により変換して得られるベクトルと他方のベクトルとの差に応じた値を持つ、
   請求項８に記載の移動体の位置及び姿勢を推定する装置。
10. 前記姿勢推定部は、
    ２つの前記受信機の前記受信位置に対応する前記観測ベクトルを算出する場合、前記衛星から伝搬する信号の搬送波位相に関する整数アンビギュイティが整数値として解かれた高精度の前記観測ベクトル、又は、前記整数アンビギュイティが非整数値として解かれた低精度の前記観測ベクトルを算出し、
    前記移動体の姿勢を推定する場合、２以上の前記高精度の観測ベクトルと、当該２以上の高精度の観測ベクトルに対応する２以上の前記基準ベクトルとに基づいて当該姿勢を推定する、
    請求項７〜９の何れか一項に記載の移動体の位置及び姿勢を推定する装置。
11. 前記姿勢推定部は、１組の前記受信機のペアについて、互いに逆方向を向く２つの前記観測ベクトルを算出する、
    請求項１０に記載の移動体の位置及び姿勢を推定する装置。
12. コンピュータを、請求項１〜１１の何れか一項に記載の装置における前記位置推定部及び前記姿勢推定部として機能させるためのプログラム。
13. 移動体の位置及び姿勢を推定するシステムであって、
    前記移動体に設置され、複数の衛星から放送される信号をそれぞれ受信し、受信した前記信号に基づいて前記複数の衛星からの距離に関する情報を含んだ観測データをそれぞれ生成する３以上の受信機と、
    請求項１〜１１の何れか一項に記載の装置とを有する、
    移動体の位置及び姿勢を推定するシステム。
14. 前記移動体の前記基準点に位置しており、前記Ｎ個の受信機における前記衛星からの信号の受信と同期して、前記基準点から対象物までの距離を測定する測距装置を有する、
    請求項１３に記載の移動体の位置及び姿勢を推定するシステム。
15. 移動体の位置及び姿勢を推定する方法であって、
    前記移動体に設置されたＮ個（Ｎは３以上の整数を示す。）の受信機の各々において、複数の衛星から受信した信号に基づいて、前記複数の衛星から前記受信機までの距離に関する情報を含んだ観測データを生成することと、
    前記観測データと、前記複数の衛星の位置に関する位置データとに基づいて、前記移動体の姿勢を推定することと、
    前記観測データと前記位置データとに基づいて、前記移動体の位置を推定することとを有し、
    前記移動体の位置を推定することは、
    前記位置データ及び前記観測データに基づいて、１以上の前記受信機が前記衛星からの信号を受信する推定受信位置を算出することと、
    推定された前記移動体の姿勢と前記推定受信位置とに基づいて、前記移動体における基準点の推定位置を算出することとを含む、
    移動体の位置及び姿勢を推定する方法。
    

Images