(57)【要約】【課題】運動方程式導出に必要な計算量、および導出した運動方程式の記述量を低減することのできる剛体多体系の運動方程式導出装置を得る。【解決手段】剛体多体系としてモデル化された、各剛体の接続構成が開ループ構成となる機械システムのモデル構成を入力する剛体多体系構成定義部（１１）と、各剛体の連結構成を導出する剛体連結構成導出部（１２）と、剛体連結部における変換行列を導出する変換行列導出部（１３）と、各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出する慣性力・慣性トルク導出部（１４）と、各剛体の一般化力を導出する一般化力導出部（１５）と、各剛体の部分速度および部分角速度を導出する部分速度・部分角速度導出部（１６）と、各剛体の一般化慣性力および運動方程式を導出する一般化慣性力導出部（１７）を備える。【選択図】図２
2014078090.tif 000002
【特許請求の範囲】
【請求項１】
各剛体の接続構成が開ループ構成となる機械システムに対して、設計者が当該機械システムを剛体多体系としてモデル化した際に、モデル構成を定義づけて入力するための手段である剛体多体系構成定義部と、
前記剛体多体系構成定義部で定義された前記モデル構成に基づいて、各剛体の連結構成を導出する剛体連結構成導出部と、
前記剛体多体系構成定義部で定義された前記モデル構成に基づいて、剛体連結部における変換行列を導出する変換行列導出部と、
前記剛体多体系構成定義部で定義された前記モデル構成、および前記変換行列導出部で導出された剛体連結部における前記変換行列に基づいて、各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出する慣性力・慣性トルク導出部と、
前記剛体多体系構成定義部で定義された前記モデル構成、前記剛体連結構成導出部で導出された前記各剛体の連結構成、および前記変換行列導出部で導出された剛体連結部における前記変換行列に基づいて、各剛体の一般化力を導出する一般化力導出部と、
前記剛体多体系構成定義部で定義された前記モデル構成、および前記慣性力・慣性トルク導出部で導出された各剛体の前記質量中心速度と前記角速度に基づいて、各剛体の部分速度および部分角速度を導出する部分速度・部分角速度導出部と、
前記剛体連結構成導出部で導出された前記各剛体の連結構成、前記慣性力・慣性トルク導出部で導出された各剛体の前記慣性力および前記質量中心まわりの慣性トルク、前記一般化力導出部で導出された前記各剛体の一般化力、および前記部分速度・部分角速度導出部で導出された各剛体の前記部分速度および前記部分角速度に基づいて、各剛体の一般化慣性力および運動方程式を導出する一般化慣性力導出部と
を備えることを特徴とする剛体多体系の運動方程式導出装置。
【請求項２】
請求項１に記載の剛体多体系の運動方程式導出装置において、
前記剛体連結構成導出部は、所定剛体に対して先端側に接続された全ての剛体を導出することで、前記各剛体の連結構成を導出する
ことを特徴とする剛体多体系の運動方程式導出装置。
【請求項３】
請求項１または２に記載の剛体多体系の運動方程式導出装置において、
前記一般化力導出部は、
所定剛体ｌと、前記所定剛体ｌが接続されているベース側剛体ｋとの剛体連結部を規定し、
かつ前記ベース側剛体ｋに固定された連結部剛体固定枠ｋｌに対して、
前記ベース側剛体ｋから前記剛体連結部を介して前記所定剛体ｌへ作用する駆動力の前記連結部剛体固定枠ｋｌによる表現を駆動力項とし、
前記所定剛体ｌおよび前記所定剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外力の和の前記連結部剛体固定枠ｋｌによる表現を外力項とし、
前記駆動力項と前記外力項との和を、前記所定剛体ｌの並進運動に対応する一般化力として導出する
ことを特徴とする剛体多体系の運動方程式導出装置。
【請求項４】
請求項１ないし３のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
前記一般化力導出部は、
所定剛体ｌと、前記所定剛体ｌが接続されているベース側剛体ｋとの剛体連結部を規定し、
かつ前記ベース側剛体ｋに固定された連結部剛体固定枠ｋｌと、所定剛体ｌに固定された連結部剛体固定枠ｌｋに対して、
前記ベース側剛体ｋから前記剛体連結部を介して前記所定剛体ｌへ作用する駆動トルクの前記連結部剛体固定枠ｋｌによる表現を駆動トルク項とし、
前記所定剛体ｌおよび前記所定剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外モーメントの和の前記連結部剛体固定枠ｋｌによる表現を外モーメント項とし、
前記所定剛体ｌおよび前記所定剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外力による、前記連結部剛体固定枠ｌｋの原点に対するモーメントの和の前記連結部剛体固定枠ｋｌによる表現を外力項として、
前記駆動トルク項、前記外モーメント項、および前記外力項の和に対して、前記変換行列導出部で導出された前記剛体連結部における前記変換行列を作用させたものを、前記所定剛体ｌの回転運動に対応する一般化力として導出する
ことを特徴とする剛体多体系の運動方程式導出装置。
【請求項５】
請求項１ないし４のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
前記一般化慣性力導出部は、所定剛体の相対並進運動に対応する部分速度と、前記所定剛体に対して先端側に接続された先端側剛体の相対並進運動に対応する部分速度とにおいて、前記所定剛体の相対並進運動に対応する部分速度のうち先端側剛体に対応する項と、前記先端側剛体の相対並進運動に対応する部分速度とが一致する場合に、並進運動に関して前記所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、前記所定剛体の運動方程式として導出する
ことを特徴とする剛体多体系の運動方程式導出装置。
【請求項６】
請求項１ないし５のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
一般化慣性力導出部は、所定剛体の相対回転運動に対応する部分角速度と、前記所定剛体に対して先端側に接続された先端側剛体の相対回転運動に対応する部分角速度とにおいて、前記所定剛体の相対回転運動に対応する部分角速度のうち先端側剛体に対応する項と、前記先端側剛体の相対回転運動に対応する部分角速度とが一致する場合に、回転運動に関して前記所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、前記所定剛体の運動方程式として導出する
ことを特徴とする剛体多体系の運動方程式導出装置。
【請求項７】
請求項１ないし６のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
前記慣性力・慣性トルク導出部は、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、各剛体の前記質量中心速度、前記角速度、前記慣性力、および前記質量中心まわりの慣性トルクを導出するに当たって、当該微小量に関する２次以上の項を消去することで、線形化を行う
ことを特徴とする剛体多体系の運動方程式導出装置。
【請求項８】
請求項１ないし７のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
前記一般化力導出部は、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、前記各剛体の一般化力を導出するに当たって、当該微小量に関する２次以上の項を消去することで、線形化を行う
ことを特徴とする剛体多体系の運動方程式導出装置。
【請求項９】
請求項１ないし８のいずれか１項に記載の剛体多体系の運動方程式導出装置において、
前記一般化慣性力導出部は、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、各剛体の一般化慣性力を導出するに当たって、当該微小量に関する２次以上の項を消去することで、線形化を行う
ことを特徴とする剛体多体系の運動方程式導出装置。
【請求項１０】
各剛体の接続構成が開ループ構成となる機械システムを設計者が剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出する剛体多体系の運動方程式導出装置に用いられる剛体多体系の運動方程式導出方法であって、
前記機械システムに対して、設計者が当該機械システムを剛体多体系としてモデル化した際に、モデル構成を定義づけて入力する剛体多体系構成定義ステップと、
前記剛体多体系構成定義ステップで定義された前記モデル構成に基づいて、各剛体の連結構成を導出する剛体連結構成導出ステップと、
前記剛体多体系構成定義ステップで定義された前記モデル構成に基づいて、剛体連結部における変換行列を導出する変換行列導出ステップと、
前記剛体多体系構成定義ステップで定義された前記モデル構成、および前記変換行列導出ステップで導出された剛体連結部における前記変換行列に基づいて、各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出する慣性力・慣性トルク導出ステップと、
前記剛体多体系構成定義ステップで定義された前記モデル構成、前記剛体連結構成導出ステップで導出された前記各剛体の連結構成、および前記変換行列導出ステップで導出された剛体連結部における前記変換行列に基づいて、各剛体の一般化力を導出する一般化力導出ステップと、
前記剛体多体系構成定義ステップで定義された前記モデル構成、および前記慣性力・慣性トルク導出ステップで導出された各剛体の前記質量中心速度と前記角速度に基づいて、各剛体の部分速度および部分角速度を導出する部分速度・部分角速度導出ステップと、
前記剛体連結構成導出ステップで導出された前記各剛体の連結構成、前記慣性力・慣性トルク導出ステップで導出された各剛体の前記慣性力および前記質量中心まわりの慣性トルク、前記一般化力導出ステップで導出された前記各剛体の一般化力、および前記部分速度・部分角速度導出部ステップ導出された各剛体の前記部分速度および前記部分角速度に基づいて、各剛体の一般化慣性力および運動方程式を導出する一般化慣性力導出ステップと
を備えることを特徴とする剛体多体系の運動方程式導出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、機械システムのダイナミクス解析に際して、機械システムを剛体多体系としてモデル化し、当該モデルに基づいて機械システムの運動方程式を導出することのできる剛体多体系の運動方程式導出装置および剛体多体系の運動方程式導出方法に関するものである。
【背景技術】
【０００２】
従来の機械システムの運動方程式導出装置として、ホロノーム機械系分析装置および方法がある（例えば、特許文献１参照）。このホロノーム機械系分析装置および方法では、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力する。そして、入力されたモデル構成に基づき、当該モデルに対して仮想パワーの原理に基づくケイン法を適用することで、対象とする機械システムの運動方程式を自動的に導出する。
【０００３】
すなわち、特許文献１に記載された従来の剛体多体系の運動方程式導出装置では、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出することができる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平６−２２３０４９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来技術には、以下のような課題がある。
特許文献１に記載された従来の剛体多体系の運動方程式導出装置では、当該システムの一般化力を、各剛体に作用する外力ベクトルと当該剛体の部分速度との内積、および各剛体の質量中心まわりに作用する外モーメントベクトルと当該剛体の部分角速度との内積を全剛体について加算して導出している。このため、運動方程式の導出に必要な計算量が増加するという課題があった。
【０００６】
また、特許文献１に記載された従来の剛体多体系の運動方程式導出装置では、当該システムの一般化慣性力を、各剛体の慣性力ベクトルと当該剛体の部分速度との内積、および各剛体の質量中心まわりの慣性トルクベクトルと当該剛体の部分角速度との内積を全剛体について加算して導出している。このため、所定剛体に接続される剛体数の増加に伴い、導出した運動方程式において、一般化慣性力の記述量が大幅に増加するという課題もあった。
【０００７】
加えて、特許文献１に記載された従来の剛体多体系の運動方程式導出装置では、ある剛体の相対運動に微小量が含まれる場合でも、当該微小量に関する高次項を消去するような線形化を行っていない。このため、運動方程式導出に必要な計算量、および導出した運動方程式の記述量が大幅に増加するという課題もあった。
【０００８】
本発明は上記のような課題を解決するためになされたものであり、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出する際に、運動方程式導出に必要な計算量、および導出した運動方程式の記述量を低減することのできる剛体多体系の運動方程式導出装置および剛体多体系の運動方程式導出方法を得ることを目的とする。
【課題を解決するための手段】
【０００９】
本発明に係る剛体多体系の運動方程式導出装置は、各剛体の接続構成が開ループ構成となる機械システムに対して、設計者が当該機械システムを剛体多体系としてモデル化した際に、モデル構成を定義づけて入力するための手段である剛体多体系構成定義部と、剛体多体系構成定義部で定義されたモデル構成に基づいて、各剛体の連結構成を導出する剛体連結構成導出部と、剛体多体系構成定義部で定義されたモデル構成に基づいて、剛体連結部における変換行列を導出する変換行列導出部と、剛体多体系構成定義部で定義されたモデル構成、および変換行列導出部で導出された剛体連結部における変換行列に基づいて、各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出する慣性力・慣性トルク導出部と、剛体多体系構成定義部で定義されたモデル構成、剛体連結構成導出部で導出された各剛体の連結構成、および変換行列導出部で導出された剛体連結部における変換行列に基づいて、各剛体の一般化力を導出する一般化力導出部と、剛体多体系構成定義部で定義されたモデル構成、および慣性力・慣性トルク導出部で導出された各剛体の質量中心速度と角速度に基づいて、各剛体の部分速度および部分角速度を導出する部分速度・部分角速度導出部と、剛体連結構成導出部で導出された各剛体の連結構成、慣性力・慣性トルク導出部で導出された各剛体の慣性力および質量中心まわりの慣性トルク、一般化力導出部で導出された各剛体の一般化力、および部分速度・部分角速度導出部で導出された各剛体の部分速度および部分角速度に基づいて、各剛体の一般化慣性力および運動方程式を導出する一般化慣性力導出部とを備えるものである。
【００１０】
また、本発明に係る剛体多体系の運動方程式導出方法は、各剛体の接続構成が開ループ構成となる機械システムを設計者が剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出する剛体多体系の運動方程式導出装置に用いられる剛体多体系の運動方程式導出方法であって、機械システムに対して、設計者が当該機械システムを剛体多体系としてモデル化した際に、モデル構成を定義づけて入力する剛体多体系構成定義ステップと、剛体多体系構成定義ステップで定義されたモデル構成に基づいて、各剛体の連結構成を導出する剛体連結構成導出ステップと、剛体多体系構成定義ステップで定義されたモデル構成に基づいて、剛体連結部における変換行列を導出する変換行列導出ステップと、剛体多体系構成定義ステップで定義されたモデル構成、および変換行列導出ステップで導出された剛体連結部における変換行列に基づいて、各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出する慣性力・慣性トルク導出ステップと、剛体多体系構成定義ステップで定義されたモデル構成、剛体連結構成導出ステップで導出された各剛体の連結構成、および変換行列導出ステップで導出された剛体連結部における変換行列に基づいて、各剛体の一般化力を導出する一般化力導出ステップと、剛体多体系構成定義ステップで定義されたモデル構成、および慣性力・慣性トルク導出ステップで導出された各剛体の質量中心速度と角速度に基づいて、各剛体の部分速度および部分角速度を導出する部分速度・部分角速度導出ステップと、剛体連結構成導出ステップで導出された各剛体の連結構成、慣性力・慣性トルク導出ステップで導出された各剛体の慣性力および質量中心まわりの慣性トルク、一般化力導出ステップで導出された各剛体の一般化力、および部分速度・部分角速度導出部ステップ導出された各剛体の部分速度および部分角速度に基づいて、各剛体の一般化慣性力および運動方程式を導出する一般化慣性力導出ステップとを備えるものである。
【発明の効果】
【００１１】
本発明によれば、各剛体の接続構成が開ループ構成となる機械システムの一般化力導出に際して、各剛体の接続構成を考慮して独自の定式化手法を適用することにより、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力することで、ケイン法に基づいて自動的に当該システムの運動方程式を導出する際に、運動方程式導出に必要な計算量を低減することのできる剛体多体系の運動方程式導出装置および剛体多体系の運動方程式導出方法を得ることができる。
【図面の簡単な説明】
【００１２】
【図１】本発明の実施の形態１〜３で説明する機械システムにおける剛体連結構成例を示す構成図である。
【図２】本発明の実施の形態１〜３による剛体多体系の運動方程式導出装置において、処理の全体構成を示すブロック図である。
【図３】本発明の実施の形態１〜３による剛体多体系の運動方程式導出装置において、各剛体に固定する剛体固定枠、および各剛体の相対変位の定義を示す構成図である。
【図４】本発明の実施の形態１〜３による剛体多体系の運動方程式導出装置において、各剛体への作用力、および作用モーメントの定義を示す構成図である。
【図５】本発明の実施の形態１〜３による剛体多体系の運動方程式導出装置において、剛体連結構成導出部の処理構成を示すフローチャートである。
【図６】本発明の実施の形態２、３による剛体多体系の運動方程式導出装置において、一般化慣性力導出部の処理構成を示すフローチャートである。
【図７】本発明の実施の形態２、３による剛体多体系の運動方程式導出装置において、並進運動に関する一般化慣性力導出部の第ｉ行ブロック処理における処理構成を示すフローチャートである。
【図８】本発明の実施の形態２、３による剛体多体系の運動方程式導出装置において、回転運動に関する一般化慣性力導出部の第ｉ行ブロック処理における処理構成を示すフローチャートである。
【図９】本発明の実施の形態３による剛体多体系の運動方程式導出装置において、線形化の処理構成を示すフローチャートである。
【図１０】本発明の実施の形態３による剛体多体系の運動方程式導出装置において、部分速度・部分角速度導出部の処理構成を示すフローチャートである。
【発明を実施するための形態】
【００１３】
以下、本発明の剛体多体系の運動方程式導出装置および剛体多体系の運動方程式導出方法の好適な実施の形態につき図面を用いて説明する。
【００１４】
実施の形態１．
図１は、本発明の実施の形態１で説明する機械システムにおける剛体連結構成例を示す構成図である。図１に示すように、一般に機械システム１０は、設置土台をベースとして、複数の剛体１〜剛体８が連結して構成された剛体多体系としてモデル化することができる。
【００１５】
なお、図１では、剛体数が８個で構成された機械システム１０を例示しているが、剛体数は、これに限定されるものではない。また、本発明では、各剛体の接続構成が、開ループ構成となる機械システム１０を前提としている。この前提のもと、機械システム１０を構成する剛体数、所定剛体に接続される剛体数（一例として、図１の剛体１に接続される剛体数は、剛体２および剛体７の２個に相当する）について、任意の機械システム１０を対象としている。
【００１６】
図２は、本発明の実施の形態１による剛体多体系の運動方程式導出装置において、処理の全体構成を示すブロック図である。図２に示すように、本発明の実施の形態１による剛体多体系の運動方程式導出装置は、剛体多体系構成定義部１１、剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６、および一般化慣性力導出部１７を備えている。
【００１７】
剛体多体系構成定義部１１は、設計者が対象とする機械システム１０を剛体多体系としてモデル化し、当該モデルの構成を入力するための手段である。剛体連結構成導出部１２は、剛体多体系構成定義部１１で定義されたモデル構成に基づいて、剛体多体系を構成する各剛体（先の図１では、剛体１〜剛体８）に対して、先端側に接続された全ての剛体を導出し、剛体連結構成として出力する。
【００１８】
変換行列導出部１３は、剛体多体系構成定義部１１で定義されたモデル構成に基づいて、各剛体連結部における変換行列を導出して出力する。慣性力・慣性トルク導出部１４は、剛体多体系構成定義部１１で定義されたモデル構成、および変換行列導出部１３で導出された各剛体連結部における変換行列に基づいて、剛体多体系を構成する各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルクを導出して出力する。
【００１９】
一般化力導出部１５は、剛体多体系構成定義部１１で定義されたモデル構成、剛体連結構成導出部１２で導出された各剛体の連結構成、および変換行列導出部１３で導出された各剛体連結部における変換行列に基づいて、剛体多体系を構成する各剛体に対応する一般化力を導出して出力する。
【００２０】
部分速度・部分角速度導出部１６は、剛体多体系構成定義部１１で定義されたモデル構成、および慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度と角速度に基づいて、剛体多体系を構成する各剛体に対応する部分速度および部分角速度を導出して出力する。
【００２１】
さらに、一般化慣性力導出部１７は、剛体連結構成導出部１２で導出した各剛体の連結構成、慣性力・慣性トルク導出部１４で導出した各剛体の慣性力および質量中心まわりの慣性トルク、一般化力導出部１５で導出した各剛体の一般化力、および部分速度・部分角速度導出部１６で導出した各剛体の部分速度および部分角速度に基づいて、剛体多体系を構成する各剛体の一般化慣性力および運動方程式を導出して出力する。
【００２２】
図３は、本発明の実施の形態１による剛体多体系の運動方程式導出装置において、各剛体に固定する剛体固定枠、および各剛体の相対変位の定義を示す構成図である。図３を用いて、剛体ｋ（符号２１）に対して先端側に剛体ｌ（符号２２）が接続されている場合について説明する。
【００２３】
この場合、各剛体に固定する剛体固定枠、および各剛体の相対変位の定義は、以下のようになる。
（１）剛体ｋに関する剛体固定枠
（１−１）枠ｋ：剛体ｋの慣性主軸に固定した慣性主軸剛体固定枠
（１−２）枠ｋｌ：剛体ｋと剛体ｌとの連結部を規定し、剛体ｋに固定した連結部剛体固定枠
（２）剛体ｌに関する剛体固定枠
（２−１）枠ｌ：剛体ｌの慣性主軸に固定した慣性主軸剛体固定枠
（２−２）枠ｌｋ：剛体ｋと剛体ｌとの連結部を規定し、剛体ｌに固定した連結部剛体固定枠
（３）剛体ｋに対する剛体ｌの相対変位
（３−１）相対並進変位ｔｋｌ＝［ｘｋｌ  ｙｋｌ  ｚｋｌ］Ｔ
：連結部剛体固定枠ｋｌで表現した、連結部剛体固定枠ｋｌに対する連結部剛体固定枠ｌｋの相対並進変位代数ベクトル
（３−２）相対回転変位θｋｌ＝［θＸｋｌ  θＹｋｌ  θＺｋｌ］Ｔ
：オイラー角で表現した、連結部剛体固定枠ｋｌに対する連結部剛体固定枠ｌｋの相対回転変位
【００２４】
図４は、本発明の実施の形態１による剛体多体系の運動方程式導出装置において、各剛体への作用力、および作用モーメントの定義を示す構成図である。図４を用いて、剛体ｋ（符号２１）に対して先端側に剛体ｌ（符号２２）が接続され、剛体ｌ（符号２２）に対して先端側に剛体ｍ（符号２３）が接続されている場合について説明する。
【００２５】
この場合、剛体ｌへの作用力、および作用モーメントを、以下の（Ｉ）〜（ＶＩ）で定義する。
【００２６】
【数１】
2014078090.tif 000003
【００２７】
図５は、本発明の実施の形態１による剛体多体系の運動方程式導出装置において、剛体連結構成導出部１２の処理構成を示すフローチャートである。図５に示すように、本実施の形態１による剛体連結構成導出部１２は、処理ブロック３１〜３７を備えている。
【００２８】
そして、剛体連結構成導出部１２は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・機械システム１０を構成する全剛体数ｎｂ（図１ではｎｂ＝８に相当する）
・設置土台となるベースを剛体０として、剛体１→剛体ｎｂの順に、各剛体のベース側接続剛体番号を定義した剛体接続元ベクトルＢｏｄｙ＿ｔｏ＿Ｂａｓｅ（図１ではＢｏｄｙ＿ｔｏ＿Ｂａｓｅ＝［０  １  ２  ３  ４  ３  １  ７］に相当する）
を用いて、剛体多体系を構成する各剛体に対して、先端側に接続された全ての剛体を導出し、剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓとして出力する。
【００２９】
次に、図１〜図５に基づいて、本実施の形態１における剛体多体系の運動方程式導出装置の動作について説明する。
図１に示すように、対象とする機械システム１０に関して、設計者は、設置土台をベース（剛体０）とし、複数の剛体が連結して構成された剛体多体系として機械システム１０をモデル化する。この際、複数の剛体に割り付けるそれぞれの剛体番号を、ベースから先端に向かって増加するように、剛体１、剛体２、・・・と定義する。
【００３０】
次に、図２に示す剛体多体系構成定義部１１において、設計者が当該モデルの構成を入力する。剛体多体系構成定義部１１において、設計者が定義するモデル構成を表１に示す。
【００３１】
【表１】
2014078090.tif 000004
【００３２】
図２に示す剛体連結構成導出部１２は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・全剛体数ｎｂ
・剛体接続元ベクトルＢｏｄｙ＿ｔｏ＿Ｂａｓｅ
を用いて、剛体多体系を構成する各剛体に対して、先端側に接続された全ての剛体を導出し、剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓとして出力する。
【００３３】
さらに、剛体連結構成導出部１２は、先の図５に示したフローチャートに従って、一連処理を行う。まず、剛体連結構成導出部１２は、処理ブロック３１において、はじめに剛体連結構成の導出対象を剛体ｉ＝ｎｂに設定する。
【００３４】
次に、剛体連結構成導出部１２は、処理ブロック３２において、剛体ｉに関するベース側接続剛体ｍを下式（１）によって取得する。
【００３５】
【数２】
2014078090.tif 000005
【００３６】
次に、剛体連結構成導出部１２は、処理ブロック３３において、剛体ｉのベース側接続剛体ｍがベース（剛体０）か否かを判定する。そして、ベース側接続剛体ｍがベース（剛体０）である場合には、処理ブロック３６へ、ベース側接続剛体ｍが機械システム１０を構成する剛体である場合には、処理ブロック３４へ分岐する。
【００３７】
処理ブロック３４に進んだ場合には、剛体連結構成導出部１２は、列が機械システム１０を構成する剛体番号１〜ｎｂ、行が所定剛体に対する先端側接続剛体番号に対応する剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓに対して、剛体ｍに対する先端側接続剛体として、剛体ｉを下式（２）で追加する。
【００３８】
【数３】
2014078090.tif 000006
【００３９】
次に、剛体連結構成導出部１２は、処理ブロック３５において、剛体ｉに対する先端側接続剛体を、剛体ｍに対する先端側接続剛体として、下式（３）で追加する。
【００４０】
【数４】
2014078090.tif 000007
【００４１】
さらに、剛体連結構成導出部１２は、処理ブロック３６において、剛体連結構成の導出対象となる剛体番号ｉをデクリメントする。
【００４２】
最後に、剛体連結構成導出部１２は、処理ブロック３７において、剛体番号ｉがｉ≧１であれば、処理ブロック３２へ分岐して処理を繰り返し実行し、ｉ＝０であれば、一連処理を完了する。剛体連結構成導出部１２における以上の処理により、剛体多体系を構成する各剛体に対して、先端側に接続された全剛体が、剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓとして導出される。例えば、図１に示す機械システム１０の場合の剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓは、下式（４）となる。
【００４３】
【数５】
2014078090.tif 000008
【００４４】
続いて、図２に示す変換行列導出部１３は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・剛体ｋに対する剛体ｌの相対回転変位θｋｌ＝［θＸｋｌ  θＹｋｌ  θＺｋｌ］Ｔ
を用いて、剛体多体系を構成する各剛体の剛体連結部における変換行列として、連結部剛体固定枠ｋｌから連結部剛体固定枠ｌｋへの座標変換行列Ａｌｋ、ｋｌを、下式（５）で導出する。
【００４５】
【数６】
2014078090.tif 000009
【００４６】
さらに、変換行列導出部１３は、剛体ｋに対する剛体ｌの相対回転速度
【数７】
2014078090.tif 000010
から、連結部剛体固定枠ｋｌに対する連結部剛体固定枠ｌｋの相対角速度代数ベクトル
【数８】
2014078090.tif 000011
（連結部剛体固定枠ｋｌ表現）への変換式として下式（６）
【数９】
2014078090.tif 000012
で定義される変換行列Ｇｋｌを、下式（７）で導出する。
【００４７】
【数１０】
2014078090.tif 000013
【００４８】
ただし、ここでは表現の簡潔のために
【数１１】
2014078090.tif 000014
という略記法を用いており、かつ剛体ｋに対する剛体ｌの相対回転変位θｋｌとして１−２−３系オイラー角を適用している。
【００４９】
図２に示す慣性力・慣性トルク導出部１４は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・剛体質量ｍｌ
・剛体慣性テンソルＣＩｌ、ｌ
・剛体ｋに対する剛体ｌの相対並進変位ｔｋｌ
・剛体ｋに対する剛体ｌの相対回転変位θｋｌ
・変換行列Ａｌ、ｌｋおよびＡｋｌ、ｋ
・剛体構成ｒｌｌ、ｌｋおよびｒｋｋ、ｋｌ
と、変換行列導出部１３で導出された座標変換行列Ａｌｋ、ｋｌおよびＧｋｌを用いて、はじめに剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）に関して、慣性基準枠Ｏに対する慣性主軸剛体固定枠ｌの角速度代数ベクトルωｌＯｌ（慣性主軸剛体固定枠ｌ表現）を、ベース側から先端側に向かって剛体ｌ＝１、２、３・・・の順に、下式（８）で導出する。
【００５０】
【数１２】
2014078090.tif 000015
【００５１】
次に、慣性力・慣性トルク導出部１４は、慣性基準枠Ｏに対する慣性主軸剛体固定枠ｌの質量中心速度代数ベクトルｖｌＯｌ（慣性主軸剛体固定枠ｌ表現）を、ベース側から先端側に向かって剛体ｌ＝１、２、３・・・の順に、下式（９）で導出する。
【００５２】
【数１３】
2014078090.tif 000016
【００５３】
次に、慣性力・慣性トルク導出部１４は、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）に関して、慣性主軸剛体固定枠ｌで表現した慣性力代数ベクトルｍｌａｌＯｌを下式（１０）
【数１４】
2014078090.tif 000017
で導出し、慣性主軸剛体固定枠ｌで表現した質量中心まわりの慣性トルク代数ベクトル
【数１５】
2014078090.tif 000018
を、下式（１１）
【数１６】
2014078090.tif 000019
で導出し、全慣性力代数ベクトルｄＬｌを下式（１２）
【数１７】
2014078090.tif 000020
で構成し、全慣性トルク代数ベクトルｄＨｌ＿ｌを下式（１３）
【数１８】
2014078090.tif 000021
で構成する。
【００５４】
ただし、ここでは任意の代数ベクトルｒ＝［ｘ  ｙ  ｚ］Ｔに関するチルダマトリックス
【数１９】
2014078090.tif 000022
として、下式（１４）を用いている。
【００５５】
【数２０】
2014078090.tif 000023
【００５６】
図２に示す一般化力導出部１５は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・剛体ｋに対する剛体ｌの相対並進変位ｔｋｌ
・変換行列Ａｌ、ｌｋおよびＡｋｌ、ｋ
・剛体構成ｒｌｌ、ｌｋおよびｒｋｋ、ｋｌ
・剛体駆動力ｆｋｌ、ｋｌおよび剛体外力ｆｌ、Ｏ
・剛体駆動トルクｎｋｌ、ｋｌおよび剛体外モーメントｎｌ、Ｏ
と、剛体連結構成導出部１２で導出された剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓと、変換行列導出部１３で導出された座標変換行列Ａｌｋ、ｋｌおよびＧｋｌを用いて、はじめに剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）に関して、ベース側からの剛体連結構成を剛体ｊ→剛体ｋ→剛体ｌとして、ベース側から先端側に向かって剛体ｌ＝１、２、３・・・の順に、下式（１５）〜（１７）で与えられる座標変換行列を導出する。
【００５７】
【数２１】
2014078090.tif 000024
【００５８】
次に、一般化力導出部１５は、剛体ｌ（ｌ＝１〜ｎｂ）に対する全ての先端側接続剛体ｍ（ｍ＝Ｃｏｎｎｅｃｔｅｄ＿ｂｏｄｙｓ［：、ｌ］の非ゼロ要素）に関するベース側からの剛体連結構成を剛体ｈ→剛体ｍとして、ベース側から先端側に向かって、下式（１８）で与えられるチルダマトリックスを導出する。
【００５９】
【数２２】
2014078090.tif 000025
【００６０】
以上の準備のもと、一般化力導出部１５は、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）に関して、相対並進運動に関する一般化力Ｑｔｌを、下式（１９）で導出する。
【００６１】
【数２３】
2014078090.tif 000026
【００６２】
上式（１９）において、右辺第１項は、剛体ｌが接続されているベース側剛体ｋから、剛体連結部を介して剛体ｌへ作用する駆動力の連結部剛体固定枠ｋｌによる表現であり、駆動力項に相当する。
【００６３】
一方、上式（１９）の右辺第２項は、剛体ｌおよび剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外力の和の連結部剛体固定枠ｋｌによる表現であり、外力項に相当する。
【００６４】
このように、本実施の形態１による剛体多体系の運動方程式導出装置は、相対並進運動に関する一般化力Ｑｔｌを、駆動力項と外力項の和で与えている。
【００６５】
次に、一般化力導出部１５は、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）に関して、相対回転運動に関する一般化力Ｑθｌを、下式（２０）で導出する。
【００６６】
【数２４】
2014078090.tif 000027
【００６７】
上式（２０）において、右辺大括弧内の第１項は、剛体ｌが接続されているベース側剛体ｋから、剛体連結部を介して剛体ｌへ作用する駆動トルクの連結部剛体固定枠ｋｌによる表現であり、駆動トルク項に相当する。
【００６８】
一方、上式（２０）における右辺大括弧内の第２項は、剛体ｌおよび剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外モーメントの和の連結部剛体固定枠ｋｌによる表現であり、外モーメント項に相当する。
【００６９】
また、上式（２０）における右辺大括弧内の第３項は、剛体ｌおよび剛体ｌに対して先端側に接続された全ての剛体に関して、各剛体の質量中心に作用する外力による、剛体連結部固定枠ｌｋの原点に対するモーメントの和の連結部剛体固定枠ｋｌによる表現であり、外力項に相当する。
【００７０】
このように、本実施の形態１による剛体多体系の運動方程式導出装置は、相対回転運動に関する一般化力Ｑθｌを、駆動トルク項、外モーメント項、および外力項の和に対して、変換行列導出部１３で導出した剛体連結部における変換行列Ｇｋｌの転置行列を作用させた式で与えている。
【００７１】
一般化力導出部１５は、最後に、全並進一般化力ベクトルＱｔを下式（２１）
【数２５】
2014078090.tif 000028
で構成し、全回転一般化力ベクトルＱｒを下式（２２）
【数２６】
2014078090.tif 000029
で構成する。
【００７２】
図２に示す部分速度・部分角速度導出部１６は、剛体多体系構成定義部１１で定義されたモデル構成のうち
・剛体ｋに対する剛体ｌの相対並進変位ｔｋｌ
・剛体ｋに対する剛体ｌの相対回転変位θｋｌ
と、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖｌＯｌ、および角速度代数ベクトルωｌＯｌを用いて、はじめに相対並進変位ｔｋｌに対応する一般化スピードｕｔ、および相対回転変位θｋｌに対応する一般化スピードｕｒを、それぞれ下式（２３）、（２４）で導出する。
【００７３】
【数２７】
2014078090.tif 000030
【００７４】
ただし、ここでは、剛体ｌのベース側接続剛体を、アンダーバーを付した剛体ｌとして表現している。
【００７５】
次に、部分速度・部分角速度導出部１６は、一般化スピードｕｔおよびｕｒのうち、所定要素ｕｋに対する部分速度を下式（２５）
【数２８】
2014078090.tif 000031
で、部分角速度を下式（２６）
【数２９】
2014078090.tif 000032
で導出し、部分速度行列ＰＶｔおよびＰＶｒ、部分角速度行列ＰＷｒを、それぞれ下式（２７）〜（２９）で構成する。
【００７６】
【数３０】
2014078090.tif 000033
【００７７】
なお、一般化スピードｕｔに対する部分角速度は、常にゼロとなる。
【００７８】
図２に示す一般化慣性力導出部１７は、慣性力・慣性トルク導出部１４で導出した全慣性力代数ベクトルｄＬｌ、および全慣性トルク代数ベクトルｄＨｌ＿ｌと、一般化力導出部１５で導出した全並進一般化力ベクトルＱｔ、および全回転一般化力ベクトルＱｒと、部分速度・部分角速度導出部１６で導出した部分速度行列ＰＶｔ、ＰＶｒ、および部分角速度行列ＰＷｒを用いて、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）の並進運動に関する一般化慣性力行列ＧＩＦｔを、下式（３０）
【数３１】
2014078090.tif 000034
で導出し、回転運動に関する一般化慣性力行列ＧＩＦｒを、下式（３１）
【数３２】
2014078090.tif 000035
で導出し、機械システムの運動方程式として、下式（３２）、（３３）を出力する。
【００７９】
【数３３】
2014078090.tif 000036
【００８０】
以上のように、本実施の形態１による剛体多体系の運動方程式導出装置によれば、処理全体を、剛体多体系構成定義部１１、剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６、および一般化慣性力導出部１７で構成している。そして、剛体多体系構成定義部１１以外の各部１２〜１７を自動化している。
【００８１】
このような構成を備えることで、設計者が対象とする機械システムを剛体多体系としてモデル化し、当該モデルの構成を剛体多体系構成定義部１１において入力するだけで、独自の定式化手法により、機械システムの運動方程式を自動的に導出することができる。
【００８２】
また、本実施の形態１による剛体多体系の運動方程式導出装置によれば、剛体連結構成導出部１２において、所定剛体に対して先端側に接続された全剛体を導出している。さらに、一般化力導出部１５において、所定剛体の並進運動に対応する一般化力を、駆動力項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和で与え、所定剛体の回転運動に対応する一般化力を、駆動トルク項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外モーメント項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和に対して、剛体連結部における変換行列を作用させたもので与えている。この結果、一般化力の導出に関して、導出に必要な計算量を低減することができる。
【００８３】
なお、本実施の形態１による剛体多体系の運動方程式導出装置は、先の図１に示す機械システム１０を対象として説明したが、本発明はこれに限るものではない。機械システムにおける各剛体の接続構成が開ループ構成となっていれば、機械システムを構成する剛体数、および所定剛体に接続される剛体数について、任意の機械システムを対象としている。
【００８４】
また、本実施の形態１による剛体多体系の運動方程式導出装置は、所定剛体の相対回転変位として１−２−３系オイラー角を適用しているが、本発明は、これに限るものではなく、任意表現のオイラー角を適用することができる。
【００８５】
このように、実施の形態１によれば、設計者が機械システムを剛体多体系としてモデル化し、当該モデルの構成を入力するだけで、ケイン法に基づいて自動的に当該システムの運動方程式が導出できることに加えて、対象とする機械システムの一般化力導出に際して独自の定式化手法を適用することにより、運動方程式導出に必要な計算量を低減するような、剛体多体系の運動方程式導出装置を得ることができる。
【００８６】
実施の形態２．
本実施の形態２では、一般化慣性力導出部１７の機能を詳細に説明する。
【００８７】
本実施の形態２による剛体多体系の運動方程式導出装置において、機械システムにおける剛体連結構成例を示す構成図、処理の全体構成を示すブロック図、および各剛体の剛体固定枠、相対変位の定義は、先の実施の形態１における図１、図２、および図３と同一である。
【００８８】
また、本実施の形態２による剛体多体系の運動方程式導出装置において、各剛体への作用力、作用モーメントの定義、および剛体連結構成導出部１２の処理構成を示すフローチャートは、先の実施の形態１における図４、および図５と同一である。
【００８９】
図６は、本発明の実施の形態２による剛体多体系の運動方程式導出装置において、一般化慣性力導出部１７の処理構成を示すフローチャートである。図６に示すように、本実施の形態２による一般化慣性力導出部１７は、処理ブロック４１〜４６を備えている。
【００９０】
そして、一般化慣性力導出部１７は、剛体連結構成導出部１２で導出した剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓと、慣性力・慣性トルク導出部１４で導出した全慣性力代数ベクトルｄＬｌ、および全慣性トルク代数ベクトルｄＨｌ＿ｌと、一般化力導出部１５で導出した全並進一般化力ベクトルＱｔ、および全回転一般化力ベクトルＱｒと、部分速度・部分角速度導出部１６で導出した部分速度行列ＰＶｔ、ＰＶｒ、および部分角速度行列ＰＷｒを用いて、剛体多体系を構成する各剛体に対する一般化慣性力として、並進運動に関する一般化慣性力行列ＧＩＦｔ、および回転運動に関する一般化慣性力行列ＧＩＦｒを導出し、機械システムの運動方程式を出力する。
【００９１】
図７は、本発明の実施の形態２による剛体多体系の運動方程式導出装置において、並進運動に関する一般化慣性力導出部１７の第ｉ行ブロック処理４３における処理構成を示すフローチャートである。
【００９２】
図７に示すように、本実施の形態２による一般化慣性力導出部１７の第ｉ行ブロック処理４３は、処理ブロック５１〜５７を備えている。そして、第ｉ行ブロック処理４３は、剛体ｉの相対並進運動に対応する部分速度ＰＶｔ［３ｉ−ｊ、：］（ｊ＝０〜２）と、剛体ｉに対して先端側に接続された先端側接続剛体ｍの相対並進運動に対応する部分速度ＰＶｔ［３ｍ−ｊ、：］（ｊ＝０〜２）とを比較し、剛体ｉの相対並進運動に対応する部分速度ＰＶｔ［３ｉ−ｊ、：］のうち先端側接続剛体ｍに対応する項と、先端側接続剛体ｍの相対並進運動に対応する部分速度ＰＶｔ［３ｍ−ｊ、：］とが一致した場合に、並進運動に関して剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］、および一般化力Ｑｔ［３ｉ−ｊ］から、先端側接続剛体ｍの部分速度ＰＶｔ［３ｍ−ｊ、：］、および一般化力Ｑｔ［３ｍ−ｊ］を減じたものを、あらためて剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］、および一般化力Ｑｔ［３ｉ−ｊ］として出力する。
【００９３】
図８は、本発明の実施の形態２による剛体多体系の運動方程式導出装置において、回転運動に関する一般化慣性力導出部１７の第ｉ行ブロック処理４３’における処理構成を示すフローチャートである。
【００９４】
図８に示すように、本発明の実施の形態２による一般化慣性力導出部１７の第ｉ行ブロック処理４３’は、処理ブロック５１、５２、５３’〜５５’、５６、および５７を備えている。そして、第ｉ行ブロック処理４３’は、剛体ｉの相対回転運動に対応する部分角速度ＰＷｒ［３ｉ−ｊ、：］（ｊ＝０〜２）と、剛体ｉに対して先端側に接続された先端側接続剛体ｍの相対回転運動に対応する部分角速度ＰＷｒ［３ｍ−ｊ、：］（ｊ＝０〜２）とを比較し、剛体ｉの相対回転運動に対応する部分角速度ＰＷｒ［３ｉ−ｊ、：］のうち先端側接続剛体ｍに対応する項と、先端側接続剛体ｍの相対回転運動に対応する部分角速度ＰＷｒ［３ｍ−ｊ、：］とが一致した場合に、回転運動に関して剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］、部分角速度ＰＷｒ［３ｉ−ｊ、：］、および一般化力Ｑｒ［３ｉ−ｊ］から、先端側接続剛体ｍの部分速度ＰＶｒ［３ｍ−ｊ、：］、部分角速度ＰＷｒ［３ｍ−ｊ、：］、および一般化力Ｑｒ［３ｍ−ｊ］を減じたものを、あらためて剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］、部分角速度ＰＷｒ［３ｉ−ｊ、：］、および一般化力Ｑｒ［３ｉ−ｊ］として出力する。
【００９５】
次に、先の図１〜図５に加え、図６〜図８に基づいて、本実施の形態２における剛体多体系の運動方程式導出装置の動作について説明する。
図１に示すような機械システム１０に関して、設計者によるモデル化の方法は、先の実施の形態１と同一である。
【００９６】
また、図２に示す剛体多体系構成定義部１１におけるモデル構成の定義、および剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６における処理内容は、全て、先の実施の形態１と同一である。
【００９７】
そこで、図２に示す一般化慣性力導出部１７における、本実施の形態２での具体的な処理構成を、図６を用いて詳述する。一般化慣性力導出部１７は、処理ブロック４１において、はじめに、一般化慣性力の導出対象となる剛体番号を、ｉ＝ｎｂに設定する。
【００９８】
次に、一般化慣性力導出部１７は、処理ブロック４２において、剛体ｉに対する先端側接続剛体の有無を、剛体連結構成導出部１２で導出した剛体連結構成行列Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｉ］で判定する。そして、一般化慣性力導出部１７は、剛体ｉに対する先端側接続剛体が存在しない場合（Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｉ］＝０）には、処理ブロック４４へ、剛体ｉに対する先端側接続剛体が存在する場合（Ｃｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｉ］≠０）には、処理ブロック４３へ分岐する。
【００９９】
一般化慣性力導出部１７は、処理ブロック４３に進んだ場合における並進運動に関する具体的な処理を、図７に示した処理ブロック５１〜５７によって行う。
【０１００】
一般化慣性力導出部１７は、処理ブロック５１において、はじめに、制御変数ｋ＝ｌに設定する。次に、一般化慣性力導出部１７は、処理ブロック５２において、剛体ｉに関してベース側からｋ番目の先端側接続剛体ｍを、下式（３４）で取得する。
【０１０１】
【数３４】
2014078090.tif 000037
【０１０２】
そして、先端側接続剛体ｍが存在しない場合（ｍ＝０）には、処理ブロック５６へ、先端側接続剛体ｍが存在する場合（ｍ≠０）には、処理ブロック５３へ分岐する。
【０１０３】
処理ブロック５３に進んだ場合には、一般化慣性力導出部１７は、剛体ｉの相対並進運動に対応する部分速度のうち、Ｚ〜Ｘ成分（ｊ＝０〜２）の各々について、先端側接続剛体ｍに対応する項である下式（３５）
【数３５】
2014078090.tif 000038
と、先端側接続剛体ｍの相対並進運動に対応する部分速度のうち、先端側接続剛体ｍに対応する項である下式（３６）
【数３６】
2014078090.tif 000039
とを比較する。そして、両者が一致する場合には、処理ブロック５４へ、一致しない場合には、処理ブロック５６へ分岐する。
【０１０４】
処理ブロック５４に進んだ場合には、以下の条件（ａ）または条件（ｂ）のどちらか一方が成立する場合に処理ブロック５５へ、条件（ａ）および条件（ｂ）がともに不成立となる場合に処理ブロック５６へ分岐する。
【０１０５】
条件（ａ）
先端側接続剛体ｍに対する先端側接続剛体が存在せず、下式（３７）
【数３７】
2014078090.tif 000040
が成立する。
【０１０６】
条件（ｂ）
先端側接続剛体ｍに対する全ての先端側接続剛体、つまりＣｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｍ］における全ての非ゼロ要素ｌについて、剛体ｉの相対並進運動に対応する部分速度のうち剛体ｌに対応する項である下式（３８）
【数３８】
2014078090.tif 000041
と、先端側接続剛体ｍの相対並進運動に対応する部分速度のうち剛体ｌに対応する項である下式（３９）
【数３９】
2014078090.tif 000042
が一致する。
【０１０７】
処理ブロック５５に進んだ場合には、一般化慣性力導出部１７は、処理ブロック５４において、条件（ａ）または条件（ｂ）のどちらか一方が成立した成分ｊについて、並進運動に関する剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］から、先端側接続剛体ｍの部分速度ＰＶｔ［３ｍ−ｊ、：］を減じた、下式（４０）を、あらためて剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］とする。
【０１０８】
【数４０】
2014078090.tif 000043
【０１０９】
さらに、一般化慣性力導出部１７は、一般化力Ｑｔ［３ｉ−ｊ］から、先端側接続剛体ｍの一般化力Ｑｔ［３ｍ−ｊ］を減じた、下式（４１）を、あらためて剛体ｉの一般化力Ｑｔ［３ｉ−ｊ］とする。
【０１１０】
【数４１】
2014078090.tif 000044
【０１１１】
最後に、一般化慣性力導出部１７は、処理ブロック５６において、制御変数ｋをインクリメントし、処理ブロック５７において、制御変数ｋがｋ≦ｎｂ−１であれば処理ブロック５２へ分岐して処理を繰り返し実行し、ｋ＝ｎｂであれば処理ブロック４３の一連処理を完了する。
【０１１２】
また、一般化慣性力導出部１７は、処理ブロック４３に進んだ場合における、回転運動に関する具体的な処理を、図８に示した処理ブロック５１、５２、５３’〜５５’、５６、および５７によって行う。なお、図８における処理ブロック５１、５２、５６、および５７は、先の図７に示した並進運動に関する処理構成の処理ブロック５１、５２、５６、および５７と同一である。
【０１１３】
処理ブロック５３’に進んだ場合には、一般化慣性力導出部１７は、剛体ｉの相対回転運動に対応する部分角速度のうち、Ｚ〜Ｘ成分（ｊ＝０〜２）の各々について、先端側接続剛体ｍに対応する項である下式（４２）
【数４２】
2014078090.tif 000045
と、先端側接続剛体ｍの相対回転運動に対応する部分角速度のうち、先端側接続剛体ｍに対応する項である下式（４３）
【数４３】
2014078090.tif 000046
とを比較する。そして、両者が一致する場合には、処理ブロック５４’へ、一致しない場合には、処理ブロック５６へ分岐する。
【０１１４】
処理ブロック５４’に進んだ場合には、以下の条件（ａ’）または条件（ｂ’）のどちらか一方が成立する場合に処理ブロック５５’へ、条件（ａ’）および条件（ｂ’）がともに不成立となる場合に処理ブロック５６へ分岐する。
【０１１５】
条件（ａ’）
先端側接続剛体ｍに対する先端側接続剛体が存在せず、上式（３７）が成立する。
【０１１６】
条件（ｂ’）
先端側接続剛体ｍに対する全ての先端側接続剛体、つまりＣｏｎｎｅｃｔｅｄ＿Ｂｏｄｙｓ［：、ｍ］における全ての非ゼロ要素ｌについて、剛体ｉの相対回転運動に対応する部分角速度のうち剛体ｌに対応する項である下式（４４）
【数４４】
2014078090.tif 000047
と、先端側接続剛体ｍの相対回転運動に対応する部分角速度のうち剛体ｌに対応する項である下式（４５）
【数４５】
2014078090.tif 000048
が一致する。
【０１１７】
処理ブロック５５’に進んだ場合には、一般化慣性力導出部１７は、処理ブロック５４’において、条件（ａ’）または条件（ｂ’）のどちらか一方が成立した成分ｊについて、回転運動に関する剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］から、先端側接続剛体ｍの部分速度ＰＶｒ［３ｍ−ｊ、：］を減じた、下式（４６）を、あらためて剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］とする。
【０１１８】
【数４６】
2014078090.tif 000049
【０１１９】
さらに、一般化慣性力導出部１７は、回転運動に関する剛体ｉの部分角速度ＰＷｒ［３ｉ−ｊ、：］から、先端側接続剛体ｍの部分角速度ＰＷｒ［３ｍ−ｊ、：］を減じた、下式（４７）を、あらためて剛体ｉの部分角速度ＰＷｒ［３ｉ−ｊ、：］とする。
【０１２０】
【数４７】
2014078090.tif 000050
【０１２１】
さらに、一般化慣性力導出部１７は、一般化力Ｑｒ［３ｉ−ｊ］から、先端側接続剛体ｍの一般化力Ｑｒ［３ｍ−ｊ］を減じた、下式（４８）を、あらためて剛体ｉの一般化力Ｑｒ［３ｉ−ｊ］とする。
【０１２２】
【数４８】
2014078090.tif 000051
【０１２３】
以上のような図７、図８に示した内容が、図６に示す一般化慣性力導出部１７のうち、処理ブロック４３に関する処理構成である。
【０１２４】
次に、一般化慣性力導出部１７は、処理ブロック４４において、一般化慣性力の導出対象となる剛体番号ｉをデクリメントする。さらに、一般化慣性力導出部１７は、処理ブロック４５において、剛体番号ｉがｉ≧１であれば、処理ブロック４２へ分岐して処理を繰り返し実行し、ｉ＝０であれば、処理ブロック４６へ分岐する。
【０１２５】
処理ブロック４６に進んだ場合には、一般化慣性力導出部１７は、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）の並進運動に関する一般化慣性力行列ＧＩＦｔを上式（３０）で導出し、回転運動に関する一般化慣性力行列ＧＩＦｒを上式（３１）で導出する。さらに、一般化慣性力導出部１７は、機械システムの運動方程式として、上式（３２）、（３３）を出力し、一連処理を終了する。
【０１２６】
上述した処理ブロック４３において、一般化慣性力導出部１７は、剛体ｉの相対並進運動に対応する部分速度ＰＶｔ［３ｉ−ｊ、：］（ｊ＝０〜２）と、剛体ｉに対して先端側に接続された先端側接続剛体ｍの相対並進運動に対応する部分速度ＰＶｔ［３ｍ−ｊ、：］（ｊ＝０〜２）とを比較し、剛体ｉの相対並進運動に対応する部分速度ＰＶｔ［３ｉ−ｊ、：］のうち先端側接続剛体ｍに対応する項と、先端側接続剛体ｍの相対並進運動に対応する部分速度ＰＶｔ［３ｍ−ｊ、：］とが一致した場合に、並進運動に関して剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］、および一般化力Ｑｔ［３ｉ−ｊ］から、先端側接続剛体ｍの部分速度ＰＶｔ［３ｍ−ｊ、：］、および一般化力Ｑｔ［３ｍ−ｊ］を減じたものを、あらためて剛体ｉの部分速度ＰＶｔ［３ｉ−ｊ、：］、および一般化力Ｑｔ［３ｉ−ｊ］として構成している。
【０１２７】
当該処理は、結果として得られる機械システムの運動方程式である上式（３２）で考えた場合、並進運動に関して剛体ｉの運動方程式から、先端側接続剛体ｍの運動方程式を減じたものを、あらためて剛体ｉの運動方程式とすることと等価となる。このとき、並進運動に関する剛体ｉの運動方程式は、下式（４９）で与えられる。
【０１２８】
【数４９】
2014078090.tif 000052
【０１２９】
また、上述した処理ブロック４３’において、一般化慣性力導出部１７は、剛体ｉの相対回転運動に対応する部分角速度ＰＷｒ［３ｉ−ｊ、：］（ｊ＝０〜２）と、剛体ｉに対して先端側に接続された先端側接続剛体ｍの相対回転運動に対応する部分角速度ＰＷｒ［３ｍ−ｊ、：］（ｊ＝０〜２）とを比較し、剛体ｉの相対回転運動に対応する部分角速度ＰＷｒ［３ｉ−ｊ、：］のうち先端側接続剛体ｍに対応する項と、先端側接続剛体ｍの相対回転運動に対応する部分角速度ＰＷｒ［３ｍ−ｊ、：］とが一致した場合に、回転運動に関して剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］、部分角速度ＰＷｒ［３ｉ−ｊ、：］、および一般化力Ｑｒ［３ｉ−ｊ］から、先端側接続剛体ｍの部分速度ＰＶｒ［３ｍ−ｊ、：］、部分角速度ＰＷｒ［３ｍ−ｊ、：］、および一般化力Ｑｒ［３ｍ−ｊ］を減じたものを、あらためて剛体ｉの部分速度ＰＶｒ［３ｉ−ｊ、：］、部分角速度ＰＷｒ［３ｉ−ｊ、：］、および一般化力Ｑｒ［３ｉ−ｊ］として構成している。
【０１３０】
当該処理は、結果として得られる機械システムの運動方程式である上式（３３）で考えた場合、回転運動に関して剛体ｉの運動方程式から、先端側接続剛体ｍの運動方程式を減じたものを、あらためて剛体ｉの運動方程式とすることと等価となる。このとき、回転運動に関する剛体ｉの運動方程式は、下式（５０）で与えられる。
【０１３１】
【数５０】
2014078090.tif 000053
【０１３２】
以上のように、本実施の形態２による剛体多体系の運動方程式導出装置によれば、処理全体を、剛体多体系構成定義部１１、剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６、および一般化慣性力導出部１７で構成している。そして、剛体多体系構成定義部１１以外の各部１２〜１７を自動化している。
【０１３３】
このような構成を備えることで、設計者が対象とする機械システムを剛体多体系としてモデル化し、当該モデルの構成を剛体多体系構成定義部１１において入力するだけで、機械システムの運動方程式を自動的に導出することができる。
【０１３４】
さらに、本実施の形態２による剛体多体系の運動方程式導出装置によれば、剛体連結構成導出部１２において、所定剛体に対して先端側に接続された全剛体を導出している。さらに、一般化力導出部１５において、所定剛体の並進運動に対応する一般化力を、駆動力項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和で与え、所定剛体の回転運動に対応する一般化力を、駆動トルク項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外モーメント項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和に対して、剛体連結部における変換行列を作用させたもので与えている。この結果、一般化力の導出に関して、導出に必要な計算量を低減することができる。
【０１３５】
加えて、本実施の形態２による剛体多体系の運動方程式導出装置によれば、一般化慣性力導出部１７において、所定剛体に対して先端側に接続された先端側剛体の相対並進運動（相対回転運動）に対応する部分速度（部分角速度）と、所定剛体の相対並進運動（相対回転運動）に対応する部分速度（部分角速度）のうち、先端側接続剛体に対応する項とが一致する場合に、並進運動（回転運動）に関して所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、所定剛体の運動方程式としている。この結果、導出した運動方程式における一般化慣性力の記述量を低減することができる。
【０１３６】
このように、実施の形態２によれば、一般化慣性力の導出に際して、所定剛体に対して先端側に接続されている先端側剛体の部分速度または部分角速度と、当該所定剛体の部分速度または部分角速度のうち先端側剛体に対応する項とが一致する場合に、所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、所定剛体の運動方程式としている。これにより、先の実施の形態１の効果に加え、導出した運動方程式における一般化慣性力の記述量を低減するような、剛体多体系の運動方程式導出装置を得ることができる。
【０１３７】
実施の形態３．
本実施の形態３では、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、運動方程式導出に必要な計算量、および導出した運動方程式の記述量の低減を図る場合について詳細に説明する。なお、この「微小量」とは、所定量以下の変位量を意味している。
【０１３８】
本実施の形態３による剛体多体系の運動方程式導出装置において、機械システムにおける剛体連結構成例を示す構成図、処理の全体構成を示すブロック図、および各剛体の剛体固定枠、相対変位の定義は、先の実施の形態１における図１、図２、および図３と同一である。
【０１３９】
また、本実施の形態３による剛体多体系の運動方程式導出装置において、各剛体への作用力、作用モーメントの定義、および剛体連結構成導出部１２の処理構成を示すフローチャートは、先の実施の形態１における図４、および図５と同一である。
【０１４０】
さらに、本実施の形態３による剛体多体系の運動方程式導出装置において、一般化慣性力導出部１７の処理構成を示すフローチャート、並進運動に関する一般化慣性力導出部１７の第ｉ行ブロック処理における処理構成を示すフローチャート、および回転運動に関する一般化慣性力導出部１７の第ｉ行ブロック処理における処理構成を示すフローチャートは、先の実施の形態２における図６、図７、および図８と同一である。
【０１４１】
図９は、本発明の実施の形態３による剛体多体系の運動方程式導出装置において、線形化の処理構成を示すフローチャートである。図９に示すように、本実施の形態３による線形化処理６０は、処理ブロック６１および６２を備えている。
【０１４２】
そして、線形化処理６０は、変数ｅｐｓを乗じた微小量について、線形化対象に含まれる正弦関数および余弦関数を１次近似で置換して展開し、さらに、ｅｐｓ２＝０として微小量に関する２次以上の項を消去することで、線形化を行う。
【０１４３】
図１０は、本発明の実施の形態３による剛体多体系の運動方程式導出装置において、部分速度・部分角速度導出部１６の処理構成を示すフローチャートである。図１０に示すように、本実施の形態３による部分速度・部分角速度導出部１６は、処理ブロック７１〜７７を備えている。
【０１４４】
そして、部分速度・部分角速度導出部１６は、ベース側接続剛体ｋに対する剛体ｌの相対並進変位ｔｋｌおよび相対回転変位θｋｌに微小量が含まれる場合に、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖｌＯｌ、および角速度代数ベクトルωｌＯｌと、所定の一般化スピードｕｋから、部分速度
【数５１】
2014078090.tif 000054
および部分角速度
【数５２】
2014078090.tif 000055
を導出する。
【０１４５】
次に、先の図１〜図８に加え、図９、図１０に基づいて、本実施の形態３における剛体多体系の運動方程式導出装置の動作について説明する。
図１に示すような機械システム１０に関して、設計者によるモデル化の方法は、先の実施の形態１と同一である。また、図２に示す剛体連結構成導出部１２、および変換行列導出部１３における処理内容も、先の実施の形態１と同一である。
【０１４６】
本実施の形態３による剛体多体系の運動方程式導出装置は、図２に示す剛体多体系構成定義部１１において、ベース側接続剛体ｋに対する剛体ｌの相対並進変位ｔｋｌおよび相対回転変位θｋｌに微小量が含まれる場合、当該微小量を変数ｅｐｓを乗じた形式で定義する。
【０１４７】
例えば、ベース側接続剛体ｋに対する剛体ｌの相対並進変位ｔｋｌを下式（５１）
【数５３】
2014078090.tif 000056
で定義した場合、ベース側接続剛体ｋに対する剛体ｌの相対並進変位に関して、ベース側接続剛体ｋに固定された連結部剛体固定枠ｋｌに対して、剛体ｌに固定された連結部剛体固定枠ｌｋが、（連結部剛体固定枠ｋｌから見て）Ｘ軸方向に微小な大きさｘｋｌで、Ｙ軸方向に有意な大きさｙｋｌで並進変位し、Ｚ軸方向には相対的な並進変位を生じないことを表している。
【０１４８】
一方で、ベース側接続剛体ｋに対する剛体ｌの相対回転変位θｋｌを、下式（５２）
【数５４】
2014078090.tif 000057
で定義した場合、ベース側接続剛体ｋに対する剛体ｌの相対回転変位に関して、ベース側接続剛体ｋに固定された連結部剛体固定枠ｋｌに対して、剛体ｌに固定された連結部剛体固定枠ｌｋが、（オイラー角で表現して）Ｘ軸方向、およびＺ軸方向に微小な大きさθＸｋｌ、およびθＺｋｌで回転変位し、Ｙ軸方向には相対的な回転変位を生じないことを表している。
【０１４９】
次に、図２に示す慣性力・慣性トルク導出部１４は、先の実施の形態１と同様に、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）に関して、慣性基準枠Ｏに対する慣性主軸剛体固定枠ｌの角速度代数ベクトルωｌＯｌ（慣性主軸剛体固定枠ｌ表現）を上式（８）で導出し、慣性基準枠Ｏに対する慣性主軸剛体固定枠ｌの質量中心速度代数ベクトルｖｌＯｌ（慣性主軸剛体固定枠ｌ表現）を上式（９）で導出する。次いで、慣性力・慣性トルク導出部１４は、慣性主軸剛体固定枠ｌで表現した慣性力代数ベクトルｍｌａｌＯｌを上式（１０）で導出し、慣性主軸剛体固定枠ｌで表現した質量中心まわりの慣性トルク代数ベクトル
【数５５】
2014078090.tif 000058
を上式（１１）で導出する。
【０１５０】
さらに、慣性力・慣性トルク導出部１４は、図９に示す線形化処理に基づき、剛体ｌの相対回転変位θｋｌ（ｌ＝１〜ｎｂ）に微小量ｅｐｓ＊θｊｋｌ（ｊ＝Ｘ、Ｙ、Ｚ）が含まれる場合には、処理ブロック６１において、角速度代数ベクトルωｌＯｌ、質量中心速度代数ベクトルｖｌＯｌ、慣性力代数ベクトルｍｌａｌＯｌ、および慣性トルク代数ベクトル
【数５６】
2014078090.tif 000059
に含まれる正弦関数を下式（５３）
【数５７】
2014078090.tif 000060
で、余弦関数を下式（５４）
【数５８】
2014078090.tif 000061
で、それぞれ１次近似して展開する。
【０１５１】
次に、慣性力・慣性トルク導出部１４は、処理ブロック６２において、ｅｐｓ２＝０とすることで、微小量に関する２次以上の項を消去し、線形化した角速度代数ベクトルωｌＯｌ、質量中心速度代数ベクトルｖｌＯｌ、慣性力代数ベクトルｍｌａｌＯｌ、および慣性トルク代数ベクトル
【数５９】
2014078090.tif 000062
を導出する。
【０１５２】
慣性力・慣性トルク導出部１４は、最後に、先の実施の形態１と同様、全慣性力代数ベクトルｄＬｌを上式（１２）で、全慣性トルク代数ベクトルｄＨｌ＿ｌを上式（１３）で構成する。
【０１５３】
図２に示す一般化力導出部１５は、先の実施の形態１と同様に、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）に関して、座標変換行列、およびチルダマトリックスを上式（１５）〜（１８）で導出し、相対並進運動に関する一般化力Ｑｔｌを上式（１９）で導出し、相対回転運動に関する一般化力Ｑθｌを上式（２０）で導出する。
【０１５４】
一般化力導出部１５は、さらに、図９に示す線形化処理に基づき、剛体ｌの相対回転変位θｋｌ（ｌ＝１〜ｎｂ）に微小量ｅｐｓ＊θｊｋｌ（ｊ＝Ｘ、Ｙ、Ｚ）が含まれる場合には、処理ブロック６１において、一般化力ＱｔｌおよびＱθｌに含まれる正弦関数を上式（５３）で、余弦関数を上式（５４）で、それぞれ１次近似して展開する。
【０１５５】
次に、一般化力導出部１５は、処理ブロック６２において、ｅｐｓ２＝０とすることで、微小量に関する２次以上の項を消去し、線形化した一般化力ＱｔｌおよびＱθｌを導出して、全並進一般化力ベクトルＱｔを上式（２１）で、全回転一般化力ベクトルＱｒを上式（２２）で構成する。
【０１５６】
図２に示す部分速度・部分角速度導出部１６は、先の実施の形態１と同様に、はじめに相対並進変位ｔｋｌに対応する一般化スピードｕｔを上式（２３）で導出し、相対回転変位θｋｌに対応する一般化スピードｕｒを上式（２４）で導出する。
【０１５７】
次いで、部分速度・部分角速度導出部１６は、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖｌＯｌ、および角速度代数ベクトルωｌＯｌと、一般化スピードｕｔおよびｕｒの所定要素ｕｋから、図１０に示す処理構成に基づいて部分速度
【数６０】
2014078090.tif 000063
および部分角速度
【数６１】
2014078090.tif 000064
を導出する。
【０１５８】
図１０に示す処理ブロック７１では、一般化スピードがｕｋ≠０である場合には、処理ブロック７３へ分岐し、ｕｋ＝０である場合には、処理ブロック７２に分岐して、対応する部分（角）速度を下式（５５）とする。
【０１５９】
【数６２】
2014078090.tif 000065
【０１６０】
部分速度・部分角速度導出部１６は、処理ブロック７３において、一般化スピードｕｋにおける変数ｅｐｓの次数を判定し、次数が１の場合には、処理ブロック７５へ分岐し、次数が０の場合には、処理ブロック７４へ分岐して、対応する部分（角）速度を、下式（５６）で導出する。
【０１６１】
【数６３】
2014078090.tif 000066
【０１６２】
ただし、上式（５６）では、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖｌＯｌ、および角速度代数ベクトルωｌＯｌが、変数ｅｐｓおよび一般化スピードｕｋの双方に関して１次以下となることを用いている。
【０１６３】
部分速度・部分角速度導出部１６は、処理ブロック７５において、下式（５７）
【数６４】
2014078090.tif 000067
によって一時的に一般化スピードｕｋから変数ｅｐｓを消去し、処理ブロック７６において、下式（５８）を算出する。
【０１６４】
【数６５】
2014078090.tif 000068
【０１６５】
ただし、上式（５８）では、慣性力・慣性トルク導出部１４で導出した各剛体の質量中心速度代数ベクトルｖｌＯｌ、および角速度代数ベクトルωｌＯｌが、変数ｅｐｓおよび一般化スピードｕｋ＝ｅｐｓ＊ｕの双方に関して１次以下となることを用いている。
【０１６６】
一般化スピードｕｋが変数ｅｐｓについて１次となる場合の部分（角）速度は、処理ブロック７７において、下式（５９）によって導出される。
【０１６７】
【数６６】
2014078090.tif 000069
【０１６８】
部分速度・部分角速度導出部１６は、最後に、先の実施の形態１と同様、部分速度行列ＰＶｔおよびＰＶｒを上式（２７）および（２８）で、部分角速度行列ＰＷｒを上式（２９）で構成する。
【０１６９】
図２に示す一般化慣性力導出部１７は、先の実施の形態２と同様に、図６〜図８に示す処理構成で剛体多体系の部分速度行列ＰＶｔ、ＰＶｒ、部分角速度行列ＰＷｒ、および一般化力ベクトルＱｔ、Ｑｒを導出し、剛体多体系を構成する剛体ｌ（ｌ＝１〜ｎｂ）の並進運動に関する一般化慣性力行列ＧＩＦｔを上式（３０）で導出し、回転運動に関する一般化慣性力行列ＧＩＦｒを上式（３１）で導出する。
【０１７０】
一般化慣性力導出部１７は、さらに、図９に示す線形化処理に基づき、剛体ｌの相対回転変位θｋｌ（ｌ＝１〜ｎｂ）に微小量ｅｐｓ＊θｊｋｌ（ｊ＝Ｘ、Ｙ、Ｚ）が含まれる場合には、処理ブロック６１において、一般化慣性力行列ＧＩＦｔおよびＧＩＦｒに含まれる正弦関数を上式（５３）で、余弦関数を上式（５４）で、それぞれ１次近似して展開する。
【０１７１】
次に、一般化慣性力導出部１７は、処理ブロック６２において、ｅｐｓ２＝０とすることで、微小量に関する２次以上の項を消去し、線形化した一般化慣性力行列ＧＩＦｔおよびＧＩＦｒを導出する。
【０１７２】
最後に、一般化慣性力導出部１７は、一般化慣性力行列ＧＩＦｔ、ＧＩＦｒ、および一般化力ベクトルＱｔ、Ｑｒに対して、下式（６０）
【数６７】
2014078090.tif 000070
を代入した上で、機械システムの運動方程式として、上式（３２）および上式（３３）を出力する。
【０１７３】
以上のように、本実施の形態３による剛体多体系の運動方程式導出装置によれば、処理全体を、剛体多体系構成定義部１１、剛体連結構成導出部１２、変換行列導出部１３、慣性力・慣性トルク導出部１４、一般化力導出部１５、部分速度・部分角速度導出部１６、および一般化慣性力導出部１７で構成している。そして、剛体多体系構成定義部１１以外の処理部１２〜１７を自動化している。
【０１７４】
このような構成を備えることで、設計者が対象とする機械システムを剛体多体系としてモデル化し、当該モデルの構成を剛体多体系構成定義部１１において入力するだけで、機械システムの運動方程式を自動的に導出することができる。
【０１７５】
さらに、本実施の形態３による剛体多体系の運動方程式導出装置によれば、剛体連結構成導出部１２において、所定剛体に対して先端側に接続された全剛体を導出している。さらに、一般化力導出部１５において、所定剛体の並進運動に対応する一般化力を、駆動力項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和で与え、所定剛体の回転運動に対応する一般化力を、駆動トルク項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外モーメント項と、所定剛体および所定剛体に対して先端側に接続された全剛体を対象とした外力項との和に対して、剛体連結部における変換行列を作用させたもので与えている。この結果、一般化力の導出に関して、導出に必要な計算量を低減することができる。
【０１７６】
さらに、本実施の形態３による剛体多体系の運動方程式導出装置によれば、一般化慣性力導出部１７において、所定剛体に対して先端側に接続された先端側剛体の相対並進運動（相対回転運動）に対応する部分速度（部分角速度）と、所定剛体の相対並進運動（相対回転運動）に対応する部分速度（部分角速度）のうち、先端側接続剛体に対応する項とが一致する場合に、並進運動（回転運動）に関して所定剛体の運動方程式から先端側剛体の運動方程式を減じたものを、所定剛体の運動方程式としている。この結果、導出した運動方程式における一般化慣性力の記述量を低減することができる。
【０１７７】
加えて、本実施の形態３による剛体多体系の運動方程式導出装置によれば、所定剛体の相対並進運動および相対回転運動に微小量が含まれる場合に、慣性力・慣性トルク導出部１４において導出する各剛体の質量中心速度、角速度、慣性力、および質量中心まわりの慣性トルク、一般化力導出部１５において導出する各剛体の一般化力、および一般化慣性力導出部１７において導出する各剛体の一般化慣性力に関して、当該微小量に関する２次以上の項を消去することで線形化を行うようにした。この結果、対象とする機械システムの動特性表現精度を維持したまま、運動方程式導出に必要な計算量、および導出した運動方程式における記述量の双方を低減することができる。
【０１７８】
このように、実施の形態３によれば、対象とする機械システムにおいて所定剛体の相対運動に微小量が含まれる場合に、当該微小量に関する２次以上の項を消去して線形化することを行っている。これにより、先の実施の形態１、２の効果に加え、運動方程式導出に必要な計算量、および導出した運動方程式の記述量の双方を低減するような、剛体多体系の運動方程式導出装置を得ることができる。
【符号の説明】
【０１７９】
１〜８  剛体、１０  機械システム、１１  剛体多体系構成定義部、１２  剛体連結構成導出部、１３  変換行列導出部、１４  慣性力・慣性トルク導出部、１５  一般化力導出部、１６  部分速度・部分角速度導出部、１７  一般化慣性力導出部。
【図１】
2014078090.tif 000071
【図２】
2014078090.tif 000072
【図３】
2014078090.tif 000073
【図４】
2014078090.tif 000074
【図５】
2014078090.tif 000075
【図６】
2014078090.tif 000076
【図７】
2014078090.tif 000077
【図８】
2014078090.tif 000078
【図９】
2014078090.tif 000079
【図１０】
2014078090.tif 000080