JP2020118586A

JP2020118586A - 移動体

Info

Publication number: JP2020118586A
Application number: JP2019011111A
Authority: JP
Inventors: 田中　稔; Minoru Tanaka; 稔田中; 亮暢藤井; Akinobu Fujii; 祐司津坂; Yuji Tsusaka; 達也三田; Tatsuya Mita; 保紀野々垣; Yasunori Nonogaki
Original assignee: Toyota Industries Corp; Toyota Central R&D Labs Inc
Current assignee: Toyota Industries Corp; Toyota Central R&D Labs Inc
Priority date: 2019-01-25
Filing date: 2019-01-25
Publication date: 2020-08-06
Anticipated expiration: 2039-01-25
Also published as: JP7181800B2

Abstract

【課題】対象物の位置および姿勢を認識する移動体を提供する。【解決手段】移動体は、複数のマーカが第１所定距離だけ離して配置されているマーカ群を備えた対象物の位置および姿勢を認識する。移動体は、マーカ群を撮像するための光学センサを備える。移動体は、移動体の位置および姿勢を算出する移動体位置姿勢算出部を備える。移動体は、対象物を撮像した画像に基づいてマーカ群の位置および姿勢を推定するマーカ群位置姿勢推定部を備える。移動体は、複数のセンサノードと、マーカノードと、複数のセンサノードとマーカノードとの相対的位置関係を規定する複数のアークと、を備えるグラフ構造データを生成するグラフ構造データ生成部を備える。移動体は、生成したグラフ構造データを、複数のアークから算出される誤差関数の和が最小となるように最適化することで、マーカ群の位置および姿勢の推定値を補正する補正部を備える。【選択図】図２

Description

本明細書に開示する技術は、対象物の位置および姿勢を認識する移動体に関する。

特許文献１には、マーカをパレットや荷台などの荷役対象物に貼り付け、カメラで撮像したマーカ画像に対してパターンマッチング処理を行うことで、マーカの位置や大きさを算出する技術が開示されている。これにより、フォークリフトに対する荷役対象物の相対的な位置や姿勢を検出している。

特許第３９２１９６８号公報

特許文献１の技術は、荷役対象物の奥行方向の３次元的な姿勢は算出できない。奥行方向の３次元的な姿勢の一例としては、荷役対象物のヨー方向（上下を軸とした回転方向）やピッチ方向（左右を軸とした回転方向）の回転姿勢が挙げられる。本明細書は、荷役対象物の三次元的な位置姿勢を高精度に推定することが可能な技術を開示する。

本明細書に開示する移動体の一実施形態は、複数のマーカが予め定められた第１所定距離だけ離して配置されているマーカ群を備えた対象物の位置および姿勢を認識する移動体である。移動体は、マーカ群を撮像するための光学センサを備える。移動体は、移動体が移動した１つまたは複数の移動点の各々で光学センサを用いて対象物を撮像した画像に基づいて、１つまたは複数の移動点の各々においてマーカ群の位置および姿勢を推定するマーカ群位置姿勢推定部を備える。移動体は、複数のセンサノードと、マーカノードと、複数のセンサノードとマーカノードとの相対的位置関係を規定する複数のアークと、を備えるグラフ構造データを生成するグラフ構造データ生成部を備える。移動体は、生成したグラフ構造データを、複数のアークから算出される誤差関数の和が最小となるように最適化することで、マーカ群の位置および姿勢の推定値を補正する補正部を備える。複数のセンサノードは、１つまたは複数の移動点のそれぞれにおける光学センサの位置および姿勢を示すノードである。マーカノードは、マーカ群位置姿勢推定部で推定されたマーカ群の位置および姿勢を示すノードである。

上記の移動体では、１つまたは複数の移動点のそれぞれにおける光学センサの位置および姿勢を示す複数のセンサノードと、マーカ群位置姿勢推定部で推定されたマーカ群の位置および姿勢を示すマーカノードと、複数のセンサノードとマーカノードとの相対的位置関係を規定する複数のアークと、を備えたグラフ構造データを生成することができる。そしてグラフ構造データを最適化することで、マーカ群の位置および姿勢の推定値を補正することができる。１つまたは複数の移動点の各々において推定した、マーカ群の位置および姿勢の複数の推定結果を用いて、全体最適化することができる。よって、ある１つの移動点で推定したマーカ群の位置および姿勢の推定結果を用いる場合と比して、マーカ群の位置および姿勢をより正確に推定することが可能となる。

移動体が時刻ｔ_ｉの移動点から時刻ｔ_ｉ＋１の移動点まで移動した場合に、時刻ｔ_ｉ＋１の移動点における移動体の位置および姿勢を算出する移動体位置姿勢算出部をさらに備えてもよい。マーカ群位置姿勢推定部は、時刻ｔ_ｉ＋１の移動点においてマーカ群の位置および姿勢を推定してもよい。グラフ構造データ生成部は、時刻ｔ_ｉ＋１の移動点におけるセンサノードおよびマーカノードと、時刻ｔ_ｉ＋１の移動点におけるセンサノードとマーカノードとの相対的位置関係を規定するアークと、を生成して、グラフ構造データに追加してもよい。効果の詳細は実施例で説明する。

移動体は、対象物を保持可能な支持構造部を備えていてもよい。移動体は、補正部によって補正されたマーカ群の位置および姿勢に基づいて、支持構造部をアクセスするアクセス位置を推定するアクセス位置推定部をさらに備えていてもい。効果の詳細は実施例で説明する。

移動体は、補正部によって補正されたマーカ群の位置および姿勢に基づいて、移動体の現在位置から対象物までの軌道および速度パターンを生成する軌道生成部をさらに備えていてもよい。移動体は、生成した軌道および速度パターンに基づいて移動体を対象物まで移動させる移動制御部をさらに備えていてもよい。移動体は、アクセス位置推定部で推定したアクセス位置に基づいて、支持構造部をアクセス位置にアクセスさせるアクセス制御部をさらに備えていてもよい。効果の詳細は実施例で説明する。

支持構造部はフォークであってもよい。対象物はパレットであってもよい。光学センサは、フォークの動作と一体化して動作してもよい。効果の詳細は実施例で説明する。

マーカ群は、水平方向に第１所定距離だけ離れた２つ以上のマーカを備えていてもよい。効果の詳細は実施例で説明する。

マーカ群は、水平方向に第１所定距離だけ離れた２つ以上のマーカよりも上方または下方側に、さらに１つ以上のマーカを備えていてもよい。効果の詳細は実施例で説明する。

マーカ群位置姿勢推定部は、光学センサで撮像された画像内にマーカ群が複数存在する場合に、複数のマーカ群の各々の位置および姿勢を推定してもよい。グラフ構造データ生成部は、複数のマーカ群の各々の位置および姿勢を示す複数のマーカノードを生成してもよい。効果の詳細は実施例で説明する。

フォークリフト１の概略斜視図である。フォークリフト１のブロック図である。パレット５０の一例を示す図である。実施例１における車体およびパレットの位置関係を示す図である。図４の位置関係から生成されたグラフ構造データを示す図である。位置姿勢推定処理の内容を示すフローチャートである。荷役処理の内容を示すフローチャートである。マーカノードの位置及び姿勢を補正できる理由を説明する図である。マーカの位置および姿勢の推定結果の一例を示す図である。実施例２における車体およびパレットの位置関係を示す図である。図１０の位置関係から生成されたグラフ構造データを示す図である。実施例３における車体、パレットおよび柱の位置関係を示す図である。図１２の位置関係から生成されたグラフ構造データを示す図である。

（フォークリフト１の構成）
以下、図１および図２を参照して、本実施例のフォークリフト１について説明する。図１はフォークリフト１の概略斜視図であり、図２はブロック図である。フォークリフト１は、無人フォークリフトである。図１に示すように、フォークリフト１は、車体２、制御部１０（図２に図示）、マスト２０、フォーク２２、バックレスト２３、カメラ固定部２４、カメラ３０、を備えている。

車体２は、その両側面のそれぞれに前輪２８及び後輪２９を備えている。前輪２８は、駆動機構を介して駆動輪モータ（不図示）が接続されており、移動制御部１７（図２に図示）によって回転駆動されるようになっている。後輪２９は、操舵装置（不図示）に接続されており、移動制御部１７によって車輪の向きが調整される。移動制御部１７が駆動輪モータ及び操舵装置を制御することで、車体２は路面を走行すると共に、車体２の進行方向を変えることができる。

マスト２０は、車体２の前面に取付けられている支柱であり、その軸線は上下方向に伸びている。フォーク２２は、マスト２０に上下方向に移動可能に取付けられている。フォーク２２は、一対のツメ２２ａ、２２ｂを有している。ツメ２２ａ、２２ｂは、車体２の左右方向に互いに離間した位置に配置されており、マスト２０側から車体２の前方に向かって伸びている。フォーク２２は、荷役装置制御部１８（図２に図示）により昇降される。フォーク２２の上下方向の位置は、荷役装置制御部１８の駆動量によって特定可能となっている。

バックレスト２３は、フォーク２２の上に載せた荷物がマスト２０の後方に落下するのを防ぐ部材である。カメラ固定部２４は、バックレスト２３と一体化するようにカメラ３０を固定する部材である。これにより、フォーク２２がマスト２０によって上下するときや、不図示のチルト機構によって前後に傾くときに、カメラ３０も一体となって動く。よって、フォーク２２を基準としたマーカ画像を取得することができる。

図２のブロック図を説明する。フォークリフト１は、制御部１０、グラフ構造記憶部１９、フォーク２２、前輪２８、後輪２９、カメラ３０、エンコーダ４０、を備える。制御部１０は、フォークリフト１を制御する装置である。制御部１０は、例えば、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータによって構成することができる。コンピュータがプログラムを実行することで、制御部１０は、図２に示すマーカ群位置姿勢推定部１１〜荷役装置制御部１８等として機能する。制御部１０は、位置姿勢推定モジュールＭＯＤ１と、車両制御モジュールＭＯＤ２とを備える。位置姿勢推定モジュールＭＯＤ１は、後述する位置姿勢推定処理（図６）を実行する。車両制御モジュールＭＯＤ２は、後述する荷役処理（図７）を実行する。位置姿勢推定モジュールＭＯＤ１と車両制御モジュールＭＯＤ２とは、互いに独立しており並列に動作可能である。位置姿勢推定モジュールＭＯＤ１は、マーカ群位置姿勢推定部１１、グラフ構造データ生成部１３、グラフ最適化部１４、アクセス位置推定部１５を備えている。車両制御モジュールＭＯＤ２は、車体位置姿勢算出部１２、軌道生成部１６、移動制御部１７、荷役装置制御部１８を備えている。マーカ群位置姿勢推定部１１〜荷役装置制御部１８の各々の動作内容の詳細は、後述する。グラフ構造記憶部１９は、後述するグラフ構造データを記憶する部位である。エンコーダ４０は、車輪の回転角を検出するセンサを備えた部位である。その他の部位については、図１で説明済みである。

（パレットの構成）
図３に、本実施形態に係るパレット５０の一例を示す。パレット５０は、カゴパレットである。パレット５０の正面下部には、フォーク２２を挿入するための空間ＳＰが配置されている。空間ＳＰ内には、パレット基準座標ＰＣが予め設定されている。ｙ方向（左右方向）におけるパレット基準座標ＰＣの原点位置は、パレット５０の正面中央である。ｚ方向（上下方向）におけるパレット基準座標ＰＣの原点位置は、フォーク２２を差す高さである。パレット基準座標ＰＣの原点位置は、設計者によって決定された位置であり、空間上の仮想的な位置である。

パレット５０の正面には、２つのマーカＭ１およびＭ２を備えたマーカ群ＭＧが配置されている。本実施形態では、マーカＭ１およびＭ２は、ＡＲ（Augmented Reality）マーカである。すなわちマーカＭ１およびＭ２は、拡張現実システムにおいて、付加情報を表示する位置を指定するための標識となるマーカである。図３において、マーカ基準座標ＭＣ１およびＭＣ２は、マーカＭ１およびＭ２の各々の位置および姿勢（向き）を示している。マーカＭ１およびＭ２は、予め定められた第１所定距離Ｄ１だけ互いに離れて、水平方向に配置されている。またマーカＭ１およびＭ２は、同一面内に配置されている。これにより、パレット基準座標ＰＣの原点とマーカ基準座標ＭＣ１およびＭＣ２の原点との相対位置が一意に決まる。具体的には、最初にマーカＭ１およびＭ２の配置位置や第１所定距離Ｄ１を決定する。そして、荷取り位置との距離を測定する、または、設計情報から距離を算出することで、マーカＭ１およびＭ２とパレット基準座標ＰＣの原点との相対位置をデータファイルに登録する。よって、マーカＭ１およびＭ２の位置と、パレット基準座標ＰＣとの関係を既定（拘束）することができる。換言すれば、マーカＭ１およびＭ２は、パレット基準座標ＰＣとの位置関係が既定できる場所に設置しなければならない。

（パレットの自動荷役）
図４〜図９を用いて、パレットの自動荷役の具体的な処理について説明する。本説明例では図４に示すように、時刻ｔ_ｉにおける位置Ｐ１から時刻ｔ_ｉ＋１における位置Ｐ２へ、軌道ＴＲ１を通って車体２が走行する場合の処理内容を説明する。図４において、車体基準座標ＶＣ１およびＶＣ２は、時刻ｔ_ｉおよびｔ_ｉ＋１の各々における車体２の位置および姿勢（向き）を示している。車体基準座標ＶＣ１およびＶＣ２の原点は、車体２の原点を示している。カメラ基準座標ＣＣ１およびＣＣ２は、時刻ｔ_ｉおよびｔ_ｉ＋１の各々におけるカメラ３０の位置および姿勢（向き）を示している。

（位置姿勢推定処理の内容）
図６の位置姿勢推定処理は、新たな荷役処理の開始でスタートする。具体的には、後述する荷役処理のＳ２１０で荷役処理を開始することが判断されることによってスタートする。Ｓ１０において、画像データ受信処理が行われる。本説明例では、図４の時刻ｔ_ｉにおける位置Ｐ１において、カメラ３０がパレット５０を撮像する。そして制御部１０は、カメラ３０から画像データを受信する。

Ｓ２０において、マーカ群位置姿勢推定部１１は、受信した画像データに基づいてマーカＭ１およびＭ２の位置および姿勢を推定する。この推定処理は、公開されているソフトライブラリを用いることができるため、説明を省略する。図９に、マーカの位置および姿勢の推定結果の一例を示す。カメラ３０による計測結果に基づく推定結果では、マーカの位置および姿勢には誤差が含まれる。例えば図９の推定結果では、マーカ基準座標ＭＣ１およびＭＣ２が示すマーカＭ１およびＭ２の姿勢は、誤差を含むため、パレット５０の正面に対して垂直になっていない状態である。すなわち図９の推定結果では、マーカＭ１およびＭ２のヨー角姿勢を正確に推定できていない。

Ｓ３０において、車体位置姿勢算出部１２は、車体２の位置および姿勢を算出する。具体的には、エンコーダ４０によって検出された前輪２８の回転角に基づいて、車体２の移動方向及び移動量を求める。

Ｓ３５において、グラフ最適化部１４は、グラフ最適化処理を実行するか否かを判断する。この判断は、例えば、グラフ最適化処理を前回実行した時点から車体２が０．５ｍ以上進んだか否かによって行われてもよい。これにより、グラフ構造が必要以上に長大ならないように抑制することができる。否定判断される場合（Ｓ３５：ＮＯ）にはＳ１０へ戻り、肯定判断される場合（Ｓ３５：ＹＥＳ）にはＳ４０へ進む。

Ｓ４０〜Ｓ７０において、グラフ構造データ生成処理が行われる。グラフ構造データは、制御部１０にて中間情報として生成されるデータである。グラフ構造データは、マーカＭ１およびＭ２の位置および姿勢の推定の誤差を抑制するために用いられるデータである。

図５に示すグラフ構造データについて説明する。グラフ構造データは、車体ノードＶＮ１およびＶＮ２、センサノードＳＮ１およびＳＮ２、マーカノードＭＮ１およびＭＮ２を備える。車体ノードＶＮ１およびＶＮ２は、時刻ｔ_ｉおよびｔ_ｉ＋１の移動点の各々における車体２の位置および姿勢を示す情報である。センサノードＳＮ１およびＳＮ２は、時刻ｔ_ｉおよびｔ_ｉ＋１の移動点の各々におけるカメラ３０の位置および姿勢を示す情報である。マーカノードＭＮ１およびＭＮ２は、時刻ｔ_ｉおよびｔ_ｉ＋１の移動点の各々において推定されたマーカＭ１およびＭ２の位置および姿勢を示す情報である。図５では説明の便宜上、マーカノードＭＮ１およびＭＮ２を円で示している。しかし実際は、図９で説明したように、マーカノードＭＮ１およびＭＮ２は、ヨー角姿勢などの姿勢情報も有している。

またグラフ構造データは、アークＡＣ１およびＡＣ２、アークＡＶＣ１およびＡＶＣ２、アークＡＶＮ１２、アークＡＭ、を備える。アークは、相対位置の制約条件である。アークＡＣ１は、センサノードＳＮ１とマーカノードＭＮ１との相対的位置関係、および、センサノードＳＮ１とマーカノードＭＮ２との相対的位置関係を規定する情報である。同様に、アークＡＣ２は、センサノードＳＮ２とマーカノードＭＮ１との相対的位置関係、および、センサノードＳＮ２とマーカノードＭＮ２との相対的位置関係を規定する情報である。アークＡＶＣ１およびＡＶＣ２は、車体ノードとセンサノードとの間の相対的位置関係を規定する情報である。アークＡＶＮ１２は、車体ノードＶＮ１とＶＮ２との相対的位置関係を規定する情報である。アークＡＭは、マーカＭ１とＭ２との間の相対的位置関係を規定する情報である。

各アークは、「ノード間の相対位置・姿勢の初期値」と「確からしさ」に関する事前知識を保持している。「確からしさ」は、情報行列で表すことができる。情報行列は、ノード間の相対位置および姿勢の誤差についての分散共分散行例の逆行列である。アークＡＶＣ１およびＡＶＣ２によって規定される車体ノードとセンサノードとの間の相対位置および姿勢は、フォークリフト１の設計事項であるため、既知である。同様に、アークＡＭによって規定されるマーカＭ１とＭ２との間の相対位置および姿勢は、図３で説明したように既知である。アークＡＶＣおよびアークＡＭは既知であるため、グラフ構造記憶部１９に予め保存しておき、グラフ構造生成時に読み出して利用することができる。

グラフ構造データの生成処理の具体例を説明する。Ｓ４０において、グラフ構造データ生成部１３は、車体位置姿勢算出部１２で得られた車体２の位置および姿勢を表す車体ノードを設定する。本説明例では、車体位置姿勢算出部１２で時刻ｔ_ｉ＋１における車体基準座標ＶＣ２（図４）が求められる。そして、車体基準座標ＶＣ２に対応する車体ノードＶＮ２（図５）が追加される。

Ｓ５０において、グラフ構造データ生成部１３は、カメラ３０の位置および姿勢を表すセンサノードを設定する。センサノードの設定は、カメラ３０と、車体２の後輪２９の車軸との既知の位置関係に基づいて行うことができる。本説明例では、センサノードＳＮ２（図５）が追加される。
Ｓ６０において、グラフ構造データ生成部１３は、Ｓ２０で推定したマーカＭ１およびＭ２の位置および姿勢を表すマーカノードＭＮ１およびＭＮ２（図５）を設定する。

Ｓ７０において、グラフ構造データ生成部１３は、センサノードとマーカノード間、車体ノードとセンサノード間、車体ノード間、マーカＭ１とＭ２との間、にアークを追加する。本説明例では、アークＡＣ２、アークＡＶＣ２、アークＡＶＮ１２およびアークＡＭが追加される。Ｓ８０において、グラフ構造データ生成部１３は、ノードやアークが追加されたグラフ構造データをグラフ構造記憶部１９に記憶する。

Ｓ１００においてグラフ最適化部１４は、グラフ構造記憶部１９に記憶されているグラフ構造データを最適化する。最適化には、例えば、Graph-Based SLAM（Simultaneous Localization and Mapping）を用いることができる。Graph-Based SLAMは、カメラ位置やマーカ位置をノード（頂点）、それらの間の相対位置の制約条件をアーク（辺）とするグラフに対して、各制約の誤差総和を最小化するように最適化計算を行う手法である。過去から現在までの車体２の移動に伴う全データを利用して全体最適化を行うため、過去に遡ってマーカ位置を修正できるという特徴を有する。

Graph-Based SLAMを具体的に説明する。各ノードの相対位置制約からの誤差の総和を評価関数として、非線形最小二乗問題を解く。この評価関数は次式となる。

ここで、Ｃは制約条件の集合である。ｅ_ｉｊ（ｘ_ｉ,ｘ_ｊ）はノードｘ_ｉとノードｘ_ｊの間の相対位置制約からの誤差ベクトル関数である。Σ_ｉｊはその制約の共分散行列である。制約の共分散によるマハラノビス距離の二乗和が最小となるように上式を最小化することで、各ノードの位置、姿勢関係を最適化する。これにより、マーカノードＭＮ１およびＭＮ２の位置および姿勢を補正することができる。

図８を用いて、マーカノードの位置及び姿勢を補正できる理由を説明する。図８では、時刻ｔ_ｉにおける位置Ｐ１、時刻ｔ_ｉ＋１における位置Ｐ２、時刻ｔ_ｉ＋2における位置Ｐ３の各々において、マーカＭ１およびＭ２の位置姿勢を推定（Ｓ２０）した場合の例である。すなわち、多視点からの観測を行った例である。マーカを用いた位置姿勢の推定では、カメラ３０からマーカを結んだ線上（奥行方向）に距離の誤差が生じる。例えば、カメラ３０とマーカとの距離が４ｍの場合に、標準偏差２０ｍｍである。一方、横方向には誤差はほとんど生じない。すると、位置Ｐ１〜Ｐ３の各々におけるマーカＭ１の位置姿勢の推定結果には、誤差の異方性を示す誤差楕円ＥＥ１１〜ＥＥ１３が存在する。そこで、複数の異なる方向からの観測結果によるグラフ構造を作成し、最適化することで、誤差楕円ＥＥ１１〜ＥＥ１３の交点ＩＳ１を解として求めることができる。また同様に、マーカＭ２においても誤差楕円ＥＥ２１〜ＥＥ２３の交点ＩＳ２を解として求めることができる。よって、マーカの位置および姿勢の誤差を補正できる。そして後述するように、マーカＭ１およびＭ２の間には位置および姿勢に関する強い制約（高い確からしさ）が存在するため、奥行き方向の位置、および、マーカ基準座標ＭＣ１およびＭＣ２のヨー角姿勢を正確に推定することが可能となる。

Ｓ１１０において、アクセス位置推定部１５は、マーカノードの補正された位置および姿勢から、フォーク２２を差し込むアクセス位置を推定する。具体的には、パレット基準座標ＰＣの原点および座標姿勢を推定する。図３で説明したように、マーカＭ１およびＭ２とパレット基準座標ＰＣとの相対位置関係は既知であるため、パレット基準座標ＰＣの原点および座標姿勢は容易に算出できる。そしてアクセス位置推定部１５は、推定したアクセス位置を、軌道生成部１６および荷役装置制御部１８へ出力する。

Ｓ１２０において、位置姿勢推定処理を終了するか否かが判断される。具体的には、後述する荷役処理のＳ２８０でパレット５０をすくう処理が完了しているか否かによって、Ｓ１２０で位置姿勢推定処理を終了するか否かが判断される。否定判断される場合（Ｓ１２０：ＮＯ）にはＳ１０へ戻る。そしてグラフ最適化処理を実行することが再度判断されると（Ｓ３５：ＹＥＳ）、時刻ｔ_ｉ＋２における車体ノードＶＮ３およびセンサノードＳＮ３がグラフ構造データに新たに追加される。また、センサノードＳＮ３とマーカノードＭＮ１およびＭＮ２との間のアークＡＣ３、車体ノードＶＮ３とセンサノードＳＮ３との間のアークＡＶＣ３、車体ノードＶＮ２とＶＮ３との間のアークＡＶＮ２３、がグラフ構造データに新たに追加される。そして、Ｓ１００でグラフ構造データが再度最適化され、Ｓ１１０でマーカノードの位置姿勢が再度補正される。一方、肯定判断される場合（Ｓ１２０：ＹＥＳ）には、位置姿勢推定処理を終了する。

（荷役処理の内容）
図７を用いて荷役処理を説明する。Ｓ２１０において車両制御モジュールＭＯＤ２は、荷役処理を開始するか否かを判断する。例えば、ユーザによる荷役開始操作の入力の有無に応じて判断してもよい。否定判断される場合（Ｓ２１０：ＮＯ）にはＳ２１０へ戻り待機し、肯定判断される場合（Ｓ２１０：ＹＥＳ）にはＳ２２０へ進む。

Ｓ２２０において軌道生成部１６は、目標位置を取得する。目標位置の取得は、前述した位置姿勢推定処理（図６）のＳ１１０でフォーク２２のアクセス位置の１回目の推定が行われたことに応じて、実行することができる。目標位置は、フォーク２２のアクセス位置より３０ｃｍほど手前に設定してもよい。Ｓ２３０において、軌道生成部１６は、Ｓ１１０で算出されたパレット基準座標ＰＣの原点および座標姿勢に基づいて、車体２の現在位置から対象物までの軌道および速度パターンを生成する。Ｓ２４０において移動制御部１７は、生成した軌道および速度パターンに基づいて、目標位置に向けて車体２を移動させる。

Ｓ２５０において移動制御部１７は、目標位置に到達したか否かを判断する。否定判断される場合（Ｓ２５０：ＮＯ）にはＳ２４０へ戻りさらに車体２を移動させ、肯定判断される場合（Ｓ２５０：ＹＥＳ）にはＳ２６０へ進む。ここで、荷役処理（図７）と並列動作している位置姿勢推定処理（図６）では、車体２が移動することに従って、グラフ構造の追加（Ｓ４０〜Ｓ７０）、グラフ構造の最適化（Ｓ１００）およびアクセス位置の推定（Ｓ１１０）が繰り返し実行される。すなわち、車体２の移動中に、フォーク２２から見た目標位置が更新される。そして、随時更新された目標位置に向かって車体２を移動させることができる。よって、パレット５０への近接動作を正確に行うことが可能となる。

Ｓ２６０において荷役装置制御部１８は、不図示の荷役アクチュエータを介してフォーク２２の位置を調整する。具体的には、リフト機構でフォーク２２の高さを調整し、サイドシフト機構でフォーク２２の横位置を調整する。Ｓ２７０において荷役装置制御部１８は、算出されたパレット基準座標ＰＣの原点および座標姿勢に基づいて、フォーク２２をアクセス位置に差し込む。具体的には、パレット５０の奥行を考慮して、車体２を直進させる。Ｓ２８０において、荷役装置制御部１８は、パレット５０をすくう。具体的には、チルト機構によってフォーク２２を傾けるとともにマスト２０によってフォーク２２を上昇させる。

Ｓ２９０において、荷降ろし動作および待機位置への復帰動作を行う。これらの動作は自動で行ってもよいし、オペレータが操作してもよい。これらの動作は従来技術で実現できるため、説明は省略する。

（効果）
パレットや荷台などの対象物に貼り付けられたマーカを、正対したカメラで遠方から撮像して対象物の位置や姿勢を認識する技術では、一般的に、対象物の奥行方向の位置や、マーカの法線ベクトルによって推測されるヨー方向やピッチ方向の回転姿勢を正確に求めることが困難なため、フォークをパレットに差し込む際に意図しない衝突等が発生してしまう場合がある。本明細書の技術では、複数のセンサノードと、マーカノードと、複数のアークと、を備えるグラフ構造データを生成し、生成したグラフ構造データを最適化する。複数のセンサノードは、異なる時刻の複数の移動点のそれぞれにおけるカメラ３０の位置および姿勢を示す情報である。マーカノードは、異なる時刻の複数の移動点のそれぞれで推定されたマーカＭ１およびＭ２の位置および姿勢を表す情報である。従って、異なる時刻の複数の移動点の各々において推定された、マーカＭ１およびＭ２の位置および姿勢の複数の推定結果を用いて、マーカＭ１およびＭ２の位置および姿勢の推定値を補正することができる。換言すると、マーカを用いて推定した対象物の奥行方向の情報と、複数の移動点の移動量の累積によって求めた車体２の位置（オドメトリ）の奥行方向の情報とを、グラフ構造を用いることで関連付けることができる。そして、対象物の奥行方向の情報の精度不足分を、オドメトリの奥行方向の情報を用いることで補完することができる。以上より、ある１つの移動点で推定したマーカＭ１およびＭ２の位置および姿勢の推定結果を用いる場合と比して、マーカＭ１およびＭ２の位置および姿勢をより正確に推定することが可能となる。

本明細書の技術では、カメラ、車体、マーカの相対位置関係を、グラフ構造データを用いて表現している。そして、マーカＭ１とＭ２との間の相対的位置関係は、既知であり、高精度に位置決めされている。換言すると、マーカ間のアークＡＭの「確からしさ」は高い。よって、最適化によって修正する必要のあるアークを、センサノードとマーカ間のアークＡＣに絞ることが可能となる。全てのアークの「確からしさ」が低い場合に比して、マーカＭ１およびＭ２の位置および姿勢をより正確に推定することが可能となる。

車体２が時刻ｔ_ｉの移動点から時刻ｔ_ｉ＋１の移動点まで移動した場合に、グラフ構造データ生成部１３は、時刻ｔ_ｉ＋１の移動点におけるセンサノードおよびマーカノードと、時刻ｔ_ｉ＋１の移動点におけるセンサノードとマーカノードとの相対的位置関係を規定するアークと、を生成する（Ｓ４０〜Ｓ７０）。そして、生成した時刻ｔ_ｉ＋１の移動点におけるセンサノード、マーカノードおよびアークを、時刻ｔ_ｉまでの移動点について生成されたグラフ構造データに、新たに追加することができる（Ｓ８０）。すなわち本実施例に係るフォークリフト１は、グラフ構造データを走行中に更新することが可能である。

図３に示すように、マーカＭ１およびＭ２は、予め定められた第１所定距離Ｄ１だけ互いに離れて、水平方向に配置されている。すなわち、ヨー方向の回転軸（図３のｚ軸）に対して直交する方向に、マーカＭ１およびＭ２を配置している。これにより、パレット５０の奥行方向の３次元的な姿勢である、ヨー方向の回転姿勢の推定精度を高くすることができる。換言すると、２つのマーカＭ１およびＭ２の各々のヨー方向の計測精度は低い。例えばマーカ正面から観測した場合の計測精度が、数度の角度の独立したばらつきを持つ場合がある。一方、位置（特に横方向や上下方向）の計測精度は、ヨー方向の計測精度に比して高い。そして図３で説明したように、二つのマーカ間の第１所定距離Ｄ１は正確に規定されている。また、マーカ基準座標ＭＣ１およびＭＣ２の法線方向（図３のｘ軸負方向）は、二つのマーカを結んだ方向（図３のｙ軸方向）に対して直交するように正確に位置決めされている。この事前知識を元にグラフ構造の最適化を行うことにより、マーカのヨー角姿勢の推定精度を高めることができる。

実施例２は、マーカ群を備えたパレットが複数存在する場合における、自動荷役の処理例である。パレットが複数存在する点以外は、実施例１と処理内容は共通する。以下に、実施例１と異なる点のみを説明する。例として、図１０に示すように、パレット５０に加えてパレット５０ａが配置されている場合について説明する。パレット５０ａは、マーカＭ１ａおよびＭ２ａを備えている。マーカＭ１ａおよびＭ２ａの内容や位置関係は、パレット５０のマーカＭ１およびＭ２と同様である。また、図１０の位置関係に基づいて、図１１のグラフ構造データが生成される場合について説明する。

図１０の時刻ｔ_ｉにおける位置Ｐ１において、カメラ３０によって、パレット５０および５０ａが撮像される（図６のＳ１０）。マーカ群位置姿勢推定部１１は、マーカＭ１、Ｍ２、Ｍ１ａ、Ｍ２ａの位置および姿勢を推定する（Ｓ２０）。グラフ構造データ生成部１３は、図１１に示すように、マーカノードＭＮ１およびＭＮ２に加えて、マーカノードＭＮ１ａおよびＭＮ２ａを設定する（Ｓ６０）。またアークＡＣ１、アークＡＶＣ１、アークＡＶＮ１２、アークＡＭに加えて、アークＡＣ１ａを設定する（Ｓ７０）。以後の処理は、実施例１と同様であるため、説明を省略する。

以上より、図１１に示すように、複数のパレットについてグラフ構造データを生成することができる。そして、複数のパレットについてのグラフ構造データを最適化（Ｓ１００）することができる。複数のパレットの各々に配置されているマーカの位置および姿勢を、同時にまとめて補正することが可能となる。

実施例３は、パレットに加えて、柱や壁など既知の固定物にもマーカを配置した、自動荷役の処理例である。固定物にもマーカが配置されている点以外は、実施例１と処理内容は共通する。以下に、実施例１と異なる点のみを説明する。例として、図１２に示すように、パレット５０に加えて柱ＰＬ１〜ＰＬ４が配置されている場合について説明する。柱ＰＬ１〜ＰＬ４は、マーカＭＰ１〜ＭＰ４を備えている。また、時刻ｔ_ｉにおける位置Ｐ１１から時刻ｔ_ｉ＋３における位置Ｐ１４へ、車体２が走行する場合の処理内容を説明する。また、図１２の位置関係に基づいて、図１３のグラフ構造データが生成される場合について説明する。図１３では、柱ＰＬ１〜ＰＬ４の各々に対応して、柱ノードＰＮ１〜ＰＮ４が配置されている。

グラフ構造データ生成部１３は、図１３に示すように、柱ノードＰＮ１〜ＰＮ４の間にアークＡＰ１〜ＡＰ５を設定する。センサノードＳＮ１１と柱ノードＰＮ１およびＰＮ２との間に、アークＡＣ１１を設定する。センサノードＳＮ１２と柱ノードＰＮ３との間に、アークＡＣ１２を設定する。センサノードＳＮ１３と柱ノードＰＮ４との間に、アークＡＣ１３を設定する。センサノードＳＮ１４とマーカノードＭＮ１およびＭＮ２との間に、アークＡＣ１４を設定する。

以上より図１３に示すように、柱ＰＬ１〜ＰＬ４についてもグラフ構造データを生成することができる。そして、生成したグラフ構造データを最適化（Ｓ１００）することができる。柱ＰＬ１〜ＰＬ４は、その相対位置や姿勢は既知であるため、柱ノードＰＮ１〜ＰＮ４の間に設定されたアークＡＰ１〜ＡＰ５は、「確からしさ」が高い。「確からしさ」が高いアークＡＰ１〜ＡＰ５をグラフ構造データに含めることができるため、アークＡＰ１〜ＡＰ５を含まない場合に比して、グラフ構造データに含まれる誤差の総和を小さくすることができる。より正確にマーカノードＭＮ１およびＭＮ２の位置および姿勢を補正することが可能となる。

以上、本明細書が開示する技術の実施例について詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。

（変形例）
本実施形態では、マーカの位置および姿勢を認識する移動体としてフォークリフトを説明したが、この形態に限られない。マーカの位置・姿勢情報を利用して移動可能な移動体であれば、本明細書の技術を適用可能である。移動体の他の例としては、移動ロボット、モバイルマニュピレータ、ドローンなどが挙げられる。

車体（移動体）ノードを含まないグラフ構造データを用いてもよい。この場合、例えば図５のグラフ構造データの例では、車体ノードＶＮ１およびＶＮ２、アークＡＶＣ１およびＡＶＣ２、アークＡＶＮ１２を省略することができる。例えば手持ちカメラのように、カメラの位置姿勢の情報がない場合においても、複数の異なる方向からの観測結果と誤差モデルに対してGraph-Based SLAMを適用することより、マーカ位置を高精度に推定することができる。この原理は、図８で説明済みである。これにより、車体２の移動前においてパレット誘導用の軌道および速度パターンを生成する段階（Ｓ２３０）で、軌道の精度を向上させる効果を得ることができる。なお、グラフ構造の最適化では、必ずしも観測時のカメラ位置の初期値は必要ない。カメラノードの位置は、グラフ構造の最適化の結果、求めることができる。

本実施例では、異なる時刻の複数の移動点における複数のセンサノードを備えるグラフ構造データを生成する場合を説明したが、この形態に限られない。例えば図９に示すように、１の時刻の１つの移動点における１つのセンサノードを備えるグラフ構造を用いても、マーカＭ１およびＭ２の位置および姿勢を推定することが可能である。

図３において、複数のマーカが同一面内に水平方向に配置されている場合を説明したが、この形態に限られない。パレット基準座標ＰＣの原点とマーカ基準座標の原点との相対位置が一意に決まる態様であれば、複数のマーカが異なる面内に配置されていてもよいし、複数のマーカが水平方向に配置されていなくてもよい。

図３において、第１所定距離Ｄ１離れて水平方向に配置されているマーカＭ１およびＭ２に加えて、マーカＭ１およびＭ２よりも上方側（ｚ軸正方向側）または下方側（ｚ軸負方向側）に、さらに１つ以上のマーカを備えてもよい。これにより、ピッチ方向の回転軸（図３のｙ軸）に対して直交する方向に、追加のマーカを配置することができる。パレット５０の奥行方向の３次元的な姿勢である、ピッチ方向の回転姿勢の推定精度を高くすることができる。

パレットに取り付けるマーカ数が２個である場合を説明したが、この形態に限られない。マーカ間にアークを配置できればよいため、マーカは複数枚であればよい。

図３では、パレット５０の正面のみにマーカを配置する形態を説明したが、この形態に限られない。パレット５０の正面に対して斜め方向からカメラ３０がパレット５０を撮像する場合に備えて、パレット５０の側面にもマーカを配置してもよい。

マーカをパレットに配置する方法は、マーカ間の相対位置が一意に決まるのであれば、様々であってよい。テープでの貼付、接着、ねじ式、磁石式、ピン止めなどの態様でもよい。レーザー印刷、ホットスタンプ印刷、スクリーン印刷などによって、パレットにマーカを直接印刷してもよい。透明なポケットにマーカが差し込まれているマーカ取り付け部を、パレットに固定する態様でもよい。

本実施例ではマーカとしてＡＲマーカを用いる態様を説明したが、この形態に限られない。マーカの位置および姿勢が求まるのであれば、どのようなマーカを用いてもよい。例えば、不可視マーカを用いてもよい。

カメラ３０は単眼カメラに限らず、ステレオカメラや不可視マーカ用カメラでもよい。

カメラ３０は、フォーク２２と一体化していれば、その取り付け方は自由である。例えば図１において、ツメ２２ａと２２ｂの間であって、フォーク２２のバックレスト面より奥側（車体２側）に配置されていてもよい。

パレット５０がカゴパレットである場合を説明したが、平パレットなど他の形状のパレットでもよい。

本実施例では、Ｓ３０において車体位置姿勢算出部１２はエンコーダ４０で車体２移動量を求めているが、この形態に限られない。２次元レーザセンサ等を使ったレーザーオドメトリを用いてもよい。

制御部１０はフォークリフト１に搭載される形態に限られない。例えば、インターネットなどのネットワーク上に配置されたサーバであってもよい。

本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

カメラ３０は、光学センサの一例である。グラフ最適化部１４は、補正部の一例である。フォーク２２は、支持構造部の一例である。

１：フォークリフト１１：マーカ群位置姿勢推定部１２：車体位置姿勢算出部１３：グラフ構造データ生成部１４：グラフ最適化部１５：アクセス位置推定部１６：軌道生成部１７：移動制御部１８：荷役装置制御部１９：グラフ構造記憶部２２：フォーク３０：カメラ

Claims

複数のマーカが予め定められた第１所定距離だけ離して配置されているマーカ群を備えた対象物の位置および姿勢を認識する移動体であって、
前記マーカ群を撮像するための光学センサと、
前記移動体が移動した１つまたは複数の移動点の各々で前記光学センサを用いて前記対象物を撮像した画像に基づいて、前記１つまたは複数の移動点の各々において前記マーカ群の位置および姿勢を推定するマーカ群位置姿勢推定部と、
複数のセンサノードと、マーカノードと、前記複数のセンサノードと前記マーカノードとの相対的位置関係を規定する複数のアークと、を備えるグラフ構造データを生成するグラフ構造データ生成部と、
生成した前記グラフ構造データを、前記複数のアークから算出される誤差関数の和が最小となるように最適化することで、前記マーカ群の位置および姿勢の推定値を補正する補正部と、
を備え、
前記複数のセンサノードは、前記１つまたは複数の移動点のそれぞれにおける前記光学センサの位置および姿勢を示すノードであり、
前記マーカノードは、前記マーカ群位置姿勢推定部で推定された前記マーカ群の位置および姿勢を示すノードである、移動体。
前記移動体が時刻ｔ_ｉの移動点から時刻ｔ_ｉ＋１の移動点まで移動した場合に、前記時刻ｔ_ｉ＋１の移動点における前記移動体の位置および姿勢を算出する移動体位置姿勢算出部をさらに備え、
前記マーカ群位置姿勢推定部は、前記時刻ｔ_ｉ＋１の移動点において前記マーカ群の位置および姿勢を推定し、
前記グラフ構造データ生成部は、前記時刻ｔ_ｉ＋１の移動点におけるセンサノードおよびマーカノードと、前記時刻ｔ_ｉ＋１の移動点におけるセンサノードとマーカノードとの相対的位置関係を規定するアークと、を生成して、前記グラフ構造データに追加する、請求項１に記載の移動体。
前記移動体は、前記対象物を保持可能な支持構造部を備えており、
前記移動体は、前記補正部によって補正された前記マーカ群の位置および姿勢に基づいて、前記支持構造部をアクセスするアクセス位置を推定するアクセス位置推定部をさらに備える、請求項１に記載の移動体。
前記補正部によって補正された前記マーカ群の位置および姿勢に基づいて、前記移動体の現在位置から前記対象物までの軌道および速度パターンを生成する軌道生成部と、
生成した前記軌道および速度パターンに基づいて前記移動体を前記対象物まで移動させる移動制御部と、
前記アクセス位置推定部で推定した前記アクセス位置に基づいて、前記支持構造部を前記アクセス位置にアクセスさせるアクセス制御部と、
をさらに備える、請求項３に記載の移動体。
前記支持構造部はフォークであり、
前記対象物はパレットであり、
前記光学センサは、前記フォークの動作と一体化して動作する、請求項３または４に記載の移動体。
前記マーカ群は、水平方向に前記第１所定距離だけ離れた２つ以上のマーカを備える、請求項１〜５の何れか１項に記載の移動体。
前記マーカ群は、水平方向に前記第１所定距離だけ離れた前記２つ以上のマーカよりも上方または下方側に、さらに１つ以上のマーカを備える、請求項６に記載の移動体。
前記マーカ群位置姿勢推定部は、前記光学センサで撮像された画像内に前記マーカ群が複数存在する場合に、複数のマーカ群の各々の位置および姿勢を推定し、
前記グラフ構造データ生成部は、前記複数のマーカ群の各々の位置および姿勢を示す複数の前記マーカノードを生成する、請求項１〜７の何れか１項に記載の移動体。