これまでの記事では、ロボットの手先の動かしやすさ示す可操作性楕円体について紹介しています。
前回の記事では、2リンクモデルのヤコビ行列Jから可操作性楕円体を求めるために必要な行列JJTを算出しました。
今回の記事では、行列JJTから固有値と固有ベクトルと固有ベクトルを算出し、可操作性楕円体の大きさや方向を求めます。
2リンクモデルのヤコビ行列Jと行列JJT
前回までの記事のおさらいです。
前回の記事で、2リンクモデルのヤコビ行列Jと行列A=JJTについて以下のように算出しました。
ヤコビ行列J:
$$ \begin{eqnarray} \boldsymbol{ J } = \begin{bmatrix} – L_1 \sin \left( \theta_1 \right) – L_2 \sin \left( \theta_1 + \theta_2 \right) & – L_2 \sin \left( \theta_1 + \theta_2 \right) \\ L_1 \cos \left( \theta_1 \right) + L_2 \cos \left( \theta_1 + \theta_2 \right) & L_2 \cos \left( \theta_1 + \theta_2 \right) \end{bmatrix} \end{eqnarray}$$
行列A=JJT:
$$ \begin{eqnarray} \boldsymbol{A} = \begin{bmatrix} \tiny{ \left( – L_1 \sin \left( \theta_1 \right) – L_2 \sin \left( \theta_1 + \theta_2 \right) \right)^2 + \left( – L_2 \sin \left( \theta_1 + \theta_2 \right) \right)^2 } & \tiny{ – \frac{1}{2} {L_1}^2 \sin \left( 2 \theta_1 \right) – L_1 L_2 \sin \left( 2 \theta_1 + \theta_2 \right) – {L_2}^2 \sin \left( 2 \theta_1 + 2 \theta_2 \right) } \\ \tiny{ – \frac{1}{2} {L_1}^2 \sin \left( 2 \theta_1 \right) – L_1 L_2 \sin \left( 2 \theta_1 + \theta_2 \right) – {L_2}^2 \sin \left( 2 \theta_1 + 2 \theta_2 \right) } & \tiny{ \left( L_1 \cos \left( \theta_1 \right) + L_2 \cos \left( \theta_1 + \theta_2 \right) \right)^2 + \left( L_2 \cos \left( \theta_1 + \theta_2 \right) \right)^2 } \end{bmatrix} \end{eqnarray} $$
詳しい算出方法は、こちらの記事を参考にしてください。
今回の記事では、この行列Aの固有値を用いて可操作性楕円体の大きさを求めていきます。
固有値の一般解の導出
可操作性楕円体は行列A=JJTの固有値と固有ベクトルから求めることが出来ます。
今回の記事では、これまでに求めた行列Aの固有値の一般解を算出します。
固有値の算出式
行列A
$$ \begin{eqnarray} \boldsymbol{A} &=& \begin{bmatrix} a_{11} & a_{12} \\ a_{21} & a_{22} \end{bmatrix} \\ &=& \begin{bmatrix} \tiny{ \left( – L_1 \sin \left( \theta_1 \right) – L_2 \sin \left( \theta_1 + \theta_2 \right) \right)^2 + \left( – L_2 \sin \left( \theta_1 + \theta_2 \right) \right)^2 } & \tiny{ – \frac{1}{2} {L_1}^2 \sin \left( 2 \theta_1 \right) – L_1 L_2 \sin \left( 2 \theta_1 + \theta_2 \right) – {L_2}^2 \sin \left( 2 \theta_1 + 2 \theta_2 \right) } \\ \tiny{ – \frac{1}{2} {L_1}^2 \sin \left( 2 \theta_1 \right) – L_1 L_2 \sin \left( 2 \theta_1 + \theta_2 \right) – {L_2}^2 \sin \left( 2 \theta_1 + 2 \theta_2 \right) } & \tiny{ \left( L_1 \cos \left( \theta_1 \right) + L_2 \cos \left( \theta_1 + \theta_2 \right) \right)^2 + \left( L_2 \cos \left( \theta_1 + \theta_2 \right) \right)^2 } \end{bmatrix} \end{eqnarray} $$
の固有値λは
$$ \begin{eqnarray} && \mathrm{ det } \left( \boldsymbol{A} – \lambda \boldsymbol{I} \right) = 0 \\ && \Rightarrow \begin{vmatrix} a_{11} – \lambda & a_{12} \\ a_{21} & a_{22} – \lambda \end{vmatrix} = 0 \end{eqnarray} $$
から求めることが出来ます。
上の式を展開すると
$$ \begin{eqnarray} \begin{vmatrix} a_{11} – \lambda & a_{12} \\ a_{21} & a_{22} – \lambda \end{vmatrix} &=& 0 \\ \left( a_{11} – \lambda \right) \left( a_{22} – \lambda \right) – a_{12} a_{21} &=& 0 \\ {\lambda}^2 – \left( a_{11} + a_{22} \right) \lambda + a_{11} a_{22} – a_{12} a_{21} &=& 0 \\ {\lambda}^2 + \alpha \lambda + \beta &=& 0 \end{eqnarray} $$
となります。
係数αの算出
ここで、式の中に含まれる係数αは
$$ \begin{eqnarray} \alpha &=& – \left\{ \begin{array}{l} \left( \left( – L_1 \sin \left( \theta_1 \right) – L_2 \sin \left( \theta_1 + \theta_2 \right) \right)^2 + \left( – L_2 \sin \left( \theta_1 + \theta_2 \right) \right)^2 \right) \\ + \left( \left( L_1 \cos \left( \theta_1 \right) + L_2 \cos \left( \theta_1 + \theta_2 \right) \right)^2 + \left( L_2 \cos \left( \theta_1 + \theta_2 \right) \right)^2 \right) \end{array} \right\} \\ &=& – \left\{ \begin{array}{l} {L_1}^2 \sin^2 \left( \theta_1 \right) + 2 L_1 L_2 \sin \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) + {L_2}^2 \sin^2 \left( \theta_1 + \theta_2 \right) + {L_2}^2 \sin^2 \left( \theta_1 + \theta_2 \right) \\ + {L_1}^2 \cos^2 \left( \theta_1 \right) + 2 L_1 L_2 \cos \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + {L_2}^2 \cos^2 \left( \theta_1 + \theta_2 \right) + {L_2}^2 \cos^2 \left( \theta_1 + \theta_2 \right) \end{array} \right\} \\ &=& – \left\{ \begin{array}{l} {L_1}^2 \left( \sin^2 \left( \theta_1 \right) + \cos^2 \left( \theta_1 \right) \right) + 2 {L_1}^2 \left( \sin^2 \left( \theta_1 + \theta_2 \right) + \cos^2 \left( \theta_1 + \theta_2 \right) \right) \\ + 2 L_1 L_2 \left( \cos \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + \sin \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \right) \end{array} \right\} \\ &=& – \left( {L_1}^2 + 2 {L_2}^2 + 2 L_1 L_2 \cos \left( \theta_2 \right) \right) \end{eqnarray} $$
と求めることが出来ます。
係数βの算出
同様に、式に含まれる係数βについて
$$ \begin{eqnarray} \beta &=& \left\{ \begin{array}{l} \left( \left( – L_1 \sin \left( \theta_1 \right) – L_2 \sin \left( \theta_1 + \theta_2 \right) \right)^2 + \left( – L_2 \sin \left( \theta_1 + \theta_2 \right) \right)^2 \right) \\ \cdot \left( \left( L_1 \cos \left( \theta_1 \right) + L_2 \cos \left( \theta_1 + \theta_2 \right) \right)^2 + \left( L_2 \cos \left( \theta_1 + \theta_2 \right) \right)^2 \right) \\ – \left( – \frac{1}{2} {L_1}^2 \sin \left( 2 \theta_1 \right) – L_1 L_2 \sin \left( 2 \theta_1 + \theta_2 \right) – {L_2}^2 \sin \left( 2 \theta_1 + 2 \theta_2 \right) \right) \\ \cdot \left( – \frac{1}{2} {L_1}^2 \sin \left( 2 \theta_1 \right) – L_1 L_2 \sin \left( 2 \theta_1 + \theta_2 \right) – {L_2}^2 \sin \left( 2 \theta_1 + 2 \theta_2 \right) \right) \end{array} \right\} \\ &=& \left\{ \begin{array}{l} \left( {L_1}^2 \sin^2 \left( \theta_1 \right) + 2 L_1 L_2 \sin \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) + 2 {L_2}^2 \sin^2 \left( \theta_1 + \theta_2 \right) \right) \\ \cdot \left( {L_1}^2 \cos^2 \left( \theta_1 \right) + 2 L_1 L_2 \cos \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + 2 {L_2}^2 \cos^2 \left( \theta_1 + \theta_2 \right) \right) \\ – \frac{1}{4} \left( {L_1}^2 \sin \left( 2 \theta_1 \right) + 2 L_1 L_2 \sin \left( 2 \theta_1 + \theta_2 \right) + 2 {L_2}^2 \sin \left( 2 \theta_1 + 2 \theta_2 \right) \right) \\ \cdot \left( {L_1}^2 \sin \left( 2 \theta_1 \right) + 2 L_1 L_2 \sin \left( 2 \theta_1 + \theta_2 \right) + 2 {L_2}^2 \sin \left( 2 \theta_1 + 2 \theta_2 \right) \right) \end{array} \right\} \\ &=& {L_1}^4 \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 \right) \\ && + 2 {L_1}^3 L_2 \left( \sin^2 \left( \theta_1 \right) \cos \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + \cos^2 \left( \theta_1 \right) \sin \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \right) \\ && + 2 {L_1}^2 {L_2}^2 \left( \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 + \theta_2 \right) + \cos^2 \left( \theta_1 \right) \sin^2 \left( \theta_1 + \theta_2 \right) \right) \\ && + 4 {L_1}^2 {L_2}^2 \left( \sin \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \cos \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) \right) \\ && + 4 L_1 {L_2}^3 \left( \sin^2 \left( \theta_1 + \theta_2 \right) \cos \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + \cos^2 \left( \theta_1 + \theta_2 \right) \sin \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \right) \\ && + 4 {L_2}^4 \sin^2 \left( \theta_1 + \theta_2 \right) \cos^2 \left( \theta_1 + \theta_2 \right) \\ && – \frac{1}{4} {L_1}^4 \sin^2 \left( 2 \theta_1 \right) \\ && – {L_1}^3 L_2 \sin \left( 2 \theta_1 \right) \sin \left( 2 \theta_1 + \theta_2 \right) \\ && – {L_1}^2 {L_2}^2 \sin \left( 2 \theta_1 \right) \sin \left( 2 \theta_1 + 2 \theta_2 \right) \\ && – {L_1}^2 {L_2}^2 \sin^2 \left( 2 \theta_1 + \theta_2 \right) \\ && – 2 L_1 {L_2}^3 \sin \left( 2 \theta_1 + \theta_2 \right) \sin \left( 2 \theta_1 + 2 \theta_2 \right) \\ && – {L_2}^4 \sin^2 \left( 2 \theta_1 + 2 \theta_2 \right) \\ &=& {L_1}^4 \left( \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 \right) – \frac{1}{4} \sin^2 \left( 2 \theta_1 \right) \right) \\ && + {L_1}^3 L_2 \left( 2 \sin^2 \left( \theta_1 \right) \cos \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + 2 \cos^2 \left( \theta_1 \right) \sin \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) – \sin \left( 2 \theta_1 \right) \sin \left( 2 \theta_1 + \theta_2 \right) \right) \\ && + {L_1}^2 {L_2}^2 \left( 2 \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 + \theta_2 \right) + 2 \cos^2 \left( \theta_1 \right) \sin^2 \left( \theta_1 + \theta_2 \right) + 4 \sin \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \cos \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) – \sin \left( 2 \theta_1 \right) \sin \left( 2 \theta_1 + 2 \theta_2 \right) – \sin^2 \left( 2 \theta_1 + \theta_2 \right) \right) \\ && + L_1 {L_2}^3 \left( 4 \sin^2 \left( \theta_1 + \theta_2 \right) \cos \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + 4 \cos^2 \left( \theta_1 + \theta_2 \right) \sin \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) – 2 \sin \left( 2 \theta_1 + \theta_2 \right) \sin \left( 2 \theta_1 + 2 \theta_2 \right) \right) \\ && + {L_2}^4 \left( 4 \sin^2 \left( \theta_1 + \theta_2 \right) \cos^2 \left( \theta_1 + \theta_2 \right) – \sin^2 \left( 2 \theta_1 + 2 \theta_2 \right) \right) \end{eqnarray} $$
と展開して、それぞれの係数ごとにまとめられます。
それぞれの係数について、三角関数の公式を用いて変換すると、
$$ \begin{eqnarray} \beta &=& {L_1}^4 \left( \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 \right) – \frac{1}{4} 4 \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 \right) \right) \\ && + {L_1}^3 L_2 \left( \sin \left( 2 \theta_1 \right) \sin \left( 2 \theta_1 + \theta_2 \right) – \sin \left( 2 \theta_1 \right) \sin \left( 2 \theta_1 + \theta_2 \right) \right) \\ && + {L_1}^2 {L_2}^2 \left( 2 \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 + \theta_2 \right) + 2 \cos^2 \left( \theta_1 \right) \sin^2 \left( \theta_1 + \theta_2 \right) + \sin \left( 2 \theta_1 \right) \sin \left( 2 \theta_1 + 2 \theta_2 \right) – \sin \left( 2 \theta_1 \right) \sin \left( 2 \theta_1 + 2 \theta_2 \right) – \left( \sin \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + \cos \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \right)^2 \right) \\ && + L_1 {L_2}^3 \left( 2 \sin \left( 2 \theta_1 + \theta_2 \right) \sin \left( 2 \theta_1 + 2 \theta_2 \right) – 2 \sin \left( 2 \theta_1 + \theta_2 \right) \sin \left( 2 \theta_1 + 2 \theta_2 \right) \right) \\ && + {L_2}^4 \left( 4 \sin^2 \left( \theta_1 + \theta_2 \right) \cos^2 \left( \theta_1 + \theta_2 \right) – 4 \sin^2 \left( \theta_1 + \theta_2 \right) \cos^2 \left( \theta_1 + \theta_2 \right) \right) \\ &=& {L_1}^2 {L_2}^2 \left( 2 \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 + \theta_2 \right) + 2 \cos^2 \left( \theta_1 \right) \sin^2 \left( \theta_1 + \theta_2 \right) – \left( \sin \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + \cos \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \right)^2 \right) \end{eqnarray} $$
のように、L12L22の項以外は消すことが出来ます。
そして、残ったL12L22の項について整理すると、
$$ \begin{eqnarray} \beta &=& {L_1}^2 {L_2}^2 \left( 2 \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 + \theta_2 \right) + 2 \cos^2 \left( \theta_1 \right) \sin^2 \left( \theta_1 + \theta_2 \right) – \left( \sin \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) + \cos \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \right)^2 \right) \\ &=& {L_1}^2 {L_2}^2 \left( \sin^2 \left( \theta_1 \right) \cos^2 \left( \theta_1 + \theta_2 \right) + \cos^2 \left( \theta_1 \right) \sin^2 \left( \theta_1 + \theta_2 \right) – 2 \sin \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) \cos \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \right) \\ &=& {L_1}^2 {L_2}^2 \left( \sin \left( \theta_1 \right) \cos \left( \theta_1 + \theta_2 \right) – \cos \left( \theta_1 \right) \sin \left( \theta_1 + \theta_2 \right) \right)^2 \\ &=& {L_1}^2 {L_2}^2 \left( – \sin \left( \theta_2 \right) \right)^2 \\ &=& {L_1}^2 {L_2}^2 \sin^2 \left( \theta_2 \right) \end{eqnarray} $$
と係数βを求めることが出来ました。
固有値の一般式
固有値の算出式
$$ {\lambda}^2 + \alpha \lambda + \beta = 0 $$
について、係数αとβを代入して固有値λについて解くと、
$$ \begin{eqnarray} \lambda &=& \frac{- \alpha \pm \sqrt{ {\alpha}^2 – 4 \beta }}{2} \\ &=& \frac{\left( {L_1}^2 + 2 {L_2}^2 + 2 L_1 L_2 \cos \left( \theta_2 \right) \right) \pm \sqrt{ \left( {L_1}^2 + 2 {L_2}^2 + 2 L_1 L_2 \cos \left( \theta_2 \right) \right)^2 – 4 {L_1}^2 {L_2}^2 \sin^2 \left( \theta_2 \right) }}{2} \end{eqnarray} $$
となります。
この式に、2リンクモデルのリンク長(L1とL2)に関節状態(θ1とθ2)を代入することで、固有値λを求めることが出来ます。
そして、求めたそれぞれの固有値λに対して固有ベクトルvを求めます。
最後に固有値λと固有ベクトルvを用いて、可操作性楕円体を求めてます。
まとめ
今回の記事では、2リンクモデルの可操作性楕円体を求めるために、ヤコビ行列Jから得られる行列A=JJTの固有値の求め方を紹介しました。
今回の記事では、固有値を求めるために少し長く複雑な計算を用いましたが、MATLAB等のソフトウェアを用いることで、もっと簡単に求めることも出来ます。
次回の記事では、実際に具体的な2リンクモデルの値を代入して、可操作性楕円体を求める流れを紹介したいと思います。