特許 7661870 位置姿勢推定装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2025-04-07

(45)【発行日】2025-04-15

(54)【発明の名称】位置姿勢推定装置

(51)【国際特許分類】

   B66F 9/24 20060101AFI20250408BHJP

   G01B 11/00 20060101ALI20250408BHJP

   G01B 11/26 20060101ALI20250408BHJP

   G01B 11/02 20060101ALI20250408BHJP

【ＦＩ】

B66F9/24 P

G01B11/00 A

G01B11/26 Z

G01B11/02 Z

G01B11/26 H

【請求項の数】 6

(21)【出願番号】P 2021189959

(22)【出願日】2021-11-24

(65)【公開番号】P2023076925

(43)【公開日】2023-06-05

【審査請求日】2024-02-13

(73)【特許権者】

【識別番号】000003218

【氏名又は名称】株式会社豊田自動織機

(74)【代理人】

【識別番号】100088155

【弁理士】

【氏名又は名称】長谷川 芳樹

(74)【代理人】

【識別番号】100113435

【弁理士】

【氏名又は名称】黒木 義樹

(74)【代理人】

【識別番号】100124062

【弁理士】

【氏名又は名称】三上 敬史

(74)【代理人】

【識別番号】100148013

【弁理士】

【氏名又は名称】中山 浩光

(72)【発明者】

【氏名】岸田 匡弘

【審査官】板澤 敏明

(56)【参考文献】

【文献】特開２０２１－０４２０７０（ＪＰ，Ａ）

【文献】特開２０１８－０５８６７９（ＪＰ，Ａ）

【文献】特開２０２１－１６９３６０（ＪＰ，Ａ）

【文献】特開２０１７－１７８５６７（ＪＰ，Ａ）

【文献】米国特許出願公開第２０１９／０１９４００５（ＵＳ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｂ６６Ｆ ９／００－１１／０４

Ｇ０１Ｂ １１／００－１１／３０

(57)【特許請求の範囲】

【請求項1】

フォークリフトに搭載され、荷物を載せるパレット部と前記パレット部の前後左右の角部に立設された４本の柱部とを有するポストパレットの位置及び姿勢を推定する位置姿勢推定装置であって、
前記ポストパレットの前面に向けてレーザを照射し、レーザの反射光を受光することにより、前記ポストパレットまでの距離を検出して点群データを取得するレーザ距離検出部と、
前記レーザ距離検出部により取得された点群データに基づいて、前記パレット部及び前記柱部の前面に相当する平面を検知する平面検知部と、
前記平面検知部により検知された前記パレット部及び前記柱部の前面に相当する平面を用いて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢の推定演算を行う推定演算部とを備え、
前記推定演算部は、前記パレット部及び前記柱部の前面に相当する平面に基づいて、前記パレット部に相当する横線と前記柱部に相当する縦線とを抽出し、前記横線及び前記縦線に基づいて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢を算出し、
前記推定演算部は、前記縦線の傾き及び寸法に基づいて前記柱部の姿勢を算出することで、前記フォークリフトに対する前記ポストパレットの姿勢を算出すると共に、前記横線と前記縦線との交点を求め、前記交点の位置に基づいて前記パレット部の位置を算出することで、前記フォークリフトに対する前記ポストパレットの位置を算出する位置姿勢推定装置。

【請求項2】

フォークリフトに搭載され、荷物を載せるパレット部と前記パレット部の前後左右の角部に立設された４本の柱部とを有するポストパレットの位置及び姿勢を推定する位置姿勢推定装置であって、
前記ポストパレットの前面に向けてレーザを照射し、レーザの反射光を受光することにより、前記ポストパレットまでの距離を検出して点群データを取得するレーザ距離検出部と、
前記レーザ距離検出部により取得された点群データに基づいて、前記パレット部及び前記柱部の前面に相当する平面を検知する平面検知部と、
前記平面検知部により検知された前記パレット部及び前記柱部の前面に相当する平面を用いて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢の推定演算を行う推定演算部とを備え、
前記平面検知部は、前記ポストパレットに対応する第１点群データに対して点群が除去された第２点群データを取得し、前記第２点群データに対して規定厚よりも大きい厚さに平面近似を行って第１平面を求め、その後前記第１点群データのうち前記第１平面に含まれる第３点群データを取り出し、前記第３点群データに対して前記規定厚に平面近似を行って第２平面を求めることにより、前記パレット部及び前記柱部の前面に相当する平面を検知し、
前記推定演算部は、前記パレット部及び前記柱部の前面に相当する平面に基づいて、前記パレット部に相当する横線と前記柱部に相当する縦線とを抽出し、前記横線及び前記縦線に基づいて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢を算出する位置姿勢推定装置。

【請求項3】

フォークリフトに搭載され、荷物を載せるパレット部と前記パレット部の前後左右の角部に立設された４本の柱部とを有するポストパレットの位置及び姿勢を推定する位置姿勢推定装置であって、
前記ポストパレットの前面に向けてレーザを照射し、レーザの反射光を受光することにより、前記ポストパレットまでの距離を検出して点群データを取得するレーザ距離検出部と、
前記レーザ距離検出部により取得された点群データに基づいて、前記パレット部及び前記柱部の前面に相当する平面を検知する平面検知部と、
前記平面検知部により検知された前記パレット部及び前記柱部の前面に相当する平面を用いて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢の推定演算を行う推定演算部とを備え、
前記推定演算部は、前記パレット部及び前記柱部の前面に相当する平面に基づいて、前記パレット部に相当する横線と前記柱部に相当する縦線とを抽出し、前記横線及び前記縦線に基づいて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢を算出し、
前記推定演算部は、前記パレット部及び前記柱部の前面に相当する平面に含まれる平面点群データに対して直線近似を行って直線を求め、その後前記平面点群データのうち前記直線上にある点群データを取り出すことで、前記横線及び前記縦線を抽出する位置姿勢推定装置。

【請求項4】

フォークリフトに搭載され、荷物を載せるパレット部と前記パレット部の前後左右の角部に立設された４本の柱部とを有するポストパレットの位置及び姿勢を推定する位置姿勢推定装置であって、
前記ポストパレットの前面に向けてレーザを照射し、レーザの反射光を受光することにより、前記ポストパレットまでの距離を検出して点群データを取得するレーザ距離検出部と、
前記レーザ距離検出部により取得された点群データに基づいて、前記パレット部及び前記柱部の前面に相当する平面を検知する平面検知部と、
前記平面検知部により検知された前記パレット部及び前記柱部の前面に相当する平面を用いて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢の推定演算を行う推定演算部とを備え、
前記推定演算部は、前記パレット部及び前記柱部の前面に相当する平面に基づいて、前記パレット部に相当する横線と前記柱部に相当する縦線とを抽出し、前記横線及び前記縦線に基づいて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢を算出し、
前記推定演算部は、前記パレット部及び前記柱部の前面に相当する平面に含まれる平面点群データが仮想的に前記レーザ距離検出部に正対するように前記平面点群データを回転させることで、前記平面点群データの座標系を変換した状態で、前記横線及び前記縦線に基づいて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢を算出し、その後前記ポストパレットの位置及び姿勢を元の座標系に戻すように逆回転させることにより、前記フォークリフトに対する最終的な前記ポストパレットの位置及び姿勢を求める位置姿勢推定装置。

【請求項5】

フォークリフトに搭載され、荷物を載せるパレット部と前記パレット部の前後左右の角部に立設された４本の柱部とを有するポストパレットの位置及び姿勢を推定する位置姿勢推定装置であって、
前記ポストパレットの前面に向けてレーザを照射し、レーザの反射光を受光することにより、前記ポストパレットまでの距離を検出して点群データを取得するレーザ距離検出部と、
前記レーザ距離検出部により取得された点群データに基づいて、前記パレット部及び前記柱部の前面に相当する平面を検知する平面検知部と、
前記平面検知部により検知された前記パレット部及び前記柱部の前面に相当する平面を用いて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢の推定演算を行う推定演算部とを備え、
前記推定演算部は、前記パレット部及び前記柱部の前面に相当する平面に基づいて、前記パレット部に相当する横線と前記柱部に相当する縦線とを抽出し、前記横線及び前記縦線に基づいて、前記フォークリフトに対する前記ポストパレットの位置及び姿勢を算出し、
前記推定演算部は、前記フォークリフトに対する前記ポストパレットの位置及び姿勢を算出した後、前記横線及び前記縦線の傾き及び寸法に基づいて、前記平面検知部により検知された平面が前記ポストパレットの前面に該当するかどうかを最終的に判定し、前記平面検知部により検知された平面が前記ポストパレットの前面に該当しないと判定したときは、前記平面検知部により検知された平面に含まれる点群データを削除し、前記平面検知部に対して前記パレット部及び前記柱部の前面に相当する平面の検知処理を再度実行させる位置姿勢推定装置。

【請求項6】

前記ポストパレットの前面を撮像して画像データを取得する撮像部と、
前記撮像部により取得された画像データに基づいて、前記ポストパレットを認識するポストパレット認識部と、
前記レーザ距離検出部により取得された前記点群データのうち前記ポストパレット認識部により認識された前記ポストパレットに対応する点群データを抽出する点群処理部とを更に備え、
前記平面検知部は、前記点群処理部により抽出された点群データに基づいて、前記パレット部及び前記柱部の前面に相当する平面を検知する請求項１～５の何れか一項記載の位置姿勢推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

従来の位置姿勢推定装置としては、例えば特許文献１に記載されている技術が知られている。特許文献１に記載の位置姿勢推定装置は、パレットを検出する単眼カメラと、この単眼カメラと接続されたコントローラとを備えている。コントローラは、単眼カメラの撮像画像データに基づいてパレットを検知し、撮像画像データにおいてパレットの前面と２つのパレット穴とを取り囲むような枠線が指定された枠付き撮像画像データを作成するパレット検知部と、枠付き撮像画像データを用いてパレットの位置及び姿勢を推定する推定演算部とを有している。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０２１－２４７１８号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

ところで、フォークリフトによる荷役作業に使用されるパレットの一つとして、荷物が載置された状態で段重ねすることが可能なポストパレットがある。そのようなポストパレットについても、フォークリフトに対する位置及び姿勢を高精度に推定する必要がある。

【0005】

本発明の目的は、フォークリフトに対するポストパレットの位置及び姿勢を高精度に推定することができる位置姿勢推定装置を提供することである。

【課題を解決するための手段】

【0006】

本発明の一態様は、フォークリフトに搭載され、荷物を載せるパレット部とパレット部の前後左右の角部に立設された４本の柱部とを有するポストパレットの位置及び姿勢を推定する位置姿勢推定装置であって、ポストパレットの前面に向けてレーザを照射し、レーザの反射光を受光することにより、ポストパレットまでの距離を検出して点群データを取得するレーザ距離検出部と、レーザ距離検出部により取得された点群データに基づいて、パレット部及び柱部の前面に相当する平面を検知する平面検知部と、平面検知部により検知されたパレット部及び柱部の前面に相当する平面を用いて、フォークリフトに対するポストパレットの位置及び姿勢の推定演算を行う推定演算部とを備え、推定演算部は、パレット部及び柱部の前面に相当する平面に基づいて、パレット部に相当する横線と柱部に相当する縦線とを抽出し、横線及び縦線に基づいて、フォークリフトに対するポストパレットの位置及び姿勢を算出する。

【0007】

このような位置姿勢推定装置においては、レーザ距離検出部によってポストパレットの前面に向けてレーザが照射されることで、ポストパレットに対応する点群データが取得される。そして、ポストパレットに対応する点群データに基づいて、ポストパレットにおけるパレット部及び柱部の前面に相当する平面が検知される。そして、パレット部及び柱部の前面に相当する平面に基づいて、パレット部に相当する横線と柱部に相当する縦線とが抽出される。そして、横線及び縦線に基づいて、フォークリフトに対するポストパレットの位置及び姿勢が算出される。このとき、柱部は上下方向に長く延びているため、ポストパレットの上下方向の位置及び角度の精度が出やすい。これにより、フォークリフトに対するポストパレットの位置及び姿勢が高精度に推定される。

【0008】

位置姿勢推定装置は、ポストパレットの前面を撮像して画像データを取得する撮像部と、撮像部により取得された画像データに基づいて、ポストパレットを認識するポストパレット認識部と、レーザ距離検出部により取得された点群データのうちポストパレット認識部により認識されたポストパレットに対応する点群データを抽出する点群処理部とを更に備え、平面検知部は、点群処理部により抽出された点群データに基づいて、パレット部及び柱部の前面に相当する平面を検知してもよい。

【0009】

このような構成では、撮像部により取得された画像データに基づいてポストパレットが認識され、レーザ距離検出部により取得された点群データのうち認識されたポストパレットに対応する点群データが抽出される。画像データは、点群データに比べて解像度が高い。このため、ポストパレットの認識精度が高くなる。従って、フォークリフトに対するポストパレットの位置及び姿勢が更に高精度に推定される。

【0010】

推定演算部は、縦線の傾き及び寸法に基づいて柱部の姿勢を算出することで、フォークリフトに対するポストパレットの姿勢を算出すると共に、横線と縦線との交点を求め、交点の位置に基づいてパレット部の位置を算出することで、フォークリフトに対するポストパレットの位置を算出してもよい。

【0011】

このような構成では、柱部に相当する縦線の傾き及び寸法に基づいて柱部の姿勢が算出されると共に、パレット部に相当する横線と柱部に相当する縦線との交点の位置に基づいてパレット部の位置が算出される。従って、フォークリフトに対するポストパレットの位置及び姿勢を簡単に算出することができる。

【0012】

平面検知部は、ポストパレットに対応する第１点群データに対して点群が除去された第２点群データを取得し、第２点群データに対して規定厚よりも大きい厚さに平面近似を行って第１平面を求め、その後第１点群データのうち第１平面に含まれる第３点群データを取り出し、第３点群データに対して規定厚に平面近似を行って第２平面を求めることにより、パレット部及び柱部の前面に相当する平面を検知してもよい。

【0013】

このような構成では、点群が除去された第２点群データに対して規定厚よりも大きい厚さに平面近似を行って第１平面を求めることにより、平面近似を繰り返し行った際に第２点群データのばらつきに対応することができる。そして、第１点群データのうち第１平面に含まれる第３点群データを取り出し、第３点群データに対して規定厚に平面近似を行って第２平面を求めることにより、パレット部及び柱部の前面に相当する平面に欠けが生じにくくなると共に、パレット部及び柱部の前面に相当する平面に含まれない不要な点群が削除される。また、パレット部及び柱部の前面に相当する平面の位置及び角度のブレが抑制される。以上により、フォークリフトに対するポストパレットの位置及び姿勢が更に高精度に推定される。

【0014】

推定演算部は、パレット部及び柱部の前面に相当する平面に含まれる平面点群データに対して直線近似を行って直線を求め、その後平面点群データのうち直線上にある点群データを取り出すことで、横線及び縦線を抽出してもよい。

【0015】

このような構成では、パレット部及び柱部の前面に相当する平面に含まれる平面点群データに対して直線近似を行って直線を求めた後、平面点群データのうち直線上にある点群データを取り出すことにより、パレット部に相当する横線及び柱部に相当する縦線に欠けが生じにくくなると共に、横線及び縦線に含まれない不要な点群が削除される。従って、フォークリフトに対するポストパレットの位置及び姿勢が更に高精度に推定される。

【0016】

推定演算部は、パレット部及び柱部の前面に相当する平面に含まれる平面点群データが仮想的にレーザ距離検出部に正対するように平面点群データを回転させることで、平面点群データの座標系を変換した状態で、横線及び縦線に基づいて、フォークリフトに対するポストパレットの位置及び姿勢を算出し、その後ポストパレットの位置及び姿勢を元の状態に戻すように逆回転させることにより、フォークリフトに対する最終的なポストパレットの位置及び姿勢を求めてもよい。

【0017】

このような構成では、パレット部及び柱部の前面に相当する平面に含まれる平面点群データが仮想的にレーザ距離検出部に正対するように平面点群データを回転させることにより、ポストパレットの前後方向を無視して２次元的に横線と縦線との交点が求められる。このため、横線と縦線との交点が精度良く抽出される。従って、フォークリフトに対するポストパレットの位置及び姿勢が更に高精度に推定される。

【0018】

推定演算部は、フォークリフトに対するポストパレットの位置及び姿勢を算出した後、横線及び縦線の傾き及び寸法に基づいて、平面検知部により検知された平面がポストパレットの前面に該当するかどうかを最終的に判定し、平面検知部により検知された平面がポストパレットの前面に該当しないと判定したときは、平面検知部により検知された平面に含まれる点群データを削除し、平面検知部に対してパレット部及び柱部の前面に相当する平面の検知処理を再度実行させてもよい。

【0019】

このような構成では、検知された平面がポストパレットの前面に該当しないと判定されたときは、当該平面に含まれる点群データが削除され、平面の検知処理が再度実行されるので、パレット部及び柱部の前面に相当する正確な平面が得られる。従って、フォークリフトに対するポストパレットの位置及び姿勢が更に高精度に推定される。

【発明の効果】

【0020】

本発明によれば、フォークリフトに対するポストパレットの位置及び姿勢を高精度に推定することができる。

【図面の簡単な説明】

【0021】

【図1】本発明の一実施形態に係る位置姿勢推定装置が搭載されたフォークリフトをポストパレットと共に示す概略平面図である。

【図2】図１に示されたポストパレットの正面図である。

【図3】本発明の一実施形態に係る位置姿勢推定装置を具備した走行制御装置を示す概略構成図である。

【図4】レーザセンサにより取得された点群データと点群処理部により抽出された点群データとを示す図である。

【図5】図３に示された平面検知部により実行される平面検知処理の手順を示すフローチャートである。

【図6】点群データに対して通常の平面近似を行う平面検知処理を簡略化して示す概念図である。

【図7】図３に示された平面検知部により実行される平面検知処理を簡略化して示す概念図である。

【図8】図３に示された推定演算部により実行される推定演算処理の手順を示すフローチャートである。

【図9】平面点群データの座標変換が行われる様子を示す概念図である。

【図10】パレット部に相当する横線、柱部に相当する縦線及び横線と縦線との交点を示す図である。

【図11】図８に示された手順Ｓ１２２による直線抽出処理手順の詳細を示すフローチャートである。

【図12】平面点群データに対して直線近似を行う処理を簡略化して示す概念図である。

【図13】ポストパレットのロール角、ヨー角及びピッチ角を示す概念図である。

【発明を実施するための形態】

【0022】

以下、本発明の実施形態について図面を参照して詳細に説明する。

【0023】

図１は、本発明の一実施形態に係る位置姿勢推定装置が搭載されたフォークリフトをポストパレットと共に示す概略平面図である。図１において、フォークリフト１は、車体２と、この車体２の前側に配置され、荷役を行う荷役装置３とを備えている。荷役装置３は、車体２の前端部に取り付けられたマスト４と、このマスト４に昇降可能に取り付けられ、ポストパレット５を持ち上げる左右１対のフォーク６とを有している。

【0024】

ポストパレット５は、図２にも示されるように、平面視で正方形状のパレット部７と、このパレット部７の前後左右４つの角部にそれぞれ立設された４本の柱部８とを有している。なお、ポストパレット５の前後方向はＸ方向に相当し、ポストパレット５の左右方向はＹ方向に相当し、ポストパレット５の上下方向はＺ方向に相当する。

【0025】

パレット部７は、荷物Ｍを載せる荷役台である。パレット部７は、フォーク６によりポストパレット５を持ち上げる際に、フォークリフト１と向き合う前面７ａを有している。パレット部７には、１対のフォーク６が差し込まれる２つのフォーク穴９が設けられている。フォーク穴９は、パレット部７の前面７ａから後面７ｂまで延びている。フォーク穴９の形状は、正面視で矩形状である。

【0026】

柱部８は、四角柱状を呈している。柱部８は、パレット部７の角部に折り畳み可能に連結されている。柱部８は、前面８ａを有している。前面８ａは、パレット部７の前面７ａと同じ側の面である。

【0027】

このようなポストパレット５は、各柱部８がパレット部７に対して立ったときには、パレット部７に荷物Ｍを載置した状態で段重ねすることが可能である。この場合、下側のポストパレット５の各柱部８の上面に、上側のポストパレット５のパレット部７が置かれた状態となる。

【0028】

図３は、本発明の一実施形態に係る位置姿勢推定装置を備えた走行制御装置を示す概略構成図である。図３において、走行制御装置１０は、フォークリフト１がトラック荷台Т（図１参照）に積まれたポストパレット５の荷取りを行う際に、フォークリフト１を荷取り位置まで自動的に走行させる装置である。荷取り位置は、フォークリフト１の各フォーク６がポストパレット５のフォーク穴９に差し込まれる位置である。

【0029】

走行制御装置１０は、フォークリフト１に搭載されている。走行制御装置１０は、カメラ１１と、レーザセンサ１２と、コントローラ１３と、駆動部１４とを備えている。

【0030】

カメラ１１は、ポストパレット５の前面（正面）を撮像して画像データを取得する撮像部である。カメラ１１としては、例えば単眼カメラやステレオカメラ等が使用される。

【0031】

レーザセンサ１２は、ポストパレット５の前面に向けてレーザを照射し、レーザの反射光を受光することにより、ポストパレット５までの距離を検出して点群データを取得するレーザ距離検出部である。点群は、ポストパレット５を含む物体におけるレーザの反射点の集まりである。レーザセンサ１２は、点密度が高い２Ｄまたは３Ｄのレーザを照射する。レーザセンサ１２としては、例えばＬＩＤＡＲやレーザレンジファインダ等が使用される。

【0032】

コントローラ１３は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ１３は、カメラ１１により取得された画像データとレーザセンサ１２により取得された点群データとを入力し、所定の処理を実行し、駆動部１４を制御する。駆動部１４は、特に図示はしないが、例えばフォークリフト１の駆動輪を回転させる走行モータと、フォークリフト１の操舵輪を転舵させる操舵モータとを有している。

【0033】

コントローラ１３は、ポストパレット認識部１５と、点群処理部１６と、平面検知部１７と、推定演算部１８と、経路生成部１９と、走行制御部２０とを有している。

【0034】

ここで、ポストパレット認識部１５、点群処理部１６、平面検知部１７及び推定演算部１８は、カメラ１１及びレーザセンサ１２と協働して、本実施形態の位置姿勢推定装置２１を構成している。位置姿勢推定装置２１は、フォークリフト１がポストパレット５の正面側に位置している状態で、フォークリフト１に対するポストパレット５の位置及び姿勢を推定する装置である。

【0035】

ポストパレット認識部１５は、カメラ１１により取得された画像データに基づいて、ポストパレット５を認識する。ポストパレット認識部１５は、例えばディープラーニング（深層学習）を利用した画像処理技術によって、ポストパレット５の大凡の位置及び寸法を認識する。

【0036】

点群処理部１６は、レーザセンサ１２により取得された点群データのうちポストパレット認識部１５により認識されたポストパレット５に対応する点群データを抽出する。例えば、レーザセンサ１２によって図４（ａ）に示されるような点群データＤｐが取得された場合、ポストパレット認識部１５によりポストパレット５が認識されると、図４（ｂ）に示されるように、ポストパレット５に対応する点群データＤｐｐが得られる。点群データＤｐｐでは、点群データＤｐに比べて点群データの範囲が左右方向（Ｙ方向）に多少狭まるようになる。

【0037】

平面検知部１７は、点群処理部１６により抽出されたポストパレット５に対応する点群データに基づいて、ポストパレット５におけるパレット部７の前面７ａ及び柱部８の前面８ａ（以下、単に前面７ａ，８ａということがある）に相当する平面を検知する。

【0038】

平面検知部１７は、例えば点群データに対して平面をフィッティングして平面方程式の係数を求めることで、前面７ａ，８ａに相当する平面を検知する。このとき、平面検知部１７は、例えばＲＡＮＳＡＣ（Random sample consensus）を用いて、点群データの何点かをランダムにピックアップして平面をフィッティングする。

【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】

１…フォークリフト、５…ポストパレット、７…パレット部、７ａ…前面、８…柱部、８ａ…前面、１１…カメラ（撮像部）、１２…レーザセンサ（レーザ距離検出部）、１５…ポストパレット認識部、１６…点群処理部、１７…平面検知部、１８…推定演算部、２１…位置姿勢推定装置、Ｄｐ…点群データ、Ｄｐｐ…点群データ、Ｄｐ１…第１点群データ、Ｄｐ２…第２点群データ、Ｄｐ３…第３点群データ、Ｄｆ…平面点群データ、Ｄｆ１…平面点群データ、Ｄｆ２…点群データ、Ｆ１…第１平面、Ｆ２…第２平面、Ｌ…直線、Ｌａ…横線、Ｌｂ…縦線、Ｉ…交点。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】