JP2017222004A - 干渉防止装置 - Google Patents

Abstract

【課題】マニピュレータの可動部の位置の検出精度を向上させる。【解決手段】干渉防止装置１は、撮像範囲Ｒに可動部（エンドエフェクタ３４）が含まれる深度センサ１０によって、可動部が可動範囲を動いているときに撮像された深度画像が、時系列に並べられた画像データを、可動部が可動範囲を動いているときに取得する取得部（深度センサ１０）と、可動部が可動範囲を動いているときに、パーティクルフィルタを用いて、その画像データを処理することにより、可動部の位置を検出する検出部１１２と、を備え、検出部１１２は、パーティクルフィルタを用いる処理に含まれる尤度の算出において、可動部の速度をパラメータとして、尤度を算出する。【選択図】図１

Description

本発明は、マニピュレータに適用される干渉防止装置に関する。

干渉防止装置は、様々な機械に適用可能であり、建設機械に適用される場合、例えば、バケットのようなアタッチメントが運転室に干渉（言い換えれば、接触）することを防止する装置である。このような干渉防止装置として、例えば、特許文献１は、アタッチメントの位置を検出する検出手段と、予め干渉防止範囲を設定し、アタッチメントが干渉防止範囲内に進入したときにアタッチメントの駆動を減速又は停止する制御手段と、を備える建設機械の干渉防止装置を開示している。

特開２０１４−１６３１５６号公報

マニピュレータの可動部（例えば、エンドエフェクタ、可動アーム）や、可動部が保持している作業対象物が、マニピュレータと干渉することを防止するために、マニピュレータに干渉防止装置が適用されることがある。可動部が作業対象物を保持するとは、例えば、エンドエフェクタが作業対象物を掴んだり、吸着したりすることである。

可動部は、マニピュレータの用途に応じて換装されることがある。可動部が換装された場合、換装前と換装後とで、可動部の形状や可動範囲が異なる。特許文献１に開示された技術のように、干渉防止範囲を設定する場合、可動部が換装される毎に、干渉防止範囲を設定しなければならず、煩わしい。

そこで、深度センサを用いる方法が考えられる。深度センサによって撮像された深度画像の各画素の画素値は、各画素に対応する箇所の深度（すなわち、深度センサとその箇所との距離）を示す。従って、深度センサによれば、可動部の位置を検出できることに加えて、可動部までの距離を測定できるので、可動部が換装される毎に、干渉防止範囲を設定する必要がない。なお、可動部が作業対象物を保持している場合、作業対象物と可動部とは一緒に動くので、深度センサは、作業対象物と可動部とを区別せずに、これらを一つの可動部として、この可動部の位置を検出する。

しかし、本発明者は、深度センサを用いた可動部の位置の検出は、可動部と、可動部以外の動く物体（例えば、虫、雪、雨）とを区別できず、この結果、可動部が動いている状態で、可動部以外の動く物体が存在したとき、深度センサは、可動部の位置を検出せずに、可動部以外の動く物体の位置を検出してしまう可能性があることを見出した。なお、可動部が作業対象物を保持している場合、可動部及び作業対象物が動いている状態で、これら以外の動く物体が存在するとき、深度センサは、これら以外の動く物体の位置を検出してしまう可能性がある。マニピュレータの可動部の位置の検出精度を向上できる技術が望まれる。

本発明は、マニピュレータの可動部の位置の検出精度を向上させることができる干渉防止装置を提供することを目的とする。

本発明に係る干渉防止装置は、所定の可動範囲内で可動する可動部を備えるマニピュレータに適用され、前記可動範囲内の対象物体と前記可動部との干渉を防止する干渉防止装置であって、撮像範囲に前記可動部が含まれる深度センサによって、前記可動部が前記可動範囲を動いているときに撮像された深度画像が、時系列に並べられた画像データを、前記可動部が前記可動範囲を動いているときに取得する取得部と、前記可動部が前記可動範囲を動いているときに、パーティクルフィルタを用いて前記画像データを処理することにより、前記可動部の位置を検出する検出部と、を備え、前記検出部は、前記パーティクルフィルタを用いる処理に含まれる尤度の算出において、前記可動部の速度をパラメータとして、前記尤度を算出する。

本発明に係る干渉防止装置は、マニピュレータの可動部とこの可動部の可動範囲内の対象物体とが干渉（すなわち、接触）することを防止する装置である。本発明に係る干渉防止装置によれば、パーティクルフィルタを利用した可動部の位置検出において、尤度の計算のパラメータとして、可動部の速度を用いる。このため、可動部が可動範囲を動いている状態で、深度センサの撮像範囲に、可動部以外の動く物体が存在するとき、検出部は、可動部以外の動く物体を位置検出の対象から排除することができる。なお、可動部が作業対象物を保持している場合、可動部及び作業対象物が動いている状態で、深度センサの撮像範囲に、これら以外の動く物体が存在するとき、検出部は、これら以外の動く物体を位置検出の対象から排除することができる。従って、本発明に係る干渉防止装置によれば、マニピュレータの可動部の位置の検出精度を向上させることができる。

対象物体とは、マニピュレータの可動部以外の構成要素（例えば、マニピュレータの固定アーム）でもよいし、このマニピュレータ以外の物体（例えば、他のマニピュレータ）でもよい。干渉防止装置が、深度センサを構成要件として備える場合、深度センサが取得部となる。干渉防止装置が、深度センサを構成要件として備えない場合、深度センサから出力された上記画像データ（深度画像が時系列に並べられた画像データ）が入力する入力部（入力インターフェース）が取得部となる。

上記構成において、前記可動部の速度を測定する測定部をさらに備え、前記検出部は、前記測定部によって測定された前記可動部の速度をパラメータとして前記尤度を算出する。

可動部の速度が予め決められた一定速度の場合、この一定速度を示す速度情報が検出部に予め設定される。しかし、可動部の速度が一定でない場合、可動部の速度を測定する必要がある。そこで、この構成は、可動部の速度を測定する測定部を備える。

上記構成において、前記マニピュレータは、前記可動部を回動させるアクチュエータと、前記アクチュエータの回転角を検出するセンサと、を備えており、前記測定部は、前記センサによって検出された前記回転角を用いて、前記可動部の速度を算出する。

マニピュレータは、可動部を回動させるアクチュエータの制御のために、アクチュエータの回転角を検出するセンサを備える。この構成は、センサが検出したアクチュエータの回転角を用いて、可動部の速度を算出する。従って、可動部の速度を算出するためのセンサを新たに設ける必要がなくなる。

本発明によれば、マニピュレータの可動部の位置の検出精度を向上させることができる。

実施形態に係る干渉防止装置を備えるマニピュレータの模式図である。 実施形態に係る干渉防止装置の構成を示すブロック図である。 実施形態に係る干渉防止装置の動作を説明するフローチャートである。 エンドエフェクタの位置の検出を説明するフローチャートである。 パーティクルフィルタの初期設定がされた１番目のフレームを説明する説明図である。 Σｆ_ｋ（ｘ）と各パーティクルの尤度との関係を説明する説明図である。 リサンプリング後のフレームを説明する説明図である。 図４のステップＳ１２〜ステップＳ１６の処理が繰り返された後のフレームを説明する説明図である。

以下、図面に基づいて本発明の実施形態を詳細に説明する。各図において、同一符号を付した構成は、同一の構成であることを示し、その構成について、既に説明している内容については、その説明を省略する。本明細書において、総称する場合には添え字を省略した参照符号で示し、個別の構成を指す場合には添え字を付した参照符号で示す。

図１は、実施形態に係る干渉防止装置を備えるマニピュレータ３の模式図である。マニピュレータ３が、ロボットアームを例にして説明する。マニピュレータ３は、ベース３０、固定アーム３１、可動アーム３２、関節３３、エンドエフェクタ３４、アクチュエータ３５、回転角センサ３６及び制御装置３７を備える。

ベース３０上には、固定アーム３１（対象物体の一例）の一端が固定されている。固定アーム３１は、上方に延びている。固定アーム３１の他端と可動アーム３２−１の一端とは、関節３３−１によって連結されている。関節３３−１は、アクチュエータ３５−１（例えば、サーボモータ）が回転することにより生じる回転駆動力によって回動する。アクチュエータ３５−１の回転角が、回転角センサ３６−１によって検出される。

可動アーム３２−１の他端と可動アーム３２−２の一端とは、関節３３−２によって連結されている。関節３３−２は、アクチュエータ３５−２（例えば、サーボモータ）が回転することにより生じる回転駆動力によって回動する。アクチュエータ３５−２の回転角が、回転角センサ３６−２によって検出される。

可動アーム３２−２の他端には、エンドエフェクタ３４が連結されている。エンドエフェクタ３４は、作業対象物を掴む機能を有する。エンドエフェクタ３４は、この機能を実行するためのアクチュエータ（不図示）及びセンサ（不図示）を備える。

エンドエフェクタ３４は、可動アーム３２−２の他端と連結されているので、可動アーム３２−２が回動するとき、可動アーム３２−２と一体となって回動する。エンドエフェクタ３４の回動範囲に、固定アーム３１の位置が含まれる。エンドエフェクタ３４は、可動部の一例であり、可動部の可動範囲に、固定アーム３１（対象物体の一例）の位置が含まれる。可動は、回動に限らず、移動でもよい。なお、エンドエフェクタ３４が作業対象物を掴んでいる場合（言い換えれば、保持している場合）、エンドエフェクタ３４及び作業対象物が、可動部とみなされる。可動部は、エンドエフェクタ３４に限らず、固定アーム３１と干渉する可能性を有する要素であればよい（例えば、可動アーム３２ー２）。

制御装置３７は、ベース３０内に設置されている。制御装置３７は、可動アーム３２及びエンドエフェクタ３４を動作させる制御をする。例えば、可動アーム３２−２で説明すると、制御装置３７は、回転角センサ３６−２で検出された角度を基にして、アクチュエータ３５−２を作動させて、関節３３−２を回動させることにより、可動アーム３２−２を回動させる。

実施形態に係る干渉防止装置を説明する。図２は、実施形態に係る干渉防止装置１の構成を示すブロック図である。図１及び図２を参照して、干渉防止装置１は、深度センサ１０と干渉防止装置本体１１とを備える。

深度センサ１０は、赤外線を利用して深度画像を撮像する。深度画像は、二次元画像であり、深度画像を構成する各画素の画素値は、各画素に対応する箇所の深度（すなわち、深度センサ１０とその箇所との距離）を示す。

深度センサ１０の撮像範囲Ｒは、固定アーム３１の前方である。従って、エンドエフェクタ３４が固定アーム３１の前方に現れたとき、エンドエフェクタ３４は、撮像範囲Ｒに入る。

干渉防止装置本体１１は、ベース３０内に設置されている。干渉防止装置本体１１は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、及び、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）等のハードウェア、並びに、干渉防止装置１の動作に必要なソフトウェア及びプログラムによって実現されるコンピュータである。干渉防止装置本体１１及び制御装置３７がベース３０内に設置されている例で説明しているが、これらがベース３０外に設定されていてもよい。

干渉防止装置本体１１は、制御装置３７と通信可能であり、機能ブロックとして、深度センサ制御部１１０、測定部１１１、検出部１１２及び算出部１１３を備える。

深度センサ制御部１１０は、深度センサ１０を制御して、深度センサ１０に動画を撮像させる。この動画を示す動画データは、深度画像が時系列に並べられた画像データの一例である。深度センサ１０は、取得部として機能する。取得部は、撮像範囲Ｒにエンドエフェクタ３４（可動部の一例）が含まれる深度センサ１０によって、エンドエフェクタ３４が回動範囲を動いているときに撮像された深度画像の動画データを、エンドエフェクタ３４が回動範囲を動いているときに取得する。なお、干渉防止装置１が、深度センサ１０を構成要件として備えない場合、深度センサ１０から出力された上記動画データが入力する入力部（入力インターフェース）が取得部となる。入力部は、干渉防止装置本体１１に備えられる。

測定部１１１は、エンドエフェクタ３４の速度を測定する。詳しく説明すると、測定部１１１は、回転角センサ３６によって検出された、アクチュエータ３５の回転角を用いて、エンドエフェクタ３４の速度を算出する。可動アーム３２−１が回動していない状態で、可動アーム３２−２が回動している場合を例にすると、エンドエフェクタ３４の速度は、下記式１を用いて算出することができる。

ｖ＝ｒω・・・式１

ｖは、エンドエフェクタ３４の速度である。ｒは、可動アーム３２−２の長さである。ωは、アクチュエータ３５−２の角速度である。この角速度は、回転角センサ３６−２によって検出された、アクチュエータ３５−２の回転角を用いて算出される。

実施形態に係る干渉防止装置１によれば、回転角センサ３６が検出したアクチュエータ３５の回転角を用いて、エンドエフェクタ３４の速度を算出する。従って、エンドエフェクタ３４の速度を算出するためのセンサを新たに設ける必要がなくなる。

検出部１１２は、エンドエフェクタ３４が回動しているときに、深度センサ１０が撮像した動画を構成する各フレーム（各深度画像）に対して、パーティクルフィルタ処理をすることにより、エンドエフェクタ３４の位置を検出する。検出部１１２は、エンドエフェクタ３４の位置検出を繰り返すことにより、エンドエフェクタ３４の位置を追跡する。

算出部１１３は、検出部１１２によって、エンドエフェクタ３４の位置が検出されたとき、エンドエフェクタ３４と深度センサ１０との距離を、深度画像を用いて算出する。この深度画像は、動画データを構成するフレームの中で、検出部１１２によってエンドエフェクタ３４の位置が検出されたときのフレーム（深度画像）である。このフレームに含まれるエンドエフェクタ像７（図８）を構成する画素の画素値は、深度センサ１０とエンドエフェクタ３４との距離を示す。深度センサ１０は、固定アーム３１に設置されているので、深度センサ１０とエンドエフェクタ３４との距離は、固定アーム３１とエンドエフェクタ３４との距離と見なすことができる。

可動アーム３２−１の回動が停止している状態で、可動アーム３２−２が回動する場合を例にして、実施形態に係る干渉防止装置１の動作を説明する。図３は、この動作を説明するフローチャートである。

図１及び図３を参照して、可動アーム３２−２が回動することにより、エンドエフェクタ３４（可動部の一例）が可動アーム３２−２と一緒に動く。可動アーム３２−２の回動が正方向のとき、エンドエフェクタ３４が固定アーム３１（対象物体の一例）から遠ざかり、可動アーム３２−２の回動が負方向のとき、エンドエフェクタ３４が固定アーム３１に近づくとする。検出部１１２は、可動アーム３２−２の回動が負方向か否かを判断する（ステップＳ１）。検出部１１２は、回転角センサ３６−２によって検出された回転角の変化に基づいて、その判断をする。

検出部１１２が、可動アーム３２−２の回動が負方向でないと判断したとき（ステップＳ１でＮｏ）、すなわち、可動アーム３２−２の回動が正方向、又は、可動アーム３２−２の回動が停止していると判断したとき、検出部１１２は、ステップＳ１の処理を繰り返す。

検出部１１２が、可動アーム３２−２の回動が負方向であると判断したとき（ステップＳ１でＹｅｓ）、深度センサ制御部１１０は、深度センサ１０に、動画の撮像を開始させる（ステップＳ２）。

検出部１１２は、深度センサ１０が撮像した動画の動画データを用いて、エンドエフェクタ３４の位置を検出する（ステップＳ３）。これについては、後で詳細に説明する。

検出部１１２は、エンドエフェクタ３４と固定アーム３１との距離が、予め定められた第１のしきい値より小さいか否かを判断する（ステップＳ４）。この距離が、第１のしきい値より小さいとき、エンドエフェクタ３４と固定アーム３１とが近い状態であり、エンドエフェクタ３４が固定アーム３１に干渉する危険性がある。この距離は、上述したように、深度画像を用いて算出される。

検出部１１２が、エンドエフェクタ３４と固定アーム３１との距離が、第１のしきい値以上と判断したとき（ステップＳ４でＮｏ）、検出部１１２は、エンドエフェクタ３４の位置検出を継続する（ステップＳ３）。すなわち、検出部１１２は、エンドエフェクタ３４の位置を追跡する。

検出部１１２が、エンドエフェクタ３４と固定アーム３１との距離が、第１のしきい値より小さいと判断したとき（ステップＳ４でＹｅｓ）、検出部１１２は、制御装置３７に対して、可動アーム３２−２の回動の停止命令をする（ステップＳ５）。すなわち、検出部１１２は、可動アーム３２−２の回動を停止する命令を示す信号を、制御装置３７に送信する。制御装置３７は、その信号を受信したとき、アクチュエータ３５−２を停止させることにより、可動アーム３２−２の回動を停止させる。これにより、エンドエフェクタ３４が固定アーム３１に干渉することを防止する。

なお、ステップＳ５において、停止命令の替わりに、減速命令でもよい。この場合、検出部１１２は、可動アーム３２−２の回動速度を減速する命令を示す信号を、制御装置３７に送信する。制御装置３７は、その信号を受信したとき、アクチュエータ３５−２の動作速度を減速させることにより、可動アーム３２−２の回動速度を減速させる。マニピュレータ３の操作者は、エンドエフェクタ３４が固定アーム３１に干渉するおそれがあると判断したとき、マニピュレータ３の動作を停止させる。

停止命令、減速命令の替わりに、干渉防止装置本体１１が、報知部（不図示）を作動させてもよい。検出部１１２が、エンドエフェクタ３４と固定アーム３１との距離が、第１のしきい値より小さいと判断したとき（ステップＳ４でＹｅｓ）、報知部は、例えば、ランプ点滅、ブザーを鳴らす動作をする。これにより、マニピュレータ３の操作者に警告を報知する。操作者は、エンドエフェクタ３４が固定アーム３１に干渉するおそれがあると判断したとき、マニピュレータ３の動作を停止させる。

エンドエフェクタ３４の位置の検出（ステップＳ３）について詳しく説明する。実施形態に係る干渉防止装置１は、エンドエフェクタ３４の位置の検出にパーティクルフィルタを用いる。図４は、エンドエフェクタ３４の位置の検出を説明するフローチャートである。

図１、図２及び図４を参照して、検出部１１２は、図３のステップＳ２で撮像された動画の１番目のフレームに対して初期設定をする（ステップＳ１１）。フレームは、エンドエフェクタ３４を含む撮像範囲Ｒの深度画像である。図５は、初期設定された１番目のフレーム５ａを説明する説明図である。１番目のフレーム５ａは、エンドエフェクタ像７を含む。検出部１１２は、１番目のフレーム５ａ上にｎ個（例えば、１０００個）のパーティクル６をランダムに配置する。一つのパーティクル６は、一つの画素上に配置される。

検出部１１２は、仮定の予測モデルを用いてフレーム上に配置されたｎ個のパーティクル６を遷移させる（ステップＳ１２）。すなわち、検出部１１２は、ｎ個のパーティクル６のそれぞれを次のフレーム上の予測される位置に配置する。ステップＳ１２の処理により、エンドエフェクタ３４の検出精度を向上させることができる。エンドエフェクタ３４の速度が遅いとき、この処理はしなくてもよい。

ここでの予測モデルは、撮像範囲Ｒの中で、各パーティクル６が配置された画素に対応する箇所の動作モデル（その箇所の移動速度）である。フレームは、深度画像であるので、深度画像を構成する各画素の画素値は、画素に対応する箇所から深度センサ１０までの距離を示す。画素に対応する箇所の移動速度は、フレーム間差分によって移動距離を求め、移動距離とフレームレートとを用いて算出することができる。

深度センサ１０が動画の撮像が開始してからある程度の時間が経過しなければ、動作モデルは確定できない。検出部１１２は、１番目のフレームから所定数のフレームに対しては、ステップＳ１２の処理をしない。

検出部１１２は、パーティクル６の尤度を算出する（ステップＳ１３）。尤度の算出には、以下のパラメータ１〜パラメータ３が用いられる。

パラメータ１：撮像範囲Ｒの中で、パーティクル６が配置された画素に対応する箇所の移動速度
パラメータ２：エンドエフェクタ３４の速度
パラメータ３：エンドエフェクタ３４の速度の分散σ

ｉ番目のフレームを例にして説明すると、パラメータ１（移動速度）は、ｉ番目のフレームとｉ−１番目のフレームとのフレーム間差分を用いて、検出部１１２によって算出される。パラメータ１は、ｎ個のパーティクル６のそれぞれについて、算出される。パラメータ２（エンドエフェクタ３４の速度）は、上述したように、測定部１１１によって算出される。ｉ番目のフレームに配置されたパーティクル６の場合、ｉ番目のフレームが撮像されたときのエンドエフェクタ３４の速度である。パラメータ３（分散σ）は、干渉防止装置１の設計者等によって、検出部１１２に予め設定されている。

例えば、尤度が正規分布していると仮定すれば、以下の式２を用いて尤度は、算出される。

ｘは、パラメータ１である。μは、パラメータ２である。σは、パラメータ３である。ｋは、１〜ｎまでの整数である。ｆ_ｋ（ｘ）は、第ｋパーティクル６の尤度である。

ｎ個のパーティクル６の中で、第１パーティクル６の尤度ｆ_１（ｘ）は、第１パーティクル６が配置された画素に対応する箇所の移動速度（パラメータ１）、パラメータ２及びパラメータ３が、式２に代入されることにより算出される。以下同様に、第２パーティクル６の尤度ｆ_２（ｘ）は、第２パーティクル６が配置された画素に対応する箇所の移動速度（パラメータ１）、パラメータ２及びパラメータ３が、式２に代入されることにより算出され、・・・、第ｎパーティクル６の尤度ｆ_ｎ（ｘ）は、第ｎパーティクル６が配置された画素に対応する箇所の移動速度（パラメータ１）、パラメータ２及びパラメータ３が、式２に代入されることにより算出される。

検出部１１２は、ｎ個のパーティクル６のそれぞれについて、以下の式３を用いて、重みを算出する（ステップＳ１４）。

Ｗ_ｋは、第ｋパーティクル６の重みである。ｆ_ｋ（ｘ）は、第ｋパーティクル６の尤度である。Σｆ_ｋ（ｘ）は、第１パーティクル６の尤度ｆ_１（ｘ）〜第ｎパーティクル６の尤度ｆ_ｎ（ｘ）を加算した値である。

図６は、Σｆ_ｋ（ｘ）と各パーティクル６の尤度との関係を説明する説明図である。Σｆ_ｋ（ｘ）は、ｎ個のボックス[１]〜ボックス[ｎ]により構成されているとする。ボックスの長さが大きいほど、尤度（重み）が大きく、ボックスの長さが小さいほど、尤度（重み）が小さいことを示す。ボックス[１]は、第１パーティクル６の尤度ｆ_１（ｘ）、ボックス[２]は、第２パーティクル６の尤度ｆ_２（ｘ）、ボックス[３]は、第３パーティクル６の尤度ｆ_３（ｘ）、ボックス[４]は、第４パーティクル６の尤度ｆ_４（ｘ）、・・・、ボックス[ｎ]は、第ｎパーティクル６の尤度ｆ_ｎ（ｘ）を示す。

式３から分かるように、尤度が大きくなるに従って、重みが大きくなり、尤度が小さくなるにしたがって、重みが小さくなる。

図２及び図４を参照して、検出部１１２は、ｎ個のパーティクル６のそれぞれについて、リサンプリングをする（ステップＳ１５）。第１パーティクル６を例にして説明すると、検出部１１２は、第１パーティクル６が配置される位置として、第１パーティクル６〜第ｎパーティクル６のいずれかの位置を決定する。この確率は、各位置で同じでない。ステップＳ１４で算出された重みが大きいパーティクル６（言い換えれば、尤度が大きいパーティクル６）の位置に決定される確率が高く、重みが小さいパーティクル６（言い換えれば、尤度が小さいパーティクル６）の位置に決定される確率が小さい。

第１パーティクル６が配置される位置が、第１パーティクル６〜第ｎパーティクル６のいずれかの位置に限定されないように、検出部１１２は、決定した位置からずれた位置に、第１パーティクル６を配置する（ランダムノイズ）。例えば、第１パーティクル６が第３パーティクル６の位置に配置する決定をした場合、第３パーティクル６の位置から多少ずれた位置に第１パーティクル６を配置する。ランダムノイズは、干渉防止装置１の設計者等が検出部１１２に予め設定する。

検出部１１２は、第２パーティクル６から第ｎパーティクル６についても同様にして、配置する。図７は、リサンプリング後のフレーム５ｂを説明する説明図である。フレーム５ｂは、エンドエフェクタ像７を含む。リサンプリング前のフレームが１番目のフレームの場合、２番目のフレームがリサンプリング後のフレーム５ｂとなる。図５に比べて、パーティクル６がエンドエフェクタ像７の近くに集まっている。

検出部１１２は、リサンプリング後のフレームについて、予め定められた範囲に予め定められた数のパーティクル６が存在するか否かを判断する（ステップＳ１６）。予め定められた範囲に予め定められた数のパーティクル６が存在するとき、その範囲がエンドエフェクタ３４の位置として確定される。検出部１１２は、例えば、重みが最も大きいパーティクル６の位置を中心にして、予め定められた範囲に予め定められた数のパーティクル６が存在するか否かを判断する。

検出部１１２が、予め定められた範囲に予め定められた数のパーティクル６が存在しないと判断したとき（ステップＳ１６でＮｏ）、検出部１１２は、ステップＳ１２の処理に戻る。検出部１１２が、ステップＳ１２〜ステップＳ１６の処理を繰り返すと、図８に示すように、エンドエフェクタ像７に集まるパーティクル６の数が増加する。図８は、ステップＳ１２〜ステップＳ１６の処理が繰り返された後のフレーム５ｃを示す。

検出部１１２が、予め定められた範囲に予め定められた数のパーティクル６が存在すると判断したとき（ステップＳ１６でＹｅｓ）、検出部１１２は、その範囲がエンドエフェクタ３４の位置として確定する。

検出部１１２は、ステップＳ１６でＹｅｓと判断したとき、予め定められた範囲に存在するそれぞれのパーティクル６について、パーティクル６に対応する箇所の深度を算出する。検出部１１２は、例えば、算出した深度の平均値である平均深度を算出する。

検出部１１２は、平均深度が予め定められた第２のしきい値より小さいか否かを判断する（ステップＳ１７）。平均深度が、第２のしきい値より小さいとき、エンドエフェクタ３４と固定アーム３１とが近い状態であり、エンドエフェクタ３４が固定アーム３１に干渉する危険性があるとする。なお、検出部１１２は、上記予め定められた範囲に存在するそれぞれのパーティクル６に対応する箇所の深度のうち、最小値を、平均深度の替わりに用いてもよい。

検出部１１２は、平均深度が第２のしきい値以上と判断したとき（ステップＳ１７でＮｏ）、図３のステップＳ４において、エンドエフェクタ３４と固定アーム３１との距離が第１のしきい値以上と決定する（ステップＳ１８）。そして、検出部１１２は、ステップＳ１２の処理に戻る（言い換えれば、図３のステップＳ３の処理をする）。

検出部１１２は、平均深度が第２のしきい値より小さいと判断したとき（ステップＳ１７でＹｅｓ）、図３のステップＳ４において、エンドエフェクタ３４と固定アーム３１との距離が第１のしきい値より小さいと決定する（ステップＳ１９）。そして、検出部１１２は、図３のステップＳ５の処理をする。

実施形態の主な効果を説明する。実施形態に係る干渉防止装置１によれば、パーティクルフィルタを利用したエンドエフェクタ３４（可動部の一例）の位置検出において、尤度の計算のパラメータとして、エンドエフェクタ３４の速度を用いる（図４のステップＳ１３）。このため、エンドエフェクタ３４が回動範囲を動いている状態で、深度センサ１０の撮像範囲Ｒに、エンドエフェクタ３４以外の動く物体が存在するとき、検出部１１２は、エンドエフェクタ３４以外の動く物体を位置検出の対象から排除することができる。なお、エンドエフェクタ３４が作業対象物を保持している場合、エンドエフェクタ３４及び作業対象物が動いている状態で、深度センサ１０の撮像範囲Ｒに、これら以外の動く物体が存在するとき、検出部１１２は、これら以外の動く物体を位置検出の対象から排除することができる。従って、実施形態に係る干渉防止装置１によれば、エンドエフェクタ３４の位置の検出精度を向上させることができる。

干渉防止装置１は、エンドエフェクタ３４と固定アーム３１（対象物体の一例）とが干渉することを防止するために、エンドエフェクタ３４と固定アーム３１との距離を測定する必要がある。固定アーム３１に深度センサ１０が設置されているので、エンドエフェクタ３４と深度センサ１０との距離が分かれば、エンドエフェクタ３４と固定アーム３１との距離が分かる。実施形態によれば、エンドエフェクタ３４と深度センサ１０との距離を、深度画像を用いて算出するので（図３のステップＳ４）、エンドエフェクタ３４と固定アーム３１との距離を測定するセンサを新たに設ける必要がなくなる。

１ 干渉防止装置
３ マニピュレータ
５ａ，５ｂ，５ｃ フレーム（深度画像の一例）
６ パーティクル
７ エンドエフェクタ像
１０ 深度センサ
１１ 干渉防止装置本体
３１ 固定アーム（対象物体の一例）
３２−１，３２−２ 可動アーム
３３−１，３３−２ 関節
３４ エンドエフェクタ（可動部の一例）
３５−１，３５−２ アクチュエータ
３６−１，３６−２ 回転角センサ
Ｒ 撮像範囲

Claims (3)

1. 所定の可動範囲内で可動する可動部を備えるマニピュレータに適用され、前記可動範囲内の対象物体と前記可動部との干渉を防止する干渉防止装置であって、
   撮像範囲に前記可動部が含まれる深度センサによって、前記可動部が前記可動範囲を動いているときに撮像された深度画像が、時系列に並べられた画像データを、前記可動部が前記可動範囲を動いているときに取得する取得部と、
   前記可動部が前記可動範囲を動いているときに、パーティクルフィルタを用いて前記画像データを処理することにより、前記可動部の位置を検出する検出部と、を備え、
   前記検出部は、前記パーティクルフィルタを用いる処理に含まれる尤度の算出において、前記可動部の速度をパラメータとして、前記尤度を算出する干渉防止装置。
2. 前記可動部の速度を測定する測定部をさらに備え、
   前記検出部は、前記測定部によって測定された前記可動部の速度をパラメータとして前記尤度を算出する請求項１に記載の干渉防止装置。
3. 前記マニピュレータは、前記可動部を回動させるアクチュエータと、前記アクチュエータの回転角を検出するセンサと、を備えており、
   前記測定部は、前記センサによって検出された前記回転角を用いて、前記可動部の速度を算出する請求項２に記載の干渉防止装置。

Images