JP6934367B2 - 位置測定装置、位置測定方法および位置測定用プログラム - Google Patents

Description

本発明は、ＵＡＶ等の航空機の位置を測定する技術に関する。

無人航空機（ＵＡＶ(Unmanned aerial vehicle)）を測量に用いる技術が知られている。この技術では、ＧＮＳＳ(Global Navigation Satellite System）を用いた位置測定装置（所謂ＧＰＳ受信機）、ＩＭＵ（慣性航法装置）、高度計、カメラを搭載したＵＡＶを予め定めた経路に沿って飛行させて地上の撮影を行い、航空写真測量等を行う。

ＵＡＶは、ＧＮＳＳを利用して自身の位置を特定できるが、単独測位であるので、その精度は水平方向で１ｍ程度、垂直方向で３ｍ程度であり、写真測量に求められる精度は得られない。ＧＮＳＳを利用したより高精度の位置測定機器（相対測位が可能な機器）をＵＡＶに搭載する方法もあるが、機器の重量や消費電力の点で汎用のＵＡＶに搭載するのは難しい。この問題に対する対応して、ＴＳ（トータルステーション）で飛行するＵＡＶを追尾し、ＴＳが備えるレーザー測距機能を用いてＵＡＶの位置を特定する方法がある（例えば、特許文献１を参照）。

ＵＳ２０１４／０２１０６６３号公報

上記のＴＳでＵＡＶを追尾する方法では、ＴＳが備えるターゲットの自動追尾機能が利用される。この技術では、探索用レーザー光でＵＡＶを捕捉および追尾する。ＵＡＶは、探索用レーザー光を入射方向に反射する反射プリズムを備え、この反射プリズムからの反射光をＴＳ側で検出することで、ＴＳによるＵＡＶの追尾が行われる。

ところでＴＳによるＵＡＶの追尾では、ＴＳがＵＡＶを見失うことが問題となる。例えば、ＴＳとＵＡＶの間に鳥、電線、電柱、鉄塔、背の高い木、建物等が入った場合に、ＴＳからの探索用レーザー光（追尾光）が遮られ、ＴＳがＵＡＶを見失う場合がある。

ＴＳがＵＡＶを見失った場合、ＴＳの望遠鏡を用いてＵＡＶを探索する操作がオペレータによって行われるが、一旦見失ったＵＡＶを望遠鏡の視野に入れるのは困難である。望遠鏡の倍率を下げれば視野を広くできるが、最大で数１００ｍ遠方を飛行するＵＡＶを広い視野の中から探し出すのは困難である。

望遠鏡でＵＡＶをある程度照準できれば、探索用レーザー光によるＵＡＶの捕捉が可能であるが、上述した理由によりそれは困難であるのが実情である。

このような背景において、本発明は、光学装置によるＵＡＶの追尾において、見失ったＵＡＶの再捕捉が容易な技術を提供することを目的とする。

請求項１に記載の発明は、測距光を用いて移動体の位置を測定する位置測定部と、移動する前記移動体を追跡する制御を行う追跡制御部と、測距用レーザー光を走査して照射し点群データを取得するレーザースキャナ部と、前記追跡制御部が前記移動体を追跡できなくなった場合に、前記点群データに基づいて前記移動体の方向を算出する方向算出部とを備える位置測定装置である。

請求項２に記載の発明は、請求項１に記載の発明において、前記移動体の位置の測定は、前記移動体が備えた反射プリズムから反射された測距光に基づいて行われ、前記点群データから反射プリズムの位置を算出する反射プリズム位置算出部を備えることを特徴とする。

請求項３に記載の発明は、請求項２に記載の発明において、前記点群データおよび予め取得した前記移動体の形状に基づいて前記反射プリズムの位置の算出が行われることを特徴とする。

請求項４に記載の発明は、請求項２または３に記載の発明において、前記点群データの中における前記移動体の形状にフィッティングする点群データに基づいて前記反射プリズムの位置の算出が行われることを特徴とする。

請求項５に記載の発明は、請求項１〜４に記載の発明において、事前に取得した前記移動体の点群データの反射強度および色強度の少なくとも一方に基づき、前記移動体の点群データの抽出が行われる点群データ抽出部を備えることを特徴とする。ここで事前というのは、移動体を追跡できなくなった時点以前という意味である。この以前という語句には、移動体が移動する前の状態も含まれる。

請求項６に記載の発明は、請求項５に記載の発明において、前記色強度がＲＧＢ強度であることを特徴とする。請求項７に記載の発明は、請求項５または６に記載の発明において、前記事前に取得した前記移動体の点群データの反射強度および色強度は、前記追跡制御部による前記移動体の追跡が行われている段階で取得された前記移動体の点群データに基づいて取得されることを特徴とする。

請求項８に記載の発明は、請求項１〜７のいずれか一項に記載の発明において、前記追跡制御部が前記移動体を追跡できなくなった場合に、それ以前における前記移動体の移動経路に基づき前記移動体の位置を予想する位置予想部を備えることを特徴とする。

請求項９に記載の発明は、請求項１〜７のいずれか一項に記載の発明において、前記移動体は、予め定められた移動計画に従って移動し、前記追跡制御部が前記移動体を追跡できなくなった場合に、予め定められた移動計画に基づき前記移動体の位置を予想する位置予想部を備えることを特徴とする。

請求項１０に記載の発明は、測距光を用いて移動体の位置を測定する位置測定ステップと、移動する前記移動体を追跡する制御を行う追跡制御ステップと、測距用レーザー光を走査して照射し点群データを取得するレーザースキャンステップと、前記追跡制御ステップにおいて前記移動体を追跡できなくなった場合に、前記点群データに基づいて前記移動体の方向を算出する方向算出ステップとを有する位置測定方法である。

請求項１１に記載の発明は、コンピュータに読み取らせて実行させるプログラムであって、コンピュータを測距光を用いて移動体の位置を測定する位置測定部と、移動する前記移動体を追跡する制御を行う追跡制御部と、測距用レーザー光を走査して照射することで得た点群データを取得する点群データ取得部と、前記追跡制御部が前記移動体を追跡できなくなった場合に、前記点群データに基づいて前記移動体の方向を算出する方向算出部として動作させる位置測定用プログラムである。

本発明によれば、光学装置によるＵＡＶの追尾において、見失ったＵＡＶの再捕捉を容易に行う技術が得られる。

実施形態の概念図である。 実施形態の位置測定装置の一例であるＴＳ（トータルステーション）のブロック図である。 ＵＡＶ探索演算部のブロック図である。 処理の手順の一例を示すフローチャートである。

（構成）
図１には、発明を利用した位置測定装置の一例であるＴＳ（トータルステーション）のブロック図が示されている。ＴＳ１００は、飛行するＵＡＶ２００を追跡しながら測距光を用いたＵＡＶ２００の三次元位置の測定を行う。ＵＡＶ２００の追跡は、探索光を用いてＵＡＶ２００が備えた反射プリズム２０２を探索することで行われ、ＵＡＶ２００の位置の測定は、測距光を反射プリズム２０２に照射し、その反射光を検出することで行われる。

そして、ＴＳ１００がＵＡＶ２００を見失った場合に、ＴＳ１００が備えたレーザースキャン機能によりＵＡＶ２００の点群データを取得し、それに基づきＴＳ１００から見たＵＡＶ２００の方向を算出し、ＴＳ１００をＵＡＶ２００の方向に向ける制御が行われる。その後、探索光による反射プリズム２０２の捕捉、測距光による反射プリズム２０２（ＵＡＶ２００）の位置の特定を再開する。

ＴＳ１００は、ＧＮＳＳを用いた位置測定装置、画像を取得するカメラ、ターゲット（ＵＡＶ２００の反射プリズム２０２）を探索する探索用レーザースキャン機能、測距用レーザー光を用いてターゲット（反射プリズム２０２）までの距離を測距するレーザー測距機能、レーザー測距されたターゲットの方向（水平角と垂直角（仰角または俯角））を計測する機能、ターゲットまでの距離と方向からターゲットの三次元位置を算出する機能、外部の機器との間で通信を行う機能、点群データを得るためのレーザースキャン機能を有する。

ターゲットまでの距離と方向を計測することで、ＴＳ１００に対するターゲットの位置を測定できる。ここで、ＴＳ１００の位置が判っていれば、ターゲット（この場合はＵＡＶ２００）の地図座標系における位置（緯度・経度・高度もしくは直交座標系上のＸＹＺ座標）を知ることができる。この機能は、市販のＴＳが持っている機能であり、特別なものではない。ＴＳに関する技術としては、例えば、特開2009-229192号公報や特開2012-202821号公報等に記載されている。なお、地図座標系というのは、地図情報を扱う座標系（例えば、緯度，経度，高度（標高））であり、例えばＧＮＳＳで得られる位置情報は通常地図座標系で記述される。

以下、本実施形態で利用するＴＳ（トータルステーション）１００の一例を説明する。図２には、ＴＳ１００のブロック図が示されている。ＴＳ１００は、カメラ１０１、ターゲット探索部１０２、測距部１０３、水平・垂直方向検出部１０４、水平・垂直方向駆動部１０５、データ記憶部１０６、位置測定部１０７、通信装置１０８、ターゲット位置算出部１０９、ＵＡＶ追跡制御部１１１、レーザースキャナ１１２、制御用マイコン１１３およびＵＡＶ探索演算部３００を備えている。

図２に示すその他の機能部は、専用のハードウェアで構成してもよいし、マイコンによりソフトウェア的に構成してもよい。図２の構成を実現するために利用するハードウェアとしては、各種の電子デバイス(例えば、カメラを構成するカメラモジュールや通信装置１０８を構成する無線モジュール)、モータ等を利用した各種の駆動機構、センサ機構、光学部品、各種の電子回路、ＣＰＵ（Central Processing Unit）、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field Programmable Gate Array）等が挙げられる。以上のハードウェアの構成に関しては、ＵＡＶ２００についても同じである。

カメラ１０１は、ＵＡＶ２００やターゲット等の測量対象の動画像または静止画像を撮影する。カメラ１０１は、ＣＣＤやＣＭＯＳセンサを用いたカメラモジュールを利用して構成され、望遠鏡を介して測位対象（例えばＵＡＶ２００）の撮影を行い、その画像データを得る。通常、カメラ１０１が望遠鏡を介して撮影した画像を利用して測量対象のターゲットの概略の視準がオペレータにより行われ、その後自動追尾機能による自律動作によりターゲットの精密な視準が行われる。カメラ１０１が撮影した画像のデータは、測距対象に係る測定時刻、測定方向、測定距離、測定対象の位置等のデータと関連付けされて適当な記憶領域に記憶することができる。

ターゲット探索部１０２は、三角錐型または扇形ビームを有した探索用レーザー光を用いたタ−ゲット（ＵＡＶ２００の反射プリズム２０２）の探索を行う。ターゲットの探索は、ＴＳ１００を基準位置として行われる。ターゲットを探索する技術については、例えば日本国特許第５１２４３１９号公報に記載されている。

測距部１０３は、測距用レーザー光を用いたターゲットまでの距離の計測を行う。測距部１０３は、測距用レーザー光の発光素子、照射光学系、受光光学系、受光素子、測距演算部、測距基準光の光路を備えている。対象物までの距離は、対象物から反射された測距光と基準光の位相差から算出される。距離の算出方法は、通常のレーザー測距と同じである。

水平・垂直方向検出部１０４は、測距部１０３が測距したターゲットの水平方向角と垂直方向角（仰角および俯角）を計測する。ターゲット探索部１０２および測距部１０３の光学系を備えた筐体部分は、水平回転および仰角（俯角）制御が可能であり、水平方向角と垂直方向角は、エンコーダにより計測される。このエンコーダの出力が水平・垂直方向角検出部１０４で検出され、水平方向角と垂直方向角（仰角および俯角）の計測が行われる。

水平・垂直方向駆動部１０５は、ターゲット探索部１０２および測距部１０３の光学系を備えた筐体部分の水平回転および仰角制御（および俯角制御）を行うモータ、該モータの駆動回路、該駆動回路の制御回路を含む。なお、この筐体部分には後述するレーザースキャナ１１２も配置されている。データ記憶部１０６は、ＴＳ１００の動作に必要な制御プログラム、各種のデータ、測量結果等を記憶する。

位置測定部１０７は、ＧＮＳＳを用いたＴＳ１００の位置の測定を行う。位置測定部１０７は、相対測位と単独測位の両方を行う機能を有する。相対測位を行える環境であれば、相対測位を用いたＴＳ１００の位置の測定が好ましいが、相対測位が困難な場合は単独測位によるＴＳ１００の位置の測定が行われる。位置が既知の場所にＴＳ１００を設置する場合、位置測定部１０７の機能は必ずしも利用されない。これは、他の機器を用いてＴＳ１００の位置を特定する場合も同じである。

通信装置１０８は、外部の機器との間で通信を行う。ＴＳ１００は、外部の端末（専用端末、ＰＣ、タブレット、スマートフォン等）による操作が可能であり、この際の通信が通信装置１０８を用いて行われる。また、通信装置１０８は、ＴＳ１００の動作に必要な各種のデータの受け付け、およびＴＳ１００が取得した各種のデータの外部への出力を行う。例えば、ＴＳ１００からインターネット回線を介して地図情報や地形情報を扱うデータサーバにアクセスし、測量に係る各種の情報をＴＳ１００が取得することができる。

ターゲット位置算出部１０９は、ターゲット（この場合は、ＵＡＶ２００搭載の反射プリズム２０２）までの距離と方向からＴＳ１００に対するターゲットの位置（座標）を算出する。ここで、ターゲットまでの距離は、測距部１０３で得られ、ターゲットの方向は水平・垂直方向検出部１０４で得られる。基準位置となるＴＳ１００の位置は、位置測定部１０７で特定されるので、ＴＳ１００に対するターゲットの位置を求めることで、ターゲットの地図座標系における位置を求めることができる。

ＵＡＶ追跡制御部１１１は、補足したＵＡＶを追尾する制御を行う。すなわち、ターゲット探索部１０２で検出した追尾光（ＵＡＶから反射した追尾光）の入射方向の変化に対応させてＴＳの方向を制御し、空中を移動するＵＡＶに常にＴＳ１００の光軸が向くようにする動的な制御がＵＡＶ追跡制御部１１１で行われる。具体的には、ターゲット（反射プリズム２０２）から反射した追尾光のＴＳ１００に対する入射方向の変化を検出し、それに基づき反射プリズム２０２の位置にＴＳ１００の光軸（測距部１０３からの測距光の光軸）が常に向くように、水平・垂直方向駆動部１０５に制御信号を出力する処理がＵＡＶ追跡制御部１１１で行われる。

レーザースキャナ１１２は、測距用レーザー光をスキャン光として用いて点群データを得る。点群データは、対象物を三次元座標が判明した点の集合として捉えたデータである。この例では、ターゲット探索部１０２とレーザースキャナ１１２は別構成であり、レーザースキャナ１１２は、ターゲット探索部１０２とは別に動作する。ここで、レーザースキャナが扱うレーザー光の波長は、ターゲット探索部１０２が用いるレーザー光と干渉しないように、ターゲット探索部１０２が用いるレーザー光とは別の波長が選択されている。点群データを得るレーザースキャナについては、例えば特開２０１０−１５１６８２号公報、特開２００８―２６８００４号公報、米国特許８７６７１９０号公報、米国特許７９６９５５８号公報等に記載されている。

以下、レーザースキャナ１１２について説明する。レーザースキャナ１１２は、測距用レーザー光の照射部、対象物から反射した測距用レーザー光を受光する受光部、測距用レーザー光の飛行時間に基づき対象物までの距離を検出する測距部、測距用レーザー光の照射方向（測距方向）を検出する測距方向検出部、自身の位置と測距距離と測距方向とに基づき測距用レーザー光の反射点の三次元位置を算出する測距対象点の位置算出部、測距用レーザー光の照射方向とその反射光の受光方向（測距光の光軸）の方向を制御するスキャン制御部を備える。

測距用レーザー光は、特定の繰り返し周波数でパルス出力され、スキャンされながら特定の範囲に照射される。測距用レーザー光の飛行時間から反射点までの距離が計算される。通常は、装置内に設けられた基準光路を飛翔したリファレンス光と対象物に照射されそこから反射された測距光との位相差から対象物までの距離が算出される。測距された距離、測距用レーザー光の照射方向およびレーザースキャナ１１２の位置から、反射点の三次元位置が計算される。この反射点の位置を多数測定することで点群データが得られる。ここで、ＴＳ１００におけるレーザースキャナ１１２の位置と向きは、予め既知の情報として取得されており、レーザースキャナ１１２の地図座標系における位置は、位置測定部１０７の測位データに基づき算出される。

測距用レーザー光が特定の発振周波数でスキャンして照射されることで対象物における多数の反射点それぞれの三次元座標（レーザースキャナ１１２を原点とする座標系における三次元座標）が取得される。この対象物における多数の反射点の集合が点群データとなる。点群データでは、三次元位置が特定された点の集合として対象物が捉えられる。

レーザースキャナ１１２は、反射点のＲＧＢ強度を取得できる。ＲＧＢ強度は、反射光をＲ用フィルタ、Ｇ用フィルタ、Ｂ用フィルタで選別し、各色の光強度を検出することで得られる。よって、得られる点群データ各点のＲＧＢ強度に関するデータも得られる。なお、ＲＧＢに限定されず一または複数の特定の色情報の強度を取得する形態も可能である。

制御用マイコン１１３は、後述する図３および図４の処理の手順の制御およびＴＳ１００全体の動作の制御を行う。ＵＡＶ探索演算部３００は、図３に示す点群データ取得部３０１、スキャン範囲設定部３０２、ＵＡＶ識別用点群データ抽出部３０３、ＵＡＶ位置予想部３０４、ＵＡＶ位置算出部３０５、反射プリズム位置算出部３０６、反射プリズムの方向算出部３０７を有する。

ＵＡＶ探索演算部３００は、コンピュータとしての機能を有し、ＣＰＵ、メモリ、その他演算回路、インターフェース回路およびインターフェース機能を有している。ＵＡＶ探索演算部３００は、専用のハードウェアで構成されＴＳ１００に組み込まれている。ＵＡＶ探索演算部３００の機能の一部または全部をＴＳ１００に接続される外部の機器で実現する構成も可能である

例えば、図３に示す各機能部は、ＣＰＵ（Central Processing Unit）、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field Programmable Gate Array）に代表されるＰＬＤ（Programmable Logic Device）などの電子回路により構成される。

図３に示す各機能部の一部または全部を専用の演算回路によって構成してもよい。また、ソフトウェア的に構成された機能部と、専用の演算回路によって構成された機能部が混在していてもよい。

各機能部を専用のハードウェアで構成するのか、ＣＰＵにおけるプログラムの実行によりソフトウェア的に構成するのかは、要求される演算速度、コスト、消費電力等を勘案して決定される。機能部を専用のハードウェアで構成することとソフトウェア的に構成することは、上述した違いはあるが、特定の機能を実現するという観点からは、等価である。

ＵＡＶ探索演算部２００を市販のＰＣ（パーソナルコンピュータ）、ＷＳ（ワークステーション）、あるいはデータ処理サーバ等を利用して構成することもできる。この場合、ＵＡＶ探索演算部２００が備える機能を実現するアプリケーションソフトウェアをこれらコンピュータ機器にインストールし、ＵＡＶ探索演算部２００として動作させる。

以上の各種の演算を行う部位に関する説明は、ターゲット位置算出部１０９やＵＡＶ追跡制御部１１１等のＴＳ１００が備える演算機能を有する部分についても同様である。

ＰＣ、ＷＳ、データ処理用サーバを用いてＵＡＶ探索演算部２００を実現する場合、操作端末としてスマートフォンやタブレット等を用いられる。この場合、ＴＳ１００、ＵＡＶ探索演算部２００として機能するＰＣ，ＷＳ，データ処理用サーバ，操作端末として機能するスマートフォンやタブレットが無線ＬＡＮ回線やインターネット回線等で接続されたシステムが構成される。

以下、ＵＡＶ探索演算部３００を構成する各機能部について説明する。点群データ取得部３０１は、レーザースキャナ１１２が得た点群データを取得する。スキャン条件設定部３０２は、レーザースキャナ１１２によるスキャン範囲、スキャン速度、測距用レーザー光の発振周波数の設定を行う。例えば、発振周波数を固定した場合、スキャン範囲を狭め、スキャン速度を速めることでスキャン密度が高くなる。

ＵＡＶ識別用点群データ抽出部３０３は、レーザースキャナ１１２が得た点群データの中から、ＵＡＶ２００から得られた点群データ（ＵＡＶ２００を反射点とする点群データ）を抽出する。

以下、ＵＡＶ識別用点群データ抽出部３０３で行われる処理について説明する。まず、ＴＳ１００がＵＡＶ２００を捕捉している状態、すなわちターゲット探索部１０２がＵＡＶ２００を捕捉している状態において、レーザースキャナ１１２が取得したＵＡＶ２００の点群データのＲＧＢ強度に関するデータを取得する。このデータは、ＵＡＶ２００の追跡中に常に取得する。

ターゲット探索部１０２がＵＡＶ２００を捕捉している状態では、ＵＡＶ２００の位置はターゲット位置算出部１０９で算出され判っており、それとレーザースキャナ１１２が得た点群データとを比較することで、レーザースキャナ１１２が得た点群データにおけるＵＡＶ２００の点群データを抽出できる。この抽出した点群データからＵＡＶ２００の点群データそれぞれのＲＧＢ強度を取得する。このＵＡＶ２００に係る点群データのＲＧＢ強度がリファレンスデータとなる。このリファレンスデータは、ＵＡＶ２００を捕捉している状態で常に（あるいは定期的に）更新する。この処理では、その時点に近い過去に更新されたリファレンスデータを用いてＵＡＶ２００のレーザースキャン点群データの抽出が継続して繰り返し行われる。以上の処理がＵＡＶ識別用点群データ抽出部３０３で行われる

ＵＡＶ位置予想部３０４は、レーザースキャナ１１２がＵＡＶ２００を見失った場合（スキャン範囲からＵＡＶ２００の情報が取得できなくなった場合）に、それ以前のＵＡＶ２００の飛行経路からＵＡＶ２００の飛行位置を予想する。ＵＡＶが捕捉されている間は、ＵＡＶ２００の位置の推移は刻々とＴＳ１００の側で取得されている。そこで、ＵＡＶ２００を見失った時点におけるＵＡＶ２００の位置、飛行方向、飛行速度から、見失った以後の特定の時刻におけるＵＡＶ２００の位置を予想する。

ＵＡＶ２００が飛行計画に従って飛行する場合、ある時刻におけるＵＡＶ２００の計画上における位置は予想できる。この処理をＵＡＶ位置予想部２０４において行ってもよい。

ＵＡＶ位置取得部３０５は、ＵＡＶ識別用点群データ取得部３０３が取得したＵＡＶ２００の点群データに基づきＵＡＶ２００の位置を取得する。ＵＡＶ２００の位置は、ＴＳ１００を原点した座標系上で取得される。

ターゲット探索部１０２がＵＡＶ２００を捕捉している状態において、ＵＡＶ２００を見失った場合に、ＵＡＶ位置取得部３０５によりＵＡＶ２００の位置が取得される。以下、この処理について説明する。ターゲット探索部１０２がＵＡＶ２００を見失うと、その直前に取得しているＵＡＶ２００の点群データのＲＧＢ情報を読み出す。次いで、その時点でレーザースキャナ１１２が得た点群データの中から、当該ＲＧＢ情報に一致または近似する点群データを抽出し、それをＵＡＶ２００の点群データとして取得する。

ここで点群データの色情報を利用してＵＡＶ２００の点群データを特定するのは、飛行中にＵＡＶに近づいてくることが多々ある鳥の点群データをＵＡＶ２００と誤認定する問題を回避するためである。また、この処理では、ＵＡＶ２００の形状に係る点群データが抽出されるので、形状の違いを利用したＵＡＶ２００の識別も同時に行われる。このため、鳥をＵＡＶ２００と誤判定する不都合の発生が効果的に抑えられる。

なお、ターゲット探索部１０２がＵＡＶ２００を見失い、更にレーザースキャナ１１２もＵＡＶ２００を見失った場合（ＵＡＶ２００の点群データが取得できなくなった場合）、レーザースキャナ１１２のスキャン範囲を変更し、ＵＡＶ２００の点群データの取得を試みる。スキャン範囲の変更は、スキャン条件設定部３０２で行われる。スキャン範囲の変更に係る処理については、後述する図４のステップＳ１１０に関連して説明する。

反射プリズム位置算出部３０６は、ＵＡＶ２００の反射プリズム２０２の位置をＵＡＶ識別用点群データ取得部３０３が取得したＵＡＶ２００の点群データに基づき算出する。反射プリズム２０２の位置は、ＴＳ１００を原点した座標系上で取得される。

反射プリズム２０２の位置の算出は以下のようにして行われる。まず、ＵＡＶ識別用点群データ抽出部３０３の機能によりＵＡＶ２００の点群データを得る。この段階で点群データの密度が規定の閾値以上であれば、得られた点群に基づきＵＡＶ２００の形状を検出する。ＵＡＶ２００における反射プリズム２０２の位置は既知であるので、ＵＡＶ２００の形状が点群データから取得（再現）され、それに基づき反射プリズム２０２の位置が算出される。なお、スキャン光に起因する反射プリズム２０２からの反射光が得られた場合、当該反射光の反射点が反射プリズム２０２の位置として取得される。

上記の処理において、ＵＡＶ２００のものと推定される点群データに既知のＵＡＶ２００の形状モデルをフィッティングさせ、そのフィッティングしたＵＡＶ２００の形状モデルに基づいて反射プリズム２０２の位置を算出してもよい。この場合、点群データの数や密度が少なくても精度よく反射プリズム２０２の位置を算出できる。

上記の処理において、ＵＡＶ２００の点群データの密度が不足している場合、スキャン範囲を狭め、またスキャン密度を高めたレーザースキャンを再度行い、そこで得られた点群データに基づき、反射プリズム２０２の位置の算出を行う。

反射プリズムの方向算出部３０７は、ＴＳ１００の位置から見た反射プリズム２０２の方向を算出する。この処理では、ＴＳ１００から反射プリズム位置算出部３０６が算出した反射プリズム３０６の位置に向かう方向線を設定し、この方向線の方向がＴＳ１００から見た反射プリズム２０２の方向として算出される。

（処理の一例）
以下、ＴＳ１００で行われる処理の一例を説明する。図４は、処理の手順の一例を示すフローチャートである。図４の処理を実行するプログラムは、データ記憶部１０６等の適当な記憶領域に記憶され、そこから読み出されて実行される。当該プログラムを適当な記憶媒体やデータサーバ等に記憶させ、そこから読み出して実行する形態も可能である。

処理が開始されると、ＵＡＶ２００の反射プリズム２０２の探索が行われる（ステップＳ１０１）。この処理は、ターゲット探索部１０２で行われる。また、レーザースキャナ１１２による点群データを取得するためのレーザースキャンが開始される。また、レーザースキャナ１１２から得られるＵＡＶ２００の点群データのＲＧＢデータの取得も開始される。

例えば、地上に着陸している状態でＴＳ１００がＵＡＶ２００を捕捉し、その状態からＵＡＶ２００の飛行を開始させ、ＴＳ１００によるＵＡＶ２００の追跡が開始される。またこの際、ＵＡＶ２００をスキャン範囲に収めたレーザースキャンがレーザースキャナ１１２により開始され、ＵＡＶ２００の点群データの取得が開始される。この段階のレーザースキャンの範囲は、ＵＡＶ２００を中心とした半径５〜２０ｍ程度の範囲が選択される。また、以下のような態様も可能である。この場合、ＵＡＶ２００の飛行が開始されると、最初に地上から規定の高さにまで上昇し、そこで停止する。この停止位置は予め飛行計画上で規定されており、ＴＳ１００の側でも把握されている。そこで、この上昇して停止するＵＡＶ２００の位置に照準してＴＳ１００はＵＡＶ２００の捕捉を行う。

ターゲットとなる反射プリズム２０２を捕捉したら、ＴＳ１００の光軸を反射プリズム２０２に向け（ステップＳ１０２）、測距部１０３から測距光を反射プリズム２０２に向けて照射し、ＵＡＶ２００の位置の測定を行う（ステップＳ１０３）。ＵＡＶ２００の位置の測定は、ターゲット位置測定部１０９で行われる。

ステップＳ１０１→Ｓ１０２→Ｓ１０３の処理は繰り返し行われ、移動するＵＡＶ２００の追跡が継続して行われる。そして、一定間隔でステップＳ１０４の判定が行われ、反射プリズム２０２の測距情報が得られている状態では、ステップＳ１０４→ステップＳ１０１と進み、移動するＵＡＶ２００に指向するＴＳ１００の向きの調整が継続して行われる。

反射プリズム２０２の測距情報が得られなくなると、ステップＳ１０４からステップＳ１０５に進み、レーザースキャナ１１２によるＵＡＶ２００の点群データが得られているか否かの判定が行われる。ステップＳ１０３の段階でＵＡＶ２００の位置は測定されていり、ＴＳ１００から見たその方向も判っている。よって、ステップＳ１０３の段階ではＵＡＶ２００の点群データは特定されている。ここで、ターゲット探索部１０２がＵＡＶ２００を見失った際に、レーザースキャナ１１２のスキャン範囲にＵＡＶ２００が存在すれば、ステップＳ１０５の判定はＹＥＳとなるが、そうでなければステップＳ１０５の判定はＮＯとなる。

ＵＡＶ２００の点群データの特定は、継続して取得しているＵＡＶ２００からのレーザースキャン光の反射光のＲＧＢ強度に基づき行われる。具体的には、得られた点群データのＲＧＢ強度と、リファレンスとなるそれ以前のＵＡＶ２００を捕捉していた段階で取得していたＵＡＶ２００の点群データのＲＧＢ強度とを比較する。ここで、ＲＧＢ強度の差が閾値以下（例えば、ＲＧＢそれぞれにおける差が３０％以下）の場合に、当該点群データをＵＡＶ２００の点群データと判定する。この処理は、ＵＡＶ識別用点群データ抽出部３０３で行われる。

ＵＡＶに鳥が近づくことが多々あり（鳥の縄張りの問題と予想される）、レーザースキャナによる点群データの有無だけでは、鳥をＵＡＶと誤判定する問題がある。上記スキャン反射光のＲＧＢ強度を利用した判定を用いることで、ＵＡＶと鳥の識別が可能となる。なお、色強度による判定は、ＲＧＢの１または２つにおいて行う方法、ＲＧＢ以外の色成分について行う方法、波長スペクトルの分布に基づいて行う方法等が挙げられる。

レーザースキャナ１１２によるレーザースキャンは、ＵＡＶ２００を含めた周辺の空域に対して行われており、測距部１０３による反射プリズム２０２の捕捉ができなくなった状態で、レーザースキャナ１１２によるＵＡＶ２００の捕捉が可能な場合もある。よって、ステップＳ１０５の判定は、ＹＥＳ／ＮＯの両方が有り得る。

ステップＳ１０５の判定がＹＥＳの場合、すなわちＵＡＶ２００の点群データが得られている場合、ステップＳ１０６に進み、当該点群データに基づくＵＡＶ２００の位置の取得を行う。ステップＳ１０６において、ＵＡＶ２００の位置を得たら、ＵＡＶ２００の点群データに基づき、反射プリズム２０２の位置の取得が行われる（ステップＳ１０７）。この処理は反射プリズム位置算出部３０６で行われる。

なお、ＵＡＶ２００の位置の取得時に利用した点群データの密度が薄く、当該点群データから反射プリズム２０２の位置の特定が難しい（あるいは精度が期待できない）場合も有り得る。この場合、ＵＡＶ２００の位置に集中したレーザースキャンを行う設定にスキャン条件を設定し直し、スキャン密度を高めた再度のレーザースキャンを行い、その後に得られたＵＡＶ２００に係る点群データに基づき、反射プリズム２０２の位置の取得処理を行う。

反射プリズム２０２の位置を算出したら、ＴＳ１００から見た反射プリズム２０２の方向を算出し（ステップＳ１０８）、ステップＳ１０１以下の処理を再度実行する。この場合、ステップＳ１０８により算出された方向を中心にターゲット探索部１０２による反射プリズム２０２の探索が行われる（ステップＳ１０１）。反射プリズム２０２の方向の算出は以下のようにして行われる。この処理では、まずＴＳ１００の位置と反射プリズム２０２の位置を結ぶ方向線を算出する。次に、ＴＳ１００から見たこの方向線の延在方向を求める。この方向がＴＳ１００から見た反射プリズム２０２の方向となる。

ステップＳ１０５に戻り、ＵＡＶ２００の点群データが取得できない場合、ステップＳ１０９に進む。ステップＳ１０９では、その時点におけるＵＡＶの飛行位置を予想する。この処理は、ＵＡＶ位置予想部２０４で行われる。

ＵＡＶ２００の位置を予想したら、当該予想位置が重点的にスキャンされるようにスキャン条件の設定を行う（ステップＳ１１０）。この処理は、スキャン条件設定部３０２で行われる。スキャン条件を設定したら、レーザースキャナ１１２によるレーザースキャンを行い（ステップＳ１１１）、ステップＳ１０５以下の処理を繰り返す。

（優位性）
ＴＳ１００は、ターゲット探索部１０２とレーザースキャナ１１２を備える。ＴＳ１００がターゲット探索部１０２の機能によってＵＡＶ２００を追跡する際、同時にレーザースキャナ１１２によりＵＡＶ２００のレーザースキャンを行う。こうすることで、ターゲット探索部１０２がＵＡＶ２００を見失った際に、レーザースキャナ１１２のレーザースキャン機能を利用して見失ったＵＡＶ２００の再捕捉が可能となる。

ターゲット探索部１０２は、反射プリズム２０２をピンポイントで捕捉する関係で探索範囲が狭い。他方で、３次元点群データを取得するためのレーザースキャナ１１２は、スキャン範囲の設定は自由であり、広い範囲を探索できる。そこで、本実施形態では、ＴＳ１００によるＵＡＶ２００の測位を行っている状態で、並行してレーザースキャナ１１２によるＵＡＶ２００およびその周囲のレーザースキャンを行い、ＵＡＶ２００を見失った場合におけるＵＡＶ２００の再捕捉にレーザースキャンによって得た点群データを利用する。これにより、ＴＳ１００が見失ったＵＡＶ２００の再捕捉が容易となる。

（変形例）
ターゲット探索部１０２のターゲット探索用のレーザー光がレーザースキャナの機能を備えていてもよい。この場合、ターゲット（ＵＡＶ２００）を見失った段階でターゲット探索モードからレーザースキャン機能に移行し、ＵＡＶ２００の点群データを探索するモードに移行する。

また、ターゲット探索部１０２におけるターゲットの探索を点群データ取得用のレーザースキャナを用いて行うこともできる。この場合、測距光のレーザースキャンを行いながら点群データを得る。この際、反射プリズム２０２からの反射光が得られれば反射プリズム２０２の位置を特定できる。なお、反射プリズム２０２からの反射は強いので、他の反射光と識別が容易である。

反射プリズム２０２からの反射光が得られなくてもＵＡＶ１００からの反射光が得られれば、反射プリズム位置算出部３０６の機能により反射プリズム２０２の位置の算出を行いステップＳ１０２以下の処理によりＵＡＶの測位を行う。なお、ＵＡＶ１００からの反射光であるか否かの判定は、予め取得しておいたＵＡＶ１００からの反射光のＲＧＢ強度に基づいて行われる。

ＵＡＶ２００の追跡は、点群データに基づくＵＡＶ２００の測位→点群データに基づくＵＡＶ２００の形状の認識→反射プリズム２０２の位置の特定→スキャンレーザ光を反射プリズム２０２に照射しての測位を繰り返し行うことで実行される。

ＵＡＶ識別用点群データ抽出部３０３は、レーザースキャン光の反射光の強度に基づきＵＡＶ２００に係る点群の抽出を行ってもよい。この場合、ＴＳ１００がＵＡＶ２００を捕捉している状態でＵＡＶ２００のレーザースキャン光の反射強度に関する情報を取得し、リファレンスデータを作成する。このリファレンスデータは、ＴＳ１００がＵＡＶ２００を捕捉している状態で刻々と更新される。そして、ＴＳ１００が反射プリズム２０２からの反射光をロストした段階（ＴＳ１００のターゲット追尾機能がＵＡＶ２００を見失った段階）で、上記のリファレンスデータに基づきレーザースキャンデータからＵＡＶ２００の点群データを抽出する。この際、閾値によりリファレンスデータに近似する反射強度の点群がＵＡＶ２００の点群として抽出される。また、レーザースキャン反射光の強度と色強度を組み合わせて、ＵＡＶ２００の点群データの抽出を行う態様も可能である。

レーザースキャナ１１２としてスキャンを電子式に行う電子式レーザースキャナを用いることもできる。電子式レーザースキャナでは、測距光の指向方向の制御を機械式のミラーではなく、電子式(Solid state optical phased array方式)で行う。この技術に関しては、例えば米国公開公報ＵＳ２０１５／０２９３２２４号に記載されている。

１００…ＴＳ（トータルステーション）、２００…ＵＡＶ、２０１…カメラ、２００…反射プリズム。

Claims (11)

1. 測距光を用いて移動体の位置を測定する位置測定部と、
   移動する前記移動体を追跡する制御を行う追跡制御部と、
   測距用レーザー光を走査して照射し点群データを取得するレーザースキャナ部と、
   前記追跡制御部が前記移動体を追跡できなくなった場合に、前記点群データに基づいて前記移動体の方向を算出する方向算出部と
   を備える位置測定装置
2. 前記移動体の位置の測定は、前記移動体が備えた反射プリズムから反射された測距光に基づいて行われ、
   前記点群データから反射プリズムの位置を算出する反射プリズム位置算出部を備える請求項１に記載の位置測定装置。
3. 前記点群データおよび予め取得した前記移動体の形状に基づいて前記反射プリズムの位置の算出が行われる請求項２に記載の位置測定装置。
4. 前記点群データの中における前記移動体の形状にフィッティングする点群データに基づいて前記反射プリズムの位置の算出が行われる請求項２または３に記載の位置測定装置。
5. 事前に取得した前記移動体の点群データの反射強度および色強度の少なくとも一方に基づき、前記移動体の点群データの抽出が行われる点群データ抽出部を備える請求項１〜４のいずれか一項に記載の位置測定装置。
6. 前記色強度がＲＧＢ強度である請求項５に記載の位置測定装置。
7. 前記事前に取得した前記移動体の点群データの反射強度および色強度は、前記追跡制御部による前記移動体の追跡が行われている段階で取得された前記移動体の点群データに基づいて取得される請求項５または６に記載の位置測定装置。
8. 前記追跡制御部が前記移動体を追跡できなくなった場合に、それ以前における前記移動体の移動経路に基づき前記移動体の位置を予想する位置予想部を備える請求項１〜７のいずれか一項に記載の位置測定装置。
9. 前記移動体は、予め定められた移動計画に従って移動し、
   前記追跡制御部が前記移動体を追跡できなくなった場合に、予め定められた移動計画に基づき前記移動体の位置を予想する位置予想部を備える請求項１〜７のいずれか一項に記載の位置測定装置。
10. 測距光を用いて移動体の位置を測定する位置測定ステップと、
    移動する前記移動体を追跡する制御を行う追跡制御ステップと、
    測距用レーザー光を走査して照射し点群データを取得するレーザースキャンステップと、
    前記追跡制御ステップにおいて前記移動体を追跡できなくなった場合に、前記点群データに基づいて前記移動体の方向を算出する方向算出ステップと
    を有する位置測定方法。
11. コンピュータに読み取らせて実行させるプログラムであって、
    コンピュータを
    測距光を用いて移動体の位置を測定する位置測定部と、
    移動する前記移動体を追跡する制御を行う追跡制御部と、
    測距用レーザー光を走査して照射することで得た点群データを取得する点群データ取得部と、
    前記追跡制御部が前記移動体を追跡できなくなった場合に、前記点群データに基づいて前記移動体の方向を算出する方向算出部として動作させる位置測定用プログラム。

Images