■ - 熊工房　ohgumaの腹凹ませたい日記

少し前から紙面上で考えていた股ヨー( $\theta_1$ )・ロール( $\theta_2$ )・ピッチ( $\theta_3$ )、ひざピッチ( $\theta_4$ )、足首ピッチ( $\theta_5$ )・ロール( $\theta_6$ )の6軸を考慮した逆運動学の計算の途中まで。

股ロール・ピッチ直行軸を原点にし、足首ロール・ピッチ直行軸の位置をワールド座標系の $\left[x_1\\y_1\\z_1\right]$ 、ピッチ軸間距離を $l$ (股ひざ、ひざ足首は等距離)とする。

股ヨー軸のローカル座標系でみる上記位置は $\left[x_2\\y_2\\z_2\right] = \left[ \begin{array} cos(\theta_1) & -sin(\theta_1)& 0 \\ sin(\theta_1) & cos(\theta_1) & 0\\ 0 & 0& 1\end{array}\right] \left[x_1\\y_1\\z_1\right]$ 。

この時点でロール軸が $\theta_2=\theta_6=-atan(\frac{y_2}{z_2})$ となる。

股直行軸と足首直行軸距離は $C=\sqrt{{x_2}^2+{y_2}^2+{z_2}^2}$ なので、
余弦定理より $C^2=2l^2(1-cos(\pi-\theta_4))$
よってひざピッチ軸 $\theta_4=-acos(1-\frac{C^2}{2l^2}+\pi$

「ヒューマノイドロボット」