JP3944566B2 - リンク機構において動力学的に関節加速度を求める方法 - Google Patents
リンク機構において動力学的に関節加速度を求める方法 Download PDFInfo
- Publication number
- JP3944566B2 JP3944566B2 JP2001228804A JP2001228804A JP3944566B2 JP 3944566 B2 JP3944566 B2 JP 3944566B2 JP 2001228804 A JP2001228804 A JP 2001228804A JP 2001228804 A JP2001228804 A JP 2001228804A JP 3944566 B2 JP3944566 B2 JP 3944566B2
- Authority
- JP
- Japan
- Prior art keywords
- joint
- link mechanism
- acceleration
- link
- links
- 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 - Lifetime
Links
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B17/00—Systems involving the use of models or simulators of said systems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/10—Geometric CAD
- G06F30/17—Mechanical parametric or variational design
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/39—Robotics, robotics to robotics hand
- G05B2219/39184—Forward compensation in robot world space, inverse in joint space
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Geometry (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Hardware Design (AREA)
- Robotics (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
- Manipulator (AREA)
- Complex Calculations (AREA)
- Transmission Devices (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は、リンクを関節で接続して構成されるリンク機構における動力学的計算法に関し、特に、リンク機構の関節値、関節速度および関節トルクが与えられたときに関節加速度を計算する順動力学計算法に好適に用いることができるリンク機構において動力学的に関節加速度を求める方法に関するものである。
【０００２】
【従来の技術】
従来から、剛体リンク機構の順動力学計算は、オフラインでの機構解析や制御系設計に用いられることが多かったが、ロボットの応用範囲が広がるにつれ多自由度のモデルをインタラクティブな動作が可能な速度でシミュレートすることが求められるようになってきている。このような剛体リンク機構の順動力学計算は、ロボットの動力学シミュレーションのほか、動力学を用いたヒューマノイドロボットやヒューマンフィギュアの運動作成などにも応用されている。いずれの分野においても、関節トルク入力や計算結果の画面表示などのインターフェースを外部に付加したソフトウェアとして利用されている。
【０００３】
このような剛体リンク機構の順動力学計算の例として、以下の文献１〜３が知られている。
文献１：Anderson,K.S. and Duan,S: "Highly Parallelizable Low Order Algorithm for the Dynamics of Complex Multi-Rigid-Body Systems," AIAA Journal of Guidance, Control and Dynamics, 2000.
文献２：Featherstone,R.: "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n)) Calculation of Rigid-Body Dynamics," International Journal of Robotics Research, vol.18, no.9, pp.867-892, 1999.
文献３：Fijany,A. and Sharf,I. and D'Eleuterio,G.M.T.: "Parallel O(logN) Algorithms for Computation of Manipulator Forward Dynamics, IEEE Transactions on Robotics and Automation, vol.11, no. 8, pp.389-400, 1995.
【０００４】
【発明が解決しようとする課題】
上述した文献１〜３の技術は、いずれも並列計算により剛体リンク機構の順動力学計算を高速に行うことを目的としており、Ｎ個のリンクがあるときＯ（Ｎ）個のプロセスを使って計算時間がＯ（ｌｏｇＮ）となる。しかし、これらの方法には、収束計算により近似値を求める方法で厳密値は計算できない（文献１の問題）、直列計算と並列計算でアルゴリズムが大きく異なる（文献２の問題）、閉リンク機構など複雑な機構への適用が難しい（文献３の問題）などの問題があった。
【０００５】
本発明の目的は上述した課題を解消して、開リンク機構・閉リンク機構を含む剛体リンク機構の順動力学計算を高速に行うことのできるリンク機構において動力学的に関節加速度を求める方法を提供しようとするものである。
【０００６】
【課題を解決するための手段】
本発明のリンク機構において動力学的に関節加速度を求める方法は、リンクを関節で接続して構成されるリンク機構において、動力学的に関節加速度を求める方法であって、（１）個々のリンクがばらばらの状態から関節を１つずつ順次追加していき、関節を追加する毎に、追加した関節で発生するその時点でリンク間の関節において働く拘束力を求める第１のステップと、（２）第１のステップとは逆の順序で関節を除いていき、切断関節の確定した拘束力を用いて次に切断する関節の拘束力を確定する処理を繰り返すことによって、各関節における最終的な拘束力を計算する第２のステップと、（３）第１のステップ及び第２のステップで求めた各リンクの関節の拘束力を総合した、各リンクに働く外力を用いて、リンクの加速度を計算する第３のステップと、（４）第３のステップで求めた各関節の両側のリンクの加速度を用いて、関節加速度を計算する第４のステップと、からなることを特徴とするものである。
【０００７】
本発明では、個々のリンクがばらばらの状態から関節を１つずつ追加していき、最終的に目的のリンク機構を組み立てている。また、最後の関節以外は後で追加された関節の影響が反映されていないので、リンク機構全体を考慮するために、逆の順序で関節を切断していく分解の作業が必要となる。これら一連の第１のステップ〜第４のステップを実行することで、リンク数が非常に多いリンク機構の動力学シミュレーションがリアルタイムに近い速度で実行できるようになる。
【０００８】
なお、組立中に現れる一部のリンクからなるリンク機構を以下「部分リンク機構」と呼ぶこととする。また、本発明において「拘束力」とは、関節においては、許された方向以外の運動を拘束する（打ち消す）ために働く力を意味する。
【０００９】
本発明の好適な具体例としては、第１のステップにおいて、関節を処理する順序を適切に選択することで、任意数のプロセッサを用いた並列計算を行うこと、第１のステップにおいて、リンク間の各関節において働く拘束力を、仮想仕事の原理に基づいて導出すること、第１のステップにおいて、リンク間の各関節において働く拘束力を、あらかじめ選ばれた１個以上のリンクに作用する力と、その力によってリンクにおいて発生する加速度との関係を表す慣性行列を用いて導出することがある。いずれの場合も本発明をより好適に実施することができるため好ましい態様となる。
【００１０】
【発明の実施の形態】
本発明のリンク機構において動力学的に関節加速度を求める方法は、部分リンク機構の拘束力の計算（第１のステップ）、最終的な拘束力の計算（第２のステップ）、リンク加速度の計算（第３のステップ）、関節加速度の計算（第４のステップ）から構成されている。以下、各ステップ毎に説明する。
【００１１】
（１）部分リンク機構の拘束力の計算（第１のステップ）
全リンクがばらばらの状態から関節を１個ずつ追加し、一部の関節からなる２つのリンク機構を接続していく手順を繰り返し、最終的に全体のリンク機構を得る。図１の例では、関節ｉによって部分リンク機構ＡとＢを接続する。これによってできたリンク機構はそれ自体部分リンク機構であり、さらにリンクｌ、ｍ、ｎを介して他の部分リンク機構と接続されることになる。第１のステップでは、関節を追加する毎に、関節ｉで発生する拘束力ｆｉを求める。拘束力ｆｉを求めるためには、好ましくは仮想仕事の原理を使用する。
【００１２】
なお、閉リンク機構を構成する場合は、閉ループ内の１個のリンクについては同じ部分リンク機構の中での接続を行うことになる。すなわち、第１ステップにおいて関節を追加するときは、異なる２つの部分リンク機構を接続することを基本とするが、閉リンク機構の場合には、図２に示すように、同じ部分リンク機構内の２リンクを接続するように関節を追加することで対応することができる。
【００１３】
（２）最終的な拘束力の計算（第２のステップ）
第１のステップで全体のリンク機構における関節の拘束力を求めることができるが、最後の関節以外は後で追加された関節の影響が反映されていないので、リンク機構全体を考慮するために、逆の順序で関節を切断していく分解の作業が必要となる。それを実行するのが第２のステップである。すなわち、完全なリンク機構における拘束力をそれぞれの関節について計算する。これは、第１のステップでの操作とは逆の順序で関節を切断していき、切断関節の確定した拘束力を用いて次に切断する関節の拘束力を確定する処理を繰り返すことによって行う。図１の例では、関節ｌ、ｍ、ｎが切断され、拘束力が確定すると関節ｉの拘束力が確定できるので、続いて関節ｉを切断する。この操作が完了すると、全関節における拘束力が確定する。
【００１４】
（３）リンク加速度の計算（第３のステップ）
第１のステップ及び第２のステップにおいて、各リンクに働く外力をすべて求めることができるため、各リンクに働く拘束力を総合して、各リンクの加速度を計算する。
（４）関節加速度の計算（第４のステップ）
第３のステップで求めた各関節の両側のリンクの相対加速度を用いて、関節加速度を計算する。
【００１５】
上述した説明の第１のステップにおいて、関節を１個追加する毎に関節ｉで発生する拘束力ｆｉを求めるには、リンク機構としての慣性行列を考えることが好ましい。すなわち、関節を１個追加する毎に、関節ｉで働く拘束力と加速度の関係を表す慣性行列と関節ｉ、ｌ、ｍ、ｎで働く拘束力と加速との関係を表す慣性行列を求め、それらを利用する。以下、ここで用いる慣性行列についてさらに詳細に説明する。
【００１６】
図３に示すようなリンク機構において、リンク１、２に対して力およびモーメントｆ１、ｆ２を作用させたときに、リンク１、２で発生する加速度および角加速度をａ１、ａ２とするとき、ｆ１、ｆ２とａ１、ａ２の関係は以下の式（１）のように表される。
【００１７】
【数１】
【００１８】
ここでΓｉｊ（ｉ、ｊ＝１、２）がステップ１において計算される慣性逆行列であり、これの逆行列を計算することにより慣性行列が得られる。この慣性行列を用いると、リンク１、２が外部からの拘束を受けているときの拘束力の計算などに応用することができる。この慣性行列は、すべてのリンクの質量と慣性モーメントに加え、関節の拘束条件を考慮して計算されたものであり、個々のリンクの質量や慣性モーメントの単なる和ではない。リンクの質量・モーメントと関節の拘束を考慮した慣性行列の関係を示す単純な例として図４を挙げる。図４に示す例では、質量ｍ１のリンクと質量ｍ２のリンクとが左右に並進移動できるように関節で接続されている。このリンク機構において、質量ｍ１のリンクに力を加えたときの全体としての質量は、上下方向に移動させようとしたときは２つのリンクを同時に動かすことになるのでｍ１＋ｍ２となるのに対し、左右方向に移動させようとしたときは質量ｍ２のリンクが移動方向の拘束を受けないのでその質量を無視することができ、全体の質量はｍ１となる。
【００１９】
このようなリンク機構全体としての慣性行列は、リンク機構内で発生する拘束力を求める目的以外にも有用である。例えば、図５に示すようにリンク機構が環境など外部の物体と接触しているときには、接触力は外部の物体がリンクを引っ張る方向に働くことはできないという制約条件があるため、接触点における拘束力を計算して実際に接触力が発生しているかどうかを判定する必要がある。この場合は、接触しているリンクの運動、すなわち、ａ１、ａ２が拘束によって決まっているため、上述した数（１）を用いて接触力ｆ１、ｆ２を計算することができる。
【００２０】
以上のように、本発明のリンク機構において動力学的に関節加速度を求める方法は、閉ループを形成する場合を除き１個の関節を追加して異なる２つの部分リンク機構を接続する作業を繰り返し行う。接続前の部分リンク機構は力学的に独立であるから、それ以前の処理はそれぞれの部分リンク機構で並列に行うことができ、もしそれらの処理が別々のＣＰＵまたは計算機で行われているならば、全体の計算時間は部分リンク機構１つの場合と同じになり、計算時間の短縮が実現される。
【００２１】
図１の例では、関節ｉを追加する前の部分リンク機構ＡおよびＢにおける処理は並列に行ってよく、計算時間はどちらか一方のみを処理した場合と変わらない。このことから、十分な数のＣＰＵまたは計算機がある計算機環境の場合には、並列計算時の計算時間を最短にするために、途中で現れる部分リンク機構ができるだけ互いに独立になるようにスケジューリングを行うことが望ましい。
【００２２】
逆に、このようなスケジューリングを行うと各部分リンク機構が他の部分リンク機構と接続される関節（図１の関節ｌ、ｍ、ｎに相当する関節）の数が増加するが、これは２つの部分リンク機構を接続する時の計算量が増加することを意味する。したがって、ＣＰＵまたは計算機が１つしかなく直列計算しかできないような計算機環境には、このようなスケジューリングは適さず、各部分リンク機構が他の部分リンク機構と接続される関節ができるだけ少なくなるような順序で処理を行うのが望ましい。
【００２３】
８リンク７関節のシリアルリンク機構において、並列計算と直列計算に最適化された処理順序をそれぞれ図６（並列計算に最適化された処理順序）、及び、図７（直列計算に最適化された処理順序）に示す。図６及び図７を比較すると、１つの関節の追加に対する処理にかかる時間を同じとすると、計算にかかるステップが少ない分だけ図６に示す並列計算の方が図７に示す直列計算の場合と比べて格段に処理が速くなることがわかる。
【００２４】
計算プログラムを実際に利用する場合には、計算機環境に応じて別々のプログラムを用意するのは非現実的である。この点において、本計算法は直列計算と並列計算の原理的な差は全くなく、両者の間の切り替えは容易であるため、さまざまな計算機環境に対応したプログラム開発が可能である。
【００２５】
上述した本発明のリンク機構において動力学的に関節加速度を求める方法により、リンク数が非常に多いリンク機構の動力学シミュレーションがリアルタイムに近い速度で実行できるようになる。使用手順は以下の通りである。
・初期関節値、関節速度を設定する。
・計算をスタートする。計算中には時々刻々の関節トルクを入力する。
・１回の計算が終了すると、積分刻み幅分だけ進んだ時刻における状態が計算されるので、それらのデータを適宜取りだして制御などに利用する。
【００２６】
【発明の効果】
以上の説明から明らかなように、従来閉リンク機構を含む複雑な機構の動力学計算には非常に多くの計算を必要としたが、本発明の方法ではこれを高速に行うことができ、またリンク数の増加に伴う計算時間の増加が少ないため、数１００の自由度を持つリンク機構でも比較的短時間に動力学的計算を行うことができる。実際、複雑な機構にも適用可能な従来法に比べて、４０自由度程度の機構に対し直列計算時で計算時間が約１／３に短縮されることが確かめられている。さらに、各関節の加速度だけでなく、各関節における拘束力が直接計算されるので、力センサなどのシミュレーションにも応用することができる。
【００２７】
これまでは、動力学計算を直列計算する場合と並列計算する場合で全く異なる計算法を適用することが多かったが、本発明の方法はそれらを特に区別する必要はなく、関節の処理順序の変更だけでＣＰＵや計算機数の変化に対応でき、より汎用的なプログラム作成が可能となる。
【図面の簡単な説明】
【図１】本発明のリンク機構において動力学的に関節加速度を求める方法における部分リンク機構の接続を説明するための図である。
【図２】本発明のリンク機構において動力学的に関節加速度を求める方法における閉リンク機構の扱いを説明するための図である。
【図３】本発明のリンク機構において動力学的に関節加速度を求める方法におけるリンクの接続性を考慮したリンク機構の慣性行列を説明するための図である。
【図４】本発明のリンク機構において動力学的に関節加速度を求める方法における個々のリンクの質量とリンク機構としての質量の関係を説明するための図である。
【図５】本発明のリンク機構において動力学的に関節加速度を求める方法における外部から拘束を受けるリンク機構を説明するための図である。
【図６】本発明のリンク機構において動力学的に関節加速度を求める方法における並列計算に最適化された処理順序を説明するための図である。
【図７】本発明のリンク機構において動力学的に関節加速度を求める方法における直列計算に最適化された処理順序を説明するための図である。
【発明の属する技術分野】
本発明は、リンクを関節で接続して構成されるリンク機構における動力学的計算法に関し、特に、リンク機構の関節値、関節速度および関節トルクが与えられたときに関節加速度を計算する順動力学計算法に好適に用いることができるリンク機構において動力学的に関節加速度を求める方法に関するものである。
【０００２】
【従来の技術】
従来から、剛体リンク機構の順動力学計算は、オフラインでの機構解析や制御系設計に用いられることが多かったが、ロボットの応用範囲が広がるにつれ多自由度のモデルをインタラクティブな動作が可能な速度でシミュレートすることが求められるようになってきている。このような剛体リンク機構の順動力学計算は、ロボットの動力学シミュレーションのほか、動力学を用いたヒューマノイドロボットやヒューマンフィギュアの運動作成などにも応用されている。いずれの分野においても、関節トルク入力や計算結果の画面表示などのインターフェースを外部に付加したソフトウェアとして利用されている。
【０００３】
このような剛体リンク機構の順動力学計算の例として、以下の文献１〜３が知られている。
文献１：Anderson,K.S. and Duan,S: "Highly Parallelizable Low Order Algorithm for the Dynamics of Complex Multi-Rigid-Body Systems," AIAA Journal of Guidance, Control and Dynamics, 2000.
文献２：Featherstone,R.: "A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log(n)) Calculation of Rigid-Body Dynamics," International Journal of Robotics Research, vol.18, no.9, pp.867-892, 1999.
文献３：Fijany,A. and Sharf,I. and D'Eleuterio,G.M.T.: "Parallel O(logN) Algorithms for Computation of Manipulator Forward Dynamics, IEEE Transactions on Robotics and Automation, vol.11, no. 8, pp.389-400, 1995.
【０００４】
【発明が解決しようとする課題】
上述した文献１〜３の技術は、いずれも並列計算により剛体リンク機構の順動力学計算を高速に行うことを目的としており、Ｎ個のリンクがあるときＯ（Ｎ）個のプロセスを使って計算時間がＯ（ｌｏｇＮ）となる。しかし、これらの方法には、収束計算により近似値を求める方法で厳密値は計算できない（文献１の問題）、直列計算と並列計算でアルゴリズムが大きく異なる（文献２の問題）、閉リンク機構など複雑な機構への適用が難しい（文献３の問題）などの問題があった。
【０００５】
本発明の目的は上述した課題を解消して、開リンク機構・閉リンク機構を含む剛体リンク機構の順動力学計算を高速に行うことのできるリンク機構において動力学的に関節加速度を求める方法を提供しようとするものである。
【０００６】
【課題を解決するための手段】
本発明のリンク機構において動力学的に関節加速度を求める方法は、リンクを関節で接続して構成されるリンク機構において、動力学的に関節加速度を求める方法であって、（１）個々のリンクがばらばらの状態から関節を１つずつ順次追加していき、関節を追加する毎に、追加した関節で発生するその時点でリンク間の関節において働く拘束力を求める第１のステップと、（２）第１のステップとは逆の順序で関節を除いていき、切断関節の確定した拘束力を用いて次に切断する関節の拘束力を確定する処理を繰り返すことによって、各関節における最終的な拘束力を計算する第２のステップと、（３）第１のステップ及び第２のステップで求めた各リンクの関節の拘束力を総合した、各リンクに働く外力を用いて、リンクの加速度を計算する第３のステップと、（４）第３のステップで求めた各関節の両側のリンクの加速度を用いて、関節加速度を計算する第４のステップと、からなることを特徴とするものである。
【０００７】
本発明では、個々のリンクがばらばらの状態から関節を１つずつ追加していき、最終的に目的のリンク機構を組み立てている。また、最後の関節以外は後で追加された関節の影響が反映されていないので、リンク機構全体を考慮するために、逆の順序で関節を切断していく分解の作業が必要となる。これら一連の第１のステップ〜第４のステップを実行することで、リンク数が非常に多いリンク機構の動力学シミュレーションがリアルタイムに近い速度で実行できるようになる。
【０００８】
なお、組立中に現れる一部のリンクからなるリンク機構を以下「部分リンク機構」と呼ぶこととする。また、本発明において「拘束力」とは、関節においては、許された方向以外の運動を拘束する（打ち消す）ために働く力を意味する。
【０００９】
本発明の好適な具体例としては、第１のステップにおいて、関節を処理する順序を適切に選択することで、任意数のプロセッサを用いた並列計算を行うこと、第１のステップにおいて、リンク間の各関節において働く拘束力を、仮想仕事の原理に基づいて導出すること、第１のステップにおいて、リンク間の各関節において働く拘束力を、あらかじめ選ばれた１個以上のリンクに作用する力と、その力によってリンクにおいて発生する加速度との関係を表す慣性行列を用いて導出することがある。いずれの場合も本発明をより好適に実施することができるため好ましい態様となる。
【００１０】
【発明の実施の形態】
本発明のリンク機構において動力学的に関節加速度を求める方法は、部分リンク機構の拘束力の計算（第１のステップ）、最終的な拘束力の計算（第２のステップ）、リンク加速度の計算（第３のステップ）、関節加速度の計算（第４のステップ）から構成されている。以下、各ステップ毎に説明する。
【００１１】
（１）部分リンク機構の拘束力の計算（第１のステップ）
全リンクがばらばらの状態から関節を１個ずつ追加し、一部の関節からなる２つのリンク機構を接続していく手順を繰り返し、最終的に全体のリンク機構を得る。図１の例では、関節ｉによって部分リンク機構ＡとＢを接続する。これによってできたリンク機構はそれ自体部分リンク機構であり、さらにリンクｌ、ｍ、ｎを介して他の部分リンク機構と接続されることになる。第１のステップでは、関節を追加する毎に、関節ｉで発生する拘束力ｆｉを求める。拘束力ｆｉを求めるためには、好ましくは仮想仕事の原理を使用する。
【００１２】
なお、閉リンク機構を構成する場合は、閉ループ内の１個のリンクについては同じ部分リンク機構の中での接続を行うことになる。すなわち、第１ステップにおいて関節を追加するときは、異なる２つの部分リンク機構を接続することを基本とするが、閉リンク機構の場合には、図２に示すように、同じ部分リンク機構内の２リンクを接続するように関節を追加することで対応することができる。
【００１３】
（２）最終的な拘束力の計算（第２のステップ）
第１のステップで全体のリンク機構における関節の拘束力を求めることができるが、最後の関節以外は後で追加された関節の影響が反映されていないので、リンク機構全体を考慮するために、逆の順序で関節を切断していく分解の作業が必要となる。それを実行するのが第２のステップである。すなわち、完全なリンク機構における拘束力をそれぞれの関節について計算する。これは、第１のステップでの操作とは逆の順序で関節を切断していき、切断関節の確定した拘束力を用いて次に切断する関節の拘束力を確定する処理を繰り返すことによって行う。図１の例では、関節ｌ、ｍ、ｎが切断され、拘束力が確定すると関節ｉの拘束力が確定できるので、続いて関節ｉを切断する。この操作が完了すると、全関節における拘束力が確定する。
【００１４】
（３）リンク加速度の計算（第３のステップ）
第１のステップ及び第２のステップにおいて、各リンクに働く外力をすべて求めることができるため、各リンクに働く拘束力を総合して、各リンクの加速度を計算する。
（４）関節加速度の計算（第４のステップ）
第３のステップで求めた各関節の両側のリンクの相対加速度を用いて、関節加速度を計算する。
【００１５】
上述した説明の第１のステップにおいて、関節を１個追加する毎に関節ｉで発生する拘束力ｆｉを求めるには、リンク機構としての慣性行列を考えることが好ましい。すなわち、関節を１個追加する毎に、関節ｉで働く拘束力と加速度の関係を表す慣性行列と関節ｉ、ｌ、ｍ、ｎで働く拘束力と加速との関係を表す慣性行列を求め、それらを利用する。以下、ここで用いる慣性行列についてさらに詳細に説明する。
【００１６】
図３に示すようなリンク機構において、リンク１、２に対して力およびモーメントｆ１、ｆ２を作用させたときに、リンク１、２で発生する加速度および角加速度をａ１、ａ２とするとき、ｆ１、ｆ２とａ１、ａ２の関係は以下の式（１）のように表される。
【００１７】
【数１】
ここでΓｉｊ（ｉ、ｊ＝１、２）がステップ１において計算される慣性逆行列であり、これの逆行列を計算することにより慣性行列が得られる。この慣性行列を用いると、リンク１、２が外部からの拘束を受けているときの拘束力の計算などに応用することができる。この慣性行列は、すべてのリンクの質量と慣性モーメントに加え、関節の拘束条件を考慮して計算されたものであり、個々のリンクの質量や慣性モーメントの単なる和ではない。リンクの質量・モーメントと関節の拘束を考慮した慣性行列の関係を示す単純な例として図４を挙げる。図４に示す例では、質量ｍ１のリンクと質量ｍ２のリンクとが左右に並進移動できるように関節で接続されている。このリンク機構において、質量ｍ１のリンクに力を加えたときの全体としての質量は、上下方向に移動させようとしたときは２つのリンクを同時に動かすことになるのでｍ１＋ｍ２となるのに対し、左右方向に移動させようとしたときは質量ｍ２のリンクが移動方向の拘束を受けないのでその質量を無視することができ、全体の質量はｍ１となる。
【００１９】
このようなリンク機構全体としての慣性行列は、リンク機構内で発生する拘束力を求める目的以外にも有用である。例えば、図５に示すようにリンク機構が環境など外部の物体と接触しているときには、接触力は外部の物体がリンクを引っ張る方向に働くことはできないという制約条件があるため、接触点における拘束力を計算して実際に接触力が発生しているかどうかを判定する必要がある。この場合は、接触しているリンクの運動、すなわち、ａ１、ａ２が拘束によって決まっているため、上述した数（１）を用いて接触力ｆ１、ｆ２を計算することができる。
【００２０】
以上のように、本発明のリンク機構において動力学的に関節加速度を求める方法は、閉ループを形成する場合を除き１個の関節を追加して異なる２つの部分リンク機構を接続する作業を繰り返し行う。接続前の部分リンク機構は力学的に独立であるから、それ以前の処理はそれぞれの部分リンク機構で並列に行うことができ、もしそれらの処理が別々のＣＰＵまたは計算機で行われているならば、全体の計算時間は部分リンク機構１つの場合と同じになり、計算時間の短縮が実現される。
【００２１】
図１の例では、関節ｉを追加する前の部分リンク機構ＡおよびＢにおける処理は並列に行ってよく、計算時間はどちらか一方のみを処理した場合と変わらない。このことから、十分な数のＣＰＵまたは計算機がある計算機環境の場合には、並列計算時の計算時間を最短にするために、途中で現れる部分リンク機構ができるだけ互いに独立になるようにスケジューリングを行うことが望ましい。
【００２２】
逆に、このようなスケジューリングを行うと各部分リンク機構が他の部分リンク機構と接続される関節（図１の関節ｌ、ｍ、ｎに相当する関節）の数が増加するが、これは２つの部分リンク機構を接続する時の計算量が増加することを意味する。したがって、ＣＰＵまたは計算機が１つしかなく直列計算しかできないような計算機環境には、このようなスケジューリングは適さず、各部分リンク機構が他の部分リンク機構と接続される関節ができるだけ少なくなるような順序で処理を行うのが望ましい。
【００２３】
８リンク７関節のシリアルリンク機構において、並列計算と直列計算に最適化された処理順序をそれぞれ図６（並列計算に最適化された処理順序）、及び、図７（直列計算に最適化された処理順序）に示す。図６及び図７を比較すると、１つの関節の追加に対する処理にかかる時間を同じとすると、計算にかかるステップが少ない分だけ図６に示す並列計算の方が図７に示す直列計算の場合と比べて格段に処理が速くなることがわかる。
【００２４】
計算プログラムを実際に利用する場合には、計算機環境に応じて別々のプログラムを用意するのは非現実的である。この点において、本計算法は直列計算と並列計算の原理的な差は全くなく、両者の間の切り替えは容易であるため、さまざまな計算機環境に対応したプログラム開発が可能である。
【００２５】
上述した本発明のリンク機構において動力学的に関節加速度を求める方法により、リンク数が非常に多いリンク機構の動力学シミュレーションがリアルタイムに近い速度で実行できるようになる。使用手順は以下の通りである。
・初期関節値、関節速度を設定する。
・計算をスタートする。計算中には時々刻々の関節トルクを入力する。
・１回の計算が終了すると、積分刻み幅分だけ進んだ時刻における状態が計算されるので、それらのデータを適宜取りだして制御などに利用する。
【００２６】
【発明の効果】
以上の説明から明らかなように、従来閉リンク機構を含む複雑な機構の動力学計算には非常に多くの計算を必要としたが、本発明の方法ではこれを高速に行うことができ、またリンク数の増加に伴う計算時間の増加が少ないため、数１００の自由度を持つリンク機構でも比較的短時間に動力学的計算を行うことができる。実際、複雑な機構にも適用可能な従来法に比べて、４０自由度程度の機構に対し直列計算時で計算時間が約１／３に短縮されることが確かめられている。さらに、各関節の加速度だけでなく、各関節における拘束力が直接計算されるので、力センサなどのシミュレーションにも応用することができる。
【００２７】
これまでは、動力学計算を直列計算する場合と並列計算する場合で全く異なる計算法を適用することが多かったが、本発明の方法はそれらを特に区別する必要はなく、関節の処理順序の変更だけでＣＰＵや計算機数の変化に対応でき、より汎用的なプログラム作成が可能となる。
【図面の簡単な説明】
【図１】本発明のリンク機構において動力学的に関節加速度を求める方法における部分リンク機構の接続を説明するための図である。
【図２】本発明のリンク機構において動力学的に関節加速度を求める方法における閉リンク機構の扱いを説明するための図である。
【図３】本発明のリンク機構において動力学的に関節加速度を求める方法におけるリンクの接続性を考慮したリンク機構の慣性行列を説明するための図である。
【図４】本発明のリンク機構において動力学的に関節加速度を求める方法における個々のリンクの質量とリンク機構としての質量の関係を説明するための図である。
【図５】本発明のリンク機構において動力学的に関節加速度を求める方法における外部から拘束を受けるリンク機構を説明するための図である。
【図６】本発明のリンク機構において動力学的に関節加速度を求める方法における並列計算に最適化された処理順序を説明するための図である。
【図７】本発明のリンク機構において動力学的に関節加速度を求める方法における直列計算に最適化された処理順序を説明するための図である。
Claims (4)
- リンクを関節で接続して構成されるリンク機構において、動力学的に関節加速度を求める方法であって、
（１）個々のリンクがばらばらの状態から関節を１つずつ順次追加していき、関節を追加する毎に、追加した関節で発生する拘束力を求める第１のステップと、
（２）第１のステップとは逆の順序で関節を除いていき、切断関節の確定した拘束力を用いて次に切断する関節の拘束力を確定する処理を繰り返すことによって、各関節における最終的な拘束力を計算する第２のステップと、
（３）第１のステップ及び第２のステップで求めた各リンクの関節の拘束力を総合した、各リンクに働く外力を用いて、リンクの加速度を計算する第３のステップと、
（４）第３のステップで求めた各関節の両側のリンクの加速度を用いて、関節加速度を計算する第４のステップと、
からなることを特徴とするリンク機構において動力学的に関節加速度を求める方法。 - 前記第１のステップにおいて、関節を処理する順序を適切に選択することで、任意数のプロセッサを用いた並列計算を行う請求項１記載のリンク機構において動力学的に関節加速度を求める方法。
- 前記第１のステップにおいて、リンク間の各関節において働く拘束力を、仮想仕事の原理に基づいて導出する請求項１記載のリンク機構において動力学的に関節加速度を求める方法。
- 前記第１のステップにおいて、リンク間の各関節において働く拘束力を、あらかじめ選ばれた１個以上のリンクに作用する力と、その力によってリンクにおいて発生する加速度との関係を表す慣性行列を用いて導出する請求項１記載のリンク機構において動力学的に関節加速度を求める方法。
Priority Applications (4)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001228804A JP3944566B2 (ja) | 2001-07-30 | 2001-07-30 | リンク機構において動力学的に関節加速度を求める方法 |
| KR10-2001-0077165A KR100426281B1 (ko) | 2001-07-30 | 2001-12-07 | 링크계 동역학 고속 계산법 |
| US10/161,609 US7058551B2 (en) | 2001-07-30 | 2002-06-05 | High-speed dynamics computation for link system |
| CA002389425A CA2389425C (en) | 2001-07-30 | 2002-06-06 | High-speed dynamics computation for link system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001228804A JP3944566B2 (ja) | 2001-07-30 | 2001-07-30 | リンク機構において動力学的に関節加速度を求める方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2003039356A JP2003039356A (ja) | 2003-02-13 |
| JP3944566B2 true JP3944566B2 (ja) | 2007-07-11 |
Family
ID=19061248
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2001228804A Expired - Lifetime JP3944566B2 (ja) | 2001-07-30 | 2001-07-30 | リンク機構において動力学的に関節加速度を求める方法 |
Country Status (4)
| Country | Link |
|---|---|
| US (1) | US7058551B2 (ja) |
| JP (1) | JP3944566B2 (ja) |
| KR (1) | KR100426281B1 (ja) |
| CA (1) | CA2389425C (ja) |
Families Citing this family (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7684896B2 (en) * | 2001-06-29 | 2010-03-23 | Honda Motor Co., Ltd. | System and method of estimating joint loads using an approach of closed form dynamics |
| US7135003B2 (en) * | 2001-06-29 | 2006-11-14 | Honda Giken Kogyo Kabushiki Kaisha | Feedback estimation of joint forces and joint moments |
| US7390309B2 (en) * | 2002-09-23 | 2008-06-24 | Honda Motor Co., Ltd. | Human assist system using gravity compensation control system and method using multiple feasibility parameters |
| US7650204B2 (en) * | 2001-06-29 | 2010-01-19 | Honda Motor Co., Ltd. | Active control of an ankle-foot orthosis |
| US7623944B2 (en) * | 2001-06-29 | 2009-11-24 | Honda Motor Co., Ltd. | System and method of estimating joint loads in a three-dimensional system |
| US7469166B2 (en) * | 2001-06-29 | 2008-12-23 | Honda Motor Co., Ltd. | System and method of predicting novel motion in a serial chain system |
| US7217247B2 (en) * | 2002-09-23 | 2007-05-15 | Honda Giken Kogyo Kabushiki Kaisha | Gravity compensation method in a human assist system and a human assist system with gravity compensation control |
| US7774177B2 (en) | 2001-06-29 | 2010-08-10 | Honda Motor Co., Ltd. | Exoskeleton controller for a human-exoskeleton system |
| JP4133216B2 (ja) * | 2001-10-29 | 2008-08-13 | 本田技研工業株式会社 | 人間補助装置のシミュレーション・システム、方法、およびコンピュータ・プログラム |
| US7402142B2 (en) * | 2002-09-23 | 2008-07-22 | Honda Giken Kogyo Kabushiki Kaisha | Method and processor for obtaining moments and torques in a biped walking system |
| JP2004220566A (ja) * | 2002-12-26 | 2004-08-05 | Toshiba Corp | 機構シミュレーション方法および機構シミュレーションプログラム |
| US8082062B2 (en) * | 2005-06-10 | 2011-12-20 | Honda Motor Co., Ltd. | Regenerative actuation in motion control |
| JP5061344B2 (ja) * | 2006-09-25 | 2012-10-31 | 国立大学法人 東京大学 | リンクの質量パラメータの推定法 |
| KR101310276B1 (ko) * | 2011-11-17 | 2013-09-24 | 이화여자대학교 산학협력단 | 연결체 강체의 동역학 시뮬레이션 방법 및 시스템 |
| CN111104563B (zh) * | 2019-11-11 | 2024-04-16 | 武汉科技大学 | 基于素数非对称邻接矩阵的运动链同构判定方法 |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5043929A (en) * | 1989-06-13 | 1991-08-27 | Schlumberger Technologies, Inc. | Closed-form kinematics |
| US5297057A (en) * | 1989-06-13 | 1994-03-22 | Schlumberger Technologies, Inc. | Method and apparatus for design and optimization for simulation of motion of mechanical linkages |
| US6004016A (en) * | 1996-08-06 | 1999-12-21 | Trw Inc. | Motion planning and control for systems with multiple mobile objects |
| JP3283532B2 (ja) * | 1997-03-17 | 2002-05-20 | 株式会社日立製作所 | 加振装置およびそれを用いた構造物の振動試験装置 |
-
2001
- 2001-07-30 JP JP2001228804A patent/JP3944566B2/ja not_active Expired - Lifetime
- 2001-12-07 KR KR10-2001-0077165A patent/KR100426281B1/ko not_active IP Right Cessation
-
2002
- 2002-06-05 US US10/161,609 patent/US7058551B2/en not_active Expired - Fee Related
- 2002-06-06 CA CA002389425A patent/CA2389425C/en not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| US7058551B2 (en) | 2006-06-06 |
| JP2003039356A (ja) | 2003-02-13 |
| CA2389425A1 (en) | 2003-01-30 |
| KR100426281B1 (ko) | 2004-04-08 |
| CA2389425C (en) | 2008-08-12 |
| KR20030012782A (ko) | 2003-02-12 |
| US20030023415A1 (en) | 2003-01-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3944566B2 (ja) | リンク機構において動力学的に関節加速度を求める方法 | |
| US8140189B2 (en) | Apparatus and method for computing operational-space physical quantity | |
| EP1334901B1 (en) | Method for generating a motion of a human type link system | |
| Long et al. | Modeling & control of a meat-cutting robotic cell | |
| Akbarzadeh et al. | Dynamics analysis of a 3-RRP spherical parallel manipulator using the natural orthogonal complement | |
| Koplik et al. | Computer generation of robot dynamics equations and the related issues | |
| Raoofian et al. | Forward dynamic analysis of parallel robots using modified decoupled natural orthogonal complement method | |
| Yang et al. | Robot kinematics and dynamics modeling | |
| Grazioso et al. | Modeling and simulation of hybrid soft robots using finite element methods: Brief overview and benefits | |
| Yamane | Simulating and generating motions of human figures | |
| McPhee et al. | Modelling multibody systems with indirect coordinates | |
| Yamane et al. | Parallel O (log n) algorithm for dynamics simulation of humanoid robots | |
| Rousso et al. | Singularity-robust full-pose workspace control of space manipulators with non-zero momentum | |
| Gonçalves et al. | Dynamic modeling of a human-inspired robot based on a Newton-Euler approach | |
| Hwang et al. | Dynamic analysis and control of industrial robotic manipulators | |
| Wee et al. | An articulated-body model for a free-flying robot and its use for adaptive motion control | |
| Chang et al. | Efficient recursive algorithm for the operational space inertia matrix of branching mechanisms | |
| Fratu et al. | Robots collision avoidance using learning through imitation | |
| Stachera et al. | Derivation and calculation of the dynamics of elastic parallel manipulators | |
| Staicu et al. | Planar Parallel Robots | |
| Lopes et al. | Dynamic Modeling of a Human-Inspired Robot Based on a Newton-Euler Approach | |
| Zhou et al. | Simplifying Dynamic Models for Parallel Robots by Introducing Virtual Degrees of Freedom | |
| Eyles | Statement of originality | |
| Yamane | 2 Inverse and Forward Dynamics of General Closed Kinematic Chains | |
| Nandihal et al. | Dynamics of Closed-Loop Systems |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20040713 |
|
| A711 | Notification of change in applicant |
Free format text: JAPANESE INTERMEDIATE CODE: A712 Effective date: 20040715 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20040913 |
|
| A911 | Transfer to examiner for re-examination before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A911 Effective date: 20041102 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A821 Effective date: 20041028 |
|
| A912 | Re-examination (zenchi) completed and case transferred to appeal board |
Free format text: JAPANESE INTERMEDIATE CODE: A912 Effective date: 20050128 |
|
| RD03 | Notification of appointment of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7423 Effective date: 20061024 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070105 |
|
| RD03 | Notification of appointment of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7423 Effective date: 20070105 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 3944566 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| EXPY | Cancellation because of completion of term |