スポンサーリンク

2リンクモデルの可操作性楕円体の大きさを固有値の一般解から求める

これまでの記事では、ロボットの手先の動かしやすさ示す可操作性楕円体について紹介しています。

ロボットの姿勢(関節状態)によって、動作しやすい姿勢(方向)や動作しづらい姿勢(方向)が存在します。 これらのどれくら...

前回の記事では、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リンクモデルのリンク長(L1L2)に関節状態(θ1θ2)を代入することで、固有値λを求めることが出来ます。

そして、求めたそれぞれの固有値λに対して固有ベクトルvを求めます。

最後に固有値λと固有ベクトルvを用いて、可操作性楕円体を求めてます。

まとめ

今回の記事では、2リンクモデルの可操作性楕円体を求めるために、ヤコビ行列Jから得られる行列A=JJTの固有値の求め方を紹介しました。

今回の記事では、固有値を求めるために少し長く複雑な計算を用いましたが、MATLAB等のソフトウェアを用いることで、もっと簡単に求めることも出来ます。

次回の記事では、実際に具体的な2リンクモデルの値を代入して、可操作性楕円体を求める流れを紹介したいと思います。

可操作性楕円体を用いることで、ロボットの動かしやすさを表すことが出来ます。 これまでの記事では、2リンクモデル...
スポンサーリンク
レクタングル(大)広告
レクタングル(大)広告

シェアする

  • このエントリーをはてなブックマークに追加

フォローする

関連コンテンツ