[發明專利]基于倍四元數的工業機器人自由曲線的軌跡規劃控制方法

有效

• 申請號: 201610266117.3
• 申請日: 2016-04-26
• 公開（公告）號: CN105773620B
• 公開（公告）日: 2017-09-12
• 發明（設計）人: 李宏勝;汪允鶴
• 申請（專利權）人: 南京工程學院
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 南京縱橫知識產權代理有限公司32224
• 代理人: 董建林

1.基于倍四元數的工業機器人自由曲線的軌跡規劃控制方法，其特征在于，包括以下步驟：

1)建立基于倍四元數的機器人末端執行器空間位姿的數學模型與機器人末端執行器在任務空間NURBS描述的自由曲線數學模型；

2)給定機器人末端執行器在任務空間NURBS描述的自由曲線的控制點序列D，以及控制點對應姿態R；

所述控制點序列D表示為：D＝{d₀，d₁，…，d_n}，n為控制點個數；

所述控制點對應姿態采用姿態旋轉矩陣R_3×3表示；

3)依據哈特利-賈德法求解所述步驟2)的控制點序列中控制點對應的節點矢量U，具體過程如下：

為給定的控制點d_i，i＝0,1,…,n，預定義一條k次非均勻有理B樣條曲線，同時確定它的節點矢量U＝[u₀,u₁,…,u_n+k+1]中的具體的節點值，節點值的求解如下：

將兩端節點的重復度取為k+1，將曲線的定義域取成規范參數域，即u∈[u_k,u_n+1]＝[0,1]，于是u₀＝u₁＝…＝u_k＝0，u_n+1＝u_n+2＝…＝u_n+k+1＝1，其余節點值需計算求解，如下：

計算公式如下：

ui-ui-1=Σj=i-ki-1ljΣs=k+1n+1Σj=s-ks-1lj,i=k+1,...,n+1---(4)]]>

式中，l_j為控制多邊形的各邊長，l_j＝|d_j-d_j-1|，

由式(4)可得：

ui=Σs=k+1i(us-us-1)=Σs=k+1iΣj=s-ks-1ljΣs=k+1n+1Σj=s-ks-1lj,i=k+1,...,n+1---(5)]]>

繼而可得所有的節點值；

其中，u為節點矢量U相鄰兩個節點間的密化節點值，u_i，i＝0,1,……,n+k+1，表示節點矢量U中具體的某個節點值；

4)依據Adams微分方程理論算法對節點矢量U進行密化處理，具體過程如下：

采用三步四階Adams微分方程的隱格式表示為：

ui+1=ui+T24(9u·i+1+19u·i-5u·i-1+u·i-2)---(6)]]>

其中，T為插補周期，分別為u_i-2、u_i-1、u_i、u_i+1的一階導數；

將代入上式，可得：

ui+1=ui+124(9ΔLi+1x·i+22+y·i+12+z·i+12+9ΔLix·i2+y·i2+z·i2-5ΔLi-1x·i-12+y·i-12+z·i-12+ΔLi-2x·i-22+y·i-22+z·i-22)---(7)]]>

ΔL_i表示控制點d_i的進給步長；

采用前、后向差分結合代替微分的方法進行簡化：

后向差分，前向差分，

前向差分，前向差分

將上式代入式(7)，得到：

ui+1=ui+T24(9u·i+1+19u·i-5u·i-1+u·i-2)---(8)]]>

進而得到簡化后的Adams微分方程插補算法迭代公式：

u~i+1=14(9ui-6ui-1+ui-2)---(9)]]>

繼而可得密化后節點矢量，其中，表示u_i+1的預估值；

5)根據步驟1)中的機器人末端執行器空間位姿的數學模型，并采用自適應速度控制算法對密化后的節點矢量進行修正處理，最終獲得最優的密化節點矢量，修正處理過程如下：

將參數作為參數插補的預估值代入NURBS方程，得到相對應的預估插補點：

p~(ui+1)=p(u~i+1)---(10)]]>

表示預估值的預估插補點，

從而得到對應的預估進給步長為：

ΔL~i=|p~(ui+1)-p(ui)|=(x~i+1-xi)2+(y~i+1-xi)2+(z~i+1-xi)2---(11)]]>

預估進給步長和進給步長ΔL_i之間存在的偏差，用相對誤差δ_i來表示：

δi=|ΔLi-ΔL~i|ΔLi×100%---(12)]]>

當相對誤差δ_i在允許范圍內時，則為所求p(u_i+1)，否則按下式進行修正，直至達到δ_i允許范圍內：

ui+1=ui+ΔLiΔL~i(u~i+1-ui)---(13)]]>

最終獲得最優的密化節點矢量；

6)利用步驟5)的最優的密化節點矢量，并根據步驟1)中機器人末端執行器在任務空間NURBS描述的自由曲線數學模型，最終獲得曲線上的插補點位置；

7)根據相鄰空間曲線的插補點位置與姿態數據，進行倍四元數轉換，具體步驟如下：

7-1)對于每一個機器人末端位姿齊次變換矩陣^BT_E，如下所示：

BTE=RP01---(15)]]>

首先將姿態旋轉矩陣R_3×3，經旋轉矩陣與四元數的轉換關系，得到姿態旋轉矩陣對應的旋轉四元數Q，同時獲得平移向量P＝[p_x(u),p_y(u),p_z(u)]^T；

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

D_p＝cos(ψ/2)+sin(ψ/2)v (16)

式中，ψ＝|P|/R_l，R_l為四維空間的大球半徑，v是平移向量上的單位矢量，v＝P/|P|；當|P|＝0時，v為零矢量；

7-3)通過以下公式，計算得到機器人末端位姿轉換至四維空間的倍四元數空間位姿的G部和H部；

G=DpQ,H=Dp*Q]]>

式中，為四元數D_p的共軛；

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

Q＝(G+H)/(2cosψ)(17)

P=Rlψsinψ(G-H)Q*(ψ≠0)0(ψ=0)---(18)]]>

式中，

8)對插補所得機器人末端執行器位姿進行逆運動學處理，獲得關節角度，并驅動關節運動。

2.根據權利要求1所述的基于倍四元數的工業機器人自由曲線的軌跡規劃控制方法，其特征在于，所述步驟1)中，

基于倍四元數的機器人末端執行器空間位姿的數學模型為：

G~=ξG+ηH---(1)]]>

其中，表示機器人末端執行器倍四元數空間位姿，ξ和η滿足ξ²＝ξ，η²＝η，ξ+η＝1，ξη＝0，G和H均為單位四元數；

機器人末端執行器在任務空間NURBS描述的自由曲線數學模型為：任意一條k次NURBS曲線均表示為一分段有理多項式矢函數：

p(u)=Σi=0nωidiNi,k(u)Σi=0nωiNi,k(u)---(2)]]>

其中，p(u)表示機器人末端執行器在任務空間NURBS描述的自由曲線的位置矢量，ω_i稱為權因子；d_i為自由曲線控制點；n為控制點個數；N_i,k(u)是由節點矢量決定的B樣條基函數，由德布爾-考克斯遞推定義公式表示：

Ni,0=1if(ui≤u≤ui+1)0otherwiseNi,k=u-uiui+k-uiNi,k-1(u)+ui+k+1-uui+k+1-ui+1Ni,k-1(u)---(3)]]>

式中，規定u為節點矢量U相鄰兩個節點間的密化節點值，u_i，i＝0,1,……,n+k+1，表示節點矢量U中具體的某個節點值。