実際に使える制御工学：応用編（５）「多関節ロボットの非干渉化制御。多関節ロボット特有の問題の解決策。」
＜この記事で伝えたい事＞
多関節ロボット制御の難しいポイントの1つは、軸間干渉がある点です
軸間干渉とは、ニュートンの作用・反作用の法則によって、ある関節（軸）を動かす力の反動が、別の関節に伝わってしまう現象のことです。
力学的にみて、単軸で動かすときと大きく違う点です。非干渉化制御とは、軸間干渉の影響を打ち消す制御方法です
非干渉化制御は、フィードバック型とフィードフォーワード型の2種類に大別できます。
いずれも軸間干渉の影響を打ち消す効果がありますが、それぞれ異なるメリット/デメリットがあります。
1. はじめに
自己紹介はこちらです。
この記事は、主に若手～中堅エンジニアに向けた技術紹介・解説記事シリーズの第５弾です。
今回は、多軸ロボット特有の問題である軸間干渉現象と、その対策である非干渉化制御を紹介・解説します。
2. 一般的な多関節ロボットコントローラ構成
本題に入る前に、一般的な多関節ロボットコントローラの話をします。
多関節ロボットコントローラは、下図のように
ロボット全体動作の処理を担う上位コントローラ
関節毎の動作の処理を担う下位コントローラ
（下位コントローラはロボット関節数に対応する分だけあります）
の２段構成をとることが多いです。
上位コントローラは、主に軌道計画や順/逆キネマティクス演算などのロボット全体動作に関わる処理を担当します。
これらは全関節情報を使う複雑な計算ですが、それほど高速な処理は求められません。
（上位コントローラのサンプリング周期は、おおむね1ms～10ms程度）
一方、下位コントローラは、主に関節の位置制御を担当します。
実行する処理は上位コントローラに比べてシンプルですが、高精度な位置決めを実現するために高速な処理が求められます。
（下位コントローラのサンプリング周期は、おおむね数十μs程度）
全ての処理を高速に実行できれば、コントローラを上位/下位に分割する必要はありません。
しかし、それをやると製造コストが上がります。
コストを考慮すると、担う役割と求められる処理速度で、コントローラを分割するのが合理的です。
こうしたわけで、多軸ロボットコントローラは上位/下位の２段構成になることが多いです。
（利益追及は大事ですからね）
ただ、
下位コントローラ側の処理で、上位コントローラ内のデータや他軸の情報を使いたい
逆に、上位コントローラ側の処理で、センサで取得したデータを使いたい
（基本的にロボット内のセンサと直接つながるのは下位コントローラ）
ということは、ままあります。
このため、上位コントローラと下位コントローラとは、相互にデータ通信をします。
（これがコントローラ間データ通信、ちなみに下位コントローラ同士が直接通信するケースは稀です）
コントローラ開発のとき、通信プロトコル（≒データの通信ルール）を適切に定めなければ、欲しいデータが使えず後で困ります。
気を付けましょう。
（不測に備えてマージンを持たせるのがセオリーなので、大抵は後からでも何とかなりますが）
3. ２リンクマニピュレータの運動方程式
最もシンプルな多関節ロボット（正確にはリンク機構）は２リンクマニピュレータです。
このため、今回は２リンクマニピュレータを具体例に説明します。
下図は、２リンクマニピュレータのイメージです。
（マニピュレータ＝ロボットアーム）
$$
\left(
\begin{array}{ll}
m_1&\text{：リンク１質量}\\
I_1&\text{：リンク１慣性モーメント}\\
L_1&\text{：リンク１長さ}\\
L_{g1}&\text{：リンク２重心位置}\\
m_2&\text{：リンク２質量}\\
I_2&\text{：リンク２慣性モーメント}\\
L_2&\text{：リンク２長さ}\\
L_{g2}&\text{：リンク２重心位置}\\
m_L&\text{：ツール質量}\\
I_L&\text{：ツール慣性モーメント}
\end{array}
\right)
$$
製造の現場では、ロボットアーム先端にツール（負荷）を取り付けずに動かすことはあまりないです。
このため、ツールを明確に考慮しています。
下式が２リンクマニピュレータの運動方程式です。
（行列形式で表現していますが、連立方程式でも問題ありません）
$$
\begin{array}{ll}
\left[
\begin{array}{c}
\tau_1\\
\tau_2
\end{array}
\right]
&=
\left[
\begin{array}{cc}
M_{11}&M_{12}\\
M_{21}&M_{22}
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}\\
\ddot{\theta_2}
\end{array}
\right]
+
\left[
\begin{array}{c}
C_1\\
C_2
\end{array}
\right]
+
\left[
\begin{array}{c}
G_1\\
G_2
\end{array}
\right]\\
&=M\cdot\ddot{\theta}+C+G
\end{array}
$$
$$
\begin{array}{ll}
M_{11}&=m_1\cdot {L_{g1}}^2+m_2\cdot({L_1}^2+{L_{g2}}^2+2L_1\cdot L_{g2}\cdot C\theta_2)\\
& +m_L\cdot({L_1}^2+{L_2}^2+2L_1\cdot L_2\cdot C\theta_2)+I_1+I_2+I_L\\
M_{12}&=M_{21}\\
&=m_2\cdot({L_{g2}}^2+L_1\cdot L_{g2}\cdot C\theta_2)+m_L\cdot({L_2}^2+L_1\cdot L_2\cdot C\theta_2)\\
M_{22}&=m_2\cdot {L_{g2}}^2+m_L\cdot {L_2}^2+I_2+I_L\\
\\
\\
C_1&=-2\dot{\theta_1}\cdot\dot{\theta_2}\cdot(m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1 \cdot S\theta_2\\
& -\dot{\theta_2}^2\cdot(m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1 \cdot S\theta_1\\
C_2&=-\dot{\theta_1}^2\cdot(m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1 \cdot S\theta_2\\
\\
\\
G_1&=\{m_1\cdot L_{g1}\cdot C\theta_1+m_2\cdot (L_1\cdot C\theta_1+L_{g2}\cdot C\theta_{12})\\
& +m_L\cdot (L_1\cdot C\theta_1+L_2\cdot C\theta_{12})\}\cdot g\\
G_2&=(m_2\cdot L_{g2}\cdot C\theta_{12}+m_L\cdot L_2\cdot C\theta_{12})\cdot g
\end{array}
$$
$$
\left(
\begin{array}{ccc}
S\theta_1=sin\theta_1&,&C\theta_1=cos\theta_1\\
S\theta_2=sin\theta_2&,&C\theta_2=cos\theta_2\\
S\theta_{12}=sin(\theta_1+\theta_2)&,&C\theta_{12}=cos(\theta_1+\theta_2)\\
\end{array}
\right)
$$
運動方程式中の
行列$${M}$$は慣性行列（$${M\cdot\ddot{\theta}}$$は慣性項）、
行列$${C}$$は遠心・コリオリ力項、
行列$${G}$$は重力項、
と呼ばれます。
特に、行列$${M}$$の対角成分（今回の場合は$${M_{11}}$$と$${M_{22}}$$）は主慣性と呼ばれます。
慣性行列は、
「対角成分を通る対角線を中心に線対称となる」
という性質を持つことが知られています。
この性質は、
導出した運動方程式の検算や、
コントローラの処理負荷軽減の工夫、
に利用できます。
なお、運動方程式はラグランジュの方法を用いて導出しました。
（詳細は付録を参照ください）
4. フィードバック型非干渉化制御
下図は、フィードバック型非干渉化制御のブロック線図です。
基本的には、軸毎に３重ループ制御をもつ構造をしています。
（図中では十分に高速高精度な電流制御がされると仮定して、電流制御部分は省略しています）
下式は、運動方程式から慣性項だけを抜き出したものです。
$$
\left[
\begin{array}{c}
\tau_1\\
\tau_2
\end{array}
\right]
=
\left[
\begin{array}{cc}
M_{11}&M_{12}\\
M_{21}&M_{22}
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}\\
\ddot{\theta_2}
\end{array}
\right]
$$
非干渉化制御とは、軸間干渉の影響を打ち消し、下式のように主慣性だけの状態に近づけることを目的とした制御方法です。
$$
\left[
\begin{array}{c}
\tau_1\\
\tau_2
\end{array}
\right]
=
\left[
\begin{array}{cc}
M_{11}&0\\
0&M_{22}
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}\\
\ddot{\theta_2}
\end{array}
\right]
$$
フィードバック型非干渉化制御は、$${\tau_1}$$に$${M_{12}\cdot\ddot{\theta_2}^{ref}}$$を、$${\tau_2}$$に$${M_{21}\cdot\ddot{\theta_1}^{ref}}$$を足し込むことで、軸間干渉の影響を抑制します。
（上図の制御構成だと、自然と速度PI制御の出力が加速度指令になります）
非干渉化制御時の運動方程式慣性項は下式のようになります。
$$
\left[
\begin{array}{c}
\tau_1+M_{12}\cdot\ddot{\theta_2}^{ref}\\
\tau_2+M_{21}\cdot\ddot{\theta_1}^{ref}
\end{array}
\right]
=
\left[
\begin{array}{cc}
M_{11}&M_{12}\\
M_{21}&M_{22}
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}\\
\ddot{\theta_2}
\end{array}
\right]
$$
$$
\begin{array}{ll}
\left[
\begin{array}{c}
\tau_1\\
\tau_2
\end{array}
\right]
&=
\left[
\begin{array}{cc}
M_{11}&M_{12}\\
M_{21}&M_{22}
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}\\
\ddot{\theta_2}
\end{array}
\right]
-
\left[
\begin{array}{cc}
0&M_{12}\\
M_{21}&0
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}^{ref}\\
\ddot{\theta_2}^{ref}
\end{array}
\right]\\
&=
\left[
\begin{array}{cc}
M_{11}&0\\
0&M_{22}
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}\\
\ddot{\theta_2}
\end{array}
\right]
+
\left[
\begin{array}{cc}
0&M_{12}\\
M_{21}&0
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}\\
\ddot{\theta_2}
\end{array}
\right]
-
\left[
\begin{array}{cc}
0&M_{12}\\
M_{21}&0
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}-\ddot{\theta_1}^{ref}\\
\ddot{\theta_2}-\ddot{\theta_2}^{ref}
\end{array}
\right]\\
&=
\left[
\begin{array}{cc}
M_{11}&0\\
0&M_{22}
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}\\
\ddot{\theta_2}
\end{array}
\right]
-
\left[
\begin{array}{cc}
0&M_{12}\\
M_{21}&0
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}-\ddot{\theta_1}^{ref}\\
\ddot{\theta_2}-\ddot{\theta_2}^{ref}
\end{array}
\right]\\
&\fallingdotseq
\left[
\begin{array}{cc}
M_{11}&0\\
0&M_{22}
\end{array}
\right]
\cdot
\left[
\begin{array}{c}
\ddot{\theta_1}\\
\ddot{\theta_2}
\end{array}
\right]
\end{array}
$$
非干渉化制御が正常に機能すれば$${\ddot{\theta_1}^{ref}\fallingdotseq\ddot{\theta_1}}$$，$${\ddot{\theta_2}^{ref}\fallingdotseq\ddot{\theta_2}}$$となり、上式中の近似が成り立ちます。
このようにして、軸間干渉の影響を抑制できます。
フィードバック型のメリットは、想定外の外乱に強いことです。
多少の外乱が入っても高精度なロボット動作が期待できます。
一方、デメリットは、１軸の制御に２軸分のデータが必要なことです。
（軸数が増えれば、それに伴って必要データ数が増えます）
コントローラ間データ通信の制約が厳しい場合、実現しづらいです。
5. フィードフォワード型非干渉化制御
下図は、フィードフォワード型非干渉化制御のブロック線図です。
フィードバック型と同様、軸毎に３重ループ制御をもつ構造をしています。
フィードバック型との主な違いは、位置指令$${\theta^{ref}(=\theta_{FF})}$$を直接２階微分した加速度フィードフォワード信号$${\ddot{\theta}_{FF}}$$を非干渉化制御に使う点です。
また、$${\theta_{FF}}$$と$${\dot{\theta}_{FF}}$$を位置制御出力と速度制御出力にそれぞれ足し込むのも、フィードバック型と違う点です。
フィードフォワード型のメリットは、比較的少ないデータで実現できることです。
コントローラ間データ通信の制約が厳しくとも実現しやすいです。
（上位コントローラから軸分の位置指令信号だけ受け取れば良いので）
また、速度・加速度のフィードフォワードによって、指令追従の高速化という副次的なメリットも得られます。
一方、デメリットは想定外の外乱に弱いことです。
外乱に対する反応は、非干渉化制御なしの場合と同等になります。
なお、$${\tau_1}$$に$${M_{12}\cdot\ddot{\theta}_{2FF}}$$を、$${\tau_2}$$に$${M_{21}\cdot\ddot{\theta}_{1FF}}$$を足し込むのと同じ要領で、遠心・コリオリ力項や重力項を足し込めば、それで遠心・コリオリ力や重力の影響を抑制できます。
（これらは、「遠心・コリオリ力補償」・「重力補償」と呼ばれます）
6. おわりに
この記事では、多関節ロボットの非干渉化制御を紹介・解説しました。
地味かもしれませんが、多関節ロボットの高速・高精度動作を実現するには有用な技術です。
なお、多関節ロボットでは、全て軸の位置制御応答周波数を同じ数値にそろえるというのも重要なポイントです。
そうしないと、アーム先端の軌跡に奇妙なうねりが生じることがあります。
ちなみに、平行リンク機構という構造を採用すれば、非干渉化制御なしで軸間干渉の影響を抑えられます。
ただ、そうすると製品はコストアップします。
コストアップなくロボット動作を高速・高精度化するのは、制御技術者のウデの見せどころです。
自分の技術をアピールできるチャンスです。
活用しましょう。
前回：応用編（４）「外乱オブザーバ」はこちら
次回：応用編（６）「ロボット関節のモデリング」はこちら
付録. ラグランジュの運動方程式
ラグランジュの運動方程式は、エネルギーに基づいて運動方程式を導出する方法です。
ラグランジュの方法とも呼ばれます。
この方法では、まずラグランジュ関数$${L}$$を導出する必要があります。
システム全体の運動エネルギーを$${K}$$、位置エネルギーを$${U}$$とすると、$${L}$$は下式のようになります。
$$
L=K-U
$$
回転型アクチュエータ（モータなど）を用いる場合、ラグランジュ関数$${L}$$から、下式のように運動方程式を求められます。
（$${\tau}$$はモータの発生するトルク）
$$
\tau=\cfrac{d}{dt}\left(\cfrac{\partial L}{\partial\dot{\theta}}\right)-\cfrac{\partial L}{\partial \theta}
$$
ここからは、２リンクマニピュレータを具体例として、ラグランジュの方法を説明します。
最初に、各リンク重心およびツールの位置・速度を求めておくと、後の流れがスムーズになります。
■リンク１重心のxy方向位置・速度
$$
\left\{
\begin{array}{ll}
x_{g1}=L_{g1}\cdot C\theta_1\\
y_{g1}=L_{g1}\cdot S\theta_1
\end{array}
\right.
$$
$$
\left\{
\begin{array}{ll}
v_{xg1}=-\dot{\theta}_1\cdot L_{g1}\cdot S\theta_1\\
v_{yg1}=\dot{\theta}_1\cdot L_{g1}\cdot C\theta_1
\end{array}
\right.
$$
■リンク２重心のxy方向位置・速度
$$
\left\{
\begin{array}{ll}
x_{g2}=L_{1}\cdot C\theta_1+L_{g2}\cdot C\theta_{12}\\
y_{g2}=L_{1}\cdot S\theta_1+L_{g2}\cdot S\theta_{12}
\end{array}
\right.
$$
$$
\left\{
\begin{array}{ll}
v_{xg2}=-\dot{\theta}_1\cdot L_1\cdot S\theta_1-(\dot{\theta}_1+\dot{\theta}_2)\cdot L_{g2}\cdot S\theta_{12}\\
v_{yg2}=\dot{\theta}_1\cdot L_1\cdot C\theta_1+(\dot{\theta}_1+\dot{\theta}_2)\cdot L_{g2}\cdot C\theta_{12}
\end{array}
\right.
$$
■ツールのxy方向位置・速度
$$
\left\{
\begin{array}{ll}
x_L=L_{1}\cdot C\theta_1+L_2\cdot C\theta_{12}\\
y_L=L_{1}\cdot S\theta_1+L_2\cdot S\theta_{12}
\end{array}
\right.
$$
$$
\left\{
\begin{array}{ll}
v_L=-\dot{\theta}_1\cdot L_1\cdot S\theta_1-(\dot{\theta}_1+\dot{\theta}_2)\cdot L_2\cdot S\theta_{12}\\
v_L=\dot{\theta}_1\cdot L_1\cdot C\theta_1+(\dot{\theta}_1+\dot{\theta}_2)\cdot L_2\cdot C\theta_{12}
\end{array}
\right.
$$
システム全体の運動エネルギーは下式になります。
（並進運動だけでなく、回転運動のエネルギーも考慮する必要があります）
$$
\begin{array}{ll}
K&=\cfrac{1}{2}\cdot m_1\cdot {v_{g1}}^2+\cfrac{1}{2}\cdot m_2\cdot {v_{g2}}^2+\cfrac{1}{2}\cdot m_L\cdot {v_L}^2\\
& +\cfrac{1}{2}\cdot (I_1+I_2+I_L)\cdot {\dot{\theta}_1}^2+\cfrac{1}{2}\cdot (I_2+I_L)\cdot {\dot{\theta}_2}^2
\end{array}
$$
xy方向速度と、運動エネルギー計算式中の速度との関係性は下図のようになります。
よって、三平方の定理から、$${{v_{g1}}^2,{v_{g2}}^2,{v_{L}}^2}$$は下式のように求められます。
$$
\begin{array}{ll}
v_{g1}^2&={v_{xg1}}^2+{v_{yg1}}^2\\
&={\dot{\theta}_1}^2\cdot {L_{g1}}^2\cdot({S\theta_1}^2+{C\theta_1}^2)\\
&={\dot{\theta}_1}^2\cdot {L_{g1}}^2
\end{array}
$$
$$
\begin{array}{ll}
v_{g2}^2&={v_{xg2}}^2+{v_{yg2}}^2\\
&={\dot{\theta}_1}^2\cdot {L_{1}}^2\cdot({S\theta_1}^2+{C\theta_1}^2)+({\dot{\theta}_1}^2+{\dot{\theta}_2}^2)\cdot {L_{g2}}^2\cdot({S\theta_{12}}^2+{C\theta_{12}}^2)\\
& +2\dot{\theta}_1\cdot(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_{g2}\cdot (C\theta_{12}\cdot C\theta_1+S\theta_{12}\cdot S\theta_1)\\
&={\dot{\theta}_1}^2\cdot {L_{1}}^2+(\dot{\theta}_1+\dot{\theta}_2)^2\cdot {L_{g2}}^2+2\dot{\theta}_1\cdot(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_{g2}\cdot C\theta_2
\end{array}
$$
（加法定理より、$${C\theta_{12}\cdot C\theta_1+S\theta_{12}\cdot S\theta_1}$$＝$${cos(\theta_1+\theta_2-\theta_1)}$$＝$${cos\theta_2}$$＝$${C\theta_2}$$）
$$
\begin{array}{ll}
v_L^2&={v_L}^2+{v_L}^2\\
&={\dot{\theta}_1}^2\cdot {L_{1}}^2\cdot({S\theta_1}^2+{C\theta_1}^2)+({\dot{\theta}_1}^2+{\dot{\theta}_2}^2)\cdot {L_2}^2\cdot({S\theta_{12}}^2+{C\theta_{12}}^2)\\
& +2\dot{\theta}_1\cdot(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_2\cdot (C\theta_{12}\cdot C\theta_1+S\theta_{12}\cdot S\theta_1)\\
&={\dot{\theta}_1}^2\cdot {L_{1}}^2+(\dot{\theta}_1+\dot{\theta}_2)^2\cdot {L_2}^2+2\dot{\theta}_1\cdot(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_2\cdot C\theta_2
\end{array}
$$
これにより、運動エネルギーは下式になります。
$$
\begin{array}{ll}
K&=\cfrac{1}{2}\cdot m_1\cdot {L_{g1}}^2\cdot{\dot{\theta}_1}^2\\
& +\cfrac{1}{2}\cdot m_2\cdot ({L_1}^2\cdot{\dot{\theta}_1}^2+{L_{g2}}^2\cdot{(\dot{\theta}_1+\dot{\theta}_2)}^2+2\dot{\theta}_1\cdot(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_{g2}\cdot C\theta_2)\\
& +\cfrac{1}{2}\cdot m_L\cdot ({L_1}^2\cdot{\dot{\theta}_1}^2+{L_2}^2\cdot{(\dot{\theta}_1+\dot{\theta}_2)}^2+2\dot{\theta}_1\cdot(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_2\cdot C\theta_2)\\
& +\cfrac{1}{2}\cdot (I_1+I_2+I_L)\cdot {\dot{\theta}_1}^2+\cfrac{1}{2}\cdot (I_2+I_L)\cdot {\dot{\theta}_2}^2
\end{array}
$$
また、システム全体の位置エネルギーは下式で求められます。
$$
\begin{array}{ll}
U&=m_1\cdot g\cdot y_{g1}+m_2\cdot g\cdot y_{g2}+m_L\cdot g\cdot y_L\\
&=(m_1\cdot y_{g1}+m_2\cdot y_{g2}+m_L\cdot y_L)\cdot g\\
&=(m_1\cdot L_{g1}\cdot S\theta_1+m_2\cdot L_{g2}\cdot S\theta_{12}+m_L\cdot L_2\cdot S\theta_{12})\cdot g
\end{array}
$$
さて、運動方程式をもう一度見返してみましょう。
$$
\begin{array}{ll}
\tau&=\cfrac{d}{dt}\left(\cfrac{\partial L}{\partial\dot{\theta}}\right)-\cfrac{\partial L}{\partial \theta}\\
&=\cfrac{d}{dt}\left(\cfrac{\partial (K-U)}{\partial\dot{\theta}}\right)-\cfrac{\partial (K-U)}{\partial \theta}\\
&=\cfrac{d}{dt}\left(\cfrac{\partial K}{\partial\dot{\theta}}\right)-\cfrac{\partial K}{\partial \theta}-\cfrac{d}{dt}\left(\cfrac{\partial U}{\partial\dot{\theta}}\right)+\cfrac{\partial U}{\partial \theta}
\end{array}
$$
位置エネルギー$${U}$$は、速度と無関係です。
このため、$${\partial U/\partial\dot{\theta}=0}$$です。
よって、運動方程式は下式で求められます。
$$
\tau=\cfrac{d}{dt}\left(\cfrac{\partial K}{\partial\dot{\theta}}\right)-\cfrac{\partial K}{\partial \theta}+\cfrac{\partial U}{\partial \theta}
$$
関節１についての運動方程式は、以下のように求められます。
$$
\begin{array}{ll}
\cfrac{\partial K}{\partial\dot{\theta}_1}&=m_1\cdot {L_{g1}}^2\cdot \dot{\theta}_1\\
& +m_2\cdot\{{L_1}^2\cdot \dot{\theta}_1+{L_{g2}}^2\cdot (\dot{\theta}_1+\dot{\theta}_2)+(2\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_{g2}\cdot C\theta_2\}\\
& +m_L\cdot\{{L_1}^2\cdot \dot{\theta}_1+{L_2}^2\cdot (\dot{\theta}_1+\dot{\theta}_2)+(2\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_2\cdot C\theta_2\}\\
& +(I_1+I_2+I_L)\cdot \dot{\theta}_1\\
&=\{m_1\cdot {L_{g1}}^2+m_2\cdot ({L_1}^2+{L_{g2}}^2+2\cdot L_1\cdot L_{g2}\cdot C\theta_2)\\
& +m_L\cdot ({L_1}^2+{L_2}^2+2\cdot L_1\cdot L_2\cdot C\theta_2)+I_1+I_2+I_L\}\cdot \dot{\theta}_1\\
& +\{m_2\cdot ({L_{g2}}^2+L_1\cdot L_{g2}\cdot C\theta_2)+m_L\cdot ({L_2}^2+L_1\cdot L_2\cdot C\theta_2)\}\cdot \dot{\theta}_2
\end{array}
$$
$$
\begin{array}{ll}
\cfrac{d}{dt}\left(\cfrac{\partial K}{\partial\dot{\theta}_1}\right)&=
\{m_1\cdot {L_{g1}}^2+m_2\cdot ({L_1}^2+{L_{g2}}^2+2\cdot L_1\cdot L_{g2}\cdot C\theta_2)\\
& +m_L\cdot ({L_1}^2+{L_2}^2+2\cdot L_1\cdot L_2\cdot C\theta_2)+I_1+I_2+I_L\}\cdot \ddot{\theta}_1\\
& +\{m_2\cdot ({L_{g2}}^2+L_1\cdot L_{g2}\cdot C\theta_2)+m_L\cdot ({L_2}^2+L_1\cdot L_2\cdot C\theta_2)\}\cdot \ddot{\theta}_2\\
& -2\dot{\theta}_1\cdot \dot{\theta}_2\cdot(m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1\cdot S\theta_2\\
& -{\dot{\theta}_2}^2\cdot (m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1\cdot S\theta_2
\end{array}
$$
$$
\cfrac{\partial K}{\partial\theta_1}=0
$$
$$
\begin{array}{ll}
\cfrac{\partial U}{\partial\theta_1}&=\{m_1\cdot _{g1}\cdot\ C\theta_1+m_2\cdot(L_1\cdot C\theta_1+L_{g2}\cdot C\theta_{12})\\
& +m_L\cdot(L_1\cdot C\theta_1+L_2\cdot C\theta_{12})\}\cdot g
\end{array}
$$
$$
\begin{array}{ll}
\tau_1&=\cfrac{d}{dt}\left(\cfrac{\partial K}{\partial\dot{\theta_1}}\right)-\cfrac{\partial K}{\partial \theta_1}+\cfrac{\partial U}{\partial \theta_1}\\
&=\{m_1\cdot {L_{g1}}^2+m_2\cdot ({L_1}^2+{L_{g2}}^2+2\cdot L_1\cdot L_{g2}\cdot C\theta_2)\\
& +m_L\cdot ({L_1}^2+{L_2}^2+2\cdot L_1\cdot L_2\cdot C\theta_2)+I_1+I_2+I_L\}\cdot \ddot{\theta}_1\\
& +\{m_2\cdot ({L_{g2}}^2+L_1\cdot L_{g2}\cdot C\theta_2)+m_L\cdot ({L_2}^2+L_1\cdot L_2\cdot C\theta_2)\}\cdot \ddot{\theta}_2\\
& -2\dot{\theta}_1\cdot \dot{\theta}_2\cdot(m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1\cdot S\theta_2\\
& -{\dot{\theta}_2}^2\cdot (m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1\cdot S\theta_2\\
& +\{m_1\cdot _{g1}\cdot\ C\theta_1+m_2\cdot(L_1\cdot C\theta_1+L_{g2}\cdot C\theta_{12})\\
& +m_L\cdot(L_1\cdot C\theta_1+L_2\cdot C\theta_{12})\}\cdot g
\end{array}
$$
関節２についての運動方程式は、以下のように求められます。
$$
\begin{array}{ll}
\cfrac{\partial K}{\partial\dot{\theta}_2}&=m_2\{{L_{g2}}^2\cdot (\dot{\theta}_1+\dot{\theta}_2)+\dot{\theta}_1\cdot L_1\cdot L_{g2}\cdot C\theta_2\}\\
& +m_L\{{L_2}^2\cdot (\dot{\theta}_1+\dot{\theta}_2)+\dot{\theta}_1\cdot L_1\cdot L_2\cdot C\theta_2\}+(I_2+I_L)\cdot \dot{\theta}_2\\
&=\{m_2\cdot({L_{g2}}^2+L_1\cdot L_{g2}\cdot C\theta_2)+m_L\cdot({L_2}^2+L_1\cdot L_2\cdot C\theta_2)\}\cdot \dot{\theta}_1\\
& +(m_2\cdot{L_{g2}}^2+m_L\cdot {L_2}^2+I_2+I_L)\cdot \dot{\theta}_2
\end{array}
$$
$$
\begin{array}{ll}
\cfrac{d}{dt}\left(\cfrac{\partial K}{\partial\dot{\theta_2}}\right)&=\{m_2\cdot({L_{g2}}^2+L_1\cdot L_{g2}\cdot C\theta_2)+m_L\cdot({L_2}^2+L_1\cdot L_2\cdot C\theta_2)\}\cdot \ddot{\theta}_1\\
& +(m_2\cdot{L_{g2}}^2+m_L\cdot {L_2}^2+I_2+I_L)\cdot \ddot{\theta}_2\\
& -\dot{\theta}_1\cdot\dot{\theta}_2\cdot(m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1\cdot S\theta_2
\end{array}
$$
$$
\begin{array}{ll}
\cfrac{\partial K}{\partial\theta_2}&=-m_2\cdot\dot{\theta}_1\cdot(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_{g2}\cdot S\theta_2\\
& -m_L\cdot\dot{\theta}_1\cdot(\dot{\theta}_1+\dot{\theta}_2)\cdot L_1\cdot L_2\cdot S\theta_2\\
&=-{\dot{\theta}_1}^2\cdot(m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1\cdot S\theta_2\\
& -\dot{\theta}_1\cdot\dot{\theta}_2\cdot(m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1\cdot S\theta_2
\end{array}
$$
$$
\begin{array}{ll}
\cfrac{\partial U}{\partial\theta_2}&=m_2\cdot g\cdot L_{g2}\cdot C\theta_{12}+m_L\cdot g\cdot L_2\cdot C\theta_{12}\\
&=(m_2\cdot L_{g2}\cdot C\theta_{12}+m_L\cdot L_2\cdot C\theta_{12})\cdot g
\end{array}
$$
$$
\begin{array}{ll}
\tau_2&=\cfrac{d}{dt}\left(\cfrac{\partial K}{\partial\dot{\theta_2}}\right)-\cfrac{\partial K}{\partial \theta_2}+\cfrac{\partial U}{\partial \theta_2}\\
&=\{m_2\cdot({L_{g2}}^2+L_1\cdot L_{g2}\cdot C\theta_2)+m_L\cdot({L_2}^2+L_1\cdot L_2\cdot C\theta_2)\}\cdot \ddot{\theta}_1\\
& +(m_2\cdot{L_{g2}}^2+m_L\cdot {L_2}^2+I_2+I_L)\cdot \ddot{\theta}_2\\
& +{\dot{\theta}_1}^2\cdot(m_2\cdot L_{g2}+m_L\cdot L_2)\cdot L_1\cdot S\theta_2\\
& +(m_2\cdot L_{g2}\cdot C\theta_{12}+m_L\cdot L_2\cdot C\theta_{12})\cdot g
\end{array}
$$
と、まぁ、２リンクマニピュレータくらいなら、ラグランジュの方法で運動方程式を導出できます。
しかし、２リンクマニピュレータの根本に旋回軸を仕込んだ３リンクマニピュレータにするだけでも、かなり数式が複雑化します。
（３×３の慣性テンソルを考慮する必要があるので）
６軸や７軸の垂直多関節ロボットになると、運動方程式を求めるのは非現実的と言えるほど大変な作業です。
そうした場合、ラグランジュの方法で解析解を得るより、ニュートン・オイラー法で数値解を得る方向に舵を切るのが効率的と思います。
