(57)【要約】【課題】移動体において対象物との相対距離及び相対角度を安定的に推定することのできる距離推定装置及び距離推定システムを提供する。【解決手段】移動体３０の自己位置及び姿勢（機体３２の重心位置Ｇの慣性座標及び姿勢角）を所定周期毎に計測する第１検出部１２と、対象物１に対する距離（ビーム長ｂｌ）を所定周期毎に計測する第２検出部１４と、第１検出部１２及び第２検出部１４が計測した計測値を蓄積して、現時点を含む所定の指定時点数分の計測値を含むデータログとして記憶する記憶部１６と、データログに基づいて移動体３０と対象物１との相対距離及び相対角度を算出する演算部１８と、を備える。【選択図】図１
2020148513.tif 000002
【特許請求の範囲】
【請求項１】
移動体の自己位置及び姿勢を所定周期毎に計測する第１検出部と、
対象物に対する距離を前記所定周期毎に計測する第２検出部と、
前記第１検出部及び前記第２検出部が計測した計測値を蓄積して、現時点を含む所定の指定時点数分の計測値を含むデータログとして記憶する記憶部と、
前記データログに基づいて前記移動体と前記対象物との相対距離及び相対角度を算出する演算部と、
を備える距離推定装置。
【請求項２】
前記演算部は、前記対象物の計測点に対する距離に基づいて前記計測点の慣性座標を算出し、前記記憶部に、前記計測点の慣性座標を蓄積させて、現時点を含む所定の指定時点数分の前記計測点の慣性座標を含むデータログとして記憶させる
請求項１に記載の距離推定装置。
【請求項３】
前記演算部は、前記計測点の慣性座標に基づいて前記対象物の仮想平面の傾きを算出する
請求項２に記載の距離推定装置。
【請求項４】
前記演算部は、前記記憶部に予め設定された所定条件において、前記データログに含まれる前記計測点の慣性座標に対して重み係数を算出し、前記重み係数に基づいて、前記計測点の加重平均を算出し、前記加重平均に基づいて前記仮想平面の傾きを算出する
請求項３に記載の距離推定装置。
【請求項５】
前記演算部は、前記所定周期毎に前記仮想平面の傾きを算出し、前記仮想平面の傾きを、前記記憶部に予め設定された所定条件において、１つ前の仮想平面の傾きに基づいて補正する
請求項３又は４に記載の距離推定装置。
【請求項６】
前記演算部は、前記移動体と前記仮想平面との相対角度を、前記移動体と前記対象物との相対角度として算出する
請求項３から５のいずれか１項に記載の距離推定装置。
【請求項７】
前記演算部は、前記移動体と前記仮想平面との相対距離を、前記移動体と前記対象物との相対距離として算出する
請求項３から６のいずれか１項に記載の距離推定装置。
【請求項８】
前記演算部は、前記記憶部に予め設定された所定条件において、前記データログに含まれる外れ値を除外する
請求項１から７のいずれか１項に記載の距離推定装置。
【請求項９】
前記移動体は、指向性を有する少なくとも１本のビームを発信させる照射部を備え、
前記第２検出部は、前記対象物の計測点に対する距離である前記ビームのビーム長を前記対象物に対する距離として計測する
請求項１から８のいずれか１項に記載の距離推定装置。
【請求項１０】
前記移動体と前記対象物との相対距離及び相対角度に基づいて前記対象物の形状を描画する表示部を備える
請求項１から９のいずれか１項に記載の距離推定装置。
【請求項１１】
前記演算部は、前記対象物の計測点の慣性座標に基づいて前記対象物の仮想平面の傾きを算出し、
前記仮想平面に基づいて前記対象物の形状を描画する表示部を備える
請求項１から９のいずれか１項に記載の距離推定装置。
【請求項１２】
請求項１から９のいずれか１項に記載の距離推定装置が設けられる前記移動体と、外部装置と、を備え、
前記距離推定装置は、前記演算部が算出したデータを前記外部装置に送信する通信部を備え、
前記外部装置は、前記通信部から受信したデータを表示する表示部を備える
距離推定システム。
【請求項１３】
前記表示部は、前記移動体と前記対象物との相対距離及び相対角度に基づいて前記対象物の形状を描画する
請求項１２に記載の距離推定システム。
【請求項１４】
請求項１から９のいずれか１項に記載の距離推定装置が設けられる前記移動体と、外部装置と、を備え、
前記演算部は、前記対象物の計測点の慣性座標に基づいて前記対象物の仮想平面の傾きを算出し、
前記距離推定装置は、前記演算部が算出したデータを前記外部装置に送信する通信部を備え、
前記外部装置は、前記通信部から受信した前記仮想平面に基づいて前記対象物の形状を描画する表示部を備える
距離推定システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、距離推定装置及び距離推定システムに関する。
【背景技術】
【０００２】
地上、空中、水上及び水中を移動する移動体又は自律移動体において、地表面、水底面及び障害物等の対象物との相対的な距離及び角度に基づいて自己位置を推定する技術が知られている。特許文献１には、４つの地点までの距離から対象物の部分面を推定し、対象物までの距離を取得する方法が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１５−１４１１７３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１の方法は、センサの取り付け位置に大きな制約があるため、移動体の設計によっては適用できない。また、いずれかのセンサが故障する又は周囲環境の都合上計測できない等の問題が発生した場合、対象物までの距離を取得できない。
【０００５】
本発明は、上記に鑑みてなされたものであって、移動体において対象物との相対距離及び相対角度を安定的に推定することのできる距離推定装置及び距離推定システムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明の距離推定装置は、移動体の自己位置及び姿勢を所定周期毎に計測する第１検出部と、対象物に対する距離を前記所定周期毎に計測する第２検出部と、前記第１検出部及び前記第２検出部が計測した計測値を蓄積して、現時点を含む所定の指定時点数分の計測値を含むデータログとして記憶する記憶部と、前記データログに基づいて前記移動体と前記対象物との相対距離及び相対角度を算出する演算部と、を備える。
【０００７】
この構成によれば、過去の時点数のデータも利用して相対距離及び相対角度の算出を行うので、現時点で有効なデータを取得できなかった場合であっても、過去の十分な時点数のデータを利用することによって、相対距離及び相対角度の推定が可能である。これにより、移動体と対象物との相対距離及び相対角度を安定的に推定することができる。また、取得するデータに有効でないデータを含んでもよいので、第２検出部の取付位置に制約がない。これにより、移動体の機体設計によらず、移動体と対象物との相対距離及び相対角度を安定的に推定することができる。また、過去の時点数のデータを利用するので、時系列上の連続性が保証される。これにより、推定した移動体と対象物との相対距離及び相対角度を、機体制御等に適用可能である。
【０００８】
また、前記演算部は、前記対象物の計測点に対する距離に基づいて前記計測点の慣性座標を算出し、前記記憶部に、前記計測点の慣性座標を蓄積させて、現時点を含む所定の指定時点数分の前記計測点の慣性座標を含むデータログとして記憶させる。
【０００９】
この構成によれば、計測点の慣性座標のデータを過去の時点数も含んでデータログとして記憶させるので、現時点で有効なデータを取得できなかった場合であっても過去の十分な時点数のデータを利用することが可能である。
【００１０】
また、前記演算部は、前記計測点の慣性座標に基づいて前記対象物の仮想平面の傾きを算出する。
【００１１】
この構成によれば、計測点の慣性座標のデータを過去の時点数も含んでデータログとして記憶させるので、現時点で有効なデータを取得できなかった場合であっても過去の十分な時点数のデータを利用することによって、移動体と対象物との相対距離及び相対角度の推定が可能である。
【００１２】
また、前記演算部は、前記記憶部に予め設定された所定条件において、前記データログに含まれる前記計測点の慣性座標に対して重み係数を算出し、前記重み係数に基づいて前記計測点の加重平均を算出し、前記加重平均に基づいて前記仮想平面の傾きを算出する。
【００１３】
この構成によれば、過去の時点数も含む計測点の慣性座標のデータログに基づいて加重平均を算出するので、現時点で有効なデータを取得できなかった場合であっても過去の十分な時点数のデータを利用することによって、仮想平面の傾きを算出することが可能である。また、計測点の慣性座標に対してそれぞれ重み係数を算出することによって、例えば、対象物に対する移動体の高度を推定する場合、移動体の鉛直下方向を重視し、移動体の鉛直下から離れた位置の座標を重視しないようにして、推定精度を向上させることができる。
【００１４】
また、前記演算部は、前記所定周期毎に前記仮想平面の傾きを算出し、前記仮想平面の傾きを、前記記憶部に予め設定された所定条件において、１つ前の仮想平面の傾きに基づいて補正する。
【００１５】
この構成によれば、例えば、対象物が海底面等の静的なものに対して距離推定を行う場合、連続する仮想平面が所定角度以上に変動しないように補正することによって、仮想平面を推定する安定性を向上させることができる。
【００１６】
また、前記演算部は、前記移動体と前記仮想平面との相対角度を、前記移動体と前記対象物との相対角度として算出する。
【００１７】
この構成によれば、過去の時点数のデータも利用して相対角度の算出を行うので、現時点で有効なデータを取得できなかった場合であっても、過去の十分な時点数のデータを利用することによって、相対角度の推定が可能である。これにより、移動体と対象物との相対角度を安定的に推定することができる。
【００１８】
また、前記演算部は、前記移動体と前記仮想平面との相対距離を、前記移動体と前記対象物との相対距離として算出する。
【００１９】
この構成によれば、過去の時点数のデータも利用して相対距離の算出を行うので、現時点で有効なデータを取得できなかった場合であっても、過去の十分な時点数のデータを利用することによって、相対距離の推定が可能である。これにより、移動体と対象物との相対距離を安定的に推定することができる。
【００２０】
また、前記演算部は、前記記憶部に予め設定された所定条件において、前記データログに含まれる外れ値を除外する。
【００２１】
この構成によれば、移動体と対象物との相対距離及び相対角度の推定精度をより向上させることができる。
【００２２】
また、前記移動体は、指向性を有する少なくとも１本のビームを発信させる照射部を備え、前記第２検出部は、前記対象物の計測点に対する距離である前記ビームのビーム長を前記対象物に対する距離として計測する。
【００２３】
この構成によれば、対象物に対する距離を容易に測定することができる。
【００２４】
また、前記移動体と前記対象物との相対距離及び相対角度に基づいて前記対象物の形状を描画する表示部を備える。
【００２５】
この構成によれば、オペレータが対象物の形状を容易に把握することができる。
【００２６】
また、前記演算部は、前記対象物の計測点の慣性座標に基づいて前記対象物の仮想平面の傾きを算出し、前記仮想平面に基づいて前記対象物の形状を描画する表示部を備える。
【００２７】
この構成によれば、オペレータが対象物の形状を容易に把握することができる。
【００２８】
本発明の距離推定システムは、前記距離推定装置が設けられる前記移動体と、外部装置と、を備え、前記距離推定装置は、前記演算部が算出したデータを前記外部装置に送信する通信部を備え、前記外部装置は、前記通信部から受信したデータを表示する表示部を備える。
【００２９】
この構成によれば、演算部が算出したデータをオペレータが容易に把握することができる。
【００３０】
また、前記表示部は、前記移動体と前記対象物との相対距離及び相対角度に基づいて前記対象物の形状を描画する。
【００３１】
この構成によれば、オペレータが対象物の形状を容易に把握することができる。
【００３２】
前記距離推定装置が設けられる前記移動体と、外部装置と、を備え、前記演算部は、前記対象物の計測点の慣性座標に基づいて前記対象物の仮想平面の傾きを算出し、前記距離推定装置は、前記演算部が算出したデータを前記外部装置に送信する通信部を備え、前記外部装置は、前記通信部から受信した前記仮想平面に基づいて前記対象物の形状を描画する表示部を備える。
【００３３】
この構成によれば、オペレータが対象物の形状を容易に把握することができる。
【図面の簡単な説明】
【００３４】
【図１】図１は、実施形態に係る距離推定装置の一例を示す模式図である。
【図２】図２は、実施形態に係る距離推定システムの構成例を示すブロック図である。
【図３】図３は、ビーム長の計測値の一例を示すグラフである。
【図４】図４は、距離重みについての減衰関数の例を示すグラフである。
【図５】図５は、連続する部分面の一例を示す模式図である。
【図６】図６は、距離推定装置の動作の一例を示すフローチャートである。
【図７】図７は、距離推定装置の変形例を示すブロック図である。
【発明を実施するための形態】
【００３５】
以下に、本発明に係る距離推定装置及び距離推定システムの実施形態について図面に基づいて詳細に説明する。なお、この実施形態によりこの発明が限定されるものではない。また、下記実施形態における構成要素には、当業者が置換可能かつ容易なもの、実質的に同一のもの、あるいは均等の範囲のものが含まれる。さらに、以下に記載した構成要素は適宜組み合わせることが可能である。以下の実施形態の要旨を逸脱しない範囲で構成要素の種々の省略、置換または変更を行うことができる。各実施形態は、適宜組み合わせることも可能である。以下の実施形態では、本発明に係る距離推定装置及び距離推定システムの実施形態の１つを例示する上で、必要となる構成要素を説明し、その他の構成要素を省略する。なお、以下の実施形態の説明において、同一構成には同一符号を付し、異なる構成には異なる符号を付すものとする。
【００３６】
本発明において、距離推定装置及び距離推定システムは、地上、空中、水上及び水中を移動する移動体において、地表面、水底面及び障害物等の対象物との相対的な距離を推定する。移動体は、例えば、自律型無人潜水機（ＡＵＶ：Ａｕｔｏｎｏｍｏｕｓ  Ｕｎｄｅｒｗａｔｅｒ  Ｖｅｈｉｃｌｅ）、先進安全自動車（Ａｄｖａｎｃｅｄ  Ｓａｆｅｔｙ  Ｖｅｈｉｃｌｅ）、無人航空機（ＵＡＶ：Ｕｎｍａｎｎｅｄ  Ａｅｒｉａｌ  Ｖｅｈｉｃｌｅ）、無人搬送車（ＡＧＶ：Ａｕｔｏｍａｔｅｄ  Ｇｕｉｄｅｄ  Ｖｅｈｉｃｌｅ）等の自律移動体、又は、船舶、潜水艇、航空機、車両等の移動体である。実施形態においては、移動体を自律型無人潜水機、対象物を海底面、距離を鉛直下方向における海底面からの距離とし、一例として、自律型無人潜水機における海底面からの高度推定について説明する。
【００３７】
［装置及びシステムの構成］
図１は、実施形態に係る距離推定装置の一例を示す模式図である。図２は、実施形態に係る距離推定システムの構成例を示す図である。図１及び図２に示すように、距離推定装置１０は、第１検出部１２と、第２検出部１４と、記憶部１６と、演算部１８と、通信部２０と、を備える。距離推定装置１０は、移動体３０に設けられる。距離推定システム１００は、実施形態において、移動体３０と、通信ネットワーク４０と、外部装置５０と、を含む。実施形態において、Ｘ軸方向は、水平面に平行な一方向である。実施形態において、Ｙ軸方向は、水平面に平行で、かつ、Ｘ軸方向と直交する方向である。実施形態において、Ｚ軸方向は、鉛直方向である。
【００３８】
第１検出部１２は、移動体３０の自己位置及び姿勢を検出するセンサを含む。第１検出部１２は、実施形態において、移動体３０の機体３２の重心位置Ｇの慣性座標及び姿勢角を検出する。慣性座標系及び姿勢角は、移動体３０の自律制御ソフトウェアの初期化位置を基準とした座標系及び角度とする。第１検出部１２による計測値は、計測値と記憶部１６に予め設定される移動体３０に関する各種のパラメータとに基づいて機体３２の重心位置Ｇの慣性座標及び姿勢角を算出するために必要なパラメータを少なくとも含む。第１検出部１２は、移動体３０の機体３２に設けられる。第１検出部１２は、例えば、慣性航法装置（ＩＮＳ：Ｉｎｅｒｔｉａｌ  Ｎａｖｉｇａｔｉｏｎ  Ｓｙｓｔｅｍ）である。距離推定装置１０が地上及び空中を移動する移動体において、地表面及び障害物等の対象物との相対的な距離を推定する場合、第１検出部１２は、ＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）であってもよい。
【００３９】
第２検出部１４は、対象物１の所定の計測点に対する距離を計測するセンサを含む。第２検出部１４は、実施形態において、移動体３０の機体３２の下方に向かって設けられる照射部３４から、指向性を有する４本のビームｂ１、ビームｂ２、ビームｂ３及びビームｂ４を発信させる。ビームｂ１が対象物１において反射する位置を照射位置ｂｐ１とした場合、所定の計測点は、照射位置ｂｐ１である。第２検出部１４は、対象物１の照射位置ｂｐ１から反射したビームｂ１を受信することによって、ビームｂ１のビーム長ｂｌ１を計測する。ビームｂ２が対象物１において反射する位置を照射位置ｂｐ２とした場合、所定の計測点は、照射位置ｂｐ２である。第２検出部１４は、対象物１の照射位置ｂｐ２から反射したビームｂ２を受信することによって、ビームｂ２のビーム長ｂｌ２を計測する。ビームｂ３が対象物１において反射する位置を照射位置ｂｐ３とした場合、所定の計測点は、照射位置ｂｐ３である。第２検出部１４は、対象物１の照射位置ｂｐ３から反射したビームｂ３を受信することによって、ビームｂ３のビーム長ｂｌ３を計測する。ビームｂ４が対象物１において反射する位置を照射位置ｂｐ４とした場合、所定の計測点は、照射位置ｂｐ４である。第２検出部１４は、対象物１の照射位置ｂｐ４から反射したビームｂ４を受信することによって、ビームｂ４のビーム長ｂｌ４を計測する。
【００４０】
なお、以下の説明において、ビームｂ１、ビームｂ２、ビームｂ３及びビームｂ４をそれぞれ区別する必要がない場合には、ビームｂと記載される。また、以下の説明において、照射位置ｂｐ１、照射位置ｂｐ２、照射位置ｂｐ３及び照射位置ｂｐ４をそれぞれ区別する必要がない場合には、照射位置ｂｐと記載される。また、以下の説明において、ビーム長ｂｌ１、ビーム長ｂｌ２、ビーム長ｂｌ３及びビーム長ｂｌ４をそれぞれ区別する必要がない場合には、ビーム長ｂｌと記載される。照射されるビームｂの本数は、４本に限定されない。ビームｂは、例えば、超音波である。
【００４１】
図１において、部分面Ｐは、対象物１の仮想平面である。部分面Ｐは、機体３２の位置、姿勢、及びビーム長ｂｌに基づいて後述する方法によって算出される。傾きθは、ＸＹ平面に対する部分面Ｐの傾斜角度である。高度ｈは、機体３２の重心位置Ｇと部分面ＰとのＺ軸方向距離である。
【００４２】
記憶部１６は、各種の制御プログラム及び各種の制御処理に用いられるデータが記憶される記憶装置を含む。記憶装置は、例えば、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）、フラッシュメモリ等の不揮発性又は揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、又はＤＶＤ等である。記憶部１６は、１つの記憶装置又は複数の記憶装置から実装される。記憶部１６は、第１検出部１２及び第２検出部１４が計測した計測値及び計測値の取得時刻を記憶する。記憶部１６は、第１検出部１２及び第２検出部１４による計測値を蓄積して、現時点を含む所定の指定時点数分の計測値を含むデータログとして記憶する。指定時点数は、予め設定された一定値でもよいし、所定条件によって動的に変動させる変動値でもよい。例えば、指定時点数が６０時点の場合、指定時点数は、現時点及び過去５９時点からなる。記憶部１６は、演算部１８による各種の演算結果を記憶する。
【００４３】
演算部１８は、予め定められる制御プログラムを実行する演算処理装置を含む。演算処理装置は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、マイクロプロセッサ、マイクロコンピュータ、ＤＳＰ（Ｄｉｇｉｔａｌ  Ｓｉｇｎａｌ  Ｐｒｏｃｅｓｓｏｒ）、システムＬＳＩ（Ｌａｒｇｅ  Ｓｃａｌｅ  Ｉｎｔｅｇｒａｔｉｏｎ）等である。演算部１８は、１つの演算処理装置又は複数の演算処理装置から実装される。
【００４４】
演算部１８は、記憶部１６に記憶された制御プログラムを実行する。演算部１８は、第１検出部１２に、移動体３０の自己位置及び姿勢を所定周期毎に計測させる。演算部１８は、実施形態において、第１検出部１２に、ビーム長ｂｌを所定周期毎に計測させる。演算部１８は、第２検出部１４に、対象物１に対する距離を所定周期毎に計測させる。所定周期は、所定の時間周期、移動体３０の所定の移動距離周期等である。所定周期は、予め設定された一定値でもよいし、所定条件によって動的に変動させる変動値でもよい。所定周期は、例えば、移動体３０の移動速度に基づいて変動させる時間周期でもよい。演算部１８は、第１検出部１２及び第２検出部１４が計測した計測値及び計測値の取得時刻を記憶部１６に記憶させる。演算部１８は、記憶部１６に記憶されたデータログに基づいて移動体３０と対象物１との相対距離及び相対角度を算出する。演算部１８は、実施形態において、移動体３０の機体３２の重心位置Ｇと対象物１の部分面ＰとのＺ軸方向の距離を高度ｈとして算出する。演算部１８は、人工知能（ＡＩ：Ａｒｔｉｆｉｃｉａｌ  Ｉｎｔｅｌｌｉｇｅｎｃｅ）を有していてもよい。蓄積したデータを人工知能に学習させることで、高度ｈの推定精度を向上させることができる。演算部１８は、移動体３０の駆動を制御する演算処理装置と共通のものでも別個のものでもよい。
【００４５】
通信部２０は、通信ネットワーク４０を介して、外部装置５０との間で各種データをやり取りするためのデータ通信を実行する。外部装置５０との間の通信を可能とする各種通信方式をサポートできる。通信部２０の通信方式は、無線通信及び有線通信のいずれの通信方式を適用してもよい。通信部２０は、演算部１８が算出したデータを外部装置５０に送信する。
【００４６】
通信ネットワーク４０は、距離推定装置１０と外部装置５０とを、相互にデータ通信可能に接続する。通信ネットワーク４０は、公衆通信回線及び専用通信回線等を含んで構築されてよい。
【００４７】
外部装置５０は、通信ネットワーク４０を介して距離推定装置１０と接続する。外部装置５０は、例えば、パーソナルコンピュータ又はスマートデバイス等である。外部装置５０は、例えば、オペレータの操作によって、演算部１８の処理に使用するために記憶部１６に予め記憶されるデータ及び設定等を距離推定装置１０へ送信する。外部装置５０は、距離推定装置１０から受信するデータをオペレータに提示する。外部装置５０は、入力部５２と、表示部５４と、記憶部５６と、演算部５８と、通信部６０と、を備える。
【００４８】
入力部５２は、例えば、キーボード又はタッチパネル等である。入力部５２は、演算部５８に接続される。オペレータは、入力部５２を介して、演算部５８に各種のコマンドを入力することができる。
【００４９】
表示部５４は、通信ネットワーク４０を介して受信した各種データを表示する。表示部５４は、例えば、対象物１との相対距離及び相対角度の時系列のデータに基づいて対象物１の形状を描画する。表示部５４は、例えば、部分面Ｐの時系列のデータに基づいて対象物１の形状を描画する。表示部５４は、例えば、液晶ディスプレイ（ＬＣＤ：Ｌｉｑｕｉｄ  Ｃｒｙｓｔａｌ  Ｄｉｓｐｌａｙ）、有機ＥＬディスプレイ（ＯＥＬＤ：Ｏｒｇａｎｉｃ  Ｅｌｅｃｔｒｏ−Ｌｕｍｉｎｅｓｃｅｎｃｅ  Ｄｉｓｐｌａｙ）、又は無機ＥＬディスプレイ（ＩＥＬＤ：Ｉｎｏｒｇａｎｉｃ  Ｅｌｅｃｔｒｏ−Ｌｕｍｉｎｅｓｃｅｎｃｅ  Ｄｉｓｐｌａｙ）等を含む。表示部５４は、タッチパネル等の入力部５２を含んでもよい。
【００５０】
記憶部５６は、各種の制御プログラム及び各種の制御処理に用いられるデータが記憶される記憶装置を含む。記憶装置は、例えば、ＲＯＭ（Ｒｅａｄ  Ｏｎｌｙ  Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ  Ａｃｃｅｓｓ  Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ  Ｄｉｓｋ  Ｄｒｉｖｅ）、フラッシュメモリ等の不揮発性又は揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、又はＤＶＤ等である。記憶部５６は、１つの記憶装置又は複数の記憶装置から実装される。記憶部５６は、通信ネットワーク４０を介して後述の通信部６０が受信した各種のデータを記憶する。演算部５８による各種の演算結果を記憶する。
【００５１】
演算部５８は、予め定められる制御プログラムを実行する演算処理装置を含む。演算処理装置は、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ  Ｐｒｏｃｅｓｓｉｎｇ  Ｕｎｉｔ）、マイクロプロセッサ、マイクロコンピュータ、ＤＳＰ（Ｄｉｇｉｔａｌ  Ｓｉｇｎａｌ  Ｐｒｏｃｅｓｓｏｒ）、システムＬＳＩ（Ｌａｒｇｅ  Ｓｃａｌｅ  Ｉｎｔｅｇｒａｔｉｏｎ）等である。演算部１８は、１つの演算処理装置又は複数の演算処理装置から実装される。演算部５８は、入力部５２に入力されたコマンドに基づいて各種の演算処理を実行する。演算部５８は、記憶部５６に記憶された制御プログラムを実行する。演算部５８は、通信ネットワーク４０を介して後述の通信部６０が受信した各種のデータを表示部５４に表示させる。
【００５２】
通信部６０は、通信ネットワーク４０を介して、距離推定装置１０との間で各種データをやり取りするためのデータ通信を実行する。距離推定装置１０との間の通信を可能とする各種通信方式をサポートできる。通信部６０の通信方式は、無線通信及び有線通信のいずれの通信方式を適用してもよい。通信部６０は、入力部５２が入力したコマンドに基づいて各種のデータ及び設定等を距離推定装置１０に送信する。
【００５３】
［ビーム座標の算出］
まず、演算部１８は、所定時間毎に第１検出部１２及び第２検出部１４による計測値を取得する。演算部１８は、第１検出部１２及び第２検出部１４から取得した計測値のデータを取得時刻とともに記憶部１６に記憶させる。演算部１８は、記憶部１６におけるビーム長ｂｌのデータログに最新の計測値のデータを追加して、データログを更新させる。データログは、現時点を含む指定時点数分の計測データからなる。ビーム長ｂｌのデータログは、ビームｂの本数をＭ、指定時点数をＮとした場合、Ｍ行Ｎ列の行列によって表される。実施形態において、ビームｂの本数は、４本である。演算部１８は、第１検出部１２による計測値に基づいて機体３２の重心位置Ｇの慣性座標及び姿勢角を算出する。演算部１８は、算出した機体３２の重心位置Ｇの慣性座標及び姿勢角のデータを計測値の取得時刻とともに記憶部１６に記憶させる。演算部１８は、記憶部１６における機体３２の重心位置Ｇの慣性座標及び姿勢角のデータログに最新の計測値のデータを追加して、データログを更新させる。
【００５４】
次に、演算部１８は、記憶部１６からビーム長ｂｌのデータログ、機体３２の重心位置Ｇの慣性座標及び姿勢角のデータログを記憶部１６から取得する。演算部１８は、記憶部１６において更新されたビーム長ｂｌ、機体３２の重心位置Ｇの慣性座標及び姿勢角のデータログに基づいてビームｂの照射位置ｂｐの慣性座標を算出する。ビームｂの照射位置ｂｐの慣性座標をＢｉ、機体３２の重心位置Ｇの慣性座標をＸ＝［Ｘ，Ｙ，Ｚ］Ｔ、姿勢角をΘ、照射部３４の取付位置をｂ０、取付角をｐとした場合、演算部１８は、下記式（１）及び式（２）からＢｉを得る。
【００５５】
【数１】
2020148513.tif 000003
【００５６】
【数２】
2020148513.tif 000004
【００５７】
［無効値及び外れ値の除外］
第２検出部１４によるビーム長ｂｌの計測値は、計測手法の制約又は計測環境による計測ノイズの増大、センサの故障による欠測等によって、無効な計測値及び外れ値を含むことがある。図３は、ビーム長の計測値の一例を示すグラフである。図３に示すように、ビーム長ｂｌの計測値は、有効値ｂｌｖと、無効値ｂｌｏ１と、外れ値ｂｌｏ２と、を含む。ここで、無効値ｂｌｏ１及び外れ値ｂｌｏ２は、真の値の推定値からの残差が異常に大きい計測値を示すものとする。また、無効値ｂｌｏ１は、センサの故障等の原因が特定できる異常値を示すものとする。図１に示す部分面Ｐの算出において、演算部１８は、指定時点数分を含む範囲Ｄにおいて、有効値ｂｌｖのデータのみを使用するため、ビーム長ｂｌの計測値から、無効値ｂｌｏ１及び外れ値ｂｌｏ２を除外する。まず、演算部１８は、下記式（３）に例示するｓａｎｉｔｙ配列を作成する。
【００５８】
【数３】
2020148513.tif 000005
【００５９】
ｓａｎｉｔｙ配列（ｓａｎｉｔｙ  ｍａｔｒｉｘ）は、ビーム長ｂｌのデータログと同様のサイズであるＭ行Ｎ列の行列である。ｓａｎｉｔｙ配列は、「０」又は「１」の成分からなるｂｏｏｌ行列である。ｓａｎｉｔｙ配列における各成分の初期値は、「１」である。演算部１８は、ビーム長ｂｌのデータログにおいて無効値である（ｉ，ｊ）成分に対応するｓａｎｉｔｙ配列の（ｉ，ｊ）成分の値を「０」に置換する。すなわち、ｓａｎｉｔｙ配列における（ｉ，ｊ）成分の値は、ビーム長ｂｌのデータログにおける（ｉ，ｊ）成分の値が無効値である場合、「０」で示される。
【００６０】
次に、演算部１８は、新たに取得したビーム長ｂｌの計測値が外れ値か否かを判定する。外れ値を判定する方法として、各種の棄却検定法があるが、実施形態においては、増山の棄却検定を適用する。新たに取得したビーム長ｂｌの計測値をｘ、データログにおけるビーム長ｂｌの標準平均をｘａｖｇ、標準偏差をσ、自由度をｎ、有意水準をα、自由度がｎ−２かつ有意水準がαである標準平均値のｔ分布をｔ（ｎ−２，α）とした場合、演算部１８は、下記式（４）から外れ値を判定する。なお、ビーム長ｂｌの平均値、標準偏差及び自由度は、無効値でないビーム長ｂｌについて算出するものとする。
【００６１】
【数４】
2020148513.tif 000006
【００６２】
演算部１８は、ビーム長ｂｌのデータログにおいて外れ値である（ｉ，ｊ）成分に対応するｓａｎｉｔｙ配列の（ｉ，ｊ）成分の値を「０」に置換する。ビーム長ｂｌのデータログにおいて（ｉ，ｊ）成分が無効値でも外れ値でもない場合、ｓａｎｉｔｙ配列における（ｉ，ｊ）成分の値は、有効値として「１」で示される。ビーム長ｂｌのデータログにおいて（ｉ，ｊ）成分が少なくとも無効値及び外れ値のいずれかである場合、ｓａｎｉｔｙ配列における（ｉ，ｊ）成分の値は、無効値として「０」で示される。以降の説明において、ｓａｎｉｔｙ配列において値が「１」である（ｉ，ｊ）成分に対応するビームｂを、有効ビームと称するものとする。
【００６３】
［部分面の推定］
演算部１８は、有効値であるビーム長ｂｌに対応する有効ビームの照射位置ｂｐの慣性座標に基づいて部分面Ｐの傾きを算出する。まず、演算部１８は、有効ビームについて、照射位置ｂｐの慣性座標及び計測値の取得時刻を取得する。演算部１８は、機体３２の重心位置Ｇとビームｂの照射位置ｂｐとのＸＹ距離、及び現在時刻からの経過時間を算出する。演算部１８は、減衰関数に基づいて距離重み及び時間重みを算出する。図４は、距離重みについての減衰関数の例を示すグラフである。減衰関数は、予め記憶部１６に記憶される。演算部１８は、距離重み及び時間及びを要素積することによって、重み係数を算出する。演算部１８は、重み行列をＷ、重み係数をＷｉとした場合、対角化することによって、下記式（５）に示す重み行列を算出する。
【００６４】
【数５】
2020148513.tif 000007
【００６５】
次に、有効ビームの照射位置ｂｐの加重平均をＢａｖｇとした場合、演算部１８は、式（１）から式（５）に基づいて式（６）から有効ビームの照射位置ｂｐの加重平均を算出する。
【００６６】
【数６】
2020148513.tif 000008
【００６７】
部分面Ｐの式は、変数ｘＴ＝［ｘ，ｙ，ｚ］Ｔに対する係数をａＴ＝［ａ，ｂ，ｃ］Ｔ、切片をｄとした場合、下記式（７）によって定義される。
【００６８】
【数７】
2020148513.tif 000009
【００６９】
ａＴａ＝１の条件下において、評価関数をＪとした場合、Ｊは、下記式（８）及び式（９）によって示される。
【００７０】
【数８】
2020148513.tif 000010
【００７１】
【数９】
2020148513.tif 000011
【００７２】
部分面Ｐの傾きは、ａＴａ＝１の条件下で式（９）を最小化するａである。ラグランジュの未定乗数法により、下記式（１０）及び式（１１）に示す関係が成立する。
【００７３】
【数１０】
2020148513.tif 000012
【００７４】
【数１１】
2020148513.tif 000013
【００７５】
したがって、部分面Ｐの傾きであるａは、ＢＷＢＴの固有ベクトルである。演算部１８は、ＢＷＢＴの固有ベクトルを算出し、部分面Ｐの傾きとして記憶部１６に記憶させる。
【００７６】
上述した方法において、演算部１８が有効ビームの照射位置ｂｐの加重平均を算出する際に、有効ビームが存在しない場合、演算部１８は、ダミーの加重平均を出力する。また、演算部１８が部分面Ｐの傾きを算出する際に、有効ビームが３つ未満である場合、演算部１８は、ダミーの部分面の傾きを出力する。
【００７７】
［部分面の角度補正］
図５は、連続する部分面の一例を示す模式図である。演算部１８は、新たに算出した現部分面Ｐｎｏｗの傾きと１つ前に算出した前部分面Ｐｏｌｄの傾きとを比較し、角度差θｄが所定値以上である場合、現部分面Ｐｎｏｗの傾きの係数を補正する。所定値は、例えば、３°である。補正後の現部分面の傾きをａｎｅｗ、現部分面Ｐｎｏｗの傾きをａｎｏｗ、前部分面Ｐｏｌｄの傾きをａｏｌｄ、所定値をθｌｉｍｉｔとした場合、演算部１８は、下記式（１２）の条件下において、式（１３）及び式（１４）から、ａｎｅｗを得る。
【００７８】
【数１２】
2020148513.tif 000014
【００７９】
【数１３】
2020148513.tif 000015
【００８０】
【数１４】
2020148513.tif 000016
【００８１】
［距離の算出］
演算部１８は、機体３２の座標であるＸ＝［Ｘ，Ｙ，Ｚ］Ｔ及び部分面Ｐの傾きであるａＴ＝［ａ，ｂ，ｃ］に基づいて機体３２の高度ｈを算出する。高度ｈは、機体３２の重心位置Ｇと部分面ＰとのＺ軸方向距離である。演算部１８は、下記式（１５）及び式（１６）から高度ｈを得る。
【００８２】
【数１５】
2020148513.tif 000017
【００８３】
【数１６】
2020148513.tif 000018
【００８４】
［装置の動作］
図６は、距離推定装置の動作の一例を示すフローチャートである。距離推定装置１０は、距離推定システム１００において、例えば、オペレータが外部装置５０の入力部５２に所定の開始コマンドを入力することによって、図６に示すステップＳ１０に移行して処理を開始する。距離推定装置１０は、図６に示すフローチャートの処理を終了すると、例えば、オペレータが外部装置５０の入力部５２に所定の修了コマンドを入力まで、所定周期毎にステップＳ１０から処理を開始する。
【００８５】
まず、ステップＳ１０において、演算部１８は、第２検出部１４からビーム長ｂｌの計測値を取得する。実施形態においてビームｂの本数は４本であるので、演算部１８は、ビーム長ｂｌの計測値のデータを４個取得する。演算部１８は、取得したビーム長ｂｌの計測値のデータを記憶部１６に記憶させる。
【００８６】
ステップＳ１２において、演算部１８は、記憶部１６におけるにおけるビーム長ｂｌのデータログを更新させる。指定時点数が予め定められた一定値である場合、演算部１８は、データログにおける最も過去の時点の計測値のデータを除外し、最新の計測値のデータを追加する。
【００８７】
ステップＳ１４において、演算部１８は、第１検出部１２から移動体３０の機体３２における重心位置Ｇの慣性座標及び姿勢角の計測値を取得する。演算部１８は、ステップＳ１０より前にステップＳ１４を実行してもよい。演算部１８は、ステップＳ１０と同時にステップＳ１４を実行してもよい。
【００８８】
ステップＳ１６において、演算部１８は、ステップＳ１２において更新したビーム長ｂｌのデータログ、ステップＳ１４において取得した機体３２の重心位置Ｇの慣性座標及び姿勢角、記憶部１６に予め記憶された照射部３４の取付位置及び取付角のデータに基づいてビームｂの照射位置ｂｐの慣性座標を算出する。
【００８９】
ステップＳ２０において、演算部１８は、ビーム長ｂｌのデータログの行列と同様のサイズかつ各成分の初期値が「１」であるｓａｎｉｔｙ配列を作成する。ステップＳ２２において、演算部１８は、ビーム長ｂｌのデータログにおいて無効値である（ｉ，ｊ）成分に対応するｓａｎｉｔｙ配列の（ｉ，ｊ）成分の値を「０」に置換する。これにより、ｓａｎｉｔｙ配列における（ｉ，ｊ）成分の値は、ビーム長ｂｌのデータログにおける（ｉ，ｊ）成分の値が無効値である場合、「０」で示される。演算部１８は、更新されたデータログを記憶部１６に記憶させる。なお、ステップＳ２０及びステップＳ２２は、ステップＳ１２より後かつ後述のステップＳ３２より前であれば、いつ実行されてもよい。
【００９０】
ステップＳ３０において、演算部１８は、ステップＳ１２において取得したビーム長ｂｌの計測しに対し、外れ値の除外処理を実行するか否かを判断する。外れ値の除外処理を実行するか否かは、記憶部１６予めに設定されてもよいし、入力部５２から適宜変更可能であってもよいし、記憶部１６に予め設定された所定条件に基づいて判断されてもよい。ステップＳ３０において、演算部１８が外れ値の除外処理を実行すると判断した場合、演算部１８の処理は、ステップＳ３２に移行する。ステップＳ３０において、演算部１８が外れ値の除外処理を実行しないと判断した場合、演算部１８の処理は、ステップＳ４０に移行する。
【００９１】
ステップＳ３２において、演算部１８は、ステップＳ１２において取得したビーム長ｂｌの標準平均及び標準偏差を算出する。演算部１８は、算出した標準平均及び標準偏差、記憶部１６に予め設定された自由度、有意水準及び標準平均値のｔ分布に基づいてステップＳ１２において取得したビーム長ｂｌの計測値が外れ値か否かを判断する。ステップＳ３２において、演算部１８がビーム長ｂｌの計測値が外れ値であると判断した場合、演算部１８の処理は、ステップＳ３４に移行する。ステップＳ３２において、演算部１８がビーム長ｂｌの計測値が外れ値ではないと判断した場合、演算部１８の処理は、ステップＳ４０に移行する。
【００９２】
ステップＳ３４において、演算部１８は、ビーム長ｂｌのデータログにおいて外れ値である（ｉ，ｊ）成分に対応するｓａｎｉｔｙ配列の（ｉ，ｊ）成分の値を「０」に置換する。これにより、ｓａｎｉｔｙ配列における（ｉ，ｊ）成分の値は、ビーム長ｂｌのデータログにおける（ｉ，ｊ）成分の値が無効値又は外れ値である場合、「０」で示される。演算部１８は、更新されたデータログを記憶部１６に記憶させる。なお、ステップＳ３２及びステップＳ３４は、各ビームｂのビーム長ｂｌの計測値毎に実行する。
【００９３】
次に、ステップＳ４０において、演算部１８は、有効ビームについて、照射位置ｂｐの慣性座標及び計測値の取得時刻を取得する。ステップＳ４２において、演算部１８は、ステップＳ１４において取得した移動体３０における機体３２の重心位置Ｇと、ステップＳ１６において算出したビームｂの照射位置ｂｐと、のＸＹ距離を算出する。演算部１８は、記憶部１６に記憶されたビーム長ｂｌの取得時刻及び現在時刻からの経過時間を算出する。ステップＳ４４において、演算部１８は、ステップＳ４２において算出した機体３２の重心位置Ｇとビームｂの照射位置ｂｐとのＸＹ距離、現在時刻からの経過時間、及び記憶部１６に予め記憶される減衰関数に基づいて距離重み及び時間重みを算出する。ステップＳ４６において、演算部１８は、ステップＳ４４において算出した距離重み及び時間重みを要素積することによって、重み係数を算出する。演算部１８は、算出した重み係数を記憶部１６に記憶させる。演算部１８は、記憶部１６に記憶された指定時点数分の重み係数を対角化することによって、重み行列を算出する。演算部１８は、算出した重み行列を記憶部１６に記憶させる。
【００９４】
ステップＳ５０において、演算部１８は、有効ビームが存在するか否かを判断する。ステップＳ５０において、演算部１８が有効ビームは存在すると判断した場合、演算部１８の処理は、ステップＳ５２に移行する。ステップＳ５２において、演算部１８は、記憶部１６に記憶された有効ビームの照射位置ｂｐの慣性座標、及びステップＳ４６において算出した重み行列に基づいて有効ビームの照射位置ｂｐの加重平均を算出する。演算部１８は、算出した加重平均を記憶部１６に記憶させる。ステップＳ５０において、演算部１８が有効ビームは存在しないと判断した場合、演算部１８の処理は、ステップＳ５４に移行する。ステップＳ５４において、演算部１８は、ダミーの加重平均を出力する。ダミーの加重平均は、記憶部１６に予め設定された所定条件に基づいて算出された値である。演算部１８は、出力したダミーの加重平均を記憶部１６に記憶させる。
【００９５】
次に、ステップＳ６０において、演算部１８は、有効ビームが３本以上存在するか否かを判断する。ステップＳ６０において、演算部１８が有効ビームは３本以上存在すると判断した場合、演算部１８の処理は、ステップＳ６２に移行する。ステップＳ６２において、演算部１８は、記憶部１６に記憶された有効ビームの照射位置ｂｐの慣性座標、ステップＳ５２又はＳ５４において算出した加重平均、及び記憶部１６に予め記憶された評価関数に基づいて部分面Ｐの傾きの変数を算出する。演算部１８は、算出した部分面Ｐの傾きの変数を記憶部１６に記憶させる。ステップＳ６０において、演算部１８が有効ビームは３本以上存在しないと判断した場合、演算部１８の処理は、ステップＳ６４に移行する。ステップＳ６４において、演算部１８は、ダミーの部分面Ｐの傾きの変数を出力する。ダミーの部分面Ｐの傾きの変数は、記憶部１６に予め設定された所定条件に基づいて算出された値である。演算部１８は、出力したダミーの部分面Ｐの傾きの変数を記憶部１６に記憶させる。
【００９６】
次に、ステップＳ７０において、演算部１８は、新たに算出した現部分面Ｐｎｏｗの傾きと１つ前に算出した前部分面Ｐｏｌｄの傾きとを比較し、角度差θｄが所定値以上であるか否かを判断する。ステップＳ７０において、演算部１８が角度差θｄは所定値以上であると判断した場合、演算部１８の処理は、ステップＳ７２に移行する。ステップＳ７２において、演算部１８は、ステップＳ６２又はステップＳ６４において算出した現部分面Ｐｎｏｗの傾き、記憶部１６に記憶された前部分面Ｐｏｌｄの傾き、記憶部１６に予め設定された所定値、及び記憶部１６に予め記憶された補正式に基づいて補正された現部分面の傾きの変数を算出する。演算部１８は、算出した補正後の現部分面の傾きの変数を記憶部１６に記憶させる。ステップＳ７０において、演算部１８が角度差θｄは所定値以上ではないと判断した場合、演算部１８の処理は、ステップＳ８０に移行する。
【００９７】
次に、ステップＳ８０において、演算部１８は、ステップＳ６２、ステップＳ６４又はステップＳ７２において算出した部分面Ｐの傾きを記憶部１６から取得する。演算部１８は、ステップＳ５２又はＳ５４において算出した加重平均を取得する。演算部１８は、ステップＳ１４において取得した移動体３０における機体３２の重心位置Ｇの慣性座標を記憶部１６から取得する。演算部１８は、部分面Ｐの傾き、加重平均、及び機体３２の重心位置Ｇの慣性座標に基づいて移動体３０における機体３２の重心位置Ｇと部分面ＰとのＺ軸方向距離を高度ｈとして算出する。演算部１８は、ステップＳ８０が終了すると、上述したようにステップＳ１０の処理を再開する。
【００９８】
上述したステップＳ１０からステップＳ８０において、各処理の実行結果及び演算結果は、通信部２０によって、通信ネットワーク４０を介して外部装置５０の通信部６０に送信されてもよい。外部装置５０の演算部５８は、通信部６０が受信した各処理の実行結果及び演算結果を、記憶部５６に予め設定された所定のプログラムに基づいて表示部５４に表示させてもよい。演算部５８は、例えば、時系列の機体３２の重心位置Ｇの慣性座標及び高度ｈに基づいて対象物１の形状を三次元画像として表示部５４に表示させてもよい。演算部５８は、例えば、時系列の部分面Ｐのデータを三次元画像として表示部５４に表示させてもよい。
【００９９】
演算部１８は、ステップＳ３０、ステップＳ３２及びステップＳ３４を省略してもよい。すなわち、演算部１８は、ビーム長ｂｌの計測値から外れ値を除外する処理を省略してもよい。演算部１８は、さらに、ステップＳ２０及びステップＳ２２を省略してもよい。すなわち、演算部１８は、ビーム長ｂｌの計測値から無効値を除外する処理を省略してもよい。演算部１８は、ステップＳ４０、ステップＳ４２、ステップＳ４４及びステップＳ４６を省略してもよい。すなわち、演算部１８は、照射位置ｂｐの慣性座標に重み付けをする処理を省略してもよい。演算部１８は、ステップＳ７０及びステップＳ７２を省略してもよい。すなわち、演算部１８は、部分面Ｐの傾きを補正する処理を省略してもよい。
【０１００】
演算部１８は、各種の時系列のデータに関し、記憶部１６に予め記憶された所定の時間方向のフィルタを適用してもよい。演算部１８は、例えば、第１検出部１２が検出した計測値又は計測値に基づいて算出した機体３２における重心位置Ｇの慣性座標及び姿勢角に関し、記憶部１６に予め記憶された所定の時間方向のフィルタを適用してもよい。演算部１８は、例えば、算出した部分面Ｐの傾き及び高度ｈに関し、記憶部１６に予め記憶された所定の時間方向のフィルタを適用してもよい。
【０１０１】
以上説明したように、実施形態の距離推定装置１０は、第１検出部１２と、第２検出部１４と、記憶部１６と、演算部１８と、を備える。第１検出部１２は、移動体３０の自己位置及び姿勢（機体３２の重心位置Ｇの慣性座標及び姿勢角）を所定周期毎に計測する。第２検出部１４は、対象物１に対する距離（ビーム長ｂｌ）を所定周期毎に計測する。記憶部１６は、第１検出部１２及び第２検出部１４が計測した計測値を蓄積して、現時点を含む所定の指定時点数分の計測値を含むデータログとして記憶する。演算部１８は、データログに基づいて移動体３０と対象物１との相対距離及び相対角度を算出する。
【０１０２】
これにより、過去の時点数のデータも利用して相対距離及び相対角度の算出を行うので、現時点で有効なデータを取得できなかった場合であっても、過去の十分な時点数のデータを利用することによって、相対距離及び相対角度の推定が可能である。したがって、移動体３０と対象物１との相対距離及び相対角度を安定的に推定することができる。また、取得するデータに有効でないデータを含んでもよいので、第２検出部１４の取付位置に制約がない。これにより、移動体３０の機体設計によらず、移動体３０と対象物１との相対距離及び相対角度を安定的に推定することができる。また、過去の時点数のデータを利用するので、時系列上の連続性が保証される。これにより、推定した移動体３０と対象物１との相対距離及び相対角度を、機体制御等に適用可能である。
【０１０３】
また、演算部１８は、対象物１の計測点（ビームｂの照射位置ｂｐ）に対する距離（ビーム長ｂｌ）に基づいて計測点の慣性座標を算出する。演算部１８は、記憶部１６に、計測点の慣性座標を蓄積させて、現時点を含む所定の指定時点数分の計測点の慣性座標を含むデータログとして記憶させる。これにより、計測点の慣性座標のデータを過去の時点数も含んでデータログとして記憶させるので、現時点で有効なデータを取得できなかった場合であっても過去の十分な時点数のデータを利用することが可能である。
【０１０４】
また、演算部１８は、計測点（ビームｂの照射位置ｂｐ）の慣性座標に基づいて対象物１の仮想平面（部分面Ｐ）の傾きを算出する。これにより、計測点の慣性座標のデータを過去の時点数も含んでデータログとして記憶させるので、現時点で有効なデータを取得できなかった場合であっても過去の十分な時点数のデータを利用することによって、移動体３０と対象物１との相対距離及び相対角度の推定が可能である。
【０１０５】
また、演算部１８は、記憶部１６に予め設定された所定条件において、データログに含まれる計測点（ビームｂの照射位置ｂｐ）の慣性座標に対して重み係数を算出する。演算部１８は、重み係数に基づいて計測点の加重平均を算出する。演算部１８は、加重平均に基づいて仮想平面（部分面Ｐ）の傾きを算出する。これにより、過去の時点数も含む計測点の慣性座標のデータログに基づいて加重平均を算出するので、現時点で有効なデータを取得できなかった場合であっても過去の十分な時点数のデータを利用することによって、仮想平面の傾きを算出することが可能である。また、計測点の慣性座標に対してそれぞれ重み係数を算出することによって、対象物１に対する移動体３０の高度ｈを推定する場合、移動体３０の鉛直下方向（Ｚ軸方向）を重視し、移動体３０の鉛直下から離れた位置の座標を重視しないようにして、推定精度を向上させることができる。
【０１０６】
また、演算部１８は、所定周期毎に仮想平面（部分面Ｐ）の傾きを算出する。演算部１８は、仮想平面（現部分面Ｐｎｏｗ）の傾きを、記憶部１６に予め設定された所定条件において、１つ前の仮想平面（前部分面Ｐｏｌｄ）の傾きに基づいて補正する。これにより、対象物１が海底面等の静的なものに対して距離推定を行う場合、連続する仮想平面が所定角度以上に変動しないように補正することによって、仮想平面を推定する安定性を向上させることができる。
【０１０７】
また、演算部１８は、移動体３０と仮想平面（部分面Ｐ）との相対角度を、移動体３０と対象物１との相対角度として算出する。これにより、過去の時点数のデータも利用して相対角度の算出を行うので、現時点で有効なデータを取得できなかった場合であっても、過去の十分な時点数のデータを利用することによって、相対角度の推定が可能である。したがって、移動体３０と対象物１との相対角度を安定的に推定することができる。
【０１０８】
また、演算部１８は、移動体３０と仮想平面（部分面Ｐ）との相対距離を、移動体３０と対象物１との相対距離として算出する。これにより、過去の時点数のデータも利用して相対距離の算出を行うので、現時点で有効なデータを取得できなかった場合であっても、過去の十分な時点数のデータを利用することによって、相対距離の推定が可能である。したがって、移動体３０と対象物１との相対距離を安定的に推定することができる。
【０１０９】
また、演算部１８は、記憶部１６に予め設定された所定条件において、データログに含まれる外れ値を除外する。これにより、移動体３０と対象物１との相対距離及び相対角度の推定精度をより向上させることができる。
【０１１０】
また、移動体３０は、指向性を有する少なくとも１本のビームｂを発信させる照射部３４を備える。第２検出部１４は、対象物１の計測点（ビームｂの照射位置ｂｐ）に対する距離であるビームｂのビーム長ｂｌを対象物１に対する距離として計測する。これにより、対象物１に対する距離を容易に測定することができる。
【０１１１】
また、実施形態の距離推定システム１００は、距離推定装置１０が設けられる移動体３０と、外部装置５０と、を備える。距離推定装置１０は、演算部１８が算出したデータを外部装置５０に送信する通信部２０を備える。外部装置５０は、通信部２０から受信したデータを表示する表示部５４を備える。これにより、演算部１８が算出したデータをオペレータが容易に把握することができる。
【０１１２】
また、表示部５４は、移動体３０と対象物１との相対距離及び相対角度に基づいて対象物１の形状を描画する。これにより、オペレータが対象物１の形状を容易に把握することができる。
【０１１３】
距離推定システム１００は、距離推定装置１０が設けられる移動体３０と、外部装置５０と、を備える。演算部１８は、対象物１の計測点（ビームｂの照射位置ｂｐ）の慣性座標に基づいて対象物１の仮想平面（部分面Ｐ）の傾きを算出する。距離推定装置１０は、演算部１８が算出したデータを外部装置５０に送信する通信部２０を備える。外部装置５０は、通信部２０から受信した仮想平面に基づいて対象物１の形状を描画する表示部５４を備える。これにより、オペレータが対象物１の形状を容易に把握することができる。
【０１１４】
以上、本発明の実施形態を説明したが、この実施形態の内容によって実施形態が限定されるものではない。例えば、部分面Ｐは、平面に限定されない。部分面Ｐは、数式で記述可能な面であれば、取得可能な三次元座標数に対して面のパラメータの自由度が小さい場合、二次曲面又は球面等の曲面でも構わない。
【０１１５】
また、実施形態において、第２検出部１４は、照射部３４から４本のビームｂを発信させることによって、１時点につき４つの計測点を計測したが、１時点当たりの計測点の数は４つに限定されない。第２検出部１４は、例えば、５つ以上の計測点を計測してもよい。計測点の数を増加させることにより、推定制度をより向上させることができる。第２検出部１４は、例えば、１時点につき３つの計測点を計測してもよい。計測点が３つの場合、３つの計測点を通る平面を部分面としてもよい。第２検出部１４は、例えば１時点につき１つ又は２つの計測点を計測してもよい。計測点が３つ未満の場合、演算部１８は、例えば、連続する過去の計測点及び現在の計測点の慣性座標に基づいて部分面を算出してもよい。
【０１１６】
また、第２検出部１４は、照射部３４を移動体３０の鉛直下方向から進行方向に傾斜した方向に向けて設けてもよい。これにより、移動体３０の進行方向の高度ｈも同時に推定することが可能である。したがって、一定時間後の高度ｈを予測することができるので、高度に関する予測制御を実現できる。
【０１１７】
また、移動体３０と対象物１との過去の相対距離及び相対角度に応じて、現在の相対距離及び相対角度を推定するための指定時点数を変動させてもよい。これにより、第２検出部１４による計測点（ビームｂの照射位置ｂｐ）の密度を調整することができるので、移動体３０と対象物１との相対距離が大きい場合であっても、対象物１の変動に対して低感度となることを抑制することが可能である。
【０１１８】
また、距離推定装置１０が船舶、潜水艇、航空機、車両等のオペレータが搭乗可能な有人機である移動体に搭載される場合、外部装置５０は、移動体に設けられてもよい。また、距離推定装置１０の演算部１８が行う演算処理の一部は、外部装置５０の演算部５８が行ってもよい。
【０１１９】
［変形例］
図７は、変形例の距離推定装置を示すブロック図である。変形例の距離推定装置１０Ａは、船舶、潜水艇、航空機、車両等のオペレータが搭乗可能な有人機である移動体に搭載されるものとする。変形例の距離推定装置１０Ａにおいて、上述の距離推定装置１０及び距離推定システム１００と同一の構成については同一の参照符号を付して説明を省略し、異なる構成について説明する。図７に示すように、距離推定装置１０Ａは、第１検出部１２と、第２検出部１４と、記憶部１６Ａと、演算部１８Ａと、入力部５２Ａと、表示部５４Ａと、を備える。
【０１２０】
記憶部１６Ａは、実施形態における記憶部１６の機能と、外部装置５０の記憶部５６の機能とを有する。すなわち、記憶部１６Ａは、第１検出部１２及び第２検出部１４が計測した計測値及び計測値の取得時刻を記憶する。記憶部１６Ａは、第１検出部１２及び第２検出部１４による計測値を蓄積して、現時点を含む所定の指定時点数分の計測値を含むデータログとして記憶する。記憶部１６Ａは、演算部１８Ａによる各種の演算結果を記憶する。
【０１２１】
演算部１８Ａは、実施形態における演算部１８の機能と、外部装置５０の演算部５８の機能とを有する。すなわち、演算部１８Ａは、第１検出部１２に、移動体３０の自己位置及び姿勢を所定周期毎に計測させる。演算部１８Ａは、第２検出部１４に、対象物１に対する距離を所定周期毎に計測させる。演算部１８Ａは、第１検出部１２及び第２検出部１４が計測した計測値及び計測値の取得時刻を記憶部１６Ａに記憶させる。演算部１８Ａは、記憶部１６Ａに記憶されたデータログに基づいて移動体と対象物との相対距離及び相対角度を算出する。
【０１２２】
演算部１８Ａは、入力部５２Ａに入力されたコマンドに基づいて各種の演算処理を実行する。演算部１８Ａは、入力部５２Ａに入力されたコマンド、記憶部１６Ａに記憶された各種のデータ及び設定、演算部１８Ａが算出した各処理の実行結果及び演算結果等を表示部５４Ａに表示させる。演算部１８Ａは、例えば、時系列の移動体と対象物との相対距離及び相対角度に基づいて対象物の形状を三次元画像として表示部５４Ａに表示させてもよい。  
【０１２３】
入力部５２Ａは、例えば、キーボード又はタッチパネル等である。入力部５２Ａは、演算部１８Ａに接続される。オペレータは、入力部５２Ａを介して、演算部１８Ａに各種のコマンドを入力することができる。
【０１２４】
表示部５４Ａは、演算部１８Ａの制御によって、入力部５２Ａに入力されたコマンド、記憶部１６Ａに記憶された各種のデータ及び設定、演算部１８Ａが算出した各処理の実行結果及び演算結果等を表示する。表示部５４Ａは、例えば、対象物との相対距離及び相対角度の時系列のデータに基づいて対象物の形状を描画する。表示部５４Ａは、例えば、図１に示す部分面Ｐの時系列のデータに基づいて対象物の形状を描画する。表示部５４Ａは、例えば、液晶ディスプレイ（ＬＣＤ：Ｌｉｑｕｉｄ  Ｃｒｙｓｔａｌ  Ｄｉｓｐｌａｙ）、有機ＥＬディスプレイ（ＯＥＬＤ：Ｏｒｇａｎｉｃ  Ｅｌｅｃｔｒｏ−Ｌｕｍｉｎｅｓｃｅｎｃｅ  Ｄｉｓｐｌａｙ）、又は無機ＥＬディスプレイ（ＩＥＬＤ：Ｉｎｏｒｇａｎｉｃ  Ｅｌｅｃｔｒｏ−Ｌｕｍｉｎｅｓｃｅｎｃｅ  Ｄｉｓｐｌａｙ）等を含む。表示部５４Ａは、タッチパネル等の入力部５２Ａを含んでもよい。
【０１２５】
以上説明したように、変形例の距離推定装置１０Ａは、移動体３０と対象物１との相対距離及び相対角度に基づいて対象物１の形状を描画する表示部５４を備える。これにより、オペレータが対象物の形状を容易に把握することができる。
【０１２６】
また、演算部１８は、対象物１の計測点（ビームｂの照射位置ｂｐ）の慣性座標に基づいて対象物１の仮想平面（部分面Ｐ）の傾きを算出する。距離推定装置１０は、仮想平面に基づいて対象物１の形状を描画する表示部５４を備える。これにより、オペレータが対象物の形状を容易に把握することができる。
【符号の説明】
【０１２７】
１  対象物
１０、１０Ａ  距離推定装置
１２  第１検出部
１４  第２検出部
１６、１６Ａ  記憶部
１８、１８Ａ  演算部
２０  通信部
３０  移動体
３２  機体
３４  照射部
４０  通信ネットワーク
５０  外部装置
５２、５２Ａ  入力部
５４、５４Ａ  表示部
５６  記憶部
５８  演算部
６０  通信部
１００  距離推定システム
Ｇ  重心位置
ｂ、ｂ１、ｂ２、ｂ３、ｂ４  ビーム
ｂｌ、ｂｌ１、ｂｌ２、ｂｌ３、ｂｌ４  ビーム長
ｂｐ、ｂｐ１、ｂｐ２、ｂｐ３、ｂｐ４  照射位置
ｂｌｖ  有効値
ｂｌｏ１  無効値
ｂｌｏ２  外れ値
Ｐ  部分面
Ｐｎｏｗ  現部分面
Ｐｏｌｄ  前部分面
ｈ  高度
θ  傾き
θｄ  角度差
Ｄ  範囲
【図１】
2020148513.tif 000019
【図２】
2020148513.tif 000020
【図３】
2020148513.tif 000021
【図４】
2020148513.tif 000022
【図５】
2020148513.tif 000023
【図６】
2020148513.tif 000024
【図７】
2020148513.tif 000025