張 巖 過仕安 李 爭 安國慶 薛智宏 蓋祥虎

(①河北科技大學(xué)電氣工程學(xué)院，河北 石家莊 050018；②陸軍工程大學(xué)石家莊校區(qū)電磁環(huán)境效應(yīng)國家級重點實驗室，河北 石家莊 050003)

串聯(lián)型機(jī)械臂是廣泛使用的一種機(jī)械臂類型。而在機(jī)械臂的軌跡規(guī)劃上，令機(jī)械臂末端在空間上沿給定直線運動，這樣的直線規(guī)劃是最基本的一種軌跡規(guī)劃。僅采用直線解析式規(guī)劃時，由于是任意直線，斜率往往不是常數(shù)，在微控制器中做除法運算會出現(xiàn)誤差，這樣在微控制器中累計疊加容易使軌跡出現(xiàn)偏差，并且運算量較大。

為克服上述直線規(guī)劃中出現(xiàn)的一些缺陷，本文采用在計算機(jī)繪圖中常用的Bresenham直線繪制算法規(guī)劃機(jī)械臂末端運動。該算法能夠避免除法運算，具有較好的精度，同時能夠減小微控制器的運算量[1]。

本文依托設(shè)計的用于搬運用途的四軸機(jī)械臂，對該機(jī)械臂建立D-H參數(shù)的數(shù)學(xué)模型，在該模型的基礎(chǔ)上對機(jī)械臂做運動學(xué)分析求解；然后分析Bresenham直線插補(bǔ)算法在該機(jī)械臂的應(yīng)用過程和對機(jī)械臂的影響，并針對其中的不良影響做出步數(shù)補(bǔ)償?shù)膬?yōu)化方案。

1 機(jī)械臂的數(shù)學(xué)模型和運動學(xué)分析

1.1 機(jī)械臂的數(shù)學(xué)模型

常用于描述串聯(lián)型機(jī)械臂的數(shù)學(xué)模型有D-H模型、改進(jìn)的D-H模型和POE模型[2-3]。其中改進(jìn)的D-H模型物理意義表示更為明確，能夠清晰表述類似本系統(tǒng)所設(shè)計的串聯(lián)型機(jī)械臂的位姿，因此本系統(tǒng)采用該模型作為機(jī)械臂的位姿描述方式和控制基礎(chǔ)。

改進(jìn)的D-H模型是通過對每一個構(gòu)件描述其參數(shù)而建立。對于一個構(gòu)件k的D-H模型使用4個參數(shù)來描述：連桿長度lk、扭角αk、關(guān)節(jié)偏距ck和關(guān)節(jié)轉(zhuǎn)角θk[4-7]。本系統(tǒng)設(shè)計的機(jī)械臂為關(guān)節(jié)可轉(zhuǎn)動的類型，以關(guān)節(jié)轉(zhuǎn)角作為運動參數(shù)。機(jī)械臂的模型示意圖如圖1所示，其中標(biāo)注了所需的D-H參數(shù)位置和各個關(guān)節(jié)的坐標(biāo)系。在本系統(tǒng)的設(shè)計中各構(gòu)件的D-H參數(shù)如表1所示，其中關(guān)節(jié)轉(zhuǎn)角為初始值。

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

1.2 基于位姿矩陣的機(jī)械臂的正運動學(xué)求解

機(jī)械臂的運動學(xué)求解是使用已建立的數(shù)學(xué)模型，對控制量：如關(guān)節(jié)的旋轉(zhuǎn)角度，和運動學(xué)中的變量：如位置、速度、加速度等相互轉(zhuǎn)換。其中，由控制量轉(zhuǎn)換為運動學(xué)變量的過程為正運動學(xué)求解；由運動學(xué)變量轉(zhuǎn)換為控制量的過程為逆運動學(xué)求解[8]。逆運動學(xué)求解是本文設(shè)計實現(xiàn)中的一個難點。

機(jī)械臂運動參數(shù)的改變會導(dǎo)致機(jī)械臂各個關(guān)節(jié)坐標(biāo)系發(fā)生變化，因此可以通過描述關(guān)節(jié)坐標(biāo)系變化求解運動學(xué)變量，即求解正運動學(xué)。

兩個不同的坐標(biāo)系Oxyz和O′x′y′z′，描述點O′相對于點O的變化可用向量OO′來描述，稱為位置向量r；而坐標(biāo)系的變化除了坐標(biāo)系原點發(fā)生位移，坐標(biāo)系的各個坐標(biāo)軸也產(chǎn)生了旋轉(zhuǎn)，通過姿態(tài)矩陣R描述坐標(biāo)系旋轉(zhuǎn)變化。為方便矩陣的推導(dǎo)運算，將姿態(tài)矩陣和位置向量合并，并增加一行單位行向量，構(gòu)成4×4的一個矩陣，稱為位姿矩陣M，如式(1)所示。

(1)

在機(jī)械臂中當(dāng)構(gòu)件k的運動參數(shù)關(guān)節(jié)轉(zhuǎn)角θk發(fā)生變化的時候，通過描述兩個關(guān)節(jié)坐標(biāo)系位姿矩陣相對變化，并通過最終求解的位姿矩陣獲得機(jī)械臂轉(zhuǎn)動后的位姿信息。對于構(gòu)件k的位姿矩陣如式(2)所示。[9]

M=

(2)

對于本文設(shè)計的四軸機(jī)械臂，由于每個連桿機(jī)構(gòu)都是首尾相連的串聯(lián)型構(gòu)件，因此在知道各個關(guān)節(jié)的D-H參數(shù)后，即能推導(dǎo)出機(jī)械臂末端執(zhí)行器的位姿[10]，對于本系統(tǒng)的末端位姿推導(dǎo)公式如式(3)所示。

(3)

Mk代表構(gòu)件k的相鄰兩個關(guān)節(jié)坐標(biāo)系變化的位姿矩陣，從定義式(2)中可以看出既與機(jī)械臂的固有參數(shù)有關(guān)，又與機(jī)械臂的運動參數(shù)有關(guān)。正運動學(xué)求解結(jié)果即為式(3)?？蓮木仃嚱Y(jié)果的最后一列列向量中位置向量得到我們所需要的機(jī)械臂末端執(zhí)行器的位置信息。

1.3 基于簡化自由度的平面解析算法的機(jī)械臂的逆運動學(xué)求解

本系統(tǒng)中的機(jī)械臂逆運動學(xué)求解是在給定的目標(biāo)末端執(zhí)行器位置下求解各個關(guān)節(jié)舵機(jī)的旋轉(zhuǎn)角度。在文獻(xiàn)[2,11]中，提及了多種解算的方法，包括幾何法[12]、解析法、數(shù)值法和人工智能法[13]。對于除幾何法以外的三種方法，大多都是運用成熟的開源運算庫實現(xiàn)完成的，這些運算庫一般需要系統(tǒng)在上位機(jī)上實現(xiàn)，增加了控制成本。而普通的平面幾何法求解結(jié)果對于具有空間運動 特性的機(jī)械臂解算具有一定的局限性。針對本文的機(jī)械臂結(jié)構(gòu)，設(shè)計了一種簡化自由度的平面解析法，能夠?qū)崿F(xiàn)以相對較少的運算量使執(zhí)行器末端到達(dá)預(yù)定的位置，同時也便于將程序移植入微控制器中。

首先，采用合適的坐標(biāo)系。由于本系統(tǒng)是以底座為中心旋轉(zhuǎn)的結(jié)構(gòu)，采用柱坐標(biāo)系為基礎(chǔ)坐標(biāo)系。因此，當(dāng)目標(biāo)位置為空間坐標(biāo)系位置時，須以底座原點為中心，將目標(biāo)的空間直角坐標(biāo)轉(zhuǎn)換為柱坐標(biāo)系。對于柱坐標(biāo)系中另外兩個坐標(biāo)軸構(gòu)成的平面直角坐標(biāo)系，可采用平面幾何法，因為除底座外的其他舵機(jī)和構(gòu)件都是在同一平面內(nèi)運動。但在該平面內(nèi)仍存在3個連桿構(gòu)件，采用幾何法求解時對二連桿求解十分直觀，對三連桿求解的過程較為冗雜[14]。根據(jù)本機(jī)械臂系統(tǒng)的實際用途，為平穩(wěn)夾持物體。因此最末端的連桿構(gòu)件應(yīng)時刻保持水平，這樣就可以簡化自由度，將最后一個連桿構(gòu)件添加固定約束，先對目標(biāo)位置作修正后再對二連桿做平面幾何法求解，即可得到目標(biāo)解。簡化自由度的平面解析法運算流程如圖2所示。

2 使用Bresenham算法的直線軌跡規(guī)劃

2.1 Bresenham算法介紹

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

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

y=kx+b

(4)

假設(shè)Δx>Δy，即斜率k∈(0,1)，求解圖中D點(xi+1,yi+1)的擬合點，應(yīng)選取D1點(xi+1,yi)還是D2點(xi+1,yi+1)。通過比較D1、D2，選取與D最近的點即為最佳擬合點，即比較圖4中d1與d2的大小判斷：若d1d2則選擇D2點；若d1=d2則可選任意一點，這里固定選擇D2點。距離d1和d2的表達(dá)式為

(5)

代入直線解析式(4)為

(6)

設(shè)像素長度和寬度為單位長度1，對距離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)中，并且等式兩邊同時乘以Δx

Δx(d1-d2)=2Δyxi+1-2Δxyi+Δx(2b-1)

(9)

新定義一個變量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時，則應(yīng)取yi+1=yi+1，此時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時，可任取yi+1值是否在原值基礎(chǔ)上增長，按照上文選取點的約定，取yi+1=yi+1，因此式(13)適用于pi≥0的情況。

由此，根據(jù)式(13)和式(14)，可以在已知pi的結(jié)果時，可以順序推出后續(xù)所有點p的結(jié)果；而已知p的結(jié)果，就能判斷d1與d2的大小，進(jìn)而判斷出點的選取結(jié)果[16]。而此時還要解決的問題是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點處直線解析式

(16)

等式兩側(cè)同時乘以Δ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方向每移動一個單位長度，判斷y方向是否前進(jìn)插值，判斷的依據(jù)是解析式上的點與兩個待插值點之間的距離，取最近點。上述算法只描述了Δx>Δy的情況；若Δx<Δy，則應(yīng)在y方向每移動一個單位長度，判斷x方向是否前進(jìn)插值，判斷思路與上述對稱；若Δx=Δy，則按照上述任意一種情況計算即可。因此在程序設(shè)計時應(yīng)有兩個分支，先判斷Δx與Δy的大小[17]。該算法通過距離比較判斷插值點位置，避免了在微控制器中不精確的斜率累加帶來的誤差，提高了輸出結(jié)果的精確度。

將上述算法再擴(kuò)展到三維空間中，將空間坐標(biāo)分解為兩個平面上的坐標(biāo)去判斷，則應(yīng)先判斷Δx、Δy與Δz的大小，取最大值的坐標(biāo)為單位移動方向，每移動一步判斷另外兩個坐標(biāo)是否前進(jìn)插值[18]。因此空間直線插補(bǔ)在程序設(shè)計時應(yīng)有3個分支。完整的程序流程如圖5所示。

2.2 Bresenham算法直線規(guī)劃仿真校驗

使用MATLAB Robotics Toolbox工具，通過運用Bresenham直線插補(bǔ)算法，規(guī)劃機(jī)械臂末端在空間中沿直線軌跡運行。通過仿真校驗，獲得使用該算法下的機(jī)械臂的相關(guān)運動學(xué)參數(shù)，校驗算法的可靠性。

仿真程序的運行流程如圖6所示[19]。

使用MATLAB Robotics Toolbox工具仿真的機(jī)械臂模型如圖7所示，仿真運行結(jié)果如圖8所示。從圖中可以看出機(jī)械臂末端能夠在空間中沿直線軌跡運行。在圖像中，可以看出直線軌跡有鋸齒狀，這是直線插值較為稀疏的緣故導(dǎo)致的。在程序代碼中，設(shè)立了一個變量控制插值密度，稱為軌跡精細(xì)度的放大倍數(shù)。放大倍數(shù)越高，步數(shù)越多，軌跡越平滑。

從圖8a中可以看出，使用該Bresenham算法可以規(guī)劃機(jī)械臂末端軌跡為直線，驗證了算法能夠規(guī)劃機(jī)械臂末端軌跡為直線；圖8b是形成直線軌跡時機(jī)械臂各關(guān)節(jié)舵機(jī)的旋轉(zhuǎn)角度；圖8c是根據(jù)各關(guān)節(jié)舵機(jī)的旋轉(zhuǎn)角度計算出的旋轉(zhuǎn)角速度，從圖像可以看出各關(guān)節(jié)舵機(jī)起動時速度要求很高，隨后趨緩。

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

從仿真結(jié)果中，注意到了該算法存在起動轉(zhuǎn)速突變的缺點。為了緩解這一不足，本文提出一種變步長的步數(shù)補(bǔ)償優(yōu)化方案，對該算法在機(jī)械臂軌跡規(guī)劃上的運用進(jìn)行優(yōu)化。

在闡述Bresenham直線插補(bǔ)算法時提到，該算法的大致過程是差值最大的方向每移動一步，判斷另一個方向是否需要移動，形成插值點。從實驗結(jié)果的圖像中可以看出，起動時電機(jī)的轉(zhuǎn)速較高，易形成沖擊。因此對起動時的相應(yīng)步數(shù)內(nèi)進(jìn)行補(bǔ)償，插補(bǔ)更多的步數(shù)，來減緩起動轉(zhuǎn)速和起動沖擊。

接下來設(shè)計一個較為簡易的步數(shù)補(bǔ)償優(yōu)化。從實驗結(jié)果的圖像中可以看出，前1/4的步數(shù)內(nèi)電機(jī)的轉(zhuǎn)速較高，因此對前1/4的步數(shù)內(nèi)進(jìn)行補(bǔ)償，插補(bǔ)更多的步數(shù)。改進(jìn)后的程序流程如圖9所示。虛線框所標(biāo)的流程是相較于之前仿真程序增加的流程。

使用MATLAB Robotics Toolbox工具仿真校驗的結(jié)果如圖10所示，圖像中選取了肩關(guān)節(jié)舵機(jī)的優(yōu)化前后情況進(jìn)行對比。從圖10a中可以看出，算法優(yōu)化前后運行軌跡一致，沒有影響算法使用的目的；圖10b中可以看出由于步數(shù)插補(bǔ)使得優(yōu)化后的旋轉(zhuǎn)角度變化滯后于優(yōu)化前的旋轉(zhuǎn)角度曲線；圖10c中可以看出起動時轉(zhuǎn)速要求降低了近1/2，但當(dāng)步數(shù)補(bǔ)償結(jié)束后轉(zhuǎn)速發(fā)生了突變，隨后的變化趨勢與補(bǔ)償前一致。

使用直線解析式規(guī)劃軌跡和Bresenham算法及其補(bǔ)償優(yōu)化算法在運行測試在運行時間、末端執(zhí)行器誤差和起動角速度比較如表2所示。該測試是在生成同樣插值點數(shù)量條件下多次測試的結(jié)果，起動角速度選取了其中一個關(guān)節(jié)。

表2 算法運行測試結(jié)果

根據(jù)算法的仿真實驗結(jié)果，與直線解析式規(guī)劃軌跡方法比較，得出Bresenham算法的一些優(yōu)點和缺點。

使用Bresenham算法規(guī)劃結(jié)果的優(yōu)點有：

(1)空間上路徑為直線，距離最短，用時少；

(2)算法較簡潔，運算量較小；

(3)算法規(guī)避了除法運算，精確度高。

使用Bresenham算法規(guī)劃結(jié)果的缺點有：

(1)起動或制動時要求電機(jī)轉(zhuǎn)速突變，對電機(jī)沖擊大；

(2)插補(bǔ)步數(shù)稀疏時震動較大。

因此，該算法適合應(yīng)用于對機(jī)械臂的空間路徑要求非常嚴(yán)格的場合，并且希望時間盡可能縮短，選擇Bresenham直線插補(bǔ)算法較為合適。

從仿真結(jié)果中可以看出簡單的補(bǔ)償優(yōu)化前后算法運行軌跡完全一致，起動時的轉(zhuǎn)速要求降低了，得到了預(yù)期的優(yōu)化效果。但是簡單的補(bǔ)償算法帶來的問題也不可忽視：一是補(bǔ)償帶來電機(jī)起動減緩是以延長起動時間為代價，因此該算法運行用時少的優(yōu)點被弱化了；二是簡單補(bǔ)償前后存在轉(zhuǎn)速差，會對電機(jī)造成二次沖擊；三是起動步數(shù)增多可能會造成機(jī)械臂的抖動增加。因此實際使用時需考慮這些會對機(jī)械臂系統(tǒng)造成的影響。還可以考慮更復(fù)雜的變步數(shù)補(bǔ)償優(yōu)化方法，動態(tài)地補(bǔ)償起動步數(shù)，優(yōu)化算法的起動過程。

3 結(jié)語

本文針對機(jī)械臂末端軌跡規(guī)劃問題中的直線軌跡規(guī)劃，在采用直線軌跡規(guī)劃時使用微處理器處理斜率中除法運算不精確、運算量大的問題，引入Bresenham算法解決。建立機(jī)械臂的D-H參數(shù)數(shù)學(xué)模型，在該模型的基礎(chǔ)上根據(jù)模型參數(shù)設(shè)計的基于位姿矩陣的正運動學(xué)求解和基于簡化自由度的平面解析法的逆運動學(xué)求解，作為算法輸出機(jī)械臂指定位置時解算為主控制器控制舵機(jī)指令的基礎(chǔ)。使用Bresenham算法規(guī)劃機(jī)械臂的直線軌跡運動。Bresenham算法以比較待插值點距離的方式規(guī)避了除法運算，且運算量較小。而Bresenham算法規(guī)劃中存在對起動時起動和制動時轉(zhuǎn)速可能突變的缺陷，本文采用步數(shù)補(bǔ)償優(yōu)化加以解決。使用步數(shù)補(bǔ)償優(yōu)化有效降低了起動時的轉(zhuǎn)速要求。但也要注意使用簡單的步數(shù)補(bǔ)償優(yōu)化時帶來的時間延緩、二次沖擊等附加影響，實際需權(quán)衡使用。