k1sinθ+k2cosθ=k3の解

k1sinθ+k2cosθ=k3の解を求める方法ついて紹介します。

k1sinθ+k2cosθ=k3の解の導出

【定理1】

(1)   \begin{eqnarray*} k_1sin\theta+k_2cos\theta=k_3 \end{eqnarray*}

の解は,つぎの式になります。

(2)   \begin{eqnarray*} \theta =atan2(k_1,k_2) \pm atan2( \sqrt{k_1^2+k_2^2-k_3^2} ,k_3) \end{eqnarray*}

【証明】

(3)   \begin{eqnarray*} \phi =atan2(k_1,k_2) \end{eqnarray*}

より

(4)   \begin{eqnarray*} \phi- \theta &=& atan2{sin(\phi- \theta),cos(\phi- \theta)} \end{eqnarray*}

(5)   \begin{eqnarray*} \theta &=& \phi -atan2{sin(\phi- \theta),cos(\phi- \theta)} \end{eqnarray*}

ここで,sin(\phi- \theta), cos(\phi- \theta)を求めることを考えます。 図より

(6)   \begin{eqnarray*} k_1=\sqrt{k_1^2+k_2^2}sin\phi \end{eqnarray*}

(7)   \begin{eqnarray*} k_2=\sqrt{k_1^2+k_2^2}cos\phi \end{eqnarray*}

なので

(8)   \begin{eqnarray*} (\sqrt{k_1^2+k_2^2}sin\phi )sin\theta+(\sqrt{k_1^2+k_2^2}cos\phi )cos\theta = k_3 \end{eqnarray*}

(9)   \begin{eqnarray*} \sqrt{k_1^2+k_2^2}(sin\phi sin\theta+cos\phi cos\theta) = k_3 \end{eqnarray*}

(10)   \begin{eqnarray*} \sqrt{k_1^2+k_2^2}cos(\phi- \theta) = k_3 \end{eqnarray*}

(11)   \begin{eqnarray*} cos(\phi- \theta) = \frac{k_3}{\sqrt{k_1^2+k_2^2}} \end{eqnarray*}

また,sin^2\theta+cos^2\theta=1より

(12)   \begin{eqnarray*} sin(\phi- \theta) &=& \pm \sqrt{1-cos(\phi- \theta)^2} \end{eqnarray*}

(13)   \begin{eqnarray*} &=& \pm \sqrt{1-\frac{k_3^2}{k_1^2+k_2^2}}\ \end{eqnarray*}

(14)   \begin{eqnarray*} &=& \pm \sqrt{\frac{k_1^2+k_2^2-k_3^2}{k_1^2+k_2^2}} \end{eqnarray*}

sin(\phi- \theta), cos(\phi- \theta)が求まったので

(15)   \begin{eqnarray*} \theta =\phi -atan2(\pm {\frac{\sqrt{k_1^2+k_2^2-k_3^2}}{\sqrt{k_1^2+k_2^2}}},\frac{k_3}{\sqrt{k_1^2+k_2^2}}) \end{eqnarray*}

\sqrt{k_1^2+k_2^2}>0より分母を消去すると

(16)   \begin{eqnarray*} \theta = \phi -atan2(\pm \sqrt{k_1^2+k_2^2-k_3^2},k_3) = \phi \pm atan2(\sqrt{k_1^2+k_2^2-k_3^2},k_3) \end{eqnarray*}

(17)   \begin{eqnarray*} \theta = atan2(k_1,k_2) \pm atan2(\sqrt{k_1^2+k_2^2-k_3^2},k_3) \end{eqnarray*}

(証明終わり)

関連ページ

ロボット工学入門
ロボット工学の基本的なアルゴリズムや公式について入門者向けに解説します。

コメント