JP2017222004A - 干渉防止装置 - Google Patents

干渉防止装置 Download PDF

Info

Publication number
JP2017222004A
JP2017222004A JP2016119512A JP2016119512A JP2017222004A JP 2017222004 A JP2017222004 A JP 2017222004A JP 2016119512 A JP2016119512 A JP 2016119512A JP 2016119512 A JP2016119512 A JP 2016119512A JP 2017222004 A JP2017222004 A JP 2017222004A
Authority
JP
Japan
Prior art keywords
movable
movable part
end effector
unit
detection unit
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
JP2016119512A
Other languages
English (en)
Inventor
聖士 濱口
Seiji Hamaguchi
聖士 濱口
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Kobe Steel Ltd
Original Assignee
Kobe Steel Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Kobe Steel Ltd filed Critical Kobe Steel Ltd
Priority to JP2016119512A priority Critical patent/JP2017222004A/ja
Publication of JP2017222004A publication Critical patent/JP2017222004A/ja
Pending legal-status Critical Current

Links

Images

Landscapes

  • Operation Control Of Excavators (AREA)
  • Component Parts Of Construction Machinery (AREA)
  • Manipulator (AREA)
  • Image Analysis (AREA)

Abstract

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

Description

本発明は、マニピュレータに適用される干渉防止装置に関する。
干渉防止装置は、様々な機械に適用可能であり、建設機械に適用される場合、例えば、バケットのようなアタッチメントが運転室に干渉(言い換えれば、接触)することを防止する装置である。このような干渉防止装置として、例えば、特許文献1は、アタッチメントの位置を検出する検出手段と、予め干渉防止範囲を設定し、アタッチメントが干渉防止範囲内に進入したときにアタッチメントの駆動を減速又は停止する制御手段と、を備える建設機械の干渉防止装置を開示している。
特開2014−163156号公報
マニピュレータの可動部(例えば、エンドエフェクタ、可動アーム)や、可動部が保持している作業対象物が、マニピュレータと干渉することを防止するために、マニピュレータに干渉防止装置が適用されることがある。可動部が作業対象物を保持するとは、例えば、エンドエフェクタが作業対象物を掴んだり、吸着したりすることである。
可動部は、マニピュレータの用途に応じて換装されることがある。可動部が換装された場合、換装前と換装後とで、可動部の形状や可動範囲が異なる。特許文献1に開示された技術のように、干渉防止範囲を設定する場合、可動部が換装される毎に、干渉防止範囲を設定しなければならず、煩わしい。
そこで、深度センサを用いる方法が考えられる。深度センサによって撮像された深度画像の各画素の画素値は、各画素に対応する箇所の深度(すなわち、深度センサとその箇所との距離)を示す。従って、深度センサによれば、可動部の位置を検出できることに加えて、可動部までの距離を測定できるので、可動部が換装される毎に、干渉防止範囲を設定する必要がない。なお、可動部が作業対象物を保持している場合、作業対象物と可動部とは一緒に動くので、深度センサは、作業対象物と可動部とを区別せずに、これらを一つの可動部として、この可動部の位置を検出する。
しかし、本発明者は、深度センサを用いた可動部の位置の検出は、可動部と、可動部以外の動く物体(例えば、虫、雪、雨)とを区別できず、この結果、可動部が動いている状態で、可動部以外の動く物体が存在したとき、深度センサは、可動部の位置を検出せずに、可動部以外の動く物体の位置を検出してしまう可能性があることを見出した。なお、可動部が作業対象物を保持している場合、可動部及び作業対象物が動いている状態で、これら以外の動く物体が存在するとき、深度センサは、これら以外の動く物体の位置を検出してしまう可能性がある。マニピュレータの可動部の位置の検出精度を向上できる技術が望まれる。
本発明は、マニピュレータの可動部の位置の検出精度を向上させることができる干渉防止装置を提供することを目的とする。
本発明に係る干渉防止装置は、所定の可動範囲内で可動する可動部を備えるマニピュレータに適用され、前記可動範囲内の対象物体と前記可動部との干渉を防止する干渉防止装置であって、撮像範囲に前記可動部が含まれる深度センサによって、前記可動部が前記可動範囲を動いているときに撮像された深度画像が、時系列に並べられた画像データを、前記可動部が前記可動範囲を動いているときに取得する取得部と、前記可動部が前記可動範囲を動いているときに、パーティクルフィルタを用いて前記画像データを処理することにより、前記可動部の位置を検出する検出部と、を備え、前記検出部は、前記パーティクルフィルタを用いる処理に含まれる尤度の算出において、前記可動部の速度をパラメータとして、前記尤度を算出する。
本発明に係る干渉防止装置は、マニピュレータの可動部とこの可動部の可動範囲内の対象物体とが干渉(すなわち、接触)することを防止する装置である。本発明に係る干渉防止装置によれば、パーティクルフィルタを利用した可動部の位置検出において、尤度の計算のパラメータとして、可動部の速度を用いる。このため、可動部が可動範囲を動いている状態で、深度センサの撮像範囲に、可動部以外の動く物体が存在するとき、検出部は、可動部以外の動く物体を位置検出の対象から排除することができる。なお、可動部が作業対象物を保持している場合、可動部及び作業対象物が動いている状態で、深度センサの撮像範囲に、これら以外の動く物体が存在するとき、検出部は、これら以外の動く物体を位置検出の対象から排除することができる。従って、本発明に係る干渉防止装置によれば、マニピュレータの可動部の位置の検出精度を向上させることができる。
対象物体とは、マニピュレータの可動部以外の構成要素(例えば、マニピュレータの固定アーム)でもよいし、このマニピュレータ以外の物体(例えば、他のマニピュレータ)でもよい。干渉防止装置が、深度センサを構成要件として備える場合、深度センサが取得部となる。干渉防止装置が、深度センサを構成要件として備えない場合、深度センサから出力された上記画像データ(深度画像が時系列に並べられた画像データ)が入力する入力部(入力インターフェース)が取得部となる。
上記構成において、前記可動部の速度を測定する測定部をさらに備え、前記検出部は、前記測定部によって測定された前記可動部の速度をパラメータとして前記尤度を算出する。
可動部の速度が予め決められた一定速度の場合、この一定速度を示す速度情報が検出部に予め設定される。しかし、可動部の速度が一定でない場合、可動部の速度を測定する必要がある。そこで、この構成は、可動部の速度を測定する測定部を備える。
上記構成において、前記マニピュレータは、前記可動部を回動させるアクチュエータと、前記アクチュエータの回転角を検出するセンサと、を備えており、前記測定部は、前記センサによって検出された前記回転角を用いて、前記可動部の速度を算出する。
マニピュレータは、可動部を回動させるアクチュエータの制御のために、アクチュエータの回転角を検出するセンサを備える。この構成は、センサが検出したアクチュエータの回転角を用いて、可動部の速度を算出する。従って、可動部の速度を算出するためのセンサを新たに設ける必要がなくなる。
本発明によれば、マニピュレータの可動部の位置の検出精度を向上させることができる。
実施形態に係る干渉防止装置を備えるマニピュレータの模式図である。 実施形態に係る干渉防止装置の構成を示すブロック図である。 実施形態に係る干渉防止装置の動作を説明するフローチャートである。 エンドエフェクタの位置の検出を説明するフローチャートである。 パーティクルフィルタの初期設定がされた1番目のフレームを説明する説明図である。 Σf(x)と各パーティクルの尤度との関係を説明する説明図である。 リサンプリング後のフレームを説明する説明図である。 図4のステップS12〜ステップS16の処理が繰り返された後のフレームを説明する説明図である。
以下、図面に基づいて本発明の実施形態を詳細に説明する。各図において、同一符号を付した構成は、同一の構成であることを示し、その構成について、既に説明している内容については、その説明を省略する。本明細書において、総称する場合には添え字を省略した参照符号で示し、個別の構成を指す場合には添え字を付した参照符号で示す。
図1は、実施形態に係る干渉防止装置を備えるマニピュレータ3の模式図である。マニピュレータ3が、ロボットアームを例にして説明する。マニピュレータ3は、ベース30、固定アーム31、可動アーム32、関節33、エンドエフェクタ34、アクチュエータ35、回転角センサ36及び制御装置37を備える。
ベース30上には、固定アーム31(対象物体の一例)の一端が固定されている。固定アーム31は、上方に延びている。固定アーム31の他端と可動アーム32−1の一端とは、関節33−1によって連結されている。関節33−1は、アクチュエータ35−1(例えば、サーボモータ)が回転することにより生じる回転駆動力によって回動する。アクチュエータ35−1の回転角が、回転角センサ36−1によって検出される。
可動アーム32−1の他端と可動アーム32−2の一端とは、関節33−2によって連結されている。関節33−2は、アクチュエータ35−2(例えば、サーボモータ)が回転することにより生じる回転駆動力によって回動する。アクチュエータ35−2の回転角が、回転角センサ36−2によって検出される。
可動アーム32−2の他端には、エンドエフェクタ34が連結されている。エンドエフェクタ34は、作業対象物を掴む機能を有する。エンドエフェクタ34は、この機能を実行するためのアクチュエータ(不図示)及びセンサ(不図示)を備える。
エンドエフェクタ34は、可動アーム32−2の他端と連結されているので、可動アーム32−2が回動するとき、可動アーム32−2と一体となって回動する。エンドエフェクタ34の回動範囲に、固定アーム31の位置が含まれる。エンドエフェクタ34は、可動部の一例であり、可動部の可動範囲に、固定アーム31(対象物体の一例)の位置が含まれる。可動は、回動に限らず、移動でもよい。なお、エンドエフェクタ34が作業対象物を掴んでいる場合(言い換えれば、保持している場合)、エンドエフェクタ34及び作業対象物が、可動部とみなされる。可動部は、エンドエフェクタ34に限らず、固定アーム31と干渉する可能性を有する要素であればよい(例えば、可動アーム32ー2)。
制御装置37は、ベース30内に設置されている。制御装置37は、可動アーム32及びエンドエフェクタ34を動作させる制御をする。例えば、可動アーム32−2で説明すると、制御装置37は、回転角センサ36−2で検出された角度を基にして、アクチュエータ35−2を作動させて、関節33−2を回動させることにより、可動アーム32−2を回動させる。
実施形態に係る干渉防止装置を説明する。図2は、実施形態に係る干渉防止装置1の構成を示すブロック図である。図1及び図2を参照して、干渉防止装置1は、深度センサ10と干渉防止装置本体11とを備える。
深度センサ10は、赤外線を利用して深度画像を撮像する。深度画像は、二次元画像であり、深度画像を構成する各画素の画素値は、各画素に対応する箇所の深度(すなわち、深度センサ10とその箇所との距離)を示す。
深度センサ10の撮像範囲Rは、固定アーム31の前方である。従って、エンドエフェクタ34が固定アーム31の前方に現れたとき、エンドエフェクタ34は、撮像範囲Rに入る。
干渉防止装置本体11は、ベース30内に設置されている。干渉防止装置本体11は、CPU(Central Processing Unit)、RAM(Random Access Memory)、及び、ROM(Read Only Memory)等のハードウェア、並びに、干渉防止装置1の動作に必要なソフトウェア及びプログラムによって実現されるコンピュータである。干渉防止装置本体11及び制御装置37がベース30内に設置されている例で説明しているが、これらがベース30外に設定されていてもよい。
干渉防止装置本体11は、制御装置37と通信可能であり、機能ブロックとして、深度センサ制御部110、測定部111、検出部112及び算出部113を備える。
深度センサ制御部110は、深度センサ10を制御して、深度センサ10に動画を撮像させる。この動画を示す動画データは、深度画像が時系列に並べられた画像データの一例である。深度センサ10は、取得部として機能する。取得部は、撮像範囲Rにエンドエフェクタ34(可動部の一例)が含まれる深度センサ10によって、エンドエフェクタ34が回動範囲を動いているときに撮像された深度画像の動画データを、エンドエフェクタ34が回動範囲を動いているときに取得する。なお、干渉防止装置1が、深度センサ10を構成要件として備えない場合、深度センサ10から出力された上記動画データが入力する入力部(入力インターフェース)が取得部となる。入力部は、干渉防止装置本体11に備えられる。
測定部111は、エンドエフェクタ34の速度を測定する。詳しく説明すると、測定部111は、回転角センサ36によって検出された、アクチュエータ35の回転角を用いて、エンドエフェクタ34の速度を算出する。可動アーム32−1が回動していない状態で、可動アーム32−2が回動している場合を例にすると、エンドエフェクタ34の速度は、下記式1を用いて算出することができる。
v=rω・・・式1
vは、エンドエフェクタ34の速度である。rは、可動アーム32−2の長さである。ωは、アクチュエータ35−2の角速度である。この角速度は、回転角センサ36−2によって検出された、アクチュエータ35−2の回転角を用いて算出される。
実施形態に係る干渉防止装置1によれば、回転角センサ36が検出したアクチュエータ35の回転角を用いて、エンドエフェクタ34の速度を算出する。従って、エンドエフェクタ34の速度を算出するためのセンサを新たに設ける必要がなくなる。
検出部112は、エンドエフェクタ34が回動しているときに、深度センサ10が撮像した動画を構成する各フレーム(各深度画像)に対して、パーティクルフィルタ処理をすることにより、エンドエフェクタ34の位置を検出する。検出部112は、エンドエフェクタ34の位置検出を繰り返すことにより、エンドエフェクタ34の位置を追跡する。
算出部113は、検出部112によって、エンドエフェクタ34の位置が検出されたとき、エンドエフェクタ34と深度センサ10との距離を、深度画像を用いて算出する。この深度画像は、動画データを構成するフレームの中で、検出部112によってエンドエフェクタ34の位置が検出されたときのフレーム(深度画像)である。このフレームに含まれるエンドエフェクタ像7(図8)を構成する画素の画素値は、深度センサ10とエンドエフェクタ34との距離を示す。深度センサ10は、固定アーム31に設置されているので、深度センサ10とエンドエフェクタ34との距離は、固定アーム31とエンドエフェクタ34との距離と見なすことができる。
可動アーム32−1の回動が停止している状態で、可動アーム32−2が回動する場合を例にして、実施形態に係る干渉防止装置1の動作を説明する。図3は、この動作を説明するフローチャートである。
図1及び図3を参照して、可動アーム32−2が回動することにより、エンドエフェクタ34(可動部の一例)が可動アーム32−2と一緒に動く。可動アーム32−2の回動が正方向のとき、エンドエフェクタ34が固定アーム31(対象物体の一例)から遠ざかり、可動アーム32−2の回動が負方向のとき、エンドエフェクタ34が固定アーム31に近づくとする。検出部112は、可動アーム32−2の回動が負方向か否かを判断する(ステップS1)。検出部112は、回転角センサ36−2によって検出された回転角の変化に基づいて、その判断をする。
検出部112が、可動アーム32−2の回動が負方向でないと判断したとき(ステップS1でNo)、すなわち、可動アーム32−2の回動が正方向、又は、可動アーム32−2の回動が停止していると判断したとき、検出部112は、ステップS1の処理を繰り返す。
検出部112が、可動アーム32−2の回動が負方向であると判断したとき(ステップS1でYes)、深度センサ制御部110は、深度センサ10に、動画の撮像を開始させる(ステップS2)。
検出部112は、深度センサ10が撮像した動画の動画データを用いて、エンドエフェクタ34の位置を検出する(ステップS3)。これについては、後で詳細に説明する。
検出部112は、エンドエフェクタ34と固定アーム31との距離が、予め定められた第1のしきい値より小さいか否かを判断する(ステップS4)。この距離が、第1のしきい値より小さいとき、エンドエフェクタ34と固定アーム31とが近い状態であり、エンドエフェクタ34が固定アーム31に干渉する危険性がある。この距離は、上述したように、深度画像を用いて算出される。
検出部112が、エンドエフェクタ34と固定アーム31との距離が、第1のしきい値以上と判断したとき(ステップS4でNo)、検出部112は、エンドエフェクタ34の位置検出を継続する(ステップS3)。すなわち、検出部112は、エンドエフェクタ34の位置を追跡する。
検出部112が、エンドエフェクタ34と固定アーム31との距離が、第1のしきい値より小さいと判断したとき(ステップS4でYes)、検出部112は、制御装置37に対して、可動アーム32−2の回動の停止命令をする(ステップS5)。すなわち、検出部112は、可動アーム32−2の回動を停止する命令を示す信号を、制御装置37に送信する。制御装置37は、その信号を受信したとき、アクチュエータ35−2を停止させることにより、可動アーム32−2の回動を停止させる。これにより、エンドエフェクタ34が固定アーム31に干渉することを防止する。
なお、ステップS5において、停止命令の替わりに、減速命令でもよい。この場合、検出部112は、可動アーム32−2の回動速度を減速する命令を示す信号を、制御装置37に送信する。制御装置37は、その信号を受信したとき、アクチュエータ35−2の動作速度を減速させることにより、可動アーム32−2の回動速度を減速させる。マニピュレータ3の操作者は、エンドエフェクタ34が固定アーム31に干渉するおそれがあると判断したとき、マニピュレータ3の動作を停止させる。
停止命令、減速命令の替わりに、干渉防止装置本体11が、報知部(不図示)を作動させてもよい。検出部112が、エンドエフェクタ34と固定アーム31との距離が、第1のしきい値より小さいと判断したとき(ステップS4でYes)、報知部は、例えば、ランプ点滅、ブザーを鳴らす動作をする。これにより、マニピュレータ3の操作者に警告を報知する。操作者は、エンドエフェクタ34が固定アーム31に干渉するおそれがあると判断したとき、マニピュレータ3の動作を停止させる。
エンドエフェクタ34の位置の検出(ステップS3)について詳しく説明する。実施形態に係る干渉防止装置1は、エンドエフェクタ34の位置の検出にパーティクルフィルタを用いる。図4は、エンドエフェクタ34の位置の検出を説明するフローチャートである。
図1、図2及び図4を参照して、検出部112は、図3のステップS2で撮像された動画の1番目のフレームに対して初期設定をする(ステップS11)。フレームは、エンドエフェクタ34を含む撮像範囲Rの深度画像である。図5は、初期設定された1番目のフレーム5aを説明する説明図である。1番目のフレーム5aは、エンドエフェクタ像7を含む。検出部112は、1番目のフレーム5a上にn個(例えば、1000個)のパーティクル6をランダムに配置する。一つのパーティクル6は、一つの画素上に配置される。
検出部112は、仮定の予測モデルを用いてフレーム上に配置されたn個のパーティクル6を遷移させる(ステップS12)。すなわち、検出部112は、n個のパーティクル6のそれぞれを次のフレーム上の予測される位置に配置する。ステップS12の処理により、エンドエフェクタ34の検出精度を向上させることができる。エンドエフェクタ34の速度が遅いとき、この処理はしなくてもよい。
ここでの予測モデルは、撮像範囲Rの中で、各パーティクル6が配置された画素に対応する箇所の動作モデル(その箇所の移動速度)である。フレームは、深度画像であるので、深度画像を構成する各画素の画素値は、画素に対応する箇所から深度センサ10までの距離を示す。画素に対応する箇所の移動速度は、フレーム間差分によって移動距離を求め、移動距離とフレームレートとを用いて算出することができる。
深度センサ10が動画の撮像が開始してからある程度の時間が経過しなければ、動作モデルは確定できない。検出部112は、1番目のフレームから所定数のフレームに対しては、ステップS12の処理をしない。
検出部112は、パーティクル6の尤度を算出する(ステップS13)。尤度の算出には、以下のパラメータ1〜パラメータ3が用いられる。
パラメータ1:撮像範囲Rの中で、パーティクル6が配置された画素に対応する箇所の移動速度
パラメータ2:エンドエフェクタ34の速度
パラメータ3:エンドエフェクタ34の速度の分散σ
i番目のフレームを例にして説明すると、パラメータ1(移動速度)は、i番目のフレームとi−1番目のフレームとのフレーム間差分を用いて、検出部112によって算出される。パラメータ1は、n個のパーティクル6のそれぞれについて、算出される。パラメータ2(エンドエフェクタ34の速度)は、上述したように、測定部111によって算出される。i番目のフレームに配置されたパーティクル6の場合、i番目のフレームが撮像されたときのエンドエフェクタ34の速度である。パラメータ3(分散σ)は、干渉防止装置1の設計者等によって、検出部112に予め設定されている。
例えば、尤度が正規分布していると仮定すれば、以下の式2を用いて尤度は、算出される。
Figure 2017222004
xは、パラメータ1である。μは、パラメータ2である。σは、パラメータ3である。kは、1〜nまでの整数である。f(x)は、第kパーティクル6の尤度である。
n個のパーティクル6の中で、第1パーティクル6の尤度f(x)は、第1パーティクル6が配置された画素に対応する箇所の移動速度(パラメータ1)、パラメータ2及びパラメータ3が、式2に代入されることにより算出される。以下同様に、第2パーティクル6の尤度f(x)は、第2パーティクル6が配置された画素に対応する箇所の移動速度(パラメータ1)、パラメータ2及びパラメータ3が、式2に代入されることにより算出され、・・・、第nパーティクル6の尤度f(x)は、第nパーティクル6が配置された画素に対応する箇所の移動速度(パラメータ1)、パラメータ2及びパラメータ3が、式2に代入されることにより算出される。
検出部112は、n個のパーティクル6のそれぞれについて、以下の式3を用いて、重みを算出する(ステップS14)。
Figure 2017222004
は、第kパーティクル6の重みである。f(x)は、第kパーティクル6の尤度である。Σf(x)は、第1パーティクル6の尤度f(x)〜第nパーティクル6の尤度f(x)を加算した値である。
図6は、Σf(x)と各パーティクル6の尤度との関係を説明する説明図である。Σf(x)は、n個のボックス[1]〜ボックス[n]により構成されているとする。ボックスの長さが大きいほど、尤度(重み)が大きく、ボックスの長さが小さいほど、尤度(重み)が小さいことを示す。ボックス[1]は、第1パーティクル6の尤度f(x)、ボックス[2]は、第2パーティクル6の尤度f(x)、ボックス[3]は、第3パーティクル6の尤度f(x)、ボックス[4]は、第4パーティクル6の尤度f(x)、・・・、ボックス[n]は、第nパーティクル6の尤度f(x)を示す。
式3から分かるように、尤度が大きくなるに従って、重みが大きくなり、尤度が小さくなるにしたがって、重みが小さくなる。
図2及び図4を参照して、検出部112は、n個のパーティクル6のそれぞれについて、リサンプリングをする(ステップS15)。第1パーティクル6を例にして説明すると、検出部112は、第1パーティクル6が配置される位置として、第1パーティクル6〜第nパーティクル6のいずれかの位置を決定する。この確率は、各位置で同じでない。ステップS14で算出された重みが大きいパーティクル6(言い換えれば、尤度が大きいパーティクル6)の位置に決定される確率が高く、重みが小さいパーティクル6(言い換えれば、尤度が小さいパーティクル6)の位置に決定される確率が小さい。
第1パーティクル6が配置される位置が、第1パーティクル6〜第nパーティクル6のいずれかの位置に限定されないように、検出部112は、決定した位置からずれた位置に、第1パーティクル6を配置する(ランダムノイズ)。例えば、第1パーティクル6が第3パーティクル6の位置に配置する決定をした場合、第3パーティクル6の位置から多少ずれた位置に第1パーティクル6を配置する。ランダムノイズは、干渉防止装置1の設計者等が検出部112に予め設定する。
検出部112は、第2パーティクル6から第nパーティクル6についても同様にして、配置する。図7は、リサンプリング後のフレーム5bを説明する説明図である。フレーム5bは、エンドエフェクタ像7を含む。リサンプリング前のフレームが1番目のフレームの場合、2番目のフレームがリサンプリング後のフレーム5bとなる。図5に比べて、パーティクル6がエンドエフェクタ像7の近くに集まっている。
検出部112は、リサンプリング後のフレームについて、予め定められた範囲に予め定められた数のパーティクル6が存在するか否かを判断する(ステップS16)。予め定められた範囲に予め定められた数のパーティクル6が存在するとき、その範囲がエンドエフェクタ34の位置として確定される。検出部112は、例えば、重みが最も大きいパーティクル6の位置を中心にして、予め定められた範囲に予め定められた数のパーティクル6が存在するか否かを判断する。
検出部112が、予め定められた範囲に予め定められた数のパーティクル6が存在しないと判断したとき(ステップS16でNo)、検出部112は、ステップS12の処理に戻る。検出部112が、ステップS12〜ステップS16の処理を繰り返すと、図8に示すように、エンドエフェクタ像7に集まるパーティクル6の数が増加する。図8は、ステップS12〜ステップS16の処理が繰り返された後のフレーム5cを示す。
検出部112が、予め定められた範囲に予め定められた数のパーティクル6が存在すると判断したとき(ステップS16でYes)、検出部112は、その範囲がエンドエフェクタ34の位置として確定する。
検出部112は、ステップS16でYesと判断したとき、予め定められた範囲に存在するそれぞれのパーティクル6について、パーティクル6に対応する箇所の深度を算出する。検出部112は、例えば、算出した深度の平均値である平均深度を算出する。
検出部112は、平均深度が予め定められた第2のしきい値より小さいか否かを判断する(ステップS17)。平均深度が、第2のしきい値より小さいとき、エンドエフェクタ34と固定アーム31とが近い状態であり、エンドエフェクタ34が固定アーム31に干渉する危険性があるとする。なお、検出部112は、上記予め定められた範囲に存在するそれぞれのパーティクル6に対応する箇所の深度のうち、最小値を、平均深度の替わりに用いてもよい。
検出部112は、平均深度が第2のしきい値以上と判断したとき(ステップS17でNo)、図3のステップS4において、エンドエフェクタ34と固定アーム31との距離が第1のしきい値以上と決定する(ステップS18)。そして、検出部112は、ステップS12の処理に戻る(言い換えれば、図3のステップS3の処理をする)。
検出部112は、平均深度が第2のしきい値より小さいと判断したとき(ステップS17でYes)、図3のステップS4において、エンドエフェクタ34と固定アーム31との距離が第1のしきい値より小さいと決定する(ステップS19)。そして、検出部112は、図3のステップS5の処理をする。
実施形態の主な効果を説明する。実施形態に係る干渉防止装置1によれば、パーティクルフィルタを利用したエンドエフェクタ34(可動部の一例)の位置検出において、尤度の計算のパラメータとして、エンドエフェクタ34の速度を用いる(図4のステップS13)。このため、エンドエフェクタ34が回動範囲を動いている状態で、深度センサ10の撮像範囲Rに、エンドエフェクタ34以外の動く物体が存在するとき、検出部112は、エンドエフェクタ34以外の動く物体を位置検出の対象から排除することができる。なお、エンドエフェクタ34が作業対象物を保持している場合、エンドエフェクタ34及び作業対象物が動いている状態で、深度センサ10の撮像範囲Rに、これら以外の動く物体が存在するとき、検出部112は、これら以外の動く物体を位置検出の対象から排除することができる。従って、実施形態に係る干渉防止装置1によれば、エンドエフェクタ34の位置の検出精度を向上させることができる。
干渉防止装置1は、エンドエフェクタ34と固定アーム31(対象物体の一例)とが干渉することを防止するために、エンドエフェクタ34と固定アーム31との距離を測定する必要がある。固定アーム31に深度センサ10が設置されているので、エンドエフェクタ34と深度センサ10との距離が分かれば、エンドエフェクタ34と固定アーム31との距離が分かる。実施形態によれば、エンドエフェクタ34と深度センサ10との距離を、深度画像を用いて算出するので(図3のステップS4)、エンドエフェクタ34と固定アーム31との距離を測定するセンサを新たに設ける必要がなくなる。
1 干渉防止装置
3 マニピュレータ
5a,5b,5c フレーム(深度画像の一例)
6 パーティクル
7 エンドエフェクタ像
10 深度センサ
11 干渉防止装置本体
31 固定アーム(対象物体の一例)
32−1,32−2 可動アーム
33−1,33−2 関節
34 エンドエフェクタ(可動部の一例)
35−1,35−2 アクチュエータ
36−1,36−2 回転角センサ
R 撮像範囲

Claims (3)

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

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2016119512A JP2017222004A (ja) 2016-06-16 2016-06-16 干渉防止装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2016119512A JP2017222004A (ja) 2016-06-16 2016-06-16 干渉防止装置

Publications (1)

Publication Number Publication Date
JP2017222004A true JP2017222004A (ja) 2017-12-21

Family

ID=60686725

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2016119512A Pending JP2017222004A (ja) 2016-06-16 2016-06-16 干渉防止装置

Country Status (1)

Country Link
JP (1) JP2017222004A (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2020020145A (ja) * 2018-07-31 2020-02-06 株式会社小松製作所 指標値特定装置および指標値特定方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2020020145A (ja) * 2018-07-31 2020-02-06 株式会社小松製作所 指標値特定装置および指標値特定方法
JP7175666B2 (ja) 2018-07-31 2022-11-21 株式会社小松製作所 指標値特定装置および指標値特定方法

Similar Documents

Publication Publication Date Title
US9122266B2 (en) Camera-based monitoring of machines with mobile machine elements for collision prevention
US10195741B2 (en) Controlling a robot in the presence of a moving object
KR100922494B1 (ko) 이동 로봇의 자세 측정 방법 및 상기 방법을 이용한 위치측정 방법 및 장치
JP5080333B2 (ja) 自律移動体のための物体認識装置
KR101234798B1 (ko) 이동 로봇의 위치 측정 방법 및 장치
JP2010120139A (ja) 産業用ロボットの安全制御装置
US8634959B2 (en) Apparatus and method detecting a robot slip
EP2495079A1 (en) Slip detection apparatus and method for a mobile robot
TWI544994B (zh) 機器人及機器人之雜訊除去方法
JPWO2008047872A1 (ja) マニピュレータ
US20110166763A1 (en) Apparatus and method detecting a robot slip
JP2009500709A (ja) 物体の動きのパターンを認識する方法
CN108713217A (zh) 除去干扰装置
JP2022527059A (ja) 衝突検出
JP2017222004A (ja) 干渉防止装置
JP2003051015A (ja) 環境認識装置
JP4774401B2 (ja) 自律移動体の経路設定装置
CN114137241A (zh) 样品容器运送系统
JP2019093152A5 (ja)
JP2022097513A (ja) 建設機械の障害物検出装置
JP4421121B2 (ja) 侵入物体検出方法
KR20220061821A (ko) 로봇 및 그 제어 방법
CN112882461A (zh) 机器人、控制方法以及存储介质
CN112775961A (zh) 控制装置、控制装置的控制方法以及控制系统
WO2023157380A1 (ja) ロボット監視システム、監視装置、監視装置の制御方法およびプログラム