操作端制御システム及び操作端制御方法
【課題】ロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する操作端制御システム及び操作端制御方法を提供する。
【解決手段】操作端制御システムは、他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する。本発明による操作端制御システムは、前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定める目標位置設定手段１０１と、前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定める速度ベクトル設定手段１０３と、を備えている。
【解決手段】操作端制御システムは、他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する。本発明による操作端制御システムは、前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定める目標位置設定手段１０１と、前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定める速度ベクトル設定手段１０３と、を備えている。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する操作端制御システム及び操作端制御方法に関する。
【背景技術】
【０００２】
従来、縄跳びに使用される縄回し装置が開発されている（たとえば、特許文献１）。しかし、該縄回し装置は、測定値に基づいて縄の回転の制御を行う制御システムを備えていない。
【０００３】
したがって、何らかの外乱により縄の回転状態が変化した場合には、縄の回転を維持することができなくなる事態が生じうる。
【０００４】
本出願の時点で未公開の、本出願と同一出願人による、特願２００９−１２５８４１は、ロープの操作端の目標位置を実現するように、操作端に働く力の目標値を定め、力フィードバック制御を行う、ロープの回転の制御方法を記載している。しかし、この方法においては、ロープの回転中心軸の位置が変化し、一定の位置にとどまらなかった。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】実用新案公告昭６１−４３５０６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
したがって、外乱によりロープの回転状態が変化した場合であっても、その変化に応じて操作量を変化させ、ロープの回転を維持することができるようにし、ロープの回転中心軸の移動が少ない操作端制御システムに対するニーズがある。
【課題を解決するための手段】
【０００７】
本発明による操作端制御システムは、他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する。本発明による操作端制御システムは、前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定める目標位置設定手段と、前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定める速度ベクトル設定手段と、を備えている。
【０００８】
本発明による操作端制御方法は、他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する。本発明による操作端制御方法は、前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定めるステップと、前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定めるステップと、を含む。
【０００９】
本発明によれば、前記操作端の目標速度ベクトルの大きさを一定とし、前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に定めた目標位置に向かうように、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定めることによって、ロープの回転を動的に安定させ、またロープの回転中心軸の移動を少なくすることができる。
【００１０】
本発明の実施形態による操作端制御システムは、前記仮想円の前記中心を起点とした、前記操作端に作用する前記力のベクトルを想定し、前記ベクトルの方向から前記ロープの回転方向へ６０度乃至１２０度回転させたベクトルと前記仮想円との交点を前記目標位置とする。
【００１１】
本実施形態によれば、前記操作端に作用する力のベクトルから、容易に前記目標位置を定めることができる。
【００１２】
本発明の実施形態による操作端制御システムにおいて、前記操作端がロボットの部分である。
【００１３】
本実施形態によれば、ロボットによるロープ回し（縄回し）を安定的に行わせることができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施形態による操作端制御システムと、アームの操作端とを含むブロック図である。
【図２】縄と操作端との位置関係を示す図である。
【図３】操作端を含むアームの速度制御システムの構成の一例を示す図である。
【図４】操作端制御システムの構成を示す図である。
【図５】操作端に握持された縄を示す図である。
【図６】目標位置設定手段が、操作端の目標位置（Ｔｘ，Ｔｙ）を定める方法を示す流れ図である。
【図７】操作端が回転する平面を示す図である。
【図８】速度ベクトル設定手段が、速度ベクトルの目標値を定める方法を示す流れ図である。
【図９】仮想円と操作端の初期位置との関係を示す図である。
【図１０】操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係を示す図である。
【図１１】操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、初期位置からシミュレーションを行った場合について、仮想円の中心から、縄の質点の位置Ｍの投影面上への投影点までの距離と時間との関係を示す図である。
【図１２】操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、１個の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面上への投影点までの距離と時間との関係を示す図である。
【図１３】操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、他の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面上への投影点までの距離と時間との関係を示す図である。
【図１４】操作端の目標位置を実現するように、操作端の目標速度の代わりに操作端に働く力の目標値を定め、力フィードバック制御を行う制御方法を適用して、１個の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。
【発明を実施するための形態】
【００１５】
図１は、本発明の一実施形態による操作端制御システム１００と、アーム及びアームの操作端２００とを含むブロック図である。アーム及びアームの操作端２００は、たとえば、ロボットのアーム及びアームの操作端であってもよい。操作端は、ロープの一端を握持している。ロープの他端は、固定されているか、他のロボットのアーム又は人間に握持されていてもよい。ここで、ロープとは、織糸綱及び鋼索を含む。さらに、ロープは、所定の弾性を有する棒状体であってもよい。以下の実施形態において、ロープは縄とする。
【００１６】
操作端制御システム１００は、操作端の位置及び縄を介して操作端に作用する力を検出する。操作端制御システム１００は、これらの検出値を使用してアームの操作端に作用させる速度の目標値を定める。操作端制御システム１００の構成及び機能の詳細については後で説明する。
【００１７】
図２は、縄３００と操作端２０７１との位置関係を示す図である。操作端制御システム１００は、操作端２０７１が、縄回しの目標回転中心軸１６０に対して垂直に配置した投影面１５０上で移動するように制御を行う。
【００１８】
図３は、操作端を含むアームの速度制御システムの構成の一例を示す図である。速度制御システムは、速度フィードバック制御系２０１、モータドライバ２０３、モータ２０５、アーム２０７及びアームの速度検出システム２０９を含む。
【００１９】
速度フィードバック制御系２０１は、操作端制御システム１００から速度の目標値を受け取り、速度検出システム２０９からの速度の測定値を受け取り、速度の測定値が速度の目標値と一致するようにモータドライバ２０３の指令値を定める。速度フィードバック制御系２０１によるフィードバック制御は、たとえば、速度の測定値と速度の目標値との偏差に比例する量及び偏差を時間積分した量の和を出力とする比例積分制御であってもよい。モータドライバ２０３は、指令値にしたがってアーム駆動用モータ２０５を制御する。アーム２０７は、モータ２０５によって駆動される。アーム２０７は、アームの速度検出システム２０９を備えている。アームの速度検出システム２０９は、アームの関節に取り付けられたエンコーダによって測定されるアームの操作端位置を時間微分処理することによって、アームの操作端の速度（速度ベクトル）を検出してもよい。
【００２０】
図４は、操作端制御システム１００の構成を示す図である。操作端制御システム１００は、目標位置設定手段１０１と、速度ベクトル設定手段１０３と、を備える。
【００２１】
目標位置設定手段１０１は、力センサ１０５から、縄を経由して操作端に作用する力の測定値（ｆｘ，ｆｙ）を受け取る。目標位置設定手段１０１は、縄を経由して操作端に作用する力の測定値（ｆｘ，ｆｙ）から、操作端の目標位置（Ｔｘ，Ｔｙ）を定める。操作端の目標位置（Ｔｘ，Ｔｙ）の定め方については後で詳細に説明する。
【００２２】
図５は、アーム２０７の操作端２０７１に握持された縄３００を示す図である。縄３００は、本体３０１及び端部３０３からなり、端部３０３は、持ち手４００を介して、アーム２０７の操作端２０７１に握持される。縄の端部３０３は、持ち手４００の端部４０１にネジなどによって取り付けられる。持ち手４００の端部４０１と本体４０３との間に力センサ１０５が設置されている。力センサ１０５は、６軸力覚センサであってもよい。持ち手４００の本体４０３は、操作端２０７１に握持される。力センサ１０５は、縄を経由して操作端に作用する力を測定する。
【００２３】
図６は、目標位置設定手段１０１が、操作端の目標位置（Ｔｘ，Ｔｙ）を定める方法を示す流れ図である。
【００２４】
図７は、操作端２０７１が回転する平面を示す図である。この平面は、図２に示した投影面１５０に相当する。操作端２０７１の位置は、Ｅ及び１０１０１で示され、中心位置Ｏ及び１０１０３の周囲を回転する。操作端２０７１に作用する力のベクトルは、１０１０７で示される。該平面上に、中心位置１０１０３を中心とする仮想円１０１０５を定める。
【００２５】
図６のステップＳ０１０において、目標位置設定手段１０１は、力センサ１０５から、縄を経由して操作端に作用する力の測定値（ｆｘ，ｆｙ）を受け取り、力のベクトル１０１０７を求める。
【００２６】
図６のステップＳ０２０において、目標位置設定手段１０１は、力のベクトル１０１０７の始点を、操作端の回転の中心位置１０１０３とする。
【００２７】
図６のステップＳ０３０において、目標位置設定手段１０１は、平面内において力のベクトル１０１０７を回転方向に所定の角度だけ回転させる。所定の角度は、６０度乃至１２０度であるのが好ましい。図７において、所定の角度はθで示した。ここでは、θは９０度とした。
【００２８】
図６のステップＳ０４０において、目標位置設定手段１０１は、回転後のベクトルと仮想円との交点を目標位置とする。図７において、交点はＴ及び１０１１１で示した。
【００２９】
図４に戻り、速度ベクトル設定手段１０３は、位置測定手段１０７から、操作端の測定位置（Ｅｘ，Ｅｙ）（図７の１０１０１）を受け取り、目標位置設定手段１０１から、操作端の目標位置（Ｔｘ，Ｔｙ）（図７の１０１１１）を受け取る。位置測定手段１０７は、アームの関節に取り付けられたエンコーダであってもよい。速度ベクトル設定手段１０３は、操作端の測定位置（Ｅｘ，Ｅｙ）及び操作端の目標位置（Ｔｘ，Ｔｙ）から、アームの操作端の速度ベクトルの目標値
【数１】
を定める。
【数２】
の定め方については、以下に説明する。
【００３０】
図８は、速度ベクトル設定手段１０３が、アームの操作端の速度ベクトルの目標値
【数３】
を定める方法を示す流れ図である。
【００３１】
図８のステップＳ２１０において、速度ベクトル設定手段１０３は、操作端の測定位置１０１０１、目標位置１０１１１及び操作端の回転の中心位置１０１０３を取得する。
【００３２】
図８のステップＳ２２０において、速度ベクトル設定手段１０３は、図７に示される操作端の測定位置Ｅから図７に示される操作端の目標位置Ｔに向けた単位方向ベクトル
【数４】
を求める。つぎに、速度ベクトル設定手段１０３は、操作端の速度の大きさは一定値ｖであるとして、操作端の速度ベクトルの目標値を以下の式によって定める。
【数５】
【００３３】
つぎに、操作端と縄の運動モデルを導入する。操作対象物である縄を、長手方向片側へのみ弾性を有する自然長ｌのリンク及びそのリンクの先端にある質量ｍ、位置ベクトルＭの質点とし、操作端の位置ベクトルをＥとする。また、重力加速度ベクトルをＧ、弾性の有無を定める変数をδとする。操作端と縄の運動モデルは、以下の式で表せる。
【数６】
ここで、パラメータ及び時間ステップの値は以下の表１のとおりである。
【表１】
【００３４】
図９は、仮想円と操作端の初期位置との関係を示す図である。図９において、わかりやすくするため、仮想円の半径は１．０ｍとし、仮想円の周辺に３６個の点を格子状に定めた。操作端の初期位置は、３６個の点のいずれかの位置とする。
【００３５】
実際に、本シミュレーションにおいて、仮想円の半径は０．１５ｍとした。一般に、仮想円の半径を大きくすると、縄の回転中心軸を仮想円の中心へと制御する作用が減少する一方、縄の回転を維持する作用が増加する。仮想円の半径は、操作端の稼動範囲内で経験的に定める。また、縄に相当する質点は、最下点（仮想円の中心よりリンク長ｌだけ下方に移動した位置）より所速度２ｍ／ｓで円周方向に向けて打ち出されるとものとした。また、縄の他端は固定した。
【００３６】
このような条件において、操作端の目標速度を定めて速度フィードバック制御を行う制御方法を適用して、操作端の位置及び縄の質点の位置を、式（１）を使用したシミュレーションによって求めた。
【００３７】
図１０は、操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係を示す図である。上述のように、操作端２０７１は、図２の投影面１５０上を移動する。また、仮想円の中心は、投影面１５０上に位置する。図１０の横軸は、時間を示す。図１０の縦軸は、仮想円の中心から操作端位置までの距離を示す。図１０は、３６個の操作端の初期位置からシミュレーションを行った場合を全て示す。いずれの場合も、仮想円の中心から操作端位置までの距離は、ほぼ一定値に収束している。このことは、操作端２０７１の運動が、最終的に仮想円の中心を中心とする円運動となっていることを意味する。
【００３８】
図１１は、操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、初期位置からシミュレーションを行った場合について、仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。図１１の横軸は、時間を示す。図１１の縦軸は、仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離を示す。図１１は、３６個の操作端の初期位置からシミュレーションを行った場合を全て示す。いずれの場合も、仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離は、ほぼ一定値に収束している。このことは、縄の質点の位置Ｍの投影面１５０上への投影点の運動が、最終的に仮想円の中心を中心とする円運動となっていることを意味する。
【００３９】
図１２は、操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、１個の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。図１２の横軸は時間を示し、図１２の縦軸は距離を示す。操作端位置を点線で示し、縄の質点の位置Ｍの投影面１５０上への投影点の位置を実線で示す。
【００４０】
仮想円の中心から操作端及び縄の質点の位置Ｍの投影面１５０上への投影点までの距離は、ほぼ一定値に収束している。このことは、操作端及び縄の質点の位置Ｍの投影面１５０上への投影点の運動が、最終的に、ほぼ仮想円の中心を中心とする円運動となっていることを意味する。また、操作端の速度の大きさは一定値に制御されているので、操作端及び縄の質点の位置Ｍの運動は等速円運動である。
【００４１】
図１３は、操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、他の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。図１３の横軸は時間に対応するシムレーションのステップ数を示し、図１３の縦軸は距離を示す。操作端位置を点線で示し、縄の質点の位置Ｍの投影面１５０上への投影点の位置を実線で示す。
【００４２】
この場合も、仮想円の中心から操作端及び縄の質点の位置Ｍの投影面１５０上への投影点までの距離は、ほぼ一定値に収束している。このことは、操作端及び縄の質点の位置Ｍの投影面１５０上への投影点の運動が、最終的に、ほぼ仮想円の中心を中心とする円運動となっていることを意味する。また、操作端の速度の大きさは一定値に制御されているので、操作端及び縄の質点の位置Ｍの運動は等速円運動である。
【００４３】
図１４は、操作端の目標位置を実現するように、操作端の目標速度の代わりに操作端に働く力の目標値を定め、力フィードバック制御を行う制御方法を適用して、１個の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。図１４の横軸は時間に対応するシムレーションのステップ数を示し、図１４の縦軸は距離を示す。操作端位置を点線で示し、縄の質点の位置Ｍの投影面１５０上への投影点の位置を実線で示す。
【００４４】
図１３と図１４を比較すると、図１３に示した、速度フィードバック制御を適用した場合は、操作端及び縄の回転中心位置が定まっているのに対し、図１４に示した、力フィードバック制御を適用した場合は、操作端及び縄の回転中心位置が定まっていない。このように、速度フィードバック制御を適用することにより、操作端及び縄の回転中心位置を、事前に任意に設定することのできる仮想円の中心位置に定めて、安定した制御を行うことができる。
【符号の説明】
【００４５】
１００…操作端制御システム、１０１…目標位置設定手段、１０３…速度ベクトル設定手段、１０５…力センサ
【技術分野】
【０００１】
本発明は、他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する操作端制御システム及び操作端制御方法に関する。
【背景技術】
【０００２】
従来、縄跳びに使用される縄回し装置が開発されている（たとえば、特許文献１）。しかし、該縄回し装置は、測定値に基づいて縄の回転の制御を行う制御システムを備えていない。
【０００３】
したがって、何らかの外乱により縄の回転状態が変化した場合には、縄の回転を維持することができなくなる事態が生じうる。
【０００４】
本出願の時点で未公開の、本出願と同一出願人による、特願２００９−１２５８４１は、ロープの操作端の目標位置を実現するように、操作端に働く力の目標値を定め、力フィードバック制御を行う、ロープの回転の制御方法を記載している。しかし、この方法においては、ロープの回転中心軸の位置が変化し、一定の位置にとどまらなかった。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】実用新案公告昭６１−４３５０６号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
したがって、外乱によりロープの回転状態が変化した場合であっても、その変化に応じて操作量を変化させ、ロープの回転を維持することができるようにし、ロープの回転中心軸の移動が少ない操作端制御システムに対するニーズがある。
【課題を解決するための手段】
【０００７】
本発明による操作端制御システムは、他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する。本発明による操作端制御システムは、前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定める目標位置設定手段と、前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定める速度ベクトル設定手段と、を備えている。
【０００８】
本発明による操作端制御方法は、他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する。本発明による操作端制御方法は、前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定めるステップと、前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定めるステップと、を含む。
【０００９】
本発明によれば、前記操作端の目標速度ベクトルの大きさを一定とし、前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に定めた目標位置に向かうように、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定めることによって、ロープの回転を動的に安定させ、またロープの回転中心軸の移動を少なくすることができる。
【００１０】
本発明の実施形態による操作端制御システムは、前記仮想円の前記中心を起点とした、前記操作端に作用する前記力のベクトルを想定し、前記ベクトルの方向から前記ロープの回転方向へ６０度乃至１２０度回転させたベクトルと前記仮想円との交点を前記目標位置とする。
【００１１】
本実施形態によれば、前記操作端に作用する力のベクトルから、容易に前記目標位置を定めることができる。
【００１２】
本発明の実施形態による操作端制御システムにおいて、前記操作端がロボットの部分である。
【００１３】
本実施形態によれば、ロボットによるロープ回し（縄回し）を安定的に行わせることができる。
【図面の簡単な説明】
【００１４】
【図１】本発明の一実施形態による操作端制御システムと、アームの操作端とを含むブロック図である。
【図２】縄と操作端との位置関係を示す図である。
【図３】操作端を含むアームの速度制御システムの構成の一例を示す図である。
【図４】操作端制御システムの構成を示す図である。
【図５】操作端に握持された縄を示す図である。
【図６】目標位置設定手段が、操作端の目標位置（Ｔｘ，Ｔｙ）を定める方法を示す流れ図である。
【図７】操作端が回転する平面を示す図である。
【図８】速度ベクトル設定手段が、速度ベクトルの目標値を定める方法を示す流れ図である。
【図９】仮想円と操作端の初期位置との関係を示す図である。
【図１０】操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係を示す図である。
【図１１】操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、初期位置からシミュレーションを行った場合について、仮想円の中心から、縄の質点の位置Ｍの投影面上への投影点までの距離と時間との関係を示す図である。
【図１２】操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、１個の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面上への投影点までの距離と時間との関係を示す図である。
【図１３】操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、他の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面上への投影点までの距離と時間との関係を示す図である。
【図１４】操作端の目標位置を実現するように、操作端の目標速度の代わりに操作端に働く力の目標値を定め、力フィードバック制御を行う制御方法を適用して、１個の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。
【発明を実施するための形態】
【００１５】
図１は、本発明の一実施形態による操作端制御システム１００と、アーム及びアームの操作端２００とを含むブロック図である。アーム及びアームの操作端２００は、たとえば、ロボットのアーム及びアームの操作端であってもよい。操作端は、ロープの一端を握持している。ロープの他端は、固定されているか、他のロボットのアーム又は人間に握持されていてもよい。ここで、ロープとは、織糸綱及び鋼索を含む。さらに、ロープは、所定の弾性を有する棒状体であってもよい。以下の実施形態において、ロープは縄とする。
【００１６】
操作端制御システム１００は、操作端の位置及び縄を介して操作端に作用する力を検出する。操作端制御システム１００は、これらの検出値を使用してアームの操作端に作用させる速度の目標値を定める。操作端制御システム１００の構成及び機能の詳細については後で説明する。
【００１７】
図２は、縄３００と操作端２０７１との位置関係を示す図である。操作端制御システム１００は、操作端２０７１が、縄回しの目標回転中心軸１６０に対して垂直に配置した投影面１５０上で移動するように制御を行う。
【００１８】
図３は、操作端を含むアームの速度制御システムの構成の一例を示す図である。速度制御システムは、速度フィードバック制御系２０１、モータドライバ２０３、モータ２０５、アーム２０７及びアームの速度検出システム２０９を含む。
【００１９】
速度フィードバック制御系２０１は、操作端制御システム１００から速度の目標値を受け取り、速度検出システム２０９からの速度の測定値を受け取り、速度の測定値が速度の目標値と一致するようにモータドライバ２０３の指令値を定める。速度フィードバック制御系２０１によるフィードバック制御は、たとえば、速度の測定値と速度の目標値との偏差に比例する量及び偏差を時間積分した量の和を出力とする比例積分制御であってもよい。モータドライバ２０３は、指令値にしたがってアーム駆動用モータ２０５を制御する。アーム２０７は、モータ２０５によって駆動される。アーム２０７は、アームの速度検出システム２０９を備えている。アームの速度検出システム２０９は、アームの関節に取り付けられたエンコーダによって測定されるアームの操作端位置を時間微分処理することによって、アームの操作端の速度（速度ベクトル）を検出してもよい。
【００２０】
図４は、操作端制御システム１００の構成を示す図である。操作端制御システム１００は、目標位置設定手段１０１と、速度ベクトル設定手段１０３と、を備える。
【００２１】
目標位置設定手段１０１は、力センサ１０５から、縄を経由して操作端に作用する力の測定値（ｆｘ，ｆｙ）を受け取る。目標位置設定手段１０１は、縄を経由して操作端に作用する力の測定値（ｆｘ，ｆｙ）から、操作端の目標位置（Ｔｘ，Ｔｙ）を定める。操作端の目標位置（Ｔｘ，Ｔｙ）の定め方については後で詳細に説明する。
【００２２】
図５は、アーム２０７の操作端２０７１に握持された縄３００を示す図である。縄３００は、本体３０１及び端部３０３からなり、端部３０３は、持ち手４００を介して、アーム２０７の操作端２０７１に握持される。縄の端部３０３は、持ち手４００の端部４０１にネジなどによって取り付けられる。持ち手４００の端部４０１と本体４０３との間に力センサ１０５が設置されている。力センサ１０５は、６軸力覚センサであってもよい。持ち手４００の本体４０３は、操作端２０７１に握持される。力センサ１０５は、縄を経由して操作端に作用する力を測定する。
【００２３】
図６は、目標位置設定手段１０１が、操作端の目標位置（Ｔｘ，Ｔｙ）を定める方法を示す流れ図である。
【００２４】
図７は、操作端２０７１が回転する平面を示す図である。この平面は、図２に示した投影面１５０に相当する。操作端２０７１の位置は、Ｅ及び１０１０１で示され、中心位置Ｏ及び１０１０３の周囲を回転する。操作端２０７１に作用する力のベクトルは、１０１０７で示される。該平面上に、中心位置１０１０３を中心とする仮想円１０１０５を定める。
【００２５】
図６のステップＳ０１０において、目標位置設定手段１０１は、力センサ１０５から、縄を経由して操作端に作用する力の測定値（ｆｘ，ｆｙ）を受け取り、力のベクトル１０１０７を求める。
【００２６】
図６のステップＳ０２０において、目標位置設定手段１０１は、力のベクトル１０１０７の始点を、操作端の回転の中心位置１０１０３とする。
【００２７】
図６のステップＳ０３０において、目標位置設定手段１０１は、平面内において力のベクトル１０１０７を回転方向に所定の角度だけ回転させる。所定の角度は、６０度乃至１２０度であるのが好ましい。図７において、所定の角度はθで示した。ここでは、θは９０度とした。
【００２８】
図６のステップＳ０４０において、目標位置設定手段１０１は、回転後のベクトルと仮想円との交点を目標位置とする。図７において、交点はＴ及び１０１１１で示した。
【００２９】
図４に戻り、速度ベクトル設定手段１０３は、位置測定手段１０７から、操作端の測定位置（Ｅｘ，Ｅｙ）（図７の１０１０１）を受け取り、目標位置設定手段１０１から、操作端の目標位置（Ｔｘ，Ｔｙ）（図７の１０１１１）を受け取る。位置測定手段１０７は、アームの関節に取り付けられたエンコーダであってもよい。速度ベクトル設定手段１０３は、操作端の測定位置（Ｅｘ，Ｅｙ）及び操作端の目標位置（Ｔｘ，Ｔｙ）から、アームの操作端の速度ベクトルの目標値
【数１】
を定める。
【数２】
の定め方については、以下に説明する。
【００３０】
図８は、速度ベクトル設定手段１０３が、アームの操作端の速度ベクトルの目標値
【数３】
を定める方法を示す流れ図である。
【００３１】
図８のステップＳ２１０において、速度ベクトル設定手段１０３は、操作端の測定位置１０１０１、目標位置１０１１１及び操作端の回転の中心位置１０１０３を取得する。
【００３２】
図８のステップＳ２２０において、速度ベクトル設定手段１０３は、図７に示される操作端の測定位置Ｅから図７に示される操作端の目標位置Ｔに向けた単位方向ベクトル
【数４】
を求める。つぎに、速度ベクトル設定手段１０３は、操作端の速度の大きさは一定値ｖであるとして、操作端の速度ベクトルの目標値を以下の式によって定める。
【数５】
【００３３】
つぎに、操作端と縄の運動モデルを導入する。操作対象物である縄を、長手方向片側へのみ弾性を有する自然長ｌのリンク及びそのリンクの先端にある質量ｍ、位置ベクトルＭの質点とし、操作端の位置ベクトルをＥとする。また、重力加速度ベクトルをＧ、弾性の有無を定める変数をδとする。操作端と縄の運動モデルは、以下の式で表せる。
【数６】
ここで、パラメータ及び時間ステップの値は以下の表１のとおりである。
【表１】
【００３４】
図９は、仮想円と操作端の初期位置との関係を示す図である。図９において、わかりやすくするため、仮想円の半径は１．０ｍとし、仮想円の周辺に３６個の点を格子状に定めた。操作端の初期位置は、３６個の点のいずれかの位置とする。
【００３５】
実際に、本シミュレーションにおいて、仮想円の半径は０．１５ｍとした。一般に、仮想円の半径を大きくすると、縄の回転中心軸を仮想円の中心へと制御する作用が減少する一方、縄の回転を維持する作用が増加する。仮想円の半径は、操作端の稼動範囲内で経験的に定める。また、縄に相当する質点は、最下点（仮想円の中心よりリンク長ｌだけ下方に移動した位置）より所速度２ｍ／ｓで円周方向に向けて打ち出されるとものとした。また、縄の他端は固定した。
【００３６】
このような条件において、操作端の目標速度を定めて速度フィードバック制御を行う制御方法を適用して、操作端の位置及び縄の質点の位置を、式（１）を使用したシミュレーションによって求めた。
【００３７】
図１０は、操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係を示す図である。上述のように、操作端２０７１は、図２の投影面１５０上を移動する。また、仮想円の中心は、投影面１５０上に位置する。図１０の横軸は、時間を示す。図１０の縦軸は、仮想円の中心から操作端位置までの距離を示す。図１０は、３６個の操作端の初期位置からシミュレーションを行った場合を全て示す。いずれの場合も、仮想円の中心から操作端位置までの距離は、ほぼ一定値に収束している。このことは、操作端２０７１の運動が、最終的に仮想円の中心を中心とする円運動となっていることを意味する。
【００３８】
図１１は、操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、初期位置からシミュレーションを行った場合について、仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。図１１の横軸は、時間を示す。図１１の縦軸は、仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離を示す。図１１は、３６個の操作端の初期位置からシミュレーションを行った場合を全て示す。いずれの場合も、仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離は、ほぼ一定値に収束している。このことは、縄の質点の位置Ｍの投影面１５０上への投影点の運動が、最終的に仮想円の中心を中心とする円運動となっていることを意味する。
【００３９】
図１２は、操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、１個の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。図１２の横軸は時間を示し、図１２の縦軸は距離を示す。操作端位置を点線で示し、縄の質点の位置Ｍの投影面１５０上への投影点の位置を実線で示す。
【００４０】
仮想円の中心から操作端及び縄の質点の位置Ｍの投影面１５０上への投影点までの距離は、ほぼ一定値に収束している。このことは、操作端及び縄の質点の位置Ｍの投影面１５０上への投影点の運動が、最終的に、ほぼ仮想円の中心を中心とする円運動となっていることを意味する。また、操作端の速度の大きさは一定値に制御されているので、操作端及び縄の質点の位置Ｍの運動は等速円運動である。
【００４１】
図１３は、操作端の速度目標値を定めて速度フィードバック制御を行う制御方法を適用して、他の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。図１３の横軸は時間に対応するシムレーションのステップ数を示し、図１３の縦軸は距離を示す。操作端位置を点線で示し、縄の質点の位置Ｍの投影面１５０上への投影点の位置を実線で示す。
【００４２】
この場合も、仮想円の中心から操作端及び縄の質点の位置Ｍの投影面１５０上への投影点までの距離は、ほぼ一定値に収束している。このことは、操作端及び縄の質点の位置Ｍの投影面１５０上への投影点の運動が、最終的に、ほぼ仮想円の中心を中心とする円運動となっていることを意味する。また、操作端の速度の大きさは一定値に制御されているので、操作端及び縄の質点の位置Ｍの運動は等速円運動である。
【００４３】
図１４は、操作端の目標位置を実現するように、操作端の目標速度の代わりに操作端に働く力の目標値を定め、力フィードバック制御を行う制御方法を適用して、１個の操作端の初期位置からシミュレーションを行った場合について、仮想円の中心から操作端位置までの距離と時間との関係及び仮想円の中心から縄の質点の位置Ｍの投影面１５０上への投影点までの距離と時間との関係を示す図である。図１４の横軸は時間に対応するシムレーションのステップ数を示し、図１４の縦軸は距離を示す。操作端位置を点線で示し、縄の質点の位置Ｍの投影面１５０上への投影点の位置を実線で示す。
【００４４】
図１３と図１４を比較すると、図１３に示した、速度フィードバック制御を適用した場合は、操作端及び縄の回転中心位置が定まっているのに対し、図１４に示した、力フィードバック制御を適用した場合は、操作端及び縄の回転中心位置が定まっていない。このように、速度フィードバック制御を適用することにより、操作端及び縄の回転中心位置を、事前に任意に設定することのできる仮想円の中心位置に定めて、安定した制御を行うことができる。
【符号の説明】
【００４５】
１００…操作端制御システム、１０１…目標位置設定手段、１０３…速度ベクトル設定手段、１０５…力センサ
【特許請求の範囲】
【請求項１】
他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する操作端制御システムであって、
前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定める目標位置設定手段と、
前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定める速度ベクトル設定手段と、を備えた操作端制御システム。
【請求項２】
前記仮想円の前記中心を起点とした、前記操作端に作用する前記力のベクトルを想定し、前記ベクトルの方向から前記ロープの回転方向へ６０度乃至１２０度回転させたベクトルと前記仮想円との交点を前記目標位置とする請求項１に記載の操作端制御システム。
【請求項３】
前記操作端がロボットの部分である請求項１又は２に記載の操作端制御システム。
【請求項４】
他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する操作端制御方法であって、
前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定めるステップと、
前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定めるステップと、を含む操作端制御方法。
【請求項５】
前記仮想円の前記中心を起点とした、前記操作端に作用する前記力のベクトルを想定し、前記ベクトルの方向から前記ロープの回転方向へ６０度乃至１２０度回転させたベクトルと前記仮想円との交点を前記目標位置とする請求項４に記載の操作端制御方法。
【請求項６】
前記操作端がロボットの部分である請求項４又は５に記載の操作端制御方法。
【請求項１】
他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する操作端制御システムであって、
前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定める目標位置設定手段と、
前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定める速度ベクトル設定手段と、を備えた操作端制御システム。
【請求項２】
前記仮想円の前記中心を起点とした、前記操作端に作用する前記力のベクトルを想定し、前記ベクトルの方向から前記ロープの回転方向へ６０度乃至１２０度回転させたベクトルと前記仮想円との交点を前記目標位置とする請求項１に記載の操作端制御システム。
【請求項３】
前記操作端がロボットの部分である請求項１又は２に記載の操作端制御システム。
【請求項４】
他端が保持されたロープの一端を保持した状態で、平面内で回転してロープを回す操作端を制御する操作端制御方法であって、
前記操作端の回転の中心位置を中心として定めた仮想円上において、前記ロープによって前記操作端に作用する力の方向よりも所定の角度だけ位相を進めた方向に前記操作端の目標位置を定めるステップと、
前記操作端の目標速度ベクトルの大きさを一定とし、前記目標速度ベクトルの向きが前記目標位置に向かうように、前記目標速度ベクトルを定めるステップと、を含む操作端制御方法。
【請求項５】
前記仮想円の前記中心を起点とした、前記操作端に作用する前記力のベクトルを想定し、前記ベクトルの方向から前記ロープの回転方向へ６０度乃至１２０度回転させたベクトルと前記仮想円との交点を前記目標位置とする請求項４に記載の操作端制御方法。
【請求項６】
前記操作端がロボットの部分である請求項４又は５に記載の操作端制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１１−５６１９９（Ｐ２０１１−５６１９９Ａ）
【公開日】平成２３年３月２４日（２０１１．３．２４）
【国際特許分類】
【出願番号】特願２００９−２１２４９９（Ｐ２００９−２１２４９９）
【出願日】平成２１年９月１４日（２００９．９．１４）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【公開日】平成２３年３月２４日（２０１１．３．２４）
【国際特許分類】
【出願日】平成２１年９月１４日（２００９．９．１４）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
[ Back to top ]