ニュートン・ラフソン法

$$ \boldsymbol{f}(\boldsymbol{x}) = \boldsymbol{f}(\boldsymbol{x}_0) + \left( \dfrac{\partial{\boldsymbol{f}}}{\partial{\boldsymbol{x}}} \right) (\boldsymbol{x} - \boldsymbol{x}_0) + \cdots $$ ここで$J=\left( \dfrac{\partial{\boldsymbol{f}}}{\partial{\boldsymbol{x}}} \right)$はヤコビ行列.

簡略化して書くと $$ \boldsymbol{f}(\boldsymbol{x}) \simeq \boldsymbol{f}(\boldsymbol{x}_0) + J(\boldsymbol{x} - \boldsymbol{x}_0) $$

$\boldsymbol{f}(\boldsymbol{x}) = \boldsymbol{0}$となる$\boldsymbol{x}$を求める

1. まず$\boldsymbol{\theta}_0 = \left( \begin{array}{c} \theta_{0,1} \\ \theta_{0,2} \end{array}\right)$を開始状態とする．
2. $\boldsymbol{\theta}_i$の順運動学解$\boldsymbol{p}_i$を求める．
3. $\boldsymbol{p}_i$が$\boldsymbol{p}_T$に十分一致していれば終了．
   ある程度十分に近ければその差を$\Delta\boldsymbol{p}_i$とし，離れている場合はその方向の微小ステップを$\Delta\boldsymbol{p}_i$とする．
4. $\Delta\boldsymbol{\theta}_i=J^{-1}\Delta\boldsymbol{p}_i$を計算し，$\boldsymbol{\theta}_{i+1} = \boldsymbol{\theta}_i + \Delta\boldsymbol{\theta}_i$とする．
5. 終了するまで2から4を繰り返す．

終了時点での$\boldsymbol{\theta}_i$が$\boldsymbol{p}_T$の逆運動学解の数値解$\boldsymbol{\theta}_T$となっている．