微分運動学を用いた逆運動学の解法を考える。
シリアルマニュピレータでは順運動学を幾何的に導くのは簡単だが、逆問題である逆運動学の式を幾何的に導くのは一般的に難しい。
そこで微分運動学を考える。微分運動学を考える理由は、非線形な順運動学の関係式を線形化できるため、逆行列計算（線形代数）で逆問題が解けるため。つまり非線形方程式の解法であるニュートン・ラプソン法（ひいてはレーベンバーグ・マーカート法）の形に持ち込めるということ。ちなみにヤコビ行列は正方行列（ないしは正則行列）であると限らないので、逆行列ではなく擬似逆行列を考える。
もう少し具体的にいうと、順運動学はr = f(q)の形をしている。rはエンドエフェクタの手先位置ベクトル。３次元でいうとr=[x,y,z]あるいはr=[x,y,z,Φ,θ,ψ]。
qは関節パラメータのベクトル。関節がすべてモータだとすると、q=[θ1,θ2,θ3,...]という感じ。r=f(q)の両辺を微分するとr' = J q'となる。rは手先位置の変位速度ベクトル、qは関節パラメータの変位速度ベクトル。注目すべきは、q'からr'の写像が非線形関数ではなく、行列J（ヤコビアン）とベクトルq'の乗算の形になっていること。これが逆運動学を代数的に解くことを可能にするトリック。
ちなみに幾何的に解ける問題であっても微分運動学で解くほうが楽に解ける。幾何的に解く場合、手先に近い関節θ3→θ2→θ1とさかのぼって計算していく必要があるが、微分運動学で代数的に解く場合、一発で関節パラメータベクトルqの修正量ベクトルΔqが求まるため、簡単である。関節パラメータが増えると効果を発揮しそう。
微分運動学による逆運動学では初期位置を適当に与えて解くため収束計算が必要になる。ただし、現在の位置に近い点を目的地として与え続けてロボットを動かす場合、目的地と現在地の誤差がそもそも小さいため、収束回数は１ループで済むと仮定できる。その場合、ヤコビアンの逆行列をフィードバックゲインとして誤差フィードック制御すれば、オンラインの制御アルゴリズムとして用いることができる。これは補間点を与え続けてロボットを動かすCP（Continuous Path）制御にうってつけである。
実際の微分運動学を用いた解法だが、まずはニュートン・ラフソン法から。使うのは修正量のこの式だけ。Wは重み行列。eは誤差ベクトルで、（目標値の手先位置ベクトル）-（現在の手先位置ベクトル）で定義される。
(J^T*W*J)^-1の逆行列計算で発散するのを避けるためにイプシロン項を付加したのがレーベンバーグ・マーカート法。ニュートン・ラプソン法の実装に微小項を付け加えるだけなので、手間はほとんどかからずに特異姿勢近くでの発散を避けることができる。
以下では２次元３DOFのロボットの例を考える。
ロボット図示
順運動学
以下が実装。シンプルな仕組みで逆運動学が求まるのがめっちゃ面白い。
（すべての関節位置のヤコビアンJ1,J2,J3を計算しているが、実際に使うのは手先位置のJ3だけで良い。）
clear; close all; syms L1 L2 L3; syms th1 th2 th3; x1 = L1*cos(th1); y1 = L1*sin(th1); x2 = L1*cos(th1)+L2*cos(th1+th2); y2 = L1*sin(th1)+L2*sin(th1+th2); x3 = L1*cos(th1)+L2*cos(th1+th2)+L3*cos(th1+th2+th3); y3 = L1*sin(th1)+L2*sin(th1+th2)+L3*sin(th1+th2+th3); J1 = [diff(x1, th1); diff(y1, th1)]; J2 = [diff(x2, th1) diff(x2, th2); diff(y2, th1) diff(y2, th2)]; J3 = [diff(x3, th1) diff(x3, th2) diff(x3, th3); diff(y3, th1) diff(y3, th2) diff(y3, th3)]; l1 = 1; l2 = 0.6; l3 = 0.8; %destination (px,py)=(2,1.5); px = 1.9; py = 1.5; % px = 0.9; % py = 1.2; W = eye(2);%x, yに対する重み行列 t1 = 0; t2 = 0; t3 = 0; epsilon = 0.1; e = 1e10;% big number (placeholder) while norm(e) > 0.1 %誤差ベクトルのノルムで評価 px3_tmp = vpa(subs(x3, ... [L1 L2 L3 th1 th2 th3], ... [l1 l2 l3 t1 t2 t3])); py3_tmp = vpa(subs(y3, ... [L1 L2 L3 th1 th2 th3], ... [l1 l2 l3 t1 t2 t3])); J3_tmp = vpa(subs(J3,... [L1 L2 L3 th1 th2 th3],... [l1 l2 l3 t1 t2 t3])); ex = px - px3_tmp; ey = py - py3_tmp; e = [ex;ey]; %誤差ベクトル dq = inv(transpose(J3_tmp)*W*J3_tmp+epsilon*eye(3))*... transpose(J3_tmp)*W*e; t1 = t1 + dq(1); t2 = t2 + dq(2); t3 = t3 + dq(3); norm(e) end x1i = vpa(subs(x1, ... [L1 th1], ... [l1 t1])); y1i = vpa(subs(y1, ... [L1 th1], ... [l1 t1])); x2i = vpa(subs(x2, ... [L1 L2 th1 th2], ... [l1 l2 t1 t2])); y2i = vpa(subs(y2, ... [L1 L2 th1 th2], ... [l1 l2 t1 t2])); x3i = vpa(subs(x3, ... [L1 L2 L3 th1 th2 th3], ... [l1 l2 l3 t1 t2 t3])); y3i = vpa(subs(y3, ... [L1 L2 L3 th1 th2 th3], ... [l1 l2 l3 t1 t2 t3])); cla; hold on; plot([0 x1i],[0 y1i],'k-'); plot([x1i x2i],[y1i y2i],'k-'); plot([x2i x3i],[y2i y3i],'k-'); plot(0,0,'ko','LineWidth',3); plot(x1i,y1i,'bo','LineWidth',3); plot(x2i,y2i,'go','LineWidth',3); plot(x3i,y3i,'ro','LineWidth',3); xlim([-1,3]); ylim([-1,3]); title(sprintf('destination x=%.2f,y=%.2f',px,py)); big;
