本發(fā)明涉及諸如焊接、切割、噴涂、搬運(yùn)等工業(yè)機(jī)器人先進(jìn)制造產(chǎn)業(yè)技術(shù)領(lǐng)域，特別是涉及一種參數(shù)自適應(yīng)密化的機(jī)器人NURBS曲線運(yùn)動(dòng)插補(bǔ)方法。

背景技術(shù)：

工業(yè)機(jī)器人控制系統(tǒng)要求能夠在一定的控制策略的作用下，使機(jī)器人能夠快速準(zhǔn)確地根據(jù)預(yù)先規(guī)劃的軌跡運(yùn)行，其中，插補(bǔ)算法作為實(shí)現(xiàn)工業(yè)機(jī)器人運(yùn)動(dòng)控制的軟件核心，是實(shí)現(xiàn)工業(yè)機(jī)器人復(fù)雜軌跡規(guī)劃算法的基礎(chǔ)，其性能決定著工業(yè)機(jī)器人運(yùn)動(dòng)的實(shí)時(shí)性和精度的好壞，是工業(yè)機(jī)器人機(jī)器人運(yùn)動(dòng)控制特性的重要評價(jià)指標(biāo)之一。

傳統(tǒng)的插補(bǔ)算法主要有直線和圓弧插補(bǔ)等，當(dāng)需要復(fù)雜曲線時(shí)，需要通過直線或圓弧的逼近來完成，這種實(shí)現(xiàn)方法存在以下問題：①采用直線或圓弧逼近時(shí)，會(huì)產(chǎn)生大量的端點(diǎn)數(shù)據(jù)，造成數(shù)據(jù)量過大；②大量的小線段插補(bǔ)引起進(jìn)給速度的波動(dòng)，影響加工效率和精度。

隨著復(fù)雜曲線加工需求的增多以及加工精度和效率要求越來越高，曲線插補(bǔ)算法成為研究熱點(diǎn)。其中，NURBS(Non-Uniform Rational B-Spline，非均勻有理B樣條)描述的曲線曲面技術(shù)得到重視，并在CNC中得到廣泛研究，也引起了機(jī)器人界的關(guān)注。NURBS曲線插補(bǔ)算法可以應(yīng)用于復(fù)雜軌跡插補(bǔ)，但計(jì)算過程復(fù)雜，且機(jī)器人關(guān)節(jié)在加減速過程中存在振動(dòng)和沖擊。總的來說，上述常用的工業(yè)機(jī)器人運(yùn)動(dòng)軌跡插補(bǔ)方法存在以下問題：1)現(xiàn)有的插補(bǔ)算法應(yīng)用于復(fù)雜曲線插補(bǔ)過程的運(yùn)動(dòng)平穩(wěn)性和運(yùn)動(dòng)精度較低，導(dǎo)致目前對于復(fù)雜曲線插補(bǔ)問題仍然沒有完善的解決方法；2)現(xiàn)有的NURBS插補(bǔ)算法多采用前瞻方法來確定減速點(diǎn)的位置，只能對減速過程進(jìn)行規(guī)劃，而不能完全保證加速過程的柔順性，無法應(yīng)用于工業(yè)場合；3)現(xiàn)有的插補(bǔ)算法不能解決在曲率變化大的地方，所需減速距離長的情況，導(dǎo)致很難滿足生產(chǎn)過程的安全要求。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是為了克服已有的技術(shù)缺陷，解決基于三次NURBS曲線的工業(yè)機(jī)器人運(yùn)動(dòng)軌跡插補(bǔ)方法中復(fù)雜曲線插補(bǔ)過程的運(yùn)動(dòng)平穩(wěn)性和運(yùn)動(dòng)精度較低問題、只能對減速過程進(jìn)行規(guī)劃而不能完全保證加速過程的柔順性問題以及在曲率變化較大的地方所需減速距離較長的問題，本發(fā)明提供一種參數(shù)自適應(yīng)密化的機(jī)器人NURBS曲線運(yùn)動(dòng)插補(bǔ)方法，實(shí)現(xiàn)工業(yè)機(jī)器人高效率、高精度且兼顧柔性的軌跡插補(bǔ)。

本發(fā)明采用一種參數(shù)自適應(yīng)密化的機(jī)器人NURBS曲線運(yùn)動(dòng)插補(bǔ)方法，該方法包括以下步驟：

步驟10)根據(jù)輸入的運(yùn)動(dòng)參數(shù)，獲得NURBS曲線表達(dá)式及參數(shù)密化過程中采用的導(dǎo)函數(shù)表達(dá)式；

步驟20)利用改進(jìn)的Admas微分方程，對步驟10)輸入的運(yùn)動(dòng)參數(shù)中的節(jié)點(diǎn)矢量u進(jìn)行預(yù)估，并根據(jù)約束條件，自適應(yīng)對運(yùn)動(dòng)參數(shù)中的節(jié)點(diǎn)矢量u進(jìn)行校正；

步驟30)通過分段進(jìn)行S型曲線位置插補(bǔ)，對自適應(yīng)過程中的運(yùn)動(dòng)參數(shù)進(jìn)行平滑處理；

步驟40)采用球面線性插值slerp方法完成姿態(tài)插補(bǔ)，得到的位置插補(bǔ)坐標(biāo)和姿態(tài)插補(bǔ)坐標(biāo)；

步驟50)對步驟40)得到的位置插補(bǔ)坐標(biāo)和姿態(tài)插補(bǔ)坐標(biāo)，進(jìn)行機(jī)器人正逆解計(jì)算，得到關(guān)節(jié)角插補(bǔ)序列。

作為優(yōu)選例：所述的步驟10)具體包括：

步驟101)確定三次NURBS曲線表達(dá)式：對于給定的控制頂點(diǎn){d₀,d₁,...,d_n}及其權(quán)值{ω₀,ω₁,...,ω_n}和節(jié)點(diǎn)矢量{u₀,u₁,...,u_n+k+1}，其中，k＝3，得到三次NURBS曲線表達(dá)式p(u)，如式(1)所示：

其中，ω_i表示控制頂點(diǎn)d_i的權(quán)值，d_i表示第i個(gè)控制頂點(diǎn)，N_i,3表示3次B樣條基函數(shù)，B₃、B₂、B₁和B₀分別為分子系數(shù)；b₃、b₂、b₁和b₀分別為分母系數(shù)；

步驟102)求解三次NURBS曲線第i-2段表達(dá)式：

令其中3≤j≤n+1，代入式(1)，得到式(2)：

式中，d_j表示第j個(gè)控制頂點(diǎn)，ω_j表示控制頂點(diǎn)d_j的權(quán)值，N_j,3表示3次B樣條基函數(shù)，d_i-3表示第i-3個(gè)控制頂點(diǎn)，ω_i-3表示控制頂點(diǎn)d_i-3的權(quán)值，d_i-2表示第i-2個(gè)控制頂點(diǎn)，ω_i-2表示控制頂點(diǎn)d_i-2的權(quán)值，d_i-1表示第i-1個(gè)控制頂點(diǎn)，ω_i-1表示控制頂點(diǎn)d_i-1的權(quán)值，d_i表示第i個(gè)控制頂點(diǎn)，ω_i表示控制頂點(diǎn)d_i的權(quán)值，上標(biāo)T表示轉(zhuǎn)置，

步驟103)求解NUBRS曲線第i-2段表達(dá)式的分子系數(shù)和分母系數(shù)：將M_i代入式(2)，得分子系數(shù)和分母系數(shù)分別為：

步驟104)計(jì)算NURBS曲線的導(dǎo)函數(shù)表達(dá)式：首先在式(2)所示的NURBS曲線表達(dá)式上，利用求導(dǎo)公式對節(jié)點(diǎn)矢量u求導(dǎo)，獲得NURBS曲線的一階導(dǎo)函數(shù)；然后對一階導(dǎo)函數(shù)再次利用求導(dǎo)公式對節(jié)點(diǎn)矢量u求導(dǎo)，獲得NURBS曲線的二階導(dǎo)函數(shù)。

作為優(yōu)選例：所述的步驟20)具體包括：

步驟201)密化自變量u：

當(dāng)參數(shù)u索引i>2時(shí)，根據(jù)式(6)預(yù)估下一個(gè)參數(shù)；

當(dāng)參數(shù)u索引1≤i≤2時(shí)，采用一階泰勒級數(shù)展開，如式(3)所示：

式中，u_i+1表示規(guī)劃節(jié)點(diǎn)矢量，u_i表示已知節(jié)點(diǎn)矢量，ΔL_i表示規(guī)劃周期內(nèi)的步長，T表示規(guī)劃周期，表示機(jī)器人末端速度，p_x(u)表示直角坐標(biāo)系下x軸方向位置插補(bǔ)序列，p_y(u)表示直角坐標(biāo)系下y軸方向位置插補(bǔ)序列，p_z(u)表示直角坐標(biāo)系下z軸方向位置插補(bǔ)序列；

根據(jù)三步四階Admas微分方程，表示參數(shù)密化的遞推公式如式(4)所示：

其中，u_i+1表示規(guī)劃節(jié)點(diǎn)矢量，表示規(guī)劃節(jié)點(diǎn)矢量的導(dǎo)數(shù)，u_i表示已知的第i個(gè)節(jié)點(diǎn)矢量，表示已知的第i個(gè)節(jié)點(diǎn)矢量的導(dǎo)數(shù)，表示已知的第i-1個(gè)節(jié)點(diǎn)矢量的導(dǎo)數(shù)，表示已知的第i-2個(gè)節(jié)點(diǎn)矢量的導(dǎo)數(shù)；

步驟202)用差分代替微分：式(4)中的微分采用式(5)替換為差分，如式(6)所示：

其中，表示預(yù)估參數(shù)；

步驟203)計(jì)算相對誤差：根據(jù)預(yù)估參數(shù)獲得預(yù)估點(diǎn)位置坐標(biāo)并獲得相應(yīng)的預(yù)估步長如式(7)所示：

其中，表示預(yù)估步長，表示預(yù)估點(diǎn)x軸位置坐標(biāo)，表示預(yù)估點(diǎn)y軸位置坐標(biāo)，表示預(yù)估點(diǎn)z軸位置坐標(biāo)，x_i表示已知點(diǎn)x軸位置坐標(biāo)，y_i表示已知點(diǎn)y軸位置坐標(biāo)，z_i表示已知點(diǎn)z軸位置坐標(biāo)；

計(jì)算預(yù)估步長與期望步長ΔL_i的相對誤差δ_i，如式(8)所示：

其中，期望步長ΔL_i為自適應(yīng)步長調(diào)節(jié)過程中獲得的步長；

步驟204)校正預(yù)估參數(shù)：如果相對誤差δ_i超出給定閾值，則采用式(9)對預(yù)估參數(shù)進(jìn)行校正：

其中，表示預(yù)估的插補(bǔ)點(diǎn)參數(shù)；

若預(yù)估的插補(bǔ)點(diǎn)數(shù)目大于等于1，則對預(yù)估的插補(bǔ)點(diǎn)進(jìn)行位置插補(bǔ)，否則對。

作為優(yōu)選例：所述的步驟203)中，計(jì)算期望步長具體包括：

步驟2031)計(jì)算弦高誤差約束：采用式(10)計(jì)算弦高誤差：

其中，為預(yù)估的插補(bǔ)點(diǎn)參數(shù)，u_i為已知的插補(bǔ)點(diǎn)參數(shù)，p_i為u_i的插補(bǔ)點(diǎn)坐標(biāo)，為的插補(bǔ)點(diǎn)坐標(biāo)，為和u_i的中位點(diǎn)坐標(biāo)，e_i為預(yù)估點(diǎn)與已知點(diǎn)的距離，為弦高誤差；

設(shè)允許的最大弦高誤差為h_max，則最大弦高誤差約束下，允許的最大步長ΔL_i1如式(11)所示：

步驟2032)計(jì)算最大進(jìn)給速度約束：在給定最大進(jìn)給速度v_max約束下，允許的最大步長ΔL_i2如式(12)所示：

ΔL_i2＝v_maxT 式(12)

步驟2033)計(jì)算給定最大進(jìn)給加速度約束：在給定最大進(jìn)給加速度約束下，允許的步長ΔL_i3如式(13)所示，其中，進(jìn)給加速度不能超過給定最大進(jìn)給加速度a_max；

ΔL_i-1-a_maxT²≤ΔL_i3≤ΔL_i-1+a_maxT² 式(13)

其中，ΔL_i-1表示第i-2插補(bǔ)點(diǎn)和第i-1插補(bǔ)點(diǎn)間的步長；

即步長滿足式(14)：

其中，ΔL_i表示第i-1插補(bǔ)點(diǎn)和第i插補(bǔ)點(diǎn)間的步長；

如果ΔL_i3>ΔL_i-1+a_maxT²，則令ΔL_i3＝ΔL_i-1+a_maxT²；如果ΔL_i-1-a_maxT²>ΔL_i3，則令ΔL_i3＝ΔL_i-1-a_maxT²；

步驟2034)計(jì)算給定最大法向加速度約束：

首先根據(jù)NURBS插補(bǔ)過程中進(jìn)給速度v_i與法向加速度a_ni的關(guān)系，采用式(15)計(jì)算法向加速度a_ni：

式中，ρ_i表示ΔL_i的曲率半徑；

設(shè)允許的最大法向加速度為a_nmax，采用式(16)獲得最大法向加速度約束下允許的最大步長ΔL_i4：

根據(jù)式(15)和式(16)，得出即最大法向加速度約束下允許的最大步長為ΔL_i4，如式(17)所示：

步驟2035)步長自適應(yīng)優(yōu)化調(diào)節(jié)：在自適應(yīng)步長規(guī)劃過程中，當(dāng)前段的期望步長ΔL_i滿足式(18)：

通過上述自適應(yīng)步長調(diào)節(jié)獲得一系列期望步長序列，然后選取步長序列中的極小值點(diǎn)作為曲率最大的點(diǎn)，即拐點(diǎn)，最后根據(jù)獲得的拐點(diǎn)將期望步長序列分成若干段，并記錄每段的總長度，以及拐點(diǎn)處的期望步長。

作為優(yōu)選例：所述的步驟30)具體包括：首先根據(jù)拐點(diǎn)步長和各段的長度，對每段采用七段式S曲線速度規(guī)劃，獲得速度序列，從而得到各個(gè)周期的進(jìn)給步長；然后再次通過參數(shù)密化過程獲得參數(shù)u序列；最后每隔插補(bǔ)周期T＇取參數(shù)u，獲得在直角坐標(biāo)系下的位置插補(bǔ)序列p(u)。

作為優(yōu)選例：所述的步驟40)具體包括：

步驟401)采用四元數(shù)表示姿態(tài)：將起點(diǎn)、終點(diǎn)姿態(tài)以及中間插補(bǔ)姿態(tài)采用四元數(shù)的表示形式，記法如下：

[r,q]＝[r,(R,P,Y)]

其中，r為姿態(tài)插補(bǔ)系數(shù)，取值范圍為[0，1]；q為姿態(tài)向量；R為插補(bǔ)點(diǎn)的α旋轉(zhuǎn)軸的姿態(tài)值，P為插補(bǔ)點(diǎn)的β旋轉(zhuǎn)軸的姿態(tài)值，Y為插補(bǔ)點(diǎn)的γ旋轉(zhuǎn)軸的姿態(tài)值；

步驟402)選定姿態(tài)插補(bǔ)系數(shù)r：將已完成位置插補(bǔ)的總步長之和與NURBS曲線的總長作為姿態(tài)插補(bǔ)系數(shù)r；

步驟403)采用slerp插補(bǔ)方法完成姿態(tài)插補(bǔ)，如式(19)所示：

q_r＝k₀q_beg+k₁q_end 式(19)

其中，q_r表示系數(shù)r的姿態(tài)向量，q_beg表示起始姿態(tài)向量，q_end表示終止姿態(tài)向量，k₀表示起始點(diǎn)系數(shù)，k₁表示終止點(diǎn)系數(shù)；

ω表示q_beg到q_end弧所截的角；

采用式(20)所示slerp插補(bǔ)方法完成姿態(tài)插補(bǔ)：

作為優(yōu)選例：所述步驟50)具體為：首先根據(jù)位置插補(bǔ)坐標(biāo)和姿態(tài)插補(bǔ)坐標(biāo)，經(jīng)機(jī)器人正逆解計(jì)算得到對應(yīng)的關(guān)節(jié)角插補(bǔ)序列，然后將該序列發(fā)送給伺服控制器，以控制機(jī)器人完成相應(yīng)的運(yùn)動(dòng)操作。

有益結(jié)果：本發(fā)明采用參數(shù)自適應(yīng)密化的方法有效地解決了NURBS曲線插補(bǔ)加減速過程的柔順性問題以及曲率變化較大的地方所需減速距離較長的問題，通過改進(jìn)的Adams微分方程方法及自適應(yīng)步長調(diào)節(jié)對參數(shù)進(jìn)行預(yù)估-校正，從弦高誤差、給定最大進(jìn)給速度、給定最大進(jìn)給加速度和給定最大法向加速度四個(gè)約束條件下獲得步長序列，提高了預(yù)估參數(shù)的精度，通過規(guī)劃周期T與插補(bǔ)周期T＇不同的方式，對自適應(yīng)參數(shù)密化過程進(jìn)行進(jìn)一步密化，降低規(guī)劃誤差。結(jié)合S型曲線速度規(guī)劃方法對運(yùn)動(dòng)參數(shù)進(jìn)行分段平滑處理，極大的降低了計(jì)算量的同時(shí)保證了計(jì)算精度，實(shí)現(xiàn)了柔性加減速。此方法簡單高效，能夠滿足工業(yè)機(jī)器人在復(fù)雜曲線插補(bǔ)過程中的的運(yùn)動(dòng)平穩(wěn)性和運(yùn)動(dòng)精度的需求，提高機(jī)器人的工作效率和柔性加工能力，具有廣闊的應(yīng)用前景和良好的經(jīng)濟(jì)效益。

附圖說明

圖1為本發(fā)明實(shí)施例的流程圖；

圖2為本發(fā)明實(shí)施例中步驟10)的流程圖；

圖3為本發(fā)明實(shí)施例中步驟20)和步驟30)的流程圖；

圖4為本發(fā)明實(shí)施例中步驟20)參數(shù)密化校正過程示意圖；

圖5為本發(fā)明實(shí)施例中步驟203)的流程圖；

圖6為本發(fā)明實(shí)施例中步驟2031)中弦高誤差示意圖；

圖7為本發(fā)明實(shí)施例中步驟2031)中弦高誤差計(jì)算方法對比示意圖；

圖8為本發(fā)明實(shí)施例中步驟40)的流程圖。

具體實(shí)施方式

下面結(jié)合附圖和具體實(shí)施方式對本發(fā)明做進(jìn)一步說明。

如圖1所示，本發(fā)明的一種參數(shù)自適應(yīng)密化的機(jī)器人NURBS曲線運(yùn)動(dòng)插補(bǔ)方法，包括以下步驟：

步驟10)根據(jù)輸入的運(yùn)動(dòng)參數(shù)，獲得NURBS曲線表達(dá)式及參數(shù)密化過程中采用的導(dǎo)函數(shù)表達(dá)式。

步驟10)首先根據(jù)給定的NURBS控制頂點(diǎn)及其權(quán)值、節(jié)點(diǎn)矢量等參數(shù)確定三次NURBS曲線的表達(dá)式形式；其次選擇合理的時(shí)間間隔，確定每段NURBS曲線的表達(dá)式；然后求解每段NURBS曲線的分子分母系數(shù)；最后在NURBS曲線表達(dá)式的基礎(chǔ)上，獲得NURBS曲線的一階和二階導(dǎo)函數(shù)表達(dá)式。步驟10)可以以降低插補(bǔ)階段任務(wù)計(jì)算量。如圖2所示，所述的步驟10)具體包括步驟101)至步驟104)：

步驟101)確定三次NURBS曲線表達(dá)式：對于給定的控制頂點(diǎn){d₀,d₁,...,d_n}及其權(quán)值{ω₀,ω₁,...,ω_n}和節(jié)點(diǎn)矢量{u₀,u₁,...,u_n+k+1}，其中，k＝3，得到三次NURBS曲線表達(dá)式p(u)，如式(1)所示：

其中，ω_i表示控制頂點(diǎn)d_i的權(quán)值，d_i表示第i個(gè)控制頂點(diǎn)，N_i,3表示3次B樣條基函數(shù)，B₃、B₂、B₁和B₀分別為分子系數(shù)；b₃、b₂、b₁和b₀分別為分母系數(shù)；

步驟102)求解三次NURBS曲線第i-2段表達(dá)式：

令其中3≤j≤n+1，代入式(1)，得到式(2)：

式中，d_j表示第j個(gè)控制頂點(diǎn)，ω_j表示控制頂點(diǎn)d_j的權(quán)值，N_j,3表示3次B樣條基函數(shù)，d_i-3表示第i-3個(gè)控制頂點(diǎn)，ω_i-3表示控制頂點(diǎn)d_i-3的權(quán)值，d_i-2表示第i-2個(gè)控制頂點(diǎn)，ω_i-2表示控制頂點(diǎn)d_i-2的權(quán)值，d_i-1表示第i-1個(gè)控制頂點(diǎn)，ω_i-1表示控制頂點(diǎn)d_i-1的權(quán)值，d_i表示第i個(gè)控制頂點(diǎn)，ω_i表示控制頂點(diǎn)d_i的權(quán)值，上標(biāo)T表示轉(zhuǎn)置，

步驟103)求解NUBRS曲線第i-2段表達(dá)式的分子系數(shù)和分母系數(shù)：將M_i代入式(2)，得分子系數(shù)和分母系數(shù)分別為：

步驟104)計(jì)算NURBS曲線的導(dǎo)函數(shù)表達(dá)式：首先在式(2)所示的NURBS曲線表達(dá)式上，利用求導(dǎo)公式對節(jié)點(diǎn)矢量u求導(dǎo)，獲得NURBS曲線的一階導(dǎo)函數(shù)；然后對一階導(dǎo)函數(shù)再次利用求導(dǎo)公式對節(jié)點(diǎn)矢量u求導(dǎo)，獲得NURBS曲線的二階導(dǎo)函數(shù)。

步驟20)利用改進(jìn)的Admas微分方程，對步驟10)輸入的運(yùn)動(dòng)參數(shù)中的節(jié)點(diǎn)矢量u進(jìn)行預(yù)估，并根據(jù)約束條件，自適應(yīng)對運(yùn)動(dòng)參數(shù)中的節(jié)點(diǎn)矢量u進(jìn)行校正。

步驟20)首先對NURBS曲線表達(dá)式的自變量u采用Admas微分方程法進(jìn)行密化；然后采用差分代替微分法以降低計(jì)算量；最后根據(jù)預(yù)估步長與期望步長的相對誤差對預(yù)估參數(shù)進(jìn)行校正以保證計(jì)算精度。如圖3所示，所述的步驟20)具體包括：

步驟201)密化自變量u：

當(dāng)參數(shù)u索引i>2時(shí)，根據(jù)式(6)預(yù)估下一個(gè)參數(shù)；

當(dāng)參數(shù)u索引1≤i≤2時(shí)，采用一階泰勒級數(shù)展開，如式(3)所示：

式中，u_i+1表示規(guī)劃節(jié)點(diǎn)矢量，u_i表示已知節(jié)點(diǎn)矢量，ΔL_i表示規(guī)劃周期內(nèi)的步長，T表示規(guī)劃周期，表示機(jī)器人末端速度，p_x(u)表示直角坐標(biāo)系下x軸方向位置插補(bǔ)序列，p_y(u)表示直角坐標(biāo)系下x軸方向位置插補(bǔ)序列，p_z(u)表示直角坐標(biāo)系下x軸方向位置插補(bǔ)序列；

當(dāng)精度要求高時(shí)，泰勒級數(shù)展開需要高階求導(dǎo)，計(jì)算復(fù)雜，因此根據(jù)三步四階Admas微分方程，表示參數(shù)密化的遞推公式如式(4)所示：

其中，u_i+1表示規(guī)劃節(jié)點(diǎn)矢量，表示規(guī)劃節(jié)點(diǎn)矢量的導(dǎo)數(shù)，u_i表示已知的第i個(gè)節(jié)點(diǎn)矢量，表示已知的第i個(gè)節(jié)點(diǎn)矢量的導(dǎo)數(shù)，表示已知的第i-1個(gè)節(jié)點(diǎn)矢量的導(dǎo)數(shù)，表示已知的第i-2個(gè)節(jié)點(diǎn)矢量的導(dǎo)數(shù)；

步驟202)用差分代替微分：式(4)中的微分采用式(5)替換為差分，如式(6)所示：

其中，表示預(yù)估參數(shù)；

步驟203)計(jì)算相對誤差：根據(jù)預(yù)估參數(shù)獲得預(yù)估點(diǎn)位置坐標(biāo)并獲得相應(yīng)的預(yù)估步長如式(7)所示：

其中，表示預(yù)估步長，表示預(yù)估點(diǎn)x軸位置坐標(biāo)，表示預(yù)估點(diǎn)y軸位置坐標(biāo)，表示預(yù)估點(diǎn)z軸位置坐標(biāo)，x_i表示已知點(diǎn)x軸位置坐標(biāo)，y_i表示已知點(diǎn)y軸位置坐標(biāo)，z_i表示已知點(diǎn)z軸位置坐標(biāo)；

計(jì)算預(yù)估步長與期望步長ΔL_i的相對誤差δ_i，如式(8)所示：

其中，期望步長ΔL_i為自適應(yīng)步長調(diào)節(jié)過程中獲得的步長；

步驟204)校正預(yù)估參數(shù)：如圖4所示，如果相對誤差δ_i超出給定閾值，則采用式(9)對預(yù)估參數(shù)進(jìn)行校正：

其中，表示預(yù)估的插補(bǔ)點(diǎn)參數(shù)；

若預(yù)估的插補(bǔ)點(diǎn)數(shù)目大于等于1，則對預(yù)估的插補(bǔ)點(diǎn)進(jìn)行位置插補(bǔ)。

在步驟203)中，根據(jù)約束條件，自適應(yīng)對運(yùn)動(dòng)參數(shù)中的節(jié)點(diǎn)矢量u進(jìn)行校正。

步驟203)首先采用自適應(yīng)步長調(diào)節(jié)的方法，根據(jù)弦高誤差、給定最大進(jìn)給速度、給定最大進(jìn)給加速度和給定最大法向加速度等四個(gè)約束條件，對步長進(jìn)行自適應(yīng)優(yōu)化調(diào)節(jié)；然后通過選取拐點(diǎn)將步長序列分為若干段。如圖5所示，所述的步驟203)具體包括步驟2031)至步驟2035)：

步驟2031)計(jì)算弦高誤差約束：弦高誤差是指以直線代替曲線的所產(chǎn)生的誤差，如圖6所示。本發(fā)明采用式(10)所示的近似方法計(jì)算弦高誤差，即近似等于如圖7中所示的弧長ε。

其中，為預(yù)估的插補(bǔ)點(diǎn)參數(shù)，u_i為已知的插補(bǔ)點(diǎn)參數(shù)，p_i為u_i的插補(bǔ)點(diǎn)坐標(biāo)，為的插補(bǔ)點(diǎn)坐標(biāo)，為和u_i的中位點(diǎn)坐標(biāo)，ε_i為預(yù)估點(diǎn)與已知點(diǎn)的距離，為弦高誤差；

設(shè)允許的最大弦高誤差為h_max，則最大弦高誤差約束下，允許的最大步長ΔL_i1如式(11)所示：

步驟2032)計(jì)算最大進(jìn)給速度約束：在給定最大進(jìn)給速度v_max約束下，允許的最大步長ΔL_i2如式(12)所示：

ΔL_i2＝v_maxT 式(12)

步驟2033)計(jì)算給定最大進(jìn)給加速度約束：在給定最大進(jìn)給加速度約束下，允許的步長ΔL_i3如式(13)所示，其中，進(jìn)給加速度不能超過給定最大進(jìn)給加速度a_max；

ΔL_i-1-a_maxT²≤ΔL_i3≤ΔL_i-1+a_maxT² 式(13)

其中，ΔL_i-1表示第i-2插補(bǔ)點(diǎn)和第i-1插補(bǔ)點(diǎn)間的步長；

即步長滿足式(14)：

其中，ΔL_i表示第i-1插補(bǔ)點(diǎn)和第i插補(bǔ)點(diǎn)間的步長；

如果ΔL_i3>ΔL_i-1+a_maxT²，則令ΔL_i3＝ΔL_i-1+a_maxT²；如果ΔL_i-1-a_maxT²>ΔL_i3，則令ΔL_i3＝ΔL_i-1-a_maxT²；

步驟2034)計(jì)算給定最大法向加速度約束：

首先根據(jù)NURBS插補(bǔ)過程中進(jìn)給速度v_i與法向加速度a_ni的關(guān)系，采用式(15)計(jì)算法向加速度a_ni：

式中，ρ_i表示ΔL_i的曲率半徑；

設(shè)允許的最大法向加速度為a_nmax，采用式(16)獲得最大法向加速度約束下允許的最大步長ΔL_i4：

根據(jù)式(15)和式(16)，得出即最大法向加速度約束下允許的最大步長為ΔL_i4，如式(17)所示：

步驟2035)步長自適應(yīng)優(yōu)化調(diào)節(jié)：在自適應(yīng)步長規(guī)劃過程中，當(dāng)前段的期望步長ΔL_i滿足式(18)：

通過上述自適應(yīng)步長調(diào)節(jié)獲得一系列期望步長序列，然后選取步長序列中的極小值點(diǎn)作為曲率最大的點(diǎn)，即拐點(diǎn)，最后根據(jù)獲得的拐點(diǎn)將期望步長序列分成若干段，并記錄每段的總長度，以及拐點(diǎn)處的期望步長。

步驟30)通過分段進(jìn)行S型曲線位置插補(bǔ)，對自適應(yīng)過程中的運(yùn)動(dòng)參數(shù)進(jìn)行平滑處理。

步驟30)可以實(shí)現(xiàn)柔性加減速。步驟30)具體包括：首先根據(jù)拐點(diǎn)步長和各段的長度，對每段采用七段式S曲線速度規(guī)劃，獲得速度序列，從而得到各個(gè)周期的進(jìn)給步長；然后再次通過參數(shù)密化過程獲得參數(shù)u序列；最后每隔插補(bǔ)周期T＇取參數(shù)u，獲得在直角坐標(biāo)系下的位置插補(bǔ)序列p(u)。

步驟40)采用球面線性插值slerp方法完成姿態(tài)插補(bǔ)，得到的位置插補(bǔ)坐標(biāo)和姿態(tài)插補(bǔ)坐標(biāo)。

步驟40)采用球面線性插值slerp(Spherical Linear Interpolation)完成姿態(tài)插補(bǔ)，首先將起點(diǎn)、終點(diǎn)姿態(tài)以及中間插補(bǔ)姿態(tài)采用四元數(shù)的表示形式；然后將已完成位置插補(bǔ)的總步長之和與NURBS曲線的總長作為姿態(tài)插補(bǔ)參數(shù)r；最后根據(jù)起點(diǎn)和終點(diǎn)的姿態(tài)采用slerp插補(bǔ)方法完成NURBS姿態(tài)插補(bǔ)。如圖8所述，所述的步驟40)具體包括步驟401)至步驟403)：

步驟401)采用四元數(shù)表示姿態(tài)：將起點(diǎn)、終點(diǎn)姿態(tài)以及中間插補(bǔ)姿態(tài)采用四元數(shù)的表示形式，記法如下：

[r,q]＝[r,(R,P,Y)]

其中，r為姿態(tài)插補(bǔ)系數(shù)，取值范圍為[0，1]；q為姿態(tài)向量；R為插補(bǔ)點(diǎn)的a旋轉(zhuǎn)軸的姿態(tài)值，P為插補(bǔ)點(diǎn)的β旋轉(zhuǎn)軸的姿態(tài)值，Y為插補(bǔ)點(diǎn)的γ旋轉(zhuǎn)軸的姿態(tài)值；

步驟402)選定姿態(tài)插補(bǔ)系數(shù)r：將已完成位置插補(bǔ)的總步長之和與NURBS曲線的總長作為姿態(tài)插補(bǔ)系數(shù)r；

步驟403)采用slerp插補(bǔ)方法完成姿態(tài)插補(bǔ)，如式(19)所示：

q_r＝k₀q_beg+k₁q_end 式(19)

其中，q_r表示系數(shù)r的姿態(tài)向量，q_beg表示起始姿態(tài)向量，q_end表示終止姿態(tài)向量，k₀表示起始點(diǎn)系數(shù)，k₁表示終止點(diǎn)系數(shù)；

ω表示q_beg到q_end弧所截的角；

采用式(20)所示slerp插補(bǔ)方法完成姿態(tài)插補(bǔ)：

步驟50)對步驟40)得到的位置插補(bǔ)坐標(biāo)和姿態(tài)插補(bǔ)坐標(biāo)，進(jìn)行機(jī)器人正逆解計(jì)算，得到關(guān)節(jié)角插補(bǔ)序列。

所述步驟50)具體為：首先根據(jù)位置插補(bǔ)坐標(biāo)和姿態(tài)插補(bǔ)坐標(biāo)，經(jīng)機(jī)器人正逆解計(jì)算得到對應(yīng)的關(guān)節(jié)角插補(bǔ)序列，然后將該序列發(fā)送給伺服控制器，以控制機(jī)器人完成相應(yīng)的運(yùn)動(dòng)操作。

應(yīng)當(dāng)指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進(jìn)和潤飾，這些改進(jìn)和潤飾也應(yīng)視為本發(fā)明的保護(hù)范圍。本實(shí)施例中未明確的各組成部分均可用現(xiàn)有技術(shù)加以實(shí)現(xiàn)。