特許 7488222 相対位置推定装置及びプログラム

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

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-05-13

(45)【発行日】2024-05-21

(54)【発明の名称】相対位置推定装置及びプログラム

(51)【国際特許分類】

   G01B 11/00 20060101AFI20240514BHJP

   G05D 1/43 20240101ALI20240514BHJP

【ＦＩ】

G01B11/00 H

G05D1/43

【請求項の数】 12

(21)【出願番号】P 2021076784

(22)【出願日】2021-04-28

(65)【公開番号】P2022170574

(43)【公開日】2022-11-10

【審査請求日】2023-08-07

(73)【特許権者】

【識別番号】000208891

【氏名又は名称】ＫＤＤＩ株式会社

(74)【代理人】

【識別番号】100092772

【弁理士】

【氏名又は名称】阪本 清孝

(74)【代理人】

【識別番号】100119688

【弁理士】

【氏名又は名称】田邉 壽二

(72)【発明者】

【氏名】道後 千尋

【審査官】櫻井 仁

(56)【参考文献】

【文献】特開２００９－２４４９６５（ＪＰ，Ａ）

【文献】特開２００９－０８５６２８（ＪＰ，Ａ）

【文献】実公昭５０－０３１７４７（ＪＰ，Ｙ１）

【文献】特開２００３－１３０６４４（ＪＰ，Ａ）

【文献】特開２００２－３５９８３８（ＪＰ，Ａ）

【文献】特開２０１８－１７３３８８（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｂ １１／００

Ｇ０５Ｄ １／４３

Ｇ０１Ｃ ３／０６

Ｇ０１Ｃ １５／００

(57)【特許請求の範囲】

【請求項1】

観察体に配置されたカメラによって、高反射素材が配置された被観察体を撮像した画像より、被観察体を基準とした観察体の第１相対座標を推定する相対位置推定装置であって、
前記画像より、前記観察体の前記高反射素材による鏡像を検出する自己検出部と、
前記画像内において検出された鏡像より、当該鏡像の前記観察体を基準とした第２相対座標を推定する相対位置推定部と、
前記第２相対座標より、前記高反射素材における第１所定位置の前記観察体を基準とした第３相対座標を推定する反射素材位置推定部と、
前記第３相対座標と、前記高反射素材における第１所定位置と前記被観察体に予め設定される第２所定位置との間の第４相対座標と、を合成することで、前記第１相対座標を推定する相対位置計算部と、を備えることを特徴とする相対位置推定装置。

【請求項2】

前記自己検出部では、前記画像より前記高反射素材の占める範囲を推定したうえで、当該範囲内において、前記鏡像を検出することを特徴とする請求項１に記載の相対位置推定装置。

【請求項3】

前記自己検出部で推定した範囲が、前記高反射素材による鏡像が前記画像内に撮像されない状況にあると判定される場合に、前記観察体及び／又は前記被観察体を移動させる制御を行うことで当該撮像されない状況の解消を試みることを特徴とする請求項２に記載の相対位置推定装置。

【請求項4】

前記自己検出部では、前記画像が撮像されるフィールドにおいて３次元点群データ又は深度データを取得し、当該３次元点群データ又は深度データの空間分布が異常値を示すと判定される領域を、前記高反射素材が占める範囲として推定することを特徴とする請求項２または３に記載の相対位置推定装置。

【請求項5】

前記自己検出部では、前記観察体の外観表示を変化させる制御を行い、前記画像のうち、当該外観表示の変化が検出される領域の付近に限定して、前記鏡像を検出することを特徴とする請求項１ないし４のいずれかに記載の相対位置推定装置。

【請求項6】

前記自己検出部では、前記観察体を移動させる制御を行い、前記画像として、当該移動する前の画像と当該移動した後の画像を解析して、当該移動に整合して変化している箇所を前記鏡像として検出することを特徴とする請求項１ないし４のいずれかに記載の相対位置推定装置。

【請求項7】

前記自己検出部及び前記相対位置推定部では、前記画像よりクエリとして抽出される局所特徴量を予め登録されているリファレンスと照合することにより、または、深層学習ネットワークを用いることにより、それぞれ前記鏡像を検出し、前記第２相対座標を推定することを特徴とする請求項１ないし６のいずれかに記載の相対位置推定装置。

【請求項8】

前記反射素材位置推定部は、前記画像において前記高反射素材の境界部を検出することにより、前記第３相対座標を推定することを特徴とする請求項１ないし７のいずれかに記載の相対位置推定装置。

【請求項9】

前記反射素材位置推定部は、前記画像において前記鏡像が前記高反射素材において見切れている箇所として前記境界部を検出することを特徴とする請求項８に記載の相対位置推定装置。

【請求項10】

前記観察体に配置されたカメラは、前記自己検出部で鏡像を検出する対象となる前記画像を撮像する第１カメラであり、当該第１カメラとは反対側を撮像して背面側画像を得る第２カメラがさらに存在し、
前記反射素材位置推定部は、前記背面側画像より背景オブジェクトを抽出し、前記画像において、前記背景オブジェクトの鏡像が前記高反射素材において見切れている箇所として前記境界部を検出することを特徴とする請求項８に記載の相対位置推定装置。

【請求項11】

前記高反射素材は凸面鏡として構成されており、
前記反射素材位置推定部は、前記自己検出部において前記凸面鏡によって検出された歪んだ鏡像と、前記凸面鏡の既知の歪みと、を用いて前記凸面鏡の中心位置として、前記第３相対座標を推定することを特徴とする請求項１ないし７のいずれかに記載の相対位置推定装置。

【請求項12】

コンピュータを請求項１ないし１１のいずれかに相対位置推定装置として機能させること特徴とするプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、少ない制約で簡素に測位を行うことのできる相対位置推定装置及びプログラムに関する。

【背景技術】

【0002】

ロボットにおける測位に関する従来技術として例えば特許文献１，２があり、それぞれ以下の構成を開示している。

【0003】

特許文献１…相対位置推定装置
・レーザを使い、コンテナとフォークリフトの相対位置を検出するシステムである。
・レーザが当たった照射点までの距離を照射角度に対応付けて測定する。
・コンテナの正面、側面とその間の角の位置の反射の候補点を出す。
・候補点を直線で結ぶなどして、DB等でルール的にコンテナの角度を推定する。

【0004】

特許文献２…自律移動装置
・鏡面素材と拡散反射素材を交互に設置し、レーザ反射のエラーと正常を利用して情報を伝達するシステムである。
・鏡面素材部分に当たったレーザは全反射する為、レーザが返らず、エラーになる。
・交互に配置することでバーコードのように情報を付与する。
・移動経路の障害物の配置を示した環境地図と照合して相対位置の推定に利用する。

【先行技術文献】

【特許文献】

【0005】

【文献】特開2020-175979号公報

【文献】特開2014-006835号公報

【発明の概要】

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

【0006】

しかしながら、上記従来技術には以下のような制約があった。

【0007】

特許文献１のように、観測側（以下、観察体）でレーザ装置やプロジェクタを使用しその反射を観測する場合全般において、以下（１）～（４）のような制約がある。

【0008】

（１）観察体が、少なくともレーザを発生させる装置を備えている必要がある。
（２）観測対象側（以下、被観察体）がコンテナ以外で形状が予測できない場合、レーザを走査する部分に反射可能な素材が存在するとは限らない。
例：Keenon T1(https://keenonrobot.jp/peanut/)のように棚型のロボットを相手にする場合、中央部でレーザが貫通するため、正しい角度を算出できない。

【0009】

（３）投影が反射した場所との距離/被観察体の体長・角度のみを観測可能であり、被観察体上の任意の箇所の特定や、正面と背面の区別ができない。例えば被観察体上に物を置くタスクを実行する際の被観察体との位置関係(距離)に加えて、置く位置を特定する必要がある。
（４）同様の機構を所有する装置が同時に位置推定を行った場合に、自機から発せられたレーザの反射を弁別する手法に乏しい。また、被観察体が複数ある環境の場合、どの被観察体で反射したレーザ光なのかを弁別する手段に乏しい。

【0010】

特許文献２やQRコード（登録商標）等に代表される判別用マーカを自律移動装置に搭載する場合全般において、以下の（５）～（９）のような制約がある。

【0011】

（５）標識を設置する側、読み取る側で予め仕様を決定し、共有している必要がある。自律移動装置が複数のプラットフォームに属する場合に、それぞれの外見的要求を満たすようにロボットを設計する必要がある。
例）プラットフォームAでQRコード（登録商標）、プラットフォームBでバーコード、プラットフォームCで別の仕様に基づいたQRコード（登録商標）を使用して位置合わせをする場合、この3つの仕様統一されていないプラットフォームを行き来するロボットは、3つのコードを搭載する必要がある。

【0012】

（６）上記の制約（５）は被観察体の物体認識を利用して、被観察体のデザインは自由に行い、完成した外見を認識対象として学習することで解決可能である。この場合、プラットフォーム側で自律移動装置の検出と同定を行い、それぞれの機器への対応を別途定義する必要がある。

【0013】

（７）マーカ部分が一部遮蔽した際に、認識不能に陥る。
（８）観察対象の移動速度が高速であるとき、マーカの撮影においてブレが生じて認識精度が低下する。
（９）これらの画像的特徴の顕著な機構を採用することで、装置の形状やデザインに制約が生じる場合があり、機構の柔軟性を許容するほど、認識性能を犠牲にすることとなる。

【0014】

以上のような個別具体的な制約（１）～（９）として現れているように、従来技術では、相手物体の外見情報や相互通信、共通のタグ等に依存することから、測位のための事前取り決めの設定や測位に用いる設備の構成に多くの手間やコストを要してしまい、簡素な測位を行うことができないという課題があった。

【0015】

上記従来技術の課題に鑑み、本発明は、少ない制約で簡素に測位を行うことのできる相対位置推定装置及びプログラムを提供することを目的とする。

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

【0016】

上記目的を達成するため、本発明は、観察体に配置されたカメラによって、高反射素材が配置された被観察体を撮像した画像より、被観察体を基準とした観察体の第１相対座標を推定する相対位置推定装置であって、前記画像より、前記観察体の前記高反射素材による鏡像を検出する自己検出部と、前記画像内において検出された鏡像より、当該鏡像の前記観察体を基準とした第２相対座標を推定する相対位置推定部と、前記第２相対座標より、前記高反射素材における第１所定位置の前記観察体を基準とした第３相対座標を推定する反射素材位置推定部と、前記第３相対座標と、前記高反射素材における第１所定位置と前記被観察体に予め設定される第２所定位置との間の第４相対座標と、を合成することで、前記第１相対座標を推定する相対位置計算部と、を備えることを特徴とする。

【発明の効果】

【0017】

本発明によれば、被観察体に高反射素材を配置しておくことにより、事前に既知としておく情報は少なくとも、観察体の鏡像を検出するための情報と、高反射素材及び被観察体の両方における所定位置の関係としての第４相対座標の情報と、のみでよいため少ない制約で簡素に観察体の被観察体を基準とした測位を行うことができる。

【図面の簡単な説明】

【0018】

【図1】一実施形態に係る相対位置推定システムの構成を模式的に示す図である。

【図2】一実施形態に係る相対位置推定装置の機能ブロック図である。

【図3】一実施形態に係る自己検出部の機能ブロック図である。

【図4】一実施形態に係る相対位置推定システムの動作のフローチャートである。

【図5】各機能ブロックの処理の模式例を示す図である。

【図6】第２カメラを用いて鏡の境目として見切れる部分を割り出す手法の模式例を示す図である。

【図7】凸面鏡上の鏡像の歪みから鏡の中心を推定する模式例を示す図である。

【図8】一般的なコンピュータにおけるハードウェア構成を示す図である。

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

【0019】

図１は、一実施形態に係る相対位置推定システム100の構成を模式的に示す図である。相対位置推定システム100は、共通のフィールドF（例えば工場や倉庫など）に存在する、撮像部1を有する観察体OB1と、目標座標の原点TGが設定される被観察体OB2と、を有する。被観察体OB2上には、鏡などの高反射素材R（鏡像を生じさせることの不可能な拡散反射素材と区別されるものとしての鏡面を有する高反射素材）が配置されている。観察体OB1及び被観察体OB2の両方または片方は、自律移動機能を備えるロボットであってよい。

【0020】

なお、高反射素材に関しては適宜、鏡とも称する。

【0021】

相対位置推定システム100には相対位置推定装置10（図１では不図示）が備わる。相対位置推定装置10は、反射素材Rを利用して、目標座標の原点TGを基準とした相対座標として観察体OB1の相対座標を推定することで、観察体OB1の被観察体OB2を基準とした相対位置を推定するというタスクを実行する。当該タスクによる応用例として、例えば目標座標の原点TGが倉庫の棚の所定位置であり荷物が置かれている場合に、荷物運搬ロボットとして構成した観察体OB1により、原点TGに対する自身の相対位置の認識結果に基づいて当該原点TGに向かって移動したうえで、当該荷物のピッキング作業を自動で行う等が可能となる。

【0022】

上記タスクを実行する相対位置推定装置10は、一般的なコンピュータ装置として構成され、観察体OB1に設置されていてもよいし、被観察体OB2に設置されていてもよいし、フィールドFとは別の遠隔地に設置され、ネットワークを介して通信により観察体OB1及び／又は被観察体OB2の動作を制御するものであってもよい。相対位置推定装置10はまた、ネットワークを介して相互に通信する複数のコンピュータ装置として構成され、各コンピュータ装置が上記の全部または一部のような任意ロケーションに設置されているものであってもよい。

【0023】

図２は、一実施形態に係る相対位置推定装置10の機能ブロック図である。図示するように、相対位置推定装置10は、撮像部1、自己検出部2、相対位置推定部3、反射素材位置推定部4、相対位置計算部5、DB（データベース）6、アクチュエータ78、移動機構79及び表示部80を備える。

【0024】

なお、図２に示す各部のうち、撮像部1は図１で示したように観察体OB1に物理的に設置されるものであり、アクチュエータ78及び移動機構79はロボットとしての自律移動を実現するための構成として観察体OB1及び／又は被観察体OB2に物理的に設置されるものであればよい。表示部80は観察体OB1に備わるものであればよい。図２では相対位置推定装置10での情報処理の流れを示す観点から、これら撮像部1、アクチュエータ78、移動機構79及び表示部80の配置（物理設置とは独立に設定可能な、機能上の配置）も示している。

【0025】

図３は、一実施形態に係る自己検出部2の機能ブロック図である。図示するように、自己検出部2は、検出部21、反射素材検出部22、移動制御部23及び表示制御部24を備える。図４は、一実施形態に係る相対位置推定システム100の動作のフローチャートである。以下、図４の各ステップを説明しながら、図２及び図３の各機能ブロックの処理の詳細について説明する。図５は、各機能ブロックの処理の模式例を示す図であり、以下の説明において適宜、参照する。

【0026】

ステップS1では、ハードウェアとしては記憶媒体で構成されるDB6に、本実施形態で必要となる種々の情報を管理者による手動作業等で登録することでDB6を構築してから、ステップS2へと進む。DB6に対して当該登録する情報（及び当該情報をリファレンス情報として参照して利用する機能ブロック）は以下に列挙される通りである。
・観察体OB1の外見を識別するための情報（局所特徴量等）
→自己検出部2で利用
・観察体OB1の外見から距離を推定するための観察体OB1のサイズデータ及び撮像部1のカメラの内部パラメータ等の情報
→相対位置推定部3で利用
・目標座標の原点と鏡の相対位置関係の情報
→相対位置計算部5で利用

【0027】

以上のDB6に登録しておく情報の詳細は、当該情報をリファレンスとして参照して利用する機能ブロックの説明において後述する。なお、実施形態によっては上記の情報が不要となったり、別の情報を当該ステップS1においてDB6に登録しておいて利用したりする場合もあるが、これらの変形例に関しても当該機能ブロックの説明において後述する。

【0028】

ステップS2では、管理者の手作業等によりフィールドFを図１で説明した状態の形でセットアップしてからステップS3へと進む。すなわちステップS2では、フィールドFに観察体OB1及び被観察体OB2を配置し、相対座標の原点を有する被観察体OB2に鏡などの高反射素材Rを付与する。（なお、被観察体OB2が「相対座標の原点を有する」ことは、ステップS1で構築するDB6の情報と整合するように、被観察体OB2の所定位置に高反射素材Rを付与することで実現される。この詳細については後述する。）また、ステップS2で観察体OB1及び被観察体OB2をフィールドF内に配置する際は、図１に模式的に示されるように、これら両者が既に互いに向き合って近接した状態として配置することは必須ではなく、フィールドF内を観察体OB1及び／又は被観察体OB2が移動することで当該向き合って近接した状態となることが可能であれば、任意位置に配置するようにしてよい。

【0029】

ステップS3では、相対位置を推定しようとするシステムである観察体OB1が、撮像部1（観察体OB1に備わるカメラ）よって周辺を探索しながら撮像し、画像Pにおける高反射素材Rに映る観察体OB1の鏡像MOB1の検出を行ってから、ステップS4へと進む。図５の例EX1では、ステップS3の処理の例として、画像Pから高反射素材R（平面鏡）の領域内に撮像されている観察体OB1の鏡像MOB1が検出されることを示している。

【0030】

ステップS3では観察体OB1及び被観察体OB2が静止した状態において撮像部1が撮像を行い、得られた画像を自己検出部2へと出力し、自己検出部2がこの画像内における観察体の鏡像の検出を試み、成功すればステップS4へと進む。自己検出部2が当該画像から観察体の鏡像の検出に失敗した場合には、観察体OB1及び／又は被観察体OB2が一定量だけ移動した後に静止して再度、撮像を行って自己検出部2による検出を試みること等を検出に成功するまで当該ステップS3において繰り返す。

【0031】

あるいは、ステップS3～S6の処理はリアルタイムの各時刻t=1,2,…において、移動している観察体OB1及び／又は被観察体OB2のもとで繰り返し実行されるものであってもよい。（この場合、ステップS3の検出が成功しない時刻tにおいてはステップS4以降の処理は省略されることとなり、ステップS4以降の各ステップも同様に前段側のステップが成功したことを前提として実行されるものとなる。）

【0032】

ステップS3での検出は、図３に示される自己検出部2の構成から選択して実施されることによって、様々な態様が可能である。

【0033】

撮像部1はハードウェアとしては観察体OB1に設置されるカメラで実現され、観察体OB1周辺の環境（フィールドF）を撮影画像として取り込む。本実施形態では撮影画像がRGB画像であると想定して説明するが、これに限定されず周辺環境を記録できるデータ方式であれば、RGB-D画像（RGB等の通常の色チャネルの各画素値にさらに深度Dが紐づけられた画像）や三次元点群等を用いてもよい。

【0034】

一実施形態に係る自己検出部2では、高反射素材Rが平面鏡として構成されることを利用して、検出部21により画像から観察体OB1の鏡像を検出することができる。この際、反射素材検出部22、移動制御部23及び表示制御部24の全部または一部の処理を加えて検出するようにしてもよい。

【0035】

（Ａ） 検出部21では、画像から検出される特徴点（クエリ）とDB6上の特徴点（リファレンス）とをマッチングし、画像内における鏡像の観察体を検出する。なお、局所特徴量のマッチングを元にした手法以外（例えば以下の[opition1]の深層学習等）を用いてもよく、それぞれに対応した手法の情報が登録されているDB6を参照して鏡像の観察体の同定を行うようにすればよい。

【0036】

この検出部21による検出が可能なように、ステップS1でDB6には次のような情報を登録しておけばよい。

【0037】

・観察体OB1自らの外見を撮影した画像からSIFT等の既存の任意種類の局所特徴量を抽出し、DB6に登録しておき、これらとマッチングすることで鏡像の観察体を検出可能とする。
・観察体OB1自らの外見を撮影した画像は、観察体OB1の背面ではなく前面が鏡に相対した状態となるように、観察体OB1の撮像機能のレンズのある方向から撮影することで少なくとも1枚以上を用意し、その左右反転画像を使用する。

【0038】

あるいは、以下option1～3のような変形例でもよい。

【0039】

［option1］画像を入力として、保持情報から観察体OB1の鏡像が探せるものであれば特徴点マッチング以外の方法、例えば深層学習による物体認識やOCR（光学文字認識）、color analysis（カラー分析）、エッジ処理などの画像処理技術を用いてもよい。
［option2］被観察体OB2に備わっている高反射素材Rが、平面鏡ではなく拡大鏡または広角鏡等の歪みを生じさせる凸面鏡である場合、生じうる歪みを画像処理または実環境で再現したのちに、局所特徴量を抽出してDB6に登録してもよい。（後述する図７の実施形態ではこのoption2を適用してもよい。）
［option3］駆動による形状変形、モニタ等の表示変更、照明の変化などで外見的変化が起こりうる環境においては、それぞれの状態において撮影した画像から局所特徴量を抽出してDB6に登録してもよい。この際に形状の変化等を局所特徴量にタグ付けして登録を行い、DB6探索の際の探索範囲の削減や探索優先順位付けに利用してもよい。

【0040】

以上の（Ａ）の検出部21の処理を補助するための実施形態として、（Ｂ）反射素材検出部22により、画像内から高反射素材Rの領域を検出しておくことで、検出部21による検出処理を画像全体ではなく、この高反射素材Rの領域に限定するようにしてもよいし、（Ｃ）検出部21による検出が困難となりうる場合等には、図３中に線L1,L2で示すように検出部21での処理と連動させる形で移動制御部23及び／又は表示制御部24の処理を加えるようにしてもよい。

【0041】

（Ｂ） 検出部21において撮影画像から観察体OB1の鏡像を認識する前に、反射素材検出部22において鏡を検出し、観察体OB1が写りうる範囲を限定して探索してもよい。

【0042】

・反射素材検出部22では、例えばLiDAR等のレーザを照射してその反射を観測する既存システムを用いて、その反射が正常値から外れる部分（正常値としての平面領域において突然現れる、穴が生じていると判定されたり、３次元座標が不規則に変動していると判定されたり、計測値がエラーを表している部分等）を高反射素材Rの領域として認識する。この検出には例えば以下の非特許文献１の技術を用いることが可能であり、鏡の位置候補の３次元座標が取得できるので、既知のカメラパラメータによりこれを２次元画像領域に変換して取得すればよい。このようなLiDAR等による３次元点群に限らず、RGB-D画像より深度分布を取得して、それらの空間分布が異常を示すと判定される箇所を高反射素材Rの領域として認識してもよい。
非特許文献1 二宮舜, 徳留成亮, 西田健, "LiDARの計測欠落の補完," 日本知能情報ファジィ学会ファジィシステムシンポジウム, 2018.

【0043】

あるいは、以下option1,2のような変形例でもよい。

【0044】

・［option1］レーザを使用する既存システムの代替として、撮影画像から深度推定を行い、深度推定結果と反射体（鏡R）の移動軌跡を総合して三次元再構成を行った際に、幾何学的に整合性の保てない部分を鏡として認識してもよい。
・［option2］レーザを使用する既存システムの代替として、撮影画像から3次元一般物体認識を行い、対称位置で同じ物体が検出される空間に対し、その対称線となる部分を鏡として認識してもよい。（例えば同じ物体ob1が２つの位置p11,p12で検出され、同じ物体ob2が２つの位置p21,p22で検出される場合であって、２点p11,p12の垂直２等分面や２点p21,p22の垂直２等分面が概ね一致する場合に、当該概ね一致する垂直２等分面を鏡に該当するものとして認識してよい。）例として、以下の非特許文献の技術が利用可能である。
非特許文献2 金崎朝子, "深層学習を用いた三次元物体認識," 確率場と深層学習に関する第2回CRESTシンポジウム, 2018

【0045】

（Ｃ） 移動制御部23では、反射素材検出部22を適用し鏡を検出した際に、観察体OB1が鏡に写らない位置であった場合に、写る位置まで観察体OB1を移動させる制御を行う。反射素材検出部22ではLiDAR等により観察体OB1を基準とした３次元座標において３次元領域として高反射素材Rの領域を検出しているので、この高反射素材Rの正面方向に対して観察体OB1の位置が所定角度範囲よりも外にある場合に、観察体OB1が写らない位置であるものとして判定することができる。移動は以下のようにすればよい。

【0046】

・鏡の検出された座標群を直線に近似し、その直線のある平面に正対する位置に観察体を移動させる。

【0047】

移動制御部23ではモータ等のハードウェアで構成されるアクチュエータ78に対して制御信号を出力し、アクチュエータ78が当該制御信号に従った駆動力を、観察体OB1に備わるロボットの車輪や脚として構成させる移動機構79へと伝達することで、観察体OB1が移動することができる。

【0048】

移動制御部23（及び表示制御部24）はまた、以下のような態様で検出部21の検出補助に利用されてもよい。

【0049】

・観察体OB1と同様の外見を持つ装置が複数存在する場合、または照明条件などの環境要因によって画像認識の精度が十分でない場合には、動作を使用した検出または検出補助を行う。
・あるいは、観察体OB1以外に移動物体のない環境、または観察体OB1の外見が短期間で変更になるような環境においては、動作を使用した検出または検出補助を行う。

【0050】

例えば、鏡に対して水平に移動し、移動前の画像と移動後の画像とで動きベクトル（オプディカルフロー等）を求め、この動きベクトルの大きさや向きの分布がその周辺と異なると判定される領域を鏡の領域として推定し、検出部21の検出処理を当該領域の付近のみに限定するように検出補助してよい。なお、自己検出部2以降の各機能ブロックの処理は、この移動前後の画像の両方について行ってもよいし、片方のみについて行ってもよい。

【0051】

・観察体OB1は、可動部品（アクチュエータ78及び移動機構79）の駆動、表示制御部24によって制御される表示部80としての照明の点灯またはモニタ表示の変更のいずれかまたは複数の動作によって観察体OB1の外見を変更し、鏡像の観察体への影響を測る。例えば、赤色のLEDを点灯させ、画像内で当該点灯している領域を検出して、検出部21の検出処理を当該点灯領域の付近のみに限定してよい。

【0052】

・あるいは、観察体OB1は、前進/後退/旋回その他の移動によって観察体の位置を変更し、鏡像の観察体への影響を測るようにしてもよい。
・観察体OB1の外見或いは位置移動に加えた変更が観察体OB1の鏡像へ反映されたとき、その鏡像の観察体を観察体の姿と決定することができる。

【0053】

位置移動の変更を検出する処理として、例えば以下の手順１～３とこれに続く手順４Ａ～４Ｃのいずれかによる方法があげられる。
・＜手順１＞ 撮像部でフィールドFを撮影し、鏡像を含む撮影画像Aを取得する。
・＜手順２＞ アクチュエータ78及び移動機構79を使用し前進後退または旋回を行う。なお、鏡に対して正対した状態でカメラの撮影方向に進むことを前進、その逆方向に進むことを後退と定義する。なお、反射素材検出部21において画像に撮影されている鏡の３次元平面領域を決定しておき、この３次元平面領域を基準として移動するように制御することで、当該前進や後退等を行うことができる。
・＜手順３＞撮像部でフィールドFを撮影し、鏡像を含む撮影画像Bを取得する。

【0054】

・＜手順４Ａ＞ (前進の場合)AとBの差分を比較し、画像上の鏡像に該当する箇所が拡大した場合、変更が観察体OB1の鏡像へ反映されたと判断し、当該拡大した箇所を検出部21での検出結果とする。
・＜手順４Ｂ＞ (後退の場合)AとBの差分を比較し、画像上の鏡像に該当する箇所が縮小した場合、変更が観察体OB1の鏡像へ反映されたと判断し、当該縮小した箇所を検出部21での検出結果とする。
・＜手順４Ｃ＞ (旋回の場合) AとBに対して局所特徴量検出を行い、その位置関係の移動から変更が観察体OB1の鏡像へ反映されたと判断する。例えばroll軸で回転した場合、画像AまたはBに対して回転の画像処理をかけると局所特徴点の検出位置が重なる。yaw軸またはpitch軸方向に回転した場合、奥行き方向に移動する側面に含まれる局所特徴点同士の間隔が狭まり、より回転角度を増やすと順に観測できなくなっていく。検出部21では、当該両画像A,Bで検出された局所特徴量を両画像間においてマッチング（ベクトル等として与えられる局所特徴量の間の類似度計算で同一判定することによるマッチング）したうえで、当該両画像A,B間での局所特徴量に対応する特徴点の位置変化が上記のように旋回移動に整合している箇所を、検出結果とする。なお、前述の手順４Ａ，４Ｂにおける拡大または縮小した箇所の判定も、当該両画像A,B間でマッチングされた局所特徴量に対応する特徴点の位置変化から判定するようにしてもよい。

【0055】

・なお、判断の信頼性の向上のため、前進後退または旋回と、この判定とを繰り返し、位置移動に加えた変更と反映が同期していることを確認してもよい。また、後段側の処理としてステップS4以降及び相対位置推定部3以降の処理は、移動後の画像Bを対象とすることにより、観察体OB1の現在の位置に関する推定が行われるようにすればよい。

【0056】

以上、ステップS3における自己検出部2による各実施形態の処理を説明した。

【0057】

ステップS4では、相対位置推定部3が、ステップS3で自己検出部2が得た画像内での観察体OB1の鏡像MOB1の情報（２次元画像領域の情報）より、予めDB6に登録された既知のサイズ情報を利用して、この鏡像MOB1の、撮像部1のカメラ座標での３次元空間における位置を推定してから、ステップS5へと進む。図５の例EX2では、位置推定部3による処理の模式例として、観察体OB1のサイズ情報SZより、撮像部1の位置（星印（☆）で示す）を基準とした鏡像MOB1の３次元空間位置Pos1を推定することが示されている。

【0058】

相対位置推定部3では具体的に以下のようにすればよい。

【0059】

・DB6に予め登録しておく「観察体のサイズデータ」と、撮影画像中の鏡像との比率を比較し、鏡像の三次元位置を算出する。
・保有する「観察体のサイズデータ」は、体長或いは視覚的に特徴的な観察体中の一部のパーツのサイズを使用すればよい。（当該パーツの各箇所は、自己検出部2で検出する２次元画像上の特徴点の情報（２次元座標及びSIFT等の特徴量の情報）に加えて、その３次元座標情報の情報を紐づけておくことで、３次元座標間の距離として「観察体のサイズデータ」を予め登録しておくことができる。）

【0060】

・「観察体のサイズデータ」と撮影画像中の鏡像とを比較する際に、撮影画像中の鏡像の姿勢が登録時に想定された正面でない場合、その姿勢から「観察体のサイズデータ」に修正を加えた、見かけ上の観察体のサイズデータの値を用いて鏡像の観察体の三次元位置を算出する。
・鏡像の観察体の３次元位置を算出する手法として、具体的には鏡像の観察体の画像上のピクセル数と、カメラの内部パラメータを用いる既存手法が利用可能である。この既存手法として、以下の非特許文献３に開示されるコードが挙げられる。
非特許文献3 Hiroaki Matsuda, Distance Measurement (C++), VENUS ROBOTIX,
https://hmatsudaiac.wixsite.com/venus-robotix/distance-measurement-c (2021/02/02閲覧)

【0061】

当該非特許文献３の手法では球体の認識で３次元位置を推定しているが、任意形状のパーツの特徴点（例えば所定サイズの矩形の４頂点）について３次元座標を登録しておくことで、画像上で当該検出された４頂点との間でマッチング（拡張現実分野等においてカメラの位置姿勢を推定するために用いられている正方マーカ検出に類する手法）を行い、３次元位置を推定することができる。その他にも、２次元画像上の検出点を予めDB6に登録された既知の３次元座標点とマッチングする任意の既存手法により、３次元座標を推定することができる。

【0062】

また、相対位置推定部3では以下のoption1～4のような変形例を用いてもよい。

【0063】

・［option1］観察体の鏡像の３次元位置を算出する手法として、事前にカメラ画像上に写る観察体（鏡像）の見え方と位置関係の関係を表すリストを、シミュレーション上で計算したデータによって求めておくことでDB6に保持し、そのリストと対応させることで三次元位置を算出してもよい。
・［option2］観察体にディスプレイが備わっている場合に、ディスプレイ上にマーカ（前述の拡張現実分野等で用いられる正方マーカ等）あるいは特徴的な表示を行うことで鏡像の観察体の３次元位置を算出してもよい。
・［option3］環境やタスクが許す場合、前進または可動部品の伸縮変形によって鏡像と接触し、その距離がゼロになることを検知して観察体の位置を推定してもよい。
・［option4］深層学習による物体認識か、それに類する画像からの深度推定の既存技術を以って、鏡像との距離を推定してもよい。なお、深層学習による物体認識では、相対位置推定部3の処理と、その前段の自己検出部2による処理と、の結果を同時に得るような深層学習ネットワークを用いてもよいし、自己検出部2の処理と相対位置推定部3の処理とを別途の深層学習ネットワークで実現してもよい。

【0064】

以上、ステップS4の相対位置推定部3を説明した。ステップS5では、ステップS4で相対位置推定部3が推定した鏡像の位置より、反射素材位置推定部4が、高反射素材Rの位置姿勢（撮像部1の３次元カメラ座標での位置姿勢）を推定してから、ステップS6へと進む。

【0065】

反射素材位置推定部4では、撮影画像上で認識された鏡像の観察体を元に、鏡の位置と姿勢を計算する。本実施形態では鏡R（高反射素材R）の領域が長方形としてDB6に予め登録されており、同じく予め登録されている「目標座標の原点と鏡の相対位置関係」は、図５に例EX3として示すように、原点TG（始点）と鏡の下辺の中点M（終点）とを結ぶ３次元ベクトルVとする。このような事前設定では以下の処理で鏡の位置姿勢を計算することができるが、中点M等は一例に過ぎず、任意形状（例えば円や球）で構成される高反射素材Rの任意の所定位置M（例えば重心や中心など）を用いてもよい。

【0066】

・鏡像の観察体の三次元位置（複数の位置の各々）と撮像が行われた位置（撮像部1のカメラ中心の１点の位置）とを結び、その中点を鏡の位置とする。この作業を鏡像の観察体の輪郭部すべて又は一部（輪郭部からサンプリングされる一部）に適用することで鏡像が写る部分について、鏡の位置及び傾きが決定する。輪郭部については、DB6に予め鏡像の観察体のポリゴン等による３次元モデルを登録しておき、検出部21で検出した特徴点等からこの３次元モデルの３次元位置を推定して当該輪郭部を定めてもよいし、検出部21において深層学習等により鏡像の観察体を、矩形のような概形ではなく輪郭部形状をとらえた領域として検出することで、当該輪郭部を２次元画像領域内に定め、鏡像の観察体から検出されるその他の特徴点の３次元位置から、当該輪郭部が当該特徴点と同様の深度を有する平面で構成される等の仮定を用いて、当該輪郭部の３次元位置を定めてもよい。

【0067】

なお、撮像画像から鏡像の観察体を検出部21で検出する際に、撮像部1のカメラ中心の３次元座標も算出可能なように、鏡像の観察体の３次元モデル内での相対位置として、撮像部1のカメラ中心の３次元座標をDB6に予め登録しておいてもよい。この登録情報を用いて、検出結果から撮像部1のカメラ中心の３次元座標を、当該カメラ座標系（撮像部1のカメラ中心を原点とする座標）の３次元座標として定め、当該原点と当該３次元座標とを結ぶ線分の中点を鏡の位置とし、当該中点を通る当該線分の垂直２等分面の向きを鏡の向きとする（当該垂直２等分面に鏡が乗っていると判定する）ようにしてもよい。

【0068】

あるいは、撮像画像から鏡像の観察体を検出部21で検出する際に、鏡像としての撮像部1のカメラ位置を、その外見のレンズ部分を直接検出する等によって直接検出して、上記垂直２等分面を求めるようにしてもよい。

【0069】

図５の例EX2ではこのようにして鏡像MOB1と観察体OB1の輪郭対応点の中点の軌跡として鏡像が写る部分のシルエットSilを求め、当該シルエットSilが乗っている平面領域として、鏡Rの位置及び傾きが求まることが示されている。あるいは、輪郭部を結ぶのではなく、鏡像の観察体上の特徴点（３次元座標位置）を、対応する予めDB6に保持しておく観察体上の特徴点（リファレンス３次元座標）と結んだ中点を複数求め、これら複数の中点が乗る平面を鏡Rのある平面として決定してもよい。

【0070】

・以上の各手法では、鏡の傾きは鏡全体を通して一定であると仮定することで、鏡のある平面を決定することができる。複数の点が乗るものとして鏡の平面を決定する際は、誤差等があるためフィッティングで当該平面を決定してよい。

【0071】

・鏡の形状が長方形であることを前提とした本実施形態では、鏡像の観察体が例えば下方や左右に見切れるとき、その境目（３次元座標での境目の位置）を特徴点マッチング等による画像認識によって割り出し、下方に見切れる場合であれば鏡の下辺全体を検出することで、鏡の形成する長方形における所定位置として例えば下辺の中点を決定することができる。同様に、左右に見切れる場合であれば鏡の左辺又は右辺全体を検出し、その中点等として鏡の形成する長方形における所定位置を決定することができる。図５の例EX1の例は鏡像MOB1が下方に見切れて鏡Rの下辺全体に渡っている例となっている。

【0072】

また、反射素材位置推定部4では以下のoption1～6のような変形例を用いてもよい。

【0073】

・［option1］鏡像の観察体が下方及び左右（鏡の境界）に見切れていない際に、観察体が移動またはその他の手段によって、鏡像の観察体が鏡の下辺等の全体に渡って見切れる状態にし、上記を適用して鏡の長方形の所定位置を決定してもよい。この際、前述の手順４Ａ～４Ｃによる移動及び移動の前後の画像A,Bの解析を所定規則で繰り返す等の試行錯誤的な手法により、見切れる状態となるように制御するようにしてよい。あるいは例えば、鏡を所定サイズとし、鏡の配置を、一定幅のみを有する廊下の壁面の一定高さとし、観察体OB1はその撮像部1の高さが一定値を有するものとしておくことで、撮像部1を構成するハードウェアとしてのカメラの所定画角範囲のもとで、鏡に観察体OB1が写る場合には必ず鏡の下辺の全体に渡って見切れた状態となるようにフィールドFを予め設定しておいてもよい。
・[option2]上記の方法で下辺の中点等の所定位置が決定できない場合（鏡の下辺等の全体ではなく一部のみで見切れる場合や、観察体の全体が鏡の内部にあって鏡の境目で見切れる箇所が存在しない場合など）、エッジ検出または隣り合う画素との比較によって、鏡の境目を検出することで下辺の中点等の所定位置を決定するようにしてよい。
・［option3］鏡の境目を検出する際に、深層学習或いはそれに類する画像からの深度推定の既存技術(例えば以下の非特許文献4)を用いて、鏡の存在する面上で奥行が大きい部分（鏡の存在する平面上では奥行きが平面状に連続変化するのに対して、奥行きが不連続に大きくなり、穴のような深度分布となっている部分）を検出し、鏡の境目としてもよい。
非特許文献4 S. F. Bhat, I. Alhashim, and P. Wonka, "AdaBins: Depth Estimation using Adaptive Bins," arXiv preprint arXiv:2011.14141, 2020.

【0074】

・［option4］鏡の境目を検出する際に、観察体を対称点に鏡と反対方向を撮影する第２のカメラ或いは観察体から鏡までの床面を撮影する第２のカメラを搭載し、その撮影映像と鏡中の鏡像を特徴点マッチングすることで、撮影映像が見切れる部分を割り出し、鏡の境目としてもよい。図６に当該手法の模式例を示す。上段側の例EX4に示されるカメラ等の配置のもとで撮影して、下段側の例EX5に示されるような画像処理を行うことができる。

【0075】

図６の例EX4,5に示されるように、撮像部1は鏡Rを撮像している第１方向D1を向く第１カメラC1（以上の自己検出部2での処理対象となる前面側撮像画像P1を得る第１カメラC1）に加え、その反対側の第２方向D2を向いて撮像する第２カメラC2も追加で有し、第２カメラC2は鏡の存在しない背面側撮像画像P2を撮像する。前面側撮像画像P1では鏡Rの中に鏡像として見切れていない観察体MOB1と、上辺が見切れた背景オブジェクトMBGとが存在する。この上辺が見切れた背景オブジェクトMBG（鏡像）は、見切れていない状態且つ鏡像ではない状態として背面側撮像画像P2に背面オブジェクトBGとして撮像されている。従って、反射素材位置推定部4では、背面側撮像画像P2の背景オブジェクトBGと前面側撮像画像P1の鏡R領域内の鏡像背景オブジェクトMBGとを特徴点マッチングして、鏡像背景オブジェクトMBGの見切れている箇所SGを割り出すことができる。なお、背景オブジェクトOBについてはその局所特徴量を予めDB6に登録しておけばよい。
・［option5］鏡の境目を検出する際に、観察体がLiDAR等のレーザ系のセンサを搭載している場合、その反射値の計測がエラーとなる部分を鏡として認識し、鏡の境目としてもよい。

【0076】

・［option6］平面鏡ではなく広角鏡等の曲率をもつ鏡を使用し、目標座標の原点と鏡との相対位置関係を、目標座標の原点と鏡の中心（後述の図７に例示する球としての中心C1）とを結ぶ三次元ベクトルとしてもよい。この時、観察体上の直線である部位の、鏡像上の歪みを検出し、その歪みの形状を元に鏡の中心を推定する。歪みが一様ではない鏡を利用する場合、その歪みを予め登録しておいてもよい。
計算例）図７に当該模式例を示すように、凸ミラーRを使う場合、自己位置の三次元座標p1（及びp3で直線部位p1-p3を形成）と鏡像上の同一箇所の三次元座標p2（及びp4で直線部位p1-p3が鏡像MOB1上で歪んだ部位p2-p4を形成）を結ぶことで鏡の焦点Fを算出できる。（３次元直線p1-p2と３次元直線p3-p4との交点として鏡の球としての中心C1を求め、光軸AXや焦点fを定めることができる。光学分野で既知のようにレンズの公式「1/a+1/(-b)=1/(-f)」の関係が成立する。）なお、焦点Fから鏡の存在する面Rにおろした垂線の足が鏡Rの中心C2となり、C1-C2の中点が焦点F（2f=r）となる。図７では一様な歪みの球面（放物面の近似としての球面）の場合を例としたが、歪みが一様でない場合もDB6に予め登録しておくことで同様に中心等を求めることができる。例えば、以下の既存手法を用いてもよい。
Park, Jeong Joon, Aleksander Holynski, and Steven M. Seitz. "Seeing the World in a Bag of Chips." Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2020.

【0077】

以上、ステップS5における反射素材位置推定部4による各実施形態の処理を説明した。ステップS6では、相対位置推定部5が、ステップS4で反射素材位置推定部4が推定した観察体OB1の撮像部1のカメラ座標における高反射素材Rの所定位置M（例えば図５の例EX3で示した下辺の中点M）に対して、DB6に予め登録しておく「目標座標の原点と鏡の相対位置関係のデータ」（例えば図５の例EX3での原点TGから中点Mへの３次元ベクトルV）を用いて、鏡Rと観察体OB1と相対位置から、被観察体OB2を基準とした（被観察体OB2上の所定点としての原点TGを基準とした）、観察体OB1の相対座標（位置及び姿勢）を算出し、図４のフローを終了する。

【0078】

この相対座標は、図５の模式例等でも示される通り、以下の２つの相対座標の合成（共通座標系におけるベクトル和）として得ることができる。
（１）「目標座標の原点TGと鏡の（所定位置Mの）相対位置関係のデータ」との相対座標（すなわち、図５の例EX3のベクトルV）
（２）鏡の所定位置Mと観察体OB1との相対座標（すなわち、鏡の所定位置Mを原点とみなしたときの、観察体OB1の撮像部1のカメラの位置ベクトル）

【0079】

以上、本実施形態によれば、鏡Rを被観察体OB2に配置しておくことにより、観察体OB1の鏡像を検出して得られる鏡の相対位置を被観察体OB2と鏡Rとの相対位置（図５の例EX3のベクトルV）に合成するだけで観察体OB1の被観察体OB2に対する相対位置を推定することができ、簡素に測位を行うことが可能となる。

【0080】

以下、種々の補足例、追加例、代替例などに関して説明する。

【0081】

（１） 本実施形態の応用例として、遠隔の熟練者が１箇所以上の現場の作業者等にロボット等を介して遠隔通信を行ってアドバイス等を行う際に、当該ロボット等が現場でスムーズに移動するための準備を簡素化することが可能となる。これにより、アドバイスを実施する熟練者は遠隔地に留まって現場に向かうことを必須とせず、ユーザ移動に必要となるエネルギー資源を節約することで二酸化炭素排出量を抑制できることから、国連が主導する持続可能な開発目標（ＳＤＧｓ）の目標１３「気候変動とその影響に立ち向かうため、緊急対策を取る」に貢献することが可能となる。

【0082】

（２） 図８は、一般的なコンピュータ装置70におけるハードウェア構成の例を示す図である。相対位置推定システム100を構成する相対位置推定装置10、観察体OB1及び被観察体OB2の各々は、このような構成を有する１台以上のコンピュータ装置70として実現可能である。（なお、被観察体OB2に関して、ロボットとしての自律移動が不要な単純な設備（例えば棚）として構成され、情報処理機能が不要である場合には、このようなコンピュータ装置70として構成されなくともよい。）なお、２台以上のコンピュータ装置70で相対位置推定装置10を実現する場合、ネットワーク経由で処理に必要な情報の送受を行うようにしてよい。コンピュータ装置70は、所定命令を実行するCPU（中央演算装置）71、CPU71の実行命令の一部又は全部をCPU71に代わって又はCPU71と連携して実行する専用プロセッサとしてのGPU（グラフィックス演算装置）72、CPU71（及びGPU72）にワークエリアを提供する主記憶装置としてのRAM73、補助記憶装置としてのROM74、通信インタフェース75、ディスプレイ76、マウス、キーボード、タッチパネル等によりユーザ入力を受け付ける入力インタフェース77、ロボットとしての自律移動機能を提供するアクチュエータ78、表示部80及びカメラ81と、これらの間でデータを授受するためのバスBSと、を備える。

【0083】

相対位置推定装置10の各機能部は、各部の機能に対応する所定のプログラムをROM74から読み込んで実行するCPU71及び／又はGPU72によって実現することができる。なお、CPU71及びGPU72は共に、演算装置（プロセッサ）の一種である。ここで、表示関連の処理が行われる場合にはさらに、液晶等のモニタあるいはLED等の点灯装置として構成される表示部80が連動して動作し、データ送受信に関する通信関連の処理が行われる場合にはさらに通信インタフェース75が連動して動作する。撮像部1のカメラはカメラ81として実現してよい。

【符号の説明】

【0084】

10…相対位置推定装置、OB1…観察体、OB2…被観察体、R…高反射素材、M…所定位置、TG…原点
1…撮像部、2…自己検出部、3…相対位置推定部、4…反射素材位置推定部、5…相対位置計算部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】