特開 2023-125675 位置推定装置

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

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

(11)【公開番号】P2023125675

(43)【公開日】2023-09-07

(54)【発明の名称】位置推定装置

(51)【国際特許分類】

   G01B 11/00 20060101AFI20230831BHJP

   G06T 7/70 20170101ALI20230831BHJP

【ＦＩ】

G01B11/00 H

G01B11/00 B

G06T7/70 Z

【審査請求】未請求

【請求項の数】5

【出願形態】ＯＬ

(21)【出願番号】P 2022029914

(22)【出願日】2022-02-28

【新規性喪失の例外の表示】特許法第３０条第２項適用申請有り ・ウェブサイトのアドレス ｈｔｔｐｓ：／／ｗｗｗ．ｙｏｕｔｕｂｅ．ｃｏｍ／ｗａｔｃｈ？ｖ＝Ｊ３ｐ６ｗｕＦｚＸ４０ 掲載日 令和３年９月１０日 公開者 中野 可也

(71)【出願人】

【識別番号】000191009

【氏名又は名称】新東工業株式会社

(74)【代理人】

【識別番号】110000338

【氏名又は名称】弁理士法人 ＨＡＲＡＫＥＮＺＯ ＷＯＲＬＤ ＰＡＴＥＮＴ ＆ ＴＲＡＤＥＭＡＲＫ

(72)【発明者】

【氏名】中野 可也

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

2F065

5L096

【Ｆターム（参考）】

2F065AA04

2F065AA06

2F065AA17

2F065BB28

2F065DD03

2F065FF04

2F065FF11

2F065FF41

2F065FF61

2F065GG04

2F065JJ03

2F065JJ26

2F065QQ24

2F065QQ28

2F065QQ31

2F065UU05

5L096BA05

5L096CA02

5L096FA66

5L096FA67

5L096FA69

(57)【要約】

【課題】マーカが付された対象物の位置を推定する精度の向上を図る位置推定装置を提供する。
【解決手段】対象物（Ｏ）の位置を推定する位置推定装置（２０）であって、第１の測定装置（１０）からの、マーカ（Ｍ）の詳細画像及び深度画像から、マーカ（Ｍ）の４隅の位置情報のうちの２つ以上の位置情報及びマーカ（Ｍ）の深度情報を特定することでマーカ（Ｍ）を検出し、２つ以上の位置情報からマーカ（Ｍ）の第１の測定装置に対するベクトル情報である並進情報及び回転情報を算出することで検出されたマーカ（Ｍ）の位置を推定し、並進情報から第１の測定装置とマーカ（Ｍ）との平面距離を算出し、回転情報から第１の測定装置に対するマーカ（Ｍ）の方位を算出し、マーカ（Ｍ）の深度情報から奥行における距離である深度距離を算出する。
【選択図】図３

【特許請求の範囲】

【請求項1】

プロセッサと第１のメモリとを備え、対象物の位置を推定する位置推定装置であって、
前記プロセッサは、前記第１のメモリに展開されたプログラムによって、詳細画像を撮像する第１の撮像器と深度画像を撮像する第１の深度測定器とを備える第１の測定装置からの同期された前記詳細画像及び前記深度画像を処理し、
同期された前記詳細画像及び前記深度画像には前記対象物に付された所定の条件を満たすマーカが撮像され、
前記詳細画像から前記マーカの４隅の位置情報のうちの２つ以上の位置情報及び前記マーカの深度情報を特定することで前記マーカを検出するマーカ検出ステップと、
前記マーカの４隅の位置情報のうちの２つ以上の位置情報から前記マーカの前記第１の測定装置に対するベクトル情報である並進情報及び回転情報を算出することで検出された前記マーカの位置を推定するマーカ位置推定ステップと、
前記並進情報から前記第１の測定装置と前記マーカとの平面における距離である平面距離を算出する平面位置算出ステップと、
前記回転情報から前記第１の測定装置に対する前記マーカの方位を算出する方位算出ステップと、
前記マーカの前記深度情報から奥行における距離である深度距離を算出する中心深度算出ステップと、を有するフレーム関連処理、
を実行する、位置推定装置。

【請求項2】

前記プロセッサは、
フレーム関連処理によって出力される前記平面距離より算出される平面座標及び前記深度距離より算出される深度座標から構成される出力情報が所定の条件を満たす場合、正常出力情報とし、
所定数以上の正常出力情報の平均を平均正常出力情報として、出力する、
請求項１に記載の位置推定装置。

【請求項3】

前記プロセッサは、
前記第１の測定装置及び第２の測定装置に同期設定指示を行い、前記第１の測定装置及び前記第２の測定装置に対してそれぞれ順に前記フレーム関連処理を実行する、
請求項１に記載の位置推定装置。

【請求項4】

前記所定の条件を満たすマーカは、中央に表示される表示パターンが５×５～７×７マスであって、２値で表され、１０００個までとする、
請求項１に記載の位置推定装置。

【請求項5】

前記フレーム関連処理によって出力される前記平面距離より算出される平面座標及び前記深度距離より算出される深度座標から構成される出力座標は、表示画面において、前記対象物の近傍に表示される、
請求項１に記載の位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、位置推定装置に関する。

【背景技術】

【0002】

例えば、特許文献１には、撮像手段によって撮像された撮像画面からマーカを検出し、検出されたマーカに基づいて位置情報を取得する技術が開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】国際公開ＷＯ２０２０／０１３３３７号公報

【発明の概要】

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

【0004】

このように取得された位置情報に基づいて、例えば、ロボットに対象物を把持させる場合、ロボットが把持可能な位置を高精度で推定する必要がある。しかしながら、上述のような従来技術では取得された位置の精度が、例えば、ロボットに対象物を把持させるために使用する位置の精度などとしては、十分でない場合がある。

【0005】

本発明の一態様は、推定する位置の精度向上を図った位置推定装置を実現することを目的とする。

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

【0006】

プロセッサと第１のメモリとを備え、対象物の位置を推定する位置推定装置であって、プロセッサは、第１のメモリに展開されたプログラムによって、詳細画像を撮像する第１の撮像器と深度画像を撮像する第１の深度測定器とを備える第１の測定装置からのフレーム関連に同期された詳細画像及び深度画像を処理し、同期された詳細画像及び深度画像には対象物に付されたマーカが撮像され、詳細画像からマーカの４隅の位置情報のうちの２つ以上の位置情報及びマーカの深度情報を特定することでマーカを検出するマーカ検出ステップと、マーカの４隅の位置情報のうちの２つ以上の位置情報からマーカの第１の測定装置に対するベクトル情報である並進情報及び回転情報を算出することで検出されたマーカの位置を推定するマーカ位置推定ステップと、並進情報から第１の測定装置とマーカとの平面における距離である平面距離を算出する平面位置算出ステップと、回転情報から第１の測定装置に対するマーカの方位を算出する方位算出ステップと、マーカの深度情報から奥行における距離である深度距離を算出する中心深度算出ステップと、を有するフレーム関連処理、を実行する、位置推定装置を提供する。

【発明の効果】

【0007】

本発明の一態様によれば、マーカが付された対象物の位置を推定する精度の向上を図る位置推定装置を実現できる。

【図面の簡単な説明】

【0008】

【図1】本実施の形態に係る位置推定システムの構成を表すハードウェアブロック図である。

【図2】本実施の形態に係るマーカの一例を表す図である。

【図3】位置推定処理の一例を表すフローチャートである。

【図4】位置推定処理の各ステップをブロックとして表したソフトウェアブロック図である。

【図5】位置推定処理のシーケンス図である。

【図6】平均化処理の一例を表すフローチャートである。

【図7】測定装置を複数備える場合の位置推定システムを表す概略図である。

【図8】測定装置を複数備える場合の位置推定処理の一例を表すフローチャートである。

【図9】出力装置における表示画面を表す図である。

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

【0009】

（本実施の形態）
以下、本発明の一実施の形態について、詳細に説明する。図１は、本実施の形態に係る位置推定システムを表すブロック図である。位置推定システムは、測定装置１０、位置推定装置２０及び出力装置３０を有する。測定装置１０は、例えば、後述の測定装置１０ａ（以下、これを第１の測定装置と呼んでもよい）とする。

【0010】

測定装置１０は、撮像器１１、深度測定器１２、プロセッサ１３、メモリ１４、入出力インタフェース１５及びバス１６を有し、マーカＭの例えば詳細画像及び深度画像を取得する。撮像器１１、深度測定器１２、プロセッサ１３、メモリ１４、入出力インタフェース１５は、バス１６を介して相互に接続されている。特に注記がない場合マーカＭは１つとする。

【0011】

撮像器１１は、例えば、イメージセンサ、より具体的には、ＣＭＯＳカメラであり、マーカＭからの反射光Ｌ０に基づき、マーカＭの詳細画像を撮像する。深度測定器１２は、例えば、深度センサ、より具体的には、ＴｏＦ（Time of Flight）であり、マーカＭの深度画像を測定する。測定装置１０は、例えば、解像度がｗ１２８０ｈ７２０である、Ｉｎｔｅｌ社のＬｉＤＡＲである、ＲＥＡＬＳＥＮＳＥＬ５１５とする。

【0012】

詳細画像は主に画像における平面の情報である平面情報を表すが画像における奥行を示す深度情報も輝度などとして含むものとし、深度画像は主に深度情報を表すが平面情報も含むものとする。

【0013】

深度情報は、具体的には、マーカＭと測定装置１０（深度測定器１２）との間の距離の分布（三次元的な距離）を表し、深度測定器１２から見た、マーカＭの三次元的な形状に対応する。

【0014】

深度測定器１２は、例えば、電磁波Ｌｒ（ビーコン、例えば、レーザー光）を照射し、マーカＭで反射された電磁波Ｌｒに基づいて、測定装置１０（深度測定器１２）とマーカＭの距離の分布を測定する。すなわち、測定装置１０（深度測定器１２）は、マーカＭに電磁波Ｌｒを照射して、マーカＭから反射された電磁波Ｌｒに基づいて、深度情報を導出する。

【0015】

メモリ１４には、プログラムＰ１が例えば不揮発性の記憶として格納されている。プロセッサ１３は、プログラムＰ１に含まれる命令に従って、マーカＭの平面画像及び深度情報の取得に係る処理を実行する。

【0016】

プロセッサ１３は、撮像器１１及び深度測定器１２から詳細画像及び深度画像を取得し、詳細画像及び深度画像を同期させ、例えば、１フレーム毎に入出力インタフェース１５を介して、位置推定装置２０に出力する。

【0017】

例えば、測定装置１０では、１／６０秒や１／３０秒といった所定時間毎に１フレーム毎の処理が行われる。なお別フレーム間の詳細画像と深度画像とを同期してもよいものとする。

【0018】

ここで、平面情報及び深度情報は、測定時座標で表すことができる。測定時座標は、測定装置１０を基準とするＸＹＺ座標とする。そのＺ軸は、測定装置１０からマーカＭに向かう方向の軸であり、ＸＹ平面は、Ｚ軸に垂直な平面である。

【0019】

ここで、プロセッサ１３は、次の画角の調整及び深度情報の補完を行ってもよい。画角調整は、撮像器１１と深度測定器１２との画角が一致するように調整（ａｌｉｇｎ）することを意味する。

【0020】

すなわち、詳細画像と深度画像の範囲（画角）、または解像度が異なる場合に、範囲または解像度の調整がなされる。これにより、詳細画像と、深度画像とを対応させることができる。具体的には、平面情報と平面情報の場所での深度情報との対応関係を明確化できる。

【0021】

深度情報の補完は、マーカＭの一部での反射不良などによって、深度情報の一部が欠落する場合に、深度情報を補完することを意味する。すなわち、深度情報が欠落した箇所の周りの深度情報から補完したり、この箇所付近の深度情報を平滑化したりすることで、深度情報が補完される。

【0022】

プロセッサ１３は、例えば、ＣＰＵ（Central Processing Unit）とする。また、メモリ１４は、例えば、ＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）とする。

【0023】

また、入出力インタフェース１５は、例えば、ＰＣＩ（Peripheral Component Interconnect）インタフェースとする。入出力インタフェース１５には、位置推定装置２０が接続される。

【0024】

位置推定装置２０は、対象物Ｏの位置を推定する装置であって、例えば、汎用コンピュータを用いて実現されており、図１に示すように、プロセッサ２１と、第１のメモリ（一次メモリと呼んでもよい）２２と、第２のメモリ（二次メモリと呼んでもよい）２３と、入出力インタフェース２４と、バス２５とを備えている。プロセッサ２１、第１のメモリ２２、第２のメモリ２３、及び入出力インタフェース２４は、バス２５を介して相互に接続されている。

【0025】

第２のメモリ２３には、プログラムＰ２及びテーブルＴが例えば不揮発性の記憶として格納されている。プロセッサ２１は、第２のメモリ２３に格納されているプログラムＰ２を第１のメモリ２２上に展開する。

【0026】

プロセッサ２１は、第１のメモリ２２上に展開されたプログラムＰ１に含まれる命令に従って、位置推定処理として、測定装置１０の、例えば、１フレーム毎の処理に対応して行われる１フレーム毎の処理である後述するフレーム関連処理Ｓ１０を実行する。広義にはフレーム関連処理Ｓ１０には測定装置１０のフレームに関連する処理も含めてよいものとする。

【0027】

テーブルＴとして、位置推定に用いるための、サイズ、中央に表示される表示パターン（以下、これを単に表示パターンと呼んでもよい）の情報などが保持される。テーブルＴとしてさらにマーカＭのＩＤ（マーカＩＤ）が保持されてもよい。

【0028】

例えば、マーカＩＤは、後述の、対象物Ｏの名称や対象物Ｏに関しての注釈（例えば対象物Ｏの消費期限）などの組み合わせの情報と１対１で結び付けられ、テーブルＴとして保存される。

【0029】

入出力インタフェース２４には、測定装置１０及び出力装置３０が接続される。なおテーブルＴは、表示パターンを枠である黒のマスまで含めて保持していてもよいものとする。

【0030】

プロセッサ２１は、例えば、ＣＰＵとする。また、第１のメモリ２２は、例えば揮発性のメモリであって、例えば、ＲＡＭ（Random Access Memory）とする。また、第２のメモリ２３は、例えば、ＨＤＤ（Hard Disk Drive）やＳＳＤ（Solid State Drive）とする。また、入出力インタフェース２４は、例えば、ＰＣＩインタフェースとする。

【0031】

出力装置３０は、例えば、後述のロボットアームＡ、画像表示装置Ｂ、音声出力装置であり、位置推定装置２０によって、制御される。なお例えば、位置推定装置２０は、測定装置１０を含んでもよく、プロセッサ２１はプロセッサ１３を、第１のメモリ２２や第２のメモリ２３はメモリ１４を、含んでもよい。

【0032】

図２は、本実施の形態に係るマーカＭの一例を表す図である。マーカＭは、例えば所定の条件を満たすものとし、例えば、ＡＲマーカ（一例として、ＡｒＵｃｏマーカ）であり、対象物Ｏに付して用いられる。マーカＭは、２値で表され、例えば、複数の白または黒のマスが縦横、２次元に配置される。

【0033】

ここでは、表示パターンである７×７のマスが、マーカＭの実体であり、マーカＭを識別するための辞書データとして機能する。ここでは、７×７のマスの周りに、枠として、黒のマスが配置され、見かけ上、マーカＭは、９×９のマスから構成される。

【0034】

マーカＭの表示パターンは、回転非対称性を有し、マーカＭの方位（姿勢、例えば、ロール角φ、ピッチ角θ、ヨー角ψ）の算出に用いることができる。マーカＭにはエラー耐性を示すハミング距離があり、最小のハミング距離である最小ハミング距離はより大きいことが望ましい。

【0035】

表示パターンは、７×７のマスには限定されず、例えば、５×５～７×７のマスであってもよい。またマーカＭは、例えば、サイズは、一辺が３～１００ｍｍである正方形であり、２値で表されることが好ましいが、形状は正方形でもなくてもよく、例えば、縦横比が１：２や２：１などでもよい。このようにマーカＭはサイズや表示パターンにおいて所定の条件を満たすものとする。

【0036】

また、例えば、マーカＭと測定装置１０との距離は２０ｃｍ～９ｍであることが好ましく、識別可能なマーカＭは１０００個までが好ましいものとする。このため位置推定装置２０は同時に最大で１０００個のマーカＭの位置を推定することが可能となり、１０００個の対象物Ｏを認識することが可能となる。

【0037】

なおマーカＭが鮮明に取得できているかは、詳細画像に撮像されたマーカＭに対して、例えば射影変換を利用して歪をただしサイズ調整をして、ラプラシアンフィルタで処理した結果の分散を算出した結果が所定の条件を満たすことで判断する。

【0038】

図３は、位置推定装置２０による位置推定処理（フレーム関連処理Ｓ１０）の一例を表すフローチャートである。フレーム関連処理Ｓ１０は、例えば、プロセッサ２１によって実行される処理であって、マーカ検出ステップＳ１１、マーカ位置推定ステップＳ１２、平面位置算出ステップＳ１３、方位算出ステップＳ１４及び中心深度算出ステップＳ１６を有する。さらにフレーム関連処理Ｓ１０は、中心座標算出ステップＳ１５を有してもよい。

【0039】

位置推定装置２０は、詳細画像を撮像する第１の撮像器１１（単に撮像器１１と呼んでもよい）と深度画像を撮像する第１の深度測定器１２（単に深度測定器１２と呼んでもよい）とを備える第１の測定装置１０ａからの、例えば、フレーム毎に同期された詳細画像及び深度画像を処理する。なお、例えば、同期された詳細画像及び深度画像には対象物Ｏに付されたマーカＭが撮像される。

【0040】

プロセッサ２１は、詳細画像から平面情報であるマーカＭの４隅の位置情報（Ｘ１，Ｙ１）～（Ｘ４，Ｙ４）のうちの２つ以上の位置情報及びマーカＭの深度情報を特定することでマーカＭを検出する（マーカ検出ステップＳ１１）。

【0041】

なおマーカＭの４隅の位置情報（Ｘ１，Ｙ１）～（Ｘ４，Ｙ４）のうちの２つ以上の位置情報とは４隅全てであってもよいし、対角線上にある２つの位置情報などであってもよい。また、マーカ検出ステップＳ１１では、さらにマーカＩＤも特定されてもよいものとする。

【0042】

プロセッサ２１は、マーカＭの４隅の位置情報（Ｘ１，Ｙ１）～（Ｘ４，Ｙ４）のうちの２つ以上の位置情報からマーカＭの第１の測定装置１０ａに対するベクトル情報である並進情報及び回転情報を算出することで検出されたマーカＭの位置を推定する（マーカ位置推定ステップＳ１２）。

【0043】

プロセッサ２１は、並進情報から第１の測定装置１０ａとマーカＭとの平面における距離である平面距離を算出する（平面位置算出ステップＳ１３）。プロセッサ２１は、回転情報から第１の測定装置に対するマーカＭの方位を算出する（方位算出ステップＳ１４）。

【0044】

プロセッサ２１は、マーカＭの深度情報から奥行における距離である深度距離を算出し（中心深度算出ステップＳ１６）、フレーム関連処理Ｓ１０を終了する。例えば、中心深度算出ステップＳ１６において、プロセッサ２１は、４隅のうちの２つ以上の深度情報を平均することで深度距離を算出する。

【0045】

また他の例としては、中心深度算出ステップＳ１６において、プロセッサ２１は、４隅のうちの１つの深度情報と方位とを用いて１つの深度情報を中心方向にオフセットすることで深度距離を算出する。

【0046】

なお、中心深度算出ステップＳ１６において、プロセッサ２１は、中心座標（Ｘｃ，Ｙｃ）及びマーカＭの深度情報から奥行における距離である深度距離を算出してもよい。

【0047】

例えば、プロセッサ２１は、マーカＭの４隅の位置情報（Ｘ１，Ｙ１）～（Ｘ４，Ｙ４）のうちの２つ以上の位置情報から平面情報であるマーカＭの平面における中心座標（Ｘｃ，Ｙｃ）を算出してもよい（中心座標算出ステップＳ１５）。

【0048】

プロセッサ２１は、フレーム関連処理Ｓ１０の次に、例えば、出力装置３０を制御するための出力情報を出力装置３０に出力する出力処理を行う。出力情報は、例えば、平面距離より算出される平面座標及び深度距離より算出される深度座標から構成される。

【0049】

また出力処理においては、出力情報の他に、マーカＩＤや方位などを含む詳細出力情報が出力装置３０に出力されてもよい。なお出力情報は平面情報と深度情報とを備える立体情報とする。

【0050】

なお中心座標（Ｘｃ，Ｙｃ）は、例えば、４隅全ての位置情報を使用する場合、マーカＭの４隅の位置情報（Ｘ１，Ｙ１）～（Ｘ４，Ｙ４）から次の式（１）、（２）のように算出できる。
Ｘｃ＝（Ｘ１＋…＋Ｘ４）／４ ……式（１）
Ｙｃ＝（Ｙ１＋…＋Ｙ４）／４ ……式（２）

【0051】

なお平面位置算出ステップＳ１３と方位算出ステップＳ１４とは、マーカ位置推定ステップＳ１２のあとであれば図３の順序とは逆に実行されてもよい。また中心深度算出ステップＳ１６は、中心座標算出ステップＳ１５のあとであれば、例えば平面位置算出ステップＳ１３のあとに実行されてもよい。またマーカ位置推定ステップＳ１２と中心座標算出ステップＳ１５とは、マーカ検出ステップＳ１１のあとであれば図３の順序とは逆に実行されてもよい。

【0052】

図４は、位置推定処理の各ステップをブロックとして表したソフトウェアブロック図である。具体的には、例えば、マーカ検出ステップＳ１１はマーカ検出部２１１が実行し、マーカ位置推定ステップＳ１２はマーカ位置推定部２１２が実行し、平面位置算出ステップＳ１３は平面位置算出部２１３が実行する。

【0053】

また、例えば、方位算出ステップＳ１４は方位算出部２１６が実行し、中心座標算出ステップＳ１５は中心座標算出部２１４が実行し、中心深度算出ステップＳ１６は中心深度算出部２１５が実行し、出力処理は出力処理部２１７が実行する。

【0054】

なお、例えば、測定装置１０においては、詳細画像取得部１３１は、撮像器１１から詳細画像を取得する詳細画像取得ステップを実行し、深度画像取得部１３２は、深度測定器１２から深度画像を取得する深度画像取得ステップを実行する。また、同期処理部１３３は、例えば、フレーム毎に同期された詳細画像及び深度画像を同期する同期ステップを実行する。

【0055】

例えば詳細画像取得部１３１や深度画像取得部１３２や同期処理部１３３やマーカ検出部２１１やマーカ位置推定部２１２や平面位置算出部２１３や中心座標算出部２１４や中心深度算出部２１５や方位算出部２１６や出力処理部２１７などの各部は、プログラムＰ１，Ｐ２に保存されているそれぞれのモージュール（関数）であってもよいし、ＦＰＧＡ（Field Programmable Gate Array）のような論理回路によってそれぞれ実装されてもよい。なお各部のうちの一部のみＦＰＧＡなどであってもよいものとする。

【0056】

図５は、位置推定処理のシーケンス図であって、位置推定処理における各ステップにおける深度画像や詳細画像やマーカＭの４隅の位置情報（Ｘ１，Ｙ１）～（Ｘ４，Ｙ４）のうちの２つ以上の位置情報や深度情報や並進情報や回転情報や平面距離や深度距離や出力情報や、マーカＩＤや方位などの情報の流れの一例を示している。

【0057】

以上のように、位置推定装置２０は、撮像器１１から取得する詳細画像をもととする平面距離と深度測定器１２から取得する深度画像をもととする深度距離とを組み合わせる。また位置推定装置２０は、対象物Ｏに付された、所定の条件を満たすマーカＭを認識することで対象物Ｏの位置を推定する。このことで、本実施の形態における位置推定装置２０は、精度高く対象物Ｏの位置を推定することができる。

【0058】

マーカＭが満たす所定の条件とは、例えば、サイズが一辺３～１００ｍｍであったり、表示パターンが５×５～７×７のマスであり回転非対称性を有することであったりするという条件とする。

【0059】

図６は、平均化処理の一例を表すフローチャートである。プロセッサ２１は、フレーム関連処理Ｓ１０の結果として出力される出力情報（詳細出力情報）が所定の条件を満たす場合、正常出力情報（正常詳細出力情報）とし、所定数以上の正常出力情報（正常詳細出力情報）の平均を平均正常出力情報（平均正常詳細出力情報）として出力する、平均値算出処理Ｓ２０を実行してもよい。

【0060】

出力情報が満たす所定の条件とは、例えば、出力情報に関する標準偏差に対して所定の範囲内に収まることなどを指す。例えば平均値算出処理Ｓ２０（フレーム関連処理Ｓ１０以外）は出力処理部２１７によって実行される。

【0061】

具体的には、平均値算出処理Ｓ２０において、プロセッサ２１は、フレーム関連処理Ｓ１０を実行する。プロセッサ２１は、フレーム関連処理Ｓ１０などの結果として出力される出力情報が所定の条件を満たし、異常値でないか否かを判定する（Ｓ２１）。

【0062】

出力情報が異常値と判定した場合（Ｓ２１：ＮＯ）、プロセッサ２１は、フレーム関連処理Ｓ１０を実行する。出力情報が異常値でなく、所定の条件を満たし、正常値と判定した場合（Ｓ２１：ＹＥＳ）、プロセッサ２１は、正常値を正常出力情報として、例えば第１のメモリ２２に保存する（Ｓ２２）。

【0063】

プロセッサ２１は、第１のメモリ２２に所定数以上の正常出力情報が保存されているか否かを判定する（Ｓ２３）。所定数以上が保存されていると判定した場合（Ｓ２３：ＹＥＳ）、プロセッサ２１は、例えば、正常出力情報の平均である平均正常出力情報を出力して、平均値算出処理Ｓ２０を終了させる。なおプロセッサ２１は、第１のメモリ２２に保存した正常出力情報を、例えば、ＦＩＦＯ（First In, First Out）の形式で扱う。

【0064】

所定数以上が保存されていないと判定した場合（Ｓ２３：ＮＯ）、プロセッサ２１は、再びフレーム関連処理Ｓ１０を実行する。以上のように、位置推定装置２０は、平均値算出処理Ｓ２０を実行する場合、異常値を除外することが可能となり、位置推定の精度をより高めることができる。

【0065】

図７は、測定装置１０を複数備える場合の位置推定システムを表す概略図である。位置推定装置２０が、例えば、第１の測定装置１０ａ、第２の測定装置１０ｂのような複数の測定装置１０に接続され、複数のマーカＭ（マーカＭａ、マーカＭｂ）が配置されている場合について説明する。

【0066】

測定装置１０ａは、マーカＭａを検出できるが、マーカＭｂを検出できない。マーカＭｂは、測定装置１０ａから見ると、ロボットアームＡの陰に配置されるようになるためである。一方、測定装置１０ｂは、マーカＭａ、マーカＭｂの双方を検出できる。

【0067】

なお、例えば、アーム本体３１と吸着ハンド３２とを備えるロボットアームＡは、位置推定装置２０からの出力情報などに基づいて、アーム本体３１を動かし、対象物Ｏを吸着ハンド３２によって吸着する装置とする。

【0068】

またロボットアームＡのような出力装置３０に限らず、位置推定装置２０とは情報をやり取りしない建物の柱のような構造物がマーカＭｂを検出できない原因となっている場合についても同様とする。

【0069】

複数の測定装置１０（深度測定器１２）での電磁波Ｌｒが互いの測定を阻害することを防止するために、位置推定装置２０は、図８に示すような複数測定装置対応処理Ｓ３０を実行することが好ましい。例えば複数測定装置対応処理Ｓ３０（フレーム関連処理Ｓ１０以外）は出力処理部２１７によって実行される。

【0070】

複数測定装置対応処理Ｓ３０を実行せず、通常のフレーム関連処理Ｓ１０を並列して実行した場合、例えば、第１の測定装置１０ａ（深度測定器１２）は、意図しない第２の測定装置１０ｂからの電磁波Ｌｒによる測定をしてしまい、意図せず連続して測定を行ってしまう可能性がある。

【0071】

複数測定装置対応処理Ｓ３０において、位置推定装置２０からの指示に応じて、測定装置１０（深度測定器１２）が、シャッターのように電磁波Ｌｒの照射のＯＮ／ＯＦＦを切り替える。

【0072】

複数測定装置対応処理Ｓ３０においては、測定装置１０を１つだけ使う場合とは異なり１フレームの処理時間は固定ではなく可変となる。具体的には、複数測定装置対応処理Ｓ３０においては、１つの測定装置１０の１フレームの処理が終わると他の測定装置１０の１フレームの処理を開始するような処理を位置推定装置２０は行う。

【0073】

図８は、測定装置を複数備える場合の位置推定処理である複数測定装置対応処理Ｓ３０の一例を表すフローチャートである。複数測定装置対応処理Ｓ３０において、プロセッサ２１は、第１の測定装置１０ａ及び第２の測定装置１０ｂに同期設定指示を行い、第１の測定装置１０ａ及び第２の測定装置１０ｂに対してそれぞれ順にフレーム関連処理Ｓ１０を実行する。

【0074】

複数測定装置対応処理Ｓ３０において、プロセッサ２１は、フレーム関連処理Ｓ１０の代わりに平均値算出処理Ｓ２０を実行してもよいものとする。また、例えば、複数測定装置対応処理Ｓ３０は、位置推定装置２０に複数の測定装置１０が接続されると開始される。

【0075】

具体的には、複数測定装置対応処理Ｓ３０において、プロセッサ２１は、同期設定指示を測定装置１０に送信する（Ｓ３１）。プロセッサ２１は、位置推定装置２０に登録されている全ての測定装置１０への同期設定指示を送信したか否かを判定する（Ｓ３２）。

【0076】

位置推定装置２０に登録されている全ての測定装置１０への同期設定指示を送信していないと判定した場合（Ｓ３２：ＮＯ）、プロセッサ２１はステップＳ３１を実行する。なお位置推定装置２０に登録されているか否かの登録情報は、例えば、第２のメモリ２３のテーブルＴに保存されているものとする。

【0077】

位置推定装置２０に登録されている全ての測定装置１０への同期設定指示を送信したと判定した場合（Ｓ３２：ＹＥＳ）、プロセッサ２１は、制御開始指示を測定装置１０に送信する（Ｓ３３）。プロセッサ２１は、制御開始指示を送信した測定装置１０に関するフレーム関連処理Ｓ１０を実行する。

【0078】

プロセッサ２１は、位置推定装置２０に登録されている全ての測定装置１０に制御開始指示を送信したか否かを判定する（Ｓ３４）。位置推定装置２０に登録されている全ての測定装置１０に制御開始指示を送信していないと判定した場合（Ｓ３４：ＮＯ）、プロセッサ２１はステップＳ３３を実行する。

【0079】

位置推定装置２０に登録されている全ての測定装置１０に制御開始指示を送信したと判定した場合（Ｓ３４：ＹＥＳ）、プロセッサ２１は、例えば、それぞれの測定装置１０に関する出力情報を出力して、複数測定装置対応処理Ｓ３０を終了させる。

【0080】

以上のように、位置推定装置２０は、複数測定装置対応処理Ｓ３０を実行する場合、測定装置１０ａ、１０ｂそれぞれから取得する情報において、例えば、異常値を弾いたり、よりマーカＭに近い測定装置１０ａからの情報を取得したりなどの選択をしたり、平均をとったりすることが可能となる。

【0081】

このため、位置推定装置２０は、位置を推定できない領域である死角を少なくすることができ、より精度高く対象物Ｏの位置を推定することが可能となる。なお前述の調整や補完は第１の測定装置１０ａ及び第２の測定装置１０ｂをまたいで行われてもよい。

【0082】

また、複数測定装置対応処理Ｓ３０において、電磁波Ｌｒの照射のＯＮ／ＯＦＦに合わせて、それぞれの測定装置１０において、詳細画像及び深度画像が撮像される。複数の測定装置１０（深度測定器１２）からの電磁波Ｌｒの照射のタイミングを異ならせることによって、複数の測定装置１０（深度測定器１２）による意図しない測定のない測定が可能となる。例えば、意図しない測定とは、干渉（深度画像に波紋のようなノイズがのってしまい、そもそも測定が正常にできていない状態）が起きている状態での測定を指す。

【0083】

図９は、出力装置３０である、例えば、ディスプレイのような、画像表示装置Ｂにおける表示画面を表す図である。フレーム関連処理Ｓ１０などによって出力される前記平面距離より算出される平面座標及び前記深度距離より算出される深度座標から構成される出力情報及び方位は、表示画面において、対象物Ｏの近傍に表示される。マーカＩＤも表示画面において、対象物Ｏの近傍に表示されてもよい。

【0084】

なお対象物Ｏの名称や対象物Ｏに関しての注釈（例えば対象物Ｏの消費期限）などが詳細出力情報などとして表示されてもよいものとする。対象物Ｏは、例えば、コンビニなどの小売店の販売用の棚に商品として配置されたサンドイッチなどとする。対象物Ｏには、複数のマーカＭ（マーカＭａ，Ｍｂ，Ｍｃ）が張りつけられている。

【0085】

表示画面には、第１の測定装置１０ａ及び第２の測定装置１０ｂのうちの少なくとも１つが撮像した詳細画像と、マーカＭに対する出力情報や詳細出力情報、が重畳して表される。

【0086】

なお表示画面にはロボットアームＡや測定装置１０が撮像されていてもよいものとする。また表示画面は位置推定装置２０から送信される情報を経時的に反映するものとしてもよい。

【0087】

サンドイッチ（対象物Ｏ）に付されたマーカＭ（Ｍａ，Ｍｂ，Ｍｃ）は、それぞれの脇に、マーカ情報が示される。例えば、マーカＭは、サンドイッチのパッケージの撮像されているそれぞれの面に付される。マーカＭ（対象物Ｏ）の近傍にマーカＭに対する出力情報や詳細出力情報、が重畳して表される。なお、マーカＭは、マスや輪郭などが強調して表示されてもよい。

【0088】

例えば、表示画面において、出力情報はＸ，Ｙ，Ｚ座標がそれぞれ何ミリメートル（ｍｍ）かが表示され、対象物Ｏの名称は英字で表示され、対象物Ｏの消費期限は英字と数字などで表示される。例えば、表示画面において、マーカＩＤは数字で表示され、方位はロール角φ、ピッチ角θ、ヨー角ψが、何度（ｄｅｇ）かが表示される。

【0089】

以上のように、本実施の形態における位置推定装置２０において表示画面が表示される場合、どのような情報が取得されているかが視認できるため、位置推定装置２０の使用者は、位置推定装置２０や測定装置１０の調整や変更などがしやすくなる。

【0090】

本発明は上述した各実施の形態に限定されるものではなく、請求項に示した範囲で種々の変更が可能であり、異なる実施の形態にそれぞれ開示された技術的手段を適宜組み合わせて得られる実施の形態についても本発明の技術的範囲に含まれる。

【0091】

例えば、マーカＭは、ＡＲマーカに限らず、２次元コードや、バーコードのような１次元コードや、例えば写真のようなものであっても良いし、２値ではなく、３以上の複数の色によって表されてもよいものとする。

【0092】

また、対象物Ｏは、形状などが一定で複数パターンの識別がしやすい、回転非対称性を有し方位が算出できる、といったような一定の条件を満たす場合は、マーカＭは対象物Ｏそのものであってもよい。なお測定装置１０は３つ以上であってもよいものとする。

【0093】

なお、出力装置３０としてスピーカなどの音声出力装置が位置推定装置２０に接続されてもよい。例えば、ロボットアームＡが目標のマーカＭに近づいたときに、出力装置３０は、ロボットアームＡが対象物Ｏを吸着する予告としての予告音を出力してもよい。

【0094】

出力装置３０から出力される予告音は、例えば、ロボットアームＡと対象物Ｏ（マーカＭ）との距離によって、音高や音量が変わってもよい。具体的には、出力装置３０から出力される予告音は、ロボットアームＡと対象物Ｏとの距離が近づくにつれ、音量が大きくなったり、音高が高くなったりしてもよい。

【0095】

なお、複数のマーカＭａ，Ｍｂ，Ｍｃに対しての処理を行う場合、位置推定装置２０は、それぞれの複数のマーカＭａ，Ｍｂ，Ｍｃに対して、１つずつ順に処理を行うものとする。

【0096】

複数のマーカＭａ，Ｍｂ，Ｍｃが１つの対象物Ｏに付されている場合、位置推定装置２０は、１０００個以上の対象物Ｏを、認識することが可能となる。例えば、マーカＩＤの００１から０９９までを位置特定用とし、マーカＩＤの０１００から０２００までを賞味期限定義用などとすることで、位置推定装置２０は、１０００個以上の対象物Ｏを、認識することが可能となる。

【0097】

また、例えば、マーカＩＤの００４が３つの頂点を持つような対象物Ｏであるおにぎりの頂点１を示し、マーカＩＤの００５がおにぎりの頂点２を示し、マーカＩＤの００６がおにぎりの頂点３を示すような場合について考える。

【0098】

この場合、位置推定装置２０は、複数のマーカＩＤの位置情報の中心をおにぎりの位置だと認識することが可能となる。なお、例えば、マーカＭａはマーカＩＤが００４とし、マーカＭｂはマーカＩＤが００５とし、マーカＭｃはマーカＩＤが００６とする。

【符号の説明】

【0099】

１０……測定装置、１１……撮像器、１２……深度測定器、２０……位置推定装置、３０……出力装置、Ｍ……マーカ、Ｏ……対象物。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】