ホーム > 特許ランキング > 三菱ロジスネクスト株式会社
知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-06-27
(45)【発行日】2024-07-05
(54)【発明の名称】移動体の制御方法、移動体及びプログラム
(51)【国際特許分類】
G05D 1/43 20240101AFI20240628BHJP
B62D 6/00 20060101ALI20240628BHJP
B62D 1/28 20060101ALI20240628BHJP
【ＦＩ】
G05D1/43
B62D6/00
B62D1/28
【請求項の数】
12
(21)【出願番号】P 2021065478
(22)【出願日】2021-04-07
(65)【公開番号】P2022160941
(43)【公開日】2022-10-20
【審査請求日】2023-03-29
(73)【特許権者】
【識別番号】000232807
【氏名又は名称】三菱ロジスネクスト株式会社
(74)【代理人】
【識別番号】110002147
【氏名又は名称】弁理士法人酒井国際特許事務所
(72)【発明者】
【氏名】藤島 泰郎
(72)【発明者】
【氏名】荒木 亮次
(72)【発明者】
【氏名】高木 一茂
(72)【発明者】
【氏名】▲高▼尾 健司
(72)【発明者】
【氏名】知識 陽平
【審査官】影山 直洋
(56)【参考文献】
【文献】特開２０２０－０７０１２１（ＪＰ，Ａ）
【文献】特開２０２０－１１８５８６（ＪＰ，Ａ）
【文献】特表２０２０－５３２７７５（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０５Ｄ １／４３
Ｂ６２Ｄ ６／００
Ｂ６２Ｄ １／２８
(57)【特許請求の範囲】
【請求項1】
自動で移動する移動体の制御方法であって、前記移動体を第１パスに沿って移動させるステップと、
前記移動体が第１パスに沿って移動中に、前記移動体に設けられたセンサに目標物の位置及び姿勢を検出させるステップと、
前記目標物の位置及び姿勢に基づき、前記目標物に対して所定の位置及び姿勢となる目標位置までの第２パスを設定するステップと、
前記第１パスから前記第２パスに切り替えて、前記移動体を前記第２パスに沿って移動させるステップと、
現在以降の各離散時刻である先読みステップ毎の、前記目標物の位置と前記移動体の位置との距離を示す偏差が小さくなるほど、評価が高くなる評価関数に基づき、前記偏差が最小となるような前記先読みステップ毎の前記移動体の位置及び姿勢を、最適化計算により算出して、算出した前記先読みステップ毎の前記移動体の位置及び姿勢を、第３パスとして設定するステップと、
前記第２パスから前記第３パスに切り替えて、前記移動体を前記第３パスに沿って移動させるステップと、
を含む、
移動体の制御方法。
【請求項2】
前記第１パスは、前記目標物が設置される設置領域よりも第１方向側で、前記第１方向と交差する第２方向に前記設置領域を横切る経路であり、前記第１パスに沿って移動させるステップにおいては、前記第２方向において前記設置領域よりも一方側の第１位置から、前記第２方向において前記設置領域よりも他方側の第２位置まで、前記第１パスに沿って前記移動体を移動させ、
前記目標物の位置及び姿勢を検出させるステップにおいては、前記移動体が前記第１位置から前記第２位置までの移動の最中に、前記センサに前記目標物の位置及び姿勢を検出させる、請求項１に記載の移動体の制御方法。
【請求項3】
前記第２パスを設定するステップにおいては、前記第１パスの前記第１位置から前記第２位置までの軌道に重なる直線軌道と、前記直線軌道に接続される円弧軌道とを含むように、前記第２パスを設定する、請求項２に記載の移動体の制御方法。
【請求項4】
前記第２パスを設定するステップにおいては、前記円弧軌道として、前記第１方向において前記設置領域と反対側に向かう第１円弧軌道と、前記第１円弧軌道に接続されて前記第１方向において前記設置領域側に向かう第２円弧軌道とを含むように、前記第２パスを設定する、請求項３に記載の移動体の制御方法。
【請求項5】
前記移動体が前記第２パスに沿って移動中における前記センサによる前記目標物の検出精度と、前記移動体の進行方向と、前記移動体と前記目標物との間の距離との、少なくとも１つに基づき、前記第２パスから前記第３パスに切り替えるかを判断するステップをさらに含む、請求項１から請求項４のいずれか１項に記載の移動体の制御方法。
【請求項6】
前記第２パスから前記第３パスに切り替えるかを判断するステップにおいては、前記目標物の検出精度の信頼度が所定値以上であり、前記移動体が前記目標物に近づく方向に進行しており、かつ、前記移動体と前記目標物との間の距離が所定距離以下である場合に、前記第２パスから前記第３パスに切り替えると判断する、請求項５に記載の移動体の制御方法。
【請求項7】
前記第３パスを設定するステップにおいては、前記目標物の位置と前記移動体の位置とを逐次検出させて、前記第３パスを逐次更新する、請求項１から請求項６のいずれか１項に記載の移動体の制御方法。
【請求項8】
前記移動体及び前記目標物の位置及び姿勢の少なくとも一方を逐次検出して、前記移動体の位置及び姿勢の少なくとも一方が前記目標物の位置及び姿勢の少なくとも一方と合致する第４パスを、前記移動体及び前記目標物の位置及び姿勢の少なくとも一方を検出する毎に逐次設定するステップと、前記第３パスから前記第４パスに切り替えて、前記移動体を前記第４パスに沿って移動させるステップと、をさらに含む、請求項１から請求項７のいずれか１項に記載の移動体の制御方法。
【請求項9】
前記移動体が切替位置に到達した場合に、前記第３パスから前記第４パスに切り替えると判断するステップをさらに含み、前記切替位置は、前記移動体が有する移動及び操舵の制約下で、前記目標物の位置及び姿勢に応じて前記目標物に到達する際の前記移動体の位置及び姿勢に課せられる条件を満たして、前記移動体を前記目標物に到達させることが可能な位置である、請求項８に記載の移動体の制御方法。
【請求項10】
前記第４パスを設定するステップにおいては、前記移動体の操舵角の正負を切り替えることなく前記移動体が前記目標物に到達可能であるかを判断し、到達可能でない場合には、前記目標物の位置が前記移動体の位置と合致するように前記第４パスを設定し、到達可能である場合には、前記目標物の姿勢が前記移動体の姿勢と合致するように前記第４パスを設定する、請求項８又は請求項９に記載の移動体の制御方法。
【請求項11】
自動で移動する移動体であって、前記移動体を第１パスに沿って移動させる移動制御部と、
前記移動体が前記第１パスに沿って移動中に、前記移動体に設けられたセンサに目標物の位置及び姿勢を検出させる検出制御部と、
を含み、
前記移動制御部は、
前記第１パスから、前記目標物の位置及び姿勢に基づき設定された前記目標物に対して所定の位置及び姿勢となる目標位置までの第２パスに切り替えて、前記移動体を前記第２パスに沿って移動させ、
前記第２パスから、現在以降の各離散時刻である先読みステップ毎の、前記目標物の位置と前記移動体の位置との距離を示す偏差が小さくなるほど、評価が高くなる評価関数に基づき実行された、前記偏差が最小となるような前記先読みステップ毎の前記移動体の位置及び姿勢を算出する最適化計算により設定された、前記先読みステップ毎の前記移動体の位置及び姿勢である第３パスに切り替えて、前記移動体を前記第３パスに沿って移動させる、
移動体。
【請求項12】
自動で移動する移動体の制御方法をコンピュータに実行させるプログラムであって、前記移動体を第１パスに沿って移動させるステップと、
前記移動体が第１パスに沿って移動中に、前記移動体に設けられたセンサに目標物の位置及び姿勢を検出させるステップと、
前記目標物の位置及び姿勢に基づき、前記目標物に対して所定の位置及び姿勢となる目標位置までの第２パスを設定するステップと、
前記第１パスから前記第２パスに切り替えて、前記移動体を前記第２パスに沿って移動させるステップと、
現在以降の各離散時刻である先読みステップ毎の、前記目標物の位置と前記移動体の位置との距離を示す偏差が小さくなるほど、評価が高くなる評価関数に基づき、前記偏差が最小となるような前記先読みステップ毎の前記移動体の位置及び姿勢を、最適化計算により算出して、算出した前記先読みステップ毎の前記移動体の位置及び姿勢を、第３パスとして設定するステップと、
前記第２パスから前記第３パスに切り替えて、前記移動体を前記第３パスに沿って移動させるステップと、
を、コンピュータに実行させる、
プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、移動体の制御方法、移動体及びプログラムに関する。
【背景技術】
【0002】
例えばフォークリフトなどの移動体を、自動的に目標位置まで移動させる技術が知られている。特許文献１には、移動体に設けられた測域センサで検出した目標物の位置情報に基づき、目標物までのアプローチ軌道を決定する旨が記載されている。
【先行技術文献】
【特許文献】
【0003】
【文献】特許第６４９２０２４号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかし、特許文献１に記載の方法は、フォークリフトがパレットにある程度近接してからの制御方法として予め用意されたアプローチ軌道データを逐次補正しながらフォークリフトを移動させるとしている。しかしながら、係るアプローチ軌道データがどのようなデータであるのかが不明である。また、不明なアプローチ軌道データをどのように補正するのかも当然不明である。また、例えばパレットなどの目標物の位置が想定からずれている場合には、特許文献１に記載の方法では目標位置に適切にアプローチできない可能性もある。従って、目標位置まで適切にアプローチすることが求められている。
【0005】
本開示は、上述した課題を解決するものであり、目標位置まで適切にアプローチ可能な移動体の制御方法、移動体及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【0006】
上述した課題を解決し、目的を達成するために、本開示に係る移動体の制御方法は、自動で移動する移動体の制御方法であって、前記移動体を第１パスに沿って移動させるステップと、前記移動体が第１パスに沿って移動中に、前記移動体に設けられたセンサに目標物の位置及び姿勢を検出させるステップと、前記目標物の位置及び姿勢に基づき、前記目標物に対して所定の位置及び姿勢となる目標位置までの第２パスを設定するステップと、前記第１パスから前記第２パスに切り替えて、前記移動体を前記第２パスに沿って移動させるステップと、前記目標物の位置と前記移動体の位置との偏差が小さくなるほど評価が高くなる評価関数に基づき最適化計算を行い、第３パスを設定するステップと、前記第２パスから前記第３パスに切り替えて、前記移動体を前記第３パスに沿って移動させるステップと、を含む。
【0007】
上述した課題を解決し、目的を達成するために、本開示に係る移動体は、自動で移動する移動体であって、前記移動体を第１パスに沿って移動させる移動制御部と、前記移動体が前記第１パスに沿って移動中に、前記移動体に設けられたセンサに目標物の位置及び姿勢を検出させる検出制御部と、を含み、前記移動制御部は、前記第１パスから、前記目標物の位置及び姿勢に基づき設定された前記目標物に対して所定の位置及び姿勢となる目標位置までの第２パスに切り替えて、前記移動体を前記第２パスに沿って移動させ、前記第２パスから、前記目標物の位置と前記移動体の位置との偏差が小さくなるほど評価が高くなる評価関数に基づき実行された最適化計算により設定された第３パスに切り替えて、前記移動体を前記第３パスに沿って移動させる。
【0008】
上述した課題を解決し、目的を達成するために、本開示に係るプログラムは、自動で移動する移動体の制御方法をコンピュータに実行させるプログラムであって、前記移動体を第１パスに沿って移動させるステップと、前記移動体が第１パスに沿って移動中に、前記移動体に設けられたセンサに目標物の位置及び姿勢を検出させるステップと、前記目標物の位置及び姿勢に基づき、前記目標物に対して所定の位置及び姿勢となる目標位置までの第２パスを設定するステップと、前記第１パスから前記第２パスに切り替えて、前記移動体を前記第２パスに沿って移動させるステップと、前記目標物の位置と前記移動体の位置との偏差が小さくなるほど評価が高くなる評価関数に基づき最適化計算を行い、第３パスを設定するステップと、前記第２パスから前記第３パスに切り替えて、前記移動体を前記第３パスに沿って移動させるステップと、を、コンピュータに実行させる。
【発明の効果】
【0009】
本開示によれば、目標位置まで適切にアプローチすることができる。
【図面の簡単な説明】
【0010】
【発明を実施するための形態】
【0011】
以下に添付図面を参照して、本発明の好適な実施形態を詳細に説明する。なお、この実施形態により本発明が限定されるものではなく、また、実施形態が複数ある場合には、各実施形態を組み合わせて構成するものも含むものである。
【0012】
（第１実施形態）
（移動制御システムの全体構成）
図１は、第１実施形態に係る移動制御システムの模式図である。図１に示すように、第１実施形態に係る移動制御システム１は、移動体１０、管理システム１２、及び情報処理装置１４を含む。移動制御システム１は、設備Ｗに所属する移動体１０の移動を制御するシステムである。設備Ｗは、例えば倉庫など、物流管理される設備である。移動制御システム１においては、移動体１０によって設備Ｗの領域ＡＲ内に配置された目標物Ｐをピックアップして搬送させる。領域ＡＲは、例えば設備Ｗの床面であり、目標物Ｐが設置されたり移動体１０が移動したりする領域である。目標物Ｐは、本実施形態では、パレット上に荷物が積載された搬送対象物である。目標物Ｐは、前面Ｐａに、移動体１０の後述するフォーク２４が挿入される開口Ｐｂが形成されている。ただし、目標物Ｐは、パレット上に荷物が積載されたものに限られず任意の形態であってよく、例えばパレットを有さず荷物のみであってもよい。以下、領域ＡＲに沿った一方向を、方向Ｘとし、領域Ａに沿った方向であって方向Ｘに交差する方向を、方向Ｙとする。本実施形態では、方向Ｙは、方向Ｘに直交する方向である。方向Ｘ、方向Ｙは、水平方向といってもよい。また、方向Ｘ、方向Ｙに直交する方向を、すなわち鉛直方向を、方向Ｚとする。
（移動制御システムの全体構成）
図１は、第１実施形態に係る移動制御システムの模式図である。図１に示すように、第１実施形態に係る移動制御システム１は、移動体１０、管理システム１２、及び情報処理装置１４を含む。移動制御システム１は、設備Ｗに所属する移動体１０の移動を制御するシステムである。設備Ｗは、例えば倉庫など、物流管理される設備である。移動制御システム１においては、移動体１０によって設備Ｗの領域ＡＲ内に配置された目標物Ｐをピックアップして搬送させる。領域ＡＲは、例えば設備Ｗの床面であり、目標物Ｐが設置されたり移動体１０が移動したりする領域である。目標物Ｐは、本実施形態では、パレット上に荷物が積載された搬送対象物である。目標物Ｐは、前面Ｐａに、移動体１０の後述するフォーク２４が挿入される開口Ｐｂが形成されている。ただし、目標物Ｐは、パレット上に荷物が積載されたものに限られず任意の形態であってよく、例えばパレットを有さず荷物のみであってもよい。以下、領域ＡＲに沿った一方向を、方向Ｘとし、領域Ａに沿った方向であって方向Ｘに交差する方向を、方向Ｙとする。本実施形態では、方向Ｙは、方向Ｘに直交する方向である。方向Ｘ、方向Ｙは、水平方向といってもよい。また、方向Ｘ、方向Ｙに直交する方向を、すなわち鉛直方向を、方向Ｚとする。
【0013】
設備Ｗ内の領域ＡＲには、複数の設置領域ＡＲ０が設けられている。設置領域ＡＲ０は、目標物Ｐが設置される領域である。設置領域ＡＲ０は、目標物Ｐを設置すべき領域として、予め設定される。設置領域ＡＲ０は、例えば白線などで区分されており、設置領域ＡＲ０の位置（座標）、形状、及び大きさは、予め設定されている。設置領域ＡＲ０内においては、目標物Ｐは、前面Ｐａが方向Ｘ側を向くように配置されている。目標物Ｐは、方向Ｚから見て前面Ｐａに直交する軸ＰＸが方向Ｘと沿うように、すなわち目標物Ｐの向きが設置領域ＡＲ０に対してずれないように、設置領域ＡＲ内に配置されることが好ましい。しかし、目標物Ｐは、図１に示すように、軸ＰＸが方向Ｘと沿わずに軸ＰＸが方向Ｘから傾斜して、すなわち設置領域ＡＲに対して向きがずれて設置されることがある。例えば、目標物Ｐは、軸ＰＸと方向Ｘとの傾斜角度が、４５度以下となるように、設置領域ＡＲ０に配置されることが好ましい。
【0014】
なお、本実施形態では、設置領域ＡＲ０は、設備Ｗの床である領域ＡＲに設けられているが、それに限られず、例えば目標物Ｐを設備Ｗに搬入した車両の荷台内に設けられてもよい。また、本実施形態では、設置領域ＡＲ０は、目標物Ｐ毎に区画されており、設置領域ＡＲ０には目標物Ｐが１つ配置されるが、それに限られない。例えば、設置領域ＡＲ０は、フリースペースとして、複数の目標物Ｐが設置されるように設定されていてもよい。また、図１の例では設置領域ＡＲ０は矩形であるが、形状及び大きさは任意であってよい。また、領域ＡＲに設けられる設置領域ＡＲ０の数も任意であってよい。
【0015】
移動体１０は、自動で移動可能な装置である。本実施形態では、移動体１０は、フォークリフトであり、さらにいえば、いわゆるＡＧＦ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｆｏｒｋｌｉｆｔ）である。図１に例示すように、移動体１０は、設備Ｗにおける領域ＡＲ上を移動する。移動体１０は、第１パスＲ１（オリジナルパス）に従って、第１位置Ａ１から第２位置Ａ２まで移動しつつ、後述するセンサ２６によって目標物Ｐの位置及び姿勢を検出する。ここでの目標物Ｐの位置とは、方向Ｘ及び方向Ｙに沿った二次元面における座標を指し、目標物Ｐの姿勢とは、方向Ｘ及び方向Ｙに直交する方向から見た場合の、Ｘ方向を０°とした際の軸ＰＸの傾き角度を指す。移動体１０は、第２位置Ａ２に到達したら、目標物Ｐの位置及び姿勢に基づいて設定された第２パスＲ２（グローバルパス）に従って、第２位置Ａ２から目標物Ｐに向けて移動し、途中で、最適化計算により設定された第３パスＲ３（ローカルパス）に切り替えて、第３パスＲ３に沿ってさらに目標物Ｐの近くまでアプローチする。そして、移動体１０は、第３パスＲ３から、フィードバック制御により設定された第４パスＲ４に切り替えて、第４パスＲ４に沿って目標物Ｐに到達して、目標物Ｐをピックアップする。なお、方向Ｘ及び方向Ｙに沿った二次元面における座標系は、領域ＡＲにおける基準点を原点とした領域ＡＲ基準のローカル座標系であってもよいし、地球上の基準点を原点としたグローバル座標系としてもよい。なお、図１に示す第１パスＲ１、第２パスＲ２、第３パスＲ３、及び第４パスＲ４は一例であり、第１パスＲ１、第２パスＲ２、第３パスＲ３、及び第４パスＲ４の詳細については後述する。
【0016】
（移動体）
図２は、移動体の構成の模式図である。図２に示すように、移動体１０は、車体２０と、車輪２０Ａと、ストラドルレッグ２１と、マスト２２と、フォーク２４と、センサ２６と、制御装置２８とを備えている。ストラドルレッグ２６は、車体２０の前後方向における一方の端部に設けられて、車体２０から突出する一対の軸状の部材である。車輪２０Ａは、それぞれのストラドルレッグ２６の先端と、車体２０とに設けられている。すなわち、車輪２０Ａは、合計３個設けられているが、車輪２０Ａの設けられる位置や個数は任意であってよい。マスト２２は、ストラドルレッグ２１に移動可能に取り付けられ、車体２０の前後方向に移動する。マスト２２は、前後方向に直交する上下方向（ここでは方向Ｚ）に沿って延在する。フォーク２４は、マスト２２に方向Ｚに移動可能に取付けられている。フォーク２４は、マスト２２に対して、車体２０の横方向（上下方向及び前後方向に交差する方向）にも移動可能であってよい。フォーク２４は、一対のツメ２４Ａ、２４Ｂを有している。ツメ２４Ａ、２４Ｂは、マスト２２から車体２０の前方向に向けて延在している。ツメ２４Ａとツメ２４Ｂとは、マスト２２の横方向に、互いに離れて配置されている。以下、前後方向のうち、移動体１０においてフォーク２４が設けられている側の方向を、前方向とし、フォーク２４が設けられていない側の方向を、後方向とする。
図２は、移動体の構成の模式図である。図２に示すように、移動体１０は、車体２０と、車輪２０Ａと、ストラドルレッグ２１と、マスト２２と、フォーク２４と、センサ２６と、制御装置２８とを備えている。ストラドルレッグ２６は、車体２０の前後方向における一方の端部に設けられて、車体２０から突出する一対の軸状の部材である。車輪２０Ａは、それぞれのストラドルレッグ２６の先端と、車体２０とに設けられている。すなわち、車輪２０Ａは、合計３個設けられているが、車輪２０Ａの設けられる位置や個数は任意であってよい。マスト２２は、ストラドルレッグ２１に移動可能に取り付けられ、車体２０の前後方向に移動する。マスト２２は、前後方向に直交する上下方向（ここでは方向Ｚ）に沿って延在する。フォーク２４は、マスト２２に方向Ｚに移動可能に取付けられている。フォーク２４は、マスト２２に対して、車体２０の横方向（上下方向及び前後方向に交差する方向）にも移動可能であってよい。フォーク２４は、一対のツメ２４Ａ、２４Ｂを有している。ツメ２４Ａ、２４Ｂは、マスト２２から車体２０の前方向に向けて延在している。ツメ２４Ａとツメ２４Ｂとは、マスト２２の横方向に、互いに離れて配置されている。以下、前後方向のうち、移動体１０においてフォーク２４が設けられている側の方向を、前方向とし、フォーク２４が設けられていない側の方向を、後方向とする。
【0017】
センサ２６は、車体２０の周辺に存在する対象物の位置及び姿勢の少なくとも１つを検出する。センサ２６は、移動体１０に対する対象物の位置と、移動体１０に対する対象物の姿勢とを検出するともいえる。本実施形態では、センサ２６はそれぞれのストラドルレッグ２１の先端と、車体２０の後方向側とに設けられている。ただし、センサ２６の設けられる位置はこれに限られず、任意の位置に設けられてもよいし、設けられる数も任意であってよい。例えば、移動体１０に設けられる安全センサを、センサ２６として流用してもよい。安全センサを流用することで、新たにセンサを設ける必要がなくなる。
【0018】
センサ２６は、例えばレーザ光を照射するセンサである。センサ２６は、一方向（ここでは横方向）に走査しつつレーザ光を照射し、照射したレーザ光の反射光から、対象物の位置及び向きを検出する。すなわち、センサ２６は、いわゆる２Ｄ－ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）であるともいえる。ただし、センサ２６は、以上のものに限られず任意の方法で対象物を検出するセンサであってよく、例えば、複数の方向に走査されるいわゆる３Ｄ－ＬｉＤＡＲであってもよいし、カメラであってもよい。
【0019】
制御装置２８は、移動体１０の移動を制御する。制御装置２８については後述する。
【0020】
（管理システム）
図３は、管理システムの模式的なブロック図である。管理システム１２は、設備Ｗにおける物流を管理するシステムである。管理システム１２は、本実施形態ではＷＭＳ（Ｗａｒｅｈｏｕｓｅ Ｍａｎａｇｅｍｅｎｔ Ｓｙｓｔｅｍ）であるが、ＷＭＳに限られず任意のシステムであってよく、例えば、その他の生産管理系システムのようなバックエンドシステムでも構わない。管理システム１２が設けられる位置は任意であり、設備Ｗ内に設けられてもよいし、設備Ｗから離れた位置に設けられて、離れた位置から設備Ｗを管理するものであってもよい。管理システム１２は、コンピュータであり、図３に示すように、通信部３０と記憶部３２と制御部３４とを含む。
図３は、管理システムの模式的なブロック図である。管理システム１２は、設備Ｗにおける物流を管理するシステムである。管理システム１２は、本実施形態ではＷＭＳ（Ｗａｒｅｈｏｕｓｅ Ｍａｎａｇｅｍｅｎｔ Ｓｙｓｔｅｍ）であるが、ＷＭＳに限られず任意のシステムであってよく、例えば、その他の生産管理系システムのようなバックエンドシステムでも構わない。管理システム１２が設けられる位置は任意であり、設備Ｗ内に設けられてもよいし、設備Ｗから離れた位置に設けられて、離れた位置から設備Ｗを管理するものであってもよい。管理システム１２は、コンピュータであり、図３に示すように、通信部３０と記憶部３２と制御部３４とを含む。
【0021】
通信部３０は、制御部３４に用いられて、情報処理装置１４などの外部の装置と通信するモジュールであり、例えばアンテナなどを含んでよい。通信部３０による通信方式は、本実施形態では無線通信であるが、通信方式は任意であってよい。記憶部３２は、制御部３４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）のような主記憶装置と、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）などの外部記憶装置とのうち、少なくとも１つ含む。
【0022】
制御部３４は、演算装置であり、例えばＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの演算回路を含む。制御部３４は、作業決定部３６を含む。制御部３４は、記憶部３２からプログラム（ソフトウェア）を読み出して実行することで、作業決定部３６を実現して、その処理を実行する。なお、制御部３４は、１つのＣＰＵによって処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、作業決定部３６を、ハードウェア回路で実現してもよい。また、記憶部３２が保存する制御部３４用のプログラムは、管理システム１２が読み取り可能な記録媒体に記憶されていてもよい。
【0023】
作業決定部３６は、搬送する対象となる目標物Ｐを決定する。具体的には、作業決定部３６は、例えば入力された作業計画に基づき、搬送する対象となる目標物Ｐの情報を示す作業内容を決定する。作業内容は、搬送する対象となる目標物Ｐを特定する情報であるともいえる。本実施形態の例では、作業内容は、どの設備にあるどの目標物Ｐを、いつまでに、どこに搬送するかを、作業内容として決定する。すなわち、作業決定部３６は、対象となる目標物Ｐが保管されている設備と、対象となる目標物Ｐと、目標物Ｐの搬送先と、目標物Ｐの搬送時期とを、を示す情報である。作業決定部３６は、決定した作業内容を、通信部３０を介して、情報処理装置１４に送信する。
【0024】
（情報処理装置）
図４は、情報処理装置の模式的なブロック図である。情報処理装置１４は、設備Ｗに設けられ、少なくとも、移動体１０の移動に関する情報などを演算する装置、いわゆる地上システムである。情報処理装置１４は、コンピュータであり、図４に示すように、通信部４０と記憶部４２と制御部４４とを含む。通信部４０は、制御部４４に用いられて、管理システム１２や移動体１０などの外部の装置と通信するモジュールであり、例えばアンテナなどを含んでよい。通信部４０による通信方式は、本実施形態では無線通信であるが、通信方式は任意であってよい。記憶部４２は、制御部４４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭと、ＲＯＭのような主記憶装置と、ＨＤＤなどの外部記憶装置とのうち、少なくとも１つ含む。なお、本実施形態では、管理システム１２と情報処理装置１４とが別の装置であったが、一体の装置であってもよい。すなわち、管理システム１２が情報処理装置１４の少なくとも一部の機能を兼ね備えてよいし、情報処理装置１４が管理システム１２の少なくとも一部の機能を兼ね備えてよい。
図４は、情報処理装置の模式的なブロック図である。情報処理装置１４は、設備Ｗに設けられ、少なくとも、移動体１０の移動に関する情報などを演算する装置、いわゆる地上システムである。情報処理装置１４は、コンピュータであり、図４に示すように、通信部４０と記憶部４２と制御部４４とを含む。通信部４０は、制御部４４に用いられて、管理システム１２や移動体１０などの外部の装置と通信するモジュールであり、例えばアンテナなどを含んでよい。通信部４０による通信方式は、本実施形態では無線通信であるが、通信方式は任意であってよい。記憶部４２は、制御部４４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭと、ＲＯＭのような主記憶装置と、ＨＤＤなどの外部記憶装置とのうち、少なくとも１つ含む。なお、本実施形態では、管理システム１２と情報処理装置１４とが別の装置であったが、一体の装置であってもよい。すなわち、管理システム１２が情報処理装置１４の少なくとも一部の機能を兼ね備えてよいし、情報処理装置１４が管理システム１２の少なくとも一部の機能を兼ね備えてよい。
【0025】
制御部４４は、演算装置であり、例えばＣＰＵなどの演算回路を含む。制御部４４は、作業内容取得部５０と、移動体選定部５２と、第１パス取得部５４とを含む。制御部４４は、記憶部４２からプログラム（ソフトウェア）を読み出して実行することで、作業内容取得部５０と移動体選定部５２と第１パス取得部５４とを実現して、それらの処理を実行する。なお、制御部４４は、１つのＣＰＵによってこれらの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、作業内容取得部５０と移動体選定部５２と第１パス取得部５４との少なくとも一部を、ハードウェア回路で実現してもよい。また、記憶部４２が保存する制御部４４用のプログラムは、情報処理装置１４が読み取り可能な記録媒体に記憶されていてもよい。
【0026】
（作業内容取得部及び移動体選定部）
作業内容取得部５０は、管理システム１２が決定した作業内容の情報、すなわち搬送対象となる目標物Ｐの情報を取得する。作業内容取得部５０は、作業内容における目標物Ｐの情報から、目標物Ｐが設置されている設置領域ＡＲ０を特定する。例えば、記憶部４２には、目標物Ｐと、その目標物Ｐが設置されている設置領域ＡＲ０とが、関連付けて記憶されており、作業内容取得部５０は、記憶部４２からその情報を読み出すことで、設置領域ＡＲ０を特定する。移動体選定部５２は、対象となる移動体１０を選定する。移動体選定部５２は、例えば、設備Ｗに所属する複数の移動体から、対象となる移動体１０を選定する。移動体選定部５２は、任意の方法で対象となる移動体１０を選定してよいが、例えば、作業内容取得部５０が特定した設置領域ＡＲ０に基づき、その設置領域ＡＲ０にある目標物Ｐの搬送に適した移動体１０を、対象となる移動体１０として選定してよい。
作業内容取得部５０は、管理システム１２が決定した作業内容の情報、すなわち搬送対象となる目標物Ｐの情報を取得する。作業内容取得部５０は、作業内容における目標物Ｐの情報から、目標物Ｐが設置されている設置領域ＡＲ０を特定する。例えば、記憶部４２には、目標物Ｐと、その目標物Ｐが設置されている設置領域ＡＲ０とが、関連付けて記憶されており、作業内容取得部５０は、記憶部４２からその情報を読み出すことで、設置領域ＡＲ０を特定する。移動体選定部５２は、対象となる移動体１０を選定する。移動体選定部５２は、例えば、設備Ｗに所属する複数の移動体から、対象となる移動体１０を選定する。移動体選定部５２は、任意の方法で対象となる移動体１０を選定してよいが、例えば、作業内容取得部５０が特定した設置領域ＡＲ０に基づき、その設置領域ＡＲ０にある目標物Ｐの搬送に適した移動体１０を、対象となる移動体１０として選定してよい。
【0027】
（第１パス取得部）
第１パス取得部５４は、作業内容取得部５０が特定した設置領域ＡＲ０までの第１パスＲ１の情報を、取得する。第１パスＲ１は、例えば設置領域ＡＲ０毎に、予め設定されている。第１パス取得部５４は、例えば記憶部４２から、作業内容取得部５０が特定した設置領域ＡＲ０に対して設定された第１パスＲ１を、取得する。以下、第１パスＲ１について具体的に説明する。第１パスＲ１は、方向Ｘ及び方向Ｙに沿った二次元面における座標系を基準として設定されるため、領域ＡＲ基準のローカル座標系における軌道であってもよいし、グローバル座標系における軌道であってもよい。
第１パス取得部５４は、作業内容取得部５０が特定した設置領域ＡＲ０までの第１パスＲ１の情報を、取得する。第１パスＲ１は、例えば設置領域ＡＲ０毎に、予め設定されている。第１パス取得部５４は、例えば記憶部４２から、作業内容取得部５０が特定した設置領域ＡＲ０に対して設定された第１パスＲ１を、取得する。以下、第１パスＲ１について具体的に説明する。第１パスＲ１は、方向Ｘ及び方向Ｙに沿った二次元面における座標系を基準として設定されるため、領域ＡＲ基準のローカル座標系における軌道であってもよいし、グローバル座標系における軌道であってもよい。
【0028】
図１に示すように、第１パスＲ１は、移動体１０が向かう設置領域ＡＲ０（目標物Ｐ）よりも方向Ｘ側で、方向Ｙに沿って設置領域ＡＲ０（目標物Ｐ）を横切る軌道となっている。より詳しくは、第１パスＲ１は、検出軌道Ｒ１ａと、検出軌道Ｒ１ａに接続されるアプローチ軌道Ｒ１ｂとを含む。
【0029】
図１に示すように、検出軌道Ｒ１ａは、設置領域ＡＲ０（目標物Ｐ）よりも方向Ｘ側で、方向Ｙに沿って設置領域ＡＲ０（目標物Ｐ）を横切る軌道である。検出軌道Ｒ１ａは、設置領域ＡＲ０までの方向Ｘにおける距離が、所定距離の範囲内となるように設定されることが好ましい。ここでの所定距離は、検出軌道Ｒ１ａを移動中の移動体１０のセンサ２６によって、設置領域ＡＲ０内の目標物Ｐの位置及び姿勢が検出可能な距離である。より詳しくは、本実施形態においては、検出軌道Ｒ１ａは、第１位置Ａ１から第２位置Ａ２までの軌道である。第１位置Ａ１は、設置領域ＡＲ０よりも方向Ｘ側であり、かつ、設置領域ＡＲ０よりも方向Ｙと反対方向側の位置として設定されている。第２位置Ａ２は、設置領域ＡＲ０よりも方向Ｘ側であり、かつ、設置領域ＡＲ０よりも方向Ｙ側の位置として設定されている。本実施形態においては、第１位置Ａ１と第２位置Ａ２は、設置領域ＡＲ０までの方向Ｘにおける距離が、所定距離の範囲内となるように設定されており、第１位置Ａ１と第２位置Ａ２との方向Ｘにおける位置（Ｘ座標）は、一致している。検出軌道Ｒ１ａは、第１位置Ａ１から第２位置Ａ２までの、方向Ｙに沿った直線軌道として設定される。ただし、第１位置Ａ１と第２位置Ａ２との、方向Ｘにおける位置（Ｘ座標）は、一致していなくてもよい。また、検出軌道Ｒ１ａは、直線軌道であることに限られず、第１位置Ａ１から第２位置Ａ２までで任意の軌跡を描く軌道であってよい。
【0030】
図１に示すように、アプローチ軌道Ｒ１ｂは、第２位置Ａ２から設置領域ＡＲ０に向かう軌道である。より詳しくは、アプローチ軌道Ｒ１ｂは、第２位置Ａ２から、設定位置Ａ３ｚまでの軌道である。設定位置Ａ３ｚは、設置領域ＡＲ０内における目標物Ｐの位置及び姿勢が所定の状態を満たす（目標物Ｐが設置領域ＡＲ０に対してずれ無く理想的に配置されている）と仮定した場合に、目標物Ｐに対して所定の位置及び姿勢となる位置及び姿勢である。すなわち、設定位置Ａ３ｚは、目標物Ｐの位置及び姿勢が所定の状態を満たすと仮定した場合に、移動体１０が目標物Ｐをピックアップ可能な位置及び姿勢であり、目標物Ｐの位置及び姿勢が所定の状態を満たす場合の目標位置Ａ３であるともいえる。図１の例では、アプローチ軌道Ｒ１ｂは、第２位置Ａ２から、第２位置Ａ２よりも方向Ｙと反対側の中間位置ＡＳＢ０までの直線軌道と、中間位置ＡＳＢ０から設定位置Ａ３ｚまでの曲線軌道と、を含む。第２位置Ａ２から中間位置ＡＳＢ０までの直線軌道は、検出軌道Ｒ１ａに重なることが好ましい。
【0031】
なお、図示は省略するが、第１パスＲ１は、移動体１０の移動開始位置から第１位置Ａ１までの軌道も含んでよい。
【0032】
ただし、第１パスＲ１は、以上のような軌道であることに限られない。例えば、第１パスＲ１の検出軌道Ｒ１ａは、方向Ｙに平行な軌道でなくてもよい。また例えば、第１パスＲ１は、アプローチ軌道Ｒ１ｂを含まないものであってよい。すなわち、第１パスＲ１は、少なくとも、第１位置Ａ１から第２位置Ａ２までの検出軌道Ｒ１ａを含むものであってよい。さらにいえば、第１パスＲ１は、検出軌道Ｒ１ａのような設置領域ＡＲ０（目標物Ｐ）を横切る軌道を含むことにも限られず、例えば、設置領域ＡＲ０（目標物Ｐ）の前面に向けて直線的に近づいていく軌道などであってもよい。またさらにいえば、第１パスＲ１は、移動体１０が後方向に直進して設置領域ＡＲ０（目標物Ｐ）を横切り第２位置Ａ２で切り返すことに限られず、移動体１０が前方向に直進して旋回することで設置領域ＡＲ０（目標物Ｐ）の前面に向けて近づいていく軌道などであってもよい。
【0033】
第１パスＲ１は、設備Ｗの地図情報に基づき予め設定される。設備Ｗの地図情報は、設備Ｗに設置されている障害物（柱など）や移動体１０が走行可能な通路などの位置情報を含んだ情報であり、領域ＡＲ内で移動体１０が移動可能な領域を示す情報といえる。また、第１パスＲ１は、設備Ｗの地図情報に加えて、移動体１０の車両仕様の情報にも基づき、設定されてよい。車両仕様の情報とは、例えば、移動体１０の大きさや最小旋回半径など、移動体１０が移動可能な経路に影響を及ぼす仕様である。車両仕様の情報にも基づき第１パスＲ１が設定されている場合、第１パスＲ１は、移動体毎に設定されてよい。なお、第１パスＲ１は、人によって、地図情報や車両仕様の情報などに基づき設定されてもよいし、情報処理装置１４などの装置によって、地図情報や車両仕様の情報などに基づき、自動的に設定されてもよい。自動的に第１パスＲ１を設定する場合、例えば通過して欲しいポイント（Ｗａｙｐｏｉｎｔ）を指定してもよく、この場合、通過して欲しいポイントを通過しつつ、最短、かつ障害物（壁などの固定物）を避けた第１パスＲ１の設定が可能となる。
【0034】
なお、第１パス取得部５４は、予め設定された第１パスＲ１を読み出すことなく、第１パスＲ１を設定してもよい。この場合、第１パス取得部５４は、対象となる移動体１０の位置情報と、設置領域ＡＲ０の位置情報と、設備Ｗの地図情報とに基づき、移動体１０の現在位置から、第１位置Ａ１、第２位置Ａ２を経由して、移動先である設定位置Ａ３ｚまでの経路を、第１パスＲ１として生成してよい。
【0035】
情報処理装置１４は、取得した第１パスＲ１の情報を、通信部４０を介して、対象となる移動体１０に送信する。第１パスＲ１は、設置領域ＡＲ０に向かう経路であるため、移動体１０の移動に関する情報であるといえる。
【0036】
（移動体の制御装置）
次に、移動体１０の制御装置２８について説明する。図５は、移動体の制御装置の模式的なブロック図である。制御装置２８は、移動体１０を制御する。制御装置２８は、コンピュータであり、図５に示すように、通信部６０と記憶部６２と制御部６４とを含む。通信部６０は、制御部６４に用いられて、情報処理装置１４などの外部の装置と通信するモジュールであり、例えばアンテナなどを含んでよい。通信部６０による通信方式は、本実施形態では無線通信であるが、通信方式は任意であってよい。記憶部６２は、制御部６４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭと、ＲＯＭのような主記憶装置と、ＨＤＤなどの外部記憶装置とのうち、少なくとも１つ含む。
次に、移動体１０の制御装置２８について説明する。図５は、移動体の制御装置の模式的なブロック図である。制御装置２８は、移動体１０を制御する。制御装置２８は、コンピュータであり、図５に示すように、通信部６０と記憶部６２と制御部６４とを含む。通信部６０は、制御部６４に用いられて、情報処理装置１４などの外部の装置と通信するモジュールであり、例えばアンテナなどを含んでよい。通信部６０による通信方式は、本実施形態では無線通信であるが、通信方式は任意であってよい。記憶部６２は、制御部６４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭと、ＲＯＭのような主記憶装置と、ＨＤＤなどの外部記憶装置とのうち、少なくとも１つ含む。
【0037】
制御部６４は、演算装置であり、例えばＣＰＵなどの演算回路を含む。制御部６４は、第１パス情報取得部７０と、移動制御部７２と、検出制御部７４と、第２パス情報取得部７６と、第３パス情報取得部７８と、第４パス情報取得部８０とを含む。制御部６４は、記憶部６２からプログラム（ソフトウェア）を読み出して実行することで、第１パス情報取得部７０と移動制御部７２と検出制御部７４と第２パス情報取得部７６と第３パス情報取得部７８と第４パス情報取得部８０とを実現して、それらの処理を実行する。なお、制御部６４は、１つのＣＰＵによってこれらの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、第１パス情報取得部７０と移動制御部７２と検出制御部７４と第２パス情報取得部７６と第３パス情報取得部７８と第４パス情報取得部８０との少なくとも一部を、ハードウェア回路で実現してもよい。また、記憶部６２が保存する制御部６４用のプログラムは、制御装置２８が読み取り可能な記録媒体に記憶されていてもよい。
【0038】
（第１パス情報取得部）
第１パス情報取得部７０は、第１パスＲ１の情報を取得する。第１パス情報取得部７０は、その移動体１０が作業対象として選定された際に、情報処理装置１４から第１パスＲ１の情報を取得してもよいし、予め記憶部６２に記憶された第１パスＲ１の情報を読み出してもよい。
第１パス情報取得部７０は、第１パスＲ１の情報を取得する。第１パス情報取得部７０は、その移動体１０が作業対象として選定された際に、情報処理装置１４から第１パスＲ１の情報を取得してもよいし、予め記憶部６２に記憶された第１パスＲ１の情報を読み出してもよい。
【0039】
（移動制御部）
移動制御部７２は、移動体１０の駆動部やステアリングなどの移動機構を制御して、移動体１０の移動を制御する。移動制御部７２は、第１パス情報取得部７０が取得した第１パスＲ１に従って、移動体１０を移動させる。移動制御部７２は、現在の移動体１０の位置から、第１位置Ａ１を経由して第２位置Ａ２まで、第１パスＲ１を通るように、移動体１０を移動させる。移動制御部７２は、移動体１０の位置情報を逐次把握することで、第１パスＲ１を通るように、移動体１０を移動させる。移動体１０の位置情報の取得方法は任意であるが、例えば本実施形態では、設備Ｗに図示しない検出体が設けられており、移動制御部７２は、検出体の検出に基づき移動体１０の位置及び姿勢の情報を取得する。具体的には、移動体１０は、検出体に向けてレーザ光を照射し、検出体によるレーザ光の反射光を受光して、設備Ｗにおける自身の位置及び姿勢を検出する。ここでの移動体１０の位置とは、設備Ｗの領域ＡＲにおける方向Ｘ及び方向Ｙの二次元座標であり、以下においても、位置とは、別途説明が無い限り、領域ＡＲにおける二次元座標を指す。また、移動体１０の姿勢とは、方向Ｘ及び方向Ｙに直交する方向Ｚから見た場合に、Ｘ方向を０°とした際の移動体１０のヨー角(回転角度)である。また、移動体１０の位置及び姿勢の情報の取得方法は、検出体を用いることに限られず、例えば、ＳＬＡＭ（Ｓｌｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）を用いてもよい。
移動制御部７２は、移動体１０の駆動部やステアリングなどの移動機構を制御して、移動体１０の移動を制御する。移動制御部７２は、第１パス情報取得部７０が取得した第１パスＲ１に従って、移動体１０を移動させる。移動制御部７２は、現在の移動体１０の位置から、第１位置Ａ１を経由して第２位置Ａ２まで、第１パスＲ１を通るように、移動体１０を移動させる。移動制御部７２は、移動体１０の位置情報を逐次把握することで、第１パスＲ１を通るように、移動体１０を移動させる。移動体１０の位置情報の取得方法は任意であるが、例えば本実施形態では、設備Ｗに図示しない検出体が設けられており、移動制御部７２は、検出体の検出に基づき移動体１０の位置及び姿勢の情報を取得する。具体的には、移動体１０は、検出体に向けてレーザ光を照射し、検出体によるレーザ光の反射光を受光して、設備Ｗにおける自身の位置及び姿勢を検出する。ここでの移動体１０の位置とは、設備Ｗの領域ＡＲにおける方向Ｘ及び方向Ｙの二次元座標であり、以下においても、位置とは、別途説明が無い限り、領域ＡＲにおける二次元座標を指す。また、移動体１０の姿勢とは、方向Ｘ及び方向Ｙに直交する方向Ｚから見た場合に、Ｘ方向を０°とした際の移動体１０のヨー角(回転角度)である。また、移動体１０の位置及び姿勢の情報の取得方法は、検出体を用いることに限られず、例えば、ＳＬＡＭ（Ｓｌｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）を用いてもよい。
【0040】
（検出制御部）
検出制御部７４は、センサ２６に、目標物Ｐの位置及び姿勢を検出させ、センサ２６による目標物Ｐの位置及び姿勢の検出結果を取得する。
検出制御部７４は、センサ２６に、目標物Ｐの位置及び姿勢を検出させ、センサ２６による目標物Ｐの位置及び姿勢の検出結果を取得する。
【0041】
（第２パス情報取得部）
第２パス情報取得部７６は、目標物Ｐの位置及び姿勢に基づいて設定される第２パスＲ２の情報を取得する。第２パス情報取得部７６の具体的な処理は後述する。
第２パス情報取得部７６は、目標物Ｐの位置及び姿勢に基づいて設定される第２パスＲ２の情報を取得する。第２パス情報取得部７６の具体的な処理は後述する。
【0042】
（第３パス情報取得部）
第３パス情報取得部７８は、最適化計算により設定される第３パスＲ３の情報を取得する。第３パス情報取得部７８の具体的な処理は後述する。
第３パス情報取得部７８は、最適化計算により設定される第３パスＲ３の情報を取得する。第３パス情報取得部７８の具体的な処理は後述する。
【0043】
（第４パス情報取得部）
第４パス情報取得部８０は、目標物Ｐと移動体１０との相対位置及び相対姿勢の少なくとも一方に基づきフィードバック制御を行うことで設定される第４パスＲ４の情報を取得する。第４パス情報取得部８０の具体的な処理は後述する。
第４パス情報取得部８０は、目標物Ｐと移動体１０との相対位置及び相対姿勢の少なくとも一方に基づきフィードバック制御を行うことで設定される第４パスＲ４の情報を取得する。第４パス情報取得部８０の具体的な処理は後述する。
【0044】
（制御装置の処理）
次に、移動体１０が目標物Ｐに向かう際の制御装置２８の処理について説明する。
次に、移動体１０が目標物Ｐに向かう際の制御装置２８の処理について説明する。
【0045】
（第１パスに沿った移動）
図６は、第１パスを説明する模式図である。図６に示すように、制御装置２８の移動制御部７２は、第１パス情報取得部７０が取得した第１パスＲ１に従って、移動体１０を移動させる。移動制御部７２は、現在の移動体１０の位置から、第１位置Ａ１を経由して第２位置Ａ２まで、第１パスＲ１を通るように、移動体１０を移動させる。第１パスＲ１は、目標物Ｐや移動体１０の位置及び姿勢によらず、予め設定される軌道といえる。
図６は、第１パスを説明する模式図である。図６に示すように、制御装置２８の移動制御部７２は、第１パス情報取得部７０が取得した第１パスＲ１に従って、移動体１０を移動させる。移動制御部７２は、現在の移動体１０の位置から、第１位置Ａ１を経由して第２位置Ａ２まで、第１パスＲ１を通るように、移動体１０を移動させる。第１パスＲ１は、目標物Ｐや移動体１０の位置及び姿勢によらず、予め設定される軌道といえる。
【0046】
（第１パスでの目標物の検出）
検出制御部７４は、図６に示すように、移動体１０が、第１パスＲ１に沿って第１位置Ａ１から第２位置Ａ２までの間を移動中に、すなわち検出軌道Ｒ１ａを移動中に、センサ２６に、目標物Ｐの位置及び姿勢を検出させる。検出制御部７４は、センサ２６に、位置が既知である設置領域ＡＲ０に向けて検出を行わせることで、目標物Ｐを検出させる。
検出制御部７４は、図６に示すように、移動体１０が、第１パスＲ１に沿って第１位置Ａ１から第２位置Ａ２までの間を移動中に、すなわち検出軌道Ｒ１ａを移動中に、センサ２６に、目標物Ｐの位置及び姿勢を検出させる。検出制御部７４は、センサ２６に、位置が既知である設置領域ＡＲ０に向けて検出を行わせることで、目標物Ｐを検出させる。
【0047】
例えばセンサ２６がレーザ光を照射する構成の場合、検出制御部７４は、移動体１０が第１位置Ａ１から第２位置Ａ２までの間を移動中に、センサ２６を横方向（水平方向）に走査させつつ、センサ２６からレーザ光ＬＴを照射させる。移動体１０のＸ方向と反対方向側にある目標物Ｐは、センサ２６からのレーザ光ＬＴを反射する。センサ２６は、目標物Ｐからの反射光を受光する。検出制御部７４は、センサ２６が受光した反射光の検出結果に基づき、計測点の集合である点群を取得する。計測点とは、レーザ光ＬＴが反射された位置（座標）を示す点であり、点群とは、レーザ光ＬＴが反射された位置を示す点の集合を指す。本実施形態では、検出制御部７４は、反射光の検出結果に基づき、反射光が反射された箇所の位置（座標）を、計測点として算出する。検出制御部７４は、各計測点（点群）に基づき、例えばＲＡＮＳＡＣアルゴリズムを用いて直線を抽出し、その直線の位置及び姿勢を、目標物Ｐの位置及び姿勢（目標物Ｐの前面Ｐａの位置及び姿勢）として算出する。例えば、検出制御部７４は、各計測点（点群）のうちで設置領域ＡＲ０に近いＲＯＩ（Ｒｅｇｉｏｎｓ Ｏｆ Ｉｎｔｅｒｅｓｔ）内にある計測点を抽出し、抽出した計測点のうちからＲＡＮＳＡＣアルゴリズムにより選択した２つの計測点を結ぶ直線を、直線候補として取得する。そして、検出制御部７４は、複数の直線候補のうちから、例えば各計測点と直線候補との垂直距離が短い直線候補を、目標物Ｐの前面Ｐａの位置及び姿勢に相当する直線として抽出して、目標物Ｐの位置及び姿勢を算出する。ただし、センサ２６の検出結果に基づく目標物Ｐの位置及び姿勢の算出方法は、任意であってよい。
【0048】
検出制御部７４は、第１位置Ａ１に位置してから第２位置Ａ２に到達するまでの期間全体にわたって、センサ２６に目標物Ｐの位置及び姿勢を検出させてよい。ただし、検出制御部７４は、第１位置Ａ１から第２位置Ａ２までの期間全体にわたってセンサ２６に検出させることに限られず、第１位置Ａ１に位置してから第２位置Ａ２に到達するまでの期間で、少なくとも１回センサ２６に検出させればよく、その期間の間で複数回センサ２６に検出させることがより好ましい。さらに言えば、検出制御部７４は、移動体１０が第１位置Ａ１から第２位置Ａ２までの間を移動中に、センサ２６が目標物Ｐの位置及び姿勢の検出に成功するまで検出を続けさせ、検出に成功したら、検出を停止させてよい。
【0049】
なお、本実施形態においては、移動制御部７２は、第１パスＲ１に沿って、移動開始位置から第１位置Ａ１を経て第２位置Ａ２まで移動体１０を移動させる際に、第１位置Ａ１において停止させることなく移動体１０を移動させる。さらに言えば、移動制御部７２は、第１パスＲ１に沿って移動体１０を移動させる際に、第１位置Ａ１から第２位置Ａ２までの移動体１０の移動速度を、第１位置Ａ１までの移動体１０の移動速度から変化させない。ただし、移動制御部７２は、第１位置Ａ１において移動体１０を一旦停止させてもよい。また、移動制御部７２は、第１位置Ａ１から第２位置Ａ２までの移動体１０の移動速度を、第１位置Ａ１までの移動体１０の移動速度よりも遅くしてもよい。第１位置Ａ１から第２位置Ａ２までの移動体１０の移動速度を遅くすることで、目標物Ｐの位置及び姿勢を適切に検出させることができる。
【0050】
以下、センサ２６が検出した目標物Ｐの位置及び姿勢を示す情報を、適宜、位置姿勢情報と記載する。検出制御部７４は、センサ２６が検出した位置姿勢情報を取得する、といえる。
【0051】
（第２パスの設定）
図７は、第２パスを説明する模式図である。第２パス情報取得部７６は、第１パスＲ１に沿って移動中にセンサ２６が検出した位置姿勢情報に基づき設定された第２パスＲ２の情報を取得する。図７に示すように、第２パスＲ２は、目標物Ｐの位置姿勢情報に基づき設定される、第２位置Ａ２から目標位置Ａ３までの軌道である。目標位置Ａ３は、目標物Ｐに対して所定の位置及び姿勢となる位置及び姿勢である。本実施形態では、目標位置Ａ３は、移動体１０が目標物Ｐをピックアップ可能な位置及び姿勢といえる。なお、第２パスＲ２は、第２位置Ａ２を開始位置とする軌道であることに限られず、例えば、第２位置Ａ２からずれた開始位置から目標位置Ａ３までの軌道であってもよい。この場合、移動体１０は、第２パスＲ２の開始位置まで、予め設定された軌道（例えば第１パスＲ１）に沿って移動してよい。なお、図７に示す第１パスＲ１及び第２パスＲ２は、一例である。第２パスＲ２は、方向Ｘ及び方向Ｙに沿った二次元面における座標系を基準として設定されるため、領域ＡＲ基準のローカル座標系における軌道であってもよいし、グローバル座標系における軌道であってもよい。
図７は、第２パスを説明する模式図である。第２パス情報取得部７６は、第１パスＲ１に沿って移動中にセンサ２６が検出した位置姿勢情報に基づき設定された第２パスＲ２の情報を取得する。図７に示すように、第２パスＲ２は、目標物Ｐの位置姿勢情報に基づき設定される、第２位置Ａ２から目標位置Ａ３までの軌道である。目標位置Ａ３は、目標物Ｐに対して所定の位置及び姿勢となる位置及び姿勢である。本実施形態では、目標位置Ａ３は、移動体１０が目標物Ｐをピックアップ可能な位置及び姿勢といえる。なお、第２パスＲ２は、第２位置Ａ２を開始位置とする軌道であることに限られず、例えば、第２位置Ａ２からずれた開始位置から目標位置Ａ３までの軌道であってもよい。この場合、移動体１０は、第２パスＲ２の開始位置まで、予め設定された軌道（例えば第１パスＲ１）に沿って移動してよい。なお、図７に示す第１パスＲ１及び第２パスＲ２は、一例である。第２パスＲ２は、方向Ｘ及び方向Ｙに沿った二次元面における座標系を基準として設定されるため、領域ＡＲ基準のローカル座標系における軌道であってもよいし、グローバル座標系における軌道であってもよい。
【0052】
本実施形態においては、第２パス情報取得部７６が、すなわち移動体１０自身が、センサ２６が検出した位置姿勢情報に基づき、第２パスＲ２を設定する。第２パス情報取得部７６は、第１パスＲ１に沿った移動の最中にセンサ２６が検出した位置姿勢情報から、目標位置Ａ３を設定する。例えば、第２パス情報取得部７６は、目標物Ｐの位置及び姿勢から、目標物Ｐをピックアップ可能な（直進することでフォーク２４を目標物Ｐの開口Ｐｂに挿入することができる）位置及び姿勢を算出して、目標位置Ａ３として設定する。一例として、開口Ｐｂの入口から、目標物Ｐの開口Ｐｂの軸方向に１０００ｍｍ平行移動した箇所を、目標位置Ａ３としてもよい。そして、第２パス情報取得部７６は、開始位置である第２位置Ａ２から、設定した目標位置Ａ３までの軌道を、第２パスＲ２として設定する。
【0053】
本実施形態では、第２パス情報取得部７６は、目標物Ｐの位置及び姿勢に基づき、直線軌道Ｒ２ａと円弧軌道Ｒ２ｂとを含むように、第２パスＲ２を設定する。直線軌道Ｒ２ａとは、直線状の軌跡を描く軌道である。直線軌道Ｒ２ａは、第２パスＲ２の開始位置である第２位置Ａ２から、中間位置ＡＳＢまでの軌道である。中間位置ＡＳＢは、第１パスＲ１上の第１位置Ａ１と第２位置Ａ２との間に位置する。すなわち、直線軌道Ｒ２ａは、第２位置Ａ２から中間位置ＡＳＢまでの、第１パスＲ１（より詳しくは検出軌道Ｒ１ａ）に重なる軌道であり、進行方向が検出軌道Ｒ１ａとは反対方向となる。円弧軌道Ｒ２ｂは、円弧上の軌跡を描く軌道を含んだ軌道である。円弧軌道Ｒ２ｂにおける円弧状の軌跡の半径を旋回半径ｒとすると、第２パス情報取得部７６は、旋回半径ｒを設定して、設定した旋回半径ｒと目標物Ｐの位置及び姿勢とに基づき、円弧軌道Ｒ２ｂを設定する。旋回半径ｒは、例えば車両仕様などから、任意に設定してよい。
【0054】
円弧軌道Ｒ２ｂは、直線軌道Ｒ２ａに接続される軌道であり、中間位置ＡＳＢから目標位置Ａ３までの軌道である。従って、円弧軌道Ｒ２ｂは、Ｘ方向と反対方向側に、言い換えればＹ方向に直交する方向において設置領域ＡＲ０（目標物Ｐ）に近づく側に、進行する軌道である。図７の例では、円弧軌道Ｒ２ｂは、中間位置ＡＳＢから、中間位置ＡＳＢと目標位置Ａ３との間の中間位置までの円弧状の軌道と、中間位置から目標位置Ａ３までの直線状の軌道とを含む。ただし、円弧軌道Ｒ２ｂは、中間位置ＡＳＢから目標位置Ａ３までの全区間において、円弧状の軌道であってもよい。すなわち、円弧軌道Ｒ２ｂは、円弧状の軌道と直線状の軌道で構成されていてもよいし、円弧状の軌道のみから構成されていてもよい。円弧軌道Ｒ２ｂは、円弧及び直線以外の軌跡を描く軌道（非線形の軌道）を含まないことが好ましい。
【0055】
なお、第２パスＲ２は、以上のように直線軌道Ｒ２ａと円弧軌道Ｒ２ｂを含む軌道であることに限られず、第１パスＲ１に沿って移動中にセンサ２６が検出した位置姿勢情報に基づいて設定された、目標位置Ａ３までの任意の軌道であってよい。また、本実施形態においては、第２パスＲ２は、一度設定された後に更新されることがなく、移動体１０が第２パスＲ２に沿って移動中に、第２パスＲ２が設定し直されないことが好ましい。
【0056】
（第２パスに沿った移動）
本実施形態では、移動制御部７２は、移動体１０が第２位置Ａ２に到着したら、移動体１０の移動を一旦停止させる。そして、第２パス情報取得部７６は、移動体１０が第２位置Ａ２で停止したら、演算を開始して、第２パスＲ２を取得する。移動制御部７２は、第２パスＲ２が取得されたら、使用する経路を第１パスＲ１から第２パスＲ２に切り替えて、第２パスＲ２を通るように、第２位置Ａ２から目標位置Ａ３に向けて、移動体１０を移動させる。移動制御部７２は、移動体１０の位置及び姿勢を逐次検出して、移動体１０の位置及び姿勢が第２パスＲ２に沿うように、すなわち第２パスＲ２に追従するように、移動体１０を移動させる（経路追従制御）。具体的には、移動制御部７２は、第２位置Ａ２で切り返して、第２位置Ａ２から中間位置ＡＳＢまで、第２パスＲ２の直線軌道Ｒ２ａに沿って、第１パスＲ１の検出軌道Ｒ１ａとは反対方向に、すなわち方向Ｙと反対方向に、移動体１０を移動させる。そして、中間位置ＡＳＢに到達したら、移動制御部７２は、直線軌道Ｒ２ａから円弧軌道Ｒ２ｂに切り替えて、すなわち進行方向を変化させて、円弧軌道Ｒ２ｂに沿って、移動体１０を目標位置Ａ３に向けて移動させる。
本実施形態では、移動制御部７２は、移動体１０が第２位置Ａ２に到着したら、移動体１０の移動を一旦停止させる。そして、第２パス情報取得部７６は、移動体１０が第２位置Ａ２で停止したら、演算を開始して、第２パスＲ２を取得する。移動制御部７２は、第２パスＲ２が取得されたら、使用する経路を第１パスＲ１から第２パスＲ２に切り替えて、第２パスＲ２を通るように、第２位置Ａ２から目標位置Ａ３に向けて、移動体１０を移動させる。移動制御部７２は、移動体１０の位置及び姿勢を逐次検出して、移動体１０の位置及び姿勢が第２パスＲ２に沿うように、すなわち第２パスＲ２に追従するように、移動体１０を移動させる（経路追従制御）。具体的には、移動制御部７２は、第２位置Ａ２で切り返して、第２位置Ａ２から中間位置ＡＳＢまで、第２パスＲ２の直線軌道Ｒ２ａに沿って、第１パスＲ１の検出軌道Ｒ１ａとは反対方向に、すなわち方向Ｙと反対方向に、移動体１０を移動させる。そして、中間位置ＡＳＢに到達したら、移動制御部７２は、直線軌道Ｒ２ａから円弧軌道Ｒ２ｂに切り替えて、すなわち進行方向を変化させて、円弧軌道Ｒ２ｂに沿って、移動体１０を目標位置Ａ３に向けて移動させる。
【0057】
なお、移動制御部７２は、第２位置Ａ２で移動体１０の移動を停止させることに限られない。この場合例えば、第２パス情報取得部７６は、目標物Ｐの位置姿勢情報が取得されたら、第１位置Ａ１から第２位置Ａ２までの移動中に、すなわち第２位置Ａ２への到着前に、第２パスＲ２を取得しておく。そして、移動制御部７２は、移動体１０が第１パスＲ１を通って第２位置Ａ２に到着したら、第２位置Ａ２で停止させることなく（進行方向の切替に伴い必要となる停止時間を除く）、第１パスＲ１から第２パスＲ２に切り替えて、第２位置Ａ２から、第２パスＲ２を通って移動体１０を移動させる。
【0058】
なお、目標物Ｐの位置及び姿勢によっては、第２パスＲ２に切り替えず、引き続き第１パスＲ１を用いて目標物Ｐにアプローチしてもよい。この場合、第２パス情報取得部７６は、センサ２６が検出した目標物Ｐの位置姿勢情報に基づき、第２パスＲ２の設定が必要かを判断する。第２パス情報取得部７６は、目標物Ｐの位置及び姿勢が、所定の範囲内にある場合に、第２パスＲ２の設定が必要ではないと判断し、所定の範囲外にある場合に、第２パスＲ２の設定が必要であると判断する。所定の範囲内とは、設置領域ＡＲ０に対する目標物Ｐの位置及び姿勢のずれが少ないことを指し、目標物Ｐの位置及び姿勢が所定の範囲内にある場合には、第１パスＲ１のアプローチ軌道Ｒ１ｂ（図１参照）を用いて、目標物Ｐをピックアップ可能な目標位置Ａ３まで到達可能となる。言い換えれば、第２パス情報取得部７６は、目標物Ｐの位置姿勢情報に基づき、アプローチ軌道Ｒ１ｂの到達位置である設定位置Ａ３ｚに向かえば、目標物Ｐのピックアップが可能かを判断する。第２パス情報取得部７６は、目標物Ｐの位置及び姿勢が所定の範囲内にある場合に、設定位置Ａ３ｚから目標物Ｐをピックアップ可能として、第２パスＲ２の設定が不要と判断する。一方、第２パス情報取得部７６は、目標物Ｐの位置及び姿勢が所定の範囲外にある場合に、設定位置Ａ３ｚから目標物Ｐをピックアップできないとして、第２パスＲ２の設定が必要と判断する。なお、ここでの所定の範囲は、車両仕様の情報などから、任意の方法で設定されてよく、予め算出しておいてよい。
【0059】
第２パス情報取得部７６が第２パスＲ２の設定が不要と判断した場合には、移動制御部７２は、引き続き第１パスＲ１を使用し、第１パスＲ１のアプローチ軌道Ｒ１ｂを通るように、第２位置Ａ２から設定位置Ａ３ｚに向けて移動体１０を移動させる。一方、第２パス情報取得部７６が第２パスＲ２の設定が必要と判断した場合には、移動制御部７２は、上述のように、第２パスＲ２に切り替えて、目標位置Ａ３に向けて移動体１０を移動させる。
【0060】
（第３パスの設定）
図８は、第３パスを説明する模式図である。上述のように、第２パスＲ２は、目標物Ｐをピックアップ可能な目標位置Ａ３までの経路であるため、移動体１０は、第２パスＲ２に沿って移動することで目標位置Ａ３まで到達して、目標物Ｐをピックアップできる。しかし、移動体１０が外乱の影響などで第２パスＲ２から外れたり、第２パスＲ２の設定に用いたセンサ２６の検出結果に誤差があったりするなどの理由により、移動体１０が目標物Ｐをピックアップ可能な位置に到達できないおそれがある。それに対し、本実施形態の移動体１０は、図８に示すように、第２パスＲ２に沿って移動中に、センサ２６で目標物Ｐを適切に検出できる程度まで目標物Ｐに近づいたら、第２パスＲ２から第３パスＲ３に切り替えて、第３パスＲ３に沿って目標物Ｐにアプローチしてゆくことで、目標物Ｐに高精度にアプローチすることが可能となる。以下、第３パスＲ３について説明する。
図８は、第３パスを説明する模式図である。上述のように、第２パスＲ２は、目標物Ｐをピックアップ可能な目標位置Ａ３までの経路であるため、移動体１０は、第２パスＲ２に沿って移動することで目標位置Ａ３まで到達して、目標物Ｐをピックアップできる。しかし、移動体１０が外乱の影響などで第２パスＲ２から外れたり、第２パスＲ２の設定に用いたセンサ２６の検出結果に誤差があったりするなどの理由により、移動体１０が目標物Ｐをピックアップ可能な位置に到達できないおそれがある。それに対し、本実施形態の移動体１０は、図８に示すように、第２パスＲ２に沿って移動中に、センサ２６で目標物Ｐを適切に検出できる程度まで目標物Ｐに近づいたら、第２パスＲ２から第３パスＲ３に切り替えて、第３パスＲ３に沿って目標物Ｐにアプローチしてゆくことで、目標物Ｐに高精度にアプローチすることが可能となる。以下、第３パスＲ３について説明する。
【0061】
本実施形態においては、第３パス情報取得部７８は、第２パスＲ２に沿って移動中の移動体１０が、センサ２６によって目標物Ｐを連続して適切に検出可能な検出可能位置Ａ４に到達したら、第３パスＲ３を設定する。すなわち、検出制御部７４は、移動体１０が第２パスＲ２に沿って移動中にも、センサ２６に目標物Ｐを検出させる。そして、第３パス情報取得部７８は、検出可能位置Ａ４における、センサ２６による目標物Ｐの位置及び姿勢の検出結果を取得する。また、第３パス情報取得部７８は、移動体１０の位置及び姿勢の検出結果も取得する。そして、第３パス情報取得部７８は、目標物Ｐの位置と移動体１０の位置との偏差が小さくなるほど評価が高くなる評価関数に基づき最適化計算を行うことで、第３パスＲ３を設定する。すなわち、第３パス情報取得部７８は、移動体１０の現在位置から目標物Ｐに近づく経路を最適化計算で算出して、第３パスＲ３とする。第３パスＲ３に切り替える位置である検出可能位置Ａ４は、任意の基準で設定してよい。
【0062】
なお、第３パスＲ３の設定の際に目標物Ｐを検出するセンサ２６は、第２パスＲ２の設定の際に用いたセンサ２６と同じであるが、それに限られず、第２パスＲ２の設定の際に用いたセンサ２６と異なるセンサであってもよい。
【0063】
（第３パスに沿った移動）
移動制御部７２は、第２パスＲ２に沿って移動する移動体１０が、検出可能位置Ａ４に到達し、第３パスＲ３が設定されたら、使用する経路を第２パスＲ２から第３パスＲ３に切り替えて、第３パスＲ３に沿って移動体１０を移動させる。移動制御部７２は、第３パスＲ３に沿って移動体１０を移動させる場合には、第２パスＲ２のように経路追従制御を行わない。第３パス情報取得部７８は、所定周期毎に、センサ２６による目標物Ｐの位置及び姿勢の検出結果と、移動体１０の位置及び姿勢の検出結果を取得して、第３パスＲ３を更新する。移動制御部７２は、第３パスＲ３が更新される毎に、更新された第３パスＲ３に切り替えて、更新された第３パスＲ３に沿って移動体１０を移動させる。このように第３パスＲ３を逐次更新することで、経路追従制御が不要となる。
移動制御部７２は、第２パスＲ２に沿って移動する移動体１０が、検出可能位置Ａ４に到達し、第３パスＲ３が設定されたら、使用する経路を第２パスＲ２から第３パスＲ３に切り替えて、第３パスＲ３に沿って移動体１０を移動させる。移動制御部７２は、第３パスＲ３に沿って移動体１０を移動させる場合には、第２パスＲ２のように経路追従制御を行わない。第３パス情報取得部７８は、所定周期毎に、センサ２６による目標物Ｐの位置及び姿勢の検出結果と、移動体１０の位置及び姿勢の検出結果を取得して、第３パスＲ３を更新する。移動制御部７２は、第３パスＲ３が更新される毎に、更新された第３パスＲ３に切り替えて、更新された第３パスＲ３に沿って移動体１０を移動させる。このように第３パスＲ３を逐次更新することで、経路追従制御が不要となる。
【0064】
なお、第２パスＲ２を用いずに、第１パスＲ１に沿って移動を継続していた場合には、移動制御部７２は、検出可能位置Ａ４に到達したら、第１パスＲ１から第３パスＲ３に切り替える。この場合の第３パスＲ３への切替方法や第３パスＲ３の設定方法は、上述の第２パスＲ２から第３パスＲ３に切り替える場合と同様である。
【0065】
また、第２パスＲ２（又は第１パスＲ１）から第３パスＲ３への切り替えは必須ではない。例えば、第２パスＲ２（又は第１パスＲ１）に沿って移動中に、検出可能位置Ａ４に到達に到達したと判定されないが、後述の切替位置Ａ５に到達した場合には、第２パスＲ２（又は第１パスＲ１）から第４パスＲ４に切り替えてよい。このように第３パスＲ３に切り替えない場合でも、検出可能位置Ａ４に到達したかの判断は行われるため、第２パスＲ２（又は第１パスＲ１）から第３パスＲ３へ切り替えるかの判断自体は実施される。
【0066】
（第４パスの設定）
図９は、第４パスを説明する模式図である。図９に示すように、第４パス情報取得部８０は、第３パスＲ３に沿って移動中の移動体１０が、目標物Ｐに十分近い位置である切替位置Ａ５に到達したら、第４パスＲ４を設定する。より詳しくは、移動体１０が切替位置Ａ５に到達したら、検出制御部７４は、センサ２６に目標物Ｐの位置及び姿勢を検出させる。切替位置Ａ５は、任意の基準で設定されてよい。第４パス情報取得部８０は、目標物Ｐの位置及び姿勢の少なくとも一方の検出結果と、移動体１０の位置及び姿勢の少なくとも一方の検出結果とに基づき、フィードバック制御（直接フィードバック制御）を行うことで、移動体１０を目標物Ｐにアプローチさせる第４パスＲ４を設定する。すなわち、第４パス情報取得部８０は、目標物Ｐと移動体１０との相対位置と、目標物Ｐと移動体１０との相対姿勢との少なくとも一方に基づき、フィードバック制御を行うことで、第４パスＲ４を設定する。本実施形態では、第４パス情報取得部８０は、目標物Ｐと移動体１０との相対位置と、目標物Ｐと移動体１０との相対姿勢との両方に基づき、フィードバック制御を行うことで、第４パスＲ４を設定する。すなわち、第４パスＲ４は、移動体１０及び目標物Ｐの位置及び姿勢を逐次検出して、移動体１０の位置及び姿勢が目標物Ｐの位置及び姿勢と合致するように、移動体１０の位置及び姿勢がフィードバック制御される経路といえる。
図９は、第４パスを説明する模式図である。図９に示すように、第４パス情報取得部８０は、第３パスＲ３に沿って移動中の移動体１０が、目標物Ｐに十分近い位置である切替位置Ａ５に到達したら、第４パスＲ４を設定する。より詳しくは、移動体１０が切替位置Ａ５に到達したら、検出制御部７４は、センサ２６に目標物Ｐの位置及び姿勢を検出させる。切替位置Ａ５は、任意の基準で設定されてよい。第４パス情報取得部８０は、目標物Ｐの位置及び姿勢の少なくとも一方の検出結果と、移動体１０の位置及び姿勢の少なくとも一方の検出結果とに基づき、フィードバック制御（直接フィードバック制御）を行うことで、移動体１０を目標物Ｐにアプローチさせる第４パスＲ４を設定する。すなわち、第４パス情報取得部８０は、目標物Ｐと移動体１０との相対位置と、目標物Ｐと移動体１０との相対姿勢との少なくとも一方に基づき、フィードバック制御を行うことで、第４パスＲ４を設定する。本実施形態では、第４パス情報取得部８０は、目標物Ｐと移動体１０との相対位置と、目標物Ｐと移動体１０との相対姿勢との両方に基づき、フィードバック制御を行うことで、第４パスＲ４を設定する。すなわち、第４パスＲ４は、移動体１０及び目標物Ｐの位置及び姿勢を逐次検出して、移動体１０の位置及び姿勢が目標物Ｐの位置及び姿勢と合致するように、移動体１０の位置及び姿勢がフィードバック制御される経路といえる。
【0067】
なお、第４パスＲ４の設定の際に目標物Ｐを検出するセンサ２６は、第２パスＲ２や第３パスＲ３の設定の際に用いたセンサ２６と同じであるが、それに限られず、第２パスＲ２や第３パスＲ３の設定の際に用いたセンサ２６と異なるセンサであってもよい。
【0068】
（第４パスに沿った移動）
移動制御部７２は、第３パスＲ３に沿って移動する移動体１０が切替位置Ａ５に到達し、第４パスＲ４が設定されたら、使用する経路を第３パスＲ３から第４パスＲ４に切り替えて、第４パスＲ４に沿って移動体１０を移動させる。第４パス情報取得部８０は、所定周期毎に、センサ２６による目標物Ｐの位置及び姿勢の検出結果と、移動体１０の位置及び姿勢の検出結果を取得して、第４パスＲ４を更新する。移動制御部７２は、第４パスＲ４が更新される毎に、更新された第４パスＲ４に切り替えて、更新された第４パスＲ４に沿って移動体１０を移動させる。移動体１０は、第４パスＲ４に沿って移動して、目標物Ｐに到達して開口Ｐｂにフォーク２４を挿入し、目標物Ｐをピックアップする。移動制御部７２は、目標物Ｐをピックアップした移動体１０を、設定された搬送先まで搬送させる。
移動制御部７２は、第３パスＲ３に沿って移動する移動体１０が切替位置Ａ５に到達し、第４パスＲ４が設定されたら、使用する経路を第３パスＲ３から第４パスＲ４に切り替えて、第４パスＲ４に沿って移動体１０を移動させる。第４パス情報取得部８０は、所定周期毎に、センサ２６による目標物Ｐの位置及び姿勢の検出結果と、移動体１０の位置及び姿勢の検出結果を取得して、第４パスＲ４を更新する。移動制御部７２は、第４パスＲ４が更新される毎に、更新された第４パスＲ４に切り替えて、更新された第４パスＲ４に沿って移動体１０を移動させる。移動体１０は、第４パスＲ４に沿って移動して、目標物Ｐに到達して開口Ｐｂにフォーク２４を挿入し、目標物Ｐをピックアップする。移動制御部７２は、目標物Ｐをピックアップした移動体１０を、設定された搬送先まで搬送させる。
【0069】
なお、第３パスＲ３を用いずに、第２パスＲ２（又は第１パスＲ１）に沿って移動中に切替位置Ａ５に到達した場合には、移動制御部７２は、第２パスＲ２（又は第１パスＲ１）から第４パスＲ４に切り替える。この場合の第４パスＲ４への切り替え方法や第３パスＲ３の設定方法は、上述の第３パスＲ３から第４パスＲ４に切り替える場合と同様である。
【0070】
また、第４パスＲ４への切り替えは必須でなく、移動制御部７２は、第１パスＲ１、第２パスＲ２、及び第３パスＲ３の少なくとも１つを用いて、移動体１０を移動させてよい。
【0071】
（移動制御フロー）
以上説明した移動体１０の移動制御のフローを、フローチャートに基づき説明する。図１０は、第１実施形態に係る移動体の移動制御フローを説明するフローチャートである。図１０に示すように、移動体１０の制御装置２８は、第１パス情報取得部７０により、第１パスＲ１の情報を取得して、移動制御部７２により、第１パスＲ１に従って、移動体１０を第１位置Ａ１まで移動させる（ステップＳ１０）。制御装置２８は、移動制御部７２により、第１位置Ａ１から第２位置Ａ２まで第１パスＲ１（検出軌道Ｒ１ａ）に従って、移動体１０を移動させつつ、検出制御部７４により、センサ２６に目標物Ｐの位置姿勢情報の検出を実行させる（ステップＳ１２）。そして、制御装置２８は、第２パス情報取得部７６により、第１パスＲ１に従って移動中の目標物Ｐの位置姿勢情報の検出結果に基づき、第２パスＲ２を設定する（ステップＳ１４）。制御装置２８は、移動制御部７２により、第１パスＲ１から第２パスＲ２に切り替えて、第２パスＲ２に従って移動体１０を移動させる（ステップＳ１６）。
以上説明した移動体１０の移動制御のフローを、フローチャートに基づき説明する。図１０は、第１実施形態に係る移動体の移動制御フローを説明するフローチャートである。図１０に示すように、移動体１０の制御装置２８は、第１パス情報取得部７０により、第１パスＲ１の情報を取得して、移動制御部７２により、第１パスＲ１に従って、移動体１０を第１位置Ａ１まで移動させる（ステップＳ１０）。制御装置２８は、移動制御部７２により、第１位置Ａ１から第２位置Ａ２まで第１パスＲ１（検出軌道Ｒ１ａ）に従って、移動体１０を移動させつつ、検出制御部７４により、センサ２６に目標物Ｐの位置姿勢情報の検出を実行させる（ステップＳ１２）。そして、制御装置２８は、第２パス情報取得部７６により、第１パスＲ１に従って移動中の目標物Ｐの位置姿勢情報の検出結果に基づき、第２パスＲ２を設定する（ステップＳ１４）。制御装置２８は、移動制御部７２により、第１パスＲ１から第２パスＲ２に切り替えて、第２パスＲ２に従って移動体１０を移動させる（ステップＳ１６）。
【0072】
移動体１０が第２パスＲ２に従って移動中に、センサ２６で目標物Ｐを適切に検出可能な検出可能位置Ａ４に到達したら（ステップＳ１８；Ｙｅｓ）、制御装置２８は、第３パス情報取得部７８により、最適化計算により第３パスＲ３を設定し、移動制御部７２により、第２パスＲ２から第３パスＲ３に切り替えて、第３パスＲ３に従って移動体１０を移動させる（ステップＳ２０）。制御装置２８は、第３パスＲ３を逐次更新しつつ、第３パスＲ３に従って移動体１０を移動させる。なお、移動体１０が第２パスＲ２に従って移動中に、検出可能位置Ａ４に到達していない場合には（ステップＳ１８；Ｎｏ）、ステップＳ１８に戻り、検出可能位置Ａ４に到達するまで第２パスＲ２に従った移動を続ける。
【0073】
移動体１０が第３パスＲ３に従って移動中に、切替位置Ａ５に到達したら（ステップＳ２２；Ｙｅｓ）、制御装置２８は、第４パス情報取得部８０により、フィードバック制御により第４パスＲ４を設定し、移動制御部７２により、第３パスＲ３から第４パスＲ４に切り替えて、第４パスＲ４に従って移動体１０を移動させる（ステップＳ２４）。制御装置２８は、第４パスＲ４を逐次更新しつつ、第４パスＲ４に従って移動体１０を移動させることで、移動体１０を目標物Ｐまで到達させる。なお、移動体１０が第３パスＲ３に従って移動中に、切替位置Ａ５に到達していない場合には（ステップＳ２２；Ｎｏ）、ステップＳ２２に戻り、切替位置Ａ５に到達するまで第３パスＲ３に従った移動を続ける。
【0074】
（効果）
以上説明したように、本実施形態に係る移動体１０は、第１パスＲ１、第２パスＲ２、第３パスＲ３、第４パスＲ４に移動経路を切り替えつつ、目標物Ｐにアプローチすることで、目標物Ｐ（目標位置）まで適切にアプローチすることができる。より具体的には、第１パスＲ１に沿って移動中に検出した目標物Ｐの位置姿勢情報に基づき設定した第２パスＲ２を用いることで、例えば目標物Ｐの位置や姿勢が設置領域ＡＲ０からずれていても、実際の目標物Ｐの位置及び姿勢に合わせて、目標物Ｐに適切にアプローチできる。さらに、移動体１０が外乱の影響などで第２パスＲ２から外れたり、第２パスＲ２の設定に用いたセンサ２６の検出結果に誤差があったりした場合でも、最適化計算を用いた第３パスＲ３に切り替えることで、目標物Ｐに適切にアプローチできる。さらに、目標物Ｐにより近接した切替位置Ａ５で、サーボ方式の第４パスＲ４に切り替えることで、目標物Ｐに到達する直前に微調整しつつ、目標物Ｐに近づくことができる。
以上説明したように、本実施形態に係る移動体１０は、第１パスＲ１、第２パスＲ２、第３パスＲ３、第４パスＲ４に移動経路を切り替えつつ、目標物Ｐにアプローチすることで、目標物Ｐ（目標位置）まで適切にアプローチすることができる。より具体的には、第１パスＲ１に沿って移動中に検出した目標物Ｐの位置姿勢情報に基づき設定した第２パスＲ２を用いることで、例えば目標物Ｐの位置や姿勢が設置領域ＡＲ０からずれていても、実際の目標物Ｐの位置及び姿勢に合わせて、目標物Ｐに適切にアプローチできる。さらに、移動体１０が外乱の影響などで第２パスＲ２から外れたり、第２パスＲ２の設定に用いたセンサ２６の検出結果に誤差があったりした場合でも、最適化計算を用いた第３パスＲ３に切り替えることで、目標物Ｐに適切にアプローチできる。さらに、目標物Ｐにより近接した切替位置Ａ５で、サーボ方式の第４パスＲ４に切り替えることで、目標物Ｐに到達する直前に微調整しつつ、目標物Ｐに近づくことができる。
【0075】
なお、以上の説明では、第１パスＲ１を情報処理装置１４が設定し、第２パスＲ２、第３パスＲ３、及び第４パスＲ４を移動体１０の制御装置２８が設定していたが、各パスの設定主体は、それらに限られず任意であってよい。例えば、第１パスＲ１を移動体１０の制御装置２８が設定してもよい。また例えば、第２パスＲ２、第３パスＲ３、及び第４パスＲ４の少なくとも１つを、制御装置２８以外の装置（例えば情報処理装置１４）が設定し、制御装置２８が、その装置から第２パスＲ２、第３パスＲ３、及び第４パスＲ４の少なくとも１つ情報を取得してもよい。
【0076】
また、以上の説明では、移動体１０は、第２位置Ａ２まで、フォーク２４が設けられていない後方向を進行方向前方として移動し、第２位置Ａ２から切り返して、フォーク２４が設けられている前方向を進行方向前方として、目標物Ｐにアプローチしていたが、移動体１０の移動時の向きはそれに限られない。移動体１０は、第２位置Ａ２までにおいても、フォーク２４が設けられている前方向を進行方向前方として移動してもよい。
【0077】
（第２パスの他の例）
次に、第２パスＲ２の他の例について説明する。本例では、第２パスＲ２を、目標物Ｐとは反対方向（Ｘ方向側）に膨らんで切り返しさせる必要があるかを判断する。以下、具体的に説明する。
次に、第２パスＲ２の他の例について説明する。本例では、第２パスＲ２を、目標物Ｐとは反対方向（Ｘ方向側）に膨らんで切り返しさせる必要があるかを判断する。以下、具体的に説明する。
【0078】
図１１は、本例に係る第２パスの設定フローを示すフローチャートである。図１２は、単円弧パスを第２パスとして設定する場合を説明する模式図である。以降の説明においては、図１２に示すように、目標物Ｐの位置を、位置Ｐａ１とする。そして、位置Ｐａ１の方向Ｘ、方向Ｙにおける座標を、それぞれＸｒｅｆ、Ｙｒｅｆとし、位置Ｐａ１における目標物Ｐの姿勢（回転角度）を、θｒｅｆとする。［Ｘｒｅｆ、Ｙｒｅｆ、θｒｅｆ］は、センサ２６によって検出された目標物Ｐの位置及び姿勢に相当する。位置Ｐａ１は、図１２の例では目標物Ｐの前面Ｐａの中央位置であるが、位置Ｐａ１は、前面Ｐａの中央位置に限られず、目標物Ｐの任意の箇所の位置であってよい。また、図１２に示すように、第２パスＲ２の終点である目標位置Ａ３の方向Ｘ、方向Ｙにおける座標を、それぞれＸＴＧ、ＹＴＧとし、目標位置Ａ３において移動体１０のとる姿勢（回転角度）を、θＴＧとする。［ＸＴＧ、ＹＴＧ、θＴＧ］は、目標物Ｐの位置及び姿勢に相当する［Ｘｒｅｆ、Ｙｒｅｆ、θｒｅｆ］に基づき算出されて、［ＸＴＧ、ＹＴＧ、θＴＧ］に基づいて第２パスＲ２が設定される。以下、第２パスＲ２の設定フローを説明する。
【0079】
図１１に示すように、本例においては、第２パス情報取得部７６は、第２パスＲ２を設定する際に、目標物Ｐの位置姿勢情報に基づき、仮目標位置Ａ３ａの座標を算出する（ステップＳ３０）。仮目標位置Ａ３ａは、図１２に示すように、目標物Ｐの位置Ｐａ１から、目標物Ｐの前面Ｐａの向いている方向に沿って所定の距離ｄｍｉｎ離れた位置である。距離ｄｍｉｎは、予め設定されており、目標物Ｐをピックアップするための最後の直線軌道をどの程度残すかに応じて決定される。仮目標位置Ａ３ａの方向Ｘ、方向Ｙにおける座標を、それぞれＸＴＧａ、ＹＴＧａとすると、ＸＴＧａ、ＹＴＧａは、次の式（１）、（２）に基づき算出される。
【0080】
ＸＴＧａ＝Ｘｒｅｆ＋ｄｍｉｎ・ｃｏｓθｒｅｆ ・・・（１）
ＹＴＧａ＝Ｙｒｅｆ＋ｄｍｉｎ・ｓｉｎθｒｅｆ ・・・（２）
ＹＴＧａ＝Ｙｒｅｆ＋ｄｍｉｎ・ｓｉｎθｒｅｆ ・・・（２）
【0081】
そして、図１１に示すように、第２パス情報取得部７６は、仮目標位置Ａ３ａの座標と目標物Ｐの位置姿勢情報とに基づき、仮円弧軌道中心Ｃａの座標を算出する（ステップＳ３２）。仮円弧軌道中心Ｃａは、図１２に示すように、仮目標位置Ａ３ａや目標物Ｐの位置Ｐａ１よりも第２位置Ａ２側（ここではＹ方向側）に位置しており、かつ、半径が旋回半径ｒとなり周が仮目標位置Ａ３ａに接する円の中心を指す。仮円弧軌道中心Ｃａの方向Ｘ、方向Ｙにおける座標を、それぞれＣＸａ、ＣＹａとすると、ＣＸａ、ＣＹａは、次の式（３）、（４）に基づき算出される。
【0082】
ＣＸａ＝ＸＴＧａ－ｒ・ｓｉｎθｒｅｆ ・・・（３）
ＣＹａ＝ＹＴＧａ－ｒ・ｃｏｓθｒｅｆ ・・・（４）
ＣＹａ＝ＹＴＧａ－ｒ・ｃｏｓθｒｅｆ ・・・（４）
【0083】
そして、図１１に示すように、第２パス情報取得部７６は、目標物Ｐの位置姿勢情報に基づき、第２パスＲ２において、目標位置Ａ３（設置領域ＡＲ０）と反対側に向かう軌道が必要か、すなわちＸ方向側に向かう軌道が必要かを、判断する（ステップＳ３４）。言い換えれば、第２パス情報取得部７６は、目標物Ｐに到達するために、目標物Ｐとは反対方向（Ｘ方向側）に膨らんで切り返しさせる必要があるかを判断する。本実施形態では、第２パス情報取得部７６は、第１パスＲ１の検出軌道Ｒ１ａのＸ方向における座標ＸＳＢ１（図１２参照）と、仮円弧軌道中心ＣａのＸ方向における座標ＣＸａと、旋回半径ｒとに基づき、目標位置Ａ３と反対側に向かう軌道が必要かを判断する。具体的には、第２パス情報取得部７６は、次の式（５）を満たす場合に、目標位置Ａ３と反対側に向かう軌道が必要と判断し、次の式（５）を満たさない場合に、目標位置Ａ３と反対側に向かう軌道が不要と判断する。
【0084】
ＸＳＢ１＜ＣＸａ＋ｒ ・・・（５）
【0085】
すなわち、仮円弧軌道中心Ｃａの座標ＣＸａから旋回半径ｒの長さ分方向Ｘに移動した位置が、検出軌道Ｒ１ａの座標ＸＳＢ１よりもＸ方向側にある場合に、式（５）を満たし、検出軌道Ｒ１ａの座標ＸＳＢ１よりもＸ方向側にない場合（Ｘ方向において座標ＸＳＢ１と同じ位置、又は座標ＸＳＢ１よりＸ方向と反対側にある場合）、式（５）を満たさないと判断する。なお、図１２に示すように、直線軌道Ｒ２ａから円弧軌道Ｒ２ｂに切り替わる中間位置ＡＳＢ１は、検出軌道Ｒ１ａ上にあるため、座標ＸＳＢ１は、中間位置ＡＳＢ１のＸ方向における座標であるともいえる。座標ＸＳＢ１は、第１パスＲ１の情報として、第１パス情報取得部７０によって取得される。
【0086】
（単円弧パスの設定）
図１１に示すように、目標位置Ａ３と反対側に向かう軌道が不要と判断した場合（ステップＳ３４：Ｎｏ）、第２パス情報取得部７６は、円弧軌道Ｒ２ｂが１つである単円弧パスを第２パスＲ２として設定する。この場合、第２パス情報取得部７６は、目標物Ｐの位置姿勢情報と旋回半径ｒとに基づき、中間位置ＡＳＢ１の座標を算出する（ステップＳ３６）。中間位置ＡＳＢ１のＸ方向における座標ＸＳＢ１は上述のように既知であり、中間位置ＡＳＢ１における移動体１０の姿勢θＳＢ１も、移動体１０が検出軌道Ｒ１ａと重なる直線軌道Ｒ２ａに沿って移動するために、既知である。そのためここでは、第２パス情報取得部７６は、中間位置ＡＳＢ１の方向Ｙにおける座標ＹＳＢ１を算出する。本実施形態では、第２パス情報取得部７６は、仮円弧軌道中心Ｃａの座標と、座標ＸＳＢ１と、旋回半径ｒとに基づき、中間位置ＡＳＢ１の座標ＹＳＢ１を算出する。具体的には、第２パス情報取得部７６は、次の式（６）を用いて中間位置ＡＳＢ１の座標ＹＳＢ１を算出する。
図１１に示すように、目標位置Ａ３と反対側に向かう軌道が不要と判断した場合（ステップＳ３４：Ｎｏ）、第２パス情報取得部７６は、円弧軌道Ｒ２ｂが１つである単円弧パスを第２パスＲ２として設定する。この場合、第２パス情報取得部７６は、目標物Ｐの位置姿勢情報と旋回半径ｒとに基づき、中間位置ＡＳＢ１の座標を算出する（ステップＳ３６）。中間位置ＡＳＢ１のＸ方向における座標ＸＳＢ１は上述のように既知であり、中間位置ＡＳＢ１における移動体１０の姿勢θＳＢ１も、移動体１０が検出軌道Ｒ１ａと重なる直線軌道Ｒ２ａに沿って移動するために、既知である。そのためここでは、第２パス情報取得部７６は、中間位置ＡＳＢ１の方向Ｙにおける座標ＹＳＢ１を算出する。本実施形態では、第２パス情報取得部７６は、仮円弧軌道中心Ｃａの座標と、座標ＸＳＢ１と、旋回半径ｒとに基づき、中間位置ＡＳＢ１の座標ＹＳＢ１を算出する。具体的には、第２パス情報取得部７６は、次の式（６）を用いて中間位置ＡＳＢ１の座標ＹＳＢ１を算出する。
【0087】
ＹＳＢ１＝ＣＹａ＋（ＸＳＢ１－ＣＸａ－ｒ）・ｓｉｎθｒｅｆ ・・・（６）
【0088】
そして、図１１に示すように、第２パス情報取得部７６は、中間位置ＡＳＢ１の座標と旋回半径ｒとに基づき、円弧軌道中心Ｃ１の座標を算出する（ステップＳ３８）。円弧軌道中心Ｃ１は、図１２に示すように、設定する第２パスＲ２（単円弧パス）の円弧軌道Ｒ２ｂの、円弧を描く軌跡の中心を指す。円弧軌道中心Ｃ１の方向Ｘ、方向Ｙにおける座標を、それぞれＣ１Ｘ、Ｃ１Ｙとすると、Ｃ１Ｘ、Ｃ１Ｙは、次の式（７）、（８）に基づき算出される。
【0089】
Ｃ１Ｘ＝ＸＳＢ１－ｒ ・・・（７）
Ｃ１Ｙ＝ＹＳＢ１ ・・・（８）
Ｃ１Ｙ＝ＹＳＢ１ ・・・（８）
【0090】
そして、図１１に示すように、第２パス情報取得部７６は、円弧軌道中心Ｃ１の座標と目標物Ｐの位置姿勢情報とに基づき、目標位置Ａ３の位置及び姿勢を算出する（ステップＳ４０）。第２パス情報取得部７６は、次の式（９）から式（１１）を用いて、目標位置Ａ３の位置及び姿勢である［ＸＴＧ、ＹＴＧ、θＴＧ］を算出する。
【0091】
ＸＴＧ＝Ｃ１Ｘ＋ｒ・ｃｏｓθｒｅｆ ・・・（９）
ＹＴＧ＝Ｃ１Ｙ＋ｒ・ｓｉｎθｒｅｆ ・・・（１０）
θＴＧ＝θｒｅｆ ・・・（１１）
ＹＴＧ＝Ｃ１Ｙ＋ｒ・ｓｉｎθｒｅｆ ・・・（１０）
θＴＧ＝θｒｅｆ ・・・（１１）
【0092】
図１１に示すように、第２パス情報取得部７６は、目標位置Ａ３の位置及び姿勢である［ＸＴＧ、ＹＴＧ、θＴＧ］に基づき、第２パスＲ２を算出する（ステップＳ４２）。すなわち、第２パス情報取得部７６は、第２位置Ａ２の座標と中間位置ＡＳＢ１の座標とから、第２位置Ａ２から中間位置ＡＳＢ１までの直線軌道Ｒ２ａを設定し、中間位置ＡＳＢ１の位置姿勢及び目標位置Ａ３の位置姿勢と、旋回半径ｒとから、中間位置ＡＳＢ１から目標位置Ａ３までの円弧軌道Ｒ２ｂを設定する。このように、単円弧パスとしての第２パスＲ２は、直線軌道Ｒ２ａと、直線軌道Ｒ２ａに接続されて設置領域ＡＲ０側（すなわちＸ方向と反対方向側）に向かう円弧軌道Ｒ２ｂとで構成される。
【0093】
以上が、Ｘ方向側に膨らむ軌道を有さない単円弧パスを第２パスＲ２とする場合の、設定方法である。単円弧パスを第２パスＲ２とした場合、移動体１０は、第２位置Ａ２から中間位置ＡＳＢ１までを直線軌道Ｒ２ａに沿って移動し、中間位置ＡＳＢ１において、直線軌道Ｒ２ａから円弧軌道Ｒ２ｂに切り替えて、すなわち進行方向を変化させて、円弧軌道Ｒ２ｂに沿って目標位置Ａ３に向けて移動する。
【0094】
（複円弧パスの設定）
一方、図１１に示すように、目標位置Ａ３と反対側に向かう軌道が必要と判断された場合（ステップＳ３４：Ｙｅｓ）、第２パス情報取得部７６は、円弧軌道Ｒ２ｂが複数（本例では２つ）である複円弧パスを第２パスＲ２として設定する。図１３は、複円弧パスを第２パスとして設定する場合を説明する模式図である。この場合、第２パス情報取得部７６は、旋回半径ｒ及び目標物Ｐの位置姿勢情報に基づき、円弧軌道中心Ｃ２の座標を算出する（ステップＳ４４）。円弧軌道中心Ｃ２は、目標位置Ａ３と反対側に向かう円弧軌道Ｒ２ｂ１の軌跡の中心を指す。より詳しくは、図１３に示すように、第２パス情報取得部７６は、仮円弧軌道中心Ｃａを円弧軌道中心Ｃ１として取り扱い、円弧軌道中心Ｃ１の座標Ｃ１Ｘ、Ｃ１Ｙを、ＣＸａ、ＣＹａとして設定する。そして、第２パス情報取得部７６は、円弧軌道中心Ｃ２を中心とする旋回半径ｒとなる円が、円弧軌道中心Ｃ１を中心とする旋回半径ｒとなる円と１点で接するように、円弧軌道中心Ｃ２の座標を算出する。具体的には、円弧軌道中心Ｃ２の方向Ｘ、方向Ｙにおける座標を、それぞれＣ２Ｘ、Ｃ２Ｙとすると、第２パス情報取得部７６は、円弧軌道中心Ｃ１とこれから算出する円弧軌道中心Ｃ２との間の方向Ｘに沿った距離ｗを、次の式（１２）を用いて算出する。
一方、図１１に示すように、目標位置Ａ３と反対側に向かう軌道が必要と判断された場合（ステップＳ３４：Ｙｅｓ）、第２パス情報取得部７６は、円弧軌道Ｒ２ｂが複数（本例では２つ）である複円弧パスを第２パスＲ２として設定する。図１３は、複円弧パスを第２パスとして設定する場合を説明する模式図である。この場合、第２パス情報取得部７６は、旋回半径ｒ及び目標物Ｐの位置姿勢情報に基づき、円弧軌道中心Ｃ２の座標を算出する（ステップＳ４４）。円弧軌道中心Ｃ２は、目標位置Ａ３と反対側に向かう円弧軌道Ｒ２ｂ１の軌跡の中心を指す。より詳しくは、図１３に示すように、第２パス情報取得部７６は、仮円弧軌道中心Ｃａを円弧軌道中心Ｃ１として取り扱い、円弧軌道中心Ｃ１の座標Ｃ１Ｘ、Ｃ１Ｙを、ＣＸａ、ＣＹａとして設定する。そして、第２パス情報取得部７６は、円弧軌道中心Ｃ２を中心とする旋回半径ｒとなる円が、円弧軌道中心Ｃ１を中心とする旋回半径ｒとなる円と１点で接するように、円弧軌道中心Ｃ２の座標を算出する。具体的には、円弧軌道中心Ｃ２の方向Ｘ、方向Ｙにおける座標を、それぞれＣ２Ｘ、Ｃ２Ｙとすると、第２パス情報取得部７６は、円弧軌道中心Ｃ１とこれから算出する円弧軌道中心Ｃ２との間の方向Ｘに沿った距離ｗを、次の式（１２）を用いて算出する。
【0095】
ｗ＝√｛（２ｒ）２－（ｒ＋ＸＳＢ１－Ｃｘ）２｝ ・・・（１２）
【0096】
そして、第２パス情報取得部７６は、次の式（１３）、（１４）に基づき、円弧軌道中心Ｃ２の座標Ｃ２Ｘ、Ｃ２Ｙを算出する。
【0097】
Ｃ２Ｘ＝ＸＳＢ１＋ｒ ・・・（１３）
Ｃ２Ｙ＝Ｃ１Ｙ－ｗ ・・・（１４）
Ｃ２Ｙ＝Ｃ１Ｙ－ｗ ・・・（１４）
【0098】
次に、図１１に示すように、第２パス情報取得部７６は、中間位置ＡＳＢ１の位置及び姿勢を算出する（ステップＳ４６）。図１３に示すように、複円弧パスにおける中間位置ＡＳＢ１は、直線軌道Ｒ２ａから、円弧軌道中心Ｃ２を中心とする円弧軌道Ｒ２ｂ１に切り替わる位置である。中間位置ＡＳＢ１のＸ方向における座標ＸＳＢ１及び姿勢θＳＢ１は既知なので、第２パス情報取得部７６は、中間位置ＡＳＢ１の方向Ｙにおける座標ＹＳＢ１を算出する。第２パス情報取得部７６は、円弧軌道中心Ｃ２の座標に基づき、中間位置ＡＳＢ１の座標ＹＳＢ１を算出する。具体的には、第２パス情報取得部７６は、次の式（１５）に示すように、円弧軌道中心Ｃ２の座標Ｃ２Ｙを、中間位置ＡＳＢ１の座標ＹＳＢ１を算出する。
【0099】
ＹＳＢ１＝Ｃ２Ｙ ・・・（１５）
【0100】
次に、図１１に示すように、第２パス情報取得部７６は、中間位置ＡＳＢ２の位置及び姿勢を算出する（ステップＳ４８）。図１３に示すように、複円弧パスにおける中間位置ＡＳＢ２は、円弧軌道中心Ｃ２を中心とする円弧軌道Ｒ２ｂ１から、円弧軌道中心Ｃ１を中心とする円弧軌道Ｒ２ｂ２に切り替わる位置である。中間位置ＡＳＢ２は、検出軌道Ｒ１ａよりも設置領域ＡＲ０と反対側（すなわちＸ方向側）に位置する。第２パス情報取得部７６は、円弧軌道中心Ｃ１の座標及び円弧軌道中心Ｃ２の座標に基づき、中間位置ＡＳＢ２の位置及び姿勢を算出する。中間位置ＡＳＢ２の方向Ｘ、方向Ｙにおける座標を、それぞれＸＳＢ２、ＹＳＢ２とし、中間位置ＡＳＢ２における移動体１０の姿勢（回転角度）を、θＳＢ２とすると、第２パス情報取得部７６は、次の式（１６）から式（１８）を用いて、中間位置ＡＳＢ２の位置及び姿勢を算出する。
【0101】
ＸＳＢ２＝（Ｃ１Ｘ＋Ｃ２Ｘ）／２ ・・・（１６）
ＹＳＢ２＝（Ｃ１Ｙ＋Ｃ２Ｙ）／２ ・・・（１７）
θＳＢ２＝ｔａｎ－１｛（Ｃ２Ｘ－Ｃ１Ｘ）／（Ｃ２Ｙ－Ｃ１Ｙ）｝＋π／２ ・・・（１８）
ＹＳＢ２＝（Ｃ１Ｙ＋Ｃ２Ｙ）／２ ・・・（１７）
θＳＢ２＝ｔａｎ－１｛（Ｃ２Ｘ－Ｃ１Ｘ）／（Ｃ２Ｙ－Ｃ１Ｙ）｝＋π／２ ・・・（１８）
【0102】
そして、図１１に示すように、第２パス情報取得部７６は、仮目標位置Ａ３ａの座標と目標物Ｐの位置姿勢情報とに基づき、目標位置Ａ３の位置及び姿勢を算出する（ステップＳ５０）。第２パス情報取得部７６は、次の式（１９）から式（２１）を用いて、目標位置Ａ３の位置及び姿勢である［ＸＴＧ、ＹＴＧ、θＴＧ］を算出する。
【0103】
ＸＴＧ＝ＸＴＧａ ・・・（１９）
ＹＴＧ＝ＹＴＧａ ・・・（２０）
θＴＧ＝θｒｅｆ ・・・（２１）
ＹＴＧ＝ＹＴＧａ ・・・（２０）
θＴＧ＝θｒｅｆ ・・・（２１）
【0104】
すなわち、第２パス情報取得部７６は、仮目標位置Ａ３ａを目標位置Ａ３として設定する。
【0105】
そして、図１１に示すように、第２パス情報取得部７６は、中間位置ＡＳＢ１、ＡＳＢ２の位置姿勢と、目標位置Ａ３の位置姿勢とに基づき、第２パスＲ２を算出する（ステップＳ４２）。図１３に示すように、第２パス情報取得部７６は、第２位置Ａ２の座標と中間位置ＡＳＢ１の座標とから、第２位置Ａ２から中間位置ＡＳＢ１までの直線軌道Ｒ２ａを設定し、中間位置ＡＳＢ１の位置姿勢及び中間位置ＡＳＢ２の位置姿勢と、旋回半径ｒとから、中間位置ＡＳＢ１から中間位置ＡＳＢ２までの円弧軌道Ｒ２ｂ１を設定し、中間位置ＡＳＢ２の位置姿勢及び目標位置Ａ３の位置姿勢と、旋回半径ｒとから、中間位置ＡＳＢ２から目標位置Ａ３までの円弧軌道Ｒ２ｂ２を設定する。このように、複円弧パスとしての第２パスＲ２は、直線軌道Ｒ２ａと、直線軌道Ｒ２ａに接続されて設置領域ＡＲ０側と反対側（すなわちＸ方向側）に向かう円弧軌道Ｒ２ｂ１（第１円弧軌道）と、円弧軌道Ｒ２ｂ１に接続されて設置領域ＡＲ０側（すなわちＸ方向と反対側）に向かう円弧軌道Ｒ２ｂ２（第２円弧軌道）とで構成される。
【0106】
以上が、Ｘ方向側に膨らむ軌道（円弧軌道Ｒ２ｂ１）を有する複円弧パスを第２パスＲ２とする場合の、設定方法である。複円弧パスを第２パスＲ２とした場合、移動体１０は、第２位置Ａ２から中間位置ＡＳＢ１までを直線軌道Ｒ２ａに沿って移動し、中間位置ＡＳＢ１において、直線軌道Ｒ２ａから円弧軌道Ｒ２ｂ１に切り替えて、すなわち切り返して、円弧軌道Ｒ２ｂ１に沿って中間位置ＡＳＢ２まで移動する。移動体１０は、中間位置ＡＳＢ２において、円弧軌道Ｒ２ｂ１から円弧軌道Ｒ２ｂ２に切り替えて、すなわち進行方向を変化させて、円弧軌道Ｒ２ｂ２に沿って目標位置Ａ３に向けて移動する。
【0107】
以上説明したように、本例においては、Ｘ方向側に膨らむ軌道が必要かを判断して、必要でない場合には、Ｘ方向側に膨らむ軌道を含まない単円弧パスを設定し、必要である場合には複円弧パスを設定する。そのため、本例においては、目標物Ｐの位置姿勢に応じて、例えばＸ方向側への切り返しが可能な、適切な第２パスＲ２を設定することが可能となる。さらに、直線軌道Ｒ２ａと円弧軌道Ｒ２ｂとの組み合わせだけで第２パスＲ２を設定することで、演算負荷を抑えて、高速に第２パスＲ２を生成することが可能である。なお、円弧軌道Ｒ２ｂの旋回半径ｒと移動体１０の舵角指令は一対一で対応しているため、外乱が一切無い場合に円弧軌道Ｒ２ｂ上を移動できる移動体１０の舵角指令も、同時に求めることができる。舵角が一定である場合には、旋回半径ｒは移動速度に依存しないため、スリップなどが発生しない範囲で任意の移動速度を設定してもよい。
【0108】
目標物Ｐに適切にアプローチするには、目標物Ｐから一旦遠ざかり、切り返した経路とする方が効率的な場合がある。しかし、第３パスＲ３は、最適化計算で設定されるため、目標物Ｐから一旦遠ざかって切り返す経路とすることができない。それに対し、第３パスＲ３の前に第２パスＲ２を設けることで、切り返しを含む効率的な経路を設定することができる。
【0109】
（第２実施形態）
次に、第２実施形態について説明する。第２実施形態では、第２パスＲ２から第３パスＲ３に切り替える方法の具体例を説明する。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。第２実施形態は、第１実施形態、及び第１実施形態の他の例にも適用可能である。
次に、第２実施形態について説明する。第２実施形態では、第２パスＲ２から第３パスＲ３に切り替える方法の具体例を説明する。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。第２実施形態は、第１実施形態、及び第１実施形態の他の例にも適用可能である。
【0110】
第２実施形態においては、第３パス情報取得部７８は、移動体１０が第２パスＲ２に沿って移動中における、センサ２６による目標物Ｐの検出精度と、移動体１０の進行方向と、移動体１０と目標物Ｐとの間の距離との、少なくとも１つに基づき、第２パスＲ２から第３パスＲ３に切り替えるかを判断する。以下、具体的にする。
【0111】
図１４は、第２パスから第３パスへの切り替えを判断する手順を説明するフローチャートである。検出制御部７４は、移動体１０が第２パスＲ２に沿って移動中に、センサ２６に目標物Ｐを逐次検出させる。図１４に示すように、検出制御部７４は、センサ２６による目標物Ｐの検出結果についての信頼度を算出する（ステップＳ５０）。信頼度とは、目標物Ｐを適切に検出できたかを示す指標であり、言い換えれば検出精度についての信頼度といえる。検出制御部７４は、センサ２６による１回の検出（１回のスキャン）毎に、信頼度を算出する。すなわち、検出制御部７４は、センサ２６による同じ検出（同じスキャン）で取得された検出結果（点群）について、同じ信頼度を割り当てる。
【0112】
検出制御部７４は、任意の方法で目標物Ｐの検出結果の信頼度を算出してよいが、信頼度の算出方法の一例を以下で説明する。例えば、検出制御部７４は、センサ２６による１回の検出で取得すべき理想の計測点の数に対する、センサ２６による１回の検出で取得された実際の計測点の数の比率に基づき、その検出での検出結果の信頼度を算出する。理想の計測点の数は、任意の方法で算出してよいが、例えば、検出した際の移動体１０と、検出制御部７４が点群に基づき算出した直線（目標物Ｐの前面Ｐａに対応する直線）との位置関係に基づき、算出してよい。すなわち、直線を目標物Ｐの前面Ｐａとして扱えば、検出した際の移動体１０の位置と、目標物Ｐの前面Ｐａ(直線)の位置と、センサ２６の性能（例えば角度分解能）とから、目標物Ｐの前面Ｐａで観測されるはずの計測点Ｍの個数を算出できる。本例では、検出制御部７４は、検出した際の移動体１０の位置と、目標物Ｐの前面Ｐａ(直線)の位置と、センサ２６の角度分解能とから、目標物Ｐの前面Ｐａで観測されるはずの計測点Ｍの個数を、理想の計測点Ｍの数として算出する。なお、角度分解能とは、２つの対象点を分離して検知できる角度を示している。
【0113】
そして、検出制御部７４は、理想の測定点の数の合計値に対する、実際の測定点の数の合計値の比率を、その検出での検出結果の信頼度として算出する。なお、前面Ｐａから離れた位置の計測点を除外して、残った計測点の数を実際の計測点の数としてもよい。
【0114】
以上のように、検出制御部７４は、移動体１０が第２パスＲ２に沿って移動中に、センサ２６に目標物Ｐを逐次検出させつつ、目標物Ｐの検出結果についての信頼度を算出する。そして、第３パス情報取得部７８は、目標物Ｐの検出結果の信頼度が所定値以上であるかを判断する。より詳しくは、第３パス情報取得部７８は、直近で検出された目標物Ｐの検出結果を所定数抽出し、抽出した直近の所定数の検出結果のうちで、信頼度が閾値以上となるものが規定数以上であるかを判断する（ステップＳ５２）。ここでの所定数、閾値、規定数は任意に設定してよいが、例えば、所定数が１０回、閾値が７０％、規定数が９個（９０％）とすると、第３パス情報取得部７８は、直近の１０回の検出結果のうちで、信頼度が７０％以上となるものが９個ある場合には、信頼度が閾値以上となるものが規定数以上であると判断する。
【0115】
信頼度が閾値以上となるものが規定数以上ない場合（ステップＳ５２；Ｎｏ）、第３パスＲ３には切り替えず、ステップＳ５０に戻り、第２パスＲ２に従った移動と目標物Ｐの検出を続ける。このステップＳ５２の処理は、検出可能位置Ａ４に到達したかを判断する処理の一例といえる。ステップＳ５２のように信頼度が閾値以上となるものが規定数以上あるかを判断することで、センサ２６によって目標物Ｐを連続して適切に検出可能な位置に到達したかを、適切に判断できる。すなわち、信頼度が閾値以上となるものが規定数以上ない場合には、目標物Ｐを連続して適切に検出できないため、逐次更新される第３パスＲ３を適切に設定できない場合がある。そのような場合には第３パスＲ３に切り替えないことで、第３パスＲ３の設定不良を抑制できる。
【0116】
一方、信頼度が閾値以上となるものが規定数以上ある場合（ステップＳ５２；Ｙｅｓ）、第３パス情報取得部７８は、移動体１０が目標物Ｐに近づいているかを判断する（ステップＳ５４）。すなわち、第３パス情報取得部７８は、移動体１０が、目標物Ｐから離れる切り返し経路（例えば円弧軌道Ｒ２ｂ１）に従って移動しているかを判断する。例えば、第３パス情報取得部７８は、移動体１０の現在の進行方向に基づき、移動体１０が、フォーク２４が設けられていない後方向を進行方向前方として移動しているかを判断する。第３パス情報取得部７８は、移動体１０が後方向を進行方向前方として移動している場合に、移動体１０が目標物Ｐに近づいていない（すなわち切り返し経路を移動している）と判断し、移動体１０がフォーク２４側の前方向を進行方向前方として移動している場合に、移動体１０が目標物Ｐに近づいている（すなわち切り返し経路を移動していない）と判断する。
【0117】
第３パス情報取得部７８は、移動体１０が目標物Ｐに近づいていないと判断した場合（ステップＳ５４；Ｎｏ）、第３パスＲ３には切り替えず、ステップＳ５０に戻り、第２パスＲ２に従った移動と目標物Ｐの検出を続ける。一方、第３パス情報取得部７８は、移動体１０が目標物Ｐに近づいていると判断した場合（ステップＳ５４；Ｙｅｓ）、移動体１０と目標物Ｐとの距離が所定距離以下であるかを判断する（ステップＳ５６）。第３パス情報取得部７８は、移動体１０と目標物Ｐとの距離が所定距離以下である場合には（ステップＳ５６；Ｙｅｓ）、第３パスＲ３に切り替えると判断して（ステップＳ５８）、第３パスＲ３を設定して、第３パスＲ３に経路を切り替える。一方、移動体１０と目標物Ｐとの距離が所定距離以下でない場合には（ステップＳ５６；Ｎｏ）、第３パスＲ３には切り替えず、ステップＳ５０に戻り、第２パスＲ２に従った移動と目標物Ｐの検出を続ける。このように、移動体１０が、切り返し経路を移動しており目標物Ｐに近づいていない場合には、移動体１０がまだ目標物Ｐと正対していないため、目標物Ｐを連続して適切に検出できなくなるおそれがある。そのため、以上のような判定基準で第３パスＲ３に切り替えるかを判断することで、目標物Ｐを連続して適切に検出できなくなることを抑制して、第３パスＲ３の設定不良を抑制できる。
【0118】
（第３実施形態）
次に、第３実施形態について説明する。第３実施形態では、第３パスＲ３の設定方法の具体例を説明する。第３実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。第３実施形態は、第１実施形態、第１実施形態の他の例、及び第２実施形態にも適用可能である。
次に、第３実施形態について説明する。第３実施形態では、第３パスＲ３の設定方法の具体例を説明する。第３実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。第３実施形態は、第１実施形態、第１実施形態の他の例、及び第２実施形態にも適用可能である。
【0119】
図１５は、第３パスを説明するための模式図である。第３パス情報取得部７８は、最適化計算を実行して、最適化された移動体１０の第３パスＲ３を算出して、算出した第３パスＲ３を実現できる移動体１０の駆動条件を算出する。駆動条件とは、移動体１０の動力部を動作させるための入力値を指し、ここでは移動体１０の速度と操舵角とを指す。本実施形態では、第３パス情報取得部７８は、先読みステップ毎の移動体１０の位置及び姿勢を設定することで、第３パスＲ３を設定する。先読みステップとは、現在（移動体１０や目標物Ｐの位置及び姿勢が直近で検出されたタイミング）以降の、離散化した各時刻を意味する。第３パス情報取得部７８は、先読みステップ毎の移動体１０の位置と目標物Ｐとの偏差が小さくなるほど評価が高くなる評価関数に基づき、最適化計算を行い、最適化計算で最適化された第３パスＲ３を算出して、その第３パスＲ３を実現できる移動体の駆動条件を、算出する。以下、計算実行部５６による最適化計算を具体的に説明する。なお、以降では、二次元座標平面上での移動体１０の運動を例として説明するが、三次元座標上での運動モデルに対しても適用可能である。
【0120】
（先読みステップ毎の移動体の位置）
ここで、第３パス情報取得部７８によって算出される最適化された第３パスＲ３は、先読みステップ毎の（現在以降の離散時刻毎の）移動体１０の位置及び姿勢の集合といえ、未来の経路であるといえる。第３パス情報取得部７８は、図１５に示すように、検出された移動体１０の位置及び姿勢を、目標物Ｐの座標系（ｘ方向及びｙ方向の２次元座標系）における位置及び姿勢に座標変換する。すなわち、ここでは、自己位置姿勢（移動体１０の位置姿勢）と目標位置姿勢（目標物Ｐの位置姿勢）をインプットとするが、最適化問題の中で経路を生成する際には、自己位置姿勢を目標位置姿勢基準で座標変換して扱うことにより、自己位置姿勢を起点に作成した経路の座標系が目標位置姿勢基準となる。本手順は、方向Ｘ及び方向Ｙに沿った二次元面における座標系（領域ＡＲ基準のローカル座標系又はグローバル座標系）で自己位置姿勢および目標位置姿勢を得ている場合も、移動体１０のセンサ座標系基準のローカルな目標位置姿勢を得ている場合も、同様に適用可能である。
ここで、第３パス情報取得部７８によって算出される最適化された第３パスＲ３は、先読みステップ毎の（現在以降の離散時刻毎の）移動体１０の位置及び姿勢の集合といえ、未来の経路であるといえる。第３パス情報取得部７８は、図１５に示すように、検出された移動体１０の位置及び姿勢を、目標物Ｐの座標系（ｘ方向及びｙ方向の２次元座標系）における位置及び姿勢に座標変換する。すなわち、ここでは、自己位置姿勢（移動体１０の位置姿勢）と目標位置姿勢（目標物Ｐの位置姿勢）をインプットとするが、最適化問題の中で経路を生成する際には、自己位置姿勢を目標位置姿勢基準で座標変換して扱うことにより、自己位置姿勢を起点に作成した経路の座標系が目標位置姿勢基準となる。本手順は、方向Ｘ及び方向Ｙに沿った二次元面における座標系（領域ＡＲ基準のローカル座標系又はグローバル座標系）で自己位置姿勢および目標位置姿勢を得ている場合も、移動体１０のセンサ座標系基準のローカルな目標位置姿勢を得ている場合も、同様に適用可能である。
【0121】
ここで、移動体１０のｘ方向及びｙ方向での位置（座標）を［ｘ，ｙ］Ｔ、移動体１０の姿勢をθ、先読みステップ（離散時刻）をｋとすると、先読みステップｋでの移動体１０の位置及び姿勢は、すなわち先読みステップｋでの第３パスＲ３は、次の式（２２）で表される。なお、Ｔは転置を示す。なお、上述のように、本実施形態の説明は、二次元平面座標上での運動を例としたものであり、例えば三次元座標上での運動モデルとする場合には、以降の式などは三次元座標に合わせたものとなる。
【0122】
【数22】
【0123】
この場合、移動体１０の運動モデルは、次の式（２３）のように表される。
【0124】
【数23】
【0125】
式（２３）のΔｔは、（ｋ）から（ｋ＋１）への遷移に係る時間幅である。Δｔは、第３パスＲ３の更新周期（第３パスＲ３を更新してから次に第３パスＲ３を更新するまでの時間幅）と同じであってもよいが、例えば、更新周期よりも短くてもよいし、更新周期よりも長くてもよい。Δｔを更新周期より短くすることで、詳細な第３パスＲ３を生成でき、Δｔを更新周期より長くすることで、計算負荷を低減できる。また、ｕ（ｋ）は、システムの入力であり、移動体１０の駆動条件を指す。本実施形態の例では、ｕ（ｋ）は、移動体１０の速度ｖ（ｋ）、移動体１０の曲率ｃ（ｋ）を用いて、次の式（２４）のように表される。
【0126】
【数24】
【0127】
曲率とは、現在の操舵角で移動体１０が移動したと仮定した場合の、移動体１０が描く円の半径（曲率半径）の逆数を指す。例えば、先読みステップｋにおける曲率ｃ（ｋ）は、次の式（２５）のように定義される。Φ（ｋ）は、先読みステップｋにおける移動体１０の操舵角を指し、Ｌは、ホイールベースを指し、例えば前輪と後輪との間の距離である。
【0128】
【数25】
【0129】
二次元平面座標上での運動を想定した場合、システムの状態方程式であるｆは、一例として、次の式（２６）のように表される。
【0130】
【数26】
【0131】
第３パス情報取得部７８は、モデル予測制御の考え方を用い、移動経路を予測（先読）し、各先読みステップでの移動体１０と目標物Ｐの関係において、目標位置との偏差が小さいほど、評価が高くなる評価関数を用いて、移動体１０の各予測ステップの位置に関する移動体１０と目的位置の偏差を評価して、第３パスＲ３を特定する。これにより、第３パス情報取得部７８は、最適な第３パスＲ３を、すなわち先読みステップ毎の最適な移動体１０の位置及び姿勢を、特定できる。言い換えれば、第３パス情報取得部７８は、先読みステップ毎の移動体１０の位置と目的物Ｐの位置との偏差が最小となるような、先読みステップ毎の最適な移動体１０の位置及び姿勢（すなわち最適な第３パスＲ３）を、特定する。
【0132】
本実施形態では、第３パスＲ３を最適化する問題Ｖ（ｕ，ｐ，ｋ）が、次の式（２７）で表されると想定する。
【0133】
【数27】
【0134】
ここで、Ｊ（ｕ，ｐ，ｋ）は、最適化の目的を表す評価関数であり、経路計画であれば目的位置への接近や到達が含まれるような関数に設定される。本実施形態の例では、第３パス情報取得部７８は、評価関数Ｊ（ｕ，ｐ，ｋ）を、次の式（２８）のように設定する。
【0135】
【数28】
【0136】
ここで、ｐＴ＝［ｘ（ｋ），ｙ（ｋ）］Ｔは、すなわち式（２８）でのｘＴ、ｙＴは、目的となる位置（目標物Ｐの位置）である。この評価関数の元では、移動体１０の各先読みステップｋにおける位置（ｐ（ｋ））と目標物Ｐの位置（ｐＴ）との偏差が小さくなるように最適化される。すなわち、移動体１０が目的の位置に最短距離で近づくような最適化結果となる。
【0137】
また、Ａ１、Ａ２は、ｘ座標及びｙ座標の誤差を最小化する重み係数であり任意に設定されてよい。例えば目標物Ｐ（目標位置姿勢）に対して移動体１０の向きを正対させることを重視する場合には、Ａ１よりＡ２を大きくすることで、ｙ座標の重みをｘ座標の重みより大きくする。すなわち、ｙ座標において目標物Ｐと移動体１０との偏差を小さくすることを、ｘ座標において目標物Ｐと移動体１０との偏差を小さくすることよりも優先させる。これにより、第３パスＲ３は、ｘ軸に沿うように生成され、結果として移動体１０の向きを目標位置姿勢に早期に正対させることが可能となる。
【0138】
また、本実施形態においては、評価関数に姿勢（θ）が含まれず、移動体１０と目標物Ｐとの姿勢の偏差を最小にする計算が含まれない。このように評価関数に姿勢を含めないことで、最適化問題を単純化することができ、計算負荷を低減できる。なお、評価関数に姿勢を含めなくても、移動体１０と目標物Ｐとの位置の偏差が小さくなるに従って、姿勢の偏差も小さくなる。
【0139】
以上のような最適化計算を実行することで第３パス情報取得部７８は、最適化された第３パスＲ３を設定し、最適化計算のアウトプット（最適解）として、先読みステップ毎の駆動条件（システムの入力）であるｕ（ｋ）の値を得る。第３パス情報取得部７８が得る先読みステップ毎の駆動条件は、最適化された第３パスＲ３を実現するための駆動条件であるといえる。なお、第３パス情報取得部７８は、最適化問題の解を直接車両制御に用いるため、逐次得た最適解の最初の時刻（ｋ＝１）の値であるｕ（１）＝（ｖ（１）ｃ（１））を制御に使用する。なおこの場合、曲率ｃは直接車両制御に使用するには適さないため、式（２７）の関係を用いて、曲率ｃを操舵角Φに変換して使用することが好ましい。本実施形態のように、最適化計算に操舵角Φを陽に含めずに曲率ｃを用いることで、非線形な特性（三角関数）を緩和して、最適化計算の計算精度と収束速度を向上できる。ただしそれに限られず、最適化計算において、曲率ｃの替わりに操舵角Φを用いてもよい。
【0140】
第３実施形態においては、以上説明したように、所定の評価関数の下で最適な経路を先読みしながら、同時にそれに対応する制御入力を生成しているため、単純にある経路に追従する制御よりも変動の少ないスムーズな車両の挙動、軌跡を設定できる。また、移動体１０及び目標物Ｐの位置姿勢を逐次検出して第３パスＲ３を更新しているため、フィードバック制御となり、モデル化誤差等に起因する誤差も吸収しながら車両を制御できる。
【0141】
（第４実施形態）
次に、第４実施形態について説明する。第４実施形態では、第４パスＲ４の設定方法の具体例を説明する。第４実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。第４実施形態は、第１実施形態、第１実施形態の他の例、第２実施形態、及び第３実施形態にも適用可能である。
次に、第４実施形態について説明する。第４実施形態では、第４パスＲ４の設定方法の具体例を説明する。第４実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。第４実施形態は、第１実施形態、第１実施形態の他の例、第２実施形態、及び第３実施形態にも適用可能である。
【0142】
ここで、移動体１０は、非ホロノミックであり、真横に移動することはできないなど、位置と姿勢が互いに連動している。そのため、位置だけ目標値（目標物Ｐの手前）に合わせるように操舵すると、姿勢が目標値と一致せず、移動体１０のフォーク２４を目標物Ｐの開口Ｐｂに差せない等の問題が発生し得る。それに対し、本実施形態では、モデル予測制御を用いて自己位置及び姿勢の先読みを行って設定した第３パスＲ３を用いることで、両方を目標値に収束させることができる。それに対し、第４パス情報取得部８０が設定する第４パスＲ４は、センサ２６により検出した目標物Ｐの位置及び姿勢と自己位置及び姿勢の偏差とのフィードバックにより、操舵指令値を出力する、サーボに関する技術を用いて設定される。第４パスＲ４においては、第３パスＲ３のような、先読みや最適化を行わないことで、高速、かつ安定な処理を実現しており、目標物Ｐの位置及び姿勢のばらつきに対して、ロバストに目標物Ｐにアプローチできる。以下、第３パスＲ３から第４パスＲ４への切り替えと、第４パスＲ４の設定方法とを、具体的に説明する。
【0143】
（第４パスへの切り替え）
第４パス情報取得部８０は、移動体１０が切替位置Ａ５に到達したら、第３パスＲ３から第４パスＲ４に切り替えると判断する。より具体的には、切替位置Ａ５は、移動体１０が有する移動及び操舵の制約下で、目標物Ｐの位置及び姿勢に応じて目標物Ｐに到達する際の移動体１０の位置及び姿勢に課せられる条件を満たして、移動体１０を目標物Ｐに到達させることが可能な位置である。ここでの条件とは、例えば、目標物Ｐの基準位置と移動体１０の基準位置との位置合わせが成立することを指す。また、ここでの制約とは、上述のように非ホロノミックであることを指し、例えば、速力の範囲（上限～下限）と、操舵角の範囲（上限～下限）であり、予め設定されている。
第４パス情報取得部８０は、移動体１０が切替位置Ａ５に到達したら、第３パスＲ３から第４パスＲ４に切り替えると判断する。より具体的には、切替位置Ａ５は、移動体１０が有する移動及び操舵の制約下で、目標物Ｐの位置及び姿勢に応じて目標物Ｐに到達する際の移動体１０の位置及び姿勢に課せられる条件を満たして、移動体１０を目標物Ｐに到達させることが可能な位置である。ここでの条件とは、例えば、目標物Ｐの基準位置と移動体１０の基準位置との位置合わせが成立することを指す。また、ここでの制約とは、上述のように非ホロノミックであることを指し、例えば、速力の範囲（上限～下限）と、操舵角の範囲（上限～下限）であり、予め設定されている。
【0144】
例えば、第４パス情報取得部８０は、移動体１０が目標物Ｐに正対する目標位置（ｑ＝［ｘＰＰ ｙＰＰ θＰＰ］）を求める処理を行う。目標位置（ｑ）は、以下の式（２９）に基づいて導出される。
【0145】
【数29】
【0146】
式（２９）のｐは目標物Ｐの位置及び姿勢である。また、式（２９）のＤは、ターゲット位置、すなわち、目標位置（ｑ）を目標物Ｐの位置及び向きからどの程度離すかを表す設計パラメータであり、予め定められている。なお、第３パスＲ３は、逐次更新されるため、第４パス情報取得部８０は、例えば予め定められた周期でｐ及び目標位置（ｑ）を更新する。移動体１０が目標位置（ｑ）に近接するに従ってｑの値は０に収束するよう変化する。
【0147】
第４パス情報取得部８０は、ｑの値が予め定められた閾値以下になった場合に、移動体１０が切替位置Ａ５に到達したと判断して、第３パスＲ３から第４パスＲ４に切り替えると判断する。切替位置Ａ５は、例えば目標物Ｐの向きに対して正対した位置となる。なお、切替位置Ａ５は、所定の広さをもつ領域であってもよい。
【0148】
（第４パスの設定）
図１６は、第４パスの設定を説明するフローチャートである。第４パス情報取得部８０は、移動体１０が切替位置Ａ５に到達したら、第４パスＲ４を設定する。図１６に示すように、第４パス情報取得部８０は、移動体１０が切替位置Ａ５に到達したら、センサ２６による目標物Ｐの位置及び姿勢の検出結果と、移動体１０の位置及び姿勢の検出結果とを取得する（ステップＳ６０）。そして、第４パス情報取得部８０は、目標物Ｐ及び移動体１０の位置及び姿勢と、移動体１０が有する移動及び操舵の制約とに基づき、移動体１０の操舵角の正負を切り替えることなく目標物Ｐに到達可能かを判断する（ステップＳ６２）。操舵角の正負を切り替えるとは、操舵角を一方向側に変化させた後に他方向側に変化させることを指す。すなわち、第４パス情報取得部８０は、操舵角を一方向側にのみ変化させることで、移動体１０が目標物Ｐに到達可能かを判断する。操舵角の正負を切り替えることなく目標物Ｐに到達可能と判断した場合（ステップＳ６２；Ｙｅｓ）、第４パス情報取得部８０は、移動体１０と目標物Ｐとの相対位置の偏差が許容範囲内であるかを判断し（ステップＳ６４）、許容範囲内である場合には（ステップＳ６４；Ｙｅｓ）、移動体１０と目標物Ｐとの相対姿勢に基づきフィードバック制御して、第４パスＲ４を設定する。すなわち、第４パス情報取得部８０は、移動体１０が目標物Ｐに対して十分正対した位置まで近づいていると判断して、相対姿勢を合致させるフィードバック制御を行う。この場合例えば、第４パス情報取得部８０は、次の式（３０）に基づき、移動体１０と目標物Ｐとの相対姿勢の偏差が小さくなるようにフィードバック制御して、第４パスＲ４を設定してよい。
図１６は、第４パスの設定を説明するフローチャートである。第４パス情報取得部８０は、移動体１０が切替位置Ａ５に到達したら、第４パスＲ４を設定する。図１６に示すように、第４パス情報取得部８０は、移動体１０が切替位置Ａ５に到達したら、センサ２６による目標物Ｐの位置及び姿勢の検出結果と、移動体１０の位置及び姿勢の検出結果とを取得する（ステップＳ６０）。そして、第４パス情報取得部８０は、目標物Ｐ及び移動体１０の位置及び姿勢と、移動体１０が有する移動及び操舵の制約とに基づき、移動体１０の操舵角の正負を切り替えることなく目標物Ｐに到達可能かを判断する（ステップＳ６２）。操舵角の正負を切り替えるとは、操舵角を一方向側に変化させた後に他方向側に変化させることを指す。すなわち、第４パス情報取得部８０は、操舵角を一方向側にのみ変化させることで、移動体１０が目標物Ｐに到達可能かを判断する。操舵角の正負を切り替えることなく目標物Ｐに到達可能と判断した場合（ステップＳ６２；Ｙｅｓ）、第４パス情報取得部８０は、移動体１０と目標物Ｐとの相対位置の偏差が許容範囲内であるかを判断し（ステップＳ６４）、許容範囲内である場合には（ステップＳ６４；Ｙｅｓ）、移動体１０と目標物Ｐとの相対姿勢に基づきフィードバック制御して、第４パスＲ４を設定する。すなわち、第４パス情報取得部８０は、移動体１０が目標物Ｐに対して十分正対した位置まで近づいていると判断して、相対姿勢を合致させるフィードバック制御を行う。この場合例えば、第４パス情報取得部８０は、次の式（３０）に基づき、移動体１０と目標物Ｐとの相対姿勢の偏差が小さくなるようにフィードバック制御して、第４パスＲ４を設定してよい。
【0149】
Φ（ｋ）＝Ｇ｛θｒｅｆ－θ（ｋ）｝ ・・・（３０）
【0150】
なお、θｒｅｆは目標物Ｐの姿勢であり、Ｇは評価関数である。またこの場合、速度ｖ（ｋ）は一定としてよい。
【0151】
一方、移動体１０の操舵角の正負を切り替えなければ目標物Ｐに到達できないと判断した場合（ステップＳ６２；Ｎｏ）であって、移動体１０と目標物Ｐとの相対位置の偏差が許容範囲内でない場合（ステップＳ６４；Ｎｏ）、第４パス情報取得部８０は、移動体１０と目標物Ｐとの相対位置に基づきフィードバック制御して、第４パスＲ４を設定する（ステップＳ６８）。すなわち、第４パス情報取得部８０は、移動体１０が目標物Ｐに対して十分正対した位置まで近づいていないと判断して、相対位置を合致させるフィードバック制御を行う。例えば、第４パス情報取得部８０は、次の式（３１）に基づき、移動体１０と目標物Ｐとの相対位置の偏差が小さくなるようにフィードバック制御して、第４パスＲ４を設定してよい。
【0152】
Φ（ｋ）＝Ｇ｛ｙｒｅｆ－ｙ（ｋ）｝ ・・・（３１）
【0153】
なお、ｙｒｅｆは目標物Ｐの位置であり、Ｇは評価関数である。またこの場合、速度ｖ（ｋ）は一定としてよい。
【0154】
ステップＳ６６，ステップＳ６８を実行し、処理を終了しない場合（ステップＳ７０；Ｎｏ）、ステップＳ６０に戻り第４パスＲ４を更新し、処理を終了する場合（ステップＳ７０；Ｙｅｓ）、本処理を終了する。なお、例えば移動体１０と目標物Ｐとの相対位置及び相対姿勢が許容範囲内となった場合に、処理を終了すると判断してよい。
【0155】
このように、第４パスＲ４は、モデル予測制御のような最適化計算を行わず，瞬時値の偏差のみで駆動条件が設定される経路である。そのため、処理時間が、最適化のアプローチに比べて短く、目標物Ｐに到達する直前の微調整が可能である。また、計算結果が発散することがないため、安定して動作させることができる。
【0156】
（本開示の効果）
以上説明したように、本開示の制御方法は、自動で移動する移動体１０の制御方法であって、移動体１０を第１パスＲ１に沿って移動させるステップと、移動体１０が第１パスＲ１に沿って移動中に、移動体１０に設けられたセンサ２６に目標物Ｐの位置及び姿勢を検出させるステップと、目標物Ｐの位置及び姿勢に基づき、目標物Ｐに対して所定の位置及び姿勢となる目標位置Ａ３までの第２パスＲ２を設定するステップと、第１パスＲ１から第２パスＲ２に切り替えて、移動体１０を第２パスＲ２に沿って移動させるステップと、目標物Ｐの位置と移動体１０の位置との偏差が小さくなるほど評価が高くなる評価関数に基づき最適化計算を行い、第３パスＲ３を設定するステップと、第２パスＲ２から第３パスＲ３に切り替えて、移動体１０を第３パスＲ３に沿って移動させるステップと、を含む。
以上説明したように、本開示の制御方法は、自動で移動する移動体１０の制御方法であって、移動体１０を第１パスＲ１に沿って移動させるステップと、移動体１０が第１パスＲ１に沿って移動中に、移動体１０に設けられたセンサ２６に目標物Ｐの位置及び姿勢を検出させるステップと、目標物Ｐの位置及び姿勢に基づき、目標物Ｐに対して所定の位置及び姿勢となる目標位置Ａ３までの第２パスＲ２を設定するステップと、第１パスＲ１から第２パスＲ２に切り替えて、移動体１０を第２パスＲ２に沿って移動させるステップと、目標物Ｐの位置と移動体１０の位置との偏差が小さくなるほど評価が高くなる評価関数に基づき最適化計算を行い、第３パスＲ３を設定するステップと、第２パスＲ２から第３パスＲ３に切り替えて、移動体１０を第３パスＲ３に沿って移動させるステップと、を含む。
【0157】
本制御方法によると、第１パスＲ１、第２パスＲ２、第３パスＲ３に移動経路を切り替えつつ、目標物Ｐにアプローチすることで、目標物Ｐ（目標位置）まで適切にアプローチすることができる。より具体的には、第１パスＲ１に沿って移動中に検出した目標物Ｐの位置姿勢情報に基づき設定した第２パスＲ２を用いることで、例えば目標物Ｐの位置が設置領域ＡＲ０からずれていても、実際の目標物Ｐの位置及び姿勢に合わせて、目標物Ｐに適切にアプローチできる。さらに、移動体１０が外乱の影響などで第２パスＲ２から外れたり、第２パスＲ２の設定に用いたセンサ２６の検出結果に誤差があったりした場合でも、最適化計算を用いた第３パスＲ３に切り替えることで、目標物Ｐに適切にアプローチできる。
【0158】
また、第１パスＲ１は、目標物Ｐが設置される設置領域ＡＲ０よりも第１方向（Ｘ方向）側で、第１方向と交差する第２方向（Ｙ方向）に設置領域ＡＲ０を横切る経路である。第１パスＲ１に沿って移動させるステップにおいては、第２方向において設置領域ＡＲ０よりも一方側の第１位置Ａ１から、第２方向において設置領域ＡＲ０よりも他方側の第２位置Ａ２まで、第１パスＲ１に沿って移動体１０を移動させる。目標物Ｐの位置及び姿勢を検出させるステップにおいては、移動体１０が第１位置Ａ１から第２位置Ａ２までの移動の最中に、センサ２６に目標物Ｐの位置及び姿勢を検出させる。本方法によると、第１位置Ａ１から第２位置Ａ２までの移動の最中に検出を行わせるため、検出精度の低下が抑制されて、第２パスＲ２を適切に設定できる。
【0159】
また、第２パスＲ２を設定するステップにおいては、第１パスＲ１の第１位置Ａ１から第２位置Ａ２までの軌道に重なる直線軌道Ｒ２ａと、直線軌道Ｒ２ａに接続される円弧軌道Ｒ２ｂとを含むように、第２パスＲ２を設定する。このように直線と円弧で軌道を生成することで、幾何学的に軌道計算することが可能となり、計算負荷を低減できる。
【0160】
第２パスＲ２を設定するステップにおいては、円弧軌道Ｒ２ｂとして、第１方向において設置領域ＡＲ０と反対側に向かう第１円弧軌道（円弧軌道Ｒ２ｂ１）と、第１円弧軌道に接続されて第１方向において設置領域ＡＲ０側に向かう第２円弧軌道（円弧軌道Ｒ２ｂ２）とを含むように、第２パスＲ２を設定する。このように第２パスＲ２に切り返し軌道を含めることで、切り返した経路とする方が効率的な場合に、効率的な経路を設定できる。
【0161】
本制御方法は、移動体１０が第２パスＲ２に沿って移動中におけるセンサ２６による目標物Ｐの検出精度と、移動体１０の進行方向と、移動体１０と目標物Ｐとの間の距離との、少なくとも１つに基づき、第２パスＲ２から第３パスＲ３に切り替えるかを判断するステップをさらに含む。このように第３パスＲ３に切り替えるか判断することで、第３パスＲ３を設定する際の目標物Ｐの検出精度が低下することを抑制して、第３パスＲ３を適切に設定できる。
【0162】
第２パスＲ２から第３パスＲ３に切り替えるかを判断するステップにおいては、目標物Ｐの検出精度の信頼度が所定値以上であり、移動体１０が目標物Ｐに近づく方向に進行しており、かつ、移動体１０と目標物Ｐとの間の距離が所定距離以下である場合に、第２パスＲ２から第３パスＲ３に切り替えると判断する。これにより、第３パスＲ３を設定する際の目標物Ｐの検出精度が低下することを抑制して、第３パスＲ３を適切に設定できる。
【0163】
第３パスＲ３を設定するステップにおいては、目標物Ｐの位置と移動体１０の位置とを逐次検出させて、第３パスＲ３を逐次更新する。第３パスＲ３を逐次更新することで、フィードバック制御の要素を取り入れて、目標物Ｐにより適切にアプローチできる第３パスＲ３を設定できる。
【0164】
本制御方法は、目標物Ｐと移動体１０との相対位置及び相対姿勢の少なくとも一方に基づきフィードバック制御を行うことで移動体１０を目標物Ｐにアプローチさせる第４パスＲ４を設定するステップと、第３パスＲ３から第４パスＲ４に切り替えて、移動体１０を第４パスＲ４に沿って移動させるステップと、をさらに含む。本制御方法は、最適化計算で設定された第３パスＲ３から、サーボ方式の第４パスＲ４に切り替えることで、目標物Ｐに到達する直前に微調整しつつ、目標物Ｐに近づくことができる。
【0165】
本制御方法は、移動体１０が切替位置Ａ５に到達した場合に、第３パスＲ３から第４パスＲ４に切り替えると判断するステップをさらに含む。切替位置Ａ５は、移動体１０が有する移動及び操舵の制約下で、目標物Ｐの位置及び姿勢に応じて目標物Ｐに到達する際の移動体１０の位置及び姿勢に課せられる条件を満たして、移動体１０を目標物Ｐに到達させることが可能な位置である。このような条件で第４パスＲ４に切り替えることで、第４パスＲ４を適切に設定できる。
【0166】
第４パスＲ４を設定するステップにおいては、移動体１０の操舵角の正負を切り替えることなく移動体１０が目標物Ｐに到達可能であるかを判断し、到達可能でない場合には、目標物Ｐと移動体１０との相対位置に基づきフィードバック制御を行って第４パスＲ４を設定し、到達可能である場合には目標物Ｐと移動体１０との相対姿勢に基づきフィードバック制御を行って第４パスＲ４を設定する。このように第４パスＲ４を設定することで、相対位置及び相対姿勢を適切に合致させて、移動体１０を目標物Ｐに到達させることができる。
【0167】
本開示の移動体１０は、自動で移動するものであって、移動体１０を第１パスＲ１に沿って移動させる移動制御部７２と、移動体１０が第１パスＲ１に沿って移動中に、移動体１０に設けられたセンサ２６に目標物Ｐの位置及び姿勢を検出させる検出制御部７４は、と、を含む。移動制御部７２は、第１パスＲ１から、目標物Ｐの位置及び姿勢に基づき設定された目標物Ｐに対して所定の位置及び姿勢となる目標位置Ａ３までの第２パスＲ２に切り替えて、移動体１０を第２パスＲ２に沿って移動させる。また、移動制御部７２は、第２パスＲ２から、目標物Ｐの位置と移動体１０の位置との偏差が小さくなるほど評価が高くなる評価関数に基づき実行された最適化計算により設定された第３パスＲ３に切り替えて、移動体１０を第３パスＲ３に沿って移動させる。この移動体１０によると、目標物Ｐ（目標位置）まで適切にアプローチすることができる。
【0168】
本開示のプログラムは、自動で移動する移動体１０の制御方法をコンピュータに実行させるものであって、移動体１０を第１パスＲ１に沿って移動させるステップと、移動体１０が第１パスＲ１に沿って移動中に、移動体１０に設けられたセンサ２６に目標物Ｐの位置及び姿勢を検出させるステップと、目標物Ｐの位置及び姿勢に基づき、目標物Ｐに対して所定の位置及び姿勢となる目標位置Ａ３までの第２パスＲ２を設定するステップと、第１パスＲ１から第２パスＲ２に切り替えて、移動体１０を第２パスＲ２に沿って移動させるステップと、目標物Ｐの位置と移動体１０の位置との偏差が小さくなるほど評価が高くなる評価関数に基づき最適化計算を行い、第３パスＲ３を設定するステップと、第２パスＲ２から第３パスＲ３に切り替えて、移動体１０を第３パスＲ３に沿って移動させるステップとを、コンピュータに実行させる。このプログラムによると、目標物Ｐ（目標位置）まで適切にアプローチすることができる。
【0169】
以上、本発明の実施形態を説明したが、この実施形態の内容により実施形態が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。さらに、前述した実施形態の要旨を逸脱しない範囲で構成要素の種々の省略、置換又は変更を行うことができる。
【符号の説明】
【0170】
１０ 移動体
２６ センサ
７０ 第１パス情報取得部
７２ 移動制御部
７４ 検出制御部
７６ 第２パス情報取得部
７８ 第３パス情報取得部
８０ 第４パス情報取得部
Ｐ 目標物
Ｒ１ 第１パス
Ｒ２ 第２パス
Ｒ３ 第３パス
Ｒ４ 第４パス
２６ センサ
７０ 第１パス情報取得部
７２ 移動制御部
７４ 検出制御部
７６ 第２パス情報取得部
７８ 第３パス情報取得部
８０ 第４パス情報取得部
Ｐ 目標物
Ｒ１ 第１パス
Ｒ２ 第２パス
Ｒ３ 第３パス
Ｒ４ 第４パス
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】