WO2018070486A1

WO2018070486A1 - 移動体の姿勢制御システム、立体マーカー

Info

Publication number: WO2018070486A1
Application number: PCT/JP2017/037041
Authority: WO
Inventors: 賢哉金田; 崇弘三木; 領此村
Original assignee: 本郷飛行機株式会社
Priority date: 2016-10-12
Filing date: 2017-10-12
Publication date: 2018-04-19
Also published as: JP6925603B2; JP2018063512A

Abstract

本発明は、新規且つ簡単な構成の移動体（例えば、小型無人飛行機）の自立姿勢制御システムを提供することを目的とする。更に、本発明は、この自立姿勢制御システムに使用して好適な新規且つ簡単な構成のマーカーを提供することを目的とする。　本発明に係る移動体（例えば、小型無人飛行機）の自立姿勢制御システムは、前記移動体に搭載された単眼カメラ及び姿勢制御装置と、所定の場所に設置された立体マーカーとを備え、前記姿勢制御装置は、前記単眼カメラが捉える前記立体マーカーの画像データに基づき前記移動体を自立姿勢制御する。

Description

移動体の姿勢制御システム、立体マーカー

　本発明は、移動体の姿勢制御システム、立体マーカーに関する。更に具体的には、本発明は、移動体として３次元空間を移動可能な小型無人飛行機（ドローン）の自立姿勢制御システム、立体マーカーに関する。

　近年、小型無人飛行機、典型的はドローンが、広い分野で利用されている。一般に、ドローンは、ＧＰＳ衛星からの電波をドローン搭載のＧＰＳセンサでとらえて位置データを算出し、更に、ドローン搭載のＩＭＵ（慣性計測装置）により飛行中の動き（速度、加速度等）や姿勢（傾き等）データを算出し、これら位置データ及び飛行データを利用者の操縦装置に送信している。利用者は、これらデータを参照しながら、ドローンに対して、移動命令及び姿勢制御命令等の飛行制御命令を送信して操縦制御している。

　ドローンの飛行制御に関し、従来、地上に反射マーカーを設置し、ドローンに赤外線高速カメラを搭載して、モーションキャプチャシステムを利用する方法が有る。赤外線高速カメラに代えて、単眼カメラ、立体カメラ、広範囲測定器等を利用したシステムも有る。

　なお、本発明者は、小型無人飛行機の姿勢制御に関して、先行技術文献として、次の非特許文献が存在することを承知している。

Ｄ．　Ｍｅｌｌｉｎｇｅｒ　ａｎｄ　Ｖ．　Ｋｕｍａｒ，　"Ｍｉｎｉｍｕｍ　ｓｎａｐ　ｔｒａｊｅｃｔｏｒｙ　ｇｅｎｅｒａｔｉｏｎ　ａｎｄ　ｃｏｎｔｒｏｌ　ｆｏｒ　ｑｕａｄｒｏｔｏｒｓ，"　ｉｎ　ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ，　Ｍａｙ　２０１１，　ｐｐ．２５２０－２５２５Ｍ．　Ｈｅｈｎ　ａｎｄ　Ｒ．　ＤＡｎｄｒｅａ，　Ｑｕａｄｃｏｐｔｅｒ　ｔｒａｊｅｃｔｏｒｙ　ａｎｄ　ｃｏｎｔｒｏｌ，　ｉｎ　Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　ｔｈｅ　ＩＦＡＣ　Ｗｏｒｌｄ　Ｃｏｎｇｒｅｓｓ，　２０１１Ｍｉｃｈａｅｌ，　Ｎａｔｈａｎ，　ｅｔ　ａｌ．　"Ｔｈｅ　ＧＲＡＳＰ　ｍｕｌｔｉｐｌｅ　ｍｉｃｒｏ－ＵＡＶ　ｔｅｓｔｂｅｄ．"　Ｒｏｂｏｔｉｃｓ　Ａｕｔｏｍａｔｉｏｎ　Ｍａｇａｚｉｎｅ，　ＩＥＥＥ　１７．３　（２０１０）：　５６－６５Ｈｅｎｇ，　Ｌｉｏｎｅｌ；　Ｍｅｉｓｅｒ，　Ｌ．；　Ｔａｎｓｋａｎｅｎ，　Ｐ．；　Ｆｒａｕｎｄｏｒｆｅｒ，　Ｆ．；　Ｐｏｌｌｅｆｅｙｓ，　Ｍ．，　"Ａｕｔｏｎｏｍｏｕｓ　ｏｂｓｔａｃｌｅ　ａｖｏｉｄａｎｃｅ　ａｎｄ　ｍａｎｅｕｖｅｒｉｎｇ　ｏｎ　ａ　ｖｉｓｉｏｎ－ｇｕｉｄｅｄ　ＭＡＶ　ｕｓｉｎｇ　ｏｎ－ｂｏａｒｄ　ｐｒｏｃｅｓｓｉｎｇ，"　ｉｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ　（ＩＣＲＡ），　２０１１　ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ，　ｖｏｌ．，　ｎｏ．，　ｐｐ．２４７２－２４７７，　９－１３　Ｍａｙ　２０１１Ｓｔｅｐｈａｎ　Ｗｅｉｓｓ，　Ｍａｒｋｕｓ　Ｗ．　Ａｃｈｔｅｌｉｋ，　Ｓｉｍｏｎ　Ｌｙｎｅｎ，　Ｍａｒｇａｒｉｔａ　Ｃｈｌｉ，　Ｒｏｌａｎｄ　Ｓｉｅｇｗａｒｔ，　"Ｒｅａｌ－ｔｉｍｅ　ｏｎｂｏａｒｄ　ｖｉｓｕａｌ－ｉｎｅｒｔｉａｌ　ｓｔａｔｅ　ｅｓｔｉｍａｔｉｏｎ　ａｎｄ　ｓｅｌｆ－ｃａｌｉｂｒａｔｉｏｎ　ｏｎ　ＭＡＶｓ　ｉｎ　ｕｎｋｎｏｗｎ　ｅｎｖｉｒｏｎｍｅｎｔｓ．"　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ　（ＩＣＲＡ），　２０１２　ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｐｐ．９５７，　Ｍａｙ　２０１２Ｓｈａｏｊｉｅ　ｓｈｅｎ；　Ｍｉｃｈａｅｌ，　Ｎ．；　Ｋｕｍａｒ，　Ｖ．，　"Ｔｉｇｈｔｌｙ－ｃｏｕｐｌｅｄ　ｍｏｎｏｃｕｌａｒ　ｖｉｓｕａｌ－ｉｎｅｒｔｉａｌ　ｆｕｓｉｏｎ　ｆｏｒ　ａｕｔｏｎｏｍｏｕｓ　ｆｌｉｇｈｔ　ｏｆ　ｒｏｔｏｒｃｒａｆｔ　ＭＡＶｓ，"　ｉｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ　（ＩＣＲＡ），　２０１５　ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ，　ｖｏｌ．，　ｎｏ．，　ｐｐ．５３０３－５３１０，　２６－３０　Ｍａｙ　２０１５Ｓ．　Ｇｒｚｏｎｋａ，　Ｇ．　Ｇｒｉｓｅｔｔｉ，　ａｎｄ　Ｗ．　Ｂｕｒｇａｒｄ，　"Ｔｏｗａｒｄｓ　ａ　ｎａｖｉｇａｔｉｏｎ　ｓｙｓｔｅｍ　ｆｏｒ　ａｕｔｏｎｏｍｏｕｓ　ｉｎｄｏｏｒ　ｆｌｙｉｎｇ，"　ｉｎ　ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ，　２００９Ｇ．　Ｘｕ，　Ｙ．　Ｚｈａｎｇ，　Ｓ．　Ｊｉ，　Ｙ．　Ｃｈｅｎｇ，　Ｙ．　Ｔｉａｎ，　Ｒｅｓｅａｒｃｈ　ｏｎ　ｃｏｍｐｕｔｅｒ　ｖｉｓｉｏｎ－ｂａｓｅｄ　ｆｏｒ　ＵＡＶ　ａｕｔｏｎｏｍｏｕｓ　ｌａｎｄｉｎｇ　ｏｎ　ａ　ｓｈｉｐ，　Ｐａｔｔｅｒｎ　Ｒｅｃｏｇｎｉｔｉｏｎ　Ｌｅｔｔｅｒｓ，　Ｖｏｌ．　３０（６），　２００９，　ｐｐ．６００－６０５Ｌａｎｇｅ，　Ｓ．；　Ｓｕｎｄｅｒｈａｕｆ，　Ｎ．；　Ｐｒｏｔｚｅｌ，　Ｐ．，　"Ａ　ｖｉｓｉｏｎ　ｂａｓｅｄ　ｏｎｂｏａｒｄ　ａｐｐｒｏａｃｈ　ｆｏｒ　ｌａｎｄｉｎｇ　ａｎｄ　ｐｏｓｉｔｉｏｎ　ｃｏｎｔｒｏｌ　ｏｆ　ａｎ　ａｕｔｏｎｏｍｏｕｓｅ　ｍｕｌｔｉｒｏｔｏｒ　ＵＡＶ　ｉｎ　ＧＰＳ－ｄｅｆｉｎｅｄ　ｅｎｖｉｒｏｎｍｅｎｔｓ"　ｉｎ　Ａｄｖａｎｃｅｄ　Ｒｏｂｏｔｉｃｓ，　２００９．　ＩＣＡＲ　２００９．　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ　，　ｖｏｌ．，　ｐｐ．１－６，　２２－２６　Ｊｕｎｅ　２００９Ｌｕｇｏ，　Ｊａｃｏｂｏ　Ｊｉｍｅｎｅｚ，　ａｎｄ　Ａｎｄｒｅｓ　Ｚｅｌｌ，　"Ｆｒａｍｅｗｏｒｋ　ｆｏｒ　ａｕｔｏｎｏｍｏｕｓ　ｏｎ－ｂｏａｒｄ　ｎａｖｉｇａｔｉｏｎ　ｗｉｔｈ　ｔｈｅ　ＡＲ．　Ｄｒｏｎｅ．"　Ｊｏｕｒｎａｌ　ｏｆ　Ｉｎｔｅｌｌｉｇｅｎｔ　ａｎｄ　Ｒｏｂｏｔｉｃ　Ｓｙｓｔｅｍｓ　７３．１－４（２０１４）：　４０１－４１２Ｔｒｏｉａｎｉ，　Ｃｈｉａｒａ，　Ｓｔｅｆａｎｏ　Ａｌ　Ｚａｎａｔｉ，　ａｎｄ　Ａｌｅｓｓｉｏ　Ｍａｒｔｉｎｅｌｉ．　"Ａ　３　ｐｏｉｎｔｓ　ｖｉｓｉｏｎ　ｂａｓｅｄ　ａｐｐｒｏａｃｈ　ｆｏｒ　ｍａｖ　ｌｏｃａｌｉｚａｔｉｏｎ　ｉｎ　ＧＰＳ　ｄｅｆｉｎｅｄ　ｅｎｖｉｒｏｎｍｅｎｔｓ．"　Ｍｏｂｉｌｅ　Ｒｏｂｏｔｓ　（ＥＣＭＲ），　２０１３　Ｅｕｒｏｐｅａｎ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ．　ＩＥＥＥ，　２０１３．Ｖｏｇｔ，　Ｓｅｂａｓｔｉａｎ，　ｅｔ　ａｌ．　"Ｓｉｎｇｌｅ　ｃａｍｅ　ｔｒａｃｋｉｎｇ　ｏｆ　ｍａｒｋｅｒ　ｃｌｕｓｔｅｒｓ：　Ｍｕｌｔｉｐａｒａｍｅｔｅｒ　ｃｌｕｓｔｅｒ　ｏｐｔｉｍｉｚａｔｉｏｎ　ａｎｄ　ｅｘｐｅｒｉｍｅｎｔａｌ　ｖｅｒｉｆｉｃａｔｉｏｎ"　Ｐｒｏｃｅｅｄｉｎｇｓ　ｏｆ　ｔｈｅ　１ｓｔ　ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｓｙｍｐｏｓｉｕｍ　ｏｎ　Ｍｉｘｅｄ　ａｎｄ　Ａｕｇｍｅｎｔｅｄ　Ｒｅａｌｉｔｙ．　ＩＥＥＥ　Ｃｏｍｐｕｔｅｒ　Ｓｏｃｉｅｔｙ，　２００２．Ｎｉｋｏｌｉｃ，　Ｊａｎｏｓｃｈ，　ｅｔ　ａｌ．　"Ａ　ｓｙｎｃｈｒｏｎｉｚｅｄ　ｖｉｓｕａｌ－ｉｎｅｒｔｉａｌ　ｓｅｎｓｏｒ　ｓｙｓｔｅｍ　ｗｉｔｈ　ＦＰＧＡ　ｐｒｅ－ｐｒｏｃｅｓｓｉｎｇ　ｆｏｒ　ａｃｃｕｒａｔｅ　ｒｅａｌ－ｔｉｍｅ　ＳＬＡＭ．"　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ　（ＩＣＲＡ），　２０１４　ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ．　ＩＥＥＥ，　２０１４Ｚｈｏｕ，　Ｇｕｙｕｅ，　ｅｔ　ａｌ．　"Ｏｎ－ｂｏａｒｄ　ｉｎｅｒｔｉａ－ａｓｓｉｓｔｅｄ　ｖｉｓｕａｌ　ｏｄｏｍｅｔｅｒ　ｏｎ　ａｎ　ｅｍｂｅｄｄｅｄ　ｓｙｓｔｅｍ．"　Ｒｏｂｏｔｉｃｓ　ａｎｄ　Ａｕｔｏｍａｔｉｏｎ　（ＩＣＲＡ），　２０１４　ＩＥＥＥ　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ．　ＩＥＥＥ，　２０４４．Ｃｈｅｎ，　Ｑｉａｎ，　Ｈａｉｙｕａｎ　Ｗｕ，　ａｎｄ　Ｔｏｓｈｉｋａｚｕ　Ｗａｄａ．　"Ｃａｍｅｒａ　ｃａｌｉｂｒａｔｉｏｎ　ｗｉｔｈ　ｔｗｏ　ａｒｂｉｔｒａｒｙ　ｃｏｐｌａｎａｒ　ｃｉｒｃｌｅｓ．"　Ｃｏｍｐｕｔｅｒ　Ｖｉｓｉｏｎ－ＥＣＶＶ　２００４．　Ｓｐｒｉｎｇｅｒ　Ｂｅｒｌｉｎ　Ｈｅｉｄｅｌｂｅｒｇ，　２００４．　５２１－５３２Ｅｌｑｕｒｓｈ，　Ａｌｉ，　ａｎｄ　Ａｈｍｅｄ　Ｅｌｇａｍｍａｌ．　"Ｌｉｎｅ－ｂａｓｅｄ　ｒｅｌａｔｉｖｅ　ｐｏｓｅ　ｅｓｔｉｍａｔｉｏｎ．"　Ｃｏｍｐｕｔｅｒ　Ｖｉｓｉｏｎ　ａｎｄ　Ｐａｔｔｅｒｎ　Ｒｅｃｏｇｎｉｔｉｏｎ　（ＣＶＰＲ），　２０１１　ＩＥＥＥ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ．　ＩＥＥＥＥ，　２０１１Ｚｈａｎｇ，　Ｚｈｅｎｇｙｏｕ．　"Ａ　ｆｌｅｘｉｂｌｅ　ｎｅｗ　ｔｅｃｈｎｉｑｕｅ　ｆｏｅ　ｃａｍｅｒａ　ｃａｌｉｂｒａｔｉｏｎ．"　Ｐａｔｔｅｒｎ　Ａｎａｌｙｓｉｓ　ａｎｄ　Ｍａｃｈｉｎｅ　Ｉｎｔｅｌｌｉｇｅｎｃｅ，　ＩＥＥＥ　Ｔｒａｎｓａｃｔｉｏｎ　ｏｎ　２２．１１　（２０００）：　１３３０－１３３４Ｅ．　Ｒｏｓｔｅｎ，　Ｒ．　Ｐｏｒｔｅｒ，　ａｎｄ　Ｔ．　Ｄｒｕｍｍｏｎｄ．　"Ｆａｓｔｅｒ　ａｎｄ　ｂｅｔｔｅｒ：　Ａ　ｍａｃｈｉｎｅ　ｌｅａｒｎｉｎｇ　ａｐｐｒｏａｃｈ　ｔｏ　ｃｏｒｎｅｒ　ｄｅｔｅｃｔｉｏｎ．"　ＩＥＥＥ　Ｔｒａｎｓ．　Ｐａｔｔｅｒｎ　Ａｎａｌｙｓｉｓ　ａｎｄ　Ｍａｃｈｉｎｅ　ｉｎｔｅｌｌｉｇｅｎｃｅ，　３２：１０５１１９，　２０１０Ｈａｒｒｉｓ，　Ｃｈｒｉｓ，　ａｎｄ　Ｍｉｋｅ　Ｓｔｅｐｈｅｎｓ．　"Ａ　ｃｏｍｂｉｎｅｄ　ｃｏｒｎｅｒ　ａｎｄ　ｅｄｇｅ　ｄｅｔｅｃｔｏｒ．"　Ａｌｖｅｙ　ｖｉｓｉｏｎ　ｃｏｎｆｅｒｅｎｃｅ．　Ｖｏｌ．１５．　１９８８Ｋｏｎｏｍｕｒａ，　Ｒｙｏ，　ａｎｄ　Ｋｏｉｃｈｉ　Ｈｏｒｉ．　"Ｐｈｏｅｎｉｘ：　Ｚｙｎｑ　７０００　ｂａｓｅｄ　ｑｕａｄｃｏｐｔｅｒ　ｒｏｂｏｔ．　ＲｅＣｏｎＦｉｇｕｒａｂｌｅ　Ｃｏｍｐｕｔｉｎｇ　ａｎｄ　ＦＰＧＡｓ　（ＲｅＣｏｎＦｉｇ），　２０１４　Ｉｎｔｅｒｎａｔｉｏｎａｌ　Ｃｏｎｆｅｒｅｎｃｅ　ｏｎ．　ＩＥＥＥ，　２０１４．

　従来提案されている姿勢制御システムは、反射マーカーと赤外線高速カメラを利用したモーションキャプチャシステムのように構成が比較的複雑であり、姿勢制御の誤差が比較的大きい等の問題が有った。また、小型無人飛行機に搭載する姿勢制御のハードウェアには、厳しい重量制限が有り、軽量化が求められる。

　そこで、本発明者は、従来の姿勢制御方法とは異なる新規且つ簡単な構成の小型無人飛行機の姿勢制御システムを提案する。

　具体的には、本発明は、新規且つ簡単な構成の移動体（例えば、小型無人飛行機）の自立姿勢制御システムを提供することを目的とする。

　更に、本発明は、移動体（例えば、小型無人飛行機）の自立姿勢制御システムに使用して好適な新規且つ簡単な構成のマーカーを提供することを目的とする。

　本発明に係る移動体の姿勢制御システムは、その発明の一面では、前記移動体に搭載された単眼カメラ及び姿勢制御装置と、所定の場所に設置された立体マーカーとを備え、前記姿勢制御装置は、前記単眼カメラが捉える前記立体マーカーの画像データに基づき前記移動体を自立姿勢制御している。

　更に、上記移動体の姿勢制御システムでは、前記移動体は、無人小型飛行機であってよい。

　更に、上記移動体の姿勢制御システムでは、前記立体マーカーは、少なくとも、１つの平面とその平面に含まれない他の平面又は点を有する形状から成るものでよい。

　更に、上記移動体の姿勢制御システムでは、前記姿勢制御装置は、前記単眼カメラが捉える前記立体マーカーの１つの平面と、その平面に含まれない他の平面又は点の画像データに関し、事前に判明している位置と、測定時の位置との差分に基づき座標化することにより三次元の姿勢制御を行っていてもよい。

　更に、上記移動体の姿勢制御システムでは、前記姿勢制御装置は、前記単眼カメラから立体マーカーの画像を取得し、取得画像をスカラー画像に変換し、該スカラー画像からカラー物体の輪郭を取得し、カラー物体の輪郭から立体マーカー及びサブ画素レベルで各頂点を検出してマーカー管理ユニットへ送り、平面の各頂点を使って最初の姿勢推定を行い、立体の各頂点を使ってバンドル調整を通じて対話型姿勢推定処理を行っていてもよい。

　更に、上記移動体の姿勢制御システムでは、前記立体マーカーは、円錐、三角錐、四角錐、その他多角錐のいずれかの形状のマーカーであってよい。

　更に、上記移動体の姿勢制御システムでは、前記立体マーカーは、四角錐形状であって、表面の４つの三角面の少なくとも一面の色を変えてマーカーを非対称に着色していてもよい。

　更に、上記移動体の姿勢制御システムでは、前記立体マーカーは、複数個の発光物で三次元形状を表すように配置されたマーカーであってよい。

　更に、上記移動体の姿勢制御システムでは、前記移動体は、産業用ロボットであってよい。

　更に、上記移動体の姿勢制御システムでは、前記移動体は、直線レール上を走行する物品移送用のトロッコであってよい。

　更に、本発明に係るマーカーは、その発明の一面では、三次元に移動可能な移動体の自立姿勢制御システムに使用される立体マーカーである。

　更に、上記立体マーカーでは、少なくとも、１つの平面とその平面に含まれない他の平面又は点を有する形状から成る立体マーカーである。

　本発明によれば、新規且つ簡単な構成の移動体（例えば、小型無人飛行機）の自立姿勢制御システムを提供することが出来る。

　更に、本発明によれば、移動体（例えば、小型無人飛行機）の自立姿勢制御システムに使用して好適な新規且つ簡単な構成のマーカーを提供することが出来る。

図１Ａは、本実施形態に係るドローンの自立姿勢制御システムのイメージを説明する図である。図１Ｂは、ドローンの６－Ｄｏｆ（６自由度）姿勢制御を説明する図である。図２Ａは、図１に示すドローンの自立姿勢制御システムにおいて、ドローンに搭載される姿勢制御装置のブロック図である。図２Ｂは、図１に示すドローンの自立姿勢制御システムにおいて使用されるマーカーの一例を示す図である。図３は、本実施形態に係る６－Ｄｏｆ姿勢計測のフローを示す図である。図４Ａは、二次元形状マーカーと三次元形状マーカーを比較するため、二次元形状マーカーが小さな移動ｔｘをしたときのマーカー上の１６ポイントを示す図である。図４Ｂは、二次元形状マーカーと三次元形状マーカーを比較するため、三次元形状マーカーが小さな移動ｔｘをしたときのマーカー上の１６ポイントを示す図である。図４Ｃは、二次元形状マーカーと三次元形状マーカーを比較するため、二次元形状マーカーが小さな回転θｙをしたときのマーカー上の１６ポイントを示す図である。図４Ｄは、二次元形状マーカーと三次元形状マーカーを比較するため、三次元形状マーカーが小さな回転θｙをしたときのマーカー上の１６ポイントを示す図である。

　以下、本発明に係る移動体の姿勢制御システムの実施形態に関し、移動体として３次元空間を移動可能な小型無人飛行機（ドローン）を例にとって、添付の図面に沿って説明する。なお、図面上、同じ参照符号は、同じ要素を示し、重複した説明を省略する。

　［ドローンの姿勢制御のイメージ］
　図１Ａは、本実施形態に係るドローンの自立姿勢制御システムのイメージを説明する図である。本実施例の特徴は、ドローンの自立姿勢制御システムが、簡単な構成である点にある。即ち、ドローン２は、予め、地上に設置されたマーカー６を、ドローン２に搭載の単眼（一眼）カメラ２０で撮影しながら、自立的に姿勢制御をしている。

　自立姿勢制御のために、図１Ａにおいて、破線で示す要素のＧＰＳ衛星１からのデータ、ドローン利用者３のドローン操縦機器４からのデータ等を必要としない、簡単な構成となっている。

　図１Ｂは、ドローン２の６－Ｄｏｆ（６自由度）姿勢制御を説明する図である。一般に、物体が三次元空間で取り得る動きの自由度を６－Ｄｏｆ（６自由度）といい、具体的には、物体が前又は後、上又は下、左又は右に移動できること（換言すれば、３次元の各直交座標軸に沿って移動できること）、また直交座標軸の各軸の周りを回転できることをいう。この回転に関しては、ドローン２のような航空機では、ピッチング、ヨーイング、ローリングが用いられる。即ち、ピッチングは、ドローン２の左右を軸として（所謂、上下に）回転することをいう。ヨーイングは、ドローン２の上下を軸として（所謂、水平面内で）回転することをいう。ローリングは、ドローン２の前後の軸に対して回転（或いは傾斜）することをいう。

　［自立姿勢制御システム］
　（ハードウェア）
　図２Ａは、図１Ａに示すドローン２の自立姿勢制御システムにおいて、ドローン２に搭載される姿勢制御装置８のブロック図である。姿勢制御装置８は、基板１０の姿勢制御モジュール１２に実装された単眼カメラ２０と、１個のＣＰＵ１６と、メモリ１８とを備えている。これらをＦＰＧＡで構成してもよい。姿勢制御モジュール１２は、従来型のドローン２に後付けで搭載させることができる。

　三軸ジャイロセンサ１４は、本実施形態の効果を検証するために搭載されたものであり、本実施形態にとって必須な要素ではない。しかし、所望により、三軸ジャイロセンサ１４を搭載してもよい。

　従来、ドローン２の重力方向に対するローリング角度及びピッチング角度を計測するため、加速度計は不可欠であった。しかし、本実施形態の６－Ｄｏｆ姿勢計測では、姿勢制御装置８に加速度計を備えずに、ドローン２の安定した飛行姿勢及び位置の飛行制御が可能となった。

　単眼（一眼）カメラ２０は、１個のレンズのカメラである。通常、距離測定、３Ｄ写真の撮影には視差を利用するため、２個のレンズを使用する双眼（二眼）カメラが必要である。本実施形態では、後述するマーカー６と姿勢計測アルゴリズムを工夫することにより、単眼カメラでの姿勢制御が可能となった。

　メモリ１８は、姿勢制御のアルゴリズムを書き込む記憶領域と、ＣＰＵ１６の演算処理の作業領域とを有している。

　なお、ドローン２には、本実施例の姿勢測定の効果を測定するに際し、基準データを得るためにモーションキャプチャ装置も搭載している。

　（マーカーの形状、設計及び設置場所）
　本実施形態で使用するマーカー６は、「立体マーカー」である。本出願書類では、「立体マーカー」は、少なくとも、１つの平面とその平面に含まれない他の平面又は点を有する形状から成るものをいう。代表的には、三次元形状のマーカーである。

　好ましくは、図２Ｂに示すような四角錐形状のマーカー６である。四角錐形状のマーカー６は、平面６ａ，６ｂ，６ｃ，６ｄと、平面６ａ，６ｂ，６ｅと、平面６ｂ，６ｃ，６ｅと、平面６ｃ，６ｄ，６ｅと、平面６ｄ，６ａ，６ｅとの組み合わせである。見方を変えると、四角錐形状のマーカー６は、平面６ａ，６ｂ，６ｃ，６ｄと、点６ｅと組み合わせた形状となっている。
　マーカー６の形状は、四角錐に限定されない。円錐、多角錐でもよい。

　また、ＬＥＤ等の発光物で非同一平面を表現してもよい。

　マーカー６の個数は、１個に限定されない。２個以上であってもよい。従って、同一平面でない平面上に、２個の平面形状のマーカー（以下「平面マーカー」と呼ぶ）６を夫々設置し、２個の平面マーカー６を全体として「立体マーカー」６としてもよい。

　四角錐形状のマーカー６では、表面の４つの三角面の少なくとも一面の色を変えてマーカー６を非対称にすることにより、ヨー回転方向の向きを区別することが出来る。

　マーカー６の設置場所は、地上、空中等何れであってもよい。リアルタイムで位置が明らかであれば、車上に時系列で移動してもよい。車上に設置して、ドローン２が追従することも出来る。

　マーカー６の開発過程において、マーカー６の形状に関し、平面形状（二次元形態）と立体形状（三次元形状）とを比較検討した。最終的に、マーカー６の形状は、姿勢制御の推定誤差が少ない立体形状とし、好ましくは、四角錐とした。その理由は後で説明する。

　（６－Ｄｏｆアルゴリズム）
　この６－Ｄｏｆアルゴリズムの内容は、例えば、四角錐形状のマーカー６を特定する５つの頂点（底辺の正方形の４点と頂点の１点）に関して、事前に判明している位置と、測定時の位置との差分に基づき座標化することにある。ここで、二次元を写す単眼（一眼）カメラ２０であっても、四角錐の正方形の４点内にある頂点の挙動により、三次元の姿勢制御が可能となる。

　最初に、本実施形態で使用された姿勢の記号及び方法を開示する。ベクトルｕｉ＝［ｕｉ，ｖｉ］Ｔは、画像座標（０≦ｕｉ＜Ｈ及び０≦ｖｉ≦Ｖ）のｉ番目の位置である。Ｘｉ∈Ｒ３は、ワールド座標である。Ｔ＝［ｔｘ，ｔｙ，ｔｚ］Ｔ∈Ｒ３及びＲ∈ＳＯ（３）は、相対的な単眼カメラ２０の移動及び回転である。Ｔｚを最初の単眼カメラ２０の方向と定義し、θ＝［θｘ，θｙ，θｚ］をＲのＺ－Ｘ－Ｙオイラー角と定義する。次に、単一の画像シーンの再投影誤差のＬ２基準は、次のように定義出来る。

　マーカー６の開発過程において、マーカー６の形状に関し、平面形状（二次元形態）と立体形状（三次元形状）とを比較検討した。最終的にマーカー６の形状は、姿勢制御の推定誤差が少ない「立体形状」とし、好ましくは、四角錐とした。その理由を簡単に説明する。

　単眼カメラ２０で撮影した同一平面マーカー（二次元形状マーカー）６と非同一平面マーカー（三次元形状マーカー）６の相違を説明するため、簡単な例を示す。図４Ａ～図４Ｄは、単眼カメラ２０を２つの方法、小さな移動ｔｘ（図４Ａ及び図４Ｂ）と小さな回転θｙ（図４Ｃ及び図４Ｄ）で移動したときのマーカー６の１６ポイントを示している。二次元形状のマーカー６では、並進運動と回転運動では僅かな相違しか見られない。しかし、三次元形状のマーカー６では、他の同一平面の１５ポイントと奥行きが異なる１つのポイント（◎）が有ることから、これら移動ｔｘ　及び回転θｙの変化の相違は明瞭に分かる。

　　（６－Ｄｏｆ姿勢計測のフロー）
　図３は、本実施形態に係る６－Ｄｏｆ姿勢計測のフローを示す図である。この手順は、予めメモリ１８に書き込まれたアルゴリズムに従って、ＣＰＵ１６により処理される。

　ステップＳ０１で、単眼カメラ２０からマーカー６を含む画像を取得する。

　ステップＳ０２で、取得画像はスカラー画像に変換される。

　ステップＳ０３で、スカラー画像からカラー物体の輪郭を得て、ＣＰＵ１６に送られる。

　ステップＳ０４で、ＣＰＵ１６により、カラー物体の輪郭からマーカー６及びサブ画素レベルで各頂点６ａ～６ｅを検出する。

　ステップＳ０５で、マーカー６及び各頂点６ａ～６ｅをマーカー管理ユニット（図示せず）へ送られる。

　ステップＳ０６で、マーカー６の平面の各頂点６ａ～６ｄを使って、最初の姿勢推定を行う。

　ステップＳ０７で、マーカー６の立体の各頂点６ａ～６ｅを使ってバンドル調整を通じて対話型姿勢推定を行う。

　　［検証実験：クワッドコプターによる飛行制御］
　６－Ｄｏｆアルゴリズムの内容では、四角錐の頂点である５点により姿勢計測を行うと説明したが、以下の実験ではこの５点を含む１６点で検証している。検証実験では、同一平面形状として正方形を選択し、立体形状として図２Ｂに示す四角錐を選択した。

　２つのタイプのマーカー６は、地面に置かれ、姿勢推定を行うため地面に対して固定されている。平面マーカー６のサイズは１９ｃｍ×１９ｃｍ×０．３ｃｍであり、立体マーカー６のサイズは１９ｃｍ×１９ｃｍ×１２ｃｍであった。
　検証のための６－ＤｏＦ姿勢の正しいデータは、モーションキャプチャ装置を使用したデータを用いた。

　本発明者等は、単一のマーカー６を使って小型無人飛行機を移動して６－ＤｏＦ姿勢推定の結果を記録した。平面マーカー６の場合、θｘ，θｙ，ｔｘ，ｔｙ推定に対して大きな白色雑音（ホワイトノイズ）が発生していた。しかし、これら白色雑音は、立体マーカー６の場合には大幅に改善することが出来た。

　立体マーカー６を２個の使った場合と１個使った場合との６－ＤｏＦ姿勢推定の比較も行った。この実験では、２個のマーカー６の位置関係（距離２８ｃｍ）は、事前に姿勢制御装置８に登録してある。２個の立体マーカー６を使った場合、更に、白色雑音は減少し、精度よく推定できることが確認された。

　　［本実施形態の利点・効果］
　（１）　三次元空間を自由に移動可能な移動体（小型無人飛行機）の単眼カメラと立体マーカー６を使った姿勢推定制御により、移動体の自立飛行が可能となった。
　（２）　三次元空間を自由に移動可能な移動体（小型無人飛行機）の単眼カメラを使った姿勢推定で、マーカー６の形状は、平面形状よりも立体形状の方が優位であること立証することが出来た。

　　［変形例等］
　（１）本発明の実施形態を、移動体として小型無人飛行機を例に挙げて説明した。しかし、本発明の姿勢制御は、これに限定されない。移動体として、小型無人飛行機は三次元空間（ＸＹＺ方向）を自由に移動する。

　二次元空間を移動する産業用ロボットに関しても、本発明の姿勢制御は応用できる。この場合、床等からの高さ方法（Ｚ方向）の制御は、他の手段で正確に測定制御出来るので、Ｚ方向の制御部分は省略され、二次元（ＸＹ方向）の制御となる。

　更に、一次元空間を移動する、例えば、工場内の直線レール上を走行する物品移送用のトロッコに関しても、本発明の姿勢制御は応用できる。この場合、ＹＺ方向の制御部分は省略され、一次元（Ｘ方向）の制御となる。

　（２）マーカー６をＧＰＳと連動させることで、ロボットとマーカー６の相対位置表現を、ＧＰＳ座標系に変換することも出来る。

　１：ＧＰＳ衛星、　２：小型無人飛行機，ドローン、　３：ドローン利用者、　４：ドローン操縦機器、　６：マーカー，平面マーカー，立体マーカー、　８：姿勢制御装置、　１０：基板、　１２：姿勢制御モジュール、　１４：三軸ジャイロセンサ、　１６：ＣＰＵ、　１８：メモリ、　２０：単眼カメラ、一眼カメラ

Claims

　移動体の姿勢制御システムにおいて、
　前記移動体に搭載された単眼カメラ及び姿勢制御装置と、
　所定の場所に設置された立体マーカーと、を備え、
　前記姿勢制御装置は、前記単眼カメラが捉える前記立体マーカーの画像データに基づき前記移動体を自立姿勢制御する、姿勢制御システム。
　請求項１記載の移動体の姿勢制御システムにおいて、
　前記移動体は、無人小型飛行機である、姿勢制御システム。
　請求項１又は２記載の移動体の姿勢制御システムにおいて、
　前記立体マーカーは、少なくとも、１つの平面とその平面に含まれない他の平面又は点を有する形状から成る、姿勢制御システム。
　請求項１～３のいずれか一項記載の移動体の姿勢制御システムにおいて、
　前記姿勢制御装置は、前記単眼カメラが捉える前記立体マーカーの１つの平面と、その平面に含まれない他の平面又は点の画像データに関し、事前に判明している位置と、測定時の位置との差分に基づき座標化することにより三次元の姿勢制御を行っている、姿勢制御システム。
　請求項４記載の移動体の姿勢制御システムにおいて、
　前記姿勢制御装置は、前記単眼カメラから立体マーカーの画像を取得し、取得画像をスカラー画像に変換し、該スカラー画像からカラー物体の輪郭を取得し、カラー物体の輪郭から立体マーカー及びサブ画素レベルで各頂点を検出してマーカー管理ユニットへ送り、平面の各頂点を使って最初の姿勢推定を行い、立体の各頂点を使ってバンドル調整を通じて対話型姿勢推定処理を行っている、姿勢制御システム。
　請求項１～５のいずれか一項記載の移動体の姿勢制御システムにおいて、
　前記立体マーカーは、円錐、三角錐、四角錐、その他多角錐のいずれかの形状のマーカーである、姿勢制御システム。
　請求項１～６のいずれか一項記載の移動体の姿勢制御システムにおいて、
　前記立体マーカーは、四角錐形状であって、表面の４つの三角面の少なくとも一面の色を変えてマーカーを非対称に着色している、姿勢制御システム。
　請求項１～７のいずれか一項記載の移動体の姿勢制御システムにおいて、
　前記立体マーカーは、複数個の発光物で三次元形状を表すように配置されたマーカーである、姿勢制御システム。
　請求項１又は４記載の移動体の姿勢制御システムにおいて、
　前記移動体は、産業用ロボットである、姿勢制御システム。
　請求項１又は４記載の移動体の姿勢制御システムにおいて、
　前記移動体は、直線レール上を走行する物品移送用のトロッコである、姿勢制御システム。
　三次元に移動可能な移動体の自立姿勢制御システムに使用される立体マーカー。
　請求項１１記載の立体マーカーは、少なくとも、１つの平面とその平面に含まれない他の平面又は点を有する形状から成る、立体マーカー。