特開 2024-21131 姿勢角補正装置および姿勢角補正方法

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

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

(11)【公開番号】P2024021131

(43)【公開日】2024-02-16

(54)【発明の名称】姿勢角補正装置および姿勢角補正方法

(51)【国際特許分類】

   G05D 1/49 20240101AFI20240208BHJP

   B64C 39/02 20060101ALI20240208BHJP

   B64C 13/18 20060101ALI20240208BHJP

   G01S 17/875 20200101ALN20240208BHJP

【ＦＩ】

G05D1/08

B64C39/02

B64C13/18 F

G01S17/875

【審査請求】未請求

【請求項の数】8

【出願形態】ＯＬ

(21)【出願番号】P 2022123742

(22)【出願日】2022-08-03

(71)【出願人】

【識別番号】000010098

【氏名又は名称】アルプスアルパイン株式会社

(74)【代理人】

【識別番号】100105784

【弁理士】

【氏名又は名称】橘 和之

(74)【代理人】

【識別番号】100098497

【弁理士】

【氏名又は名称】片寄 恭三

(74)【代理人】

【識別番号】100099748

【弁理士】

【氏名又は名称】佐藤 克志

(74)【代理人】

【識別番号】100103171

【弁理士】

【氏名又は名称】雨貝 正彦

(72)【発明者】

【氏名】西堀 毅

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

5H301

5J084

【Ｆターム（参考）】

5H301AA06

5H301AA10

5H301GG07

5H301GG08

5H301GG17

5J084AA04

5J084AA05

5J084AA10

5J084AC04

5J084AC06

5J084AC07

5J084AC10

5J084AD01

5J084BA03

5J084CA31

5J084CA70

5J084DA07

5J084DA09

5J084EA19

5J084EA34

(57)【要約】

【課題】位置姿勢検出装置により検出される姿勢角を適切に補正可能な「姿勢角補正装置および姿勢角補正方法」を提供する。
【解決手段】第１時点の検出データおよび第２時点の検出データから生成された３次元点群データのそれぞれから、水平座標が所定距離以内にある第１時点の点および第２時点の点を抽出する点抽出部１４と、当該抽出した点のそれぞれについて、複数の異なる仮ピッチ角補正値を適用した場合の補正点を生成する補正点生成部１５と、第１時点の補正点と第２時点の補正点との高度差が最小となる仮ピッチ角補正値をピッチ角補正値として特定する補正値特定部１６とを備え、水平座標が所定距離以内にあるものとして抽出される点を対象として、複数の異なる仮ピッチ角補正値を用いた場合の補正点の高度差が最小となる仮ピッチ角補正値を探索することで、誤差を抑制するためのピッチ角補正値を特定可能にする。
【選択図】図１

【特許請求の範囲】

【請求項1】

光学式レーダおよび位置姿勢検出装置が搭載された移動体を反射対象物の周囲を移動させて上記光学式レーダにより検出した距離方向情報および上記位置姿勢検出装置により検出した位置姿勢情報を含む検出データを取得する検出データ取得部と、
上記検出データ取得部により取得された上記検出データに基づいて、直角座標系において上記反射対象物の３次元点群データを生成する点群データ生成部と、
上記点群データ生成部により第１時点の検出データから生成された第１時点の３次元点群データと、上記第１時点とは異なる第２時点の検出データから生成された第２時点の３次元点群データとのそれぞれから、水平座標が互いに所定距離以内にある第１時点の点および第２時点の点を、同一の反射対象物に関して生成されたものと推定される点の組み合わせとして抽出する点抽出部と、
上記点抽出部により上記第１時点の３次元点群データから抽出された上記第１時点の点および上記第２時点の３次元点群データから抽出された上記第２時点の点のそれぞれについて、複数の異なる仮ピッチ角補正値を上記位置姿勢情報に適用した場合の補正点を上記複数の異なる仮ピッチ角補正値ごとに生成する補正点生成部と、
上記複数の異なる仮ピッチ角補正値の中から、上記第１時点の点について上記補正点生成部により生成された第１時点の補正点と、上記第２時点の点について上記補正点生成部により生成された第２時点の補正点との高度差である補正後高度差が、上記点抽出部により抽出された上記第１時点の点と上記第２時点の点との高度差である補正前高度差よりも小さくなる仮ピッチ角補正値をピッチ角補正値として特定する補正値特定部とを備えた
ことを特徴とする姿勢角補正装置。

【請求項2】

上記補正値特定部は、上記第１時点の３次元点群データから抽出された複数の上記第１時点の点について上記補正点生成部により生成された複数の上記第１時点の補正点と、上記第２時点の３次元点群データから抽出された複数の上記第２時点の点について上記補正点生成部により生成された複数の上記第２時点の補正点との総当りの組み合わせごとに、上記補正後高度差が上記補正前高度差よりも小さくなる仮ピッチ角補正値を求め、得られた複数の仮ピッチ角補正値の統計値を上記ピッチ角補正値として特定する
ことを特徴とする請求項１に記載の姿勢角補正装置。

【請求項3】

上記点抽出部は、上記第１時点の３次元点群データと上記第２時点の３次元点群データとのそれぞれから、水平座標が互いに第１距離以内にある上記第１時点の点および上記第２時点の点を抽出し、
上記補正点生成部により生成された複数の上記第１時点の補正点および複数の上記第２時点の補正点の中から、水平座標が互いに上記第１距離より短い第２距離以内にある上記第１時点の補正点および上記第２時点の補正点を抽出する補正点抽出部を更に備え、
上記補正値特定部は、上記補正点抽出部により抽出された複数の上記第１時点の補正点と複数の上記第２時点の補正点との総当りの組み合わせごとに、上記補正後高度差が上記補正前高度差よりも小さくなる仮ピッチ角補正値を求め、得られた複数の仮ピッチ角補正値の統計値を上記ピッチ角補正値として特定する
ことを特徴とする請求項２に記載の姿勢角補正装置。

【請求項4】

上記補正点抽出部は、上記補正点生成部により生成された複数の上記第１時点の補正点および複数の上記第２時点の補正点の中から、水平座標が互いに上記第２距離以内にあり、かつ、上記補正後高度差が所定値以内である上記第１時点の補正点および上記第２時点の補正点を抽出することを特徴とする請求項３に記載の姿勢角補正装置。

【請求項5】

上記補正値特定部は、上記複数の異なる仮ピッチ角補正値の中から、上記補正後高度差が最小となる仮ピッチ角補正値を上記ピッチ角補正値として特定することを特徴とする請求項１～４の何れか１項に記載の姿勢角補正装置。

【請求項6】

上記点抽出部は、上記光学式レーダからレーザ光が所定のアジマス角で照射されたときの上記検出データに基づいて上記点群データ生成部により生成された３次元点群データの中から、上記第１時点の点および上記第２時点の点を抽出することを特徴とする請求項１～４の何れか１項に記載の姿勢角補正装置。

【請求項7】

上記複数の異なる仮ピッチ角補正値を設定する仮補正値設定部を更に備えたことを特徴とする請求項１～４の何れか１項に記載の姿勢角補正装置。

【請求項8】

姿勢角補正装置の検出データ取得部が、光学式レーダおよび位置姿勢検出装置が搭載された移動体を反射対象物の周囲を移動させて上記光学式レーダにより検出した距離方向情報および上記位置姿勢検出装置により検出した位置姿勢情報を含む検出データを取得する第１のステップと、
上記姿勢角補正装置の点群データ生成部が、上記検出データ取得部により取得された上記検出データに基づいて、直角座標系において上記反射対象物の３次元点群データを生成する第２のステップと、
上記姿勢角補正装置の点抽出部が、上記点群データ生成部により第１時点の検出データから生成された第１時点の３次元点群データと、上記第１時点とは異なる第２時点の検出データから生成された第２時点の３次元点群データとのそれぞれから、水平座標が互いに所定距離以内にある第１時点の点および第２時点の点を、同一の反射対象物に関して生成されたものと推定される点の組み合わせとして抽出する第３のステップと、
上記姿勢角補正装置の補正点生成部が、上記点抽出部により上記第１時点の３次元点群データから抽出された上記第１時点の点および上記第２時点の３次元点群データから抽出された上記第２時点の点のそれぞれについて、複数の異なる仮ピッチ角補正値を上記位置姿勢情報に適用した場合の補正点を上記複数の異なる仮ピッチ角補正値ごとに生成する第４のステップと、
上記姿勢角補正装置の補正値特定部が、上記複数の異なる仮ピッチ角補正値の中から、上記第１時点の点について上記補正点生成部により生成された第１時点の補正点と、上記第２時点の点について上記補正点生成部により生成された第２時点の補正点との高度差である補正後高度差が、上記点抽出部により抽出された上記第１時点の点と上記第２時点の点との高度差である補正前高度差よりも小さくなる仮ピッチ角補正値をピッチ角補正値として特定する第５のステップとを有する
ことを特徴とする姿勢角補正方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、姿勢角補正装置および姿勢角補正方法に関し、特に、機器姿勢のピッチ角を補正する装置および方法に関するものである。

【背景技術】

【0002】

ＬｉＤＡＲ（Light Detection And Ranging）は、周囲にレーザ光を照射し、物体に当たって跳ね返ってくるまでの時間を計測することで、物体までの距離や方向の測定、ひいてはＬｉＤＡＲを中心とする座標系における物体の位置座標の取得が可能な装置である。ＬｉＤＡＲは、地質学、気象学、リモートセンシングなどの分野で主に用いられてきた。例えば、ＬｉＤＡＲと共にＩＭＵ（慣性航法装置）等の位置姿勢検出装置をドローン等の移動体に搭載して、地形調査やインフラの劣化・故障検査を行うといった使い方がある。近年では、ＬｉＤＡＲは自動運転車用センサとしても注目されている。

【0003】

例えば、ＬｉＤＡＲおよびＩＭＵを取り付けたドローンを飛行させ、ＩＭＵにより検出されるデータが示す自己の位置姿勢情報をもとにＬｉＤＡＲの位置姿勢を特定し、ＬｉＤＡＲにより検出される距離方向情報から３次元点群データを取得することが可能である。例えばインフラの劣化・故障検査は、以上のようにして生成される点群データを利用して行われる。例えば、点群データにより点検箇所を特定した後、その特定箇所を別途光学カメラで撮影し、その撮影映像からインフラに異常が発生していないか等を目視により確認することが可能である。

【0004】

上述のように、ＬｉＤＡＲの計測データから３次元点群データを生成するに当たり、ＩＭＵにより検出される位置姿勢情報は、ＬｉＤＡＲの位置姿勢を特定するために利用される。そのため、ドローンに対するＬｉＤＡＲとＩＭＵの取付角度に誤差が生じないことが重要となる。取付角度誤差があると、ＩＭＵにより検出される姿勢角がＬｉＤＡＲの姿勢を正しく捉えたものとならないため、本来は同一点に重なるべき点がずれてしまい、点群データがブレているように見える要因になってしまうからである。

【0005】

しかしながら、従来はこの取付角度誤差が生じないことを、ＬｉＤＡＲとＩＭＵの実物を目視することで確認していた。そのため、視認できない程度の角度差は生じてしまっていた。また、ＩＭＵの性能上、センサが出力する値に定常誤差を含んでいる。この定常誤差も、点群データがブレているように見える要因となる。以上のことから、ＬｉＤＡＲとＩＭＵの取付角度誤差およびＩＭＵのセンサ出力の定常誤差を除去するため、姿勢角の補正を適切に行うことが望まれる。

【0006】

なお、第１時刻の計測によって生成された第１レーザ点群および第２時刻の計測によって生成された第２レーザ点群のそれぞれから、第１時刻に計測された第１計測領域と第２時刻に計測された第２計測領域とに共通する共通計測領域に含まれる第１共通点群および第２共通点群を抽出し、第１共通点群と第２共通点群とを比較することによって、第１時刻における移動体の姿勢に対する第２時刻における移動体の姿勢の変化量を算出する技術が知られている（例えば、特許文献１参照）。しかしながら、この技術は、点群データの変化量から移動体の姿勢の変化量を算出するものであり、姿勢角の補正を行うものではない。

【0007】

また、航空レーザ測量や空中写真測量で得られた３次元地形データについて、領域内にわたる経路上で正しい座標値を計測した基準線を導入し、領域内各点において地形データと間近の基準線の高さとを比較して、その差の重み付き平均を補正量とすることで地形データの高さの値を補正する技術も知られている（例えば、特許文献２参照）。しかしながら、この技術は、航空レーザで検出した点群の高さとあらかじめ検出した地形の高さ情報とを比較して、地形データの高さの値を補正するものであり、姿勢角の補正を行うものではない。

【先行技術文献】

【特許文献】

【0008】

【特許文献1】特開２０２０－１４３９９４号公報

【特許文献2】特開２００７－２９８３３２号公報

【発明の概要】

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

【0009】

本発明は、以上のような問題を解決するために成されたものであり、移動体に光学式レーダと共に取り付けられる位置姿勢検出装置により検出される姿勢角を適切に補正できるようにすることを目的とする。

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

【0010】

上記した課題を解決するために、本発明では、移動体を反射対象物の周囲を移動させて光学式レーダおよび位置姿勢検出装置により検出した第１時点の検出データから生成された第１時点の３次元点群データと、第１時点とは異なる第２時点の検出データから生成された第２時点の３次元点群データとのそれぞれから、水平座標が互いに所定距離以内にある第１時点の点および第２時点の点を抽出し、当該抽出した点のそれぞれについて、複数の異なる仮ピッチ角補正値を適用した場合の補正点を複数の異なる仮ピッチ角補正値ごとに生成する。そして、複数の異なる仮ピッチ角補正値の中から、第１時点の点について生成された第１時点の補正点と、第２時点の点について生成された第２時点の補正点との高度差である補正後高度差が、第１時点の点と第２時点の点との高度差である補正前高度差よりも小さくなる仮ピッチ角補正値をピッチ角補正値として特定するようにしている。

【発明の効果】

【0011】

移動体に対する光学式レーダと位置姿勢検出装置との取付角度誤差や、位置姿勢検出装置の出力の定常誤差が仮にないとすると、同じ反射対象物について異なる時点（第１時点および第２時点）で検出されるデータに基づき生成される３次元点群データは互いに一致するはずである。上記のように構成した本発明はこのことに鑑みてなされたものであり、第１時点の３次元点群データと第２時点の３次元点群データとのそれぞれから、水平座標が互いに所定距離以内にある第１時点の点および第２時点の点を、同一の反射対象物に関して生成されたものと推定される点の組み合わせとして抽出する。そして、当該抽出した点を対象として、複数の異なる仮ピッチ角補正値を用いて、当該仮ピッチ角補正値を第１時点の検出データ（位置姿勢情報）に適用して生成される第１時点の補正点と、仮ピッチ角補正値を第２時点の検出データ（位置姿勢情報）に適用して生成される第２時点の補正点との高度差である補正後高度差が補正前高度差よりも小さくなる仮ピッチ角補正値を探索すれば、それを取付角度誤差や定常誤差を抑制するためのピッチ角補正値として特定することができる。これにより、本発明によれば、光学式レーダと共に取り付けられる位置姿勢検出装置により検出される姿勢角（ピッチ角）を適切に補正することができる。

【図面の簡単な説明】

【0012】

【図1】本実施形態による姿勢角補正装置の機能構成例を示すブロック図である。

【図2】ドローンの構成を単純化して模式的に示す図である。

【図3】ＬｉＤＡＲのレーザ光の照射範囲を模式的に示す図である。

【図4】本実施形態による点抽出部の処理内容を説明するための模式図である。

【図5】本実施形態による補正点生成部および補正値特定部の処理内容を説明するための模式図である。

【図6】本実施形態による姿勢角補正装置の動作例を示すフローチャートである。

【図7】変形例に係る姿勢角補正装置の機能構成例を示すブロック図である。

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

【0013】

以下、本発明の一実施形態を図面に基づいて説明する。図１は、本実施形態による姿勢角補正装置１の機能構成例を示すブロック図である。図１に示すように、本実施形態の姿勢角補正装置１は、機能構成として、検出データ取得部１１、仮補正値設定部１２、点群データ生成部１３、点抽出部１４、補正点生成部１５および補正値特定部１６を備えている。

【0014】

上記機能ブロック１１～１６は、ハードウェア、ＤＳＰ（Digital Signal Processor）、ソフトウェアの何れによっても構成することが可能である。例えばソフトウェアによって構成する場合、上記機能ブロック１１～１６は、実際にはコンピュータのＣＰＵ、ＲＡＭ、ＲＯＭなどを備えて構成され、ＲＡＭやＲＯＭ、ハードディスクまたは半導体メモリ等の記憶媒体に記憶されたプログラムが動作することによって実現される。

【0015】

検出データ取得部１１は、光学式レーダおよび位置姿勢検出装置が搭載された移動体を反射対象物の周囲を移動させて光学式レーダにより検出した距離方向情報および位置姿勢検出装置により検出した位置姿勢情報を含む検出データを取得する。光学式レーダは、本実施形態ではＬｉＤＡＲである。また、位置姿勢検出装置は、本実施形態ではＩＭＵ（慣性航法装置）である。移動体は、本実施形態ではドローン（無人飛行体）である。ドローンは、ＬｉＤＡＲおよびＩＭＵによるセンシング動作中、高度を一定に保ちながら直線状に飛行する。この飛行経路の周囲に反射対象物が存在する。

【0016】

図２は、ドローン１００の構成を単純化して模式的に示す図である。図２に示すように、ドローン１００は、飛行体本体１０１および４つのプロペラ１０２を備えている。ドローン１００は、４つのプロペラ１０２の回転数を調整することによって、上昇や下降、前進、後進、旋回等を行う。飛行体本体１０１の下部にはＬｉＤＡＲ１０３およびＩＭＵ１０４が取り付けられている。

【0017】

ＬｉＤＡＲ１０３は、周囲にレーザ光を照射し、物体に当たって跳ね返ってくるまでの時間を計測することで、物体までの距離および方向を検出し、距離方向情報（距離情報および方向情報）を出力する。ＬｉＤＡＲ１０３は、レーダ測定動作の実行中、所定の周期でセンシングを繰り返し実行し、当該所定の周期で距離方向情報を逐次生成する。

【0018】

図３は、ＬｉＤＡＲ１０３のレーザ光の照射範囲を模式的に示す図である。図３（ａ）はドローン１００（ＬｉＤＡＲ１０３）を側方から見た様子を示し、図３（ｂ）はドローン１００（ＬｉＤＡＲ１０３）を正面から見た様子を示している。

【0019】

図３（ａ）に示すように、１周期のセンシングでＬｉＤＡＲ１０３は、飛行体本体１０１の前後方向に所定の照射角ωの範囲で放射状に所定数のレーザ光を照射し、受光機構によって反射光を検出する。照射角ωはＬｉＤＡＲ１０３の真下が０度であり、ＬｉＤＡＲ１０３は、例えば２度ずつの刻みで前後の最大値をそれぞれ１５度とする照射角ωの範囲でレーザ光を照射する。

【0020】

また、図３（ｂ）に示すように、１周期のセンシングでＬｉＤＡＲ１０３は、飛行体本体１０１の左右方向に所定のアジマス角αの範囲で放射状に所定数のレーザ光を照射し、受光機構によって反射光を検出する。アジマス角αは真下が０度であり、ＬｉＤＡＲ１０３は、例えば２度ずつの刻みで左右の最大値をそれぞれ１５度とするアジマス角αの範囲でレーザ光を照射する。

【0021】

ＩＭＵ１０４は、ＧＮＳＳ（Global Navigation Satellite System）センサ、ジャイロセンサ、加速度センサ等の各種センサを備えており、ＩＭＵ１０４の自身の位置および姿勢を検出し、位置姿勢情報（位置情報および姿勢情報）を出力する。ＩＭＵ１０４の位置情報とは、緯度、経度および高度により特定される三次元的な位置の情報である。ＩＭＵ１０４の姿勢情報とは、ＩＭＵ１０４のロール角、ピッチ角およびヨー角の組み合わせから成る姿勢角の情報である。

【0022】

ドローン１００は記憶媒体を搭載しており、ＬｉＤＡＲ１０３およびＩＭＵ１０４により逐次検出される検出データ（反射対象物に対する距離方向情報およびＩＭＵ１０４の位置姿勢情報）を時刻情報と共に記憶媒体にログデータとして記憶する。これにより、ドローン１００の記憶媒体には、ＬｉＤＡＲ１０３により検出される距離方向情報が累積的に記憶されるとともに、ＩＭＵ１０４により検出される位置姿勢情報が累積的に記録される。距離方向情報と位置姿勢情報とは時刻情報により関連付けられる。

【0023】

検出データ取得部１１は、ドローン１００の記憶媒体に記憶されている検出データを取得する。例えば、検出データ取得部１１は、Ｗｉ－Ｆｉ（登録商標）またはＢｌｕｅｔｏｏｔｈ（登録商標）などの無線通信手段を用いて、ドローン１００の記憶媒体に記憶されている検出データを取得する。あるいは検出データ取得部１１は、リムーバブル記憶媒体またはＵＳＢ（Universal Serial Bus）などの通信ケーブルを用いて、ドローン１００の記憶媒体に記憶されている検出データを取得してもよい。

【0024】

仮補正値設定部１２は、検出データ取得部１１により取得された位置姿勢情報に対する複数の異なる仮ピッチ角補正値を設定する。例えば、仮補正値設定部１２は、仮ピッチ角補正値＝０．０度を含め、プラス方向およびマイナス方向に所定角度ずつの刻みで複数の仮ピッチ角補正値を設定する。ここで、仮補正値設定部１２は、例えばユーザによる操作に基づく指示に従って、複数の仮ピッチ角補正値の刻み幅となる所定角度の値を設定する。また、仮ピッチ角補正値の上限値と下限値を更に設定するようにしてもよい。

【0025】

複数の仮ピッチ角補正値の刻み幅となる所定角度の値を小さくするほど、より高精度にピッチ角補正値を求めることが可能となる一方で、計算負荷が大きくなる。精度または計算負荷のどちらをどの程度優先するかに応じて、所定角度の値を適宜設定することが可能である。また、仮ピッチ角補正値の上限値と下限値を適切に設定することにより、設定される仮ピッチ角補正値の数を調整することが可能であり、これにより計算負荷を調整することが可能である。なお、仮補正値設定部１２を省略し、所定角度の値、上限値および下限値を固定値として姿勢角補正装置１にあらかじめ設定しておくようにしてもよい。

【0026】

点群データ生成部１３は、検出データ取得部１１により取得された検出データに基づいて、直角座標系において反射対象物の３次元点群データを生成する。直角座標系は、例えば東西方向をＸ軸、南北方向をＹ軸、鉛直方向（高度方向）をＺ軸とする座標系であり、Ｘ軸の値とＹ軸の値とにより水平座標が形成される。点群データ生成部１３は、距離方向情報と位置姿勢情報とが時刻情報により関連付けられて成る時系列の検出データに基づいて、各時点の３次元点群データを生成する。この点群データ生成部１３の処理は、公知技術を適用して行うことが可能である。

【0027】

点抽出部１４は、点群データ生成部１３により第１時点の検出データから生成された第１時点の３次元点群データと、第１時点とは異なる第２時点の検出データから生成された第２時点の３次元点群データとのそれぞれから、水平座標が互いに所定距離以内にある第１時点の点および第２時点の点を、同一の反射対象物に関して生成されたものと推定される点の組み合わせとして抽出する。すなわち、ＩＭＵ１０４により検出される姿勢角のうち、ロール角とヨー角については補正の必要がない状態であるとすると、水平座標が互いに所定距離以内にある第１時点の点および第２時点の点は、同一の反射対象物に関して生成されたものと推定可能である。点抽出部１４は、このような関係にある第１時点の点および第２時点の点の組み合わせを抽出する。

【0028】

図４は、この点抽出部１４の処理内容を説明するための模式図である。図４では、ＬｉＤＡＲ１０３の移動経路と、検出データから生成される３次元点群データの一部の点群とを模式的に示している。図４に示すように、ＬｉＤＡＲ１０３を搭載したドローン１００は、水平座標平面（Ｘ－Ｙ平面）に対して平行に、一定の高度を保ちながら移動する。上述したように、ＬｉＤＡＲ１０３は、ドローン１００の移動中に所定の周期で繰り返しセンシングを行う。第１時点Ｔ１は、ある周期のタイミングを示し、第２時点Ｔ２は、第１時点Ｔ１からｋ周期後のタイミングを示す。

【0029】

図４において■印で示す複数の点は、点群データ生成部１３により第１時点Ｔ１の検出データから生成された第１時点の３次元点群データの一部である。また、●印で示す複数の点は、点群データ生成部１３により第２時点Ｔ２の検出データから生成された第２時点の３次元点群データの一部である。ここでは、照射角ωおよびアジマス角αを所定角度ずつ変えながらレーザ光を照射して検出される検出データに基づき生成される３次元点群データのうち、レーザ光が所定のアジマス角α（例えばα＝０度）で異なる照射角ωの方向に照射されたときの検出データに基づき生成された点群を、説明用に簡易的に示している。

【0030】

図４に示す例において、点抽出部１４は、第１時点Ｔ１の検出データから生成された３つの点のうち１つの点Ｐ１（ＬｉＤＡＲ１０３より前方に照射したレーザ光に基づき生成される第１時点Ｔ１の点）と、第１時点Ｔ１より後の第２時点Ｔ２の検出データから生成された３つの点のうち１つの点Ｐ２（ＬｉＤＡＲ１０３より後方に照射したレーザ光に基づき生成される第２時点Ｔ２の点）を、水平座標（Ｘ座標およびＹ座標で特定される２次元平面上に投影された点の位置）が互いに所定距離以内にある点の組み合わせとして抽出する。

【0031】

ＩＭＵ１０４により検出されるピッチ角に誤差がある場合、第１時点Ｔ１の点Ｐ１と第２時点Ｔ２の点Ｐ２との高度差（Ｚ座標の差）は大きくなる可能性があるが、ロール角およびヨー角に大きな誤差がないとすると、同一の反射対象物に関して異なる時点の検出データから生成される点どうしは、水平座標が互いに所定距離以内に存在することになる。ピッチ角の誤差は水平座標にも影響を与えるが、Ｚ座標ほど大きな誤差は与えない。よって、点抽出部１４は、水平座標が互いに所定距離以内（例えば、１ｍ以内）にある点の組み合わせを、同一の反射対象物に関して生成されたものと推定される点の組み合わせとして抽出する。

【0032】

補正点生成部１５は、点抽出部１４により第１時点の３次元点群データから抽出された第１時点の点および第２時点の３次元点群データから抽出された第２時点の点のそれぞれについて、仮補正値設定部１２により設定された複数の異なる仮ピッチ角補正値を位置姿勢情報に適用した場合の補正点を複数の異なる仮ピッチ角補正値ごとに生成する。

【0033】

図４に示した例の場合、補正点生成部１５は、第１時点Ｔ１の３次元点群データ（■印の点群）から抽出された第１時点Ｔ１の点Ｐ１について、仮補正値設定部１２により設定された複数の異なる仮ピッチ角補正値を位置姿勢情報に適用した場合の補正点を複数の異なる仮ピッチ角補正値ごとに生成する。これにより、第１時点Ｔ１の点Ｐ１に対して、複数の異なる仮ピッチ角補正値を適用した複数の補正点が生成される。

【0034】

また、補正点生成部１５は、第２時点Ｔ２の３次元点群データ（●印の点群）から抽出された第２時点Ｔ２の点Ｐ２についても同様に、仮補正値設定部１２により設定された複数の異なる仮ピッチ角補正値を位置姿勢情報に適用した場合の補正点を複数の異なる仮ピッチ角補正値ごとに生成する。これにより、第２時点Ｔ２の点Ｐ２に対して、複数の異なる仮ピッチ角補正値を適用した複数の補正点が生成される。

【0035】

なお、補正点生成部１５は、次のように補正点を生成してもよい。すなわち、補正点生成部１５は、検出データ取得部１１により取得された検出データに基づいて、複数の仮ピッチ角補正値を位置姿勢情報に適用して得られる複数の補正検出データごとに、第１時点の補正３次元点群データおよび第２時点の補正３次元点群データをそれぞれ生成する。そして、このようにして生成される補正３次元点群データの中から、点抽出部１４により抽出された点に対する補正点を特定する。

【0036】

ここで、仮ピッチ角補正値を位置姿勢情報に適用するとは、位置姿勢情報に含まれるピッチ角を仮ピッチ角補正値で補正することを意味する。例えば、仮ピッチ角補正値が＋１．０度であれば、位置姿勢情報に含まれるピッチ角を１．０度大きくすることを意味する。補正検出データとは、このように仮ピッチ角補正値で補正されたピッチ角を含む位置姿勢情報および距離方向情報であり、ピッチ角以外は補正されていない検出データである。補正３次元点群データは、補正検出データから直角座標系において生成される３次元点群データである。

【0037】

図５は、この補正点生成部１５および後述する補正値特定部１６の処理内容を説明するための模式図である。図５では、仮ピッチ角補正値としてθを位置姿勢情報に適用した場合に生成される複数の補正点を示している。ここでは、点抽出部１４により抽出された点Ｐ１，Ｐ２だけでなく、補正検出データに基づいて図４に示した６つの点のそれぞれについて補正点を生成する例を示している。補正点生成部１５は、生成した６つの補正点の中から、点Ｐ１に対する補正点Ｐ１’および点Ｐ２に対する補正点Ｐ２’を特定する。

【0038】

補正値特定部１６は、仮補正値設定部１２により設定された複数の異なる仮ピッチ角補正値の中から、第１時点の３次元点群データから抽出された第１時点の点について補正点生成部１５により生成された第１時点の補正点と、第２時点の３次元点群データから抽出された第２時点の点について補正点生成部１５により生成された第２時点の補正点との高度差（以下、補正後高度差という）が、点抽出部１４により抽出された第１時点の点と第２時点の点との高度差（以下、補正前高度差という）よりも小さくなる仮ピッチ角補正値を抽出し、これをピッチ角補正値として特定する。一例として、補正値特定部１６は、補正後高度差が最小となる仮ピッチ角補正値をピッチ角補正値として特定する。

【0039】

図４に示した例の場合、補正値特定部１６は、第１時点Ｔ１の点Ｐ１について複数の異なる仮ピッチ角補正値ごとに補正点生成部１５により生成された複数の補正点と、第２時点Ｔ２の点Ｐ２について複数の異なる仮ピッチ角補正値ごとに補正点生成部１５により生成された複数の補正点とをそれぞれ比較し、補正点どうしの高度差が最小となる仮ピッチ角補正値を探索する。図５は、第１時点Ｔ１の補正点Ｐ１’と第２時点Ｔ２の補正点Ｐ２’との補正後高度差が最小なる場合を示しており、この場合に適用した仮ピッチ角補正値θを、求めるピッチ角補正値として特定する。

【0040】

ドローン１００に対するＬｉＤＡＲ１０３とＩＭＵ１０４の取付角度誤差や、ＩＭＵ１０４のセンサ出力の定常誤差が仮にないとすると、同じ反射対象物について異なる時点（第１時点Ｔ１および第２時点Ｔ２）で検出されるデータに基づき生成される３次元点群データは互いに一致するはずである。一方、ピッチ角に誤差があったとしても、適切なピッチ角補正値を設定すれば、同じ反射対象物について異なる時点で検出されるデータに基づき生成される補正３次元点群データは互いに一致することになる。

【0041】

本実施形態ではこのことに鑑みて、第１時点の３次元点群データと第２時点の３次元点群データとのそれぞれから、水平座標が互いに所定距離以内にある点を同一の反射対象物に関して生成された点の組み合わせとして抽出し、当該抽出した点を対象として、複数の異なる仮ピッチ角補正値を適用した場合の第１時点の補正点と第２時点の補正点とを生成してそれらの補正後高度差が最小となる仮ピッチ角補正値を探索し、それを取付角度誤差や定常誤差を抑制するためのピッチ角補正値として特定するようにしている。

【0042】

補正値特定部１６により特定されたピッチ角補正値は、点群データ生成装置（図示せず）に設定される。点群データ生成装置はビューアに内蔵されるものであってもよいし、ビューアとは別に存在するものであってもよい。ピッチ角補正値を点群データ生成装置に設定することにより、ビューアが３次元点群データを表示する際には、ＩＭＵ１０４により検出されたピッチ角（誤差を含む）が適切に補正された状態で３次元点群データを表示することが可能となる。

【0043】

なお、補正値特定部１６により特定されたピッチ角補正値をＩＭＵ１０４に設定するようにしてもよい。このようにすれば、ピッチ角補正値に基づき補正されたピッチ角を含む姿勢情報をＩＭＵ１０４から得ることが可能となる。

【0044】

図４では、第１時点Ｔ１の３次元点群データおよび第２時点の３次元点群データのそれぞれから、水平座標が互いに所定距離以内にある点Ｐ１，Ｐ２を１つずつ抽出する状態を示しているが、これは説明用であり、実際には水平座標が互いに所定距離以内にある点が複数個ずつ抽出される可能性がある。この場合、例えば補正値特定部１６は、第１時点Ｔ１の３次元点群データから抽出された複数の第１時点の点Ｐ１_-1，Ｐ１_-2，・・・，Ｐ１_-mについて補正点生成部１５により生成された複数の補正点Ｐ１_-1’，Ｐ１_-2’，・・・，Ｐ１_-m’と、第２時点Ｔ２の３次元点群データから抽出された複数の第２時点の点Ｐ２_-1，Ｐ２_-2，・・・，Ｐ２_-nについて補正点生成部１５により生成された複数の補正点Ｐ２_-1’，Ｐ２_-2’，・・・，Ｐ２_-n’との総当りの組み合わせごとに、補正後高度差が最小となる仮ピッチ角補正値θ_-11，θ_-12，・・・，θ_-mnを求め、得られた複数の仮ピッチ角補正値θ_-11，θ_-12，・・・，θ_-mnの統計値をピッチ角補正値θとして特定する。統計値は、例えば平均値としてもよいが、外れ値の影響をより受けにくくなる中央値を用いるのが好ましい。

【0045】

図６は、以上のように構成した本実施形態による姿勢角補正装置１の動作例を示すフローチャートである。図６に示すフローチャートは、反射対象物の周囲におけるドローン１００の移動が完了し、その移動中にＬｉＤＡＲ１０３およびＩＭＵ１０４により検出された距離方向情報および位置姿勢情報が記憶媒体に記憶された後に実行される。

【0046】

まず、検出データ取得部１１は、ドローン１００の記憶媒体に記憶されている検出データ（距離方向情報および位置姿勢情報）を取得する（ステップＳ１）。次に、仮補正値設定部１２は、位置姿勢情報に対する複数の異なる仮ピッチ角補正値を設定する（ステップＳ２）。なお、この処理はステップＳ１の前にあらかじめ行っておいてもよい。また、所定の仮ピッチ角補正値を用いることにより、ステップＳ２の処理を省略してもよい。

【0047】

次いで、点群データ生成部１３は、検出データ取得部１１により取得された検出データに基づいて、直角座標系において反射対象物の３次元点群データを生成する（ステップＳ３）。そして、点抽出部１４は、点群データ生成部１３により第１時点の検出データから生成された第１時点の３次元点群データと、第２時点の検出データから生成された第２時点の３次元点群データとのそれぞれから、水平座標が互いに所定距離以内にある第１時点の点および第２時点の点を抽出する（ステップＳ４）。

【0048】

次いで、補正点生成部１５は、第１時点の３次元点群データから抽出された第１時点の点および第２時点の３次元点群データから抽出された第２時点の点のそれぞれについて、複数の異なる仮ピッチ角補正値を位置姿勢情報に適用した場合の補正点を複数の異なる仮ピッチ角補正値ごとに生成する（ステップＳ５）。

【0049】

次に、補正値特定部１６は、仮補正値設定部１２により設定された複数の異なる仮ピッチ角補正値の中から、第１時点の補正点と第２時点の補正点との補正後高度差が最小となる仮ピッチ角補正値をピッチ角補正値として特定する（ステップＳ６）。これにより、図６に示すフローチャートの処理が終了する。

【0050】

以上詳しく説明したように、本実施形態では、第１時点の検出データから生成された第１時点の３次元点群データと、第２時点の検出データから生成された第２時点の３次元点群データとのそれぞれから、水平座標が互いに所定距離以内にある第１時点の点および第２時点の点を抽出し、当該抽出した点のそれぞれについて、複数の異なる仮ピッチ角補正値を位置姿勢情報に適用した場合の補正点を複数の異なる仮ピッチ角補正値ごとに生成する。そして、複数の異なる仮ピッチ角補正値の中から、第１時点の補正点と第２時点の補正点との補正後高度差が最小となる仮ピッチ角補正値をピッチ角補正値として特定するようにしている。

【0051】

このように、水平座標が互いに所定距離以内にあるものとして抽出される第１時点の点および第２時点の点を対象として、複数の異なる仮ピッチ角補正値を用いて、仮ピッチ角補正値を第１時点の検出データに適用して生成される第１時点の補正点と、仮ピッチ角補正値を第２時点の検出データに適用して生成される第２時点の補正点との補正後高度差が最小となる仮ピッチ角補正値を探索することで、取付角度誤差や定常誤差を抑制するためのピッチ角補正値を特定することができる。これにより、本実施形態によれば、光学式レーダと共に取り付けられる位置姿勢検出装置により検出される姿勢角（ピッチ角）を適切に補正することができる。

【0052】

また、本実施形態では、点抽出部１４において、ＬｉＤＡＲ１０３からレーザ光が所定のアジマス角α（例えばα＝０度）で照射されたときの検出データに基づいて点群データ生成部１３により生成された３次元点群データの中から、第１時点の点および第２時点の点を抽出するようにしている。このようにすることにより、同一の反射対象物に関して生成されたものと推定される点の抽出を、より効率的かつより精度よく実施することが可能である。

【0053】

なお、同一の反射対象物に関して生成されたものと推定される点の抽出を、点群をより絞り込んだ状態で行うようにしてもよい。言い換えると、同一の反射対象物に関して生成された点と推定するための条件をより厳しくしてもよい。図７は、この場合における姿勢角補正装置１’の機能構成例を示すブロック図である。この図７において、図１に示した符号と同一の符号を付したものは同一の機能を有するものであるので、ここでは重複する説明を省略する。

【0054】

図７に示すように、変形例に係る姿勢角補正装置１’は、機能構成として、補正点抽出部１７を更に備えている。また、姿勢角補正装置１’は、補正値特定部１６に代えて補正値特定部１６’を備えている。

【0055】

点抽出部１４は、上記のように説明した実施形態と同様に、第１時点の３次元点群データと第２時点の３次元点群データとのそれぞれから、水平座標が互いに所定距離以内にある第１時点の点および第２時点の点を抽出する。なお、ここでは、所定距離（例えば、１ｍ）を第１距離として説明する。

【0056】

補正点抽出部１７は、補正点生成部１５により生成された複数の第１時点の補正点および複数の第２時点の補正点の中から、水平座標が互いに第１距離より短い第２距離以内（例えば、２０ｃｍ以内）にある第１時点の補正点および第２時点の補正点を抽出する。上述したように、ＩＭＵ１０４に検出されるピッチ角に誤差がある場合、点Ｐ１と点Ｐ２との間でＺ座標に差が生じるだけでなく、Ｘ座標にも差が生じる。このため、仮ピッチ角補正値を適用して補正点Ｐ１’，Ｐ２’を生成した場合、点Ｐ１，Ｐ２が同一の反射対象物に関して生成されたものであり、かつ仮ピッチ角補正値が適切であれば、Ｚ座標の差が小さくなるだけでなく、Ｘ座標の差も小さくなる。

【0057】

このことに鑑みて、点Ｐ１，Ｐ２を抽出する際に閾値として用いた第１距離より短い第２距離を閾値として用いて、補正点Ｐ１’，Ｐ２’の水平座標が互いに第２距離以内にある場合にのみ、補正点Ｐ１’，Ｐ２’を同一の反射対象物に関して生成されたものと推定して補正値特定部１６’の処理対象とする。ここでは２つの補正点Ｐ１’，Ｐ２’について説明しているが、実際には複数の補正点Ｐ１_-1’，Ｐ１_-2’，・・・，Ｐ１_-m’と複数の補正点Ｐ２_-1’，Ｐ２_-2’，・・・，Ｐ２_-n’との総当りで確認し、条件を満たす組み合わせのみを抽出する。

【0058】

補正値特定部１６’は、補正点抽出部１７により抽出された複数の第１時点の補正点と、補正点抽出部１７により抽出された複数の第２時点の補正点との総当りの組み合わせごとに、補正後高度差が最小となる仮ピッチ角補正値を求め、得られた複数の仮ピッチ角補正値の統計値をピッチ角補正値として特定する。このようにすることにより、同一の反射対象物に関して生成されたものと推定される点の抽出を、より効率的かつより精度よく実施することが可能である。

【0059】

なお、補正点抽出部１７は、補正点生成部１５により生成された複数の第１時点の補正点および複数の第２時点の補正点の中から、水平座標が互いに第２距離以内にあり、かつ、補正後高度差が所定値以内（例えば、３ｍｍ以内）である第１時点の補正点および第２時点の補正点を抽出するようにしてもよい。このようにすれば、同一の反射対象物に関して生成されたものと推定される点の抽出を、更に効率的かつ更に精度よく実施することが可能である。

【0060】

なお、上記実施形態では、移動体の例としてドローン１００を挙げて説明したが、これに限定されない。また、上記実施形態では、光学式レーダの例としてＬｉＤＡＲ１０３を挙げ、位置姿勢検出装置の例としてＩＭＵ１０４を挙げて説明したが、これに限定されない。上記実施形態で説明した処理と同様の処理を行うことが可能な移動体や、光学式レーダまたは位置姿勢検出装置であれば、何れも用いることが可能である。

【0061】

また、上記実施形態では、補正後高度差が補正前高度差よりも小さくなる仮ピッチ角補正値をピッチ角補正値として特定する処理の一例として、補正後高度差が最小となる仮ピッチ角補正値を探索してそれをピッチ角補正値として特定する処理を説明したが、本発明はこれに限定されない。すなわち、探索される仮ピッチ角補正値が、必ずしも補正後高度差が最小となるものであることを必須とするものではない。

【0062】

その他、上記実施形態は、何れも本発明を実施するにあたっての具体化の一例を示したものに過ぎず、これによって本発明の技術的範囲が限定的に解釈されてはならないものである。すなわち、本発明はその要旨、またはその主要な特徴から逸脱することなく、様々な形で実施することができる。

【符号の説明】

【0063】

１ 姿勢角補正装置
１１ 検出データ取得部
１２ 仮補正値設定部
１３ 点群データ生成部
１４ 点抽出部
１５ 補正点生成部
１６ 補正値特定部
１７ 補正点抽出部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】