JP6430073B2 - 姿勢推定装置、姿勢推定方法及び観測システム - Google Patents

Description

本発明は、観測機器を搭載した航空機などの飛行体の飛行姿勢を推定する技術に関する。

上空から地表面または構造物などの地物（地上にある物）の測量または調査を行うために、撮像カメラ及びレーザ測距計などの観測機器を搭載した飛行体を使用することがある。この種の飛行体を使用した測量システムは、たとえば、特許文献１（特開２０１４−１４５７６２号公報）に開示されている。

航空レーザ測量の場合には、飛行体の現在位置を取得する技術と、飛行体の飛行姿勢（たとえば、ローリング方向、ピッチング方向、ヨーイング方向の角度）を検出する技術とが必要である。飛行体の現在位置は、たとえば、ＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ：全地球的航法衛星システム）装置を用いて取得することができる。ＧＮＳＳ装置は、飛行体に搭載された状態でＧＮＳＳ衛星から電波を一定周期ごとに受信し、その受信電波を解析して当該飛行体の現在位置を検出することができる。また、飛行体の飛行姿勢は、慣性計測装置（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ，ＩＭＵ）を用いて検出することができる。しかしながら、ＩＭＵは、非常に高価であるうえ、その持ち運びに一定の制限があり、その取り扱いに注意が必要であることから、ＩＭＵを搭載可能な飛行体の種類が限定されてしまうという課題がある。高い測定精度を維持するように構成されたＩＭＵの場合、その重量が比較的重い（たとえば４ｋｇ〜２０ｋｇ程度）ことから、積載可能重量（たとえば積載可能重量２．５ｋｇ〜３．５ｋｇ程度）が制限された飛行体（たとえば、無線遠隔操縦される小型無人ヘリコプター）にそのＩＭＵを搭載することが難しい。

特許文献１に開示されている測量システムは、ＩＭＵを使用せずに、２以上の異なる位置から撮像された複数の撮像画像を用いたバンドル計算を実行することによって飛行姿勢を推定する。この測量システムにおける測量用カメラは、スタビライザーと呼ばれる姿勢安定化機構によって支持されるので、飛行中の飛行体の姿勢に依らずに撮影方向を鉛直下向きに保つことができる。

特開２０１４−１４５７６２号公報（たとえば、図１，図３及び図２０、並びに段落０１５２〜０１６０）

特許文献１に開示されている測量システムでは、測量用カメラの撮影方向を常に鉛直下向きに維持する姿勢安定化機構を飛行体に搭載することが必要であるため、測量システムの構成が複雑化するという課題がある。また、撮像画像のみを用いて飛行体の姿勢が推定されるので、その推定精度には限界がある。

本発明の一態様による姿勢推定装置は、測地座標系における飛行体の位置を示す測位データを出力する測位部と、前記飛行体から地上の測距点までの距離と当該測距点に対する角度方向とを計測して測距データを出力する測距部と、前記飛行体に搭載された状態で複数の撮像位置から当該測距点を撮像して撮像画像群を出力する撮像部と、重力方向を基準とした当該飛行体の姿勢を検出して実姿勢データを出力する姿勢センサとを含む観測装置と連携して動作する姿勢推定装置であって、前記測位データ、前記測距データ、前記実姿勢データ及び前記撮像画像群を前記観測装置から取得するデータ取得部と、前記測位データ、前記測距データ、前記実姿勢データ及び前記飛行体の姿勢を示す未知のパラメータの推定値を用いて、前記測地座標系における当該測距点の位置を示す測地座標を算出する測距点座標算出部と、前記測地座標系から、前記撮像画像群の中から選択された第１の撮像画像に対応する第１の画像座標系への投影変換を前記測地座標に施して第１の投影点座標を算出し、前記測地座標系から、前記撮像画像群の中から選択された第２の撮像画像に対応する第２の画像座標系への投影変換を前記測地座標に施して第２の投影点座標を算出する画像座標算出部と、前記第２の撮像画像から前記第２の投影点座標で指定される局所領域画像を抽出し、当該局所領域画像と前記第１の撮像画像との間の画像マッチングを実行して前記第１の撮像画像に現れる当該測距点の画像座標を検出する画像マッチング部と、前記第１の投影点座標と前記画像座標との間の差分の大きさが小さくなるように前記推定値を補正する姿勢推定部とを備えることを特徴とする。

本発明の一態様による姿勢推定装置は、測地座標系における飛行体の位置を示す測位データを出力する測位部と、前記飛行体から地上の測距点までの距離と当該測距点に対する角度方向とを計測して測距データを出力する測距部と、前記飛行体に搭載された状態で複数の撮像位置から当該測距点を撮像して撮像画像群を出力する撮像部と、重力方向を基準とした当該飛行体の姿勢を検出して実姿勢データを出力する姿勢センサとを含む観測装置と連携して動作する姿勢推定装置であって、前記測位データ、前記測距データ、前記実姿勢データ及び前記撮像画像群を前記観測装置から取得するデータ取得部と、前記測位データ、前記測距データ、前記実姿勢データ及び前記飛行体の姿勢を示す未知のパラメータの推定値を用いて、前記測地座標系における当該測距点の位置を示す測地座標を算出する測距点座標算出部と、前記測地座標系から画像座標系への投影変換を前記測地座標に施して投影点座標を算出する画像座標算出部と、前記撮像画像群から選択された第１の撮像画像と第２の撮像画像との間の画像マッチングを実行して前記第１の撮像画像に現れる当該測距点の画像座標を検出する画像マッチング部と、前記投影点座標と前記画像座標との間の差分の大きさが小さくなるように前記推定値を補正する姿勢推定部とを備えることを特徴とする。

本発明の他の態様による観測システムは、前記観測装置と前記姿勢推定装置とを備えることを特徴とする。

本発明によれば、簡単な構成で飛行体の姿勢を高精度に推定することができる。

本発明に係る実施の形態１である観測システムの概略構成を示すブロック図である。 実施の形態１の飛行体の一例を示す概略斜視図である。 実施の形態１の飛行ルート設定装置の概略構成を示すブロック図である。 図４Ａ及び図４Ｂは、実施の形態１の飛行ルート設定装置のハードウェア構成例を概略的に示すブロック図である。 実施の形態１に係る飛行ルート設定処理の手順の一例を示すフローチャートである。 実施の形態１に係るルートデータの一例をテーブル形式で示す図である。 ルートデータで定められた飛行ルートを説明するための図である。 飛行ルートの他の例を説明するための図である。 実施の形態１の観測装置におけるレーザ測距部、右カメラ及び左カメラの構成例の斜視図である。 図１０Ａは、図９に示した観測装置の正面図であり、図１０Ｂは、図９に示した観測装置の上面図であり、図１０Ｃは、図９に示した観測装置の右側面図である。 時刻ｔ＝０，１，２，３におけるレーザ測距部の照射基準点ＲＰの３次元座標の例をテーブル形式で示す図である。 図１２Ａ〜図１２Ｃは、時刻ｔ＝０におけるレーザ測距部、右カメラ及び左カメラの位置座標を示すグラフである。 図１３Ａ〜図１３Ｃは、時刻ｔ＝１におけるレーザ測距部、右カメラ及び左カメラの位置座標を示すグラフである。 図１４Ａ〜図１４Ｃは、時刻ｔ＝２におけるレーザ測距部、右カメラ及び左カメラの位置座標を示すグラフである。 図１５Ａ〜図１５Ｃは、時刻ｔ＝３におけるレーザ測距部、右カメラ及び左カメラの位置座標を示すグラフである。 図１６Ａは、レーザ測距部によって得られた照射角度及び測定距離の数値例をテーブル形式で示す図であり、図１６Ｂは、図１６Ａに示した照射角度と測定距離との間の関係を示すグラフである。 時刻ｔ＝０，１，２における右カメラ画像及び左カメラ画像の例を示す図である。 実施の形態１の姿勢推定装置の概略構成を示すブロック図である。 図１９Ａ及び図１９Ｂは、実施の形態１の姿勢推定装置のハードウェア構成例を概略的に示すブロック図である。 実施の形態１に係る姿勢推定処理の手順の一例を概略的に示すフローチャートである。 図２１Ａは、時刻ｔ＝ｔ_０，ｔ_１における実施の形態１の観測装置の配置を概略的に示す図であり、図２１Ｂは、時刻ｔ＝ｔ_０，ｔ_１にそれぞれ取得された撮像画像の例を示す図であり、図２１Ｃは、時刻ｔ＝ｔ_０，ｔ_１における観測装置と測距点との間の位置関係を概略的に示す図である。 図２２Ａは、時刻ｔ＝ｔ_０，ｔ_１にそれぞれ取得された撮像画像の他の例を示す図であり、図２２Ｂは、時刻ｔ＝ｔ_０，ｔ_１における観測装置と測距点との間の位置関係を概略的に示す図である。 図２３Ａ及び図２３Ｂは、実施の形態１に係る観測結果及び計算結果の数値例をテーブル形式で示す図である。 実施の形態１に係る他の観測結果及び計算結果の数値例をテーブル形式で示す図である。 画像座標の数値例をテーブル形式で示す図である。 誤差ベクトルの計算例を示す図である。 計画行列の一例を示す図である。 補正ベクトルの計算例を示す図である。 時刻ｔ＝０．００〜３．００における姿勢角の計算結果をテーブル形式で示す図である。 本発明に係る実施の形態２である観測システムの概略構成を示すブロック図である。 実施の形態２の姿勢推定装置の概略構成を示すブロック図である。 実施の形態２の飛行ルート設定装置の概略構成を示すブロック図である。

以下、図面を参照しつつ、本発明に係る種々の実施の形態について詳細に説明する。なお、図面全体において同一符号を付された構成要素は、同一構成及び同一機能を有するものとする。

実施の形態１．
図１は、本発明に係る実施の形態１である観測システム１の概略構成を示すブロック図である。図１に示されるようにこの観測システム１は、飛行体である航空機２に搭載された状態で使用される観測装置１０と、この観測装置１０で得られた観測データが記憶されるメモリ１７と、当該観測データに基づいて航空機２の飛行姿勢（飛行中の姿勢）を推定する姿勢推定装置３０と、航空機２の飛行ルートを設定する飛行ルート設定装置４０とを備えて構成されている。或る瞬間の航空機２の姿勢状態は、重力方向を基準とした３つの回転角の組み合わせによって特定可能である。たとえば、航空機２のローリング方向、ピッチング方向及びヨーイング方向の姿勢角であるロール角、ピッチ角及びヨー角という３つの回転角の組み合わせによって航空機２の姿勢状態を特定することができる。

なお、本実施の形態の姿勢推定装置３０及び飛行ルート設定装置４０は、航空機２とは物理的に分離して別に設けられているが、これに限定されるものではない。姿勢推定装置３０及び飛行ルート設定装置４０の一方または双方が航空機２に搭載されていてもよい。

航空機２は、当該航空機２の推力（推進力）を発生させる推力発生機構２２と、メモリ１７と、このメモリ１７に記憶されているデータを用いて推力発生機構２２の動作を制御する飛行制御装置２１とを備えている。航空機２は、当該航空機２に搭乗したパイロットによって操縦される有人航空機でもよいし、あるいは、無人航空機（Ｕｎｍａｎｎｅｄ Ａｅｒｉａｌ Ｖｅｈｉｃｌｅ：ＵＡＶ）であってもよい。推力発生機構２２は、固定翼機または回転翼機を構成するためのプロペラ、ロータまたはジェットエンジンなどの推力発生源を有する。飛行制御装置２１は、推力発生機構２２を制御することにより機体の位置及び姿勢を自動的にコントロールすることができ、メモリ１７に記憶されているルートデータ（飛行ルートを定めるデータ）に従って推力発生機構２２を制御することにより機体の速度、進路及び姿勢を制御することができる。ルートデータは、飛行ルート設定装置４０によって設定される。飛行ルート設定装置４０の構成については後述する。

メモリ１７としては、たとえば、ＳＤ（Ｓｅｃｕｒｅ Ｄｉｇｉｔａｌ）メモリカードなどの携帯型の不揮発性メモリである可搬記録媒体が使用可能である。メモリ１７は、ルートデータだけでなく、観測装置１０で得られる観測データ（撮像画像群、測距データ、測位データ及び実姿勢データ）、及び観測装置１０におけるデータ取得周期を記憶するために使用される。

図２は、航空機２の構成例を示す斜視図である。図２に示される航空機２は、機体２０と、この機体２０の周縁部に取り付けられた複数の推力発生器２２Ａ〜２２Ｆとを有する無人航空機（ＵＡＶ）である。機体２０には、図示されていないが、図１に示した観測装置１０、メモリ１７及び飛行制御装置２１が取り付けられている。推力発生器２２Ａ〜２２Ｆの各々は回転翼（ロータ）で構成されている。飛行制御装置２１は、メモリ１７から読み出されたルートデータで定められた飛行ルートに沿って航空機２を飛行させることができる。すなわち、飛行制御装置２１は、そのルートデータに従って推力発生器２２Ａ〜２２Ｆの動作を個別に制御することにより、航空機２を図２のＺ軸方向に沿って上昇または下降させたり、Ｘ−Ｙ面における任意の方向に沿って、指定された飛行速度で水平移動させたり、あるいは、指定された時間だけ空中で静止（ホバリング）させたりすることが可能である。観測装置１０は、鉛直下方（Ｚ軸正方向）を観測することができるように機体２０の下部に取り付けられている。

観測装置１０は、図１に示されるように、地上を撮像する撮像部１１と、航空機２から地上の単数または複数の測距点までの距離及びこれらの角度方向を測定するレーザ測距部１３と、複数のＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）衛星を利用して観測装置１０の現在位置を測位するＧＮＳＳ測位部１４と、重力方向を基準とした航空機２の姿勢を検出して実姿勢データを出力する姿勢センサ１５と、これら撮像部１１，レーザ測距部１３、ＧＮＳＳ測位部１４及び姿勢センサ１５のそれぞれの動作を制御する観測制御部１６とを備えて構成されている。

撮像部１１は、第１撮像部である左カメラ１２Ｌと、第２撮像部である右カメラ１２Ｒとを有する。左カメラ１２Ｌ及び右カメラ１２Ｒの各々は、たとえば、光学レンズと固体撮像素子とを有するディジタルカメラで構成されていればよい。固体撮像素子としては、ＣＣＤ（Ｃｈａｒｇｅ−Ｃｏｕｐｌｅｄ Ｄｅｖｉｃｅ）イメージセンサまたはＣＭＯＳ（Ｃｏｍｐｌｅｍｅｎｔａｒｙ Ｍｅｔａｌ Ｏｘｉｄｅ Ｓｅｍｉｃｏｎｄｕｃｔｏｒ）イメージセンサが挙げられる。右カメラ１２Ｒ及び左カメラ１２Ｌの配置例については後述する。観測制御部１６は、撮像部１１で撮像された各撮像画像を観測日時（撮像日時）と関連付けてメモリ１７に格納する。

観測制御部１６は、左カメラ１２Ｌ及び右カメラ１２Ｒを制御して、メモリ１７に記憶されている予め定められたデータ取得周期すなわち撮像周期ごとに（たとえば、１秒ごとに）地表面の撮像を実行させることができる。撮像周期については、航空機２の実際の飛行前にユーザが飛行ルート設定装置４０を用いてその撮像周期の値を設定することができる。

レーザ測距（Ｌａｓｅｒ Ｒａｎｇｉｎｇ）部１３は、照射角度を変えながらレーザ光を測量対象である地表面に向けて照射し、当該地表面の単数または複数の測距点から反射レーザ光をそれぞれ受光する。レーザ測距部１３は、その受光結果に基づいて、観測装置１０の基準点（具体的には、レーザ測距部１３の照射基準点）から各測距点までの距離Ｌを計測し、且つ当該各測距点に対する角度方向を示す照射角度θを計測することができる。レーザ測距部１３は、たとえば、ＴＯＦ（Ｔｉｍｅ ｏｆ Ｆｌｉｇｈｔ）方式に従って、レーザ光を各測距点に向けて出射した時点から反射レーザ光が戻ってきた時点までの伝播時間を計測し、その計測結果に基づいて各測距点までの距離Ｌ及び照射角度θを計測すればよい。照射角度θは、予め定められた特定方向からの角度が計測される。観測制御部１６は、各測距点までの距離Ｌ及び照射角度θを示す測距データを観測日時（測距日時）と関連付けてメモリ１７に格納する。

ＧＮＳＳ測位部１４は、ＧＮＳＳ（全地球航法衛星システム）による航法信号を複数のＧＮＳＳ衛星から受信し、当該航法信号に基づいて測地座標系における観測装置１０の現在位置（具体的には、レーザ測距部１３の照射基準点の現在位置）を測位する。その測位結果を示す測位データは観測制御部１６に出力される。測地座標系には、たとえば、ＷＧＳ（Ｗｏｒｌｄ Ｇｅｏｄｅｔｉｃ Ｓｙｓｔｅｍ）−８４座標系あるいはＩＴＲＦ（Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｔｅｒｒｅｓｔｒｉａｌ Ｒｅｆｅｒｅｎｃｅ Ｆｒａｍｅ）座標系が使用可能であるが、これに限定されるものではない。ＧＮＳＳとしては、たとえば、米国により運用されるＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）、ロシア連邦により運用されるＧＬＯＮＡＳＳ（ＧＬＯｂａｌ ＮＡｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）、欧州連合により運用されるＧａｌｉｌｅｏシステム、または日本により運用されるＱＺＳＳ（Ｑｕａｓｉ−Ｚｅｎｉｔｈ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ：準天頂衛星システム）を利用することができる。観測制御部１６は、その測位データを観測時刻（測位時刻）と関連付けてメモリ１７に格納する。

観測制御部１６は、ＧＮＳＳ測位部１４を制御して、メモリ１７に記憶されている予め定められたデータ取得周期すなわち測位周期ごとに（たとえば、１秒ごとに）測位を実行させることができる。測位周期については、航空機２の実際の飛行前にユーザが飛行ルート設定装置４０を用いて測位周期の値を設定することができる。観測制御部１６は、上記の撮像周期と同期した測位周期でＧＮＳＳ測位部１４に測位を実行させてもよい。

なお、ＧＮＳＳ測位部１４の測位基準点とレーザ測距部１３の照射基準点との間の差は、ＧＮＳＳ測位部１４の測定精度に対して許容範囲内にあるものとする。すなわち、ＧＮＳＳ測位部１４の測位基準点は、レーザ測距部１３の照射基準点と同一の位置に配置されているとみなされる。本実施の形態では、レーザ測距部１３の照射基準点の位置は、観測装置１０の位置とみなされている。

姿勢センサ１５は、静加速度である重力加速度の方向すなわち重力方向を検出し、当該重力方向と当該姿勢センサ１５の基準方向との間の角度情報を実姿勢データとして検出する。具体的には、姿勢センサ１５は、航空機２の進行方向（すなわちＸ軸方向）の周りの回転角であるロール角と、当該進行方向及び重力方向（すなわちＺ軸方向）の双方に垂直なＹ軸方向の周りの回転角であるピッチ角との組み合わせを実姿勢データとして検出することができる。このような姿勢センサ１５は、たとえば、単体で２つの回転角を検出可能な２軸式重力方向センサでもよいし、あるいは、２個の１軸式重力方向センサの組み合わせであってもよい。ここで、２軸式加速度センサの２本の基準軸、または２個の１軸式加速度センサの組み合わせの２本の基準軸は、互いに９０度で交差するように配置される。また、２軸式重力方向センサまたは１軸式重力方向センサは、たとえば、ピエゾ抵抗型または静電容量型の加速度センサで構成可能であり、ＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍｓ）デバイスとして構成されてもよい。

本実施の形態の航空機２は、予め設定されたルートデータで定められる周期ごとにホバリングする。このホバリング時に、姿勢センサ１５は、航空機２のロール角及びピッチ角の組み合わせを実姿勢データとして検出する。観測制御部１６は、その実姿勢データを観測日時（検出日時）と関連付けてメモリ１７に格納する。航空機２が水平状態から傾いたとき、Ｘ軸方向周りのロール角、Ｙ軸方向周りのピッチ角及びＺ軸方向周りのヨー角という３種類の姿勢角のうちの少なくとも１つの姿勢角が非零の値となる。或る時刻における航空機２の完全な姿勢状態を特定するには、ロール角及びピッチ角だけでなく、ヨー角を取得する必要がある。このヨー角は、後述するように姿勢推定装置３０によって推定される。

次に、飛行ルート設定装置４０の構成について説明する。図３は、本発明に係る実施の形態１の飛行ルート設定装置４０の概略構成を示すブロック図である。

図３に示されるように飛行ルート設定装置４０は、入出力インタフェース部（入出力Ｉ／Ｆ部）４１、入力データ処理部４２、ルートデータ生成部４３及びメモリインタフェース部（メモリＩ／Ｆ部）４４を有する。入出力Ｉ／Ｆ部４１は、操作入力部４６及び表示部４７と接続されている。メモリＩ／Ｆ部４４には、可搬記録媒体であるメモリ１７が着脱自在に装着される。

操作入力部４６は、ユーザによる操作入力を検出し、当該検出された操作入力を入出力Ｉ／Ｆ部４１に出力する。操作入力部４６は、ユーザによる操作入力を受け付ける入力ボタンまたは入力キーを有する入力デバイスである。たとえば、キーボード、あるいは、マウスもしくはタッチパネルなどのポインティングデバイスを操作入力部４６として使用可能である。入出力Ｉ／Ｆ部４１は、その操作入力を、航空機２の飛行ルートまたは上述のデータ取得周期を指示する設定データとして入力データ処理部４２に供給することができる。また、入力データ処理部４２は、設定用画面を表示部４７に表示させることもできる。ユーザは、その設定用画面を視認しつつ操作入力部４６を操作して設定データを入力することが可能である。表示部４７は、たとえば、液晶表示パネルまたは有機ＥＬ表示パネルなどの画像表示デバイスで構成されればよい。

入力データ処理部４２は、飛行ルートまたはデータ取得周期を指示する設定データを受け付けると、当該設定データを解釈してその解釈結果をルートデータ生成部４３に出力する。ルートデータ生成部４３は、その解釈結果に基づき、航空機２の飛行ルートを定めるルートデータを生成し、またはデータ取得周期を定める周期データを生成する。メモリＩ／Ｆ部４４は、当該ルートデータまたは当該周期データをメモリ１７に格納する。メモリ１７にルートデータまたは周期データが格納された後は、ユーザは、このメモリ１７を飛行ルート設定装置４０から取り外して当該航空機２に装着することができる。

上記飛行ルート設定装置４０のハードウェア構成は、たとえば、ワークステーションまたはメインフレームなどのＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）内蔵のコンピュータで実現可能である。あるいは、飛行ルート設定装置４０のハードウェア構成は、ＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）もしくはＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）またはこれらの組み合わせなどのＬＳＩ（Ｌａｒｇｅ Ｓｃａｌｅ Ｉｎｔｅｇｒａｔｅｄ ｃｉｒｃｕｉｔ）により実現されてもよい。

図４Ａは、飛行ルート設定装置４０のハードウェア構成例である情報処理装置４０Ｓの概略構成を示すブロック図である。図４Ａに示されるように情報処理装置４０Ｓは、ＣＰＵ６０ｃを含むプロセッサ６０と、メモリインタフェース回路（メモリＩ／Ｆ回路）６１と、プロセッサ６０の作業用メモリとして使用されるＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）６２と、ソフトウェア及びファームウェアの一方または双方のコンピュータ・プログラムが格納される不揮発性メモリ６３と、操作入力部４６に接続される入力インタフェース回路（入力Ｉ／Ｆ回路）６４Ａと、表示部４７に接続される表示インタフェース回路（表示Ｉ／Ｆ回路）６４Ｂとを備えて構成されている。この情報処理装置４０Ｓの構成要素６０〜６３，６４Ａ，６４Ｂは、バス回路などの信号路６５を介して相互に接続されている。メモリＩ／Ｆ回路６１は、図３のメモリＩ／Ｆ部４４の機能を実現する回路であり、入力Ｉ／Ｆ回路６４Ａ及び表示Ｉ／Ｆ回路６４Ｂは、図３の入出力Ｉ／Ｆ部４１の機能を実現する回路である。

プロセッサ６０は、不揮発性メモリ６３から、ソフトウェアもしくはファームウェアまたはこれらの組み合わせであるコンピュータ・プログラムを読み出して実行することにより、図３の入力データ処理部４２及びルートデータ生成部４３の機能を実現することができる。不揮発性メモリ６３としては、たとえば、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ ＲＯＭ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ ＥＰＲＯＭ）、磁気ディスクまたは光ディスクを使用することができるが、これらに限定されるものではない。

図４Ｂは、飛行ルート設定装置４０の他のハードウェア構成例である情報処理装置４０Ｈの概略構成を示すブロック図である。図４Ｂに示されるように情報処理装置４０Ｈは、ＤＳＰ、ＡＳＩＣまたはＦＰＧＡなどの専用ハードウェアの処理回路６６と、メモリインタフェース回路（メモリＩ／Ｆ回路）６７と、操作入力部４６に接続される入力インタフェース回路（入力Ｉ／Ｆ回路）６８Ａと、表示部４７に接続される表示インタフェース回路（表示Ｉ／Ｆ回路）６８Ｂとを備えて構成されている。この情報処理装置４０Ｈの構成要素６６，６７，６８Ａ，６８Ｂは、バス回路などの信号路６９を介して相互に接続されている。メモリＩ／Ｆ回路６７は、図３のメモリＩ／Ｆ部４４の機能を実現する回路であり、入力Ｉ／Ｆ回路６８Ａ及び表示Ｉ／Ｆ回路６８Ｂは、図３の入出力Ｉ／Ｆ部４１の機能を実現する回路である。処理回路６６は、入力データ処理部４２及びルートデータ生成部４３の機能を実現する。

次に、図５を参照しつつ、上記飛行ルート設定装置４０の動作例を以下に説明する。図５は、実施の形態１に係る飛行ルート設定処理の手順の一例を概略的に示すフローチャートである。

図５を参照すると、入力データ処理部４２は、操作入力部４６から入出力Ｉ／Ｆ部４１を介して設定データが入力されるまで待機している（ステップＳＴ１のＮＯ）。設定データが入力されると（ステップＳＴ１のＹＥＳ）、入力データ処理部４２は、当該入力された設定データを解釈する（ステップＳＴ２）。当該解釈結果は、ルートデータ生成部４３に供給される。ユーザは、所定のフォーマットによる設定コマンドを設定データとして入力することができる。入力データ処理部４２は、設定終了コマンドが入力されていないと解釈した場合には（ステップＳＴ３のＮＯ）、「次の設定データを入力してください」などの、更なる入力を促すメッセージを表示部４７に表示させる（ステップＳＴ４）。その後、入力データ処理部４２は、ステップＳＴ１を実行する。

たとえば、日付「２０１５年１２月２４日」の時刻「２１時５６分３０秒」において緯度「３５度２１分１４．５７２秒」及び経度「１３９度３１分５２．６７６秒」で指定される地点の通過を指示する場合、ユーザは、以下の例１の通過コマンド（ダブルクォーテーションマークを除く。）を設定コマンドとして入力することができる。緯度及び経度の表記は、ＷＧＳ−８４座標系によるものである。
例１："35.354048,139.531299, 2015/12/24/21:56:30"

また、日付「２０１５年１２月２４日」の時刻「２１時５６分３０秒」において緯度「３５度２１分１４．５７２秒」及び経度「１３９度３１分５２．６７６秒」で指定される地点での「１０秒間」のホバリングを指示する場合、ユーザは、以下の例２のホバリングコマンド（ダブルクォーテーションマークを除く。）を設定コマンドとして入力することができる。
例２："35.354048,139.531299, 2015/12/24/21:56:30 10secHov"

また、右カメラ画像のデータ取得周期（取得間隔）を１秒間に設定する場合、ユーザは、以下の例３のデータ取得間隔設定コマンド（ダブルクォーテーションマークを除く。）を入力することができる。
例３："Set RightCamera Interval 1sec"

そして、ユーザは、以下の例４の設定終了コマンド（ダブルクォーテーションマークを除く。）を入力することができる。
例４："END"

入力データ処理部４２が設定終了コマンドが入力されたと解釈した場合（ステップＳＴ３のＹＥＳ）、ルートデータ生成部４３は、設定終了コマンド以外の入力済み設定コマンドに基づいてルートデータを生成し（ステップＳＴ５）、このルートデータをメモリＩ／Ｆ部４４を介してメモリ１７に格納する（ステップＳＴ６）。なお、データ取得間隔設定コマンドが入力された場合には、ステップＳＴ５で周期データが生成され、ステップＳＴ６でこの周期データがメモリ１７に格納される。以上で飛行ルート設定処理は完了する。

図６は、ルートデータの内容の一例をテーブル形式で示す図である。図６に示されるように、ルートデータは、１件当たり、「地点ＩＤ（地点識別子）」、「緯度」、「経度」、「日付」及び「時刻」からなるレコードの集合である。図７は、図６のテーブルに示した地点Ａ１から地点Ａ５までの飛行ルートを説明するための図である。図７の例では、航空機２は、２０１６年ｙｙ月ｚｚ日の１０時３０分３０秒に地点Ａ１の上空を出発する。その後、航空機２は、１０時３１分３０秒に地点Ａ２の上空に到着し、１０時３１分３０秒から１０時３２分００秒までの３０秒間だけホバリングする。次に、航空機２は、１０時３２分００秒に地点Ａ２の上空を出発する。その後、航空機２は、１０時３４分００秒に地点Ａ３の上空を通過し且つ旋回して地点Ａ４の上空に向けて飛行する。航空機２は、１０時３６分００秒に地点Ａ４の上空を通過し且つ旋回して地点Ａ５の上空に向けて飛行する。そして、航空機２は、１０時３８分００秒に地点Ａ５の上空に到着する。地点Ａ１〜Ａ５の各地点は、「緯度」及び「経度」で指定される点である。

また、ルートデータ生成部４３は、指定された飛行ルートに基づいて地点間の各区間における航空機２の飛行速度を自動で計算する。ルートデータ生成部４３は、各区間における当該飛行速度を指定するデータをルートデータの一部としてメモリ１７に格納することができる。

なお、図７の例では、航空機２の飛行経路は直線経路であるが、これに限定されるものではない。たとえば、ルートデータ生成部４３は、複数地点を結ぶ曲線経路を多項式近似で算出してもよい。曲線経路を設定することで航空機２の急旋回を防ぐことができるので、航空機２の姿勢制御にかかる負荷の軽減が可能となる。

また、ルートデータ生成部４３は、ユーザにより、複数の地点を示す座標データと計測開始時間とが入力されれば、当該座標データと当該計測開始時間とに基づいてルートデータを自動で生成することもできる。この場合、ルートデータ生成部４３は、最短時間で計測が完了し且つ高い計測精度が実現されるようにそのルートデータを生成することが望ましい。図８は、自動生成されたルートデータで定められる飛行ルートの一例を概略的に示す図である。このルートデータは、図面右下の計測開始地点ＳＰ、図面左下の通過地点、図面右上の通過地点及び図面左上の計測終了地点ＥＰという４つの地点を示す座標データと計測開始時間とが入力された場合に自動生成されるものである。

図８の例では、飛行ルートは、矢印付きの太線で示されている。図８に示されるように、航空機２は、計測開始地点ＳＰと図面左側（西側）の通過地点との間の第１の飛行ルートを東側から西側へ一軸方向に沿って飛行した後に、この第１の飛行ルートと並行して北側に隣接する第２の飛行ルートを西側から東側へ一軸方向に沿って飛行する。ここで、航空機２が第１の飛行ルートと第２の飛行ルートとをそれぞれ飛行する際の測距用レーザ光の走査範囲は、互いに異なるパターンのハッチングで示されている。図８に示されるように、航空機２が第１の飛行ルートを飛行する際の測距用レーザ光の走査範囲と、航空機２が第１の飛行ルートを飛行する際の測距用レーザ光の走査範囲とは、互いにオーバラップする。これにより、レーザ測距部１３は高い計測精度で測距データを生成することができる。

また、航空機２は、図８に示されるように、計測開始地点ＳＰに近い通過地点と図面上側（北側）の通過地点との間の第３の飛行ルートを南側から北側へ一軸方向に沿って飛行した後に、この第３の飛行ルートと並行し西側に隣接する第４の飛行ルートを北側から南側へ一軸方向に沿って飛行する。その後、航空機２は、第４の飛行ルートと並行し西側に隣接する第５の飛行ルートを南側から北側へ一軸方向に沿って飛行し、この第５の飛行ルートと並行し西側に隣接する第６の飛行ルートを北側から南側へ一軸方向に沿って飛行する。そして、航空機２は、第６の飛行ルートと並行し西側に隣接する第７の飛行ルートを南側から北側へ一軸方向に沿って計測終了地点ＥＰまで飛行する。このとき、航空機２が第３及び第４の飛行ルートを飛行する際の測距用レーザ光の走査範囲は、互いにオーバラップし、航空機２が第４及び第５の飛行ルートを飛行する際の測距用レーザ光の走査範囲も、互いにオーバラップし、航空機２が第５及び第６の飛行ルートを飛行する際の測距用レーザ光の走査範囲も、互いにオーバラップする。

このようにルートデータ生成部４３は、互いに隣接する飛行ルートにそれぞれ対応する測距用レーザ光の走査範囲が互いにオーバラップするようにルートデータを生成するので、レーザ測距部１３は、オーバラップする走査範囲における各測距点までの距離Ｌ及び角度方向θを複数回測定することができる。これにより、レーザ測距部１３は、当該測定結果に基づいて、たとえば当該測定結果を平均化することにより、誤差の小さな測距データを生成することが可能である。また、図８に示されるように、東西方向にオーバラップする走査範囲と南北方向にオーバラップする走査範囲とが生成されることから、東西方向の誤差の小さな測距データの生成が可能であり、南北方向の誤差の小さな測距データの生成も可能である。特に、東西方向及び南北方向の双方にオーバラップする走査範囲内の測距点については、東西方向及び南北方向の双方において誤差の小さな測距データを生成することができる。

次に、図１に示した観測装置１０の構成について説明する。図９は、観測装置１０のうちのレーザ測距部１３及び撮像部１１からなるユニットの構成例を概略的に示す斜視図である。

図９に示されるようにレーザ測距部１３は、照射基準点ＲＰを有し、鉛直下方（Ｚ軸正方向）の地上面に向けてレーザ光を出射する。本実施の形態のレーザ測距部１３は、航空機２の進行方向（すなわちＸ軸方向）とＺ軸方向との双方に垂直なＹ軸方向にレーザ光を走査させることができる。右カメラ１２Ｒ及び左カメラ１２Ｌは、照射基準点ＲＰに関して幾何学的に互いに対称な位置に配置され、且つ鉛直下方の地上面を撮像できるように配置されている。右カメラ１２Ｒは、レーザ測距部１３からＹ軸負方向に延びるアーム１９Ｒの一端部と接続されており、左カメラ１２Ｌは、レーザ測距部１３からＹ軸正方向に延びるアーム１９Ｌの一端部と接続されている。たとえば、右カメラ１２Ｒは、レーザ測距部１３の照射基準点ＲＰからＹ軸負方向に１メートル離れた位置に配置され、左カメラ１２Ｌは、照射基準点ＲＰからＹ軸正方向に１メートル離れた位置に配置されればよい。

図１０Ａは、Ｘ軸正方向から視たときのレーザ測距部１３及び撮像部１１からなるユニットの概略構成図であり、図１０Ｂは、Ｚ軸負方向から視たときの当該ユニットの概略構成図であり、図１０Ｃは、Ｙ軸正方向から視たときの当該ユニットの概略構成図である。図１０Ａに示されるレーザ測距部１３は、地上の測距点Ｋ_ｐにレーザ光を照射し、当該測距点Ｋ_ｐから反射レーザ光を受光する。レーザ測距部１３は、照射基準点ＲＰと測距点Ｋ_ｐとの間の距離Ｌを計測し、且つ、照射基準点ＲＰと測距点Ｋ_ｐとの間の線分がＹ軸正方向となす角度を照射角度θとして計測することができる。レーザ測距部１３は、Ｘ軸正方向から視たときに照射基準点ＲＰを中心とした照射角度θを時計回りに１８度刻みで変化させてレーザ光で地上を走査することができる。なお、照射基準点ＲＰから鉛直下方（Ｙ軸正方向）にレーザ光が照射されたときの照射角度θは、９０度である。

本実施の形態では、航空機２の位置は、照射基準点ＲＰの位置と同じであると仮定する。よって、航空機２がＸ軸正方向に飛行するとき、レーザ測距部１３、右カメラ１２Ｒ及び左カメラ１２Ｌも、Ｘ軸正方向に飛行する。図１１は、航空機２の飛行に伴うレーザ測距部１３の照射基準点ＲＰの位置座標（Ｘ_Ｏ，Ｙ_Ｏ，Ｚ_Ｏ）の一例をテーブル形式で示す図である。図１１のテーブルは、航空機２がＸ軸正方向に沿って一定速度で３秒間の飛行をしたときの位置を示すものである。図１１のテーブルでは、時刻ｔ＝０，１，２，３（単位：秒）でのそれぞれの照射基準点ＲＰの位置座標（Ｘ_Ｏ，Ｙ_Ｏ，Ｚ_Ｏ）の座標値が示されている。なお、位置座標（Ｘ_Ｏ，Ｙ_Ｏ，Ｚ_Ｏ）は、ＧＮＳＳ測位部１４の測位結果に基づいて算出されるべき測位座標であるが、図１１のテーブルに示される座標値Ｘ_Ｏ，Ｙ_Ｏ，Ｚ_Ｏは、説明の便宜上、時刻ｔ＝０での位置座標を基準とする値（単位：メートル）である。

図１２Ａ，図１２Ｂ及び図１２Ｃは、図１１に示した時刻ｔ＝０での照射基準点ＲＰの位置座標に対応する右カメラ１２Ｒ、左カメラ１２Ｌ及びレーザ測距部１３のそれぞれの位置を表すグラフである。また、図１３Ａ，図１３Ｂ及び図１３Ｃは、図１１に示した時刻ｔ＝１での照射基準点ＲＰの位置座標に対応する右カメラ１２Ｒ、左カメラ１２Ｌ及びレーザ測距部１３のそれぞれの位置を表すグラフであり、図１４Ａ，図１４Ｂ及び図１４Ｃは、図１１に示した時刻ｔ＝２での照射基準点ＲＰの位置座標に対応する右カメラ１２Ｒ、左カメラ１２Ｌ及びレーザ測距部１３のそれぞれの位置を表すグラフであり、図１５Ａ，図１５Ｂ及び図１５Ｃは、図１１に示した時刻ｔ＝２での照射基準点ＲＰの位置座標に対応する右カメラ１２Ｒ、左カメラ１２Ｌ及びレーザ測距部１３のそれぞれの位置を表すグラフである。図１２Ａ，図１３Ａ，図１４Ａ及び図１５Ａは、Ｘ−Ｚ面における位置座標を表し、図１２Ｂ，図１３Ｂ，図１４Ｂ及び図１５Ｂは、Ｙ−Ｚ面における位置座標を表し、図１２Ｃ，図１３Ｃ，図１４Ｃ及び図１５Ｃは、Ｘ−Ｙ面における位置座標を表している。ここで、右カメラ１２Ｒの位置は、右カメラ１２Ｒの投影中心の位置であり、三角形の記号で示されている。左カメラ１２Ｌの位置は、左カメラ１２Ｌの投影中心の位置であり、四角形の記号で示されている。そして、レーザ測距部１３の位置は、照射基準点ＲＰの位置であり、丸印で示されている。

時刻ｔ＝０では、図１２Ａ〜図１２Ｃに示されるように、右カメラ１２Ｒ、レーザ測距部１３及び左カメラ１２Ｌは、Ｙ軸方向に沿って配列されている。時刻ｔ＝１では、図１３Ａ〜図１３Ｃに示されるように、右カメラ１２Ｒ、レーザ測距部１３及び左カメラ１２Ｌは、Ｘ軸正方向に１メートルだけ進行すると同時に、Ｙ軸正方向に０．５メートルだけシフトしている。時刻ｔ＝２では、図１４Ａ〜図１４Ｃに示されるように、右カメラ１２Ｒ、レーザ測距部１３及び左カメラ１２Ｌは、Ｘ軸正方向に更に１メートルだけ進行すると同時に、Ｚ軸負方向に０．５メートルだけシフトしている。そして、時刻ｔ＝３では、図１５Ａ〜図１５Ｃに示されるように、右カメラ１２Ｒ、レーザ測距部１３及び左カメラ１２Ｌは、Ｘ軸正方向に更に１メートルだけ進行すると同時に、Ｙ軸負方向に０．５メートルだけシフトし、且つＺ軸負方向に０．５メートルだけシフトしている。よって、時刻ｔ＝３でのＺ軸方向の位置は、時刻ｔ＝０でのＺ軸方向の位置に戻っている。

実際の飛行環境では、Ｘ−Ｙ面に沿った水平飛行を意図していても、航空機２は、風などの外的要因の影響によって水平飛行することは難しい。たとえば、航空機２は、Ｘ軸周りのローリング方向、Ｙ軸周りのピッチング方向、及びＺ軸周りのヨーイング方向にそれぞれに傾いた状態で飛行し得る。

次に、図１６Ａ及び図１６Ｂは、航空機２の飛行に伴うレーザ測距部１３の測定結果（距離Ｌ及び照射角度θ）の例を示す図である。図１６Ａは、時刻ｔ＝０．００〜２．１５（単位：秒）の期間におけるレーザ測距部１３の測定結果の変化をテーブル形式で示す図であり、図１６Ｂは、時刻ｔ＝０．００〜２．１５の期間におけるレーザ測距部１３の測定結果の変化をグラフで示す図である。図１６Ｂにおいて、白色三角形の記号は、時刻ｔ＝０．００〜０．１５に計測された距離Ｌ及び照射角度θを示し、黒色三角形の記号は、時刻ｔ＝１．００〜１．１５に計測された距離Ｌ及び照射角度θを示し、白色四角形の記号は、時刻ｔ＝２．００〜２．１５に計測された距離Ｌ及び照射角度θを示している。

図１６Ａに示されるようにレーザ測距部１３の測定結果は、１回の測定ごとに、測定された時刻ｔ、照射角度θ及び距離Ｌの組み合わせを１レコードとしてメモリ１７に記憶される。図１６Ａの例では、１秒間に４回の測定が行われている。

右カメラ１２Ｒ及び左カメラ１２Ｌの各々は、たとえば、１秒ごとに鉛直下方の地上の矩形範囲を撮像範囲として撮像することができる。図１７は、時刻ｔ＝０，１，２（単位：秒）における撮像画像群の例を示す図である。時刻ｔ＝０では、右カメラ画像Ｒ０及び左カメラ画像Ｌ０が撮像され、時刻ｔ＝１では、右カメラ画像Ｒ１及び左カメラ画像Ｌ１が撮像され、時刻ｔ＝２では、右カメラ画像Ｒ２及び左カメラ画像Ｌ２が撮像されている。このような撮像画像群がメモリ１７に記憶される。

次に、図１に示した姿勢推定装置３０の構成について説明する。図１８は、本発明に係る実施の形態１の姿勢推定装置３０の概略構成を示すブロック図である。

図１８に示されるように姿勢推定装置３０は、メモリインタフェース部（メモリＩ／Ｆ部）３１、データ取得部３２、座標算出部３３、画像マッチング部３４及び姿勢推定部３５を備えている。座標算出部３３は、測距点座標算出部３３Ａ及び画像座標算出部３３Ｂを有する。メモリＩ／Ｆ部３１には、可搬記録媒体であるメモリ１７が着脱自在に装着される。

データ取得部３２は、メモリ１７から、メモリＩ／Ｆ部３１を介して、測位データ、測距データ、実姿勢データ及び撮像画像群を含む観測データを取得する。本実施の形態では、姿勢推定装置３０は観測装置１０と直接接続されていない。このため、データ取得部３２が観測データを取得するために、ユーザは、航空機２からメモリ１７を取り外し、この取り外されたメモリ１７を姿勢推定装置３０に装着する必要がある。なお、後述する実施の形態２の姿勢推定装置３０Ａは、観測装置１０Ａから観測データを無線通信で取得することが可能である。

測距点座標算出部３３Ａは、当該観測データのうちの測位データ、測距データ及び実姿勢データと、航空機２の姿勢を示す未知パラメータの推定値とを用いて、ＷＧＳ−８４座標系などの測地座標系における単数または複数の測距点の位置座標（以下「測地座標」ともいう。）を算出することができる。今、ｐ番目の測距点の測地座標の３次元ベクトルをＱ^（ｐ）＝［Ｘ_Ｇ，Ｙ_Ｇ，Ｚ_Ｇ］^Ｔで表すものとする。ここで、上付き添え字Ｔは、行列の転置を示す記号である。測距点座標算出部３３Ａは、たとえば、次式（１）に従ってｐ番目の測距点の測地座標Ｑ^（ｐ）を算出することができる。

ここで、［Ｘ_Ｏ，Ｙ_Ｏ，Ｚ_Ｏ］^Ｔは、測位データを基に算出される照射基準点ＲＰの位置座標を示す３次元位置ベクトルである。また、Ｄは、照射基準点ＲＰを基準として測距データを基に算出される測距点の相対位置ベクトルである。言い換えれば、相対位置ベクトルＤは、照射基準点ＲＰを原点とする測距点の位置座標を表すベクトルである。測距点座標算出部３３Ａは、たとえば、次式（２）に従って相対位置ベクトルＤを算出することができる。

また、上式（１）において、Ｍ（ω）は、３次元座標をＸ軸方向周りに回転角ωだけ回転させる回転行列であり、Ｍ（φ）は、３次元座標をＹ軸方向周りに回転角φだけ回転させる回転行列であり、Ｍ（κ）は、３次元座標をＺ軸方向周りに回転角κだけ回転させる回転行列である。これら回転行列Ｍ（ω），Ｍ（φ），Ｍ（κ）は、たとえば、次式（３）で表現される。

測位座標Ｑ^（ｐ）を算出するための時刻ｔにおける回転角ω，φ，κが存在しない場合は、測距点座標算出部３３Ａは、撮像時刻ｔ＝ｔ_ｉにおける回転角ω_ｉ，φ_ｉ，κ_ｉと、別の撮像時刻ｔ＝ｔ_ｊ（ｉ≠ｊ）における回転角ω_ｊ，φ_ｊ，κ_ｊとを用いて、撮像時刻ｔ_ｉ，ｔ_ｊ間の時刻ｔにおける回転角ω，φ，κを補間する。たとえば、測距点座標算出部３３Ａは次式（３ａ）により回転角ω，φ，κを補間することができる。

上式（３ａ）において、ｒは、補間係数であり、たとえば次式（３ｂ）で与えられる。

ここで、Δｔは、サンプリング間隔（＝｜ｔ_ｉ−ｔ_ｊ｜）であり、ｆｌｏｏｒ（ｘ）は、実数ｘに対してｘ以下の最大の整数を与える床関数である。

回転角ωはＸ軸方向周りのロール角に対応し、回転角φはＹ軸周りのピッチ角に対応し、回転角κはＺ軸周りのヨー角に対応する。航空機２が傾いたとき、Ｘ軸方向周りのロール角、Ｙ軸周りのピッチ角及びＺ軸周りのヨー角という３種類の姿勢角のうちの少なくとも１つの姿勢角が非零の値となる。この航空機２の傾きに起因して、測距データを基に算出される相対位置ベクトルＤに誤差が発生する。回転行列Ｍ（ω），Ｍ（φ），Ｍ（κ）は、航空機２の傾きに起因する相対位置ベクトルＤの誤差を補正するものである。

上述のとおり、本実施の形態の姿勢センサ１５は、航空機２の姿勢を示すロール角とピッチ角との組み合わせを実姿勢データとして検出することができるが、ヨー角を検出しない。よって、一定精度の実姿勢データが検出されている場合、測距点座標算出部３３Ａは、その実姿勢データから、回転行列Ｍ（ω），Ｍ（φ）を定める回転角ω，φを知ることができるが、回転行列Ｍ（κ）を定める回転角κは未知のパラメータである。この場合、測距点座標算出部３３Ａは、回転角κの推定値の初期値を０度に設定する。これにより、測距点座標算出部３３Ａは、上式（１）に従って各測距点の測地座標Ｑ^（ｐ）を算出することができる。

一方、実姿勢データが検出されていない場合、あるいは、実姿勢データの検出精度が低い場合には、回転角ω，φ，κのすべてが未知のパラメータとなる。実姿勢データが検出されていない場合、測距点座標算出部３３Ａは、回転角ω，φ，κの推定値の初期値をすべて０度に設定する。一方、実姿勢データの検出精度が低い場合には、測距点座標算出部３３Ａは、回転角ω，φの推定値の初期値を当該実姿勢データに基づく値に設定し、回転角κの推定値の初期値を０度に設定する。これにより、測距点座標算出部３３Ａは、上式（１）に従って各測距点の測地座標Ｑ^（ｐ）を算出することができる。後述するように、これら未知のパラメータの推定値に対する補正値が、非線形最小自乗法に基づいて算出される。

画像座標算出部３３Ｂは、各測距点について測地座標系から右カメラ１２Ｒの投影面座標系（画像座標系）への第１の投影変換を測地座標Ｑ^（ｐ）に施して２次元の画像座標である第１の投影点座標Ｓ_Ｒを算出するとともに、各測距点について測地座標系から左カメラ１２Ｌの投影面座標系（画像座標系）への第２の投影変換を測地座標Ｑ^（ｐ）に施して２次元の画像座標である第２の投影点座標Ｓ_Ｌを算出する。これら第１及び第２の投影変換の詳細については後述する。

画像マッチング部３４は、撮像画像群から選択された２つの撮像画像（第１の撮像画像及び第２の撮像画像）間の画像マッチングを実行してそれら２つの撮像画像のうちの一方に現れる測距点の画像座標を検出する。

姿勢推定部３５は、画像マッチング部３４で検出された画像座標とこれに対応する投影点座標Ｓ_ＬまたはＳ_Ｒとの間の差分の大きさが小さくなるように上記の未知のパラメータの推定値に対する補正量を算出する。ここで、姿勢推定部３５は、後述する非線形最小自乗法に基づいて当該補正量を算出することが可能である。姿勢推定部３５は、当該補正量を用いて未知パラメータの推定値を補正することができる。

上記姿勢推定装置３０のハードウェア構成は、たとえば、ワークステーションまたはメインフレームなどのＣＰＵ内蔵のコンピュータで実現可能である。あるいは、姿勢推定装置３０のハードウェア構成は、ＤＳＰ、ＡＳＩＣもしくはＦＰＧＡまたはこれらの組み合わせなどのＬＳＩにより実現されてもよい。

図１９Ａは、姿勢推定装置３０のハードウェア構成例である情報処理装置３０Ｓの概略構成を示すブロック図である。図１９Ａに示されるように情報処理装置３０Ｓは、ＣＰＵ５０ｃを含むプロセッサ５０と、メモリインタフェース回路（メモリＩ／Ｆ回路）５１と、プロセッサ５０の作業用メモリとして使用されるＲＡＭ５２と、ソフトウェア及びファームウェアの一方または双方のコンピュータ・プログラムが格納される不揮発性メモリ５３とを備えて構成されている。この情報処理装置３０Ｓの構成要素５０〜５３は、バス回路などの信号路５４を介して相互に接続されている。メモリＩ／Ｆ回路５１は、図１８のメモリＩ／Ｆ部３１の機能を実現する回路である。プロセッサ５０は、不揮発性メモリ５３から、ソフトウェアもしくはファームウェアまたはこれらの組み合わせであるコンピュータ・プログラムを読み出して実行することにより、図１８のデータ取得部３２，座標算出部３３、画像マッチング部３４及び姿勢推定部３５の機能を実現することができる。不揮発性メモリ５３としては、たとえば、ＲＯＭ、フラッシュメモリ、ＥＰＲＯＭ、ＥＥＰＲＯＭ、磁気ディスクまたは光ディスクを使用することができるが、これらに限定されるものではない。

図１９Ｂは、姿勢推定装置３０の他のハードウェア構成例である情報処理装置３０Ｈの概略構成を示すブロック図である。図１９Ｂに示されるように情報処理装置３０Ｈは、ＤＳＰ、ＡＳＩＣまたはＦＰＧＡなどの専用ハードウェアの処理回路５６と、メモリインタフェース回路（メモリＩ／Ｆ回路）５７とを備えている。これら処理回路５６とメモリＩ／Ｆ回路５７とは、バス回路などの信号路５８を介して相互に接続されている。メモリＩ／Ｆ回路５７は、図１８のメモリＩ／Ｆ部３１の機能を実現する回路である。処理回路５６は、図１８のデータ取得部３２，座標算出部３３、画像マッチング部３４及び姿勢推定部３５の機能を実現する。

なお、データ取得部３２，座標算出部３３、画像マッチング部３４及び姿勢推定部３５の機能の一部が専用のハードウェアで実現されるとともに、その機能の他の一部がソフトウェアまたはファームウェアで実現されてもよい。たとえば、データ取得部３２の機能が専用ハードウェアの処理回路で実現されるとともに、座標算出部３３、画像マッチング部３４及び姿勢推定部３５の機能がＣＰＵを含むプロセッサで実現されてもよい。

次に、図２０を参照しつつ、上記姿勢推定装置３０の動作例を以下に説明する。図２０は、実施の形態１に係る姿勢推定処理の手順の一例を概略的に示すフローチャートである。

図２０を参照すると、先ず、データ取得部３２は、測位データ、測距データ、実姿勢データ及び撮像画像群を含む観測データを取得する（ステップＳＴ１１）。実姿勢データは、上述のとおり、航空機２が傾いたときに非零の姿勢角を示すデータである。図２１Ａ〜図２１Ｃは、航空機２が傾いていないときの撮像画像ＩＭＧ０，ＩＭＧ１と測距点Ｋ_ｐとの間の関係の例を示す図であり、図２２Ａ及び図２２Ｂは、航空機２が傾いたときの撮像画像ＩＭＧ０，ＩＭＧ１と測距点Ｋ_ｐとの間の関係の例を示す図である。図２１Ａは、時刻ｔ_０，ｔ_１における右カメラ１２Ｒ及び左カメラ１２Ｌの配置例を示し、図２１Ｂは、時刻ｔ_０に左カメラ１２Ｌで撮像された左カメラ画像ＩＭＧ０と、時刻ｔ_１に右カメラ１２Ｒで撮像された右カメラ画像ＩＭＧ１とを示している。図２１Ｃは、時刻ｔ_０，ｔ_１における右カメラ１２Ｒ及び左カメラ１２Ｌと測距点Ｋ_ｐとの間の位置関係を概略的に示す図である。航空機２が傾いていないとき、図２１Ｂに示されるように左カメラ画像ＩＭＧ０及び右カメラ画像ＩＭＧ１には同一測距点Ｋ_ｐを表す測距点像８０，８１が現れている。一方、図２２Ｂに示されるように時刻ｔ_１で航空機２が傾いたとき、右カメラ画像ＩＭＧ１には、図２２Ａに示されるように、図２１Ｂの測距点像８１の位置から右方へΔｕだけシフトした位置に、測距点像８２が現れる。

ステップＳＴ１１の後、座標算出部３３における測距点座標算出部３３Ａは、未知のパラメータである未知の姿勢角を特定し、当該未知の姿勢角の推定値に対する補正量を初期化する（ステップＳＴ１２）。具体的には、測距点座標算出部３３Ａは、未知の姿勢角の推定値をすべて０度に初期化することができる。たとえば、時刻ｔ_０では、ロール角、ピッチ角及びヨー角のうちヨー角のみが未知であり、時刻ｔ_１では、ロール角、ピッチ角及びヨー角のすべてが未知である場合、時刻ｔ_０でのヨー角と、時刻ｔ_１でのロール角、ピッチ角及びヨー角とが未知の姿勢角となるので、測距点座標算出部３３Ａは、これら未知の姿勢角の推定値を初期値（０度）に設定することができる。

次に、測距点座標算出部３３Ａは、測位データ、測距データ、実姿勢データ及び未知の姿勢角の推定値に基づいて、測地座標系における各測距点の位置座標である測地座標Ｑ^（ｐ）を算出する（ステップＳＴ１３）。測地座標Ｑ^（ｐ）の算出方法は、上述したとおりである。

次に、画像座標算出部３３Ｂは、各測距点の測地座標Ｑ^（ｐ）に対して、測地座標系から右カメラ１２Ｒの投影面座標系への第１の投影変換を実行して２次元の画像座標である第１の投影点座標を算出し（ステップＳＴ１４）、更に、各測距点の測地座標Ｑ^（ｐ）に対して、測地座標系から左カメラ１２Ｌの投影面座標系への第２の投影変換を実行して２次元の画像座標である第２の投影点座標を算出する（ステップＳＴ１５）。

ステップＳＴ１４，ＳＴ１５において、具体的には、画像座標算出部３３Ｂは、先ず、撮像画像群の中から、撮像時刻ｔ＝ｔ_ｉに撮像された左カメラ画像と、撮像時刻ｔ＝ｔ_ｊ（ｉ≠ｊ）に撮像された右カメラ画像とのペアを選択する。撮像時刻ｔ_ｉ，ｔ_ｊは、撮像画像のサンプリング間隔（データ取得周期）だけ互いに離れている。以下、このペアの選択をペアリングと呼ぶ。また、その右カメラ画像を第１の撮像画像と呼び、その左カメラ画像を第２の撮像画像と呼ぶこととする。説明の便宜上、本実施の形態では、撮像部１１は、１秒間隔で測距点を撮像するものとする。本実施の形態では、撮像時刻ｔ_ｉ，ｔ_ｊは互いに異なる時刻であるが、これに限定されず、同一の時刻であってもよい。また、本実施の形態では、左カメラ画像と右カメラ画像とのペアが選択されているが、これに限定されるものではなく、互いに異なる時刻に異なる位置から撮像された２枚の左カメラ画像のペア、あるいは、互いに異なる時刻に異なる位置から撮像された２枚の右カメラ画像のペアが選択されてもよい。このような撮像画像のペアを用いることで、航空機２の姿勢に応じた画像上の被写体の変化を航空機２の姿勢推定に利用することができる。言い換えれば、航空機２の姿勢に応じた画像上の被写体の変化が航空機２の姿勢推定に利用されるため、左カメラ１２Ｌと右カメラ１２Ｒとの視野安定化のためのスタビライザーを使用せずに済む。

次に、画像座標算出部３３Ｂは、次式（４Ｌ）に従って、測地座標Ｑ^（ｐ）を、左カメラ１２Ｌの投影中心を基準とする相対位置ベクトルＣ_Ｌｉ ^（ｐ）に変換し、次式（４Ｒ）に従って、測地座標Ｑ^（ｐ）を、右カメラ１２Ｒの投影中心を基準とする相対位置ベクトルＣ_Ｒｊ ^（ｐ）に変換する。

ここで、回転行列Ｍ（−ω_ｉ），Ｍ（−φ_ｉ），Ｍ（−κ_ｉ）を定める−ω_ｉ，−φ_ｉ，−κ_ｉは、それぞれ、撮像時刻ｔ＝ｔ_ｉでのロール角，ピッチ角及びヨー角（未知の姿勢角の推定値を含む。）に対応する量である。また、Ｑ_Ｌ（ｔ_ｉ）は、撮像時刻ｔ_ｉにおける左カメラ１２Ｌの投影中心座標であり、Ｑ_Ｒ（ｔ_ｊ）は、時刻ｔ_ｊにおける右カメラ１２Ｒの投影中心座標である。投影中心座標Ｑ_Ｌ（ｔ_ｉ），Ｑ_Ｒ（ｔ_ｊ）は、次式（５Ｌ），（５Ｒ）で与えられる。

ここで、Ｒ_ＬＣは、左カメラ１２Ｌの投影中心の相対位置ベクトルであり、Ｒ_ＲＣは、右カメラ１２Ｒの投影中心の相対位置ベクトルである。たとえば、相対位置ベクトルＲ_ＬＣ，Ｒ_ＲＣは次式（６）で与えられる。

そして、画像座標算出部３３Ｂは、上記相対位置ベクトルＣ_Ｌｉ ^（ｐ），Ｃ_Ｒｊ ^（ｐ）を用いて、次式（７）に従って第１の投影点座標Ｓ_Ｒ ^（ｐ）及び第２の投影点座標Ｓ_Ｌ ^（ｐ）を算出する。

ここで、ｆは、左カメラ１２Ｌ及び右カメラ１２Ｒの焦点距離である。

上記したステップＳＴ１４，ＳＴ１５の後は、画像マッチング部３４は、ペアをなす第１の撮像画像（右カメラ画像）と第２の撮像画像（左カメラ画像）との間で画像マッチングを実行して、第１の撮像画像に現れる各測距点の画像座標を検出する（ステップＳＴ１６）。具体的には、画像マッチング部３４は、第２の撮像画像から第２の投影点座標で指定される局所領域画像をテンプレート画像として抽出し、このテンプレート画像と第１の撮像画像との間で周知のテンプレートマッチング法を実行することにより、第１の撮像画像に現れる各測距点の画像座標を検出すればよい。

以下、ｐ番目の測距点の画像座標のｘ座標値をＳＣＡＮ_ｘ（ｘ_Ｌｉ ^（ｐ），ｙ_Ｌｉ ^（ｐ））と表し、当該画像座標のｙ座標値をＳＣＡＮ_ｙ（ｘ_Ｌｉ ^（ｐ），ｙ_Ｌｉ ^（ｐ））と表すものとする。ここで、（ｘ_Ｌｉ ^（ｐ），ｙ_Ｌｉ ^（ｐ））は、ｐ番目の測距点の第２の投影点座標である。

次に、姿勢推定部３５は、未知の姿勢角の推定値に対して、第１の投影点座標と画像座標との間の差分の大きさを小さくする補正量を算出する（ステップＳＴ１７）。具体的には、非線形最小自乗法を用いて補正量が算出されればよい。非線形最小自乗法については後述する。次いで、姿勢推定部３５は、補正量を用いて未知の姿勢角の推定値を補正する（ステップＳＴ１８）。そして、ステップＳＴ１２〜ＳＴ１８の繰り返し回数が所定回数に到達していない場合には（ステップＳＴ１９のＮＯ）、姿勢推定部３５は、ステップＳＴ１２に処理を移行させる。ステップＳＴ１２〜ＳＴ１８が一定回数繰り返されると、未知の姿勢角の推定値は補正により収束する。ステップＳＴ１２〜ＳＴ１８の繰り返し回数が所定回数に到達した場合には（ステップＳＴ１９のＹＥＳ）、姿勢推定部３５は、未知の姿勢角の推定値と実姿勢データとを含む姿勢データをメモリＩ／Ｆ部３１に出力する（ステップＳＴ２０）。メモリＩ／Ｆ部３１は、その姿勢データをメモリ１７に格納する。以上で姿勢推定処理は終了する。

次に、実施の形態１に係る非線形最小自乗法について説明する。未知の姿勢角α_０，α_１，…，α_ＮがＮ＋１個あるとき、未知の姿勢角ベクトルαは、次式（８）で与えられる。

ここで、α_Ｃは、未知の姿勢角の推定値からなる推定値ベクトルである。また、Δαは、推定値ベクトルα_Ｃに対する補正量（補正ベクトル）であり、次式（９）で与えられるものとする。

ここで、ｐ番目の測距点についての画像座標と第１の投影点座標との間の差分を示す観測関数Ｆ_ｘｉｊ ^（ｐ）（α），Ｆ_ｙｉｊ ^（ｐ）（α）を次式（１０），（１１）で定義する。

このとき、非線形最小自乗法により次式（１２），（１３）の１組の観測方程式を構成することができる。

ここで、右辺第１項に付された下付き添え字α_Ｃは、推定値ベクトルα_Ｃの値を代入することを意味する。また、左辺のｖ_ｘｉｊ ^（ｐ），ｖ_ｙｉｊ ^（ｐ）は残差である。

第１の撮像画像及び第２の撮像画像のペアは（ｉ，ｊ）の組で指定可能である。よって、（ｉ，ｊ）＝（０，１），（１，２），（２，３），…の各々について観測方程式（１０），（１１）を構成することができる。今、残差ベクトルＶを次式（１４）で定義し、誤差ベクトルｗを次式（１５）で定義することができる。

また、推定値ベクトルα_Ｃにおける計画行列Φを次式（１６）で定義することができる。

このとき、観測方程式の組は、次式（１７）で表現可能である。

残差ベクトルＶの二乗和Ｖ^Ｔ・Ｖを最小にする補正ベクトルΔαは、次式（１８）のとおりである。

したがって、姿勢推定部３５は、上記ステップＳＴ１７では、式（１８）に従って補正ベクトルΔαを算出し、上記ステップＳＴ１８では、上式（８）に従って推定値を補正することが可能である。

たとえば、未知の姿勢角として、α_０＝κ_１（時刻ｔ_１），α_１＝ω_２（時刻ｔ_２），α_２＝φ_２（時刻ｔ_２），α_３＝κ_２（時刻ｔ_２）が与えられたとき、補正ベクトルΔαは、次式（１９）で表現される。

このとき、未知の姿勢角ベクトルαは、次式（２０）で与えられる。

ここで、＜κ_１＞，＜ω_２＞，＜φ_２＞，＜κ_２＞は、推定値ベクトルα_Ｃの要素である。また、このときの観測方程式は、次式（２１），（２２）のとおりである。

更に、これら観測方程式（２１），（２２）に基づいて次式（２３）の計画行列Φを構成することができる。

図２３Ａは、上式（１）に基づいて算出された０番目〜１１番目の測距点の各測距点の測地座標（Ｘ_Ｇ，Ｙ_Ｇ，Ｚ_Ｇ）の数値例（ｐは測距点番号）を表すテーブルであり、図２３Ｂは、上式（５Ｌ），（５Ｒ）に基づいて算出された投影中心座標（Ｘ_Ｌ，Ｙ_Ｌ，Ｚ_Ｌ），（Ｘ_Ｒ，Ｙ_Ｒ，Ｚ_Ｒ）の数値例を表すテーブルである。時刻ｔ_０＝０．０，ｔ_１＝１．０，ｔ_２＝２．０，ｔ_３＝３．０のそれぞれの時刻で、右カメラ画像及び左カメラ画像が撮像される。また、図２４は、上式（７）に基づいて算出された第１の投影点座標及（ｘ_Ｒ，ｙ_Ｒ）び第２の投影点座標（ｘ_Ｌ，ｙ_Ｌ）の数値例を表すテーブルである。そして、図２５は、画像座標（ＳＣＡＮ_ｘ（ｘ_Ｌｉ，ｙ_Ｌｉ）、ＳＣＡＮ_ｙ（ｘ_Ｌｉ，ｙ_Ｌｉ））の数値例を表す図である。α_０＝κ_１（時刻ｔ_１），α_１＝ω_２（時刻ｔ_２），α_２＝φ_２（時刻ｔ_２），α_３＝κ_２（時刻ｔ_２）が与えられたとき、これら図２３Ａ，２３Ｂ、図２４及び図２５に示す数値を用いて、図２６の誤差ベクトルｗ、図２７の計画行列Φ、図２８の補正ベクトルΔαをそれぞれ算出することができた。また、図２９に示すように、時刻ｔ＝０．００，１．００，２．００，３．００での姿勢データが得られることが確認された。

なお、異なる位置のカメラにより撮像された画像間で対応点を探索して観測対象までの距離及び奥行き情報などの３次元情報を取得するステレオ画像処理には、固定ステレオと呼ばれる方法と、モーションステレオと呼ばれる方法とがある。固定ステレオは、２つのカメラを間隔を空けて配置して撮像する手法である。時刻ｔ_ｉでの左カメラ画像と時刻ｔ_ｊの右カメラ画像のペアリングは、固定ステレオに相当する。また、モーションステレオでは、カメラを飛行させて異なる撮影位置から撮影する方法である。時刻ｔ_ｉでのカメラ画像と時刻ｔ_ｊでのカメラ画像とのペアリングは、モーションステレオに相当する。

また、これまでの説明では、左カメラ１２Ｌと右カメラ１２Ｒとを利用する構成を示したが、１台のカメラであってもよい。この場合、時刻ｔ_ｉのカメラ画像と時刻ｔ_ｊのカメラ画像とのペアが用いられればよい。

更に、上記では、未知のパラメータは、時刻ごとの姿勢角の３つのパラメータのいずれかであったが、航空機２の３次元位置座標を加えた６つのパラメータとされてもよく、焦点距離ｆなどの内部パラメータを含んでもよい。

以上に説明したように実施の形態１に係る観測システム１は、異なる撮像位置でそれぞれ撮像された画像間の対応する点の座標が航空機２の姿勢によってずれることに着目し、これらの座標の差分の大きさが小さくなるように未知の姿勢角の推定値を補正することができる。したがって、ＩＭＵやスタビライザーを使用しなくても航空機２の姿勢を高精度に推定することができる。

なお、実施の形態１では、回転角κの推定値の初期値を０度として座標計算が実行されたが、回転角κの近似解は、ＧＮＳＳ測位部１４によって測定された測位座標の時系列から得ることもできる。よって、回転角κの推定値の初期値をその近似解に設定してもよい。

更に、実施の形態１に係る観測システム１では、可搬記録媒体であるメモリ１７が航空機２に搭載されて使用される。このように航空機２の飛行中にメモリ１７に蓄積された観測データを用いることで、航空機２の姿勢角を飛行終了後に推定することができ、推定された姿勢角を用いて測量結果を補正することもできる。

実施の形態２．
次に、本発明に係る実施の形態２について説明する。図３０は、本発明に係る実施の形態２の観測システム１Ａの概略構成を示すブロック図である。図３０に示されるように、この観測システム１Ａは、観測装置１０Ａ，メモリ１７，飛行制御装置２１及び推力発生機構２２を含む航空機２Ａと、姿勢推定装置３０Ａと、飛行ルート設定装置４０Ａとを備えて構成されている。

航空機２Ａに搭載される観測装置１０Ａの構成は、無線通信部１８を有する点を除いて、上記実施の形態１の観測装置１０の構成と同じである。無線通信部１８は、姿勢推定装置３０Ａ及び飛行ルート設定装置４０Ａとの間で無線通信を実行してルートデータや観測データなどのデータを送受信する機能を有する。

図３１は、実施の形態２の姿勢推定装置３０Ａの概略構成を示すブロック図である。この姿勢推定装置３０Ａの構成は、図１８のメモリＩ／Ｆ部３１に代えて無線インタフェース部（無線Ｉ／Ｆ部）３１ＷＬを有する点を除いて、上記実施の形態１の姿勢推定装置３０の構成と同じである。無線Ｉ／Ｆ部３１ＷＬは、図３０に示した観測装置１０Ａの無線通信部１８との間で無線通信する機能を有するので、無線通信部１８から上記の観測データなどのデータを受信し、姿勢推定部３５で生成された姿勢データを無線通信部１８に送信することができる。

図３２は、実施の形態２の飛行ルート設定装置４０Ａの概略構成を示すブロック図である。この飛行ルート設定装置４０Ａの構成は、図３のメモリＩ／Ｆ部４４に代えて無線インタフェース部（無線Ｉ／Ｆ部）４４ＷＬを有する点を除いて、上記実施の形態１の飛行ルート設定装置４０の構成と同じである。無線Ｉ／Ｆ部４４ＷＬは、図３０に示した観測装置１０Ａの無線通信部１８との間で無線通信する機能を有するので、ルートデータ生成部４３で生成されたルートデータ及び周期データを無線通信部１８に送信することができる。観測制御部１６は、無線通信部１８で受信された当該周期データを用いて撮像部１１、レーザ測距部１３、ＧＮＳＳ測位部１４及び姿勢センサ１５を制御することができる。

また、観測制御部１６は、無線通信部１８で受信された当該ルートデータをメモリ１７に格納する。飛行制御装置２１は、当該ルートデータに従って航空機２の速度、進路及び姿勢を制御することができる。したがって、飛行ルート設定装置４０Ａは、メモリ１７を使用せずに、飛行中の航空機２Ａにルートデータをリアルタイムに供給して航空機２を遠隔制御することが可能である。

以上、図面を参照して本発明に係る種々の実施の形態について述べたが、これら実施の形態は本発明の例示であり、これら実施の形態以外の様々な形態を採用することもできる。なお、本発明の範囲内において、上記実施の形態１，２の自由な組み合わせ、各実施の形態の任意の構成要素の変形、または各実施の形態の任意の構成要素の省略が可能である。

本発明に係る姿勢推定装置、観測システム及び姿勢推定方法は、観測機器を搭載した飛行体の姿勢を高精度に推定することができるので、たとえば、測量システム及びＵＡＶ用の航法（ナビゲーション）システムに適用可能である。

１，１Ａ 観測システム、２，２Ａ 航空機、１０，１０Ａ 観測装置、１１ 撮像部、１２Ｌ 左カメラ、１２Ｒ 右カメラ、１３ レーザ測距部、１４ ＧＮＳＳ測位部、１５ 姿勢センサ、１６ 観測制御部、１７ メモリ、１８ 無線通信部、１９Ｒ，１９Ｌ アーム、２０ 機体、２１ 飛行制御装置、２２ 推力発生機構、２２Ａ〜２２Ｆ 推力発生器、３０，３０Ａ 姿勢推定装置、３０Ｓ 情報処理装置、３０Ｈ 情報処理装置、３１ メモリインタフェース部（メモリＩ／Ｆ部）、３１ＷＬ 無線インタフェース部（無線Ｉ／Ｆ部）、３２ データ取得部、３３ 座標算出部、３３Ａ 測距点座標算出部、３３Ｂ 画像座標算出部、３４ 画像マッチング部、３５ 姿勢推定部、４０，４０Ａ 飛行ルート設定装置、４０Ｓ 情報処理装置、４０Ｈ 情報処理装置、４１ 入出力インタフェース部（入出力Ｉ／Ｆ部）、４２ 入力データ処理部、４３ ルートデータ生成部、４４ メモリインタフェース部（メモリＩ／Ｆ部）、４４ＷＬ 無線インタフェース部（無線Ｉ／Ｆ部）、４６ 操作入力部、４７ 表示部、５０ プロセッサ、５０ｃ ＣＰＵ、５１ メモリインタフェース回路（メモリＩ／Ｆ回路）、５２ ＲＡＭ、５３ 不揮発性メモリ、５４ 信号路、５６ 処理回路、５７ メモリインタフェース回路（メモリＩ／Ｆ回路）、５８ 信号路、６０ プロセッサ、６０ｃ ＣＰＵ、６１ メモリインタフェース回路（メモリＩ／Ｆ回路）、６１ メモリＩ／Ｆ回路、６２ ＲＡＭ、６３ 不揮発性メモリ、６４Ａ 入力インタフェース回路（入力Ｉ／Ｆ回路）、６４Ｂ 表示インタフェース回路（表示Ｉ／Ｆ回路）、６５ 信号路、６６ 処理回路、６７ メモリＩ／Ｆ回路、６８Ａ 入力インタフェース回路（入力Ｉ／Ｆ回路）、６８Ｂ 表示インタフェース回路（表示Ｉ／Ｆ回路）、６９ 信号路。

Claims (18)

1. 測地座標系における飛行体の位置を示す測位データを出力する測位部と、前記飛行体から地上の測距点までの距離と当該測距点に対する角度方向とを計測して測距データを出力する測距部と、前記飛行体に搭載された状態で複数の撮像位置から当該測距点を撮像して撮像画像群を出力する撮像部と、重力方向を基準とした当該飛行体の姿勢を検出して実姿勢データを出力する姿勢センサとを含む観測装置と連携して動作する姿勢推定装置であって、
   前記測位データ、前記測距データ、前記実姿勢データ及び前記撮像画像群を前記観測装置から取得するデータ取得部と、
   前記測位データ、前記測距データ、前記実姿勢データ及び前記飛行体の姿勢を示す未知のパラメータの推定値を用いて、前記測地座標系における当該測距点の位置を示す測地座標を算出する測距点座標算出部と、
   前記測地座標系から、前記撮像画像群の中から選択された第１の撮像画像に対応する第１の画像座標系への投影変換を前記測地座標に施して第１の投影点座標を算出し、前記測地座標系から、前記撮像画像群の中から選択された第２の撮像画像に対応する第２の画像座標系への投影変換を前記測地座標に施して第２の投影点座標を算出する画像座標算出部と、
   前記第２の撮像画像から前記第２の投影点座標で指定される局所領域画像を抽出し、当該局所領域画像と前記第１の撮像画像との間の画像マッチングを実行して前記第１の撮像画像に現れる当該測距点の画像座標を検出する画像マッチング部と、
   前記第１の投影点座標と前記画像座標との間の差分の大きさが小さくなるように前記推定値を補正する姿勢推定部と
   を備えることを特徴とする姿勢推定装置。
2. 請求項１記載の姿勢推定装置であって、前記測距点座標算出部、前記画像座標算出部、前記画像マッチング部及び前記姿勢推定部は、非線形最小自乗法に基づき、前記測地座標を算出する処理と、前記第１の投影点座標及び前記第２の投影点座標を算出する処理と、前記画像座標を検出する処理と、前記推定値を補正する処理とを反復して実行することを特徴とする姿勢推定装置。
3. 請求項２記載の姿勢推定装置であって、前記第１の撮像画像及び前記第２の撮像画像は、互いに異なる第１の撮像時刻及び第２の撮像時刻にそれぞれ撮像された画像であることを特徴とする姿勢推定装置。
4. 請求項３記載の姿勢推定装置であって、前記撮像部は、互いに異なる位置に配置された第１撮像部及び第２撮像部を有し、前記第１撮像部が前記第１の撮像画像を出力し、前記第２撮像部が前記第２の撮像画像を出力することを特徴とする姿勢推定装置。
5. 請求項１記載の姿勢推定装置であって、前記測位部は、全地球的航法衛星システムを構成する複数の衛星から航法信号を受信し、当該航法信号を解析して前記測位データを生成することを特徴とする姿勢推定装置。
6. 請求項１記載の姿勢推定装置であって、前記測距部は、前記測距点にレーザ光を照射し、前記測距点から反射レーザ光を受光して前記距離及び前記角度方向を測定することを特徴とする姿勢推定装置。
7. 測地座標系における飛行体の位置を示す測位データを出力する測位部と、前記飛行体から地上の測距点までの距離と当該測距点に対する角度方向とを計測して測距データを出力する測距部と、前記飛行体に搭載された状態で複数の撮像位置から当該測距点を撮像して撮像画像群を出力する撮像部と、重力方向を基準とした当該飛行体の姿勢を検出して実姿勢データを出力する姿勢センサとを含む観測装置と、
   前記観測装置と連携して動作する姿勢推定装置と
   を備え、
   前記姿勢推定装置は、
   前記測位データ、前記測距データ、前記実姿勢データ及び前記撮像画像群を前記観測装置から取得するデータ取得部と、
   前記測位データ、前記測距データ、前記実姿勢データ及び前記飛行体の姿勢を示す未知のパラメータの推定値を用いて、前記測地座標系における当該測距点の位置を示す測地座標を算出する測距点座標算出部と、
   前記測地座標系から、前記撮像画像群の中から選択された第１の撮像画像に対応する第１の画像座標系への投影変換を前記測地座標に施して第１の投影点座標を算出し、前記測地座標系から、前記撮像画像群の中から選択された第２の撮像画像に対応する第２の画像座標系への投影変換を前記測地座標に施して第２の投影点座標を算出する画像座標算出部と、
   前記第２の撮像画像から前記第２の投影点座標で指定される局所領域画像を抽出し、当該局所領域画像と前記第１の撮像画像との間の画像マッチングを実行して前記第１の撮像画像に現れる当該測距点の画像座標を検出する画像マッチング部と、
   前記第１の投影点座標と前記画像座標との間の差分の大きさが小さくなるように前記推定値を補正する姿勢推定部と
   を含むことを特徴とする観測システム。
8. 請求項７記載の観測システムであって、前記測距点座標算出部、前記画像座標算出部、前記画像マッチング部及び姿勢推定部は、非線形最小自乗法に基づき、前記測地座標を算出する処理と、前記第１の投影点座標及び前記第２の投影点座標を算出する処理と、前記画像座標を検出する処理と、前記推定値を補正する処理とを反復して実行することを特徴とする観測システム。
9. 請求項７記載の観測システムであって、前記飛行体の飛行ルートを設定するための飛行ルート設定装置を更に備え、
   前記飛行ルート設定装置は、
   前記飛行ルートを指示する入力データを受け付ける入力データ処理部と、
   前記入力データ処理部で受け付けられた当該入力データに基づき、前記飛行ルートを定めるルートデータを生成するルートデータ生成部と
   を含み、
   前記飛行体は、前記ルートデータで定められた当該飛行ルートに沿って前記飛行体を飛行させる飛行制御装置を備える
   ことを特徴とする観測システム。
10. 請求項９記載の観測システムであって、
    前記飛行ルート設定装置は、ユーザによる操作入力を検出する操作入力部と接続された入力インタフェース部を更に含み、
    前記入力インタフェース部は、前記操作入力部で検出された操作入力を前記入力データとして前記入力データ処理部に供給する
    ことを特徴とする観測システム。
11. 請求項９記載の観測システムであって、前記飛行ルート設定装置は、前記ルートデータを記録媒体に格納するメモリインタフェース部を更に備えることを特徴とする観測システム。
12. 請求項９記載の観測システムであって、
    前記飛行ルート設定装置は、前記ルートデータを無線で送信する無線インタフェース部を更に備え、
    前記飛行体は、
    前記無線インタフェース部から前記ルートデータを受信する無線通信部と、
    前記無線通信部で受信された当該ルートデータで定められた当該飛行ルートに沿って前記飛行体を飛行させる飛行制御装置とを備える
    ことを特徴とする観測システム。
13. 請求項７記載の観測システムであって、
    前記観測装置は、前記測位データ、前記測距データ、前記実姿勢データ及び前記撮像画像群を含む観測データを無線で送信する無線通信部を更に備え、
    前記姿勢推定装置は、前記無線通信部から当該観測データを受信する無線インタフェース部を更に備える
    ことを特徴とする観測システム。
14. 請求項９記載の観測システムであって、前記測距部は、レーザ光で前記地上を走査し前記測距点から反射レーザ光を受光して前記距離及び前記角度方向を測定することを特徴とする観測システム。
15. 請求項１４記載の観測システムであって、
    前記飛行制御装置は、前記ルートデータで定められた第１の飛行ルートを所定方向に沿って前記飛行体に自動で飛行させた後に、前記第１の飛行ルートと並行する第２の飛行ルートを前記所定方向とは逆方向に沿って前記飛行体に自動で飛行させ、
    前記飛行体が前記第１の飛行ルートを飛行する際の前記レーザ光の走査範囲と、前記飛行体が前記第２の飛行ルートを飛行する際の前記レーザ光の走査範囲とは、互いにオーバラップすることを特徴とする観測システム。
16. 請求項１５記載の観測システムであって、前記所定方向は一軸方向であることを特徴とする観測システム。
17. 測地座標系における飛行体の位置を示す測位データを出力する測位部と、前記飛行体から地上の測距点までの距離と当該測距点に対する角度方向とを計測して測距データを出力する測距部と、前記飛行体に搭載された状態で複数の撮像位置から当該測距点を撮像して撮像画像群を出力する撮像部と、重力方向を基準とした当該飛行体の姿勢を検出して実姿勢データを出力する姿勢センサとを含む観測装置と連携して動作する姿勢推定装置において実行される姿勢推定方法であって、
    前記測位データ、前記測距データ、前記実姿勢データ及び前記撮像画像群を前記観測装置から取得するステップと、
    前記測位データ、前記測距データ、前記実姿勢データ及び前記飛行体の姿勢を示す未知のパラメータの推定値を用いて、前記測地座標系における当該測距点の位置を示す測地座標を算出するステップと、
    前記測地座標系から、前記撮像画像群の中から選択された第１の撮像画像に対応する第１の画像座標系への投影変換を前記測地座標に施して第１の投影点座標を算出するステップと、
    前記測地座標系から、前記撮像画像群の中から選択された第２の撮像画像に対応する第２の画像座標系への投影変換を前記測地座標に施して第２の投影点座標を算出するステップと、
    前記第２の撮像画像から前記第２の投影点座標で指定される局所領域画像を抽出するステップと、
    当該局所領域画像と前記第１の撮像画像との間の画像マッチングを実行して前記第１の撮像画像に現れる当該測距点の画像座標を検出するステップと、
    前記第１の投影点座標と前記画像座標との間の差分の大きさが小さくなるように前記推定値を補正するステップと
    を備えることを特徴とする姿勢推定方法。
18. 請求項１７記載の姿勢推定方法であって、前記測地座標を算出する当該ステップと、前記第１の投影点座標を算出する当該ステップと、前記第２の投影点座標を算出する当該ステップと、前記局所領域画像を抽出する当該ステップと、前記画像座標を検出する当該ステップと、前記推定値を補正する当該ステップとは、非線形最小自乗法に基づいて反復して実行されることを特徴とする姿勢推定方法。

Images