計算機器人運動學逆解首先要考慮可解性(solvability)，即考慮無解、多解等情況。在機器人工作空間外的目標點顯然是無解的。對於多解的情況從下面的例子可以看出平面二桿機械臂（兩個關節可以360°旋轉）在工作空間內存在兩個解：
如果逆運動學有多個解，那么控制程序在運行時就必須選擇其中一個解，然后發給驅動器驅動機器人關節旋轉或平移。如何選擇合適的解有許多不同的准則，其中一種比較合理的方法就是選擇“最近”的解（the closest solution）。如下圖所示，如果機器人在A點，並期望運動到B點，合理的解是關節運動量最小的那一個（A good choice would be the solution that minimizes the amount that each joint is required to move）。因此在不存在障礙物的情況下，上面的虛線構型會被選為逆解。在計算逆解時我們可以考慮將當前位置作為輸入參數，這樣我們就可以選擇關節空間中離當前位置最近的解。
這個“最近”有多種定義方式。比如對於典型的6自由度關節型機器人來說，其前三個關節較大，后三個關節較小。因此在定義關節空間內的距離遠近時要考慮給不同關節賦予不同的權重，比如前三個關節設置大權重，后三個關節設置小權重。那么在選擇解的時候會優先考慮移動較小的關節而非移動大關節。而當存在障礙物時，“最近”的解的運動路徑會與其發生碰撞，這時就要選擇另一個運動距離較遠的解（"farther" solution）。因此在考慮碰撞、路徑規划等問題時我們需要計算出可能存在的全部解。
逆解個數取決於機器人關節數目（the number of joints）、機器人的構型（link parameters）以及關節運動范圍(the allowable ranges of motion of the joints)。決定機器人構型的D-H參數表中的非零值越多，就有越多的解存在。對於通用型6軸轉動關節的機械臂來說，最多可能存在16個不同的解。下圖展示了最大解的數量與非零值的連桿長度參數$a$（兩關節轉軸之間的最短距離，即兩軸線之間公垂線的長度）的數量之間的關系：
另外機器人逆運動學求解也有多種方法，一般分為兩類：封閉解（closed-form solutions）和數值解（numerical solutions）。不同學者對同一機器人的運動學逆解也提出不同的解法。應該從計算方法的計算效率、計算精度等要求出發，選擇較好的解法。通常來說數值迭代解法比計算封閉解的解析表達式更慢、更耗時，因此在設計機器人的構型時就要考慮封閉解的存在性。
求解逆運動學方程的解析解（給出解的具體函數形式，從解的表達式中就可以算出任何對應值）時主要采用代數法（Algebraic solution）或幾何法（Geometric solution）。下面我們先用代數法來計算平面二連桿機械臂的運動學逆解（不考慮末端關節的旋轉）。正向運動學很容易得到：$$x=l_1c_1+l_2c_{12}\\y=l_1s_1+l_2s_{12}$$
將上面方程兩邊取平方再相加得到$x^2+y^2=l_1^2+l_2^2+2l_1l_2c_2$，消除$\theta_1$。這里用到了三角函數的和差角公式：$$c_{12}=c_1c_2-s_1s_2\\s_{12}=s_1c_2+c_1s_2$$
可以求得$\cos\theta_2$：$$c_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}$$
為了使解存在，上式的值必須在-1~1之間，因為余弦函數$\cos x$的取值范圍就是$[-1,1]$。在計算逆解時需要檢查這一條件，當不滿足時說明目標位置已經位於工作空間之外（the goal point is too far away for the manipulator to reach）。當目標位置$(x,y)$位於工作空間內時可以求得$\sin\theta_2$：$$s_2=\pm\sqrt{1-c_2^2}$$
為了計算$\theta_2$，可以使用Atan2函數，即：$$\theta_2=Atan2(s_2,c_2)$$
-
當 (x, y) 在第一象限, 0 < θ < PI/2
-
當 (x, y) 在第二象限 PI/2 < θ≤PI
-
當 (x, y) 在第三象限, -PI < θ < -PI/2
-
當 (x, y) 在第四象限, -PI/2 < θ < 0
$s_2$的符號有兩種選擇，對應的我們可以選擇"elbow-up"或"elbow-down"兩種不同構型。求出$\theta_2$后我們可以根據正解方程再計算出$\theta_1$。將正解方程改寫為$$x=k_1c_1-k_2s_1\\y=k_1s_1+k_2c_1$$
其中$$k_1=l_1+l_2c_2\\k_2=l_2s_2$$
為了求解方程對$k_1$、$k_2$進行變量替換：$$k_1=r\cos\gamma\\k_2=r\sin\gamma$$
其中$r=\sqrt{k_1^2+k_2^2}$，$\gamma=Atan2(k_2,k_1)$
於是正解方程可寫為：$$\frac{x}{r}=\cos\gamma\cos\theta_1-\sin\gamma\sin\theta_1\\ \frac{y}{r}=\cos\gamma\sin\theta_1+\sin\gamma\cos\theta_1$$
因此有：$$\cos(\gamma+\theta_1)=\frac{x}{r}\\ \sin(\gamma+\theta_1)=\frac{y}{r}$$
使用Atan2函數可得到：$\gamma+\theta_1=Atan2(y,x)$
於是第一個關節的轉角$\theta_1$為：$$\theta_1=Atan2(y,x)-Atan2(k_2,k_1)=Atan2(y,x)-Atan2(l_2s_2,l_1+l_2c_2)$$ 注意之前在求解$\theta_2$時對$s_2$的符號進行了選擇，這會引起$k_2$符號的變化，並影響$\theta_1$的求解。另外當x=y=0時，函數Atan2是未定義的狀態，這種情況下$\theta_1$可以任意取值。
- Geometric solution
根據機構平面圖，由$L_1$、$L_2$以及原點與末端之間的連線構成的三角形的余弦定理可求得$\theta_2$：$$x^2+y^2=l_1^2+l_2^2-2l_1l_2\cos(180^{\circ}+\theta_2)$$
由於$\cos(180^{\circ}+\theta_2)=-\cos(\theta_2)$，可解得：$$c_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}$$
為了保證三角形存在（三角形兩邊之和大於第三邊），即$\sqrt{x^2+y^2}$必須小於或等於連桿長度之和$l_1+l_2$。在求逆解時需要驗證是否滿足這一條件，判斷解的存在性。另一個可能的解（虛線所示）與之對稱，$\theta_2'=-\theta_2$
為了計算$\theta_1$，先求出圖中的$\beta$和$\psi$角。$\beta$可能位於坐標系四象限中的任一象限，取決於$x$和$y$的符號，因此使用Atan2函數來求解：$\beta=Atan2(y,x)$
對$\psi$用余弦定理來計算：$$\cos\psi=\frac{x^2+y^2+l_1^2-l_2^2}{2l_1\sqrt{x^2+y^2}}$$
於是$$\theta_1=\beta\pm\psi$$
當$\theta_2<0$時取正號，$\theta_2>0$時取負號
編寫Mathematica代碼進行平面二桿機械臂運動模擬，使用Locator來改變工作空間中機械臂末端的目標位置。
- Pieper's solution when three axes intersect
機器人運動學中的Pieper准則是：機器人的三個相鄰關節軸交於一點或三軸線平行。
對於6自由度的機器人來說，運動學反解非常復雜，一般沒有封閉解。在應用D-H法建立運動學方程的基礎上，進行一定的解析計算后發現，位置反解往往有很多個，不能得到有效地封閉解。Pieper方法就是在此基礎上進行研究發現，如果機器人滿足兩個充分條件中的一個，就會得到封閉解，這兩個條件是：
（1）三個相鄰關節軸相交於一點
（2）三個相鄰關節軸相互平行（在無限遠處交於一點）
現在的大多數商品化機器人都滿足封閉解的兩個充分條件之一。如PUMA和STANFORD機器人滿足第一條件，而ASEA和MINIMOVER機器人滿足第二條件。以PUMA560機器人為例，它的最后3個關節軸相交於一點。
下面考慮6自由度轉動關節機器人最后三根軸交於一點的情況。根據D-H參數坐標系建立方法，這種情況下桿件坐標系{4}、{5}、{6}的原點將位於三根軸線的交匯點。該點在機器人基座標系中的齊次坐標可表示為：$$^0P_{4ORG}=_{1}^{0}T \cdot_{2}^{1}T\cdot_{3}^{2}T\cdot^3P_{4ORG}=\begin{bmatrix}x\\ y\\ z\\ 1\end{bmatrix}\tag{1}$$ 或者根據相鄰桿件坐標系之間的變換關系：$$_{i}^{i-1}\textrm{T}=\begin{bmatrix}c\theta_i & -s\theta_i & 0 & a_{i-1}\\ s\theta_ic\alpha_{i-1} & c\theta_ic\alpha_{i-1} & -s\alpha_{i-1} & -s\alpha_{i-1}d_i\\ s\theta_is\alpha_{i-1} & c\theta_is\alpha_{i-1} & c\alpha_{i-1} & c\alpha_{i-1}d_i\\ 0 & 0 & 0 & 1\end{bmatrix}\tag{2}$$ 可以得到：$$^0P_{4ORG}=_{1}^{0}T \cdot_{2}^{1}T\cdot_{3}^{2}T\begin{bmatrix}a_3\\ -d_4s\alpha_3\\ d_4c\alpha_3\\ 1\end{bmatrix}\tag{3}$$ 或：$$^0P_{4ORG}=_{1}^{0}T \cdot_{2}^{1}T\begin{bmatrix}f_1(\theta_3)\\ f_2(\theta_3)\\ f_3(\theta_3)\\ 1\end{bmatrix}\tag{4}$$ 其中：$$\begin{bmatrix}f_1\\ f_2\\ f_3\\ 1\end{bmatrix}=_{3}^{2}T\begin{bmatrix}a_3\\ -d_4s\alpha_3\\ d_4c\alpha_3\\ 1\end{bmatrix}\tag{5}$$ 將參數帶入$_{i}^{i-1}T$矩陣得到$_{3}^{2}T$，可以列出$f_1$、$f_2$、$f_3$的表達式：$$\begin{align*}f_1&=a_3c_3+d_4s\alpha_3s_3+a_2\\ f_2&=a_3c\alpha_2s_3-d_4s\alpha_3c\alpha_2c_3-d_4s\alpha_2c\alpha_3-d_3s\alpha_2 \\ f_3&=a_3s\alpha_2s_3-d_4s\alpha_3s\alpha_2c_3+d_4c\alpha_2c\alpha_3+d_3c\alpha_2\end{align*}\tag{6}$$ 再根據 $_{1}^{0}T$和 $_{2}^{1}T$我們可以得到$$^0P_{4ORG}=\begin{bmatrix}c_1g_1-s_1g_2\\ s_1g_1+c_1g_2\\ g_3\\ 1\end{bmatrix}\tag{7}$$ 其中$$\begin{align*}g_1&=c_2f_1-s_2f_2+a_1 \\ g_2&=s_2c\alpha_1f_1+c_2c\alpha_1f_2-s\alpha_1f_3-d_2s\alpha_1 \\ g_3&=s_2s\alpha_1f_1+c_2s\alpha_1f_2+c\alpha_1f_3+d_2c\alpha_1 \end{align*}\tag{8}$$ 接下來我們計算$^0P_{4ORG}$坐標的平方和：$$r=x^2+y^2+z^2=g_1^2+g_2^2+g_3^2\tag{9}$$ 根據方程(8)有：$$r=f_1^2+f_2^2+f_3^2+a_1^2+d_2^2+2d_2f_3+2a_1(c_2f_1-s_2f_2)\tag{10}$$ 再接着進行變量替換消除對關節轉角$\theta_1$的依賴，令：$$\begin{align*}r&=(k_1c_2+k_2s_2)2a_1+k_3\\z&=(k_1s_2-k_2c_2)s\alpha_1+k_4 \end{align*}\tag{11}$$ 其中：$$\begin{align*}k_1&=f_1\\k_2&=-f_2\\k_3&=f_1^2+f_2^2+f_3^2+a_1^2+d_2^2+2d_2f_3\\k_4&=f_3c\alpha_1+d_2c\alpha_1\end{align*}\tag{12}$$ 下面考慮根據方程(11)求解$\theta_3$，分下面3種情況：
1. 如果$a_1=0$，則$r=k_3$，由於$r$是已知量，等式右邊的$k_3$是$\theta_3$的函數。用三角函數萬能公式進行變量替換后可求解$\theta_3$
2. 如果$s\alpha_1=0$，則$z=k_4$，由於$z$已知，進行變量替換后可求出$\theta_3$
3. 如果條件1、2不成立，則從公式(11)中消除$s_2$和$c_2$，得到：$$\frac{(r-k_3)^2}{4a_1^2}+\frac{(z-k_4)^2}{s^2\alpha_1}=k_1^2+k_2^2\tag{13}$$ 對方程(13)進行變量替換后可求解$\theta_3$，之后可根據方程(11)求解$\theta_2$，根據方程(7)求解$\theta_1$。
最后還需要求解$\theta_4$、$\theta_5$、$\theta_6$。機械臂最后三個關節的軸線交於一點，這三個關節的轉角會影響末端姿態，可以從代表末端姿態的旋轉矩陣$_6^0R$中求解出$\theta_4$、$\theta_5$、$\theta_6$。在之前求解得到$\theta_1$、$\theta_2$、$\theta_3$后我們可以計算出矩陣$_4^0R|_{\theta_4=0}$，表示$\theta_4=0$時桿件坐標系{4}相對於基坐標系的姿態。從坐標系{4}到末端坐標系{6}的姿態變化由最后三根軸決定，寫成矩陣變換的形式如下：$$_6^4R|_{\theta_4=0}=_4^0R^{-1}|_{\theta_4=0} \cdot_6^0R\tag{14}$$ 已知$_6^4R|_{\theta_4=0}$后最后三個關節轉角可根據歐拉角與旋轉矩陣之間的關系計算出來。
參考：
Introduction to Robotics - Mechanics and Control. Chapter 4 Inverse manipulator kinematics
Forward and Inverse Kinematics for Two-Link Arm
V-rep學習筆記：機器人逆運動學數值解法（The Jacobian Transpose Method）
V-rep學習筆記：機器人逆運動學數值解法（The Pseudo Inverse Method）
V-rep學習筆記：機器人逆運動學數值解法（Damped Least Squares / Levenberg-Marquardt Method）