JP6029794B1 - 航法装置および測量システム - Google Patents

Abstract

異なる撮影位置でそれぞれ撮影された画像データペアの画像マッチングを行って、ペアの一方の画像データの画像上の測距点の座標に対応する点を、ペアのもう一方の画像データの画像から探索する。ペアのもう一方の画像データの画像上の測距点の座標と画像マッチングによって探索された対応する点の座標との差が小さくなるように航空機（２）の姿勢を示すパラメータの値を補正して航空機（２）の姿勢を推定する。

Description

この発明は、測量用のカメラおよびレーザ測距装置を搭載した移動体の姿勢を推定する航法装置およびこれを備えた測量システムに関する。

例えば、特許文献１には、飛行体に搭載したカメラとレーザ発信受信装置を使用して、写真測量と航空レーザ測量を行う測量システムが記載されている。
この測量システムにおいて、飛行体から測量対象を撮影するカメラは、スタビライザーと呼ばれる姿勢安定化装置に支持されており、飛行中の飛行体の姿勢によらず、撮影方向を鉛直下向きに保つことができる。
また、レーザ発信受信装置は、飛行体から予め定められた周期でレーザ光を測量対象に照射し、この測量対象からの反射光を受信する。この測量システムにおける制御装置は、レーザ発信受信装置によって測量対象から受信された反射光の情報を使用して航空レーザ測量を行う。なお、レーザ発信受信装置は、本発明におけるレーザ測距装置に相当する。

航空レーザ測量では、上記の情報の他、飛行体の３次元座標データ（飛行体の水平方向の位置と標高）と飛行中の飛行体の姿勢を示す情報が必要である。これらの情報のうち、飛行体の３次元座標データは、飛行体に搭載されたＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）装置により検出される。すなわち、ＧＮＳＳ装置は、ＧＮＳＳ衛星からＧＮＳＳ情報を一定の周期ごとに受信し、このＧＮＳＳ情報を解析して飛行体の３次元座標データを得る。

一方、レーザ発信受信装置によってレーザ光が測量対象に照射される周期は、ＧＮＳＳ装置によってＧＮＳＳ情報が受信される周期よりも短い周期である。このため、制御装置は、レーザ発信受信装置によって測量対象からの反射光が受信されても、ＧＮＳＳ情報が受信される周期以外の一定の間隔で飛行体の３次元座標データを得ることができない。
これに対して、従来の一般的な航空レーザ測量においては、飛行体に搭載されたＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）により計測された３軸の加速度および３軸の角加速度の情報を使用して、ＧＮＳＳ情報が受信される周期以外の一定の間隔における飛行体の３次元座標データを得ていた。

しかしながら、ＩＭＵは、非常に高価である上、比較的重い装置であることから、これを搭載可能な飛行体の種類も限定されてしまう。
そこで、特許文献１に記載される測量システムでは、ＩＭＵの代わりに、ＩＭＵよりも安価でかつ小型な加速度計および角加速度計を備えている。
すなわち、この測量システムでは、加速度計からの３軸の加速度および角加速度計からの３軸の角加速度の情報を使用して、ＧＮＳＳ情報の受信周期以外の一定の間隔における飛行体の３次元座標データを得ている。

また、飛行体の姿勢を示す情報は、飛行体のローリング方向、ピッチング方向、ヨーイング方向の角度（以下、ロール角、ピッチ角、ヨー角と記載する）であり、カメラによって２つ以上の異なる位置から撮影された画像間の対応点を対象としたバンドル計算で得られた値が使用される。制御装置は、バンドル計算で得られた飛行体の姿勢に基づいて、加速度計からの加速度と角加速度計からの角加速度とを利用して、レーザ光の走査周期分（ＧＮＳＳ情報の受信周期以外の一定の間隔分）の飛行体の姿勢を算出している。

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

特許文献１に記載される測量システムでは、カメラによって異なる位置から撮像された画像データを用いたバンドル計算で飛行体の姿勢を推定しており、画像データのみを姿勢の推定に用いている。このため、姿勢の推定精度に限界があった。

また、上記測量システムにおいて、飛行体の姿勢の推定に用いられる画像情報は、飛行体の姿勢に関わらず、飛行体の鉛直下向きで撮影されたことを前提としている。
このため、カメラの撮影方向を常に鉛直下向きに維持するスタビライザーを備える必要があり、システム構成が複雑化する。

この発明は、上記課題を解決するもので、ＩＭＵ、スタビライザーを搭載しない構成で移動体の姿勢を精度よく推定することができる航法装置および測量システムを得ることを目的とする。

この発明に係る航法装置は、データ取得部、座標算出部、画像マッチング部および姿勢推定部を備える。データ取得部は、移動体に搭載されたレーザ測距装置により測定されたレーザ光の照射基準点から測距点までの距離を示す距離データとレーザ光の照射角度を示す角度データ、移動体に搭載された座標測定装置により測定されたレーザ光の照射基準点の３次元座標を示す座標データおよび移動体に搭載された撮影装置により撮影された撮影対象に測距点を含む画像データを取得する。座標算出部は、データ取得部により取得された距離データと角度データ、座標データおよび移動体の姿勢を示すパラメータに基づいて、画像データの画像上の測距点の座標を算出する。画像マッチング部は、撮影装置により異なる撮影位置でそれぞれ撮影された画像データのペアの画像マッチングを行って、座標算出部により算出されたペアの一方の画像データの画像上の測距点の座標に対応する点を、ペアのもう一方の画像データの画像から探索する。姿勢推定部は、座標算出部により算出されたペアのもう一方の画像データの画像上の測距点の座標と画像マッチング部により探索された対応する点の座標との差が小さくなるように移動体の姿勢を仮定するパラメータの値を補正して当該移動体の姿勢を推定する。

この発明によれば、異なる撮影位置でそれぞれ撮影された画像間の対応する点の座標が移動体の姿勢によってずれることに着目し、これらの座標の差が小さくなるように移動体の姿勢を示すパラメータの値を補正して移動体の姿勢を推定するので、ＩＭＵ、スタビライザーを使用しなくても移動体の姿勢を推定することができる。
また、測距点が撮影された画像データに加え、レーザ光の照射基準点から測距点までの距離、レーザ光の照射角度、レーザ光の照射基準点の３次元座標を使用して移動体の姿勢を推定するので、移動体の姿勢を精度よく推定することが可能である。

この発明の実施の形態１に係る測量システムの構成を示すブロック図である。 実施の形態１に係る航法装置の機能構成を示すブロック図である。 実施の形態１に係る航法装置のハードウェア構成を示すブロック図である。図３（ａ）は、航法装置の機能を実現するハードウェアの処理回路を示し、図３（ｂ）は、航法装置の機能を実現するソフトウェアを実行するハードウェア構成を示している。 実施の形態１に係る航法装置の動作の概要を示すフローチャートである。 左カメラと右カメラとレーザ測距装置との位置関係を模式的に示す図である。図５（ａ）は、左カメラと右カメラとレーザ測距装置とからなるユニットの斜視図、図５（ｂ）は、当該ユニットをＸ軸方向に見た図であり、図５（ｃ）は、当該ユニットをＺ軸方向に見た図、図５（ｄ）は、当該ユニットをＹ軸方向に見た図である。 航空機の飛行に伴う左カメラ、右カメラおよびレーザ測距装置の位置変化を示す図である。図６（ａ）は、レーザ測距装置の位置座標データを示している。図６（ｂ）は、左カメラ、右カメラおよびレーザ測距装置の位置座標をＸＺ平面にプロットしたグラフであり、図６（ｃ）は、これらの位置座標をＹＺ平面にプロットしたグラフであり、図６（ｄ）は、これらの位置座標をＸＹ平面にプロットしたグラフである。 航空機の飛行に伴ったレーザ測距装置の測定結果の変化を示す図である。図７（ａ）は、各時刻における角度データと距離データを示しており、図７（ｂ）は、図７（ａ）に示すデータをプロットしたグラフである。 左カメラと右カメラによって１秒ごとに撮影された画像を示す図である。 航空機が水平に飛行しているときに左カメラおよび右カメラに撮影されたレーザ測距装置の測距点を含む画像を示す図である。 航空機がピッチ方向に傾いて飛行しているときに左カメラおよび右カメラに撮影されたレーザ測距装置の測距点を含む画像を示す図である。 航空機が水平に飛行していると仮定して算出された画像上の測距点の座標と航空機がピッチ方向に傾いて飛行しているときの画像上の対応する点の座標との間の誤差を示す図である。 実施の形態１に係る航法装置の動作を示すフローチャートである。 測距点の３次元座標の算出結果を示す図である。 左カメラと右カメラの投影中心座標を示す図である。 左カメラと右カメラによって撮影された画像上の測距点の座標を示す図である。 時刻ｉの左カメラ画像上の測距点の座標と、これに対応する画像マッチングにより探索された時刻ｊの右カメラ画像上の点の座標を示す図である。 観測方程式の定数ベクトルを示す図である。 計画行列を示す図である。 図１８の計画行列の転置行列と計画行列の積を示す図である。 図１８の計画行列を転置したものと図１７の定数ベクトルの積によって得られる行列と、図１９の行列から計算した逆行列との積を示す図である。 姿勢角の補正量を示す図である。 最終的な姿勢角の推定結果を示す図である。 この発明の実施の形態２に係る測量システムの構成を示すブロック図である。

以下、この発明をより詳細に説明するため、この発明を実施するための形態について、添付の図面に従って説明する。
実施の形態１．
図１は、この発明の実施の形態１に係る測量システム１の構成を示すブロック図である。測量システム１は、航空機２から地形を測量するシステムであり、航空機２に搭載された左カメラ２０ａ、右カメラ２０ｂ、レーザ測距装置２１、ＧＮＳＳ装置２２およびメモリカード２３と、航法装置３とを備える。航法装置３は、飛行中の航空機２の姿勢を推定する装置であって、図１に示すように航空機２とは別に設けられる。ただし、航法装置３を航空機２に搭載してもよい。また、航空機２の姿勢は、航空機２のローリング方向、ピッチング方向、ヨーイング方向の姿勢角であるロール角ω、ピッチ角φ、ヨー角κの３つのパラメータによって特定される。

航空機２は、この発明における移動体を具体化したものであり、左カメラ２０ａ、右カメラ２０ｂ、レーザ測距装置２１、ＧＮＳＳ装置２２およびメモリカード２３を搭載して飛行することができる。例えば、搭乗したパイロットが操縦する航空機であってもよく、ＵＡＶ（Ｕｎｍａｎｎｅｄ Ａｅｒｉａｌ Ｖｅｈｉｃｌｅ）であってもよい。

左カメラ２０ａおよび右カメラ２０ｂは、この発明における第１の撮影部および第２の撮影部を具体化した構成要素であり、レーザ測距装置２１の測距点を含んだ地表面を撮影する。この場合、左カメラ２０ａおよび右カメラ２０ｂと、これらの撮影処理を制御する制御装置とを含む装置が、この発明における撮影装置に相当する。例えば、制御装置は、左カメラ２０ａおよび右カメラ２０ｂに指示して予め定めた周期で地表面の撮影を行い、撮影により得られた画像と撮影日時とを対応付けた画像データをメモリカード２３に記憶する。なお、予め定めた周期としては、１秒ごとに撮影を行うことが考えられる。

レーザ測距装置２１は、レーザ光の照射角度θを変えながらレーザ光を測量対象である地表面へ照射して地表面の測距点からの反射光を受信することで、レーザ光の照射基準点から測距点までの距離ｌを測定する。
また、レーザ測距装置２１は、距離ｌを測定するごとに、この距離ｌを示す距離データと、この距離ｌが得られたレーザ光の照射角度θを示す角度データとをメモリカード２３に記憶する。

ＧＮＳＳ装置２２は、この発明における座標測定装置を具体化した構成要素であって、レーザ測距装置２１におけるレーザ光の照射基準点の３次元座標を測定する。
また、ＧＮＳＳ装置２２は、予め定めた周期で照射基準点の３次元座標を示す座標データをメモリカード２３に記憶する。例えば、左カメラ２０ａおよび右カメラ２０ｂによる撮影に同期して１秒ごとに座標を測定する。
なお、ＧＮＳＳ装置２２と照射基準点の位置の違いは、ＧＮＳＳ装置２２の測定精度に対して許容範囲内である。すなわち、ＧＮＳＳ装置２２は照射基準点と同一の位置にあるものとし、さらに当該照射基準点の位置は航空機２の位置と同じ意味であるものとする。

メモリカード２３は、この発明における記憶装置を具体化した構成要素であり、航空機２の飛行中に測定された距離データ、角度データ、画像データ、座標データを記憶する。
メモリカード２３として、例えばＳＤ（Ｓｅｃｕｒｅ Ｄｉｇｉｔａｌ）メモリカードを使用してもよい。

図２は、航法装置３の機能構成を示すブロック図である。航法装置３は、図２に示すように、データ取得部３０、座標算出部３１、画像マッチング部３２および姿勢推定部３３を備える。データ取得部３０は、航空機２のメモリカード２３に蓄えられた距離データ、角度データ、座標データおよび画像データを取得する構成要素である。
例えば、データ取得部３０は、メモリカード２３のカードドライブに有線または無線で接続して上記データを読み出して取得する。

座標算出部３１は、データ取得部３０により取得された距離データ、角度データ、座標データおよび航空機２の姿勢角（ロール角ω、ピッチ角φ、ヨー角κ）に基づいて、画像データの画像上の測距点の座標を算出する。例えば、レーザ光の照射基準点から測距点までの距離ｌ、レーザ光の照射角度θ、レーザ光の照射基準点の３次元座標、航空機２のロール角ω、ピッチ角φ、ヨー角κに基づいて、測距点の３次元座標を算出する。そして、測距点の３次元座標と左カメラ２０ａおよび右カメラ２０ｂの投影中心座標に基づいて、左カメラ２０ａおよび右カメラ２０ｂに撮影された画像データの画像上の測距点の座標を算出する。
なお、航空機２のロール角ω、ピッチ角φ、ヨー角κは未知であり、最初は、姿勢角の補正量が算出されていないため、ロール角ω＝０、ピッチ角φ＝０、ヨー角κ＝０を初期値として座標の計算を行う。この座標算出の詳細は後述する。

画像マッチング部３２は、左カメラ２０ａおよび右カメラ２０ｂのうちの少なくとも一方により異なる撮影位置でそれぞれ撮影された画像データのペアの画像マッチングを行って、ペアの一方の画像データ（以下、第１の画像データと適宜記載する）の画像上の測距点の座標に対応する点を、ペアのもう一方の画像データ（以下、第２の画像データと適宜記載する）の画像から探索する。
画像マッチングの方法としては、２つの画像の類似度などを調べる周知のテンプレートマッチング法を用いることができる。例えば、第１の画像データをテンプレート画像とし、第２の画像データを対象画像データとして双方の画像データを比較し、テンプレート画像上の測距点の座標に対応する点を対象画像データの画像から探索する。

また、画像データのペアとしては、異なる撮影位置でそれぞれ撮影された画像データのペアであればよいので、航空機２の飛行中に時刻ｉで撮影された画像データとこの時刻ｉよりも進んだ時刻ｊで撮影された画像データとを用いてもよい。
さらに、画像データのペアとしては、左カメラ２０ａおよび右カメラ２０ｂとによって時刻ｉにそれぞれ撮影された画像データのペアであってもよい。
さらに、画像データのペアは、左カメラ２０ａおよび右カメラ２０ｂのうちの少なくとも一方によって時刻ｉと時刻ｉよりも進んだ時刻ｊとにそれぞれ撮影された画像データのペアであってもよい。
このような画像データのペアを用いることで、航空機２の姿勢に応じた画像上の被写体の変化を航空機２の姿勢推定に利用することができる。
換言すると、この発明では、航空機２の姿勢に応じた画像上の被写体の変化を航空機２の姿勢推定に利用するため、左カメラ２０ａと右カメラ２０ｂにスタビライザーが不要である。

姿勢推定部３３は、座標算出部３１によって算出されたペアのもう一方の画像データ（第２の画像データ）の画像上の測距点の座標と画像マッチング部３２によって探索された対応する点の座標との差が小さくなるように航空機２の姿勢角の値を補正して、航空機２の姿勢を推定する。前述したように、座標算出部３１は、姿勢角（ω，φ，κ）＝（０，０，０）を初期値として画像上の測距点の座標を算出している。このため、航空機２が傾いて飛行していた場合、座標算出部３１によって算出された上記測距点の座標と画像マッチング部３２によって探索された上記対応する点の座標が一致しなくなる。
そこで、姿勢推定部３３は、これら２点の座標の差が小さくなるように航空機２の姿勢角の値の補正量を算出し、２点の座標の差が最小となる姿勢角を最終的な航空機２の姿勢角と推定する。これにより、距離データ、角度データ、座標データ、画像データに基づいて、航空機２の姿勢角を高精度に推定することが可能となる。

図３は、航法装置３のハードウェア構成を示すブロック図である。図３（ａ）は航法装置３の機能を実現するハードウェアの処理回路１００を示し、図３（ｂ）は航法装置３の機能を実現するソフトウェアを実行するハードウェア構成を示している。また、図４は、航法装置３の動作の概要を示すフローチャートである。
航法装置３におけるデータ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の各機能は、処理回路により実現される。
すなわち、航法装置３は、図４に示す、距離データ、角度データ、座標データ、画像データを取得するステップＳＴ１、距離データ、角度データ、座標データおよび航空機２の姿勢角に基づいて、画像データの画像上の測距点の座標を算出するステップＳＴ２、異なる撮影位置でそれぞれ撮影された画像データのペアの画像マッチングを行って、ペアの一方の画像データの画像上の測距点の座標に対応する点を、ペアのもう一方の画像データの画像から探索するステップＳＴ３、ペアのもう一方の画像データの画像上の測距点の座標と画像マッチング部３２によって探索された対応する点の座標との差が小さくなるように航空機２の姿勢角の値を補正して、航空機２の姿勢を推定するステップＳＴ４を行うための処理回路を備えている。
処理回路は、専用のハードウェアであっても、メモリに格納されるプログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）であってもよい。

図３（ａ）に示すように、上記処理回路が専用のハードウェアの処理回路１００である場合、処理回路１００は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、または、これらを組み合わせたものが該当する。
また、データ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の各部の機能をそれぞれ処理回路で実現してもよいし、各部の機能をまとめて１つの処理回路で実現してもよい。

図３（ｂ）に示すように、上記処理回路がＣＰＵ１０１である場合、データ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の機能は、ソフトウェア、ファームウェアまたはソフトウェアとファームウェアとの組み合わせにより実現される。
ソフトウェアとファームウェアはプログラムとして記述され、メモリ１０２に格納される。ＣＰＵ１０１は、メモリ１０２に格納されたプログラムを読み出して実行することにより、各部の機能を実現する。
すなわち、航法装置３は、ＣＰＵ１０１によって実行されるときに、図４に示すステップＳＴ１からステップＳＴ４までの処理が結果的に実行されるプログラムを格納するためのメモリ１０２を備える。また、これらのプログラムは、データ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の手順または方法をコンピュータに実行させるものである。

ここで、メモリとは、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ ＲＯＭ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ ＥＰＲＯＭ）などの不揮発性または揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ）などが該当する。

なお、データ取得部３０、座標算出部３１、画像マッチング部３２、姿勢推定部３３の各機能について一部を専用のハードウェアで実現し、一部をソフトウェアまたはファームウェアで実現してもよい。
例えば、データ取得部３０は、専用のハードウェアの処理回路１００でその機能を実現し、座標算出部３１、画像マッチング部３２および姿勢推定部３３は、ＣＰＵ１０１が、メモリ１０２に格納されたプログラム実行することによりその機能を実現する。
このように、上記処理回路は、ハードウェア、ソフトウェア、ファームウェア、またはこれらの組み合わせによって前述の機能を実現することができる。

図５は、左カメラ２０ａと右カメラ２０ｂとレーザ測距装置２１の位置関係を模式的に示す図である。図５（ａ）は、左カメラ２０ａと右カメラ２０ｂとレーザ測距装置２１とからなるユニットの斜視図、図５（ｂ）は、当該ユニットをＸ軸方向に見た図である。図５（ｃ）は、当該ユニットをＺ軸方向に見た図、図５（ｄ）は、当該ユニットをＹ軸方向に見た図である。図５（ａ）に示すように、左カメラ２０ａは、レーザ測距装置２１の左側に延びるアーム２０ｃの端部に取り付けられ、右カメラ２０ｂは、レーザ測距装置２１の右側に延びるアーム２０ｄの端部に取り付けられている。両側のアーム２０ｃ，２０ｄのそれぞれの長さは、例えば、１ｍとする。また、左カメラ２０ａおよび右カメラ２０ｂの撮影方向は、航空機２の直下方向（Ｚ軸方向）に向いている。

レーザ測距装置２１は、図５（ｂ）に示すように、レーザ光の照射角度θを変えながら照射基準点２１ａからレーザ光を地表面の測距点Ｐ_０に照射し測距点Ｐ_０からの反射光を受信して、照射基準点２１ａから測距点Ｐ_０までの距離ｌを測定する。なお、照射基準点２１ａから直下にレーザ光を照射したときの照射角度θを９０度とする。
左カメラ２０ａおよび右カメラ２０ｂは、図５（ｃ）に示すように、矩形の撮影範囲で撮影を行うことにより、図８を用いて後述するような画像データを得る。

ここで、照射基準点２１ａの位置と航空機２の位置は同じであると仮定する。従って、航空機２がＸ軸方向に水平飛行する場合、左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１からなるユニットも、図５（ｄ）に示すようにＸ軸方向に移動する。
しかしながら、実際の飛行環境では水平飛行を意図しても、航空機２は、風などの影響によって直線状の進路をとることはできない。すなわち、航空機２は、ローリング方向、ピッチング方向、ヨーイング方向のそれぞれに傾いた状態で飛行する。

図６は、航空機の飛行に伴う左カメラ２０ａと右カメラ２０ｂとレーザ測距装置２１の位置変化を示す図である。図６（ａ）は、レーザ測距装置２１の位置座標データを示している。図６（ｂ）は、左カメラ２０ａと右カメラ２０ｂとレーザ測距装置２１の位置座標をＸＺ平面にプロットしたグラフである。図６（ｃ）は、これらの位置座標をＹＺ平面にプロットしたグラフであり、図６（ｄ）は、これらの位置座標をＸＹ平面にプロットしたグラフである。図６（ａ）に示すように、航空機２は、時刻ｔ＝０でＸＹＺ座標系の原点（０，０，０）に位置し、図６（ｂ）に示すＸ軸方向に一定の速度で３秒間の水平飛行を行っている。

なお、レーザ測距装置２１の位置座標は、ＧＮＳＳ装置２２により１秒ごとに測定された照射基準点２１ａの位置座標である。左カメラ２０ａと右カメラ２０ｂの位置座標は、照射基準点２１ａの位置から１ｍのアーム２０ｃ，２０ｄの分だけＹ軸方向に離れているものとして計算している。

図６（ｂ）から図６（ｄ）において、大きな四角形のプロットは、時刻ｔ＝０における左カメラ２０ａの位置座標である。大きな逆三角形のプロットは、時刻ｔ＝０における右カメラ２０ｂの位置座標であり、小さな四角形のプロットは、時刻ｔ＝０におけるレーザ測距装置２１の位置座標である。
また、点のある大きな四角形のプロットは、時刻ｔ＝１における左カメラ２０ａの位置座標である。点のある大きな逆三角形のプロットは、時刻ｔ＝１における右カメラ２０ｂの位置座標であり、点のある小さな四角形のプロットは、時刻ｔ＝１におけるレーザ測距装置２１の位置座標である。
大きな円形のプロットは、時刻ｔ＝２における左カメラ２０ａの位置座標である。大きな三角形のプロットは、時刻ｔ＝２における右カメラ２０ｂの位置座標であり、小さな円形のプロットは、時刻ｔ＝２におけるレーザ測距装置２１の位置座標である。
点のある大きな円形のプロットは、時刻ｔ＝３における左カメラ２０ａの位置座標である。点のある大きな三角形のプロットは、時刻ｔ＝３における右カメラ２０ｂの位置座標であり、点のある小さな円形のプロットは、時刻ｔ＝３におけるレーザ測距装置２１の位置座標である。

図５を用いて説明したように、左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１からなるユニットは、左カメラ２０ａ、レーザ測距装置２１、右カメラ２０ｂの順でＹ軸方向に沿って連結されている。このため、図６（ｂ）に示すように左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１の各位置はＹ軸方向に重なっている。
一方、時刻ｔ＝１を過ぎると、左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１の位置がＺ軸方向に逸れている。

また、図６（ｃ）および図６（ｄ）に示すように、時刻ｔ＝１〜２において、左カメラ２０ａ、右カメラ２０ｂおよびレーザ測距装置２１の位置は、Ｙ軸方向に０．５ｍ逸れてから、時刻ｔ＝３で原点と同じ位置に戻っている。
図６（ｂ）から図６（ｄ）までのグラフを考慮すると、航空機２の位置が３秒間でＹ軸方向とＺ軸方向にずれており、航空機２が傾いて飛行したことがわかる。

図７は航空機２の飛行に伴ったレーザ測距装置２１の測定結果の変化を示す図であり、図６の状態で航空機２が飛行した場合における測定結果を示している。
図７（ａ）は、各時刻における角度データと距離データを示しており、図７（ｂ）は、図７（ａ）に示すデータをプロットしたグラフである。
また、図８は、左カメラ２０ａと右カメラ２０ｂによって１秒ごとに撮影された画像を示す図であり、図６の状態で航空機２が飛行したときに撮影された画像を示している。

図７（ａ）に示すように、レーザ測距装置２１の測定結果は、測定時刻ｔ、照射角度θおよび距離ｌを１レコードとしてメモリカード２３に記憶される。ここでは、レーザ測距装置２１が１秒当たり４回の測定を行うものとする。また、照射角度θは、図５（ｂ）に示した照射基準点２１ａの直下を９０度とする。
レーザ測距装置２１は、図５（ｂ）に示したように、Ｘ軸の正方向から見てＸ軸を中心として時計回りに照射基準点２１ａを１８度ずつ回転させてレーザ光を走査する。
図７（ｂ）において、白い三角形のプロットは、時刻ｔ＝０．００〜０．１５における角度データおよび距離データ、黒い三角形のプロットは時刻ｔ＝１．００〜１．１５における角度データおよび距離データ、白い四角形のプロットは時刻ｔ＝２．００〜２．１５における角度データおよび距離データである。

図６で説明したように航空機２が傾いて飛行した場合、レーザ光もレーザ測距装置２１から傾いて照射される。すなわち、図７（ａ）および図７（ｂ）に示すような距離データおよび角度データも、航空機２の姿勢角に応じて変化する。
また、航空機２が傾いて飛行すると、左カメラ２０ａおよび右カメラ２０ｂの撮像方向も傾く。これにより、図８に示すような左カメラ２０ａに撮影された左カメラ画像および右カメラ２０ｂに撮影された右カメラ画像も、航空機２の姿勢角に応じて変化する。
従って、航空機２が水平に飛行したと仮定して距離データ、角度データ、座標データ、画像データを用いて算出した測距点の画像上の座標と、航空機２が傾いて飛行したときの同一の測距点の座標との間には、航空機２の姿勢角に応じた誤差が生じる。
そこで、この発明では、上記誤差が小さくなるように姿勢角を補正していき、上記誤差が最小となる姿勢角を航空機２の姿勢角の推定値とする。以下、この発明における姿勢角の推定処理の概要を説明する。

図９は、航空機２が水平に飛行しているときに左カメラ２０ａおよび右カメラ２０ｂに撮影されたレーザ測距装置２１の測距点Ｐ_０を含む画像１００ａ，１００ｂを示す図である。図９において、航空機２は、Ｘ軸の正方向に沿って水平に飛行したと仮定する。
ここでは、１秒ごとに、左カメラ２０ａおよび右カメラ２０ｂによって航空機２の下方が撮影され、レーザ測距装置２１が、照射角度θを９０度として機体の直下にある測距点Ｐ_０との距離を測定する。

また、左カメラ２０ａによって時刻ｔ＝０に撮影された画像１００ａ上の測距点Ｐ_０の座標Ｐ_０ａは、時刻ｔ＝０におけるレーザ光の照射基準点２１ａの３次元座標と航空機２の姿勢角とから算出することができる。
同様に、右カメラ２０ｂによって時刻ｔ＝１に撮影された画像１００ｂ上の測距点Ｐ_０の座標Ｐ_０ｂは、時刻ｔ＝１における照射基準点２１ａの３次元座標と航空機２の姿勢角とから算出することができる。
なお、図９では、航空機２が水平に飛行すると仮定しているので、姿勢角は０である。

図１０は、航空機２がピッチ方向に傾いて飛行しているときに左カメラ２０ａおよび右カメラ２０ｂに撮影されたレーザ測距装置２１の測距点Ｐ_０を含む画像１００ａ，１００ｃを示す図である。図１０において、時刻ｔ＝１に航空機２がピッチ角φだけ傾いて飛行したと仮定している。このとき、右カメラ２０ｂによって時刻ｔ＝１に撮影された画像１００ｃでは、鎖線で示すように、図９の場合よりも被写体が全体的に右寄りになった画像となる。

図１１は、航空機２が水平に飛行していると仮定して算出された画像上の測距点Ｐ_０の座標と航空機２がピッチ方向に傾いて飛行しているときの画像上の対応する点の座標との間の誤差を示す図である。図１１における画像１００ｃ上の測距点Ｐ_０点の座標Ｐ_０ｂは、図９と同様に航空機２が時刻ｔ＝１においても傾き無く飛行していると仮定して算出した座標である。一方、画像１００ｃ上の点の座標Ｐ_０ｂ’は、左カメラ２０ａによって時刻ｔ＝０に撮影された画像１００ａ上の測距点Ｐ_０の座標Ｐ_０ａに対応する点を、画像マッチングによって画像１００ｃから探索した結果として得られた点の座標である。

このような座標Ｐ_０ｂと座標Ｐ_０ｂ’との差Δｕ_Ｌは、実際の飛行では航空機２が傾いていたにも拘わらず、傾きなく飛行したと仮定して画像１００ｃ上の測距点Ｐ_０の座標Ｐ_０ｂを算出したために生じたものである。従って、差Δｕ_Ｌが最小となる航空機２の姿勢角は実際の航空機２の姿勢を適切に表した推定結果となる。
例えば、図１１の場合、航空機２をピッチ角φだけ傾けたときに差Δｕ_Ｌが最小となるため、ピッチ角φが姿勢の推定結果として得られる。
なお、実際の航空機２では、ピッチング方向に加え、ローリング方向とヨーイング方向のそれぞれにも傾くが、この場合も同様にロール角ω、ヨー角κを推定すればよい。

次に動作について説明する。
図１２は、実施の形態１に係る航法装置３の動作を示すフローチャートであり、飛行中の航空機２の姿勢角を推定する一連の処理を示している。
以降では、航空機２の姿勢角がロール角ω、ピッチ角φ、ヨー角κの３つのパラメータで表され、これらの角度は１秒ごとに推定される。
便宜上、時刻ｔ＝０および時刻ｔ＝３の姿勢角（ω，φ，κ）を（０，０，０）と仮定するので、時刻ｔ＝１および時刻ｔ＝２における未知の姿勢角（ω，φ，κ）を推定すればよい。すなわち、未知数である計６つの姿勢角を推定する。

まず、データ取得部３０が、航空機２に搭載されたメモリカード２３から距離データ、角度データ、座標データ、画像データを読み出して取得する（ステップＳＴ１ａ）。
距離データは、レーザ測距装置２１によって測定されたレーザ光の照射基準点２１ａから測距点Ｐ_０までの距離ｌを示すデータであり、角度データは、レーザ光の照射角度θを示すデータである。座標データは、ＧＮＳＳ装置２２によって測定されたレーザ光の照射基準点２１ａの３次元座標（Ｘ_０，Ｙ_０，Ｚ_０）である。画像データは、左カメラ２０ａと右カメラ２０ｂによって撮影された撮影対象に測距点Ｐ_０を含む画像データである。
このように航空機２の飛行中にメモリカード２３に蓄積されたデータを用いることで、航空機２の姿勢を飛行終了後に推定することができ、推定された姿勢角を用いて測量結果を補正することも可能である。

次に、座標算出部３１が、データ取得部３０により取得された距離データ、角度データ、座標データおよび航空機２の姿勢角（ω，φ，κ）の設定値に基づいて、下記式（１）に従い、測距点Ｐ_０ごとの３次元座標（Ｘ，Ｙ，Ｚ）を算出する（ステップＳＴ２ａ）。
下記式（１）において、ａ_１１〜ａ_３３は、航空機２の姿勢に応じたレーザ測距装置２１、左カメラ２０ａおよび右カメラ２０ｂの傾きを表す３×３の回転行列の要素である。

また、上記式（１）において、（Ｘ_０，Ｙ_０，Ｚ_０）は、上記座標データが示すレーザ光の照射基準点２１ａの３次元座標である。θは、上記角度データが示すレーザ光の照射角度であり、ｌは、上記距離データが示すレーザ光の照射基準点２１ａから測距点Ｐ_０までの距離である。なお、照射角度θは、航空機２の鉛直下向きを９０度としている。
また、姿勢角の設定値は、航空機２が水平に飛行していると仮定して（ω，φ，κ）＝（０，０，０）とする。
図６（ａ）に示した時刻ｔ＝０〜２における座標データおよび図７（ａ）に示した時刻ｔ＝０〜２における測距点Ｐ_０ごとの角度データと距離データを用いて、測距点Ｐ_０ごとの３次元座標（Ｘ，Ｙ，Ｚ）を算出した結果を図１３に示す。

次に、座標算出部３１が、座標データと姿勢角の設定値に基づいて、下記式（２）と下記式（３）に従って、１秒ごとの左カメラ２０ａの投影中心座標（Ｘ_Ｌ，Ｙ_Ｌ，Ｚ_Ｌ）および右カメラ２０ｂの投影中心座標（Ｘ_Ｒ，Ｙ_Ｒ，Ｚ_Ｒ）を算出する（ステップＳＴ３ａ）。
姿勢角の設定値を（ω，φ，κ）＝（０，０，０）とし、図６（ａ）に示した時刻ｔ＝０〜２における座標データを用いて投影中心座標を算出した結果を図１４に示す。

次に、座標算出部３１は、座標データ、姿勢角の設定値、測距点Ｐ_０の３次元座標、左カメラ２０ａと右カメラ２０ｂの各投影中心座標に基づいて、下記式（４）と下記式（５）に従って、左カメラ画像上の測距点Ｐ_０の座標（ｘ_Ｌ，ｙ_Ｌ）と右カメラ画像上の測距点Ｐ_０の座標（ｘ_Ｒ，ｙ_Ｒ）を算出する（ステップＳＴ４ａ）。
下記式（４）と下記式（５）において、ｃは、左カメラ２０ａと右カメラ２０ｂの焦点距離である。

ただし、

図６（ａ）に示した時刻ｔ＝０〜２における座標データ、図１３に示した測距点Ｐ_０ごとの３次元座標（Ｘ，Ｙ，Ｚ）および図１４に示した投影中心座標を用いて、左カメラ画像上の測距点Ｐ_０の座標（ｘ_Ｌ，ｙ_Ｌ）と右カメラ画像上の座標（ｘ_Ｒ，ｙ_Ｒ）を算出した結果を図１５に示す。

次に、画像マッチング部３２は、データ取得部３０により取得された画像データから、時刻ｉに撮影された左カメラ画像とこの時刻ｉよりも＋１だけ進んだ時刻ｊに撮影された右カメラ画像をペアとして抽出する。このように画像マッチングの対象となる画像データのペアを抽出する処理をペアリングと呼ぶ。このペアリングによって、異なる撮影位置で撮影された画像データのペアが得られる。
続いて、画像マッチング部３２は、時刻ｉの左カメラ画像と時刻ｊの右カメラ画像とのテンプレートマッチングを行うことにより、座標算出部３１によって算出された時刻ｉの左カメラ画像上の測距点Ｐ_０の座標（ｘ_Ｌ，ｙ_Ｌ）に対応する点を、時刻ｊの右カメラ画像から探索する（ステップＳＴ５ａ）。

図１６は、時刻ｉの左カメラ画像上の測距点Ｐ_０の座標と、これに対応するテンプレートマッチングで得られた時刻ｊの右カメラ画像上の点の座標との対応関係を示している。
ＳＣＡＮ_ｘ（ｘ_Ｌｉ，ｙ_Ｌｉ）は、時刻ｉの左カメラ画像上の測距点Ｐ_０の座標（ｘ_Ｌｉ，ｙ_Ｌｉ）を中心とした小領域について時刻ｊの右カメラ画像上でテンプレートマッチングを行って得られた対応する点のｘ座標値である。また、ＳＣＡＮ_ｙ（ｘ_Ｌｉ，ｙ_Ｌｉ）は、同様にテンプレートマッチングを行って得られた対応する点のｙ座標値である。

図１６に示すように、座標算出部３１によって算出された時刻ｊの右カメラ画像上の測距点Ｐ_０の座標（ｘ_Ｒｊ，ｙ_Ｒｊ）と、画像マッチング部３２によって探索された対応点の座標（ＳＣＡＮ_ｘ（ｘ_Ｌｉ，ｙ_Ｌｉ），ＳＣＡＮ_ｙ（ｘ_Ｌｉ，ｙ_Ｌｉ））との間には乖離が生じている。これは、航空機２の姿勢角（ω，φ，κ）が０以外の値であることに起因する。
すなわち、飛行中の航空機２の姿勢を示す適切な姿勢角（ω，φ，κ）を設定して、図１６に示した座標を再計算すれば、上記座標（ｘ_Ｒｊ，ｙ_Ｒｊ）と上記座標（ＳＣＡＮ_ｘ（ｘ_Ｌｉ，ｙ_Ｌｉ），ＳＣＡＮ_ｙ（ｘ_Ｌｉ，ｙ_Ｌｉ））は一致する。
従って、上記座標の差が最小化される姿勢角が航空機２の姿勢角の推定値となる。

図１２の説明に戻る。以降では、姿勢推定部３３が、非線形最小二乗法に基づく手順で航空機２の姿勢角を推定する場合について説明する。
姿勢推定部３３は、時刻ｊの右カメラ画像上の測距点Ｐ_０の座標（ｘ_Ｒｊ，ｙ_Ｒｊ）と、画像マッチング部３２によって探索された対応点の座標（ＳＣＡＮ_ｘ（ｘ_Ｌｉ，ｙ_Ｌｉ），ＳＣＡＮ_ｙ（ｘ_Ｌｉ，ｙ_Ｌｉ））との差が小さくなる姿勢角の補正量を算出する（ステップＳＴ６ａ）。例えば、下記式（６）に示す観測方程式ｖ_ｘ，ｖ_ｙを用いる。

上記式（６）において、チルダω（^〜ω）、チルダφ（^〜φ）、チルダκ（^〜κ）は、未知数であるロール角ω、ピッチ角φ、ヨー角κの近似解である。δω、δφ、δκは近似解チルダω、チルダφ、チルダκに対する補正量である。
また、∂Ｆ_ｘ／∂ωは、Ｆ_ｘのロール角ωでの偏微分、∂Ｆ_ｘ／∂φは、Ｆ_ｘのピッチ角φでの偏微分されたＦ_ｘ、∂Ｆ_ｘ／∂κは、Ｆ_ｘのヨー角κでの偏微分である。これらの偏微分は、近似解チルダω、チルダφ、チルダκを代入して値が得られる係数である。
同様に、∂Ｆ_ｙ／∂ωは、Ｆ_ｙのロール角ωでの偏微分、∂Ｆ_ｙ／∂φは、Ｆ_ｙのピッチ角φでの偏微分、∂Ｆ_ｙ／∂κは、Ｆ_ｙのヨー角κでの偏微分である。これらの偏微分も近似解チルダω、チルダφ、チルダκを代入して値が得られる係数である。
チルダＦ_ｘ（^〜Ｆ_ｘ）は、Ｆ_ｘに対してＳＣＡＮ_ｘ（ｘ_Ｌ，ｙ_Ｌ）とｘ_Ｒの近似解を代入して得られる値であり、チルダＦ_ｙ（^〜Ｆ_ｙ）は、Ｆ_ｙに対してＳＣＡＮ_ｙ（ｘ_Ｌ，ｙ_Ｌ）とｙ_Ｒの近似解を代入して得られる値である。

図１６に示したデータを用いる場合、時刻ｔ＝ｉの左カメラ画像と時刻ｔ＝ｉ＋１の右カメラ画像の各ペアにおいて４個ずつ、ｘとｙについての観測方程式ｖ_ｘ，ｖ_ｙが得られる。従って、観測方程式の数は３×４×２＝２４となる。このときの観測方程式の定数ベクトルを図１７に示す。

次に、姿勢推定部３３は、観測方程式を６つの未知数のそれぞれについて偏微分する。例えば、時刻ｔ＝１におけるロール角ω、ピッチ角φ、ヨー角κで偏微分してから、時刻ｔ＝２におけるロール角ω、ピッチ角φ、ヨー角κで偏微分する。
このようにして観測方程式ごとに算出された偏微分係数からなる２４×６の計画行列を図１８に示す。

続いて、姿勢推定部３３は、この計画行列の転置行列と計画行列との積を算出する。図１８に示した計画行列を用いた算出結果を図１９に示す。
さらに、姿勢推定部３３は、この計画行列の転置行列と図１７に示した定数ベクトルの積を算出する。この算出の結果を図２０に示す。
この後、姿勢推定部３３は、図１９に示す行列から算定した逆行列と図２０に示すベクトルの積を算出する。この算出結果が、図２１に示す姿勢角の補正量（δω，δφ，δκ）である。
なお、最初は、航空機２が傾き無く水平に飛行していると仮定して、姿勢角（ω，φ，κ）の初期値として（０，０，０）を設定しているので、上記補正量がそのまま姿勢角の近似解となる。

姿勢推定部３３は、上記のように算出した補正量を従前の近似解に加算して補正し、補正後の近似解を姿勢角の設定値に決定する（ステップＳＴ７ａ）。このとき、予め定めた繰り返し回数が経過していなければ（ステップＳＴ８ａ；ＮＯ）、姿勢推定部３３は、上記と同様な座標計算を行うように座標算出部３１に指示する。
これにより、座標算出部３１が、補正後の近似解を姿勢角の設定値としてステップＳＴ２ａからステップＳＴ４ａまでの処理を行い、画像マッチング部３２がステップＳＴ５ａの処理を行う。

上記のような一連の処理を繰り返して算出される補正量（δω，δφ，δκ）によって、時刻ｊの右カメラ画像上の測距点Ｐ_０の座標（ｘ_Ｒｊ，ｙ_Ｒｊ）と画像マッチング部３２により探索された対応点の座標（ＳＣＡＮ_ｘ（ｘ_Ｌｉ，ｙ_Ｌｉ），ＳＣＡＮ_ｙ（ｘ_Ｌｉ，ｙ_Ｌｉ））との差が小さくなっていく。
上記一連の処理が予め定めた繰り返し回数だけ実行されて（ステップＳＴ８ａ；ＹＥＳ）、上記座標の差が最小となる補正量が得られると、姿勢推定部３３は、この補正量で補正した近似解を、最終的な姿勢角の推定結果として出力する（ステップＳＴ９ａ）。
このようにして得られた時刻ｔ＝０．００〜３．００の姿勢角の推定結果を図２２に示す。

これまで、画像データのペアとして、時刻ｉの左カメラ画像と時刻ｊ（＝ｉ＋１）の右カメラ画像とのペアを用いる場合を示したが、時刻ｉの左カメラ画像と時刻ｉの右カメラ画像とのペアであってもよい。すなわち、この発明は、異なる撮影位置で撮影された画像データのペアであればよい。
なお、異なる位置のカメラにより撮影された画像間で対応点を探索して観測対象までの距離および奥行き情報などの３次元情報を取得するステレオ画像処理には、固定ステレオと呼ばれる方法と、モーションステレオと呼ばれる方法がある。
固定ステレオは、２つのカメラを間隔を空けて配置して撮影するものである。時刻ｉの左カメラ画像と時刻ｉの右カメラ画像のペアリングは、固定ステレオに相当する。
モーションステレオでは、カメラを移動させて異なる撮影位置から撮影する。時刻ｉの左カメラ画像と時刻ｊ（＝ｉ＋１）の右カメラ画像のペアリングは、モーションステレオに相当する。

また、これまでの説明では、左カメラ２０ａと右カメラ２０ｂを利用する構成を示したが、１台のカメラであってもよい。この場合、時刻ｉのカメラ画像と時刻ｊ（＝ｉ＋１）のカメラ画像のペアを用いることになる。

さらに、これまで未知数が時刻ごとの姿勢角の３つのパラメータ（ω，φ，κ）である場合を示したが、航空機２の位置座標（Ｘ，Ｙ，Ｚ）を加えた６つのパラメータとしてもよく、カメラの焦点距離ｃなどの内部パラメータを含めてよい。

以上のように、実施の形態１に係る航法装置３では、異なる撮影位置でそれぞれ撮影された画像間の対応する点の座標が航空機２の姿勢によってずれることに着目し、これらの座標の差が小さくなるように航空機２の姿勢を示すパラメータ（ω，φ，κ）の値を補正して航空機２の姿勢を推定する。これにより、ＩＭＵ、スタビライザーを使用しなくても航空機２の姿勢を推定することができる。
また、測距点Ｐ_０が撮影された画像データに加え、レーザ光の照射基準点２１ａから測距点Ｐ_０までの距離ｌ、レーザ光の照射角度θ、レーザ光の照射基準点２１ａの３次元座標（Ｘ，Ｙ，Ｚ）を使用して航空機２の姿勢を推定するので、航空機２の姿勢を精度よく推定することが可能である。

また、実施の形態１に係る航法装置３において、画像データのペアは、航空機２の飛行中に、時刻ｉに撮影された画像データとこの時刻ｉよりも進んだ時刻ｊに撮影された画像データである。このような画像データのペアを用いることにより、航空機２の姿勢に応じた画像上の被写体の変化を航空機２の姿勢推定に利用することができる。

さらに、実施の形態１に係る航法装置３において、画像データのペアは、左カメラ２０ａと右カメラ２０ｂによって時刻ｉに撮影された画像データのペア、または、左カメラ２０ａおよび右カメラ２０ｂのうちの少なくとも一方によって時刻ｉと当該時刻ｉよりも進んだ時刻ｊとに撮影された画像データのペアである。
このような画像データを用いても、航空機２の姿勢に応じた画像上の被写体の変化を航空機２の姿勢推定に利用することができる。

さらに、実施の形態１に係る航法装置３において、ヨー角κ＝０を初期値として座標の計算を行うように実施したが、ヨー角κの近似解は、ＧＮＳＳ装置２２によって測定された時系列の３次元座標から得ることもできる。従って、ヨー角κの初期値としてＧＮＳＳ装置２２によって測定された時系列の３次元座標から算定された値を用いてもよい。

さらに、実施の形態１に係る測量システム１は、航空機２に搭載されたメモリカード２３を備える。データ取得部３０は、メモリカード２３に記憶された距離データ、角度データ、座標データおよび画像データを読み出して取得する。
このように航空機２の飛行中にメモリカード２３に蓄積されたデータを用いることで、航空機２の姿勢を飛行終了後に推定でき、推定された姿勢角を用いて測量結果を補正することもできる。

実施の形態２．
図２３は、この発明の実施の形態２に係る測量システム１Ａの構成を示すブロック図である。測量システム１Ａは、航空機２Ａから地形を測量するシステムであり、航空機２Ａに搭載された左カメラ２０ａ、右カメラ２０ｂ、レーザ測距装置２１、ＧＮＳＳ装置２２および無線通信装置２４と、航法装置３とを備える。
無線通信装置２４は、航空機２Ａの飛行中に得られた距離データ、角度データ、座標データおよび画像データを航法装置３へ送信する。
航法装置３は、図２３に示すように、航空機２Ａとは別に設けられる。ただし、航法装置３を航空機２Ａに搭載してもよい。

航法装置３のデータ取得部３０は、無線通信装置２４によって送信された距離データ、角度データ、座標データおよび画像データを受信して取得する。
航法装置３は、このようにして取得された上記データに基づいて、実施の形態１と同様の処理で航空機２Ａの姿勢を推定する。

以上のように、実施の形態２に係る測量システム１Ａは、航空機２に搭載された無線通信装置２４を備える。データ取得部３０は、無線通信装置２４によって送信された距離データ、角度データ、座標データおよび画像データを受信して取得する。
このように無線通信装置２４から無線送信されたデータを用いることで、航空機２Ａの飛行中に姿勢を推定することができる。推定された姿勢角を用いて航空機２Ａの飛行中に測量結果を補正することも可能である。

なお、これまでの説明では、この発明における移動体が航空機２のような飛行体である場合を示したが、これに限定されるものではない。例えば、この発明に係る航法装置は、モービルマッピングシステムに適用することができ、このシステムを搭載する車両が移動体となる。また、この発明に係る航法装置は、鉄道車両、船舶、ロボットを移動体とし、これらの姿勢を推定する装置として利用することも可能である。このような移動体においても、姿勢を示すパラメータは同様に移動体の姿勢角（ω，φ，κ）を用いることができ、場合によっては位置情報をパラメータに含めてもよい。

なお、本発明はその発明の範囲内において、各実施の形態の自由な組み合わせあるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。

この発明に係る航法装置は、ＩＭＵ、スタビライザーを搭載しない構成で移動体の姿勢を精度よく推定することができるので、例えば、ＵＡＶの航法装置として好適である。

１，１Ａ 測量システム、２，２Ａ 航空機、３ 航法装置、２０ａ 左カメラ、２０ｂ 右カメラ、２０ｃ，２０ｄ アーム、２１ レーザ測距装置、２１ａ 照射基準点、２２ ＧＮＳＳ装置、２３ メモリカード、２４ 無線通信装置、３０ データ取得部、３１ 座標算出部、３２ 画像マッチング部、３３ 姿勢推定部、１００ 処理回路、１００ａ〜１００ｃ 画像、１０１ ＣＰＵ、１０２ メモリ。

Claims (6)

1. 移動体に搭載されたレーザ測距装置により測定されたレーザ光の照射基準点から測距点までの距離を示す距離データと前記レーザ光の照射角度を示す角度データ、前記移動体に搭載された座標測定装置により測定された前記レーザ光の照射基準点の３次元座標を示す座標データおよび前記移動体に搭載された撮影装置により撮影された撮影対象に測距点を含む画像データを取得するデータ取得部と、
   前記データ取得部により取得された前記距離データと前記角度データ、前記座標データおよび前記移動体の姿勢を示すパラメータに基づいて、前記画像データの画像上の測距点の座標を算出する座標算出部と、
   前記撮影装置により異なる撮影位置でそれぞれ撮影された前記画像データのペアの画像マッチングを行って、前記座標算出部により算出された前記ペアの一方の前記画像データの画像上の測距点の座標に対応する点を前記ペアのもう一方の前記画像データの画像から探索する画像マッチング部と、
   前記座標算出部により算出された前記ペアのもう一方の前記画像データの画像上の測距点の座標と前記画像マッチング部により探索された前記対応する点の座標との差が小さくなるように前記移動体の姿勢を示すパラメータの値を補正して当該移動体の姿勢を推定する姿勢推定部と
   を備えたことを特徴とする航法装置。
2. 前記ペアは、前記移動体の移動中に、前記撮影装置により時刻ｉに撮影された前記画像データと前記時刻ｉよりも進んだ時刻ｊに撮影された前記画像データであることを特徴とする請求項１記載の航法装置。
3. 前記撮影装置は、前記移動体に搭載された第１の撮影部および第２の撮影部を備え、
   前記ペアは、前記第１の撮影部と前記第２の撮影部により時刻ｉにそれぞれ撮影された前記画像データまたは前記第１の撮影部および前記第２の撮影部のうちの少なくとも一方により前記時刻ｉと当該時刻ｉよりも進んだ時刻ｊとにそれぞれ撮影された前記画像データであることを特徴とする請求項１記載の航法装置。
4. 移動体に搭載されて、レーザ光の照射基準点から測距点までの距離を示す距離データと前記レーザ光の照射角度を示す角度データを測定するレーザ測距装置と、
   前記移動体に搭載されて、前記レーザ光の照射基準点の３次元座標を示す座標データを測定する座標測定装置と、
   前記移動体に搭載されて、撮影対象に測距点を含む画像データを撮影する撮影装置と、
   前記距離データ、前記角度データ、前記座標データおよび前記画像データを取得するデータ取得部と、前記データ取得部により取得された前記距離データと前記角度データ、前記座標データおよび前記移動体の姿勢を示すパラメータに基づいて、前記画像データの画像上の測距点の座標を算出する座標算出部と、前記撮影装置により異なる撮影位置でそれぞれ撮影された前記画像データのペアの画像マッチングを行って、前記座標算出部により算出された前記ペアの一方の前記画像データの画像上の測距点の座標に対応する点を前記ペアのもう一方の前記画像データの画像から探索する画像マッチング部と、前記座標算出部により算出された前記ペアのもう一方の前記画像データの画像上の測距点の座標と前記画像マッチング部により探索された前記対応する点の座標との差が小さくなるように前記移動体の姿勢を示すパラメータの値を補正して当該移動体の姿勢を推定する姿勢推定部とを有する航法装置と
   を備えたことを特徴とする測量システム。
5. 前記移動体に搭載され、前記距離データ、前記角度データ、前記座標データおよび前記画像データを記憶する記憶装置を備え、
   前記データ取得部は、前記記憶装置に記憶された前記距離データ、前記角度データ、前記座標データおよび前記画像データを読み出して取得することを特徴とする請求項４記載の測量システム。
6. 前記移動体に搭載され、前記距離データ、前記角度データ、前記座標データおよび前記画像データを送信する無線通信装置を備え、
   前記データ取得部は、前記無線通信装置により送信された前記距離データ、前記角度データ、前記座標データおよび前記画像データを受信して取得することを特徴とする請求項４記載の測量システム。

Images