串聯(lián)型機械臂直線軌跡規(guī)劃Bresenham算法應用與改進*

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

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

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

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

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

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

1.1 機械臂的數(shù)學模型

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

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

表1 機械臂構件的D-H參數(shù)表

1.2 基于位姿矩陣的機械臂的正運動學求解

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

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

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

(1)

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

M=

(2)

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

(3)

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

1.3 基于簡化自由度的平面解析算法的機械臂的逆運動學求解

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

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

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

2.1 Bresenham算法介紹

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

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

y=kx+b

(4)

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

(5)

代入直線解析式(4)為

(6)

設像素長度和寬度為單位長度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)

差值的表達式中仍然存在斜率k，斜率k的表達式為

(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)

當xi+1處d1>d2時，則應取yi+1=yi+1，此時pi>0，由此可得

pi+1=2Δy-2Δx(yi+1-yi)+pi+2(Δy-Δx)

(13)

而當xi+1處d1

pi+1=2Δy-2Δx(yi+1-yi)+pi+2Δy

(14)

當xi+1處d1=d2時，可任取yi+1值是否在原值基礎上增長，按照上文選取點的約定，取yi+1=yi+1，因此式(13)適用于pi≥0的情況。

由此，根據(jù)式(13)和式(14)，可以在已知pi的結果時，可以順序推出后續(xù)所有點p的結果；而已知p的結果，就能判斷d1與d2的大小，進而判斷出點的選取結果[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)

等式兩側同時乘以Δ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，進而確定所有取值的位置。

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

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

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

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

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

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

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

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

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

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

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

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

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

表2 算法運行測試結果

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

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

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

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

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

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

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

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

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

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

3 結語

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