機械臂運動學逆解（Analytical solution）

 　　計算機器人運動學逆解首先要考慮可解性(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)$$

　　注意對於tan(θ) = y / x ，兩種反正切函數的區別是：θ = ATan(y / x)求出的θ取值範圍是$(-\frac{\pi}{2},\frac{\pi}{2})$；θ = ATan2(y, x)求出的θ取值範圍是$(-\pi,\pi]$。

• 當 (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. 若是條件一、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學習筆記：機器人路徑規劃2

V-rep學習筆記：機器人逆運動學數值解法（The Jacobian Transpose Method）

V-rep學習筆記：機器人逆運動學數值解法（The Pseudo Inverse Method）

V-rep學習筆記：機器人逆運動學數值解法（Damped Least Squares / Levenberg-Marquardt Method）