JP2023030983A - フォークリフト - Google Patents

Abstract

【課題】積付位置を検出すること。【解決手段】フォークリフトは、外界センサと、制御装置と、を備える。制御装置は、点群データから水平面を表す点を抽出する。制御装置は、水平面から上下方向に所定範囲の点を、荷台を表す点として抽出する。制御装置は、荷台を表す点から、荷台の縁であってフォークリフトの正面にある縁を表す点を抽出する。制御装置は、荷台の縁を表す点から縁を表す直線を検知する。制御装置は、水平面から上方に所定距離以上離れた点を荷台に積載された積載物を表す点として抽出する。制御装置は、直線の延びる方向に積載物から規定距離離れた位置を荷の積み付けを行う積付位置として検出する。【選択図】図５

Description

本開示は、フォークリフトに関する。

荷台への荷の積み付けを行うフォークリフトは、フォークに積載した荷を荷台まで搬送し、荷台の上面である積載面に荷の積み付けを行う。特許文献１に開示のフォークリフトは、積載面の高さを検出している。これにより、積載面の高さに合わせてフォークを上昇させて、フォークに積載した荷を積載面に積み付けすることができる。

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

特許文献１に開示のフォークリフトは、積載面の高さを検出することができる。しかしながら、積載面には積載物が積載されている場合がある。このような場合、フォークに積載された荷を積載面に積み付けることができない場合がある。

上記課題を解決するフォークリフトは、荷台の上面である積載面に荷の積み付けを行うフォークリフトであって、３次元座標系の座標で物体の位置を検出する外界センサと、制御装置と、を備え、前記制御装置は、前記物体の位置を表した点の集合である点群データから水平面を表す前記点を抽出し、前記水平面から上下方向に所定範囲の前記点を、前記荷台を表す前記点として抽出し、前記荷台を表す前記点から前記荷台の縁であって前記フォークリフトの正面にある縁を表す前記点を抽出し、前記縁を表す前記点から前記縁を表す直線を検知し、前記水平面から上方に所定距離以上離れた前記点を前記荷台に積載された積載物を表す前記点として抽出し、前記直線の延びる方向に前記積載物から規定距離離れた位置を前記荷の積み付けを行う積付位置として検出する。

積載面は、水平方向に拡がる面である。水平面から上下方向に所定範囲の点を抽出すると、荷台を表す点を抽出することができる。荷台を表す点のうちフォークリフトの正面にある縁を表す点は、フォークリフトが積み付けをする際に、フォークリフトが近付く縁である。即ち、この縁は、積付位置に沿う縁である。制御装置は、この縁の位置を直線として検知する。水平面から上方に所定距離以上離れた点は、荷台に積載された積載物を表す点である。制御装置は、直線の延びる方向に積載物から規定距離離れた位置を積付位置として検出する。この積付位置は、荷台において積載物から離れた位置である。このため、制御装置は、積載面に積載された積載物とフォークに積まれた荷とが干渉しにくい位置を積付位置として検出することができる。

上記フォークリフトについて、前記制御装置は、前記点群データの各点の法線ベクトルを導出し、前記法線ベクトルの向きが上下方向の前記点を抽出し、当該抽出された前記点から平面方程式を導出し、前記平面方程式で表される平面を前記水平面としてもよい。

上記フォークリフトについて、前記制御装置は、前記縁を表す前記点に対して、複数の点を１つのクラスタとしてグルーピングするクラスタリングを行ってもよい。
上記フォークリフトについて、前記フォークリフトは、前記外界センサとは異なる自己位置推定用外界センサと、前記自己位置推定用外界センサから前記フォークリフトの自己位置を推定する自己位置推定用制御装置と、を備えていてもよい。

本発明によれば、積付位置を検出することができる。

トラック、及びフォークリフトを模式的に示す図である。 トラックの斜視図である。 フォークリフトの概略構成図である。 自己位置推定制御の処理手順を示すフローチャートである。 積付位置検出制御の処理手順を示すフローチャートである。 積付位置検出制御で得られる第１点群データの一例を示す図である。 積付位置検出制御で得られる第２点群データの一例を示す図である。 積付位置検出制御で得られる第３点群データの一例を示す図である。 積付位置検出制御で得られる第４点群データの一例を示す図である。 積載物検出制御の処理手順を示すフローチャートである。 積載物検出制御で得られる第５点群データの一例を示す図である。

フォークリフトの一実施形態について説明する。
図１に示すように、区域Ａ１には、トラック１０が停車される。本実施形態のトラック１０は、ウィングトラックである。区域Ａ１では、フォークリフト２０が運用されている。フォークリフト２０は、フォークＦ１を備える。フォークＦ１には、荷Ｃ１が積載される。フォークリフト２０は、フォークＦ１に積載された荷Ｃ１をトラック１０に積み付ける。

図２に示すように、トラック１０は、キャビン１１と、フロントパネル１２と、リヤドア１３と、荷台１４と、ウィングサイドパネル１６と、あおり１７と、を備える。
キャビン１１は、トラック１０の運転者が搭乗する位置である。フロントパネル１２は、キャビン１１よりも後方に設けられている。フロントパネル１２は、キャビン１１に隣り合って設けられている。リヤドア１３は、フロントパネル１２よりも後方に設けられている。フロントパネル１２とリヤドア１３は、前後方向に互いに間隔を空けて設けられている。荷台１４は、フロントパネル１２とリヤドア１３との間で、前後方向に延びている。荷台１４は、積載面１５を備える。積載面１５は、荷台１４の上面である。積載面１５には、荷Ｃ１が積み付けられる。ウィングサイドパネル１６は、フロントパネル１２とリヤドア１３との間に設けられている。ウィングサイドパネル１６は、トラック１０の車幅方向の中心位置を中心として、上下方向に回転可能に設けられている。ウィングサイドパネル１６は、トラック１０の車幅方向の両側に１つずつ設けられている。図２では、説明の便宜上、ウィングサイドパネル１６の１つを省略して図示している。トラック１０に積み付けをする際には、少なくとも１つのウィングサイドパネル１６が開かれる。あおり１７は、前後方向に延びるように設けられている。あおり１７は、荷台１４の縁Ｅ１であって前後方向に延びる縁Ｅ１に沿って設けられている。あおり１７は、トラック１０の車幅方向の両側に１つずつ設けられている。

図３に示すように、フォークリフト２０は、自己位置推定用外界センサ２１と、自己位置推定用制御装置２２と、補助記憶装置２５と、外界センサ３１と、制御装置３２と、車両制御装置４１と、走行アクチュエータ４４と、荷役アクチュエータ４５と、を備える。

自己位置推定用外界センサ２１は、フォークリフト２０の周辺に存在する物体の３次元座標を自己位置推定用制御装置２２に認識させることができるセンサである。自己位置推定用外界センサ２１としては、例えば、ミリ波レーダー、ステレオカメラ、ToF（Time of Flight）カメラ、及びLIDAR（Laser Imaging Detection and Ranging）を挙げることができる。本実施形態では、自己位置推定用外界センサ２１としてLIDARを用いている。自己位置推定用外界センサ２１は、周囲にレーザーを照射し、レーザーが当たった点から反射された反射光を受光することで点までの距離を導出する。レーザーが当たった点は、物体の表面の一部を表す。点の位置は、極座標系の座標で表すことができる。極座標系における点の座標は、直交座標系の座標に変換される。極座標系から直交座標系への変換は、自己位置推定用外界センサ２１によって行われてもよいし、自己位置推定用制御装置２２で行われてもよい。本実施形態では、自己位置推定用外界センサ２１により極座標系から直交座標系への変換が行われているとする。自己位置推定用外界センサ２１は、自己位置推定用センサ座標系での点の座標を導出する。自己位置推定用センサ座標系は、自己位置推定用外界センサ２１を原点とする３軸直交座標系である。自己位置推定用外界センサ２１は、レーザーを照射することにより得られた複数の点の座標を点群データとして自己位置推定用制御装置２２に出力する。

自己位置推定用制御装置２２は、プロセッサ２３と、記憶部２４と、を備える。記憶部２４は、RAM（Random Access Memory）、ROM（Read Only Memory）、及び書き換え可能な不揮発性記憶装置を含む。記憶部２４は、処理をプロセッサ２３に実行させるように構成されたプログラムコードまたは指令を格納している。記憶部２４、即ち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆる利用可能な媒体を含む。自己位置推定用制御装置２２は、ASICやFPGA等のハードウェア回路によって構成されていてもよい。処理回路である自己位置推定用制御装置２２は、コンピュータプログラムに従って動作する１つ以上のプロセッサ、ASICやFPGA等の１つ以上のハードウェア回路、或いは、それらの組み合わせを含み得る。

補助記憶装置２５は、自己位置推定用制御装置２２が読み取り可能な情報を記憶している。補助記憶装置２５としては、例えば、ハードディスクドライブ、及びソリッドステートドライブを挙げることができる。

補助記憶装置２５は、フォークリフト２０が用いられる区域Ａ１の環境を示す環境地図を記憶している。環境地図とは、区域Ａ１に存在する物体の形状、区域Ａ１の広さ等、区域Ａ１の物理的構造に関する情報である。本実施形態において環境地図は、区域Ａ１の構造を地図座標系の座標で表したデータである。地図座標系は、３軸直交座標系である。地図座標系は、区域Ａ１の任意の一点を原点とする座標系である。地図座標系において水平方向は互いに直交するＸ軸及びＹ軸で規定される。Ｘ軸及びＹ軸で規定されるＸＹ平面は、水平面を表しているといえる。地図座標系において上下方向は、Ｘ軸及びＹ軸に直交するＺ軸で規定される。適宜、地図座標系の座標を地図座標と称する。地理座標系は、３次元の位置を表す３次元座標系である。

自己位置推定用制御装置２２は、フォークリフト２０の自己位置を推定する。自己位置とは、環境地図上でのフォークリフト２０の位置である。自己位置とは、地図座標系でのフォークリフト２０の一点を示す座標である。フォークリフト２０の一点は任意であるが、例えば、フォークリフト２０の水平方向での中心位置を挙げることができる。自己位置推定用制御装置２２は、自己位置推定制御を行う。

自己位置推定用制御装置２２によって行われる自己位置推定制御について説明する。自己位置推定制御は、所定の制御周期で繰り返し実行される。
図４に示すように、ステップＳ１において、自己位置推定用制御装置２２は、自己位置推定用外界センサ２１の検出結果を取得する。これにより、自己位置推定用制御装置２２は、フォークリフト２０の周囲の形状を点群データとして取得することができる。

次に、ステップＳ２において、自己位置推定用制御装置２２は、点群データと環境地図とを照合することで、自己位置を推定する。自己位置推定用制御装置２２は、点群データから得られたランドマークと同一形状のランドマークを環境地図から抽出する。自己位置推定用制御装置２２は、環境地図からランドマークの位置を認識する。ランドマークの位置とフォークリフト２０との位置関係は、自己位置推定用外界センサ２１の検出結果から把握できる。従って、自己位置推定用制御装置２２は、ランドマークの位置を認識することで、自己位置を推定することができる。ランドマークとは自己位置推定用外界センサ２１により識別可能な特徴を有する物体である。ランドマークは、位置の変化しにくい物理的構造物である。ランドマークとしては、例えば、壁、及び柱を挙げることができる。ステップＳ２の処理を終えると、自己位置推定用制御装置２２は、自己位置推定制御を終了する。

外界センサ３１は、フォークリフト２０の周辺に存在する物体の３次元座標を制御装置３２に認識させることができるセンサである。外界センサ３１としては、自己位置推定用外界センサ２１と同様のセンサを用いることができる。本実施形態では、外界センサ３１としてLIDARを用いている。自己位置推定用外界センサ２１と、外界センサ３１とは、垂直方向のFOV(Field of View）が異なっている。外界センサ３１の垂直方向のFOVは、自己位置推定用外界センサ２１の垂直方向のFOVよりも広い。即ち、外界センサ３１は、自己位置推定用外界センサ２１に比べて、上下方向に対して広い検出範囲を備える。外界センサ３１は、センサ座標系での点の座標を導出する。センサ座標系は、外界センサ３１を原点とする３軸直交座標系である。センサ座標系において、水平方向は互いに直交するＸ軸及びＹ軸で規定される。Ｘ軸及びＹ軸で規定されるＸＹ平面は、水平面を表しているといえる。センサ座標系において上下方向は、Ｘ軸及びＹ軸に直交するＺ軸で規定される。センサ座標系は、３次元の位置を表す３次元座標系である。センサ座標系での点の座標は、物体の位置を表しているといえる。外界センサ３１は、レーザーを照射することにより得られた複数の点の座標を点群データとして制御装置３２に出力する。点は、物体の位置を表す。点群データは、物体の位置を３次元座標で表した点の集合である。

制御装置３２は、例えば、自己位置推定用制御装置２２と同様のハードウェア構成を備える。制御装置３２は、プロセッサ３３と、記憶部３４と、を備える。制御装置３２は、積付位置を検出する。積付位置とは、積載面１５上の位置であってフォークＦ１に積載された荷Ｃ１を積み付ける位置である。

車両制御装置４１は、例えば、自己位置推定用制御装置２２と同様のハードウェア構成を備える。車両制御装置４１は、プロセッサ４２と、記憶部４３と、を備える。車両制御装置４１は、自己位置推定用制御装置２２によって推定される自己位置、及び制御装置３２によって検出される積付位置を取得可能である。車両制御装置４１は、自己位置、及び積付位置に基づいて走行アクチュエータ４４、及び荷役アクチュエータ４５を制御する。

走行アクチュエータ４４は、フォークリフト２０を走行させるアクチュエータである。走行アクチュエータ４４は、例えば、駆動輪を回転させるモータ、及び操舵機構を含む。車両制御装置４１は、自己位置を把握しながら走行アクチュエータ４４を制御してフォークリフト２０を走行させる。車両制御装置４１は、荷Ｃ１の積み付けを行う際には、自己位置を把握しながら、フォークリフト２０を積付位置に向けて移動させる。

荷役アクチュエータ４５は、フォークリフト２０に荷役を行わせるアクチュエータである。荷役アクチュエータ４５は、例えば、油圧機器に作動油を供給するポンプを駆動するモータ、及び作動油の供給を制御する制御弁を含む。車両制御装置４１は、荷役アクチュエータ４５を制御することによってフォークＦ１の昇降、及びフォークＦ１の傾動を行う。荷Ｃ１を積付位置に積み付ける際には、荷Ｃ１が積付位置に積み付けられるようにフォークＦ１の昇降、及びフォークＦ１の傾動が行われる。

フォークリフト２０は、車両制御装置４１によって走行アクチュエータ４４が制御されることによって自動で走行する。フォークリフト２０は、車両制御装置４１によって荷役アクチュエータ４５が制御されることによって自動で荷役を行う。フォークリフト２０は、自動運転フォークリフトである。

制御装置３２が行う積付位置検出制御について説明する。積付位置検出制御は、積付位置を検出する制御である。一例として、図２に示すトラック１０の積付位置を検出する場合について説明する。トラック１０の荷台１４には、既に積載物Ｃ２が積載されているとする。フォークリフト２０は、例えば、上位制御装置の指令に基づき、トラック１０に近付く。そして、フォークリフト２０とトラック１０との離間距離が所定離間未満になると、積付位置検出制御が開始される。所定離間距離は、例えば、外界センサ３１によってトラック１０が検出できる距離である。

図５及び図６に示すように、ステップＳ１１において、制御装置３２は、点群処理を行う。点群処理は、外界センサ３１の検出結果から得られた点群データを重ね合わせる処理である。制御装置３２は、フォークリフト２０が移動しているときに外界センサ３１から複数回点群データを取得する。制御装置３２は、自己位置推定用制御装置２２によって推定された自己位置から点群データの各点Ｐ１の座標をセンサ座標系の座標から地図座標に変換する。自己位置推定用制御装置２２によって推定された自己位置から、地図座標系におけるセンサ座標系の原点を認識できる。自己位置推定用制御装置２２によって推定された自己位置から、地図座標系の座標軸とセンサ座標系の座標軸とのずれを認識できる。制御装置３２は、地図座標系におけるセンサ座標系の原点と、地図座標系の座標軸とセンサ座標系の座標軸とのずれと、に基づき点群データの各点Ｐ１をセンサ座標系の座標から地図座標に変換する。制御装置３２は、点群データを取得する度に、地図座標に変換された各点Ｐ１を重ね合わせることで第１点群データＰＧ１を生成する。第１点群データＰＧ１は、点群データの集合である。このため、１つの点群データの点Ｐ１に比べて、第１点群データＰＧ１の点Ｐ１は密である。

図６には、ステップＳ１１の処理によって得られた第１点群データＰＧ１を示す。第１点群データＰＧ１に含まれる各点Ｐ１は、物体の地図座標を表している。説明の便宜上、第１点群データＰＧ１に含まれる各点Ｐ１を第１点Ｐ１１、第２点Ｐ１２及び第３点Ｐ１３に分類して説明を行う。

第１点Ｐ１１は、荷台１４の積載面１５にレーザーが照射されることで得られた点Ｐ１である。第１点Ｐ１１は、荷台１４の積載面１５の地図座標を表しているといえる。説明の便宜上、第１点Ｐ１１は黒丸で図示している。

第２点Ｐ１２は、荷台１４の積載面１５に置かれている積載物Ｃ２にレーザーが照射されることで得られた点Ｐ１である。第２点Ｐ１２は、荷台１４の積載面１５に置かれた積載物Ｃ２の地図座標を表しているといえる。説明の便宜上、第２点Ｐ１２は斜線を付した丸で図示している。

第３点Ｐ１３は、第１点Ｐ１１及び第２点Ｐ１２のいずれにも該当しない点Ｐ１である。説明の便宜上、第３点Ｐ１３は白丸で図示している。
図５に示すように、次に、ステップＳ１２において、制御装置３２は、第１点群データＰＧ１から水平面を表す点Ｐ１の抽出を行う。制御装置３２は、各点Ｐ１の法線ベクトルを算出する。法線ベクトルは、複数の点Ｐ１に囲まれる平面に対して垂直な方向に向けたベクトルである。法線ベクトルを導出する手法としては各点Ｐ１から曲面を求め、曲面から各点Ｐ１の法線ベクトルを導出する手法や、ベクトルの外積を用いる手法を挙げることができる。例えば、制御装置３２が、１つの点Ｐ１の法線ベクトルを求める場合、この点Ｐ１から所定範囲内に位置する２つの点Ｐ１のそれぞれに向かうベクトルの外積を求める。この外積が法線ベクトルである。

制御装置３２は、法線ベクトルの向きが上下方向の点Ｐ１を抽出する。詳細にいえば、制御装置３２は、地図座標系のＸＹ平面に対する法線ベクトルの角度が予め定められた範囲となるか否かを判定する。点Ｐ１が水平面を表す点Ｐ１であれば、ＸＹ平面に対する当該点Ｐ１の法線ベクトルの角度は９０°となる。本実施形態では、トラック１０の停車位置による荷台１４の傾斜や外界センサ３１の精度等に基づき予め定められた範囲を設定している。範囲は、例えば、９０°±所定角度である。所定角度は、例えば、１０°である。制御装置３２は、ＸＹ平面に対する法線ベクトルの角度が予め定められた範囲に収まる点Ｐ１を、法線ベクトルの向きが上下方向の点Ｐ１として抽出する。この点Ｐ１が、水平面を表す点Ｐ１である。

図７には、ステップＳ１２の処理によって得られた第２点群データＰＧ２を示す。第２点群データＰＧ２は、第１点群データＰＧ１から水平面を表す点Ｐ１を抽出したデータである。第２点群データＰＧ２には、第１点Ｐ１１及び第２点Ｐ１２が含まれる。即ち、第１点群データＰＧ１から、積載面１５を表す点Ｐ１、及び積載物Ｃ２における水平面を表す点Ｐ１が抽出される。

図５に示すように、次に、ステップＳ１３において、制御装置３２は、荷台１４を表す点Ｐ１を抽出する。制御装置３２は、ステップＳ１２で抽出された水平面を表す点Ｐ１から、平面方程式を導出する。制御装置３２は、ステップＳ１２で抽出された各点Ｐ１の地図座標（ｘ，ｙ，ｚ）に基づいて、ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０を満たすようなａ、ｂ、ｃ、ｄの値を算出する。平面方程式は、例えば、RANSAC（Random Sample Consensus）等のロバスト推定法や、最小二乗法を用いることで導出することができる。平面方程式で表される平面は、水平面を表しているといえる。制御装置３２は、平面方程式で表される平面を水平面とする。

制御装置３２は、平面方程式で表される水平面から上下方向に所定範囲の点Ｐ１を第１点群データＰＧ１から抽出する。水平面から上下方向に所定範囲の点Ｐ１は、水平面から地図座標系のＺ軸の両側に対して所定範囲の点Ｐ１といえる。これにより得られる点Ｐ１は、荷台１４、詳細には、荷台１４の積載面１５を表しているといえる。所定範囲としては、例えば、積載面１５に積まれている積載物Ｃ２を第１点群データＰＧ１から除外できるように設定されている。

図８には、ステップＳ１３の処理によって得られた第３点群データＰＧ３を示す。第３点群データＰＧ３は、平面方程式で表される水平面から上下方向に対して所定範囲の点Ｐ１を第１点群データＰＧ１から抽出したデータである。第３点群データＰＧ３には、第１点Ｐ１１に加えて、キャビン１１にレーザーが照射されることで生じた第３点Ｐ１３が含まれている。

図５に示すように、次に、ステップＳ１４において、制御装置３２は、荷台１４の縁Ｅ１であってフォークリフト２０の正面にある縁Ｅ１を表す点Ｐ１を抽出する。フォークリフト２０の正面にある縁Ｅ１は、フォークリフト２０が荷Ｃ１の積み付けをする際に荷Ｃ１が上方を通過する縁Ｅ１ともいえる。制御装置３２は、第３点群データＰＧ３の各点Ｐ１の直交座標を極座標に変換する。極座標は、動径ｒと２つの偏角θ，φとで表すことができる。動径ｒは極座標系の原点からの距離である。偏角θは水平方向に対する偏角である。偏角θは、地図座標系におけるＸＹ平面においてＸ軸に対する角度ともいえる。偏角φは鉛直方向に対する偏角である。偏角φは、地図座標系におけるＸＺ平面においてＸ軸に対する角度ともいえる。ＸＺ平面は、地図座標系のＸ軸及びＺ軸で規定される平面である。制御装置３２は、極座標系を水平方向に対して規定の角度分解能で分割し、角度分解能で分割された角度毎に最も自己位置に近い座標の点Ｐ１を抽出する。制御装置３２は、角度分解能で分割された角度に複数の点Ｐ１が含まれている場合には、複数の点Ｐ１のうち最も動径ｒが短い点Ｐ１を抽出するといえる。これらの点Ｐ１は、荷台１４の縁Ｅ１を表す点Ｐ１である。

図９には、ステップＳ１４の処理によって得られた第４点群データＰＧ４を示す。第４点群データＰＧ４は、フォークリフト２０の正面にある荷台１４の縁Ｅ１を表す点Ｐ１を第３点群データＰＧ３から抽出したデータである。第４点群データＰＧ４には、第１点Ｐ１１に加えて、キャビン１１にレーザーが照射されることで生じた第３点Ｐ１３が含まれている。即ち、ステップＳ１４の処理では、荷台１４の縁Ｅ１を表す点Ｐ１のみを抽出できない場合が生じる。

図５に示すように、ステップＳ１５において、制御装置３２は、直線検知を行う。制御装置３２は、ステップＳ１４で極座標に変換した点Ｐ１の座標を地図座標に戻す。そして、制御装置３２は、ステップＳ１４で得られた点Ｐ１の地図座標から直線を検知する。本実施形態において、制御装置３２はRANSACによって直線を検知する。制御装置３２は、最小二乗法などの手法によって直線を検知してもよい。この直線は、荷台１４の縁Ｅ１を表す直線である。図９には、ステップＳ１５で検知された直線Ｌ１を示す。

次に、ステップＳ１６において、制御装置３２は、クラスタリングを行う。クラスタリングは、荷台１４の縁Ｅ１を表す点Ｐ１に対して行われる。即ち、クラスタリングは、第４点群データＰＧ４に対して行われる。クラスタリングとは、同一物体を表していると想定される複数の点Ｐ１を１つのクラスタとしてグルーピングすることである。制御装置３２は、例えば、点Ｐ１同士の距離が所定の範囲内の点Ｐ１を１つのクラスタとする。そして、制御装置３２は、第４点群データＰＧ４から荷台１４の縁Ｅ１を表す点Ｐ１を抽出する。言い換えれば、制御装置３２は、第４点群データＰＧ４から荷台１４の縁Ｅ１を表す点Ｐ１とは異なる点Ｐ１を除去する。本実施形態では、第４点群データＰＧ４から、キャビン１１を表す点Ｐ１が除去される。制御装置３２は、例えば、クラスタリングで得られた複数のクラスタのうち、最も大きいクラスタに属する点Ｐ１を第４点群データＰＧ４から抽出する。これにより、ステップＳ１５で検知された直線Ｌ１の範囲を荷台１４の縁Ｅ１の範囲にすることができる。即ち、ステップＳ１５で検知された直線Ｌ１を荷台１４の縁Ｅ１とみなせるようになる。

次に、ステップＳ１７において、制御装置３２は、積付位置を決定する。積付位置は、ステップＳ１５で検知された直線Ｌ１の延びる方向に積載物Ｃ２から規定距離離れた位置である。制御装置３２は、直線Ｌ１と、以下の積載物検出制御によって抽出される点Ｐ１に基づいて積付位置を検出する。まず、積載物検出制御について説明し、ステップＳ１７については、後述する。

図１０に示すように、ステップＳ２１において、制御装置３２は、積付位置検出制御によって得られた第１点群データＰＧ１を取得する。即ち、制御装置３２は、ステップＳ１１の点群処理によって得られた第１点群データＰＧ１を取得する。

次に、ステップＳ２２において、制御装置３２は、第１点群データＰＧ１の外れ値除去を行う。外れ値除去は、例えば、外れ値除去フィルタ、又はロバスト推定を用いることで行うことができる。

次に、ステップＳ２３において、制御装置３２は、積載面１５に置かれている積載物Ｃ２の検知を行う。制御装置３２は、外れ値を除去した後の第１点群データＰＧ１と、ステップＳ１３で導出された平面方程式から積載物Ｃ２の検知を行う。制御装置３２は、平面方程式で表される水平面から上方に所定距離以上離れた点Ｐ１を第１点群データＰＧ１から抽出する。所定距離としては、例えば、第１点群データＰＧ１から荷台１４を表す点Ｐ１を除外できるように設定されている。積載物Ｃ２は、荷台１４の上にあるため、これにより積載物Ｃ２を表す点Ｐ１を抽出できる。

図１１には、ステップＳ２３の処理によって得られた第５点群データＰＧ５を示す。第５点群データＰＧ５には、積載物Ｃ２を表す第２点Ｐ１２に加えて、積載物Ｃ２とは異なる物体を表す第３点Ｐ１３が含まれている。第５点群データＰＧ５が得られると、制御装置３２は、積載物検出制御を終了する。

ステップＳ１７について説明する。制御装置３２は、直線Ｌ１と、第５点群データＰＧ５から積付位置を検出する。制御装置３２は、直線Ｌ１に対して垂直な位置であって第５点群データＰＧ５の第２点Ｐ１２から直線Ｌ１の延びる方向に規定距離離れた位置を積付位置とする。本実施形態では、第２点Ｐ１２で表される積載物Ｃ２において直線Ｌ１の延びる方向での端から規定距離離れた位置が積付位置とされる。図２に示すように、フロントパネル１２からリヤドア１３に向けて積載物Ｃ２を積載している場合には、最もリヤドア１３に近い積載物Ｃ２の端であって最もリヤドア１３に近い端から規定距離離れた位置が積付位置とされる。荷Ｃ１を積付位置に積み付けるときには、荷Ｃ１の中心位置が積付位置に一致するように積み付けが行われる。この際、荷Ｃ１が積載物Ｃ２に干渉しないように規定距離は設定されている。荷Ｃ１の寸法を予め把握できていれば、規定距離は予め定められた固定値であってもよい。例えば、荷Ｃ１及び積載物Ｃ２が同一形状のパレットであれば、規定距離は、パレット幅／２＋所定値としてもよい。所定値は、任意に設定することができる。この場合、荷Ｃ１と積載物Ｃ２とのクリアランスは所定値となる。荷Ｃ１の中心位置とは、直線Ｌ１の延びる方向での荷Ｃ１の中心位置である。

荷Ｃ１の寸法が不定の場合には、荷Ｃ１の寸法を測定し、測定した寸法から規定距離を定めてもよい。荷Ｃ１の寸法は、例えば、フォークリフト２０に設けられたセンサによって測定できる。このセンサとして、自己位置推定用外界センサ２１、又は外界センサ３１を用いてもよい。第５点群データＰＧ５には、第３点Ｐ１３が含まれているが、第３点Ｐ１３は直線Ｌ１に対して垂直な位置には存在しない。このため、第５点群データＰＧ５に第３点Ｐ１３が含まれていても、積付位置の検出を行うことができる。

図６には、積付位置ＬＰ１の一例を示す。本実施形態では、直線Ｌ１に対して垂直な位置として、直線Ｌ１に対して水平方向に垂直な位置Ｌ２と、直線Ｌ１に対して鉛直方向に垂直な位置Ｌ３と、を積付位置ＬＰ１として検出している。積付位置ＬＰ１は、地図座標として検出される。制御装置３２は、位置Ｌ２及び位置Ｌ３のうちいずれか一方を検出してもよい。制御装置３２は、ステップＳ１７の処理を終えると、積付位置検出制御を終了する。なお、積載物Ｃ２が検出されない場合には、直線Ｌ１の端から規定距離離れた位置を積付位置ＬＰ１とすればよい。

本実施形態の作用について説明する。
積載面１５は、水平方向に拡がる面である。このため、ステップＳ１２で点群データから水平面を表す点Ｐ１を抽出すると、この点Ｐ１は積載面１５により得られた点Ｐ１を含んでいるといえる。この点Ｐ１から平面方程式を導出すると、水平面を表す平面方程式が得られる。制御装置３２は、水平面から上下方向に所定範囲の点Ｐ１を抽出する。これにより、荷台１４を表す点Ｐ１を抽出することができる。即ち、積載物Ｃ２を表す第２点Ｐ１２を除くことができる。荷台１４を表す点Ｐ１のうちフォークリフト２０の正面にある縁Ｅ１を表す点Ｐ１は、フォークリフト２０が積み付けをする際に、フォークリフト２０が近付く縁Ｅ１である。即ち、この縁Ｅ１は、積付位置に沿う縁Ｅ１である。制御装置３２は、この縁Ｅ１の位置を直線として検知する。水平面から上方に所定距離以上離れた点Ｐ１は、荷台１４に積載された積載物Ｃ２を表す点Ｐ１である。制御装置３２は、直線と積載物Ｃ２から、積付位置を検出できる。

本実施形態の効果について説明する。
（１）制御装置３２は、直線の延びる方向に積載物Ｃ２から規定距離離れた位置を積付位置として検出する。この積付位置は、荷台１４において積載物Ｃ２から離れた位置である。このため、制御装置３２は、積載面１５に積載された積載物Ｃ２とフォークＦ１に積まれた荷Ｃ１とが干渉しにくい位置を積付位置として検出することができる。

（２）制御装置３２は、各点Ｐ１の法線ベクトルを導出する。制御装置３２は、法線ベクトルの向きが上下方向の点Ｐ１を抽出し、当該抽出された点Ｐ１から平面方程式を導出する。法線ベクトルは、平面に対して垂直なベクトルである。このため、点Ｐ１毎に法線ベクトルを導出し、法線ベクトルの向きが上下方向の点Ｐ１を抽出することで、水平面を表す点Ｐ１を抽出できる。このため、平面方程式で表される平面を水平面と捉えることができる。

（３）制御装置３２は、クラスタリングを行うことで、同一物体とみなせる点Ｐ１のグルーピングを行う。これにより、荷台１４とは異なる物体を表す点Ｐ１を除外することができる。

（４）フォークリフト２０は、自己位置推定用外界センサ２１と、自己位置推定用外界センサ２１とは異なる外界センサ３１と、を備える。自己位置推定用外界センサ２１を用いて積付位置の検出をする場合に比べて、積付位置の検出に適した外界センサ３１を用いて、積付位置の検出を行うことができる。例えば、実施形態のように、自己位置推定用外界センサ２１よりもFOVの広い外界センサ３１を用いることで、トラック１０に近付いても積載面１５がFOVに含まれやすい。このため、自己位置推定用外界センサ２１によって積付位置を検出する場合に比べて、積付位置の検出を行いやすい。

（５）制御装置３２は、荷台１４の縁Ｅ１を検出し、積載面１５に積まれている積載物Ｃ２に干渉しない位置を積付位置として検出している。トラック１０の停車位置、トラック１０の停車向きが不定であったとしても、外界センサ３１によって荷台１４の縁Ｅ１と積載物Ｃ２を検出できれば積付位置を検出できる。このため、トラック１０の停車位置、トラック１０の停車向きが不定であったとしても、フォークリフト２０による積み付けを行うことができる。

実施形態は、以下のように変更して実施することができる。実施形態及び以下の変形例は、技術的に矛盾しない範囲で互いに組み合わせて実施することができる。
○積付位置を検出するための外界センサとして、自己位置推定用外界センサ２１を用いてもよい。この場合、自己位置推定用外界センサ２１が積付位置を検出するための外界センサとして兼用されるといえる。

○荷Ｃ１が積み付けられる対象は、荷台を備えるものであればよい。例えば、平ボディのトラック等のウィングトラックとは異なるトラック、棚、又はコンテナに荷Ｃ１が積み付けられてもよい。

○制御装置３２は、センサ座標系で積付位置を導出した後に、当該積付位置を地図座標に変換してもよい。即ち、制御装置３２は、外界センサ３１から取得した点群データを地図座標系に変換してから積付位置を導出してもよいし、点群データを地図座標系に変換せずに積付位置を導出してもよい。この場合、制御装置３２は、フォークリフト２０の自己移動量を把握できれば、点群データを重ね合わせることができる。自己移動量は、自己位置推定処理によって求めてもよいし、デッドレコニングによって求めてもよい。

○水平面を表す点Ｐ１の抽出は、任意の手法で行うことができる。例えば、制御装置３２は、各点Ｐ１の高さ方向の分布において最も数が多い点Ｐ１を、水平面を表す点Ｐ１として抽出してもよい。

○制御装置３２は、クラスタリングを行わなくてもよい。この場合、制御装置３２は、縁Ｅ１を表す点Ｐ１と、縁Ｅ１とは異なる物体を表す点Ｐ１とをクラスタリングとは異なる手法によって判別してもよい。例えば、縁Ｅ１を表す点Ｐ１であれば、当該点Ｐ１の上方には空間が存在している。このため、縁Ｅ１を表す点Ｐ１の上方には点Ｐ１が存在しない領域が存在する。一方で、キャビン１１を表す点Ｐ１であれば、当該点Ｐ１の上方にもキャビン１１が存在する。このため、制御装置３２は、第４点群データＰＧ４の各点Ｐ１のＺ軸の座標に基づき、点Ｐ１に対して上方に重なり合う点Ｐ１が存在するか否かによって縁Ｅ１を表す点Ｐ１か否かを判別してもよい。

○制御装置３２は、直線検知を行う前にクラスタリングを行ってもよい。
○制御装置３２は、積載物Ｃ２の直線Ｌ１の延びる方向での中心位置から規定距離離れた位置を積付位置としてもよい。

○制御装置３２は、ステップＳ１１において、環境地図とのマッチングによって点群データの各点Ｐ１を地図座標に変換してもよい。そして、制御装置３２は、地図座標に変換された後の点群データを重ね合わせて第１点群データＰＧ１を生成してもよい。環境地図とのマッチングには、例えば、ICP(Iterative Closest Point)、又はNDT(Normal Distribution Transform)を用いることができる。

○制御装置３２は、外界センサ３１から取得した点群データの点Ｐ１が密であれば、当該点群データを用いてステップＳ１２以降の処理を行ってもよい。即ち、複数の点群データを重ね合わせなくてもよい。

○フォークリフト２０は、手動運転のフォークリフトであってもよい。この場合、制御装置３２は、積付位置を表示部に表示するようにしてもよい。表示部は、フォークリフト２０を操作している操作者が視認可能な位置に設けられている。表示部に積付位置が表示されることによって、操作者は表示部を視認しながらフォークＦ１の位置合わせを行うことができる。操作者は、フォークリフト２０に搭乗していてもよい。操作者は、フォークリフト２０から離れた遠隔地でフォークリフト２０を操作していてもよい。

Ｃ１…荷、Ｃ２…積載物、Ｅ１…縁、Ｌ１…直線、ＬＰ１…積付位置、Ｐ１…点、１４…荷台、１５…積載面、２０…フォークリフト、２１…自己位置推定用外界センサ、２２…自己位置推定用制御装置、３１…外界センサ、３２…制御装置。

Claims (4)

1. 荷台の上面である積載面に荷の積み付けを行うフォークリフトであって、
   ３次元座標系の座標で物体の位置を検出する外界センサと、
   制御装置と、を備え、
   前記制御装置は、
   前記物体の位置を表した点の集合である点群データから水平面を表す前記点を抽出し、
   前記水平面から上下方向に所定範囲の前記点を、前記荷台を表す前記点として抽出し、
   前記荷台を表す前記点から前記荷台の縁であって前記フォークリフトの正面にある縁を表す前記点を抽出し、
   前記縁を表す前記点から前記縁を表す直線を検知し、
   前記水平面から上方に所定距離以上離れた前記点を前記荷台に積載された積載物を表す前記点として抽出し、
   前記直線の延びる方向に前記積載物から規定距離離れた位置を前記荷の積み付けを行う積付位置として検出する、フォークリフト。
2. 前記制御装置は、
   前記点群データの各点の法線ベクトルを導出し、
   前記法線ベクトルの向きが上下方向の前記点を抽出し、当該抽出された前記点から平面方程式を導出し、
   前記平面方程式で表される平面を前記水平面とする、請求項１に記載のフォークリフト。
3. 前記制御装置は、
   前記縁を表す前記点に対して、複数の点を１つのクラスタとしてグルーピングするクラスタリングを行う、請求項１又は請求項２に記載のフォークリフト。
4. 前記フォークリフトは、
   前記外界センサとは異なる自己位置推定用外界センサと、
   前記自己位置推定用外界センサから前記フォークリフトの自己位置を推定する自己位置推定用制御装置と、を備える請求項１～請求項３のうちいずれか一項に記載のフォークリフト。

Images