WO2022190285A1 - 自己位置推定システム及び自己位置推定方法 - Google Patents

Abstract

作業情報投影システムは、作業場における位置を推定する。作業情報投影システムは、カメラ（１３）と、ステレオカメラ（１４）と、制御装置と、を備える。カメラ（１３）は、第１方向を向いて配置され、第１方向及び周囲に配置された物体の情報である第１環境情報を取得する。ステレオカメラ（１４）は、第２方向を向いて配置され、第２方向及び周囲に配置された物体の情報である第２環境情報を取得する。制御装置は、第２環境情報が取得できなくなったと判断した場合に、第１環境情報に基づいて第１座標系における位置を推定する。

Description

自己位置推定システム及び自己位置推定方法

　本発明は、主として、作業場における自己位置を推定する自己位置推定システムに関する。

　特許文献１は、作業者端末と、制御装置と、を備えるシステムを開示する。作業者端末は、作業者の頭部に装着される。作業者端末は、ステレオカメラとプロジェクタを備える。制御装置は、ステレオカメラが撮像した画像に基づいて、作業場にある物体の形状及び位置を示すマップ情報を作成する。制御装置は、更に、マップ情報におけるステレオカメラ（即ち、作業者端末）の位置を推定する。制御装置は、作業者端末の位置に応じた画像を作成し、プロジェクタに送信する。プロジェクタは、制御装置が作成した画像を作業場に投影する。これにより、作業者端末の位置に応じた画像を投影して、作業を補助することができる。

特開２０２０－９８４５１号公報

　特許文献１のシステムでは、ステレオカメラが作業場にある物体の形状及び位置を取得できなくなった場合、作業者端末の位置を適切に推定できなくなる。

　本発明は以上の事情に鑑みてされたものであり、その主要な目的は、１つのセンサが作業場の周囲の環境を取得できなくなった場合でも、自己位置の推定を継続できるシステムを提供することにある。

　本発明の解決しようとする課題は以上の如くであり、次にこの課題を解決するための手段とその効果を説明する。

　本発明の第１の観点によれば、以下の構成の位置推定システムが提供される。即ち、位置推定システムは、作業場における位置を推定する。位置推定システムは、第１センサと、第２センサと、制御装置と、を備える。前記第１センサは、第１方向を向いて配置され、当該第１方向及び周囲に配置された物体の情報である第１環境情報を取得する。前記第２センサは、第２方向を向いて配置され、当該第２方向及び周囲に配置された物体の情報である第２環境情報を取得する。前記制御装置は、前記第１環境情報に基づいて第１座標系における自己位置が算出可能であり、前記第２環境情報に基づいて第２座標系における自己位置が算出可能である。前記制御装置は、前記第２環境情報が取得できなくなった場合に、前記第１環境情報に基づいて自己位置を推定する。

　本発明の第２の観点によれば、以下の位置推定方法が提供される。即ち、位置推定方法は、第１取得工程と、第２取得工程と、位置推定工程と、を含む。前記第１取得工程では、第１方向を向いて作業場に配置され、当該第１方向及び周囲に配置された物体の情報である第１環境情報を第１センサで取得する。前記第２取得工程では、第２方向を向いて前記作業場に配置され、当該第２方向及び周囲に配置された物体の情報である第２環境情報を第２センサで取得する。前記位置推定工程では、前記第２環境情報が取得できなくなった場合に、前記第１環境情報に基づいて自己位置を推定する。

　これにより、第２センサで環境情報が取得ができなくなった場合でも、作業場における位置の推定を継続することができる。

　本発明によれば、１つのセンサが作業場の周囲の環境を取得できなくなった場合でも、自己位置の推定を継続できるシステムを提供できる。

本発明の一実施形態に係る作業情報投影システムの概要図。 作業情報投影システムのブロック図。 較正処理を示すフローチャート。 較正治具に較正画像を投影している様子を示す斜視図。 ワークに基準マーカを設置して計測を行う様子を示す斜視図。 作業時における処理を示すフローチャート。 ＳＬＡＭ計測を行う状況、ＳＬＡＭ計測を行いつつ変換情報を算出する状況を説明する図。 マーカ計測を行う状況、再びＳＬＡＭ計測に戻る状況を説明する図。

　次に、図面を参照して本発明の実施形態を説明する。初めに、図１及び図２を参照して、作業情報投影システム１及び位置認識方法の概要について説明する。

　本実施形態の作業情報投影システム（位置認識システム）１は、作業場に設けられる。作業場とは、作業を行うための場所であり、例えば、工場、オフィス、又は施設である。作業とは、作業者が手作業で、道具を使って、又は機械を操作して、対象物に何かを行うことであり、例えば、部品の組立や塗装、清掃、運搬等である。本実施形態では、作業者は、工場に配置されたワーク３１に対して部品を組み付ける作業を行う。

　作業情報投影システム１は、作業場における位置を認識し、認識した位置に基づいて、作業場に補助画像１０１を投影する。補助画像１０１とは、作業を補助する画像であり、例えば、作業内容、作業位置、又は作業順序等を作業者に伝えるための画像である。図１に示すように、本実施形態の補助画像１０１は、ワーク３１に対して投影されており、組み付ける部品の名称と、組付け位置と、を示す。作業情報投影システム１は、ワーク３１の位置を認識し（詳細は後述）、ワーク３１の適切な位置に補助画像１０１を投影する。

　図１及び図２に示すように、作業情報投影システム１は、台車１１と、プロジェクタ１２と、カメラ１３と、ステレオカメラ１４と、制御装置２０と、を備える。以下の説明では、特に断らない限り、「位置」は、物体が存在している場所だけでなく、物体がどの方向を向いているかを含むものとする。従って、例えば位置関係という用語は、２つの物体の相対位置だけでなく、相対的な向きも含む。

　台車１１は、車輪及び台座を備えている。本実施形態の台車１１は、駆動源を備えておらず、作業者に押されることによって作業場内を移動する。なお、台車１１は、駆動源を備え、自律走行可能であってもよい。台座は、プロジェクタ１２、カメラ（第１センサ）１３、及びステレオカメラ（第２センサ）１４等を支持する。台車１１は、床又は天井等に設けられたレールに沿って移動可能であってもよい。また、台車１１は、必須の構成要素ではなく、省略することもできる。

　プロジェクタ１２は、台車１１に載せられている。プロジェクタ１２は、制御装置２０の制御により、補助画像１０１を投影する。

　カメラ１３及びステレオカメラ１４は、プロジェクタ１２の上面に固定されている。これにより、プロジェクタ１２とカメラ１３とステレオカメラ１４のそれぞれの相対位置は変化しない。言い換えれば、プロジェクタ１２とカメラ１３とステレオカメラ１４は、一体的に移動する。なお、プロジェクタ１２とカメラ１３とステレオカメラ１４の取付方法は、本実施形態とは異なっていてもよい。例えば、台車１１に、プロジェクタ１２とカメラ１３とステレオカメラ１４が取り付けられていてもよい。あるいは、台車１１に支持部材が取り付けられており、支持部材に、プロジェクタ１２とカメラ１３とステレオカメラ１４が取り付けられていてもよい。

　以下の説明において、プロジェクタ１２、カメラ１３、ステレオカメラ１４の向く方向とは、それぞれの機器から光軸が延びる方向である。光軸とは、光学素子（撮像素子、発光素子）を通る点を軸方向に延長した直線である。

　カメラ１３は、撮像素子が１つの単眼カメラである。カメラ１３の向く方向（第１方向、図１を参照）は、プロジェクタ１２の向く方向と実質的に同じである。実質的に同じ方向とは、例えば、平面視において２つの光軸の差が１５度以下又は１０度以下であり、かつ、２つの光軸の仰角の差が１５度以下又は１０度以下となることである。別の観点から説明すると、カメラ１３の光軸は、プロジェクタ１２が投影光を投影可能な範囲と重なっている。つまり、投影光は、プロジェクタ１２を起点として広がる空間を形成するが、この空間と、カメラ１３の光軸と、が交差している。このように、カメラ１３の向く方向は、プロジェクタ１２の向く方向と実質的に同じなので、カメラ１３は、プロジェクタ１２が投影した画像を撮像することができる。

　また、工場内の適宜の位置（例えば、ワーク３１の表面）には位置計測のための基準マーカ５１及び補間マーカ５２が設置されている。カメラ１３が撮像する画像には、基準マーカ５１又は補間マーカ５２が含まれることもある。

　ステレオカメラ１４の向く方向（第２方向、図１）は、第１方向とは異なるとともに、プロジェクタ１２の向く方向とも異なる。異なる方向とは、平面視において、例えば２つの光軸の差が３０度以上、６０度以上、又は９０度以上である（本実施形態では平面視における光軸の差は１８０度）。あるいは、異なる方向とは、２つの光軸の仰角の差が３０度以上又は６０度以上であることを指してもよい。従って、ステレオカメラ１４は、プロジェクタ１２が投影した画像を撮像することはできない。ステレオカメラ１４は、工場内に設置された設備、機器、工具、及びワーク３１等の物体を撮像する。図１では、ステレオカメラ１４が撮像する物体の例として、棚５３が示されている。

　ステレオカメラ１４は、２つの撮像素子を備えており、それぞれの撮像素子が個別に作業場を撮像する。２つの撮像素子は、互いに適宜の距離だけ離して配置される。それぞれの撮像素子は、例えばＣＣＤ（Ｃｈａｒｇｅ　Ｃоｕｐｌｅｄ　Ｄｅｖｉｃｅ）である。２つの撮像素子は互いに同期して動作し、作業場を同時に撮影することで、一対の画像データを作成する。本実施形態では、リアルタイムで検出した情報を補助映像として投影することが想定されているため、ステレオカメラ１４は例えば１秒間に複数回の撮影を行うことが好ましい。

　また、ステレオカメラ１４は、この一対の画像データを処理する画像処理部を備える。画像処理部は、ステレオカメラ１４により得られた一対の画像データに対して公知のステレオマッチング処理を行うことによって、それぞれの画像が対応する位置のズレ（視差）を求める。視差は、写ったものとの距離が近いほど、距離に反比例して大きくなる。画像処理部は、この視差に基づいて、画像データの各画素に距離の情報を対応付けた距離画像を作成する。

　ステレオカメラ１４は、１つの筐体内に２つの撮像素子が配置される構成である。これに代えて、個別の２つのカメラを組み合わせてステレオカメラを構成してもよい。また、画像処理部は、ステレオカメラ１４とは別の装置（例えば制御装置２０）に設けられてもよい。

　制御装置２０は、ＣＰＵ、ＲＯＭ及びＲＡＭ等を備えたコンピュータである。本実施形態の制御装置２０は、台車１１に配置される。制御装置２０は、図略の信号線を介して、プロジェクタ１２、カメラ１３、及びステレオカメラ１４と通信可能である。なお、制御装置２０は、台車１１の外部に配置されてもよい。この場合、制御装置２０は、例えば無線でプロジェクタ１２、カメラ１３、及びステレオカメラ１４と通信する。

　制御装置２０は、カメラ１３が撮像した画像（第１環境情報）を取得するとともに（第１取得工程）、ステレオカメラ１４が撮像した距離画像（第２環境情報）を取得する（第２取得工程）。制御装置２０は、これらの情報及びその他の情報に基づいて補助画像１０１を作成し、プロジェクタ１２へ送信する。図１に示すように、制御装置２０は、通信装置２１と、解析部２２と、画像作成部２３と、投影制御部２４と、を備える。制御装置２０が備える各部は、制御装置２０が行う処理毎に（制御装置２０が有する機能毎に）制御装置２０を概念的に分けたものである。本実施形態の制御装置２０は１台のコンピュータにより実現されているが、制御装置２０は複数台のコンピュータから構成されていてもよい。この場合、これらの複数のコンピュータは、ネットワークを介して接続される。

　通信装置２１は、プロジェクタ１２、カメラ１３、及びステレオカメラ１４と通信するための通信モジュールであり、例えば信号線を接続するためのコネクタ、又は、無線通信のためのアンテナ等を含んでいる。通信装置２１は、カメラ１３が撮像した画像を受信したり、ステレオカメラ１４が撮像した画像を受信したり、画像作成部２３が作成した補助画像１０１をプロジェクタ１２へ送信したりする。

　解析部２２は、カメラ１３が撮像した画像に基準マーカ５１又は補間マーカ５２が含まれる場合、基準マーカ５１又は補間マーカ５２の位置、大きさ、歪み具合等に基づいて公知の解析処理を行うことにより、基準マーカ５１又は補間マーカ５２に対するカメラ１３の相対位置（自己位置）を算出する。なお、自己位置とは、計測を行う機器自身の位置であり、カメラ１３が撮像した画像に基づいて位置を算出する場合は、カメラ１３（又は作業情報投影システム１）の位置を示すものとする。

　また、解析部２２は、ステレオカメラ１４が撮像した距離画像に対してＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ　Ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　Ｍａｐｐｉｎｇ）処理を行う。解析部２２は、距離画像を解析することで、作業場にある物体の形状及び位置を示すマップ情報（環境地図）を作成するとともにステレオカメラ１４の位置（自己位置）を推定する。

　ＳＬＡＭ処理は公知であるため、以下では簡単に説明する。即ち、解析部２２は、距離画像を解析することにより、適宜の特徴点を設定し、その動きを取得する。そして、解析部２２は、距離画像から複数の特徴点を抽出して追跡することで、画像に相当する平面内での特徴点の動きをベクトルで表現したデータを求める。解析部２２は、このデータに基づいて、マップ情報を作成する。マップ情報は、上述したように作業場にある物体の形状及び位置を示すデータであり、より詳細には、抽出した複数の特徴点（点群）の３次元位置を示すデータである。また、解析部２２は、入力された特徴点の位置及び距離の変化と、マップ情報における当該特徴点の位置と、に基づいて、ステレオカメラ１４の位置の変化を推定する。なお、ＳＬＡＭ処理は、撮像素子が１つの単眼カメラが撮影した画像に基づいて行うこともできる。従って、ステレオカメラ１４に代えて単眼カメラを設けてもよい。

　画像作成部２３は、補助画像１０１を作成する。制御装置２０は、作業に関する情報である作業情報を記憶している。本実施形態の作業情報は、ワーク３１に取り付ける部品の名称、及び、部品の取付位置である。画像作成部２３は、作業情報と、カメラ１３又はステレオカメラ１４が撮像した画像に基づいて推定した位置と、プロジェクタ１２が投影すべき補助画像１０１を作成する。

　投影制御部２４は、画像作成部２３が作成した補助画像１０１をプロジェクタ１２に送信して、補助画像１０１を投影させる。以上により、作業場に補助画像１０１を投影することができる。

　以下、プロジェクタ１２の位置の推定に関して、詳細に説明する。初めに、図３から図５を参照して、作業の前に行う較正処理について説明する。

　初めに、較正治具３２をプロジェクタ１２の前方に配置する。較正治具３２は、プロジェクタ１２とカメラ１３の較正を行うための部材である。制御装置２０は、較正処理を開始する旨の指示を受領すると、プロジェクタ１２に較正画像１０２を送信する。これにより、図４に示すように、プロジェクタ１２が較正画像１０２を較正治具３２に投影する（Ｓ１０１）。カメラ１３は、較正治具３２に投影された較正画像１０２を撮像する（Ｓ１０２）。

　次に、制御装置２０は、カメラ１３が撮像した画像に含まれている較正画像１０２の位置、向き、大きさ、歪み度合い等に基づいて、公知の解析方法を行うことにより、プロジェクタ１２とカメラ１３の相対的な位置関係を算出する（Ｓ１０３）。

　次に、基準マーカ５１を設置する。基準マーカ５１を設置した位置が基準座標系（マーカ座標系）の原点となる。本実施形態では、マーカ（基準マーカ５１又は補間マーカ５２）を原点とする座標系をマーカ座標系と称し、その中で基準マーカ５１を原点とする座標系を特に基準座標系と称する。基準座標系とは、上述の作業指示を記述するため等に用いられる座標系である。基準座標系の原点の位置は任意であるが、例えばワーク３１に対して作業を行う場合は、ワーク３１上に原点を設置することが好ましい。これにより、仮にワーク３１の位置が僅かに変化した場合でも、補助画像１０１を投影する位置がズレにくい。

　図５に示すように、基準マーカ５１を設置した後に、カメラ１３は基準マーカ５１を含む範囲を撮像し、ステレオカメラ１４は周囲の作業場を撮像する（Ｓ１０４）。次に、制御装置２０は、基準座標系（マーカ座標系）とＳＬＡＭ座標系の変換情報を算出する（Ｓ１０５）。

　変換情報は、基準座標系とＳＬＡＭ座標系の間で座標変換を行うための情報である。言い換えれば、変換情報は、カメラ１３とステレオカメラ１４の位置関係を示す情報である。制御装置２０は、カメラ１３が撮像した画像に含まれる基準マーカ５１の位置、大きさ、歪み具合等に基づいて、基準マーカ５１を基準とした（即ち基準座標系における）カメラ１３の位置を算出する。制御装置２０は、ステレオカメラ１４が撮像した距離画像に基づいて上述のＳＬＡＭ処理を行うことにより、ＳＬＡＭ座標系におけるステレオカメラ１４の位置を算出する。そして、制御装置２０は、同じタイミングで撮像した基準座標系におけるカメラ１３の位置と、ＳＬＡＭ座標系におけるステレオカメラ１４の位置と、を１組として、複数組の位置情報を算出する。

　このようにして算出された複数組の位置情報に基づいて、変換情報が作成される。具体的には、変換情報は、図３に示す式（１）に基づいて算出される。式（１）の左辺は、「カメラ１３の位置からステレオカメラ１４の位置へのベクトル」と「ステレオカメラ１４の向きのベクトル」の内積を示している。カメラ１３とステレオカメラ１４は相対移動しないので、内積の値であるλは一定である。従って、複数組の位置情報を式（１）に代入することにより、ＳＬＡＭ座標系でのマーカ原点座標（ｔ）と、ＳＬＡＭ座標系からマーカ座標系への回転座標（Ｒ）を算出することができる。これらの値が変換情報に相当する。

　変換情報を用いることにより、ＳＬＡＭ処理で推定したＳＬＡＭ座標系のステレオカメラ１４の位置及び向きを、基準座標に変換できる。具体的には、基準座標系でのステレオカメラ１４の位置は、Ｒ（Ｐ_S－ｔ）と示され、基準座標系でのステレオカメラ１４の向きはＲｄ_sと示される。なお、同様の計算を行うことにより、マーカ原点座標系からＳＬＡＭ座標系へ変換する変換情報を算出することもできる。

　次に、作業時における処理について、図６から図８を参照して説明する。以下の説明では、ステレオカメラ１４が撮像した距離画像に基づいてＳＬＡＭ処理を行って位置を推定することをＳＬＡＭ計測と称する。カメラ１３が撮像した画像に含まれる補間マーカ５２に基づいて位置を推定することをマーカ計測と称する。

　本実施形態では、ＳＬＡＭ計測をベースとしており、ＳＬＡＭ計測を適切に行うことができない場合に、マーカ計測を行う。ＳＬＡＭ計測を適切に行うことができない場合とは、ステレオカメラ１４が適切な情報を取得できない場合であり、具体的にはステレオカメラ１４が平坦な壁のみを撮像して、特徴点の数が少ない場合等である。以下、具体的に説明する。

　制御装置２０は、ステレオカメラ１４が撮像した距離画像からＳＬＡＭ座標系の位置を推定し、較正処理で算出した変換情報を適用することにより、基準座標系の位置に変換する（Ｓ２０１、位置推定工程）。次に、制御装置２０は、カメラ１３が撮像した画像に補間マーカ５２が含まれているか否かを判定する（Ｓ２０２）。

　図７の「１．ＳＬＡＭ計測」と記載された状況では、カメラ１３が撮像する画像に補間マーカ５２が含まれていない。このような場合、制御装置２０は、ステレオカメラにより得られた基準座標系の位置に基づいて補助画像１０１を作成して、プロジェクタ１２から投影する（Ｓ２０６）。

　図７の「２．ＳＬＡＭ計測，変換情報算出」と記載された状況では、カメラ１３が撮像する画像に補間マーカ５２が含まれている。このような場合、制御装置２０は、補間マーカ５２のマーカ座標系を基準座標系に変換する変換情報を算出する（Ｓ２０３）。この変換情報を算出する処理は、較正処理のステップＳ１０５と同様である。即ち、補間マーカ５２を原点としたマーカ座標系における位置と、ＳＬＡＭ座標系における位置と、の両方が得られているため、それらを組にして式（１）に代入する。これにより、補間マーカ５２のマーカ座標系をＳＬＡＭ座標系に変換する変換情報を算出できる。また、ＳＬＡＭ座標系を基準座標系に変換する変換情報は、ステップＳ１０５で既に算出済みである。従って、２つの変換情報を組み合わせることにより、補間マーカ５２のマーカ座標系を基準座標系に変換する変換情報を算出できる。

　次に、制御装置２０は、ＳＬＡＭ処理の特徴点が閾値以下か否かを判定する（Ｓ２０４）。言い換えれば、制御装置２０は、第２環境情報を取得できているか否かを判定する。図７の「２．ＳＬＡＭ計測，変換情報算出」と記載された状況では、ステレオカメラ１４が棚５３を検出していること等から十分な数の特徴点がある。このような場合、補間マーカ５２を用いた位置計測は用いずに、ＳＬＡＭ計測に基づく位置計測を用いる。つまり、制御装置２０は、ステレオカメラ１４により得られた基準座標系の位置に基づいて補助画像１０１を作成して、プロジェクタ１２から投影する（Ｓ２０６）。

　ステップＳ２０４の処理を言い換えると、ステレオカメラ１４から取得した情報又はそれに基づいて算出された情報が適切か否かを判定するための判定条件があり、その判定条件を満たすか否かを制御装置２０が判断する、ということである。

　図８の「３．マーカ＋カメラ計測」と記載された状況では、ステレオカメラ１４が撮像する範囲に棚５３等の物体が存在していないため、ＳＬＡＭ処理の特徴点が閾値以下となる。このような場合、ＳＬＡＭ計測に基づく位置計測は用いずに、補間マーカ５２を用いた位置計測を用いる。従って、制御装置２０は、カメラ１３が撮像した画像（補間マーカ５２が含まれる画像）から得られるマーカ座標系の位置を基準座標系の位置に変換する（Ｓ２０５、位置推定工程）。この変換は、ステップＳ２０３で算出した変換情報を用いて行われる。その後、制御装置２０は、カメラ１３により得られた基準座標系の位置に基づいて補助画像１０１を作成してプロジェクタ１２から投影する（Ｓ２０７）。

　なお、ＳＬＡＭ処理による特徴点が閾値以下となる状況は、事前に予測可能である。従って、本実施形態では、ＳＬＡＭ処理による特徴点が閾値以下となる状況におけるカメラ１３の近傍に補間マーカ５２を設置している。これにより、ＳＬＡＭ計測とマーカ計測の少なくとも一方を実行可能となる。

　図６に示すフローチャートは繰り返し実行される。従って、図８の「３．マーカ＋カメラ計測」と記載された状況の後に、図８の「４．ＳＬＡＭ計測」と記載された状況になった場合、即ち、ＳＬＡＭ処理の特徴点が閾値を超えるようになった場合は、補間マーカ５２を用いた位置計測を止めて、ＳＬＡＭ計測に基づく位置計測を用いる。

　以上により、ＳＬＡＭ計測をベースとしつつ、ＳＬＡＭ計測による位置の推定精度が低下する状況でマーカ計測を行うことにより、作業場における位置の推定精度を高く維持することができる。

　以上に説明したように、本実施形態の作業情報投影システム１は、作業場における位置を推定する位置推定方法を行う。作業情報投影システム１は、カメラ１３と、ステレオカメラ１４と、制御装置２０と、を備える。カメラ１３は、第１方向を向いて配置され、第１方向及び（第１方向の）周囲に配置された物体の情報である第１環境情報を取得する。ステレオカメラ１４は、第２方向を向いて配置され、第２方向及び（第２方向の）周囲に配置された物体の情報である第２環境情報を取得する。制御装置２０は、第１環境情報に基づいて第１座標系における自己位置が算出可能であり、第２環境情報に基づいて第２座標系における自己位置が算出可能である。制御装置２０は、第２環境情報が取得できなくなった場合は、第１環境情報に基づいて第１座標系における位置を推定する。

　これにより、ステレオカメラ１４で環境情報が取得ができなくなった場合でも、作業場における位置の推定を継続することができる。

　本実施形態の位置推定システムにおいて、ステレオカメラ１４は、カメラ１３と一体的に移動するように配置されている。

　これにより、カメラ１３とステレオカメラ１４の位置関係が変化しないので、より適切に作業場の位置を推定できる。

　本実施形態の位置推定システムにおいて、第１方向と第２方向は異なる方向である。

　これにより、カメラ１３とステレオカメラ１４の検知範囲が大きく異なるので、一方のセンサが他方のセンサを補うことができる。

　本実施形態の作業情報投影システム１において、制御装置２０は、第１環境情報と第２環境情報とが取得できると判断した場合は第１座標系と第２座標系の変換を行う変換情報を算出する。

　これにより、位置計測に使用するセンサを切り替えた場合であっても、同じ座標系を使い続けることができる。

　本実施形態の作業情報投影システム１は、作業を補助する補助画像１０１を作業場に投影するプロジェクタ１２を備える。制御装置２０は、作業場における位置に応じた補助画像１０１を作成して、補助画像１０１をプロジェクタ１２へ送信する。

　これにより、作業者の作業を補助することができる。特に、本実施形態の作業情報投影システム１は自己位置を見失いにくいので、高い信頼性を実現できる。

　本実施形態の作業情報投影システム１において、カメラ１３の光軸は、プロジェクタ１２が補助画像１０１を投影可能な領域と重なっている。カメラ１３は、作業場に設けられたマーカを含む範囲を撮像するカメラ１３である。ステレオカメラ１４は、第２方向及びその周囲に配置された物体を撮像するためのステレオカメラ１４である。

　これにより、プロジェクタ１２が投影した補助画像１０１をステレオカメラ１４が撮像することがないので、補助画像１０１が特徴点として認識されることを防止できる。

　本実施形態の作業情報投影システム１において、制御装置２０は、プロジェクタ１２が補助画像１０１を作業場に投影する作業中において、第１環境情報と第２環境情報とが取得できると判断した場合は、第１座標系と第２座標系の変換を行う変換情報を算出する。

　これにより、作業中に必要な変換情報を算出できるので、事前の準備の手間を軽減できる。

　本実施形態の位置推定システムでは、カメラ１３の位置からステレオカメラ１４の位置へのベクトルと、ステレオカメラ１４の向きを示すベクトルと、の内積を算出する式に基づいて、変換情報を算出する。

　これにより、簡単な処理で変換情報を算出できる。

　以上に本発明の好適な実施の形態を説明したが、上記の構成は例えば以下のように変更することができる。

　上記実施形態では、プロジェクタ１２と実質的に同じ方向を向く第１センサがカメラ１３であり、その反対側の第２センサがステレオカメラ１４である。これに代えて、第１センサがステレオカメラ１４であり、第２センサがカメラ１３であってもよい。この場合、ワーク３１ではなく、作業場の適宜の位置に補間マーカ５２が設置される。

　上記実施形態では、第１センサと第２センサ（即ち、カメラ１３とステレオカメラ１４）が異なる方向を向く。これに代えて、第１センサと第２センサの検知範囲が異なるのであれば（例えば検知距離が異なる、検知範囲の水平方向の広さが異なる、検知範囲の仰俯方向の広さが異なる等）、第１センサと第２センサが同じ方向を向いてもよい。言い換えれば、第１方向と第２方向は同じ方向であってもよい。この場合、第１センサで検出できない範囲を第２センサで検出できるので、上記実施形態と同じ処理が適用できる。

　上記実施形態では、３次元計測装置の例としてステレオカメラ１４を挙げて説明したが、それ以外の装置、例えばＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　ａｎｄ　Ｒａｎｇｉｎｇ）を用いてもよい。ＬＩＤＡＲとは、様々な方向に電波を照射するとともに、当該電波の反射波を受信するまでの時間を計測することで、周囲の物体等の位置及び形状を取得する技術である。

　上記実施形態で示したフローチャートは一例であり、一部の処理を省略したり、一部の処理の内容を変更したり、新たな処理を追加したりしてもよい。例えば、上記実施形態では図６に示すようにＳＬＡＭ計測とマーカ計測のうちＳＬＡＭ計測を優先的に行うが、これに代えて、マーカ計測を優先的に行ってもよい。つまり、カメラ１３が撮像する画像に補間マーカ５２が含まれる場合は常にマーカ計測を行い、カメラ１３が撮像する画像に補間マーカ５２が含まれない場合はＳＬＡＭ計測を行う。

　上記実施形態では、推定した位置を基準座標系に変換する処理を行ったが、その処理を省略してもよい。

　本発明の位置推定システムは、作業情報投影システムに限られず、自己位置を推定する様々なシステムに適用できる。例えば、自律移動を行う走行システムに本発明を適用できる。

　１　作業情報投影システム（位置認識システム）
　１１　台車
　１２　プロジェクタ
　１３　カメラ（第１センサ）
　１４　ステレオカメラ（第２センサ、３次元計測装置）
　２０　制御装置

Claims (9)

1. 　作業場における位置を推定する位置推定システムにおいて、
   　第１方向を向いて配置され、当該第１方向及び周囲に配置された物体の情報である第１環境情報を取得する第１センサと、
   　第２方向を向いて配置され、当該第２方向及び周囲に配置された物体の情報である第２環境情報を取得する第２センサと、
   　前記第１環境情報に基づいて第１座標系における自己位置が算出可能であり、前記第２環境情報に基づいて第２座標系における自己位置が算出可能であり、前記第２環境情報が取得できなくなった場合に、前記第１環境情報に基づいて自己位置を推定する制御装置と、
   を備えることを特徴とする位置推定システム。
2. 　請求項１に記載の位置推定システムであって、
   　前記第２センサは、前記第１センサと一体的に移動するように配置されていることを特徴とする位置推定システム。
3. 　請求項１又は２に記載の位置推定システムであって、
   　前記第１方向が前記第２方向と異なる方向であることを特徴とする位置推定システム。
4. 　請求項１から３までの何れか一項に記載の位置推定システムであって、
   　前記制御装置は、前記第１環境情報と前記第２環境情報とが取得できる場合は前記第１座標系と前記第２座標系の変換を行う変換情報を算出することを特徴とする位置推定システム。
5. 　請求項１から４までの何れか一項に記載の位置推定システムであって、
   　作業を補助する補助画像を前記作業場に投影するプロジェクタを備え、
   　前記制御装置は、前記作業場における位置に応じた前記補助画像を作成して、当該補助画像を前記プロジェクタへ送信することを特徴とする位置推定システム。
6. 　請求項５に記載の位置推定システムであって、
   　前記第１センサの光軸は、前記プロジェクタが前記補助画像を投影可能な領域と重なっており、
   　前記第１センサは、前記作業場に設けられたマーカを含む範囲を撮像するカメラであり、
   　前記第２センサは、前記第２方向及び周囲に配置された物体の形状及び位置を取得するための３次元計測装置であることを特徴とする位置推定システム。
7. 　請求項６に記載の位置推定システムであって、
   　前記制御装置は、前記プロジェクタが前記補助画像を前記作業場に投影する作業中において、前記第１環境情報と前記第２環境情報とが取得できる場合は、前記第１座標系と前記第２座標系の変換を行う変換情報を算出することを特徴とする位置推定システム。
8. 　請求項４に記載の位置推定システムであって、
   　前記第１センサの位置から前記第２センサの位置へのベクトルと、第２センサの向きを示すベクトルと、の内積を算出する式に基づいて、前記変換情報を算出することを特徴とする位置推定システム。
9. 　第１方向を向いて作業場に配置され、当該第１方向及び周囲に配置された物体の情報である第１環境情報を第１センサで取得する第１取得工程と、
   　第２方向を向いて前記作業場に配置され、当該第２方向及び周囲に配置された物体の情報である第２環境情報を第２センサで取得する第２取得工程と、
   　前記第２環境情報が取得できなくなった場合に、前記第１環境情報に基づいて自己位置を推定する位置推定工程と、
   を含むことを特徴とする位置推定方法。

Images