張 巖 過仕安 李 爭(zhēng) 安國(guó)慶 薛智宏 蓋祥虎
(①河北科技大學(xué)電氣工程學(xué)院,河北 石家莊 050018;②陸軍工程大學(xué)石家莊校區(qū)電磁環(huán)境效應(yīng)國(guó)家級(jí)重點(diǎn)實(shí)驗(yàn)室,河北 石家莊 050003)
串聯(lián)型機(jī)械臂是廣泛使用的一種機(jī)械臂類型。而在機(jī)械臂的軌跡規(guī)劃上,令機(jī)械臂末端在空間上沿給定直線運(yùn)動(dòng),這樣的直線規(guī)劃是最基本的一種軌跡規(guī)劃。僅采用直線解析式規(guī)劃時(shí),由于是任意直線,斜率往往不是常數(shù),在微控制器中做除法運(yùn)算會(huì)出現(xiàn)誤差,這樣在微控制器中累計(jì)疊加容易使軌跡出現(xiàn)偏差,并且運(yùn)算量較大。
為克服上述直線規(guī)劃中出現(xiàn)的一些缺陷,本文采用在計(jì)算機(jī)繪圖中常用的Bresenham直線繪制算法規(guī)劃?rùn)C(jī)械臂末端運(yùn)動(dòng)。該算法能夠避免除法運(yùn)算,具有較好的精度,同時(shí)能夠減小微控制器的運(yùn)算量[1]。
本文依托設(shè)計(jì)的用于搬運(yùn)用途的四軸機(jī)械臂,對(duì)該機(jī)械臂建立D-H參數(shù)的數(shù)學(xué)模型,在該模型的基礎(chǔ)上對(duì)機(jī)械臂做運(yùn)動(dòng)學(xué)分析求解;然后分析Bresenham直線插補(bǔ)算法在該機(jī)械臂的應(yīng)用過程和對(duì)機(jī)械臂的影響,并針對(duì)其中的不良影響做出步數(shù)補(bǔ)償?shù)膬?yōu)化方案。
常用于描述串聯(lián)型機(jī)械臂的數(shù)學(xué)模型有D-H模型、改進(jìn)的D-H模型和POE模型[2-3]。其中改進(jìn)的D-H模型物理意義表示更為明確,能夠清晰表述類似本系統(tǒng)所設(shè)計(jì)的串聯(lián)型機(jī)械臂的位姿,因此本系統(tǒng)采用該模型作為機(jī)械臂的位姿描述方式和控制基礎(chǔ)。
改進(jìn)的D-H模型是通過對(duì)每一個(gè)構(gòu)件描述其參數(shù)而建立。對(duì)于一個(gè)構(gòu)件k的D-H模型使用4個(gè)參數(shù)來描述:連桿長(zhǎng)度lk、扭角αk、關(guān)節(jié)偏距ck和關(guān)節(jié)轉(zhuǎn)角θk[4-7]。本系統(tǒng)設(shè)計(jì)的機(jī)械臂為關(guān)節(jié)可轉(zhuǎn)動(dòng)的類型,以關(guān)節(jié)轉(zhuǎn)角作為運(yùn)動(dòng)參數(shù)。機(jī)械臂的模型示意圖如圖1所示,其中標(biāo)注了所需的D-H參數(shù)位置和各個(gè)關(guān)節(jié)的坐標(biāo)系。在本系統(tǒng)的設(shè)計(jì)中各構(gòu)件的D-H參數(shù)如表1所示,其中關(guān)節(jié)轉(zhuǎn)角為初始值。

表1 機(jī)械臂構(gòu)件的D-H參數(shù)表

機(jī)械臂的運(yùn)動(dòng)學(xué)求解是使用已建立的數(shù)學(xué)模型,對(duì)控制量:如關(guān)節(jié)的旋轉(zhuǎn)角度,和運(yùn)動(dòng)學(xué)中的變量:如位置、速度、加速度等相互轉(zhuǎn)換。其中,由控制量轉(zhuǎn)換為運(yùn)動(dòng)學(xué)變量的過程為正運(yùn)動(dòng)學(xué)求解;由運(yùn)動(dòng)學(xué)變量轉(zhuǎn)換為控制量的過程為逆運(yùn)動(dòng)學(xué)求解[8]。逆運(yùn)動(dòng)學(xué)求解是本文設(shè)計(jì)實(shí)現(xiàn)中的一個(gè)難點(diǎn)。
機(jī)械臂運(yùn)動(dòng)參數(shù)的改變會(huì)導(dǎo)致機(jī)械臂各個(gè)關(guān)節(jié)坐標(biāo)系發(fā)生變化,因此可以通過描述關(guān)節(jié)坐標(biāo)系變化求解運(yùn)動(dòng)學(xué)變量,即求解正運(yùn)動(dòng)學(xué)。
兩個(gè)不同的坐標(biāo)系Oxyz和O′x′y′z′,描述點(diǎn)O′相對(duì)于點(diǎn)O的變化可用向量OO′來描述,稱為位置向量r;而坐標(biāo)系的變化除了坐標(biāo)系原點(diǎn)發(fā)生位移,坐標(biāo)系的各個(gè)坐標(biāo)軸也產(chǎn)生了旋轉(zhuǎn),通過姿態(tài)矩陣R描述坐標(biāo)系旋轉(zhuǎn)變化。為方便矩陣的推導(dǎo)運(yùn)算,將姿態(tài)矩陣和位置向量合并,并增加一行單位行向量,構(gòu)成4×4的一個(gè)矩陣,稱為位姿矩陣M,如式(1)所示。
(1)
在機(jī)械臂中當(dāng)構(gòu)件k的運(yùn)動(dòng)參數(shù)關(guān)節(jié)轉(zhuǎn)角θk發(fā)生變化的時(shí)候,通過描述兩個(gè)關(guān)節(jié)坐標(biāo)系位姿矩陣相對(duì)變化,并通過最終求解的位姿矩陣獲得機(jī)械臂轉(zhuǎn)動(dòng)后的位姿信息。對(duì)于構(gòu)件k的位姿矩陣如式(2)所示。[9]
M=
(2)
對(duì)于本文設(shè)計(jì)的四軸機(jī)械臂,由于每個(gè)連桿機(jī)構(gòu)都是首尾相連的串聯(lián)型構(gòu)件,因此在知道各個(gè)關(guān)節(jié)的D-H參數(shù)后,即能推導(dǎo)出機(jī)械臂末端執(zhí)行器的位姿[10],對(duì)于本系統(tǒng)的末端位姿推導(dǎo)公式如式(3)所示。
(3)
Mk代表構(gòu)件k的相鄰兩個(gè)關(guān)節(jié)坐標(biāo)系變化的位姿矩陣,從定義式(2)中可以看出既與機(jī)械臂的固有參數(shù)有關(guān),又與機(jī)械臂的運(yùn)動(dòng)參數(shù)有關(guān)。正運(yùn)動(dòng)學(xué)求解結(jié)果即為式(3)。可從矩陣結(jié)果的最后一列列向量中位置向量得到我們所需要的機(jī)械臂末端執(zhí)行器的位置信息。
本系統(tǒng)中的機(jī)械臂逆運(yùn)動(dòng)學(xué)求解是在給定的目標(biāo)末端執(zhí)行器位置下求解各個(gè)關(guān)節(jié)舵機(jī)的旋轉(zhuǎn)角度。在文獻(xiàn)[2,11]中,提及了多種解算的方法,包括幾何法[12]、解析法、數(shù)值法和人工智能法[13]。對(duì)于除幾何法以外的三種方法,大多都是運(yùn)用成熟的開源運(yùn)算庫(kù)實(shí)現(xiàn)完成的,這些運(yùn)算庫(kù)一般需要系統(tǒng)在上位機(jī)上實(shí)現(xiàn),增加了控制成本。而普通的平面幾何法求解結(jié)果對(duì)于具有空間運(yùn)動(dòng) 特性的機(jī)械臂解算具有一定的局限性。針對(duì)本文的機(jī)械臂結(jié)構(gòu),設(shè)計(jì)了一種簡(jiǎn)化自由度的平面解析法,能夠?qū)崿F(xiàn)以相對(duì)較少的運(yùn)算量使執(zhí)行器末端到達(dá)預(yù)定的位置,同時(shí)也便于將程序移植入微控制器中。
首先,采用合適的坐標(biāo)系。由于本系統(tǒng)是以底座為中心旋轉(zhuǎn)的結(jié)構(gòu),采用柱坐標(biāo)系為基礎(chǔ)坐標(biāo)系。因此,當(dāng)目標(biāo)位置為空間坐標(biāo)系位置時(shí),須以底座原點(diǎn)為中心,將目標(biāo)的空間直角坐標(biāo)轉(zhuǎn)換為柱坐標(biāo)系。對(duì)于柱坐標(biāo)系中另外兩個(gè)坐標(biāo)軸構(gòu)成的平面直角坐標(biāo)系,可采用平面幾何法,因?yàn)槌鬃獾钠渌鏅C(jī)和構(gòu)件都是在同一平面內(nèi)運(yùn)動(dòng)。但在該平面內(nèi)仍存在3個(gè)連桿構(gòu)件,采用幾何法求解時(shí)對(duì)二連桿求解十分直觀,對(duì)三連桿求解的過程較為冗雜[14]。根據(jù)本機(jī)械臂系統(tǒng)的實(shí)際用途,為平穩(wěn)夾持物體。因此最末端的連桿構(gòu)件應(yīng)時(shí)刻保持水平,這樣就可以簡(jiǎn)化自由度,將最后一個(gè)連桿構(gòu)件添加固定約束,先對(duì)目標(biāo)位置作修正后再對(duì)二連桿做平面幾何法求解,即可得到目標(biāo)解。簡(jiǎn)化自由度的平面解析法運(yùn)算流程如圖2所示。

空間中的最短路徑便是空間中起點(diǎn)與終點(diǎn)直接相連的直線,容易得出直接通過求解空間直線的解析式,使機(jī)械臂末端沿解析式軌跡運(yùn)動(dòng)。但解析式中直線的斜率往往不是整數(shù),而微控制器對(duì)于除法運(yùn)算的求解存在除不盡時(shí)結(jié)果不精確的缺陷。在微控制器中利用解析式求解空間直線上各個(gè)點(diǎn)的位置時(shí),會(huì)不斷累加這些不精確的斜率,使得實(shí)際運(yùn)行軌跡與既定運(yùn)行軌跡產(chǎn)生偏差。Bresenham直線插補(bǔ)算法很好地規(guī)避了這一缺陷。Bresenham直線插補(bǔ)算法原本是用在圖像繪制上的一種算法,使用像素?cái)M合出解析式規(guī)定的直線形狀。這種算法是通過比較距離大小的方式規(guī)避除法運(yùn)算,從而求解得到精確的結(jié)果[15]。圖3中圓離散點(diǎn)和像素塊表示了該算法對(duì)圖中直線的擬合效果。

由圖4說明Bresenham算法思想。直線的起始點(diǎn)為(x1,y1),終止點(diǎn)為(x2,y2),擬合這兩點(diǎn)之間連成的直線。直線解析式為
y=kx+b
(4)

假設(shè)Δx>Δy,即斜率k∈(0,1),求解圖中D點(diǎn)(xi+1,yi+1)的擬合點(diǎn),應(yīng)選取D1點(diǎn)(xi+1,yi)還是D2點(diǎn)(xi+1,yi+1)。通過比較D1、D2,選取與D最近的點(diǎn)即為最佳擬合點(diǎn),即比較圖4中d1與d2的大小判斷:若d1
(5)
代入直線解析式(4)為
(6)
設(shè)像素長(zhǎng)度和寬度為單位長(zhǎng)度1,對(duì)距離d1和d2比較作差得
d1-d2=(kxi+1+b-yi)-(yi+1-kxi=1-b)
=2kxi+1+2b-yi-yi+1
=2kxi+1+2b-yi-1
(7)
差值的表達(dá)式中仍然存在斜率k,斜率k的表達(dá)式為
(8)
將k代入式(7)中,并且等式兩邊同時(shí)乘以Δx
Δx(d1-d2)=2Δyxi+1-2Δxyi+Δx(2b-1)
(9)
新定義一個(gè)變量p,令pi定義為
pi=2Δx(d1-d2)=2Δyxi+1-2Δxyi+Δx(2b-1)
(10)
則pi+1為
pi+1=2Δx(d1-d2)=2Δyxi+2-2Δxyi+1+Δx(2b-1)
(11)
將式(11)與式(10)相減,可得
pi+1-pi=2Δy(xi+2-xi+1)-2Δx(yi+1-yi)
=2Δy-2Δx(yi+1-yi)
(12)
當(dāng)xi+1處d1>d2時(shí),則應(yīng)取yi+1=yi+1,此時(shí)pi>0,由此可得
pi+1=2Δy-2Δx(yi+1-yi)+pi+2(Δy-Δx)
(13)
而當(dāng)xi+1處d1 pi+1=2Δy-2Δx(yi+1-yi)+pi+2Δy (14) 當(dāng)xi+1處d1=d2時(shí),可任取yi+1值是否在原值基礎(chǔ)上增長(zhǎng),按照上文選取點(diǎn)的約定,取yi+1=yi+1,因此式(13)適用于pi≥0的情況。 由此,根據(jù)式(13)和式(14),可以在已知pi的結(jié)果時(shí),可以順序推出后續(xù)所有點(diǎn)p的結(jié)果;而已知p的結(jié)果,就能判斷d1與d2的大小,進(jìn)而判斷出點(diǎn)的選取結(jié)果[16]。而此時(shí)還要解決的問題是p1的值如何求出,按照p的定義 p1=2Δx(d1-d2) =2Δyx2-2Δxy1+Δx(2b-1) =2Δy(x1+1)-2Δxy1+Δx(2b-1) (15) 由x1點(diǎn)處直線解析式 (16) 等式兩側(cè)同時(shí)乘以Δx,整理可得 Δxy1=Δyx1+Δxb (17) 將式(17)代入式(15) p1=2Δyx1+2Δy-2Δxy1+Δx(2b-1) =2Δyx1+2Δy-2Δxy1+2bΔx+2bΔx-Δx =2Δy-Δx (18) 由此可以確定初始值p1,進(jìn)而確定所有取值的位置。 上述算法過程的過程描述中可以看出,算法的思路是x方向每移動(dòng)一個(gè)單位長(zhǎng)度,判斷y方向是否前進(jìn)插值,判斷的依據(jù)是解析式上的點(diǎn)與兩個(gè)待插值點(diǎn)之間的距離,取最近點(diǎn)。上述算法只描述了Δx>Δy的情況;若Δx<Δy,則應(yīng)在y方向每移動(dòng)一個(gè)單位長(zhǎng)度,判斷x方向是否前進(jìn)插值,判斷思路與上述對(duì)稱;若Δx=Δy,則按照上述任意一種情況計(jì)算即可。因此在程序設(shè)計(jì)時(shí)應(yīng)有兩個(gè)分支,先判斷Δx與Δy的大小[17]。該算法通過距離比較判斷插值點(diǎn)位置,避免了在微控制器中不精確的斜率累加帶來的誤差,提高了輸出結(jié)果的精確度。 將上述算法再擴(kuò)展到三維空間中,將空間坐標(biāo)分解為兩個(gè)平面上的坐標(biāo)去判斷,則應(yīng)先判斷Δx、Δy與Δz的大小,取最大值的坐標(biāo)為單位移動(dòng)方向,每移動(dòng)一步判斷另外兩個(gè)坐標(biāo)是否前進(jìn)插值[18]。因此空間直線插補(bǔ)在程序設(shè)計(jì)時(shí)應(yīng)有3個(gè)分支。完整的程序流程如圖5所示。 使用MATLAB Robotics Toolbox工具,通過運(yùn)用Bresenham直線插補(bǔ)算法,規(guī)劃?rùn)C(jī)械臂末端在空間中沿直線軌跡運(yùn)行。通過仿真校驗(yàn),獲得使用該算法下的機(jī)械臂的相關(guān)運(yùn)動(dòng)學(xué)參數(shù),校驗(yàn)算法的可靠性。 仿真程序的運(yùn)行流程如圖6所示[19]。 使用MATLAB Robotics Toolbox工具仿真的機(jī)械臂模型如圖7所示,仿真運(yùn)行結(jié)果如圖8所示。從圖中可以看出機(jī)械臂末端能夠在空間中沿直線軌跡運(yùn)行。在圖像中,可以看出直線軌跡有鋸齒狀,這是直線插值較為稀疏的緣故導(dǎo)致的。在程序代碼中,設(shè)立了一個(gè)變量控制插值密度,稱為軌跡精細(xì)度的放大倍數(shù)。放大倍數(shù)越高,步數(shù)越多,軌跡越平滑。 從圖8a中可以看出,使用該Bresenham算法可以規(guī)劃?rùn)C(jī)械臂末端軌跡為直線,驗(yàn)證了算法能夠規(guī)劃?rùn)C(jī)械臂末端軌跡為直線;圖8b是形成直線軌跡時(shí)機(jī)械臂各關(guān)節(jié)舵機(jī)的旋轉(zhuǎn)角度;圖8c是根據(jù)各關(guān)節(jié)舵機(jī)的旋轉(zhuǎn)角度計(jì)算出的旋轉(zhuǎn)角速度,從圖像可以看出各關(guān)節(jié)舵機(jī)起動(dòng)時(shí)速度要求很高,隨后趨緩。 從仿真結(jié)果中,注意到了該算法存在起動(dòng)轉(zhuǎn)速突變的缺點(diǎn)。為了緩解這一不足,本文提出一種變步長(zhǎng)的步數(shù)補(bǔ)償優(yōu)化方案,對(duì)該算法在機(jī)械臂軌跡規(guī)劃上的運(yùn)用進(jìn)行優(yōu)化。 在闡述Bresenham直線插補(bǔ)算法時(shí)提到,該算法的大致過程是差值最大的方向每移動(dòng)一步,判斷另一個(gè)方向是否需要移動(dòng),形成插值點(diǎn)。從實(shí)驗(yàn)結(jié)果的圖像中可以看出,起動(dòng)時(shí)電機(jī)的轉(zhuǎn)速較高,易形成沖擊。因此對(duì)起動(dòng)時(shí)的相應(yīng)步數(shù)內(nèi)進(jìn)行補(bǔ)償,插補(bǔ)更多的步數(shù),來減緩起動(dòng)轉(zhuǎn)速和起動(dòng)沖擊。 接下來設(shè)計(jì)一個(gè)較為簡(jiǎn)易的步數(shù)補(bǔ)償優(yōu)化。從實(shí)驗(yàn)結(jié)果的圖像中可以看出,前1/4的步數(shù)內(nèi)電機(jī)的轉(zhuǎn)速較高,因此對(duì)前1/4的步數(shù)內(nèi)進(jìn)行補(bǔ)償,插補(bǔ)更多的步數(shù)。改進(jìn)后的程序流程如圖9所示。虛線框所標(biāo)的流程是相較于之前仿真程序增加的流程。 使用MATLAB Robotics Toolbox工具仿真校驗(yàn)的結(jié)果如圖10所示,圖像中選取了肩關(guān)節(jié)舵機(jī)的優(yōu)化前后情況進(jìn)行對(duì)比。從圖10a中可以看出,算法優(yōu)化前后運(yùn)行軌跡一致,沒有影響算法使用的目的;圖10b中可以看出由于步數(shù)插補(bǔ)使得優(yōu)化后的旋轉(zhuǎn)角度變化滯后于優(yōu)化前的旋轉(zhuǎn)角度曲線;圖10c中可以看出起動(dòng)時(shí)轉(zhuǎn)速要求降低了近1/2,但當(dāng)步數(shù)補(bǔ)償結(jié)束后轉(zhuǎn)速發(fā)生了突變,隨后的變化趨勢(shì)與補(bǔ)償前一致。 使用直線解析式規(guī)劃軌跡和Bresenham算法及其補(bǔ)償優(yōu)化算法在運(yùn)行測(cè)試在運(yùn)行時(shí)間、末端執(zhí)行器誤差和起動(dòng)角速度比較如表2所示。該測(cè)試是在生成同樣插值點(diǎn)數(shù)量條件下多次測(cè)試的結(jié)果,起動(dòng)角速度選取了其中一個(gè)關(guān)節(jié)。 表2 算法運(yùn)行測(cè)試結(jié)果 根據(jù)算法的仿真實(shí)驗(yàn)結(jié)果,與直線解析式規(guī)劃軌跡方法比較,得出Bresenham算法的一些優(yōu)點(diǎn)和缺點(diǎn)。 使用Bresenham算法規(guī)劃結(jié)果的優(yōu)點(diǎn)有: (1)空間上路徑為直線,距離最短,用時(shí)少; (2)算法較簡(jiǎn)潔,運(yùn)算量較小; (3)算法規(guī)避了除法運(yùn)算,精確度高。 使用Bresenham算法規(guī)劃結(jié)果的缺點(diǎn)有: (1)起動(dòng)或制動(dòng)時(shí)要求電機(jī)轉(zhuǎn)速突變,對(duì)電機(jī)沖擊大; (2)插補(bǔ)步數(shù)稀疏時(shí)震動(dòng)較大。 因此,該算法適合應(yīng)用于對(duì)機(jī)械臂的空間路徑要求非常嚴(yán)格的場(chǎng)合,并且希望時(shí)間盡可能縮短,選擇Bresenham直線插補(bǔ)算法較為合適。 從仿真結(jié)果中可以看出簡(jiǎn)單的補(bǔ)償優(yōu)化前后算法運(yùn)行軌跡完全一致,起動(dòng)時(shí)的轉(zhuǎn)速要求降低了,得到了預(yù)期的優(yōu)化效果。但是簡(jiǎn)單的補(bǔ)償算法帶來的問題也不可忽視:一是補(bǔ)償帶來電機(jī)起動(dòng)減緩是以延長(zhǎng)起動(dòng)時(shí)間為代價(jià),因此該算法運(yùn)行用時(shí)少的優(yōu)點(diǎn)被弱化了;二是簡(jiǎn)單補(bǔ)償前后存在轉(zhuǎn)速差,會(huì)對(duì)電機(jī)造成二次沖擊;三是起動(dòng)步數(shù)增多可能會(huì)造成機(jī)械臂的抖動(dòng)增加。因此實(shí)際使用時(shí)需考慮這些會(huì)對(duì)機(jī)械臂系統(tǒng)造成的影響。還可以考慮更復(fù)雜的變步數(shù)補(bǔ)償優(yōu)化方法,動(dòng)態(tài)地補(bǔ)償起動(dòng)步數(shù),優(yōu)化算法的起動(dòng)過程。 本文針對(duì)機(jī)械臂末端軌跡規(guī)劃問題中的直線軌跡規(guī)劃,在采用直線軌跡規(guī)劃時(shí)使用微處理器處理斜率中除法運(yùn)算不精確、運(yùn)算量大的問題,引入Bresenham算法解決。建立機(jī)械臂的D-H參數(shù)數(shù)學(xué)模型,在該模型的基礎(chǔ)上根據(jù)模型參數(shù)設(shè)計(jì)的基于位姿矩陣的正運(yùn)動(dòng)學(xué)求解和基于簡(jiǎn)化自由度的平面解析法的逆運(yùn)動(dòng)學(xué)求解,作為算法輸出機(jī)械臂指定位置時(shí)解算為主控制器控制舵機(jī)指令的基礎(chǔ)。使用Bresenham算法規(guī)劃?rùn)C(jī)械臂的直線軌跡運(yùn)動(dòng)。Bresenham算法以比較待插值點(diǎn)距離的方式規(guī)避了除法運(yùn)算,且運(yùn)算量較小。而Bresenham算法規(guī)劃中存在對(duì)起動(dòng)時(shí)起動(dòng)和制動(dòng)時(shí)轉(zhuǎn)速可能突變的缺陷,本文采用步數(shù)補(bǔ)償優(yōu)化加以解決。使用步數(shù)補(bǔ)償優(yōu)化有效降低了起動(dòng)時(shí)的轉(zhuǎn)速要求。但也要注意使用簡(jiǎn)單的步數(shù)補(bǔ)償優(yōu)化時(shí)帶來的時(shí)間延緩、二次沖擊等附加影響,實(shí)際需權(quán)衡使用。
2.2 Bresenham算法直線規(guī)劃仿真校驗(yàn)



2.3 Bresenham算法的步數(shù)補(bǔ)償優(yōu)化



3 結(jié)語(yǔ)