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

Description

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

従来の位置姿勢推定装置としては、例えば特許文献１に記載されている技術が知られている。特許文献１に記載の位置姿勢推定装置は、物体との距離を測定する測距センサと、この測距センサの検出値に基づいて物体を認識する物体認識部と、測距センサの検出値に基づいて物体に存在する複数のポケットを検出し、フォークポケットのルールを用いて、２つのポケットをフォークポケットと判定するフォークポケット判定部と、パレットのルールを用いて、フォークポケットと判定された複数のポケットを備えた物体をパレットと判定するパレット判定部と、フォークポケット及びパレットの座標を算出する位置算出部とを備えている。

特開２０１８－２０８８１号公報

上記従来技術においては、フォークポケット及びパレットのルールに基づいて、フォークポケット及びパレットの位置を推定することはできるが、パレットの姿勢を高精度に推定することができない。

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

本発明の一態様は、１対のフォークを有するフォークリフトに対する荷役対象のパレットの位置及び姿勢を推定する位置姿勢推定装置であって、パレットまでの距離を検出する距離検出部と、距離検出部の検出データに基づいて、パレットの前面の平面方程式を算出する平面方程式算出部と、平面方程式算出部により算出されたパレットの前面の平面方程式を用いて、パレットの位置及び姿勢を推定する推定演算部とを備え、推定演算部は、パレットの姿勢としてパレットのヨー角、ピッチ角及びロール角を計算する。

このような位置姿勢推定装置においては、距離検出部によりパレットまでの距離が検出され、距離検出部の検出データに基づいてパレットの前面の平面方程式が算出される。そして、パレットの前面の平面方程式を用いてパレットの位置及び姿勢が推定される。このとき、パレットの姿勢として、パレットのヨー角、ピッチ角及びロール角が計算される。これにより、パレットの姿勢の推定精度が高くなる。このように荷役対象のパレットの位置だけでなく、荷役対象のパレットの姿勢も高精度に推定することができる。

位置姿勢推定装置は、パレットの前面の平面方程式に基づいて、パレットに設けられ１対のフォークが差し込まれる２つのフォーク穴を検知するフォーク穴検知部を更に備え、推定演算部は、パレットの前面の平面方程式に基づいて、パレットのヨー角及びピッチ角を計算すると共に、フォーク穴検知部により検知された２つのフォーク穴の位置関係に基づいて、パレットのロール角を計算してもよい。

このような構成では、パレットの前面の平面方程式に基づいて、パレットのヨー角及びピッチ角を計算することにより、パレットのヨー角及びピッチ角が簡単に且つ確実に求めることができる。また、２つのフォーク穴の位置関係に基づいて、パレットのロール角を計算することにより、パレットのロール角を簡単に且つ確実に求めることができる。

位置姿勢推定装置は、パレットを撮像する撮像部と、撮像部の撮像画像データに基づいて、パレットを認識するパレット認識部と、距離検出部の検出データのうちパレット認識部により認識されたパレットに対応する検出データを抽出するデータ抽出部とを更に備え、平面方程式算出部は、データ抽出部により抽出された検出データに基づいて、パレットの前面の平面方程式を算出してもよい。

このような構成では、撮像部の撮像画像データに基づいてパレットが認識され、距離検出部の検出データのうち認識されたパレットに対応する検出データが抽出される。撮像部の撮像画像データは、距離検出部の検出データに比べて解像度が高い。このため、パレットの認識精度が高くなる。従って、荷役対象のパレットの位置及び姿勢を更に高精度に推定することができる。また、パレットの位置及び姿勢の推定に要する時間を短縮することができる。

位置姿勢推定装置は、推定演算部により位置及び姿勢が推定されたパレットの前面の寸法が予め定められた規定値と合致しているかどうかを判定する判定部を更に備えてもよい。

このような構成では、位置及び姿勢が推定されたパレットの前面の寸法が規定値と合致しているかどうかを判定することにより、位置及び姿勢が推定されたパレットが荷役対象のパレットとして妥当であるかどうかが分かる。

位置姿勢推定装置は、距離検出部の検出データに対してフィルタリング処理を施すフィルタ部を更に備え、平面方程式算出部は、フィルタ部によりフィルタリング処理が施された検出データに基づいて、パレットの前面の平面方程式を算出してもよい。

このような構成では、距離検出部の検出データに対してフィルタリング処理を施すことにより、ノイズとなる検出データが除去された状態におけるパレットの前面の平面方程式が得られる。従って、荷役対象のパレットの位置及び姿勢を更に高精度に推定することができる。

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

本発明の一実施形態に係る位置姿勢推定装置が搭載されたフォークリフトを荷役対象のパレットと共に示す概略平面図である。 本発明の一実施形態に係る位置姿勢推定装置を備えた自動運転システムを示す概略構成図である。 図２に示されたコントローラにより実行される推定・制御処理手順の詳細を示すフローチャートである。 レーザ計測点データの一例を概略的に示す図である。 カメラの撮像画像データの一例を示す図である。 図２に示されたフィルタ部によりフィルタリング処理が施される前後のレーザ計測点データの一例を概略的に示す図である。

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

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

パレット５は、荷物を載せるための荷役台である。パレット５は、例えば平パレットである。パレット５は、平面視で四角形状を呈している。パレット５は、前面５ａと、この前面５ａと対向する後面５ｂと、前面５ａ及び後面５ｂと直交する２つの側面５ｃとを有している。前面５ａは、フォーク６によりパレット５を持ち上げる際に、フォークリフト１と向き合う面である。パレット５には、１対のフォーク６が差し込まれる２つのフォーク穴７が設けられている。フォーク穴７は、パレット５の前面５ａから後面５ｂまで延びている。フォーク穴７の形状は、正面視で矩形状である（図５参照）。

図２は、本発明の一実施形態に係る位置姿勢推定装置を備えた自動運転システムを示す概略構成図である。図２において、自動運転システム１０は、フォークリフト１の自動運転を実施するシステムである。自動運転システム１０は、フォークリフト１に搭載されている。自動運転システム１０は、レーザセンサ１１と、カメラ１２と、コントローラ１３と、駆動部１４と、通知部１５とを備えている。

レーザセンサ１１は、荷役対象のパレット５までの距離を検出する距離検出部である。レーザセンサ１１は、パレット５に向けてレーザを照射し、レーザの反射光を受光することにより、パレット５までの距離を計測する。レーザセンサ１１は、３Ｄ（３次元）のレーザを照射する。カメラ１２は、荷役対象のパレット５を撮像する撮像部である。

駆動部１４は、特に図示はしないが、例えば駆動輪を回転させる走行モータと、操舵輪を転舵させる操舵モータとを有している。通知部１５は、例えば表示器または警報器等である。

コントローラ１３は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ１３は、パレット認識部１６と、データ抽出部１７と、フィルタ部１８と、平面方程式算出部１９と、フォーク穴検知部２０と、推定演算部２１と、最終判定部２２（判定部）と、制御部２３とを有している。

ここで、レーザセンサ１１、カメラ１２、パレット認識部１６、データ抽出部１７、フィルタ部１８、平面方程式算出部１９、フォーク穴検知部２０、推定演算部２１及び最終判定部２２は、本実施形態の位置姿勢推定装置２４を構成している。位置姿勢推定装置２４は、フォークリフト１に対する荷役対象のパレット５の位置及び姿勢を推定する装置である。荷役対象のパレット５は、荷役装置３により荷役を開始しようとするパレットであり、フォークリフト１の前方に位置している。

パレット認識部１６は、カメラ１２の撮像画像データに基づいて、パレット５を認識する。データ抽出部１７は、レーザセンサ１１の計測点データ（検出データ）のうちパレット認識部１６により認識されたパレット５に対応する計測点データを抽出する。フィルタ部１８は、データ抽出部１７により抽出されたレーザセンサ１１の計測点データに対してフィルタリング処理を施す。

平面方程式算出部１９は、フィルタ部１８によりフィルタリング処理が施されたレーザセンサ１１の計測点データに基づいて、パレット５の前面５ａの平面方程式を算出する。フォーク穴検知部２０は、平面方程式算出部１９により算出されたパレット５の前面５ａの平面方程式に基づいて、２つのフォーク穴７を検知する。

推定演算部２１は、パレット５の前面５ａの平面方程式を用いて、パレット５の位置及び姿勢を推定する。このとき、推定演算部２１は、パレット５の姿勢として、パレット５のヨー角、ピッチ角及びロール角を計算する。推定演算部２１は、パレット５の前面５ａの平面方程式に基づいて、パレット５のヨー角及びピッチ角を計算すると共に、フォーク穴検知部２０により検知された２つのフォーク穴７の位置関係に基づいて、パレット５のロール角を計算する。

最終判定部２２は、推定演算部２１により位置及び姿勢が推定されたパレット５の前面５ａの寸法が予め定められた規定値と合致しているかどうかを最終的に判定する。

制御部２３は、推定演算部２１により推定されたパレット５の位置及び姿勢に基づいて、フォークリフト１を荷役対象のパレット５の手前の近傍位置まで移動させるように駆動部１４を制御する。

図３は、コントローラ１３により実行される推定・制御処理手順の詳細を示すフローチャートである。なお、本処理は、例えばフォークリフト１の自動運転の開始が指示されると、実行される。

図３において、コントローラ１３は、レーザセンサ１１の計測点データ（以下、レーザ計測点データという）を取得する（手順Ｓ１０１）。図４は、レーザ計測点データの一例が概略的に示された図である。図４では、載置台２６上にパレット５が３段で積み重ねられていると共にパレット５以外の複数の物体２７が配置された状態についてのレーザ計測点データＤ_Ｌ１が示されている。なお、レーザ計測点データ_Ｌ１上におけるパレット５については、便宜上実際のパレット５と同じ符号を付している。

続いて、コントローラ１３は、カメラ１２の撮像画像データを取得する（手順Ｓ１０２）。図５は、カメラ１２の撮像画像データの一例が示された図である。図５に示される撮像画像データＤｃでは、載置台２６上に３段で積み重ねられた状態のパレット５が写っている。

そして、コントローラ１３は、カメラ１２の撮像画像データに基づいて、ディープラーニング（深層学習）を利用した画像処理技術によってパレット５を認識する（手順Ｓ１０３）。

ディープラーニングは、人工知能の要素技術の一つである。ディープラーニングとは、十分なデータ量があれば、人間の力なしに機械が自動的にデータから特徴を抽出してくれるディープニュートラルネットワークを用いた学習のことである。ディープラーニングは、入力層と出力層との間の中間層を多層にすることで情報伝達及び処理を増やし、特徴量の精度や汎用性を上げたり、予測精度を向上させることが可能となる。

具体的には、コントローラ１３は、図５に示されるように、撮像画像データＤｃにおいてパレット５を取り囲むような枠線Ｗを指定する。枠線Ｗは、ディープラーニングを利用した物体検知において、bounding boxと称される矩形の枠である。そして、コントローラ１３は、枠線Ｗが指定された撮像画像データＤｃを学習データと比較することにより、パレット５を認識する。

続いて、コントローラ１３は、手順Ｓ１０１で取得されたレーザ計測点データのうち手順Ｓ１０３で認識されたパレット５に対応するレーザ計測点データを抽出する（手順Ｓ１０４）。これにより、図６（ａ）に示されるように、レーザ計測点データＤ_Ｌ１のうち認識されたパレット５に対応するレーザ計測点を含むレーザ計測点データＤ_Ｌ２が得られる。

続いて、コントローラ１３は、手順Ｓ１０４で抽出されたレーザ計測点データに対してフィルタリング処理を施す（手順Ｓ１０５）。このとき、点密度が所定量よりも低いレーザ計測点が除去される。これにより、図６（ｂ）に示されるように、パレット５以外のノイズに相当するレーザ計測点が除去されたレーザ計測点データＤ_Ｌ３が得られる。

続いて、コントローラ１３は、手順Ｓ１０５で得られたレーザ計測点データに基づいて、パレット５の前面５ａの平面方程式を算出する（手順Ｓ１０６）。コントローラ１３は、例えばＲＡＮＳＡＣ（Random SampleConsensus）等のロバスト推定法を用いて、レーザ計測点データにおける平面に相当するレーザ計測点以外のレーザ計測点を外れ値として除去することにより、パレット５の前面５ａの平面方程式を求める。ロバスト推定は、計測値（ここではレーザ計測点）に含まれる外れ値の影響を抑えることを目的とした手法である。なお、ロバスト推定法の代わりに最小二乗法等を用いて、パレット５の前面５ａの平面方程式を算出してもよい。

続いて、コントローラ１３は、手順Ｓ１０６で算出されたパレット５の前面５ａの平面方程式に基づいて、２つのフォーク穴７を検知する（手順Ｓ１０７）。このとき、レーザ計測点データにおいて、レーザ計測点の点密度が高い領域がパレット５の前面５ａとして検知され、レーザ計測点の点密度が低い２つの領域がフォーク穴７として検知される。

続いて、コントローラ１３は、パレット５の前面５ａの平面方程式に基づいて、パレット５の位置とパレット５のヨー角及びピッチ角とを計算する（手順Ｓ１０８）。パレット５のヨー角は、パレット５の上下方向（高さ方向）を軸とした回転角度である。パレット５のピッチ角は、パレット５の左右方向（幅方向）を軸とした回転角度である。

続いて、コントローラ１３は、手順Ｓ１０７で検知された２つのフォーク穴７の位置関係に基づいて、パレット５のロール角を計算する（手順Ｓ１０９）。パレット５のロール角は、パレット５の前後方向（奥行方向）を軸とした回転角度である。具体的には、コントローラ１３は、２つのフォーク穴７の中心位置を算出し、２つのフォーク穴７の中心位置の関係からパレット５のロール角を計算する。なお、パレット５の位置の推定演算も、２つのフォーク穴７の位置関係に基づいて行ってもよい。

続いて、コントローラ１３は、手順Ｓ１０８，Ｓ１０９で位置及び姿勢が推定されたパレット５の前面５ａの各部の寸法が予め定められた規定値と合致しているかどうかを判定する（手順Ｓ１１０）。パレット５の前面５ａの各部の寸法としては、パレット５の幅及び高さ、２つのフォーク穴７の寸法及び２つのフォーク穴７の中心間距離等がある。

コントローラ１３は、パレット５の前面５ａの各部の寸法が規定値と合致していると判定されたときは、パレット５の位置及び姿勢に基づいて、フォークリフト１を荷役対象のパレット５の手前の近傍位置まで移動させるように駆動部１４を制御する（手順Ｓ１１１）。コントローラ１３は、パレット５の前面５ａの各部の寸法が規定値と合致していないと判定されたときは、ＮＧである旨を通知部１５に通知する（手順Ｓ１１２）。

ここで、パレット認識部１６は、手順Ｓ１０２，Ｓ１０３を実行する。データ抽出部１７は、手順Ｓ１０１，Ｓ１０４を実行する。フィルタ部１８は、手順Ｓ１０５を実行する。平面方程式算出部１９は、手順Ｓ１０６を実行する。フォーク穴検知部２０は、手順Ｓ１０７を実行する。推定演算部２１は、手順Ｓ１０８，Ｓ１０９を実行する。最終判定部２２は、手順Ｓ１１０を実行する。制御部２３は、手順Ｓ１１１，Ｓ１１２を実行する。

以上のように本実施形態にあっては、レーザセンサ１１によりパレット５までの距離が検出され、レーザセンサ１１の計測点データに基づいてパレット５の前面５ａの平面方程式が算出される。そして、パレット５の前面５ａの平面方程式を用いてパレット５の位置及び姿勢が推定される。このとき、パレット５の姿勢として、パレット５のヨー角、ピッチ角及びロール角が計算される。これにより、パレット５の姿勢の推定精度が高くなる。このように荷役対象のパレット５の位置だけでなく、荷役対象のパレット５の姿勢も高精度に推定することができる。また、パレット５とフォークリフト１との間の距離が離れている場合でも、パレット５の前面５ａにマーカを付けることなく、荷役対象のパレット５の位置及び姿勢を高精度に推定することができる。

また、本実施形態では、パレット５の前面５ａの平面方程式に基づいて、パレット５のヨー角及びピッチ角を計算することにより、パレット５のヨー角及びピッチ角を簡単に且つ確実に求めることができる。また、パレット５の前面５ａの平面方程式に基づいて、２つのフォーク穴７を検知し、２つのフォーク穴７の位置関係に基づいて、パレット５のロール角を計算することにより、パレット５のロール角を簡単に且つ確実に求めることができる。

また、本実施形態では、カメラ１２の撮像画像データに基づいてパレット５が認識され、レーザセンサ１１の計測点データのうち認識されたパレット５に対応する計測点データが抽出される。カメラ１２の撮像画像データは、色情報を有すると共に、レーザセンサ１１の計測点データに比べて解像度が高い。このため、パレット５の認識精度が高くなる。従って、荷役対象のパレット５の位置及び姿勢を更に高精度に推定することができる。また、パレット５の位置及び姿勢の推定に要する時間を短縮することができる。

また、本実施形態では、位置及び姿勢が推定されたパレット５の前面５ａの寸法が規定値と合致しているかどうかを判定することにより、位置及び姿勢が推定されたパレット５が荷役対象のパレット５として妥当であるかどうかが分かる。

また、本実施形態では、レーザセンサ１１の計測点データに対してフィルタリング処理を施すことにより、ノイズとなる計測点データが除去された状態のパレット５の前面５ａの平面方程式が得られる。従って、荷役対象のパレット５の位置及び姿勢を更に高精度に推定することができる。

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、カメラ１２の撮像画像データに基づいて、ディープラーニングを利用した画像処理技術によってパレット５が認識されているが、特にその形態には限られず、例えばテンプレートマッチングやパターンマッチング等を用いてパレット５を認識してもよい。

また、上記実施形態では、カメラ１２の撮像画像データに基づいてパレット５が認識され、レーザセンサ１１の計測点データのうち認識されたパレット５に対応する計測点データに基づいて、パレット５の前面５ａの平面方程式が算出されているが、特にその形態には限られず、例えばカメラ１２の撮像画像データを用いずに、レーザセンサ１１の計測点データのみに基づいて、パレット５の前面５ａの平面方程式を算出してもよい。

また、上記実施形態では、レーザを照射するレーザセンサ１１によって、パレット５までの距離が検出されているが、パレット５までの距離を検出する距離検出部としては、特にレーザセンサ１１には限られず、例えば赤外線を照射する赤外線センサ等を使用してもよい。

１…フォークリフト、５…パレット、５ａ…前面、６…フォーク、７…フォーク穴、１１…レーザセンサ（距離検出部）、１２…カメラ（撮像部）、１６…パレット認識部、１７…データ抽出部、１８…フィルタ部、１９…平面方程式算出部、２０…フォーク穴検知部、２１…推定演算部、２２…最終判定部（判定部）、２４…位置姿勢推定装置。

Claims (4)

1. １対のフォークを有するフォークリフトに対する荷役対象のパレットの位置及び姿勢を推定する位置姿勢推定装置であって、
   前記パレットに向けてレーザを照射し、レーザの反射光を受光することにより、前記パレットまでの距離を計測するレーザセンサと、
   前記パレットを撮像する撮像部と、
   前記撮像部の撮像画像データに基づいて、深層学習を利用した画像処理技術によって前記パレットを認識するパレット認識部と、
   前記レーザセンサの計測点データのうち前記パレット認識部により認識されたパレットに対応する計測点データを抽出するデータ抽出部と、
   前記データ抽出部により抽出された前記パレットに対応する計測点データに基づいて、前記パレットの前面の平面方程式を算出する平面方程式算出部と、
   前記平面方程式算出部により算出された前記パレットの前面の平面方程式を用いて、前記パレットの位置及び姿勢を推定する推定演算部とを備え、
   前記撮像部の撮像画像データは、前記レーザセンサの計測点データに比べて解像度が高く、
   前記推定演算部は、前記パレットの姿勢として前記パレットのヨー角、ピッチ角及びロール角を計算する位置姿勢推定装置。
2. 前記パレットの前面の平面方程式に基づいて、前記パレットに設けられ前記１対のフォークが差し込まれる２つのフォーク穴を検知するフォーク穴検知部を更に備え、
   前記推定演算部は、前記パレットの前面の平面方程式に基づいて、前記パレットのヨー角及びピッチ角を計算すると共に、前記フォーク穴検知部により検知された前記２つのフォーク穴の位置関係に基づいて、前記パレットのロール角を計算する請求項１記載の位置姿勢推定装置。
3. 前記推定演算部により位置及び姿勢が推定されたパレットの前面の寸法が予め定められた規定値と合致しているかどうかを判定する判定部を更に備える請求項１または２記載の位置姿勢推定装置。
4. 前記データ抽出部により抽出された前記パレットに対応する計測点データに対してフィルタリング処理を施すフィルタ部を更に備え、
   前記平面方程式算出部は、前記フィルタ部により前記フィルタリング処理が施された前記パレットに対応する計測点データに基づいて、前記パレットの前面の平面方程式を算出する請求項１～３の何れか一項記載の位置姿勢推定装置。

Images