JP7187388B2 - 位置姿勢推定装置及び位置姿勢推定方法 - Google Patents

Description

本発明は、果菜の位置及び姿勢を推定する位置姿勢推定装置及び位置姿勢推定方法に関する。

ロボットハンドを用いて果菜を収穫する場合、果菜の三次元の位置及び姿勢を正しく推定する必要がある。そのためには、例えば、カメラを用いて収穫対象の果菜を撮像し、撮像した果菜の距離画像から得られる三次元点群に対して、既定の三次元モデルによるフィッティングを行うことで、該果菜の三次元の位置及び姿勢を推定する。

特許文献１には、果実の画像を取得する画像取得部と、取得された画像における果実の位置と複数の果実間の距離とに基づいて、果実に連結している果梗の位置を推定する推定部とを果梗位置推定装置が備えることにより、安定的な収穫動作を実現可能とすることが開示されている。

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

しかしながら、カメラで収穫対象の果菜を撮像する場合、他の果菜や葉等の他の物体によって該収穫対象の果菜の少なくとも一部が隠れてしまい、撮像した画像から果菜の一部の三次元点群しか取得することができない。これにより、三次元モデルによるフィッティングの結果が不安定となる。この結果、果菜の三次元の位置及び姿勢を正しく推定することが困難となり、ロボットハンドによる果菜の収穫が失敗する可能性がある。

本発明は、このような課題を考慮してなされたものであり、果菜の三次元の位置及び姿勢を正しく推定することが可能となる位置姿勢推定装置及び位置姿勢推定方法を提供することを目的とする。

本発明の態様は、果菜の位置及び姿勢を推定する位置姿勢推定装置及び位置姿勢推定方法に関する。

前記位置姿勢推定装置は、前記果菜の画像を取得する画像取得部と、前記画像に基づいて前記果菜の中心位置を推定する中心位置推定部と、前記画像に基づいて前記果菜の姿勢を決定付ける特徴部の位置を抽出する特徴部抽出部と、推定された前記中心位置に対する信頼性を評価する推定結果評価部と、抽出された前記特徴部の位置に対する信頼性を評価する抽出結果評価部と、前記推定結果評価部及び前記抽出結果評価部の双方で信頼性が有ると評価された場合に、前記中心位置及び前記特徴部の位置から前記果菜の中心軸を推定し、推定した前記中心軸から前記果菜の姿勢を推定する姿勢推定部とを有する。

前記位置姿勢推定方法は、画像取得部を用いて前記果菜の画像を取得するステップと、中心位置推定部を用いて前記画像に基づき前記果菜の中心位置を推定し、一方で、特徴部抽出部を用いて前記画像に基づき前記果菜の姿勢を決定付ける特徴部の位置を抽出するステップと、推定結果評価部を用いて、推定された前記中心位置に対する信頼性を評価し、一方で、抽出結果評価部を用いて、抽出された前記特徴部の位置に対する信頼性を評価するステップと、前記推定結果評価部及び前記抽出結果評価部の双方で信頼性が有ると評価された場合に、姿勢推定部を用いて前記中心位置及び前記特徴部の位置から前記果菜の中心軸を推定し、推定した前記中心軸から前記果菜の姿勢を推定するステップとを有する。

本発明によれば、推定した果菜の中心位置及び特徴部の位置に対する信頼性を評価するので、信頼性が低いと評価した場合には、果菜を異なる方向から撮像した画像を取得する等の対応を取ることができる。これにより、果菜の三次元の位置及び姿勢を正しく推定することが可能となる。この結果、ロボットハンドを用いて果菜を確実に収穫することができる。

本実施形態に係る推定装置を搭載した果菜収穫装置の構成図である。 図１のロボットの側面図である。 図１の果菜収穫装置の動作を示すフローチャートである。 図３のステップＳ３の詳細を示すフローチャートである。 図４のステップＳ１１の詳細を示すフローチャートである。 図４のステップＳ１２の詳細を示すフローチャートである。 図４のステップＳ１３の詳細を示すフローチャートである。 図４のステップＳ１５の詳細を示すフローチャートである。 図５のステップＳ１１の処理の説明図である。 球フィッティングの説明図である。 図６のステップＳ１２の処理の説明図である。 図１２Ａ～図１２Ｃは、図７のステップＳ１３の処理の説明図である。 図８のステップＳ１５の処理の説明図である。 信頼性有りと評価される中心位置の抽出例を図示した説明図である。 信頼性有りと評価される特徴部の抽出例を図示した説明図である。 信頼性無しと評価される中心位置の抽出例を図示した説明図である。 信頼性無しと評価される特徴部の抽出例を図示した説明図である。

以下、本発明に係る位置姿勢推定装置及び位置姿勢推定方法について、好適な実施形態を例示し、添付の図面を参照しながら説明する。

［１．本実施形態の構成］
図１に示すように、本実施形態に係る位置姿勢推定装置１０（以下、本実施形態に係る推定装置１０ともいう。）は、果菜収穫装置１２に適用される。果菜収穫装置１２は、収穫対象の果菜１４の三次元の位置及び姿勢を推定する推定装置１０と、該推定装置１０の推定結果に基づき動作することで果菜１４を収穫するロボット１６とを備える。

なお、本実施形態において、果菜１４は、離層を有する果梗の先端に生る果物や野菜である。このような果菜１４としては、例えば、トマトがある。また、図１は、果菜収穫装置１２の構成を概念的に図示している。そのため、本実施形態では、推定装置１０とロボット１６とを一体的に構成してもよいし、又は、推定装置１０とロボット１６とを別体で構成してもよい。推定装置１０とロボット１６とを一体的に構成する場合、例えば、ロボット１６に推定装置１０が搭載される。

推定装置１０は、画像取得部１８、画像処理部２０（中心位置推定部、特徴部抽出部）、信頼性評価部２２（推定結果評価部、抽出結果評価部）、姿勢推定部２４及び通信部２６を有する。推定装置１０は、コンピュータであって、不図示のメモリに記憶されたプログラムを読み出して実行することにより、画像処理部２０、信頼性評価部２２及び姿勢推定部２４等の機能を実現する。

画像取得部１８は、果菜１４を含む距離画像を取得するステレオカメラ等の撮像手段である。画像処理部２０は、距離画像に基づいて、果菜１４の中心位置を推定すると共に、該果菜１４の姿勢を決定付ける離層又は小果梗等の特徴部の位置を抽出する。信頼性評価部２２は、画像処理部２０が推定した果菜１４の中心位置に対する信頼性を評価すると共に、該画像処理部２０が抽出した特徴部の位置に対する信頼性を評価する。

姿勢推定部２４は、果菜１４の中心位置の推定結果、及び、特徴部の位置の抽出結果の双方について、信頼性が有る（高い）と信頼性評価部２２が評価した場合に、中心位置及び特徴部の位置から果菜１４の中心軸を推定し、推定した中心軸から果菜１４の三次元の姿勢を推定する。従って、果菜１４の中心位置又は特徴部の位置の少なくとも一方について、信頼性が無い（低い）と信頼性評価部２２が評価した場合、姿勢推定部２４は、果菜１４の三次元の姿勢の推定処理を行わない。なお、果菜１４の中心軸とは、果菜１４の中心位置と特徴部の位置とを結ぶ軸（三次元のベクトル）をいう。

通信部２６は、ロボット１６との間で信号又は情報の送受信を行う。例えば、通信部２６は、姿勢推定部２４の推定結果等をロボット１６に送信する。なお、ロボット１６に推定装置１０が搭載される場合、通信部２６を省略してもよい。

ロボット１６は、通信部２８、制御部３０、移動機構３２及びロボットハンド３４を有する。通信部２８は、推定装置１０の通信部２６や外部との間で、信号又は情報の送受信を行う。制御部３０は、各通信部２６、２８を介して供給される姿勢推定部２４の推定結果等に基づき、ロボット１６内の各部を制御する。移動機構３２は、モータやアーム等を含み構成され、制御部３０からの制御に従って、ロボットハンド３４を所望の位置及び方向に移動させる。ロボットハンド３４は、果菜１４を収穫するエンドエフェクタである。

図２は、ロボット１６の具体的構成例を示す側面図である。ロボット１６は、収穫した果菜３５を収納するストッカ部３６と、鉛直移動機構３８と、該鉛直移動機構３８を支持する支持台部４０と、鉛直移動機構３８を構成する鉛直移動スライダ４２に支持された移動台座４４と、該移動台座４４の上方に設けられた収穫用アーム４６とを備える。収穫用アーム４６の先端には、ロボットハンド３４が装着されている。移動台座４４の内部には、収穫用アーム４６を紙面に直交する水平方向に変位させる水平移動機構４８が設けられる。ストッカ部３６の下部における四方の隅角部には、ロボット１６を前進又は後退させることや、方向変換を行うことが可能な車輪５０が設けられる。

従って、ロボット１６は、鉛直移動機構３８を駆動させて移動台座４４を上下動させることにより、ロボットハンド３４を上下方向に移動させる。また、ロボット１６は、水平移動機構４８を駆動させて収穫用アーム４６を水平方向に変位させることにより、ロボットハンド３４を水平方向に移動させる。さらに、ロボット１６は、該ロボット１６を車輪５０によって前進、後退又は方向変換することにより、ロボットハンド３４を左右及び前後に移動させることが可能である。つまり、鉛直移動機構３８、移動台座４４、水平移動機構４８、収穫用アーム４６及び車輪５０によって図１の移動機構３２が構成される。

この場合、移動台座４４にはベースカメラ５２が配置され、ロボットハンド３４にはハンドカメラ５４が配置されている。これらのカメラは、図１の画像取得部１８を構成する。ベースカメラ５２は、収穫対象の果菜１４の周辺を撮像するステレオカメラである。ハンドカメラ５４は、ロボットハンド３４を収穫対象の果菜１４に近接させた際に、該果菜１４を撮像するステレオカメラである。

なお、図２のロボット１６は一例である。本実施形態では、果菜１４を収穫可能であれば、どのような構成のロボット１６でもよいことは勿論である。

［２．本実施形態の動作］
次に、本実施形態に係る推定装置１０を含む果菜収穫装置１２の動作について、図３～図１７を参照しながら説明する。図３～図８は、果菜収穫装置１２の動作を示すフローチャートである。また、図９～図１７は、図３～図８の各フローチャート中の所定のステップに対応する図である。なお、この動作説明では、必要に応じて、図１及び図２も参照しながら説明する。

＜２．１ 果菜収穫装置１２全体の動作＞
先ず、図３のステップＳ１において、ロボット１６（図１及び図２参照）は移動を行い、収穫対象となる果菜１４の手前に停止する。

次のステップＳ２において、ベースカメラ５２を用いて該果菜１４の周辺を撮像する。これにより、ベースカメラ５２が取得した距離画像から、収穫対象の果菜１４や、該果菜１４を収穫する際の障害物（例えば、周囲の果菜）を検出することができる。この場合、ベースカメラ５２が取得した距離画像に基づいて、ハンドカメラ５４を果菜１４が撮像可能な位置及び方向に移動させればよい。

次のステップＳ３において、ハンドカメラ５４を用いて収穫対象の果菜１４を撮像する。これにより、ハンドカメラ５４が取得した距離画像から、該果菜１４を検出することができる。そして、ステップＳ３において、推定装置１０では、取得された距離画像に基づき、距離画像に写り込んでいる収穫対象の果菜１４について、三次元の位置及び姿勢を推定する。三次元の位置及び姿勢の推定結果は、推定装置１０の通信部２６からロボット１６に送信される。

次のステップＳ４において、ロボット１６の制御部３０は、各通信部２６、２８を介して供給された推定結果に基づき移動機構３２を制御し、ロボットハンド３４を所望の位置及び方向に移動させる。また、制御部３０は、ハンドカメラ５４が取得した距離画像に基づき、ロボットハンド３４によって果菜１４に連なる果梗（離層）を折り曲げる方向を推定する。

次のステップＳ５において、ロボットハンド３４は、制御部３０からの制御に基づき、果梗（離層）を折り曲げることで、収穫対象の果菜１４を枝から分離し、該果菜１４を収穫する。

次のステップＳ６において、他の果菜も収穫する場合には（ステップＳ６：ＮＯ）、ステップＳ１に戻り、他の果菜を収穫対象として、ステップＳ１～Ｓ６の動作を繰り返し実行する。一方、収穫が完了した場合には（ステップＳ６：ＹＥＳ）、図３の収穫動作を完了する。

＜２．２ 推定装置１０全体の動作説明＞
次に、図３のステップＳ３の動作、すなわち、本実施形態に係る推定装置１０の動作（位置姿勢推定方法であって、以下、「推定方法」ともいう。）について、図４～図８のフローチャートと、図９～図１７の説明図とを参照しながら説明する。

先ず、図４のステップＳ１１において、推定装置１０の画像処理部２０（図１参照）は、画像取得部１８（図２のハンドカメラ５４）が撮像した収穫対象の果菜１４の距離画像に基づき、該果菜１４の中心位置を推定する。また、画像処理部２０は、ステップＳ１２において、果菜１４の距離画像から、果菜１４に連なる小果梗又は離層等の特徴部の位置を抽出する。

次のステップＳ１３において、信頼性評価部２２は、画像処理部２０が推定した果菜１４の中心位置に対する信頼性を評価する。

ここで、中心位置に対する信頼性が低い（無い）と評価した場合、信頼性評価部２２は、ハンドカメラ５４が撮像した距離画像に示す果菜１４の方向では、該果菜１４を収穫することができないと判断する。そして、ステップＳ１４において、果菜１４を別の方向（視点）から撮像するよう、画像取得部１８に指示する（ステップＳ１４：ＹＥＳ）。これにより、視点が変更された距離画像を用いて、ステップＳ１１～Ｓ１４の処理が再度行われる。

一方、中心位置に対する信頼性が高い（有る）と評価した場合、信頼性評価部２２は、ハンドカメラ５４が撮像した距離画像により求められた果菜１４の方向で、該果菜１４を収穫することが可能と判断する。そして、ステップＳ１４において、信頼性評価部２２は、再撮影は不要と判断し（ステップＳ１４：ＮＯ）、ステップＳ１５に進む。

次のステップＳ１５において、信頼性評価部２２は、画像処理部２０が抽出した特徴部の位置に対する信頼性を評価する。

ここで、特徴部の位置に対する信頼性が低い（無い）と評価した場合、信頼性評価部２２は、ハンドカメラ５４が撮像した距離画像に示す果菜１４の方向では、該果菜１４を収穫することができないと判断する。そして、ステップＳ１６において、果菜１４を別の視点から撮像するよう、画像取得部１８に指示する（ステップＳ１６：ＹＥＳ）。この場合でも、視点が変更された距離画像を用いて、ステップＳ１１～Ｓ１６の処理が再度行われる。

一方、特徴部に対する信頼性が高い（有る）と評価した場合、信頼性評価部２２は、ハンドカメラ５４が撮像した距離画像に示す果菜１４の方向で該果菜１４を収穫することが可能と判断すると共に、ステップＳ１６において再撮影が不要と判断し（ステップＳ１６：ＮＯ）、ステップＳ１７に進む。

次のステップＳ１７において、姿勢推定部２４は、果菜１４の中心位置及び特徴部の位置から果菜１４の中心軸を推定し、推定した中心軸から果菜１４の三次元の姿勢を推定する。

＜２．３ 果菜１４の中心位置の推定処理＞
次に、図４のステップＳ１１の詳細について、図５のフローチャートと、図９の説明図とを参照しながら説明する。

先ず、図５のステップＳ２１において、画像取得部１８（図１）は、収穫対象の果菜１４の距離画像を取得する。図９には、取得した果菜１４の距離画像５６の一例を図示している。

次のステップＳ２２において、画像処理部２０は、「Ｍａｓｋ－ＲＣＮＮ」等の深層学習による物体検知技術を利用して、取得した距離画像５６に写り込んでいる果菜１４の画像領域５８を抽出するセグメンテーション処理を実行する。なお、抽出される画像領域５８は、距離画像５６に写り込んでいる果菜１４の表面に対応する画像領域である。図９には、抽出された果菜１４の表面の画像領域５８を図示している。

次のステップＳ２３において、画像処理部２０は、抽出した果菜１４の表面の画像領域５８から三次元点群６０を抽出する。図９には、抽出された三次元点群６０を図示している。

次のステップＳ２４において、画像処理部２０は、抽出した三次元点群６０を、果菜１４を模擬した球モデル６２にフィッティングする。これにより、画像処理部２０は、三次元点群６０がフィッティングされた球モデル６２の中心位置を、果菜１４の中心位置と推定する。すなわち、本実施形態では、果菜１４を回転体モデルである球モデル６２とみなすことで、該球モデル６２の中心位置を果菜１４の中心位置として推定する。

図９には、三次元点群６０を球モデル６２にフィッティングした結果を図示している。すなわち、トマト等の果菜１４は、球に近い形状を有するため、三次元点群６０を球モデル６２にフィッティングすることで、果菜１４の中心位置を精度よく推定することが可能である。

なお、図１０に示すように、楕円体モデル６４に三次元点群６０をフィッティングした場合、片寄った形状で三次元点群６０がフィッティングされるので、果菜１４の中心位置を精度よく推定することができない。

＜２．４ 特徴部６８の位置の抽出処理＞
次に、図４のステップＳ１２の詳細について、図６のフローチャートと、図１１の説明図とを参照しながら説明する。

先ず、図６のステップＳ３１において、画像取得部１８（図１）は、収穫対象の果菜１４の距離画像５６を取得する。図１１には、取得した果菜１４の距離画像５６の一例を図示している。なお、図５のステップＳ２１において、距離画像５６を既に取得しているので、ステップＳ３１の処理をスキップすることは可能である。

次のステップＳ３２において、画像処理部２０は、「Ｓｅｇｎｅｔ」等の深層学習による画像分割技術を利用して、取得した距離画像５６に写り込んでいる小果梗又は離層の画像領域を、特徴部６８の画像領域として抽出するセグメンテーション処理を実行する。図１１には、抽出される特徴部６８の画像領域を図示している。

次のステップＳ３３において、画像処理部２０は、抽出した特徴部６８の画像領域から三次元点群７０を抽出する。図１１には、抽出された三次元点群７０を図示している。

＜２．５ 中心位置の信頼性評価＞
次に、図４のステップＳ１３の詳細について、図７のフローチャートと、図１２Ａ～図１２Ｃの説明図とを参照しながら説明する。

先ず、図７のステップＳ４１において、信頼性評価部２２（図１参照）は、球モデル６２の表面に対する果菜１４の三次元点群６０の広がりが大きいか否かを評価する。三次元点群６０の広がりがある程度大きい場合、画像処理部２０での三次元点群６０の抽出結果の信頼性が高いと評価することができるためである。図１２Ａには、球モデル６２の中心位置７２に対して、角度θの方向に三次元点群６０が広がって分布している場合を図示している。

三次元点群６０の広がりが大きいと評価した場合（ステップＳ４１：ＹＥＳ）、信頼性評価部２２は、次のステップＳ４２に進む。ステップＳ４２において、信頼性評価部２２は、画像取得部１８から果菜１４の二次元画像を取得し、該二次元画像（果菜１４の画像領域）にフィッティングする第１円７４を設定する（図１２Ｂ参照）。

次のステップＳ４３において、信頼性評価部２２は、球モデル６２を果菜１４の二次元画像に投影した第２円７６を設定する（図１２Ｂ参照）。

次のステップＳ４４において、信頼性評価部２２は、フィッティングさせた第１円７４と、投影した第２円７６とについて、両者の差（誤差）が小さいか否かを評価する。誤差としては、図１２Ｃに示すように、例えば、第１円７４と第２円７６とが重ならない領域Ｒ１、Ｒ２の面積Ｓ１、Ｓ２を、第１円７４と第２円７６とが重なる領域Ｒ３（面積Ｓ３）を含めた、各領域Ｒ１～Ｒ３全体の面積（Ｓ１＋Ｓ２＋Ｓ３）で割ったもの等が考えられる。すなわち、誤差＝（Ｓ１＋Ｓ２）／（Ｓ１＋Ｓ２＋Ｓ３）として、該誤差が算出される。誤差が小さい場合、画像処理部２０での三次元点群６０の抽出結果、すなわち、中心位置７２の推定結果の信頼性が高いと評価することができる。

誤差が小さいと評価した場合（ステップＳ４４：ＹＥＳ）、信頼性評価部２２は、次のステップＳ４５に進む。ステップＳ４５において、信頼性評価部２２は、抽出した果菜１４の表面の三次元点群６０、すなわち、推定した中心位置７２の信頼性が高い（信頼性有り）と評価する。

一方、ステップＳ４１において、果菜１４の三次元点群６０の広がりが小さいと評価した場合（ステップＳ４１：ＮＯ）、又は、ステップＳ４４において、誤差が大きいと評価した場合（ステップＳ４４：ＮＯ）、信頼性評価部２２は、ステップＳ４６に進む。ステップＳ４６において、信頼性評価部２２は、ハンドカメラ５４が撮像した距離画像５６に示す果菜１４の方向では、該果菜１４を収穫することができないと判断する。そして、信頼性評価部２２は、果菜１４を別の視点から撮像するよう、画像取得部１８に指示する。これにより、視点が変更された距離画像５６を用いて、ステップＳ１１～Ｓ１３（図４参照）の処理が再度行われる。

＜２．６ 特徴部６８の位置の信頼性評価＞
次に、図４のステップＳ１５の詳細について、図８のフローチャートと、図１３の説明図とを参照しながら説明する。

先ず、図８のステップＳ５１において、信頼性評価部２２（図１参照）は、特徴部６８の画像領域から抽出した三次元点群７０の位置と球モデル６２の中心位置７２との距離が、球モデル６２の半径に近いか否かを評価する。前述のように、球モデル６２は、果菜１４を模擬した回転体モデルである。また、三次元点群７０は、果菜１４に連なる小果梗又は離層（特徴部６８の画像領域）を抽出したものである。そのため、図１３に示すように、三次元点群７０の位置と球モデル６２（果菜１４）の中心位置７２との距離が、球モデル６２の半径に近い程、画像処理部２０での三次元点群７０の位置に対する抽出結果の信頼性が高いと評価することができるためである。

該距離が球モデル６２の半径に近いと評価した場合（ステップＳ５１：ＹＥＳ）、信頼性評価部２２は、次のステップＳ５２に進む。ステップＳ５２において、信頼性評価部２２は、三次元点群７０が球モデル６２の表面（果菜１４の表面）に重なり合っていないか否かを評価する。特徴部６８である小果梗又は離層は、球モデル６２によって模擬される果菜１４の表面に重なり合わないためである。

三次元点群７０が球モデル６２の表面に重なり合っていない場合（ステップＳ５２：ＹＥＳ）、信頼性評価部２２は、次のステップＳ５３に進む。ステップＳ５３において、信頼性評価部２２は、三次元点群７０を構成する点の数が一定数以上であるか否かを評価する。一定数以上の点で該三次元点群７０が構成されていれば、画像処理部２０での特徴部６８の位置に対する抽出結果の信頼性が高いと評価することができるためである。

点の数が一定数以上である場合（ステップＳ５３：ＹＥＳ）、信頼性評価部２２は、次のステップＳ５４に進み、抽出された特徴部６８の位置の信頼性が高い（信頼性有り）と評価する。

一方、ステップＳ５１～Ｓ５３において、否定的な評価結果となった場合（ステップＳ５１～Ｓ５３：ＮＯ）、信頼性評価部２２は、ステップＳ５５に進む。ステップＳ５５において、信頼性評価部２２は、ハンドカメラ５４が撮像した距離画像５６に示す果菜１４の方向では、該果菜１４を収穫することができないと判断する。そして、信頼性評価部２２は、果菜１４を別の視点から撮像するよう、画像取得部１８に指示する。これにより、視点が変更された距離画像５６を用いて、ステップＳ１１～Ｓ１５（図４参照）の処理が再度行われる。

図１４～図１７は、信頼性有りの評価結果を受けた場合（図１４及び図１５参照）と、信頼性無しの評価結果を受けた場合（図１６及び図１７参照）とを図示している。

図１４は、収穫対象の果菜１４について、信頼性有りの評価結果を受けた、果菜１４の表面の三次元点群６０の抽出結果（果菜１４の中心位置７２の推定結果）を示す。また、図１５は、収穫対象の果菜１４に連なる小果梗又は離層の特徴部６８について、信頼性有りの評価結果を受けた、特徴部６８の三次元点群７０の抽出結果（特徴部６８の位置の推定結果）を示す。図１４及び図１５のいずれの場合も、三次元点群６０、７０の点の数が相対的に多い。また、果菜１４の表面の三次元点群６０については、球モデル６２の表面に対する三次元点群６０の広がりが相対的に大きい。従って、図１４及び図１５の場合については、信頼性有りの評価結果になったものと考えられる。

一方、図１６は、収穫対象の果菜１４について、信頼性無しの評価結果を受けた、果菜１４の表面の三次元点群６０の抽出結果（果菜１４の中心位置７２の推定結果）を示す。また、図１７は、収穫対象の果菜１４に連なる小果梗又は離層の特徴部６８について、信頼性無しの評価結果を受けた、特徴部６８の三次元点群７０の抽出結果（特徴部６８の位置の推定結果）を示す。図１６及び図１７のいずれの場合も、三次元点群６０、７０の点の数が相対的に少ない。また、果菜１４の表面の三次元点群６０については、球モデル６２の表面に対する三次元点群６０の広がりが相対的に小さい。従って、図１６及び図１７の場合については、信頼性無しの評価結果になったものと考えられる。

［３．変形例］
上記の説明では、ステップＳ１１の中心位置の推定処理を実行した後に、ステップＳ１２の特徴部の位置の抽出処理を実行する場合について説明した。本実施形態では、ステップＳ１２の抽出処理の後にステップＳ１１の推定処理を実行してもよいし、ステップＳ１１、Ｓ１２の各処理を同時並行で実行してもよい。

また、上記の説明では、ステップＳ１３での中心位置に対する評価処理を実行した後に、ステップＳ１５での特徴部の位置に対する評価処理を実行する場合について説明した。本実施形態では、ステップＳ１５の評価処理の後にステップＳ１３の評価処理を実行してもよいし、ステップＳ１３、Ｓ１５の各評価処理を同時並行で実行してもよい。

また、本実施形態では、果菜１４の形状を事前に測定し、果菜１４が他の物体（例えば、他の果菜）によって、どの程度隠れているかを判定し、その判定結果を果菜１４の三次元の位置及び姿勢の推定処理に反映させてもよい。

さらに、本実施形態では、トマト以外の果菜１４の収穫に適用可能であることは勿論である。

［４．本実施形態の効果］
以上説明したように、本実施形態に係る推定装置１０及び推定方法は、果菜１４の位置及び姿勢を推定する。

この場合、推定装置１０は、果菜１４の画像を取得する画像取得部１８と、該画像に基づいて果菜１４の中心位置７２を推定し、一方で、該画像に基づいて果菜１４の姿勢を決定付ける特徴部６８の位置を抽出する画像処理部２０（中心位置推定部、特徴部抽出部）と、推定された中心位置７２に対する信頼性を評価する一方で、抽出された特徴部６８の位置に対する信頼性を評価する信頼性評価部２２（推定結果評価部、抽出結果評価部）と、信頼性評価部２２で信頼性が有ると評価された場合に、中心位置７２及び特徴部６８の位置から果菜１４の中心軸を推定し、推定した中心軸から果菜１４の姿勢を推定する姿勢推定部２４とを有する。

推定方法は、画像取得部１８を用いて果菜１４の画像を取得するステップ（ステップＳ２１、Ｓ３１）と、画像処理部２０を用いて該画像に基づき果菜１４の中心位置７２を推定し、一方で、該画像に基づき果菜１４の姿勢を決定付ける特徴部６８の位置を抽出するステップ（ステップＳ１１、Ｓ１２）と、信頼性評価部２２を用いて、推定された中心位置７２に対する信頼性を評価し、一方で、抽出された特徴部６８の位置に対する信頼性を評価するステップ（ステップＳ１３、Ｓ１５）と、信頼性評価部２２で信頼性が有ると評価された場合に、姿勢推定部２４を用いて中心位置７２及び特徴部６８の位置から果菜１４の中心軸を推定し、推定した中心軸から果菜１４の姿勢を推定するステップ（ステップＳ１７）とを有する。

このように、推定した果菜１４の中心位置７２及び特徴部６８の位置に対する信頼性を評価するので、信頼性が低いと評価した場合には、果菜１４を異なる方向から撮像した画像を取得する等の対応を取ることができる。これにより、果菜１４の三次元の位置及び姿勢を正しく推定することが可能となる。この結果、ロボットハンド３４を用いて果菜１４を確実に収穫することができる。

ここで、画像取得部１８が果菜１４の距離画像５６を取得した場合、画像処理部２０は、距離画像５６から、特徴部６８の三次元点群７０を抽出する。信頼性評価部２２は、特徴部６８の位置と中心位置７２との距離が果菜１４を模擬した球モデル６２の半径に近く、特徴部６８の三次元点群７０が球モデル６２の表面になく、且つ、該三次元点群７０を構成する点の数が一定数以上である場合、抽出された特徴部６８の位置に信頼性があると評価する。このように、特徴部６８の位置が精度よく特定されるので、果菜１４の三次元の位置及び姿勢をより正確に推定することができる。

また、画像取得部１８が果菜１４の距離画像５６を取得した場合、画像処理部２０は、距離画像５６から果菜１４の表面の三次元点群６０を抽出し、抽出した該三次元点群６０を、果菜１４を模擬した球モデル６２にフィッティングすることにより、中心位置７２を推定する。また、信頼性評価部２２は、果菜１４の表面の三次元点群６０が球モデル６２の表面全体に広がり、画像取得部１８が果菜１４の二次元画像をさらに取得した際に、該二次元画像にフィッティングする第１円７４を設定する。さらに、信頼性評価部２２は、球モデル６２を二次元画像に投影した第２円７６を設定したときに、第１円７４と第２円７６との誤差が小さい場合、推定された中心位置７２に信頼性が有ると評価する。

このように、果菜１４を回転体モデルである球モデル６２とみなし、該球モデル６２の中心位置７２を果菜１４の中心位置とみなすことで、該中心位置を精度よく特定することができる。この結果、果菜１４の三次元の位置及び姿勢をより正確に推定することができる。

この場合、特徴部６８は、果菜１４に連なる小果梗又は離層であるので、果菜１４の中心軸を一層精度よく推定することが可能となる。

なお、本発明は、上述の実施形態に限らず、この明細書の記載内容に基づき、種々の構成を採り得ることは勿論である。

１０…推定装置（位置姿勢推定装置） １４…果菜
１８…画像取得部
２０…画像処理部（中心位置推定部、特徴部抽出部）
２２…信頼性評価部（推定結果評価部、抽出結果評価部）
２４…姿勢推定部 ５６…距離画像
６８…特徴部 ７２…中心位置

Claims (3)

1. 果菜の位置及び姿勢を推定する位置姿勢推定装置において、
   前記果菜の画像を取得する画像取得部と、
   前記画像に基づいて、前記果菜の中心位置を推定する中心位置推定部と、
   前記画像に基づいて、前記果菜の姿勢を決定付ける特徴部の位置を抽出する特徴部抽出部と、
   推定された前記中心位置に対する信頼性を評価する推定結果評価部と、
   抽出された前記特徴部の位置に対する信頼性を評価する抽出結果評価部と、
   前記推定結果評価部及び前記抽出結果評価部の双方で信頼性が有ると評価された場合に、前記中心位置及び前記特徴部の位置から前記果菜の中心軸を推定し、推定した前記中心軸から前記果菜の姿勢を推定する姿勢推定部と、
   を有し、
   前記画像取得部は、前記果菜の距離画像を取得し、
   前記特徴部抽出部は、前記距離画像から、前記特徴部の三次元点群を抽出し、
   前記抽出結果評価部は、前記特徴部の位置と前記中心位置との距離が前記果菜を模擬した球の半径に近く、前記特徴部の三次元点群が前記球の表面になく、且つ、該三次元点群を構成する点の数が一定数以上である場合、抽出された前記特徴部の位置に信頼性があると評価し、
   前記中心位置推定部は、前記距離画像から前記果菜の表面の三次元点群を抽出し、抽出した該三次元点群を、前記果菜を模擬した球にフィッティングすることにより、前記中心位置を推定し、
   前記推定結果評価部は、前記果菜の表面の三次元点群が前記球の表面全体に広がり、前記画像取得部が前記果菜の二次元画像をさらに取得した際に、該二次元画像にフィッティングする第１円を設定し、前記球を前記二次元画像に投影した第２円を設定したときに、前記第１円と第２円との誤差が小さい場合、推定された前記中心位置に信頼性が有ると評価する、位置姿勢推定装置。
2. 請求項１記載の位置姿勢推定装置において、
   前記特徴部は、前記果菜に連なる小果梗又は離層である、位置姿勢推定装置。
3. 果菜の位置及び姿勢を推定する位置姿勢推定方法において、
   画像取得部を用いて、前記果菜の画像を取得するステップと、
   中心位置推定部を用いて、前記画像に基づき前記果菜の中心位置を推定し、一方で、特徴部抽出部を用いて、前記画像に基づき前記果菜の姿勢を決定付ける特徴部の位置を抽出するステップと、
   推定結果評価部を用いて、推定された前記中心位置に対する信頼性を評価し、一方で、抽出結果評価部を用いて、抽出された前記特徴部の位置に対する信頼性を評価するステップと、
   前記推定結果評価部及び前記抽出結果評価部の双方で信頼性が有ると評価された場合に、姿勢推定部を用いて、前記中心位置及び前記特徴部の位置から前記果菜の中心軸を推定し、推定した前記中心軸から前記果菜の姿勢を推定するステップと、
   を有し、
   し、
   前記画像取得部は、前記果菜の距離画像を取得し、
   前記特徴部抽出部は、前記距離画像から、前記特徴部の三次元点群を抽出し、
   前記抽出結果評価部は、前記特徴部の位置と前記中心位置との距離が前記果菜を模擬した球の半径に近く、前記特徴部の三次元点群が前記球の表面になく、且つ、該三次元点群を構成する点の数が一定数以上である場合、抽出された前記特徴部の位置に信頼性があると評価し、
   前記中心位置推定部は、前記距離画像から前記果菜の表面の三次元点群を抽出し、抽出した該三次元点群を、前記果菜を模擬した球にフィッティングすることにより、前記中心位置を推定し、
   前記推定結果評価部は、前記果菜の表面の三次元点群が前記球の表面全体に広がり、前記画像取得部が前記果菜の二次元画像をさらに取得した際に、該二次元画像にフィッティングする第１円を設定し、前記球を前記二次元画像に投影した第２円を設定したときに、前記第１円と第２円との誤差が小さい場合、推定された前記中心位置に信頼性が有ると評価する、位置姿勢推定方法。

Images