JP2001277161A - 有限要素法を用いたマニピュレータの制御方法 - Google Patents

Abstract

(57)【要約】 【課題】 冗長度の高いマニピュレータの制御方法を提
供する。 【解決手段】複数のリンク部材３，４と該リンク部材を
接合する能動的あるいは受動的な複数の関節１，２とか
らなるマニピュレータの有限要素法を用いた制御方法で
あって、有限要素法による該マニピュレータのモデルに
おいて、該関節に対応した関節節点１０，１１，１２に
加えて、リンク部材と関節とからなるユニットであるリ
ンクの重心に実質的に対応する点に重心節点と呼ぶ節点
８，９を設けてリンク部材３，４に対応する要素を２分
し、数値積分点を該重心節点８，９にシフトすることに
より各関節におけるトルクを算出することを特徴とする
マニピュレータの制御方法。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、有限要素法を用い
たマニピュレータの制御方法に関する。より具体的に
は、本発明は、有限要素法を用いてリンク数が大きいマ
ニピュレータの制御を可能にする並列制御方法に関す
る。

【０００２】

【従来の技術】近年、環境や作業条件の変化に伴い、制
御システムのソフトウエアは変えずにハードウェアのみ
を様々な形状に再構成するロボットの並列制御という概
念が注目を浴びている。このような適応性に優れたロボ
ットシステムの概念はパラレルロボティクスと呼ばれ、
宇宙、海洋、採鉱、建築などの作業条件が時々刻々変化
する場での応用が期待されている。システムの小サイズ
化のみならず、例えば遠隔操作中にハードウェア構成を
変化させたり一部の要素が機能を失ったとしても、系全
体の機能が維持できる高い冗長性を持つことがその大き
な特徴である。このような概念に基づくシステムを構築
するためには、ハードウェアの変化に柔軟に対応する制
御方法の開発が必要不可欠である。

【０００３】

【発明が解決しようとする課題】ニュートン・オイラー
法やラグランジュ法などを用いた従来の制御法では、系
の形状及びリンク数の違いなどによって運動方程式が変
化する。例えば、図７に示すようなリンク部材３１，３
２，３３と関節３５，３６，３７とからなるマニピュレ
ータの場合、根元のリンク部材３１の関節４１のトルク
を算出する際には、リンク部材３０から外向きにリンク
部材３２、最終端のリンク部材３３へと速度と加速度を
順次計算し、その後反対向きに根元に向かって関節トル
クを順次計算していく。この際、リンク部材ごとに異な
る原点と軸を持つ相対的な回転座標系が用いられている
ため、リンク部材間の座標変換を繰り返し行って、リン
ク部材間の力の相互作用を求める必要がある。すなわ
ち、各リンク部材の情報が相互のリンク部材情報に依存
しあった、直列的な運動方程式の立て方になっている。
このため、例えば、リンク部材数の増減が生じたり、関
節が動かなくなるといった系の一部に故障が発生した場
合などには、ソフトウエア内の記述を大幅に変更する必
要が生じ、敏速かつ柔軟な対応ができないという欠点が
ある。

【０００４】また、系全体の冗長度を高めるためには、
取り扱えるリンクの数を大きくしておく必要が必然的に
生じる。ニュートン・オイラー法などでは、リンク数が
増えるに従って計算量は幾何級数的に増加し、一桁のリ
ンク数でも通常のワークステーションの容量の限界に達
してしまう。したがって、リアルタイムの制御を行うこ
とは冗長度の高いマニピュレータの場合には不可能にな
っていた。

【０００５】本発明は、これらの従来技術による制御方
法が本質的に有する限界を取り払うことを目的とするも
のである。

【０００６】

【課題を解決するための手段】本発明は、複数のリンク
部材と該リンク部材を接合する能動的あるいは受動的な
複数の関節とからなるマニピュレータの有限要素法を用
いた制御方法であって、有限要素を用いた該マニピュレ
ータのモデル化において、該関節に対応した関節節点に
加えて、リンクの重心に実質的に対応する点に重心節点
と呼ぶ節点を設けてリンクに対応する要素を２分し、数
値積分点を該重心節点に向けてシフトすることにより各
関節におけるトルクを算出することを特徴とするマニピ
ュレータの制御方法を提供するものである。ここで、
「リンク」とは、１個のリンク部材と１個の関節を含ん
でなるものとして認識される。

【０００７】数値積分点は、通常、上記の有限要素の中
心にあるものであるが、本発明によれば、この数値積分
点を予めシフトして塑性ヒンジを有限要素端に表現し
て、最小限の要素数で高精度な関節トルクの値を得るこ
とができる。つまり、数値積分点を有限要素の中で「関
節節点」と中央に関して対称の位置、すなわち反対側に
シフトする。そうすることにより「関節節点」での関節
を表現することができるようになる。逆に言うと、有限
要素の中間に積分点がある場合には，その位置に関節が
表現されることになり、マニピュレータの機構をうまく
モデル化したことにはならないおそれがある。したがっ
て、以下に説明するように、「重心節点」に重心がうま
く位置するように質量マトリックスを定義する。

【０００８】なお、関節節点は剛節状態にあるものと考
えても、塑性状態にあると考えてもよい。このような制
御方法を、マニピュレータを構成するリンク部材と関節
の一部に適用することもできる。

【０００９】実際の制御においては、例えば、直交座標
系に基づいた目標軌道データ（座標値や回転角などを時
系列的に並べたもの）を作成する。これは、目標軌道を
大まかにいくつかの点で定義することにより自動的に計
算することもできるし、あるいは、各軌道上の点を数値
で定義することによっても作成することができるのであ
る。この目標軌道データを入力して、その後プログラム
を実行することにより、各関節の関節トルクを計算する
ことができる。

【００１０】また、本発明は、制御方法を実施するため
のプログラムを記憶したコンピュータ読み取り可能な記
憶媒体にも関するものであり、そのような制御方法を実
施するシステムあるいは制御装置にも関する。プログラ
ム自体はどのような言語でも記述可能である。そして、
このようなプログラムは通常のコンピュータ上で実行す
ることができるが、ＲＯＭに格納して、ＤＳＰボードな
どの特別のハードウェアを使用して制御を行うようにす
ることもできる。

【００１１】さらに、本発明の制御方法は、その他公知
のフィードバック制御技術や、センサ技術と組み合わせ
て、壊れやすいものを柔らかく把持するといった特定の
目的に適合する制御あるいはより精密な制御を行うため
に利用することができる。本発明の制御方法は、ハード
ウェアの条件次第では、リンク数が１０から１００以上
といった冗長度の極めて高いマニピュレータの実時間に
おける制御を可能にする。

【００１２】

【発明の実施の形態】本発明の方法においては、有限要
素法によるアルゴリズムにＳＩ法（ShiftedIntegration
法）を導入する。これは、マニピュレータをモデル化す
る際に要素数を削減することが、計算時間を短縮するた
めに必要であるためである。そこで、骨組構造の有限要
素解析において用いられるＳＩ法を、マニピュレータ
（リンク機構）のモデル化に適用する。マニピュレータ
を構成するリンクが複数あるとき、各リンクを構成する
リンク部材と関節のうちのリンク部材に対応する有限要
素の中心にある数値積分点を予めシフトすることによ
り、関節を要素の端部に表現できるようにするものであ
る。この手法によれば、最小限の要素数で高精度な関節
トルク値を得ることができる。ここで、要素端のヒンジ
をピン状態（ヒンジの自由な回転が可能な状態）から剛
節状態（ヒンジが全く動かない状態）まで変化させるこ
とができ、リンク部材を剛体棒とした通常のマニピュレ
ータからフレキシブル・マニピュレータまで、種々のマ
ニピュレータの挙動解析を行うことができる。

【００１３】図１の（ａ）に示す有限要素法における線
形チモシェンコはり要素と、それと等価な、同図（ｂ）
に示すヒンジが回転バネおよびせん断バネによって表現
される剛体バネモデルとを考える。この図１においては
２次元のモデルを考えており、はりの中心を原点とし
て、両端をｚ＝１，−１になるように正規化して表現し
ている。図１の（ｂ）に示す剛体バネモデルにおいて
は、２本の剛体棒が回転バネとせん断バネ（それぞれ黒
い丸で表す）により接続されている形で表現される。そ
れぞれのモデルのひずみエネルギ近似式を考えることに
より、数値積分点の位置（ｓ₁）とバネ（あるいは塑性
ヒンジ）の位置（ｒ₁）の関係が次の式で与えられるこ
とが知られている（都井裕、「骨組み構造および回転対
称シェル構造の有限要素解析におけるShifted Integrat
ion法について」、日本造船学会論文集、１６８、１９
９０、ｐｐ．３５７−３６９）。 ｓ₁＝−ｒ₁ または ｒ₁＝−ｓ₁ すなわち、有限要素法における数値積分点ｓを右にシフ
トすると剛体バネモデルにおける回転バネおよびせん断
バネ（黒丸）は左に移動する。この式に従って、ヒンジ
が生じうる点と原点に関して対称の位置に数値積分点を
予め配置することにより、精度よく解を得ることができ
る。そして、このような剛体バネモデルにおけるバネ剛
性を正規化した剛度値Ｃ_motで表すと、Ｃ_mot＝０がピン
状態、Ｃ _mot＝１が剛節状態を表すことになる。したが
って、０から１の間の数値により部材端または関節の半
剛節状態を表すことができ、リンク部材の剛性を考慮し
たフレキシブル・マニピュレータの挙動解析も可能とな
る。

【００１４】図２に、ＳＩ法によるリンク機構のモデル
化の概念図を示す。図２の左側の（ａ）にモデル化する
現実のリンク機構を示す。このリンク機構は２個の関節
１，２と２個のリンク部材３，４とからなっている。リ
ンク部材３は、関節１を介して、固定された基部５に接
続しており、ここでは、説明を単純化するため、完全な
剛体であると仮定して取り扱う。しかし、上述のよう
に、この仮定は本発明に必須のものでは全くない。それ
ぞれの関節１，２はサーボモータなどの駆動モータが仕
込まれているものと考えることができる。このリンク部
材３，４には、それぞれ重心６，７があるが、それらを
＋で表す。これに対して、図２の右側（ｂ）にＳＩ法に
基づく有限要素によるモデル化の例を示す。図２（ｂ）
では、数値モデルをより視覚的に表現するために剛体バ
ネモデルの形式を用いる。ここでは、現実のリンク部材
３，４にある重心６，７を、数値モデルにおける節点
８，９として表現し、現実の関節１，２に対応する節点
１０，１１（「関節節点」という）と区別するため、こ
のような節点８，９を「重心節点」と呼ぶこととする。
そして、現実の関節１，２がピン状態であると仮定する
のがよい近似であることが多いのに対応して、関節節点
１０，１１，１２はピン状態にあるが（要素１６の質量
を節点９側に寄せるために関節接点１２もピン状態にあ
るものとする）、重心６，７に対応して考える重心節点
８，９は、剛節状態にあるものと考えることができる。

【００１５】そして、重心節点８，９を考えたことに対
応して、各リンク部材３，４は数値モデルにおける二つ
の要素１３，１４と要素１５，１６とに分かれることに
なる。これらの要素１３〜１６は線形チモシェンコはり
要素（有限要素）である。そして、リンク部材３に対応
する要素１３，１４の数値積分点は関節節点１０，１１
の反対の重心節点８上にシフトするものと考え、リンク
部材４に対応する要素１５，１６の数値積分点は重心節
点９上にシフトするものと考える。これらのシフトした
数値積分点を×で表す。重なっているので各重心節点上
には一つの×しか図示されていないが、これは実際には
二つの数値積分点が重なり合ったものである。このよう
に仮想の節点（重心節点）を考えてマニピュレータを構
成するリンク部材の重心を表現することは、従来なされ
ていないことに留意されたい。すなわち、重心節点に数
値積分点をシフトすることによって、リンク部材の重心
を有限要素法において正しくモデル化できるようにな
る。要素１６の遠端１２は、ここでは自由端であるが、
外力が端部１２に作用した場合、端部１２も同様にピン
関節として働くので、節点の一つと考えておくべきであ
る。また上述のように、モデル化の際の積分点のシフト
のためにこのような節点１２が必要である。

【００１６】本実施例においては、ニューマークのβ法
（γ＝１／２、β＝１／４、定加速度法）による陰解法
を時間積分法として採用する。陰解法とは、求解の対象
である変位が陽に与えられておらず、連立方程式を解く
ことによってはじめて解が得られる解法のことである。
逆行列の計算などが必要という特徴を持つ。１ステップ
あたりの計算時間は、陽解法に比べると長いが、時間増
分が比較的長くても、解が安定的に算出可能である。ま
た、ニューマークのβ法は、陰解法の一種で、運動方程
式を近似的に解く時間積分手法である。問題や条件によ
って定加速度法と線形加速度法を選択することができ、
解の安定性もよいことが一般に知られている。ニューマ
ークのβ法は、時系列的に解く問題に対して利用でき
る。これらの解法は、数学分野・工学分野を問わず広い
分野で一般的に使用されている数値積分手法である。

【００１７】このような計算手法を用いて、前ステップ
の１回微分、２回微分の情報と、現ステップの増分値を
利用して、線形的に現ステップの１回微分と２回微分の
情報を得る。これを用いて、増分形連立方程式を解くこ
とにより、次ステップで必要な増分値を求める。そし
て、これを順次繰り返してゆく。

【００１８】外力増分を入力し、変位増分を求める順解
析では、時刻ｔにおける速度ベクトルおよび加速度ベク
トルを用いて次式の運動方程式を解く。ここで、順解析
とは、外力から変位を求める解析をいい、変位から外力
を求める逆解析に対応するものである。この運動方程式
は図２（ｂ）のモデル全体に一般的に当てはまるもの
で、従来から知られているものなので、ここでは導出を
繰り返さない（例えば、鷲津久一郎、宮本博、山田嘉
昭、山本善之、川井忠彦共編、有限要素法ハンドブック
II応用編、ｐｐ．２９−６４参照）。

【００１９】

【数１】 ここで、［Ｍ］は全体質量マトリクス、［Ｃ］は全体減
衰マトリクス、｛Ｆ｝は外力ベクトル（図２の例では重
力および関節に働く力のみが外力である）、｛Ｒ｝は内
力ベクトル（内部応力を表すベクトル）、△ｔは時間増
分である。なお、ここで、関節における力はトルクでは
なく力の単位で与えられていることに留意されたい。
［Ｋ］は全体剛性マトリクスで、ここでは、［Ｄ］マト
リクス（断面力-ひずみマトリクス）内の全ての成分に
バネのかたさを表すＣ_motをかけることにより、各要素
については次のように表される。

【００２０】

【数２】 また、時刻ｔ＋△ｔにおける速度と加速度ベクトルは、
次式のように変位増分と前ステップ（時刻ｔ）の速度、
加速度成分によって与えられることが分かっている。

【００２１】

【数３】 時刻ｔ＋△ｔにおける外力項が時刻ｔの外力項と節点力
（有限要素における節点の位置にはたらく力である。ト
ルクの単位ではない。）の増分との和で表されることに
注意して数１の式を変形すると、節点力の増分｛△ｆ｝
は、

【００２２】

【数４】 と与えられる。ここでは、数値積分点位置をｓ₁（−１
≦ｓ₁≦１）として、以下のような要素質量マトリクス
［ｍ］を定義することができる。対角項以外の成分はゼ
ロになるので、対角項成分のみを示すと、

【００２３】

【数５】 ここで、

【００２４】

【数６】 である。ここで、ｍ₁，ｍ₂は質量の並進方向成分であ
り、ｔ₁，ｔ₂は質量のねじれ方向成分である。また、
ρ、Ａ，ｌ，Ｉ_zは、それぞれ、リンク部材の密度、リ
ンク部材の断面積、有限要素の長さ、断面２次極モーメ
ントである。上記の質量マトリクスでは、関節がｒ₁＝
−１（すなわちｓ₁＝１）の位置にある場合、ｒ ₁＝１の
位置に質量が集中する。

【００２５】数６の関係は、ＳＩ法の考え方を導入した
ために、通常、ｍ₁＝ρＡｌ／２＝ｍ₂で与えられる関係
（つまり、関節節点および重心節点の双方に質量が集中
している形になっている）がｓ₁によりシフトされたも
のになっているものであり、本発明において新たに導入
された考え方を直接的に表現する関係である。

【００２６】一方、関節がｒ₁＝１（すなわちｓ₁＝−
１）の位置にある場合には、ｒ₁＝−１の位置に要素の
質量が集中することとなる。すなわち、１個のリンク部
材を２個の有限要素により表現することにより、リンク
部材の重心位置に質量を集中させたリンク部材の表現が
可能となるものである（図２参照）。

【００２７】ここでは、ピン関節と剛体リンクにより構
成される通常のマニピュレータを考え、関節節点につい
てはＣ_mot＝０であると仮定し、関節の持つ剛性を考慮
しないこととする。また、簡単のために減衰マトリック
ス［Ｃ］を無視すると（リンク部材の剛性が十分に高け
れば内部での減衰は小さいので、この場合、これは悪い
仮定ではない）、数４は以下のように表される。

【００２８】

【数７】 この式に変位増分および時刻ｔにおける速度と加速度を
入力することにより、節点における力の増分が算出でき
る。これを逐次的に足しあわせると節点における力が求
まる。そして、下記に示すトルク算出式に節点における
力を代入することにより、リンク機構の各関節のトルク
を求めることができる。このような関節トルクを制御す
ることによりリンク機構の動きを制御することができ
る。

【００２９】なお、有限要素法においては、マニピュレ
ータのような複数のリンク部材がつながった連続体の挙
動を各要素についても直交座標系で記述するため、コリ
オリ力のような回転座標系に特有の見かけの力などの非
線形項と呼ばれる力について、別個に考える必要はな
い。また、リンク部材と関節に対する重力の作用を考慮
したいわゆる重力補償項については、各節点に静的に自
重をかけることで対処することができる。そのため、直
交座標系におけるトルクの算出式は比較的に簡潔なもの
となる。ここでは、一般的なｎ個のリンク部材からなる
リンク機構を例として考える。

【００３０】図３にｎ個のリンク部材からなるリンク機
構を示す。ｉ番目のリンク部材にはたらく力と、リンク
部材の番号と節点番号との間の関係を示す。ｉ番目のリ
ンク部材には、その重心に作用する並進力に起因する回
転モーメントと、リンク機構のさらに先にある関節に作
用するｉ＋１番目からｎ番目のリンクの並進力の合力に
起因する回転モーメントと、重心回りに作用する慣性モ
ーメントとが生じる。したがって、ｉ番目のリンク部材
の関節トルクτ_iは、これらの和にｉ＋１番目のリンク
の関節トルクτ_i+1を加算したものとなり、リンク部材
（または要素）の座標系における節点にかかる力を用い
ると、

【００３１】

【数８】 で表される。ここで、ｌ_iCは関節からリンク部材の重心
位置までの距離を表し、ｌ_iはリンク部材の長さであ
る。Ｆ_iCは重心にはたらく力を意味する。以後、変数右
下の添え字ｉ，ｊなどは対象とするリンク部材の番号を
示し、ｈ，ｋは節点番号を示す。Ｃは重心位置での各成
分、ｘ，ｚは要素座標系におけるｘ軸とｚ軸の各軸方向
成分、Ｘ，Ｚは全体座標系におけるＸ軸とＺ軸の各軸方
向成分、φは同じく全体座標系におけるＹ軸回りの成分
であることを示す。また、変数右上の添え字ｎは、対象
とするリンク機構におけるリンク部材の総数を示す。

【００３２】数８の式をｉ＝１，２，．．．，ｎについ
て並べてマトリクス形式に直し、全体座標系で整理する
と、関節トルクベクトルは、

【００３３】

【数９】 と表現することができる。ここで、｛Ｐⁿ｝，｛Ｔⁿ｝、
［Ｌⁿ］は、下記に説明するものである。

【００３４】まず、｛Ｐⁿ｝は次のように定義される５
ｎ×１の節点における力に関するベクトルである。

【００３５】

【数１０】 ただし、

【００３６】

【数１１】 である。すなわち、ｉ番目のリンク部材についての全体
座標系で表した重心における力Ｆ_iCX、Ｆ_iCZと、ｉ＋１
番目からｎ番目のリンク部材についての合力と、回転方
向の力Ｆ_iφとを成分とするベクトルである。ここで
は、２次元のリンク機構を考えているので成分の数は５
であるが、３次元の場合でも全く同様で、単に成分の数
が８になる点で異なるだけである。

【００３７】また、［Ｔⁿ］は次のように表される５ｎ
×５ｎの座標変換マトリックスである。

【００３８】

【数１２】 ただし、［Ｔ_i］はｉ番目のリンク部材の全体座標系か
らリンク部材（または要素）座標系への座標変換マトリ
クスで、座標系間の回転角をφ_iとすると、

【００３９】

【数１３】 と表される。従来の方法により関節トルクを算出する場
合には、リンク部材間の座標変換をリンク部材ごとに繰
り返していかなればならなかったが、数９により算出す
る場合には、全体座標系から要素座標系への変換が一回
行われるのみで済む。このため、特にリンク部材の数が
大きい場合、従来の方法に比べてトルク算出の計算時間
は大幅に短縮される。

【００４０】次に、［Ｌⁿ］は、以下のように表される
部材長マトリクスである。

【００４１】

【数１４】 ただし、

【００４２】

【数１５】 である。要素座標系で表されているので、部材に直交す
るｘ軸方向成分のみが必要となり、２番目と４番目の成
分がゼロとなる。また、回転方向成分は部材の長さに関
係しないので、５つ目の成分は１となる。要素座標系に
変換された節点における力に関するベクトル［Ｔⁿ］
｛Ｐⁿ｝が上記マトリクスに掛けられることにより、対
象とするリンク部材より先の節点力に関する情報が加算
されることとなる。

【００４３】ｉ番目のリンク部材の重心に対応する重心
節点ｋ（＝２ｉ）に生じる節点力の増分に関するベクト
ル｛Δｐ_k｝を以下のように定義する。

【００４４】

【数１６】 このベクトルを用いると、時刻ｔ＋Δｔにおけるｉ番目
のリンク部材の重心節点における力に関するベクトル
は、以下のように逐次的に求めることができる。

【００４５】

【数１７】 重心節点における力の増分に関するベクトル｛Δｐ_k｝
の各成分は、それぞれ、重心節点ｋの全体座標系におけ
る２軸方向並進力の増分と、ｋ＋１から２ｎ＋１番目の
節点までの全体座標系２軸方向並進力の増分の和と、節
点ｋの全体座標系Ｙ軸回りのモーメント増分となってい
る。数１７の式によって逐次的に求められたベクトル
｛Ｐⁿ｝を数９の式に代入することにより、ｎ個の関節
トルクの逐次的な値が計算される。

【００４６】ある動きを実現するために必要なトルクを
表すトルク曲線の算出例として、ｎ＝８の場合について
考える。ここで算出されたトルク曲線に沿ったトルクを
各関節節点においてサーボモータなどにより生成するこ
とにより、リンク機構が所望の動きをするよう制御する
ことができる。図４に８個のリンク部材からなるリンク
機構に与えた目標軌道を示す。長さ２０ｃｍのリンク部
材（重心位置は中央で、質量は１０７．５ｇ）を８本結
合し、１秒間（１００ステップ）で動くように軌道を設
定した。図５と図６には、それぞれ、従来のニュートン
・オイラー法と本発明の制御方法によって算出されたト
ルク曲線を示す。この例では、全長１６０ｃｍにも及ぶ
機構に短時間で急激な動きを与えているため、重力や遠
心力などの影響が大きいことが予想される。しかし、こ
のような問題でも、重力補償やコリオリ力を考慮してい
る従来の手法による結果（図５）に対して、本発明の制
御方法による結果（図６）がよく一致していることが確
認できる。なお、ニュートン・オイラー法によるトルク
の算出にはMathematica 4.0を使用し、本発明の制御方
法においてはフォートランで記述したプログラムをマイ
クロソフト社のFortran PowerStation 4.0でコンパイル
したものを使用した。双方ともデル社のDimension XPS
T600(CPU: Intel Pentium III 600MHz, RAM:383MB)上で
実行した。ニュートン・オイラー法では、リンク部材の
数に応じて運動方程式を新しく記述し直す必要がある。
たとえば、故障により関節が一つ動かなくなった場合を
想定すると、系全体の運動方程式を立て直す必要が生じ
る。また、ニュートン・オイラー法では、リンク部材の
数が増加するについて膨大な量の式をメモリ内に蓄える
必要がある。そのため、上記のＰＣシステムでは、ｎ＝
４で早くもＲＡＭの上限を越えてしまい、仮想メモリの
使用が始まってしまう。ｎ＝９では計算不能に陥った。
計算時間は、リンク部材の数の増加につれて指数関数的
に増加し、例えば、８個のリンク部材からなる機構の場
合、Mathematica 4.0により運動方程式を導出するまで
に１３分、それから関節トルクを算出するまで７０分を
要した。

【００４７】

【発明の効果】本発明による制御方法によれば、リンク
部材の数が増大した場合も含め、計算時間が圧倒的に短
い。リンク部材の数が８個の場合、制御対象時間が１秒
であったのに対し、計算時間は０．１秒以下に抑えられ
た。また、従来はほぼ実行不可能なリンク部材数が２０
の場合でも、０．６秒で制御のための計算が終了した。
使用ソフトが異なるので単純に比較することはできない
が、本発明の制御方法は、特にリンク部材の数が多い場
合には圧倒的に優れていることが確認できた。例えば、
リンク数が１００といった冗長度の極めて高いマニピュ
レータの制御も可能であり、蛇のような多関節のマニピ
ュレータの動きの制御が可能になる。

【図面の簡単な説明】

【図１】本発明において用いられるＳＩ法に基づく有限
要素モデル化の原理を、剛体バネモデルによるモデル化
と対比して示すための模式図である。

【図２】本発明によるモデル化の対象となる現実のリン
クシステム（マニピュレータ）（ａ）とＳＩ法に基づく
有限要素によるモデル化の例（ｂ）を示す模式図であ
る。

【図３】ｎ個のリンク部材からなるマニピューレータの
節点における力のかかり方および有限要素によるモデル
化例を示す模式図である。

【図４】本発明の実施例において使用された８リンク機
構における目標軌道を示すグラフである。

【図５】従来例であるニュートン・オイラー法により計
算された、図４の軌道を実現されるために求められる各
関節節点のトルクを時系列的に示すグラフである。

【図６】本発明によるＳＩ法に基づく有限要素法を用い
た実施例による図４の軌道を実現されるために求められ
る各関節節点のトルクを時系列的に示すグラフである。

【図７】従来法を説明するためのマニピュレータの模式
図である。

【符号の説明】

１，２ 関節 ３，４ リンク部材 ５ 基部 ６，７ リンクの重心 ８，９ 重心節点 １０，１１，１２ 関節節点 １３，１４，１５，１６ 有限要素

Claims (6)

【特許請求の範囲】
1. 【請求項１】 複数のリンク部材と該リンク部材を接合
   する能動的あるいは受動的な複数の関節とからなるマニ
   ピュレータの有限要素法を用いた制御方法であって、有
   限要素法による該マニピュレータのモデルにおいて、該
   関節に対応した関節節点に加えて、該リンク部材１個と
   該関節１個とからなるリンクの重心に実質的に対応する
   点に重心節点と呼ぶ節点を設けて該リンク部材に対応す
   る要素を２分し、数値積分点を該重心節点にシフトする
   ことにより各関節におけるトルクを算出することを特徴
   とするマニピュレータの制御方法。
2. 【請求項２】 関節節点が塑性状態にあることを特徴と
   する請求項１記載のマニピュレータの制御方法。
3. 【請求項３】 関節節点が剛節状態にあることを特徴と
   する請求項１記載のマニピュレータの制御方法
4. 【請求項４】 請求項１記載の制御方法を、マニピュレ
   ータを構成するリンク部材と関節の一部に適用すること
   を特徴とするマニピュレータの制御方法。
5. 【請求項５】 請求項１〜４のいずれかに記載の制御方
   法を実施するためのプログラムを記憶したコンピュータ
   読み取り可能な記憶媒体。
6. 【請求項６】 請求項１〜４のいずれかに記載の制御方
   法を実施することを特徴とするマニピュレータの制御装
   置。