計測自動制御学会東北支部第177回研究集会（199呂．10．30）
資料番号177＿5

Mathematicaを用いたマニピュレータの運動方程式の構築
UsingMathematicafbrManipulatorDynamicsEquation
Described○孫 信偉フ 大久保 垂範○ⅩinweiSun，ShigenoriOkubo
山形大学工学部

FacultyofEngineerlng，Y瓦magataUniversity
キーワ⊥ド＝ マニビュレp夕（Manil）ulator），軌特性方程式〔Dynamics・Equation），Ma［henlatjca

連絡先＝ 〒9g2山形県米沢市城南4−3−16 山形大学工学部機械システム⊥学科大久保研究室
拝 信凰恥1∴（023印6−3246・Fax・：〔02叫26−3205．H十mail：Ⅹ鵬皿触ipult工a．yZ．yamagata−u．aC．jp

Abstract：工nthispaperwtHhowsadesiEnb】・maIlip11latorusinE mathelTlatica■WeLllCIldeveLopthemanjpulator，in

theLagrangianIVerydiff；c111trrlerelytoob［ainthedynamic呂eqtlationsforasystenlaSCOmplicateda占aanydegreeoffreedoTrl
manipuコator・＾ccordjngtolinkparanlCterS．WeW里1makeu5eOfmath巳fflati｛＝af】rUgraTTlmeChanicsasthisnlethodall（川SuSLo
Obtai工1Lhedynamicsequationsh）TVeryCOmPlicatedsysLcmi【1thesimplestmannerpossible・

1． はじめにコンピュータ数学システムⅣIatheI¶ati仁nは数値計算、数式処理、グラフィックスが強力でありユー¶ザ
インターフェースの優れたシステムの構造に適している。
従来のロボットマニビュレ山夕は，高精度な位置決め，あるいは連動は可能であるが人間が実際に行っている巧みな運動は苦手である。ただし，ロボットマニピュレータの設計するとき，運動方程式中に含まれる各種パラメータの値がわかっていなければならない。マニピュレータの運動方程式を導き，正確な結果を得るのはかなり困難であるので∴本構想は九・I丸山el艮aticaを用いて一 各リンクの質量，質量中心位置，慣性テンソルー 摩擦JJなどのリンクパラメータから，マニピュレータの運動方程式を構築することである。

2．マニピュレータの動特性方程式
丁変換行列で表された任意のマニピュレータに対する動特性方程式を導くときに，これを5段階に分けて行う。まず任意のリンクの点の速度を計算し．つぎに運動のエネルギーを計算する．それから位置一1

エネルギーを出し，ラグランジアンを作り，最後にこれを微分して動特性方程式を求める。

2．1リンクの座標系についてリンク座標系を固定すれば，座標系虐一1と元の関係をつぎの回転および並進によって確定できる。
1）ヱi−1軸まわりに角度βiだけ回転させる。

2）ヱト1に沿って距離琉だけ並進させる。

3）回転後の町ルすなわちェiに沿って長さ叫だけ並進させる。
4）Ei軸まわりにねじれ角αlだけ回転させる。

以上の操作は，リンク宜の座標系をリンク宜−1の座標系に関係づける4つの同次変換の積として表現でき，これがT行列とよばれるものである白 すなわち

豆￣1了1＝nr肌血】1，叫＿．トT。血］1，勘1トれ「射、。（z竃，dt）・n。南，鋸COS拘）−SiIl帆）0（1）叫＿1

COS〔叫−1〕呂in〔叫 cos〔叫−1）cos〔鋸 −5in（凸i＿1）−Sin（叫＿1抽〔2）Sill（叫−1）sin〔鋸 ＝如何−1）cos（町 cos（叫ト1） cos（叫＿1）diO

OOlとなる。リンク座標系が定まってしまえば，リンクパラメータの値からαなどの正弦や余弦を計算しておくことができる。r行列は関節変数βまたはdだけの関数となる。

2．コ マニビュレ一夕上の位置、速度の計算
同次変換行列を用い，各リンクの位置、速度、角速度、加速度、角加速度を求める関係式を導く。
ペース座標系∑。より見たリンク宜の座標系Eiの位躍引ま，同次変換行卯riにより表される。すなわち
り■l＝ロブ】∫J了七月rβ．＿，．i￣Jri（3）となる。
∑iに対してipのイ立置にあるt」ンク盲上の点が，∑。l・こ対してp土の位置にあるとするとJリンク宜に関して表示した任意の点凱が与えられたとき，基準座標から見たその点のイ立置は

ハ・ご；丁・，−ぎ・・（4）と表きれる。したがってその速度は

告＝（妄剖−p〔1）となる。速度のg乗は＝鈍・丸＝rT・比Ce匝■オ「）ごテ・・二山2一（6）と表される。式〔古）を代入すると
7（雷トrγ碓雛壇〔認可］〔7）＝Tr相室深鍋督 抽］（即を得る。

2．3 運動エネルギー
リンク盲上の一戸の位置にある微小質量dmの運軌エネルギーは

嘩＝喜rr相室

苦情思ヰm（9）抽＝喜rr相室葦 tpdmipr諾丁］〔10）である。したがってリンク五の運動エネルギーは

凡＝上mムー￠匝陸肯揖轟）許（11）軸］

式（JJ）右辺中央の積分は擬似慣性行列丑として知られており，次式（1コ）で与えられる。したがってマニピュレータの全逓勤エネルギーは

1

打＝妄動＝括升碓妄語檻軸〔13〕］となる。

ヨ．4 位置エネルギー
重力場gにおいてある基準高さからだけ上にある質量m虻）物体によって，r」ンクーよの座標系1に関してその質量小心がベクトルr｛で表されるリンクの位置エネルギーは昂＝− T瑚Tu打r土−3−（叫となる。ただし
g訂

動

g＝

（lこ一、．・

鮎0である。ゆえにマニピュレータの皇位撞エネルギーはTI

P＝−∑Tnlタブーロrか1（16）云＝Jで与えられる。

2．5 動特性方程式
式（Jβ），（J爵 よりラグランジアン 上＝∬＿Pはム＝塵妾m塘檻〕

抽拙（17）となる。

勤特性方程式は．運動および位置エネルギーを表した座標によって，つぎのように与えられる。

現＝一−．1トJ

・：ノさ−−Jl二．

譲可算瑞T〕鮎−ト萎左圭加（∂恥∂ヴm卑語丁〕

軸一萎瑚菅

巧l．1り
ここで囲ま対応する力またはトルクである。従って，式は総和をとる順序に無関係であるから，さらに粘性摩擦Ⅴと外乱dを含めた場合．n自由度アームの運動方程式の一般形はつぎのように書き換えられる。
F＋d＝〟（9沌＋J血豆）＋Ⅴ亘＋葺（ヴ）〔2n）ただし．」帆ん，gそれぞれの式は以ドのようにある。

仙＝p＝孟，ゴ〕恥e〔語鴫語r〕（21）∂叫如 上∂叫れ尤ん厨＝Tl ∑

p＝打山エ（り，た）触〔認略語T〕＝（22）∂恥g ∂9孟氾Tl

丘（両）−＝∑∑九小も如lコ：−；：・

ワニJ上＝J†L 抽＝¶∑mpゴア語plノP−i

ここで∫財〔鉦の形の項は関節よの有効慣性を蓋し．JW〔両は関節玉とJの間の相互慣性を表すpJl仰の項は関節jおよびたにおける速度によって関節五に生じるコリオリカを表す。最後にガ（抽の形の項は関節瓦における重力負荷を表す。
−4−（24）3．Mathematicaを用いた方程式の構築するプログラム
且抽克emα蕗亡αで方程式の構築するプログラム流れ図を次のようにする。まず最初リンクのパラメータを入力して．つぎにパラメータによって慣性行列、遠心力及びコリオリカと重量負荷を計算する．最後にこれを微分して劫特性方程式を求める。

重量負荷飢を計算する∫由．＝勒肌兄畑軒軸w肌且抽九emα土血の方程式を構築するブロゲラミンゲ以下のようになる。

（粕＊粕＊＊相＊ホ＊＊＊＊＊粕柑LinkPar乱nleLer仁IeEnefbrcreatdymaT¶ic占叩uation＊＊＊頃♯相木拍＊＊＊ヰ）L∃nkNuIIlbers＝3；リンク散乱iO＝（叩，12）；リンク長さ
diOこ（0⊃叩‡；直勤変数
aliO＝（01−Pi／ち0‡；軸ねじれ角度
tiO＝（叫L2，t3）；まわり角度変数
miO＝†ml⊃1T旦n13‡；t」ンク質量rCii＝（（0，町gl胡，11g2。0。0，0），（1g3】0】0，0））；リンク質量中心位置gT＝（（gl，（0‡，（0‡、‡0）‡；堅力場ベクトル（拙出目＝＝＊叫Fun亡tionde山1巳払rusingmaLll巳rrlaticafromhrmulatic l♯＊粕料半＊串＊♯★＊村本）√加txatalphai−］＝＝i‡㍉0，0，叫，拍Co串1ptlai】，−Sinlalpha畑0），抑S叫alph叫ぷos［alph細0‡⊃（0凡0，1）‡；Ttra・nSX可ai一丁＝＝（（1コOAaiI⊃（0ん0⊃0）−（0凧1月），（0凡0，1｝1；
1totzo［七hetaL］＝＝‡（Co5［thetai］．−Sin［thetai］，0，Ol．（ShttheLail，Cos［ttl巳tail，0，0‡，（0，口，l，0）一（0っ0っ0｝l‡‡；Ttrans如［di」】＝＝（‡1，0，0，0‡，牲1，叩‡，（0，OJ，di‡，（OA軋1‡）；−5一nt［alphaL，aL，thetai−，diL］＝＝Ttra・11SXa［ai］・nDtXa［alphai］・Ttra11SEd［di］・Tmtzo［theta≡】；Ti【Ni−】：＝TatFa］1iO［［Ni＋l］］，aiO脚i＋1】l，t；0［［Ni＋l］］．d嘲叩＋l］］3；
TijINi−，Nj−1＝＝Simplihr［〔TS＝IdentityMatrix［4］；Do［1 S＝「rS・Ti［i］，‡i，町Nj−1‡］；

Return［TS仙dtiTi［Nir，Nj−，Nk−1＝＝D【Tij［Ni，Nj］．tiO［［Nk］】］；
TTacePlus【ListA］：＝AppLyEPl11R，Ta・bleLLi6tA［【i，i］］，（i，41］］；
11kiri−］＝＝（‡rGii［Fi，1］］†，（rGii【［i，2］1），（rGiit【i，3】】）†（1†1．
¶anspose［（（rG；it肘］］），irGi鼎2】］‡，（rGii【［周】‡，（1川♯nl叫【i】］；

h・1抑町Nj−］＝＝Simpl…hT［Sum（rIhcePlu中tiTi【0．k，Nj］，Hki［k］．Transpose［dtiTi［0，k，Ni］J］，‡k，Nj，LinkNumbers）】］
dtkMij［Ni−，Njユ，tk1＝＝T］［MijLNi．Nj］，tiO［ttkl］］；
hjjk［Ni，NjT，Nkr］＝＝dtkMij［N王，Nj，NkHdtkMlj［Nj，Nk，Ni］）／2；
hitNi−】二＝Sum［Su工n［hijk［Nij，k］＋dtiO［臼］］＊dt叫【kl］，（k，1，Li11kNumber＄）］，‡j⊃l，LinkNumberB）］；
gi［NL］＝＝LSum［miO［Li］］＋Tmnspose【gT］▼dtiTi［0⊃j，Ni］lrGii［Li］］，（j，Ni．LinkNumbers）］；〔＊＋H榊＊拙＊＊＊＝ホホホ綿♯＊Fu工1CtiロnrunningDutPutequation＊＋＊＋＊榊＊＊＊＊＊＊＊‖抽出＊＊＊出＊＊牛）t＊〟〔抽j＊〕For［i＝1，i＜こLinkNumbers】i＋十Fbr臼二i，j＜＝LinkNumbers，j＋＋，Col）ect［Mij［㍉j］，（11，12，］3）］］］（叫町抽♯）

For【巨1】j＜＝LinkNumbcrs，i−I十hi ］（♯ガ（抽♯）For匝1っj＜＝T山一kNu・nl」巳rS，i＋＋感［i］］

4．実行例
この例には．3リンクマニピュレータに対して有効慣性項，相互慣性項．重力項を算出して∫リンク
パラメータは以下蓑を示すようになる
コセムヱeJ上古mたPαrdTrほ王er

リンクパラメータt」ンク

凸ト1

α慮＿1d豆軋Jロロロ

♂1ml

ヱガ1gロ

タロ〃鮎m2fクコ．冒

g2β〟

鮎TT13Jク1

銅まずt」ンクパラメータより，プログラムを運転するによって，以下のような結果になる。

叫1＝聖＋聖＋2m3亡憬（つβ封＋J22（苧＋m3C甘堕））＋

コm3⊂画3（♂っ＋軸））Jフ帖コm13亡OS梅）＋上耶サー13COS（2伊2＋屯〕〕〟】2＝几才21＝0
几すユ3＝几毎1＝0

城コ＝Jg23打tコ＋且タコ2m3十JココTT7・3・ト2亡p。Jコm3COS（鮎〕
几屯3＝几先コ＝g甘3コm3＋J如上2丁了13COS〔叫〃ココ＝Jpユ2TrLコ∂1毎（2【タコ2†T12Si】申咄−コ亡2コ叫8i・串厨2トコ上官。コ叩lユ即（叩コ＋軸）ト4亡頂か−13叫コPコト咄）コ
一別犯hmヨ3in（町−2亡9コm36i昭叩汗瑚トコーク3iコm3轟（2鞄＋輿））
コh〔付加＝＝2恥βj上知J2汀！・3Sir−（叫u力〔吼引1＝軋3J細。S岬3ト雲逗

コmコSln（2β2トコEココmユS叫2βヱト2エリ−6−㌔㌣J6i叫（8汗咄卜叫3J∃mコ叫叫＋βむユ

＝コご・、ト川主≡ニー：・：・て・1」二三土⊥ユ三ニ、：・・llさご、・一J‥一っ・ニュ土土坤折損＝勘タ。J2m3S叫叫

叫

gl＝O
g2＝Z朗叩lヨガC帖（鮎）＋J93TT13〟COS岬2＋亀〕
g3＝ヱタコTr拍葺COS岬2＋叫以上結果を式（即）ような整理して，マニピュレータの運動方程式は

lりんnJ−皿0−托．d3飢﹂・鞄・飢リ0

鴨0鴨00・．nリー．βU．爪HV．βL‖

り一．︹J．．い■

l1︐在．

叫職場

3り山3り︼

l3l l
叫職場となる。

つ5．あとがき

叫鴨場

このマニピュレータの運動方程式の構成方法は．各リンクの質量．質量中心位置，慣性テンソル，摩擦力などのリンクパラメータから，運動方程式を導くことができる。設計効率が高く，かつ止確である。

また，任意のt」ンクのパラメータでの設計が可能である。但し，6リンク以上計算するときに．パソコンのメモリが不足する。

参考文献
1）吉田和信，川辺尚志＝釦直変動支点を持つ多日出度振子に対する可変ゲインーフィードバック捌‡凱第35回
学術講演会予稿嵐105Aヰ125／Ⅷ叩聞嘲
2い」、背一弘竹尾光敵テレオペレーションおける仮想・実環境の統一的マニピュレーション，篇33国学縮減満会予稿集β08M−瑚訂／6購〔1994〕

3〕ロボット工学ハンドフッ久 日本ロボット学会コロナ札192／23〔申990）
4）吉川恒夫（緬）＝ロボット制御基礎型乱 コロナ社（19邑射呂）伊藤宏軋伊藤正美：生体とロボットにおける運軌剛臥 計測自動制御学貧上叩．135ノ140（1991）
G）浪速泰之：三次元ブリーフライング宇宙ロボットの実験的研乳FrorIlInterNeLbyComputer（19D8）7）StephenWolfra．m＝TheMatheInaticBookThir〔111〔1弓tioll，WolthmR巳SearCh．Ine．（19呵呂）Paul．Richard P．Robot manipulators＝ MaLheJItatics，Programming and CoLltrD），MIT Prtm，CaT［・bTi〔1ge†
Ma路（1986）−7−