2−2:片足(自由度5)の運動方程式を解く。同じく重心の算出方法の確立
二週間過ぎてますね…駄目じゃん、俺。
運動方程式の解法
1:自由度6の脚
まず、「足」の運動方程式を云々する以前の問題として、「足」をモデル化するところから始めないといけません。
それには、まず1脚(片足)の自由度を考えなければなりません。
自由度とはそもそも、何であるかと言うと、「自由に動かせる程度」とでも言えるでしょうか。
一般的に、物理的なモータの数=自由度と言う認識がありますが、概ねその通りです。もっとも、機構によっては複数のモータで1自由度を扱うこともあるので、一概には規定できません。
また、モータ以外のアクチュエータに関しても同じことで、1つ以上のアクチュエータで1自由度を構成することはあっても、単一のアクチュエータで二つ以上の自由度を実現した機構はありません。(少なくとも、私は聞いたことがありません。)
取り敢えず、今回の歩行ロボット製作に関しては、物理的なモータの数=自由度という認識で話を続けます。
さて、人間の脚というモノは、大まかに分けて、股関節、膝関節、足首関節の3つの関節から成っています。(つま先と足の指の関節は、ここでは除外してます。)
そして、このうち膝関節は1方向にのみ可動角を持つので、1自由度です。
足首に関しては脛に対して前後、左右のそれぞれの方向に可動角を持ち、2自由度となります。
残りの股関節ですが、これについては少々話がややこしくなります。
まず、人間の脚は、つま先を前方に向けたまま開脚が可能です。また、当然、つま先に対して同じ方向にも(つまり前後にも)自由に動かす事が可能です。
これで2自由度ですが、あと1つ、人間の脚「ねじる」という動作が可能です。
つま先を任意の方向に向けたとき、普通であれば膝も同じ方向に向いている筈です。
これは、膝より身体に近い部分、つまり股関節付近に「ねじる」という動作の自由度が存在するからです。
つまり、股関節は3自由度を持つ事になり、足首、膝、股関節で合計6自由度を持つ事になります。
実際の人間(に限らず生物は皆そうですが)の脚には、筋肉と言うアクチュエーターが複雑に配置されていますが、6自由度で単純化すると、図.1のようになります。
図.1
ここで、「L〜」というのは、関節の間の距離を表します。要は脛や太腿の長さという事です。
また「S〜」の円筒は、自由度の回転軸を表し、この円筒の中心軸に沿って図.1の1脚のモデルは回転運動を行う事になります。
S1〜S3が股関節、S4で膝関節、S5〜S6で足首間接に相当しています。
これで、脚のモデル化はできました。
2:順運動学の解
いつぞやの日記にも書いた思いますが、順運動学解とは、関節もっと正確に言うならば対象の各部の自由度の角度を与えることにより、対象の姿勢を求めることです。
ここから先は、3次元座標上のベクトルの話が殆どで、行列も出てきます。それらに関する基礎的な知識が無いと、かなり難しい内容になってますが、出て来た用語については私の解る範囲でなるべく説明する様にしてきますので、可能な限り、お付き合いしてみて下さい。
とは言っても無理な人も居るかと思います。そーいうひとは、睡眠薬代わりにして下さい。(^^;
まず、前項の脚をモデル化した図.1から、ベクトル図を作ります。(図.2)
図.2
図.2は基準姿勢におけるベクトル図です。「P〜」は各リンク部の位置ベクトル、「l〜」はリンク間のベクトル、「s〜」は各自由度の回転軸の方向を表す単位ベクトルです。
(単位ベクトルとは、ベクトルの長さが1(最小単位)で、方向のみを表したベクトルです。)
尚、「a」はつま先の方向を表す単位ベクトル、「b」はベクトル「a」の姿勢を表すベクトル…というか、この場合は足裏の方向を表す単位ベクトルだと考えて問題ありません。
さて、ここで基準位置におけるそれぞれのベクトルを見てみます。
{x座標}
ベクトル = {y座標} = (x座標、Y座標、z座標)T
{z座標}
とすると、
l1 = (0、0、−L1)T
l2 = (0、0、−L2)T
l3 = (0、0、−L3)T
l4 = (0、0、−L4)T
l5 = (0、0、−L5)T
l6 = (L6'、0、−L6’’)T
a = (1、0、0)T
b = (0、0、−1)T
となります。
ここからが順運動学解析です。
まず、脚の先端(つま先)を表すベクトル「P」を示すベクトル方程式を考えます。
これはごく簡単に、l1〜l6までのベクトルを合計すれば求まります。
但し、基準位置であれば上記のl1〜l6までを合計すればOKですが、実際の動作中には当然、各リンクの回転軸にはそれぞれの角度が与えられています。その回転分を考慮しなければ、正しい姿勢は求まりません。
そこで必要になってくるのが、回転変換行列E(ωθ)です。
あるベクトルA=(Ax、Ay、Az)Tが3次元座標上のz軸周りにθだけ回転したときのベクトルをA’とすると、
A’ = E(kθ)A
{ Cθ −Sθ 0}
E(kθ) = { Sθ Cθ 0}
{ 0 0 1}
但し、
Sθ = sinθ
Cθ = cosθ
という関係があります。
このE(kθ)をk軸(z軸)回りの回転変換行列と言います。
同様に、i軸(x軸)、j軸(y軸)回りの回転変換行列も、
{ 1 0 0}
E(iθ) = { 0 Cθ −Sθ}
{ 0 Sθ Cθ}
{ Cθ 0 Sθ}
E(jθ) = { 0 1 0}
{−Sθ 0 Cθ}
と求められます。
これを考慮してPに関するベクトル方程式を立てると、
P = E(kθ1)l1 + E(kθ1)E(iθ2)l2 + E(kθ1)E(iθ2)E(jθ3)l3 + E(kθ1)E(iθ2)E(jθ3)E(jθ4)l4
+ E(kθ1)E(iθ2)E(jθ3)E(jθ4)E(jθ5)l5 + E(kθ1)E(iθ2)E(jθ3)E(jθ4)E(jθ5)E(iθ6)l6 (1)
P = R(1)l1 + R(2)l2 + R(3)l3 + R(4)l4 + R(5)l5 + R(6)l6 (2)
となります。
要は、任意のリンク点P〜に影響を及ぼす角度を考慮している、と言う事です。
例えば、一番根元のP1では、考慮するのは自身の回転角θ1のみで良い事になります。(今回の場合は、回転軸とl1の方向が同じな為、実際にはP1の位置にはθ1は関係ありませんが)
次のP2では、基準位置では回転軸はx軸と同じ方向ですが、θ1が仮に90°回転していたとすれば、s2はy軸の方向を向いていることになります。
更に次のP3では、θ1、θ2の角度、P4ではθ1、θ2、θ3の角度…という様に、先端になるほど、そのリンクより根元に近いリンクの回転角の影響を受けていきます。
これはむしろ当然と言えば当然のことで、感覚的に分かり易いと思います。
では、実際にR(〜)を求めてみます。
又ここで、L2とL5を0とします。(S2とS3、S5とS6の回転軸をそれぞれ同じ位置にする為です。この方が、計算は楽になるからです)
更に、理由は後述しますが、L3=L4とします。
よって、(2)は、
P = R(1)l1 + R(3)l3 + R(4)l4 + R(6)l6 (2’)
と変形されます。
{ C1 −S1 0}{ 0 }
R(1)l1 = { S1 C1 0}{ 0 }
{ 0 0 1}{−L1}
{ 0 }
R(1)l1 = { 0 } (3)
{−L1}
{ C1 −S1 0}{ 1 0 0}{ 0 }
R(2)l2 = { S1 C1 0}{ 0 C2 −S2}{ 0 }
{ 0 0 1}{ 0 S2 C2}{−L2}
但し、L2 = 0のため、
R(2)l2 = 0 (4)
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ 0 }
R(3)l3 = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 0 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{−L3}
{−C1S3−S1S2C3}
R(3)l3 = {−S1S3+C1S2C3}L3 (5)
{−C2C3 }
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ C4 0 S4}{ 0 }
R(4)l4 = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 0 1 0}{ 0 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{−S4 0 C4}{−L4}
{−C1S(3+4)−S1S2C(3+4)}
R(4)l4 = {−S1S(3+4)+C1S2C(3+4)}L4 (6)
{−C2C(3+4) }
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ C4 0 S4}{ C5 0 S5}{ 0 }
R(5)l5 = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 0 1 0}{ 0 1 0}{ 0 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{−S4 0 C4}{−S5 0 C5}{−L5}
但し、L5 = 0のため、
R(5)l5 = 0 (7)
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ C4 0 S4}{ C5 0 S5}{ 1 0 0}{L6’}
R(6)l6 = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 0 1 0}{ 0 1 0}{ 0 C6 −S6}{ 0 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{−S4 0 C4}{−S5 0 C5}{ 0 S6 C6}{−L6’’}
{C1C(3+4+5)−S1S2S(3+4+5)} {−C1S(3+4+5)C6−S1C2S6−S1S2C(3+4+5)C6}
R(6)l6 = {S1C(3+4+5)+C1S2S(3+4+5)}L6’ + {−S1S(3+4+5)C6+C1C2S6+C1S2C(3+4+5)C6}L6’’ (8)
{−C2S(3+4+5) } {S2S6−C2C(3+4+5)C6 }
また、a、bを求めると、
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ C4 0 S4}{ C5 0 S5}{ 1 0 0}{ 1 }
a = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 0 1 0}{ 0 1 0}{ 0 C6 −S6}{ 0 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{−S4 0 C4}{−S5 0 C5}{ 0 S6 C6}{ 0 }
{C1C(3+4+5)−S1S2S(3+4+5)}
a = {S1C(3+4+5)+C1S2S(3+4+5)} (9)
{−C2S(3+4+5) }
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ C4 0 S4}{ C5 0 S5}{ 1 0 0}{ 0 }
b = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 0 1 0}{ 0 1 0}{ 0 C6 −S6}{ 0 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{−S4 0 C4}{−S5 0 C5}{ 0 S6 C6}{−1 }
{−C1S(3+4+5)C6−S1C2S6−S1S2C(3+4+5)C6}
b = {−S1S(3+4+5)C6+C1C2S6+C1S2C(3+4+5)C6} (10)
{S2S6−C2C(3+4+5)C6 }
解いてみると、a、bはそれぞれl6のL6’、L6’’の成分の単位ベクトルであることが分ると思います。
次に、回転軸ベクトルs1〜s6をそれぞれ求めます。
求め方は基本的に R(〜)l〜 と同じです。
要はその回転軸に影響を与えるリンクを考慮して式を立てろ、ということですね。
…というか、求め方どころか、式自体も殆ど同じなんですが。
各回転軸ベクトルの解
{ C1 −S1 0}{ 0 }
s1 = { S1 C1 0}{ 0 }
{ 0 0 1}{ 1 }
{ 0 }
s1 = { 0 } (11)
{ 1 }
{ C1 −S1 0}{ 1 0 0}{ 1 }
s2 = { S1 C1 0}{ 0 C2 −S2}{ 0 }
{ 0 0 1}{ 0 S2 C2}{ 0 }
{ C1}
s2 = { S1} (12)
{ 0 }
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ 0 }
s3 = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 1 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{ 0 }
{−S1C2}
s3 = { C1C2} (13)
{ S2 }
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ C4 0 S4}{ 0 }
s4 = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 0 1 0}{ 1 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{−S4 0 C4}{ 0 }
{−S1C2}
s4 = { C1C2} (14)
{ S2 }
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ C4 0 S4}{ C5 0 S5}{ 0 }
s5 = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 0 1 0}{ 0 1 0}{ 1 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{−S4 0 C4}{−S5 0 C5}{ 0 }
{−S1C2}
s5 = { C1C2} (15)
{ S2 }
{ C1 −S1 0}{ 1 0 0}{ C3 0 S3}{ C4 0 S4}{ C5 0 S5}{ 1 0 0}{ 1 }
s6 = { S1 C1 0}{ 0 C2 −S2}{ 0 1 0}{ 0 1 0}{ 0 1 0}{ 0 C6 −S6}{ 0 }
{ 0 0 1}{ 0 S2 C2}{−S3 0 C3}{−S4 0 C4}{−S5 0 C5}{ 0 S6 C6}{ 0 }
{C1C(3+4+5)−S1S2S(3+4+5)}
s6 = {S1C(3+4+5)+C1S2S(3+4+5)} (16)
{−C2S(3+4+5) }
回転軸ベクトルは、回転軸の方向を表すベクトルであり、位置や距離を示すものではありません。L〜の成分を含んでいないのはその為です。
以上のことについて、「分るかボケ!」とか言う方は、とにかく式(3)〜(16)で順運動学が解けたということだけ憶えておいて下さい。
3:逆運動学の解
逆運動学の問題の解決は、幾何学の解決に似て直感的なところがあり、一定の解法はない。またすべての機構についても逆運動学が解けるわけでもない。しかし、現状ではこの解の誘導は不可欠である。なぜなら通常使用されているマニピュレータの場合……
(参考図書:ロボット工学(改訂版) 広瀬茂男著、裳華房より抜粋)
要は感覚が大事、ということでしょうか。(ちょっと違う)
実際、行列式とにらめっこするよりも、自分の足を振り回しながら考える時間の方が長かったです。
「つま先の位置や姿勢を確定したら、本当に足首や膝、股関節の角度が決まるのだろうか?」
などと自問自答を繰り返しましたが、ようやっと(片足分の逆運動学解は)解けました。
本項の序文にあったように、逆運動学問題の解き方には一定の解法はありません。
ここで紹介するのは、私が無いアタマをこねくり回して導いた解法であり、もっと効率の良い解法が存在する可能性は十分にあります。
矛盾点、疑問、質問がありましたら、どしどしお寄せ下さい。
さて、前項で6自由度の脚の順運動学を解いた訳ですが、次はその逆、つま先の位置と姿勢を与えて、各リンクの回転角を求めます。
最初に既知であるパラメータを整理します。
1:a=(ax、ay、az)T
つま先の向いている方向を示します。任意の値を与えます。
2:b=(bx、by、bz)T
ベクトルa自体の姿勢…というかこの場合は足裏の方向を示すとした方が分かり易いでしょう。
これまた、任意値を与えますが、前提としてベクトルaに直交するベクトルである事が必須条件となります。(物理的にそういう構造で考えている為)
3:P=(Px、Py、Pz)T
つま先の位置を示します。今回の場合(1脚のみの場合)は原点(脚の根元)からの位置になります。
当たり前ですが、脚を一杯に伸ばした距離より遠い点では解は得られないので、「つま先の届く範囲の位置」というのが必須条件です。
4:L1〜L5、L6’、L6’’の長さ
脚の各部の寸法です。設計時に決定されます。当然、現実的な値です。
以上の既知値から、前項で導出した式も用いて、θ1〜θ6を求める事になります。
T:θ1導出
まず、θ1の導出から始める事にします。
ここで注目したいのは、回転軸ベクトルs3〜s5です。
前項で導出した通り、この3つのベクトルは全く同じ解になります。
これは、この3つのベクトルが、機構上の理由から常に平行になるためで、s5が判明すれば自動的にs3まで確定できることになります。
このs5は、
s5 = (s5x、s5y、s5z)T = (−S1C2、C1C2、S2)T
であり、
-1 S1 -1 −(−S1C2)
θ1 = tan (―――) = tan (――――――――)
C1 C1C2
-1 −s5x
θ1 = tan (――――) (17)
s5y
として、s5が分ればθ1が求められます。
ここで、図.1、図.2を見直して下さい。
前項でも述べましたが、L5=0とします。
すると、P5=P4となり、P5が求まればP4も求まる事になります。
以上を踏まえて、図.2を見ると、ベクトルs5はベクトルl4とベクトルaの両方に直交するベクトルであることが分かります。
要は足首の方向を示すベクトルとベクトルaとに直交する単位ベクトルがs5(=s4=s3)であるということです。
ベクトルaは既知なので、足首の方向を表すベクトルを求める訳ですが、足首点P4に対して、P1のベクトル分を差し引けば、足首の方向が分ります。(P1とP2はL2=0の為、同じ位置になります)
機構上P1は、l1と等価であり、l1=(0、0、−L1)Tとしてθ1に関係無くP1は既知となります。
そして、P5はPに対して、ベクトルaの逆方向にL6’分、ベクトルbの逆方向L6’’分戻った点です。
ベクトルa、bは既知であるので、PよりP5が求まる事になります。
以上の事をまとめると、
P5 = P4 = P − aL6’ − bL6’’ (18)
から、足首点P4’は、
P4’ = P4 − P1 = P − aL6’ − bL6’’ − P1 (19)
よって、
P4’x = Px − axL6’ − bxL6’’
P4’y = Py − ayL6’ − byL6’’
P4’z = Pz − azL6’ − bzL6’’ + L1
但し、このままではP4’は単位ベクトルではないので、ベクトルの長さ|P4’|で割ってやる必要があります。
2 2 2 0.5
|P4’| = (P4’x + P4’y + P4’z )
よって、P4’の単位ベクトルP4’’は、
P4’’ = P4’/ |P4’|
P4’’x = P4’x / |P4’|
P4’’y = P4’y / |P4’|
P4’’z = P4’z / |P4’|
となります。
このP4’’とaの外積を求めれば、直交ベクトルが得られます。
また、P4’’、a共に単位ベクトルであるので、外積も自動的に単位ベクトルとなります。
a、P4’’との直交ベクトルをe=(ex、ey、ez)Tとすると、
ex = ay(P4’’z) − az(P4’’y)
ey = az(P4’’x) − ax(P4’’z)
ez = ax(P4’’y) − ay(P4’’x)
但し、上記の求め方は、θ6=90°の時には成立しないので、足首の可動範囲は、±90°未満にする必要があります。
これで、
s5 = s4 = s3 = e
となり、式(17)より、
-1 −ex
θ1 = tan (――――)
ey
と導かれます。
U:θ2導出
θ1が求まれば、比較的簡単に求めることが出来ます。
まず、
s3 = (−S1C2、C1C2、S2)T = (ex、ey、ez)T
である事を利用して、
-1 S2 -1 S2
θ2 = tan (―――) = tan (――――――――)
C2 C1C2/C1
-1 ez
θ2 = tan (――――) (20)
ey/C1
と求める事ができます。
V:θ4導出
θ3が先じゃないの?
と思われるかもしれませんが、θ4の方がθ3よりも導出しやすいので、こちらから先に行います。
まず、図.3のように、足首の方向と位置を示すベクトルP4’の長さ|P4’|を底辺とし、他の二辺をL3、L4(=L3)とした二等辺三角形を考えます。(前述したL3=L4の条件はこの為に設定したものです)
図.3
この場合、s3〜s5は平行なため、ベクトルは常に同じ方向となるため、|P4’|の値さえ分ればs3〜s5の方向は考える必要はありません。
さて、図.3において、θAの角度を求めるには、
A・sin(θa) = (1/2) × B
より、
-1
θa = sin (B/2A)
ここで、θaは図.3における直角三角形ABCの角の一つであり、
θA = 2 × θa (21)
の関係があります。
最終的に、θ4は図.3にある通り、
θ4 = θA − 180°
から、
-1
θ4 = 2(sin (|P4’|/2L3) − 180°) (22)
として求める事が出来ます。
W:θ3導出
θ4が求まれば比較的簡単に…という程楽でも無いです。残念ながら。
まず、P4について考えてみます。
順運動学解から、P4は、
{ 0 } {−C1S3−S1S2C3} {−C1S(3+4)−S1S2C(3+4)}
P4 = { 0 } + {−S1S3+C1S2C3}L3 + {−S1S(3+4)+C1S2C(3+4)}L4
{−L1} {−C2C3 } {−C2C(3+4) }
であり、前述したように、P4=P5であり、
{−C1S3−S1S2C3} {−C1S(3+4)−S1S2C(3+4)}
P4’ = {−S1S3+C1S2C3}L3 + {−S1S(3+4)+C1S2C(3+4)}L4
{−C2C3 } {−C2C(3+4) }
となります。このP4’は先程のP4’のことであり、この時点で既知の値です。
ここから、θ1、θ2の影響を除外します。
具体的には、θ1、θ2の回転分を除去し、基準位置に戻すことになります。
式としては、以下の様になります。
P4’’’ = E(−iθ2)E(−kθ1)P4’
ここでE(−〜)というのは、いわゆる逆行列のことで、例えば E(−kθ1)E(kθ1)=1 という性質を持ちます。
P4’は、
P4’ = E(kθ1)E(iθ2)E(jθ3)l3 + E(kθ1)E(iθ2)E(jθ3)E(jθ4)l4
となるので、P4’’’は、
P4’’’ = E(−iθ2)E(−kθ1)E(kθ1)E(iθ2)E(jθ3)l3 + E(−iθ2)E(−kθ1)E(kθ1)E(iθ2)E(jθ3)E(jθ4)l4
よって、
P4’’’ = E(jθ3)l3 + E(jθ3)E(jθ4)l4
{−S3} {−C3S4−S3C4}
P4’’’ = { 0 }l3 + { 0 }l4
{−C3} { S3S4−C3C4}
となります。
実際には、
{ 1 0 0 }
E(−iθ) = { 0 Cθ Sθ}
{ 0 −Sθ Cθ}
{ Cθ Sθ 0}
E(−kθ) = {−Sθ Cθ 0}
{ 0 0 1}
より、
{ C1 S1 0 }
E(−iθ2)E(−kθ) = {−S1C1 C1C2 S2}
{ S1S2 −C1S2 C2}
から、
{ C1 S1 0}{P4x’}
P4’’’ = {−S1C1 C1C2 S2}{P4y’} (23)
{ S1S2 −C1S2 C2}{P4z’}
この変換により、P4’’’はy成分を無くし、x−z平面上の2次元座標系で考えることができるようになります。
ここで、先の図.3(左図)を見なおすと、y軸側から見たx−z平面の図と等価であることが分ります。
そうすると、Vでθ4を求める際に利用した二等辺三角形AABの鋭角(∠AB)と、P4’’’の角度を合計すれば、θ3を導けることになります。
式としては、
-1
θ3 = ∠AB + tan (−P4z’’’/P4x’’’)
-1
θ3 = (90° − θa) + tan (−P4z’’’/P4x’’’) (24)
X:θ5導出
…いい加減くどくなってきましたか?
私はもーめげそうです。
とり合えず、θ5の導出には(16)式を使います。
{C1C(3+4+5)−S1S2S(3+4+5)}
s6 = {S1C(3+4+5)+C1S2S(3+4+5)} (16)
{−C2S(3+4+5) }
回転軸s6のベクトルは、つま先の方向を示すベクトルaと同じ方向を示します。
どちらも単位ベクトルであり、
a = s6
ということになります。
さて、式(16)より、s6zに注目します。
s6z = az = −C2S(3+4+5)
−az/C2 = S(3+4+5)
より、
-1
θ3 + θ4 + θ5 = sin (−az/C2)
-1
θ5 = sin (−az/C2) − θ3 −θ4 (25)
となり、θ5を導出できます。
Y:θ6導出
やっと本章最後です。
頑張りましょう。(むしろ自分に言い聞かせ)
θ6を導出するのに使用するのは式(10)です。
{−C1S(3+4+5)C6−S1C2S6−S1S2C(3+4+5)C6}
b = {−S1S(3+4+5)C6+C1C2S6+C1S2C(3+4+5)C6} (10)
{S2S6−C2C(3+4+5)C6 }
そのまんまbを表しているので、既知であるbと、これまで求めて来たθ1〜θ5を用いれば求めることができます。
まず、
C1bx + S1by = −S(3+4+5)C6
とできるので、
−(C1bx + S1by)/S(3+4+5) = C6
より、
-1
θ6 = cos (−(C1bx + S1by)/S(3+4+5))
これで、θ1〜θ6が全て求まりました。
最終的には、重心に加わるモーメントや姿勢を考慮した上で、足に目標値を与える訳で、これで終了という事は無く、場合によっては全身についての逆運動学を解く必要が生じるかもしれませんが、順運動学、逆運動学の解の考え方自体は今回で身に付いたと考えます。
重心についても、順運動学を応用することで算出可能と思われます。
…今回は導出してませんが。(^^;
戻る