JP2023064294A - 位置姿勢推定装置 - Google Patents

Abstract

【課題】フォークによりパレットを持ち上げた状態で、フォークに対するパレットの位置及び姿勢を推定することができる位置姿勢推定装置を提供する。【解決手段】位置姿勢推定装置３１は、フォーク９により持ち上げられたパレット１２の下方からパレット１２の下面１２ｂまでの距離を検出する測距センサ２２と、測距センサ２２の検出データに基づいて、パレット１２の下面１２ｂにおける角部の位置を検知する角部検知部３２と、角部検知部３２により検知されたパレット１２の下面１２ｂにおける角部の位置に基づいて、フォーク９に対するパレット１２の位置及び姿勢の推定演算を行う推定演算部２９とを備える。【選択図】図４

Description

本発明は、位置姿勢推定装置に関する。

従来の位置姿勢推定装置としては、例えば特許文献１に記載されている技術が知られている。特許文献１に記載の位置姿勢推定装置は、レーザ光を照射すると共に照射したレーザ光の反射光から周辺物体までの距離を計測する測域センサにより距離データを取得し、距離データを３次元空間上の観測点群に座標変換し、その観測点群に基づいてフォークリフトの前方に配置されたパレットの位置及び姿勢を特定する。

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

しかしながら、上記従来技術に記載の位置姿勢推定装置は、フォークリフトのフォークによりパレットを持ち上げる前に、フォークリフトの前方に配置されたパレットの位置及び姿勢を推定する技術であり、フォークによりパレットを持ち上げた状態で、パレットの位置及び姿勢を推定することは想定していない。また、上記従来技術に記載の位置姿勢推定装置では、測域センサがフォークに取り付けられているため、フォークによりパレットを持ち上げた状態では、測域センサの計測値に基づいてフォークに対するパレットの位置及び姿勢を推定することは困難である。

本発明の目的は、フォークによりパレットを持ち上げた状態で、フォークに対するパレットの位置及び姿勢を推定することができる位置姿勢推定装置を提供することである。

本発明の一態様は、フォークリフトのフォークにより持ち上げられたパレットの位置及び姿勢を推定する位置姿勢推定装置であって、フォークにより持ち上げられたパレットの下方からパレットの下面までの距離を検出する距離検出部と、距離検出部の検出データに基づいて、パレットの下面における角部の位置を検知する角部検知部と、角部検知部により検知されたパレットの下面における角部の位置に基づいて、フォークに対するパレットの位置及び姿勢の推定演算を行う推定演算部とを備える。

このような位置姿勢推定装置においては、フォークリフトのフォークによりパレットが持ち上げられた状態で、距離検出部によってパレットの下方からパレットの下面までの距離が検出される。そして、距離検出部の検出データに基づいて、パレットの下面における角部の位置が検知される。そして、パレットの下面における角部の位置に基づいて、フォークに対するパレットの位置及び姿勢の推定演算が行われる。このように距離検出部によりパレットの下方からパレットの下面までの距離が検出されるため、パレットの上面に荷物が載置されているか否かに関わらず、パレットの下面における角部の位置が検知される。これにより、フォークによりパレットを持ち上げた状態で、フォークに対するパレットの位置及び姿勢を推定することができる。また、距離検出部によりパレットの下方からパレットの下面までの距離を検出することにより、パレットの側面までの距離を検出するためにフォークリフトの車体の外側に距離検出部を配置しなくて済む。従って、フォークリフト全体の車幅を広げることなく、フォークに対するパレットの位置及び姿勢を推定することができる。

角部検知部は、距離検出部の検出データに基づいて、パレットにおける互いに隣り合う側面に対応する複数の直線を推定し、複数の直線の交点の位置をパレットの下面における角部の位置として算出し、推定演算部は、交点の位置に基づいて、フォークに対するパレットの位置及び姿勢の推定演算を行ってもよい。このような構成では、パレットにおける互いに隣り合う側面に対応する複数の直線の交点の位置が、パレットの下面における角部の位置として検知されることになる。従って、実際のパレットの角部が丸くなっていても、フォークに対するパレットの位置及び姿勢を高精度に推定することができる。

角部検知部は、距離検出部の検出データに基づいて、パレットの下面における左右両側の角部の位置を検知し、推定演算部は、パレットの下面における左右両側の角部の位置に基づいて、フォークに対するパレットの位置及び姿勢の推定演算を行ってもよい。このような構成では、パレットの下面における左右両側の角部の位置を検知することにより、パレットのサイズが不明である場合でも、フォークに対するパレットの位置及び姿勢を高精度に推定することができる。

角部検知部は、距離検出部の検出データに基づいて、パレットにおいてフォークの差込方向に垂直な第１側面に対応する第１直線と第１側面にそれぞれ隣り合う左右両側の第２側面に対応する２本の第２直線とを推定し、第１直線と２本の第２直線との左右２つの交点の位置をパレットの下面における左右両側の角部の位置として算出し、推定演算部は、左右２つの交点の位置に基づいて、フォークに対するパレットの位置及び姿勢の推定演算を行ってもよい。このような構成では、パレットの第１側面に対応する第１直線とパレットの左右両側の第２側面に対応する２本の第２直線との左右２つの交点の位置が、パレットの下面における左右両側の角部の位置として検知されることとなる。従って、実際のパレットの角部が丸くなっていても、フォークに対するパレットの位置及び姿勢を高精度に推定することができる。

推定演算部は、左右２つの交点の位置に基づいて、パレットの中心位置を算出し、パレットの中心位置からフォークに対するパレットの位置を算出してもよい。このような構成では、パレットの中心位置を算出することにより、フォークに対するパレットの位置を単純な計算処理で推定することができる。

推定演算部は、左右２つの交点の位置、第１直線及び第２直線の何れかに基づいて、パレットのヨー角を算出し、パレットのヨー角からフォークに対するパレットの姿勢を算出してもよい。このような構成では、パレットのヨー角を算出することにより、フォークに対するパレットの姿勢を単純な計算処理で推定することができる。

角部検知部は、パレットの下面におけるフォークリフトの車体に対して手前側の角部の位置を検知し、推定演算部は、パレットの下面における車体に対して手前側の角部の位置に基づいて、フォークに対するパレットの位置及び姿勢の推定演算を行ってもよい。このような構成では、パレットの下面における４つの角部のうち車体に近い角部の位置がパレットの位置及び姿勢の推定演算に使用されるため、フォークに対するパレットの位置及び姿勢を高精度に推定することができる。

本発明によれば、フォークによりパレットを持ち上げた状態で、フォークに対するパレットの位置及び姿勢を推定することができる。

本発明の一実施形態に係る位置姿勢推定装置が適用されるフォークリフトを示す斜視図である。 図１に示されたフォークリフトのフォークがパレットを持ち上げた状態を示す側面図である。 図１に示されたフォークリフトのフォークがパレットを持ち上げた状態を示す平面図である。 本発明の一実施形態に係る位置姿勢推定装置を備えた荷役制御装置の構成を示すブロック図である。 図４に示されたコントローラにより実行される制御処理の手順を示すフローチャートである。 測距センサの検出データに基づいてパレットの下面における左右両側の角部を検知する手法を示す図である。 荷置き場においてフォークリフトが荷置きを行う様子を示す平面図である。 測距センサの検出データに基づいてパレットの下面における左右両側の角部を検知する手法の変形例を示す図である。

以下、本発明の実施形態について図面を参照して詳細に説明する。なお、図面において、同一または同等の要素には同じ符号を付し、重複する説明を省略する。

図１は、本発明の一実施形態に係る位置姿勢推定装置が適用されるフォークリフトを示す斜視図である。図１において、フォークリフト１は、走行装置２と、この走行装置２の前側に配置され、荷役を行う荷役装置３とを備えている。

走行装置２は、車体４と、この車体４の前部に配置された左右１対の駆動輪である前輪５と、車体４の後部に配置された左右１対の操舵輪である後輪６とを有している。

荷役装置３は、車体４の前端部に立設されたマスト７と、このマスト７にリフトブラケット８を介して取り付けられた左右１対のフォーク９と、このフォーク９を昇降させるリフトシリンダ１０と、マスト７を傾動させるティルトシリンダ１１とを有している。フォーク９は、図２及び図３に示されるように、パレット１２を保持する。

パレット１２は、荷物Ｍを載せるための荷役台である。パレット１２の種類としては、平パレットまたはシートパレット等が挙げられる。パレット１２には、４つの角部１３が設けられている。各角部１３は、Ｒ加工が施されて丸くなっている。また、パレット１２には、フォーク９が差し込まれる２つのフォーク穴１４が設けられている。

パレット１２は、上面１２ａ、下面１２ｂ及び４つの側面１２ｃ～１２ｆを有している。上面１２ａ及び下面１２ｂは、互いに対向している。上面１２ａ及び下面１２ｂは、略四角形状を呈している。側面１２ｃ～１２ｆは、上面１２ａと下面１２ｂとの間に設けられている。

側面１２ｃ，１２ｄは、互いに対向している。側面１２ｃ，１２ｄは、フォーク９の差込方向に垂直な第１側面である。つまり、フォーク穴１４は、側面１２ｃ，１２ｄの対向方向に延びている。側面１２ｃは、フォークリフト１の車体４に対して手前側（前側）の側面である。側面１２ｄは、車体４に対して奥側（後側）の側面である。

側面１２ｅ，１２ｆは、側面１２ｃ，１２ｄの対向方向に垂直な方向において互いに対向している。側面１２ｅ，１２ｆは、側面１２ｃ，１２ｄにそれぞれ隣り合う左右両側の第２側面である。側面１２ｅ，１２ｆは、フォーク９の差込方向に平行な側面である。

図４は、本発明の一実施形態に係る位置姿勢推定装置を備えた荷役制御装置の構成を示すブロック図である。図２において、荷役制御装置２０は、フォーク９に保持されたパレット１２を所定の荷置き場Ｚ（図７参照）に置く、いわゆる荷置きを行う装置である。荷役制御装置２０は、フォークリフト１の自動運転によって荷置きを行う。

荷役制御装置２０は、指示スイッチ２１と、２つの測距センサ２２と、コントローラ２３と、駆動部２４とを備えている。測距センサ２２、コントローラ２３及び駆動部２４は、フォークリフト１に搭載されている。

指示スイッチ２１は、作業者が荷置き動作の指示を行うための手動操作スイッチである。指示スイッチ２１は、フォークリフト１の運転席に搭載されていてもよいし、或いは作業者が所持する携帯端末等であってもよい。

測距センサ２２は、図１～図３に示されるように、フォークリフト１の左右両側に配置されている。測距センサ２２は、例えば車体４における前輪５の上方位置に取り付けられている。測距センサ２２は、フォーク９により持ち上げられたパレット１２の下方からパレット１２の下面１２ｂまでの距離を検出する距離検出部である。

測距センサ２２としては、ＬＩＤＡＲ、レーザレンジファインダまたはТｏＦ（Time of Flight）カメラ等が使用される。測距センサ２２は、パレット１２の下面１２ｂに向けてレーザ光Ｌを照射し、レーザ光Ｌの反射光を受光することにより、パレット１２の下面１２ｂまでの距離を検出して点群データを取得する。点群データは、測距センサ２２の検出データである。点群は、レーザ光Ｌの反射点の集まりである。

フォークリフト１の左側に配置された測距センサ２２は、主としてパレット１２の下面１２ｂにおける左側の領域にレーザ光Ｌを照射する。フォークリフト１の右側に配置された測距センサ２２は、主としてパレット１２の下面１２ｂにおける右側の領域にレーザ光Ｌを照射する。

コントローラ２３は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ２３は、指示スイッチ２１により荷置き動作が指示されると、測距センサ２２の検出信号に基づいて、フォーク９に対するパレット１２の位置及び姿勢を推定し、パレット１２の荷置きを行うようにリフトシリンダ１０及び駆動部２４を制御する。

駆動部２４は、特に図示はしないが、例えば駆動輪である前輪５を回転させる走行モータと、操舵輪である後輪６を転舵させる操舵モータとを有している。

コントローラ２３は、上昇制御部２５と、パレット点群抽出部２６と、直線推定部２７と、交点算出部２８と、推定演算部２９と、荷置き制御部３０とを有している。

測距センサ２２、コントローラ２３のパレット点群抽出部２６、直線推定部２７、交点算出部２８及び推定演算部２９は、フォークリフト１のフォーク９により持ち上げられたパレット１２の位置及び姿勢を推定する位置姿勢推定装置３１を構成している。

上昇制御部２５は、指示スイッチ２１により荷置き動作が指示されると、フォーク９により持ち上げられたパレット１２が測距センサ２２よりも高い検知位置まで上昇するようにリフトシリンダ１０を制御する。検知位置は、図２に示されるように、測距センサ２２によりパレット１２の下面１２ｂまでの距離を検出することで、パレット１２の下面１２ｂの角部を検知することが可能な位置である。

パレット点群抽出部２６は、測距センサ２２により取得された点群データに基づいて、パレット１２の下面１２ｂに相当する点群を抽出する。

直線推定部２７は、パレット点群抽出部２６により抽出されたパレット１２の下面１２ｂに相当する点群に基づいて、パレット１２における互いに隣り合う側面１２ｃ，１２ｅ，１２ｆに対応する複数（ここでは３本）の直線を推定する。

交点算出部２８は、直線推定部２７により抽出された複数の直線の交点の位置をパレット１２の下面１２ｂにおける角部の位置として算出する。

パレット点群抽出部２６、直線推定部２７及び交点算出部２８は、測距センサ２２の検出データに基づいて、パレット１２の下面１２ｂにおける角部の位置を検知する角部検知部３２を構成している。

推定演算部２９は、角部検知部３２により検知されたパレット１２の下面１２ｂにおける角部の位置に基づいて、フォーク９に対するパレット１２の位置及び姿勢の推定演算を行う。パレット１２の位置及び姿勢は、フォークリフト１の規定位置を原点としたパレット１２の２次元の位置及び姿勢である。

荷置き制御部３０は、推定演算部２９により推定されたフォーク９に対するパレット１２の位置及び姿勢に基づいて、パレット１２が荷置き場Ｚ（図７参照）に適切な位置及び姿勢で置かれるようにリフトシリンダ１０及び駆動部２４を制御する。

図５は、コントローラ２３により実行される制御処理の手順を示すフローチャートである。図５において、コントローラ２３は、まず指示スイッチ２１の操作信号に基づいて、フォーク９に保持されたパレット１２の荷置き動作が指示されたかどうかを判断する（手順Ｓ１０１）。

コントローラ２３は、フォーク９に保持されたパレット１２の荷置き動作が指示されたと判断したときは、パレット１２が検知位置（前述）まで上昇するようにリフトシリンダ１０を制御する（手順Ｓ１０２）。

続いて、コントローラ２３は、測距センサ２２からパレット１２の下面１２ｂにレーザ光Ｌを照射させることで、測距センサ２２の検出データを取得する（手順Ｓ１０３）。

そして、コントローラ２３は、測距センサ２２の検出データに基づいて、パレット１２の下面１２ｂに相当する点群を抽出する（手順Ｓ１０４）。このとき、コントローラ２３は、図６（ａ）に示されるように、測距センサ２２により取得された点群データのうち、パレット１２の下面１２ｂに相当する点群Ｇを抽出する。

続いて、コントローラ２３は、パレット１２の下面１２ｂに相当する点群に基づいて、パレット１２の側面１２ｃ，１２ｅ，１２ｆに対応する３本の直線を推定する（手順Ｓ１０５）。このとき、コントローラ２３は、図６（ｂ）に示されるように、例えばＲＡＮＳＡＣ（Random sample consensus）または最小二乗法等を用いて、パレット１２の側面１２ｃに対応する直線Ｌ１（第１直線）と、パレット１２の１２ｅ，１２ｆに対応する２本の直線Ｌ２，Ｌ３（第２直線）とを推定する。

続いて、コントローラ２３は、パレット１２の側面１２ｃに対応する直線とパレット１２の側面１２ｅ，１２ｆに対応する直線との左右２つの交点を求め、左右２つの交点の位置をパレット１２の下面１２ｂにおける角部の位置として算出する（手順Ｓ１０６）。ここでは、図６（ｂ）に示されるように、パレット１２の側面１２ｃに対応する直線Ｌ１とパレット１２の側面１２ｅ，１２ｆに対応する直線Ｌ２，Ｌ３との交点Ｐ１，Ｐ２の位置が、パレット１２の下面１２ｂにおける角部の位置として算出される。

続いて、コントローラ２３は、左右２つの交点の位置に基づいて、パレット１２の中心位置を算出する（手順Ｓ１０７）。パレット１２の中心位置は、図６（ｂ）に示されるように、パレット１２の下面１２ｂにおける車体４に対して手前側縁部の車幅方向の中心位置Ｃに相当する。

また、コントローラ２３は、左右２つの交点の位置の交点に基づいて、パレット１２のヨー角を算出する（手順Ｓ１０８）。パレット１２のヨー角は、パレット１２の上下軸回りの回転角度である。

続いて、コントローラ２３は、パレット１２の中心位置から、フォーク９に対するパレット１２の位置を算出すると共に、パレット１２のヨー角から、フォーク９に対するパレット１２の姿勢を算出する（手順Ｓ１０９）。

続いて、コントローラ２３は、フォーク９に対するパレット１２の位置及び姿勢が基準値に対してずれているかどうかを判断する（手順Ｓ１１０）。基準値は、パレット１２を荷置きした際の位置及び姿勢が許容範囲内となるような値である。

コントローラ２３は、フォーク９に対するパレット１２の位置及び姿勢が基準値に対してずれていると判断したときは、パレット１２の位置及び姿勢を修正するように駆動部２４を制御する（手順Ｓ１１１）。例えば、コントローラ２３は、フォークリフト１の走行経路を変更することでパレット１２の位置及び姿勢が修正されるように駆動部２４を制御する。

なお、フォークリフト１がフォーク９を左右両側にシフトさせるサイドシフトシリンダを備えている場合には、サイドシフトシリンダを制御することにより、フォーク９に対するパレット１２の位置を修正してもよい。

コントローラ２３は、手順Ｓ１１０でフォーク９に対するパレット１２の位置及び姿勢が基準値に対してずれていないと判断したとき、または手順Ｓ１１１を実行した後、フォークリフト１が荷置き場Ｚ（図７参照）まで走行するように駆動部２４を制御すると共に、フォーク９が下降することで荷置き場Ｚにパレット１２が置かれるようにリフトシリンダ１０を制御する（手順Ｓ１１２）。

以上において、上昇制御部２５は、上記の手順Ｓ１０１，Ｓ１０２を実行する。パレット点群抽出部２６は、上記の手順Ｓ１０３，Ｓ１０４を実行する。直線推定部２７は、上記の手順Ｓ１０５を実行する。交点算出部２８は、上記の手順Ｓ１０６を実行する。推定演算部２９は、上記の手順Ｓ１０７～Ｓ１０９を実行する。荷置き制御部３０は、上記の手順Ｓ１１０～Ｓ１１２を実行する。

ところで、パレット１２の荷置きを行う際、図７に示されるように、フォーク９に対するパレット１２の位置及び姿勢がずれている場合には、フォークリフト１を荷置き場Ｚの正面まで自動走行させたとしても、リフトシリンダ１０によりフォーク９を下降させたときに、パレット１２が荷置き場Ｚにおける正規の位置及び姿勢に対してずれてしまう。

そのような課題を解決するためには、測距センサ２２よりパレット１２の上面１２ａにレーザ光Ｌを照射することで、フォーク９に対するパレット１２の位置及び姿勢を推定することが考えられる。しかし、図７に示されるように、パレット１２の上面１２ａに大きな荷物Ｍが載置されていると、パレット１２の上面１２ａが荷物Ｍにより隠れるため、測距センサ２２によりパレット１２が検知されない。従って、フォーク９に対するパレット１２の位置及び姿勢を推定することができない。

また、測距センサ２２よりパレット１２の側面１２ｅ，１２ｆにレーザ光Ｌを照射することで、フォーク９に対するパレット１２の位置及び姿勢を推定することも考えられる。ただし、この場合には、パレット１２よりも車幅方向の外側に測距センサ２２を配置する必要があるため、測距センサ２２の配置位置がフォークリフト１の車体４よりも車幅方向の外側にならざるを得ないことがある。従って、フォークリフト１全体の車幅が広くなってしまう可能性がある。

そこで、本実施形態においては、フォークリフト１のフォーク９によりパレット１２が持ち上げられた状態で、測距センサ２２によってパレット１２の下方からパレット１２の下面１２ｂまでの距離が検出される。そして、測距センサ２２の検出データに基づいて、パレット１２の下面１２ｂにおける角部の位置が検知される。そして、パレット１２の下面１２ｂにおける角部の位置に基づいて、フォーク９に対するパレット１２の位置及び姿勢の推定演算が行われる。このように測距センサ２２によりパレット１２の下方からパレット１２の下面１２ｂまでの距離が検出されるため、パレット１２の上面１２ａに荷物Ｍが載置されているか否かに関わらず、パレット１２の下面１２ｂにおける角部の位置が検知される。これにより、フォーク９によりパレット１２を持ち上げた状態で、フォーク９に対するパレット１２の位置及び姿勢を推定することができる。また、測距センサ２２によりパレット１２の下方からパレット１２の下面１２ｂまでの距離を検出することにより、パレット１２の側面１２ｅ，１２ｆまでの距離を検出するためにフォークリフト１の車体４の外側に測距センサ２２を配置しなくて済む。従って、フォークリフト１全体の車幅を広げることなく、フォーク９に対するパレット１２の位置及び姿勢を推定することができる。

また、本実施形態では、測距センサ２２の検出データに基づいて、パレット１２の下面１２ｂにおける左右両側の角部の位置が検知され、パレット１２の下面１２ｂにおける左右両側の角部の位置に基づいて、フォーク９に対するパレット１２の位置及び姿勢の推定演算が行われる。このようにパレット１２の下面１２ｂにおける左右両側の角部の位置を検知することにより、パレット１２のサイズが不明である場合でも、フォーク９に対するパレット１２の位置及び姿勢を高精度に推定することができる。

また、本実施形態では、測距センサ２２の検出データに基づいて、パレット１２においてフォーク９の差込方向に垂直な側面１２ｃに対応する直線Ｌ１と側面１２ｃにそれぞれ隣り合う左右両側の側面１２ｅ，１２ｆに対応する直線Ｌ２，Ｌ３とが推定され、直線Ｌ１と直線Ｌ２，Ｌ３との左右２つの交点Ｐ１，Ｐ２の位置がパレット１２の下面１２ｂにおける左右両側の角部の位置として算出される。このため、パレット１２の側面１２ｃに対応する直線Ｌ１とパレット１２の左右両側の側面１２ｅ，１２ｆに対応する直線Ｌ２，Ｌ３との左右２つの交点Ｐ１，Ｐ２の位置が、パレット１２の下面１２ｂにおける左右両側の角部の位置として検知されることとなる。従って、実際のパレット１２の角部が丸くなっていても、フォーク９に対するパレット１２の位置及び姿勢を高精度に推定することができる。

また、本実施形態では、左右２つの交点Ｐ１，Ｐ２の位置に基づいて、パレット１２の中心位置Ｃが算出され、パレット１２の中心位置Ｃからフォーク９に対するパレット１２の位置が算出される。このようにパレット１２の中心位置Ｃを算出することにより、フォーク９に対するパレット１２の位置を単純な計算処理で推定することができる。

また、本実施形態では、左右２つの交点Ｐ１，Ｐ２の位置に基づいて、パレット１２のヨー角が算出され、パレット１２のヨー角からフォーク９に対するパレット１２の姿勢が算出される。このようにパレット１２のヨー角を算出することにより、フォーク９に対するパレット１２の姿勢を単純な計算処理で推定することができる。

また、本実施形態では、パレット１２の下面１２ｂにおけるフォークリフト１の車体４に対して手前側の角部の位置が検知され、パレット１２の下面１２ｂにおける車体４に対して手前側の角部の位置に基づいて、フォーク９に対するパレット１２の位置及び姿勢の推定演算が行われる。従って、パレット１２の下面１２ｂにおける４つの角部のうち車体４に近い角部の位置がパレット１２の位置及び姿勢の推定演算に使用されるため、フォーク９に対するパレット１２の位置及び姿勢を更に高精度に推定することができる。

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、パレット１２の側面１２ｃに対応する直線Ｌ１とパレット１２の側面１２ｅ，１２ｆに対応する直線Ｌ２，Ｌ３との交点Ｐ１，Ｐ２の位置に基づいて、パレット１２のヨー角が算出されているが、特にその形態には限られず、直線Ｌ１～Ｌ３の少なくとも１つに基づいて、パレット１２のヨー角を算出してもよい。

また、上記実施形態では、測距センサ２２がフォークリフト１の左右両側にそれぞれ配置されているが、特にそのような形態には限られない。フォーク９により持ち上げられたパレット１２の下方からパレット１２の下面１２ｂまでの距離を検出し、パレット１２の下面１２ｂにおける左右両側の角部の位置を検知することが可能であれば、測距センサ２２の数は１つであってもよい。この場合、測距センサ２２は、フォークリフト１の左右一方側に配置されていてもよいし、フォークリフト１の車幅方向の中央部に配置されていてもよい。

また、上記実施形態では、測距センサ２２の検出データに基づいて、パレット１２の下面１２ｂにおける左右両側の角部の位置が検知されているが、特にそのような形態には限られない。パレット１２の縦横サイズが既知である場合には、図８に示されるように、１つの測距センサ２２の検出データに基づいて、パレット１２の下面１２ｂにおける左右一方側の角部の位置のみを検知してもよい。

図８に示される変形例では、パレット１２の側面１２ｃに対応する直線Ｌ１とパレット１２の側面１２ｆに対応する直線Ｌ２とが推定され、これらの直線Ｌ１，Ｌ２の交点Ｐの位置が算出され、その交点Ｐの位置に基づいて、フォーク９に対するパレット１２の位置及び姿勢が推定される。具体的には、交点Ｐの位置とパレット１２の縦横サイズとに基づいて、パレット１２の中心位置Ｃが算出されると共に、直線Ｌ１，Ｌ２の少なくとも一方に基づいて、パレット１２のヨー角が算出される。

このような変形例では、パレット１２における互いに隣り合う側面１２ｃ，１２ｆに対応する直線Ｌ１，Ｌ２の交点Ｐの位置が、パレット１２の下面１２ｂにおける角部の位置として検知されることになる。従って、実際のパレット１２の角部１３が丸くなっていても、フォーク９に対するパレット１２の位置及び姿勢を推定することができる。

また、上記実施形態では、パレット１２の下面１２ｂにおけるフォークリフト１の車体４に対して手前側（前側）の角部の位置が検知されているが、特にその形態には限られず、パレット１２の下面１２ｂにおける車体４に対して奥側（後側）の角部の位置を検知してもよい。

また、上記実施形態では、パレット１２の中心位置Ｃが算出され、そのパレット１２の中心位置Ｃからフォーク９に対するパレット１２の位置が算出されているが、特にその形態には限られず、パレット１２の中心位置Ｃ以外の位置、例えばパレット１２の両端位置等から、フォーク９に対するパレット１２の位置を算出してもよい。

また、上記実施形態では、指示スイッチ２１によりパレット１２の荷置き動作が指示されると、リフトシリンダ１０によりパレット１２を持ち上げているが、特にその形態には限られず、例えばフォークリフト１の自動運転を管理する上位システムの管理装置によってパレット１２の荷置き動作が指示されると、リフトシリンダ１０によりパレット１２を持ち上げてもよい。

１…フォークリフト、４…車体、９…フォーク、１２…パレット、１２ｂ…下面、１２ｃ…側面（第１側面）、１２ｅ，１２ｆ…側面（第２側面）、２２…測距センサ（距離検出部）、２６…パレット点群抽出部、２７…直線推定部、２８…交点算出部、２９…推定演算部、３１…位置姿勢推定装置、３２…角部検知部、Ｃ…中心位置、Ｌ１…直線（第１直線）、Ｌ２，Ｌ３…直線（第２直線）、Ｐ…交点、Ｐ１，Ｐ２…交点。

Claims (7)

1. フォークリフトのフォークにより持ち上げられたパレットの位置及び姿勢を推定する位置姿勢推定装置であって、
   前記フォークにより持ち上げられた前記パレットの下方から前記パレットの下面までの距離を検出する距離検出部と、
   前記距離検出部の検出データに基づいて、前記パレットの下面における角部の位置を検知する角部検知部と、
   前記角部検知部により検知された前記パレットの下面における角部の位置に基づいて、前記フォークに対する前記パレットの位置及び姿勢の推定演算を行う推定演算部とを備える位置姿勢推定装置。
2. 前記角部検知部は、前記距離検出部の検出データに基づいて、前記パレットにおける互いに隣り合う側面に対応する複数の直線を推定し、前記複数の直線の交点の位置を前記パレットの下面における角部の位置として算出し、
   前記推定演算部は、前記交点の位置に基づいて、前記フォークに対する前記パレットの位置及び姿勢の推定演算を行う請求項１記載の位置姿勢推定装置。
3. 前記角部検知部は、前記距離検出部の検出データに基づいて、前記パレットの下面における左右両側の角部の位置を検知し、
   前記推定演算部は、前記パレットの下面における左右両側の角部の位置に基づいて、前記フォークに対する前記パレットの位置及び姿勢の推定演算を行う請求項１記載の位置姿勢推定装置。
4. 前記角部検知部は、前記距離検出部の検出データに基づいて、前記パレットにおいて前記フォークの差込方向に垂直な第１側面に対応する第１直線と前記第１側面にそれぞれ隣り合う左右両側の第２側面に対応する２本の第２直線とを推定し、前記第１直線と前記２本の第２直線との左右２つの交点の位置を前記パレットの下面における左右両側の角部の位置として算出し、
   前記推定演算部は、前記左右２つの交点の位置に基づいて、前記フォークに対する前記パレットの位置及び姿勢の推定演算を行う請求項３記載の位置姿勢推定装置。
5. 前記推定演算部は、前記左右２つの交点の位置に基づいて、前記パレットの中心位置を算出し、前記パレットの中心位置から前記フォークに対する前記パレットの位置を算出する請求項４記載の位置姿勢推定装置。
6. 前記推定演算部は、前記左右２つの交点の位置、前記第１直線及び前記第２直線の何れかに基づいて、前記パレットのヨー角を算出し、前記パレットのヨー角から前記フォークに対する前記パレットの姿勢を算出する請求項４または５記載の位置姿勢推定装置。
7. 前記角部検知部は、前記パレットの下面における前記フォークリフトの車体に対して手前側の角部の位置を検知し、
   前記推定演算部は、前記パレットの下面における前記車体に対して手前側の角部の位置に基づいて、前記フォークに対する前記パレットの位置及び姿勢の推定演算を行う請求項１～６の何れか一項記載の位置姿勢推定装置。

Images