梁艷陽(yáng)，王利，姚超智，王瓊瑤

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

0 前言

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

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

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

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

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

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

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

P=v·x

(1)

式中：v表示運(yùn)動(dòng)方向，即v=(pe-ps)/L。

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

圖1 構(gòu)造圓弧

定義

(2)

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

(3)

其中：|s2a1|為投影點(diǎn)a1到s2長(zhǎng)度;|s2a2|是s2e2在s2a1上的投影長(zhǎng)度。

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

R=[noa]

(4)

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

機(jī)器人姿態(tài)一般可通過(guò)旋轉(zhuǎn)矩陣、歐拉角、四元數(shù)、軸-角等方法表示。旋轉(zhuǎn)矩陣、歐拉角和軸-角在路徑插補(bǔ)或軌跡過(guò)渡時(shí)存在缺陷或者效率問(wè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í)數(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ù)和對(duì)數(shù)

q=[cosθ,sinθ·v]為單位四元數(shù)，則對(duì)數(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 球面線性插值

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

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)

其弧長(zhǎng)積分為

(13)

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

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

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

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

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

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

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

根據(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)路徑的過(guò)渡路徑構(gòu)造

如圖3所示，根據(jù)給定的過(guò)渡區(qū)大小(zone)，計(jì)算確定過(guò)渡點(diǎn)qa和qb。姿態(tài)路徑采用球面四元數(shù)曲線，所以可以根據(jù)式(12)球面線性插值方程的一階導(dǎo)數(shù)計(jì)算角速度ω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 位姿路徑長(zhǎng)度計(jì)算及插值

3.1 基于數(shù)值積分方法計(jì)算路徑長(zhǎng)度

(18)

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

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

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

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

圖4 計(jì)算位姿路徑長(zhǎng)度的算法流程

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

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

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

圖5 給定路徑長(zhǎng)度查找插值算法

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

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

4.1 位置過(guò)渡曲線仿真

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

表1 隨機(jī)確定的圓弧和直線及過(guò)渡點(diǎn)

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

表2 隨機(jī)確定的兩圓弧及過(guò)渡點(diǎn)

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

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

4.2 位姿過(guò)渡曲線仿真

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

表3 隨機(jī)確定四元數(shù)曲線及過(guò)渡點(diǎn)

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

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

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

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

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

5 結(jié)論

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