特開 2021-173577 位置姿勢計測方法及び位置姿勢計測プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】特開2021-173577(P2021-173577A)

(43)【公開日】2021年11月1日

(54)【発明の名称】位置姿勢計測方法及び位置姿勢計測プログラム

(51)【国際特許分類】

   G01C 15/00 20060101AFI20211004BHJP

   G05D 1/10 20060101ALI20211004BHJP

   G01C 21/16 20060101ALI20211004BHJP

【ＦＩ】

   G01C15/00 104C

   G01C15/00 103E

   G05D1/10

   G01C21/16

【審査請求】未請求

【請求項の数】9

【出願形態】ＯＬ

【全頁数】25

(21)【出願番号】特願2020-75993(P2020-75993)

(22)【出願日】2020年4月22日

(71)【出願人】

【識別番号】517133415

【氏名又は名称】株式会社ＦＡＤｒｏｎｅ

(74)【代理人】

【識別番号】100095430

【弁理士】

【氏名又は名称】廣澤 勲

(72)【発明者】

【氏名】島次 武雄

【テーマコード（参考）】

2F129

5H301

【Ｆターム（参考）】

2F129AA11

2F129BB09

2F129BB21

2F129BB22

2F129BB26

2F129BB33

2F129BB63

2F129GG17

5H301AA06

5H301BB10

5H301CC04

5H301CC07

5H301CC10

5H301DD01

5H301DD06

5H301DD15

5H301GG09

5H301GG17

5H301HH03

(57)【要約】

【課題】インフラ構造物等を調査する用途に好適で、調査用の移動体の位置を容易且つ高精度に計測できる位置姿勢計測方法及び位置姿勢計測プログラムを提供する。
【解決手段】移動体１０の初期位置の絶対座標を示す初期位置座標情報と、構造物K1の特定部位P1の絶対座標を示す特定部位座標情報（構造物情報）を取得する既知情報取得処理S1を備える。角速度センサ１２及び加速度センサ１４の出力信号から位置移動量を算出する位置移動量算出処理S6s，S6uを備える。特定部位撮像指令を受けて撮像装置１６で特定部位P1を撮像し、画像データを取得する特定部位撮像処理S7を備える。初期位置座標情報及び位置移動量から導出した第１の位置関係情報と、画像データから導出した第２の位置関係情報との差から位置補正量を算出する位置補正量算出処理S8を備える。初期位置座標情報、位置移動量及び位置補正量から位置を算出する位置算出処理S9を備える。
【選択図】図３

【特許請求の範囲】

【請求項1】

ある場所に設置された構造物の状態を、移動体を用いて調査するシステムおいて、前記移動体の位置姿勢計測用に設けた処理装置が実行する位置姿勢計測方法であって、
前記移動体が移動を開始する前の初期位置の絶対座標を示す初期位置座標情報と、前記構造物の表面にある特定部位の絶対座標を示す特定部位座標情報を含む構造物情報とを取得する既知情報取得処理と、
前記移動体の機体に取り付けた角速度センサが出力する角速度信号、及び加速度センサが出力する加速度信号に基づいて前記移動体の位置移動量を算出する位置移動量算出処理と、
使用者が操作又は操縦することによって前記移動体が前記特定部位を撮像可能な場所に配置された状態で、使用者からの指令を受けて実行する処理であって、前記移動体の機体に取り付けた撮像装置で前記特定部位を撮像し、前記特定部位の画像データを取得する特定部位撮像処理と、
前記初期位置座標情報及び前記位置移動量に基づいて、前記特定部位を撮像した時の前記移動体の位置を算出するとともに、前記特定部位座標情報と比較して、撮像時の前記移動体と前記特定部位との相対的な位置関係を示す第１の位置関係情報を導出し、前記画像データに基づいて、前記特定部位を撮像した時の前記移動体と前記特定部位との相対的な位置関係を示す第２の位置関係情報を導出し、前記第１及び第２の位置関係情報の差から位置補正量を算出して記憶する位置補正量算出処理と、
前記初期位置座標情報に前記位置移動量の累積和を加算し、これを前記位置補正量で補正することによって、前記移動体の位置を算出する位置算出処理とを備えることを特徴とする位置姿勢計測方法。

【請求項2】

前記移動体は、使用者からの指令を受けて前記特定部位撮像処理を行うと、前記位置補正量算出処理を実行し、先に記憶した前記位置補正量を更新する請求項１記載の位置姿勢計測方法。

【請求項3】

複数の前記構造物が離れた場所に設置され、複数の前記構造物に前記特定部位が各々設定されており、調査する前記構造物が変わる毎に、前記特定部位撮像処理及び前記位置補正量算出処理を実行する請求項２記載の位置姿勢計測方法。

【請求項4】

前記構造物の表面が複数のエリアに区分され、複数の前記エリアに前記特定部位が各々設定されており、調査する前記エリアが変わる毎に、前記特定部位撮像処理及び前記位置補正量算出処理を実行する請求項２記載の位置姿勢計測方法。

【請求項5】

前記角速度信号に重畳しているノイズ成分を低減するフィルタリング処理と、
前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整するアライメント処理と、
前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理と、
前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理と、
前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する前記位置移動量算出処理とを備え、
前記移動体が静止している時に、前記フィルタリング処理、前記アライメント処理及び前記角速度・加速度バイアス算出処理を順番に実行し、その後、前記フィルタリング処理、前記姿勢角算出処理及び前記位置移動量算出処理を順番に繰り返し実行し、
前記移動体が静止している時の前記フィルタリング処理、前記移動体が静止している時の前記アライメント処理、前記移動体が静止している時の前記姿勢角算出処理、及び前記移動体が静止している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減するとともに、前記各バイアスに起因する積分値の誤差の増大を抑制する処理により行い、
前記移動体が運動している時の前記フィルタリング処理、前記移動体が運動している時の前記姿勢角算出処理、及び前記移動体が運動している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減する処理により行う請求項１乃至４のいずれか記載の位置姿勢計測方法。

【請求項6】

前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記アライメント処理では、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出し、前記角速度・加速度バイアス算出処理では、地球角速度をゼロと仮定して前記角速度バイアスを算出し、前記姿勢角算出処理では、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出する請求項５記載の位置姿勢計測方法。

【請求項7】

前記撮像装置は、移動体に対して、移動体の前方、後方又は側方を撮像する角度に取り付けられ、
鉛直方向をＵ軸とするＥＮＵ地理座標系を定義した時、
前記特定部位撮像処理で取得される前記画像データは、前記移動体が、前記特定部位に対してＵ軸方向の同じ位置に配置されて前記特定部位を撮像したものであり、
前記位置補正量算出処理では、Ｅ軸及びＮ軸方向の前記位置補正量のみを算出し、
前記位置算出処理では、Ｅ軸及びＮ軸方向の位置を、Ｅ軸及びＮ軸方向の前記位置補正量により補正して算出する請求項１乃至６のいずれか記載の位置姿勢計測方法。

【請求項8】

鉛直方向をＵ軸とするＥＮＵ地理座標系を定義した時、
前記位置補正量算出処理では、Ｅ軸、Ｎ軸及びＵ軸方向の前記位置補正量を算出し、
前記位置算出処理では、Ｅ軸、Ｎ軸及びＵ軸方向の位置を、Ｅ軸、Ｎ軸及びＵ軸方向の前記位置補正量により補正して算出する請求項１乃至６のいずれか記載の位置姿勢計測方法。

【請求項9】

請求項１乃至８記載の位置姿勢計測方法を実行させるための前記各処理の実行用プログラムにより構成された位置姿勢計測プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、マルチコプタ、ロボット、自動車等の移動体の位置・姿勢を計測する時に使用され、非ＧＰＳ環境下でも高精度な計測が可能な位置姿勢計測方法及び位置姿勢計測プログラムに関する。

【背景技術】

【0002】

移動体の動作を適切に制御するためには、移動体の位置・姿勢を高精度に計測することが重要である。従来、本願出願人による特許文献１に開示されているように、角速度センサ及び加速度センサ等の出力信号から移動体の姿勢角及び位置移動量を算出する慣性計測方法があった。この慣性計測方法は、移動体がある場所に着地した静止状態になると、独特なアライメント処理と角速度・加速度バイアス算出処理とを行い、姿勢角及び位置移動量を算出する時に、重力加速度及び地球角速度の成分をキャンセルする処理を行うので、重力加速度及び地球角速度に起因する誤差を小さくできる。さらに、演算処理を行う時に独特なフィルタを使用し、出力信号に重畳しているノイズ成分を低減したり、積分値を高い精度で推定したりするので、慣性計測の誤差が効果的に抑制され、姿勢角及び位置移動量を非常に高い精度で計測できるという特徴がある。

【0003】

また、特許文献２に開示されているように、飛行体に取り付けた慣性センサを用いて生成される第１の情報（飛行体の加速度を示す情報）と、飛行体に設置した撮像装置を用いて生成される第２の情報（飛行体の位置を示す情報）とから、飛行体の位置を推定する位置推定方法があった。第２の情報は、例えば、特定のエリア内の既知の位置に複数のマーカが分散させて配置され、飛行体に設置した撮像装置で撮像した各マーカの画像データに基づいて推定する。

【0004】

慣性センサからの情報（第１の情報）だけで位置推定を行うと、積分や加算によって誤差が蓄積し、精度が低下してしまうところ、特許文献２の位置推定方法は、さらに撮像装置からの情報（第２の情報）も利用するので、慣性センサによる位置推定の精度を補完することができる。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】国際公開第２０１８／１３９６２１号

【特許文献2】国際公開第２０１６／０５９９３０号

【発明の概要】

【発明が解決しようとする課題】

【0006】

特許文献１の慣性計測方法は、移動体の位置移動量を、角速度センサ及び加速度センサ等の慣性センサからの情報だけで高精度に計測できるものであるが、移動体の位置を計測することについて記載していない。

【0007】

一方、特許文献２の位置推定方法は、撮像装置からの情報（第２の情報）も利用するので、慣性センサによる位置推定の精度を補完できる。しかし、第２の情報は、複数のマーカの位置情報と各マーカを撮像した複数の画像データとを解析して生成する構成なので、複雑な画像解析や演算処理を実行しなければならず、処理装置の負担が非常に大きくなる。従って、移動体の姿勢制御に利用するには、応答速度の問題や高性能の処理装置が必要になり、コストアップの問題もある。

【0008】

また、近年、橋梁、堤防、トンネル等のインフラ構造物の老朽化が社会問題になっており、これらの調査（点検や監視等）を効率よく行うため、ドローンやマルチコプタ等の移動体を利用することが検討されている。この場合、位置姿勢計測方法は、様々な場所で容易に使用できる汎用性を備えた方法であることが求められるが、特許文献２の位置推定方法は、この件においても十分とは言えない。

【0009】

本発明は、上記背景技術に鑑みて成されたものであり、インフラ構造物等を調査する用途に好適で、調査用の移動体の位置を容易且つ高精度に計測できる位置姿勢計測方法及び位置姿勢計測プログラムを提供することを目的とする。

【課題を解決するための手段】

【0010】

本発明は、ある場所に設置された構造物の状態を、移動体を用いて調査するシステムにおいて、前記移動体の位置姿勢計測用に設けた処理装置が実行する位置姿勢計測方法であって、
前記移動体が移動を開始する前の初期位置の絶対座標を示す初期位置座標情報と、前記構造物の表面にある特定部位の絶対座標を示す特定部位座標情報を含む構造物情報とを取得する既知情報取得処理と、
前記移動体の機体に取り付けた角速度センサが出力する角速度信号、及び加速度センサが出力する加速度信号に基づいて前記移動体の位置移動量を算出する位置移動量算出処理と、
使用者が操作又は操縦することによって前記移動体が前記特定部位を撮像可能な場所に配置された状態で、使用者からの指令を受けて実行する処理であって、前記移動体の機体に取り付けた撮像装置で前記特定部位を撮像し、前記特定部位の画像データを取得する特定部位撮像処理と、
前記初期位置座標情報及び前記位置移動量に基づいて、前記特定部位を撮像した時の前記移動体の位置を算出するとともに、前記特定部位座標情報と比較して、撮像時の前記移動体と前記特定部位との相対的な位置関係を示す第１の位置関係情報を導出し、前記画像データに基づいて、前記特定部位を撮像した時の前記移動体と前記特定部位との相対的な位置関係を示す第２の位置関係情報を導出し、前記第１及び第２の位置関係情報の差から位置補正量を算出して記憶する位置補正量算出処理と、
前記初期位置座標情報に前記位置移動量の累積和を加算し、これを前記位置補正量で補正することによって、前記移動体の位置を算出する位置算出処理とを備える位置姿勢計測方法である。

【0011】

前記移動体は、使用者からの指令を受けて前記特定部位撮像処理を行うと、前記位置補正量算出処理を実行し、先に記憶した前記位置補正量を更新する。この場合、複数の前記構造物が離れた場所に設置され、複数の前記構造物に前記特定部位が各々設定されており、調査する前記構造物が変わる毎に、前記特定部位撮像処理及び前記位置補正量算出処理を実行する構成にすることができる。あるいは、前記構造物の表面が複数のエリアに区分され、複数の前記エリアに前記特定部位が各々設定されており、調査する前記エリアが変わる毎に、前記特定部位撮像処理及び前記位置補正量算出処理を実行する構成にしてもよい。

【0012】

また、前記角速度信号に重畳しているノイズ成分を低減するフィルタリング処理と、前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整するアライメント処理と、前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理と、前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理と、前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する前記位置移動量算出処理とを備え、
前記移動体が静止している時に、前記フィルタリング処理、前記アライメント処理及び前記角速度・加速度バイアス算出処理を順番に実行し、その後、前記フィルタリング処理、前記姿勢角算出処理及び前記位置移動量算出処理を順番に繰り返し実行し、
前記移動体が静止している時の前記フィルタリング処理、前記移動体が静止している時の前記アライメント処理、前記移動体が静止している時の前記姿勢角算出処理、及び前記移動体が静止している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減するとともに、前記各バイアスに起因する積分値の誤差の増大を抑制する処理により行い、
前記移動体が運動している時の前記フィルタリング処理、前記移動体が運動している時の前記姿勢角算出処理、及び前記移動体が運動している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減する処理により行う構成にすることが好ましい。

【0013】

この場合、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記アライメント処理では、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出し、前記角速度・加速度バイアス算出処理では、地球角速度をゼロと仮定して前記角速度バイアスを算出し、前記姿勢角算出処理では、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出する構成にしてもよい。

【0014】

また、前記撮像装置は、移動体に対して、移動体の前方、後方又は側方を撮像する角度に取り付けられ、鉛直方向をＵ軸とするＥＮＵ地理座標系を定義した時、前記特定部位撮像処理で取得される前記画像データは、前記移動体が、前記特定部位に対してＵ軸方向の同じ位置に配置されて前記特定部位を撮像したものであり、前記位置補正量算出処理では、Ｅ軸及びＮ軸方向の前記位置補正量のみを算出し、前記位置算出処理では、Ｅ軸及びＮ軸方向の位置を、Ｅ軸及びＮ軸方向の前記位置補正量により補正して算出する構成にすることができる。

【0015】

あるいは、鉛直方向をＵ軸とするＥＮＵ地理座標系を定義した時、前記位置補正量算出処理では、Ｅ軸、Ｎ軸及びＵ軸方向の前記位置補正量を算出し、前記位置算出処理では、Ｅ軸、Ｎ軸及びＵ軸方向の位置を、Ｅ軸、Ｎ軸及びＵ軸方向の前記位置補正量により補正して算出する構成にすることができる。

【0016】

また、本発明は、上記の位置姿勢計測方法を実行させるための前記各処理の実行用プログラムにより構成された位置姿勢計測プログラムである。

【発明の効果】

【0017】

本発明の位置姿勢計測方法及び位置姿勢計測プログラムは、特定の場所に設置されたインフラ構造物等の表面の状態を、移動体を用いて調査する用途に好適で、現場がトンネルの内部のような非ＧＰＳ環境下であっても、問題なく実行することができる。

【0018】

また、移動体に取り付けた角速度センサ及び加速度センサ等の情報だけから位置を算出すると、積分や加算によって誤差が蓄積して精度が低下してしまうところ、本発明の位置姿勢計測方法及び位置姿勢プログラムでは、個々の構造物に適用される位置補正量を独特な方法で算出し、角速度センサ及び加速度センサ等の情報を位置補正量で補正して位置を算出するので、移動体の位置を高精度に計測することができる。

【0019】

また、１つの現場に複数の構造物が分散して設置されている場合、構造物毎に位置補正量を算出し更新することによって、移動体の位置を常に高精度に計測することができる。同様に、１つの構造物の表面積が非常に大きい場合も、エリア毎に位置補正量を算出し更新することによって、移動体の位置を常に高精度に計測することができる。

【0020】

本発明は、特定部位の画像データを取得するため、使用者が、「移動体を所定の場所でホバリングさせ、特定部位撮像指令を出す」という作業を行わなければならないが、使用者にとってそれほど大きな負担にはならず、これによって、位置補正量算出処理を非常にシンプルなアルゴリズムで実行することができ、処理装置の負担も小さくなる。また、非常に汎用性が高くなるので、橋梁、堤防、トンネル等の様々な構造物について、容易に調査を行うことができる。

【図面の簡単な説明】

【0021】

【図1】調査対象の構造物が設置されている現場の一例を示す斜視図（ａ）、ＥＮＵ地理座標系及びＸＹＺ機体座標系を示す図である。

【図2】本発明の位置姿勢計測方法の第一の実施形態を実行する処理装置が搭載された移動体のシステム構成図である。

【図3】第一の実施形態の位置姿勢計測方法の全体の流れを示すフローチャートである。

【図4】第一の実施形態の位置姿勢計測方法で行うアライメント処理の内容を示すフローチャートである。

【図5】図４の中の初期姿勢角算出ステップの内容を示すフローチャートである。

【図6】図４の中の第１の初期姿勢角調整ステップの内容を示すフローチャートである。

【図7】図４の中の第２の初期姿勢角調整ステップの内容を示すフローチャートである。

【図8】図４の中の第３の初期姿勢角調整ステップの内容を示すフローチャートである。

【図9】第一の実施形態の位置姿勢計測方法で行う角速度・加速度バイアス算出処理の内容を示すフローチャートである。

【図10】第一の実施形態の位置姿勢計測方法で行う姿勢角算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。

【図11】第一の実施形態の位置姿勢計測方法で行う位置移動量算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。

【図12】静止状態推定フィルタと運動状態推定フィルタの設定方法の違いを説明するタイミングチャート（ａ）、（ｂ）である。

【図13】第一の実施形態の位置姿勢計測方法で行う特定部位撮像処理、補正量算出処理及び位置算出処理の内容を示すフローチャートである。

【図14】図１３の中の特定部位撮像処理及び位置補正量算出処理の第一の具体例を示す図表である。

【図15】図１３の中の特定部位撮像処理及び位置補正量算出処理の第二の具体例を示す図表である。

【図16】本発明の位置姿勢計測方法の第二の実施形態を実行する処理装置が搭載された移動体のシステム構成図である。

【図17】第二の実施形態の位置姿勢計測方法で行うアライメント処理の内容を示すフローチャートである。

【図18】図１７の中の初期姿勢角算出ステップの内容を示すフローチャートである。

【図19】第二の実施形態の位置姿勢計測方法で行う角速度・加速度バイアス算出処理の内容を示すフローチャートである。

【図20】第二の実施形態の位置姿勢計測方法で行う姿勢角算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。

【図21】調査対象の構造物が設置されている現場の他の例を示す斜視図である。

【発明を実施するための形態】

【0022】

以下、本発明の位置姿勢計測方法及び位置姿勢計測プログラムの第一の実施形態について、図１〜図１５に基づいて説明する。図１（ａ）は、ある場所に設置されたコンクリート製の橋脚（構造物Ｋ１，Ｋ２，Ｋ３）の状態を、移動体１０を用いて調査するシステムを示しており、この実施形態の位置姿勢計測方法は、移動体１０に搭載された処理装置１８により実行される。また、この実施形態の位置姿勢計測プログラムは、この実施形態の位置姿勢計測方法を実行させるための各処理の実行用プログラムで、処理装置１８にインストールされて実行される。図１（ｂ）は、この位置姿勢計測方法に用いるＥＮＵ地理座標系とＸＹＺ機体座標系とを示している。

【0023】

移動体１０は、例えばドローンやマルチコプタ等であり、図２に示すように、機体１０ａ内部に、角速度センサ１２、加速度センサ１４、撮像装置１６及び処理装置１８が搭載されている。処理装置１８は、角速度センサ１２が出力する角速度信号ω^B及び加速度センサ１４が出力する加速度信号ａ^Bに基づいて、移動体１０の姿勢角φ（ロール），θ（ピッチ），ψ（ヨー）、位置移動量ｒ^G及び位置Ｔ^G算出する処理を行う。なお、□^B，□^Gという表記の記号はベクトルであり、「B」は各軸成分をＸＹＺ機体座標系で表したものであることを示し、「G」は各軸成分をＥＮＵ地理座標系で表したものであることを示している。

【0024】

処理装置１８が実行する処理は、既知情報取得処理S1、フィルタリング処理S2s，S2u、アライメント処理S3、角速度・加速度バイアス算出処理S4、姿勢角算出処理S5s，S5u、位置移動量算出処理S6s，S6u、特定部位撮像処理S7、位置補正量算出処理S8、及び位置算出処理S9である。まず、各処理の概要を説明する。

【0025】

既知情報取得処理S1は、移動体１０が規定の発着場HT（移動を開始する前の初期位置）に着地している時に実行する処理で、基地局のデータベース等から、移動体１０の初期位置の絶対座標を示す初期位置座標情報と、構造物に関する構造物情報とを取得する処理である。構造物情報には、調査対象の構造物K1〜K3の表面にある特定部位P1〜P3の絶対座標を示す特定部位座標情報が含まれている。特定部位P1〜P3は、外観的に他の部分と区別できる部位であり、例えば、先の調査で発見された要監視部位（ひび割れ発生した部位等）でもよいし、目印となるマーカを付した部位でもよい。

【0026】

フィルタリング処理S2s，S2uは、角速度信号ω^Bに重畳しているノイズ成分を低減させる処理である。

【0027】

アライメント処理S3は、移動体１０が静止状態の時（ある場所に着地している時）に実行する処理で、加速度信号ａ^B及び角速度信号ω^Bから概略の初期姿勢角φ0，θ0，ψ0を算出した後、加速度信号ａ^Bから重力加速度ｇp^Bを差し引いた加速度ah^Bを積分して求まる速度Vh^Bの絶対値が一定以下になり、且つ角速度信号ω^Bから地球角速度ωp^Bを差し引いた角速度αh^Bを積分して求まる角度の絶対値が一定以下の値になるように、初期姿勢角φ0，θ0，ψ0を微調整する処理である。

【0028】

角速度・加速度バイアス算出処理S4は、移動体１０が静止状態の時（ある場所に着地している時）に実行する処理で、角速度信号ω^Bから地球角速度ωp^Bを差し引いた角速度バイアスωb^Bを算出するとともに、加速度信号ａ^Bから重力加速度ｇp^Bを差し引いた加速度バイアスａb^Bを算出する処理である。

【0029】

姿勢角算出処理S5s，S5uは、角速度信号ω^Bから地球角速度ωp^B及び角速度バイアスωb^Bを差し引いた補正角速度ωh^Bを算出し、これを積分して姿勢角φ，θ，ψを算出する処理である。

【0030】

位置移動量算出処理S6s，S6uは、加速度信号ａ^Bから重力加速度ｇp^B及び加速度バイアスａb^Bを差し引いた補正加速度ａh^Gを算出し、これを２階積分して位置移動量ｒ^Gを算出する処理である。

【0031】

特定部位撮像処理S7は、例えば構造物K1を調査する場合、使用者が操作又は操縦することによって移動体１０が構造物K1の特定部位P1を撮像可能な場所に配置された状態で、使用者からの指令（以下、特定部位撮像指令と称する。）を受けて実行する処理で、移動体１０に取り付けた撮像装置１６で特定部位P1を撮像し、特定部位P1の画像データを取得する処理である。

【0032】

位置補正量算出処理S8は、特定部位撮像処理S7の後に速やかに実行する処理で、初期位置座標情報及び位置移動量に基づいて、特定部位P1を撮像した時の移動体１０の位置を算出し、その時の移動体１０と特定部位P1との相対的な位置関係を特定する第１の位置関係情報を導出するとともに、画像データ及び構造物情報に基づいて、特定部位P1を撮像した時の移動体１０と特定部位P1との相対的な位置関係を特定する第２の位置関係情報を導出し、第１及び第２の位置関係情報の差から位置補正量を算出して記憶する処理である。

【0033】

位置算出処理S9は、初期位置座標情報に位置移動量の累積和を加算し、これを位置補正量で補正することによって、移動体１０の位置Ｔ^Gを算出する処理である。

【0034】

図３のフローチャートに示すように、処理装置１８は、まず、移動体１０が発着場HTに着地している時に既知情報取得処理S1を実行し、移動体１０が静止している時に、フィルタリング処理S2s、アライメント処理S3、角速度・加速度バイアス算出処理S4を順番に実行し、その後、フィルタリング処理S2s又はS2u、姿勢角算出処理S5s又はS5u、位置移動量算出処理S6s又はS6u、位置算出処理S9を順番に繰り返し実行する。また、移動体１０がある構造物の近くに到着した後、その構造物の調査を開始する前に、使用者から特定部位撮像指令を受け、特定部位撮像処理S7、位置補正量算出処理S8を実行する。

【0035】

なお、移動体１０の静止状態とは、地面等に着地してスタンバイしている状態のことであり、運動状態とは、飛行を開始して移動またはホバリングしている状態のことである。静止状態か運動状態かの判断は、処理装置１８が角速度信号ω^Bと加速度信号ａ^Bを適宜の方法で分析することによって行う。

【0036】

以下、３つの構造物K1〜K3の中の構造物K1の調査を行う場合を想定し、各処理の詳細な内容を図３に基づいて説明する。まず、前準備として、移動体１０が発着場HTに着地している時、既知情報取得処理S1を行い、移動体１０の位置計測の基準となる初期位置座標情報（既知の絶対座標情報）と、構造物K1の特定部位P1の位置を規定する特定部位座標情報（既知の絶対座標情報）とを取得する。そして、既知情報取得処理S1が終了すると、移動体１０が発着場HTから移動し、状況に応じて、その他の処理を行う。

【0037】

処理装置１８は、まず静止状態か運動状態かの判断を行い、静止状態と判断した場合、フィルタリング処理S2sを行う。フィルタリングは、角速度信号ω^Bに重畳しているノイズ成分を低減する処理を、静止状態推定フィルタＡs1を使用した推定によって行う。静止状態推定フィルタＡs1は、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。

【0038】

フィルタリング処理S2sが終了した後、静止状態でアライメントが未終了の場合は、アライメント処理S3を行う。アライメント処理S3は、初期姿勢角算出ステップS31と３つの初期姿勢角調整ステップS32．S33，S34とで構成される。図４に示すように、アライメント処理S3を開始し、初期姿勢角φ0，θ0，ψ0が未算出の場合に、初期姿勢角算出ステップS31を行う。

【0039】

初期姿勢角算出ステップS31では、図５に示すように、まず、ステップS311で、加速度信号ａ^Bとフィルタリング処理後の角速度信号ω^Bを取得し、規定時間における各信号の平均値と分散を計算する。そして、規定時間の経過後、ステップS312に進み、ステップS311で算出した加速度信号ａ^Bの平均値が真の重力加速度のｇp^Bと等しいと仮定し、さらに角速度信号ω^Bの平均値が真の地球角速度ωp^Bと等しいと仮定して、初期姿勢角φ0，θ0，ψ0を算出する。この仮定は、アライメント処理S3は静止時の加速度信号ａ^B及び角速度信号ω^Bに基づいて行うことが前提であり、上記の仮定により、概略の初期姿勢角φ0，θ0，ψ0を、図５の中に記載したシンプルな式で簡単に求めることができる。

【0040】

初期姿勢角φ0，θ0，ψ0を算出した後、初期姿勢角θ0が未調整の場合は、第１の初期姿勢角調整ステップS32に進む。第１の初期姿勢角調整ステップS32は、初期姿勢角θ0を微調整するステップである。図６に示すように、まずステップS321に進み、ＥＮＵ地理座標系の重力加速度ｇp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いてＸＹＺ機体座標系の重力加速度ｇp^Bに変換する。座標変換行列Ｃ(G-B)は公知の行列であり、右辺のCφ，Cθ，Cψは各々cosφ，cosθ，cosψであり、Sφ，Sθ，Sψは各々sinφ，sinθ，sinψである。

【数1】

【0041】

ステップS321を行った後はステップS322に進み、加速度信号ａ^BのＸ軸成分a(x)から重力加速度ｇp^BのＸ軸成分gp(x)を差し引いて、重力加速度の影響をキャンセルした補正加速度ah(x)を算出する。そして、補正加速度ah(x)に静止状態推定フィルタＡs1を適用し、積分値であるＸ軸方向の速度vh(x)を推定する（ステップS323）。この静止状態推定フィルタＡs1は、フィルタリング処理S1sで使用したのと同じものである。

【0042】

速度vh(x)を推定すると、規定時間における速度vh(x)の平均値を計算する（ステップS324）。そして、速度vh(x)の絶対値が一定以上に大きい場合はステップS325に進み、初期姿勢角θ0を直前の値から変更（増減）させる。初期姿勢角θ0を変更してステップS321〜S324を繰り返し実行すると、やがて速度vh(x)の絶対値が一定以下（≒0）になるので、ステップS326に進み、その時のθ0を調整後の初期姿勢角θ0とする。これでθ0の調整が終了する。

【0043】

初期姿勢角θ0の調整が終了した後、初期姿勢角φ0が未調整の場合は、図４に示すように、第２の初期姿勢角調整ステップS33に進む。第２の初期姿勢角調整ステップS33は、初期姿勢角φ0を微調整するステップである。図７に示すように、まずステップS331に進み、ＥＮＵ地理座標系の重力加速度ｇp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いてＸＹＺ機体座標系の重力加速度ｇp^Bに変換する。ステップS331は、先に説明したステップS321と同様の処理である。

【0044】

ステップS331を行った後はステップS332に進み、加速度信号ａ^BのＹ軸成分a(y)から重力加速度ｇp^BのＹ軸成分gp(y)を差し引いて、重力加速度の影響をキャンセルした補正加速度ah(y)を算出する。そして、補正加速度ah(y)に静止状態推定フィルタＡs1を適用し、積分値であるＹ軸方向の速度vh(y)を推定する（ステップS333）。この静止状態推定フィルタＡs1も、フィルタリング処理S1sで使用したのと同じものである。

【0045】

速度vh(y)を推定すると、規定時間における速度vh(y)の平均値を計算する（ステップS334）。そして、速度vh(y)の絶対値が一定以上に大きい場合はステップS335に進み、初期姿勢角φ0を直前の値から変更（増減）させる。初期姿勢角φ0を変更してステップS331〜S334を繰り返し実行すると、やがて速度vh(y)の絶対値が一定以下（≒0）になるので、ステップS336に進み、その時のφ0を調整後の初期姿勢角φ0とする。これでφ0の調整が終了する。

【0046】

初期姿勢角φ0の調整が終了した後、初期姿勢角ψ0が未調整の場合は、図４に示すように、第３の初期姿勢角調整ステップS34に進む。第３の初期姿勢角調整ステップS34は、初期姿勢角ψ0を微調整するステップである。図８に示すように、まずステップS341に進み、ＥＮＵ地理座標系の地球角速度ωp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いてＸＹＺ機体座標系の地球角速度ωp^Bに変換する。

【0047】

ステップS341を行った後はステップS342に進み、角速度信号ω^BのＸ軸成分ω(x)から地球角速度ωp^BのＸ軸成分ωp(x)を差し引いて、地球角速度の影響をキャンセルした補正角速度ωh(x)を算出する。そして、補正角速度ωh(x)に静止状態推定フィルタＡs1を適用し、積分値であるＸ軸方向の角度αh(x)を推定する（ステップS343）。この静止状態推定フィルタＡs1も、フィルタリング処理S1sで使用したのと同じものである。

【0048】

角度αh(x)を推定すると、規定時間における角度αh(x)の平均値を計算する（ステップS344）。そして、角度αh(y)の絶対値が一定以上に大きい場合はステップS345に進み、初期姿勢角ψ0を直前の値から変更（増減）させる。初期姿勢角ψ0を変更してステップS341〜S344を繰り返し実行すると、やがて角度αh(x)の絶対値が一定以下（≒0）になるので、S346に進み、その時のψ0を調整後の初期姿勢角ψ0とする。これでψ0の調整が終了し、アライメント処理S3が終了する。

【0049】

アライメント処理S3が終了した後、静止状態で角速度・加速度バイアスωb^B，ａb^B未算出の場合、図３に示すように、角速度・加速度バイアス算出処理S4を行う。角速度・加速度バイアス算出処理S4は、図９に示すように、角速度バイアスωb^Bが未算出の場合、まずステップS41に進み、ＥＮＵ地理座標系の地球角速度ωp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωp^Bに変換する。そして、ステップS42に進み、角速度信号ω^Bから地球角速度ωp^Bを差し引いて、角速度誤差Δω^Bを算出する。

【0050】

角速度誤差Δω^Bを算出すると、規定時間における角速度誤差Δω^Bの平均値を計算し、その結果を角速度バイアスωb^Bとし（ステップS44）、これで角速度バイアスωb^Bの算出が終了する。

【0051】

角速度バイアスωb^Bの算出が終了した後、加速度バイアスａb^Bが未算出の場合はステップS45に進み、ＥＮＵ地理座標系の重力加速度ｇp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いてＸＹＺ機体座標系の重力加速度ｇp^Bに変換する。そして、ステップS46に進み、加速度信号ａ^Bから重力加速度ωp^Bを差し引いて、加速度誤差Δａ^Bを算出する。

【0052】

加速度誤差Δａ^Bを算出すると、規定時間における加速度誤差Δａ^Bの平均値を計算し、その結果を加速度バイアスａb^Bとする（ステップS48）。これで加速度バイアスａb^Bの算出が終了し、角速度・加速度バイアス算出処理S4が終了する。

【0053】

角速度・加速度バイアス算出処理S3が終了した後、静止状態の時は、図３に示すように、フィルタリング処理S2sを経て、姿勢角算出処理S5sを行う。姿勢角算出処理S5sは、図１０（ａ）に示すように、まずステップS51sに進み、ＥＮＵ地理座標系の地球角速度ωp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いてＸＹＺ機体座標系の地球角速度ωp^Bに変換する。そして、ステップS52sに進み、角速度信号ω^Bから地球角速度ωp^Bと角速度バイアスωb^Bを差し引いて、地球角速度等の誤差要因をキャンセルした補正角速度ωh^Bを算出する。

【0054】

補正角速度ωh^Bを算出すると、ステップS53sに進み、ＸＹＺ機体座標系の補正角速度ωh^Bを、式（２）に示す座標変換行列Ｒ(B-A)を用いて姿勢角速度φ（ドット），θ（ドット），ψ（ドット）に変換する。座標変換行列Ｒ(B-A)は公知の行列である。

【数2】

【0055】

ステップS53sを行った後はステップS54sに進み、姿勢角速度φ（ドット），θ（ドット），ψ（ドット）にそれぞれ静止状態推定フィルタＡs1を適用し、積分値である姿勢角φ，θ，ψを推定する。この静止状態推定フィルタＡs1は、フィルタリング処理S1sで使用したのと同じものである。そして、座標変換行列Ｃ(G-B)を更新して姿勢角算出処理S5sが終了する（ステップS55s）。

【0056】

姿勢角算出処理S5sが終了した後、静止状態の時は、図３に示すように、位置移動量算出処理S6sを行う。位置移動量算出処理S6sは、図１１（ａ）に示すように、まずステップS61sに進み、ＥＮＵ地理座標系の重力加速度ｇp^Gを、式（１）に示す座標変換行列Ｃ(G-B)を用いてＸＹＺ機体座標系の重力加速度ｇp^Bに変換する。そして、ステップS62sに進み、加速度信号ａ^Bから重力加速度ｇp^Bと加速度バイアスａb^Bとを差し引いて、重力加速度等の誤差要因をキャンセルした補正加速度ａh^Bを算出する。

【0057】

補正加速度ａh^Bを算出すると、ステップS63sに進み、ＸＹＺ機体座標系の補正加速度ａh^Bを、式（１）に示す座標変換行列Ｃ(G-B)を用いてＥＮＵ地理座標系の加速度ａh^Gに変換する。

【0058】

ステップS63sを行った後はステップS64sに進み、加速度ａh^Gの各軸成分に静止状態推定フィルタＡs2を適用し、２階積分値である位置移動量ｒ^Gを推定する。静止状態推定フィルタＡs2は、先の静止状態推定フィルタＡs1と同様に、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。これで位置移動量算出処理S6sが終了する。

【0059】

角速度・加速度バイアス算出処理S4が終了した後、運動状態の時は、図３に示すようにフィルタリング処理S2uを経て、姿勢角算出処理S5uを行う。フィルタリング処理S2uは、上記のフィルタリング処理S2sとは異なり、角速度信号ω^Bに重畳しているノイズ成分を低減する処理を、運動状態推定フィルタＡu1を使用した推定によって行う。運動状態推定フィルタＡu1は、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。

【0060】

姿勢角算出処理S5uは、図１０（ｂ）に示すように、ステップS51u〜S55uを順に行う。この中のステップS51u〜S53u，S55uは静止時のステップS51s〜S53s，S55sと同じ内容であり、異なるのはステップS54u（下線部）である。

【0061】

ステップS54uでは、ステップS53uの処理結果である姿勢角速度φ（ドット），θ（ドット），ψ（ドット）に対して運動状態推定フィルタＡu1を適用し、積分値である姿勢角φ，θ，ψを推定する。この運動状態推定フィルタＡu1は、フィルタリング処理S2uで使用したのと同じものである。そして、座標変換行列Ｃ(G-B)を更新し、姿勢角算出処理S5uが終了する（ステップS55u）。

【0062】

姿勢角算出処理S5uが終了した後、運動状態の時は、図３に示すように、位置移動量算出処理S6uを行う。位置移動量算出処理S6uは、図１１（ｂ）に示すように、ステップS61u〜S64uを順に行う。この中のステップS61u〜S63uは静止時のステップS61s〜S63sと同じ内容であり、異なるのはステップS64u（下線部）である。

【0063】

ステップS64uでは、ステップS63uの処理結果である加速度ａh^Gの各軸成分に対して運動状態推定フィルタＡu2を適用し、２階積分値である位置移動量ｒ^Gを推定する。この運動状態推定フィルタＡu2は、先の運動状態推定フィルタＡu1と同様に、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。これで位置移動量算出処理S6uが終了する。

【0064】

このように、処理装置１８は、移動体１０が静止している時に、フィルタリング処理S2s、アライメント処理S3、角速度・加速度バイアス算出処理S4を順番に実行し、その後、フィルタリング処理S2s又はS2u、姿勢角算出処理S5s又はS5u、位置移動量算出処理S6s又はS6uを順番に繰り返し実行することによって、角速度信号ω^B及び加速度信号ａ^Bに基づいて、移動体１０の姿勢角φ（ロール），θ（ピッチ），ψ（ヨー）及び位置移動量ｒ^Gを算出する。

【0065】

ここで、４つの状態推定フィルタＡs1，Ａs2，Ａu1，Ａu2の中の、静止状態推定フィルタＡs1と運動状態推定フィルタＡu1について説明する。

【0066】

静止状態推定フィルタＡs1は、静止時のアライメント処理S3、姿勢角算出処理S5sで１階積分を行う際に使用されるフィルタであり、静止時のフィルタリング処理S2sでも使用されている。静止状態推定フィルタＡs1には、推定を行うための演算式として次の式（３）、（４）が設定されている。

【数3】

【数4】

【0067】

式（３）、（４）では、状態変数をｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。ｖはａの積分値であり、例えばａを加速度とすればｖが速度になり、ａを角速度とすればｖが角度になる。従って、ここでは変数は時間であり、一般的にtで表される。そして、式（３）の中の４行４列の行列が、静止時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値のランダムウォーク、及び積分を行う際の、バイアス起因の積分値の誤差の増大を抑制するように設定されている。行列式（３）の中のdtは、時間軸での変数tの微小期間である。この実施形態では例えば、時間軸上の上記タイミングｋの微小間隔（サンプリング周期）であり、以下の式においても同様である。

【0068】

運動状態推定フィルタＡu1は、運動時の姿勢角算出処理S5uで１階積分を行う際に使用されるフィルタであり、運動時のフィルタリング処理S2uでも使用されている。運動状態推定フィルタＡu1には、推定を行うための演算式として、上記の式（４）と次の式（５）とが設定されている。

【数5】

【0069】

式（４）、（５）では、状態変数をｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。そして、式（５）の中の４行４列の行列は、運動時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されている。

【0070】

式（３）と式（５）を比較して分かるように、各行列の内容が異なるのは第２行である。静止状態推定フィルタＡs1の第２行は、図１２（ａ）に示すように、タイミングk-1の後のしばらくの間（例えば、タイミングk-1とタイミングkの中間点までの微小期間）、積分値ｖが一定に保持されるという考え方で設定されている。一方、運動状態推定フィルタＡu1の第２行は、図１２（ｂ）に示すように、タイミングk-1からタイミングkまでの微小期間、積分値ｖが一定の傾きで変化するという考え方で設定されている。この第２行の設定方法の違いにより、静止状態推定フィルタＡs1は静止状態に適した推定を行うことができ、運動状態推定フィルタＡu1は、運動状態に適した推定を行うことができる。

【0071】

次に、４つの状態推定フィルタＡs1，Ａs2，Ａu1，Ａu2の中の、静止状態推定フィルタＡs2と運動状態推定フィルタＡu2について説明する。

【0072】

静止状態推定フィルタＡs2は、静止時の位置移動量算出処理S6sで２階積分を行う際に使用されるフィルタである。静止状態推定フィルタＡs2には、推定を行うための演算式として次の式（６）、（７）が設定されている。

【数6】

【数7】

【0073】

式（６）、（７）では、状態変数をｒ(k)、ｒ(k-1)、ｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。ｖはaの積分値であり、ｒはｖの積分値である。つまり、ａを加速度とすれば、ｖが速度でｒが位置移動量になる。そして、式（６）の中の６行６列の行列が、静止時の信号に重畳しているノイズを低減し、且つノイズ起因の積分値のランダムウォーク及びバイアス起因の積分値の誤差の増大を抑制するように設定されている。

【0074】

運動状態推定フィルタＡu2は、運動時の位置移動量算出処理S6uで２階積分を行う際に使用されるフィルタである。運動状態推定フィルタＡu2には、推定を行うための演算式として、上記の式（７）と次の式（８）とが設定されている。

【数8】

【0075】

式（７）、（８）では、状態変数をｒ(k)、ｒ(k-1)、ｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。そして、式（８）の中の６行６列の行列は、運動時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されている。

【0076】

式（６）と式（８）を比較して分かるように、各行列の内容が異なるのは第２行と第４行である。第４行の違いは、図１２（ａ）、（ｂ）を用いて先に説明した通りである。すなわち、静止状態推定フィルタＡs2の第４行は、タイミングk-1の後のしばらくの間、積分値ｖが一定に保持されるという考え方で設定され、運動状態推定フィルタＡu2の第４行は、タイミングk-1からタイミングkまでの間、積分値ｖが一定の傾きで変化するという考え方で設定されている。

【0077】

第２行の考え方も、第４行の考え方と同じである。すなわち、静止状態推定フィルタＡs2の第２行は、タイミングk-1の後のしばらくの間、積分値ｒが一定に保持されるという考え方で設定され、運動状態推定フィルタＡu2の第２行は、タイミングk-1からタイミングkまでの間、積分値ｒが一定の傾きで変化するという考え方で設定されている。

【0078】

この第２行及び第４行の設定方法の違いにより、静止状態推定フィルタＡs2は、静止状態に適した推定を行うことができ、運動状態推定フィルタＡu2は、運動状態に適した推定を行うことができる。

【0079】

位置移動量算出処理S6uが終了した後、処理装置１８が使用者からの特定部位撮像指令を受けると、図３、図１３に示すように、処理装置１８が特定部位撮像処理S7を行う。使用者が特定部位撮像指令を出すのは、移動体１０が構造物K1の近くに到着した後、構造物K1の調査を開始する前のタイミングである。

【0080】

使用者は、撮像装置１６から送られる連続動画を見ながら移動体１０を操縦又は操作し、移動体１０を、特定部位P1が画像フレームのほぼ中央部に映る位置でホバリングさせ、この状態で処理装置１８に特定部位撮像指令を出す。そして、この指令を受けた処理装置１８は、撮像装置１６で特定部位P1を撮像し、特定部位P1の画像データを取得する。

【0081】

特定部位撮像処理S7が終了すると、図３、図１３に示すように、速やかに位置補正量算出処理S8を行う。位置補正量算出処理S8は、まずステップS81に進み、初期位置座標情報（既知の絶対座標情報）及び位置移動量ｒ^Gに基づいて、特定部位P1を撮像した時の移動体１０の位置Ｔ^Gを算出し、特定部位座標情報（既知の絶対座標情報）と比較して、撮像時の移動体１０と特定部位P1との相対的な位置関係を示す第１の位置関係情報を導出する。さらにステップS82に進み、画像データに基づいて、特定部位P1を撮像した時の移動体１０と特定部位P1との相対的な位置関係を示す第２の位置関係情報を導出する。そして、ステップS83に進み、第１及び第２の位置関係情報の差から位置補正量を算出し、記憶する。

【0082】

図１４は、特定部位撮像処理S7及び位置補正量算出処理S8の第一の具体例を示しており、ここでは、Ｕ軸方向の位置計測は高精度なので補正は不要であるという想定の下で、Ｅ軸及びＮ軸方向の位置補正量ΔＴ(E)，ΔＴ(N)だけを算出している。

【0083】

撮像装置１６は、移動体１０に対して、移動体１０の前方、後方又は側方を撮像する角度に取り付けられている。使用者は、撮像装置１６から送られる連続動画を見ながら移動体１０を操縦又は操作し、移動体１０を、特定部位P1が画像フレームのほぼ中央部に映る位置（特定部位P1に対してＵ軸方向の同じ位置）で水平にホバリングさせ、この状態で処理装置１８に向けて特定部位撮像指令を出す。そして、この指令を受けた処理装置１８が、撮像装置１６で特定部位P1を撮像し、特定部位P1の画像データを取得する（ステップS7）。

【0084】

その後、ステップS81では、撮像時の移動体１０の座標T1(E)，T1(N)を初期位置座標情報及び位置移動量ｒ^Gから算出し、座標T1(E)，T1(N)及び特定部位座標情報の中の座標P1(U)により、撮像時の移動体１０の位置T1を特定する。そして、特定部位座標情報と比較して、次の式（９）により、位置T1と特定部位P1との離間距離m1（第１の位置関係情報）を算出する。
ｍ1＝√[[T1(E)−P1(E)]²＋[T1(N)−P1(N)]²] （９）
さらに、ステップS82では、特定部位P1の画像データから、移動体１０と特定部位P1との離間距離m2（第２の位置関係情報）を算出する。近年、単眼カメラで対象部位の画像データを取得し、立体認識ＡＩを用いて解析することによって、ステレオカメラ並みに高い精度で距離計測を行うことができるという報告がなされており、このような技術を応用することによって、離間距離m2を容易且つ高精度に算出することができる。

【0085】

そして、ステップS83では、離間距離m2が、次の式（１０）のように表されると仮定して、離間距離m1とm2とが等しくなる位置補正量ΔＴ(E)，ΔＴ(N)を算出し、記憶する。
ｍ2＝√[[T1(E)＋ΔT(E)−P1(E)]²＋[T1(N)＋ΔT(N)−P1(N)]²] （１０）
このように、第一の具体例の場合は、位置補正量算出処理S8で２つの位置補正量（Ｅ軸及びＮ軸方向の位置補正量）を算出する。

【0086】

図１５は、特定部位撮像処理S7及び位置補正量算出処理S8の第二の具体例を示しており、ここでは、Ｅ軸及びＮ軸方向の位置補正量ΔＴ(E)，ΔＴ(N)に加え、Ｕ軸方向の位置補正量ΔＴ(U)も算出している。

【0087】

図１５に示す構造物K1は、特定部位P1が、構造物K1の表面の、地面と対向するほど斜め下向きになった位置にあるので、撮像装置１６は、移動体１０に対して、移動体１０の斜め上方を撮像する角度に取り付けられている。使用者は、撮像装置１６から送られる連続動画を見ながら移動体１０を操縦又は操作し、移動体１０を、特定部位P1が画像フレームのほぼ中央部に映る位置で水平にホバリングさせ、この状態で処理装置１８に向けて特定部位撮像指令を出す。そして、この指令を受けた処理装置１８が、撮像装置１６で特定部位P1を撮像し、特定部位P1の画像データを取得する（ステップS7）。

【0088】

その後、ステップS81では、撮像時の移動体１０の座標T1(E)，T1(N)，T1(U)を初期位置座標情報及び位置移動量ｒ^Gから算出し、撮像時の移動体１０の位置T1を特定する。そして、特定部位座標情報と比較して、次の式（１１）により、位置T1と特定部位P1との離間距離m1（第１の位置関係情報）を算出する。
ｍ1＝√[[T1(E)−P1(E)]²＋[T1(N)−P1(N)]²＋[T1(U)−P1(U)]²] （１１）
さらに、ステップS82では、特定部位P1の画像データから、移動体１０と特定部位P1との離間距離m2（第２の位置関係情報）を算出する。

【0089】

そして、ステップS83では、離間距離m2が、次の式（１２）のように表されると仮定して、離間距離m1とm2とが等しくなる位置補正量ΔＴ(E)，ΔＴ(N)，ΔＴ(U)を算出し、記憶する。
ｍ2＝√[[T1(E)＋ΔT(E)−P1(E)]²＋[T1(N)＋ΔT(N)−P1(N)]²
＋[T1(U)＋ΔT(U)−P1(U)]²] （１２）
このように、第二の具体例の場合は、位置補正量算出処理S8で３つの位置補正量（Ｅ軸、Ｎ軸及びＵ軸方向の位置補正量）を算出する。

【0090】

その後、位置移動量算出処理S6s，S6uが終了すると、図３、図１３に示すように、位置算出処理S9を行う。位置算出処理S9では、初期位置座標情報に位置移動量ｒ^Gの累積和を加算し、これを位置補正量で補正することによって、移動体１０の現在位置を算出する。ただし、位置補正量を算出する前に位置算出処理S9を行う時は、補正を行わずに（或いは位置補正量をゼロとして）現在位置を算出する。

【0091】

以上が図３に示す位置姿勢計測の処理の全体の流れであり、処理装置１８は、移動体１０による構造物K1の調査を開始して終了するまでの間、各処理を順番に繰り返す。そして、構造物K1の調査が終了すると、処理装置１８は、構造物K2，K3の調査を行うため、構造物K2，K3に対し、同様の処理を実行する。

【0092】

構造物K2とK3の調査を行う時、わざわざ移動体１０を発着場HTに戻して既知情報取得処理S1を実行するのが面倒であれば、構造物K1に対する既知情報取得処理S1の時に、構造物K2，K3の特定部位座標情報（構造物情報）も合わせて取得するようにすることができる。この実施形態の位置姿勢計測方法では、構造物の変わる毎に位置補正量を算出して更新されるので、仮に初期位置座標情報が更新されなくても、位置計測の精度はほとんど低下しない。

【0093】

以上説明したように、第一の実施形態の位置姿勢計測方法及び位置姿勢プログラムは、インフラ構造物等の表面の状態を、移動体を用いて調査する用途に用いられるものであり、現場がトンネルの内部のような非ＧＰＳ環境下であっても、問題なく実行することができる。

【0094】

また、移動体１０に取り付けた角速度センサ１２及び加速度センサ１４の情報だけから位置を算出すると、積分や加算によって誤差が蓄積して精度が低下してしまうところ、この実施形態の位置姿勢計測方法及び位置姿勢プログラムでは、個々の構造物に適用される位置補正量を独特な方法で算出し、角速度センサ１２及び加速度センサ１４の情報を位置補正量で補正して位置を算出するので、移動体１０の位置を高精度に計測することができる。また、図１に示ように、１つの現場に複数の構造物が分散して設置されている場合でも、構造物毎に位置補正量を算出して更新する構成なので、移動体１０の位置を常に高精度に計測することができる。

【0095】

この実施形態は、特定部位P1の画像データを取得する際、使用者が、「移動体１０を所定の場所T1でホバリングさせ、特定部位撮像指令を出す」という作業を行わなければならないが、使用者にとってそれほど大きな負担にはならず、これによって、図１３に示す位置補正算出処理S8（ステップS81〜S83）は、第一の具体例（図１４）や第二の具体例（図１５）に示すように、シンプルなアルゴリズムで実行でき、処理装置の負担も小さくなる。また、条件が合えば、第一の具体例（図１４）のように、Ｕ軸方向の位置補正量を算出するのを省略することができるので、位置補正量算出処理のアルゴリズムをさらにシンプルにすることができる。

【0096】

次に、本発明の位置姿勢計測方法及び位置姿勢計測プログラムの第二の実施形態について、図１６〜図２０に基づいて説明する。この実施形態の位置姿勢計測方法は、第一の実施形態と同様に、移動体１０を用いて構造物Ｋ１，Ｋ２，Ｋ３の状態を調査するシステム（図１）において、移動体１０に搭載された処理装置１８(h)により実行される。また、この実施形態の位置姿勢計測プログラムは、この実施形態の位置姿勢計測方法を実行させるための各処理の実行用プログラムで、処理装置１８(h)にインストールして実行される。以下、第二の実施形態について、第一の実施形態と同様の構成は同一の符号を付して説明を省略し、構成が異なる部分を中心に説明する。

【0097】

第一の実施形態は、移動体１０に搭載される慣性センサの種類が少ない（角速度センサ１２及び加速度センサ１４の２種類だけ）という特徴があるが、上記のアライメントを効果的に行うには、地球角速度を精度よく検出できる高性能な角速度センサ１２を使用する必要があり、装置が高価になる可能性がある。これに対して、第二の実施形態は、図１６に示すように、初期姿勢角ψ0の算出に電子コンパス２０の方位信号Ｈを利用する構成になっており、これによって安価な角速度センサ１２が使用可能になり、装置全体のコストを抑えることができるという特徴がある。

【0098】

処理装置１８(h)が行う位置姿勢計測方法の全体の流れは、図３と同様であるが、アライメント処理S3、角速度・加速度バイアス算出処理S4及び姿勢角算出処理S5s，S5uの内容に少し異なる部分があるので、異なる部分について説明する。

【0099】

処理装置１８(h)が行うアライメント処理S3は、図１７に示すように、３つの初期姿勢角φ0，θ0，ψ0のうち、φ0，θ0についての処理は処理装置１８と同じであるが、初期姿勢角ψ0は、ステップS34(h)に示すように、電子コンパス２０の方位信号Ｈに基づいて算出し、調整は行わない。従って、初期姿勢角算出ステップS31(h)では、図１８に示すように、初期姿勢角φ0，θ0だけを算出し(ステップS312(h))、これらを第1及び第２の初期姿勢角調節ステップS22，S23で調整するという流れになる。安価な角速度センサは、微小な地球角速度を高精度に検出することが難しいが、電子コンパス２０の方位信号Ｈを利用することによって初期姿勢角ψ0を算出することができ、アライメント処理を行うことができる。地球角速度よりも高速の角速度の検出は、安価な角速度センサでも可能なので、角速度センサ１２のコストダウンを図ることができる。

【0100】

その他、処理装置１８(h)が行う角速度・加速度バイアス算出処理S4は、図１９のステップS41(h)，S42(h)に示すように、角速度誤差Δω^Bを算出する時、地球角速度ωp^Bをゼロと仮定して行う。同様に、処理装置１８(h)が行う姿勢角算出処理S5s，S5uは、図２０のステップS51s(h)，S52s(h)とステップS51u(h)，S52u(h)に示すように、角速度誤差Δω^Bを算出する時、地球角速度ωp^Bをゼロと仮定して行う。

【0101】

第二の実施形態の位置姿勢計測方法及び位置姿勢計測プログラムにおいても、第一の実施形態と同様の効果を得ることができ、移動体１０に搭載するセンサの種類は増えるものの、装置全体のコストダウンを図ることができる。

【0102】

なお、本発明の位置姿勢制御方法及び位置姿勢制御プログラムは、上記実施形態に限定されるものではない。例えば、図１４、図１５に示した位置補正量算出処理の２つの具体例では、移動体１０と特定部位P1との離間距離m1，m2を第１及び第２の位置関係情報としたが、本発明が目的とする位置補正量を導出可能なものであれば、離間距離以外の情報を第１及び第２の位置関係情報としてもよい。

【0103】

上記実施形態の説明では、処理装置が既知情報として取得する構造物情報（構造物に関する情報）について、特定部位座標情報しか説明しなかった。しかし、さらに他の情報を追加してもよく、例えば、構造物の表面の、特定部位P1を通る法線の方向を示す情報を構造物情報に加える方法が考えられる。補正量算出工程では、特定部位P1の画像データから第２の位置関係情報を導出する時、より高精度に導出するため、画像データは、できるだけ特定部位P1を正面から見て撮像されたものであることが好ましい。したがって、構造物K1の表面の、特定部位P1を通る法線の方向を示す情報があれば、移動体を撮像するのに適した位置が概ね特定できるので、処理装置から移動体を操縦又は操作する使用者に向けて、その適した位置を知らせるようにすることができる。

【0104】

上記実施形態の中で示した静止状態推定フィルタ及び運動状態推定フィルタは、あくまで好ましい例を示したものであり、上述した効果が得られる範囲で、式の内容は変更してもよい。

【0105】

本発明の位置姿勢計測方法を実行する処理装置は、移動体の内部に搭載してもよいし、移動体と通信可能な外部機器（例えば、制御用コンピュータ等）の内部に設けてもよい。また、処理装置に送信される初期位置座標情報及び構造物情報は、上記外部機器の内部に設けたデータベース等に格納されていてもよいし、現場から離れた基地局のデータベース等に格納されていてもよい。

【0106】

調査対象の構造物の種類は特に限定されず、橋梁や橋脚、堤防、トンネル、送電鉄塔、高層ビル等、様々な構造物等を対象にすることができる。例えば、図１に示す構造物K1〜K3は「橋脚」であり、１つ１つの大きさは比較的小さく互いに間隔を空けて設置されているという特徴があり、このような現場で調査を行う時は、上記のように、複数の構造物に各々特定部位P1〜P3を設定し、調査する構造物が変わる毎に、特定部位撮像処理及び位置補正量算出処理を実行することが好ましい。また、図２１に示す構造物Kは「堤防」であり、数は１つであるが表面積が大きいという特徴があり、このような現場で調査を行う時は、構造物の表面を複数のエリアE1〜E3に区分してエリア毎に特定部位P1〜P3を設定し、調査するエリアが変わる毎に、特定部位撮像処理及び位置補正量算出処理を実行することが好ましい。

【符号の説明】

【0107】

１０ 移動体
１０ａ 機体
１２ 角速度センサ
１４ 加速度センサ
１６ 撮像装置
１８，１８(h) 処理装置
２０ 電子コンパス
E1〜E3 エリア
K1〜K3 構造物
P1〜P3 特定部位
S1 既知情報取得処理
S2s，S2u フィルタリング処理
S3 アライメント処理
S4 角速度・加速度バイアス算出処理
S5s，S5u 姿勢角算出処理
S6s，S6u 位置移動量算出処理
S7 特定部位撮像処理
S8 位置補正量算出処理
S9 位置算出処理

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】