機(jī)械腿逆運(yùn)動學(xué)模型求解

胡平志, 李澤滔

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

0 引 言

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

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

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

1 建立坐標(biāo)系

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

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

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

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

為簡化計算,模型方程被轉(zhuǎn)化為圓柱坐標(biāo),如圖2 所示。

圖2 直角坐標(biāo)變換到柱坐標(biāo)Fig. 2 Rectangular coordinate to polar coordinate

其中,

2 方程求解

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

2.1 迭代法求解

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

為了讓末端點P 沿曲線S 運(yùn)動,在曲線S 上取得點(x0,y0,z0),求取一組ρ 以及θ1,再在ZOρ 平面上迭代,分別求解出一組θ2、θ3,重復(fù)這一過程,即可得到向量組Θi(θ1i,θ2i, θ3i)T,使得末端點P 運(yùn)動軌跡擬合于曲線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) 變換可得到關(guān)于sin(θ2) 的一元二次方程,從而可以求解出θ2。

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

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

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

2.2 迭代收斂證明

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

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

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

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

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

2.3 對任意點的迭代

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3 結(jié)束語

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