機械臂運動學逆解（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個不同的解。下圖展示了最大解的數量與非零值的連桿長度參數aa （兩關節轉軸之間的最短距離，即兩軸線之間公垂線的長度）的數量之間的關系：

​

　　另外機器人逆運動學求解也有多種方法，一般分為兩類：封閉解（closed-form solutions）和數值解（numerical solutions）。不同學者對同一機器人的運動學逆解也提出不同的解法。應該從計算方法的計算效率、計算精度等要求出發，選擇較好的解法。通常來說數值迭代解法比計算封閉解的解析表達式更慢、更耗時，因此在設計機器人的構型時就要考慮封閉解的存在性。

 　　求解逆運動學方程的解析解（給出解的具體函數形式，從解的表達式中就可以算出任何對應值）時主要采用代數法（Algebraic solution）或幾何法（Geometric solution）。下面我們先用代數法來計算平面二連桿機械臂的運動學逆解（不考慮末端關節的旋轉）。正向運動學很容易得到：

x=l1c1+l2c12y=l1s1+l2s12x=l1c1+l2c12y=l1s1+l2s12

 

　　將上面方程兩邊取平方再相加得到x2+y2=l21+l22+2l1l2c2x2+y2=l12+l22+2l1l2c2 ，消除θ1θ1 。這里用到了三角函數的和差角公式：

c12=c1c2−s1s2s12=s1c2+c1s2c12=c1c2−s1s2s12=s1c2+c1s2

 

　　可以求得cosθ2cos⁡θ2 ：

c2=x2+y2−l21−l222l1l2c2=x2+y2−l12−l222l1l2

 

　　為了使解存在，上式的值必須在-1~1之間，因為余弦函數cosxcos⁡x 的取值范圍就是[−1,1][−1,1] 。在計算逆解時需要檢查這一條件，當不滿足時說明目標位置已經位於工作空間之外（the goal point is too far away for the manipulator to reach）。當目標位置(x,y)(x,y) 位於工作空間內時可以求得sinθ2sin⁡θ2 ：

s2=±1−c22−−−−−√s2=±1−c22

 

　　為了計算θ2θ2 ，可以使用Atan2函數，即：

θ2=Atan2(s2,c2)θ2=Atan2(s2,c2)

 

　　注意對於tan(θ) = y / x ，兩種反正切函數的區別是：θ = ATan(y / x)求出的θ取值范圍是(−π2,π2)(−π2,π2) ；θ = ATan2(y, x)求出的θ取值范圍是(−π,π](−π,π] 。

• 當 (x, y) 在第一象限, 0 < θ < PI/2

• 當 (x, y) 在第二象限 PI/2 < θ≤PI

• 當 (x, y) 在第三象限, -PI < θ < -PI/2

• 當 (x, y) 在第四象限, -PI/2 < θ < 0

　　s2s2 的符號有兩種選擇，對應的我們可以選擇"elbow-up"或"elbow-down"兩種不同構型。求出θ2θ2 后我們可以根據正解方程再計算出θ1θ1 。將正解方程改寫為

x=k1c1−k2s1y=k1s1+k2c1x=k1c1−k2s1y=k1s1+k2c1

 

歡迎光臨程序代寫小店https://item.taobao.com/item.htm?spm=a230r.1.14.59.255028c3ALNkZ0&id=586797758241&ns=1&abbucket=15#detail

可直接聯系客服QQ交代需求：953586085

歡迎點擊鏈接加入群聊【程序代寫-接單群】共同致富：https://jq.qq.com/?_wv=1027&k=5WxihsL 

群號：733065427

　　其中

k1=l1+l2c2k2=l2s2k1=l1+l2c2k2=l2s2

 

　　為了求解方程對k1k1 、k2k2 進行變量替換：

k1=rcosγk2=rsinγk1=rcos⁡γk2=rsin⁡γ

 

　　其中r=k21+k22−−−−−−√r=k12+k22 ，γ=Atan2(k2,k1)γ=Atan2(k2,k1)

　　於是正解方程可寫為：

xr=cosγcosθ1−sinγsinθ1yr=cosγsinθ1+sinγcosθ1xr=cos⁡γcos⁡θ1−sin⁡γsin⁡θ1yr=cos⁡γsin⁡θ1+sin⁡γcos⁡θ1

 

　　因此有：

cos(γ+θ1)=xrsin(γ+θ1)=yrcos⁡(γ+θ1)=xrsin⁡(γ+θ1)=yr

 

　　使用Atan2函數可得到：γ+θ1=Atan2(y,x)γ+θ1=Atan2(y,x)

　　於是第一個關節的轉角θ1θ1 為：

θ1=Atan2(y,x)−Atan2(k2,k1)=Atan2(y,x)−Atan2(l2s2,l1+l2c2)θ1=Atan2(y,x)−Atan2(k2,k1)=Atan2(y,x)−Atan2(l2s2,l1+l2c2)

　　注意之前在求解θ2θ2 時對s2s2 的符號進行了選擇，這會引起k2k2 符號的變化，並影響θ1θ1 的求解。另外當x=y=0時，函數Atan2是未定義的狀態，這種情況下θ1θ1 可以任意取值。

 

• Geometric solution

　　根據機構平面圖，由L1L1 、L2L2 以及原點與末端之間的連線構成的三角形的余弦定理可求得θ2θ2 ：

x2+y2=l21+l22−2l1l2cos(180∘+θ2)x2+y2=l12+l22−2l1l2cos⁡(180∘+θ2)

 

​

　　由於cos(180∘+θ2)=−cos(θ2)cos⁡(180∘+θ2)=−cos⁡(θ2) ，可解得：

c2=x2+y2−l21−l222l1l2c2=x2+y2−l12−l222l1l2

 

　　為了保證三角形存在（三角形兩邊之和大於第三邊），即x2+y2−−−−−−√x2+y2 必須小於或等於連桿長度之和l1+l2l1+l2 。在求逆解時需要驗證是否滿足這一條件，判斷解的存在性。另一個可能的解（虛線所示）與之對稱，θ′2=−θ2θ2′=−θ2

　　為了計算θ1θ1 ，先求出圖中的ββ 和ψψ 角。ββ 可能位於坐標系四象限中的任一象限，取決於xx 和yy 的符號，因此使用Atan2函數來求解：β=Atan2(y,x)β=Atan2(y,x)

　　對ψψ 用余弦定理來計算：

cosψ=x2+y2+l21−l222l1x2+y2−−−−−−√cos⁡ψ=x2+y2+l12−l222l1x2+y2

 

　　於是

θ1=β±ψθ1=β±ψ

 

　　當θ2<0θ2<0 時取正號，θ2>0θ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}的原點將位於三根軸線的交匯點。該點在機器人基座標系中的齊次坐標可表示為：

0P4ORG=01T⋅12T⋅23T⋅3P4ORG=⎡⎣⎢⎢⎢xyz1⎤⎦⎥⎥⎥(1)(1)0P4ORG=10T⋅21T⋅32T⋅3P4ORG=[xyz1]

　　或者根據相鄰桿件坐標系之間的變換關系：

i−1iT=⎡⎣⎢⎢⎢cθisθicαi−1sθisαi−10−sθicθicαi−1cθisαi−100−sαi−1cαi−10ai−1−sαi−1dicαi−1di1⎤⎦⎥⎥⎥(2)(2)ii−1T=[cθi−sθi0ai−1sθicαi−1cθicαi−1−sαi−1−sαi−1disθisαi−1cθisαi−1cαi−1cαi−1di0001]

　　可以得到：

0P4ORG=01T⋅12T⋅23T⎡⎣⎢⎢⎢a3−d4sα3d4cα31⎤⎦⎥⎥⎥(3)(3)0P4ORG=10T⋅21T⋅32T[a3−d4sα3d4cα31]

　　或：

0P4ORG=01T⋅12T⎡⎣⎢⎢⎢⎢f1(θ3)f2(θ3)f3(θ3)1⎤⎦⎥⎥⎥⎥(4)(4)0P4ORG=10T⋅21T[f1(θ3)f2(θ3)f3(θ3)1]

　　其中：

⎡⎣⎢⎢⎢f1f2f31⎤⎦⎥⎥⎥=23T⎡⎣⎢⎢⎢a3−d4sα3d4cα31⎤⎦⎥⎥⎥(5)(5)[f1f2f31]=32T[a3−d4sα3d4cα31]

　　將參數帶入i−1iTii−1T 矩陣得到23T32T ，可以列出f1f1 、f2f2 、f3f3 的表達式：

f1f2f3=a3c3+d4sα3s3+a2=a3cα2s3−d4sα3cα2c3−d4sα2cα3−d3sα2=a3sα2s3−d4sα3sα2c3+d4cα2cα3+d3cα2(6)(6)f1=a3c3+d4sα3s3+a2f2=a3cα2s3−d4sα3cα2c3−d4sα2cα3−d3sα2f3=a3sα2s3−d4sα3sα2c3+d4cα2cα3+d3cα2

　　再根據 01T10T 和 12T21T 我們可以得到

0P4ORG=⎡⎣⎢⎢⎢c1g1−s1g2s1g1+c1g2g31⎤⎦⎥⎥⎥(7)(7)0P4ORG=[c1g1−s1g2s1g1+c1g2g31]

　　其中

g1g2g3=c2f1−s2f2+a1=s2cα1f1+c2cα1f2−sα1f3−d2sα1=s2sα1f1+c2sα1f2+cα1f3+d2cα1(8)(8)g1=c2f1−s2f2+a1g2=s2cα1f1+c2cα1f2−sα1f3−d2sα1g3=s2sα1f1+c2sα1f2+cα1f3+d2cα1

　　接下來我們計算0P4ORG0P4ORG 坐標的平方和：

r=x2+y2+z2=g21+g22+g23(9)(9)r=x2+y2+z2=g12+g22+g32

　　根據方程(8)有：

r=f21+f22+f23+a21+d22+2d2f3+2a1(c2f1−s2f2)(10)(10)r=f12+f22+f32+a12+d22+2d2f3+2a1(c2f1−s2f2)

　　再接着進行變量替換消除對關節轉角θ1θ1 的依賴，令：

rz=(k1c2+k2s2)2a1+k3=(k1s2−k2c2)sα1+k4(11)(11)r=(k1c2+k2s2)2a1+k3z=(k1s2−k2c2)sα1+k4

　　其中：

k1k2k3k4=f1=−f2=f21+f22+f23+a21+d22+2d2f3=f3cα1+d2cα1(12)(12)k1=f1k2=−f2k3=f12+f22+f32+a12+d22+2d2f3k4=f3cα1+d2cα1

　　下面考慮根據方程(11)求解θ3θ3 ，分下面3種情況：

　　1. 如果a1=0a1=0 ，則r=k3r=k3 ，由於rr 是已知量，等式右邊的k3k3 是θ3θ3 的函數。用三角函數萬能公式進行變量替換后可求解θ3θ3 　　

　　2. 如果sα1=0sα1=0 ，則z=k4z=k4 ，由於zz 已知，進行變量替換后可求出θ3θ3

　　3. 如果條件1、2不成立，則從公式(11)中消除s2s2 和c2c2 ，得到：

(r−k3)24a21+(z−k4)2s2α1=k21+k22(13)(13)(r−k3)24a12+(z−k4)2s2α1=k12+k22

　　對方程(13)進行變量替換后可求解θ3θ3 ，之后可根據方程(11)求解θ2θ2 ，根據方程(7)求解θ1θ1 。

　　最后還需要求解θ4θ4 、θ5θ5 、θ6θ6 。機械臂最后三個關節的軸線交於一點，這三個關節的轉角會影響末端姿態，可以從代表末端姿態的旋轉矩陣06R60R 中求解出θ4θ4 、θ5θ5 、θ6θ6 。在之前求解得到θ1θ1 、θ2θ2 、θ3θ3 后我們可以計算出矩陣04R|θ4=040R|θ4=0 ，表示θ4=0θ4=0 時桿件坐標系{4}相對於基坐標系的姿態。從坐標系{4}到末端坐標系{6}的姿態變化由最后三根軸決定，寫成矩陣變換的形式如下：

46R|θ4=0=04R−1|θ4=0⋅06R(14)(14)64R|θ4=0=40R−1|θ4=0⋅60R

　　已知46R|θ4=064R|θ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）