機械腿逆運動學模型求解

胡平志, 李澤滔

(貴州大學 電氣工程學院, 貴陽550025)

0 引 言

近年來,越來越多機器人被廣泛應用。 輪式機器人不能適應復雜的地形環境,飛行機器人雖然能無視地形因素,但是載重量有限,成本極高。 隨著機器人技術的迅猛發展,對能在復雜環境下行走的特種機器人的需求日益增加。 本文以一種十二自由度機器人為研究對象,該機器人實際包含四個三自由度機械腿,其關于機器人幾何軸心中心對稱式排布。機器人的行走,實際可看作是四條機械腿運動的線性組合,所以為了研究機器人行走,就需要優先研究一條機械腿的模型與控制。

對于單腿建模,學術上通用DH 矩陣法進行建模［1］,該模型被稱為運動學正模型,可由每個關節的角度求取末端點的三維空間位置。 但是,實際應用中往往需要利用末端點三維坐標,逆向求取每個關節的角度,這就需要求取運動學逆模型。 然而逆模型方程往往是多元高次方程組,解析解難以求取,甚至不存在。 多自由度機械腿控制,采用建立運動學方程, 由于逆運動學方程中存在交叉項sin (θi) , cos (θi),所以通常是多元高次方程組,解析解求取困難甚至不存在,傳統的解決逆問題的辦法有兩種：(1)利用多特殊點采樣,再線性插值擬合的辦法；(2)特殊值求解；方法(1)的不足之處在于,進行高精度的線性擬合,需要極大數量的特殊點,由于只能依靠正向模型計算所得的特殊點,其空間位置坐標是隨機的,絕大多數點不在期望可行域范圍內,不具備實際使用價值。 方法(2)的不足之處在于,所謂特殊值求解,一般而言是將多個關節進行合并,即令θi= C(常數),來對方程進行簡化,或者利用機械臂結構上的特殊性進行合并化簡［2］,本質上使得高自由度機械腿變為低自由度機械腿。

近年來也出現很多利用智能算法,對逆運動學方程進行求解的論文,這些算法包括,改進粒子群算法［3］,改進神經網絡算法［4］等。 但是實際使用中這些算法存在運算量巨大,求解速度慢,求解結果波動范圍大的問題。 為更好的解決這一問題,本文提出了一種基于迭代法的方式求解逆模型數值的方法,該方法收斂性好,收斂速度快,準確度高,運算量小,適用范圍廣。

1 建立坐標系

圖1(a)是機械腿的3d 模型,圖1(b)是機械腿的3d 模型主視圖,紅線是轉軸與轉軸或者轉軸與端點之間的垂直距離連線,將這些連線抽象成為連桿,即得到圖1(c)。 其中各參數實際取值如表1 所示。

圖1 三自由度機器人實物抽象模型Fig. 1 Three-degree-of-freedom-robot-leg model

表1 三自由度機械腿變量聲明Tab. 1 Variables of three-degree-of-freedom-robot-leg

對應這一情形,計算化簡之后得到方程(1)：

為簡化計算,模型方程被轉化為圓柱坐標,如圖2 所示。

圖2 直角坐標變換到柱坐標Fig. 2 Rectangular coordinate to polar coordinate

其中,

2 方程求解

為了控制終端到達任意位置(x0,y0,z0),必須通過該位置求解方程得到各個關節應當輸出的角度。 由于不存在解析解,本文提出迭代法進行求解。

2.1 迭代法求解

因為y =f(x),故以下只用討論x 與z 的關系。

為了讓末端點P 沿曲線S 運動,在曲線S 上取得點(x0,y0,z0),求取一組ρ 以及θ1,再在ZOρ 平面上迭代,分別求解出一組θ2、θ3,重復這一過程,即可得到向量組Θi(θ1i,θ2i, θ3i)T,使得末端點P 運動軌跡擬合于曲線S。

為了方便計算,令k =3,利用θ3控制ρ 的變化,并修正由θ2變化帶來的偏差Δρ, 由ρ = a1＋a2cos(θ2) ＋a3cos(θ3- θ2) 可以直接分解得

簡記為θ3= g(θ2,θ1,ρ)。

利用θ2控制z 的變化,并修正由θ3變化帶來的偏差Δz,由z =a2sin(θ2) - a3sin(θ3- θ2) 變換可得到關于sin(θ2) 的一元二次方程,從而可以求解出θ2。

其中該一元二次方程相關參數如式(5)所示。

由一元二次方程求根公式可得求解sin(θ2),利用反三角函數求解出θ2,公式(6)：

θ2表達式簡記為：θ2= f(θ3,z),聯立式(3) ～ (6) 可得到迭代式(7)：

2.2 迭代收斂證明

該迭代方法斂散性直接影響到是否能得到最后的結果,這里需要證明,隨著θ2、θ3的增加(θ2、θ3的初值是其最小值,所以經過迭代只會逐漸增加),P能否從ZOρ 平面上初始位置(ρ*,z*) 收斂到ZOρ平面上目標位置(ρ0,z0)。

因為求解方程z(n)=z0可以得到準確的,但是會使得之前重合的ρ()=ρ0出現新的偏差,即ρ()= ρ0＋Δρ,可以看出,由于系統存在耦合性,每次使得求解z(n)= z0,得到時,都會不斷產生偏差Δρ ＞0。 為了使得Δx 絕對值減小,只有在Δρ=ρ() -ρ0是關于的減函數時,迭代才會使得ρ(θ2,θ3) 收斂于ρ0。 由于ρ0為常數,所以求證關于ρ 的迭代收斂,可轉換為求證ρ(θ2,θ3) 是關于θ2的減函數。

因為求解方程ρ(n)= ρ0可以得到準確的θ3(n),但是會使得之前重合的z()= z0出現新的偏差,即z() =z0＋Δz,同理每次使得求解ρ(n)= ρ0,得到θ3(n)時,都會不斷產生偏差Δz＜0。 為了使得Δz 絕對值減小, 只有在Δz =z() - z0是關于的增函數時,迭代才會使得z(θ2,θ3) 收斂于z0。 由于z0為常數,所以求證關于z 的迭代收斂,可轉換為求證z(θ2,θ3) 是關于θ3的增函數。

再次強調,因為ρ(θ3)的偏差是修正z(θ2)帶來的,所以Δρ 的自變量是θ2；z(θ2) 的偏差是修正ρ(θ3)帶來的,所以Δz 的自變量分別是θ3,這里二者自變量會互換。 通過分別對其相關變量求解偏導數,見公式(8),可以判斷這兩個函數的增減性。

從中可以得到這樣的結論：由于θ2∈(-π／6,π／2) ,θ3∈(0,5π／6),并且(θ3-θ2) ∈［0,π／2］使由此可以證明ρ(θ2,θ3)是關于θ2的減函數,z(θ2,θ3)是關于θ3的增函數,所以該迭代方法必然收斂。

2.3 對任意點的迭代

由于機械腿的運動范圍是有限的,這意味著迭代存在一個可行域,超過可行域迭代必然無法收斂,下面著重討論迭代的可行域

圖3 可行域邊界條件示意圖Fig. 3 Boundary condition of feasible region

先要求取ρmin,由圖3(a) 可以看出隨著θ2繼續減小,ρmin將進一步減小,但是由陰影部分可看出,這種方法會導致在恒定高度不變的情況下,P 點無法在z0平面內連續移動,這樣會導致支撐面減小,影響機器人站立的穩定性,所以這里采用圖3(b)的方式來求解ρmin。 如圖3(b) 所示a3垂直于水平面,這時有(θ3-θ2)= π／2,ρmin的取值與z 和θ2有關,關系式為(9) 和(10)：

可以看出ρmin與ρmax都是關于z 的函數,由此得出3 條位于ZOρ 包絡線,在兩條包絡線之間就是末端點P 可以運動的范圍。 當(z ＋a3)／a2∈［- 1,1］時,ρmin曲線為ρmin= a1＋a2cos(θ2),如圖4 中曲線(a) 所示。 當(z ＋a3)／a2＞1 時反正弦函數不再為實數,所以在z ＞47.11 時,θ2=arcsin((z ＋a3)／a2)不再適用,這時令θ2= π／2, ρmin曲線是以(a3, a2)為圓心a3半徑的圓弧,如圖4 中曲線(b)。 圖4 中曲線(c) 為可行域最大邊界的包絡線。

圖4 可行域邊界曲線示意圖Fig. 4 Curves of feasible region

以空間內一點(120,0,-50)為例,進行24 次迭代后,各個參數變化如圖5 所示。

圖5 單一點迭代各參數收斂情況Fig. 5 Iterative convergence issue of single point

如圖5 所示,截取了前15 次迭代的結果,從中可以看到,在第十次的時候就已經達到了實際應用的標準。 經過10 次迭代,θ3收斂于125.5°,θ2收斂于55.29°,二者之差為70.21°＜90°,末端位置坐標與期望坐標誤差收斂于0.001 136 mm。24 次迭代完成后x 收斂于120＋5.89×10-11mm,y 收斂于0.0 mm,z 收斂于-50-2.124 5×10-11mm,與期望坐標(120,0,-50)接近。

迭代過程如圖6 所示,可以看出當對準z 軸時會使得ρ 產生偏差Δρ,并且Δρ 隨著迭代的進行絕對值在逐漸減小,當對準ρ 軸時會使得z 產生偏差Δz,并且Δz 隨著迭代的進行絕對值也在逐漸減小。 可以看出迭代收斂的范圍,并不在目標點附近,所以該迭代方法對于初值的選取十分為寬松。

圖6 單一點迭代過程Fig. 6 Iterative process of single point

為了研究點迭代的效果,在zoρ 平面內,每一個毫米取一個點,先保持z 不變逐行取點,然后z=z ＋1再繼續取點,經過迭代,得到相對應的(θ3,θ2) 集合共40 000 組數據(其中7983 組數據誤差小于0.000 1,為有效數據),回代到式(1) 中進行驗算,繪制點結果如圖7 所示,選取四組有代表性的數據如表2 所示結果。

表2 多點迭代結果比較Tab. 2 Comparisons among multiple points

這里存在一個必然的問題, ρmax是實際的最大可行域邊界,而ρmin是根據實際情況定義出來的最小可行域邊界,這意味著存在ρ ＜ρmin(最小值可以到0 附近,但是無實際用途),但是絕對不會出現ρ＞ρmax,當(ρ,z) 接近最大邊界時,收斂效果會嚴重下降,即說明實際可行域內不可能存在這樣一個點。 所以,如表2 所示第13039 號和26452 號點均在最大值邊界附近,誤差均大于一。 其中,圖7 所示的第28210 號數據(在哪里？ ),由于同時靠近最大值和最小值邊界,誤差達到最大的37.12。 對于第13039 號點,盡管在最小邊界附近,結果與期望的誤差接近于零,在可行域內的第9860 和15085 號點,誤差在允許范圍內。 從28217 號數據到第40000 號均為無效數據,由于未被錄入且初值為零,所以各項數據均為零。

圖7 多點迭代誤差情況Fig. 7 Iterative error of multiple points

迭代點序數與誤差關系如圖7 所示,其中有明顯誤差的均為接近最大可行域范圍的數值,由于序號越大,可行域越小,誤差也逐漸增大。

以上是針對三關節機械臂進行求解的分析過程,對于多關節機械腿,該方法依然適用,其運動學正模型如式(12)所示。

同理可以利用本文所提出方法進行迭代求解,由于篇幅限制不再展開敘述。

3 結束語

本文巧妙利用了方程的非線性,做到了對點的精確快速迭代,對于更為復雜的幾何曲線,可以在該曲線上取若干離散點,利用本文所介紹的迭代法逐一進行擬合得到每個關節輸出角度的序列,機械腿依次執行,便可以擬合出該條曲線。 同時,如果該點在邊界附近,誤差會增大,大誤差出現的頻率會增加,所以選取這些點的時,可以利用誤差剔除掉靠近邊界的點,同時可行域范圍可以適當縮減,z 取值范圍可以控制在［0,100］。 并且該迭代法可以推廣到更高自由度機械腿逆模型的求解上,有廣泛的適用性。