3DoFの機構の設計と制御
関節というものは実にさまざまな種類がありますが、ボールジョイントはかなり自由な姿勢を取れる代表的な関節といえます。フィギュアやプラモでもよく見かけますね。
そもそも3次元空間において剛体のとり得る運動というのは並進3軸＋回転3軸の6DoF（Degree of Freedom）と考えられます。最近盛り上がっているVR界隈でもこの言葉をよく見かけますね。
一般的なボールジョイントは3DoFであり、並進移動はできないけれども直行3軸が1点で交わり、それぞれの軸中心に回転が出来る関節を指しますから、その自由度の高さが伺えます。
さて、ボールジョイントのような受動的で1点交差な3DoFを実現するのは比較的容易なのですが、能動的な3DoFとなるととても難しくなります。
その中でも、産業用ロボットの手首機構などで比較的よく見かけるYPY配置（ヨー・ピッチ・ヨー）の機構はまだ複雑さは穏やかでかつ設計も容易に見えたので、動かしていくために考えたことをまとめておこうとした次第です。
モータの入力角度をθ
各軸まわりの角度をφ
3DoFの中心点から手先点までの位置ベクトルをPとします。
回転方向は基本的に右ネジの法則を適用する感じです。
位置ベクトルの添え字は座標系と定義します。記事トップ画像にあるΣ3は位置ベクトルの根元に置いたほうが正しかったですね。
傘歯車での減速比が1:2であり、φ1とθ1の減速比も1:2であることを考慮して、θとφの関係は下記のとおりです。
回転行列は下添え字の座標系から上添え字の座標系への回転と定義します。
ワールド座標系をΣ0、第1ヨー軸まわりに回転した後の座標系をΣ1、第1ピッチ軸まわりに回転した後の座標系をΣ2、第2ヨー軸まわりに回転した後の座標系をΣ3と定義したとき、Σ0から見た手先位置は下記のようになります。
このようにφ3に依らず手先位置が記述できることが分かります。
次に、任意の軸周りの回転へと拡張するとともに、φ3へ拘束を施すような動きを考えます。
まずは簡単に、現在の姿勢からΣ3におけるx軸周り回転を考えます。この際の任意軸ｌについては下記のように示すことが出来ます。
大雑把な解釈をするとP3=(1,0,0)Tの際のP0が上記のようになるといえます。
ロドリーグの式より
（<a,b>はaとbの内積とします。）
P0に適用すると下記のようになります。
この際、ｌ軸周りにΩだけ回転した時の各軸周りの角度をφ’とします。
当然ですが回転後だろうと回転軸は不変ですからφ3’も定まります。
最初に示したようにθとφの間には相関関係があるのでφ’に対応するθ’が導出できます。
したがって、モータ入力角θ→θ’を与えることで、姿勢角φ→φ’（ｌ軸周りは０→Ω）、位置ベクトルP0→P0’となることは明らかです。
この一連の回転は特定の1本の軸中心の回転に限定されますが、細かく回転軸を再定義し直していくことで曲線の軌跡を描くような回転も可能になります。
ただしこの制御方法だと、たとえばφ＝（0,0,π/2）TからΩを与えると回転方向と直交して姿勢移動が行えない等、特異点問題があります。（ジンバルロックでしょうか？）
四元数を導入することで特異点をπだけズラすことも可能ではあるのでもう少し検証を深める必要はあります。
現状おおよそ形には出来ているので、次回は造詣のことを書いて、火入れまで進めたいです。
