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

Description

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

周囲を検出するセンサを備えて自動で移動する移動体が知られている。例えば特許文献１には、障害物を検出して、移動体が走行可能な走行可能領域と、障害物が位置する障害物領域とを含む局所地図を生成する旨が記載されている。

特開２０１４－１６４４２４号公報

このような移動体においては、障害物を適切に回避することが求められている。

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

上述した課題を解決し、目的を達成するために、本開示に係る移動体の制御方法は、自動で移動する移動体の制御方法であって、前記移動体に設けられたセンサに、障害物を検出させるステップと、前記障害物の検出結果に基づき、前記障害物の前記移動体の進行方向に対向する前面の位置及び姿勢を特定するステップと、前記前面の位置及び姿勢に基づき、前記進行方向に交差する第１方向側に向かいつつ前記障害物を回避する回避パスを生成するステップと、前記回避パスに沿って前記移動体を移動させるステップと、前記移動体が前記回避パスを移動中に、前記障害物を検出させるステップと、前記回避パスを移動中の検出結果に基づき、前記障害物の前記第１方向側の側面の位置及び姿勢を特定するステップと、前記側面の位置及び姿勢に基づき、前記障害物を回避しつつ前記第１方向と反対側の第２方向側に戻るように、前記回避パスを更新するステップと、を含む。

上述した課題を解決し、目的を達成するために、本開示に係る移動体は、自動で移動する移動体であって、前記移動体に設けられたセンサに、障害物を検出させる検出制御部と、前記障害物の検出結果から特定された前記移動体の進行方向に対向する前面の位置及び姿勢に基づき生成された、前記進行方向に交差する第１方向側に向かいつつ前記障害物を回避する回避パスの情報を取得する回避パス情報取得部と、前記回避パスに沿って前記移動体を移動させる移動制御部と、を含み、前記回避パスは、前記回避パスを移動中の前記センサによる前記障害物の検出結果から、前記障害物の前記第１方向側の側面の位置及び姿勢が特定され、前記側面の位置及び姿勢に基づき、前記障害物を回避しつつ前記第１方向と反対側の第２方向側に戻るように、更新される。

上述した課題を解決し、目的を達成するために、本開示に係るプログラムは、自動で移動する移動体の制御方法をコンピュータに実行させるプログラムであって、前記移動体に設けられたセンサに、障害物を検出させるステップと、前記障害物の検出結果に基づき、前記移動体の進行方向に対向する前面の位置及び姿勢を特定するステップと、前記前面の位置及び姿勢に基づき、前記進行方向に交差する第１方向側に向かいつつ前記障害物を回避する回避パスを生成するステップと、前記回避パスに沿って前記移動体を移動させるステップと、前記移動体が前記回避パスを移動中に、前記障害物を検出させるステップと、前記回避パスを移動中の検出結果に基づき、前記障害物の前記第１方向側の側面の位置及び姿勢を特定するステップと、前記側面の位置及び姿勢に基づき、前記障害物を回避しつつ前記第１方向と反対側の第２方向側に戻るように、前記回避パスを更新するステップと、を、コンピュータに実行させる。

本開示によれば、障害物を適切に回避することができる。

図１は、第１実施形態に係る移動制御システムの模式図である。 図２は、移動体の構成の模式図である。 図３は、情報処理装置の模式的なブロック図である。 図４は、移動体の制御装置の模式的なブロック図である。 図５は、障害物の検出を説明する模式図である。 図６は、点群の抽出を説明するための模式図である。 図７は、直線候補の具体例を示す模式図である。 図８は、スコアの一例を示すグラフである。 図９は、点群の重畳の具体例を説明するための模式図である。 図１０は、近似線に射影された計測点の一例を示す模式図である。 図１１は、計測点のヒストグラムの一例を示す模式図である。 図１２は、障害物の前面の位置及び姿勢の特定方法の一例を説明する模式図である。 図１３は、回避パスの一例を示す模式図である。 図１４は、点群の抽出を説明するための模式図である。 図１５は、点群の重畳の具体例を説明するための模式図である。 図１６は、障害物の側面の位置及び姿勢の特定方法の一例を説明する模式図である。 図１７は、回避パスの更新の一例を説明するための模式図である。 図１８は、本実施形態に係る回避パスの更新フローを説明するフローチャートである。 図１９は、第２実施形態における障害物の側面の位置の推定方法を説明するための模式図である。 図２０は、第２実施形態における障害物の側面の位置の推定方法を説明するための模式図である。 図２１は、第２実施形態における回避パスの生成フローを説明するフローチャートである。

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

（第１実施形態）
（移動制御システムの全体構成）
図１は、第１実施形態に係る移動制御システムの模式図である。図１に示すように、第１実施形態に係る移動制御システム１は、移動体１０及び情報処理装置１２を含む。移動制御システム１は、設備Ｗに所属する移動体１０の移動を制御するシステムである。設備Ｗは、例えば倉庫など、物流管理される設備である。移動制御システム１においては、移動体１０を設備Ｗの領域ＡＲ内で移動させる。領域ＡＲは、例えば設備Ｗの床面である。以下、領域ＡＲに沿った一方向を、方向Ｘとする。また、領域Ａに沿った方向であって方向Ｘに交差する方向を、方向Ｙとし、方向Ｙに沿った一方向を、方向Ｙ１とし、方向Ｙに沿った他方向を、すなわち方向Ｙ１と反対方向を、方向Ｙ２とする。本実施形態では、方向Ｙ１、Ｙ２は、方向Ｘに直交する方向である。方向Ｘ、方向Ｙ１、Ｙ２は、水平方向といってもよい。また、方向Ｘ、方向Ｙ１、Ｙ２に直交する方向を、すなわち鉛直方向を、方向Ｚとする。

本実施形態においては、移動体１０は、移動パスＲ１に沿って移動する。しかし、移動パスＲ１上に障害物Ｐが存在した場合には、移動体１０が障害物Ｐに干渉するおそれがある。本実施形態においては、移動体１０が障害物Ｐに干渉する恐れがある場合に、障害物Ｐを回避する回避パスＲ２を設定して、回避パスＲ２に切り替えることで、障害物Ｐとの干渉を抑制して、障害物Ｐを適切に回避できる。回避パスＲ２の詳細については後述する。なお、障害物Ｐは、移動体１０が回避する対象となる物体である。障害物Ｐは、本実施形態では、Ｚ方向から見た場合に、曲線（曲面）を含まない多角形状であってよい。さらに言えば、障害物Ｐは、Ｚ方向から見た場合に、頂点を介して接続される２辺のなす角度が、９０度以下であることが好ましく、例えば矩形状であることがより好ましい。以降の説明では、障害物ＰがＺ方向から見た場合に矩形である場合を例にする。ただし、障害物Ｐの形状は、任意であってよい。例えば、障害物Ｐは、Ｚ方向から見た場合に曲線を含む形状であってもよく、例えばＺ方向から見た場合に円形状（円筒状）であってもよい。また、本実施形態の例では、障害物Ｐは、移動パスＲ１の進行方向に対して傾斜して位置しているが、障害物Ｐの向きは任意であってよい。

（移動体）
図２は、移動体の構成の模式図である。移動体１０は、自動で移動可能な装置である。本実施形態では、移動体１０は、フォークリフトであり、さらにいえば、いわゆるＡＧＦ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｆｏｒｋｌｉｆｔ）である。図２に示すように、移動体１０は、車体２０と、マスト２２と、フォーク２４と、センサ２６と、制御装置２８とを備えている。車体２０は、車輪２０Ａを備えている。マスト２２は、車体２０の前後方向における一方の端部に設けられている。マスト２２は、前後方向に直交する上下方向（ここでは方向Ｚ）に沿って延在する。フォーク２４は、マスト２２に方向Ｚに移動可能に取付けられている。フォーク２４は、マスト２２に対して、車体２０の横方向（上下方向及び前後方向に交差する方向）にも移動可能であってよい。フォーク２４は、一対のツメ２４Ａ、２４Ｂを有している。ツメ２４Ａ、２４Ｂは、マスト２２から車体２０の前方向に向けて延在している。ツメ２４Ａとツメ２４Ｂとは、マスト２２の横方向に、互いに離れて配置されている。以下、前後方向のうち、移動体１０においてフォーク２４が設けられている側の方向を、前方向とし、フォーク２４が設けられていない側の方向を、後方向とする。

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

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

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

なお、本実施形態では、移動体１０は、フォークリフトであったが、それに限られず、任意の移動体であってよい。ただし、移動体１０は、非ホノロミックなシステム、すなわち真横に移動できないものであってよく、旋回半径及び速度に制約があり、上下限の範囲外では動作できないものであってよい。また、本実施形態では、移動体１０は、設備Ｗ内を移動するものであるが、移動体１０が移動する場所は設備Ｗ内に限られず任意であってよい。

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

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

移動パス取得部４０は、移動体１０の移動パスＲ１の情報を取得する。移動パス取得部４０は、例えば移動体１０に割り当てされた作業内容に基づき、移動パスＲ１を設定してよい。移動パス取得部４０は、取得した移動パスＲ１の情報を、通信部３０を介して、対象となる移動体１０に送信する。なお、本実施形態では、図１の例に示すように、移動パスＲ１は、方向Ｘに沿った経路であるが、移動パスＲ１はそれに限られず任意である。また、移動パスＲ１は、情報処理装置１２によって設定されることに限られず、移動体１０に設定されてもよいし、設備Ｗにおける物流を管理するＷＭＳ（Ｗａｒｅｈｏｕｓｅ Ｍａｎａｇｅｍｅｎｔ Ｓｙｓｔｅｍ）などの管理システムによって設定されてもよい。

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

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

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

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

（検出制御部）
検出制御部７４は、センサ２６に障害物Ｐを複数回検出させる。検出制御部７４の具体的な処理内容は後述する。

（障害物位置情報取得部）
障害物情報取得部７６は、センサ２６による障害物Ｐの検出結果に基づいて特定された、障害物Ｐの前面Ｐａの位置及び姿勢の特定結果を取得する。また、障害物情報取得部７６は、回避パスＲ２を移動中におけるセンサ２６による障害物Ｐの検出結果に基づいて特定された、障害物Ｐの側面Ｐｂの位置及び姿勢の特定結果を取得する。障害物情報取得部７６の具体的な処理は後述する。

（回避パス情報取得部）
回避パス情報取得部７８は、障害物Ｐの前面Ｐａの位置及び姿勢の特定結果に基づいて生成された、回避パスＲ２の情報を取得する。また、回避パス情報取得部７８は、障害物Ｐの側面Ｐｂの位置及び姿勢の特定結果に基づき更新された、回避パスＲ２の情報を取得する。回避パス情報取得部７８の具体的な処理は後述する。

（制御装置の処理）
次に、移動体１０が障害物Ｐを回避する際の制御装置２８の処理について説明する。

（移動パスに沿った移動）
図５は、障害物の検出を説明する模式図である。制御装置２８の移動制御部７２は、移動パス情報取得部７０が取得した移動パスＲ１に従って、移動体１０を移動させる。検出制御部７４は、図５に示すように、移動体１０が移動パスＲ１に沿って移動中に、センサ２６に検出を行わせる。検出制御部７４は、移動体１０が移動パスＲ１に沿って移動中に、センサ２６に、移動パスＲ１の進行方向側を検出させる。検出制御部７４は、センサ２６による検出結果から、移動パスＲ１の進行方向側に障害物Ｐが存在するかを、言い換えればこのまま移動体１０が移動パスＲ１に沿って進行した場合に障害物Ｐに干渉するかを、判定する。進行方向側に障害物Ｐが存在するかの判定は任意の方法で行ってよいが、例えば、検出制御部７４は、進行方向に向けて照射したレーザ光ＬＴの反射光を受信した場合に、進行方向側に障害物Ｐが存在すると判定してよい。

（障害物の前面の位置及び姿勢の特定処理）
制御装置２８は、進行方向側に障害物Ｐが存在すると判断したら、その障害物Ｐの前面Ｐａの位置及び姿勢を特定する処理を実行する。障害物Ｐの前面Ｐａとは、障害物Ｐの、移動パスＲ１の進行方向（本実施形態の例では方向Ｘ）に対向する表面を指し、言い換えれば、移動パスＲ１の進行方向と反対側（本実施形態の例では方向Ｘの反対側）における障害物Ｐの表面といえる。以下、前面Ｐａの位置及び姿勢を特定する処理について説明する。

（点群の取得）
障害物Ｐの前面Ｐａの位置及び姿勢を特定する処理を実行する場合、移動制御部７２は、移動体１０の移動速度を減速させ、検出制御部７４は、センサ２６に進行方向側の検出を続けさせることで、センサ２６に、障害物Ｐの前面Ｐａを含む領域を検出させる。具体的には、検出制御部７４は、センサ２６に、移動パスＲ１の進行方向側に向けて、レーザ光ＬＴを走査させつつ照射させる。レーザ光ＬＴは障害物Ｐの前面Ｐａで反射され、センサ２６は、障害物Ｐの前面Ｐａからの反射光を受光する。検出制御部７４は、センサ２６が受光した反射光の検出結果に基づき、計測点Ｍの集合である点群Ｍ０を取得する。計測点Ｍとは、レーザ光ＬＴが反射された位置（座標）を示す点であり、点群Ｍ０とは、レーザ光ＬＴが反射された位置を示す点の集合を指す。本実施形態では、検出制御部７４は、反射光の検出結果と移動体１０の位置に基づき、反射光が反射された箇所の、方向Ｘ及び方向Ｙの二次元座標系ＣＯにおける位置（座標）を、計測点Ｍとして算出する。ただし、検出制御部７４は、二次元座標系ＣＯにおける位置を計測点Ｍとすることに限られず、センサ２６や移動体１０を基準とした座標系における位置を、計測点Ｍとしてもよい。また、本実施形態では、移動体１０の移動速度を減速させたが、減速させる処理は必須ではない。

（点群の抽出）
図６は、点群の抽出を説明するための模式図である。検出制御部７４は、センサ２６による１回の検出（レーザ光ＬＴの１回のスキャン）により取得した点群Ｍ０に含まれる計測点Ｍのうちから、後述する障害物情報取得部７６によって重畳される計測点Ｍを、言い換えれば障害物Ｐの前面Ｐａの位置及び姿勢の特定に用いる計測点Ｍを、抽出する。検出制御部７４は、センサ２６による１回の検出で取得された計測点Ｍのうちから、障害物Ｐの前面Ｐａに対応する計測点Ｍを、障害物情報取得部７６によって重畳される計測点Ｍとして抽出する。検出制御部７４は、抽出した計測点Ｍの情報を、記憶部６２に含まれるバッファに保存する。本実施形態では、検出制御部７４は、センサ２６による１回の検出により取得した計測点Ｍに基づき、障害物Ｐの前面Ｐａに対応する直線Ｌａ（直線Ｌａの位置及び姿勢）を算出し、直線Ｌａと計測点Ｍとの位置関係に基づいて、抽出する計測点Ｍを選定する。以下、計測点Ｍを抽出する処理の例を具体的に説明する。

(直線候補の取得)
図７は、直線候補の具体例を示す模式図である。図７に示すように、検出制御部７４は、障害物Ｐの前面Ｐａに対応する直線Ｌ０を検出するために、一以上の直線候補を取得する。本実施形態では、検出制御部７４は、センサ２６による１回の検出により取得した点群Ｍ０に含まれる計測点Ｍから選択した２つの計測点Ｍを結ぶ直線を、直線候補として取得する。より詳しくは、検出制御部７４は、センサ２６による１回の検出により取得した計測点ＭからＲＡＮＳＡＣアルゴリズムにより選択した、２つの計測点Ｍを結ぶ直線を、直線候補として取得するように構成されてもよい。なお、この場合、検出制御部７４は、１回の検出により取得した全ての計測点Ｍのうちから２つの計測点Ｍを選択してもよいし、いわゆるＲＯＩ（Ｒｅｇｉｏｎｓ Ｏｆ Ｉｎｔｅｒｅｓｔ）である検知対象領域を設定し、検知対象領域に含まれる計測点Ｍのうちから、２つの計測点Ｍを選択してもよい。この場合の検知対象領域は、任意の方法で設定してよい。なお、ＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）は、ランダムに２つの点を選択して評価値を算出する手法である。ただし、本開示に係る直線候補の取得手法には、ＲＡＮＳＡＣを用いた手法に限られず、ＰＲＯＳＡＣ（ＰＲＯｇｒｅｓｓｉｖｅ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）等の他のアルゴリズムが使用されてもよい。

(直線Ｌａの検出)
検出制御部７４は、一以上の直線候補のそれぞれについてスコア積算値を算出し、スコア積算値に基づいて、障害物Ｐの前面Ｐａに対応する直線Ｌａを検出する。以降では、一例として、検出制御部７４が、スコア積算値に基づいて、図７に示す直線候補Ｌ１を直線Ｌａとして選択するものとする。

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

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

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

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

検出制御部７４は、以上のようにスコア積算値に基づき、直線候補のうちから直線Ｌａを選択する。ただし、検出制御部７４は、スコア積算値に基づき選択した直線候補を２点で区切った線分を、直線Ｌａとしてもよい。この場合の２点は、障害物Ｐの幅方向における両端点に相当する。この場合、検出制御部７４は、選択した直線候補から一以上の線分を抽出し、抽出した一以上の線分の中から障害物Ｐの両端の長さに対応する両端長さを有する一つの線分又は二以上の線分の組み合わせを探索する。「対応する長さ」とは、許容誤差の範囲内の長さを意味する。障害物Ｐの両端の長さは、例えば障害物Ｐの寸法が既知である場合には、障害物Ｐの設計情報として、制御装置２８の記憶部６２に予め記憶されていてよい。検出制御部７４は、見つかった線分又は二以上の線分の組み合わせを、直線Ｌａとしてよい。

（直線Ｌに基づく点群の抽出）
検出制御部７４は、以上のようにして検出した直線Ｌａと計測点Ｍとの位置関係に基づき、障害物情報取得部７６によって重畳される計測点Ｍを抽出する。例えば、検出制御部７４は、直線Ｌａに対して所定の距離範囲内にある計測点Ｍを、障害物情報取得部７６によって重畳される計測点Ｍとして抽出してよい。このように直線Ｌａと計測点Ｍとの位置関係に基づき計測点Ｍを抽出することで、障害物Ｐの前面Ｐａに対応しない計測点Ｍを除去し、障害物Ｐの前面Ｐａに対応する計測点Ｍを適切に抽出して、前面Ｐａの位置及び姿勢の検出精度を向上させることができる。

なお、以上の説明では、検出制御部７４は、センサ２６による１回の検出により取得した点群Ｍ０を用いて直線Ｌａを検出していたが、それに限られず、センサ２６による複数回の検出により取得した点群Ｍ０を用いて直線Ｌａを検出してもよい。すなわち、検出制御部７４は、少なくとも１回の検出によって取得された点群Ｍ０に基づき、直線Ｌａを検出してもよい。また、直線Ｌａの検出方法は、以上の方法に限られず、任意の方法を用いてもよい。

また、計測点Ｍを抽出する方法は、直線Ｌａを用いることに限られず、検出制御部７４は、任意の方法で計測点Ｍを抽出してもよい。さらに言えば、計測点Ｍを抽出する処理自体も必須ではなく、センサ２６による１回の検出により取得した点群Ｍ０に含まれる計測点Ｍの全てを、障害物情報取得部７６によって重畳される計測点Ｍとしてもよい。

（複数回の検出）
検出制御部７４は、移動体１０が移動パスＲ１を移動中に、センサ２６に、障害物Ｐの前面Ｐａの検出を、複数回行わせる。言い換えれば、検出制御部７４は、移動体１０が移動パスＲ１を移動中に、センサ２６に、障害物Ｐの前面Ｐａへのレーザ光ＬＴのスキャンを、複数回行わせる。検出制御部７４は、センサ２６による複数回の検出結果を、点群Ｍ０として取得する。検出制御部７４は、センサ２６の検出結果毎に（センサ２６が検出する度に）、以上で説明した、計測点Ｍを算出して計測点Ｍを抽出する処理を、実行する。

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

（点群の重畳）
図９は、点群の重畳の具体例を説明するための模式図である。図９に示すように、障害物情報取得部７６は、検出制御部７４によって抽出された計測点Ｍ（点群Ｍ０）を、同一の座標系で重畳する。言い換えれば、障害物情報取得部７６は、センサ２６による複数回の検出結果に対応する計測点Ｍのうちで、検出制御部７４によって抽出された計測点Ｍの、同一の座標系における位置（座標）の情報を取得する。本実施形態における同一の座標系とは、方向Ｘ及び方向Ｙの二次元座標系ＣＯであり、設備Ｗにおける座標系であるともいえる。すなわち、障害物情報取得部７６は、検出制御部７４によって抽出されたそれぞれの計測点Ｍを、二次元座標系ＣＯで重畳する（二次元座標系ＣＯ上でプロットする）。なお、計測点Ｍが二次元座標系ＣＯでの位置として算出される場合には、それぞれの計測点Ｍの座標系が共通するため、障害物情報取得部７６は、それぞれの計測点Ｍに対して座標系の変換処理を行うことなく、それぞれの計測点Ｍの二次元座標系ＣＯでの位置の情報を取得できる。ただし例えば、それぞれの計測点Ｍが、移動体１０を基準とした座標系など、同一の座標系における位置として算出されていない場合には、障害物情報取得部７６は、計測点Ｍが検出された際の移動体１０の二次元座標系ＣＯでの位置に基づき、それぞれの計測点Ｍの座標変換を行って、二次元座標系ＣＯでの計測点Ｍの位置を算出する。なお、それぞれの計測点Ｍを重畳する座標系は、二次元座標系ＣＯに限られず、例えば三次元座標系などであってもよい。

（障害物の前面の姿勢の特定）
障害物情報取得部７６は、二次元座標系ＣＯで重畳した各計測点Ｍ（点群Ｍ０）の位置に基づき、障害物Ｐの前面Ｐａの位置及び姿勢を特定する。本実施形態では、障害物情報取得部７６は、重畳された各計測点Ｍの近似線Ｎａ（二次元座標系ＣＯでの近似線Ｎａの位置及び姿勢）を算出し、近似線Ｎａに基づき、障害物Ｐの前面Ｐａの姿勢を特定する。例えば、障害物情報取得部７６は、二次元座標系ＣＯにおける近似線Ｎａの傾きを、障害物Ｐの姿勢とする。図９に示すように、近似線Ｎａは、重畳された各計測点Ｍの近似線であり、センサ２６による複数の検出結果に対応する各計測点Ｍの近似線であるといえる。本実施形態においては、障害物情報取得部７６は、重畳された各計測点Ｍに対して最小二乗法によって直線フィッティングを行うことで、近似線Ｎａを算出する。ただし、近似線Ｎａの算出方法は、最小二乗法に限られず、任意の方法を用いてよい。例えば、センサ２６による最初の検出で取得された点群Ｍ０に基づき算出した直線Ｌａを近似線Ｎａとしてもよいし、重畳された各計測点Ｍを用いて、ＲＡＮＳＡＣを用いて直線を複数生成し、それぞれの直線の傾き分布から、前面Ｐａに近い直線を、近似線Ｎａとしてよい。なお、ＲＡＮＳＡＣを用いた直線の生成方法は、上述の直線候補の生成方法と同様としてよい。また、本実施形態では、障害物Ｐの前面Ｐａが平面状であるため、近似線Ｎａを直線として算出するが、近似線は直線であることに限られない。例えば前面Ｐａが曲面であることが既知である場合などには、それに合わせて、近似線Ｎａを曲線として算出してもよい。

（障害物の前面の位置の特定）
図１０は、近似線に射影された計測点の一例を示す模式図である。障害物情報取得部７６は、重畳された各計測点Ｍの位置に基づき、障害物Ｐの前面Ｐａの位置を特定する。より詳しくは、障害物情報取得部７６は、重畳された各計測点Ｍの近似線Ｎａの座標系での位置に基づき、障害物Ｐの前面Ｐａの位置を特定する。本実施形態では、障害物情報取得部７６は、重畳された各計測点Ｍ（点群Ｍ０）を、近似線Ｎ上に射影することで、図１０の例に示すように、二次元座標系ＣＯの計測点Ｍを近似線Ｎａの座標系の計測点ＭＡに変換する。言い換えれば、計測点ＭＡは、二次元座標系ＣＯにおける計測点Ｍが、射影変換によって近似線Ｎａの座標系となったものといえる。なお、近似線Ｎａの座標系は、一次元の座標であり、近似線Ｎａの座標系における計測点ＭＡは、近似線Ｎａに沿った方向における計測点の位置を指す。例えば、障害物情報取得部７６は、近似線Ｎａに対して計測点Ｍから垂線を引いて、垂線と近似線Ｎａとの交点を、計測点ＭＡとしてよい。

図１１は、計測点のヒストグラムの一例を示す模式図である。障害物情報取得部７６は、近似線Ｎａの座標系の計測点ＭＡに基づき、近似線Ｎの座標系における位置毎の計測点ＭＡの数を算出する。例えば、障害物情報取得部７６は、近似線Ｎａの座標系を複数の単位範囲に区分して、それぞれの計測点ＭＡを、いずれかの単位範囲に割り当てる。すなわち、障害物情報取得部７６は、近似線Ｎａの座標系における計測点ＭＡの位置を範囲に含む単位範囲に、その計測点ＭＡを割り当てる。障害物情報取得部７６は、計測点ＭＡを単位範囲に割り当てる処理を計測点ＭＡ毎に実行して、単位範囲毎の計測点ＭＡの数を算出する。図１１は、単位範囲毎の計測点ＭＡの数の一例を示すヒストグラムである。障害物情報取得部７６は、図１１に示すような、近似線Ｎａの座標系における単位範囲毎の計測点ＭＡの数を示すヒストグラムを生成してもよい。

障害物情報取得部７６は、近似線Ｎａの座標系における計測点ＭＡ（点群）の数に基づき、障害物Ｐの位置を特定する。例えば、障害物情報取得部７６は、計測点ＭＡの数が所定数以上となる単位範囲を含む範囲ＲＡを抽出し、近似線Ｎａの座標系における範囲ＲＡの下限の位置ＮＳ１ａと上限の位置ＮＳ２ａとに基づき、障害物Ｐの前面Ｐａの幅方向の両端点を特定する。なお、ここでの範囲ＲＡは、その範囲ＲＡに含まれている単位範囲の全てにおいて計測点ＭＡの数が所定数以上となる、近似線Ｎの座標系における範囲（区間）を指すことが好ましい。ここでの所定数は、任意に設定してよいが、例えば単位範囲毎の計測点ＭＡの数の最大値や平均値に基づき決定してよく、例えば平均値の１／２を所定数として設定してよい。

図１２は、障害物の前面の位置及び姿勢の特定方法の一例を説明する模式図である。本実施形態においては、障害物情報取得部７６は、図１２に示すように、二次元座標系ＣＯにおいて、近似線Ｎａを、移動パスＲ１の進行方向とは反対側に向けて所定距離Ｄａだけ、姿勢を変えずに平行移動させ、平行移動させた近似線Ｎａを端点ＰＳ１ａと端点ＰＳ２ａとで区分した線分である線分Ｎａ１を、障害物Ｐの前面Ｐａとして特定する。言い換えれば、障害物情報取得部７６は、線分Ｎａ１の傾きを前面Ｐａの姿勢とし、端点ＰＳ１ａと端点ＰＳ２ａとを、前面Ｐａの幅方向における一方型の端点及び他方側の端点として特定する。なお、所定距離Ｄａは、安全マージンを考慮するために設定される距離であり、任意の方法で算出されてよいが、例えば、計測点Ｍと近似線Ｎａとの間の距離の、重畳された各計測点Ｍについての標準偏差の値としてよい。また、端点ＰＳ１ａ、ＰＳ２ａは、範囲ＲＡの下限の位置ＮＳ１ａ及び上限の位置ＮＳ１ｂに基づき設定される。具体的には、二次元座標系ＣＯにおいて、近似線Ｎａ上の位置ＮＳ１ａ、ＮＳ２ａを、移動パスＲ１の進行方向とは反対側に向けて所定距離Ｄａだけ離した位置が、端点ＰＳ１ａ、ＰＳ２ａとなる。

障害物情報取得部７６は、以上のようにして、障害物Ｐの前面Ｐａの位置及び姿勢を特定する。ただし、障害物情報取得部７６による前面Ｐａの位置及び姿勢の特定方法は以上の説明に限られず、障害物情報取得部７６は、計測点Ｍ（点群Ｍ０）の位置に基づいて、任意の方法で前面Ｐａの位置及び姿勢を特定してよい。例えば、本実施形態では、障害物情報取得部７６は、重畳した計測点Ｍに基づき前面Ｐａの位置及び姿勢を特定したが、計測点Ｍを重畳する処理は必須ではなく、センサ２６による１回の検出で取得された計測点Ｍに基づき、前面Ｐａの位置及び姿勢を特定してよい。

（回避パスの生成）
図１３は、回避パスの一例を示す模式図である。回避パス情報取得部７８は、障害物情報取得部７６が特定した障害物Ｐの前面Ｐａの位置及び姿勢に基づき、回避パスＲ２を設定する。回避パスＲ２は、移動パスＲ１の進行方向（本例ではＸ方向）に交差する第１方向側に向かいつつ、障害物Ｐを回避する（障害物Ｐに干渉しない）軌道である。第１方向は、本実施形態では移動パスＲ１の進行方向に直交する方向である。以下、回避パスＲ２についてより具体的に説明する。

回避パス情報取得部７８は、移動体１０が障害物Ｐに干渉せずに第１方向側に向かう軌道Ｒ２Ａを含むように、回避パスＲ２として設定する。回避パス情報取得部７８は、移動体１０が障害物Ｐに干渉せずに第１方向側に向かい、かつ、第１方向側への突出量が最小となるように、軌道Ｒ２Ａを設定することが好ましい。以下では、方向Ｙ１を第１方向とし、端点ＰＳ１ａ、ＰＳ２ａが、前面Ｐａの方向Ｙ２側、方向Ｙ１側の端点とする場合を例にして説明する。この場合、例えば、回避パス情報取得部７８は、方向Ｙ１側に向かいつつＸ方向（移動パスＲ１の進行方向）側に進行し、かつ、障害物Ｐよりも方向Ｙ１側の位置まで到達する軌道を、軌道Ｒ２Ａとして設定する。

さらに言えば、回避パス情報取得部７８は、特定された前面Ｐａの位置及び姿勢（線分Ｎａ１）に基づき、障害物Ｐの側面Ｐｂの位置及び姿勢を推定する。そして、回避パス情報取得部７８は、移動体１０が、特定した前面Ｐａの位置（線分Ｎａ１）と側面Ｐｂの推定位置とに干渉せずに第１方向側に向かう軌道を、軌道Ｒ２Ａとして設定する。側面Ｐｂとは、障害物Ｐの第１方向（方向Ｙ１）側の表面であり、言い換えれば、障害物Ｐの第１方向側まで移動した移動体１０に対して、第２方向（方向Ｙ２）に対向する表面である。回避パス情報取得部７８は、例えば図１２に示すように、障害物情報取得部７６が特定した障害物Ｐの前面Ｐａの位置及び姿勢に基づき、障害物Ｐの、後方側（移動パスＲ１の進行方向側）であって方向Ｙ１側の後方端点ＰＳ３ａの位置を推定する。例えば、回避パス情報取得部７８は、端点ＰＳ２ａから、前面Ｐａ（線分Ｎａ１）と直交して後方側に向かう方向に、前面Ｐａの長さ分離れた位置を、後方端点ＰＳ３ａの推定位置とする。回避パス情報取得部７８は、後方端点ＰＳ３ａと端点ＰＳ２ａとを結ぶ線分を、障害物Ｐの側面Ｐｂの推定位置として扱う。そして、回避パス情報取得部７８は、方向Ｙ１側に向かいつつＸ方向側に進行し、かつ、前面Ｐａ（線分Ｎａ１）及び側面Ｐｂの推定位置よりも方向Ｙ１側の位置まで到達する軌道を、軌道Ｒ２Ａとして設定する。

さらに言えば、回避パス情報取得部７８は、軌道Ｒ２Ａに接続されて、障害物Ｐを回避しつつ第１方向と反対側の第２方向（方向Ｙ２）側に戻る軌道Ｒ２Ｂを含むように、回避パスＲ２を設定する。この場合、回避パス情報取得部７８は、側面Ｐｂの推定位置を回避しつつ、移動体１０が第１方向と反対の第２方向（本例では方向Ｙ２）側に戻る軌道を、軌道Ｒ２Ｂとして設定する。具体的には、回避パス情報取得部７８は、移動体１０が側面Ｐｂの推定位置に干渉せずに（すなわち側面Ｐｂの推定位置よりも方向Ｙ１側を通りつつ）、方向Ｙ２側に向かいつつ方向Ｘ側に進行し、障害物Ｐよりも方向Ｘ側に到達するように、軌道Ｒ２Ｂを設定する。なお、障害物Ｐよりも方向Ｘ側に到達した回避パスＲ２（軌道Ｒ２Ｂ）の終点位置は、Ｙ方向の位置が、移動パスＲ１における、方向Ｘにおいて回避パスＲ２の終点位置と同じ位置でのＹ方向の位置と、重なることが好ましい。これにより、移動体１０が回避パスＲ２の終点まで到達したら、回避パスＲ２から移動パスＲ１に適切に戻すことが可能となる。

このように、回避パス情報取得部７８は、前面Ｐａとの干渉を避けながら、第１方向に向かいつつ障害物Ｐよりも第１方向側に到達する軌道Ｒ２Ａと、軌道Ｒ２Ａに接続されて、側面Ｐｂとの干渉を避けながら第２方向側に向かいつつ、障害物Ｐよりも移動パスＲ１の進行方向側に到達する軌道Ｒ２Ｂとを含む軌道を、回避パスＲ２として設定するといえる。ただし、回避パス情報取得部７８は、回避パスＲ２に、軌道Ｒ２Ａ及び軌道Ｒ２Ｂを含めることに限られず、少なくとも軌道Ｒ２Ａを含むように、回避パスＲ２を設定してよい。

なお、回避パス情報取得部７８は、障害物Ｐの前面Ｐａ及び側面Ｐｂとは別の面（例えば前面Ｐａと反対側の後面など）にも基づき、回避パスＲ２を設定してもよい。この場合例えば、後方端点ＰＳ３ａと同様に、後方側であって第２方向（方向Ｙ２）側の後方端点ＰＳ４ａの位置を特定し、後方端点ＰＳ３ａと後方端点ＰＳ４ａとを結ぶ直線を後面の推定位置として扱う。そして、回避パス情報取得部７８は、移動体１０が後面推定位置にも干渉しないように、方向Ｙ２（第２方向）に向かう第２軌道を設定してよい。

なお、本実施形態では、移動体１０が移動可能な領域である移動可能領域ＲＥが予め設定されており、移動パスＲ１は、移動パスＲ１を通る移動体１０が移動可能領域ＲＥの範囲内に位置するように、設定されている。このように移動可能領域ＲＥが設定されている場合、回避パス情報取得部７８は、移動体１０が、移動可能領域ＲＥの範囲内を通るように、すなわち移動可能領域ＲＥの範囲外に出ないように、回避パスＲ２を設定する。移動可能領域ＲＥは、任意に設定されてよいが、移動パスＲ１の進行方向に対して両側方の壁に挟まれた領域を、移動可能領域ＲＥとして設定してよいし、移動パスＲ１の進行方向に沿った移動が許可された領域（車線）を、移動可能領域ＲＥとして設定してよい。また、移動パスＲ１の進行方向に沿った移動が許可された領域（車線）に隣接して、反対方向に沿った移動が許可された領域（反対車線）がある場合には、その車線と反対車線とを含む領域を、移動可能領域ＲＥとして設定してよい。

（回避パスに沿った移動）
回避パス情報取得部７８によって回避パスＲ２が生成されたら、移動制御部７２は、移動パスＲ１から回避パスＲ２に切り替えて、回避パスＲ２に沿って移動体１０を移動させる。

移動体１０は、回避パスＲ２に沿って移動することで、障害物Ｐとの干渉が抑制される。ただし、回避パスＲ２は、障害物Ｐの前面Ｐａの特定結果から推定された側面Ｐｂの推定位置を回避するように生成されたものであり、実際に側面Ｐｂを検出した結果を用いたものではない。そのため、例えば側面Ｐｂの実際の位置が、側面Ｐｂの推定位置よりも方向Ｙ１側に突出していた場合は、回避パスＲ２を通ったとしても、移動体１０が側面Ｐｂに干渉するおそれがある。一方、側面Ｐｂの実際の位置が、側面Ｐｂの推定位置よりも方向Ｙ２側に引っ込んでいた場合には、側面Ｐｂとの干渉は避けられるが、回避パスＲ２を通る移動体１０が、必要以上に方向Ｙ１（第１方向）に突出することになる。しかし、例えば移動可能領域ＲＥが狭い場合や、移動可能領域ＲＥに反対車線も含まれている場合には、できるだけ方向Ｙ１に突出しすぎないことが求められる。それに対し、本実施形態においては、回避パスＲ２を移動中にセンサ２６に検出を続けさせて側面Ｐｂの位置を検出し、その検出結果から回避パスＲ２を更新する。これにより、側面Ｐｂに干渉することなく、かつ、できるだけ方向Ｙ１に突出しないことが可能となる。以下、具体的に説明する。

（障害物の側面の位置及び姿勢の特定処理）
制御装置２８は、移動体１０が回避パスＲ２を移動中に、その障害物Ｐの側面Ｐｂの位置及び姿勢を特定する処理を実行する。この場合、検出制御部７４は、移動体１０が回避パスＲ２を移動中に、センサ２６に、障害物Ｐの側面Ｐｂを含む領域を検出させる。具体的には、検出制御部７４は、センサ２６に、障害物Ｐに向けてレーザ光ＬＴを走査させつつ照射させる。検出制御部７４は、センサ２６が受光した反射光の検出結果に基づき、点群Ｍ０ａを取得する。すなわち、点群Ｍ０ａは、移動体１０が回避パスＲ２を移動中でのセンサ２６の検出結果から取得された点群といえる。なお、障害物Ｐの側面Ｐｂを検出させるためのレーザ光ＬＴの照射方向は、任意であってよいが、例えば第２方向（方向Ｙ２）側にレーザ光ＬＴを照射させてよい。

（点群の抽出）
図１４は、点群の抽出を説明するための模式図である。図１４に示すように、検出制御部７４は、センサ２６による１回の検出により取得した点群Ｍ０ａに含まれる計測点Ｍａのうちから、障害物情報取得部７６によって重畳される計測点Ｍａを、言い換えれば側面Ｐｂの位置及び姿勢の特定に用いる計測点Ｍａを、抽出する。検出制御部７４は、センサ２６による１回の検出で取得された計測点Ｍａのうちから、側面Ｐｂに対応する計測点Ｍａを、障害物情報取得部７６によって重畳される計測点Ｍａとして抽出する。ここでの計測点Ｍａの抽出方法は、移動パスＲ１を移動中の検出により取得された計測点Ｍのうちから前面Ｐａに対応する計測点Ｍを抽出する方法と同様なので、説明を省略する。すなわち、一例として、検出制御部７４は、計測点Ｍａに基づき、障害物Ｐの側面Ｐｂに対応する直線Ｌｂを算出し、直線Ｌｂと計測点Ｍａとの位置関係に基づいて、抽出する計測点Ｍａを選定する。直線Ｌｂの算出方法は、前面Ｐａに対応する計測点Ｍを抽出する際の直線Ｌａの算出方法と同様である。

（複数回の検出）
検出制御部７４は、移動体１０が回避パスＲ２を移動中に、センサ２６に、障害物Ｐの側面Ｐｂの検出を、複数回行わせる。言い換えれば、検出制御部７４は、移動体１０が回避パスＲ２を移動中に、センサ２６に、障害物Ｐへのレーザ光ＬＴのスキャンを、複数回行わせる。検出制御部７４は、センサ２６による複数回の検出結果を、点群Ｍ０ａとして取得する。検出制御部７４は、センサ２６の検出結果毎に（センサ２６が検出する度に）、以上で説明した、計測点Ｍａを算出して計測点Ｍａを抽出する処理を、実行する。

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

（点群の重畳）
図１５は、点群の重畳の具体例を説明するための模式図である。図１５に示すように、障害物情報取得部７６は、検出制御部７４によって抽出された計測点Ｍａ（点群Ｍ０ａ）を、同一の座標系で、ここでは二次元座標系ＣＯで、重畳する。

（障害物の側面の位置及び姿勢の特定）
図１６は、障害物の側面の位置及び姿勢の特定方法の一例を説明する模式図である。障害物情報取得部７６は、計測点Ｍの代わりに計測点Ｍａを用いる以外は、前面Ｐａの位置及び姿勢を特定した方法と同じ方法で、障害物Ｐの側面Ｐｂの位置及び姿勢を特定する。すなわち、一例として、障害物情報取得部７６は、重畳された計測点Ｍａの近似線Ｎｂを算出して、近似線Ｎｂの傾きを側面Ｐｂの姿勢として特定する。さらに言えば、図１６に示すように、障害物情報取得部７６は、近似線Ｎｂを、第１方向側に向けて所定距離Ｄｂだけ、姿勢を変えずに平行移動させ、平行移動させた近似線Ｎｂを端点ＰＳ２ｂと端点ＰＳ３ｂとで区分した線分である線分Ｎｂ１を、障害物Ｐの側面Ｐｂとして特定する。なお、所定距離Ｄｂは、前面Ｐａを特定した際の所定距離Ｄａと同様の方法で算出され、端点ＰＳ２ｂ、ＰＳ３ｂの位置は、前面Ｐａを特定した際の端点ＰＳ１ａ、ＰＳ２ａと同様の方法で算出される。

障害物情報取得部７６は、以上のようにして、障害物Ｐの側面Ｐｂの位置及び姿勢を特定する。ただし、障害物情報取得部７６による側面Ｐｂの位置及び姿勢の特定方法は以上の説明に限られず、障害物情報取得部７６は、計測点Ｍａ（点群Ｍ０ａ）の位置に基づいて、任意の方法で側面Ｐｂの位置及び姿勢を特定してよい。例えば、本実施形態では、障害物情報取得部７６は、重畳した計測点Ｍａに基づき側面Ｐｂの位置及び姿勢を特定したが、計測点Ｍａを重畳する処理は必須ではなく、センサ２６による１回の検出で取得された計測点Ｍａに基づき、側面Ｐｂの位置及び姿勢を特定してよい。

（回避パスの更新）
回避パス情報取得部７８は、障害物情報取得部７６が特定した障害物Ｐの側面Ｐｂの位置及び姿勢に基づき、回避パスＲ２を更新する。ここで、最初に生成された回避パスＲ２は、前面Ｐａの位置及び姿勢の特定結果に基づいて推定された側面Ｐｂの推定位置に基づき、設定されていた。一方、回避パス情報取得部７８は、更新する回避パスＲ２を、回避パスＲ２を移動中の検出により特定された障害物Ｐの側面Ｐｂの位置及び姿勢（線分Ｎｂ１）に基づき、設定する。すなわち、回避パス情報取得部７８は、側面Ｐｂの位置及び姿勢として、前面Ｐａの位置及び姿勢から推定されたものの代わりに、回避パスＲ２を移動中の検出により特定されたものを使用する以外は、最初に生成された回避パスＲ２と同様の方法で、更新する回避パスＲ２を設定する。

ただし、最初に生成された回避パスＲ２は、移動パスＲ１上での障害物Ｐの手前側の位置を開始地点とする軌道であるが、更新する回避パスＲ２は、回避パスＲ２を更新する時点での移動体１０の回避パスＲ２上の位置を開始地点とする軌道として設定されてよい。すなわち例えば、回避パスＲ２を更新する時点での移動体１０が、障害物Ｐよりも第１方向側に到達している場合には、第１方向側に向かう軌道Ｒ２Ａを設定し直すことなく、側面Ｐｂとの干渉を避けながら第２方向側に向かいつつ、障害物Ｐよりも移動パスＲ１の進行方向側（Ｘ方向側）に到達する軌道Ｒ２Ｂを、更新する回避パスＲ２として設定し直してもよい。

（回避パスに沿った移動）
図１７は、回避パスの更新の一例を説明するための模式図である。回避パス情報取得部７８によって回避パスＲ２が更新されたら、移動制御部７２は、回避パスＲ２を更新して、更新した回避パスＲ２に沿って移動体１０を移動させる。更新した回避パスＲ２は、センサ２６の検出結果から特定された側面Ｐｂに基づき設定されるため、更新した回避パスＲ２を用いることで、移動体１０が側面Ｐｂと干渉することを適切に抑制できる。さらに、図１７に示すように、更新前の回避パスＲ２（図１７の破線）においては、方向Ｙ１側に突出しすぎる場合があるが、更新した回避パスＲ２（図１７の実線）は、センサ２６の検出結果から特定された側面Ｐｂを回避しつつ方向Ｙ１にできるだけ突出しないように生成されるため、方向Ｙ１側への突出を適切に抑制できる。

（処理フロー）
次に、制御装置２８による回避パスＲ２を更新するフローを説明する。図１８は、本実施形態に係る回避パスの更新フローを説明するフローチャートである。図１８に示すように、制御装置２８の検出制御部７４は、移動体１０が移動パスＲ１に沿って移動中に、センサ２６に、移動パスＲ１の進行方向側に障害物Ｐが存在するかを検出させる（ステップＳ１０）。そして、障害物Ｐを検出しない場合（ステップＳ１０；Ｎｏ）、ステップＳ１０に戻り移動パスＲ１に沿った移動と障害物Ｐの検出を続けさせる。一方、障害物を検出した場合（ステップＳ１０；Ｙｅｓ）、制御装置２８の障害物情報取得部７６は、センサ２６による検出結果に基づき、障害物Ｐの前面Ｐａの位置及び姿勢を特定する（ステップＳ１２）。本実施形態の例では、移動体１０を減速させつつセンサ２６に障害物Ｐを複数回検出させ、障害物情報取得部７６は、複数回の検出で取得された点群Ｍ０を重畳して、重畳した点群Ｍ０に基づき、障害物Ｐの前面Ｐａの位置及び姿勢を特定する。制御装置２８の回避パス情報取得部７８は、障害物Ｐの前面Ｐａの位置及び姿勢の特定結果に基づき、回避パスＲ２を設定し（ステップＳ１４）、制御装置２８は、回避パスＲ２に沿って移動体１０を移動させつつ、センサ２６に検出を行わせる（ステップＳ１６）。制御装置２８の障害物情報取得部７６は、回避パスＲ２を移動中のセンサ２６による検出結果に基づき、障害物Ｐの側面Ｐｂの位置及び姿勢を特定する（ステップＳ１８）。本実施形態の例では、回避パスＲ２を移動中にセンサ２６に障害物Ｐを複数回検出させ、障害物情報取得部７６は、複数回の検出で取得された点群Ｍ０ａを重畳して、重畳した点群Ｍ０ａに基づき、障害物Ｐの側面Ｐｂの位置及び姿勢を特定する。回避パス情報取得部７８は、障害物Ｐの側面Ｐｂの位置及び姿勢の特定結果に基づき、回避パスＲ２を更新する（ステップＳ２０）。制御装置２８は、更新した回避パスＲ２に切り替えて、更新した回避パスＲ２に沿って移動体１０を移動させる。

なお、回避パスＲ２を更新する回数は任意であってよく、１回だけ更新してもよいし、複数回更新してもよい。また、例えば障害物Ｐが多角形であり前面Ｐａや側面Ｐｂが複数ある場合には、制御装置２８は、それぞれの前面Ｐａ及び側面Ｐｂを上記の説明と同様の方法で特定して、それらを回避する回避パスＲ２を設定、更新してよい。

（効果）
以上説明したように、本実施形態に係る移動体１０は、センサ２６の検出により障害物Ｐの前面Ｐａを特定して回避パスＲ２を生成し、回避パスＲ２を移動中のセンサ２６の検出により障害物Ｐの側面Ｐｂを特定して回避パスＲ２を更新する。本実施形態によると、
側面Ｐｂを特定して回避パスＲ２を更新するため、側面Ｐｂへの衝突を回避できる適切なパスとすることができる。また、本実施形態においては、センサ２６の１回の検出による点群Ｍ０、Ｍ０ａから、障害物Ｐの位置及び姿勢の特定に適切な点群Ｍ０、Ｍ０ａを抽出し、抽出した点群Ｍ０、Ｍ０ａを検出毎に重畳して、障害物Ｐの位置及び姿勢を特定する。そのため、検出結果からノイズを除去しつつ、点群Ｍ０、Ｍ０ａの数を増やすことができるため、障害物Ｐの位置及び姿勢の特定精度を高くすることができる。また、障害物Ｐの位置及び姿勢の特定に適切な点群以外を除去するため、演算量を抑えて、演算負荷を低減することもできる。

なお、障害物Ｐの位置及び姿勢の特定結果を、複数の移動体で情報共有してもよい。すなわち、本実施形態においては、移動体１０が自身で障害物Ｐの位置及び姿勢を特定したが、例えば他の移動体などによって、その障害物Ｐの位置及び姿勢が特定済みである場合は、特定済みの障害物Ｐの位置及び姿勢の情報を取得して、回避パスＲ２を生成してもよい。この場合、移動体の制御装置は、障害物Ｐの前面Ｐａ及び側面Ｐｂの位置及び姿勢の特定結果を、情報処理装置１２や他の移動体などの外部機器に送信する。そして、移動体が回避パスＲ２を生成する際には、情報処理装置１２や他の移動体から障害物Ｐの位置及び姿勢の特定結果を取得して、回避パスＲ２を生成する。このように障害物Ｐの位置及び姿勢の特定結果を共有することで、回避パスＲ２の生成の際に障害物Ｐの位置及び姿勢を特定する演算が不要となり、演算負荷を低減できる。また、障害物Ｐを検出するために減速することも不要となり、作業効率を向上できる。

また、本実施形態では、移動体１０の制御装置２８が、センサ２６の検出結果から前面Ｐａの位置及び姿勢を特定し、前面Ｐａの位置及び姿勢から回避パスＲ２を生成し、回避パスＲ２を移動中のセンサ２６の検出結果から側面Ｐｂの位置及び姿勢を特定し、側面Ｐｂの位置及び姿勢から回避パスＲ２を更新していた。ただし、これらの処理は、移動体１０が実施することに限られず、例えば情報処理装置１２などの他の装置がこれらの処理の少なくとも一部を実行して、移動体１０は、その結果を取得するものであってもよい。すなわち例えば、移動体１０の障害物情報取得部７６は、障害物Ｐの検出結果に基づいて特定された前面Ｐａの位置及び姿勢の特定結果を取得するものであってよく、自身で前面Ｐａの位置及び姿勢を特定してもよいし、外部の装置から前面Ｐａの位置及び姿勢の特定結果を取得してもよい。また例えば、移動体１０の回避パス情報取得部７８は、前面Ｐａの位置及び姿勢の特定結果に基づき生成された回避パスＲ２の情報を取得するものであってよく、自身で回避パスＲ２を生成してもよいし、外部の装置から回避パスＲ２の情報を取得してもよい。回避パス情報取得部７８が外部の装置から回避パスＲ２の情報を取得する場合は、移動体１０は、前面Ｐａの位置及び姿勢から回避パスＲ２を生成する処理が不要となるため、前面Ｐａの位置及び姿勢の特定結果を取得する必要はないといえる。また例えば、移動体１０の障害物情報取得部７６は、回避パスＲ２を移動中の検出結果に基づいて特定された側面Ｐｂの位置及び姿勢の特定結果を取得するものであってよく、自身で側面Ｐｂの位置及び姿勢を特定してもよいし、外部の装置から側面Ｐｂの位置及び姿勢の特定結果を取得してもよい。また例えば、移動体１０の回避パス情報取得部７８は、側面Ｐｂの位置及び姿勢の特定結果に基づき更新された回避パスＲ２の情報を取得するものであってよく、自身で回避パスＲ２を更新してもよいし、外部の装置から更新された回避パスＲ２の情報を取得してもよい。回避パス情報取得部７８が外部の装置から更新された回避パスＲ２の情報を取得する場合は、移動体１０は、側面Ｐｂの位置及び姿勢から回避パスＲ２を更新する処理が不要となるため、側面Ｐｂの位置及び姿勢の特定結果を取得する必要はないといえる。

（第２実施形態）
次に、第２実施形態について説明する。第２実施形態においては、最初に回避パスＲ２を生成する際に、第１方向（回避する方向）を方向Ｙ１と方向Ｙ２のどちらにするかを決める処理を行う点で、第１実施形態とは異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。

第２実施形態に係る回避パス情報取得部７８は、障害物情報取得部７６によって特定された障害物Ｐの前面Ｐａの位置と、移動可能領域ＲＥの境界線とに基づき、第１方向を設定する。より詳しくは、回避パス情報取得部７８は、特定された障害物Ｐの前面Ｐａの位置と、移動可能領域ＲＥの境界線との距離に基づき、障害物Ｐの側面Ｐｂの位置を推定する。言い換えれば、回避パス情報取得部７８は、障害物Ｐの前面Ｐａの位置と移動可能領域ＲＥの境界線との距離に基づき、最初に設定する回避パスＲ２の第１方向を方向Ｙ１と方向Ｙ２のどちらにするかを判断し、その判断結果を加味できるように、障害物Ｐの側面Ｐｂの推定位置を設定する。以下、具体的に説明する。なお、移動可能領域ＲＥの境界線とは、移動可能領域ＲＥの範囲内と範囲外とを区切る線を指し、移動可能領域ＲＥの外縁ともいえる。

図１９及び図２０は、第２実施形態における障害物の側面の位置の推定方法を説明するための模式図である。第２実施形態に係る回避パス情報取得部７８は、特定した前面Ｐａ（線分Ｎａ１）の端点ＰＳ１ａと端点ＰＳ２ａとのうちで、移動パスＲ１上の移動体１０から離れている方の端点を、すなわち移動パスＲ１の進行方向側（Ｘ方向側）の端点を、基準点である第１端点として特定する。また、端点ＰＳ１ａと端点ＰＳ２ａとのうちで、第１端点ではない他方の端点を、すなわち移動パスＲ１の進行方向と反対側の端点を、第２端点とする。回避パス情報取得部７８は、第１端点と、第２端点から第１端点に向かう方向側の移動可能領域ＲＥの境界線（第１端点側の境界線）との距離Ｄ１を算出し、距離Ｄ１が所定の閾値以上であるかを判断する。以降では、端点ＰＳ２ａを第１端点とし端点ＰＳ１ａを第２端点とした場合を例にして説明を進める。この場合、回避パス情報取得部７８は、第１端点である端点ＰＳ２ａと、移動可能領域ＲＥの方向Ｙ１側（第１端点側）の境界線ＲＥ１との距離を、距離Ｄ１として算出し、距離Ｄ１が所定の閾値以上であるかを判断する。なお、閾値は任意に設定されてよいが、例えば移動体１０の横方向（幅方向）における長さに所定のマージンを加えた値としてよい。

（距離Ｄ１が閾値以上である場合）
図１９は、距離Ｄ１が閾値以上である場合の模式図である。距離Ｄ１が閾値以上であるには、回避パス情報取得部７８は、障害物Ｐの第２端点側の後方端点から、移動可能領域ＲＥの第２端点側の境界線までの距離Ｄ２が、所定値となるように、第２端点側の後方端点の推定位置を設定する。なお、第２端点側の後方端点とは、障害物Ｐの後面の両端点のうちで、第１端点から第２端点に向かう方向側の端点を指す。また、移動可能領域ＲＥの第２端点側の境界線とは、移動可能領域ＲＥの、第２端点から第１端点に向かう方向側の境界線を指す。従って、図１９の例においては、回避パス情報取得部７８は、第２端点側の後方端点である後方端点ＰＳ４ａから、方向Ｙ２側（第２端点側）の境界線ＲＥ２までの距離Ｄ２が、第１所定値となるように、後方端点ＰＳ４ａの推定位置を設定する。ここでの第１所定値は、上述の閾値より低い値であり、移動体１０の横方向（幅方向）における長さよりも低い値であることが好ましい。

第２端点側の後方端点の推定位置が設定されたら、回避パス情報取得部７８は、第１端点の位置、第２端点の位置、及び第２端点側の後方端点の推定位置に基づき、第１端点側の後方端点の推定位置を設定する。第１端点側の後方端点とは、障害物Ｐの後面の両端点のうちで、第２端点側の後方端点とは別の端点を指し、第２端点から第１端点に向かう方向側の端点を指すといえる。例えば、回避パス情報取得部７８は、第２端点側の後方端点から、第２端点と第１端点とを結ぶ線分Ｎ１ａの距離及び方向だけ離れた位置を、第１端点側の後方端点として設定する。回避パス情報取得部７８は、第１端点の位置、第２端点の位置、及び後方端点の推定位置に基づき、障害物Ｐの側面Ｐｂの推定位置を算出する。図１９の例では、回避パス情報取得部７８は、第１端点側の後方端点としての後方端点ＰＳ３ａの位置を算出し、後方端点ＰＳ３ａから端点ＰＳ２ａまでを結ぶ線分を、側面Ｐｂの推定位置とする。

回避パス情報取得部７８は、障害物Ｐの前面Ｐａ（線分Ｎａ１）と側面Ｐｂの推定位置とを回避しつつ、移動体１０が移動可能領域ＲＥの範囲内に位置するように、回避パスＲ２を生成する。図１９のように距離Ｄ１が閾値以上である場合においては、後方端点ＰＳ４ａから境界線ＲＥ２までの距離Ｄ２が、閾値未満である第１所定値として設定されている。そのため、後方端点ＰＳ４ａと境界線ＲＥ２との間を移動体１０が通ることができなくなる可能性があり、回避パス情報取得部７８は、後方端点ＰＳ４ａと境界線ＲＥ２との間を通る方向Ｙ２を第１方向（回避する方向）として回避パスＲ２を生成せずに、方向Ｙ１を第１方向として回避パスＲ２を生成することになる。すなわち、回避パス情報取得部７８は、第１端点と第１端点側の境界線との間の距離Ｄ１が閾値以上である場合においては、第２端点側の後方端点ＰＳ４ａと境界線ＲＥ２との距離Ｄ２を短く設定することで、方向Ｙ２（第２端点側の方向）を第１方向として選択させることを防止して、方向Ｙ１（第１端点側）を第１方向として選択する。このように、回避パス情報取得部７８は、第１端点と第１端点側の境界線との間の距離Ｄ１が閾値以上である場合においては、第２端点から第１端点に向かう方向を、回避パスＲ２の第１方向として設定するといえる。

（距離Ｄ１が閾値未満である場合）
図２０は、距離Ｄ１が閾値未満である場合の模式図である。距離Ｄ１が閾値未満であるには、回避パス情報取得部７８は、第２端点から第２端点側の後方端点までの長さＤ３が、第２所定値となるように、第２端点側の後方端点の推定位置を設定する。なお、第２所定値は任意の値に設定されてよいが、例えば第１端点から第２端点までの長さ（前面Ｐａの長さ）に基づき設定されてよく、第１端点から第２端点までの長さの２倍に設定されてよい。従って、図２０の例においては、回避パス情報取得部７８は、第２端点である端点ＰＳ１ａから、第２端点側の後方端点である後方端点ＰＳ４ａまでの長さＤ３が、第２所定値となるように、後方端点ＰＳ４ａの推定位置を設定する。ただし、このようにして設定された後方端点ＰＳ４ａ（第２端点側の後方端点）の推定位置と、第２端点側の境界線ＲＥ２との距離Ｄ２が、閾値未満となる場合には、回避パス情報取得部７８は、距離Ｄ２が閾値以上となるように、より好ましくは距離Ｄ２が閾値となるように、後方端点ＰＳ４ａ（第２端点側の後方端点）の推定位置を設定しなおしてよい。

第２端点側の後方端点の推定位置が設定されたら、回避パス情報取得部７８は、第１端点の位置、第２端点の位置、及び第２端点側の後方端点の推定位置に基づき、第１端点側の後方端点の推定位置を設定する。回避パス情報取得部７８は、第１端点の位置、第２端点の位置、及び後方端点の推定位置に基づき、障害物Ｐの側面Ｐｂの推定位置を算出する。図１９の例では、回避パス情報取得部７８は、後方端点ＰＳ４ａから端点ＰＳ１ａまでを結ぶ線分を、側面Ｐｂの推定位置とする。

回避パス情報取得部７８は、障害物Ｐの前面Ｐａ（線分Ｎａ１）と側面Ｐｂの推定位置とを回避しつつ、移動体１０が移動可能領域ＲＥの範囲内に位置するように、回避パスＲ２を生成する。図２０のように端点ＰＳ２ａと境界線ＲＥ１との距離Ｄ１が閾値未満である場合においては、端点ＰＳ２ａと境界線ＲＥ１との間を移動体１０が通ることができなくなる可能性がある。一方、距離Ｄ１が閾値未満である場合、後方端点ＰＳ４ａから境界線ＲＥ２までの距離Ｄ２が、閾値以上の値として設定される。従って、回避パス情報取得部７８は、端点ＰＳ２ａと境界線ＲＥ１との間を通る方向Ｙ１を第１方向（回避する方向）として回避パスＲ２を生成せずに、方向Ｙ２を第１方向として回避パスＲ２を生成することになる。すなわち、回避パス情報取得部７８は、第１端点と第１端点側の境界線との間の距離Ｄ１が閾値未満である場合においては、第２端点側の後方端点ＰＳ４ａと境界線ＲＥ２との距離Ｄ２を長く設定することで、方向Ｙ１（第１端点側の方向）を第１方向として選択させることを防止して、方向Ｙ２（第２端点側）を第１方向として選択する。このように、回避パス情報取得部７８は、第１端点と第１端点側の境界線との間の距離Ｄ１が閾値未満である場合においては、第１端点から第２端点に向かう方向を、回避パスＲ２の第１方向として設定するといえる。

第２実施形態においては、このように、障害物Ｐの前面Ｐａの位置と、移動可能領域ＲＥの境界線とに基づき、回避パスＲ２の第１方向を設定する。通常、障害物Ｐａの前面Ｐａを回避した後は、障害物Ｐの側面Ｐｂと接触することがないため、例えば移動可能領域ＲＥの境界線との距離が敷値以上である場合は、そちら側の経路を選択したほうが、より安全となる可能性が高いといえる。従って、第２実施形態のように、障害物Ｐの前面Ｐａの位置と移動可能領域ＲＥの境界線とに基づき、回避パスＲ２の第１方向を設定することで、障害物Ｐとの接触リスクがより低い回避方向を選択することが可能となり、より安全な回避パスを生成できる。

なお、移動パスＲ１の進行方向における端点ＰＳ１ａと端点ＰＳ２ａとの間の距離が所定距離未満である場合には、言い換えれば移動パスＲ１の進行方向に直交する方向に対する前面Ｐａ（線分Ｎａ１）の傾きが所定角度未満である場合には、以上の説明のように側面Ｐｂの推定位置を算出することなく、以下のようにして側面Ｐｂの推定位置を算出してよい。すなわち、移動パスＲ１の進行方向における端点ＰＳ１ａと端点ＰＳ２ａとの間の距離が所定距離未満である場合には、端点ＰＳ１ａから後方端点ＰＳ４ａまでの距離が第２所定値となり、端点ＰＳ２ａから後方端点ＰＳ３ａまでの距離が第２所定値となるように、後方端点ＰＳ３ａ、ＰＳ４ａの推定位置を算出する。そして、端点ＰＳ１ａと後方端点ＰＳ４ａの推定位置とを結ぶ線分と、端点ＰＳ２ａと後方端点ＰＳ３ａの推定位置とを結ぶ線分とを、側面Ｐｂの推定位置として算出する。回避パス情報取得部７８は、この側面Ｐｂの推定位置に基づき、第１実施形態と同様の方法で、回避パスＲ２を生成する。

（処理フロー）
次に、第２実施形態における回避パスの生成方法の処理フローを説明する。図２１は、第２実施形態における回避パスの生成フローを説明するフローチャートである。図２１に示すように、回避パス情報取得部７８は、障害物Ｐの前面Ｐａ（線分Ｎａ１）と、移動パスＲ１の進行方向に直交する方向とのなす角度が、所定角度以上であるかを判断する（ステップＳ３０）。すなわち、回避パス情報取得部７８は、障害物Ｐの傾き度合いを判断する。回避パス情報取得部７８は、所定角度以上である場合には（ステップＳ３０；Ｙｅｓ）、移動可能領域ＲＥの第１方向側の境界線ＲＥ１と、第１端点である端点ＰＳ２ａとの距離Ｄ１が閾値以上であるかを判断する（ステップＳ３２）。距離Ｄ１が閾値以上である場合（ステップＳ３２；Ｙｅｓ）、回避パス情報取得部７８は、第２端点側の後方端点ＰＳ４ａの推定位置から、第２端点側の境界線ＲＥ２までの距離Ｄ２が第１所定値となるように、第２端点側の後方端点ＰＳ４ａの推定位置を設定する（ステップＳ３４）。回避パス情報取得部７８は、後方端点ＰＳ４ａの推定位置に基づき側面Ｐｂの推定位置を算出して、回避パスＲ２を生成する（ステップＳ３６）。

一方、距離Ｄ１が閾値未満である場合（ステップＳ３２；Ｎｏ）、回避パス情報取得部７８は、第２端点側の後方端点ＰＳ４ａの推定位置から、第２端点である端点ＰＳ１ａまでの長さＤ３が第２所定値となるように、第２端点側の後方端点ＰＳ４ａの推定位置を設定する（ステップＳ３８）。回避パス情報取得部７８は、後方端点ＰＳ４ａの推定位置に基づき側面Ｐｂの推定位置を算出して、回避パスＲ２を生成する（ステップＳ３６）。

また、障害物Ｐの前面Ｐａ（線分Ｎａ１）と、移動パスＲ１の進行方向に直交する方向とのなす角度が所定角度未満である場合（ステップＳ３０；Ｎｏ）、回避パス情報取得部７８は、後方端点ＰＳ３ａ、ＰＳ４ａの推定位置から、第１端点及び第２端点（端点ＰＳ１ａ、ＰＳ２ａ）までの距離が第２所定値となるように、後方端点ＰＳ３ａ、ＰＳ４ａの推定位置を設定する（ステップＳ４０）。回避パス情報取得部７８は、後方端点ＰＳ３ａ、ＰＳ４ａの推定位置に基づき側面Ｐｂの推定位置を算出して、回避パスＲ２を生成する（ステップＳ３６）。

(本開示の効果)
以上説明したように、本開示に係る制御方法は、自動で移動する移動体１０の制御方法であって、移動体１０に設けられたセンサ２６に、障害物Ｐを検出させるステップと、障害物Ｐの検出結果に基づき、障害物Ｐの移動体１０の進行方向に対向する前面Ｐａの位置及び姿勢を特定するステップと、前面Ｐａの位置及び姿勢に基づき、進行方向に交差する第１方向側に向かいつつ障害物Ｐを回避する回避パスＲ２を生成するステップと、回避パスＲ２に沿って移動体１０を移動させるステップと、移動体１０が回避パスＲ２を移動中に、障害物Ｐを検出させるステップと、回避パスＲ２を移動中の検出結果に基づき、障害物Ｐの第１方向側の側面Ｐｂの位置及び姿勢を特定するステップと、側面Ｐｂの位置及び姿勢に基づき、障害物Ｐを回避しつつ第１方向と反対側の第２方向側に戻るように、回避パスＲ２を更新するステップと、を含む。本制御方法によると、側面Ｐｂを特定して回避パスＲ２を更新するため、側面Ｐｂへの衝突を適切に回避できるように回避パスＲ２を更新して、障害物Ｐを適切に回避させることができる。

回避パスＲ２を更新するステップにおいては、回避パスＲ２に切り替わる前に用いられていた移動パスＲ１の第２方向における位置と、回避パスＲ２の終点の第２方向における位置とが重なるように、回避パスＲ２を更新する。このように、回避パスＲ２の終点の第２方向における位置を移動パスＲ１と合わせることで、回避パスＲ２で障害物Ｐを回避した後に移動パスＲ１にスムーズに切り替えて、移動を続けることができる。

障害物Ｐを検出させるステップにおいては、センサ２６に障害物Ｐを複数回検出させて、センサ２６の複数回の検出結果を点群Ｍ０として取得する。そして、前面Ｐａの位置及び姿勢を特定するステップにおいては、センサ２６の複数の検出結果に対応する点群Ｍ０を同一座標系で重畳し、重畳した点群Ｍ０の位置に基づいて、前面Ｐａの位置及び姿勢を特定する。本制御方法によると、点群Ｍ０を重畳することで、点群Ｍ０の数を増やして、障害物Ｐの位置及び姿勢の特定精度を高くすることができる。

前面Ｐａの位置及び姿勢を特定するステップにおいては、重畳した点群Ｍ０の近似線Ｎａを算出し、重畳した点群Ｍ０を近似線Ｎａ上に射影することで、同一座標系の点群Ｍ０を近似線Ｎａの座標系の点群Ｍ０に変換し、近似線Ｎａの座標系における点群Ｍ０の位置に基づき、前面Ｐａの位置を特定し、近似線Ｎａに基づき、前面Ｐａの姿勢を特定する。本制御方法によると、このように近似線Ｎａと近似線Ｎａの座標系の点群Ｍ０を用いることで、障害物Ｐの位置及び姿勢の特定精度を高くすることができる。

前面Ｐａの位置及び姿勢を特定するステップにおいては、少なくとも１回の検出によって取得された点群Ｍ０に基づき、前面Ｐａに対応する直線Ｌａを算出し、直線Ｌａと点群Ｍ０との位置関係に基づき、同一座標系で重畳する点群Ｍ０を抽出する。本制御方法によると、このように前面Ｐａの特定に用いる点群Ｍ０を抽出することで、検出結果からノイズを除去して、障害物Ｐの位置及び姿勢の特定精度を高くすることができる。また、演算量を抑えて、演算負荷を低減することもできる。

本制御方法は、前面Ｐａの位置及び姿勢の特定結果と、側面Ｐｂの位置及び姿勢の特定結果とを、移動体１０以外の装置に出力するステップをさらに含む。本制御方法によると、障害物Ｐの位置及び姿勢の特定結果を他の移動体と共有することが可能となるため、回避パスＲ２の生成の際に障害物Ｐの位置及び姿勢を特定する演算が不要となり、演算負荷を低減できる。また、障害物Ｐを検出するために減速することも不要となり、作業効率を向上できる。

回避パスＲ２を生成するステップにおいては、移動体１０が移動可能な移動可能領域ＲＥと前面Ｐａの位置とに基づいて第１方向を設定する。本制御方法によると、回避方向である第１方向をこのように設定することで、障害物Ｐを適切に回避できる回避パスＲ２を生成することが可能となる。

回避パスＲ２を生成するステップにおいては、進行方向に交差して前面Ｐａの他方の端点（第２端点）から一方の端点（第１端点）に向かう方向側における移動可能領域ＲＥの境界線と、第１端点との距離Ｄ１が、閾値以上である場合には、前面Ｐａの第２端点から第１端点に向かう方向を、第１方向として設定する。また、境界線ＲＥと第１端点との距離Ｄ１が、閾値未満である場合には、第１端点から第２端点に向かう方向を、第１方向として設定する。本制御方法によると、回避方向である第１方向をこのように設定することで、障害物Ｐを適切に回避できる回避パスＲ２を生成することが可能となる。

本開示の移動体１０は、自動で移動するものであって、移動体１０に設けられたセンサ２６に、障害物Ｐを検出させる検出制御部７４と、障害物Ｐの検出結果から特定された移動体１０の進行方向に対向する前面Ｐａの位置及び姿勢に基づき生成された、進行方向に交差する第１方向側に向かいつつ障害物Ｐを回避する回避パスＲ２の情報を取得する回避パス情報取得部７８と、回避パスＲ２に沿って移動体１０を移動させる移動制御部７２とを含む。回避パスＲ２は、回避パスＲ２を移動中のセンサ２６による障害物Ｐの検出結果から、障害物Ｐの第１方向側の側面Ｐｂの位置及び姿勢が特定され、側面Ｐｂの位置及び姿勢の基づき、障害物Ｐを回避しつつ第２方向側に戻るように、更新される。この移動体１０によると、側面Ｐｂへの衝突を適切に回避できるように回避パスＲ２が更新されるため、障害物Ｐを適切に回避することができる。

本開示のプログラムは、自動で移動する移動体１０の制御方法をコンピュータに実行させるプログラムであって、移動体１０に設けられたセンサ２６に、障害物Ｐを検出させるステップと、障害物Ｐの検出結果に基づき、障害物Ｐの移動体１０の進行方向に対向する前面Ｐａの位置及び姿勢を特定するステップと、前面Ｐａの位置及び姿勢に基づき、進行方向に交差する第１方向側に向かいつつ障害物Ｐを回避する回避パスＲ２を生成するステップと、回避パスＲ２に沿って移動体１０を移動させるステップと、移動体１０が回避パスＲ２を移動中に、障害物Ｐを検出させるステップと、回避パスＲ２を移動中の検出結果に基づき、障害物Ｐの第１方向側の側面Ｐｂの位置及び姿勢を特定するステップと、側面Ｐｂの位置及び姿勢に基づき、障害物Ｐを回避しつつ第１方向と反対側の第２方向側に戻るように、回避パスＲ２を更新するステップと、をコンピュータに実行させる。本プログラムによると、側面Ｐｂを特定して回避パスＲ２を更新するため、側面Ｐｂへの衝突を適切に回避できるように回避パスＲ２を更新して、障害物Ｐを適切に回避させることができる。

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

１０ 移動体
２６ センサ
７０ 移動パス情報取得部
７２ 移動制御部
７４ 検出制御部
７６ 障害物情報取得部
７８ 回避パス情報取得部
Ｍ、Ｍａ 計測点
Ｍ０、Ｍ０ａ 点群
Ｐ 障害物
Ｐａ 前面
Ｐｂ 側面
Ｒ１ 移動パス
Ｒ２ 回避パス

Claims (9)

1. 自動で移動する移動体の制御方法であって、
   前記移動体に設けられたセンサに、障害物を検出させるステップと、
   前記障害物の検出結果に基づき、前記障害物の前記移動体の進行方向に対向する前面の位置及び姿勢を特定するステップと、
   前記前面の位置及び姿勢に基づき、前記進行方向に交差する第１方向側に向かいつつ前記障害物を回避する回避パスを生成するステップと、
   前記回避パスに沿って前記移動体を移動させるステップと、
   前記移動体が前記回避パスを移動中に、前記障害物を検出させるステップと、
   前記回避パスを移動中の検出結果に基づき、前記障害物の前記第１方向側の側面の位置及び姿勢を特定するステップと、
   前記側面の位置及び姿勢に基づき、前記障害物を回避しつつ、できるだけ前記第１方向側に突出しないで前記第１方向と反対側の第２方向側に戻るように、前記回避パスを更新するステップと、
   を含み、
   前記障害物を検出させるステップにおいては、前記センサに前記障害物を複数回検出させて、前記センサの複数回の検出結果を点群として取得し、
   一以上の直線候補を取得し、
   前記直線候補から規定距離以上の手前側の前記点群については他の前記点群よりも優先度が低い定数のスコアを付与し、少なくとも１回の検出によって取得された前記点群について前記スコアを積算したスコア積算値を算出し、前記スコア積算値に基づいて前記直線候補から直線を選択し、
   前記直線と前記センサの複数の検出結果に対応する点群との位置関係に基づき、前記センサの複数の検出結果に対応する点群のうちから、同一座標系で重畳する点群を抽出し、
   前記前面の位置及び姿勢を特定するステップ及び前記側面の位置及び姿勢を特定するステップにおいては、抽出した前記センサの複数の検出結果に対応する点群を前記前面と前記側面とのそれぞれの同一座標系で重畳し、
   重畳した前記点群のそれぞれの近似線を算出し、
   重畳した前記点群を前記それぞれの近似線上に射影することで、前記それぞれの同一座標系の点群を前記それぞれの近似線の座標系の点群に変換する、
   移動体の制御方法。
2. 前記回避パスを更新するステップにおいては、前記回避パスに切り替わる前に用いられていた移動パスの前記第２方向における位置と、前記回避パスの終点の前記第２方向における位置とが重なるように、前記回避パスを更新する、請求項１に記載の移動体の制御方法。
3. 前記前面の位置及び姿勢を特定するステップにおいては、
   少なくとも１回の検出によって取得された前記点群に基づき、前記前面に対応する直線を算出し、
   前記直線と前記点群との位置関係に基づき、前記同一座標系で重畳する点群を抽出する、請求項１に記載の移動体の制御方法。
4. 前記前面の位置及び姿勢を特定するステップにおいては、前記直線に対して所定の距離範囲内にある前記点群を、前記同一座標系で重畳する点群として抽出する、請求項３に記載の移動体の制御方法。
5. 前記前面の位置及び姿勢の特定結果と、前記側面の位置及び姿勢の特定結果とを、前記移動体以外の装置に出力するステップをさらに含む、請求項１から請求項４のいずれか１項に記載の移動体の制御方法。
6. 前記回避パスを生成するステップにおいては、
   前記移動体が移動可能な移動可能領域と前記前面の位置とに基づいて前記第１方向を設定し、設定した前記第１方向側に向かいつつ前記障害物を回避するように、前記回避パスを生成する、請求項１から請求項５のいずれか１項に記載の移動体の制御方法。
7. 前記回避パスを生成するステップにおいては、
   前記進行方向に交差して前記前面の他方の端点から一方の端点に向かう方向側における前記移動可能領域の境界線と、前記一方の端点との距離が、閾値以上である場合には、前記前面の他方の端点から一方の端点に向かう方向を、前記第１方向として設定し、
   前記境界線と前記一方の端点との距離が、前記閾値未満である場合には、前記一方の端点から他方の端点に向かう方向を、前記第１方向として設定する、請求項６に記載の移動体の制御方法。
8. 自動で移動する移動体であって、
   前記移動体に設けられたセンサに、障害物を検出させる検出制御部と、
   前記障害物の検出結果から特定された前記移動体の進行方向に対向する前面の位置及び姿勢に基づき生成された、前記進行方向に交差する第１方向側に向かいつつ前記障害物を回避する回避パスの情報を取得する回避パス情報取得部と、
   前記回避パスに沿って前記移動体を移動させる移動制御部と、
   を含み、
   前記回避パスは、
   前記回避パスを移動中の前記センサによる前記障害物の検出結果から、前記障害物の前記第１方向側の側面の位置及び姿勢が特定され、
   前記側面の位置及び姿勢に基づき、前記障害物を回避しつつ、できるだけ前記第１方向側に突出しないで前記第１方向と反対側の第２方向側に戻るように、更新され、
   前記検出制御部は、前記センサに前記障害物を複数回検出させて、前記センサの複数回の検出結果を点群として取得し、
   一以上の直線候補を取得し、
   前記直線候補から規定距離以上の手前側の前記点群については他の前記点群よりも優先度が低い定数のスコアを付与し、少なくとも１回の検出によって取得された前記点群について前記スコアを積算したスコア積算値を算出し、前記スコア積算値に基づいて前記直線候補から直線を選択し、
   前記直線と前記センサの複数の検出結果に対応する点群との位置関係に基づき、前記センサの複数の検出結果に対応する点群のうちから、同一座標系で重畳する点群を抽出し、
   抽出した前記センサの複数の検出結果に対応する点群を前記前面と前記側面とのそれぞれの同一座標系で重畳し、
   重畳した前記点群のそれぞれの近似線を算出し、
   重畳した前記点群を前記それぞれの近似線上に射影することで、前記それぞれの同一座標系の点群を前記それぞれの近似線の座標系の点群に変換する、
   移動体。
9. 自動で移動する移動体の制御方法をコンピュータに実行させるプログラムであって、
   前記移動体に設けられたセンサに、障害物を検出させるステップと、
   前記障害物の検出結果に基づき、前記移動体の進行方向に対向する前面の位置及び姿勢を特定するステップと、
   前記前面の位置及び姿勢に基づき、前記進行方向に交差する第１方向側に向かいつつ前記障害物を回避する回避パスを生成するステップと、
   前記回避パスに沿って前記移動体を移動させるステップと、
   前記移動体が前記回避パスを移動中に、前記障害物を検出させるステップと、
   前記回避パスを移動中の検出結果に基づき、前記障害物の前記第１方向側の側面の位置及び姿勢を特定するステップと、
   前記側面の位置及び姿勢に基づき、前記障害物を回避しつつ、できるだけ前記第１方向側に突出しないで前記第１方向と反対側の第２方向側に戻るように、前記回避パスを更新するステップと、
   を含み、
   前記障害物を検出させるステップにおいては、前記センサに前記障害物を複数回検出させて、前記センサの複数回の検出結果を点群として取得し、
   一以上の直線候補を取得し、
   前記直線候補から規定距離以上の手前側の前記点群については他の前記点群よりも優先度が低い定数のスコアを付与し、少なくとも１回の検出によって取得された前記点群について前記スコアを積算したスコア積算値を算出し、前記スコア積算値に基づいて前記直線候補から直線を選択し、
   前記直線と前記センサの複数の検出結果に対応する点群との位置関係に基づき、前記センサの複数の検出結果に対応する点群のうちから、同一座標系で重畳する点群を抽出し、
   前記前面の位置及び姿勢を特定するステップ及び前記側面の位置及び姿勢を特定するステップにおいては、抽出した前記センサの複数の検出結果に対応する点群を前記前面と前記側面とのそれぞれの同一座標系で重畳し、
   重畳した前記点群のそれぞれの近似線を算出し、
   重畳した前記点群を前記それぞれの近似線上に射影することで、前記それぞれの同一座標系の点群を前記それぞれの近似線の座標系の点群に変換すること、
   を、コンピュータに実行させる、
   プログラム。
   

Images