特許 7151041 位置姿勢推定装置

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

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

(11)【特許番号】

(24)【登録日】2022-10-03

(45)【発行日】2022-10-12

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

(51)【国際特許分類】

   B66F 9/24 20060101AFI20221004BHJP

   G01B 11/00 20060101ALI20221004BHJP

   G01B 11/26 20060101ALI20221004BHJP

【ＦＩ】

B66F9/24 P

G01B11/00 D

G01B11/26 Z

【請求項の数】 5

(21)【出願番号】P 2019146591

(22)【出願日】2019-08-08

(65)【公開番号】P2021024728

(43)【公開日】2021-02-22

【審査請求日】2021-11-11

【国等の委託研究の成果に係る記載事項】（出願人による申告）国立研究開発法人農業・食品産業技術総合研究機構生物系特定産業技術研究支援センター「革新的技術開発・緊急展開事業（うち人工知能未来農業創造プロジェクト）」、産業技術力強化法第１７条の適用を受ける特許出願

(73)【特許権者】

【識別番号】000003218

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

(74)【代理人】

【識別番号】100088155

【弁理士】

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

(74)【代理人】

【識別番号】100113435

【弁理士】

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

(74)【代理人】

【識別番号】100124062

【弁理士】

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

(74)【代理人】

【識別番号】100148013

【弁理士】

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

(72)【発明者】

【氏名】服部 晋悟

【審査官】太田 義典

(56)【参考文献】

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

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

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

【文献】国際公開第２０１９／１２５５５４（ＷＯ，Ａ１）

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

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

Ｇ０１Ｂ １１／００

Ｇ０１Ｂ １１／２６

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

【請求項1】

フォークリフトのフォークにより持ち上げられたパレットの位置及び姿勢を推定する位置姿勢推定装置であって、
前記フォークリフトの左右少なくとも一方側に配置され、前記フォークにより持ち上げられたパレットの側面にレーザを照射し、前記レーザの反射光を受光してレーザ計測点群を取得するレーザセンサと、
前記レーザセンサにより取得されたレーザ計測点群に基づいて、前記フォークに対する前記パレットの位置及び姿勢を推定する推定演算部とを備える位置姿勢推定装置。

【請求項2】

前記推定演算部は、前記レーザセンサにより取得されたレーザ計測点群に基づいて、前記レーザ計測点群の関係性を表す関数を算出して、前記フォークに対する前記パレットの位置及び姿勢を推定する請求項１記載の位置姿勢推定装置。

【請求項3】

前記レーザセンサにより取得されたレーザ計測点群のうちの外れ値を除去する外れ値除去部を更に備え、
前記推定演算部は、前記外れ値が除去されたレーザ計測点群の関係性を表す関数を算出する請求項２記載の位置姿勢推定装置。

【請求項4】

前記レーザセンサにより取得されたレーザ計測点群のうち前記パレットの側面で反射された反射光に相当するレーザ計測点群のみを抽出する側面候補抽出部を更に備え、
前記推定演算部は、前記側面候補抽出部により抽出されたレーザ計測点群の関係性を表す関数を算出する請求項２または３記載の位置姿勢推定装置。

【請求項5】

前記フォークを昇降させるリフトシリンダを制御する駆動制御部を更に備え、
前記駆動制御部は、前記パレットが前記レーザセンサの高さ位置に対応する高さ位置まで昇降するように前記リフトシリンダを制御する請求項１～４の何れか一項記載の位置姿勢推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【先行技術文献】

【特許文献】

【0003】

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

【発明の概要】

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

【0004】

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

【0005】

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

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

【0006】

本発明の一態様は、フォークリフトのフォークにより持ち上げられたパレットの位置及び姿勢を推定する位置姿勢推定装置であって、フォークリフトの左右少なくとも一方側に配置され、フォークにより持ち上げられたパレットの側面にレーザを照射し、レーザの反射光を受光してレーザ計測点群を取得するレーザセンサと、レーザセンサにより取得されたレーザ計測点群に基づいて、フォークに対するパレットの位置及び姿勢を推定する推定演算部とを備える。

【0007】

このような位置姿勢推定装置においては、フォークリフトのフォークによりパレットが持ち上げられた状態で、フォークリフトの左右少なくとも一方側に取り付けられたレーザセンサからパレットの側面にレーザが照射され、レーザの反射光がレーザセンサに受光されることで、レーザ計測点群が取得される。そして、レーザ計測点群に基づいて、フォークに対するパレットの位置及び姿勢が推定される。このとき、パレットの側面にレーザが照射されるため、パレット上の荷物の有無に関わらず、レーザセンサによってパレットの側面が検知される。これにより、フォークによりパレットを持ち上げた状態で、フォークに対するパレットの位置及び姿勢を推定することができる。

【0008】

推定演算部は、レーザセンサにより取得されたレーザ計測点群に基づいて、レーザ計測点群の関係性を表す関数を算出して、フォークに対するパレットの位置及び姿勢を推定してもよい。このようにレーザ計測点群の関係性を表す関数を算出することにより、フォークに対するパレットの位置及び姿勢を容易に推定することができる。

【0009】

位置姿勢推定装置は、レーザセンサにより取得されたレーザ計測点群のうちの外れ値を除去する外れ値除去部を更に備え、推定演算部は、外れ値が除去されたレーザ計測点群の関係性を表す関数を算出してもよい。このようにレーザ計測点群の外れ値を除去することにより、パレットの側面が精度良く検知される。従って、フォークに対するパレットの位置及び姿勢を正確に推定することができる。

【0010】

位置姿勢推定装置は、レーザセンサにより取得されたレーザ計測点群のうちパレットの側面で反射された反射光に相当するレーザ計測点群のみを抽出する側面候補抽出部を更に備え、推定演算部は、側面候補抽出部により抽出されたレーザ計測点群の関係性を表す関数を算出してもよい。このようにパレットの側面で反射された反射光に相当するレーザ計測点群のみを抽出することにより、パレットの側面が精度良く検知される。従って、フォークに対するパレットの位置及び姿勢を正確に推定することができる。

【0011】

位置姿勢推定装置は、フォークを昇降させるリフトシリンダを制御する駆動制御部を更に備え、駆動制御部は、パレットがレーザセンサの高さ位置に対応する高さ位置まで昇降するようにリフトシリンダを制御してもよい。このような構成では、フォークにより持ち上げられたパレットの高さ位置に対応する高さ位置にレーザセンサを配置しなくて済むため、レーザセンサの取付箇所の自由度が高くなる。

【発明の効果】

【0012】

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

【図面の簡単な説明】

【0013】

【図1】本発明の一実施形態に係る位置姿勢推定装置が適用されるフォークリフトを示す概略平面図である。

【図2】本発明の一実施形態に係る位置姿勢推定装置の構成を示すブロック図である。

【図3】図２に示された制御ユニットにより実行される推定処理手順の詳細を示すフローチャートである。

【図4】パレットの側面で反射された反射光に相当するレーザ計測点群の一例を示すイメージ図である。

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

【0014】

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

【0015】

図１は、本発明の一実施形態に係る位置姿勢推定装置が適用されるフォークリフトを示す概略平面図である。図１において、フォークリフト１は、パレット２を持ち上げる１対のフォーク３を備えている。フォーク３は、マスト４にリフトブラケット５を介して昇降可能に取り付けられている。

【0016】

パレット２上には、荷物が載置される。パレット２の種類としては、平パレット、ボックスパレットまたはシートパレット等が挙げられる。また、パレット２としては、ボックス型の網パレットでもよい。

【0017】

図２は、本発明の一実施形態に係る位置姿勢推定装置の構成を示すブロック図である。図２において、本実施形態の位置姿勢推定装置６は、フォークリフト１の自動運転が実施される際に、フォーク３により持ち上げられたパレット２の位置及び姿勢を推定する装置である。フォークリフト１の自動運転は、例えば上位システムの管理装置によって管理される。

【0018】

位置姿勢推定装置６は、受信器７と、２つのレーザセンサ９と、制御ユニット１０とを備えている。位置姿勢推定装置６は、フォークリフト１に搭載されている。

【0019】

受信器７は、上位システムからの情報を受信する。上位システムからの情報としては、例えば指示情報がある。指示情報は、上位システムからフォークリフト１に送信される情報の１つであり、パレット２がフォーク３に積み込まれたことが確認された後に、上位システムよりパレット２の位置及び姿勢の推定開始を指示するための情報である。なお、フォーク３にパレット２が積み込まれたことが検知されたら、制御ユニット１０内で指示情報が出されてもよい。

【0020】

レーザセンサ９は、図１に示されるように、フォークリフト１の左右両側に配置されている。レーザセンサ９としては、例えば障害物検知用のレーザセンサが使用される。レーザセンサ９は、例えばマスト４またはリフトブラケット５に固定されたバックレスト（図示せず）に取り付けられている。

【0021】

レーザセンサ９は、フォーク３により持ち上げられたパレット２の側面２ａにレーザＬを照射し、レーザＬの反射光を受光してレーザ計測点群を取得することにより、パレット２の側面２ａまでの距離を計測する。レーザセンサ９は、２次元方向（ＸＹ方向）にレーザＬを照射する。レーザセンサ９の計測範囲は、例えば２７０度である。レーザ計測点群は、複数のレーザ計測点からなっている（図４参照）。

【0022】

パレット２の側面２ａは、パレット２におけるフォークリフト１の左右方向（Ｙ方向）に垂直な面である。左右２つのレーザセンサ９間の距離は、パレット２の幅寸法（Ｙ方向寸法）よりも長い。なお、パレット２が網パレットである場合には、パレット２の側面２ａは、網目を除いた僅かな領域からなっている。

【0023】

制御ユニット１０は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。制御ユニット１０は、上位システムからの指示情報が受信器７で受信されると、レーザセンサ９の計測信号に基づいて、フォーク３に対するパレット２の位置及び姿勢を推定する。

【0024】

制御ユニット１０は、駆動制御部１１と、側面候補抽出部１２と、外れ値除去部１３と、推定演算部１４と、推定結果記憶部１５とを有している。

【0025】

駆動制御部１１は、フォーク３により持ち上げられたパレット２がレーザセンサ９の高さ位置に対応する高さ位置まで昇降するようにリフトシリンダ１６を制御する。リフトシリンダ１６は、フォーク３を昇降させる油圧シリンダである。

【0026】

側面候補抽出部１２は、レーザセンサ９により取得されたレーザ計測点群のうちパレット２の側面２ａで反射された反射光に相当するレーザ計測点群のみをパレット２の側面２ａの候補として抽出する。このようなパレット２の側面２ａで反射された反射光に相当するレーザ計測点群のみを抽出する処理については、後で詳述する。

【0027】

外れ値除去部１３は、レーザセンサ９により取得されたレーザ計測点群のうちの外れ値を除去する。このとき、外れ値除去部１３は、側面候補抽出部１２により抽出されたレーザ計測点群のうちの外れ値を除去する。このような外れ値を除去する処理については、後で詳述する。

【0028】

推定演算部１４は、レーザセンサ９により取得されたレーザ計測点群に基づいて、レーザ計測点群の関係性を表す関数を算出して、フォーク３に対するパレット２の位置及び姿勢を推定する。このとき、推定演算部１４は、側面候補抽出部１２により抽出されると共に外れ値除去部１３により外れ値が除去されることで得られたレーザ計測点群の関係性を表す関数を算出して、フォーク３に対するパレット２の位置及び姿勢を推定する。なお、パレット２の位置及び姿勢を推定する処理については、後で詳述する。

【0029】

推定結果記憶部１５は、推定演算部１４により得られたパレット２の位置及び姿勢の推定結果を記憶する。

【0030】

図３は、制御ユニット１０により実行される推定処理手順の詳細を示すフローチャートである。図３において、制御ユニット１０は、まず上位システム（前述）からの指示情報を受信器７を介して受信したかどうかを判断する（手順Ｓ１０１）。つまり、制御ユニット１０は、指示情報によってパレット２の位置及び姿勢の推定開始が指示されたかどうかを判断する。

【0031】

制御ユニット１０は、指示情報を受信したと判断したときは、フォーク３により持ち上げられたパレット２がレーザセンサ９の高さ位置に対応する高さ位置まで昇降するようにリフトシリンダ１６を制御する（手順Ｓ１０２）。なお、レーザセンサ９の高さ位置に対応する高さ位置は、設計誤差等によるレーザセンサ９の高さ位置との僅かな差異を含んでいる。これにより、パレット２の高さがレーザセンサ９の高さとほぼ等しくなる。

【0032】

続いて、制御ユニット１０は、レーザセンサ９により取得されたレーザ計測点群を抽出する（手順Ｓ１０３）。そして、制御ユニット１０は、図４に示されるように、手順Ｓ１０２で抽出されたレーザ計測点群のうちパレット２の側面２ａで反射された反射光に相当するレーザ計測点群Ｐのみを抽出する（手順Ｓ１０４）。

【0033】

このとき、パレット２の側面２ａで反射された反射光に相当するレーザ計測点と、パレット２の側面２ａ以外の部分で反射された反射光に相当するレーザ計測点とでは、レーザセンサ９から反射位置までの距離が異なる。従って、制御ユニット１０は、パレット２の側面２ａで反射された反射光に相当するレーザ計測点群Ｐのみを容易に抽出することができる。

【0034】

特にパレット２が網パレットである場合には、レーザセンサ９から照射されたレーザの一部が網パレットの網目を通り抜けるため、レーザセンサ９により取得されたレーザ計測点がばらつきやすくなる。網パレットの網目の大きさ又はレーザセンサ９の照射方向によっては、レーザが網パレットの網目を通り抜けて網パレットの側面以外の部分で反射されることで得られるレーザ計測点に比べて、レーザが網パレットの側面で反射されることで得られるレーザ計測点が極端に少なくなる。

【0035】

本処理手順を実行することにより、レーザが網パレットの網目を通り抜けて網パレットの側面以外の部分で反射されることで得られるレーザ計測点が除去され、レーザが網パレットの側面で反射されることで得られるレーザ計測点が残るようになる。

【0036】

続いて、制御ユニット１０は、図４に示されるように、手順Ｓ１０４で抽出されたレーザ計測点群Ｐのうちの外れ値ｒを除去する（手順Ｓ１０５）。外れ値ｒとは、他のレーザ計測点から大きく外れた値のことである。外れ値ｒは、雨や埃等が原因で出やすくなる。また、パレット２が網パレットである場合にも、外れ値ｒが出やすくなる。

【0037】

外れ値ｒかどうかの検定は、例えば平均値及び標準偏差を用いて行われる。具体的には、レーザ計測点が正規分布に従うと仮定した場合に、平均値から２σ～３σ（σ：標準偏差）程度以上離れた値が外れ値ｒとしてみなされる。

【0038】

続いて、制御ユニット１０は、手順Ｓ１０５で外れ値ｒが除去されたレーザ計測点群Ｐに基づいて、フォーク３に対するパレット２の位置及び姿勢を推定する（手順Ｓ１０６）。制御ユニット１０は、図４に示されるように、最小二乗法等を用いて、外れ値ｒが除去されたレーザ計測点群Ｐの関係性を表す関数Ｑを算出して、フォーク３に対するパレット２の位置及び姿勢を推定する。レーザ計測点群Ｐの関係性を表す関数Ｑは、例えば一次関数（回帰直線）である。

【0039】

最小二乗法は、誤差を伴う計測値（ここではレーザ計測点）の処理において、誤差の二乗の和を最小にすることで、最も確からしい回帰直線を求める既知の手法である。回帰直線は、平均値、共分散及び標準偏差等を用いて表される。

【0040】

続いて、制御ユニット１０は、手順Ｓ１０６で推定されたパレット２の位置及び姿勢を記憶する（手順Ｓ１０７）。

【0041】

ここで、駆動制御部１１は、手順Ｓ１０１，Ｓ１０２を実行する。側面候補抽出部１２は、手順Ｓ１０３，Ｓ１０４を実行する。外れ値除去部１３は、手順Ｓ１０５を実行する。推定演算部１４は、手順Ｓ１０６を実行する。推定結果記憶部１５は、手順Ｓ１０７を実行する。

【0042】

このような処理により、フォーク３に対するパレット２の積載状態を認識することができる。そして、フォーク３に対するパレット２の位置及び姿勢のずれがある場合には、推定結果記憶部１５の情報に基づいて、パレット２上の荷物を置く際にパレット２の位置及び姿勢の調整を行う。

【0043】

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

【0044】

ところで、レーザセンサ９からパレット２の上面にレーザを照射する場合には、パレット２上に荷物が載置されていると、レーザセンサ９によってパレット２が検知されないことがある。この場合には、フォーク３に対するパレット２の位置及び姿勢を推定することができない。

【0045】

そのような不具合に対し、本実施形態においては、フォークリフト１のフォーク３によりパレット２が持ち上げられた状態で、フォークリフト１の左右両側に取り付けられたレーザセンサ９からパレット２の側面２ａにレーザが照射され、レーザの反射光がレーザセンサ９に受光されることで、レーザ計測点群が取得される。そして、レーザ計測点群に基づいて、フォーク３に対するパレット２の位置及び姿勢が推定される。このとき、パレット２の側面２ａにレーザが照射されるため、パレット２上の荷物の有無に関わらず、レーザセンサ９によってパレット２の側面２ａが検知される。これにより、フォーク３によりパレット２を持ち上げた状態で、フォーク３に対するパレット２の位置及び姿勢を推定することができる。

【0046】

また、本実施形態では、最小二乗法等を用いて、レーザ計測点群の関係性を表す関数を算出することにより、フォーク３に対するパレット２の位置及び姿勢を容易に推定することができる。

【0047】

また、本実施形態では、レーザ計測点群の外れ値を除去することにより、パレット２の側面２ａが精度良く検知される。従って、フォーク３に対するパレット２の位置及び姿勢を正確に推定することができる。

【0048】

また、本実施形態では、パレット２の側面２ａで反射された反射光に相当するレーザ計測点群のみを抽出することにより、パレット２の側面２ａがより精度良く検知される。従って、フォーク３に対するパレット２の位置及び姿勢を更に正確に推定することができる。

【0049】

また、本実施形態では、パレット２がレーザセンサ９の高さ位置に対応する高さ位置まで昇降するようにリフトシリンダ１６が制御される。従って、フォーク３により持ち上げられたパレット２の高さ位置に対応する高さ位置にレーザセンサ９を配置しなくて済むため、レーザセンサ９の取付箇所の自由度が高くなり、レーザセンサ９を取り付けやすい箇所に取り付けることが可能となる。

【0050】

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、最小二乗法等を用いて、レーザ計測点群の関係性を表す関数を算出して、フォーク３に対するパレット２の位置及び姿勢を推定しているが、特にその形態には限られず、例えばＲＡＮＳＡＣ（Random SampleConsensus）等のロバスト推定法を用いて、レーザ計測点群の関係性を表す関数を算出して、フォーク３に対するパレット２の位置及び姿勢を推定してもよい。ロバスト推定は、計測値（ここではレーザ計測点）に含まれる外れ値の影響を抑えることを目的とした手法である。この場合には、レーザ計測点群の外れ値を除去する外れ値除去部１３については、特に無くてもよい。

【0051】

また、上記実施形態では、側面候補抽出部１２によって、レーザセンサ９により取得されたレーザ計測点群のうちパレット２の側面２ａで反射された反射光に相当するレーザ計測点群のみが抽出されているが、パレット２として網パレットを使用しない場合には、そのような側面候補抽出部１２については、特に無くてもよい。

【0052】

また、上記実施形態では、レーザセンサ９がフォークリフト１の左右両側に配置されているが、特にその形態には限られず、レーザセンサ９がフォークリフト１の左右一方側のみに配置されていてもよい。つまり、レーザセンサ９は、フォークリフト１の左右少なくとも一方側に配置されていればよい。

【0053】

また、上記実施形態では、パレット２がレーザセンサ９の高さ位置に対応する高さ位置まで昇降するようにリフトシリンダ１６が制御されているが、レーザセンサ９がパレット２の高さ位置に対応する高さ位置に配置されている場合には、そのようなリフトシリンダ１６の駆動制御は不要である。

【符号の説明】

【0054】

１…フォークリフト、２…パレット、３…フォーク、６…位置姿勢推定装置、９…レーザセンサ、１１…駆動制御部、１２…側面候補抽出部、１３…外れ値除去部、１４…推定演算部、Ｌ…レーザ、Ｐ…レーザ計測点群、ｒ…外れ値、Ｑ…関数。

【図1】

【図2】

【図3】

【図4】