本發(fā)明涉及一種基于倍四元數(shù)的工業(yè)機(jī)器人自由曲線的軌跡規(guī)劃控制方法，屬于機(jī)器人軌跡規(guī)劃技術(shù)領(lǐng)域。

背景技術(shù)：
現(xiàn)代制造業(yè)對(duì)機(jī)器人性能的要求也越來(lái)越高，而機(jī)器人在任務(wù)空間的軌跡規(guī)劃算法在機(jī)器人控制系統(tǒng)中占有重要地位，直接影響著機(jī)器人末端運(yùn)動(dòng)性能和效率。且機(jī)器人運(yùn)動(dòng)控制中，基本的直線、圓弧軌跡曲線已不能滿足工業(yè)加工的應(yīng)用需求，而常用的B樣條曲線、Bézier曲線、Clothoid曲線，均不能用一種精確統(tǒng)一的表示方法描述標(biāo)準(zhǔn)解析曲線和自由曲線。通常情況下，機(jī)器人的期望軌跡是事先給定一系列笛卡爾或者關(guān)節(jié)空間的點(diǎn)，且給定通過(guò)該點(diǎn)的速度或兩點(diǎn)間的時(shí)間，另外還會(huì)限制機(jī)器人運(yùn)動(dòng)允許的最大速度，繼而分別實(shí)現(xiàn)機(jī)器人末端執(zhí)行器的位置軌跡規(guī)劃和姿態(tài)軌跡規(guī)劃，而工業(yè)機(jī)器人姿態(tài)軌跡規(guī)劃通常采用歐拉角法、等效軸法對(duì)姿態(tài)進(jìn)行插補(bǔ)，但歐拉角存在萬(wàn)向死鎖的缺陷，等效軸法在旋轉(zhuǎn)量為0時(shí)存在無(wú)法確定旋轉(zhuǎn)軸的問(wèn)題。雖然使用四元數(shù)法可解決機(jī)器人姿態(tài)軌跡插補(bǔ)的上述問(wèn)題，但軌跡的位置插補(bǔ)則需使用其它的插補(bǔ)算法，運(yùn)算量大，影響控制系統(tǒng)對(duì)軌跡規(guī)劃的實(shí)時(shí)性要求。

技術(shù)實(shí)現(xiàn)要素：
本發(fā)明所要解決的技術(shù)問(wèn)題是克服現(xiàn)有技術(shù)的缺陷，提供一種基于倍四元數(shù)的工業(yè)機(jī)器人NURBS曲線軌跡規(guī)劃控制方法，能夠?yàn)楣I(yè)機(jī)器人實(shí)現(xiàn)自由曲線軌跡規(guī)劃提供一種高效、高精度的控制方法。為解決上述技術(shù)問(wèn)題，本發(fā)明采用的技術(shù)方案如下：基于倍四元數(shù)的工業(yè)機(jī)器人自由曲線的軌跡規(guī)劃控制方法，包括以下步驟：1)建立基于倍四元數(shù)的機(jī)器人末端執(zhí)行器空間位姿的數(shù)學(xué)模型與機(jī)器人末端執(zhí)行器在任務(wù)空間NURBS描述的自由曲線數(shù)學(xué)模型；2)給定機(jī)器人末端執(zhí)行器在任務(wù)空間NURBS描述的自由曲線的控制點(diǎn)序列D，以及控制點(diǎn)對(duì)應(yīng)姿態(tài)R；所述控制點(diǎn)序列D表示為：D＝{d0，d1，…，dn}，n為控制點(diǎn)個(gè)數(shù)；所述控制點(diǎn)對(duì)應(yīng)姿態(tài)采用姿態(tài)旋轉(zhuǎn)矩陣R3×3表示；3)依據(jù)哈特利-賈德法求解所述步驟2)的控制點(diǎn)序列中控制點(diǎn)對(duì)應(yīng)的節(jié)點(diǎn)矢量U，具體過(guò)程如下：為給定的控制點(diǎn)di，i＝0,1,…,n，預(yù)定義一條k次非均勻有理B樣條曲線，同時(shí)確定它的節(jié)點(diǎn)矢量U＝[u0,u1,…,un+k+1]中的具體的節(jié)點(diǎn)值，節(jié)點(diǎn)值的求解如下：將兩端節(jié)點(diǎn)的重復(fù)度取為k+1，將曲線的定義域取成規(guī)范參數(shù)域，即u∈[uk,un+1]＝[0,1]，于是u0＝u1＝…＝uk＝0，un+1＝un+2＝…＝un+k+1＝1，其余節(jié)點(diǎn)值需計(jì)算求解，如下：計(jì)算公式如下：式中，lj為控制多邊形的各邊長(zhǎng)，lj＝|dj-dj-1|，由式(4)可得：繼而可得所有的節(jié)點(diǎn)值；其中，u為節(jié)點(diǎn)矢量U相鄰兩個(gè)節(jié)點(diǎn)間的密化節(jié)點(diǎn)值，ui，i＝0,1,……,n+k+1，表示節(jié)點(diǎn)矢量U中具體的某個(gè)節(jié)點(diǎn)值；4)依據(jù)Adams微分方程理論算法對(duì)節(jié)點(diǎn)矢量U進(jìn)行密化處理，具體過(guò)程如下：采用三步四階Adams微分方程的隱格式表示為：其中，T為插補(bǔ)周期，分別為ui-2、ui-1、ui、ui+1的一階導(dǎo)數(shù)；將代入上式，可得：ΔLi表示控制點(diǎn)di的進(jìn)給步長(zhǎng)；采用前、后向差分結(jié)合代替微分的方法進(jìn)行簡(jiǎn)化：后向差分，前向差分，前向差分，前向差分將上式代入式(7)，得到：進(jìn)而得到簡(jiǎn)化后的Adams微分方程插補(bǔ)算法迭代公式：繼而可得密化后節(jié)點(diǎn)矢量，其中，表示ui+1的預(yù)估值；5)根據(jù)步驟1)中的機(jī)器人末端執(zhí)行器空間位姿的數(shù)學(xué)模型，并采用自適應(yīng)速度控制算法對(duì)密化后的節(jié)點(diǎn)矢量進(jìn)行修正處理，最終獲得最優(yōu)的密化節(jié)點(diǎn)矢量，修正處理過(guò)程如下：將參數(shù)作為參數(shù)插補(bǔ)的預(yù)估值代入NURBS方程，得到相對(duì)應(yīng)的預(yù)估插補(bǔ)點(diǎn)：表示預(yù)估值的預(yù)估插補(bǔ)點(diǎn)，從而得到對(duì)應(yīng)的預(yù)估進(jìn)給步長(zhǎng)為：預(yù)估進(jìn)給步長(zhǎng)和進(jìn)給步長(zhǎng)ΔLi之間存在的偏差，用相對(duì)誤差δi來(lái)表示：當(dāng)相對(duì)誤差δi在允許范圍內(nèi)時(shí)，則為所求p(ui+1)，否則按下式進(jìn)行修正，直至達(dá)到δi允許范圍內(nèi)：最終獲得最優(yōu)的密化節(jié)點(diǎn)矢量；6)利用步驟5)的最優(yōu)的密化節(jié)點(diǎn)矢量，并根據(jù)步驟1)中機(jī)器人末端執(zhí)行器在任務(wù)空間NURBS描述的自由曲線數(shù)學(xué)模型，最終獲得曲線上的插補(bǔ)點(diǎn)位置；7)根據(jù)相鄰空間曲線的插補(bǔ)點(diǎn)位置與姿態(tài)數(shù)據(jù)，進(jìn)行倍四元數(shù)轉(zhuǎn)換，具體步驟如下：7-1)對(duì)于每一個(gè)機(jī)器人末端位姿齊次變換矩陣BTE，如下所示：首先將姿態(tài)旋轉(zhuǎn)矩陣R3×3，經(jīng)旋轉(zhuǎn)矩陣與四元數(shù)的轉(zhuǎn)換關(guān)系，得到姿態(tài)旋轉(zhuǎn)矩陣對(duì)應(yīng)的旋轉(zhuǎn)四元數(shù)Q，同時(shí)獲得平移向量P＝[px(u),py(u),pz(u)]T；7-2)將三維空間的平移向量P轉(zhuǎn)換成四維空間的四元數(shù)，轉(zhuǎn)換公式如下：Dp＝cos(ψ/2)+sin(ψ/2)v(16)式中，Rl為四維空間的大球半徑，ψ＝|P|/Rl，v是平移向量上的單位矢量，v＝P/|P|；當(dāng)|P|＝0時(shí)，v為零矢量；7-3)通過(guò)以下公式，計(jì)算得到機(jī)器人末端位姿轉(zhuǎn)換至四維空間的倍四元數(shù)空間位姿的G部和H部；式中，為四元數(shù)Dp的共軛；7-4)倍四元數(shù)的雙旋轉(zhuǎn)軌跡進(jìn)行離散化得到一系列插值倍四元數(shù)點(diǎn)，需要將其轉(zhuǎn)換成旋轉(zhuǎn)四元數(shù)和平移向量，轉(zhuǎn)換算法如下：Q＝(G+H)/(2cosψ)(17)式中，8)對(duì)插補(bǔ)所得機(jī)器人末端執(zhí)行器位姿進(jìn)行逆運(yùn)動(dòng)學(xué)處理，獲得關(guān)節(jié)角度，并驅(qū)動(dòng)關(guān)節(jié)運(yùn)動(dòng)。前述的步驟1)中，基于倍四元數(shù)的機(jī)器人末端執(zhí)行器空間位姿的數(shù)學(xué)模型為：其中，表示機(jī)器人末端執(zhí)行器倍四元數(shù)空間位姿，ξ和η滿足ξ2＝ξ，η2＝η，ξ+η＝1，ξη＝0，G和H均為單位四元數(shù)；機(jī)器人末端執(zhí)行器在任務(wù)空間NURBS描述的自由曲線數(shù)學(xué)模型為：任意一條k次NURBS曲線均表示為一分段有理多項(xiàng)式矢函數(shù)：其中，p(u)表示機(jī)器人末端執(zhí)行器在任務(wù)空間NURBS描述的自由曲線的位置矢量，ωi稱為權(quán)因子；di為自由曲線控制點(diǎn)；n為控制點(diǎn)個(gè)數(shù)；Ni,k(u)是由節(jié)點(diǎn)矢量U＝[u0,u1,…,un+k+1]決定的B樣條基函數(shù)，由德布爾-考克斯遞推定義公式表示：式中，規(guī)定u為節(jié)點(diǎn)矢量U相鄰兩個(gè)節(jié)點(diǎn)間的密化節(jié)點(diǎn)值，ui，i＝0,1,……,n+k+1，表示節(jié)點(diǎn)矢量U中具體的某個(gè)節(jié)點(diǎn)值。本發(fā)明所達(dá)到的有益效果：本發(fā)明能夠?yàn)閷?shí)現(xiàn)工業(yè)機(jī)器人在笛卡爾空間的NURBS自由曲線的軌跡規(guī)劃，提供一種有效提高工業(yè)機(jī)器人的工作效率和工作質(zhì)量、能夠減小速度波動(dòng)、改善機(jī)器人的工作環(huán)境的控制方法。附圖說(shuō)明圖1為本發(fā)明基于倍四元數(shù)的工業(yè)機(jī)器人自由曲線的軌跡規(guī)劃控制方法流程示意圖；圖2為本發(fā)明基于倍四元數(shù)的工業(yè)機(jī)器人自由曲線的軌跡規(guī)劃控制方法的倍四元數(shù)表述空間直線段位置的示意圖；圖3為本發(fā)明基于倍四元數(shù)的工業(yè)機(jī)器人自由曲線的軌跡規(guī)劃控制方法的倍四元數(shù)表述空間直線段姿態(tài)的示意圖，圖中箭頭方向?yàn)樽藨B(tài)用四元數(shù)表示的旋轉(zhuǎn)矢量軸。具體實(shí)施方式下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步描述。以下實(shí)施例僅用于更加清楚地說(shuō)明本發(fā)明的技術(shù)方案，而不能以此來(lái)限制本發(fā)明的保護(hù)范圍。自由曲線概念的提出就是為了描述比較復(fù)雜的幾何形狀，以提高工業(yè)機(jī)器人的加工效率和精度。而非均勻有理B樣條(NURBS，Non-UniformRationalB-Spline)曲線，可以準(zhǔn)確的透視出曲線造型的控制點(diǎn)分布特征，并可以有效的解決型值點(diǎn)不能均勻分布的缺點(diǎn)。倍四元數(shù)是基于Clifford代數(shù)的一種新的數(shù)學(xué)建模工具，是在四元數(shù)的基礎(chǔ)上發(fā)展而來(lái)。采用倍四元數(shù)能夠?qū)Φ芽柨臻g中的位置與姿態(tài)轉(zhuǎn)換至四維空間中，既能夠分別得表示空間中的位置和姿態(tài)，也能將空間中的位置和姿態(tài)用一種統(tǒng)一的方式進(jìn)行表示，從而對(duì)四維空間中起止點(diǎn)的位姿用超球面旋轉(zhuǎn)對(duì)機(jī)器人的運(yùn)動(dòng)軌跡進(jìn)行球面線性插補(bǔ)。如圖1所示，本發(fā)明的基于倍四元數(shù)的工業(yè)機(jī)器人自由曲線的...