JP7257431B2 - 移動体の制御方法、移動体及びプログラム - Google Patents

Description

本発明は、移動体の制御方法、移動体及びプログラムに関する。

周囲を検出するセンサを備えて自動で移動する移動体が知られている。このような移動体としては、例えば、荷物が搭載されたパレットを搬送する自動フォークリフトがある。特許文献１には、測域センサで計測される距離データに基づいて、パレットの前面の中心位置及び方向を特定して、パレットの荷取り作業を行う旨が記載されている。

特開２０１７－１７８５６７号公報

このような移動体においては、移動体を目的位置まで適切に移動させることが求められている。

本開示は、上述した課題を解決するものであり、移動体を目的位置まで適切に移動させることが可能な移動体の制御方法、移動体及びプログラムを提供することを目的とする。

上述した課題を解決し、目的を達成するために、本開示に係る移動体の制御方法は、自動で移動する移動体の制御方法であって、前記移動体に設けられたセンサに目標物を検出させて、前記センサの検出結果を点群として取得するステップと、前記点群に基づいて、前記目標物の位置及び姿勢を推定するステップと、前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置と、前記センサの性能とに基づき、前記センサの検出によって取得されると想定される理想の点群数を算出するステップと、前記理想の点群数と実際に取得された点群数との比較結果に基づき、前記目標物の位置及び姿勢の推定結果の精度に対する信頼度を算出するステップと、を含む。

上述した課題を解決し、目的を達成するために、本開示に係る移動体は、自動で移動する移動体であって、前記移動体に設けられたセンサに目標物を検出させて、前記センサの検出結果を点群として取得する検出制御部と、前記点群に基づき推定された前記目標物の位置及び姿勢の推定結果の精度に対する信頼度の情報を取得する信頼度情報取得部と、を含み、前記信頼度は、前記点群に基づいて、前記目標物の位置及び姿勢が推定され、前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置と、前記センサの性能とに基づき、前記センサの検出によって取得されると想定される理想の点群数が算出されることで、前記理想の点群数と実際に取得された点群数との比較結果に基づき算出される。

上述した課題を解決し、目的を達成するために、本開示に係るプログラムは、自動で移動する移動体の制御方法をコンピュータに実行させるプログラムであって、前記移動体に設けられたセンサに目標物を検出させて、前記センサの検出結果を点群として取得するステップと、前記点群に基づいて、前記目標物の位置及び姿勢を推定するステップと、前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置と、前記センサの性能とに基づき、前記センサの検出によって取得されると想定される理想の点群数を算出するステップと、前記理想の点群数と実際に取得された点群数との比較結果に基づき、前記目標物の位置及び姿勢の推定結果の精度に対する信頼度を算出するステップと、を、コンピュータに実行させる。

本開示によれば、移動体を目的位置まで適切に移動させることができる。

図１は、第１実施形態に係る移動制御システムの模式図である。 図２は、移動体の構成の模式図である。 図３は、管理システムの模式的なブロック図である。 図４は、情報処理装置の模式的なブロック図である。 図５は、移動体の制御装置の模式的なブロック図である。 図６は、目標物の検出を説明する模式図である。 図７は、近似線候補の取得を説明するための模式図である。 図８は、近似線候補の具体例を示す模式図である。 図９は、スコアの一例を示すグラフである。 図１０は、目標物の両端点の特定処理の一例を説明する模式図である。 図１１は、信頼度の算出方法を説明するための模式図である。 図１２は、第２パスに沿った移動の一例を示す模式図である。 図１３は、第２パス設定の処理フローを説明するフローチャートである。 図１４は、第２実施形態において理想の計測点数を算出する方法を説明するための模式図である。

以下に添付図面を参照して、本発明の好適な実施形態を詳細に説明する。なお、この実施形態により本発明が限定されるものではなく、また、実施形態が複数ある場合には、各実施形態を組み合わせて構成するものも含むものである。

（第１実施形態）
（移動制御システムの全体構成）
図１は、第１実施形態に係る移動制御システムの模式図である。図１に示すように、第１実施形態に係る移動制御システム１は、移動体１０、管理システム１２、及び情報処理装置１４を含む。移動制御システム１は、設備Ｗに所属する移動体１０の移動を制御するシステムである。設備Ｗは、例えば倉庫など、物流管理される設備である。移動制御システム１においては、移動体１０によって設備Ｗの領域ＡＲ内に配置された目標物Ｐをピックアップして搬送させる。領域ＡＲは、例えば設備Ｗの床面であり、目標物Ｐが設置されたり移動体１０が移動したりする領域である。目標物Ｐは、本実施形態では、パレット上に荷物が積載された搬送対象物である。目標物Ｐは、複数の柱ＰＡと、柱ＰＡ同士の間に形成される開口ＰＢとが、前面Ｐａに形成されている。前面Ｐａとは、移動体１０がアプローチしてくる側の面を指す。移動体１０は、開口ＰＢに後述するフォーク２４を挿入することで、目標物Ｐを保持する。ただし、目標物Ｐは、パレット上に荷物が積載されたものに限られず任意の形態であってよく、例えばパレットを有さず荷物のみであってもよい。以下、領域ＡＲに沿った一方向を、方向Ｘとし、領域Ａに沿った方向であって方向Ｘに交差する方向を、方向Ｙとする。本実施形態では、方向Ｙは、方向Ｘに直交する方向である。方向Ｘ、方向Ｙは、水平方向といってもよい。また、方向Ｘ、方向Ｙに直交する方向を、すなわち鉛直方向を、方向Ｚとする。

設備Ｗ内の領域ＡＲには、複数の設置領域ＡＲ０が設けられている。設置領域ＡＲ０は、目標物Ｐが設置される領域である。設置領域ＡＲ０は、目標物Ｐを設置すべき領域として、予め設定される。設置領域ＡＲ０は、例えば白線などで区分されており、設置領域ＡＲ０の位置（座標）、形状、及び大きさは、予め設定されている。設置領域ＡＲ０内においては、目標物Ｐは、前面Ｐａが方向Ｘ側を向くように配置されている。図１の例では、目標物Ｐは、方向Ｚから見て前面Ｐａに直交する軸ＰＸが方向Ｘと沿うように、すなわち目標物Ｐの向きが設置領域ＡＲ０に対してずれないように、設置領域ＡＲ内に配置されている。ただし、目標物Ｐは、軸ＰＸが方向Ｘと沿っていることに限られず、軸ＰＸが方向Ｘから傾斜して、すなわち設置領域ＡＲに対して向きがずれて設置されていてもよい。例えば、目標物Ｐは、軸ＰＸと方向Ｘとの傾斜角度が、４５度以下となるように、設置領域ＡＲ０に配置されることが好ましい。

なお、本実施形態では、設置領域ＡＲ０は、設備Ｗの床である領域ＡＲに設けられているが、それに限られず、例えば目標物Ｐを設備Ｗに搬入した車両の荷台内に設けられてもよい。また、本実施形態では、設置領域ＡＲ０は、目標物Ｐ毎に区画されており、設置領域ＡＲ０には目標物Ｐが１つ配置されるが、それに限られない。例えば、設置領域ＡＲ０は、フリースペースとして、複数の目標物Ｐが設置されるように設定されていてもよい。また、図１の例では設置領域ＡＲ０は矩形であるが、形状及び大きさは任意であってよい。また、領域ＡＲに設けられる設置領域ＡＲ０の数も任意であってよい。

移動体１０は、自動で移動可能な装置である。本実施形態では、移動体１０は、フォークリフトであり、さらにいえば、いわゆるＡＧＦ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｆｏｒｋｌｉｆｔ）である。図１に例示するように、移動体１０は、設備Ｗにおける領域ＡＲ上を移動する。移動体１０は、第１パスＲ１（広域パス）に従って、第１位置Ａ１から第２位置Ａ２まで移動しつつ、後述するセンサ２６によって、目標物Ｐを複数回検出する。移動体１０は、センサ２６による複数回の検出結果から点群を取得し、点群に基づき目標物Ｐの位置及び姿勢を特定する。移動体１０は、第２位置Ａ２に到達したら、目標物Ｐの位置及び姿勢に基づいて設定された第２パスＲ２（アプローチパス）に従って、第２位置Ａ２から目標位置Ａ３まで移動して、目標物Ｐをピックアップする。なお、ここでの目標物Ｐの位置とは、方向Ｘ及び方向Ｙの二次元座標系ＣＯにおける目標物Ｐの座標を指し、目標物Ｐの姿勢とは、方向Ｘ及び方向Ｙに直交する方向から見た場合の目標物Ｐの向き（回転角度）を指す。また、目標位置Ａ３は、目標物Ｐに対して所定の位置及び姿勢となる位置及び姿勢である。本実施形態では、目標位置Ａ３は、移動体１０が目標物Ｐをピックアップ可能な位置及び姿勢といえる。例えば、目標位置Ａ３は、移動体１０が横方向に移動することなく、直進することで、後述の移動体１０のフォーク２４を、目標物Ｐの開口ＰＢに挿入することができる移動体１０の位置及び姿勢であってよい。この場合、移動体１０は、目標位置Ａ３から直進して目標物Ｐをピックアップして、目標物Ｐを他の場所に搬送する。なお、目標物Ｐの位置及び姿勢の具体的な特定方法や、第１パスＲ１、第２パスＲ２に従った移動の詳細については、後述する。また、図１に示す第１パスＲ１や第２パスＲ２は一例である。

（移動体）
図２は、移動体の構成の模式図である。図２に示すように、移動体１０は、車体２０と、マスト２２と、フォーク２４と、センサ２６と、制御装置２８とを備えている。車体２０は、車輪２０Ａを備えている。マスト２２は、車体２０の前後方向における一方の端部に設けられている。マスト２２は、前後方向に直交する上下方向（ここでは方向Ｚ）に沿って延在する。フォーク２４は、マスト２２に方向Ｚに移動可能に取付けられている。フォーク２４は、マスト２２に対して、車体２０の横方向（上下方向及び前後方向に交差する方向）にも移動可能であってよい。フォーク２４は、一対のツメ２４Ａ、２４Ｂを有している。ツメ２４Ａ、２４Ｂは、マスト２２から車体２０の前方向に向けて延在している。ツメ２４Ａとツメ２４Ｂとは、マスト２２の横方向に、互いに離れて配置されている。以下、前後方向のうち、移動体１０においてフォーク２４が設けられている側の方向を、前方向とし、フォーク２４が設けられていない側の方向を、後方向とする。

センサ２６は、車体２０の周辺に存在する対象物の位置及び姿勢の少なくとも１つを検出する。センサ２６は、移動体１０に対する対象物の位置と、移動体１０に対する対象物の姿勢とを検出するともいえる。本実施形態では、センサ２６は、マスト２２と、車体２０の四隅とに、すなわち車体２０の前方向側の左右の端部と後方向側の左右の端部とに、設けられている。ただし、センサ２６の設けられる位置はこれに限られず、任意の位置に設けられてもよいし、設けられる数も任意であってよい。例えば、移動体１０に設けられる安全センサを、センサ２６として流用してもよい。安全センサを流用することで、新たにセンサを設ける必要がなくなる。

センサ２６は、周囲の対象物からの反射光を検出（受光）することで、対象物の位置及び姿勢を検出する。さらに言えば、センサ２６は、光を照射するセンサであり、より詳しくは光としてレーザ光を照射する。センサ２６は、照射したレーザ光の反射光を検出することで、対象物の位置及び姿勢を検出する。センサ２６は、一方向に走査しつつレーザ光を照射し、照射したレーザ光の反射光から、対象物の位置及び姿勢を検出する。すなわち、センサ２６は、いわゆる２Ｄ－ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）であるともいえる。本実施形態では、センサ２６は、水平方向に、すなわち方向Ｚに直交する方向に、レーザ光を走査する。ただし、センサ２６は、以上のものに限られず任意の方法で対象物を検出するセンサであってよく、例えば、複数の方向に走査されるいわゆる３Ｄ－ＬｉＤＡＲであってもよいし、カメラであってもよい。

制御装置２８は、移動体１０の移動を制御する。制御装置２８については後述する。

（管理システム）
図３は、管理システムの模式的なブロック図である。管理システム１２は、設備Ｗにおける物流を管理するシステムである。管理システム１２は、本実施形態ではＷＭＳ（Ｗａｒｅｈｏｕｓｅ Ｍａｎａｇｅｍｅｎｔ Ｓｙｓｔｅｍ）であるが、ＷＭＳに限られず任意のシステムであってよく、例えば、その他の生産管理系システムのようなバックエンドシステムでも構わない。管理システム１２が設けられる位置は任意であり、設備Ｗ内に設けられてもよいし、設備Ｗから離れた位置に設けられて、離れた位置から設備Ｗを管理するものであってもよい。管理システム１２は、コンピュータであり、図３に示すように、通信部３０と記憶部３２と制御部３４とを含む。

制御部３４は、演算装置、すなわちＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）である。制御部３４は、作業決定部３６を含む。制御部３４は、記憶部３２からプログラム（ソフトウェア）を読み出して実行することで、作業決定部３６を実現して、その処理を実行する。なお、制御部３４は、１つのＣＰＵによって処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、作業決定部３６を、ハードウェア回路で実現してもよい。また、記憶部３２が保存する制御部３４用のプログラムは、管理システム１２が読み取り可能な記録媒体に記憶されていてもよい。

作業決定部３６は、搬送する対象となる目標物Ｐを決定する。具体的には、作業決定部３６は、例えば入力された作業計画に基づき、搬送する対象となる目標物Ｐの情報を示す作業内容を決定する。作業内容は、搬送する対象となる目標物Ｐを特定する情報であるともいえる。本実施形態の例では、作業内容は、どの設備にあるどの目標物Ｐを、いつまでに、どこに搬送するかを、作業内容として決定する。すなわち、作業決定部３６は、対象となる目標物Ｐが保管されている設備と、対象となる目標物Ｐと、目標物Ｐの搬送先と、目標物Ｐの搬送時期とを、を示す情報である。作業決定部３６は、決定した作業内容を、通信部３０を介して、情報処理装置１４に送信する。

（情報処理装置）
図４は、情報処理装置の模式的なブロック図である。情報処理装置１４は、設備Ｗに設けられ、少なくとも、移動体１０の移動に関する情報などを移動体１０と送受信する装置、いわゆる地上システムである。情報処理装置１４は、コンピュータであり、図４に示すように、通信部４０と記憶部４２と制御部４４とを含む。通信部４０は、制御部４４に用いられて、管理システム１２や移動体１０などの外部の装置と通信するモジュールであり、例えばアンテナなどを含んでよい。通信部４０による通信方式は、本実施形態では無線通信であるが、通信方式は任意であってよい。記憶部４２は、制御部４４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭと、ＲＯＭのような主記憶装置と、ＨＤＤなどの外部記憶装置とのうち、少なくとも１つ含む。

制御部４４は、演算装置、すなわちＣＰＵである。制御部４４は、作業内容取得部５０と、移動体選定部５２と、第１パス取得部５４とを含む。制御部４４は、記憶部４２からプログラム（ソフトウェア）を読み出して実行することで、作業内容取得部５０と移動体選定部５２と第１パス取得部５４とを実現して、それらの処理を実行する。なお、制御部４４は、１つのＣＰＵによってこれらの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、作業内容取得部５０と移動体選定部５２と第１パス取得部５４との少なくとも一部を、ハードウェア回路で実現してもよい。また、記憶部４２が保存する制御部４４用のプログラムは、情報処理装置１４が読み取り可能な記録媒体に記憶されていてもよい。

（作業内容取得部及び移動体選定部）
作業内容取得部５０は、管理システム１２が決定した作業内容の情報、すなわち搬送対象となる目標物Ｐの情報を取得する。作業内容取得部５０は、作業内容における目標物Ｐの情報から、目標物Ｐが設置されている設置領域ＡＲ０を特定する。例えば、記憶部４２には、目標物Ｐと、その目標物Ｐが設置されている設置領域ＡＲ０とが、関連付けて記憶されており、作業内容取得部５０は、記憶部４２からその情報を読み出すことで、設置領域ＡＲ０を特定する。移動体選定部５２は、対象となる移動体１０を選定する。移動体選定部５２は、例えば、設備Ｗに所属する複数の移動体から、対象となる移動体１０を選定する。移動体選定部５２は、任意の方法で対象となる移動体１０を選定してよいが、例えば、作業内容取得部５０が特定した設置領域ＡＲ０に基づき、その設置領域ＡＲ０にある目標物Ｐの搬送に適した移動体１０を、対象となる移動体１０として選定してよい。

（第１パス取得部）
第１パス取得部５４は、作業内容取得部５０が特定した設置領域ＡＲ０までの第１パスＲ１の情報を、取得する。第１パスＲ１は、例えば設置領域ＡＲ０毎に、予め設定されている。第１パス取得部５４は、例えば記憶部４２から、作業内容取得部５０が特定した設置領域ＡＲ０に対して設定された第１パスＲ１を、取得する。以下、第１パスＲ１について具体的に説明する。

図１に示すように、第１パスＲ１は、移動体１０が向かう設置領域ＡＲ０（目標物Ｐ）よりも方向Ｘ側で、方向Ｙに沿って設置領域ＡＲ０（目標物Ｐ）を横切る軌道となっている。より詳しくは、第１パスＲ１は、検出軌道Ｒ１ａと、検出軌道Ｒ１ａに接続されるアプローチ軌道Ｒ１ｂとを含む。

図１に示すように、検出軌道Ｒ１ａは、設置領域ＡＲ０（目標物Ｐ）よりも方向Ｘ側で、方向Ｙに沿って設置領域ＡＲ０（目標物Ｐ）を横切る軌道である。検出軌道Ｒ１ａは、設置領域ＡＲ０までの方向Ｘにおける距離が、所定距離の範囲内となるように設定されることが好ましい。ここでの所定距離は、検出軌道Ｒ１ａを移動中の移動体１０のセンサ２６によって、設置領域ＡＲ０内の目標物Ｐの位置及び姿勢が検出可能な距離である。より詳しくは、本実施形態においては、検出軌道Ｒ１ａは、第１位置Ａ１から第２位置Ａ２までの軌道である。第１位置Ａ１は、設置領域ＡＲ０よりも方向Ｘ側であり、かつ、設置領域ＡＲ０よりも方向Ｙと反対方向側の位置として設定されている。第２位置Ａ２は、設置領域ＡＲ０よりも方向Ｘ側であり、かつ、設置領域ＡＲ０よりも方向Ｙ側の位置として設定されている。本実施形態においては、第１位置Ａ１と第２位置Ａ２は、設置領域ＡＲ０までの方向Ｘにおける距離が、所定距離の範囲内となるように設定されており、第１位置Ａ１と第２位置Ａ２との方向Ｘにおける位置（Ｘ座標）は、一致している。検出軌道Ｒ１ａは、第１位置Ａ１から第２位置Ａ２までの、方向Ｙに沿った直線軌道として設定される。ただし、第１位置Ａ１と第２位置Ａ２との、方向Ｘにおける位置（Ｘ座標）は、一致していなくてもよい。また、検出軌道Ｒ１ａは、直線軌道であることに限られず、第１位置Ａ１から第２位置Ａ２までで任意の軌跡を描く軌道であってよい。

図１に示すように、アプローチ軌道Ｒ１ｂは、第２位置Ａ２から設置領域ＡＲ０に向かう軌道である。より詳しくは、アプローチ軌道Ｒ１ｂは、第２位置Ａ２から、設定位置Ａ３ｚまでの軌道である。設定位置Ａ３ｚは、設置領域ＡＲ０内における目標物Ｐの位置及び姿勢が所定の状態を満たす（目標物Ｐが設置領域ＡＲ０に対してずれ無く理想的に配置されている）と仮定した場合に、目標物Ｐに対して所定の位置及び姿勢となる位置及び姿勢である。すなわち、設定位置Ａ３ｚは、目標物Ｐの位置及び姿勢が所定の状態を満たすと仮定した場合に、移動体１０が目標物Ｐをピックアップ可能な位置及び姿勢であり、目標物Ｐの位置及び姿勢が所定の状態を満たす場合の目標位置Ａ３であるともいえる。図１の例では、アプローチ軌道Ｒ１ｂは、第２位置Ａ２から、第２位置Ａ２よりも方向Ｙと反対側の中間位置ＡＳＢ０までの直線軌道と、中間位置ＡＳＢ０から設定位置Ａ３ｚまでの曲線軌道と、を含む。第２位置Ａ２から中間位置ＡＳＢ０までの直線軌道は、検出軌道Ｒ１ａに重なることが好ましい。

なお、図示は省略するが、第１パスＲ１は、移動体１０の移動開始位置から第１位置Ａ１までの軌道も含んでよい。

ただし、第１パスＲ１は、以上のような軌道であることに限られない。例えば、第１パスＲ１は、アプローチ軌道Ｒ１ｂを含まないものであってよい。すなわち、第１パスＲ１は、少なくとも、第１位置Ａ１から第２位置Ａ２までの検出軌道Ｒ１ａを含むものであってよい。さらにいえば、第１パスＲ１は、検出軌道Ｒ１ａのような設置領域ＡＲ０（目標物Ｐ）を横切る軌道を含むことにも限られず、例えば、設置領域ＡＲ０（目標物Ｐ）の前面に向けて直線的に近づいていく軌道などであってもよい。またさらにいえば、第１パスＲ１は、移動体１０が後方向に直進して設置領域ＡＲ０（目標物Ｐ）を横切り第２位置Ａ２で切り返すことに限られず、移動体１０が前方向に直進して旋回することで設置領域ＡＲ０（目標物Ｐ）の前面に向けて近づいていく軌道などであってもよい。

第１パスＲ１は、設備Ｗの地図情報に基づき予め設定される。設備Ｗの地図情報は、設備Ｗに設置されている障害物（柱など）や移動体１０が走行可能な通路などの位置情報を含んだ情報であり、領域ＡＲ内で移動体１０が移動可能な領域を示す情報といえる。また、第１パスＲ１は、設備Ｗの地図情報に加えて、移動体１０の車両仕様の情報にも基づき、設定されてよい。車両仕様の情報とは、例えば、移動体１０の大きさや最小旋回半径など、移動体１０が移動可能な経路に影響を及ぼす仕様である。車両仕様の情報にも基づき第１パスＲ１が設定されている場合、第１パスＲ１は、移動体毎に設定されてよい。なお、第１パスＲ１は、人によって、地図情報や車両仕様の情報などに基づき設定されてもよいし、情報処理装置１４などの装置によって、地図情報や車両仕様の情報などに基づき、自動的に設定されてもよい。自動的に第１パスＲ１を設定する場合、例えば通過して欲しいポイント（Ｗａｙｐｏｉｎｔ）を指定してもよく、この場合、通過して欲しいポイントを通過しつつ、最短、かつ障害物（壁などの固定物）を避けた第１パスＲ１の設定が可能となる。

なお、第１パス取得部５４は、予め設定された第１パスＲ１を読み出すことなく、演算によって第１パスＲ１を設定してもよい。この場合、第１パス取得部５４は、対象となる移動体１０の位置情報と、設置領域ＡＲ０の位置情報と、設備Ｗの地図情報とに基づき、移動体１０の現在位置から、第１位置Ａ１、第２位置Ａ２を経由して、移動先である設定位置Ａ３ｚまでの経路を、第１パスＲ１として生成してよい。

情報処理装置１４は、取得した第１パスＲ１の情報を、通信部４０を介して、対象となる移動体１０に送信する。第１パスＲ１は、設置領域ＡＲ０に向かう経路であるため、移動体１０の移動に関する情報であるといえる。

（移動体の制御装置）
次に、移動体１０の制御装置２８について説明する。図５は、移動体の制御装置の模式的なブロック図である。制御装置２８は、移動体１０を制御する。制御装置２８は、移動体１０のセンサ２６による複数回の検出結果に基づいて設定された第２パスＲ２に沿って、移動体１０を目標位置Ａ３まで移動させて、移動体１０に目標物Ｐをピックアップさせる。制御装置２８は、コンピュータであり、図５に示すように、通信部６０と記憶部６２と制御部６４とを含む。通信部６０は、制御部６４に用いられて、情報処理装置１４などの外部の装置と通信するモジュールであり、例えばアンテナなどを含んでよい。通信部４０による通信方式は、本実施形態では無線通信であるが、通信方式は任意であってよい。記憶部６２は、制御部６４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭと、ＲＯＭのような主記憶装置と、ＨＤＤなどの外部記憶装置とのうち、少なくとも１つ含む。

制御部６４は、演算装置、すなわちＣＰＵである。制御部６４は、第１パス情報取得部７０と、移動制御部７２と、検出制御部７４と、目標物情報取得部７６と、信頼度情報取得部７８と、選択情報取得部８０と、第２パス情報取得部８２とを含む。制御部６４は、記憶部６２からプログラム（ソフトウェア）を読み出して実行することで、第１パス情報取得部７０と移動制御部７２と検出制御部７４と目標物情報取得部７６と信頼度情報取得部７８と選択情報取得部８０と第２パス情報取得部８２とを実現して、それらの処理を実行する。なお、制御部６４は、１つのＣＰＵによってこれらの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、第１パス情報取得部７０と移動制御部７２と検出制御部７４と目標物情報取得部７６と信頼度情報取得部７８と選択情報取得部８０と第２パス情報取得部８２との少なくとも一部を、ハードウェア回路で実現してもよい。また、記憶部６２が保存する制御部６４用のプログラムは、制御装置２８が読み取り可能な記録媒体に記憶されていてもよい。

（第１パス情報取得部）
第１パス情報取得部７０は、第１パスＲ１の情報を取得する。第１パス情報取得部７０は、その移動体１０が作業対象として選定された際に、情報処理装置１４から第１パスＲ１の情報を取得してもよいし、予め記憶部６２に記憶された第１パスＲ１の情報を読み出してもよい。

（移動制御部）
移動制御部７２は、移動体１０の駆動部やステアリングなどの移動機構を制御して、移動体１０の移動を制御する。移動制御部７２は、第１パス情報取得部７０が取得した第１パスＲ１や、後段の第２パス情報取得部８２が取得した第２パスＲ２に従って、移動体１０を移動させる。移動制御部７２は、移動体１０の位置情報を逐次把握することで、第１パスＲ１や第２パスＲ２を通るように、移動体１０を移動させる。移動体１０の位置情報の取得方法は任意であるが、例えば本実施形態では、設備Ｗに図示しない検出体が設けられており、移動制御部７２は、検出体の検出に基づき移動体１０の位置及び姿勢の情報を取得する。具体的には、移動体１０は、検出体に向けてレーザ光を照射し、検出体によるレーザ光の反射光を受光して、設備Ｗにおける自身の位置及び姿勢を検出する。ここでの移動体１０の位置とは、設備Ｗの領域Ａにおける方向Ｘ及び方向Ｙの二次元座標系ＣＯにおける座標であり、以下においても、位置とは、別途説明が無い限り、二次元座標系ＣＯにおける座標を指す。また、移動体１０の姿勢とは、方向Ｘ及び方向Ｙに直交する方向Ｚから見た場合の移動体１０の向き(回転角度)である。また、移動体１０の位置及び姿勢の情報の取得方法は、検出体を用いることに限られず、例えば、ＳＬＡＭ（Ｓｌｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）などの自己位置推定技術を用いてもよい。

（検出制御部）
検出制御部７４は、センサ２６に目標物Ｐの前面Ｐａを検出させて、センサ２６の検出結果を点群として取得する。検出制御部７４の具体的な処理内容は後述する。

（目標物位置情報取得部）
目標物情報取得部７６は、点群の位置に基づいて推定された、目標物Ｐの位置及び姿勢の推定結果を取得する。目標物情報取得部７６の具体的な処理は後述する。

（信頼度情報取得部）
信頼度情報取得部７８は、目標物Ｐの位置及び姿勢の推定結果の精度に対する信頼度の情報を、取得する。信頼度情報取得部７８の具体的な処理は後述する。

（選択情報取得部）
選択情報取得部８０は、目標物Ｐの位置及び姿勢の複数の推定結果のうちから、信頼度に基づいて選択された推定結果の情報を取得する。選択情報取得部８０の具体的な処理は後述する。

（第２パス情報取得部）
第２パス情報取得部８２は、選択された推定結果に基づいて設定された、目標位置Ａ３までの第２パスＲ２の情報を取得する。第２パス情報取得部８２の具体的な処理は後述する。

（制御装置の処理）
次に、移動体１０が目標物Ｐに向かう際の制御装置２８の処理について説明する。

（第１パスに沿った移動）
制御装置２８の移動制御部７２は、第１パス情報取得部７０が取得した第１パスＲ１に従って、移動体１０を移動させる。移動制御部７２は、現在の移動体１０の位置から、第１位置Ａ１を経由して第２位置Ａ２まで、第１パスＲ１を通るように、移動体１０を移動させる。

（点群の取得）
図６は、目標物の検出を説明する模式図である。検出制御部７４は、図６に示すように、移動体１０が、第１パスＲ１に沿って第１位置Ａ１から第２位置Ａ２までの間を移動中に、すなわち検出軌道Ｒ１ａを移動中に、センサ２６に、目標物Ｐの前面Ｐａを複数回検出させる。検出制御部７４は、センサ２６に、位置が既知である設置領域ＡＲ０に向けて検出を行わせることで、目標物Ｐの前面Ｐａを検出させる。具体的には、検出制御部７４は、センサ２６に、目標物Ｐが設置された設置領域ＡＲに向けて、レーザ光ＬＴを照射させる。レーザ光ＬＴは目標物Ｐの前面Ｐａで反射され、センサ２６は、目標物Ｐの前面Ｐａからの反射光を受光する。検出制御部７４は、センサ２６が受光した反射光の検出結果に基づき、計測点Ｍの集合である点群Ｍ０を取得する。計測点Ｍとは、レーザ光ＬＴが反射された位置（座標）を示す点であり、点群Ｍ０とは、レーザ光ＬＴが反射された位置を示す点の集合を指す。本実施形態では、検出制御部７４は、反射光の検出結果と移動体１０の位置に基づき、反射光が反射された箇所の、方向Ｘ及び方向Ｙの二次元座標系ＣＯにおける位置（座標）を、計測点Ｍとして算出する。ただし、検出制御部７４は、二次元座標系ＣＯにおける位置を計測点Ｍとすることに限られず、センサ２６や移動体１０を基準とした座標系における位置を、計測点Ｍとしてもよい。

（目標物の位置及び姿勢の特定）
目標物情報取得部７６は、センサ２６による１回の検出（レーザ光ＬＴの１回のスキャン）により取得された点群Ｍ０に含まれる計測点Ｍに基づき、目標物Ｐの位置及び姿勢を推定する。本実施形態では、センサ２６による１回の検出により取得された計測点Ｍに基づき、それらの計測点Ｍの近似線Ｌを検出することで、目標物Ｐの位置及び姿勢を推定する。近似線Ｌは、目標物Ｐの前面Ｐａに対応するものであり、すなわち例えば、二次元座標系ＣＯにおける近似線Ｌの傾きが目標物Ｐの姿勢に対応し、二次元座標系ＣＯにおける近似線Ｌの位置が目標物Ｐの位置に対応する。以下、近似線Ｌの検出処理の例を具体的に説明する。

(近似線候補の取得)
図７は、近似線候補の取得を説明するための模式図である。図７に示すように、目標物情報取得部７６は、設置領域ＡＲ０の位置に基づき、検知対象領域ＲＯＩａを設定する。検知対象領域ＲＯＩａは、いわゆるＲＯＩ（Ｒｅｇｉｏｎｓ Ｏｆ Ｉｎｔｅｒｅｓｔ）である。目標物情報取得部７６は、設置領域ＡＲ０の位置に基づき、目標物Ｐの前面Ｐａを含むと推定される領域を特定し、その領域を検知対象領域ＲＯＩａとする。目標物情報取得部７６は、例えば、設置領域ＡＲ０より幅が大きい検知対象領域ＲＯＩａを設定し、センサ２６による１回の検出により取得した点群Ｍ０に含まれる計測点Ｍのうちから、検知対象領域ＲＯＩａ内の計測点Ｍを抽出する。目標物情報取得部７６は、目標物Ｐの前面Ｐａに対応する近似線Ｌ０を検出するために、一以上の近似線候補を取得する。目標物情報取得部７６は、一以上の近似線候補のそれぞれについて、近似線候補から規定距離以上の手前側の計測点Ｍについては他の計測点Ｍよりも選択における優先度が低い定数のスコアを付与する。目標物情報取得部７６は、検知対象領域ＲＯＩａ内の計測点Ｍについてスコアを積算したスコア積算値に基づいて、一以上の近似線候補の中から、目標物Ｐの前面Ｐａに対応する近似線Ｌ０を選択する。なお、本実施形態では、目標物Ｐの前面Ｐａが平面状であるため、近似線候補、近似線Ｌ０、及び近似線Ｌを直線として検出するが、それらは直線であることに限られない。例えば前面Ｐａが曲面であることが既知である場合などには、それに合わせて、近似線候補、近似線Ｌ０、及び近似線ＬＮを曲線として算出してもよい。

図８は、近似線候補の具体例を示す模式図である。例えば、図８に示すように、目標物情報取得部７６は、検知対象領域ＲＯＩａより幅が狭い検知対象領域ＲＯＩｂをさらに設定し、検知対象領域ＲＯＩｂの中の点群Ｍ０から選択した２つの計測点Ｍを結ぶ直線を、近似線候補として取得してもよい。図８では、目標物情報取得部７６が、検知対象領域ＲＯＩｂの中の点群Ｍ０に基づいて４つの近似線候補Ｌ１、Ｌ２、Ｌ３、Ｌ４を取得する例を示している。検知対象領域ＲＯＩｂは、隣接する荷物などが入り込まない程度の幅に設定されることが好ましい。検知対象領域ＲＯＩｂは、設置領域ＡＲ０より幅が大きくてもよいし、小さくてもよい。

ここで、近似線候補の取得方法について具体例を説明する。目標物情報取得部７６は、検知対象領域内の点群Ｍ０からＲＡＮＳＡＣアルゴリズムにより選択した２つの計測点Ｍを結ぶ直線を、近似線候補として取得するように構成されてもよい。この場合の検知対象領域は、検知対象領域ＲＯＩｂであるが、検知対象領域ＲＯＩａであってもよいし、検知対象領域ＲＯＩａ、ＲＯＩｂとは異なる領域であってもよい。ＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）は、ランダムに２つの点を選択して評価値を算出する手法である。なお、本開示に係る近似線候補の取得手法には、ＲＡＮＳＡＣを用いた手法に限られず、ＰＲＯＳＡＣ（ＰＲＯｇｒｅｓｓｉｖｅ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）等の他のアルゴリズムが使用されてもよい。

目標物情報取得部７６は、検知対象領域の幅方向中央の右側及び左側のそれぞれの点群Ｍ０から１つずつ選択した２つの計測点Ｍを結ぶ直線を、近似線候補として取得するように構成されてもよい。この場合、組み合わせの数が有限となるため、すべての２点の組み合わせについて近似線候補としてもよい。

(近似線Ｌ０の検出)
目標物情報取得部７６は、一以上の近似線候補のそれぞれについてスコア積算値を算出し、スコア積算値に基づいて、目標物Ｐの前面Ｐａに対応する近似線Ｌ０を検出する。目標物情報取得部７６は、スコア積算値が示す優先度が高い近似線候補に対して、検知対象領域ＲＯＩｂ外かつ検知対象領域ＲＯＩａ内の点群Ｍ０を含めて、それを近似線Ｌ０として選択（すなわち検出）し、この様な点群Ｍ０についても後続の処理の対象としてもよい。以降では、一例として、目標物情報取得部７６が、スコア積算値に基づいて、図８に示す近似線候補Ｌ１を近似線Ｌ０として選択するものとする。

ここで、スコア積算値について説明する。例えば、スコア積算値の高い近似線候補を優先的に線分検出に使用する直線として選択する場合には、「優先度が低い定数のスコア」は、スコア積算値を小さくするような値のスコアを意味する。一方、例えば、スコア積算値の低い近似線候補を優先的に選択する場合には、「優先度が低い定数のスコア」は、スコア積算値を大きくするような値のスコアを意味する。

目標物情報取得部７６は、スコア積算値の算出において、点群Ｍ０を構成する各計測点Ｍと近似線候補との垂直距離を利用する。具体的には、目標物情報取得部７６は、近似線候補から規定距離未満の計測点Ｍについては、近似線候補から規定距離以上の手前側（検出時の移動体１０の位置に近い側）の計測点Ｍよりも優先度が高く、かつ近似線候補に近いほど優先度が高いスコアを付与する。例えば、目標物情報取得部７６は、規定距離以上の手前側の計測点Ｍはマイナスの定数のスコアを付与するのに対し、規定距離未満の計測点Ｍはプラスのスコアを付与する。プラスのスコアの値は、近似線候補からの距離に依存する変数である。

図９は、スコアの一例を示すグラフである。このグラフの横軸は、Ｙ方向（深度）に対応し、計測点Ｍと近似線候補との距離を示し、横軸方向の右に行くほど奥側（検出時の移動体１０の位置から遠い側）であることを意味する。深度がゼロの位置は近似線候補の位置である。縦軸は付与するスコアの値を示している。図９に示す例では、近似線候補から規定距離以上の手前側の計測点Ｍにはマイナスの定数のスコアが付与される。このように、近似線候補から規定距離以上の手前側の計測点Ｍには、近似線候補と計測点Ｍとの距離によらず一定のペナルティを課している。近似線候補から規定距離未満の計測点Ｍには、近似線候補に近いほど大きいプラスのスコアが付与される。

目標物情報取得部７６は、スコア積算値の算出において、近似線候補から規定距離以上の奥側の計測点Ｍについては、スコア積算値の積算対象から除外する。「スコア積算値の積算対象から除外する」とは、それらの計測点Ｍをスコア積算値の積算対象から実質的に除外することを意味し、それらの計測点Ｍにゼロのスコアを付与することであってもよいし、それらの計測点Ｍにスコアを付与しないことであってもよい。図９に示す例では、近似線候補から規定距離以上の奥側の計測点Ｍにゼロのスコアが付与される。この様なスコア積算値の算出方法を採用すると、図８に示す例では、近似線候補Ｌ１が近似線Ｌ０として選択される。なお、目標物情報取得部７６は、スコア積算値が示す優先度が高い複数の近似線を検出してもよい。

(両端点の特定)
目標物情報取得部７６は、目標物Ｐの幅方向における両端点の位置を特定し、近似線Ｌ０を両端点で区切った線分を、近似線Ｌとして検出する。具体的には、目標物情報取得部７６は、近似線Ｌ０から一以上の線分を抽出し、抽出した一以上の線分の中から設計情報から定まる目標物Ｐの両端の長さに対応する両端長さを有する一つの線分又は二以上の線分の組み合わせを探索する。「対応する長さ」とは、許容誤差の範囲内の長さを意味する。目標物Ｐの設計情報は、目標物Ｐの寸法（例えば、両端の長さ、開口ＰＢの位置及び間隔等）を示す情報であり、例えば、制御装置２８の記憶部６２から読み出される。目標物情報取得部７６は、見つかった線分又は二以上の線分の組み合わせの両端を、目標物Ｐの幅方向における両端点として特定する。なお、目標物Ｐの両端点とは、目標物Ｐの前面Ｐａの幅方向における一方側の端点と他方側の端点とを指す。

図１０は、目標物の両端点の特定処理の一例を説明する模式図である。図１０に示すように、例えば、目標物情報取得部７６は、選択した近似線Ｌ０から３つの線分（丸で囲まれた計測点Ｍ）を抽出し、目標物Ｐの両端点間の長さに対応する両端長さＷＤを有する一つの線分又は二以上の線分の組み合わせを探索する。図１０に示す例では、探索の結果、３つの線分の組み合わせの両端長さが検出されている。

（近似線Ｌの検出）
目標物情報取得部７６は、目標物Ｐの両端点の位置を特定したら、各計測点Ｍに対して最小二乗法によって直線フィッティングを行うことで、近似線Ｌを算出する。具体的には、目標物情報取得部７６は、目標物Ｐの両端が検知された線分に基づいて近似線Ｌを算出する。目標物情報取得部７６は、各計測点Ｍ（センサ２６による１回の検出により取得された計測点Ｍ）のうち、目標物Ｐの開口ＰＢの位置と推定される位置の計測点Ｍを除外して、さらに線分の両端から所定距離以上の位置にある計測点Ｍを除外して、残りの計測点Ｍのみを使用して、最小二乗法によって直線フィッティングを行うことで、近似線Ｌを算出する。目標物Ｐの開口ＰＢの位置の推定は、検出された目標物Ｐの両端点と目標物Ｐの設計情報とに基づいて行われる。

（近似線Ｌに基づく目標物の位置及び姿勢の推定）
目標物情報取得部７６は、近似線Ｌに基づき、目標物Ｐの位置及び姿勢を推定する。目標物情報取得部７６は、近似線Ｌの傾きから目標物Ｐの姿勢を推定し、近似線Ｌ上の基準位置（例えば近似線Ｌの中点）に基づいて目標物Ｐの位置を推定する。

なお、近似線Ｌの検出方法は以上説明した方法に限られず、取得した計測点Ｍに基づき任意の方法で近似線Ｌを検出してよい。例えば、目標物情報取得部７６は、両端点の特定処理などを行うことなく、１回の検出で取得された全ての計測点Ｍに対して最小二乗法によって直線フィッティングを行うことで、近似線Ｌを算出してもよい。また、目標物Ｐの位置及び姿勢の推定方法も、近似線Ｌを用いた方法に限られず、取得した計測点Ｍを用いた任意の方法で目標物Ｐの位置及び姿勢を推定してよい。

（複数回の検出）
検出制御部７４は、移動体１０が第１パスＲ１（検出軌道Ｒ１ａ）を移動中に、センサ２６に、目標物Ｐの前面Ｐａの検出を、複数回行わせる。言い換えれば、検出制御部７４は、移動体１０が第１パスＲ１（検出軌道Ｒ１ａ）を移動中に、センサ２６に、目標物Ｐの前面Ｐａへのレーザ光ＬＴのスキャンを、複数回行わせる。検出制御部７４は、センサ２６の検出毎の検出結果を、点群Ｍ０として取得する。目標物情報取得部７６は、センサ２６の検出結果毎に（センサ２６が検出する度に）、目標物Ｐの位置及び姿勢を推定する。すなわち、目標物情報取得部７６は、センサ２６の１回の検出毎に、目標物Ｐの位置及び姿勢の推定結果を取得する。

このように、本実施形態では、第１パスＲ１を移動中に、センサ２６に複数回の検出を行わせるものである。すなわち、検出制御部７４は、第１パスＲ１上での移動体１０の位置毎にセンサ２６に検出を行わせるため、各検出結果が取得された際の移動体１０の位置は、互いに異なる。ただし、センサ２６に検出させるタイミングは、第１パスＲ１を移動中であることに限られない。例えば、移動体１０を停止させた状態でセンサ２６に検出を行わせてもよい。また、検出制御部７４は、１つのセンサ２６に複数回の検出を行わせることに限られず、複数のセンサ２６に同じ目標物Ｐを検出させることで、センサ２６に複数回の検出を行わせてもよい。

（信頼度の算出）
信頼度情報取得部７８は、目標物情報取得部７６が推定した目標物Ｐの位置及び姿勢の推定結果についての、推定精度に対する信頼度を算出する。すなわち、信頼度とは、目標物Ｐの位置及び姿勢の推定精度の度合いを示す指標であり、本実施形態では、信頼度が高いほど、目標物Ｐの位置及び姿勢の推定精度が高いといえる。信頼度情報取得部７８は、推定結果毎に、すなわちセンサ２６による１回の検出（１回のスキャン）毎に、信頼度を算出する。以下、信頼度の算出方法を説明する。

（理想の計測点の数の算出）
図１１は、信頼度の算出方法を説明するための模式図である。信頼度情報取得部７８は、センサ２６による１回の検出で取得されると想定される理想の計測点Ｍの数（理想の点群数）と、センサ２６による１回の検出で取得された実際の計測点Ｍの数（目標物Ｐの前面Ｐａを計測した計測点Ｍ（点群）の数）と比率に基づき、そのセンサ２６の検出結果から推定された目標物Ｐの位置及び姿勢の推定結果の信頼度を算出する。理想の計測点Ｍの数とは、計測誤差等の影響を考慮する必要のない理想的な環境において、センサ２６による１回の検出で目標物Ｐの前面Ｐａで観測されると想定される計測点Ｍの個数である。信頼度情報取得部７８は、目標物Ｐの位置及び姿勢の推定結果と、検出を行った際の移動体１０の位置と、検出を行ったセンサ２６の性能とに基づき、理想の計測点Ｍの数を算出する。センサ２６の性能とは、例えば、センサ２６の角度分解能、センサ２６の測定スピード（周期）、及びセンサ２６の測定ポイント数との少なくとも１つを意味する。なお、角度分解能とは、２つの対象点を分離して検知できる角度を示している。また、ここでの移動体１０の位置は、移動体１０の基準となる位置でもよいし、検出したセンサ２６の位置であってもよい。移動体１０の位置を検出したセンサ２６の位置とする場合には、信頼度情報取得部７８は、検出を行った際の移動体１０の位置と、移動体１０に対するセンサ２６の位置とに基づき、移動体１０の位置を特定してよい。移動体１０に対するセンサ２６の位置は、例えば設計情報として予め取得されていてよい。

ここで、センサ２６が１回の検出でレーザ光ＬＴを走査する角度をスキャン角度とする。本実施形態では、信頼度情報取得部７８は、目標物Ｐの位置及び姿勢の推定結果と、検出を行った際の移動体１０の位置とに基づき、センサ２６に基準位置ＡＲ１から検出を行わせた場合に、スキャン角度のうちで、レーザ光ＬＴが目標物Ｐの前面Ｐａに照射される角度範囲を、検出角度θとして算出する。すなわち、信頼度情報取得部７８は、目標物Ｐの前面Ｐａ以外に照射されたレーザ光ＬＴが含まれる角度範囲を除外し、目標物Ｐの前面Ｐａに照射されたレーザ光ＬＴが含まれる角度範囲を抽出して、検出角度θとする。例えば、信頼度情報取得部７８は、目標物Ｐの位置及び姿勢の推定結果から特定された前面Ｐａ上の点と基準位置ＡＲ１とを結ぶ直線と、目標物Ｐの位置及び姿勢の推定結果から特定された前面Ｐａ上の他の点と基準位置ＡＲ１とを結ぶ直線とのなす角度に基づき、検出角度θを算出する。なお、基準位置ＡＲ１は、検出を行った際の移動体１０の位置に対応する位置であり、本実施形態では、検出を行った際の移動体１０の位置そのものである。

より詳しくは、信頼度情報取得部７８は、柱ＰＡ毎に検出角度θを算出する。図１１に示すように、信頼度情報取得部７８は、目標物Ｐの位置及び姿勢の推定結果（近似線Ｌ）に基づき、柱ＰＡの両端点の位置を取得し、基準位置ＡＲ１と柱ＰＡの一方の端点ＰＳ１とを結ぶ直線ＬＡ１と、基準位置ＡＲ１と柱ＰＡの他方の端点ＰＳ２とを結ぶ直線ＬＡ２との、なす角度を、その柱ＰＡについての検出角度θとして算出する。図１１の例では、信頼度情報取得部７８は、柱ＰＡ１の検出角度θ１と、柱ＰＡ２の検出角度θ２と、柱ＰＡ３の検出角度θ３とを、算出する。

信頼度情報取得部７８は、算出した検出角度θとセンサ２６の角度分解能との比率から、理想の計測点Ｍの数を算出する。より詳しくは、信頼度情報取得部７８は、検出角度θを角度分解能で除した値を、理想の計測点Ｍの数として算出する。本実施形態では、信頼度情報取得部７８は、柱ＰＡ毎に、理想の計測点Ｍの数を算出する。図１１の例では、信頼度情報取得部７８は、柱ＰＡ１についての検出角度θ１を角度分解能で除した値を、柱ＰＡ１における理想の計測点Ｍの数（理想計測点数ＮＭ１）として算出し、柱ＰＡ２についての検出角度θ２を角度分解能で除した値を、柱ＰＡ２における理想の計測点Ｍの数（理想計測点数ＮＭ２）として算出し、柱ＰＡ３についての検出角度θ３を角度分解能で除した値を、柱ＰＡ３における理想の計測点Ｍの数（理想計測点数ＮＭ３）として算出する。

（実際の計測点数の算出）
信頼度情報取得部７８は、実際の計測点Ｍの数を算出する。実際の計測点Ｍの数とは、センサ２６の１回の検出で取得された計測点Ｍのうちで、目標物Ｐの前面Ｐａを計測した計測点Ｍを指す。例えば、信頼度情報取得部７８は、センサ２６の１回の検出で取得された計測点Ｍのうちで、目標物Ｐの位置及び姿勢の推定結果から特定された目標物Ｐの前面Ｐａ（ここでは近似線Ｌ）に対して所定の距離範囲内にある計測点Ｍを、実際の計測点Ｍとして抽出して、その数をカウントする。また例えば、信頼度情報取得部７８は、近似線Ｌの検出に用いられた計測点Ｍを、実際の計測点Ｍとしてもよい。

より詳しくは、信頼度情報取得部７８は、柱ＰＡ毎に、実際の計測点Ｍの数を算出する。信頼度情報取得部７８は、センサ２６の１回の検出で取得された計測点Ｍのうちで、推定された柱ＰＡの位置（目標物Ｐの位置及び姿勢の推定結果から特定された柱ＰＡの位置）に対して所定の距離範囲内にある計測点Ｍを、その柱ＰＡを計測した実際の計測点Ｍとして抽出して、その数をカウントする。図１１の例では、信頼度情報取得部７８は、推定された柱ＰＡ１の位置の手前側の所定面積の領域ＡＲＸ１内にある計測点Ｍの数を、柱ＰＡ１を計測した実際の計測点Ｍの数（実測計測点数ｎｍ１）として算出する。同様に、信頼度情報取得部７８は、推定された柱ＰＡ２の位置の手前側の所定面積の領域ＡＲＸ２内にある計測点Ｍの数を、柱ＰＡ２を計測した実際の計測点Ｍの数（実測計測点数ｎｍ２）として算出する。同様に、信頼度情報取得部７８は、推定された柱ＰＡ３の位置の手前側の所定面積の領域ＡＲＸ３内にある計測点Ｍの数を、柱ＰＡ３を計測した実際の計測点Ｍの数（実測計測点数ｎｍ３）として算出する。

（理想の計測点数と実際の計測点数に基づく信頼度の算出）
信頼度情報取得部７８は、理想の計測点Ｍの数と実際の計測点Ｍの数との比較結果に基づき、目標物Ｐの位置及び姿勢の推定結果についての信頼度を算出する。本実施形態では、信頼度情報取得部７８は、理想の計測点Ｍの数に対する実際の計測点Ｍの数の比率に基づき、信頼度を算出する。信頼度情報取得部７８は、理想の計測点Ｍの数と実際の計測点Ｍの数との差分が少ないほど、信頼度が高くなるように、信頼度を算出する。

本実施形態では、信頼度情報取得部７８は、柱ＰＡ毎に、信頼度を算出する。信頼度情報取得部７８は、柱ＰＡにおける理想の計測点Ｍの数に対する、その柱ＰＡについての実際の計測点Ｍの数との比率を、その柱ＰＡについての推定結果の信頼度として算出する。そして、信頼度情報取得部７８は、柱ＰＡ毎の信頼度に基づいて、センサ２６による１回の検出により推定された目標物Ｐの位置及び姿勢の推定結果についての、信頼度を算出する。本実施形態では、信頼度情報取得部７８は、それぞれの柱ＰＡの信頼度のうちの最小値を、目標物Ｐの位置及び姿勢の推定結果についての信頼度とする。すなわち、図１１の例では、１回の検出による目標物Ｐの位置及び姿勢の推定結果の信頼度をＳとすると、信頼度Ｓは、次の式（１）を用いて算出される。

Ｓ＝ｍｉｎ（ｎｍ１／ＮＭ１，ｎｍ２／ＮＭ２，ｎｍ３／ＮＭ３） ・・・（１）

信頼度情報取得部７８は、目標物Ｐの位置及び姿勢の推定結果毎に、すなわちセンサ２６による１回の検出（１回のスキャン）毎に、同様の方法で、信頼度Ｓを算出する。

（信頼度の算出の他の例）
信頼度情報取得部７８は、以上の方法で信頼度を算出するが、信頼度の算出方法は以上の説明に限られず、任意の方法を用いてよい。例えば、以上の説明では、信頼度情報取得部７８は、柱ＰＡ毎に計測点Ｍの信頼度を算出し、柱ＰＡ毎の信頼度に基づき、１回の検出による推定結果の信頼度を算出したが、それに限られない。この場合例えば、信頼度情報取得部７８は、それぞれの柱ＰＡについての理想の計測点Ｍの数を上記と同様の方法で算出し、それらを合計することで、全ての柱ＰＡについての理想の測定点の数の合計値を算出する。そして、信頼度情報取得部７８は、それぞれの柱ＰＡについての実際の計測点Ｍの数を上記と同様の方法で算出し、それらを合計することで、実際の計測点Ｍの数の合計値を算出する。そして、信頼度情報取得部７８は、理想の計測点Ｍの数の合計値に対する、実際の計測点Ｍの数の合計値の比率を、１回の検出による推定結果の信頼度として算出してよい。

（推定結果の選択）
選択情報取得部８０は、信頼度情報取得部７８が算出した、センサ２６の検出毎の推定結果の信頼度に基づき、センサ２６の検出毎の推定結果のうちから、第２パスＲ２の設定に用いる推定結果を選択する。具体的には、選択情報取得部８０は、信頼度が高い推定結果を、第２パスＲ２の設定に用いる推定結果として選択する。すなわち例えば、センサ２６による第１の検出で取得された計測点Ｍを第１計測点（第１点群）とし、センサ２６による第１の検出とは異なる第２の検出で取得された計測点Ｍを第２計測点（第２点群）とする。この場合、目標物情報取得部７６は、第１測定点に基づいて目標物の位置及び姿勢を推定し、信頼度情報取得部７８は、その推定結果（第１測定点に基づいた推定結果）についての信頼度を算出する。同様に、目標物情報取得部７６は、第２測定点に基づいて推定された目標物の位置及び姿勢を推定し、信頼度情報取得部７８は、その推定結果（第２測定点に基づいた推定結果）についての信頼度を算出する。この場合、選択情報取得部８０は、第１計測点に基づいた推定結果と第２計測点に基づいた推定結果とのうちで、信頼度がより高い方の推定結果を、第２パスＲ２の設定に用いる推定結果として選択する。

さらに言えば、選択情報取得部８０は、それぞれの推定結果のうちから、信頼度が最高となる推定結果を、第２パスＲ２の設定に用いる推定結果として選択する。ただし、推定結果の選択方法は以上の方法に限られず、それぞれの推定結果の信頼度に基づいた任意の方法で、推定結果を選択してよい。

（第２パスの設定）
図１２は、第２パスに沿った移動の一例を示す模式図である。第２パス情報取得部８２は、選択情報取得部８０が選択した目標物Ｐの位置及び姿勢の推定結果から、目標位置Ａ３を設定する。例えば、第２パス情報取得部８２は、選択情報取得部８０が選択した目標物Ｐの位置及び姿勢の推定結果から、目標物Ｐをピックアップ可能な（直進することでフォーク２４を目標物Ｐの開口ＰＢに挿入することができる）位置及び姿勢を算出して、目標位置Ａ３として設定する。一例として、開口ＰＢの入口から、目標物Ｐの開口ＰＢの軸方向に１０００ｍｍ平行移動した箇所を、目標位置Ａ３としてもよい。そして、第２パス情報取得部８２は、図１２に示すように、開始位置である第２位置Ａ２から、設定した目標位置Ａ３までの軌道を、第２パスＲ２として設定する。第２パス情報取得部８２は、目標位置Ａ３の情報に基づいて任意の方法で第２パスＲ２を設定してよいが、例えば、モデル予測制御（ＭＰＣ:Model Predictive Control）によって、第２パスＲ２を算出してもよい。

（第２パスに沿った移動）
移動制御部７２は、第２位置Ａ２から目標位置Ａ３まで、第２パスＲ２を通るように、移動体１０を移動させる。移動体１０が目標位置Ａ３に到着したら、移動制御部７２は、目標位置Ａ３から移動体１０を直進させて、フォーク２４を目標物Ｐの開口ＰＢに挿入させて、目標物Ｐをピックアップさせる。移動制御部７２は、目標物Ｐをピックアップした移動体１０を、設定された搬送先まで搬送させる。

このように、移動制御部７２は、第２位置Ａ２から目標位置Ａ３まで、第２パスＲ２に沿って移動体１０を移動させるが、それに限られず、例えば、第２パスＲ２に沿った移動と直接フィードバック制御による移動とを切り替えて、移動体１０を目標位置Ａ３まで移動させてもよい。直接フィードバックによる制御としては、例えば「尾里淳，丸典明 著「線形ビジュアルサーボによる全方向移動ロボットの位置と姿勢の制御」、日本機械学会論文集（Ｃ編）、第７７巻、第７７４号、ｐ．２１５－２２４、２０１１年２月２５日」に記載されているような、ビジュアルサーボ方式による制御が挙げられる。また例えば、移動制御部７２は、移動体１０が第２パスＲ２に沿って目標位置Ａ３に到着したら、直接フィードバックによる制御に切り替えて、目標物Ｐをピックアップするよう、移動体１０を移動させてもよい。

（目標物の位置及び姿勢検出の処理フロー）
次に、以上説明した第２パスの設定についての処理フローを説明する。図１３は、第２パス設定の処理フローを説明するフローチャートである。制御装置２８の検出制御部７４は、移動体１０が第１パスＲ１に沿って第１位置Ａ１から第２位置Ａ２までの間を移動中に、センサ２６に目標物Ｐを複数回検出させる。すなわち本実施形態では、第１パスＲ１に沿って移動させつつ、センサ２６に目標物Ｐを検出させる。図１３に示すように、検出制御部７４は、センサ２６による１回の検出結果から、点群Ｍ０（計測点Ｍ）を取得する（ステップＳ１０）。そして、制御装置２８の目標物情報取得部７６は、点群Ｍ０に基づいて、目標物Ｐの位置及び姿勢を推定する（ステップＳ１２）。そして、制御装置２８の信頼度情報取得部７８は、目標物Ｐの位置及び姿勢の推定結果と、検出を行った際の移動体１０の位置と、検出を行ったセンサ２６の性能とに基づき、その検出における理想の計測点Ｍの数を算出し（ステップＳ１４）、理想の計測点Ｍの数と実際の計測点Ｍの数との比率に基づき、その検出における目標物Ｐの位置及び姿勢の推定結果の信頼度を算出する（ステップＳ１６）。

そして、制御装置２８は、算出された推定結果の数が閾値以上であるかを判断し、閾値以上であると判定しない場合には（ステップＳ１８；Ｎｏ）、すなわち推定結果の数が閾値未満と判定した場合には、ステップＳ１０に戻り、センサ２６による他の検出結果（他のスキャンによる検出結果）による点群Ｍ０を取得させることで、推定結果の算出と信頼度の算出を繰り返す。なお、ここでの閾値は任意に設定してよい。算出された推定結果の数が閾値以上である場合（ステップＳ１８；Ｙｅｓ）、制御装置２８の選択情報取得部８０は、算出されたそれぞれの推定結果のうちから、信頼度が最高となる推定結果を、第２パスＲ２の設定に用いる推定結果として選択する（ステップＳ２０）。そして、制御装置２８の第２パス情報取得部８２は、選択された推定結果を用いて、第２パスＲ２を設定する（ステップＳ２２）。

なお、本実施形態では、センサ２６が１回検出する度に目標物Ｐの位置及び姿勢の推定と信頼度の算出を行い、例えば移動体１０が第２位置Ａ２に到着して第２パスＲ２の設定を始める際に、信頼度に基づいて使用する推定結果を選択する。ただし各処理を行うタイミングはそれに限られず任意である。例えば、センサ２６が任意の複数回検出してから、目標物Ｐの位置及び姿勢の推定と信頼度の算出をまとめて行ってよく、さらに言えば、移動体１０が第２位置Ａ２に到着して第２パスＲ２の設定を始める際に、これまでの検出結果から、目標物Ｐの位置及び姿勢の推定、信頼度の算出、及び推定結果の選択を行ってよい。

（効果）
以上説明したように、本実施形態においては、制御装置２８は、センサ２６の検出毎に推定された目標物Ｐの位置及び姿勢についての、信頼度を算出する。そのため例えば、信頼度に基づいて第２パスＲ２の設定に用いる推定結果を選択して、目的位置まで適切にアプローチできる精度の高い第２パスＲ２を設定することが可能となる。そのため、本実施形態によると、移動体１０を目的位置まで適切に移動させることが可能となる。さらに言えば、制御装置２８は、推定結果から算出される理想の計測点Ｍの数と、実際の計測点Ｍの数との比率から、信頼度を算出し、実際の計測点Ｍの数が理想の計測点Ｍの数に近いほど、推定結果の精度が高いと判断して、信頼度を高くする。このように信頼度を算出することで、精度が高い推定結果を選択して、精度の高い第２パスＲ２を設定することが可能となる。

なお、本実施形態では、移動体１０の制御装置２８が、点群Ｍ０から目標物Ｐの位置及び姿勢を推定し、推定結果の信頼度を算出し、信頼度に基づき第２パスＲ２の設定に用いる推定結果を選択し、選択した推定結果から第２パスＲ２を設定していた。ただし、これらの処理は、移動体１０が実施することに限られず、例えば情報処理装置１４などの他の装置がこれらの処理の少なくとも一部を実行して、移動体１０は、その結果を取得するものであってもよい。すなわち例えば、移動体１０の目標物情報取得部７６は、目標物Ｐの位置及び姿勢の推定結果を取得するものであってよく、自身で目標物Ｐの位置及び姿勢を推定してもよいし、外部の装置から目標物Ｐの位置及び姿勢の推定結果を取得してもよい。また例えば、移動体１０の信頼度情報取得部７８は、推定結果の信頼度の情報を取得するものであってよく、自身で信頼度を算出してもよいし、外部の装置から推定結果の信頼度の情報を取得してもよい。信頼度情報取得部７８が外部の装置から信頼度の情報を取得する場合には、移動体１０は、推定結果を用いて演算する必要はないため、推定結果を取得する必要はないといえる。また例えば、移動体１０の選択情報取得部８０は、信頼度に基づき選択された推定結果の情報を取得するものであってよく、自身で推定結果を選択してもよいし、外部の装置から選択された推定結果の情報を取得してもよい。選択情報取得部８０が外部の装置から選択された推定結果の情報を取得する場合には、移動体１０は、信頼度を用いた選択処理を実施する必要がないため、信頼度を取得する必要はないといえる。また例えば、移動体１０の第２パス情報取得部８２は、推定結果に基づいて設定された第２パスＲ２の情報を取得するものであってよく、自身で選択された推定結果に基づいて第２パスＲ２を設定してもよいし、外部の装置から第２パスＲ２の情報を取得してもよい。第２パス情報取得部８２が外部の装置から第２パスＲ２の情報を取得する場合には、移動体１０は、選択された推定結果を用いて演算する必要はないため、選択された推定結果の情報を取得する必要はないといえる。

（第２実施形態）
次に、第２実施形態について説明する。第２実施形態においては、理想の計測点Ｍの数を算出するために用いる基準位置ＡＲ１が、第１実施形態とは異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。

図１４は、第２実施形態において理想の計測点数を算出する方法を説明するための模式図である。図１４に示すように、第２実施形態においては、信頼度情報取得部７８は、検出を行った際の移動体１０の位置とは異なる位置を、基準位置ＡＲ１として設定する。信頼度情報取得部７８は、目標物Ｐの位置及び姿勢の推定結果と、検出を行った際の移動体１０の位置とに基づき、基準位置ＡＲ１を設定する。信頼度情報取得部７８は、推定した目標物Ｐの位置及び姿勢（ここでは近似線Ｌ）に対して移動体１０が所定の位置及び姿勢となるような位置を、基準位置ＡＲ１として算出する。具体的には、信頼度情報取得部７８は、基準位置ＡＲ１と近似線Ｌの基準点とを結ぶ直線ＬＸの長さが、検出した際の移動体１０の位置から近似線Ｌの基準点までの長さと等しくなり、かつ、直線ＬＸと近似線Ｌの法線とのなす角度が所定範囲内となるように、基準位置ＡＲ１を算出する。ここでの近似線Ｌ上の基準点は、目標物Ｐの位置及び姿勢の推定結果から特定された、目標物Ｐの推定位置であるといえる。近似線Ｌ上の基準点は、任意の位置であってよいが、例えば近似線Ｌの中点であってよい。また、直線ＬＸと近似線Ｌの法線とのなす角度は小さいことが好ましい。例えば、基準位置ＡＲ１は、検出した際の移動体１０の位置から近似線Ｌの基準点を結ぶ直線と近似線Ｌの法線とのなす角度よりも、直線ＬＸと近似線Ｌの法線とのなす角度が小さくなるように、設定されることが好ましい。さらに言えば、基準位置ＡＲ１は、直線ＬＸと近似線Ｌの法線とのなす角度が、０度以上所定角度（例えば５度）以下であることが好ましい。すなわち、信頼度情報取得部７８は、レーザ光ＬＴの入射角が０°など小さくなる位置を、基準位置ＡＲ１として設定することが好ましい。

第２実施形態においては、このように、レーザ光ＬＴの入射角が小さくなる位置を、基準位置ＡＲ１として設定する。その後の理想の計測点Ｍの数の算出処理は、第１実施形態と同様なので、説明を省略する。ここで、基準位置ＡＲ１からのレーザ光ＬＴの入射角が大きい場合、検出角度θ（レーザ光ＬＴが目標物Ｐに照射される角度範囲）が小さくなるため、算出される理想の計測点Ｍの数が少なくなる。理想の計測点Ｍの数は、信頼度の算出の際の分母として用いられるため、算出される理想の計測点Ｍの数が少なくなると分母の数が小さくなり、信頼度がばらついてしまい、信頼度の算出精度が低下するおそれがある。それに対し、第２実施形態においては、直線ＬＸと近似線Ｌの法線とのなす角度が小さくなるように基準位置ＡＲ１を設定するため、信頼度の算出の際の分母が小さな値となることを抑制して、信頼度の算出精度の低下を抑制できる。

（信頼度算出の他の例）
第１実施形態と第２実施形態とは、理想の計測点Ｍの数と実際の計測点Ｍの数との比率に基づき、信頼度を算出する点で共通する。ただし、信頼度は、理想の計測点Ｍの数と実際の計測点Ｍの数との比率に基づき算出することに限られず、推定結果を用いて任意の方法で算出されてもよい。以下、信頼度の算出方法の他の例を説明する。

例えば、信頼度情報取得部７８は、近似線Ｌの残差に基づき、信頼度を算出してもよい。残差とは、計測点Ｍと近似線Ｌとの間の距離を指す。信頼度情報取得部７８は、近似線Ｌの残差が小さいほど、信頼度を高く設定することが好ましい。例えば、信頼度情報取得部７８は、それぞれの計測点Ｍについて、二次元座標系ＣＯにおける、計測点Ｍと近似線Ｌとの間の距離を残差として算出し、算出した残差の合計値が小さいほど信頼度を高く設定してよい。例えば、信頼度情報取得部７８は、計測点Ｍの合計値に対する残差の合計値の比率を、推定結果の信頼度として算出してよい。

また例えば、信頼度情報取得部７８は、近似線Ｌの算出に用いた計測点Ｍのうちの一部の計測点Ｍを除去して、残った計測点Ｍを直線フィッティングして算出した近似線Ｌ’と、近似線Ｌとの差分に基づいて、推定結果の信頼度を算出してよい。例えば、信頼度情報取得部７８は、近似線Ｌ’と近似線Ｌとの最短距離に基づき、推定結果の信頼度を算出してよい。この場合例えば、信頼度情報取得部７８は、近似線Ｌ’と近似線Ｌとの最短距離が短いほど、信頼度を高くする。また例えば、信頼度情報取得部７８は、近似線Ｌ’のＸ座標と近似線ＬのＸ座標との差分と、近似線Ｌ’のＹ座標と近似線ＬのＹ座標との差分と、近似線Ｌ’の姿勢（角度）と近似線Ｌの姿勢（角度）との差分とに基づき、信頼度を算出してよい。この場合、信頼度情報取得部７８は、それらの差分が小さいほど、信頼度を高くする。

また例えば、信頼度情報取得部７８は、検出した際の移動体１０と目標物Ｐ（近似線Ｌ）との位置関係に基づき、推定結果の信頼度を算出してよい。この場合例えば、信頼度情報取得部７８は、検出した際の移動体１０と目標物Ｐ（近似線Ｌ）との距離が短いほど、信頼度を高くする。また例えば、信頼度情報取得部７８は、検出した際の移動体１０と目標物Ｐ（近似線Ｌ）とを結んだ直線の、近似線Ｌの垂線に対する角度が小さいほど、信頼度を高くする。

また例えば、信頼度情報取得部７８は、移動体１０が位置している床上の位置（すなわち領域ＡＲ内で移動体１０が位置している箇所）の、水平方向に対する傾斜角度に基づき、推定結果の信頼度を算出してよい。この場合例えば、信頼度情報取得部７８は、検出した際の移動体１０が位置している箇所の、水平方向に対する傾斜角度の情報を取得し、傾斜角度に基づき、信頼度を算出する。信頼度情報取得部７８は、傾斜角度が小さいほど、信頼度を高くする。なお、傾斜角度の情報は任意の方法で取得してよいが、例えば移動体１０に傾斜角度を検出するセンサを搭載し、そのセンサの検出結果を取得してよい。

（第３実施形態）
次に、第３実施形態について説明する。第３実施形態は、同じタイミングで複数のセンサ２６に検出させた場合の信頼度の算出方法が、第１実施形態と異なる。第３実施形態において第１実施形態と構成が共通する箇所は、説明を省略する。なお、第３実施形態は、第２実施形態にも適用可能である。

（目標物の位置及び姿勢の推定）
第３実施形態においては、検出制御部７４は、同じタイミングで、すなわち移動体１０が同じ位置にいるタイミングで、複数のセンサ２６に検出を行わせて、各センサ２６の検出結果を点群Ｍ０（計測点Ｍ）として取得する。そして、目標物情報取得部７６は、複数のセンサ２６による同じタイミングでの検出により取得された計測点Ｍを同一の座標系（本実施形態の例では二次元座標系ＣＯ）で重畳する。すなわち、目標物情報取得部７６は、複数のセンサ２６による同じタイミングでの検出により取得されたそれぞれの計測点Ｍを、同一の二次元座標系ＣＯ上でプロットする。なお、それぞれの計測点Ｍを重畳する座標系は、二次元座標系ＣＯに限られず、例えば三次元座標系などであってもよい。

目標物情報取得部７６は、重畳された計測点Ｍを用いて、第１実施形態と同様の方法で、目標物Ｐの位置及び姿勢を推定する。第３実施形態においては、移動体１０が移動する毎に複数のセンサ２６に検出を行わせて、移動体１０が移動する毎に目標物Ｐの位置及び姿勢の推定を行う。第３実施形態においては、複数のセンサ２６の検出結果を重畳するため、目標物Ｐの位置及び姿勢の推定に用いる計測点Ｍの数を増やして、推定精度を向上させることができる。

（信頼度の算出）
第３実施形態においては、センサ２６毎に個別信頼度を算出し、それらの個別信頼度に基づき、目標物Ｐの位置及び姿勢の推定結果の信頼度を算出する。以下、第１センサと第２センサとに同じタイミングで検出を行わせた場合を例に説明する。この場合、目標物情報取得部７６は、第１センサの検出により取得した計測点Ｍ（点群Ｍ０）である第１計測点（第１点群）と、第２センサの検出により取得した計測点Ｍ（点群Ｍ０）である第２計測点（第２点群）とを、二次元座標系ＣＯで重畳する。そして、目標物情報取得部７６は、重畳した第１計測点及び第２計測点に基づき、目標物Ｐの位置及び姿勢を推定する。そして、信頼度情報取得部７８は、第１センサの検出結果（第１計測点）についての信頼度である第１個別信頼度と、第２センサの検出結果（第１計測点）についての信頼度である第２個別信頼度とを算出する。信頼度情報取得部７８は、第１個別信頼度と第２個別信頼度とに基づき、重畳した第１計測点及び第２計測点に基づいて算出された推定結果についての信頼度を算出する。なお、第１個別信頼度と第２個別信頼度との算出方法は、第１実施形態や第２実施形態や他の例で説明した信頼度の算出方法と同様であってよい。

より詳しくは、信頼度情報取得部７８は、それぞれの個別信頼度と、それぞれのセンサ２６から目標物Ｐの推定位置（例えば近似線Ｌ上の基準点）までの距離とに基づき、推定結果についての信頼度を算出する。すなわち、信頼度情報取得部７８は、第１個別信頼度と、検出時の第１センサの位置から目標物Ｐの推定位置までの距離と、第２個別信頼度と、検出時の第２センサの位置から目標物Ｐの推定位置までの距離とに基づき、信頼度を算出する。具体的には、信頼度情報取得部７８は、それぞれのセンサから目標物Ｐの推定位置までの距離に応じて、個別信頼度に重み付けをし、重み付けした個別信頼度を平均化することで、信頼度を算出する。信頼度情報取得部７８は、センサ２６から目標物Ｐの推定位置までの距離が短いほど、重みを大きくする。例えば、信頼度情報取得部７８は、検出した際の第１センサの位置から目標物Ｐの推定位置までの距離に基づき、第１個別信頼度についての第１重みを算出し、検出した際の第２センサの位置から目標物Ｐの推定位置までの距離に基づき、第２個別信頼度についての第２重みを算出する。そして、信頼度情報取得部７８は、第１個別信頼度と第１重みとを乗じた値と、第２個別信頼度と第２重みとを乗じた値との合計値を、個別信頼度の数で除することで、推定結果についての信頼度を算出する。

なお、以上は２つのセンサを例にした信頼度の算出方法を例にしたが、３つ以上のセンサを用いた場合の信頼度も、同様の方法で算出してよい。すなわち例えば、信頼度情報取得部７８は、次の式（２）を用いて、推定結果についての信頼度Ｓを算出する。式（２）において、ｎはセンサの数であり、ｖは個別信頼度であり、ｓは重みである。

Ｓ＝（ｖ１・ｓ１＋ｖ２・ｓ２＋ ・・・ｖｎ・ｓｎ）／ｎ ・・・（２）

このように、センサから目標物Ｐの推定値までの距離が短いほど重みを大きくして信頼度を算出することで、距離が近く精度が高い検出結果をより好適に信頼度に反映できるため、信頼度に基づき、精度の高い推定結果を適切に選択することが可能となる。ただし、第３実施形態における信頼度の算出方法は上記の説明に限られず、センサ２６毎の個別信頼度に基づいた任意の方法で、信頼度を算出してよい。

なお、第１実施形態においても、複数のセンサ２６に同じタイミングで検出を行わせてもよいが、第１実施形態の場合は、第２実施形態のように同じタイミングで各センサ２６が検出した計測点Ｍを重畳することなく、それぞれのセンサ２６の検出毎に、個別に推定結果を算出する。

（第４実施形態）
次に、第４実施形態について説明する。第４実施形態は、第２パスＲ２の設定に用いる推定結果の選択方法が、第１実施形態とは異なる。第４実施形態において第１実施形態と構成が共通する箇所は、説明を省略する。なお、第４実施形態は、第２実施形態及び第３実施形態にも適用可能である。

第４実施形態においては、選択情報取得部８０は、信頼度に加えて、検出の際の移動体１０の位置から目標物Ｐの推定位置までの距離である検出距離にも基づき、第２パスＲ２の設定に用いる推定結果を選択する。例えば、選択情報取得部８０は、それぞれの推定結果のうちから、信頼度が閾値以上となり、かつ、信頼度が閾値以上である推定結果のうちで検出距離が最短となる推定結果を、第２パスＲ２の設定に用いる推定結果として選択する。一例として、センサ２６の第１の検出で取得された計測点Ｍから第１推定結果が算出され、センサ２６の第２の検出で取得された計測点Ｍから第２推定結果が算出され、第１推定結果と第２推定結果との両方の信頼度が閾値以上となる場合で説明する。この例においては、第１の検出における移動体１０の位置から目標物Ｐの推定位置までの第１検出距離が、第２の検出における移動体１０の位置から目標物Ｐの推定位置までの第２検出距離よりも短い場合には、第１推定結果が、第２パスＲ２の設定に用いる推定結果として選択される。

第４実施形態においては、第２パスＲ２を生成した後に、上記のように信頼度及び検出距離に基づき推定結果を選択しなおして第２パスＲ２を更新することが好ましい。すなわちこの場合、移動体１０が第２位置Ａ２に到達したら、選択情報取得部８０は、第１パスＲ１の移動中におけるセンサ２６の検出結果から算出された、目標物Ｐの位置及び姿勢の推定結果のうちから、第２パスＲ２の設定に用いる推定結果を選択する。そして、第２パス情報取得部８２は、選択された推定結果を用いて第２パスＲ２を生成し、移動体１０は、この第２パスＲ２に従って移動を開始する。この最初の第２パスＲ２の設定に用いる推定結果は、第１実施形態のように信頼度に基づいて選択してもよいし、上記で説明したように信頼度と検出距離に基づいて選択してもよい。

そして、検出制御部７４は、移動体１０が第２パスＲ２に沿って移動中にも、センサ２６に検出を行わせる。目標物情報取得部７６は、第２パスＲ２でのセンサ２６の検出により取得された計測点Ｍに基づき、目標物Ｐの位置及び姿勢を推定し、信頼度情報取得部７８は、この推定結果についての信頼度と、第２パスＲ２で検出を行った際の検出距離とを算出する。そして、選択情報取得部８０は、信頼度と検出距離とに基づき、第１パスＲ１でのセンサ２６の検出により算出された推定結果と、第１パスＲ１でのセンサ２６の検出により算出された推定結果とのうちから、第２パスＲ２の更新に用いるための推定結果を選択する。この場合は、上述のように、選択情報取得部８０は、それぞれの推定結果のうちから、信頼度が閾値以上となり、かつ、信頼度が閾値以上となる推定結果のうちで検出距離が最短となる推定結果を、第２パスＲ２の更新に用いるための推定結果として選択する。そして、第２パス情報取得部８２は、更新のために選択された推定結果が、今の第２パスＲ２の設定用に選択した推定結果と異なる場合は、更新のために選択された推定結果を用いて、第２パスＲ２を更新する。移動体１０は、更新された第２パスＲ２に切り替えて、移動を続ける。検出制御部７４は、この第２パスＲ２の更新処理を繰り返してよい。

このように、第４実施形態においては、第２パスＲ２に沿って移動中にも検出を続けて、第２パスＲ２を更新することで、目標物Ｐに対するずれが少なくなるようにアプローチしていくことが可能となる。さらに言えば、センサ２６の検出精度は、検出距離が短くなるほど高くなる傾向にあるため、検出距離が短い推定結果に切り替えて第２パスＲ２を更新することで、目標物Ｐに対するずれを少なくすることが可能となる。ただしこの場合に、単に検出距離が短い推定結果を選んでしまうと、検出距離は短いが突発的に信頼度が低くなった推定結果を選択してしまう可能性があり、目標物Ｐに対してずれてしまうおそれがある。それに対し、第４実施形態では、信頼度を閾値でフィルタリングすることで、信頼度がある程度高い推定結果を用いて、目標物Ｐに対して適切にアプローチすることが可能となる。

（本開示の効果）
以上説明したように、本開示の移動体１０の制御方法は、自動で移動する移動体１０の制御方法であって、移動体１０に設けられたセンサ２６に目標物Ｐを検出させて、センサ２６の検出結果を点群Ｍ０として取得するステップと、点群Ｍ０に基づいて目標物Ｐの位置及び姿勢を推定するステップと、目標物Ｐの位置及び姿勢の推定結果と、検出を行った際の移動体１０の位置と、センサ２６の性能とに基づき、センサ２６の検出によって取得されると想定される理想の点群数を算出するステップと、理想の点群数と実際に取得された点群数との比較結果に基づき、目標物Ｐの位置及び姿勢の推定結果の精度に対する信頼度を算出するステップと、を含む。本制御方法によると、理想の点群数と実際の点群数との比較結果から信頼度を算出することで、推定結果の精度を適切に把握することが可能となる。従って、例えば信頼度に基づいて第２パスＲ２の設定に用いる推定結果を選択することなどが可能となり、移動体１０を目的位置まで適切に移動させることが可能となる。

また、センサ２６は、光を所定方向に走査しつつ照射することで目標物Ｐを検出するものであり、本開示の制御方法は、理想の点群数を算出するステップにおいては、検出を行った際の移動体１０の位置に対応する基準位置ＡＲ１からセンサ２６が光を照射した場合に、センサ２６が１回の検出で光を走査するスキャン角度のうちで、光が目標物Ｐに照射される角度範囲である検出角度θを算出し、検出角度θと、センサ２６の角度分解能との比率に基づき、理想の点群数を算出する。本制御方法によると、適切な条件で検出された場合の点群数を理想の点群数として算出できるため、実際のセンサ２６の検出が適切に行われたかを、信頼度に反映することが可能となる。そのため、このように算出された信頼度を用いることで、推定結果の精度を適切に把握することが可能となる。

また、理想の点群数を算出するステップにおいては、検出を行った際の移動体１０の位置を基準位置ＡＲ１とする。本制御方法によると、検出を行った際の移動体１０の位置を基準位置ＡＲ１として理想の点群数を算出することで、信頼度を適切に算出して、推定結果の精度を適切に把握することが可能となる。

また、理想の点群数を算出するステップにおいては、基準位置ＡＲ１から目標物Ｐの推定位置までの距離が、検出を行った際の移動体１０の位置から目標物Ｐの推定位置までの距離と等しく、かつ、基準位置ＡＲ１から目標物Ｐまでを結ぶ直線と目標物Ｐの推定した前面Ｐａ（近似線Ｌ）の法線とのなす角度が所定範囲内となるように、基準位置ＡＲ１を設定する。本制御方法によると、このように基準位置ＡＲ１を設定することで、理想の点群数の数が小さくなることなどを抑制して、信頼度の算出精度の低下を抑制できる。

また、目標物Ｐは、複数の柱ＰＡと柱ＰＡ同士の間の開口ＰＢとが形成されている。本開示の制御方法は、理想の点群数を算出するステップにおいて、目標物Ｐの位置及び姿勢の推定結果から、それぞれの柱ＰＡについて、柱ＰＡの幅方向の一方側の端点ＰＳ１の位置と他方側の端点ＰＳ２の位置を特定する。そして、それぞれの柱ＰＡについて、基準位置ＡＲ１と端点ＰＳ１とを結ぶ直線ＬＡ１と、基準位置ＡＲ１と端点ＰＳ２とを結ぶ直線ＬＡ２とのなす角度を、検出角度θとして算出し、検出角度θとセンサ２６の角度分解能との比率に基づき、柱ＰＡ毎の理想の点群数を算出する。また、信頼度を算出するステップにおいては、柱ＰＡ毎の理想の点群数と、柱ＰＡ毎の実際に取得された点群数との比率に基づき、柱ＰＡ毎の信頼度を算出し、ＰＡ柱毎の信頼度に基づき、目標物Ｐの位置及び姿勢の推定結果についての信頼度を算出する。本制御方法によると、このように柱ＰＡ毎に信頼度を算出することで、信頼度を適切に算出して、推定結果の精度を適切に把握することが可能となる。

また、理想の点群数を算出するステップにおいては、柱ＰＡの幅方向の一方側の端点ＰＳ１の位置と他方側の端点ＰＳ２の位置を特定する。そして、それぞれの柱ＰＡについて、基準位置ＡＲ１と端点ＰＳ１とを結ぶ直線ＬＡ１と、基準位置ＡＲ１と端点ＰＳ２とを結ぶ直線ＬＡ２とのなす角度を、検出角度θとして算出し、柱ＰＡ毎の検出角度θの合計値とセンサ２６の角度分解能との比率に基づき、柱ＰＡ毎の理想の点群数の合計値を算出する。また、信頼度を算出するステップにおいては、理想の点群数の合計値と、実際に取得された点群数との比率に基づき、信頼度を算出する。本制御方法によると、それぞれの柱ＰＡをまとめて信頼度を算出するため、演算負荷を抑制できる。

また、センサ２６の検出結果を点群として取得するステップにおいては、第１センサと第２センサとに目標物Ｐを検出させて、第１センサの検出結果に対応する第１点群と、第２センサの検出結果に対応する第２点群とを取得する。そして、目標物Ｐの位置及び姿勢を推定するステップにおいては、第１点群と第２点群とを同一座標系で重畳し、重畳した第１点群及び第２点群に基づいて、目標物Ｐの位置及び姿勢を推定する。そして、信頼度を算出するステップにおいては、第１点群についての信頼度である第１個別信頼度と、第２点群についての信頼度である第２個別信頼度とを算出し、第１個別信頼度と、検出した際の第１センサの位置から目標物Ｐの推定位置までの距離と、第２個別信頼度と、検出した際の第２センサの位置から目標物Ｐの推定位置までの距離とに基づき、信頼度を算出する。本制御方法によると、目標物Ｐの位置及び姿勢を推定するために用いる点群数を増やすことで推定精度を向上させることができる。さらに、センサ毎の個別信頼度と、センサから目標物Ｐの推定位置までの距離とを用いて信頼度を算出するため、信頼度を適切に算出して、推定結果の精度を適切に把握することが可能となる。

また、本開示の移動体１０は、自動で移動するものであって、移動体１０に設けられたセンサ２６に目標物Ｐを検出させて、センサ２６の検出結果を点群Ｍ０として取得する検出制御部７４と、点群Ｍ０に基づき推定された目標物Ｐの位置及び姿勢の推定結果の精度に対する信頼度の情報を取得する信頼度情報取得部７８とを含む。信頼度は、点群Ｍ０に基づいて目標物Ｐの位置及び姿勢が推定され、目標物Ｐの位置及び姿勢の推定結果と、検出を行った際の移動体１０の位置と、センサ２６の性能とに基づき、センサ２６の検出によって取得されると想定される理想の点群数が算出されることで、理想の点群数と実際に取得された点群数との比較結果に基づき算出される。この移動体１０は、このように算出された信頼度を用いることで、目的位置まで適切に移動することが可能となる。

また、本開示のプログラムは、自動で移動する移動体１０の制御方法をコンピュータに実行させるものであって、移動体１０に設けられたセンサ２６に目標物Ｐを検出させて、センサ２６の検出結果を点群Ｍ０として取得するステップと、点群Ｍ０に基づいて目標物Ｐの位置及び姿勢を推定するステップと、目標物Ｐの位置及び姿勢の推定結果と、検出を行った際の移動体１０の位置と、センサ２６の性能とに基づき、センサ２６の検出によって取得されると想定される理想の点群数を算出するステップと、理想の点群数と実際に取得された点群数との比較結果に基づき、目標物Ｐの位置及び姿勢の推定結果の精度に対する信頼度を算出するステップと、をコンピュータに実行させる。本プログラムによると、移動体１０を目的位置まで適切に移動させることが可能となる。

また、本開示の移動体１０の制御方法は、自動で移動する移動体１０の制御方法であって、移動体１０に設けられたセンサ２６に目標物Ｐを複数回検出させて、センサ２６の検出毎の検出結果を点群Ｍ０として取得するステップと、点群Ｍ０に基づいて、センサ２６の検出毎に、目標物Ｐの位置及び姿勢を推定するステップと、センサ２６の検出毎に、目標物Ｐの位置及び姿勢の推定結果の精度に対する信頼度を算出するステップと、センサ２６の検出毎の信頼度に基づき、センサ２６の検出毎の推定結果から、目標物Ｐに対して所定の位置及び姿勢となる目標位置までの第２パスＲ２（アプローチパス）を設定するために用いる推定結果を選択するステップと、選択された推定結果に基づいて、第２パスＲ２を設定するステップと、第２パスＲ２に沿って移動体１０を移動させるステップと、を含む。本制御方法によると、推定結果毎の信頼度を算出し、信頼度に基づいて、第２パスＲ２の設定に用いる推定結果を選択するため、精度の高い推定結果を用いて、移動体１０を目的位置まで適切に移動させることが可能となる。

また、目標物Ｐの位置及び姿勢を推定するステップにおいては、センサ２６の第１の検出に対応する点群である第１点群に基づき目標物Ｐの位置及び姿勢を推定しつつ、センサ２６の第２の検出に対応する第２点群に基づき目標物Ｐの位置及び姿勢を推定する。そして、信頼度を算出するステップにおいては、第１点群に基づいた推定結果の信頼度と、第２点群に基づいた推定結果の信頼度である信頼度とを算出し、推定結果を選択するステップにおいては、第１点群に基づいた推定結果と第２点群に基づいた推定結果とのうちで、信頼度が高い方の推定結果を選択する。本制御方法によると、精度の高い推定結果を用いて、移動体１０を目的位置まで適切に移動させることが可能となる。

また、推定結果を選択するステップにおいては、センサ２６の検出毎の推定結果のうちで、信頼度が最高となる推定結果を選択する。本制御方法によると、精度の高い推定結果を用いて、移動体１０を目的位置まで適切に移動させることが可能となる。

また、推定結果を選択するステップにおいては、検出した際の移動体１０の位置から目標物Ｐの推定位置までの検出距離にも基づき、推定結果を選択する。本制御方法によると、検出距離も用いて第２パスＲ２の設定に用いる推定結果を選択するため、さらに精度の高い推定結果を選択することが可能となり、移動体１０を目的位置まで適切に移動させることが可能となる。

また、推定結果を選択するステップにおいては、信頼度が閾値以上となり、かつ、信頼度が閾値以上である推定結果のうちで検出距離が最小となる推定結果を、選択する。本制御方法によると、さらに精度の高い推定結果を選択することが可能となり、移動体１０を目的位置まで適切に移動させることが可能となる。

また、本開示の制御方法は、目標物Ｐが設置される設置領域ＡＲ０を横切る広域パス（第１パスＲ１）の情報を取得するステップと、第１パスＲ１に沿って移動体１０を移動させるステップと、をさらに含む。点群Ｍ０を取得するステップにおいては、移動体１０が第１パスＲ１に沿って移動中に、センサ２６に目標物Ｐを複数回検出させる。本制御方法によると、目標物Ｐを横切って移動している最中に、センサ２６に複数回検出させることで、目標物Ｐを適切に検出させて、目標物Ｐの検出精度の低下を好適に抑制できる。

また、本開示の移動体１０は、自動で移動するものであって、移動体１０に設けられたセンサ２６に目標物Ｐを複数回検出させて、センサ２６の検出毎の検出結果を点群Ｍ０として取得する検出制御部７４と、点群Ｍ０に基づき設定された、目標物Ｐに対して所定の位置及び姿勢となる目標位置までの第２パスＲ２の情報を取得する第２パス情報取得部８２（アプローチパス情報取得部）と、第２パスＲ２に沿って移動体１０を移動させる移動制御部７２と、を含む。第２パスＲ２は、点群Ｍ０に基づいて、センサ２６の検出毎に目標物Ｐの位置及び姿勢が推定され、センサ２６の検出毎に、目標物Ｐの位置及び姿勢の推定結果の精度に対する信頼度が算出され、センサ２６の検出毎の信頼度に基づき、センサ２６の検出毎の推定結果から、第２パスＲ２を設定するために用いる推定結果が選択されることで、選択された推定結果に基づいて、設定される。この移動体１０は、信頼度に基づいて選択された推定情報から設定された第２パスＲ２に従って移動するため、目的位置まで適切に移動することが可能となる。

また、本開示のプログラムは、自動で移動する移動体１０の制御方法をコンピュータに実行させるものであって、移動体１０に設けられたセンサ２６に目標物Ｐを複数回検出させて、センサ２６の検出毎の検出結果を点群Ｍ０として取得するステップと、点群Ｍ０に基づいて、センサ２６の検出毎に、目標物Ｐの位置及び姿勢を推定するステップと、センサ２６の検出毎に、目標物Ｐの位置及び姿勢の推定結果の精度に対する信頼度を算出するステップと、センサ２６の検出毎の信頼度に基づき、センサ２６の検出毎の推定結果から、目標物Ｐに対して所定の位置及び姿勢となる目標位置までの第２パスＲ２（アプローチパス）を設定するために用いる推定結果を選択するステップと、選択された推定結果に基づいて、第２パスＲ２を設定するステップと、第２パスＲ２に沿って移動体１０を移動させるステップと、をコンピュータに実行させる。本プログラムによると、精度の高い推定結果を用いて、移動体１０を目的位置まで適切に移動させることが可能となる。

以上、本発明の実施形態を説明したが、この実施形態の内容により実施形態が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。さらに、前述した実施形態の要旨を逸脱しない範囲で構成要素の種々の省略、置換又は変更を行うことができる。

１０ 移動体
２６ センサ
７０ 第１パス情報取得部
７２ 移動制御部
７４ 検出制御部
７６ 目標物情報取得部
７８ 信頼度情報取得部
８０ 選択情報取得部
８２ 第２パス情報取得部
Ｍ 計測点
Ｍ０ 点群
Ｐ 目標物
Ｒ２ 第２パス

Claims (9)

1. 自動で移動する移動体の制御方法であって、
   前記移動体に設けられたセンサに目標物を検出させて、前記センサの検出結果を点群として取得するステップと、
   前記点群に基づいて、前記目標物の位置及び姿勢を推定するステップと、
   前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置と、前記センサの性能とに基づき、前記センサの検出によって取得されると想定される理想の点群数を算出するステップと、
   前記理想の点群数と実際に取得された点群数との比較結果に基づき、前記目標物の位置及び姿勢の推定結果の精度に対する信頼度を算出するステップと、
   を含み、
   前記センサは、光を所定方向に走査しつつ照射することで前記目標物を検出するものであり、
   前記センサの検出結果を点群として取得するステップにおいては、第１センサと第２センサとに前記目標物を検出させて、前記第１センサの検出結果に対応する第１点群と、前記第２センサの検出結果に対応する第２点群とを取得し、
   前記理想の点群数を算出するステップにおいては、
   前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置とに基づいて、検出を行った際の前記移動体の位置に対応する基準位置から前記センサが光を照射した場合に、前記センサが１回の検出で光を走査するスキャン角度のうちで、前記光が前記目標物に照射される角度範囲であると想定される検出角度を算出し、
   前記検出角度と、前記センサの角度分解能との比率に基づき、前記理想の点群数を算出し、
   前記目標物の位置及び姿勢を推定するステップにおいては、前記第１点群と前記第２点群とを同一座標系で重畳し、重畳した前記第１点群及び前記第２点群に基づいて、前記目標物の位置及び姿勢を推定し、
   前記信頼度を算出するステップにおいては、前記第１点群についての信頼度である第１個別信頼度と、前記第２点群についての信頼度である第２個別信頼度とを算出し、前記第１個別信頼度と、検出した際の前記第１センサの位置から前記目標物の推定位置までの距離と、前記第２個別信頼度と、検出した際の前記第２センサの位置から前記目標物の推定位置までの距離とに基づき、前記信頼度を算出する、
   移動体の制御方法。
2. 前記理想の点群数を算出するステップにおいては、
   検出を行った際の前記移動体の位置を前記基準位置とする、請求項１に記載の移動体の制御方法。
3. 前記理想の点群数を算出するステップにおいては、
   前記目標物の推定位置までの距離が、検出を行った際の前記移動体の位置から前記目標物の推定位置までの距離と等しく、かつ、前記目標物までを結ぶ直線と前記目標物の推定した前面の法線とのなす角度が所定範囲内となるような位置を、前記基準位置とする、請求項１に記載の移動体の制御方法。
4. 前記目標物は、複数の柱と前記柱同士の間の開口とが形成されており、
   前記理想の点群数を算出するステップにおいては、
   前記目標物の位置及び姿勢の推定結果から、それぞれの前記柱について、前記柱の幅方向の一方側の端点の位置と他方側の端点の位置を特定し、
   それぞれの前記柱について、前記基準位置と前記一方側の端点とを結ぶ直線と、前記基準位置と前記他方側の端点とを結ぶ直線とのなす角度を、前記検出角度として算出し、
   前記検出角度と前記センサの角度分解能との比率に基づき、前記柱毎の理想の点群数を算出し、
   前記信頼度を算出するステップにおいては、
   前記柱毎の理想の点群数と、前記柱毎の実際に取得された点群数との比率に基づき、前記柱毎の信頼度を算出し、前記柱毎の信頼度に基づき、前記目標物の位置及び姿勢の推定結果についての信頼度を算出する、請求項１から請求項３のいずれか１項に記載の移動体の制御方法。
5. 前記目標物は、複数の柱と前記柱同士の間の開口とが形成されており、
   前記理想の点群数を算出するステップにおいては、
   前記目標物の位置及び姿勢の推定結果から、それぞれの前記柱について、前記柱の幅方向の一方側の端点の位置と他方側の端点の位置を特定し、
   それぞれの前記柱について、前記基準位置と前記一方側の端点とを結ぶ直線と、前記基準位置と前記他方側の端点とを結ぶ直線とのなす角度を、前記検出角度として算出し、
   前記柱毎の前記検出角度の合計値と前記センサの角度分解能との比率に基づき、前記柱毎の理想の点群数の合計値を算出し、
   前記信頼度を算出するステップにおいては、
   前記理想の点群数の合計値と、実際に取得された点群数との比率に基づき、前記信頼度を算出する、請求項１から請求項３のいずれか１項に記載の移動体の制御方法。
6. 前記センサの検出結果を点群として取得するステップにおいては、前記センサに目標物を複数回検出させ、
   前記目標物の位置及び姿勢を推定するステップにおいては、前記センサの検出毎に前記目標物の位置及び姿勢を推定し、
   前記信頼度を算出するステップにおいては、前記センサの検出毎に前記信頼度を算出し、
   さらに、
   前記センサの検出毎の前記信頼度に基づき、前記センサの検出毎の前記目標物の位置及び姿勢の推定結果のうちから、前記目標物に対して所定の位置及び姿勢となる目標位置までのアプローチパスを設定するために用いる推定結果を選択するステップと、
   選択された前記目標物の位置及び姿勢の推定結果に基づいて、前記アプローチパスを設定するステップと、
   前記アプローチパスに沿って前記移動体を移動させるステップと、を含む、請求項１から請求項５のいずれか１項に記載の移動体の制御方法。
7. 自動で移動する移動体であって、
   前記移動体に設けられたセンサに目標物を検出させて、前記センサの検出結果を点群として取得する検出制御部と、
   前記点群に基づき推定された前記目標物の位置及び姿勢の推定結果の精度に対する信頼度の情報を取得する信頼度情報取得部と、を含み、
   前記センサは、光を所定方向に走査しつつ照射することで前記目標物を検出するものであり、
   前記検出制御部は、第１センサと第２センサとに前記目標物を検出させて、前記第１センサの検出結果に対応する第１点群と、前記第２センサの検出結果に対応する第２点群とを取得し、
   前記点群に基づいて、前記目標物の位置及び姿勢が推定され、
   前記目標物の位置及び姿勢は、前記第１点群と前記第２点群とが同一座標系で重畳され、重畳された前記第１点群及び前記第２点群に基づいて推定され、
   前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置と、前記センサの性能とに基づき、前記センサの検出によって取得されると想定される理想の点群数が算出され、
   前記信頼度は、
   前記理想の点群数と実際に取得された点群数との比較結果に基づき算出され、
   前記第１点群についての信頼度である第１個別信頼度と、前記第１個別信頼度と、検出した際の前記第１センサの位置から前記目標物の推定位置までの距離と、前記第２点群についての信頼度である第２個別信頼度と、検出した際の前記第２センサの位置から前記目標物の推定位置までの距離とに基づき算出され、
   前記理想の点群数は、
   前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置とに基づいて、検出を行った際の前記移動体の位置に対応する基準位置から前記センサが光を照射した場合に、前記センサが１回の検出で光を走査するスキャン角度のうちで、前記光が前記目標物に照射される角度範囲であると想定される検出角度を算出し、
   前記検出角度と、前記センサの角度分解能との比率に基づき、前記理想の点群数を算出される、
   移動体。
8. 自動で移動する移動体の制御方法をコンピュータに実行させるプログラムであって、
   前記移動体に設けられたセンサに目標物を検出させて、前記センサの検出結果を点群として取得するステップと、
   前記点群に基づいて、前記目標物の位置及び姿勢を推定するステップと、
   前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置と、前記センサの性能とに基づき、前記センサの検出によって取得されると想定される理想の点群数を算出するステップと、
   前記理想の点群数と実際に取得された点群数との比較結果に基づき、前記目標物の位置及び姿勢の推定結果の精度に対する信頼度を算出するステップと、
   を含み、
   前記センサは、光を所定方向に走査しつつ照射することで前記目標物を検出するものであり、
   前記センサの検出結果を点群として取得するステップにおいては、第１センサと第２センサとに前記目標物を検出させて、前記第１センサの検出結果に対応する第１点群と、前記第２センサの検出結果に対応する第２点群とを取得し、
   前記理想の点群数を算出するステップにおいては、
   前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置とに基づいて、検出を行った際の前記移動体の位置に対応する基準位置から前記センサが光を照射した場合に、前記センサが１回の検出で光を走査するスキャン角度のうちで、前記光が前記目標物に照射される角度範囲であると想定される検出角度を算出し、
   前記検出角度と、前記センサの角度分解能との比率に基づき、前記理想の点群数を算出し、
   前記目標物の位置及び姿勢を推定するステップにおいては、前記第１点群と前記第２点群とを同一座標系で重畳し、重畳した前記第１点群及び前記第２点群に基づいて、前記目標物の位置及び姿勢を推定し、
   前記信頼度を算出するステップにおいては、前記第１点群についての信頼度である第１個別信頼度と、前記第２点群についての信頼度である第２個別信頼度とを算出し、前記第１個別信頼度と、検出した際の前記第１センサの位置から前記目標物の推定位置までの距離と、前記第２個別信頼度と、検出した際の前記第２センサの位置から前記目標物の推定位置までの距離とに基づき、前記信頼度を算出すること、
   を、コンピュータに実行させる、
   プログラム。
9. 自動で移動する移動体の制御方法であって、
   前記移動体に設けられたセンサに目標物を検出させて、前記センサの検出結果を点群として取得するステップと、
   前記点群に基づいて、前記目標物の位置及び姿勢を推定するステップと、
   前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置と、前記センサの性能とに基づき、前記センサの検出によって取得されると想定される理想の点群数を算出するステップと、
   前記理想の点群数と実際に取得された点群数との比較結果に基づき、前記目標物の位置及び姿勢の推定結果の精度に対する信頼度を算出するステップと、
   を含み、
   前記センサは、光を所定方向に走査しつつ照射することで前記目標物を検出するものであり、
   前記目標物の位置及び姿勢を推定するステップにおいては、
   一以上の近似線候補を取得し、前記近似線候補から規定距離以上の手前側の前記点群については他の前記点群よりも優先度が低い定数のスコアを付与し、前記一以上の近似線候補のそれぞれについてスコア積算値を算出し、前記スコア積算値に基づいて、前記目標物の前面に対応する近似線を検出し、前記近似線に基づき、前記目標物の位置及び姿勢を推定し、
   前記理想の点群数を算出するステップにおいては、
   前記目標物の位置及び姿勢の推定結果と、検出を行った際の前記移動体の位置とに基づいて、検出を行った際の前記移動体の位置に対応する基準位置から前記センサが光を照射した場合に、前記センサが１回の検出で光を走査するスキャン角度のうちで、前記光が前記目標物に照射される角度範囲であると想定される検出角度を算出し、
   前記検出角度と、前記センサの角度分解能との比率に基づき、前記理想の点群数を算出する、
   移動体の制御方法。

Images