２−２：片足（自由度５）の運動方程式を解く。同じく重心の算出方法の確立
二週間過ぎてますね…駄目じゃん、俺。
運動方程式の解法
１：自由度６の脚
まず、「足」の運動方程式を云々する以前の問題として、「足」をモデル化するところから始めないといけません。
それには、まず1脚（片足）の自由度を考えなければなりません。
自由度とはそもそも、何であるかと言うと、「自由に動かせる程度」とでも言えるでしょうか。
一般的に、物理的なモータの数＝自由度と言う認識がありますが、概ねその通りです。もっとも、機構によっては複数のモータで１自由度を扱うこともあるので、一概には規定できません。
また、モータ以外のアクチュエータに関しても同じことで、１つ以上のアクチュエータで１自由度を構成することはあっても、単一のアクチュエータで二つ以上の自由度を実現した機構はありません。（少なくとも、私は聞いたことがありません。）
取り敢えず、今回の歩行ロボット製作に関しては、物理的なモータの数＝自由度という認識で話を続けます。
さて、人間の脚というモノは、大まかに分けて、股関節、膝関節、足首関節の３つの関節から成っています。（つま先と足の指の関節は、ここでは除外してます。）
そして、このうち膝関節は１方向にのみ可動角を持つので、１自由度です。
足首に関しては脛に対して前後、左右のそれぞれの方向に可動角を持ち、２自由度となります。
残りの股関節ですが、これについては少々話がややこしくなります。
まず、人間の脚は、つま先を前方に向けたまま開脚が可能です。また、当然、つま先に対して同じ方向にも（つまり前後にも）自由に動かす事が可能です。
これで２自由度ですが、あと１つ、人間の脚「ねじる」という動作が可能です。
つま先を任意の方向に向けたとき、普通であれば膝も同じ方向に向いている筈です。
これは、膝より身体に近い部分、つまり股関節付近に「ねじる」という動作の自由度が存在するからです。
つまり、股関節は３自由度を持つ事になり、足首、膝、股関節で合計６自由度を持つ事になります。
実際の人間（に限らず生物は皆そうですが）の脚には、筋肉と言うアクチュエーターが複雑に配置されていますが、６自由度で単純化すると、図.１のようになります。
図．１
ここで、「Ｌ〜」というのは、関節の間の距離を表します。要は脛や太腿の長さという事です。
また「Ｓ〜」の円筒は、自由度の回転軸を表し、この円筒の中心軸に沿って図.１の１脚のモデルは回転運動を行う事になります。
Ｓ１〜Ｓ３が股関節、Ｓ４で膝関節、Ｓ５〜Ｓ６で足首間接に相当しています。
これで、脚のモデル化はできました。
２：順運動学の解
いつぞやの日記にも書いた思いますが、順運動学解とは、関節もっと正確に言うならば対象の各部の自由度の角度を与えることにより、対象の姿勢を求めることです。
ここから先は、３次元座標上のベクトルの話が殆どで、行列も出てきます。それらに関する基礎的な知識が無いと、かなり難しい内容になってますが、出て来た用語については私の解る範囲でなるべく説明する様にしてきますので、可能な限り、お付き合いしてみて下さい。
とは言っても無理な人も居るかと思います。そーいうひとは、睡眠薬代わりにして下さい。（＾＾；
まず、前項の脚をモデル化した図.１から、ベクトル図を作ります。（図.２）
図．２
図.２は基準姿勢におけるベクトル図です。「Ｐ〜」は各リンク部の位置ベクトル、「l〜」はリンク間のベクトル、「ｓ〜」は各自由度の回転軸の方向を表す単位ベクトルです。
（単位ベクトルとは、ベクトルの長さが１（最小単位）で、方向のみを表したベクトルです。）
尚、「a」はつま先の方向を表す単位ベクトル、「b」はベクトル「a」の姿勢を表すベクトル…というか、この場合は足裏の方向を表す単位ベクトルだと考えて問題ありません。
さて、ここで基準位置におけるそれぞれのベクトルを見てみます。
｛ｘ座標｝
ベクトル ＝ ｛ｙ座標｝ ＝ （ｘ座標、Ｙ座標、ｚ座標）Ｔ
｛ｚ座標｝
とすると、
ｌ１ ＝ （０、０、−Ｌ１）Ｔ
ｌ２ ＝ （０、０、−Ｌ２）Ｔ
ｌ３ ＝ （０、０、−Ｌ３）Ｔ
ｌ４ ＝ （０、０、−Ｌ４）Ｔ
ｌ５ ＝ （０、０、−Ｌ５）Ｔ
ｌ６ ＝ （Ｌ６'、０、−Ｌ６’’）Ｔ
ａ ＝ （１、０、０）Ｔ
ｂ ＝ （０、０、−１）Ｔ
となります。
ここからが順運動学解析です。
まず、脚の先端（つま先）を表すベクトル「Ｐ」を示すベクトル方程式を考えます。
これはごく簡単に、ｌ１〜ｌ６までのベクトルを合計すれば求まります。
但し、基準位置であれば上記のｌ１〜ｌ６までを合計すればＯＫですが、実際の動作中には当然、各リンクの回転軸にはそれぞれの角度が与えられています。その回転分を考慮しなければ、正しい姿勢は求まりません。
そこで必要になってくるのが、回転変換行列Ｅ（ωθ）です。
あるベクトルＡ＝（Ａｘ、Ａｙ、Ａｚ）Ｔが３次元座標上のｚ軸周りにθだけ回転したときのベクトルをＡ’とすると、
Ａ’ ＝ Ｅ（ｋθ）Ａ
｛ Ｃθ −Ｓθ ０｝
Ｅ（ｋθ） ＝ ｛ Ｓθ Ｃθ ０｝
｛ ０ ０ １｝
但し、
Ｓθ ＝ ｓｉｎθ
Ｃθ ＝ ｃｏｓθ
という関係があります。
このＥ（ｋθ）をｋ軸（ｚ軸）回りの回転変換行列と言います。
同様に、ｉ軸（ｘ軸）、ｊ軸（ｙ軸）回りの回転変換行列も、
｛ １ ０ ０｝
Ｅ（ｉθ） ＝ ｛ ０ Ｃθ −Ｓθ｝
｛ ０ Ｓθ Ｃθ｝
｛ Ｃθ ０ Ｓθ｝
Ｅ（ｊθ） ＝ ｛ ０ １ ０｝
｛−Ｓθ ０ Ｃθ｝
と求められます。
これを考慮してＰに関するベクトル方程式を立てると、
Ｐ ＝ Ｅ（ｋθ１）ｌ１ ＋ Ｅ（ｋθ１）Ｅ（ｉθ２）ｌ２ ＋ Ｅ（ｋθ１）Ｅ（ｉθ２）Ｅ（ｊθ３）ｌ３ ＋ Ｅ（ｋθ１）Ｅ（ｉθ２）Ｅ（ｊθ３）Ｅ（ｊθ４）ｌ４
＋ Ｅ（ｋθ１）Ｅ（ｉθ２）Ｅ（ｊθ３）Ｅ（ｊθ４）Ｅ（ｊθ５）ｌ５ ＋ Ｅ（ｋθ１）Ｅ（ｉθ２）Ｅ（ｊθ３）Ｅ（ｊθ４）Ｅ（ｊθ５）Ｅ（ｉθ６）ｌ６ （１）
Ｐ ＝ Ｒ（１）ｌ１ ＋ Ｒ（２）ｌ２ ＋ Ｒ（３）ｌ３ ＋ Ｒ（４）ｌ４ ＋ Ｒ（５）ｌ５ ＋ Ｒ（６）ｌ６ （２）
となります。
要は、任意のリンク点Ｐ〜に影響を及ぼす角度を考慮している、と言う事です。
例えば、一番根元のＰ１では、考慮するのは自身の回転角θ１のみで良い事になります。（今回の場合は、回転軸とｌ１の方向が同じな為、実際にはＰ１の位置にはθ１は関係ありませんが）
次のＰ２では、基準位置では回転軸はｘ軸と同じ方向ですが、θ１が仮に９０°回転していたとすれば、ｓ２はｙ軸の方向を向いていることになります。
更に次のＰ３では、θ１、θ２の角度、Ｐ４ではθ１、θ２、θ３の角度…という様に、先端になるほど、そのリンクより根元に近いリンクの回転角の影響を受けていきます。
これはむしろ当然と言えば当然のことで、感覚的に分かり易いと思います。
では、実際にＲ（〜）を求めてみます。
又ここで、Ｌ２とＬ５を０とします。（Ｓ２とＳ３、Ｓ５とＳ６の回転軸をそれぞれ同じ位置にする為です。この方が、計算は楽になるからです）
更に、理由は後述しますが、Ｌ３＝Ｌ４とします。
よって、（２）は、
Ｐ ＝ Ｒ（１）ｌ１ ＋ Ｒ（３）ｌ３ ＋ Ｒ（４）ｌ４ ＋ Ｒ（６）ｌ６ （２’）
と変形されます。
｛ Ｃ１ −Ｓ１ ０｝｛ ０ ｝
Ｒ（１）ｌ１ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ ｝
｛ ０ ０ １｝｛−Ｌ１｝
｛ ０ ｝
Ｒ（１）ｌ１ ＝ ｛ ０ ｝ （３）
｛−Ｌ１｝
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ ０ ｝
Ｒ（２）ｌ２ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｌ２｝
但し、Ｌ２ ＝ ０のため、
Ｒ（２）ｌ２ ＝ ０ （４）
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ ０ ｝
Ｒ（３）ｌ３ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ ０ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛−Ｌ３｝
｛−Ｃ１Ｓ３−Ｓ１Ｓ２Ｃ３｝
Ｒ（３）ｌ３ ＝ ｛−Ｓ１Ｓ３＋Ｃ１Ｓ２Ｃ３｝Ｌ３ （５）
｛−Ｃ２Ｃ３ ｝
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ Ｃ４ ０ Ｓ４｝｛ ０ ｝
Ｒ（４）ｌ４ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛−Ｓ４ ０ Ｃ４｝｛−Ｌ４｝
｛−Ｃ１Ｓ（３＋４）−Ｓ１Ｓ２Ｃ（３＋４）｝
Ｒ（４）ｌ４ ＝ ｛−Ｓ１Ｓ（３＋４）＋Ｃ１Ｓ２Ｃ（３＋４）｝Ｌ４ （６）
｛−Ｃ２Ｃ（３＋４） ｝
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ Ｃ４ ０ Ｓ４｝｛ Ｃ５ ０ Ｓ５｝｛ ０ ｝
Ｒ（５）ｌ５ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛−Ｓ４ ０ Ｃ４｝｛−Ｓ５ ０ Ｃ５｝｛−Ｌ５｝
但し、Ｌ５ ＝ ０のため、
Ｒ（５）ｌ５ ＝ ０ （７）
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ Ｃ４ ０ Ｓ４｝｛ Ｃ５ ０ Ｓ５｝｛ １ ０ ０｝｛Ｌ６’｝
Ｒ（６）ｌ６ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ Ｃ６ −Ｓ６｝｛ ０ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛−Ｓ４ ０ Ｃ４｝｛−Ｓ５ ０ Ｃ５｝｛ ０ Ｓ６ Ｃ６｝｛−Ｌ６’’｝
｛Ｃ１Ｃ（３＋４＋５）−Ｓ１Ｓ２Ｓ（３＋４＋５）｝ ｛−Ｃ１Ｓ（３＋４＋５）Ｃ６−Ｓ１Ｃ２Ｓ６−Ｓ１Ｓ２Ｃ（３＋４＋５）Ｃ６｝
Ｒ（６）ｌ６ ＝ ｛Ｓ１Ｃ（３＋４＋５）＋Ｃ１Ｓ２Ｓ（３＋４＋５）｝Ｌ６’ ＋ ｛−Ｓ１Ｓ（３＋４＋５）Ｃ６＋Ｃ１Ｃ２Ｓ６＋Ｃ１Ｓ２Ｃ（３＋４＋５）Ｃ６｝Ｌ６’’ （８）
｛−Ｃ２Ｓ（３＋４＋５） ｝ ｛Ｓ２Ｓ６−Ｃ２Ｃ（３＋４＋５）Ｃ６ ｝
また、ａ、ｂを求めると、
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ Ｃ４ ０ Ｓ４｝｛ Ｃ５ ０ Ｓ５｝｛ １ ０ ０｝｛ １ ｝
ａ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ Ｃ６ −Ｓ６｝｛ ０ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛−Ｓ４ ０ Ｃ４｝｛−Ｓ５ ０ Ｃ５｝｛ ０ Ｓ６ Ｃ６｝｛ ０ ｝
｛Ｃ１Ｃ（３＋４＋５）−Ｓ１Ｓ２Ｓ（３＋４＋５）｝
ａ ＝ ｛Ｓ１Ｃ（３＋４＋５）＋Ｃ１Ｓ２Ｓ（３＋４＋５）｝ （９）
｛−Ｃ２Ｓ（３＋４＋５） ｝
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ Ｃ４ ０ Ｓ４｝｛ Ｃ５ ０ Ｓ５｝｛ １ ０ ０｝｛ ０ ｝
ｂ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ Ｃ６ −Ｓ６｝｛ ０ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛−Ｓ４ ０ Ｃ４｝｛−Ｓ５ ０ Ｃ５｝｛ ０ Ｓ６ Ｃ６｝｛−1 ｝
｛−Ｃ１Ｓ（３＋４＋５）Ｃ６−Ｓ１Ｃ２Ｓ６−Ｓ１Ｓ２Ｃ（３＋４＋５）Ｃ６｝
ｂ ＝ ｛−Ｓ１Ｓ（３＋４＋５）Ｃ６＋Ｃ１Ｃ２Ｓ６＋Ｃ１Ｓ２Ｃ（３＋４＋５）Ｃ６｝ （１０）
｛Ｓ２Ｓ６−Ｃ２Ｃ（３＋４＋５）Ｃ６ ｝
解いてみると、ａ、ｂはそれぞれｌ６のＬ６’、Ｌ６’’の成分の単位ベクトルであることが分ると思います。
次に、回転軸ベクトルｓ１〜ｓ６をそれぞれ求めます。
求め方は基本的に Ｒ（〜）ｌ〜 と同じです。
要はその回転軸に影響を与えるリンクを考慮して式を立てろ、ということですね。
…というか、求め方どころか、式自体も殆ど同じなんですが。
各回転軸ベクトルの解
｛ Ｃ１ −Ｓ１ ０｝｛ ０ ｝
ｓ１ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ ｝
｛ ０ ０ １｝｛ １ ｝
｛ ０ ｝
ｓ１ ＝ ｛ ０ ｝ （１１）
｛ １ ｝
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ １ ｝
ｓ２ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛ ０ ｝
｛ Ｃ１｝
ｓ２ ＝ ｛ Ｓ１｝ （１２）
｛ ０ ｝
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ ０ ｝
ｓ３ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ １ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛ ０ ｝
｛−Ｓ１Ｃ２｝
ｓ３ ＝ ｛ Ｃ１Ｃ２｝ （１３）
｛ Ｓ２ ｝
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ Ｃ４ ０ Ｓ４｝｛ ０ ｝
ｓ４ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ ０ １ ０｝｛ １ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛−Ｓ４ ０ Ｃ４｝｛ ０ ｝
｛−Ｓ１Ｃ２｝
ｓ４ ＝ ｛ Ｃ１Ｃ２｝ （１４）
｛ Ｓ２ ｝
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ Ｃ４ ０ Ｓ４｝｛ Ｃ５ ０ Ｓ５｝｛ ０ ｝
ｓ５ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ １ ０｝｛ １ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛−Ｓ４ ０ Ｃ４｝｛−Ｓ５ ０ Ｃ５｝｛ ０ ｝
｛−Ｓ１Ｃ２｝
ｓ５ ＝ ｛ Ｃ１Ｃ２｝ （１５）
｛ Ｓ２ ｝
｛ Ｃ１ −Ｓ１ ０｝｛ １ ０ ０｝｛ Ｃ３ ０ Ｓ３｝｛ Ｃ４ ０ Ｓ４｝｛ Ｃ５ ０ Ｓ５｝｛ １ ０ ０｝｛ １ ｝
ｓ６ ＝ ｛ Ｓ１ Ｃ１ ０｝｛ ０ Ｃ２ −Ｓ２｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ １ ０｝｛ ０ Ｃ６ −Ｓ６｝｛ ０ ｝
｛ ０ ０ １｝｛ ０ Ｓ２ Ｃ２｝｛−Ｓ３ ０ Ｃ３｝｛−Ｓ４ ０ Ｃ４｝｛−Ｓ５ ０ Ｃ５｝｛ ０ Ｓ６ Ｃ６｝｛ ０ ｝
｛Ｃ１Ｃ（３＋４＋５）−Ｓ１Ｓ２Ｓ（３＋４＋５）｝
ｓ６ ＝ ｛Ｓ１Ｃ（３＋４＋５）＋Ｃ１Ｓ２Ｓ（３＋４＋５）｝ （１６）
｛−Ｃ２Ｓ（３＋４＋５） ｝
回転軸ベクトルは、回転軸の方向を表すベクトルであり、位置や距離を示すものではありません。Ｌ〜の成分を含んでいないのはその為です。
以上のことについて、「分るかボケ！」とか言う方は、とにかく式（３）〜（１６）で順運動学が解けたということだけ憶えておいて下さい。
３：逆運動学の解
逆運動学の問題の解決は、幾何学の解決に似て直感的なところがあり、一定の解法はない。またすべての機構についても逆運動学が解けるわけでもない。しかし、現状ではこの解の誘導は不可欠である。なぜなら通常使用されているマニピュレータの場合……
（参考図書：ロボット工学（改訂版） 広瀬茂男著、裳華房より抜粋）
要は感覚が大事、ということでしょうか。（ちょっと違う）
実際、行列式とにらめっこするよりも、自分の足を振り回しながら考える時間の方が長かったです。
「つま先の位置や姿勢を確定したら、本当に足首や膝、股関節の角度が決まるのだろうか？」
などと自問自答を繰り返しましたが、ようやっと（片足分の逆運動学解は）解けました。
本項の序文にあったように、逆運動学問題の解き方には一定の解法はありません。
ここで紹介するのは、私が無いアタマをこねくり回して導いた解法であり、もっと効率の良い解法が存在する可能性は十分にあります。
矛盾点、疑問、質問がありましたら、どしどしお寄せ下さい。
さて、前項で６自由度の脚の順運動学を解いた訳ですが、次はその逆、つま先の位置と姿勢を与えて、各リンクの回転角を求めます。
最初に既知であるパラメータを整理します。
１：ａ＝（ａｘ、ａｙ、ａｚ）Ｔ
つま先の向いている方向を示します。任意の値を与えます。
２：ｂ＝（ｂｘ、ｂｙ、ｂｚ）Ｔ
ベクトルａ自体の姿勢…というかこの場合は足裏の方向を示すとした方が分かり易いでしょう。
これまた、任意値を与えますが、前提としてベクトルａに直交するベクトルである事が必須条件となります。（物理的にそういう構造で考えている為）
３：Ｐ＝（Ｐｘ、Ｐｙ、Ｐｚ）Ｔ
つま先の位置を示します。今回の場合（１脚のみの場合）は原点（脚の根元）からの位置になります。
当たり前ですが、脚を一杯に伸ばした距離より遠い点では解は得られないので、「つま先の届く範囲の位置」というのが必須条件です。
４：Ｌ１〜Ｌ５、Ｌ６’、Ｌ６’’の長さ
脚の各部の寸法です。設計時に決定されます。当然、現実的な値です。
以上の既知値から、前項で導出した式も用いて、θ１〜θ６を求める事になります。
T：θ１導出
まず、θ１の導出から始める事にします。
ここで注目したいのは、回転軸ベクトルｓ３〜ｓ５です。
前項で導出した通り、この３つのベクトルは全く同じ解になります。
これは、この３つのベクトルが、機構上の理由から常に平行になるためで、ｓ５が判明すれば自動的にｓ３まで確定できることになります。
このｓ５は、
ｓ５ ＝ （ｓ５ｘ、ｓ５ｙ、ｓ５ｚ）Ｔ ＝ （−Ｓ１Ｃ２、Ｃ１Ｃ２、Ｓ２）Ｔ
であり、
-１ Ｓ１ -１ −（−Ｓ１Ｃ２）
θ１ ＝ ｔａｎ （―――） ＝ ｔａｎ （――――――――）
Ｃ１ Ｃ１Ｃ２
-１ −ｓ５ｘ
θ１ ＝ ｔａｎ （――――） （１７）
ｓ５ｙ
として、ｓ５が分ればθ１が求められます。
ここで、図.１、図.２を見直して下さい。
前項でも述べましたが、Ｌ５＝０とします。
すると、Ｐ５＝Ｐ４となり、Ｐ５が求まればＰ４も求まる事になります。
以上を踏まえて、図.２を見ると、ベクトルｓ５はベクトルｌ４とベクトルａの両方に直交するベクトルであることが分かります。
要は足首の方向を示すベクトルとベクトルａとに直交する単位ベクトルがｓ５（＝ｓ４＝ｓ３）であるということです。
ベクトルａは既知なので、足首の方向を表すベクトルを求める訳ですが、足首点Ｐ４に対して、Ｐ１のベクトル分を差し引けば、足首の方向が分ります。（Ｐ１とＰ２はＬ２＝０の為、同じ位置になります）
機構上Ｐ１は、ｌ１と等価であり、ｌ１＝（０、０、−Ｌ１）Ｔとしてθ１に関係無くＰ１は既知となります。
そして、Ｐ５はＰに対して、ベクトルａの逆方向にＬ６’分、ベクトルｂの逆方向Ｌ６’’分戻った点です。
ベクトルａ、ｂは既知であるので、ＰよりＰ５が求まる事になります。
以上の事をまとめると、
Ｐ５ ＝ Ｐ４ ＝ Ｐ − ａＬ６’ − ｂＬ６’’ （１８）
から、足首点Ｐ４’は、
Ｐ４’ ＝ Ｐ４ − Ｐ１ ＝ Ｐ − ａＬ６’ − ｂＬ６’’ − Ｐ１ （１９）
よって、
Ｐ４’ｘ ＝ Ｐｘ − ａｘＬ６’ − ｂｘＬ６’’
Ｐ４’ｙ ＝ Ｐｙ − ａｙＬ６’ − ｂｙＬ６’’
Ｐ４’ｚ ＝ Ｐｚ − ａｚＬ６’ − ｂｚＬ６’’ ＋ Ｌ１
但し、このままではＰ４’は単位ベクトルではないので、ベクトルの長さ｜Ｐ４’｜で割ってやる必要があります。
2 2 2 0.5
｜Ｐ４’｜ ＝ （Ｐ４’ｘ ＋ Ｐ４’ｙ ＋ Ｐ４’ｚ ）
よって、Ｐ４’の単位ベクトルＰ４’’は、
Ｐ４’’ ＝ Ｐ４’／ ｜Ｐ４’｜
Ｐ４’’ｘ ＝ Ｐ４’ｘ ／ ｜Ｐ４’｜
Ｐ４’’ｙ ＝ Ｐ４’ｙ ／ ｜Ｐ４’｜
Ｐ４’’ｚ ＝ Ｐ４’ｚ ／ ｜Ｐ４’｜
となります。
このＰ４’’とａの外積を求めれば、直交ベクトルが得られます。
また、Ｐ４’’、ａ共に単位ベクトルであるので、外積も自動的に単位ベクトルとなります。
ａ、Ｐ４’’との直交ベクトルをｅ＝（ｅｘ、ｅｙ、ｅｚ）Ｔとすると、
ｅｘ ＝ ａｙ（Ｐ４’’ｚ） − ａｚ（Ｐ４’’ｙ）
ｅｙ ＝ ａｚ（Ｐ４’’ｘ） − ａｘ（Ｐ４’’ｚ）
ｅｚ ＝ ａｘ（Ｐ４’’ｙ） − ａｙ（Ｐ４’’ｘ）
但し、上記の求め方は、θ６＝９０°の時には成立しないので、足首の可動範囲は、±９０°未満にする必要があります。
これで、
ｓ５ ＝ ｓ４ ＝ ｓ３ ＝ ｅ
となり、式（１７）より、
-１ −ｅｘ
θ１ ＝ ｔａｎ （――――）
ｅｙ
と導かれます。
U：θ２導出
θ１が求まれば、比較的簡単に求めることが出来ます。
まず、
ｓ３ ＝ （−Ｓ１Ｃ２、Ｃ１Ｃ２、Ｓ２）Ｔ ＝ （ｅｘ、ｅｙ、ｅｚ）Ｔ
である事を利用して、
-１ Ｓ２ -１ Ｓ２
θ２ ＝ ｔａｎ （―――） ＝ ｔａｎ （――――――――）
Ｃ２ Ｃ１Ｃ２／Ｃ１
-１ ｅｚ
θ２ ＝ ｔａｎ （――――） （２０）
ｅｙ／Ｃ１
と求める事ができます。
V：θ４導出
θ３が先じゃないの？
と思われるかもしれませんが、θ４の方がθ３よりも導出しやすいので、こちらから先に行います。
まず、図.３のように、足首の方向と位置を示すベクトルＰ４’の長さ｜Ｐ４’｜を底辺とし、他の二辺をＬ３、Ｌ４（＝Ｌ３）とした二等辺三角形を考えます。（前述したＬ３＝Ｌ４の条件はこの為に設定したものです）
図．３
この場合、ｓ３〜ｓ５は平行なため、ベクトルは常に同じ方向となるため、｜Ｐ４’｜の値さえ分ればｓ３〜ｓ５の方向は考える必要はありません。
さて、図.３において、θAの角度を求めるには、
Ａ・ｓｉｎ（θａ） ＝ （１／２） × Ｂ
より、
-1
θａ ＝ ｓｉｎ （Ｂ／２Ａ）
ここで、θａは図.３における直角三角形ＡＢＣの角の一つであり、
θＡ ＝ ２ × θａ （２１）
の関係があります。
最終的に、θ４は図.３にある通り、
θ４ ＝ θＡ − １８０°
から、
-1
θ４ ＝ ２（ｓｉｎ （｜Ｐ４’｜／２Ｌ３） − １８０°） （２２）
として求める事が出来ます。
W：θ３導出
θ４が求まれば比較的簡単に…という程楽でも無いです。残念ながら。
まず、Ｐ４について考えてみます。
順運動学解から、Ｐ４は、
｛ ０ ｝ ｛−Ｃ１Ｓ３−Ｓ１Ｓ２Ｃ３｝ ｛−Ｃ１Ｓ（３＋４）−Ｓ１Ｓ２Ｃ（３＋４）｝
Ｐ４ ＝ ｛ ０ ｝ ＋ ｛−Ｓ１Ｓ３＋Ｃ１Ｓ２Ｃ３｝Ｌ３ ＋ ｛−Ｓ１Ｓ（３＋４）＋Ｃ１Ｓ２Ｃ（３＋４）｝Ｌ４
｛−Ｌ１｝ ｛−Ｃ２Ｃ３ ｝ ｛−Ｃ２Ｃ（３＋４） ｝
であり、前述したように、Ｐ４＝Ｐ５であり、
｛−Ｃ１Ｓ３−Ｓ１Ｓ２Ｃ３｝ ｛−Ｃ１Ｓ（３＋４）−Ｓ１Ｓ２Ｃ（３＋４）｝
Ｐ４’ ＝ ｛−Ｓ１Ｓ３＋Ｃ１Ｓ２Ｃ３｝Ｌ３ ＋ ｛−Ｓ１Ｓ（３＋４）＋Ｃ１Ｓ２Ｃ（３＋４）｝Ｌ４
｛−Ｃ２Ｃ３ ｝ ｛−Ｃ２Ｃ（３＋４） ｝
となります。このＰ４’は先程のＰ４’のことであり、この時点で既知の値です。
ここから、θ１、θ２の影響を除外します。
具体的には、θ１、θ２の回転分を除去し、基準位置に戻すことになります。
式としては、以下の様になります。
Ｐ４’’’ ＝ Ｅ（−ｉθ２）Ｅ（−ｋθ１）Ｐ４’
ここでＥ（−〜）というのは、いわゆる逆行列のことで、例えば Ｅ（−ｋθ１）Ｅ（ｋθ１）＝１ という性質を持ちます。
Ｐ４’は、
Ｐ４’ ＝ Ｅ（ｋθ１）Ｅ（ｉθ２）Ｅ（ｊθ３）ｌ３ ＋ Ｅ（ｋθ１）Ｅ（ｉθ２）Ｅ（ｊθ３）Ｅ（ｊθ４）ｌ４
となるので、Ｐ４’’’は、
Ｐ４’’’ ＝ Ｅ（−ｉθ２）Ｅ（−ｋθ１）Ｅ（ｋθ１）Ｅ（ｉθ２）Ｅ（ｊθ３）ｌ３ ＋ Ｅ（−ｉθ２）Ｅ（−ｋθ１）Ｅ（ｋθ１）Ｅ（ｉθ２）Ｅ（ｊθ３）Ｅ（ｊθ４）ｌ４
よって、
Ｐ４’’’ ＝ Ｅ（ｊθ３）ｌ３ ＋ Ｅ（ｊθ３）Ｅ（ｊθ４）ｌ４
｛−Ｓ３｝ ｛−Ｃ３Ｓ４−Ｓ３Ｃ４｝
Ｐ４’’’ ＝ ｛ ０ ｝ｌ３ ＋ ｛ ０ ｝ｌ４
｛−Ｃ３｝ ｛ Ｓ３Ｓ４−Ｃ３Ｃ４｝
となります。
実際には、
｛ １ ０ ０ ｝
Ｅ（−ｉθ） ＝ ｛ ０ Ｃθ Ｓθ｝
｛ ０ −Ｓθ Ｃθ｝
｛ Ｃθ Ｓθ ０｝
Ｅ（−ｋθ） ＝ ｛−Ｓθ Ｃθ ０｝
｛ ０ ０ １｝
より、
｛ Ｃ１ Ｓ１ ０ ｝
Ｅ（−ｉθ２）Ｅ（−ｋθ） ＝ ｛−Ｓ１Ｃ１ Ｃ１Ｃ２ Ｓ２｝
｛ Ｓ１Ｓ２ −Ｃ１Ｓ２ Ｃ２｝
から、
｛ Ｃ１ Ｓ１ ０｝｛Ｐ４ｘ’｝
Ｐ４’’’ ＝ ｛−Ｓ１Ｃ１ Ｃ１Ｃ２ Ｓ２｝｛Ｐ４ｙ’｝ （２３）
｛ Ｓ１Ｓ２ −Ｃ１Ｓ２ Ｃ２｝｛Ｐ４ｚ’｝
この変換により、Ｐ４’’’はｙ成分を無くし、ｘ−ｚ平面上の２次元座標系で考えることができるようになります。
ここで、先の図.３（左図）を見なおすと、ｙ軸側から見たｘ−ｚ平面の図と等価であることが分ります。
そうすると、Vでθ４を求める際に利用した二等辺三角形ＡＡＢの鋭角（∠ＡＢ）と、Ｐ４’’’の角度を合計すれば、θ３を導けることになります。
式としては、
-1
θ３ ＝ ∠ＡＢ ＋ ｔａｎ （−Ｐ４ｚ’’’／Ｐ４ｘ’’’）
-1
θ３ ＝ （９０° − θａ） ＋ ｔａｎ （−Ｐ４ｚ’’’／Ｐ４ｘ’’’） （２４）
X：θ５導出
…いい加減くどくなってきましたか？
私はもーめげそうです。
とり合えず、θ５の導出には（１６）式を使います。
｛Ｃ１Ｃ（３＋４＋５）−Ｓ１Ｓ２Ｓ（３＋４＋５）｝
ｓ６ ＝ ｛Ｓ１Ｃ（３＋４＋５）＋Ｃ１Ｓ２Ｓ（３＋４＋５）｝ （１６）
｛−Ｃ２Ｓ（３＋４＋５） ｝
回転軸ｓ６のベクトルは、つま先の方向を示すベクトルａと同じ方向を示します。
どちらも単位ベクトルであり、
ａ ＝ ｓ６
ということになります。
さて、式（１６）より、ｓ６ｚに注目します。
ｓ６ｚ ＝ ａｚ ＝ −Ｃ２Ｓ（３＋４＋５）
−ａｚ／Ｃ２ ＝ Ｓ（３＋４＋５）
より、
-1
θ３ ＋ θ４ ＋ θ５ ＝ ｓｉｎ （−ａｚ／Ｃ２）
-1
θ５ ＝ ｓｉｎ （−ａｚ／Ｃ２） − θ３ −θ４ （２５）
となり、θ５を導出できます。
Y：θ６導出
やっと本章最後です。
頑張りましょう。（むしろ自分に言い聞かせ）
θ６を導出するのに使用するのは式（１０）です。
｛−Ｃ１Ｓ（３＋４＋５）Ｃ６−Ｓ１Ｃ２Ｓ６−Ｓ１Ｓ２Ｃ（３＋４＋５）Ｃ６｝
ｂ ＝ ｛−Ｓ１Ｓ（３＋４＋５）Ｃ６＋Ｃ１Ｃ２Ｓ６＋Ｃ１Ｓ２Ｃ（３＋４＋５）Ｃ６｝ （１０）
｛Ｓ２Ｓ６−Ｃ２Ｃ（３＋４＋５）Ｃ６ ｝
そのまんまｂを表しているので、既知であるｂと、これまで求めて来たθ１〜θ５を用いれば求めることができます。
まず、
Ｃ１ｂｘ ＋ Ｓ１ｂｙ ＝ −Ｓ（３＋４＋５）Ｃ６
とできるので、
−（Ｃ１ｂｘ ＋ Ｓ１ｂｙ）／Ｓ（３＋４＋５） ＝ Ｃ６
より、
-1
θ６ ＝ ｃｏｓ （−（Ｃ１ｂｘ ＋ Ｓ１ｂｙ）／Ｓ（３＋４＋５））
これで、θ１〜θ６が全て求まりました。
最終的には、重心に加わるモーメントや姿勢を考慮した上で、足に目標値を与える訳で、これで終了という事は無く、場合によっては全身についての逆運動学を解く必要が生じるかもしれませんが、順運動学、逆運動学の解の考え方自体は今回で身に付いたと考えます。
重心についても、順運動学を応用することで算出可能と思われます。
…今回は導出してませんが。（＾＾；
戻る