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

背景技術：

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

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

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

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

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

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

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

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

步驟30)通過分段進行S型曲線位置插補，對自適應過程中的運動參數(shù)進行平滑處理；

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

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

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

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

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

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

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

式中，d_j表示第j個控制頂點，ω_j表示控制頂點d_j的權值，N_j,3表示3次B樣條基函數(shù)，d_i-3表示第i-3個控制頂點，ω_i-3表示控制頂點d_i-3的權值，d_i-2表示第i-2個控制頂點，ω_i-2表示控制頂點d_i-2的權值，d_i-1表示第i-1個控制頂點，ω_i-1表示控制頂點d_i-1的權值，d_i表示第i個控制頂點，ω_i表示控制頂點d_i的權值，上標T表示轉置，

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

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

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

步驟201)密化自變量u：

當參數(shù)u索引i>2時，根據式(6)預估下一個參數(shù)；

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

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

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

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

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

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

步驟203)計算相對誤差：根據預估參數(shù)獲得預估點位置坐標并獲得相應的預估步長如式(7)所示：

其中，表示預估步長，表示預估點x軸位置坐標，表示預估點y軸位置坐標，表示預估點z軸位置坐標，x_i表示已知點x軸位置坐標，y_i表示已知點y軸位置坐標，z_i表示已知點z軸位置坐標；

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

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

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

其中，表示預估的插補點參數(shù)；

若預估的插補點數(shù)目大于等于1，則對預估的插補點進行位置插補，否則對。

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

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

其中，為預估的插補點參數(shù)，u_i為已知的插補點參數(shù)，p_i為u_i的插補點坐標，為的插補點坐標，為和u_i的中位點坐標，e_i為預估點與已知點的距離，為弦高誤差；

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

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

ΔL_i2＝v_maxT 式(12)

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

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

其中，ΔL_i-1表示第i-2插補點和第i-1插補點間的步長；

即步長滿足式(14)：

其中，ΔL_i表示第i-1插補點和第i插補點間的步長；

如果Δ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)計算給定最大法向加速度約束：

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

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

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

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

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

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

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

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

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

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

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

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

步驟403)采用slerp插補方法完成姿態(tài)插補，如式(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₀表示起始點系數(shù)，k₁表示終止點系數(shù)；

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

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

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

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

附圖說明

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

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

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

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

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

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

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

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

具體實施方式

下面結合附圖和具體實施方式對本發(fā)明做進一步說明。

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

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

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

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

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

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

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

式中，d_j表示第j個控制頂點，ω_j表示控制頂點d_j的權值，N_j,3表示3次B樣條基函數(shù)，d_i-3表示第i-3個控制頂點，ω_i-3表示控制頂點d_i-3的權值，d_i-2表示第i-2個控制頂點，ω_i-2表示控制頂點d_i-2的權值，d_i-1表示第i-1個控制頂點，ω_i-1表示控制頂點d_i-1的權值，d_i表示第i個控制頂點，ω_i表示控制頂點d_i的權值，上標T表示轉置，

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

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

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

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

步驟201)密化自變量u：

當參數(shù)u索引i>2時，根據式(6)預估下一個參數(shù)；

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

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

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

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

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

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

步驟203)計算相對誤差：根據預估參數(shù)獲得預估點位置坐標并獲得相應的預估步長如式(7)所示：

其中，表示預估步長，表示預估點x軸位置坐標，表示預估點y軸位置坐標，表示預估點z軸位置坐標，x_i表示已知點x軸位置坐標，y_i表示已知點y軸位置坐標，z_i表示已知點z軸位置坐標；

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

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

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

其中，表示預估的插補點參數(shù)；

若預估的插補點數(shù)目大于等于1，則對預估的插補點進行位置插補。

在步驟203)中，根據約束條件，自適應對運動參數(shù)中的節(jié)點矢量u進行校正。

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

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

其中，為預估的插補點參數(shù)，u_i為已知的插補點參數(shù)，p_i為u_i的插補點坐標，為的插補點坐標，為和u_i的中位點坐標，ε_i為預估點與已知點的距離，為弦高誤差；

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

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

ΔL_i2＝v_maxT 式(12)

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

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

其中，ΔL_i-1表示第i-2插補點和第i-1插補點間的步長；

即步長滿足式(14)：

其中，ΔL_i表示第i-1插補點和第i插補點間的步長；

如果Δ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)計算給定最大法向加速度約束：

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

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

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

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

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

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

步驟30)通過分段進行S型曲線位置插補，對自適應過程中的運動參數(shù)進行平滑處理。

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

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

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

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

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

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

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

步驟403)采用slerp插補方法完成姿態(tài)插補，如式(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₀表示起始點系數(shù)，k₁表示終止點系數(shù)；

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

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

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

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

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