0.前言
逆運動學，是通過操作空間（笛卡爾空間）的位姿，求解關機空間的關節參數的過程。把它放在雅克比後面，是因為有些逆運動學算法需要用到雅克比矩陣。處理逆運動學的方法很多，我們這裡介紹一些常用的方法。
1 可解性
操作臂的逆運動學求解是一個非線性問題。已知位姿矩陣T，求解對應的關節角θ1，θ2，..，θn的值。我們這裡得到的是一些很難解的非線性超越方程。我們關係解的存在性、多解性和求解方法。
1.1 存在性
如果逆運動學有解，則目標點坐標必定落在工作空間內。操作空間是末端執行器能作用到的空間範圍。機械臂的操作空間用兩個很有用的概念：可達操作空間和靈活操作空間。
● 靈活操作空間：是機器人終端執行器在所有方位都能達到的空間範圍。
● 可達操作空間：是機器人終端執行器至少在一個方位上能夠達到的空間方位。
當機器人少於6個自由度時，它不能在三維空間內獲得一般的目標位置和方向。
1.2 多解性
當多解存在時，就會產生一個問題，我們選誰。一般選取較近的解是比較合理的（下圖：左），但是有時會遇到障礙（下圖：右），一般需要計算出所有的解以供取捨。
通常解的數目取決於關節數目、關節參數和運動範圍。如PUMA560就可能有8組解（下圖給出四種）。
通常非零關節連桿參數越多，解會越多。全是旋轉關節的6自由度機器臂，最多有16個可能解。
2 解析法
解析法通過解方程的辦法求解關節角。通過這些方程轉化或者幾何關係，找出對應的多個限制關係就可以解出對應的關節變量。解析法一般都效率比較高，並且可以求解出所有的可能解。但是，因為這些方程式非線性超越方程，並不一定能求解出解析解。而且具體求解公式依機械臂具體構型相關，一般沒有通用性（構型不同時）。
2.1 代數法
代數法就是直接通過位姿矩陣T0，..Tn中的項進行組合，定義中間變量（關節參數的組合），構造一元高次方程，求解（可以通過24x24的矩陣），最後求出所有關節角。
這裡用上面R3機械臂做個栗子：
對於上述3R機械臂（3個旋轉關節記為3R），通過DH法可以得到末端位姿和各關節的角度關係：
其中：x、y為平面坐標，φ代表末端方向和x0軸的夾角，將對應的變量聯立：
求解上面方方程這裡不再展開了，使用和角公式求解出對應的關節角度即可，計算中需要用到下面函數：
因為旋轉一周是360度，使用三角函數的反函數值域只有一半，因此這裡使用擴展的atan2函數。
2.2 幾何法（向量法）
我們試著把機械臂的空間幾何問題轉化為若干平面幾何問題。當多個連桿在一個平面中時，會出現關節角的和角形式，可以通過正餘弦定理建立條件求解。
同樣用上面R3的機械臂做個栗子：
這裡整個圖形都處在一個平面中，利用第三個關節的位置，通過餘弦定理得到下面方程：
可以求解θ2，再通過餘弦定理得到下面方程：
通過觀察可以計算出β：
這樣我們就可以求解θ1，通過三個角度和為末端姿態，可以求解出θ3。
多數情況下我們使用代數和幾何組合的方法求解。當使用幾何法求解釋，有時只要獲得第一個解就可以通過對稱性求解出其他解。
2.3 pipper準則
pipper給出了一些可以求解解析解的情況。有6個旋轉關節的機械臂，具有封閉解的充分條件是三個相鄰關節交於一點（一般是末端，這樣可以位置和姿態分開求解，簡化運算）。大多數的工業機器人都符合這一準則。
這裡我就不舉例求解了，求解過程中可以將位置和姿態關節的其次矩陣分開：T1T2T3，T4T5T6。然後通過上面的代數和幾何法求解。
3.數值法
逆運動學的數值解法，一般的書上討論的不多，相關論文比較多，這部分主要是參考論文和課件中的資料。
數值方法一般採用的都是迭代的方法，使計算出來的角度值不斷地逼近目標值。
迭代一般有三種模型：
● 每次運動控制，只迭代一次（可能距離目標點很遠），然後進行控制，然後進入下次迭代；適合距離較遠的兩點；
● 每次運動控制，一直迭代到距離目標值的閾值範圍內，然後進行控制，然後進入下次迭代；適合距離較近的兩點，如果距離較遠需要先計算中一些中間點；
● 每次運動控制，迭代固定次數（不考慮到目標距離），然後進行控制，然後進入下次迭代；（一般比較近的點迭代2~5次就可以計算到很高精度）
3.1 雅克比逆
因為雅克比是速度间的映射，分別在笛卡爾空間和關節空間對速度積分有：
因為我們在控制時使用的都是離散變量所以寫成如下形式：
通過求解雅克比矩陣的逆，我們就可以求出關節增量：
利用 θ = θ + △θ進行迭代。這裡的雅克比矩陣可以使用當前或者目標位置計算（目標位置的雅克比或出現較好效果）。
利用雅克比矩陣忽略了高階的無窮小量，奇異時非線性項不是可以忽略的部分，因此這種方法無法解決奇異時的情況。奇異附近的速度也會出現巨大比例的放縮。
3.2 雅克比轉置
上面的方法，在雅克比不是方陣或者雅克比不可逆的時候就無法進行求解，因此需要進行修正。
這裡使用雅克比的轉置替換雅克比的逆即可，成立的原因如下（設F為最小代價函數）：
求梯度下降方向：
這裡α可以取值如下（e為笛卡爾末端的偏差，這裡取的比例為偏差的投影）：
3.3 雅克比偽逆
一般情況下使用雅克比轉置效果並不好，很有肯恩不收斂或者收斂的太慢。
這裡使用廣義逆矩陣的一種（具體定義可以參考線性代數和矩陣論書籍）。通過下面代價函數得到：
得到的偽逆為：
這裡還有一個重要性質：
這裡後面的部分投影到雅克比矩陣的零空間，所以可以選取任意的φ（代價函數在零空間投影）進行不同的目標控制（可以對奇異構型或則冗餘關節進行限制）。
3.4 最小阻尼二乘（DLS）
上面的方法可以達到收斂，並且在奇異值附近效果也不錯，但是不能處理奇異的狀態（在不使用零空間時）。
構造代價函數如下：
其中λ 為非零阻尼係數，整理后的方程如下：
上式前半部分是偽逆，後半部分是轉置。這種方法的缺點是需要確定阻尼係數，一般選取奇異值分解后的最大值和最小值的比值（除了0），這樣的話計算量就會較大。
3.5 奇異值分解（SVD）分析
奇異值分解是非常強大的工具，這裡使用奇異值分解來觀察偽逆和DLS。
將雅克比矩陣（nxm）奇異值分解如下：
其中U為nxn的正交單位陣，V為mxm的正交單位陣，D是對角陣，其中對角線的元素為J的特征值：
設J的秩為r，則大於r的列投影到零空間。將U，V寫成列向量ui和vi則：
通過同樣地方法求出，偽逆的SVD為：
通過同樣地方法求出，DLS的SVD為：
通過上面的式子可以知道為什麼偽逆不能處理奇異狀態，而DLS卻可以。
通過不同的代價函數對應SVD下不同的特征值，也等價于在零空間選取不同的φ，這些方法都是相似的。在奇異或冗餘情況下（降維，有些維度投影到零空間），他們構成了新的約束（不加入約束條件方程無法求解）。
3.6 改進方案
當目標距離處於工作空間之外或者距離當前位置較遠時，會出現震動現象。
對弈上面個每種方法，這裡有一個通用的優化：對每次移動的偏差上限進行限制。
將每次迭代使用的偏差值，定義如下：
其中：
這裡的di為上次移動的距離，初始d0為無窮大，Dmax對應取關節長度的一般（每次行走的最大步長不超過30度）。
4 CCD（Cyclic Coordinate Descent）
在計算機仿真中經常使用的一種發發是CCD算法，這種算伐對每個關節獨立計算，在關節空間求解，避免了奇異的情況。
如上圖所示，計算過程沒建立求解關節和末端位置的向量r，求解關節和目標位置的向量g通過兩個向量的夾角求解對應的運動角度（可以使用內積求解）。
不過這種方法在處理多關節時，就不太好用了，計算量會變得比較大，控制性能也不是很好。
5 示教
很多工業機器人使用示教的方法運動到目標點。示教過程中，用手拖動機器人使其到達目標位姿，通過傳感器記錄路徑中間的關節坐標（稱為示教點），然後（可能利用插補）依次執行對應的關節角序列即可。這裡實在關節空間控制，不涉及逆運動學問題（不涉及任務空間）。這樣就涉及到重複定位精度。
如果任務規定在直角坐標系內，就需要使用逆運動學求解關節角，這樣可以到達未經過示教的點（稱為計算點）。機械臂能到達計算點的精確程度涉及到定位精度。
為了提升對應精度就涉及到參數識別和校準技術。
6 例子
這裡利用一個3R機械臂做個例子如下圖（左圖為初始位置，右圖為目標位置）： 
對應的各關節運動如下：
代碼如下（其中有動畫）：
%% 定義參數 關節數+DH參數
JOINT_SIZE = 3+1;
a = [2, 2, 2]';
alpha = [0, 0, 0]'*pi/180.0;
d = [0, 0, 0]';
cta = [30, 30, 30]'*pi/180.0;
%% 定義數組
T = cell(JOINT_SIZE);
R = cell(JOINT_SIZE);
P = cell(JOINT_SIZE);
%% 計算正運動學
T{1} = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
P{1} = T{1}(1:3, 4);
R{1} = T{1}(1:3, 1:3);
for k=2:JOINT_SIZE
T{k} = T{k-1}*DH(a(k-1), alpha(k-1), d(k-1), cta(k-1));
P{k} = T{k}(1:3, 4);
R{k} = T{k}(1:3, 1:3);
end
% 繪圖
figure(1)
clf;
DrawCoordinate('0', P{1}, R{1});
for k=2:JOINT_SIZE
DrawLine(P{k-1}, P{k});
DrawCylinder(P{k-1}, R{k-1});
DrawCoordinate('0'+k-1, P{k}, R{k});
end
axis equal; % 顯示坐標軸比例
view(0,90); % 指定子图1的视点
%% 迭代法求逆解
target = [6/sqrt(2.0); 6/sqrt(2.0); 45*pi/180.0]; % 目標點
PATH_SIZE = 20;
save_cta = zeros(3,PATH_SIZE);
for i=1:PATH_SIZE
save_cta(1:3,i) = cta;
% 誤差
error = [target(1) - P{JOINT_SIZE}(1); target(2) - P{JOINT_SIZE}(2); target(3) - (cta(1)+cta(2)+cta(3))];
% 雅克比，手動求偏導
Jacob0 = [-a(1)*sin(cta(1))-a(2)*sin(cta(1)+cta(2))-a(3)*sin(cta(1)+cta(2)+cta(3)) -a(2)*sin(cta(1)+cta(2))-a(3)*sin(cta(1)+cta(2)+cta(3)) -a(3)*sin(cta(1)+cta(2)+cta(3))
a(1)*cos(cta(1))+a(2)*cos(cta(1)+cta(2))+a(3)*cos(cta(1)+cta(2)+cta(3)) a(2)*cos(cta(1)+cta(2))+a(3)*cos(cta(1)+cta(2)+cta(3)) a(3)*cos(cta(1)+cta(2)+cta(3))
1 1 1];
% 逆解迭代
cta = cta + pinv(Jacob0)*error;
% DH法正解
T{1} = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
P{1} = T{1}(1:3, 4);
R{1} = T{1}(1:3, 1:3);
for k=2:JOINT_SIZE
T{k} = T{k-1}*DH(a(k-1), alpha(k-1), d(k-1), cta(k-1));
P{k} = T{k}(1:3, 4);
R{k} = T{k}(1:3, 1:3);
end
% 数据更新
P_tool = T{JOINT_SIZE}(1:3, 4);
R_tool = T{JOINT_SIZE}(1:3, 1:3);
% 绘图
figure(2)
clf;
DrawCoordinate('0', P{1}, R{1});
for k=2:JOINT_SIZE
DrawLine(P{k-1}, P{k});
DrawCylinder(P{k-1}, R{k-1});
DrawCoordinate('0'+k-1, P{k}, R{k});
end
axis equal; % 顯示坐標軸比例
view(0,90); % 指定子图1的视点
drawnow;
end
figure(3)
subplot(2, 2, 1);
plot(1:PATH_SIZE, save_cta(1,1:PATH_SIZE));
title('关节1位置(弧度)');
subplot(2, 2, 2);
plot(1:PATH_SIZE, save_cta(2,1:PATH_SIZE));
title('关节2位置(弧度)');
subplot(2, 2, 3);
plot(1:PATH_SIZE, save_cta(3,1:PATH_SIZE));
title('关节3位置(弧度)');7 參考文獻
《机器人学导论：分析控制及应用》（美）尼库 著
《机器人学导论》（美）克来格 著
《机器人学导论》（美）约翰 J 卡雷格 著
《机器人学：建模、规划与控制》布鲁诺·西西里安诺、洛伦索·夏维科、路易吉·维拉尼 、朱塞佩·奥里奥洛 著
《Springer Handbook of Robotics》布鲁诺·西西里安诺、歐莎瑪 哈提卜