JP4290312B2 - Parallel mechanism, control method and control apparatus thereof - Google Patents

Parallel mechanism, control method and control apparatus thereof Download PDF

Info

Publication number
JP4290312B2
JP4290312B2 JP2000097913A JP2000097913A JP4290312B2 JP 4290312 B2 JP4290312 B2 JP 4290312B2 JP 2000097913 A JP2000097913 A JP 2000097913A JP 2000097913 A JP2000097913 A JP 2000097913A JP 4290312 B2 JP4290312 B2 JP 4290312B2
Authority
JP
Japan
Prior art keywords
motion platform
orientation
vector
sensor
movement
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.)
Expired - Fee Related
Application number
JP2000097913A
Other languages
Japanese (ja)
Other versions
JP2001282359A (en
Inventor
博善 岡崎
尚史 寺田
景範 梶原
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.)
Mitsubishi Precision Co Ltd
Original Assignee
Mitsubishi Precision Co 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 Mitsubishi Precision Co Ltd filed Critical Mitsubishi Precision Co Ltd
Priority to JP2000097913A priority Critical patent/JP4290312B2/en
Publication of JP2001282359A publication Critical patent/JP2001282359A/en
Application granted granted Critical
Publication of JP4290312B2 publication Critical patent/JP4290312B2/en
Anticipated expiration legal-status Critical
Expired - Fee Related legal-status Critical Current

Links

Images

Landscapes

  • Manipulator (AREA)
  • Transmission Devices (AREA)
  • Control Of Position Or Direction (AREA)

Description

【０００１】
【発明の属する技術分野】
本発明は、ロボットアーム、マニュピュレータ、工作機械、３次元入力装置 などに使用される多自由度パラレル機構と、その制御方法及び制御装置に関する。
【０００２】
【従来の技術】
一つの剛体の持つ運動の自由度は、並進運動が３、回転運動が３で合計６である。マニュピュレータや工作機械などでは、この自由度を得るため、直進したり、回転したりする関節を用いる。直進形関節と回転形関節とはそれぞれ、位置や力、角度やトルクを設定することができるようになっており、各関節にはモータなどのアクチュエータと位置や速度の検出器が配置され、その運動や力を制御できるようになっている。
【０００３】
従来、工作機械やマニュピュレータなどでは、対偶を介して直列に接続された複数のリンク部材によって作動部であるエンドエフェクタを支持し動作伝達が直列系列で進行するシリアル機構が使用されてきた。しかしながら、各対偶の運動を可能とさせるアクチュエータが直列に配置されるシリアル機構は、各アクチュエータの誤差が累積されてエンドエフェクタに影響を及ぼすという欠点を有していた。
【０００４】
これに対して、複数の並行に配置されたリンクセットによってエンドエフェクタである運動プラットフォームを支持しているパラレル機構では、各アクチュエータの誤差が平均化されるため、より安価なアクチュエータを使用して運動プラットフォームのより高精度の運動が可能になるという特徴を有している。
このため、近年、マニュピュレータや工作機械で、パラレル機構が使用され始めてきた。
【０００５】
パラレル機構は、ベースと、ベースに取り付けられた複数のリンクセットと、複数の並行に配置されたリンクセットに支持されたエンドエフェクタである運動プラットフォームとによって構成されており、各リンクセットは一般的には機構の有する運動の自由度（以下、単に自由度と称する）と等しい自由度を有している。すなわち、各リンクセットは、ベースとの結合部、運動プラットフォームとの結合部、及びリンクセットの中間部にそれぞれ配置された対偶と、これらの対偶の間にそれぞれ延びる複数のリンク部材とを含んでなり、各リンクセットにおける対偶の自由度の合計が機構の自由度と等しくなっている。さらに、各リンクセットは、そのリンクセット（詳細には、リンクセットの対偶）の自由度の内の一つに関する運動を可能とさせるアクチュエータをそれぞれ備えている。
【０００６】
マニュピュレータや工作機械などで使用されているパラレル機構は、一般的に、３自由度又は６自由度を有したものであり、例えば、６自由度を有したパラレル機構では、上述したように、６つのリンクセットによって運動プラットフォームが支持されたものが知られている。この運動プラットフォームを支持するリンクセットを独立して各々１自由度に関して動かすことによって、運動プラットフォームはリンクセットの数に等しい自由度の運動が可能となる。上記の６自由度パラレル機構では、自由度の総和が６となるように複数の対偶を備えた６つのリンクセットを独立して同時にアクチュエータで動かすことによって、軸方向の移動である位置に関する３自由度と軸まわりの回転である姿勢に関する３自由度とからなる６自由度の運動が可能になる。
【０００７】
リンクセットを駆動する方式は、リンクセットが伸縮する伸縮タイプ、リンクセットが屈曲する屈曲タイプ、リンクセットがベース上をスライドするスライドタイプとに大別される。
【０００８】
【発明が解決しようとする課題】
このような、パラレル機構では、運動プラットフォームの位置及び姿勢を定めても、リンクセットの位置及び姿勢、すなわちアクチュエータの出力が定まらなかったり、リンクセットの位置及び姿勢、すなわちアクチュエータの出力を定めても、運動プラットフォームの位置及び姿勢が定まらなくなることがある。すなわち、パラレル機構には、運動プラットフォームを支持しているリンクセットの対偶の移動量から演算によって求められる運動プラットフォームの位置及び姿勢が一意的に定まらず、制御不能になる特異点が存在する。
【０００９】
例えば、６自由度のパラレル機構を例にすると、前述したようにパラレル機構のリンクセットの対偶には全体として６つのセンサが設けられる（一般的にはセンサはアクチュエータによって駆動される能動対偶又はアクチュエータに取り付けられる）。このセンサからの出力値によって構成されるベクトルを（α1 ，α2 ，…，α6 ）とし、運動プラットフォームの位置及び姿勢を表すベクトルを（Ｐ1 ，Ｐ2 ，…，Ｐ6 ）とするとき、センサからの出力値と運動プラットフォームの位置及び姿勢を表すベクトルとの幾何学的な関係が次式で与えられるとする。
【００１０】
αm ＝ｆm （Ｐ1 ，Ｐ2 ，…，Ｐ6 ） （ｍ＝１，２，…，６）
従来は、これら６つの方程式を連立させて、Ｐ1 ，Ｐ2 ，…，Ｐ6 を直接的に求めていた。ところが、運動プラットフォームの位置及び姿勢がある状態にあるときに、上記の式が互いに独立ではなくなり、α1 ，α2 ，…，α6 からＰ1 ，Ｐ2 ，…，Ｐ6 が求まらなくなることがある。これがパラレル機構の特異点の問題の原因であった。
【００１１】
さらに詳細に説明すると、上記の式は一般的に非線形であり、Ｐ1 ，Ｐ2 ，…，Ｐ6 をα1 ，α2 ，…，α6 の陽関数として表すことができない。そこで、釣り合い状態からの微小変位を考えると、次式が成立する。
【００１２】
【数４】

Figure 0004290312
ここで、Δαm 及びΔＰm （ｍ＝１，２，…，６）はαm 及びＰm （ｍ＝１，２，…，６）の微小変位を表すものとする。
これをΔα＝ＤΔＰと表記すると、式
ΔＰ＝Ｄ-1Δα
によって、ΔＰが求められる。ところが、ある状態では、｜Ｄ｜＝０となり、ΔＰが不定となって特異点が生じることがあった。
【００１３】
パラレル機構のこのような特異点は、作業領域内に点として存在するのではなく、曲線又は曲面をなして存在するので、作業領域が分断される。そのため、従来は、パラレル機構の使用可能な作業領域を制限することによって、特異点が動作範囲内に含まれないようにし、特異点によって引き起こされる問題を回避していた。
【００１４】
さらに、リンクセットの対偶には対偶の移動量を検出してリンクセットの位置及び姿勢を検出するためにセンサが取り付けられるが、このセンサが誤差を有する場合やリンクセットと運動プラットフォーム又はベースとの結合部にがたが存在する場合、特異点付近では、これらの誤差やがたのために、運動プラットフォームが実際には特異点に位置していないのに、対偶の移動量から演算によって求められた運動プラットフォームの位置及び姿勢が特異点として誤認され、制御不能になる恐れがある。したがって、特異点のなす曲線又は曲面付近もまた動作範囲から除外される必要があり、パラレル機構の運動プラットフォームの動作範囲はより狭く限定されることになる。
【００１５】
したがって、本発明の目的は、従来技術に存する問題を解消して、パラレル機構が有する特異点付近における運動プラットフォームの挙動の安定性を改善させ、パラレル機構の動作範囲を広く確保することを可能にすることにある。
【００１６】
【課題を解決するための手段】
本発明は、上記目的に鑑み、ベースと、前記ベースに取り付けられた複数のリンクセットと、前記複数のリンクセットによって支持された運動プラットフォームと、前記ベースと前記リンクセットとの結合部、前記運動プラットフォームと前記リンクセットとの結合部、及び前記各リンクセットの中間部に設けられた対偶と、を備えた自由度Ｎを有するパラレル機構において、全体でＭ（＞Ｎ）自由度に関する前記対偶の運動を検出するために複数のセンサを設け、前記運動プラットフォームの仮定の位置及び姿勢から演算される前記センサを設けた各対偶の理論上の移動量と前記各センサによって実際に検出される前記センサを設けた各対偶の移動量との差を前記センサを設けた対偶の全てについて総合的に評価するための関数の値を最小とする仮定の位置及び姿勢を前記運動プラットフォームの実際の位置及び姿勢とすることにより、前記複数のセンサによって検出される前記センサを設けた各対偶の移動量から前記運動プラットフォームの実際の位置及び姿勢を推定するようにしたパラレル機構の制御方法を提供する。
【００１７】
さらに、本発明は、ベースと、前記ベースに取り付けられた複数のリンクセットと、前記複数のリンクセットによって支持された運動プラットフォームと、前記ベースと前記リンクセットとの結合部、前記運動プラットフォームと前記リンクセットとの結合部、及び前記各リンクセットの中間部に設けられた対偶と、を備えた自由度Ｎを有するパラレル機構のための制御装置であって、パラレル機構全体でＭ（＞Ｎ）自由度に関する前記リンクセットの前記対偶の運動を検出するために設けられた複数のセンサからの出力を受信する受信手段と、前記運動プラットフォームの仮定の位置及び姿勢から演算される前記センサを設けた各対偶の理論上の移動量と前記各センサによって実際に検出される前記センサを設けた各対偶の移動量との差を前記センサを設けた対偶の全てについて総合的に評価するための関数の値を最小にする仮定の位置及び姿勢を前記運動プラットフォームの実際の位置及び姿勢とすることにより、前記複数のセンサによって検出される前記センサを設けた各対偶の移動量から前記運動プラットフォームの実際の位置及び姿勢を推定する演算手段と、を備えるパラレル機構の制御装置を提供する。
【００１８】
また、本発明は、Ｎ自由度を有するパラレル機構であって、ベースと、前記ベースに取り付けられた複数のリンクセットと、前記複数のリンクセットによって支持された運動プラットフォームと、前記ベースと前記リンクセットとの結合部、前記運動プラットフォームと前記リンクセットとの結合部、及び前記各リンクセットの中間部に設けられた対偶と、全体でＭ（＞Ｎ）自由度に関する前記対偶の運動を検出するための複数のセンサと、前記複数のセンサによって検出される前記対偶の移動量から前記運動プラットフォームの実際の位置及び姿勢を推定するための制御装置と、を備えるパラレル機構を提供する。好適には、このパラレル機構で、前記制御装置が、前記運動プラットフォームの仮定の位置及び姿勢から求められる前記センサを設けた各対偶の理論上の移動量と実際に前記各センサによって検出される前記センサを設けられた各対偶の移動量との差を前記センサを設けた対偶の全てについて総合的に評価するための関数の値を最小とする仮定の位置及び姿勢を前記運動プラットフォームの実際の位置及び姿勢とすることにより、前記複数のセンサによって検出される前記センサを設けた各対偶の移動量から前記運動プラットフォームの実際の位置及び姿勢を推定するようにする。
【００１９】
好適には、本発明において、前記運動プラットフォームの実際の位置及び姿勢を表すベクトルを（Ｐ1 ，Ｐ2 ，…, ＰN ) 、前記位置及び姿勢において各センサによって検出される前記対偶の移動量によって構成されるベクトルを（α1 ，α2 ，…，αM ）、前記運動プラットフォームの仮定の位置及び姿勢を表すベクトルを（Ｐｆ1 ，Ｐｆ2 ，…，ＰｆN ）とし、
ベクトル（Ｐ1 ，Ｐ2 ，…, ＰN ) とベクトル（α1 ，α2 ，…，αM ）とが関数ベクトル（ｆ1 ，ｆ2 ，…，ｆM ）によって、式
αm ＝ｆm （Ｐ1 ，Ｐ2 ，…，ＰN ），（ｍ＝１，２，…，Ｍ）
で対応づけられているとき、前記評価するための関数を
【００２０】
【数５】
Figure 0004290312
と定義し、前記制御装置が、前記評価するための関数の値Ｅが最小となったときの前記ベクトル（Ｐｆ 1 ，Ｐｆ 2 ，…，Ｐｆ N ）を前記運動プラットフォームの実際の位置及び姿勢を表すベクトルと推定する。
パラレル機構では、センサの出力と運動プラットフォームの位置及び姿勢との幾何学的関係に基づいて定められる関係式を連立させて運動プラットフォームの位置及び姿勢を表すベクトル（Ｐ1 ，Ｐ2 ，…, ＰN ) を求めるにあたって、解を定めるためには運動プラットフォームの位置及び姿勢の自由度Ｎと等しい数の式を連立させて解く必要がある。しかしながら、Ｎ個の方程式が独立ではないときには、解を求めることができなくなる。
【００２１】
そこで、本発明は、パラレル機構の自由度より少なくとも一つ多いＭ（＞Ｎ）自由度に関する対偶の運動を検出するためにリンクセットの対偶にセンサを冗長に設け、Ｎ個の独立した関係式が得られる確率を高くしている。さらに、常にＭ個全ての関係式を使用して運動プラットフォームの位置及び姿勢を推定すべく、仮定された運動プラットフォームの位置及び姿勢から求められるＭ個の対偶の理論上の移動量と各センサによって実際に検出されているＭ個の対偶の移動量との差をセンサ全体として評価する関数を利用し、例えばこの関数の値が最も小さくなるときに、仮定された運動プラットフォームの位置及び姿勢が正しいと推定するようにしている。
【００２２】
したがって、従来のパラレル機構と比較して、特異点が発生する確率を低下させることができ、パラレル機構の作動を安定化させることが可能となる。また、Ｍ個のセンサ全ての出力を利用して総合的に運動プラットフォームの位置及び姿勢を推定するので、センサの誤差やリンクセットのがたによる誤認によって特異点近傍で動作が不安定になることも低減させることができる。
【００２３】
【発明の実施の形態】
以下、運動に関して６自由度を有するパラレル機構を例にして、本発明のパラレル機構とその制御方法及び制御装置の実施形態に関して図面を参照して説明する。しかしながら、本発明は６自由度を有するパラレル機構に限定されるものではなく、例えば、３自由度又はそれ以外の自由度を有するパラレル機構に対して適用することが可能である。
【００２４】
図１は、本発明の制御方法及び装置を適用したパラレル機構の実施形態として、屈曲タイプの６自由度パラレル機構を示す斜視図である。
本発明のパラレル機構１０は、ベース１２と、ベース１２に取り付けられている複数のリンクセット１４と、複数の並行に配置されたリンクセット１４によって支持されている運動プラットフォーム１６と、リンクセット１４を駆動するためのアクチュエータ１８と、リンクセット１４の運動を検出するためのセンサ２０、２２と、センサ２０、２２の検出値から運動プラットフォーム１６の位置及び姿勢を推定し且つアクチュエータ１８の作動を制御する制御装置２４とを備える。
【００２５】
アクチュエータ１８としては、ＤＣサーボモータやステップモータなどの回転モータ、リニアモータなどが使用され得る。
図１に示される各リンクセット１４は、アクチュエータ１８によって一つの回転軸線を中心として回転可能にベース１２に取り付けられている第一リンク部材２６と、第二リンク部材２８と、リンクセット１４の中間部すなわち第一リンク部材２６と第二リンク部材２８との間でそれらを回転可能に接続する２自由度の中間対偶３０と、第二リンク部材２８と運動プラットフォーム１６との結合部にてそれらを回転可能に接続する３自由度の先端対偶３２とを含んでなる。ベース１２と第一リンク部材２６との結合部に取り付けられており、１自由度の基端対偶（能動対偶）として機能するアクチュエータ１８と、２自由度の中間対偶３０と、３自由度の先端対偶３２とによって、各リンクセット１４は全体として６自由度の運動が可能になっている。
【００２６】
このように、一般的に、Ｎ自由度のパラレル機構においては、ベースとリンクセットとの結合部、運動プラットフォームとリンクセットとの結合部、及びリンクセットの中間部に、運動の自由度の総和がパラレル機構の自由度Ｎと等しくなるように、対偶が設けられている。しかしながら、各リンクセットの対偶の自由度の総和がパラレル機構の自由度Ｎを越えるように対偶が設けられていてもよい。
【００２７】
２自由度の中間対偶３０は例えば直交する２つの回転軸線を有するフックジョイント、ユニバーサルジョイントなどによって実現され、３自由度を有した先端対偶３２は例えばボールジョイントや多数の回転軸線を有した多軸ジョイントなどの高自由度継手手段によって実現される。もちろん、中間対偶３０及び先端対偶３２が他の適宜のタイプの対偶によって実現され得ることはいうまでもない。
【００２８】
基端対偶（アクチュエータ１８）、中間対偶３０、及び先端対偶３２の自由度は前述の実施形態に限定される必要はなく、自由度を任意の配分とすることができる。また、各対偶は回転軸線を有した回転対偶に限定される必要はない。例えば、伸縮タイプのパラレル機構であるスチュワートプラットフォームのように、中間対偶（能動対偶）を自由度１の伸縮タイプの対偶とし、基端対偶及び先端対偶をそれぞれ自由度２及び３の回転対偶として実現してもよい。伸縮タイプの対偶は例えば回転モータとボールネジを組み合わせることで実現され得る。
【００２９】
各リンクセット１４はより多くのリンク部材と対偶によって構成されることも可能である。
さらに、各リンクセット１４は、図１に示されるように、２つのリンクセット１４を組としてベース上に１２０度毎に配置され、３組のリンクセット１４の回転軸線が互いに６０度の角度をなしてもよい。また、各リンクセット１４が同一の構成をなしてもよい。
【００３０】
このように、各リンクセット１４の構成及び配置を対称的にすることによって、各対偶の移動量と運動プラットフォームとの幾何学的関係の導出を容易にする効果を奏し得る。
従来のパラレル機構においては、全体で、パラレル機構の自由度Ｎと等しい数のセンサが、詳細には、各リンクセットの対偶に１自由度の運動を与える総和でＮ個のアクチュエータにそれぞれ、設けられていた。
【００３１】
これに対して、本発明のパラレル機構１０においては、センサ２０、２２の総数がパラレル機構１０の自由度Ｎを越えるように、冗長なセンサ２２がさらに付加される（Ｎ自由度の運動を検出できるセンサは単体であってもＮ個のセンサとして数えるものとする）。すなわち、パラレル機構１０が有する自由度をＮとするとき、全体でＭ（＞Ｎ）自由度、すなわちＮより多くの自由度に関する対偶の運動を検出できるように複数のセンサ２０、２２が設けられる。
【００３２】
図１の実施形態においては、１自由度の能動対偶として機能する各アクチュエータ１８の回転運動を検出するための６つのセンサ２０と、各中間対偶３０の有する２つの回転軸線のうちの１つの回転軸線を中心とする回転運動を検出するための６つのセンサ２２との合計１２個のセンサが設けられている。
なお、上述したように、６自由度のパラレル機構の場合でも、図１のように１２個のセンサが設けられる必要はなく、少なくとも７つのセンサが設けられていればよい。しかしながら、運動プラットフォーム１６の位置及び姿勢を決定するために必要とされる数の独立した関係式が得られる確率を高めるために、パラレル機構１０の自由度Ｎと比較してより多くの自由度に関して対偶の運動を検出することができるよう、より多くのセンサを設けることが望ましい。
【００３３】
図２に示されるように、制御装置２４は、センサ２０、２２からの出力を受信する受信手段３４と、センサ２０、２２から受信したアクチュエータ１８又は各対偶３０、３２の移動量に基づいて演算を行う演算手段３６とを含んでなる。演算手段３６は、さらに、運動プラットフォーム位置及び姿勢演算手段３８と、運動プラットフォーム反力演算手段４０と、アクチュエータトルク演算手段４２とを含んでなる。
【００３４】
運動プラットフォーム位置及び姿勢演算手段３８は、仮定した運動プラットフォーム１６の位置及び姿勢に基づいて演算される各アクチュエータ１８又は各対偶３０、３２の理論上の移動量とセンサ２０、２２によって実際に検出されている各アクチュエータ１８又は各対偶３０、３２の移動量との矛盾がセンサ全体として最も少なくなるように運動プラットフォーム１６の位置及び姿勢を推定する。運動プラットフォーム反力演算手段４０は、センサ２０、２２によって検出された各アクチュエータ１８又は各対偶３０、３２の移動量からこうして推定される運動プラットフォーム１６の位置及び姿勢において運動プラットフォーム１６に加えるべき反力を演算し、その演算結果に基づいて、アクチュエータトルク演算手段４２が実際にアクチュエータ１８に発生させるべきトルクの大きさを演算する。この演算結果に基づいてアクチュエータ１８が駆動され、運動プラットフォーム１６の作動がなされる。
【００３５】
次に、図３のフローチャートを参照して、制御装置２４における演算の手順を詳述する。
以下、全体で対偶のＭ（＞Ｎ）自由度に関する運動を検出させるべく複数のセンサが設けられているＮ自由度のパラレル機構における制御装置の作動を説明する。なお、説明のため、文中、図１と類似の部分には同一の参照符号を付している。
【００３６】
運動プラットフォーム１６の実際の位置及び姿勢を表すベクトルをＰ：（Ｐ1 ，Ｐ2 ，…, ＰN ) 、その運動プラットフォーム１６の位置及び姿勢における各センサ２０、２２の検出値によって構成されるベクトルをα：（α1 ，α2 ，…，αM ）、センサ２０、２２の検出値から推定される運動プラットフォーム１６の位置及び姿勢を表すベクトルをＰｆ：（Ｐｆ1 ，Ｐｆ2 ，…，ＰｆN ）とする。
【００３７】
また、ベクトルαとベクトルＰとの関係は、関数ベクトルｆ：（ｆ1 ，ｆ2 ，…，ｆM ）を用いた次式（１）によって定められているものとする。
αm ＝ｆm （Ｐ1 ，Ｐ2 ，…，ＰN ），（ｍ＝１，２，…，Ｍ） （１）
ここで、関数ｆm はパラレル機構の幾何学的構造から求められる既知の関数である。
【００３８】
図１に示されるパラレル機構１０の場合、以下の説明でＭ＝１２、Ｎ＝６として考えればよい。
先ず、図３のステップ５０でベクトルαの各変数αm 、ベクトルＰの各変数Ｐn 、ベクトルＰｆの各変数Ｐｆn を初期化する。次に、ステップ５２で各センサ２０、２２からの出力値αm （ｍ＝１，２，…，Ｍ）を制御装置２４の受信手段３４によって受信する。
【００３９】
運動プラットフォーム位置及び姿勢演算手段３８では、運動プラットフォーム１６の位置及び姿勢を仮定し、仮定した位置及び姿勢における各アクチュエータ１８又は各対偶３０、３２の理論上の移動量を演算し、この演算によって求められた移動量とセンサ２０、２２によって実際に検出されている各アクチュエータ１８又は各対偶３０、３２の移動量との差をセンサ全体として評価するための関数の値が最も小さくなるように、センサ２０、２２によって検出された各アクチュエータ１８又は各対偶３０、３２の移動量から運動プラットフォーム１６の位置及び姿勢を推定する。
【００４０】
この評価するための関数Ｅを図３に示されるフローチャートでは次式で定義している。
【００４１】
【数６】
Figure 0004290312
ベクトルＰｆによって表される仮定された運動プラットフォーム１６の位置及び姿勢において出力されるべき各センサ２０、２２の検出値の組は式（１）からｆm （Ｐｆ1 ，Ｐｆ2 ，…，ＰｆN ）（ｍ＝１，２，…，Ｍ）によって表される。よって、上式（２）は、仮定された運動プラットフォーム１６の位置及び姿勢におけるこのセンサ２０、２２の理論上の検出値と実際のセンサ２０、２２の検出値との差の平方和をＭ個のセンサ２０、２２（Ｌ自由度に関する運動を検出できるセンサは単体であってもＬ個のセンサとして数えるものとする）の出力に関して求めていることになる。すなわち、評価するための関数Ｅは既知の関数である。
【００４２】
ここで、次式で表される関数ベクトルｇ（Ｐｆ）を定義する。
【００４３】
【数７】
Figure 0004290312
今、仮定された運動プラットフォーム１６の位置及び姿勢においては、式（３）で、ｇ（Ｐｆ）＝ｅとなるとする。ここで、ｅ：（ｅ1 ，ｅ2 ，…，ｅN ）である。
評価するための関数Ｅが最小となるときには、ｅ＝（０，０，…，０）となる。
【００４４】
したがって、
ｇ（Ｐｆ＋ｄＰｆ）＝０ （４）
が成立するｄＰｆを求めれば、ｇ（Ｐｆ）＝０を満足するＰｆを求めることができる。
【００４５】
【数８】
Figure 0004290312
が成立するので、式（４）及び（５）より、次式が導かれる。
【００４６】
【数９】
Figure 0004290312
関数ベクトルｆは既知であるから、式（３）より、関数ベクトルｇも既知となり、式（６）からｇ（Ｐｆ＋ｄＰｆ）＝０を満足するｄＰｆが求まる。
したがって、仮定されたＰｆに対し求められたｄＰｆを加算した、ベクトル（Ｐｆ＋ｄＰｆ）が評価するための関数Ｅを最小にする運動プラットフォーム１６の位置及び姿勢を表すベクトルとなる。
【００４７】
実際上の演算では、図３を参照して、ステップ５４で、式（２）に従って、ベクトルＰｆで表される仮定された運動プラットフォーム１６の位置及び姿勢における上記評価するための関数Ｅの値を求める。次に、ステップ５６で、ｇ（Ｐｆ）、すなわちベクトルｅを求める。次に、ステップ５８で演算した関数ｇ（Ｐｆ）の変数Ｐｆに関する偏微分関数の値から、ステップ６０で、式（６）に従って、ｄＰｆを求める。
【００４８】
ステップ６２では、ｄＰｆの絶対値が予め定められた定数ｃ以下となれば、Ｐｆは収束したと判定し、このときのＰｆを運動プラットフォーム１６の正しい位置及び姿勢ベクトルＰとみなす。この場合には、ステップ６４で、運動プラットフォーム反力演算手段４０によってこの位置及び姿勢ベクトルでの運動プラットフォーム１６の反力を演算し、求められた反力に基づいてアクチュエータトルク演算手段４２によってアクチュエータ１８で必要となるトルクを演算し、求められたトルクを出力するようにアクチュエータ１８に指令し、運動プラットフォームの新しい位置及び姿勢にて、ステップ５２〜ステップ６４が繰り返される。
【００４９】
一方、ステップ６２でｄＰｆの絶対値が定数ｃより大きければ、新しいＰｆとして（Ｐｆ＋ｄＰｆ）を用いて（ステップ６６）、ステップ５４で再度、評価するための関数Ｅの値を演算し、求められるｄＰｆが定数ｃ以下となるまで、ステップ５４〜ステップ６２を繰り返す。
上記の評価するための関数Ｅの式は一つの例であり、例えば、上記の式（２）のように各センサ２０、２２の誤差に関して均等に重み付けするのではなく、パラレル機構１０の幾何学的構造を考慮して各センサ２０、２２の誤差に関して任意の重み付けを行ってもよい。
【００５０】
以上、屈曲タイプの６自由度のパラレル機構１０を例にして、本発明を説明したが、当業者であれば、本発明が任意の構成を有するパラレル機構に対して適用が可能であること容易に理解されるであろう。
【００５１】
【発明の効果】
以上説明したように、本発明によれば、パラレル機構が有する運動の自由度を越える数のセンサを冗長に設けることで、特異点が発生する確率を減少させることができる。さらに、仮定された運動プラットフォームの位置及び姿勢における対偶の移動量と対偶の移動量を検出するためのセンサによって実際に検出されている移動量との矛盾をセンサ全体として評価するための関数を利用することで、求めようとする運動プラットフォームの位置及び姿勢ベクトルの変数の数以上の方程式を常に利用することができるようにしており、演算の効率化を図ると共にセンサの誤差やリンクセットのがたの影響を出にくくしている。この結果、パラレル機構において特異点が少なくとも減少して動作が安定化すると共に、より広い作動範囲を設定することが可能となる。
【図面の簡単な説明】
【図１】本発明による制御方法及び制御装置を適用した屈曲タイプの６自由度パラレル機構を示す斜視図である。
【図２】図１に示されるパラレル機構のブロック図である。
【図３】本発明のパラレル機構の制御方法を示すフローチャートである。
【符号の説明】
１０…パラレル機構
１２…ベース
１４…リンクセット
１６…運動プラットフォーム
１８…アクチュエータ又は対偶
２０…センサ
２２…センサ
２４…制御装置
３０…対偶
３２…対偶[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a multi-degree-of-freedom parallel mechanism used for a robot arm, a manipulator, a machine tool, a three-dimensional input device, etc., and a control method and control device therefor.
[0002]
[Prior art]
The degree of freedom of movement of a single rigid body is 3 for translational motion and 3 for rotational motion, for a total of 6. Manipulators and machine tools use joints that go straight or rotate to obtain this degree of freedom. Each of the straight joint and the rotary joint can be set in position, force, angle and torque, and each joint is provided with an actuator such as a motor and a position and speed detector. You can control movement and force.
[0003]
Conventionally, in a machine tool, a manipulator, and the like, a serial mechanism has been used in which an end effector that is an operation unit is supported by a plurality of link members connected in series via a pair, and operation transmission proceeds in a series. However, the serial mechanism in which the actuators that enable the movement of each pair are arranged in series has a drawback that the error of each actuator is accumulated to affect the end effector.
[0004]
On the other hand, in the parallel mechanism that supports the motion platform, which is the end effector, by a plurality of links arranged in parallel, the error of each actuator is averaged. It has the feature that the platform can move with higher precision.
For this reason, in recent years, parallel mechanisms have begun to be used in manipulators and machine tools.
[0005]
The parallel mechanism is composed of a base, a plurality of link sets attached to the base, and an exercise platform that is an end effector supported by a plurality of parallel-arranged link sets. Has a degree of freedom equal to the degree of freedom of movement of the mechanism (hereinafter simply referred to as the degree of freedom). That is, each link set includes a coupling portion with the base, a coupling portion with the motion platform, and a pair of members disposed at an intermediate portion of the link set, and a plurality of link members respectively extending between these pair of pairs. Therefore, the sum of the degrees of freedom of the even number in each link set is equal to the degree of freedom of the mechanism. Furthermore, each link set is provided with an actuator that enables movement with respect to one of the degrees of freedom of the link set (specifically, the pair of link sets).
[0006]
The parallel mechanism used in manipulators and machine tools generally has three or six degrees of freedom. For example, in a parallel mechanism having six degrees of freedom, as described above, It is known that an exercise platform is supported by six link sets. By independently moving the link sets that support the exercise platform, each with one degree of freedom, the exercise platform is capable of movement with a degree of freedom equal to the number of link sets. In the above 6-degree-of-freedom parallel mechanism, 3 links with respect to a position that is a movement in the axial direction can be obtained by independently and simultaneously moving 6 link sets having a plurality of pairs so that the sum of the degrees of freedom becomes 6. A 6-degree-of-freedom motion consisting of degrees and 3 degrees of freedom with respect to a posture that is rotation about an axis becomes possible.
[0007]
The system for driving the link set is roughly classified into an expansion / contraction type in which the link set expands / contracts, a bending type in which the link set bends, and a slide type in which the link set slides on the base.
[0008]
[Problems to be solved by the invention]
In such a parallel mechanism, even if the position and posture of the motion platform are determined, the position and posture of the link set, that is, the output of the actuator cannot be determined, or the position and posture of the link set, that is, the output of the actuator is determined. The position and posture of the exercise platform may be undefined. That is, in the parallel mechanism, there is a singular point where the position and posture of the motion platform obtained by calculation from the amount of movement of the link set that supports the motion platform is not uniquely determined and becomes uncontrollable.
[0009]
For example, taking a parallel mechanism of 6 degrees of freedom as an example, as described above, a total of six sensors are provided in a pair of parallel mechanism linksets (generally, sensors are active pairs or actuators driven by an actuator). To be attached). A vector composed of output values from this sensor is expressed as (α1, Α2, ..., α6) And a vector representing the position and orientation of the exercise platform (P1, P2, ..., P6), The geometric relationship between the output value from the sensor and the vector representing the position and orientation of the motion platform is given by the following equation.
[0010]
αm= Fm(P1, P2, ..., P6(M = 1, 2, ..., 6)
Conventionally, these six equations are combined and P1, P2, ..., P6Was directly demanding. However, when the position and posture of the exercise platform are in a certain state, the above equations are not independent of each other, and α1, Α2, ..., α6To P1, P2, ..., P6May not be found. This was the cause of the singularity problem of the parallel mechanism.
[0011]
More specifically, the above equation is generally non-linear and P1, P2, ..., P6Α1, Α2, ..., α6It cannot be expressed as an explicit function of Therefore, when considering a minute displacement from the balanced state, the following equation is established.
[0012]
[Expression 4]
Figure 0004290312
Where ΔαmAnd ΔPm(M = 1, 2,..., 6) is αmAnd PmIt is assumed that the minute displacement of (m = 1, 2,..., 6) is represented.
When this is expressed as Δα = DΔP, the equation
ΔP = D-1Δα
To obtain ΔP. However, in a certain state, | D | = 0, ΔP becomes indefinite, and a singular point may occur.
[0013]
Such a singular point of the parallel mechanism does not exist as a point in the work area, but forms a curve or a curved surface, so that the work area is divided. Therefore, conventionally, by limiting the work area in which the parallel mechanism can be used, the singular point is not included in the operation range, and the problem caused by the singular point is avoided.
[0014]
Further, a sensor is attached to the pair of the link set in order to detect the amount of movement of the pair and detect the position and posture of the link set. If this sensor has an error or the link set and the motion platform or base, When rattle is present at the joint, it can be obtained by calculation from the amount of movement of the even number near the singular point, because the motion platform is not actually located at the singular point. The position and posture of the exercise platform may be mistaken as a singular point and may become uncontrollable. Therefore, the curve or curved surface formed by the singularity also needs to be excluded from the operation range, and the operation range of the motion platform of the parallel mechanism is more narrowly limited.
[0015]
Therefore, the object of the present invention is to solve the problems existing in the prior art, improve the stability of the behavior of the motion platform near the singular point of the parallel mechanism, and ensure a wide operating range of the parallel mechanism. There is to do.
[0016]
[Means for Solving the Problems]
  In view of the above object, the present invention provides a base, a plurality of link sets attached to the base, a motion platform supported by the plurality of link sets, a coupling portion between the base and the link set, and the motion. In a parallel mechanism having a degree of freedom N, including a coupling portion between a platform and the link set, and a pair provided at an intermediate portion of each link set,At M(> N) on degrees of freedomThe even numberProvide multiple sensors to detect movement,in frontOf the movement platformHypotheticalCalculated from the position and orientationProvided a sensorThe theoretical movement amount of each kinematic pair and the actual detection by each sensorProvided a sensorThe difference between the movement amount of each kinematic pair and the sensorAbout all the even numberFunction to evaluateBy assuming the assumed position and orientation that minimizes the value ofSaidpluralSaid detected by a sensorProvided a sensorFrom the amount of movement of each kinematic pair,The actualProvided is a method for controlling a parallel mechanism that estimates a position and a posture.
[0017]
  The present invention further includes a base, a plurality of link sets attached to the base, a motion platform supported by the plurality of link sets, a coupling portion between the base and the link set, the motion platform, and the A control device for a parallel mechanism having a degree of freedom N, comprising a coupling portion with a link set, and a pair provided at an intermediate portion of each link set, the entire parallel mechanismAt M(> N) on degrees of freedomThe even number of the link setReceiving means for receiving outputs from a plurality of sensors provided for detecting movement;,in frontOf the movement platformHypotheticalCalculated from the position and orientationProvided a sensorThe theoretical movement amount of each kinematic pair and the actual detection by each sensorProvided a sensorThe difference between the movement amount of each kinematic pair and the sensorAbout all the even numberFunction to evaluateBy assuming the actual position and posture of the motion platform as the assumed position and posture that minimize the value ofThe abovepluralSaid detected by a sensorProvided a sensorFrom the amount of movement of each kinematic pair,The actualThere is provided a control device for a parallel mechanism, comprising a calculation means for estimating a position and an attitude.
[0018]
  The present invention also provides:A parallel mechanism having N degrees of freedom,A base, a plurality of link sets attached to the base, a motion platform supported by the plurality of link sets, a joint between the base and the link set, and a joint between the motion platform and the link set , And the even number provided in the middle part of each link set, and the wholeAt M(> N) on degrees of freedomThe even numberA plurality of sensors for detecting movement; andpluralSensorThe amount of movement of the even number detected byFrom the exercise platformThe actualA parallel mechanism comprising a control device for estimating a position and a posture is provided. Preferably, with this parallel mechanism, the control device is adapted toHypotheticalThe above calculated from the position and orientationProvided a sensorThe theoretical amount of movement of each kinematic pair and the actual amount detected by each sensorSensor providedThe difference between the movement amount of each kinematic pair and the sensorAbout all the even numberFunction to evaluateBy assuming the assumed position and orientation that minimizes the value ofSaidpluralSaid detected by a sensorProvided a sensorFrom the amount of movement of each kinematic pair,The actualEstimate position and orientation.
[0019]
  Preferably, in the present invention, a vector representing the actual position and posture of the motion platform is represented by (P1 , P2 , ..., PN )LeaveEach sensorThe amount of movement of the even number detected by(Α1 , Α2 , ..., αM ),in frontOf the movement platformHypotheticalA vector representing the position and orientation (Pf1 , Pf2 , ..., PfN )age,
  Vector (P1 , P2 , ..., PN ) And vector (α1 , Α2 , ..., αM ) And a function vector (f1 , F2 , ..., fM )
  αm = Fm (P1 , P2 , ..., PN ), (M = 1, 2,..., M)
  The function for the evaluation is
[0020]
[Equation 5]
Figure 0004290312
  And defineThe control device isThe value E of the function for evaluating is minimumThe vector (Pf 1 , Pf 2 , ..., Pf N ) As a vector representing the actual position and orientation of the motion platform.
  In the parallel mechanism, a vector (P) representing the position and posture of the motion platform by combining the relational expressions determined based on the geometric relationship between the output of the sensor and the position and posture of the motion platform.1 , P2 , ..., PN In order to determine the solution, it is necessary to solve a number of equations equal to the number of degrees of freedom N of the position and posture of the motion platform. However, when N equations are not independent, a solution cannot be obtained.
[0021]
Therefore, in the present invention, in order to detect the motion of the even number with respect to M (> N) degrees of freedom which is at least one greater than the degree of freedom of the parallel mechanism, redundant sensors are provided in the pair of the link set, and N independent relational expressions are provided. The probability of obtaining is increased. Further, in order to always estimate the position and posture of the motion platform using all M relational expressions, the theoretical movement amount of M pairs evenly determined from the assumed position and posture of the motion platform and each sensor A function that evaluates the difference between the actually detected movement amount of M pairs evenly as a whole sensor is used. For example, when the value of this function is the smallest, the assumed position and posture of the motion platform are correct. I am trying to estimate.
[0022]
Therefore, as compared with the conventional parallel mechanism, the probability that a singular point is generated can be reduced, and the operation of the parallel mechanism can be stabilized. Also, since the position and orientation of the motion platform are estimated comprehensively using the outputs of all M sensors, the operation becomes unstable near the singular point due to sensor error or misidentification due to link set play. Can also be reduced.
[0023]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, an embodiment of a parallel mechanism, a control method thereof, and a control apparatus of the present invention will be described with reference to the drawings, taking a parallel mechanism having six degrees of freedom as an example. However, the present invention is not limited to the parallel mechanism having 6 degrees of freedom, and can be applied to, for example, a parallel mechanism having 3 degrees of freedom or other degrees of freedom.
[0024]
FIG. 1 is a perspective view showing a bending type 6-degree-of-freedom parallel mechanism as an embodiment of a parallel mechanism to which the control method and apparatus of the present invention are applied.
The parallel mechanism 10 of the present invention includes a base 12, a plurality of link sets 14 attached to the base 12, a motion platform 16 supported by a plurality of parallel arranged link sets 14, and a link set 14. The actuator 18 for driving, the sensors 20 and 22 for detecting the movement of the link set 14, the position and posture of the movement platform 16 are estimated from the detection values of the sensors 20 and 22, and the operation of the actuator 18 is controlled. And a control device 24.
[0025]
As the actuator 18, a rotary motor such as a DC servo motor or a step motor, a linear motor, or the like can be used.
Each link set 14 shown in FIG. 1 includes a first link member 26, a second link member 28, and an intermediate link set 14 that are attached to the base 12 so as to be rotatable about an axis of rotation by an actuator 18. At a joint between the second link member 28 and the motion platform 16, and a two-degree-of-freedom intermediate pair 30 that rotatably connects the first link member 26 and the second link member 28. It comprises a tip pair 32 of 3 degrees of freedom that is rotatably connected. An actuator 18 that is attached to a joint between the base 12 and the first link member 26 and functions as a one-degree-of-freedom proximal pair (active pair), a two-degree-of-freedom intermediate pair 30, and a three-degree-of-freedom tip With the kinematic pair 32, each link set 14 is capable of movement with 6 degrees of freedom as a whole.
[0026]
As described above, in general, in a parallel mechanism with N degrees of freedom, the sum of the degrees of freedom of motion is added to the joint between the base and the link set, the joint between the motion platform and the link set, and the intermediate portion of the link set. Is provided so that is equal to the degree of freedom N of the parallel mechanism. However, the even number may be provided so that the sum of the degrees of freedom of each link set exceeds the degree of freedom N of the parallel mechanism.
[0027]
The two-degree-of-freedom intermediate pair 30 is realized by, for example, a hook joint having two rotation axes orthogonal to each other, a universal joint, or the like. It is realized by a high degree of freedom joint means such as a joint. Of course, it goes without saying that the intermediate pair 30 and the tip pair 32 can be realized by other appropriate types of pair.
[0028]
The degrees of freedom of the proximal pair (actuator 18), the intermediate pair 30, and the distal pair 32 need not be limited to the above-described embodiment, and the degrees of freedom can be arbitrarily distributed. Each pair does not need to be limited to a rotational pair having a rotation axis. For example, like the Stewart platform, which is a telescopic parallel mechanism, the middle pair (active pair) is a telescopic pair with one degree of freedom, and the base pair and the top pair are realized as rotating pairs with two and three degrees of freedom, respectively. May be. The telescopic type kinematic pair can be realized, for example, by combining a rotary motor and a ball screw.
[0029]
Each link set 14 can also be constituted by more link members and pairs.
Further, as shown in FIG. 1, each link set 14 is arranged on the base every two hundred degrees as a set of two link sets 14, and the rotation axes of the three sets of link sets 14 have an angle of 60 degrees with respect to each other. May be done. Further, each link set 14 may have the same configuration.
[0030]
In this way, by making the configuration and arrangement of each link set 14 symmetrical, an effect of facilitating the derivation of the geometric relationship between the movement amount of each pair and the motion platform can be achieved.
In the conventional parallel mechanism, a total number of sensors equal to the degree of freedom N of the parallel mechanism is provided in total, and more specifically, each of the N actuators is provided with a total that gives a motion of one degree of freedom to the even number of each link set. It was done.
[0031]
On the other hand, in the parallel mechanism 10 of the present invention, redundant sensors 22 are further added so that the total number of sensors 20 and 22 exceeds the degree of freedom N of the parallel mechanism 10 (detection of motion with N degrees of freedom). Even if it is a single sensor, it can be counted as N sensors). In other words, when the degree of freedom of the parallel mechanism 10 is N, a plurality of sensors 20 and 22 are provided so as to detect M (> N) degrees of freedom as a whole, that is, kinematic motion relating to more degrees of freedom than N. .
[0032]
In the embodiment of FIG. 1, six sensors 20 for detecting the rotational motion of each actuator 18 functioning as an active pair of one degree of freedom and one of the two rotation axes of each intermediate pair 30 are rotated. A total of twelve sensors are provided, including six sensors 22 for detecting rotational movement about the axis.
As described above, even in the case of a 6-degree-of-freedom parallel mechanism, it is not necessary to provide 12 sensors as shown in FIG. 1, and it is sufficient that at least 7 sensors are provided. However, in order to increase the probability of obtaining the number of independent relations required to determine the position and orientation of the motion platform 16, with respect to more degrees of freedom compared to the degree of freedom N of the parallel mechanism 10. It is desirable to provide more sensors so that kinematic movements can be detected.
[0033]
As shown in FIG. 2, the control device 24 calculates based on the receiving means 34 that receives the output from the sensors 20 and 22, and the amount of movement of the actuator 18 or each pair 30 and 32 received from the sensors 20 and 22. And calculating means 36 for performing the above. The calculation means 36 further includes an exercise platform position / posture calculation means 38, an exercise platform reaction force calculation means 40, and an actuator torque calculation means 42.
[0034]
The motion platform position / posture calculation means 38 is actually detected by the theoretical movement amount of each actuator 18 or each pair 30, 32 calculated based on the assumed position and posture of the motion platform 16 and the sensors 20, 22. The position and posture of the motion platform 16 are estimated so that the contradiction with the movement amount of each actuator 18 or each pair 30 and 32 is minimized as the whole sensor. The motion platform reaction force calculation means 40 is a reaction force to be applied to the motion platform 16 in the position and posture of the motion platform 16 thus estimated from the amount of movement of each actuator 18 or each pair 30, 32 detected by the sensors 20, 22. Based on the result of the calculation, the actuator torque calculation means 42 calculates the magnitude of the torque that the actuator 18 should actually generate. Based on the calculation result, the actuator 18 is driven, and the motion platform 16 is operated.
[0035]
Next, the calculation procedure in the control device 24 will be described in detail with reference to the flowchart of FIG.
Hereinafter, the operation of the control device in the parallel mechanism of N degrees of freedom in which a plurality of sensors are provided to detect the motion related to the even number of M (> N) degrees of freedom will be described. For the sake of explanation, the same reference numerals are assigned to parts similar to those in FIG.
[0036]
A vector representing the actual position and orientation of the exercise platform 16 is P: (P1, P2, ..., PN), A vector constituted by detection values of the sensors 20 and 22 in the position and posture of the motion platform 16 is expressed as α: (α1, Α2, ..., αM), A vector representing the position and orientation of the motion platform 16 estimated from the detection values of the sensors 20 and 22 is Pf: (Pf1, Pf2, ..., PfN).
[0037]
Further, the relationship between the vector α and the vector P is expressed by the function vector f: (f1, F2, ..., fM) Using the following equation (1).
αm= Fm(P1, P2, ..., PN), (M = 1, 2,..., M) (1)
Where the function fmIs a known function obtained from the geometric structure of the parallel mechanism.
[0038]
In the case of the parallel mechanism 10 shown in FIG. 1, it can be considered that M = 12 and N = 6 in the following description.
First, in step 50 of FIG.m, Each variable P of vector Pn, Each variable Pf of the vector PfnIs initialized. Next, in step 52, the output value α from each sensor 20, 22 is shown.m(M = 1, 2,..., M) is received by the receiving means 34 of the control device 24.
[0039]
The motion platform position / posture calculation means 38 assumes the position and posture of the motion platform 16, calculates the theoretical amount of movement of each actuator 18 or each pair 30, 32 at the assumed position and posture, and obtains by this calculation. Sensor so that the value of the function for evaluating the sensor as a whole is the smallest in the difference between the movement amount measured and the movement amount of each actuator 18 or each pair 30, 32 actually detected by the sensors 20, 22. The position and posture of the motion platform 16 are estimated from the amount of movement of each actuator 18 or each pair 30, 32 detected by 20, 22.
[0040]
The function E for this evaluation is defined by the following equation in the flowchart shown in FIG.
[0041]
[Formula 6]
Figure 0004290312
The set of detection values of each sensor 20, 22 to be output at the assumed position and orientation of the motion platform 16 represented by the vector Pf ism(Pf1, Pf2, ..., PfN) (M = 1, 2,..., M). Therefore, the above equation (2) is expressed as M sums of squares of differences between the theoretical detection values of the sensors 20 and 22 and the actual detection values of the sensors 20 and 22 in the assumed position and posture of the motion platform 16. Therefore, the output of the sensors 20 and 22 (the sensors that can detect the motion related to the L degrees of freedom are counted as L sensors even if a single sensor is used). That is, the function E for evaluation is a known function.
[0042]
Here, a function vector g (Pf) expressed by the following equation is defined.
[0043]
[Expression 7]
Figure 0004290312
Now, assume that g (Pf) = e in Equation (3) in the assumed position and posture of the exercise platform 16. Where e: (e1, E2, ..., eN).
When the function E for evaluation is minimum, e = (0, 0,..., 0).
[0044]
Therefore,
g (Pf + dPf) = 0 (4)
If dPf that satisfies is obtained, Pf satisfying g (Pf) = 0 can be obtained.
[0045]
[Equation 8]
Figure 0004290312
Therefore, the following equation is derived from equations (4) and (5).
[0046]
[Equation 9]
Figure 0004290312
Since the function vector f is known, the function vector g is also known from Expression (3), and dPf satisfying g (Pf + dPf) = 0 is obtained from Expression (6).
Therefore, the vector (Pf + dPf) obtained by adding the obtained dPf to the assumed Pf is a vector representing the position and orientation of the motion platform 16 that minimizes the function E for evaluation.
[0047]
In a practical operation, referring to FIG. 3, in step 54, the value of the function E for evaluating the above in the assumed position and posture of the motion platform 16 represented by the vector Pf is calculated according to the equation (2). Ask. Next, in step 56, g (Pf), that is, a vector e is obtained. Next, in step 60, dPf is obtained from the value of the partial differential function related to the variable Pf of the function g (Pf) calculated in step 58 according to the equation (6).
[0048]
In step 62, if the absolute value of dPf is equal to or smaller than a predetermined constant c, it is determined that Pf has converged, and Pf at this time is regarded as the correct position and posture vector P of the exercise platform 16. In this case, in step 64, the reaction force of the exercise platform 16 at this position and posture vector is calculated by the exercise platform reaction force calculation means 40, and the actuator torque calculation means 42 calculates the actuator 18 based on the obtained reaction force. The required torque is calculated and the actuator 18 is instructed to output the obtained torque, and steps 52 to 64 are repeated at the new position and posture of the motion platform.
[0049]
On the other hand, if the absolute value of dPf is larger than the constant c in step 62, (Pf + dPf) is used as the new Pf (step 66), and the value of the function E for evaluation is calculated again in step 54, and the obtained dPf is obtained. Step 54 to Step 62 are repeated until becomes a constant c or less.
The expression of the function E for evaluation described above is an example. For example, the weight of the errors of the sensors 20 and 22 is not equally weighted as in the above expression (2), but the geometry of the parallel mechanism 10 is used. Arbitrary weighting may be performed on the error of each of the sensors 20 and 22 in consideration of the general structure.
[0050]
As described above, the present invention has been described by taking the bending type 6-degree-of-freedom parallel mechanism 10 as an example. However, those skilled in the art can easily apply the present invention to a parallel mechanism having an arbitrary configuration. Will be understood.
[0051]
【The invention's effect】
As described above, according to the present invention, the probability that a singular point occurs can be reduced by redundantly providing a number of sensors exceeding the degree of freedom of movement of the parallel mechanism. In addition, a function is used to evaluate the contradiction between the movement amount of the even pair and the movement amount actually detected by the sensor for detecting the movement amount of the even pair in the assumed position and posture of the movement platform as a whole sensor. By doing so, it is possible to always use equations more than the number of variables of the position and posture vector of the motion platform to be obtained. It is hard to produce the influence of. As a result, the singularity is reduced at least in the parallel mechanism, the operation is stabilized, and a wider operating range can be set.
[Brief description of the drawings]
FIG. 1 is a perspective view showing a bending type 6-degree-of-freedom parallel mechanism to which a control method and a control apparatus according to the present invention are applied.
FIG. 2 is a block diagram of the parallel mechanism shown in FIG.
FIG. 3 is a flowchart showing a method for controlling a parallel mechanism of the present invention.
[Explanation of symbols]
10 ... Parallel mechanism
12 ... Base
14 ... Link set
16 ... Exercise platform
18 ... Actuator or even number
20 ... Sensor
22 ... Sensor
24 ... Control device
30 ... even number
32 ... even number

Claims (9)

ベースと、前記ベースに取り付けられた複数のリンクセットと、前記複数のリンクセットによって支持された運動プラットフォームと、前記ベースと前記リンクセットとの結合部、前記運動プラットフォームと前記リンクセットとの結合部、及び前記各リンクセットの中間部に設けられた対偶と、を備えた自由度Ｎを有するパラレル機構において、
全体でＭ（＞Ｎ）自由度に関する前記対偶の運動を検出するために複数のセンサを設け、前記運動プラットフォームの仮定の位置及び姿勢から演算される前記センサを設けた各対偶の理論上の移動量と前記各センサによって実際に検出される前記センサを設けた各対偶の移動量との差を前記センサを設けた対偶の全てについて総合的に評価するための関数の値を最小とする仮定の位置及び姿勢を前記運動プラットフォームの実際の位置及び姿勢とすることにより、前記複数のセンサによって検出される前記センサを設けた各対偶の移動量から前記運動プラットフォームの実際の位置及び姿勢を推定することを特徴とするパラレル機構の制御方法。A base, a plurality of link sets attached to the base, a motion platform supported by the plurality of link sets, a joint between the base and the link set, and a joint between the motion platform and the link set And a parallel mechanism having a degree of freedom N provided with an even number provided in an intermediate portion of each link set,
Total M (> N) a plurality of sensors for detecting the movement of the kinematic pair regarding freedom provided, before Symbol theoretical for each kinematic pair provided with the sensor, which is calculated from the position and orientation of the assumptions of the exercise platform Assuming that the value of the function for comprehensively evaluating the difference between the amount of movement and the amount of movement of each pair provided with the sensor actually detected by each sensor for all the pairs provided with the sensor is minimized with actual position and orientation of the position and orientation of the motion platform to estimate the actual position and orientation of the motion platform from the amount of movement of the kinematic pair provided with the sensor which is detected by the plurality of sensors A control method of a parallel mechanism characterized by the above. 前記運動プラットフォームの実際の位置及び姿勢を表すベクトルを（Ｐ1 ，Ｐ2 ，…, ＰN ) 、前記位置及び姿勢において各センサによって検出される前記対偶の移動量によって構成されるベクトルを（α1 ，α2 ，…，αM ）、前記運動プラットフォームの仮定の位置及び姿勢を表すベクトルを（Ｐｆ1 ，Ｐｆ2 ，…，ＰｆN ）とし、
ベクトル（Ｐ1 ，Ｐ2 ，…, ＰN ) とベクトル（α1 ，α2 ，…，αM ）とが関数ベクトル（ｆ1 ，ｆ2 ，…，ｆM ）によって、式
αm ＝ｆm （Ｐ1 ，Ｐ2 ，…，ＰN ），（ｍ＝１，２，…，Ｍ）
で対応づけられているとき、前記評価するための関数を
Figure 0004290312
と定義し、前記評価するための関数Ｅの値が最小となったときの前記ベクトル（Ｐｆ 1 ，Ｐｆ 2 ，…，Ｐｆ N ）を前記運動プラットフォームの実際の位置及び姿勢を表すベクトルと推定する、請求項１に記載のパラレル機構の制御方法。Wherein the vector representing the actual position and orientation of the motion platform (P 1, P 2, ... , P N), a vector composed of the amount of movement of the kinematic pair detected by Oite the sensors on the position and orientation (α 1, α 2, ... , α M), the vector representing the position and orientation of the assumptions of the previous SL motion platform (Pf 1, Pf 2, ... , Pf N) and,
The vector (P 1 , P 2 ,..., P N ) and the vector (α 1 , α 2 ,..., Α M ) are expressed by the function vector (f 1 , f 2 ,..., F M ), and the expression α m = f m (P 1 , P 2 ,..., P N ), (m = 1, 2,..., M)
The function for the evaluation is
Figure 0004290312
 And the vector (Pf 1 , Pf 2 ,..., Pf N ) when the value of the function E for evaluation is minimized is estimated as a vector representing the actual position and orientation of the motion platform. The parallel mechanism control method according to claim 1. ベースと、前記ベースに取り付けられた複数のリンクセットと、前記複数のリンクセットによって支持された運動プラットフォームと、前記ベースと前記リンクセットとの結合部、前記運動プラットフォームと前記リンクセットとの結合部、及び前記各リンクセットの中間部に設けられた対偶と、を備えた自由度Ｎを有するパラレル機構のための制御装置であって、
パラレル機構全体でＭ（＞Ｎ）自由度に関する前記リンクセットの前記対偶の運動を検出するために設けられた複数のセンサからの出力を受信する受信手段と、前記運動プラットフォームの仮定の位置及び姿勢から演算される前記センサを設けた各対偶の理論上の移動量と前記各センサによって実際に検出される前記センサを設けた各対偶の移動量との差を前記センサを設けた対偶の全てについて総合的に評価するための関数の値を最小にする仮定の位置及び姿勢を前記運動プラットフォームの実際の位置及び姿勢とすることにより、前記複数のセンサによって検出される前記センサを設けた各対偶の移動量から前記運動プラットフォームの実際の位置及び姿勢を推定する演算手段と、を備えることを特徴とするパラレル機構の制御装置。A base, a plurality of link sets attached to the base, a motion platform supported by the plurality of link sets, a joint between the base and the link set, and a joint between the motion platform and the link set And a control device for a parallel mechanism having a degree of freedom N provided with an even number provided in an intermediate portion of each link set,
M across a parallel mechanism (> N) and receiving means for receiving an output from a plurality of sensors provided for detecting the movement of the kinematic pair of the link set for freedom, position before Symbol motion platform assumptions and all even number of the difference between the amount of movement of the kinematic pair provided with the sensor which is actually detected by the movement amount and the sensors theoretical for each kinematic pair provided with the sensor provided with the sensor, which is calculated from the orientation Each pair provided with the sensors detected by the plurality of sensors by setting an assumed position and orientation that minimizes a value of a function for comprehensive evaluation of the motion platform as an actual position and orientation of the motion platform. And a calculation means for estimating an actual position and posture of the exercise platform from the amount of movement of the control mechanism. 前記運動プラットフォームの実際の位置及び姿勢を表すベクトルを（Ｐ1 ，Ｐ2 ，…, ＰN ) 、前記位置及び姿勢において各センサによって検出される前記対偶の移動量によって構成されるベクトルを（α1 ，α2 ，…，αM ）、前記運動プラットフォームの仮定の位置及び姿勢を表すベクトルを（Ｐｆ1 ，Ｐｆ2 ，…，ＰｆN ）とし、
ベクトル（Ｐ1 ，Ｐ2 ，…, ＰN ) とベクトル（α1 ，α2 ，…，αM ）とが関数ベクトル（ｆ1 ，ｆ2 ，…，ｆM ）によって、式
αm ＝ｆm （Ｐ1 ，Ｐ2 ，…，ＰN ），（ｍ＝１，２，…，Ｍ）
で対応づけられているとき、前記評価するための関数を
Figure 0004290312
と定義し、前記評価するための関数Ｅの値が最小となったときの前記ベクトル（Ｐｆ 1 ，Ｐｆ 2 ，…，Ｐｆ N ）を前記運動プラットフォームの実際の位置及び姿勢を表すベクトルと推定する、請求項３に記載の制御装置。Wherein the vector representing the actual position and orientation of the motion platform (P 1, P 2, ... , P N), a vector composed of the amount of movement of the kinematic pair detected by Oite the sensors on the position and orientation (α 1, α 2, ... , α M), the vector representing the position and orientation of the assumptions of the previous SL motion platform (Pf 1, Pf 2, ... , Pf N) and,
The vector (P 1 , P 2 ,..., P N ) and the vector (α 1 , α 2 ,..., Α M ) are expressed by the function vector (f 1 , f 2 ,..., F M ), and the expression α m = f m (P 1 , P 2 ,..., P N ), (m = 1, 2,..., M)
The function for the evaluation is
Figure 0004290312
 And the vector (Pf 1 , Pf 2 ,..., Pf N ) when the value of the function E for evaluation is minimized is estimated as a vector representing the actual position and orientation of the motion platform. The control device according to claim 3. Ｎ自由度を有するパラレル機構であって、
ベースと、
前記ベースに取り付けられた複数のリンクセットと、
前記複数のリンクセットによって支持された運動プラットフォームと、
前記ベースと前記リンクセットとの結合部、前記運動プラットフォームと前記リンクセットとの結合部、及び前記各リンクセットの中間部に設けられた対偶と、
全体でＭ（＞Ｎ）自由度に関する前記対偶の運動を検出するための複数のセンサと、
前記複数のセンサによって検出される前記対偶の移動量から前記運動プラットフォームの実際の位置及び姿勢を推定するための制御装置と、
を備えることを特徴とするパラレル機構。 A parallel mechanism having N degrees of freedom,
Base and
A plurality of link sets attached to the base;
An exercise platform supported by the plurality of link sets;
A coupling portion between the base and the link set, a coupling portion between the motion platform and the link set, and a pair provided at an intermediate portion of each link set;
A plurality of sensors for detecting the kinematic motion with respect to M (> N) degrees of freedom in total;
A controller for estimating an actual position and posture of the motion platform from the amount of movement of the even number detected by the plurality of sensors;
A parallel mechanism characterized by comprising: 前記制御装置が、前記運動プラットフォームの仮定の位置及び姿勢から求められる前記センサを設けた各対偶の理論上の移動量と実際に前記各センサによって検出される前記センサを設けた各対偶の移動量との差を前記センサを設けた対偶の全てについて総合的に評価するための関数の値を最小とする仮定の位置及び姿勢を前記運動プラットフォームの実際の位置及び姿勢とすることにより、前記複数のセンサによって検出される前記センサを設けた各対偶の移動量から前記運動プラットフォームの実際の位置及び姿勢を推定する、請求項５に記載のパラレル機構。Wherein the controller, the amount of movement of the kinematic pair provided with the sensor which is detected by the fact the sensors and the moving amount of the theoretical for each kinematic pair provided with the sensor to be determined from the position and orientation of assumptions the motion platform And the assumed position and posture that minimizes the value of the function for comprehensively evaluating the difference between all of the kinematic pairs provided with the sensor as the actual position and posture of the motion platform , The parallel mechanism according to claim 5, wherein an actual position and posture of the motion platform are estimated from a movement amount of each pair provided with the sensor detected by a sensor . 前記運動プラットフォームの実際の位置及び姿勢を表すベクトルを（Ｐ1 ，Ｐ2 ，…, ＰN ) 、前記位置及び姿勢において各センサによって検出される前記対偶の移動量によって構成されるベクトルを（α1 ，α2 ，…，αM ）、前記運動プラットフォームの仮定の位置及び姿勢を表すベクトルを（Ｐｆ1 ，Ｐｆ2 ，…，ＰｆN ）とし、
ベクトル（Ｐ1 ，Ｐ2 ，…, ＰN ) とベクトル（α1 ，α2 ，…，αM ）とが関数ベクトル（ｆ1 ，ｆ2 ，…，ｆM ）によって、式
αm ＝ｆm （Ｐ1 ，Ｐ2 ，…，ＰN ），（ｍ＝１，２，…，Ｍ）
で対応づけられているとき、前記評価するための関数を
Figure 0004290312
 と定義し、前記制御装置が、前記評価するための関数Ｅの値が最小となったときの前記ベクトル（Ｐｆ 1 ，Ｐｆ 2 ，…，Ｐｆ N ）を前記運動プラットフォームの実際の位置及び姿勢を表すベクトルと推定する、請求項６に記載のパラレル機構。Wherein the vector representing the actual position and orientation of the motion platform (P 1, P 2, ... , P N), a vector composed of the amount of movement of the kinematic pair detected by Oite the sensors on the position and orientation (α 1, α 2, ... , α M), the vector representing the position and orientation of the assumptions of the previous SL motion platform (Pf 1, Pf 2, ... , Pf N) and,
The vector (P 1 , P 2 ,..., P N ) and the vector (α 1 , α 2 ,..., Α M ) are expressed by the function vector (f 1 , f 2 ,..., F M ), and the expression α m = f m (P 1 , P 2 ,..., P N ), (m = 1, 2,..., M)
The function for the evaluation is
Figure 0004290312
 And the controller (Pf 1 , Pf 2 ,..., Pf N ) when the value of the function E for evaluation is minimized is the actual position and orientation of the motion platform. The parallel mechanism according to claim 6, wherein the parallel mechanism is estimated as a vector to be represented . 前記各リンクセットの対偶の少なくとも一つがアクチュエータによって駆動される能動対偶であり、前記センサが前記アクチュエータ又は前記能動対偶に取り付けられている、請求項５に記載のパラレル機構。Wherein an active kinematic pair which at least one kinematic pair of each link set is driven by an actuator, the sensor is installed in a said actuator or said active even number, the parallel mechanism according to claim 5. 前記能動対偶が前記ベースと前記リンクセットとの結合部に設けられ、前記各リンクセットの中間部に設けられた対偶がさらにセンサを備えている、請求項８に記載のパラレル機構。  The parallel mechanism according to claim 8, wherein the active pair is provided at a coupling portion between the base and the link set, and the pair provided at an intermediate portion of each link set further includes a sensor.
JP2000097913A 2000-03-30 2000-03-30 Parallel mechanism, control method and control apparatus thereof Expired - Fee Related JP4290312B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2000097913A JP4290312B2 (en) 2000-03-30 2000-03-30 Parallel mechanism, control method and control apparatus thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2000097913A JP4290312B2 (en) 2000-03-30 2000-03-30 Parallel mechanism, control method and control apparatus thereof

Publications (2)

Publication Number Publication Date
JP2001282359A JP2001282359A (en) 2001-10-12
JP4290312B2 true JP4290312B2 (en) 2009-07-01

Family

ID=18612473

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2000097913A Expired - Fee Related JP4290312B2 (en) 2000-03-30 2000-03-30 Parallel mechanism, control method and control apparatus thereof

Country Status (1)

Country Link
JP (1) JP4290312B2 (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9214095B2 (en) 2009-06-08 2015-12-15 Mitsubishi Precision Co., Ltd. Surgical simulation model generating method, surgical simulation method, and surgical simulator

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1498797A1 (en) * 2003-07-15 2005-01-19 Force Dimension S.à.r.l Device for transmitting a movement having a parallel kinematics transmission structure providing three translational degrees of freedom
CA2555334C (en) * 2004-02-04 2014-02-11 Mazor Surgical Technologies Ltd. Verification system for robot pose
JP2008264904A (en) * 2007-04-17 2008-11-06 Fanuc Ltd Parallel-link working device

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9214095B2 (en) 2009-06-08 2015-12-15 Mitsubishi Precision Co., Ltd. Surgical simulation model generating method, surgical simulation method, and surgical simulator

Also Published As

Publication number Publication date
JP2001282359A (en) 2001-10-12

Similar Documents

Publication Publication Date Title
EP2018606B1 (en) A method and apparatus for controlling a haptic device
US9060796B2 (en) Surgical robot system and control method thereof
Xu et al. Actuation compensation for flexible surgical snake-like robots with redundant remote actuation
KR101151515B1 (en) Verification system for robot pose
US8909376B2 (en) Robot hand and method of controlling the same
JP6892967B2 (en) Systems and methods for controlling robot wrists
US11850014B2 (en) Control system, control method, and surgical arm system
JP2019509907A (en) Mechanical system control method, mechanical system controller, robot manipulator, and non-transitory computer-readable storage medium
EP2760642A1 (en) Calibration and programming of robots
JP3349652B2 (en) Offline teaching method
JP2016198828A (en) Robot control method, robot apparatus, program, and recording medium
JP4302830B2 (en) Robot calibration method and apparatus
CN109176487A (en) A kind of cooperating joint section scaling method, system, equipment, storage medium
JP3369351B2 (en) Elasticity setting method and control device for articulated manipulator
JP4290312B2 (en) Parallel mechanism, control method and control apparatus thereof
WO2020050314A1 (en) Arm device, control method, and program
CN114650900A (en) Robot joint control
CN117503363A (en) Self-calibration method of moving arm system and surgical robot system
Kapoor et al. Telemanipulation of snake-like robots for minimally invasive surgery of the upper airway
Kim et al. Accurate estimation of the position and shape of the rolling joint in hyper-redundant manipulators
CN113894791B (en) Kinematic calibration method and calibration system for multi-degree-of-freedom robot
US20240009848A1 (en) Kinematics calibration method and calibration system for robot with multiple degrees of freedom
JP3930956B2 (en) Trajectory control apparatus and trajectory control method for a multi-degree-of-freedom SCARA robot in one plane, and a computer-readable recording medium recording a trajectory control program for a single-degree multi-DOF SCARA robot
JP2023549754A (en) Adaptive robot-assisted system and method for assessing trocar position in robot-assisted laparoscopic surgical procedures
JP3626331B2 (en) Offline teaching method

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20070320

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20081015

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20081021

A521 Written amendment

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20081217

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20090303

A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20090401

R150 Certificate of patent or registration of utility model

Free format text: JAPANESE INTERMEDIATE CODE: R150

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20120410

Year of fee payment: 3

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20130410

Year of fee payment: 4

FPAY Renewal fee payment (event date is renewal date of database)

Free format text: PAYMENT UNTIL: 20140410

Year of fee payment: 5

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

LAPS Cancellation because of no payment of annual fees