基于Hermite方法的機器人任務(wù)空間二階平滑位姿路徑構(gòu)造

梁艷陽，王利，姚超智，王瓊瑤

(五邑大學(xué)智能制造學(xué)部, 廣東江門 529000)

0 前言

隨著科技的進(jìn)步，工業(yè)機器人被應(yīng)用于生活的各個領(lǐng)域中[1]，人們對工業(yè)機器人的需求越來越廣泛，對其精準(zhǔn)度、靈活性等性能的需求也日益增加[2]。在工業(yè)應(yīng)用中，對工業(yè)機器人進(jìn)行軌跡規(guī)劃是機器人運動控制的基礎(chǔ)，而構(gòu)造平滑且連續(xù)的位姿是保證工業(yè)機器人能夠?qū)崿F(xiàn)平穩(wěn)運動的核心技術(shù)[3-4]，是軌跡規(guī)劃中需要重點研究的領(lǐng)域[5-6]。

路徑構(gòu)造和優(yōu)化問題備受研究者們關(guān)注。文獻(xiàn)[7]在空地異構(gòu)機器人系統(tǒng)中為使空中無人機和地面無人車的路徑高度耦合，采用遺傳算法與蟻群算法相融合的方法對兩者的路徑進(jìn)行優(yōu)化求解，但該方法僅限于對均勻離散點的路徑規(guī)劃，沒辦法適用于各類復(fù)雜的工作環(huán)境；文獻(xiàn)[8]利用貝塞爾曲線對機器人路徑進(jìn)行優(yōu)化，此方法雖縮短了運動路徑，但無法滿足機器人在某些特定點的運動；文獻(xiàn)[9]通過引入基于三角不等式的剪枝策略對路徑拐點進(jìn)行平滑處理，從而優(yōu)化局部路徑段再達(dá)到優(yōu)化全局路徑段的目的，此方法雖然對路徑優(yōu)化做了一定的改進(jìn)，但改進(jìn)效果還有些不足。針對以上存在的問題，本文作者將機器人在運動過程中會遇到的所有復(fù)雜路徑進(jìn)行統(tǒng)一規(guī)劃，將機器人運動路徑分為基本路徑和過渡路徑，對兩種情況分別予以討論。

工業(yè)機器人的連續(xù)位姿路徑包括位置路徑和姿態(tài)路徑，位置路徑一般包括直線、圓弧等基本路徑。位姿路徑表達(dá)方式有旋轉(zhuǎn)矩陣、歐拉角、四元數(shù)、軸-角等方式，旋轉(zhuǎn)矩陣、歐拉角、軸-角在路徑插補或軌跡過渡時存在缺陷或者效率問題，因此用四元數(shù)[10-11]來統(tǒng)一描述姿態(tài)問題。而關(guān)于路徑過渡，過渡路徑包括位置過渡路徑和姿態(tài)過渡路徑[12]。Hermite曲線作為空間曲線，完全可以應(yīng)用于位置過渡和空間過渡的求解中。面對當(dāng)下工業(yè)機器人復(fù)雜的運動環(huán)境和各類不同的運動軌跡，也需要設(shè)計一種適用于各類不同路徑的過渡方法。本文作者應(yīng)用Hermite方法[13]在過渡區(qū)構(gòu)造曲線，使得機器人運動在遇到各類復(fù)雜的路徑情況時都可平穩(wěn)運行。

1 基本位姿路徑構(gòu)造

1.1 基本位置路徑構(gòu)造

路徑包括直線、圓弧、關(guān)節(jié)、姿態(tài)及其組合等多種情況。自動程序運行，原則上主要有直線段、圓弧和關(guān)節(jié)這3種基本路徑，但是在實際情況中，可能存在純姿態(tài)路徑，位置(直線、圓弧)和姿態(tài)路徑同時存在，過渡路徑(多段組合)。在這里作者主要討論直線段、圓弧這兩種基本的位置路徑構(gòu)造。

1.1.1 直線路徑構(gòu)造

P=v·x

(1)

式中：v表示運動方向，即v=(pe-ps)/L。

1.1.2 圓弧路徑構(gòu)造

圖1 構(gòu)造圓弧

定義

(2)

中垂線s1e1的單位方向向量ns1e1=n1，中垂線s2e2的單位方向向量ns2e2=n2，從而求得圓心位置

(3)

其中：|s2a1|為投影點a1到s2長度;|s2a2|是s2e2在s2a1上的投影長度。

p0ps的單位向量np0ps，p0pe的單位向量np0pe，圓弧角α=arccos(np0ps·np0pe)，圓弧上確定一個支持點psu，圓弧相對于參考坐標(biāo)系姿態(tài)為

R=[noa]

(4)

1.2 姿態(tài)路徑構(gòu)造

機器人姿態(tài)一般可通過旋轉(zhuǎn)矩陣、歐拉角、四元數(shù)、軸-角等方法表示。旋轉(zhuǎn)矩陣、歐拉角和軸-角在路徑插補或軌跡過渡時存在缺陷或者效率問題，因此本文作者以四元數(shù)統(tǒng)一處理姿態(tài)路徑構(gòu)造。

1.2.1 四元數(shù)表達(dá)式

四元數(shù)一般表達(dá)式為

q=w+xi+yj+zk

(5)

其中:w,x,y,z是實數(shù);i,j,k是虛部，且滿足下式。

(6)

四元數(shù)還能表示為：[w,v]或者[w,(x,y,z)]，式中w是標(biāo)量，v=(x,y,z)是矢量。如果|q|=1，則q為單位四元數(shù)。

1.2.2 四元數(shù)的指數(shù)和對數(shù)

q=[cosθ,sinθ·v]為單位四元數(shù)，則對數(shù)為

log(q)=[0,θ·v]

(7)

q=[0,θ·v]，則其指數(shù)為

exp(q)=[cosθ,sinθ·v]

(8)

1.2.3 四元數(shù)的共軛和逆

令q=[cosθ,sinθ·v]，則其共軛為

q*=[cosθ,-sinθ·v]

(9)

1.2.4 四元數(shù)乘法

q1=[s1,v1]，q2=[s2,v2]，則有：

q1q2=[s1s2-v1·v2,v1×v2+s1v2+s2v1]

(10)

1.2.5 球面線性插值

給定兩個單位四元數(shù)p和q，其球面線性插補計算公式為

Slerp(p,q,h)=p(p*q)h=(pq*)1-hq=

(qp*)hp=q(q*p)1-h=Slerp(q,p,1-h)

(11)

一階導(dǎo)數(shù)為

(12)

其弧長積分為

(13)

其中，可以計算確定|β|為兩個單位四元數(shù)向量之間的夾角大小，因此四元數(shù)球面線性插補的總弧長可以通過計算兩向量的夾角這種方式得到。

2 基于Hermite方法的二階平滑位姿路徑構(gòu)造

在連續(xù)軌跡運動時，相鄰路徑之間需要進(jìn)行平滑過渡[14]，因此，需要構(gòu)造兩段相鄰路徑之間的過渡路徑。過渡路徑的構(gòu)造涉及的問題較多，如：前后路徑參考坐標(biāo)系不同，任務(wù)空間前后路徑不在同一平面上，前后路徑分屬任務(wù)空間或關(guān)節(jié)空間，前后路徑分別有或無姿態(tài)，前后路徑分別有或無位置變化等。這些問題都可以通過適當(dāng)?shù)奶幚?，最后核心問題就是位置過渡路徑和姿態(tài)過渡路徑的平滑構(gòu)造。

2.1 位置過渡路徑構(gòu)造

位置過渡路徑一般分為3種情況：(1)直線與直線之間的過渡：(2)直線與圓弧之間的過渡;(3)圓弧與圓弧之間的過渡。3種情況下的過渡路徑原理相同，處理步驟為：(1)根據(jù)過渡半徑或過渡區(qū)大小確定過渡點以及支持點；(2)構(gòu)造Hermite路徑曲線；(3)數(shù)值積分方法計算曲線的弧長。

以圓弧與圓弧之間的過渡曲線為例，如圖2所示，給定任務(wù)空間位置過渡區(qū)大小(zone)，首先計算得到前后兩條路徑的過渡點為qa和qb，然后根據(jù)前后路徑曲線方程計算qa和qb處的兩條切線，在兩切線的延長線上選擇距離為d=min(Lpapb/4,Dmax)得到輔助點qc和qd，其中Lpapb為過渡點qa和qb的距離，Dmax為給定的最大距離參數(shù)。

圖2 任務(wù)空間位置過渡曲線

根據(jù)qa、qc、qd和qb，可以構(gòu)造3次Hermite曲線方程：

P(u)=(1-u)3P1+3(1-u)2uP2+3(1-u)·

u2P3+u3P4

(14)

上述曲線方程的一階導(dǎo)數(shù)為

(15)

其中：P1=qa,P2=qc,P3=qd,P4=qd。

2.2 姿態(tài)路徑的過渡路徑構(gòu)造

如圖3所示，根據(jù)給定的過渡區(qū)大小(zone)，計算確定過渡點qa和qb。姿態(tài)路徑采用球面四元數(shù)曲線，所以可以根據(jù)式(12)球面線性插值方程的一階導(dǎo)數(shù)計算角速度ωa和ωb，最終得到Hermite四元數(shù)插值曲線方程為

(16)

其中：

q0=qa

q1=qaexp(ωa/3)

q2=qb(exp(ωb/3))-1

q3=qb

該Hermite四元數(shù)插值曲線方程的一階導(dǎo)數(shù)為

(17)

3 位姿路徑長度計算及插值

3.1 基于數(shù)值積分方法計算路徑長度

(18)

上式一般需要通過數(shù)值方法進(jìn)行求解，只需要滿足工程上足夠的精度即可。數(shù)值積分的方法較多，這里采用4階Newton-Cotes積分方法進(jìn)行計算。其計算公式為

12f(x2)+32f(x3)+7f(x4)]

其中:xi=a+ih(h=(b-a)/4,i=0,1,2,3,4)。

設(shè)計如圖4所示的迭代算法[15]完成給定精度的位姿路徑長度計算:

圖4 計算位姿路徑長度的算法流程

3.2 基于數(shù)值積分及二分查找實現(xiàn)插值

在速度規(guī)劃中，一般給定路徑長度要求得到對應(yīng)的路徑點。直線、圓弧所描述的位置路徑以及一般的四元數(shù)表示的姿態(tài)路徑都可以直接得到解析解，而文中所構(gòu)造的過渡位姿路徑，只能通過數(shù)值算法計算得到給定精度的數(shù)值解。

令曲線參數(shù)方程為Y(t)，弧長方程為s(t)，給定弧長為s，用數(shù)值方法求取s對應(yīng)的曲線參數(shù)t。將所得t值代入到曲線參數(shù)方程Y(t)，即可得到給定弧長對應(yīng)的點坐標(biāo)。結(jié)合弧長的數(shù)值積分和二分查找算法，設(shè)計如圖5所示的算法實現(xiàn)給定路徑長度所對應(yīng)插值點的計算。

圖5 給定路徑長度查找插值算法

4 數(shù)值仿真及分析

為驗證文中方法的正確性和準(zhǔn)確性，將算法轉(zhuǎn)化為MATLAB代碼，進(jìn)行仿真實驗，驗證基于Hermite方法的機器人任務(wù)空間二階平滑連續(xù)位姿路徑構(gòu)造的可行性。

4.1 位置過渡曲線仿真

隨機選取一組數(shù)據(jù)，確定直線與圓弧之間的過渡曲線。如表1所示，隨機選取空間中三點(起點ps，中點pm，終點pe)確定一條空間圓弧，再在空間中選取兩點(起點ps，終點pe)確定一條直線，且直線和圓弧有交點，根據(jù)實際應(yīng)用情況確定過渡區(qū)及過渡點qa、qb，應(yīng)用Hermite方法構(gòu)造位置過渡曲線。仿真結(jié)果如圖6所示。

表1 隨機確定的圓弧和直線及過渡點

圖6 直線和圓弧過渡仿真結(jié)果

表2 隨機確定的兩圓弧及過渡點

圖7 圓弧與圓弧過渡仿真結(jié)果

根據(jù)以上兩種情況的位置過渡曲線以及仿真結(jié)果可以確定在已知過渡區(qū)或過渡點的情況下，根據(jù)Hermite方法構(gòu)造的過渡曲線可以達(dá)到平滑的位置過渡路徑的要求。

4.2 位姿過渡曲線仿真

如表3所示，隨機確定3組數(shù)據(jù)，每組數(shù)據(jù)為隨機選取的3個四元數(shù)作為示教點，通過3個示教點中的起點、中點和末點確定兩段直線或圓弧，根據(jù)實際應(yīng)用情況確定好過渡點或過渡區(qū)，從而根據(jù)Hermite方法構(gòu)造過渡曲線。

表3 隨機確定四元數(shù)曲線及過渡點

仿真結(jié)果如圖8—圖10所示。

圖8 第一組數(shù)據(jù)仿真結(jié)果

第一組3個四元數(shù)所構(gòu)成的兩段圓弧成90°角，在確定過渡點的情況下應(yīng)用Hermite方法構(gòu)造過渡曲線，且構(gòu)造的過渡路徑足夠平滑,如圖8所示。同理第二組3個四元數(shù)所構(gòu)成的兩段圓弧成小于90°角，如圖9所示；第三組3個四元數(shù)所構(gòu)成兩段圓弧成大于90°角，如圖10所示。因此可以得出結(jié)論:不論兩段圓弧所構(gòu)成角度是大于90°或小于90°，都可以在確定過渡點的情況下應(yīng)用Hermite方法構(gòu)造的過渡曲線，且構(gòu)造的過渡路徑足夠平滑。因此任意3個點都可通過該算法構(gòu)造出可供工業(yè)機器人平穩(wěn)運動的路徑。經(jīng)過仿真，驗證了該算法的通用性和有效性。

圖9 第二組數(shù)據(jù)仿真結(jié)果

圖10 第三組數(shù)據(jù)仿真結(jié)果

5 結(jié)論

為使機器人在復(fù)雜的工作環(huán)境中都可以平穩(wěn)運行，將Hermite方法應(yīng)用于機器人位置過渡路徑和姿態(tài)過渡路徑，并在MATLAB中進(jìn)行仿真實驗。實驗結(jié)果表明:Hermite方法可以滿足機器人在任意的運動環(huán)境或情況下的運動路徑平滑過渡的需求。此研究在前人研究的基礎(chǔ)上更加優(yōu)化了機器人路徑構(gòu)造算法，促進(jìn)了工業(yè)機器人運動軌跡方面研究的發(fā)展。此研究的遺留問題是工業(yè)機器人的軌跡規(guī)劃問題，在此研究的基礎(chǔ)上再進(jìn)一步進(jìn)行機器人軌跡規(guī)劃是接下來需要去研究的課題。