概要

前回のxz-2次元制御を拡張し、今回はxyz-3次元で制御を行う。
また、手首(wrist)についても制御を行い、地面平行や壁平行をとらせるようにする。

計算-xyz

xyzをx'z,$\theta_1$に変換し前回の計算をx'z平面に適用する。

\displaylines{
\begin{align}
&\theta_1=\arctan2(y_e,x_e)\\
&x'_e=\sqrt{x_e^2+y_e^2}
\end{align}
}

後は前回の$x_e$を$x'_e$として計算すればよい。

計算-wrist

手首姿勢について制御を行う。
pitch軸(首振り縦方向),yaw軸(首振り横方向)をエンドエフェクタがそれぞれ地面や壁に平行をとるように制御する。地面平行と壁平行は$\theta_1\ne0$で両立しないため、モード切替で対応する。
pitch角,yaw角をそれぞれ$\alpha,\beta$として、図の関係からx'z平面で計算した$\phi$を用いて以下のように求められる。

\displaylines{
\begin{align}
&・地面平行モード\\
&\alpha=\phi_2+\phi_3+\phi_4-\frac{\pi}{2}\\
&\beta=0\\
&・壁平行モード\\
&\alpha=\phi_2+\phi_3+\phi_4\\
&\beta=\theta_1\\
\end{align}
}

(この記事全体を通して角度の向きの定義はテキトウで、動作を見ながら適宜反転したりしている。もしかすれば計算式は90度ずれているかもしれない。)
実際のところ、サーボのバックラッシュや剛性の都合で手首角の制御があまり出なかったため、$\alpha$角は$\phi_2+\phi_3+\phi_4$に操作者入力値$p_e$を加えて実装した。

実機動作

3D描画が面倒だったため、シミュレーション上での動作試験はパス、いきなり実機試験とした。
動作の様子。(やたらビカビカしているが特に関係はない)

20kサーボ版3+3軸垂直多関節アーム
xyz → x'z,θ1 → θ1,θ2,θ3 変換計算と手首制御
('25 7) pic.twitter.com/EG5Vq3Ivc3
— 6186milktech (@6186milktech_rt) August 14, 2025

限界角でe値(操作指定値)を止める機能をつけた。
可動域が球状に近いためどこまで動かせるのか直感的にわかりづらい。

次

前回に比べ、ずいぶん人間に親切になった。しかしこれでもまだ操作性の悪さは拭えない。ハード側の改修も含め、操作性の向上に努める。

ログ

2025-07-04 手首機構の固定具等が完成,電源周りを整理
2025-07-08 ledテープ装飾
2025-07-10 吸引ポンプ変更,結線等,pitchサーボ問題
2025-07-11 pitchサーボ変更,解決,xyz制御実装
2025-07-12 記事制作
2025-07-15 記事完成
2025-08-15 記事の存在を思い出す,投稿