JP7312089B2 - 計測装置 - Google Patents

Description

本明細書に開示する技術は、対象物の位置および姿勢を推定する計測装置に関する。

対象物の位置や姿勢を高精度に推定する必要がある場合がある。例えば、荷物を自走により運搬する移動体（例：無人牽引車）を、対象物近傍まで移動させる場合などである。一例として、特許文献１には、対象物に配置されたマーカをカメラを用いて認識することで、対象物の位置や姿勢を推定する技術が開示されている。また非特許文献には、関連する技術が開示されている。

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

田中秀幸、角保志、松本吉央、「正面からの観測でも高精度な姿勢推定が可能なＡＲマーカ」、情報処理学会報告、vol.2012-CG-147 No.3 2012/6/22 Daniel F.Abawi, Joachim Bienwald, Ralf Dorner, "Accuracy in optical tracking with fiducial markers: an accuracy function for ARToolKit", Third IEEE and ACM International Symposium on Mixed and Augmented Reality 5-5 Nov. 2004

特許文献１の技術のように、カメラを用いてマーカが貼られた対象物を認識する技術では、マーカおよび対象物の３次元的な姿勢の推定精度が低くなる場合がある。例えば、マーカを正面から観測した場合などである。３次元的な姿勢の一例としては、マーカおよび対象物のヨー方向（上下を軸とした回転方向）やピッチ方向（左右を軸とした回転方向）の回転姿勢が挙げられる。本明細書は、荷役対象物の三次元的な位置姿勢を高精度に推定することが可能な技術を開示する。

本明細書に開示する計測装置の一実施形態は、マーカが配置されている対象物を撮像可能なカメラと、レーザを用いて対象物の位置を検出可能なレーザセンサと、を備えた計測装置である。計測装置は、カメラを用いてマーカを撮像した画像に基づいて、マーカの位置および姿勢を推定する第１推定部を備える。計測装置は、第１推定部で推定されたマーカの位置および姿勢の誤差を示す第１誤差情報を取得する第１取得部を備える。計測装置は、レーザセンサを用いて対象物を測定した結果に基づいて、対象物の位置および姿勢を推定する第２推定部を備える。計測装置は、第２推定部で推定された対象物の位置および姿勢の誤差を示す第２誤差情報を取得する第２取得部を備える。計測装置は、第１推定部で推定したマーカの位置および姿勢と、第２推定部で推定した対象物の位置および姿勢と、第１誤差情報と、第２誤差情報と、に基づいて対象物の位置および姿勢を補正する補正部を備える。

カメラを用いてマーカの位置および姿勢を推定する場合には、カメラから見た左右方向の計測精度に比して、奥行き方向の距離の計測精度や、ヨー方向（上下を軸とした回転方向）やピッチ方向（左右を軸とした回転方向）の角度の計測精度が低い。一方、レーザセンサを用いて対象物の位置および姿勢を推定する場合には、ヨー方向の角度精度に比して、レーザセンサから見た左右方向の計測精度が低い。すなわち、幅方向はカメラによって高精度な測定が可能であり、奥行方向はレーザセンサによって高精度な測定が可能である。よって、カメラによる測定の誤差分布の異方性と、レーザセンサによる測定の誤差分布の異方性とは、略直交する関係にあるため、誤差分布の補完関係が成立している。補完関係にあるカメラとレーザセンサの観測結果と誤差の性質を考慮して補正を行うことにより、対象物体の位置・姿勢を高精度に推定することが可能となる。

補正部は、カメラの光軸方向に対して略垂直な方向における対象物の位置および姿勢を、第１誤差情報を用いて補正してもよい。補正部は、レーザセンサの光軸方向における対象物の位置および姿勢を、第２誤差情報を用いて補正してもよい。効果の詳細は実施例で説明する。

第２推定部は、対象物の外形形状を示す外形データによって表される外形モデルであって予め用意された外形モデルを、レーザセンサによる計測データに対して位置合わせを行うことで、対象物の位置および姿勢を推定してもよい。効果の詳細は実施例で説明する。

外形モデルは、マーカとの相対的な位置および姿勢を示すマーカデータを備えていてもよい。第２推定部は、第１推定部で推定されたマーカの位置と、マーカデータが示すマーカの位置が一致するように、外形モデルの初期位置を定めてもよい。効果の詳細は実施例で説明する。

補正部は、カメラの位置および姿勢を示すカメラノード、レーザセンサの位置および姿勢を示すセンサノード、第１推定部で推定したマーカの位置および姿勢を示すマーカノード、第２推定部で推定した対象物の位置および姿勢を示す対象物ノード、の間の相対的位置関係を規定する複数のアークを備えるグラフ構造データを生成するグラフ構造データ生成部をさらに備えていてもよい。補正部は、生成したグラフ構造データを、複数のアークから算出される誤差関数の和が最小となるように最適化することで、対象物の位置および姿勢の推定値を補正してもよい。効果の詳細は実施例で説明する。

グラフ構造データ生成部は、カメラの誤差モデルに基づく第１の重みと、レーザセンサの誤差モデルに基づく第２の重みとを、複数のアークに付与してもよい。カメラの誤差モデルは、レーザセンサの誤差モデルに比して、光軸方向の誤差分布が大きくてもよい。効果の詳細は実施例で説明する。

牽引車１および搭降載器材１１５の概略斜視図である。 牽引車１のブロック図である。 航空機の前方から牽引車１が接近する場合の軌道例を示す図である。 形状モデルＭＤの例を示す図である。 位置姿勢推定処理の内容を示すフローチャートである。 牽引車１の移動例を示す図である。 計測の誤差分布について説明する図である。 図６の位置関係から生成されたグラフ構造データを示す図である。 実施例２に係る物流倉庫内の様子を示す図である。

（牽引車１および搭降載器材１１５の概略説明）
図１は、牽引車１および搭降載器材１１５の概略斜視図である。牽引車１は、無人自動走行車である。牽引車１は、カメラ３０、ＬｉＤＡＲ（Light Detection and Ranging, Laser Imaging Detection and Ranging）３１を備えている。カメラ３０は、マーカＭ１、Ｍ２等を検出・計測する等の用途に用いられる。ＬｉＤＡＲ３１は、目標物の位置および姿勢を計測するためのセンサである。本実施例では、ＬｉＤＡＲ３１は２Ｄ－ＬｉＤＡＲであり、レーザを移動平面（地面)に平行に１ライン走査する。ＬｉＤＡＲ３１の走査高さは、図１の点線で示すＬｉＤＡＲ観測点ＬＰに示すように、搭降載器材１１５の側面が計測できる高さに設定されている。搭降載器材１１５は、コンテナを航空機１０１に搭降載する器材である。搭降載器材１１５の詳細については、後述する。

（牽引車１の構成）
図２に、牽引車１のブロック図を示す。牽引車１は、制御部１０、前輪２８、後輪２９、カメラ３０、ＬｉＤＡＲ３１、エンコーダ４０、を備える。

制御部１０は、牽引車１を制御する装置である。制御部１０は、例えば、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータによって構成することができる。制御部１０は、マーカ位置姿勢推定部１１、対象物位置姿勢推定部１２、グラフ構造データ生成部１３、グラフ最適化部１４、アクセス位置推定部１５、軌道生成部１６、移動制御部１７、外形モデル記憶部２０、マーカ誤差モデル計算部２１、対象物誤差モデル計算部２２、マーカ誤差モデル記憶部２３、対象物誤差モデル記憶部２４、車体位置姿勢算出部４１、を備える。コンピュータがプログラムを実行することで、制御部１０は、図２に示すマーカ位置姿勢推定部１１～移動制御部１７等として機能する。マーカ位置姿勢推定部１１～移動制御部１７、マーカ誤差モデル計算部２１、対象物誤差モデル計算部２２の各々の動作内容の詳細は、後述する。マーカ誤差モデル記憶部２３は、後述するマーカ計測誤差情報を記憶する部位である。対象物誤差モデル記憶部２４は、後述する対象物計測誤差情報を記憶する部位である。車体位置姿勢算出部４１は、エンコーダ４０によって検出された前輪２８および後輪２９の回転角に基づいて、牽引車１の移動方向及び移動量を求める部位である。

前輪２８は、操舵装置（不図示）に接続されており、移動制御部１７によって車輪の向きが調整される。後輪２９は、駆動機構を介して駆動輪モータ（不図示）が接続されており、移動制御部１７によって回転駆動されるようになっている。移動制御部１７が駆動輪モータ及び操舵装置を制御することで、牽引車１は路面を走行すると共に、進行方向を変えることができる。エンコーダ４０は、車輪の回転角および操舵角を検出するセンサを備えた部位である。

（概略説明）
本明細書で説明する技術は、航空機１０１へコンテナ（貨物）を搭降載するためのガイドレス無人牽引車の軌道生成に関する技術である。具体例を図３に示す。図３は、航空機１０１の前方から牽引車１が接近する場合（すなわち航空機１０１に貨物を積み込む場合）を示す図である。

牽引車１は、ドリー（被牽引車）１１１～１１４を牽引している。ドリー１１１～１１４の各々には、コンテナＣ１～Ｃ４が載置されている。航空機１０１の貨物室ドアの近傍には、搭降載器材１１５が配置してある。

搭降載器材１１５は、荷物出し入れ部１１５ａ、第１側面１１５ｂ、第２側面１１５ｃ、搬送経路部１１５ｄを備えている。搬送経路部１１５ｄは、方向ｙ０に伸びている。搬送経路部１１５ｄは、コンテナを滑らせて移動させる経路である。搬送経路部１１５ｄの方向ｙ０の正方向の端部には、荷物出し入れ部１１５ａが配置されている。荷物出し入れ部１１５ａは、ドリーとの間でコンテナを受け渡しする部位である。

搭降載器材１１５は、搭降載器材座標系ＣＳ０を備えている。搭降載器材座標系ＣＳ０は、基準位置Ｐ０を基準とした、搭降載器材１１５の姿勢を表している。基準位置Ｐ０は任意の位置でよい。本実施例では、荷物出し入れ部１１５ａの中央部を基準位置Ｐ０としている。搭降載器材１１５の姿勢は、搬送経路部１１５ｄが伸びている方向である方向ｙ０と、方向ｙ０に垂直な方向ｘ０とによって定められる。

第１側面１１５ｂは、方向ｘ０の正方向側における搭降載器材１１５の側面である。第１側面１１５ｂには、マーカＭ１およびＭ２が配置されている。マーカＭ１およびＭ２は、マグネットシートにより貼付されている。マーカＭ１およびＭ２は、固有ＩＤを検出可能な視覚マーカ（例えばＡＲマーカ）である。

図１および図３に示すように、マーカ基準座標ＭＣ１およびＭＣ２は、マーカＭ１およびＭ２の各々の位置および姿勢（向き）を示している。マーカＭ１およびＭ２は、予め定められた所定距離Ｄ１だけ互いに離れて、水平方向に配置されている。またマーカＭ１およびＭ２は、それぞれ、搭降載器材座標系ＣＳ０の原点との相対位置が、計測あるいは設計によって一意に決められているものとする。よって、マーカＭ１およびＭ２の位置と、搭降載器材座標系ＣＳ０との関係を既定（拘束）することができる。この幾何学的拘束は、後段のグラフ最適化において利用することができる。

搭降載器材１１５の第２側面１１５ｃは、方向ｘ０の負方向側における搭降載器材１１５の側面である。第２側面１１５ｃには、マーカＭ３およびＭ４が配置されている。マーカＭ３およびＭ４の内容は、前述したマーカＭ１およびＭ２の内容と同様であるため、詳細な説明を省略する。

牽引車１は、始点位置Ｐ１から停車位置Ｐ３まで移動する。停車位置Ｐ３は、ドリー１１１が搭降載器材１１５の荷物出し入れ部１１５ａに横付けされた状態になる位置である。このようにドリーを整列させた状態で停車位置Ｐ３に牽引車１を停車させるためには、始点位置Ｐ１から停車位置Ｐ３へ至る目標軌道ＴＴ（図３点線）を設定しなければならない。しかし、航空機１０１は決まった位置および姿勢で停車するとは限らない。従って、荷物の搭降載のたびに目標軌道ＴＴを設定する必要がある。また、ドリーを搭降載器材１１５に横付けする際に、搭降載器材１１５の位置だけでなく、ヨー角（上下軸回りの角度）も正確に計測する必要がある。ドリーと搭降載器材１１５との接触を避けつつ、搭降載器材１１５上のコンベヤにスムーズにコンテナを載せる必要があるためである。そのため、搭降載器材１１５の位置および姿勢を正確に推定する必要が生じる。また作業のリードタイムを考慮すると、搭降載器材１１５の位置および姿勢の推定は、牽引車１の停止時だけでなく移動中にも実行可能であることが望ましい。

なお、航空機１０１の「後方」から牽引車１が接近する場合（すなわち航空機１０１から貨物を降ろす場合）には、マーカＭ３およびＭ４を観測すればよい。この場合の技術内容も、航空機１０１の「前方」から牽引車１が接近する場合（図２）と同様であるため、説明を省略する。

（搭降載器材１１５の外形モデルデータの作成）
事前準備として、搭降載器材１１５の外形形状を示す外形モデルデータを用意し、外形モデル記憶部２０に記憶させておく必要がある。これは、ＬｉＤＡＲデータに基づいて搭降載器材１１５の位置および姿勢を推定するステップ（Ｓ４０）において、レーザ観測による点群と外形モデルデータによる点群とを、ＩＣＰ（Iterative Closest Point）マッチングにより重ね合わせるためである。外形モデルデータを作成する方法は、様々であって良い。例えば、ＣＡＤなどの設計情報から３次元点群データを生成する方法、３次元計測器を用いて実車を計測し直接データを生成する方法、カメラ観測からデータを生成する方法（例：ＳｆＭ（Structure from Motion）、などが挙げられる。

図４に形状モデルＭＤの例を示す。形状モデルＭＤは、搭降載器材１１５の上面図に対応したモデルである。この例では、ＬｉＤＡＲ３１がスキャンする高さの点群だけがモデル化されている。具体的には、ＬｉＤＡＲ３１でレーザ光を走査する水平面で、３次元点群を輪切り形状に抜き出している。形状モデルＭＤは、基準位置ＰＤを備えている。基準位置ＰＤは、搭降載器材１１５の基準位置Ｐ０（図２参照）と一意に対応する位置である。従って、ＩＣＰマッチングにより、搭降載器材１１５の基準位置Ｐ０および姿勢（搭降載器材座標系ＣＳ０）を特定することが可能である。また形状モデルＭＤは、マーカ位置ＭＭ１およびＭＭ２との相対的な位置および姿勢を示すマーカデータを備えている。マーカ位置ＭＭ１およびＭＭ２は、搭降載器材１１５の側面に貼り付けられているマーカＭ１およびＭ２の中心と一意に対応する位置である。

（位置姿勢推定処理の内容）
図５～図８を用いて、搭降載器材１１５の位置および姿勢の推定方法を説明する。本説明例では図６に示すように、時刻ｔにおける移動点ＭＰ１から時刻ｔ＋２における移動点ＭＰ３へ、牽引車１が移動する場合の処理内容を説明する。図６において、車体基準座標ＶＣ１～ＶＣ３は、時刻ｔ～ｔ＋２の各々における牽引車１の位置および姿勢（向き）を示している。車体基準座標ＶＣ１およびＶＣ２の原点は、牽引車１の代表位置（本実施例では、後輪接地点の中央）を示している。カメラ基準座標ＣＣ１～ＣＣ３は、時刻ｔ～ｔ＋２の各々におけるカメラ３０の位置および姿勢（向き）を示している。視野角ＶＡは、カメラ３０の撮影可能な角度範囲である。ＬｉＤＡＲ基準座標ＬＣ１～ＬＣ３は、時刻ｔ～ｔ＋２の各々におけるＬｉＤＡＲ３１の位置および姿勢（向き）を示している。

図５の位置姿勢推定処理のフローは、ドリーを搭降載器材１１５に横付けさせる指示が制御部１０に入力されたときに、開始される。Ｓ１０において、データ取得処理が行われる。データ取得処理では、カメラ３０は、搭降載器材１１５に貼られたマーカＭ１およびＭ２を撮像する。ＬｉＤＡＲ３１は、搭降載器材１１５の車体側面付近を観測する。車体位置姿勢算出部４１は、観測データ取得時の車体位置を記録する。

Ｓ２０において、マーカ位置姿勢推定部１１は、カメラ３０から受信した画像データに基づいて、マーカＭ１およびＭ２の位置および姿勢を推定する。この推定処理は、公開されているソフトライブラリを用いることができるため、説明を省略する。またマーカ位置姿勢推定部１１は、マーカＭ１およびＭ２の観測によって、各マーカに固有のＩＤ情報を取得できる。マーカＭ１およびＭ２に予め付与されたＩＤ情報と、観測によって取得されたＩＤ情報とを比較することにより、マーカＭ１およびＭ２を確実に認識することができる。ＩＤ情報を用いてマーカＭ１およびＭ２を認識することで、画像処理によって搭降載器材の存在の有無や存在位置、器材の方向などを認識する場合に比して、認識精度や識別の確実性を高めることができる。

Ｓ３０において、マーカ誤差モデル計算部２１は、マーカ位置姿勢推定部１１で推定されたマーカの位置および姿勢の誤差を示す、マーカ計測誤差情報を取得する。マーカ計測誤差情報は、マーカ誤差モデル記憶部２３に記憶される。図７の例を用いて、カメラ３０による計測の誤差分布について説明する。図７は、マーカＭ１およびＭ２の位置および姿勢を推定する場合を説明している。具体的には、カメラ３０からマーカＭ１へ、光軸ＯＰ１の方向（すなわち奥行き方向）に１本の測定線が伸びる。カメラ３０には、「光軸ＯＰ１方向(奥行方向)の誤差が、光軸ＯＰ１に垂直な方向（幅方向）の誤差に比して極端に大きい」という、位置計測の誤差の異方性がある。これは、カメラ３０による奥行方向の位置計測は、撮像素子に映ったマーカＭ１の幅のピクセル変化量に基づいて行われるためである。従って、カメラ３０によるマーカＭ１の位置計測の誤差分布ＣＥ１は、光軸ＯＰ１方向に極端に大きい長径を有する楕円形となる。また同様に、カメラ３０によるマーカＭ２の位置計測の誤差分布ＣＥ２は、光軸ＯＰ２方向に極端に大きい長径を有する楕円形となる。

Ｓ４０において、対象物位置姿勢推定部１２は、ＬｉＤＡＲ３１から受信したＬｉＤＡＲデータに基づいて、搭降載器材１１５の位置および姿勢を推定する。具体的には、ＬｉＤＡＲ観測による点群と外形モデルデータによる点群とを、ＩＣＰマッチングにより重ね合わせる。ＩＣＰマッチングでは、繰り返し計算を行うことにより、最小二乗法により誤差を最小化することができる。よってＩＣＰマッチングを用いることにより、搭降載器材１１５の位置および姿勢の推定精度を高くすることができる。

ＩＣＰマッチングを行うためには、形状モデルＭＤとＬｉＤＡＲ３１による計測データとを、おおまかに位置合わせする（初期重ね合わせする）必要がある。この初期重ね合わせに失敗すると、誤った対応関係に影響されて、収束が遅くなる場合や、正解に到達せずに誤マッチングが発生する場合がある。そこで本明細書の技術では、マーカ位置姿勢推定部１１で推定（Ｓ２０）されたマーカＭ１およびＭ２の位置と、マーカデータが示すマーカ位置ＭＭ１およびＭＭ２（図４参照）が一致するように、形状モデルＭＤの初期重ね合わせ位置および初期重ね合わせ姿勢を定める。すなわち、マーカＭ１およびＭ２が搭降載器材１１５の側面に貼り付けられている位置・姿勢の既知の設計情報を元に、マーカ位置姿勢推定部１１で推定されたマーカＭ１およびＭ２の位置から、形状モデルＭＤの初期位置および初期姿勢を求める。マーカ位置姿勢推定部１１で推定（Ｓ２０）されたマーカＭ１およびＭ２の位置は、奥行方向の確からしさは低いものの、形状モデルＭＤの初期位置を定めるためには十分な精度を有していると言える。

Ｓ５０において、対象物誤差モデル計算部２２は、対象物位置姿勢推定部１２で推定された搭降載器材１１５の位置および姿勢の誤差を示す、対象物計測誤差情報を取得する。対象物計測誤差情報は、対象物誤差モデル記憶部２４に記憶される。

ＩＣＰマッチングで発生する誤差の計算に関しては、例えば下記の文献に示すように、幾つかの方法が提案されている。（文献１：Andrea Censi, “An accurate closed-form estimate of ICP’s covariance”, 2007 IEEE International Conference on Robotics and Automation Roma, Italy, 10-14 April 2007.）（文献２：Prakhya Sai Manoj, Liu Bingbing, Yan Rui, and Weisi Lin, “A Closed-form Estimate of 3D ICP Covariance”, MVA2015 IAPR International Conference on Machine Vision Applications, May 18-22, 2015.）。また、ＩＣＰマッチングでの誤差に加え、ＬｉＤＡＲ３１のレーザの測距精度を考慮してもよい。レーザの照射方向の測距誤差は、例えば、ビームの測距精度で表すことができる。レーザのスキャン方向の測距誤差は、例えば、ビーム間隔やビームの太さなどを元に一様分布などで近似することができる。

図７の例を用いて、ＬｉＤＡＲ３１による計測の誤差分布について説明する。図７において搭降載器材１１５の第２側面１１５ｃに沿って多数並んでいる黒点は、ＬｉＤＡＲ３１による観測点を示している。ここで、ある所定点ＳＰの位置および姿勢をＬｉＤＡＲ３１で測定する場合を例として考える。ＬｉＤＡＲ３１には、「レーザのスキャン方向の誤差が、光軸ＯＰ３方向(奥行方向)の誤差に比して大きい」という、位置計測の誤差の異方性がある。これは、奥行方向の誤差範囲ＤＲは測距精度に依存する一方で、スキャン方向の誤差範囲ＬＲは、スキャン間隔(光源からの距離に比例する)と光芒の幅に基づく一様分布で近似することができるためである。従って、誤差範囲ＤＲよりも誤差範囲ＬＲの方が大きくなるためである。図７では、ＬｉＤＡＲ３１による位置計測の誤差分布ＬＥは、搭降載器材１１５の側壁方向に長径を有する楕円形となる。

なお図７において、誤差分布ＣＥ１、ＣＥ２、ＬＥの誤差楕円の大きさは、分かりやすさのために大きく記載している。実際には、標準偏差でサブｍｍから数ｃｍ程度のばらつきである。

Ｓ６０において、グラフ構造データ生成部１３によって、グラフ構造データ生成処理が行われる。例として、図６の位置関係に基づいて、図８のグラフ構造データが生成される場合について説明する。図８のグラフ構造データは、５種類のノード（車体ノードＶＮ、カメラノードＣＮ、ＬｉＤＡＲノードＬＮ、マーカノードＭ１ＮおよびＭ２Ｎ、搭降載器材ノードＨＮ）を備える。

車体ノードＶＮ１～ＶＮ３は、移動点ＭＰ１～ＭＰ３の各々における、牽引車１の代表位置および姿勢を示す情報である。牽引車１の代表位置および姿勢は、Ｓ１０において車体位置姿勢算出部４１によって取得することができる。カメラノードＣＮ１～ＣＮ３は、移動点ＭＰ１～ＭＰ３の各々における、カメラ３０の位置および姿勢を示す情報である。ＬｉＤＡＲノードＬＮ１～ＬＮ３は、移動点ＭＰ１～ＭＰ３の各々における、ＬｉＤＡＲ３１の位置および姿勢を示す情報である。マーカノードＭ１ＮおよびＭ２Ｎは、移動点ＭＰ１～ＭＰ３の各々において推定された、マーカＭ１およびＭ２の位置および姿勢を示す情報である。搭降載器材ノードＨＮは、移動点ＭＰ１～ＭＰ３の各々において推定された、搭降載器材１１５の基準位置Ｐ０の位置および姿勢を示す情報である。図８では説明の便宜上、すべてのノードを円で示している。しかし実際は、各ノードは、ヨー角姿勢などの姿勢情報も有している。

また図８のグラフ構造データは、７種類のアーク（アークＶＬＡ、アークＶＣＡ、アークＭＭＡ、アークＭＨＡ、アークＣＡ、アークＬＡ、アークＶＶＡ）を備える。アークＶＬＡ１～ＶＬＡ３、アークＶＣＡ１～ＶＣＡ３、アークＭＭＡ、アークＭＨＡ１およびＭＨＡ２は、図８において実線で示されている。これらの５種類のアークは機械的な拘束であり、基本的に変化しない。図８において、アークＣＡは、点線で示されている。アークＬＡは、一点鎖線で示されている。アークＶＶＡは、二点鎖線で示されている。これらの３種類のアークは、観測結果に基づいて形成された拘束である。これらのアークは、センサの性質や、センサと観測対象との距離などによって異なる値を示す。

アークＶＬＡ１～ＶＬＡ３は、移動点ＭＰ１～ＭＰ３の各々における、車体ノードＶＮの座標系からみたＬｉＤＡＲノードＬＮの座標系の位置および姿勢を規定している。同様に、アークＶＣＡ１～ＶＣＡ３は、移動点ＭＰ１～ＭＰ３の各々における、車体ノードＶＮの座標系からみたカメラノードＣＮの座標系の位置および姿勢を規定している。牽引車１の代表位置からみた、ＬｉＤＡＲ３１およびカメラ３０の位置関係は不変である。よって、アークＶＬＡ１～ＶＬＡ３は同一の位置および姿勢を規定するとともに、アークＶＣＡ１～ＶＣＡ３は同一の位置および姿勢を規定する。アークＭＭＡは、マーカノードＭ１ＮとＭ２Ｎの間の位置および姿勢を規定する。アークＭＨＡ１は、マーカノードＭ１Ｎと搭降載器材ノードＨＮの間の位置および姿勢を規定する。アークＭＨＡ２は、マーカノードＭ２Ｎと搭降載器材ノードＨＮの間の位置および姿勢を規定する。

アークＣＡ１１は、カメラノードＣＮ１の座標系（移動点ＭＰ１）からみた、マーカノードＭ１Ｎの座標系の位置および姿勢を規定している。また、マーカＭ２はカメラ３０の視野角ＶＡ（図６）から外れており、観測できていない。よって、カメラノードＣＮ１とマーカノードＭ２Ｎを結ぶアークは規定されていない。アークＣＡ２１およびＣＡ２２は、カメラノードＣＮ２の座標系（移動点ＭＰ２）からみた、マーカノードＭ１ＮおよびＭ２Ｎの座標系の位置および姿勢を規定している。なお、カメラノードＣＮ３の座標系（移動点ＭＰ３）からは、マーカＭ１およびＭ２はカメラ３０の視野角ＶＡから外れており観測できていない。よって、カメラノードＣＮ３とマーカノードＭ１ＮおよびＭ２Ｎとを結ぶアークは規定されていない。

アークＬＡ１は、ＬｉＤＡＲノードＬＮ１の座標系（移動点ＭＰ１）からみた、搭降載器材ノードＨＮの座標系の位置および姿勢を規定している。同様に、アークＬＡ２は、ＬｉＤＡＲノードＬＮ２の座標系（移動点ＭＰ２）からみた、搭降載器材ノードＨＮの座標系の位置および姿勢を規定している。またアークＬＡ３は、ＬｉＤＡＲノードＬＮ３の座標系（移動点ＭＰ３）からみた、搭降載器材ノードＨＮの座標系の位置および姿勢を規定している。アークＶＶＡ１は、車体ノードＶＮ１とＶＮ２との間の位置および姿勢を規定している。アークＶＶＡ２は、車体ノードＶＮ２とＶＮ３との間の位置および姿勢を規定している。

Ｓ７０において、マーカ誤差モデル記憶部２３および対象物誤差モデル記憶部２４は、各ノードを結ぶアークに対し、誤差分布をグラフの重みとして与える。すなわち、計測データの分散共分散行列に、誤差分布の分散共分散行列を加算し、その逆行列を最適化の重みとして用いる。具体的に説明する。各アークは、「ノード間の相対位置・姿勢の初期値」と「確からしさ」に関する事前知識を保持している。「確からしさ」は、情報行列で表すことができる。情報行列は、ノード間の相対位置および姿勢の誤差についての分散共分散行例の逆行列である。「確からしさ」の性質は、アーク毎に異なる。すなわち、図８において実線で示されている５種類のアーク（アークＶＬＡ１～ＶＬＡ３、アークＶＣＡ１～ＶＣＡ３、アークＭＭＡ、アークＭＨＡ１およびＭＨＡ２）は機械的な拘束であり、「確からしさ」が高い。一方、アークＣＡ（図８の点線）およびアークＬＡ（図８の一点鎖線）は、観測による拘束であるため、例えば、距離（奥行）方向の「確からしさ」が低い。

本明細書の技術では、マーカ位置姿勢推定部１１で推定されたマーカの位置および姿勢の誤差を示すマーカ計測誤差情報を、マーカ誤差モデル記憶部２３から読み出す。マーカ計測誤差情報は、カメラ３０によるマーカＭ１およびの検出に関する誤差分布を含んでいる。そして、アークＣＡに対して、マーカ計測誤差情報に基づく第１の重みを与える。また、搭降載器材１１５の位置および姿勢の誤差を示す対象物計測誤差情報を、対象物誤差モデル記憶部２４から読み出す。対象物計測誤差情報は、ＬｉＤＡＲ３０による搭降載器材１１５の位置および姿勢の推定に関する誤差分布を含んでいる。そして、アークＬＡに対して、対象物計測誤差情報に基づく第２の重みを与える。

Ｓ８０においてグラフ最適化部１４は、Ｓ６０およびＳ７０で生成したグラフ構造データを最適化する。最適化には、例えば、Graph-Based SLAM（Simultaneous Localization and Mapping）を用いることができる。Graph-Based SLAMは、カメラ位置やマーカ位置をノード（頂点）、それらの間の相対位置の制約条件をアーク（辺）とするグラフに対して、各制約の誤差総和を最小化するように最適化計算を行う手法である。これにより、マーカノードＭＮ１、ＭＮ２および搭降載器材ノードＨＮの、位置および姿勢を補正することができる。

Ｓ９０においてアクセス位置推定部１５は、最適化によって修正されたグラフ構造データを元に、搭降載器材１１５の位置および姿勢を取得する。軌道生成部１６は、搭降載器材１１５の位置および姿勢に基づいて、目標軌道ＴＴ（図３参照）を生成する。移動制御部１７は、目標軌道ＴＴと速度パターンに基づいて、停車位置Ｐ３（図３）まで牽引車１を移動するための制御指令値を算出する。

本実施例では、制御部１０は、牽引車１の代表位置から見た搭降載器材１１５の位置を常に出力する。これにより、牽引車１が移動する場合においても、目標軌道ＴＴの生成や修正を行うことが可能となる。

Ｓ１００において、位置姿勢推定処理を終了するか否かが判断される。例えば、ドリーを横付けする処理が完了しているか否かによって判断してもよい。否定判断される場合（Ｓ１２０：ＮＯ）には、Ｓ１０へ戻り処理を続行する。このようにループ処理することで、位置姿勢推定処理を、移動点ＭＰ１～ＭＰ３（図６）の各々において行うことができる。よって、牽引車１の移動中において、搭降載器材１１５の位置および姿勢の推定値を更新することができる。一方、肯定判断される場合（Ｓ１２０：ＹＥＳ）には、処理を終了する。

（効果）
搭降載器材１１５に貼り付けられたマーカをカメラで撮像することで搭降載器材１１５の位置や姿勢を認識する技術では、一般的に、搭降載器材１１５の奥行方向の位置や、ヨー方向やピッチ方向の回転姿勢を正確に求めることが困難である。よって、ドリーを搭降載器材１１５に横付けする際に、意図しない衝突等が発生してしまう場合がある。本明細書の技術では、搭降載器材１１５に貼付されたマーカの位置および姿勢をカメラ３０を用いて推定するとともに、搭降載器材１１５の位置および姿勢をＬｉＤＡＲ３１を用いて推定する。図７を用いて説明したように、カメラ３０による測定の誤差分布の異方性と、ＬｉＤＡＲ３１による測定の誤差分布の異方性とは、直交する関係にあるため、誤差分布の補完関係が成立している。そして、カメラノードＣＮからみたマーカノードの位置および姿勢を示すアークＣＡに対して、カメラ３０による測定の誤差分布に基づく第１の重みを与える。また、ＬｉＤＡＲノードＬＮからみた搭降載器材ノードＨＮの位置および姿勢を示すアークＬＡに対して、ＬｉＤＡＲ３１による測定の誤差分布に基づく第２の重みを与える。これにより、補完関係にある誤差分布を用いてグラフ構造を最適化することができる。搭降載器材１１５の位置・姿勢を高精度に推定することができるため、ドリーを搭降載器材１１５に確実に横付けすることが可能となる。

（概略説明）
実施例２は、無人フォークリフトへの本技術の適用例である。実施例２の技術の目的は、物流倉庫内の荷を無人フォークリフトに荷役させるため、荷物の乗ったパレットの位置および姿勢を正確に計測することにある。図９に、実施例２に係る物流倉庫内の様子を示す。物流倉庫では、パレットＰＬ１～ＰＬ６に載せられた荷物（例：段ボール箱）が、固定ラック上に配置されている。パレットＰＬ１～ＰＬ６の各々には、その正面中央部にマーカＭ１１～Ｍ１６が貼付されている。マーカＭ１１～Ｍ１６は、パレットの中央に正確に印刷されており、パレットとの位置関係が既定されている。

フォークリフト２０１は、フォーク２１０を備えている。フォーク２１０には、カメラ２３０およびＬｉＤＡＲ２３１が固定されている。フォーク２１０は、マストに沿って上下に移動可能である。フォーク２１０の高さを調整することで、カメラ２３０およびＬｉＤＡＲ２３１の観測高さを調整することができる。またフォークリフト２０１は、不図示の制御部を備えている。制御部の内容は、実施例１で説明した内容と同様であるため、説明を省略する。

（位置姿勢推定処理）
実施例２のフォークリフト２０１の例においても、実施例１の図５～図８で説明した位置姿勢推定処理を用いることができる。このとき、実施例１の説明において、「牽引車１」を「フォークリフト２０１」に読み替え、「搭降載器材１１５」を「パレットＰＬ」に読み替え、「搭降載器材ノードＨＮ」を「パレットノードＰＮ」に読み替えればよい。

（効果）
図９に示すように、パレットＰＬ１～ＰＬ３が同一高さに隙間なく並べられている場合がある。この場合、ＬｉＤＡＲ２３１による観測点ＬＰ２（図９点線）は、概ね長い直線状となるため、パレットＰＬ１～ＰＬ３の各々の境界を特定することは困難である。また、観測点ＬＰ２に示すように、レーザビームをパレットＰＬ１～ＰＬ３の側面に正確に走査させることは困難である。よってＬｉＤＡＲ２３１単独では、荷役対象となるパレットＰＬ１～ＰＬ３の正確な位置および姿勢を計測できない。一方、カメラ２３０によるマーカＭ１１～Ｍ１３の観測では、パレットＰＬ１～ＰＬ３の中心位置を特定することは可能である。しかし前述したように、カメラ２３０に対する奥行方向の距離や、パレットのヨー角を正確に特定することは困難である。この困難性は、遠方から観測する場合やパレット正面から観測する場合において、顕著となる。よってカメラ２３０単独では、荷役対象となるパレットＰＬ１～ＰＬ３の正確な位置および姿勢を計測できない。

そこで実施例２の技術では、カメラ２３０およびＬｉＤＡＲ２３１を組み合わせて、パレットの位置および姿勢を推定する。具体的に説明する。カメラ２３０によってマーカの高さ方向の位置を計測（Ｓ２０）できるため、ＬｉＤＡＲ２３１のレーザビームが正しくパレットの側面を走査するように、フォークリフト２０１の高さを調整できる。また、カメラ２３０によって、各マーカに固有のＩＤ情報を取得できる（Ｓ２０）。取得されたＩＤ情報に基づいて、マーカが貼付されたパレットの幅、高さ、奥行きなどの設計情報を知ることができる。従って、直線状の観測点ＬＰ２から、各々のパレットを示す複数の観測点群を特定することができる。そして、観測点群の各々傾きを調べることで、各パレットのヨー角を正確に推定することができる（Ｓ４０）。これにより、パレットの位置・姿勢を高精度に推定することができるため、フォーク２１０をパレットに確実に差し込むことが可能となる。

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

（変形例）
グラフ構造データ生成処理（Ｓ６０）では、以下に示すように、様々な態様のグラフ構造データを生成することができる。グラフ構造データの態様は、推定結果の要求精度やセンサの精度、計算時間などを総合的に考慮して決定することができる。第１の態様として、図８に示すように、過去（時刻ｔ）から現在（時刻ｔ＋２）までの全てのノードおよびアークを用いて、グラフ構造データを作成してもよい。第２の態様として、車体（移動体）ノードを含まないグラフ構造データを用いてもよい。この場合、例えば図８のグラフ構造データの例では、車体ノードＶＮ１～ＶＮ３、アークＶＬＡ１～ＶＬＡ３、ＶＣＡ１～ＶＣＡ３、ＶＶＡ１およびＶＶＡ２を省略することができる。
第３の態様として、１の時刻の１つの移動点（例：最新の観測結果）におけるセンサノードを備えるグラフ構造データを作成してもよい。例えば、時刻ｔにおいては、移動点ＭＰ１（図８参照）における３つのセンサノード（ＶＮ１、ＣＮ１、ＬＮ１）を備えたグラフ構造データを作成してもよい。また、時刻ｔ＋１においては、移動点ＭＰ２における３つのセンサノード（ＶＮ２、ＣＮ２、ＬＮ２）を備えたグラフ構造データを作成してもよい。

ＬｉＤＡＲ観測点ＬＰ（図１の点線）に示すように、ＬｉＤＡＲ３１のビームを搭降載器材１１５の側面に走査させる必要がある。しかし、牽引車１の微妙な揺れ、ＬｉＤＡＲ３１の傾き、搭降載器材１１５のアウトリガー高さの調整など、様々な理由に起因して、走査が失敗する事態が発生しうる。また、この様な事態は、２Ｄ－ＬｉＤＡＲのみならず、上下の解像度が低い３Ｄ－ＬｉＤＡＲにおいても生じ得る。そこで、カメラによるマーカの観測結果（Ｓ２０）に基づき、搭降載器材１１５の側面の高さを求めてもよい。そして、求めた高さに応じて、ＬｉＤＡＲ３１の走査高さを調整するアクチュエータを備えてもよい。これにより、搭降載器材１１５の側面を確実にレーザ走査することが可能となる。

ＩＣＰマッチング（Ｓ４０）の初期重ね合わせ方法は、マーカの観測結果を利用する方法に限られず、様々な方法が利用可能である。例えば、観測されたＬｉＤＡＲ点群を直線（線分）として近似させ、その傾きや端点位置の情報を使う方法であってもよい。

マーカを搭降載器材１１５の側面に配置する方法は、マーカ間の相対位置が一意に決まるのであれば、様々であってよい。テープでの貼付、接着、ねじ式、磁石式、ピン止めなどの態様でもよい。レーザ印刷、ホットスタンプ印刷、スクリーン印刷などによって、搭降載器材１１５にマーカを直接印刷してもよい。また１側面に配置するマーカ数が２個である場合を説明したが、この形態に限られない。

ＬｉＤＡＲ３１は２Ｄ－ＬｉＤＡＲに限られず、３Ｄ－ＬｉＤＡＲも利用可能である。

本実施例では、車体位置姿勢算出部４１がエンコーダ４０を用いて牽引車１の移動量を求める形態（Ｓ１０）を説明したが、この形態に限られない。２次元レーザセンサ等を使ったレーザーオドメトリを用いる形態、慣性センサを使う形態、カメラ画像を利用する形態などであってもよい。

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

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

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

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

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

搭降載器材１１５は、対象物の一例である。ＬｉＤＡＲ３１は、レーザセンサの一例である。牽引車１は、計測装置の一例である。マーカ位置姿勢推定部１１は、第１推定部の一例である。マーカ計測誤差情報は、第１誤差情報の一例である。マーカ誤差モデル計算部２１は、第１取得部の一例である。対象物位置姿勢推定部１２は、第２推定部の一例である。対象物計測誤差情報は、第２誤差情報の一例である。対象物誤差モデル計算部２２は、第２取得部の一例である。グラフ構造データ生成部１３およびグラフ最適化部１４は、補正部の一例である。形状モデルＭＤは、外形モデルの一例である。搭降載器材ノードＨＮは、対象物ノードの一例である。

１：牽引車 １０：制御部 １１：マーカ位置姿勢推定部 １２：対象物位置姿勢推定部 １３：グラフ構造データ生成部 １４：グラフ最適化部 ２１：マーカ誤差モデル計算部 ２２：対象物誤差モデル計算部 ３０：カメラ ３１：ＬｉＤＡＲ １１５：搭降載器材 Ｍ１～Ｍ４：マーカ

Claims (4)

1. マーカが配置されている対象物を撮像可能なカメラと、レーザを用いて前記対象物の位置を検出可能なレーザセンサと、を備えた計測装置であって、
   前記カメラを用いて前記マーカを撮像した画像に基づいて、前記マーカの位置および姿勢を推定する第１推定部と、
   前記カメラによる前記マーカの位置および姿勢の誤差を示す第１誤差情報を取得する第１取得部であって、
   前記第１誤差情報は、前記カメラの光軸方向の誤差が、前記カメラの光軸方向に対して略垂直な方向の誤差に比して大きいという誤差分布の異方性を有している、前記第１取得部と、
   前記レーザセンサを用いて前記対象物を測定した結果に基づいて、前記対象物の位置および姿勢を推定する第２推定部と、
   前記第２推定部で推定された前記対象物の位置および姿勢の誤差を示す第２誤差情報を取得する第２取得部であって、
   前記第２誤差情報は、前記レーザのスキャン方向の誤差が、前記レーザの照射方向の誤差に比して大きいという誤差分布の異方性を有している、前記第２取得部と、
   前記第１推定部で推定した前記マーカの位置および姿勢と、前記第２推定部で推定した前記対象物の位置および姿勢と、前記第１誤差情報と、前記第２誤差情報と、に基づいて前記対象物の位置および姿勢を補正する補正部と、
   を備える計測装置。
2. 前記第２推定部は、前記対象物の外形形状を示す外形データによって表される外形モデルであって予め用意された前記外形モデルを、前記レーザセンサによる計測データに対して位置合わせを行うことで、前記対象物の位置および姿勢を推定する、請求項１に記載の計測装置。
3. 前記外形モデルは、前記マーカとの相対的な位置および姿勢を示すマーカデータを備えており、
   前記第２推定部は、前記第１推定部で推定された前記マーカの位置と、前記マーカデータが示す前記マーカの位置が一致するように、前記外形モデルの初期位置を定める、請求項２に記載の計測装置。
4. 前記補正部は、
   前記カメラの位置および姿勢を示すカメラノード、
   前記レーザセンサの位置および姿勢を示すセンサノード、
   前記第１推定部で推定した前記マーカの位置および姿勢を示すマーカノード、
   前記第２推定部で推定した前記対象物の位置および姿勢を示す対象物ノード、
   の間の相対的位置関係を規定する複数のアークを備えるグラフ構造データを生成するグラフ構造データ生成部をさらに備え、
   前記グラフ構造データ生成部は、前記第１誤差情報に基づく第１の重みと、前記第２誤差情報に基づく第２の重みとを、前記複数のアークに付与し、
   前記補正部は、生成した前記グラフ構造データを、前記複数のアークから算出される誤差関数の和が最小となるように最適化することで、前記対象物の位置および姿勢の推定値を補正する、請求項１～３の何れか１項に記載の計測装置。

Images