ロボットアームの制御プログラム及び制御システム
【課題】ハンド部を適切な位置に停止させることができるロボットアームの制御プログラムを提供する。
【解決手段】本発明に係るロボットアームの制御プログラムは、ロボットアームのハンド部を、把持対象物に向かって移動させる処理と、ハンド部に設けられた距離測定部に、ハンド部と把持対象物との距離を測定させる処理と、距離測定部がハンド部と把持対象物との距離の測定が不能となると、測定不能となる直前のハンド部と把持対象物との距離と、ロボットアームの関節部の角度から算出させた、測定不能となった後のハンド部の移動距離と、に基づいて、ハンド部と把持対象物との距離を推定させる処理と、推定させたハンド部と把持対象物との距離が閾値以下となると、ハンド部を把持対象物に向かって移動させる動作を停止させる処理と、をコンピュータに実行させる。
【解決手段】本発明に係るロボットアームの制御プログラムは、ロボットアームのハンド部を、把持対象物に向かって移動させる処理と、ハンド部に設けられた距離測定部に、ハンド部と把持対象物との距離を測定させる処理と、距離測定部がハンド部と把持対象物との距離の測定が不能となると、測定不能となる直前のハンド部と把持対象物との距離と、ロボットアームの関節部の角度から算出させた、測定不能となった後のハンド部の移動距離と、に基づいて、ハンド部と把持対象物との距離を推定させる処理と、推定させたハンド部と把持対象物との距離が閾値以下となると、ハンド部を把持対象物に向かって移動させる動作を停止させる処理と、をコンピュータに実行させる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットアームの制御プログラム及び制御システムに関する。
【背景技術】
【０００２】
従来からロボットアームは、物体位置姿勢推定部、アーム軌道生成部、ロボットアーム制御部、角度測定部を用いて制御されている。物体位置姿勢推定部は、例えばステレオカメラ等の撮像部が撮像した画像情報に基づいて把持対象物の位置や姿勢などを推定し、当該位置・姿勢情報をアーム軌道生成部に出力する。アーム軌道生成部は、当該位置・姿勢情報に基づいて、ロボットアームの端部に連結されたハンド部で把持対象物を良好に把持することができるように、ロボットアームの制御信号を生成し、当該制御信号をロボットアーム制御部に出力する。ロボットアーム制御部は、当該制御信号に基づいて、ロボットアームを制御する。このとき、ロボットアーム制御部には、角度測定部からロボットアームの関節部の角度情報が入力される。ロボットアーム制御部は、当該角度情報を取得しつつ、ロボットアームを制御する。
ところで、特許文献１、２には、距離センサを用いて把持対象物との距離を測定し、当該距離情報に基づいてロボットアームを制御する技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開昭６３−１９６３９４号公報
【特許文献２】特開平６−３１６６６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、ステレオカメラ等の撮像部が撮像した画像情報に基づいて把持対象物の位置を推定する場合、ステレオカメラ等は奥行き方向に誤差が大きく、ハンド部が把持対象物から遠くて当該把持対象物を把持できなかったり、逆にハンド部が把持対象物に近付き過ぎて当該把持対象物に接触したりする場合がある。
【０００５】
一方、特許文献１、２の技術は、距離センサを用いて把持対象物との距離を測定しているので、比較的正確にハンド部と把持対象物との距離を測定できるが、距離センサは測定対象である把持対象物が遠くに配置されていたり、逆に近くに配置されていたりすると、測定不能となる場合がある。
【０００６】
本発明は、このような問題を解決するためになされたものであり、ハンド部を適切な位置に停止させることができるロボットアームの制御プログラム及び制御システムを提供することを目的とするものである。
【課題を解決するための手段】
【０００７】
本発明に係るロボットアームの制御プログラムは、ロボットアームの端部に連結されたハンド部を、把持対象物に向かって移動させる処理と、前記ハンド部に設けられた距離測定部に、前記ハンド部と前記把持対象物との距離を測定させる処理と、前記距離測定部が前記ハンド部と前記把持対象物との距離の測定が不能となると、測定不能となる直前の前記ハンド部と前記把持対象物との距離と、前記ロボットアームの関節部の角度から算出させた、測定不能となった後の前記ハンド部の移動距離と、に基づいて、前記ハンド部と前記把持対象物との距離を推定させる処理と、前記推定させた前記ハンド部と前記把持対象物との距離が閾値以下となると、前記ハンド部を前記把持対象物に向かって移動させる動作を停止させる処理と、をコンピュータに実行させる。これにより、ハンド部で把持対象物を把持する際に、ハンド部が把持対象物に対して遠すぎたり、近すぎたりすることなく、適切な位置にハンド部を停止させることができる。
【０００８】
前記ハンド部を前記把持対象物に向かって移動させる処理は、前記ハンド部で前記把持対象物を把持することができるように、前記ハンド部を前記把持対象物に向かって直線的に移動させること、が好ましい。ハンド部を把持対象物に向かって直線的に移動させるので、距離測定部から出射されるレーザーや超音波などが把持対象物を良好に捉えることができる。
【０００９】
特に、前記ハンド部の移動方向と、前記ハンド部の掌面の法線方向とを、略一致させた状態で、前記ハンド部を前記把持対象物に向かって直線的に移動させること、が好ましい。ハンド部の移動方向と、ハンド部の掌面の法線方向とを、略一致させた状態で、ハンド部を把持対象物に向かって移動させるので、ハンド部を閉じると、ハンド部で把持対象物を良好に把持することができる。
【００１０】
前記ハンド部と前記把持対象物との距離を推定させる処理は、前記距離測定部が測定不能となる直前の前記ハンド部と前記把持対象物との距離から、前記ロボットアームの関節部の角度に基づいて算出させた、前記距離測定部が測定不能となった後の前記ハンド部の移動距離を差分させて、前記ハンド部と前記把持対象物との距離を推定させる処理と、一つ前の処理で推定させた前記ハンド部と前記把持対象物との距離から、前記ロボットアームの関節部の角度に基づいて算出させた、推定後の前記ハンド部の移動距離を差分させて、前記ハンド部と前記把持対象物との距離を推定させる処理と、を備えること、が好ましい。結果として、距離測定部が測定したハンド部と把持対象物との距離に基づいて、距離測定部が測定不能となった後のハンド部と把持対象物との距離を推定するので、精度良くハンド部と把持対象物との距離を把握することができる。
【００１１】
本発明に係るロボットアームの制御システムは、把持対象物を把持するハンド部を有するロボットアームと、前記ロボットアームの関節部の角度を測定する角度測定部と、前記ロボットアームのハンド部に設けられ、前記ハンド部から前記把持対象物までの距離を測定する距離測定部と、前記ハンド部を前記把持対象物に向かって移動させるべく、前記ロボットアームを制御する演算処理部と、を備え、前記演算処理部は、前記距離測定部からの入力が途絶えると、前記入力が途絶える直前の前記ハンド部から前記把持対象物までの距離と、前記角度測定部から入力される前記ロボットアームの関節部の角度から算出した前記ハンド部の移動距離と、に基づいて、前記ハンド部と前記把持対象物との距離を推定し、推定した前記ハンド部と前記把持対象物との距離が閾値以下となると、前記ハンド部を前記把持対象物に向かって移動させる動作を停止させる。これにより、ハンド部で把持対象物を把持する際に、ハンド部が把持対象物に対して遠すぎたり、近すぎたりすることなく、適切な位置にハンド部を停止させることができる。
【００１２】
前記ハンド部は掌面を備え、前記ロボットアーム制御部は、前記ハンド部の移動方向と、前記ハンド部の掌面の法線方向とを、略一致させた状態で、前記ハンド部を前記把持対象物に向かって直線的に移動させること、が好ましい。ハンド部の移動方向と、ハンド部の掌面の法線方向とを、略一致させた状態で、ハンド部を把持対象物に向かって移動させるので、ハンド部を閉じると、ハンド部で把持対象物を良好に把持することができる。
【発明の効果】
【００１３】
以上、説明したように、本発明によると、ハンド部を適切な位置に停止させることができるロボットアームの制御プログラム及び制御システムを提供することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明に係るロボットアームの制御システムを示すブロック図である。
【図２】ハンド部と把持対象物との配置を説明する図である。
【図３】ロボットアームのリーチ区間及びアプローチ区間を説明する図である。
【図４】本発明に係るロボットアームの制御プログラムの流れを示すフローチャートである。
【発明を実施するための形態】
【００１５】
以下、本発明を実施するための最良の形態について、添付図面を参照しながら説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【００１６】
ロボットアームの制御システム（以下、単に制御システムという場合がある。）１は、図１に示すように、ロボットアーム１１、演算処理部１２、角度測定部１３、距離測定部１４を備える。ここで、演算処理部１２は、典型的なコンピュータの演算処理部である。演算処理部１２は、物体位置姿勢推定部１２１、アーム軌道生成部１２２、ロボットアーム制御部１２３、距離推定部１２４を備える。
【００１７】
ロボットアーム１１は、多関節ロボットアームである。ロボットアーム１１の端部には、図２に示すように、把持対象物２１を把持するハンド部１１１が回転可能に連結されている。ハンド部１１１は掌面１１１ａを備え、当該掌面１１１ａを挟んで両側に把持部１１１ｂを備える。
【００１８】
物体位置姿勢推定部１２１は、ステレオカメラ等の撮像部１５が撮像した画像情報に基づいて、把持対象物２１までの距離、把持対象物２１の形状、姿勢などを推定し、位置・姿勢情報を生成する。物体位置姿勢推定部１２１は、当該位置・姿勢情報をアーム軌道生成部１２２に出力する。ちなみに、撮像部１５は、物体位置姿勢推定部１２１から入力される画像取得指令信号に基づいて制御される。
【００１９】
アーム軌道生成部１２２は、当該位置・姿勢情報に基づいて、例えばロボットアーム１１の端部に連結されたハンド部１１１で把持対象物２１を良好に把持させることができるように、ロボットアーム１１の制御信号を生成する。すなわち、ロボットアームの制御には、図３に示すように、リーチ区間とアプローチ区間とに区別されている。アプローチ区間でハンド部１１１を把持対象物２１に対してどの方向から近付け、ハンド部１１１で把持対象物２１を把持するかを定義付けした複数のパターンが、例えば図示を省略したコンピュータの記憶部に格納されている。当該パターンは、アプローチ区間において、ハンド部１１１で把持対象物２１を良好に把持することができるように、ハンド部１１１を把持対象物２１に向かって直線的に移動させる。すなわち、当該パターンは、図２に示すように、ハンド部１１１の移動方向と、ハンド部１１１の掌面１１１ａの法線Ｌ方向とを、略一致させた状態で、ハンド部１１１を把持対象物２１に向かって直線的に移動させる。
【００２０】
アーム軌道生成部１２２は、当該位置・姿勢情報に基づいて、複数のパターンのうちいずれのパターンを実行させるかを選択し、当該パターンをアプローチ区間で実行するために、リーチ区間でのロボットアーム１１の制御信号を生成する。アーム軌道生成部１２２は、当該制御信号及び当該選択したパターン情報をロボットアーム制御部１２３に出力する。
【００２１】
ロボットアーム制御部１２３は、当該制御信号に基づいて、ロボットアーム１１を制御する。このとき、ロボットアーム制御部１２３には、角度測定部１３からロボットアーム１１の関節部の角度情報が入力される。ロボットアーム制御部１２３は、当該角度情報を取得しつつ、リーチ区間においてロボットアーム１１を当該制御信号に基づいて制御する。ロボットアーム制御部１２３は、当該角度情報を距離推定部１２４に出力する。
【００２２】
ロボットアーム制御部１２３は、リーチ区間からアプローチ区間にハンド部１１１が侵入すると、当該選択したパターン情報に基づいて、ロボットアーム１１を制御する。このとき、ロボットアーム制御部１２３は、距離推定部１２４から停止指令信号が入力されると、当該選択したパターン情報に基づくロボットアーム１１の制御を停止する。
【００２３】
角度測定部１３は、ロボットアーム１１の各関節部に配置されており、関節部の回転角度を測定する。角度測定部１３は、測定した関節部の角度情報をロボットアーム制御部１２３に出力する。角度測定部１３としては、例えばエンコーダ等の角度検出手段を用いることができる。角度測定部１３は、ロボットアーム制御部１２３から入力される角度測定指令信号に基づいて制御される。
【００２４】
距離測定部１４は、ハンド部１１１に設けられている。例えば距離測定部１４は、ハンド部１１１の把持部１１１ｂに設けられる。但し、ハンド部１１１がアプローチ区間を移動する際に、把持対象物２１までの距離を良好に測定できる部位に距離測定部１４が設けられていれば良い。距離測定部１４は、把持対象物２１までの距離を測定し、測定した距離情報を距離推定部１２４に出力する。距離測定部１４としては、例えばレーザーや超音波などを用いた一般的な測距センサを用いることができる。上述のようにアプローチ区間においては、ハンド部１１１を把持対象物２１に向かって直線的に移動させるので、距離測定部１４から出射されるレーザーや超音波などが把持対象物２１を良好に捉えることができる。距離測定部１４は、距離推定部１２４から入力される距離測定指令信号に基づいて制御される。
【００２５】
距離推定部１２４は、ハンド部１１１と把持対象物２１との距離を推定する。すなわち、ハンド部１１１がアプローチ区間において、ハンド部１１１が把持対象物２１に近付き、距離測定部１４が把持対象物２１までの距離を測定すると、入力される距離情報に基づいて、ハンド部１１１と把持対象物２１との距離を算出する。そして、ハンド部１１１がさらに把持対象物２１に近付き、距離測定部１４が把持対象物２１までの距離を測定できなくなると、距離推定部１２４は、入力されるロボットアーム１１の関節部の角度情報やロボットアーム１１を成すアームの長さ等に基づいて、距離測定部１４で測定不能となった位置からのハンド部１１１の移動距離を算出する。つまり、距離推定部１２４は、ロボットアーム制御部１２３から当該ロボットアーム１１の関節部の角度情報を取得する。また、距離推定部１２４は、コンピュータの記憶部に格納されているロボットアーム１１を成すアームの長さ等の情報を取得する。
【００２６】
さらに距離推定部１２４は、距離測定部１４で測定不能となった際の距離情報から、算出したハンド部１１１の移動距離を差分し、ハンド部１１１と把持対象物２１との距離を推定する。その後、距離推定部１２４は、推定したハンド部１１１と把持対象物２１との距離から、推定後のハンド部１１１の移動距離を差分することを所定の周期で繰り返し、最終的に推定したハンド部１１１と把持対象物２１との距離が予め設定された閾値以下になったか否かを判断する。距離推定部１２４は、ハンド部１１１と把持対象物２１との距離が閾値以下になっていると、当該選択したパターン情報に基づくロボットアーム１１の制御を停止させるべく、停止指令信号を生成し、ロボットアーム制御部１２３に出力する。
【００２７】
このような制御システム１を用いてロボットアームの制御プログラム（以下、単に制御プログラムという場合がある。）は、図４に示すように実施される。
先ず、ロボットアーム制御部１２３は、アーム軌道生成部１２２で生成された制御信号に基づいて、リーチ区間においてロボットアーム１１を制御し、当該ロボットアーム１１の端部に連結されたハンド部１１１を、把持対象物２１に向かって移動させる（Ｓ１）。
【００２８】
次に、ロボットアーム制御部１２３は、アーム軌道生成部１２２が選択したパターンに基づいて、アプローチ区間においてロボットアーム１１を制御し、当該ロボットアーム１１のハンド部１１１を、把持対象物２１に向かって移動させる（Ｓ２）。これに伴い、ハンド部１１１は把持対象物２１に近付き、距離測定部１４で把持対象物２１までの距離が測定可能になる。ここで、距離測定部１４は、当該指令信号に基づいて、把持対象物２１までの距離を測定し、当該距離情報を距離推定部１２４に出力する（Ｓ３）。Ｓ３の工程を所定の周期で繰り返し、距離推定部１２４は、距離測定部１４が把持対象物２１に近付き、距離測定部１４で把持対象物２１までの距離が測定不能となる不感帯に当該距離測定部１４が入ったか否かを判定する（Ｓ４）。
【００２９】
距離推定部１２４は、距離測定部１４が不感帯に入っていないと判断すると、距離測定部１４から距離情報を取得し続ける。一方、距離推定部１２４は、距離測定部１４が不感帯に入ったと判断すると、距離測定部１４が不感帯に入った直後からのハンド部１１１の移動距離を算出する。すなわち、距離推定部１２４は、当該ロボットアーム１１の関節部の角度情報やロボットアーム１１を成すアームの長さ等に基づいて、距離測定部１４が不感帯に入った直後からのハンド部１１１の移動距離を算出する。そして、距離推定部１２４は、距離測定部１４が不感帯に入る直前の距離情報から、算出したハンド部１１１の移動距離を差分し、ハンド部１１１と把持対象物２１との距離を推定する（Ｓ５）。ちなみに、距離推定部１２４は、距離測定部１４からの距離情報の入力が途絶えると、距離測定部１４が不感帯に入ったと判断することができる。したがって、距離測定部１４が不感帯に入る直前の距離情報とは、最後に入力された距離情報である。
【００３０】
距離推定部１２４は、当該推定したハンド部１１１と把持対象物２１との距離から、推定後のハンド部１１１の移動距離を差分する。このように距離推定部１２４は、一つ前の工程で推定したハンド部１１１と把持対象物２１との距離から、推定後のハンド部１１１の移動距離を差分し、ハンド部１１１と把持対象物２１との距離を推定することを所定の周期で繰り返す（Ｓ６）。
【００３１】
距離推定部１２４は、Ｓ６の工程において推定したハンド部１１１と把持対象物２１との距離が閾値以下となったか否かを判断する（Ｓ７）。距離推定部１２４は、ハンド部１１１と把持対象物２１との距離が閾値以下となったと判断すると、当該選択したパターン情報に基づくロボットアーム１１の制御を停止させるべく、停止指令信号を生成し、ロボットアーム制御部１２３に出力する（Ｓ８）。一方、距離推定部１２４は、ハンド部１１１と把持対象物２１との距離が閾値より大きいと判断すると、Ｓ５の工程に戻る。
【００３２】
ロボットアーム制御部１２３は、距離推定部１２４から停止指令信号が入力されると、当該選択したパターン情報に基づくロボットアーム１１の制御を停止させる（Ｓ９）。
このように、距離測定部１４が不感帯に入っても、結果として距離測定部１４が測定したハンド部１１１と把持対象物２１との距離に基づいて、距離測定部１４が測定不能となった後のハンド部１１１と把持対象物２１との距離を推定するので、精度良くハンド部１１１と把持対象物２１との距離を把握することができる。そのため、ハンド部１１１で把持対象物２１を把持する際に、ハンド部１１１が把持対象物２１に対して遠すぎたり、近すぎたりすることなく、適切な位置にハンド部１１１を停止させることができる。ここで、アプローチ区間においては、ハンド部１１１の移動方向と、ハンド部１１１の掌面の法線Ｌ方向とを、略一致させた状態で、ハンド部１１１を把持対象物２１に向かって移動させたので、ハンド部１１１の把持部１１１ｂを閉じると、ハンド部１１１で把持対象物２１を良好に把持することができる。
【００３３】
以上、本発明に係るロボットアームの制御プログラム及び制御システムの実施形態を説明したが、上述した内容に限らず、本発明の技術的思想を逸脱しない範囲で、変更することが可能である。
上記の実施形態では、リーチ区間とアプローチ区間とに分けたが、ハンド部１１１を把持対象物２１の近傍に移動させるまで、アーム軌道生成部１２２がロボットアーム１１の制御信号を生成し続け、当該制御信号に基づいて、ロボットアーム制御部１２３がロボットアーム１１を制御しても良い。
上記の実施形態では、掌面１１１ａを備えたハンド部１１１であったが、掌面１１１ａを備えないハンド部を用いても良い。この場合、上述したようにハンド部１１１で把持対象物２１を良好に把持することができるように、ロボットアーム１１が制御される。
【符号の説明】
【００３４】
１ 制御システム
１１ ロボットアーム
１２ 演算処理部、１２１ 物体位置姿勢推定部、１２２ アーム軌道生成部、１２３ ロボットアーム制御部、１２４ 距離推定部
１３ 角度測定部
１４ 距離測定部
１５ 撮像部
２１ 把持対象物
１１１ ハンド部、１１１ａ 掌面、１１１ｂ 把持部
Ｌ 法線
【技術分野】
【０００１】
本発明は、ロボットアームの制御プログラム及び制御システムに関する。
【背景技術】
【０００２】
従来からロボットアームは、物体位置姿勢推定部、アーム軌道生成部、ロボットアーム制御部、角度測定部を用いて制御されている。物体位置姿勢推定部は、例えばステレオカメラ等の撮像部が撮像した画像情報に基づいて把持対象物の位置や姿勢などを推定し、当該位置・姿勢情報をアーム軌道生成部に出力する。アーム軌道生成部は、当該位置・姿勢情報に基づいて、ロボットアームの端部に連結されたハンド部で把持対象物を良好に把持することができるように、ロボットアームの制御信号を生成し、当該制御信号をロボットアーム制御部に出力する。ロボットアーム制御部は、当該制御信号に基づいて、ロボットアームを制御する。このとき、ロボットアーム制御部には、角度測定部からロボットアームの関節部の角度情報が入力される。ロボットアーム制御部は、当該角度情報を取得しつつ、ロボットアームを制御する。
ところで、特許文献１、２には、距離センサを用いて把持対象物との距離を測定し、当該距離情報に基づいてロボットアームを制御する技術が開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開昭６３−１９６３９４号公報
【特許文献２】特開平６−３１６６６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかし、ステレオカメラ等の撮像部が撮像した画像情報に基づいて把持対象物の位置を推定する場合、ステレオカメラ等は奥行き方向に誤差が大きく、ハンド部が把持対象物から遠くて当該把持対象物を把持できなかったり、逆にハンド部が把持対象物に近付き過ぎて当該把持対象物に接触したりする場合がある。
【０００５】
一方、特許文献１、２の技術は、距離センサを用いて把持対象物との距離を測定しているので、比較的正確にハンド部と把持対象物との距離を測定できるが、距離センサは測定対象である把持対象物が遠くに配置されていたり、逆に近くに配置されていたりすると、測定不能となる場合がある。
【０００６】
本発明は、このような問題を解決するためになされたものであり、ハンド部を適切な位置に停止させることができるロボットアームの制御プログラム及び制御システムを提供することを目的とするものである。
【課題を解決するための手段】
【０００７】
本発明に係るロボットアームの制御プログラムは、ロボットアームの端部に連結されたハンド部を、把持対象物に向かって移動させる処理と、前記ハンド部に設けられた距離測定部に、前記ハンド部と前記把持対象物との距離を測定させる処理と、前記距離測定部が前記ハンド部と前記把持対象物との距離の測定が不能となると、測定不能となる直前の前記ハンド部と前記把持対象物との距離と、前記ロボットアームの関節部の角度から算出させた、測定不能となった後の前記ハンド部の移動距離と、に基づいて、前記ハンド部と前記把持対象物との距離を推定させる処理と、前記推定させた前記ハンド部と前記把持対象物との距離が閾値以下となると、前記ハンド部を前記把持対象物に向かって移動させる動作を停止させる処理と、をコンピュータに実行させる。これにより、ハンド部で把持対象物を把持する際に、ハンド部が把持対象物に対して遠すぎたり、近すぎたりすることなく、適切な位置にハンド部を停止させることができる。
【０００８】
前記ハンド部を前記把持対象物に向かって移動させる処理は、前記ハンド部で前記把持対象物を把持することができるように、前記ハンド部を前記把持対象物に向かって直線的に移動させること、が好ましい。ハンド部を把持対象物に向かって直線的に移動させるので、距離測定部から出射されるレーザーや超音波などが把持対象物を良好に捉えることができる。
【０００９】
特に、前記ハンド部の移動方向と、前記ハンド部の掌面の法線方向とを、略一致させた状態で、前記ハンド部を前記把持対象物に向かって直線的に移動させること、が好ましい。ハンド部の移動方向と、ハンド部の掌面の法線方向とを、略一致させた状態で、ハンド部を把持対象物に向かって移動させるので、ハンド部を閉じると、ハンド部で把持対象物を良好に把持することができる。
【００１０】
前記ハンド部と前記把持対象物との距離を推定させる処理は、前記距離測定部が測定不能となる直前の前記ハンド部と前記把持対象物との距離から、前記ロボットアームの関節部の角度に基づいて算出させた、前記距離測定部が測定不能となった後の前記ハンド部の移動距離を差分させて、前記ハンド部と前記把持対象物との距離を推定させる処理と、一つ前の処理で推定させた前記ハンド部と前記把持対象物との距離から、前記ロボットアームの関節部の角度に基づいて算出させた、推定後の前記ハンド部の移動距離を差分させて、前記ハンド部と前記把持対象物との距離を推定させる処理と、を備えること、が好ましい。結果として、距離測定部が測定したハンド部と把持対象物との距離に基づいて、距離測定部が測定不能となった後のハンド部と把持対象物との距離を推定するので、精度良くハンド部と把持対象物との距離を把握することができる。
【００１１】
本発明に係るロボットアームの制御システムは、把持対象物を把持するハンド部を有するロボットアームと、前記ロボットアームの関節部の角度を測定する角度測定部と、前記ロボットアームのハンド部に設けられ、前記ハンド部から前記把持対象物までの距離を測定する距離測定部と、前記ハンド部を前記把持対象物に向かって移動させるべく、前記ロボットアームを制御する演算処理部と、を備え、前記演算処理部は、前記距離測定部からの入力が途絶えると、前記入力が途絶える直前の前記ハンド部から前記把持対象物までの距離と、前記角度測定部から入力される前記ロボットアームの関節部の角度から算出した前記ハンド部の移動距離と、に基づいて、前記ハンド部と前記把持対象物との距離を推定し、推定した前記ハンド部と前記把持対象物との距離が閾値以下となると、前記ハンド部を前記把持対象物に向かって移動させる動作を停止させる。これにより、ハンド部で把持対象物を把持する際に、ハンド部が把持対象物に対して遠すぎたり、近すぎたりすることなく、適切な位置にハンド部を停止させることができる。
【００１２】
前記ハンド部は掌面を備え、前記ロボットアーム制御部は、前記ハンド部の移動方向と、前記ハンド部の掌面の法線方向とを、略一致させた状態で、前記ハンド部を前記把持対象物に向かって直線的に移動させること、が好ましい。ハンド部の移動方向と、ハンド部の掌面の法線方向とを、略一致させた状態で、ハンド部を把持対象物に向かって移動させるので、ハンド部を閉じると、ハンド部で把持対象物を良好に把持することができる。
【発明の効果】
【００１３】
以上、説明したように、本発明によると、ハンド部を適切な位置に停止させることができるロボットアームの制御プログラム及び制御システムを提供することができる。
【図面の簡単な説明】
【００１４】
【図１】本発明に係るロボットアームの制御システムを示すブロック図である。
【図２】ハンド部と把持対象物との配置を説明する図である。
【図３】ロボットアームのリーチ区間及びアプローチ区間を説明する図である。
【図４】本発明に係るロボットアームの制御プログラムの流れを示すフローチャートである。
【発明を実施するための形態】
【００１５】
以下、本発明を実施するための最良の形態について、添付図面を参照しながら説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【００１６】
ロボットアームの制御システム（以下、単に制御システムという場合がある。）１は、図１に示すように、ロボットアーム１１、演算処理部１２、角度測定部１３、距離測定部１４を備える。ここで、演算処理部１２は、典型的なコンピュータの演算処理部である。演算処理部１２は、物体位置姿勢推定部１２１、アーム軌道生成部１２２、ロボットアーム制御部１２３、距離推定部１２４を備える。
【００１７】
ロボットアーム１１は、多関節ロボットアームである。ロボットアーム１１の端部には、図２に示すように、把持対象物２１を把持するハンド部１１１が回転可能に連結されている。ハンド部１１１は掌面１１１ａを備え、当該掌面１１１ａを挟んで両側に把持部１１１ｂを備える。
【００１８】
物体位置姿勢推定部１２１は、ステレオカメラ等の撮像部１５が撮像した画像情報に基づいて、把持対象物２１までの距離、把持対象物２１の形状、姿勢などを推定し、位置・姿勢情報を生成する。物体位置姿勢推定部１２１は、当該位置・姿勢情報をアーム軌道生成部１２２に出力する。ちなみに、撮像部１５は、物体位置姿勢推定部１２１から入力される画像取得指令信号に基づいて制御される。
【００１９】
アーム軌道生成部１２２は、当該位置・姿勢情報に基づいて、例えばロボットアーム１１の端部に連結されたハンド部１１１で把持対象物２１を良好に把持させることができるように、ロボットアーム１１の制御信号を生成する。すなわち、ロボットアームの制御には、図３に示すように、リーチ区間とアプローチ区間とに区別されている。アプローチ区間でハンド部１１１を把持対象物２１に対してどの方向から近付け、ハンド部１１１で把持対象物２１を把持するかを定義付けした複数のパターンが、例えば図示を省略したコンピュータの記憶部に格納されている。当該パターンは、アプローチ区間において、ハンド部１１１で把持対象物２１を良好に把持することができるように、ハンド部１１１を把持対象物２１に向かって直線的に移動させる。すなわち、当該パターンは、図２に示すように、ハンド部１１１の移動方向と、ハンド部１１１の掌面１１１ａの法線Ｌ方向とを、略一致させた状態で、ハンド部１１１を把持対象物２１に向かって直線的に移動させる。
【００２０】
アーム軌道生成部１２２は、当該位置・姿勢情報に基づいて、複数のパターンのうちいずれのパターンを実行させるかを選択し、当該パターンをアプローチ区間で実行するために、リーチ区間でのロボットアーム１１の制御信号を生成する。アーム軌道生成部１２２は、当該制御信号及び当該選択したパターン情報をロボットアーム制御部１２３に出力する。
【００２１】
ロボットアーム制御部１２３は、当該制御信号に基づいて、ロボットアーム１１を制御する。このとき、ロボットアーム制御部１２３には、角度測定部１３からロボットアーム１１の関節部の角度情報が入力される。ロボットアーム制御部１２３は、当該角度情報を取得しつつ、リーチ区間においてロボットアーム１１を当該制御信号に基づいて制御する。ロボットアーム制御部１２３は、当該角度情報を距離推定部１２４に出力する。
【００２２】
ロボットアーム制御部１２３は、リーチ区間からアプローチ区間にハンド部１１１が侵入すると、当該選択したパターン情報に基づいて、ロボットアーム１１を制御する。このとき、ロボットアーム制御部１２３は、距離推定部１２４から停止指令信号が入力されると、当該選択したパターン情報に基づくロボットアーム１１の制御を停止する。
【００２３】
角度測定部１３は、ロボットアーム１１の各関節部に配置されており、関節部の回転角度を測定する。角度測定部１３は、測定した関節部の角度情報をロボットアーム制御部１２３に出力する。角度測定部１３としては、例えばエンコーダ等の角度検出手段を用いることができる。角度測定部１３は、ロボットアーム制御部１２３から入力される角度測定指令信号に基づいて制御される。
【００２４】
距離測定部１４は、ハンド部１１１に設けられている。例えば距離測定部１４は、ハンド部１１１の把持部１１１ｂに設けられる。但し、ハンド部１１１がアプローチ区間を移動する際に、把持対象物２１までの距離を良好に測定できる部位に距離測定部１４が設けられていれば良い。距離測定部１４は、把持対象物２１までの距離を測定し、測定した距離情報を距離推定部１２４に出力する。距離測定部１４としては、例えばレーザーや超音波などを用いた一般的な測距センサを用いることができる。上述のようにアプローチ区間においては、ハンド部１１１を把持対象物２１に向かって直線的に移動させるので、距離測定部１４から出射されるレーザーや超音波などが把持対象物２１を良好に捉えることができる。距離測定部１４は、距離推定部１２４から入力される距離測定指令信号に基づいて制御される。
【００２５】
距離推定部１２４は、ハンド部１１１と把持対象物２１との距離を推定する。すなわち、ハンド部１１１がアプローチ区間において、ハンド部１１１が把持対象物２１に近付き、距離測定部１４が把持対象物２１までの距離を測定すると、入力される距離情報に基づいて、ハンド部１１１と把持対象物２１との距離を算出する。そして、ハンド部１１１がさらに把持対象物２１に近付き、距離測定部１４が把持対象物２１までの距離を測定できなくなると、距離推定部１２４は、入力されるロボットアーム１１の関節部の角度情報やロボットアーム１１を成すアームの長さ等に基づいて、距離測定部１４で測定不能となった位置からのハンド部１１１の移動距離を算出する。つまり、距離推定部１２４は、ロボットアーム制御部１２３から当該ロボットアーム１１の関節部の角度情報を取得する。また、距離推定部１２４は、コンピュータの記憶部に格納されているロボットアーム１１を成すアームの長さ等の情報を取得する。
【００２６】
さらに距離推定部１２４は、距離測定部１４で測定不能となった際の距離情報から、算出したハンド部１１１の移動距離を差分し、ハンド部１１１と把持対象物２１との距離を推定する。その後、距離推定部１２４は、推定したハンド部１１１と把持対象物２１との距離から、推定後のハンド部１１１の移動距離を差分することを所定の周期で繰り返し、最終的に推定したハンド部１１１と把持対象物２１との距離が予め設定された閾値以下になったか否かを判断する。距離推定部１２４は、ハンド部１１１と把持対象物２１との距離が閾値以下になっていると、当該選択したパターン情報に基づくロボットアーム１１の制御を停止させるべく、停止指令信号を生成し、ロボットアーム制御部１２３に出力する。
【００２７】
このような制御システム１を用いてロボットアームの制御プログラム（以下、単に制御プログラムという場合がある。）は、図４に示すように実施される。
先ず、ロボットアーム制御部１２３は、アーム軌道生成部１２２で生成された制御信号に基づいて、リーチ区間においてロボットアーム１１を制御し、当該ロボットアーム１１の端部に連結されたハンド部１１１を、把持対象物２１に向かって移動させる（Ｓ１）。
【００２８】
次に、ロボットアーム制御部１２３は、アーム軌道生成部１２２が選択したパターンに基づいて、アプローチ区間においてロボットアーム１１を制御し、当該ロボットアーム１１のハンド部１１１を、把持対象物２１に向かって移動させる（Ｓ２）。これに伴い、ハンド部１１１は把持対象物２１に近付き、距離測定部１４で把持対象物２１までの距離が測定可能になる。ここで、距離測定部１４は、当該指令信号に基づいて、把持対象物２１までの距離を測定し、当該距離情報を距離推定部１２４に出力する（Ｓ３）。Ｓ３の工程を所定の周期で繰り返し、距離推定部１２４は、距離測定部１４が把持対象物２１に近付き、距離測定部１４で把持対象物２１までの距離が測定不能となる不感帯に当該距離測定部１４が入ったか否かを判定する（Ｓ４）。
【００２９】
距離推定部１２４は、距離測定部１４が不感帯に入っていないと判断すると、距離測定部１４から距離情報を取得し続ける。一方、距離推定部１２４は、距離測定部１４が不感帯に入ったと判断すると、距離測定部１４が不感帯に入った直後からのハンド部１１１の移動距離を算出する。すなわち、距離推定部１２４は、当該ロボットアーム１１の関節部の角度情報やロボットアーム１１を成すアームの長さ等に基づいて、距離測定部１４が不感帯に入った直後からのハンド部１１１の移動距離を算出する。そして、距離推定部１２４は、距離測定部１４が不感帯に入る直前の距離情報から、算出したハンド部１１１の移動距離を差分し、ハンド部１１１と把持対象物２１との距離を推定する（Ｓ５）。ちなみに、距離推定部１２４は、距離測定部１４からの距離情報の入力が途絶えると、距離測定部１４が不感帯に入ったと判断することができる。したがって、距離測定部１４が不感帯に入る直前の距離情報とは、最後に入力された距離情報である。
【００３０】
距離推定部１２４は、当該推定したハンド部１１１と把持対象物２１との距離から、推定後のハンド部１１１の移動距離を差分する。このように距離推定部１２４は、一つ前の工程で推定したハンド部１１１と把持対象物２１との距離から、推定後のハンド部１１１の移動距離を差分し、ハンド部１１１と把持対象物２１との距離を推定することを所定の周期で繰り返す（Ｓ６）。
【００３１】
距離推定部１２４は、Ｓ６の工程において推定したハンド部１１１と把持対象物２１との距離が閾値以下となったか否かを判断する（Ｓ７）。距離推定部１２４は、ハンド部１１１と把持対象物２１との距離が閾値以下となったと判断すると、当該選択したパターン情報に基づくロボットアーム１１の制御を停止させるべく、停止指令信号を生成し、ロボットアーム制御部１２３に出力する（Ｓ８）。一方、距離推定部１２４は、ハンド部１１１と把持対象物２１との距離が閾値より大きいと判断すると、Ｓ５の工程に戻る。
【００３２】
ロボットアーム制御部１２３は、距離推定部１２４から停止指令信号が入力されると、当該選択したパターン情報に基づくロボットアーム１１の制御を停止させる（Ｓ９）。
このように、距離測定部１４が不感帯に入っても、結果として距離測定部１４が測定したハンド部１１１と把持対象物２１との距離に基づいて、距離測定部１４が測定不能となった後のハンド部１１１と把持対象物２１との距離を推定するので、精度良くハンド部１１１と把持対象物２１との距離を把握することができる。そのため、ハンド部１１１で把持対象物２１を把持する際に、ハンド部１１１が把持対象物２１に対して遠すぎたり、近すぎたりすることなく、適切な位置にハンド部１１１を停止させることができる。ここで、アプローチ区間においては、ハンド部１１１の移動方向と、ハンド部１１１の掌面の法線Ｌ方向とを、略一致させた状態で、ハンド部１１１を把持対象物２１に向かって移動させたので、ハンド部１１１の把持部１１１ｂを閉じると、ハンド部１１１で把持対象物２１を良好に把持することができる。
【００３３】
以上、本発明に係るロボットアームの制御プログラム及び制御システムの実施形態を説明したが、上述した内容に限らず、本発明の技術的思想を逸脱しない範囲で、変更することが可能である。
上記の実施形態では、リーチ区間とアプローチ区間とに分けたが、ハンド部１１１を把持対象物２１の近傍に移動させるまで、アーム軌道生成部１２２がロボットアーム１１の制御信号を生成し続け、当該制御信号に基づいて、ロボットアーム制御部１２３がロボットアーム１１を制御しても良い。
上記の実施形態では、掌面１１１ａを備えたハンド部１１１であったが、掌面１１１ａを備えないハンド部を用いても良い。この場合、上述したようにハンド部１１１で把持対象物２１を良好に把持することができるように、ロボットアーム１１が制御される。
【符号の説明】
【００３４】
１ 制御システム
１１ ロボットアーム
１２ 演算処理部、１２１ 物体位置姿勢推定部、１２２ アーム軌道生成部、１２３ ロボットアーム制御部、１２４ 距離推定部
１３ 角度測定部
１４ 距離測定部
１５ 撮像部
２１ 把持対象物
１１１ ハンド部、１１１ａ 掌面、１１１ｂ 把持部
Ｌ 法線
【特許請求の範囲】
【請求項１】
ロボットアームの端部に連結されたハンド部を、把持対象物に向かって移動させる処理と、
前記ハンド部に設けられた距離測定部に、前記ハンド部と前記把持対象物との距離を測定させる処理と、
前記距離測定部が前記ハンド部と前記把持対象物との距離の測定が不能となると、測定不能となる直前の前記ハンド部と前記把持対象物との距離と、前記ロボットアームの関節部の角度から算出させた、測定不能となった後の前記ハンド部の移動距離と、に基づいて、前記ハンド部と前記把持対象物との距離を推定させる処理と、
前記推定させた前記ハンド部と前記把持対象物との距離が閾値以下となると、前記ハンド部を前記把持対象物に向かって移動させる動作を停止させる処理と、
をコンピュータに実行させるロボットアームの制御プログラム。
【請求項２】
前記ハンド部を前記把持対象物に向かって移動させる処理は、
前記ハンド部で前記把持対象物を把持することができるように、前記ハンド部を前記把持対象物に向かって直線的に移動させることを特徴とする請求項１に記載のロボットアームの制御プログラム。
【請求項３】
前記ハンド部の移動方向と、前記ハンド部の掌面の法線方向とを、略一致させた状態で、前記ハンド部を前記把持対象物に向かって直線的に移動させることを特徴とする請求項２に記載のロボットアームの制御プログラム。
【請求項４】
前記ハンド部と前記把持対象物との距離を推定させる処理は、
前記距離測定部が測定不能となる直前の前記ハンド部と前記把持対象物との距離から、前記ロボットアームの関節部の角度に基づいて算出させた、前記距離測定部が測定不能となった後の前記ハンド部の移動距離を差分させて、前記ハンド部と前記把持対象物との距離を推定させる処理と、
一つ前の処理で推定させた前記ハンド部と前記把持対象物との距離から、前記ロボットアームの関節部の角度に基づいて算出させた、推定後の前記ハンド部の移動距離を差分させて、前記ハンド部と前記把持対象物との距離を推定させる処理と、
を備えることを特徴とする請求項１に記載のロボットアームの制御プログラム。
【請求項５】
把持対象物を把持するハンド部を有するロボットアームと、
前記ロボットアームの関節部の角度を測定する角度測定部と、
前記ロボットアームのハンド部に設けられ、前記ハンド部から前記把持対象物までの距離を測定する距離測定部と、
前記ハンド部を前記把持対象物に向かって移動させるべく、前記ロボットアームを制御する演算処理部と、を備え、
前記演算処理部は、前記距離測定部からの入力が途絶えると、前記入力が途絶える直前の前記ハンド部から前記把持対象物までの距離と、前記角度測定部から入力される前記ロボットアームの関節部の角度から算出した前記ハンド部の移動距離と、に基づいて、前記ハンド部と前記把持対象物との距離を推定し、推定した前記ハンド部と前記把持対象物との距離が閾値以下となると、前記ハンド部を前記把持対象物に向かって移動させる動作を停止させるロボットアームの制御システム。
【請求項６】
前記ハンド部は掌面を備え、
前記演算処理部は、前記ハンド部の移動方向と、前記ハンド部の掌面の法線方向とを、略一致させた状態で、前記ハンド部を前記把持対象物に向かって直線的に移動させることを特徴とする請求項５に記載のロボットアームの制御システム。
【請求項１】
ロボットアームの端部に連結されたハンド部を、把持対象物に向かって移動させる処理と、
前記ハンド部に設けられた距離測定部に、前記ハンド部と前記把持対象物との距離を測定させる処理と、
前記距離測定部が前記ハンド部と前記把持対象物との距離の測定が不能となると、測定不能となる直前の前記ハンド部と前記把持対象物との距離と、前記ロボットアームの関節部の角度から算出させた、測定不能となった後の前記ハンド部の移動距離と、に基づいて、前記ハンド部と前記把持対象物との距離を推定させる処理と、
前記推定させた前記ハンド部と前記把持対象物との距離が閾値以下となると、前記ハンド部を前記把持対象物に向かって移動させる動作を停止させる処理と、
をコンピュータに実行させるロボットアームの制御プログラム。
【請求項２】
前記ハンド部を前記把持対象物に向かって移動させる処理は、
前記ハンド部で前記把持対象物を把持することができるように、前記ハンド部を前記把持対象物に向かって直線的に移動させることを特徴とする請求項１に記載のロボットアームの制御プログラム。
【請求項３】
前記ハンド部の移動方向と、前記ハンド部の掌面の法線方向とを、略一致させた状態で、前記ハンド部を前記把持対象物に向かって直線的に移動させることを特徴とする請求項２に記載のロボットアームの制御プログラム。
【請求項４】
前記ハンド部と前記把持対象物との距離を推定させる処理は、
前記距離測定部が測定不能となる直前の前記ハンド部と前記把持対象物との距離から、前記ロボットアームの関節部の角度に基づいて算出させた、前記距離測定部が測定不能となった後の前記ハンド部の移動距離を差分させて、前記ハンド部と前記把持対象物との距離を推定させる処理と、
一つ前の処理で推定させた前記ハンド部と前記把持対象物との距離から、前記ロボットアームの関節部の角度に基づいて算出させた、推定後の前記ハンド部の移動距離を差分させて、前記ハンド部と前記把持対象物との距離を推定させる処理と、
を備えることを特徴とする請求項１に記載のロボットアームの制御プログラム。
【請求項５】
把持対象物を把持するハンド部を有するロボットアームと、
前記ロボットアームの関節部の角度を測定する角度測定部と、
前記ロボットアームのハンド部に設けられ、前記ハンド部から前記把持対象物までの距離を測定する距離測定部と、
前記ハンド部を前記把持対象物に向かって移動させるべく、前記ロボットアームを制御する演算処理部と、を備え、
前記演算処理部は、前記距離測定部からの入力が途絶えると、前記入力が途絶える直前の前記ハンド部から前記把持対象物までの距離と、前記角度測定部から入力される前記ロボットアームの関節部の角度から算出した前記ハンド部の移動距離と、に基づいて、前記ハンド部と前記把持対象物との距離を推定し、推定した前記ハンド部と前記把持対象物との距離が閾値以下となると、前記ハンド部を前記把持対象物に向かって移動させる動作を停止させるロボットアームの制御システム。
【請求項６】
前記ハンド部は掌面を備え、
前記演算処理部は、前記ハンド部の移動方向と、前記ハンド部の掌面の法線方向とを、略一致させた状態で、前記ハンド部を前記把持対象物に向かって直線的に移動させることを特徴とする請求項５に記載のロボットアームの制御システム。
【図１】
【図２】
【図３】
【図４】
【図２】
【図３】
【図４】
【公開番号】特開２０１１−１０４７５６（Ｐ２０１１−１０４７５６Ａ）
【公開日】平成２３年６月２日（２０１１．６．２）
【国際特許分類】
【出願番号】特願２００９−２６５２９１（Ｐ２００９−２６５２９１）
【出願日】平成２１年１１月２０日（２００９．１１．２０）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２３年６月２日（２０１１．６．２）
【国際特許分類】
【出願日】平成２１年１１月２０日（２００９．１１．２０）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]