JP5959398B2 - 剛体多体系の運動方程式導出装置および剛体多体系の運動方程式導出方法 - Google Patents

Description

本発明は、機械システムのダイナミクス解析に際して、機械システムを剛体多体系としてモデル化し、当該モデルに基づいて機械システムの運動方程式を導出することのできる剛体多体系の運動方程式導出装置および剛体多体系の運動方程式導出方法に関するものである。

従来の機械システムの運動方程式導出装置として、ホロノーム機械系分析装置および方法がある（例えば、特許文献１参照）。このホロノーム機械系分析装置および方法では、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力する。そして、入力されたモデル構成に基づき、当該モデルに対して仮想パワーの原理に基づくケイン法を適用することで、対象とする機械システムの運動方程式を自動的に導出する。

すなわち、特許文献１に記載された従来の剛体多体系の運動方程式導出装置では、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出することができる。

特開平６−２２３０４９号公報

しかしながら、従来技術には、以下のような課題がある。
特許文献１に記載された従来の剛体多体系の運動方程式導出装置では、当該システムの一般化力を、各剛体に作用する外力ベクトルと当該剛体の部分速度との内積、および各剛体の質量中心まわりに作用する外モーメントベクトルと当該剛体の部分角速度との内積を全剛体について加算して導出している。このため、運動方程式の導出に必要な計算量が増加するという課題があった。

また、特許文献１に記載された従来の剛体多体系の運動方程式導出装置では、当該システムの一般化慣性力を、各剛体の慣性力ベクトルと当該剛体の部分速度との内積、および各剛体の質量中心まわりの慣性トルクベクトルと当該剛体の部分角速度との内積を全剛体について加算して導出している。このため、所定剛体に接続される剛体数の増加に伴い、導出した運動方程式において、一般化慣性力の記述量が大幅に増加するという課題もあった。

加えて、特許文献１に記載された従来の剛体多体系の運動方程式導出装置では、ある剛体の相対運動に微小量が含まれる場合でも、当該微小量に関する高次項を消去するような線形化を行っていない。このため、運動方程式導出に必要な計算量、および導出した運動方程式の記述量が大幅に増加するという課題もあった。

本発明は上記のような課題を解決するためになされたものであり、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出する際に、運動方程式導出に必要な計算量、および導出した運動方程式の記述量を低減することのできる剛体多体系の運動方程式導出装置および剛体多体系の運動方程式導出方法を得ることを目的とする。

本発明に係る剛体多体系の運動方程式導出装置は、各剛体の接続構成が開ループ構成となる機械システムに対して、設計者が当該機械システムを剛体多体系としてモデル化した際に、モデル構成を定義づけて入力するための手段である剛体多体系構成定義部と、剛体多体系構成定義部で定義されたモデル構成に基づいて、各剛体の連結構成を導出する剛体連結構成導出部と、剛体多体系構成定義部で定義されたモデル構成に基づいて、剛体連結部で相互に連結される剛体間の相対回転変位から，剛体間の相対回転姿勢を規定する座標変換行列と，剛体間の相対回転速度を相対角速度に変換する第１変換行列とを、剛体連結部における変換行列として導出する変換行列導出部と、剛体多体系構成定義部で定義されたモデル構成、および変換行列導出部で導出された剛体連結部における変換行列に基づいて、各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出する慣性力・慣性トルク導出部と、剛体多体系構成定義部で定義されたモデル構成、剛体連結構成導出部で導出された各剛体の連結構成、および変換行列導出部で導出された剛体連結部における変換行列に基づいて、各剛体の一般化力を導出する一般化力導出部と、剛体多体系構成定義部で定義されたモデル構成、および慣性力・慣性トルク導出部で導出された各剛体の質量中心速度と角速度に基づいて、各剛体の部分速度および部分角速度を導出する部分速度・部分角速度導出部と、剛体連結構成導出部で導出された各剛体の連結構成、慣性力・慣性トルク導出部で導出された各剛体の慣性力および質量中心まわりの慣性トルク、一般化力導出部で導出された各剛体の一般化力、および部分速度・部分角速度導出部で導出された各剛体の部分速度および部分角速度に基づいて、各剛体の一般化慣性力および運動方程式を導出する一般化慣性力導出部とを備えるものである。

また、本発明に係る剛体多体系の運動方程式導出方法は、各剛体の接続構成が開ループ構成となる機械システムを設計者が剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出する剛体多体系の運動方程式導出装置に用いられる剛体多体系の運動方程式導出方法であって、機械システムに対して、設計者が当該機械システムを剛体多体系としてモデル化した際に、モデル構成を定義づけて入力する剛体多体系構成定義ステップと、剛体多体系構成定義ステップで定義されたモデル構成に基づいて、各剛体の連結構成を導出する剛体連結構成導出ステップと、剛体多体系構成定義ステップで定義されたモデル構成に基づいて、剛体連結部で相互に連結される剛体間の相対回転変位から，剛体間の相対回転姿勢を規定する座標変換行列と，剛体間の相対回転速度を相対角速度に変換する第１変換行列とを、剛体連結部における変換行列として導出する変換行列導出ステップと、剛体多体系構成定義ステップで定義されたモデル構成、および変換行列導出ステップで導出された剛体連結部における変換行列に基づいて、各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出する慣性力・慣性トルク導出ステップと、剛体多体系構成定義ステップで定義されたモデル構成、剛体連結構成導出ステップで導出された各剛体の連結構成、および変換行列導出ステップで導出された剛体連結部における変換行列に基づいて、各剛体の一般化力を導出する一般化力導出ステップと、剛体多体系構成定義ステップで定義されたモデル構成、および慣性力・慣性トルク導出ステップで導出された各剛体の質量中心速度と角速度に基づいて、各剛体の部分速度および部分角速度を導出する部分速度・部分角速度導出ステップと、剛体連結構成導出ステップで導出された各剛体の連結構成、慣性力・慣性トルク導出ステップで導出された各剛体の慣性力および質量中心まわりの慣性トルク、一般化力導出ステップで導出された各剛体の一般化力、および部分速度・部分角速度導出ステップで導出された各剛体の部分速度および部分角速度に基づいて、各剛体の一般化慣性力および運動方程式を導出する一般化慣性力導出ステップとを備えるものである。

本発明によれば、各剛体の接続構成が開ループ構成となる機械システムの一般化力導出に際して、各剛体の接続構成を考慮して独自の定式化手法を適用することにより、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出する際に、運動方程式導出に必要な計算量を低減することのできる剛体多体系の運動方程式導出装置および剛体多体系の運動方程式導出方法を得ることができる。

本発明の実施の形態１〜３で説明する機械システムにおける剛体連結構成例を示す構成図である。 本発明の実施の形態１〜３による剛体多体系の運動方程式導出装置において、処理の全体構成を示すブロック図である。 本発明の実施の形態１〜３による剛体多体系の運動方程式導出装置において、各剛体に固定する剛体固定枠、および各剛体の相対変位の定義を示す構成図である。 本発明の実施の形態１〜３による剛体多体系の運動方程式導出装置において、各剛体への作用力、および作用モーメントの定義を示す構成図である。 本発明の実施の形態１〜３による剛体多体系の運動方程式導出装置において、剛体連結構成導出部の処理構成を示すフローチャートである。 本発明の実施の形態２、３による剛体多体系の運動方程式導出装置において、一般化慣性力導出部の処理構成を示すフローチャートである。 本発明の実施の形態２、３による剛体多体系の運動方程式導出装置において、並進運動に関する一般化慣性力導出部の第ｉ行ブロック処理における処理構成を示すフローチャートである。 本発明の実施の形態２、３による剛体多体系の運動方程式導出装置において、回転運動に関する一般化慣性力導出部の第ｉ行ブロック処理における処理構成を示すフローチャートである。 本発明の実施の形態３による剛体多体系の運動方程式導出装置において、線形化の処理構成を示すフローチャートである。 本発明の実施の形態３による剛体多体系の運動方程式導出装置において、部分速度・部分角速度導出部の処理構成を示すフローチャートである。

以下、本発明の剛体多体系の運動方程式導出装置および剛体多体系の運動方程式導出方法の好適な実施の形態につき図面を用いて説明する。

実施の形態１．
図１は、本発明の実施の形態１で説明する機械システムにおける剛体連結構成例を示す構成図である。図１に示すように、一般に機械システム１０は、設置土台をベースとして、複数の剛体１〜剛体８が連結して構成された剛体多体系としてモデル化することができる。

なお、図１では、剛体数が８個で構成された機械システム１０を例示しているが、剛体数は、これに限定されるものではない。また、本発明では、各剛体の接続構成が、開ループ構成となる機械システム１０を前提としている。この前提のもと、機械システム１０を構成する剛体数、所定剛体に接続される剛体数（一例として、図１の剛体１に接続される剛体数は、剛体２および剛体７の２個に相当する）について、任意の機械システム１０を対象としている。

図２は、本発明の実施の形態１による剛体多体系の運動方程式導出装置において、処理の全体構成を示すブロック図である。図２に示すように、本発明の実施の形態１による剛体多体系の運動方程式導出装置は、剛体多体系構成定義部１１、剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６、および一般化慣性力導出部１７を備えている。

剛体多体系構成定義部１１は、設計者が対象とする機械システム１０を剛体多体系としてモデル化し、当該モデルの構成を入力するための手段である。剛体連結構成導出部１２は、剛体多体系構成定義部１１で定義されたモデル構成に基づいて、剛体多体系を構成する各剛体（先の図１では、剛体１〜剛体８）に対して、先端側に接続された全ての剛体を導出し、剛体連結構成として出力する。

変換行列導出部１３は、剛体多体系構成定義部１１で定義されたモデル構成に基づいて、各剛体連結部における変換行列を導出して出力する。慣性力・慣性トルク導出部１４は、剛体多体系構成定義部１１で定義されたモデル構成、および変換行列導出部１３で導出された各剛体連結部における変換行列に基づいて、剛体多体系を構成する各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出して出力する。

一般化力導出部１５は、剛体多体系構成定義部１１で定義されたモデル構成、剛体連結構成導出部１２で導出された各剛体の連結構成、および変換行列導出部１３で導出された各剛体連結部における変換行列に基づいて、剛体多体系を構成する各剛体に対応する一般化力を導出して出力する。

部分速度・部分角速度導出部１６は、剛体多体系構成定義部１１で定義されたモデル構成、および慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度と角速度に基づいて、剛体多体系を構成する各剛体に対応する部分速度および部分角速度を導出して出力する。

さらに、一般化慣性力導出部１７は、剛体連結構成導出部１２で導出した各剛体の連結構成、慣性力・慣性トルク導出部１４で導出した各剛体の慣性力および質量中心まわりの慣性トルク、一般化力導出部１５で導出した各剛体の一般化力、および部分速度・部分角速度導出部１６で導出した各剛体の部分速度および部分角速度に基づいて、剛体多体系を構成する各剛体の一般化慣性力および運動方程式を導出して出力する。

図３は、本発明の実施の形態１による剛体多体系の運動方程式導出装置において、各剛体に固定する剛体固定枠、および各剛体の相対変位の定義を示す構成図である。図３を用いて、剛体ｋ（符号２１）に対して先端側に剛体ｌ（符号２２）が接続されている場合について説明する。

この場合、各剛体に固定する剛体固定枠、および各剛体の相対変位の定義は、以下のようになる。
（１）剛体ｋに関する剛体固定枠
（１−１）枠ｋ：剛体ｋの慣性主軸に固定した慣性主軸剛体固定枠
（１−２）枠ｋｌ：剛体ｋと剛体ｌとの連結部を規定し、剛体ｋに固定した連結部剛体固定枠
（２）剛体ｌに関する剛体固定枠
（２−１）枠ｌ：剛体ｌの慣性主軸に固定した慣性主軸剛体固定枠
（２−２）枠ｌｋ：剛体ｋと剛体ｌとの連結部を規定し、剛体ｌに固定した連結部剛体固定枠
（３）剛体ｋに対する剛体ｌの相対変位
（３−１）相対並進変位ｔ_ｋｌ＝［ｘ_ｋｌ ｙ_ｋｌ ｚ_ｋｌ］^Ｔ
：連結部剛体固定枠ｋｌで表現した、連結部剛体固定枠ｋｌに対する連結部剛体固定枠ｌｋの相対並進変位代数ベクトル
（３−２）相対回転変位θ_ｋｌ＝［θ_Ｘｋｌ θ_Ｙｋｌ θ_Ｚｋｌ］^Ｔ
：オイラー角で表現した、連結部剛体固定枠ｋｌに対する連結部剛体固定枠ｌｋの相対回転変位

図４は、本発明の実施の形態１による剛体多体系の運動方程式導出装置において、各剛体への作用力、および作用モーメントの定義を示す構成図である。図４を用いて、剛体ｋ（符号２１）に対して先端側に剛体ｌ（符号２２）が接続され、剛体ｌ（符号２２）に対して先端側に剛体ｍ（符号２３）が接続されている場合について説明する。

この場合、剛体ｌへの作用力、および作用モーメントを、以下の（Ｉ）〜（ＶＩ）で定義する。

図５は、本発明の実施の形態１による剛体多体系の運動方程式導出装置において、剛体連結構成導出部１２の処理構成を示すフローチャートである。図５に示すように、本実施の形態１による剛体連結構成導出部１２は、処理ブロック３１〜３７を備えている。

そして、剛体連結構成導出部１２は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・機械システム１０を構成する全剛体数ｎ_ｂ（図１ではｎ_ｂ＝８に相当する）
・設置土台となるベースを剛体０として、剛体１→剛体ｎ_ｂの順に、各剛体のベース側接続剛体番号を定義した剛体接続元ベクトルＢｏｄｙ＿ｔｏ＿Ｂａｓｅ（図１ではＢｏｄｙ＿ｔｏ＿Ｂａｓｅ＝［０ １ ２ ３ ４ ３ １ ７］に相当する）
を用いて、剛体多体系を構成する各剛体に対して、先端側に接続された全ての剛体を導出し、剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓとして出力する。

次に、図１〜図５に基づいて、本実施の形態１における剛体多体系の運動方程式導出装置の動作について説明する。
図１に示すように、対象とする機械システム１０に関して、設計者は、設置土台をベース（剛体０）とし、複数の剛体が連結して構成された剛体多体系として機械システム１０をモデル化する。この際、複数の剛体に割り付けるそれぞれの剛体番号を、ベースから先端に向かって増加するように、剛体１、剛体２、・・・と定義する。

次に、図２に示す剛体多体系構成定義部１１において、設計者が当該モデルの構成を入力する。剛体多体系構成定義部１１において、設計者が定義するモデル構成を表１に示す。

図２に示す剛体連結構成導出部１２は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・全剛体数ｎ_ｂ
・剛体接続元ベクトルＢｏｄｙ＿ｔｏ＿Ｂａｓｅ
を用いて、剛体多体系を構成する各剛体に対して、先端側に接続された全ての剛体を導出し、剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓとして出力する。

さらに、剛体連結構成導出部１２は、先の図５に示したフローチャートに従って、一連処理を行う。まず、剛体連結構成導出部１２は、処理ブロック３１において、はじめに剛体連結構成の導出対象を剛体ｉ＝ｎ_ｂに設定する。

次に、剛体連結構成導出部１２は、処理ブロック３２において、剛体ｉに関するベース側接続剛体ｍを下式（１）によって取得する。

次に、剛体連結構成導出部１２は、処理ブロック３３において、剛体ｉのベース側接続剛体ｍがベース（剛体０）か否かを判定する。そして、ベース側接続剛体ｍがベース（剛体０）である場合には、処理ブロック３６へ、ベース側接続剛体ｍが機械システム１０を構成する剛体である場合には、処理ブロック３４へ分岐する。

処理ブロック３４に進んだ場合には、剛体連結構成導出部１２は、列が機械システム１０を構成する剛体番号１〜ｎ_ｂ、行が所定剛体に対する先端側接続剛体番号に対応する剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓに対して、剛体ｍに対する先端側接続剛体として、剛体ｉを下式（２）で追加する。

次に、剛体連結構成導出部１２は、処理ブロック３５において、剛体ｉに対する先端側接続剛体を、剛体ｍに対する先端側接続剛体として、下式（３）で追加する。

さらに、剛体連結構成導出部１２は、処理ブロック３６において、剛体連結構成の導出対象となる剛体番号ｉをデクリメントする。

最後に、剛体連結構成導出部１２は、処理ブロック３７において、剛体番号ｉがｉ≧１であれば、処理ブロック３２へ分岐して処理を繰り返し実行し、ｉ＝０であれば、一連処理を完了する。剛体連結構成導出部１２における以上の処理により、剛体多体系を構成する各剛体に対して、先端側に接続された全剛体が、剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓとして導出される。例えば、図１に示す機械システム１０の場合の剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓは、下式（４）となる。

続いて、図２に示す変換行列導出部１３は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・剛体ｋに対する剛体ｌの相対回転変位θ_ｋｌ＝［θ_Ｘｋｌ θ_Ｙｋｌ θ_Ｚｋｌ］^Ｔ
を用いて、剛体多体系を構成する各剛体の剛体連結部における変換行列として、連結部剛体固定枠ｋｌから連結部剛体固定枠ｌｋへの座標変換行列Ａ^{ｌｋ、ｋｌ}を、下式（５）で導出する。

さらに、変換行列導出部１３は、剛体ｋに対する剛体ｌの相対回転速度

から、連結部剛体固定枠ｋｌに対する連結部剛体固定枠ｌｋの相対角速度代数ベクトル

（連結部剛体固定枠ｋｌ表現）への変換式として下式（６）

で定義される変換行列Ｇ_ｋｌを、下式（７）で導出する。

ただし、ここでは表現の簡潔のために

という略記法を用いており、かつ剛体ｋに対する剛体ｌの相対回転変位θ_ｋｌとして１−２−３系オイラー角を適用している。

図２に示す慣性力・慣性トルク導出部１４は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・剛体質量ｍ_ｌ
・剛体慣性テンソル^ＣＩ_ｌ、ｌ
・剛体ｋに対する剛体ｌの相対並進変位ｔ_ｋｌ
・剛体ｋに対する剛体ｌの相対回転変位θ_ｋｌ
・変換行列Ａ^ｌ、ｌｋおよびＡ^ｋｌ、ｋ
・剛体構成ｒ_ｌ ^ｌ、ｌｋおよびｒ_ｋ ^ｋ、ｋｌ
と、変換行列導出部１３で導出された座標変換行列Ａ^{ｌｋ、ｋｌ}およびＧ_ｋｌを用いて、はじめに剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）に関して、慣性基準枠Ｏに対する慣性主軸剛体固定枠ｌの角速度代数ベクトルω_ｌ ^Ｏｌ（慣性主軸剛体固定枠ｌ表現）を、ベース側から先端側に向かって剛体ｌ＝１、２、３・・・の順に、下式（８）で導出する。

次に、慣性力・慣性トルク導出部１４は、慣性基準枠Ｏに対する慣性主軸剛体固定枠ｌの質量中心速度代数ベクトルｖ_ｌ ^Ｏｌ（慣性主軸剛体固定枠ｌ表現）を、ベース側から先端側に向かって剛体ｌ＝１、２、３・・・の順に、下式（９）で導出する。

次に、慣性力・慣性トルク導出部１４は、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）に関して、慣性主軸剛体固定枠ｌで表現した慣性力代数ベクトルｍ_ｌａ_ｌ ^Ｏｌを下式（１０）

で導出し、慣性主軸剛体固定枠ｌで表現した質量中心まわりの慣性トルク代数ベクトル

を、下式（１１）

で導出し、全慣性力代数ベクトルｄＬｌを下式（１２）

で構成し、全慣性トルク代数ベクトルｄＨｌ＿ｌを下式（１３）

で構成する。

ただし、ここでは任意の代数ベクトルｒ＝［ｘ ｙ ｚ］^Ｔに関するチルダマトリックス

として、下式（１４）を用いている。

図２に示す一般化力導出部１５は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・剛体ｋに対する剛体ｌの相対並進変位ｔ_ｋｌ
・変換行列Ａ^ｌ、ｌｋおよびＡ^ｋｌ、ｋ
・剛体構成ｒ_ｌ ^ｌ、ｌｋおよびｒ_ｋ ^ｋ、ｋｌ
・剛体駆動力ｆ_{ｋｌ、ｋｌ}および剛体外力ｆ_ｌ、Ｏ
・剛体駆動トルクｎ_{ｋｌ、ｋｌ}および剛体外モーメントｎ_ｌ、Ｏ
と、剛体連結構成導出部１２で導出された剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓと、変換行列導出部１３で導出された座標変換行列Ａ^{ｌｋ、ｋｌ}およびＧ_ｋｌを用いて、はじめに剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）に関して、ベース側からの剛体連結構成を剛体ｊ→剛体ｋ→剛体ｌとして、ベース側から先端側に向かって剛体ｌ＝１、２、３・・・の順に、下式（１５）〜（１７）で与えられる座標変換行列を導出する。

次に、一般化力導出部１５は、剛体ｌ（ｌ＝１〜ｎ_ｂ）に対する全ての先端側接続剛体ｍ（ｍ＝Ｃｏｎｎｅｃｔｅｄ＿ｂｏｄｙｓ［：、ｌ］の非ゼロ要素）に関するベース側からの剛体連結構成を剛体ｈ→剛体ｍとして、ベース側から先端側に向かって、下式（１８）で与えられるチルダマトリックスを導出する。

以上の準備のもと、一般化力導出部１５は、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）に関して、相対並進運動に関する一般化力Ｑ_ｔｌを、下式（１９）で導出する。

上式（１９）において、右辺第１項は、剛体ｌが接続されているベース側剛体ｋから、剛体連結部を介して剛体ｌへ作用する駆動力の連結部剛体固定枠ｋｌによる表現であり、駆動力項に相当する。

一方、上式（１９）の右辺第２項は、剛体ｌおよび剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外力の和の連結部剛体固定枠ｋｌによる表現であり、外力項に相当する。

このように、本実施の形態１による剛体多体系の運動方程式導出装置は、相対並進運動に関する一般化力Ｑ_ｔｌを、駆動力項と外力項の和で与えている。

次に、一般化力導出部１５は、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）に関して、相対回転運動に関する一般化力Ｑ_θｌを、下式（２０）で導出する。

上式（２０）において、右辺大括弧内の第１項は、剛体ｌが接続されているベース側剛体ｋから、剛体連結部を介して剛体ｌへ作用する駆動トルクの連結部剛体固定枠ｋｌによる表現であり、駆動トルク項に相当する。

一方、上式（２０）における右辺大括弧内の第２項は、剛体ｌおよび剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外モーメントの和の連結部剛体固定枠ｋｌによる表現であり、外モーメント項に相当する。

また、上式（２０）における右辺大括弧内の第３項は、剛体ｌおよび剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外力による、剛体連結部固定枠ｌｋの原点に対するモーメントの和の連結部剛体固定枠ｋｌによる表現であり、外力項に相当する。

このように、本実施の形態１による剛体多体系の運動方程式導出装置は、相対回転運動に関する一般化力Ｑ_θｌを、駆動トルク項、外モーメント項、および外力項の和に対して、変換行列導出部１３で導出した剛体連結部における変換行列Ｇ_ｋｌの転置行列を作用させた式で与えている。

一般化力導出部１５は、最後に、全並進一般化力ベクトルＱｔを下式（２１）

で構成し、全回転一般化力ベクトルＱｒを下式（２２）

で構成する。

図２に示す部分速度・部分角速度導出部１６は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・剛体ｋに対する剛体ｌの相対並進変位ｔ_ｋｌ
・剛体ｋに対する剛体ｌの相対回転変位θ_ｋｌ
と、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖ_ｌ ^Ｏｌ、および角速度代数ベクトルω_ｌ ^Ｏｌを用いて、はじめに相対並進変位ｔ_ｋｌに対応する一般化スピードｕ_ｔ、および相対回転変位θ_ｋｌに対応する一般化スピードｕ_ｒを、それぞれ下式（２３）、（２４）で導出する。

ただし、ここでは、剛体ｌのベース側接続剛体を、アンダーバーを付した剛体ｌとして表現している。

次に、部分速度・部分角速度導出部１６は、一般化スピードｕ_ｔおよびｕ_ｒのうち、所定要素ｕ_ｋに対する部分速度を下式（２５）

で、部分角速度を下式（２６）

で導出し、部分速度行列ＰＶｔおよびＰＶｒ、部分角速度行列ＰＷｒを、それぞれ下式（２７）〜（２９）で構成する。

なお、一般化スピードｕ_ｔに対する部分角速度は、常にゼロとなる。

図２に示す一般化慣性力導出部１７は、慣性力・慣性トルク導出部１４で導出した全慣性力代数ベクトルｄＬｌ、および全慣性トルク代数ベクトルｄＨｌ＿ｌと、一般化力導出部１５で導出した全並進一般化力ベクトルＱｔ、および全回転一般化力ベクトルＱｒと、部分速度・部分角速度導出部１６で導出した部分速度行列ＰＶｔ、ＰＶｒ、および部分角速度行列ＰＷｒを用いて、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）の並進運動に関する一般化慣性力行列ＧＩＦｔを、下式（３０）

で導出し、回転運動に関する一般化慣性力行列ＧＩＦｒを、下式（３１）

で導出し、機械システムの運動方程式として、下式（３２）、（３３）を出力する。

以上のように、本実施の形態１による剛体多体系の運動方程式導出装置によれば、処理全体を、剛体多体系構成定義部１１、剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６、および一般化慣性力導出部１７で構成している。そして、剛体多体系構成定義部１１以外の各部１２〜１７を自動化している。

このような構成を備えることで、設計者が対象とする機械システムを剛体多体系としてモデル化し、当該モデルの構成を剛体多体系構成定義部１１において入力するだけで、独自の定式化手法により、機械システムの運動方程式を自動的に導出することができる。

また、本実施の形態１による剛体多体系の運動方程式導出装置によれば、剛体連結構成導出部１２において、所定剛体に対して先端側に接続された全剛体を導出している。さらに、一般化力導出部１５において、所定剛体の並進運動に対応する一般化力を、駆動力項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和で与え、所定剛体の回転運動に対応する一般化力を、駆動トルク項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外モーメント項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和に対して、剛体連結部における変換行列を作用させたもので与えている。この結果、一般化力の導出に関して、導出に必要な計算量を低減することができる。

なお、本実施の形態１による剛体多体系の運動方程式導出装置は、先の図１に示す機械システム１０を対象として説明したが、本発明はこれに限るものではない。機械システムにおける各剛体の接続構成が開ループ構成となっていれば、機械システムを構成する剛体数、および所定剛体に接続される剛体数について、任意の機械システムを対象としている。

また、本実施の形態１による剛体多体系の運動方程式導出装置は、所定剛体の相対回転変位として１−２−３系オイラー角を適用しているが、本発明は、これに限るものではなく、任意表現のオイラー角を適用することができる。

このように、実施の形態１によれば、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力するだけで、ケイン法に基づいて自動的に当該システムの運動方程式が導出できることに加えて、対象とする機械システムの一般化力導出に際して独自の定式化手法を適用することにより、運動方程式導出に必要な計算量を低減するような、剛体多体系の運動方程式導出装置を得ることができる。

実施の形態２．
本実施の形態２では、一般化慣性力導出部１７の機能を詳細に説明する。

本実施の形態２による剛体多体系の運動方程式導出装置において、機械システムにおける剛体連結構成例を示す構成図、処理の全体構成を示すブロック図、および各剛体の剛体固定枠、相対変位の定義は、先の実施の形態１における図１、図２、および図３と同一である。

また、本実施の形態２による剛体多体系の運動方程式導出装置において、各剛体への作用力、作用モーメントの定義、および剛体連結構成導出部１２の処理構成を示すフローチャートは、先の実施の形態１における図４、および図５と同一である。

図６は、本発明の実施の形態２による剛体多体系の運動方程式導出装置において、一般化慣性力導出部１７の処理構成を示すフローチャートである。図６に示すように、本実施の形態２による一般化慣性力導出部１７は、処理ブロック４１〜４６を備えている。

そして、一般化慣性力導出部１７は、剛体連結構成導出部１２で導出した剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓと、慣性力・慣性トルク導出部１４で導出した全慣性力代数ベクトルｄＬｌ、および全慣性トルク代数ベクトルｄＨｌ＿ｌと、一般化力導出部１５で導出した全並進一般化力ベクトルＱｔ、および全回転一般化力ベクトルＱｒと、部分速度・部分角速度導出部１６で導出した部分速度行列ＰＶｔ、ＰＶｒ、および部分角速度行列ＰＷｒを用いて、剛体多体系を構成する各剛体に対する一般化慣性力として、並進運動に関する一般化慣性力行列ＧＩＦｔ、および回転運動に関する一般化慣性力行列ＧＩＦｒを導出し、機械システムの運動方程式を出力する。

図７は、本発明の実施の形態２による剛体多体系の運動方程式導出装置において、並進運動に関する一般化慣性力導出部１７の第ｉ行ブロック処理４３における処理構成を示すフローチャートである。

図７に示すように、本実施の形態２による一般化慣性力導出部１７の第ｉ行ブロック処理４３は、処理ブロック５１〜５７を備えている。そして、第ｉ行ブロック処理４３は、剛体ｉの相対並進運動に対応する部分速度ＰＶｔ［３ｉ−ｊ、：］（ｊ＝０〜２）と、剛体ｉに対して先端側に接続された先端側接続剛体ｍの相対並進運動に対応する部分速度ＰＶｔ［３ｍ−ｊ、：］（ｊ＝０〜２）とを比較し、剛体ｉの相対並進運動に対応する部分速度ＰＶｔ［３ｉ−ｊ、：］のうち先端側接続剛体ｍに対応する項と、先端側接続剛体ｍの相対並進運動に対応する部分速度ＰＶｔ［３ｍ−ｊ、：］とが一致した場合に、並進運動に関して剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］、および一般化力Ｑｔ［３ｉ−ｊ］から、先端側接続剛体ｍの部分速度ＰＶｔ［３ｍ−ｊ、：］、および一般化力Ｑｔ［３ｍ−ｊ］を減じたものを、あらためて剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］、および一般化力Ｑｔ［３ｉ−ｊ］として出力する。

図８は、本発明の実施の形態２による剛体多体系の運動方程式導出装置において、回転運動に関する一般化慣性力導出部１７の第ｉ行ブロック処理４３’における処理構成を示すフローチャートである。

図８に示すように、本発明の実施の形態２による一般化慣性力導出部１７の第ｉ行ブロック処理４３’は、処理ブロック５１、５２、５３’〜５５’、５６、および５７を備えている。そして、第ｉ行ブロック処理４３’は、剛体ｉの相対回転運動に対応する部分角速度ＰＷｒ［３ｉ−ｊ、：］（ｊ＝０〜２）と、剛体ｉに対して先端側に接続された先端側接続剛体ｍの相対回転運動に対応する部分角速度ＰＷｒ［３ｍ−ｊ、：］（ｊ＝０〜２）とを比較し、剛体ｉの相対回転運動に対応する部分角速度ＰＷｒ［３ｉ−ｊ、：］のうち先端側接続剛体ｍに対応する項と、先端側接続剛体ｍの相対回転運動に対応する部分角速度ＰＷｒ［３ｍ−ｊ、：］とが一致した場合に、回転運動に関して剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］、部分角速度ＰＷｒ［３ｉ−ｊ、：］、および一般化力Ｑｒ［３ｉ−ｊ］から、先端側接続剛体ｍの部分速度ＰＶｒ［３ｍ−ｊ、：］、部分角速度ＰＷｒ［３ｍ−ｊ、：］、および一般化力Ｑｒ［３ｍ−ｊ］を減じたものを、あらためて剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］、部分角速度ＰＷｒ［３ｉ−ｊ、：］、および一般化力Ｑｒ［３ｉ−ｊ］として出力する。

次に、先の図１〜図５に加え、図６〜図８に基づいて、本実施の形態２における剛体多体系の運動方程式導出装置の動作について説明する。
図１に示すような機械システム１０に関して、設計者によるモデル化の方法は、先の実施の形態１と同一である。

また、図２に示す剛体多体系構成定義部１１におけるモデル構成の定義、および剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６における処理内容は、全て、先の実施の形態１と同一である。

そこで、図２に示す一般化慣性力導出部１７における、本実施の形態２での具体的な処理構成を、図６を用いて詳述する。一般化慣性力導出部１７は、処理ブロック４１において、はじめに、一般化慣性力の導出対象となる剛体番号を、ｉ＝ｎ_ｂに設定する。

次に、一般化慣性力導出部１７は、処理ブロック４２において、剛体ｉに対する先端側接続剛体の有無を、剛体連結構成導出部１２で導出した剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｉ］で判定する。そして、一般化慣性力導出部１７は、剛体ｉに対する先端側接続剛体が存在しない場合（Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｉ］＝０）には、処理ブロック４４へ、剛体ｉに対する先端側接続剛体が存在する場合（Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｉ］≠０）には、処理ブロック４３へ分岐する。

一般化慣性力導出部１７は、処理ブロック４３に進んだ場合における並進運動に関する具体的な処理を、図７に示した処理ブロック５１〜５７によって行う。

一般化慣性力導出部１７は、処理ブロック５１において、はじめに、制御変数ｋ＝ｌに設定する。次に、一般化慣性力導出部１７は、処理ブロック５２において、剛体ｉに関してベース側からｋ番目の先端側接続剛体ｍを、下式（３４）で取得する。

そして、先端側接続剛体ｍが存在しない場合（ｍ＝０）には、処理ブロック５６へ、先端側接続剛体ｍが存在する場合（ｍ≠０）には、処理ブロック５３へ分岐する。

処理ブロック５３に進んだ場合には、一般化慣性力導出部１７は、剛体ｉの相対並進運動に対応する部分速度のうち、Ｚ〜Ｘ成分（ｊ＝０〜２）の各々について、先端側接続剛体ｍに対応する項である下式（３５）

と、先端側接続剛体ｍの相対並進運動に対応する部分速度のうち、先端側接続剛体ｍに対応する項である下式（３６）

とを比較する。そして、両者が一致する場合には、処理ブロック５４へ、一致しない場合には、処理ブロック５６へ分岐する。

処理ブロック５４に進んだ場合には、以下の条件（ａ）または条件（ｂ）のどちらか一方が成立する場合に処理ブロック５５へ、条件（ａ）および条件（ｂ）がともに不成立となる場合に処理ブロック５６へ分岐する。

条件（ａ）
先端側接続剛体ｍに対する先端側接続剛体が存在せず、下式（３７）

が成立する。

条件（ｂ）
先端側接続剛体ｍに対する全ての先端側接続剛体、つまりＣｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｍ］における全ての非ゼロ要素ｌについて、剛体ｉの相対並進運動に対応する部分速度のうち剛体ｌに対応する項である下式（３８）

と、先端側接続剛体ｍの相対並進運動に対応する部分速度のうち剛体ｌに対応する項である下式（３９）

が一致する。

処理ブロック５５に進んだ場合には、一般化慣性力導出部１７は、処理ブロック５４において、条件（ａ）または条件（ｂ）のどちらか一方が成立した成分ｊについて、並進運動に関する剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］から、先端側接続剛体ｍの部分速度ＰＶｔ［３ｍ−ｊ、：］を減じた、下式（４０）を、あらためて剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］とする。

さらに、一般化慣性力導出部１７は、一般化力Ｑｔ［３ｉ−ｊ］から、先端側接続剛体ｍの一般化力Ｑｔ［３ｍ−ｊ］を減じた、下式（４１）を、あらためて剛体ｉの一般化力Ｑｔ［３ｉ−ｊ］とする。

最後に、一般化慣性力導出部１７は、処理ブロック５６において、制御変数ｋをインクリメントし、処理ブロック５７において、制御変数ｋがｋ≦ｎ_ｂ−１であれば処理ブロック５２へ分岐して処理を繰り返し実行し、ｋ＝ｎ_ｂであれば処理ブロック４３の一連処理を完了する。

また、一般化慣性力導出部１７は、処理ブロック４３に進んだ場合における、回転運動に関する具体的な処理を、図８に示した処理ブロック５１、５２、５３’〜５５’、５６、および５７によって行う。なお、図８における処理ブロック５１、５２、５６、および５７は、先の図７に示した並進運動に関する処理構成の処理ブロック５１、５２、５６、および５７と同一である。

処理ブロック５３’に進んだ場合には、一般化慣性力導出部１７は、剛体ｉの相対回転運動に対応する部分角速度のうち、Ｚ〜Ｘ成分（ｊ＝０〜２）の各々について、先端側接続剛体ｍに対応する項である下式（４２）

と、先端側接続剛体ｍの相対回転運動に対応する部分角速度のうち、先端側接続剛体ｍに対応する項である下式（４３）

とを比較する。そして、両者が一致する場合には、処理ブロック５４’へ、一致しない場合には、処理ブロック５６へ分岐する。

処理ブロック５４’に進んだ場合には、以下の条件（ａ’）または条件（ｂ’）のどちらか一方が成立する場合に処理ブロック５５’へ、条件（ａ’）および条件（ｂ’）がともに不成立となる場合に処理ブロック５６へ分岐する。

条件（ａ’）
先端側接続剛体ｍに対する先端側接続剛体が存在せず、上式（３７）が成立する。

条件（ｂ’）
先端側接続剛体ｍに対する全ての先端側接続剛体、つまりＣｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｍ］における全ての非ゼロ要素ｌについて、剛体ｉの相対回転運動に対応する部分角速度のうち剛体ｌに対応する項である下式（４４）

と、先端側接続剛体ｍの相対回転運動に対応する部分角速度のうち剛体ｌに対応する項である下式（４５）

が一致する。

処理ブロック５５’に進んだ場合には、一般化慣性力導出部１７は、処理ブロック５４’において、条件（ａ’）または条件（ｂ’）のどちらか一方が成立した成分ｊについて、回転運動に関する剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］から、先端側接続剛体ｍの部分速度ＰＶｒ［３ｍ−ｊ、：］を減じた、下式（４６）を、あらためて剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］とする。

さらに、一般化慣性力導出部１７は、回転運動に関する剛体ｉの部分角速度ＰＷｒ［３ｉ−ｊ、：］から、先端側接続剛体ｍの部分角速度ＰＷｒ［３ｍ−ｊ、：］を減じた、下式（４７）を、あらためて剛体ｉの部分角速度ＰＷｒ［３ｉ−ｊ、：］とする。

さらに、一般化慣性力導出部１７は、一般化力Ｑｒ［３ｉ−ｊ］から、先端側接続剛体ｍの一般化力Ｑｒ［３ｍ−ｊ］を減じた、下式（４８）を、あらためて剛体ｉの一般化力Ｑｒ［３ｉ−ｊ］とする。

以上のような図７、図８に示した内容が、図６に示す一般化慣性力導出部１７のうち、処理ブロック４３に関する処理構成である。

次に、一般化慣性力導出部１７は、処理ブロック４４において、一般化慣性力の導出対象となる剛体番号ｉをデクリメントする。さらに、一般化慣性力導出部１７は、処理ブロック４５において、剛体番号ｉがｉ≧１であれば、処理ブロック４２へ分岐して処理を繰り返し実行し、ｉ＝０であれば、処理ブロック４６へ分岐する。

処理ブロック４６に進んだ場合には、一般化慣性力導出部１７は、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）の並進運動に関する一般化慣性力行列ＧＩＦｔを上式（３０）で導出し、回転運動に関する一般化慣性力行列ＧＩＦｒを上式（３１）で導出する。さらに、一般化慣性力導出部１７は、機械システムの運動方程式として、上式（３２）、（３３）を出力し、一連処理を終了する。

上述した処理ブロック４３において、一般化慣性力導出部１７は、剛体ｉの相対並進運動に対応する部分速度ＰＶｔ［３ｉ−ｊ、：］（ｊ＝０〜２）と、剛体ｉに対して先端側に接続された先端側接続剛体ｍの相対並進運動に対応する部分速度ＰＶｔ［３ｍ−ｊ、：］（ｊ＝０〜２）とを比較し、剛体ｉの相対並進運動に対応する部分速度ＰＶｔ［３ｉ−ｊ、：］のうち先端側接続剛体ｍに対応する項と、先端側接続剛体ｍの相対並進運動に対応する部分速度ＰＶｔ［３ｍ−ｊ、：］とが一致した場合に、並進運動に関して剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］、および一般化力Ｑｔ［３ｉ−ｊ］から、先端側接続剛体ｍの部分速度ＰＶｔ［３ｍ−ｊ、：］、および一般化力Ｑｔ［３ｍ−ｊ］を減じたものを、あらためて剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］、および一般化力Ｑｔ［３ｉ−ｊ］として構成している。

当該処理は、結果として得られる機械システムの運動方程式である上式（３２）で考えた場合、並進運動に関して剛体ｉの運動方程式から、先端側接続剛体ｍの運動方程式を減じたものを、あらためて剛体ｉの運動方程式とすることと等価となる。このとき、並進運動に関する剛体ｉの運動方程式は、下式（４９）で与えられる。

また、上述した処理ブロック４３’において、一般化慣性力導出部１７は、剛体ｉの相対回転運動に対応する部分角速度ＰＷｒ［３ｉ−ｊ、：］（ｊ＝０〜２）と、剛体ｉに対して先端側に接続された先端側接続剛体ｍの相対回転運動に対応する部分角速度ＰＷｒ［３ｍ−ｊ、：］（ｊ＝０〜２）とを比較し、剛体ｉの相対回転運動に対応する部分角速度ＰＷｒ［３ｉ−ｊ、：］のうち先端側接続剛体ｍに対応する項と、先端側接続剛体ｍの相対回転運動に対応する部分角速度ＰＷｒ［３ｍ−ｊ、：］とが一致した場合に、回転運動に関して剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］、部分角速度ＰＷｒ［３ｉ−ｊ、：］、および一般化力Ｑｒ［３ｉ−ｊ］から、先端側接続剛体ｍの部分速度ＰＶｒ［３ｍ−ｊ、：］、部分角速度ＰＷｒ［３ｍ−ｊ、：］、および一般化力Ｑｒ［３ｍ−ｊ］を減じたものを、あらためて剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］、部分角速度ＰＷｒ［３ｉ−ｊ、：］、および一般化力Ｑｒ［３ｉ−ｊ］として構成している。

当該処理は、結果として得られる機械システムの運動方程式である上式（３３）で考えた場合、回転運動に関して剛体ｉの運動方程式から、先端側接続剛体ｍの運動方程式を減じたものを、あらためて剛体ｉの運動方程式とすることと等価となる。このとき、回転運動に関する剛体ｉの運動方程式は、下式（５０）で与えられる。

以上のように、本実施の形態２による剛体多体系の運動方程式導出装置によれば、処理全体を、剛体多体系構成定義部１１、剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６、および一般化慣性力導出部１７で構成している。そして、剛体多体系構成定義部１１以外の各部１２〜１７を自動化している。

このような構成を備えることで、設計者が対象とする機械システムを剛体多体系としてモデル化し、当該モデルの構成を剛体多体系構成定義部１１において入力するだけで、機械システムの運動方程式を自動的に導出することができる。

さらに、本実施の形態２による剛体多体系の運動方程式導出装置によれば、剛体連結構成導出部１２において、所定剛体に対して先端側に接続された全剛体を導出している。さらに、一般化力導出部１５において、所定剛体の並進運動に対応する一般化力を、駆動力項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和で与え、所定剛体の回転運動に対応する一般化力を、駆動トルク項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外モーメント項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和に対して、剛体連結部における変換行列を作用させたもので与えている。この結果、一般化力の導出に関して、導出に必要な計算量を低減することができる。

加えて、本実施の形態２による剛体多体系の運動方程式導出装置によれば、一般化慣性力導出部１７において、所定剛体に対して先端側に接続された先端側剛体の相対並進運動（相対回転運動）に対応する部分速度（部分角速度）と、所定剛体の相対並進運動（相対回転運動）に対応する部分速度（部分角速度）のうち、先端側接続剛体に対応する項とが一致する場合に、並進運動（回転運動）に関して所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、所定剛体の運動方程式としている。この結果、導出した運動方程式における一般化慣性力の記述量を低減することができる。

このように、実施の形態２によれば、一般化慣性力の導出に際して、所定剛体に対して先端側に接続されている先端側剛体の部分速度または部分角速度と、当該所定剛体の部分速度または部分角速度のうち先端側剛体に対応する項とが一致する場合に、所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、所定剛体の運動方程式としている。これにより、先の実施の形態１の効果に加え、導出した運動方程式における一般化慣性力の記述量を低減するような、剛体多体系の運動方程式導出装置を得ることができる。

実施の形態３．
本実施の形態３では、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、運動方程式導出に必要な計算量、および導出した運動方程式の記述量の低減を図る場合について詳細に説明する。なお、この「微小量」とは、所定量以下の変位量を意味している。

本実施の形態３による剛体多体系の運動方程式導出装置において、機械システムにおける剛体連結構成例を示す構成図、処理の全体構成を示すブロック図、および各剛体の剛体固定枠、相対変位の定義は、先の実施の形態１における図１、図２、および図３と同一である。

また、本実施の形態３による剛体多体系の運動方程式導出装置において、各剛体への作用力、作用モーメントの定義、および剛体連結構成導出部１２の処理構成を示すフローチャートは、先の実施の形態１における図４、および図５と同一である。

さらに、本実施の形態３による剛体多体系の運動方程式導出装置において、一般化慣性力導出部１７の処理構成を示すフローチャート、並進運動に関する一般化慣性力導出部１７の第ｉ行ブロック処理における処理構成を示すフローチャート、および回転運動に関する一般化慣性力導出部１７の第ｉ行ブロック処理における処理構成を示すフローチャートは、先の実施の形態２における図６、図７、および図８と同一である。

図９は、本発明の実施の形態３による剛体多体系の運動方程式導出装置において、線形化の処理構成を示すフローチャートである。図９に示すように、本実施の形態３による線形化処理６０は、処理ブロック６１および６２を備えている。

そして、線形化処理６０は、変数ｅｐｓを乗じた微小量について、線形化対象に含まれる正弦関数および余弦関数を１次近似で置換して展開し、さらに、ｅｐｓ^２＝０として微小量に関する２次以上の項を消去することで、線形化を行う。

図１０は、本発明の実施の形態３による剛体多体系の運動方程式導出装置において、部分速度・部分角速度導出部１６の処理構成を示すフローチャートである。図１０に示すように、本実施の形態３による部分速度・部分角速度導出部１６は、処理ブロック７１〜７７を備えている。

そして、部分速度・部分角速度導出部１６は、ベース側接続剛体ｋに対する剛体ｌの相対並進変位ｔ_ｋｌおよび相対回転変位θ_ｋｌに微小量が含まれる場合に、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖ_ｌ ^Ｏｌ、および角速度代数ベクトルω_ｌ ^Ｏｌと、所定の一般化スピードｕ_ｋから、部分速度

および部分角速度

を導出する。

次に、先の図１〜図８に加え、図９、図１０に基づいて、本実施の形態３における剛体多体系の運動方程式導出装置の動作について説明する。
図１に示すような機械システム１０に関して、設計者によるモデル化の方法は、先の実施の形態１と同一である。また、図２に示す剛体連結構成導出部１２、および変換行列導出部１３における処理内容も、先の実施の形態１と同一である。

本実施の形態３による剛体多体系の運動方程式導出装置は、図２に示す剛体多体系構成定義部１１において、ベース側接続剛体ｋに対する剛体ｌの相対並進変位ｔ_ｋｌおよび相対回転変位θ_ｋｌに微小量が含まれる場合、当該微小量を変数ｅｐｓを乗じた形式で定義する。

例えば、ベース側接続剛体ｋに対する剛体ｌの相対並進変位ｔ_ｋｌを下式（５１）

で定義した場合、ベース側接続剛体ｋに対する剛体ｌの相対並進変位に関して、ベース側接続剛体ｋに固定された連結部剛体固定枠ｋｌに対して、剛体ｌに固定された連結部剛体固定枠ｌｋが、（連結部剛体固定枠ｋｌから見て）Ｘ軸方向に微小な大きさｘ_ｋｌで、Ｙ軸方向に有意な大きさｙ_ｋｌで並進変位し、Ｚ軸方向には相対的な並進変位を生じないことを表している。

一方で、ベース側接続剛体ｋに対する剛体ｌの相対回転変位θ_ｋｌを、下式（５２）

で定義した場合、ベース側接続剛体ｋに対する剛体ｌの相対回転変位に関して、ベース側接続剛体ｋに固定された連結部剛体固定枠ｋｌに対して、剛体ｌに固定された連結部剛体固定枠ｌｋが、（オイラー角で表現して）Ｘ軸方向、およびＺ軸方向に微小な大きさθ_Ｘｋｌ、およびθ_Ｚｋｌで回転変位し、Ｙ軸方向には相対的な回転変位を生じないことを表している。

次に、図２に示す慣性力・慣性トルク導出部１４は、先の実施の形態１と同様に、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）に関して、慣性基準枠Ｏに対する慣性主軸剛体固定枠ｌの角速度代数ベクトルω_ｌ ^Ｏｌ（慣性主軸剛体固定枠ｌ表現）を上式（８）で導出し、慣性基準枠Ｏに対する慣性主軸剛体固定枠ｌの質量中心速度代数ベクトルｖ_ｌ ^Ｏｌ（慣性主軸剛体固定枠ｌ表現）を上式（９）で導出する。次いで、慣性力・慣性トルク導出部１４は、慣性主軸剛体固定枠ｌで表現した慣性力代数ベクトルｍ_ｌａ_ｌ ^Ｏｌを上式（１０）で導出し、慣性主軸剛体固定枠ｌで表現した質量中心まわりの慣性トルク代数ベクトル

を上式（１１）で導出する。

さらに、慣性力・慣性トルク導出部１４は、図９に示す線形化処理に基づき、剛体ｌの相対回転変位θ_ｋｌ（ｌ＝１〜ｎ_ｂ）に微小量ｅｐｓ＊θ_ｊｋｌ（ｊ＝Ｘ、Ｙ、Ｚ）が含まれる場合には、処理ブロック６１において、角速度代数ベクトルω_ｌ ^Ｏｌ、質量中心速度代数ベクトルｖ_ｌ ^Ｏｌ、慣性力代数ベクトルｍ_ｌａ_ｌ ^Ｏｌ、および慣性トルク代数ベクトル

に含まれる正弦関数を下式（５３）

で、余弦関数を下式（５４）

で、それぞれ１次近似して展開する。

次に、慣性力・慣性トルク導出部１４は、処理ブロック６２において、ｅｐｓ^２＝０とすることで、微小量に関する２次以上の項を消去し、線形化した角速度代数ベクトルω_ｌ ^Ｏｌ、質量中心速度代数ベクトルｖ_ｌ ^Ｏｌ、慣性力代数ベクトルｍ_ｌａ_ｌ ^Ｏｌ、および慣性トルク代数ベクトル

を導出する。

慣性力・慣性トルク導出部１４は、最後に、先の実施の形態１と同様、全慣性力代数ベクトルｄＬｌを上式（１２）で、全慣性トルク代数ベクトルｄＨｌ＿ｌを上式（１３）で構成する。

図２に示す一般化力導出部１５は、先の実施の形態１と同様に、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）に関して、座標変換行列、およびチルダマトリックスを上式（１５）〜（１８）で導出し、相対並進運動に関する一般化力Ｑ_ｔｌを上式（１９）で導出し、相対回転運動に関する一般化力Ｑ_θｌを上式（２０）で導出する。

一般化力導出部１５は、さらに、図９に示す線形化処理に基づき、剛体ｌの相対回転変位θ_ｋｌ（ｌ＝１〜ｎ_ｂ）に微小量ｅｐｓ＊θ_ｊｋｌ（ｊ＝Ｘ、Ｙ、Ｚ）が含まれる場合には、処理ブロック６１において、一般化力Ｑ_ｔｌおよびＱ_θｌに含まれる正弦関数を上式（５３）で、余弦関数を上式（５４）で、それぞれ１次近似して展開する。

次に、一般化力導出部１５は、処理ブロック６２において、ｅｐｓ^２＝０とすることで、微小量に関する２次以上の項を消去し、線形化した一般化力Ｑ_ｔｌおよびＱ_θｌを導出して、全並進一般化力ベクトルＱｔを上式（２１）で、全回転一般化力ベクトルＱｒを上式（２２）で構成する。

図２に示す部分速度・部分角速度導出部１６は、先の実施の形態１と同様に、はじめに相対並進変位ｔ_ｋｌに対応する一般化スピードｕ_ｔを上式（２３）で導出し、相対回転変位θ_ｋｌに対応する一般化スピードｕ_ｒを上式（２４）で導出する。

次いで、部分速度・部分角速度導出部１６は、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖ_ｌ ^Ｏｌ、および角速度代数ベクトルω_ｌ ^Ｏｌと、一般化スピードｕ_ｔおよびｕ_ｒの所定要素ｕ_ｋから、図１０に示す処理構成に基づいて部分速度

および部分角速度

を導出する。

図１０に示す処理ブロック７１では、一般化スピードがｕ_ｋ≠０である場合には、処理ブロック７３へ分岐し、ｕ_ｋ＝０である場合には、処理ブロック７２に分岐して、対応する部分（角）速度を下式（５５）とする。

部分速度・部分角速度導出部１６は、処理ブロック７３において、一般化スピードｕ_ｋにおける変数ｅｐｓの次数を判定し、次数が１の場合には、処理ブロック７５へ分岐し、次数が０の場合には、処理ブロック７４へ分岐して、対応する部分（角）速度を、下式（５６）で導出する。

ただし、上式（５６）では、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖ_ｌ ^Ｏｌ、および角速度代数ベクトルω_ｌ ^Ｏｌが、変数ｅｐｓおよび一般化スピードｕ_ｋの双方に関して１次以下となることを用いている。

部分速度・部分角速度導出部１６は、処理ブロック７５において、下式（５７）

によって一時的に一般化スピードｕ_ｋから変数ｅｐｓを消去し、処理ブロック７６において、下式（５８）を算出する。

ただし、上式（５８）では、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖ_ｌ ^Ｏｌ、および角速度代数ベクトルω_ｌ ^Ｏｌが、変数ｅｐｓおよび一般化スピードｕ_ｋ＝ｅｐｓ＊ｕの双方に関して１次以下となることを用いている。

一般化スピードｕ_ｋが変数ｅｐｓについて１次となる場合の部分（角）速度は、処理ブロック７７において、下式（５９）によって導出される。

部分速度・部分角速度導出部１６は、最後に、先の実施の形態１と同様、部分速度行列ＰＶｔおよびＰＶｒを上式（２７）および（２８）で、部分角速度行列ＰＷｒを上式（２９）で構成する。

図２に示す一般化慣性力導出部１７は、先の実施の形態２と同様に、図６〜図８に示す処理構成で剛体多体系の部分速度行列ＰＶｔ、ＰＶｒ、部分角速度行列ＰＷｒ、および一般化力ベクトルＱｔ、Ｑｒを導出し、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎ_ｂ）の並進運動に関する一般化慣性力行列ＧＩＦｔを上式（３０）で導出し、回転運動に関する一般化慣性力行列ＧＩＦｒを上式（３１）で導出する。

一般化慣性力導出部１７は、さらに、図９に示す線形化処理に基づき、剛体ｌの相対回転変位θ_ｋｌ（ｌ＝１〜ｎ_ｂ）に微小量ｅｐｓ＊θ_ｊｋｌ（ｊ＝Ｘ、Ｙ、Ｚ）が含まれる場合には、処理ブロック６１において、一般化慣性力行列ＧＩＦｔおよびＧＩＦｒに含まれる正弦関数を上式（５３）で、余弦関数を上式（５４）で、それぞれ１次近似して展開する。

次に、一般化慣性力導出部１７は、処理ブロック６２において、ｅｐｓ^２＝０とすることで、微小量に関する２次以上の項を消去し、線形化した一般化慣性力行列ＧＩＦｔおよびＧＩＦｒを導出する。

最後に、一般化慣性力導出部１７は、一般化慣性力行列ＧＩＦｔ、ＧＩＦｒ、および一般化力ベクトルＱｔ、Ｑｒに対して、下式（６０）

を代入した上で、機械システムの運動方程式として、上式（３２）および上式（３３）を出力する。

以上のように、本実施の形態３による剛体多体系の運動方程式導出装置によれば、処理全体を、剛体多体系構成定義部１１、剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６、および一般化慣性力導出部１７で構成している。そして、剛体多体系構成定義部１１以外の処理部１２〜１７を自動化している。

このような構成を備えることで、設計者が対象とする機械システムを剛体多体系としてモデル化し、当該モデルの構成を剛体多体系構成定義部１１において入力するだけで、機械システムの運動方程式を自動的に導出することができる。

さらに、本実施の形態３による剛体多体系の運動方程式導出装置によれば、剛体連結構成導出部１２において、所定剛体に対して先端側に接続された全剛体を導出している。さらに、一般化力導出部１５において、所定剛体の並進運動に対応する一般化力を、駆動力項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和で与え、所定剛体の回転運動に対応する一般化力を、駆動トルク項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外モーメント項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和に対して、剛体連結部における変換行列を作用させたもので与えている。この結果、一般化力の導出に関して、導出に必要な計算量を低減することができる。

さらに、本実施の形態３による剛体多体系の運動方程式導出装置によれば、一般化慣性力導出部１７において、所定剛体に対して先端側に接続された先端側剛体の相対並進運動（相対回転運動）に対応する部分速度（部分角速度）と、所定剛体の相対並進運動（相対回転運動）に対応する部分速度（部分角速度）のうち、先端側接続剛体に対応する項とが一致する場合に、並進運動（回転運動）に関して所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、所定剛体の運動方程式としている。この結果、導出した運動方程式における一般化慣性力の記述量を低減することができる。

加えて、本実施の形態３による剛体多体系の運動方程式導出装置によれば、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、慣性力・慣性トルク導出部１４において導出する各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルク、一般化力導出部１５において導出する各剛体の一般化力、および一般化慣性力導出部１７において導出する各剛体の一般化慣性力に関して、当該微小量に関する２次以上の項を消去することで線形化を行うようにした。この結果、対象とする機械システムの動特性表現精度を維持したまま、運動方程式導出に必要な計算量、および導出した運動方程式における記述量の双方を低減することができる。

このように、実施の形態３によれば、対象とする機械システムにおいて所定剛体の相対運動に微小量が含まれる場合に、当該微小量に関する２次以上の項を消去して線形化することを行っている。これにより、先の実施の形態１、２の効果に加え、運動方程式導出に必要な計算量、および導出した運動方程式の記述量の双方を低減するような、剛体多体系の運動方程式導出装置を得ることができる。

１〜８ 剛体、１０ 機械システム、１１ 剛体多体系構成定義部、１２ 剛体連結構成導出部、１３ 変換行列導出部、１４ 慣性力・慣性トルク導出部、１５ 一般化力導出部、１６ 部分速度・部分角速度導出部、１７ 一般化慣性力導出部。

Claims (10)

1. 各剛体の接続構成が開ループ構成となる機械システムに対して、設計者が当該機械システムを剛体多体系としてモデル化した際に、モデル構成を定義づけて入力するための手段である剛体多体系構成定義部と、
   前記剛体多体系構成定義部で定義された前記モデル構成に基づいて、各剛体の連結構成を導出する剛体連結構成導出部と、
   前記剛体多体系構成定義部で定義された前記モデル構成に基づいて、剛体連結部で相互に連結される剛体間の相対回転変位から，前記剛体間の相対回転姿勢を規定する座標変換行列と，前記剛体間の相対回転速度を相対角速度に変換する第１変換行列とを、剛体連結部における変換行列として導出する変換行列導出部と、
   前記剛体多体系構成定義部で定義された前記モデル構成、および前記変換行列導出部で導出された前記剛体連結部における変換行列に基づいて、各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出する慣性力・慣性トルク導出部と、
   前記剛体多体系構成定義部で定義された前記モデル構成、前記剛体連結構成導出部で導出された前記各剛体の連結構成、および前記変換行列導出部で導出された前記剛体連結部における変換行列に基づいて、各剛体の一般化力を導出する一般化力導出部と、
   前記剛体多体系構成定義部で定義された前記モデル構成、および前記慣性力・慣性トルク導出部で導出された各剛体の前記質量中心速度と前記角速度に基づいて、各剛体の部分速度および部分角速度を導出する部分速度・部分角速度導出部と、
   前記剛体連結構成導出部で導出された前記各剛体の連結構成、前記慣性力・慣性トルク導出部で導出された各剛体の前記慣性力および前記質量中心まわりの慣性トルク、前記一般化力導出部で導出された前記各剛体の一般化力、および前記部分速度・部分角速度導出部で導出された各剛体の前記部分速度および前記部分角速度に基づいて、各剛体の一般化慣性力および運動方程式を導出する一般化慣性力導出部と
   を備えることを特徴とする剛体多体系の運動方程式導出装置。
2. 請求項１に記載の剛体多体系の運動方程式導出装置において、
   前記剛体連結構成導出部は、所定剛体に対して先端側に接続された全ての剛体を導出することで、前記各剛体の連結構成を導出する
   ことを特徴とする剛体多体系の運動方程式導出装置。
3. 請求項１または２に記載の剛体多体系の運動方程式導出装置において、
   前記一般化力導出部は、
   所定剛体ｌと、前記所定剛体ｌが接続されているベース側剛体ｋとの剛体連結部を規定し、
   かつ前記ベース側剛体ｋに固定された連結部剛体固定枠ｋｌに対して、
   前記ベース側剛体ｋから前記剛体連結部を介して前記所定剛体ｌへ作用する駆動力の前記連結部剛体固定枠ｋｌによる表現を駆動力項とし、
   前記所定剛体ｌおよび前記所定剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外力の和の前記連結部剛体固定枠ｋｌによる表現を外力項とし、
   前記駆動力項と前記外力項との和を、前記所定剛体ｌの並進運動に対応する一般化力として導出する
   ことを特徴とする剛体多体系の運動方程式導出装置。
4. 請求項１ないし３のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
   前記一般化力導出部は、
   所定剛体ｌと、前記所定剛体ｌが接続されているベース側剛体ｋとの剛体連結部を規定し、
   かつ前記ベース側剛体ｋに固定された連結部剛体固定枠ｋｌと、所定剛体ｌに固定された連結部剛体固定枠ｌｋに対して、
   前記ベース側剛体ｋから前記剛体連結部を介して前記所定剛体ｌへ作用する駆動トルクの前記連結部剛体固定枠ｋｌによる表現を駆動トルク項とし、
   前記所定剛体ｌおよび前記所定剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外モーメントの和の前記連結部剛体固定枠ｋｌによる表現を外モーメント項とし、
   前記所定剛体ｌおよび前記所定剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外力による、前記連結部剛体固定枠ｌｋの原点に対するモーメントの和の前記連結部剛体固定枠ｋｌによる表現を外力項として、
   前記駆動トルク項、前記外モーメント項、および前記外力項の和に対して、前記変換行列導出部で導出された前記剛体連結部における変換行列のうち、前記剛体間の相対回転速度を相対角速度に変換する前記第１変換行列を作用させたものを、前記所定剛体ｌの回転運動に対応する一般化力として導出する
   ことを特徴とする剛体多体系の運動方程式導出装置。
5. 請求項１ないし４のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
   前記一般化慣性力導出部は、所定剛体の相対並進運動に対応する部分速度と、前記所定剛体に対して先端側に接続された先端側剛体の相対並進運動に対応する部分速度とにおいて、前記所定剛体の相対並進運動に対応する部分速度のうち先端側剛体に対応する項と、前記先端側剛体の相対並進運動に対応する部分速度とが一致する場合に、並進運動に関して前記所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、前記所定剛体の運動方程式として導出する
   ことを特徴とする剛体多体系の運動方程式導出装置。
6. 請求項１ないし５のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
   一般化慣性力導出部は、所定剛体の相対回転運動に対応する部分角速度と、前記所定剛体に対して先端側に接続された先端側剛体の相対回転運動に対応する部分角速度とにおいて、前記所定剛体の相対回転運動に対応する部分角速度のうち先端側剛体に対応する項と、前記先端側剛体の相対回転運動に対応する部分角速度とが一致する場合に、回転運動に関して前記所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、前記所定剛体の運動方程式として導出する
   ことを特徴とする剛体多体系の運動方程式導出装置。
7. 請求項１ないし６のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
   前記慣性力・慣性トルク導出部は、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、各剛体の前記質量中心速度、前記角速度、前記慣性力、および前記質量中心まわりの慣性トルクを導出するに当たって、当該微小量に関する２次以上の項を消去することで、線形化を行う
   ことを特徴とする剛体多体系の運動方程式導出装置。
8. 請求項１ないし７のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
   前記一般化力導出部は、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、前記各剛体の一般化力を導出するに当たって、当該微小量に関する２次以上の項を消去することで、線形化を行う
   ことを特徴とする剛体多体系の運動方程式導出装置。
9. 請求項１ないし８のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
   前記一般化慣性力導出部は、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、各剛体の一般化慣性力を導出するに当たって、当該微小量に関する２次以上の項を消去することで、線形化を行う
   ことを特徴とする剛体多体系の運動方程式導出装置。
10. 各剛体の接続構成が開ループ構成となる機械システムを設計者が剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出する剛体多体系の運動方程式導出装置に用いられる剛体多体系の運動方程式導出方法であって、
    前記機械システムに対して、設計者が当該機械システムを剛体多体系としてモデル化した際に、モデル構成を定義づけて入力する剛体多体系構成定義ステップと、
    前記剛体多体系構成定義ステップで定義された前記モデル構成に基づいて、各剛体の連結構成を導出する剛体連結構成導出ステップと、
    前記剛体多体系構成定義ステップで定義された前記モデル構成に基づいて、剛体連結部で相互に連結される剛体間の相対回転変位から，前記剛体間の相対回転姿勢を規定する座標変換行列と，前記剛体間の相対回転速度を相対角速度に変換する第１変換行列とを、剛体連結部における変換行列として導出する変換行列導出ステップと、
    前記剛体多体系構成定義ステップで定義された前記モデル構成、および前記変換行列導出ステップで導出された前記剛体連結部における変換行列に基づいて、各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出する慣性力・慣性トルク導出ステップと、
    前記剛体多体系構成定義ステップで定義された前記モデル構成、前記剛体連結構成導出ステップで導出された前記各剛体の連結構成、および前記変換行列導出ステップで導出された前記剛体連結部における変換行列に基づいて、各剛体の一般化力を導出する一般化力導出ステップと、
    前記剛体多体系構成定義ステップで定義された前記モデル構成、および前記慣性力・慣性トルク導出ステップで導出された各剛体の前記質量中心速度と前記角速度に基づいて、各剛体の部分速度および部分角速度を導出する部分速度・部分角速度導出ステップと、
    前記剛体連結構成導出ステップで導出された前記各剛体の連結構成、前記慣性力・慣性トルク導出ステップで導出された各剛体の前記慣性力および前記質量中心まわりの慣性トルク、前記一般化力導出ステップで導出された前記各剛体の一般化力、および前記部分速度・部分角速度導出ステップで導出された各剛体の前記部分速度および前記部分角速度に基づいて、各剛体の一般化慣性力および運動方程式を導出する一般化慣性力導出ステップと
    を備えることを特徴とする剛体多体系の運動方程式導出方法。

Images