工業機器人倍四元數軌跡規劃算法的研究

李宏勝　汪允鶴　黃家才　施昕昕

南京工程學院，南京，211167

?

工業機器人倍四元數軌跡規劃算法的研究

李宏勝汪允鶴黃家才施昕昕

南京工程學院，南京，211167

針對工業機器人在笛卡爾空間的軌跡規劃曲線的平滑性與實時性，提出一種用于描述超球面旋轉的倍四元數對機器人運動軌跡進行球面線性插補，并結合正弦波加速度曲線升降速控制算法對軌跡規劃的速度進行控制，為機器人的軌跡規劃提供了一種新的位姿規劃的解決方案。在六關節搬運機器人控制系統平臺上進行了仿真與試驗，驗證了該算法解決方案的有效性。

工業機器人；倍四元數；正弦波加速度曲線；軌跡規劃

0　引言

工業機器人在笛卡爾任務空間內的軌跡規劃包括位置插補、姿態插補以及速度、加速度、加加速度曲線的規劃[1]。對于工業機器人的軌跡規劃，通常采用笛卡爾空間的歐拉角法、等效軸法對姿態參數進行插補，但歐拉角存在萬向死鎖的缺陷，而等效軸法在旋轉量為0時存在無法確定旋轉軸的問題[1]。使用四元數法[2-4]可解決機器人在姿態插補中遇到的上述問題，但軌跡的位置插補則需使用其他的插補算法[5]，運算復雜，影響控制系統對軌跡規劃的實時性要求。

本文對工業機器人在笛卡爾空間的軌跡規劃進行研究，在進行正弦波加速度曲線[6]運動速度控制的同時，采用倍四元數[7-8]將笛卡爾空間中的機器人位置與姿態轉換至四維空間，用超球面旋轉對機器人的運動軌跡進行球面線性插補，轉換過程誤差可控[9]，且其中的乘法運算比齊次矩陣坐標變換矩陣方法的乘法運算要少。因此，該方法不僅能夠滿足C2連續，并且運算量較小，具有較好的實時性。

1　姿態四元數描述

四元數在動畫制作、飛行器姿態控制以及工業機器人軌跡規劃等領域具有重要作用[3-4]，是實數、復數以及三維空間矢量的擴充，它由四個元素組成，包含一個實部和三個虛部，表達式為

Q=s+xi+yj+zk=s+u

(1)

其中，x、y、z為實數；s為四元數的實部；u為虛部，表示單位矢量旋轉軸(x,y,z)；i、j、k為虛數單位，并滿足i2=j2=k2=ijk=-1。

在三維空間坐標系OXYZ中，剛體繞單位矢量軸u旋轉σ角度，如圖1所示，可用單位四元數表示為

(2)

圖1　單位四元數旋轉示意圖

坐標系中任一點位置P1繞單位矢量軸u旋轉σ角度到達P2，用歐拉角表示為

P2=RP1

(3)

式中,R為3×3的單位旋轉矩陣。

用四元數表示剛體旋轉[10-11]，則有

P2=QP1Q*

(4)

式中,Q為對應旋轉矩陣R的單位四元數;Q*為四元數Q的共軛。

剛體旋轉的歐拉定理表明，任意旋轉都可以簡化為剛體繞著固定軸的一次旋轉。已知機器人的姿態旋轉矩陣R，其對應的旋轉四元數Q的實部s和虛部u如下：

(5)

式中,rij為姿態旋轉矩陣R中第i行第j列元素。

已知旋轉四元數Q，其對應的姿態旋轉矩陣R如下：

R=

(6)

2　機器人運動學方程的倍四元數表示

2.1倍四元數描述

倍四元數把三維空間中的位置和姿態變換統一表示為四維空間中的雙旋轉[7-8]，其表達形式為

(7)

其中，ξ和η為倍四元數符號，且滿足ξ2=ξ，η2=η，ξ+η=1，ξη=0；G部和H部均為單位四元數。

(8)

即倍四元數的G部和H部可單獨計算，從而可簡化姿態變換和倍四元數插值計算。

2.2數值計算法

給定三維空間中位姿矩陣的計算精度δ和機器人工作空間的最大邊界L時，可通過公式

Rl=L/δ1/2

(9)

得到四維空間超大球的半徑Rl。在笛卡爾空間中將機器人的位置和姿態變換矩陣轉換為倍四元數的算法如下：

(1)對如下機器人末端位姿矩陣T：

采用式(5)將姿態旋轉矩陣R轉換成旋轉四元數Q，且平移向量P=(px,py,pz)T。

(2)將三維空間的平移向量P近似轉換成四維空間的四元數，轉換公式如下：

D=cos(ψp/2)+sin(ψp/2)up

(10)

其中，ψp=|P|/Rl，up為平移向量上的單位矢量，up=P/|P|，當|P|=0時，up為零矢量。

(11)

其中，Q由式(5)所得，即機器人末端姿態對應的四元數。

(4)對倍四元數的雙旋轉軌跡進行離散化得到一系列插值倍四元數點，需要將其轉換成旋轉四元數和平移向量，則轉換算法如下：

Q=(G+H)/(2cosψ)

(12)

(13)

2.3連桿矩陣法

根據機器人連桿坐標系的D-H參數，將機器人相鄰兩個坐標系Oi-1Xi-1Yi-1Zi-1和OiXiYiZi之間的位姿關系用倍四元數進行描述。

坐標系Oi-1Xi-1Yi-1Zi-1繞Zi-1軸旋轉關節角度θi，沿Zi-1軸平移di(di為機器人相鄰連桿偏距)，即平移di=0i+0j+dik，用四元數表示為

Q=cos(θi/2)+0i+0j+sin(θi/2)k

(14)

結合給定的姿態矩陣R，由式(5)、式(10)、式(11)可轉換為倍四元數:

(15)

即

(16)

γi=|di|/Rl

同樣，坐標系Oi-1Xi-1Yi-1Zi-1繞Xi軸旋轉連桿轉角αi、平移ai=ai+0j+0k(ai為機器人關節連桿長度)，則可用倍四元數表示為

(17)

即

(18)

ρi=|ai|/Rl

由式(16)、式(18)，機器人各個關節齊次變換矩陣均使用倍四元數表示，運用式(8)倍四元數乘法，可得機器人末端執行器相對于基坐標系的位姿，用倍四元數表示為

即

(19)

式中，n為機器人關節數量。

由于倍四元數的G部和H部可獨立運算，故式(19)可用如下兩個表達式表示：

(20)

(21)

3　正弦波加速度曲線運動規劃

為滿足工業機器人軌跡的平滑性要求，采用正弦波加速度曲線速度控制算法對機器人軌跡進行升降速控制，如圖2所示，其加加速度呈現余弦曲線，可以保證機器人運動軌跡的平滑性，減小機器人關節機構的機械振動[6，12-13]。圖2中，ps、pe分別為起止位移，vs、ve分別為起止速度，va、vb為兩個過渡點速度，amax為最大加速度，jmax為最大加加速度。

圖2　正弦波加速度曲線運動規劃曲線圖

圖2中，在(0,TDmax)時間段內，加速度曲線的正弦函數方程為

(22)

式中，t為變化時間；TDmax為加速度從0加速到最大加速度amax的時間。

對式(22)進行一階求導，可得到加加速度的函數方程為

(23)

根據圖2 ，可將規劃軌跡劃分為三段，即位移段D1(加加速段)、位移段D2(勻加速段)和位移段D3(減加速段)，可得三個軌跡段間的兩個過渡點的速度分別為

(24)

根據劃分的三個位移段D1、D2和D3，可得對應軌跡段的加速度函數分別為

a(t)=

(25)

由式(25)，可得對應三個軌跡段D1、D2、D3的位移量分別為

(26)

(27)

(28)

根據式(25)～式(28)，可得隨時間變化的位置函數d(t)為

d(t)=

(29)

4　倍四元數的線性插值

基于倍四元數對機器人笛卡爾空間位姿的表達，使用一種利用四維空間超球面的雙旋轉來實現機器人笛卡爾空間曲線軌跡的插補方法進行曲線插補，該方法可實現機器人空間位姿的倍四元數插值，且其正弦波加速度曲線可保證軌跡規劃曲線的C2連續，從而保證機器人在笛卡爾空間能夠連續、平穩地完成指定軌跡路線[9，14-15]。

根據倍四元數的基本性質，倍四元數的G部和H部分別用單位四元數表示，在倍四元數的插值過程中可對G部和H部分別進行運算。因此，可將四元數的“球形線性插值法”(spherical linear interpolation，Slerp)運用在倍四元數中，完成倍四元數在超球面的線性插值。

G(t)=Slerp(Gs,Ge,l(t))=

(30)

H(t)=Slerp(Hs,He,l(t))=

(31)

ωg=arccos(Gs·Ge),ωh=arccos(Hs·He)

其中，Gs·Ge、Hs·He分別為Gs與Ge、Hs與He兩四元數的點積，l(t)∈[0,1]通過上述正弦波加速度曲線運動規劃獲得。

根據式(30)、式(31)，倍四元數的球面線性插值可表示為

(32)

f=(ωg,ωh)T

實際的插值計算過程包含倍四元數的G部和H部兩個部分的球面線性插值，即式(30)和(31)，通過這兩部分的單位四元數的球面線性插值完成倍四元數的球面線性插值。

5　實驗仿真與驗證

本文以埃夫特ER10L-C10型工業機器人為實驗平臺，該型號機器人適用于機床上下料、搬運、噴釉、打磨拋光等場合。采用上述倍四元數球面線性插補和正弦波加速度曲線運動速度控制對機器人末端空間軌跡位姿進行插補，利用MATLAB工具對軌跡規劃進行仿真與驗證。該型號工業機器人DH參數見表1。

表1　ER10L-C10型機器人連桿參數

使用倍四元數球面線性插值對工業機器人任務空間的工作路線進行軌跡規劃，利用Keba機器人控制器示教功能記錄機器人末端執行器工作路線的起止點位姿數據，通過ER10L-C10型機器人實驗平臺得到路線起止點處機器人執行器末端相對于基坐標系的位姿齊次矩陣：

其中，位置數據單位為mm。

通過上述倍四元數的球面線性插補與正弦波加速度曲線的運動速度控制，可得倍四元數軌跡的位置插補仿真曲線，如圖3、圖4所示。圖3所示為倍四元數軌跡的位置插補曲線與實際軌跡誤差較大的情況下所得結果，圖4所示為倍四元數軌跡的位置插補曲線與實際軌跡誤差較小情況下所得結果。由圖3和圖4可知，使用倍四元數對機器人軌跡插補所得曲線與實際曲線間存在誤差，其原因是將齊次矩陣轉化為倍四元數時是近似計算的過程，整個過程有一定的精度損失。但誤差大小是可控的，通過采用不同的數值計算精度δ、機器人工作空間的最大邊界L等參數，計算誤差可任意改變。

圖3　倍四元數法位置曲線1

圖4　倍四元數法位置曲線2

通過倍四元數球面線性插補獲得軌跡位置插補點的同時，可獲得相應的姿態插補數據，將其轉化為超球面的旋轉矢量表示，如圖5所示。可見，四維空間超球面的旋轉矢量過渡平滑。

圖5　倍四元數姿態變化曲線

由式(6)，將實時插補所得的旋轉四元數Q轉化為旋轉矩陣R，并經滾動-俯仰-偏航角(roll-pitch-yaw,RPY)姿態表示方法的逆變換可得繞X、Y、Z軸旋轉的角度α、β、γ(圖6)。可見，在笛卡爾坐標系中α、β、γ角的插補軌跡連續且平滑，說明利用倍四元數對機器人任務空間的姿態進行描述是可行的。

圖6　RPY逆變換旋轉角軌跡曲線

為顯示倍四元數插補過程中笛卡爾空間的位姿變化，由式(12)、式(13)將倍四元數轉化為旋轉四元數和位置矩陣，其旋轉角度ψ的變化曲線如圖7所示，ψ的平滑連續變化驗證了倍四元數進行軌跡規劃的有效性。

圖7　旋轉角度ψ的插補曲線

使用倍四元數進行軌跡規劃的過程中，采用了正弦波加速度曲線升降速控制，其運動規劃曲線如圖8所示。由圖8可見，加加速度曲線、加速度曲線、速度曲線以及位移曲線均符合機器人軌跡規劃的要求，其加速度曲線平滑連續，保證了C2連續，為機器人位置與姿態插補提供了平滑連續的速度控制。

圖8　運動規劃曲線

6　結語

將機器人任務空間的齊次變換矩陣分解為旋轉與平移兩部分，通過變換得到倍四元數的表達形式，然后使用在四維空間超球面旋轉的倍四元數，對機器人運動軌跡進行球面線性插補。雖然整個計算過程中存在精度損失，但通過采用不同的數值計算精度和控制參數，計算誤差可進行調節。該方法為機器人軌跡規劃提供了一種新的位姿規劃的解決方案，具有計算速度快、魯棒性強的特點；同時，針對軌跡的平滑性要求，采用正弦波加速度曲線進行升降速控制，能夠保證C2連續，且計算量相對較小，較好地滿足了機器人控制的實時性要求。測試和仿真結果證明了算法的有效性。

[1]CraigJJ.機器人學導論[M].牟超，譯.北京：機械工業出版社,2012.

[2]DamEB,KochM,LillholmM.Quaternions,InterpolationandAnimation[R].Copenhagen，Denmark：UniversityofCopenhagen,1998.

[3]季晨. 工業機器人姿態規劃及軌跡優化研究[D].哈爾濱：哈爾濱工業大學,2013.

[4]孫斌,常曉明,段晉軍.基于四元數的機械臂平滑姿態規劃與仿真[J].機械科學與技術,2015(1):56-59.

SunBin,ChangXiaoming,DuanJinjun.SmoothOrientationPlanningandSimulationofManipulatorBasedonQuaternion[J].MechanicalScienceandTechnologyforAerospaceEngineering, 2015(1):56-59.

[5]劉松國,朱世強,王宣銀,等. 基于四元數和B樣條的機械手平滑姿態規劃器[J]. 浙江大學學報(工學版),2009,43(7):1192-1196.

LiuSongguo,ZhuShiqiang,WangXuanyin,etal.SmoothOrientationPlannerforManipulatorsBasedonQuaternionandB-spline[J].JournalofZhejiangUniversity(EngineeringScience),2009,43(7):1192-1196.

[6]高亞軍.六自由度機器人平滑軌跡規劃與控仿一體化系統研究[D].杭州：浙江工業大學,2014.

[7]王健. 6R串聯機器人軌跡規劃算法研究[D].北京：北方工業大學,2014.

[8]喬曙光,廖啟征,黃昔光. 倍四元數及其在串聯機構運動分析中的應用[J]. 機械設計與制造,2008(2):36-38.

QiaoShuguang,LiaoQizheng,HuangXiguang.ApplicationofDoubleQuaternionsinInverseSolvingofSerialMechanisms[J].MachineryDesign&Manufacture, 2008(2):36-38.

[9]廖啟征. 機構運動學建模的倍四元數法[J]. 北京工業大學學報,2015,41(11):1611-1619.

LiaoQizheng.KinematicModelingofMechanismsSsingDoubleQuaternion[J].JournalofBeijingUniversityofTechnology,2015,41(11):1611-1619.

[10]馬艷紅,胡軍. 姿態四元數相關問題[J]. 空間控制技術與應用,2008,34(3):55-60.

MaYanhong,HuJun.OnAttitudeQuaternion[J].AerospaceControlandApplication, 2008,34(3):55-60.

[11]張勁夫,蔡泰信. 對偶四元數及其在剛體定位中的應用[J]. 黃淮學刊(自然科學版),1993,9(增刊4):27-30.

ZhangJinfu,CaiTaixin.DualQuaternionandItsApplicationinRigidPositioning[J].HuanghuaiJournal(NaturalScience),1993,9(S4):27-30.

[12]劉鵬飛,楊孟興,宋科,等. ‘S’型加減速曲線在機器人軌跡插補算法中的應用研究[J]. 制造業自動化,2012,34(20):4-8.

LiuPengfei,YangMengxing,SongKe,etal.TheStudyofS-curve’sApplicationonManipulator’sTrajectoryInterpolationAlgorithm[J].ManufacturingAutomation, 2012,34(20):4-8.

[13]林仕高,劉曉麟,歐元賢. 機械手笛卡爾空間軌跡規劃研究[J]. 機械設計與制造,2013(3):49-52.

LinShigao,LiuXiaolin,OuYuanxian.TheStudyofTrajectoryPlanningofManipulatorinCartesianSpace[J].MachineryDesign&Manufacture, 2013(3):49-52.

[14]杜濱. 全方位移動機械臂協調規劃與控制[D].北京：北京工業大學,2013.

[15]張紅強. 工業機器人時間最優軌跡規劃[D].長沙：湖南大學,2004.

(編輯蘇衛國)

Research on Trajectory Planning of Industrial Robots with Double Quaternion

Li HongshengWang YunheHuang JiacaiShi Xinxin

Nanjing Institute of Technology,Nanjing,211167

For the curve smoothness and real-time requirements of industrial robot trajectories in cartesian space, the paper represented hyperspheres rotation quaternion of robot trajectory spherical linear interpolation, combined with the algorithm of sinusoidal acceleration curve lifting velocity to control the trajectory planning. It provides a new solution of position and pose planning. Finally, simulations were performed in six joints handling robot control system platform, then to prove the validity of the algorithm solution.

industrial robot; double quaternion; sinusoidal acceleration curve; trajectory planning

2015-12-08

江蘇省科技支撐計劃資助項目(BE2014025)

TP242.2

10.3969/j.issn.1004-132X.2016.20.003

李宏勝，男，1966年生。南京工程學院自動化學院教授。研究方向為數控技術、高性能伺服驅動、機器人控制、智能控制。發表論文70余篇。汪允鶴(通信作者)，男，1990年生。南京工程學院自動化學院碩士研究生。黃家才，男，1977年生。南京工程學院自動化學院教授。施昕昕，女，1985年生。南京工程學院自動化學院副教授。