[發明專利]一種可控誤差的工業機器人光順運動軌跡生成方法

有效

• 申請號: 201710097192.6
• 申請日: 2017-02-22
• 公開（公告）號: CN106826829B
• 公開（公告）日: 2019-05-03
• 發明（設計）人: 李振瀚;何姍姍;顏昌亞;張立先;汪九洲
• 申請（專利權）人: 武漢工程大學
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 湖北武漢永嘉專利代理有限公司 42102
• 代理人: 唐萬榮;楊曉燕

1.一種可控誤差的工業機器人光順運動軌跡生成方法，其特征在于，包括如下步驟：

S1、生成MOVEB運動指令：MOVEB運動指令用來描述工業機器人運動軌跡，包括軌跡點和姿態，以及用戶輸入的軌跡點誤差閾值和弦高誤差閾值；

S2、機器人軌跡點的插值：將MOVEB運動指令表示的工業機器人運動軌跡導入機器人控制系統，利用高階B樣條插值算法，將高階B樣條曲線按照軌跡點誤差和弦高誤差閾值對軌跡點進行插值，分別實現機器人軌跡點的G²插值和G³插值，得到具有高連續性并且保證滿足軌跡點誤差和弦高誤差要求的插值軌跡；

S3、機器人姿態的插值：通過B樣條插值算法分別實現機器人姿態的分段G²和G³插值，并對插值后的機器人軌跡的姿態進行單位正交化處理，得到分段G²和G³連續的機器人姿態曲線；

S4、生成機器人插值后的運動軌跡：通過上述步驟S2獲取的軌跡點插值曲線和步驟S3獲取的機器人姿態曲線共同得到機器人插值后的運動軌跡。

2.根據權利要求1所述的可控誤差的工業機器人光順運動軌跡生成方法，其特征在于，所述步驟S1中MOVEB運動指令的數據內容包括X,Y,Z,I,J,K,U,V,W,D,E共十一個變量，其中(X,Y,Z)表示工具坐標系下的軌跡點的坐標，(I,J,K)表示上述軌跡點處建立的工具坐標系的Z軸單位矢量，(U,V,W)表示工具坐標系的X軸單位矢量，D,E分別表示弦高誤差閾值和軌跡點誤差閾值。

3.根據權利要求1所述的可控誤差的工業機器人光順運動軌跡生成方法，其特征在于，所述步驟S2及步驟S3中，機器人控制系統解析MOVEB運動指令時，將軌跡點誤差閾值和弦高誤差閾值相同的一段連續軌跡組成一組進行插值。

4.根據權利要求2所述的可控誤差的工業機器人光順運動軌跡生成方法，其特征在于，所述步驟S2中，機器人軌跡點的插值具體包括如下步驟：

步驟一：初始迭代參數決定，從MOVEB運動指令中獲取一組機器人原始軌跡點N≥2、弦高誤差閾值D和軌跡點誤差閾值E；迭代次數記為k＝0，計算初始迭代軌跡點和初始迭代弦高誤差閾值

步驟二：具體包括：

①建立每個內部軌跡點處的橋接B樣條曲線，在整個步驟二中，省略迭代次數k，即等同于P_i，等同于d_i：

橋接B樣條曲線的次數為p,共有2p+1個控制點，分別為 A_1,i，A_2,i…A_p,i,Q_i,B_p,i,…B_2,i,B_1,i,節點矢量為{0,0.5,1},三個節點的重復度分別為p+1,p,p+1；該橋接B樣條曲線等價于兩條對稱的p次Bezier曲線；

②建立在Q_i對稱的兩條p次Bezier曲線，兩條Bezier曲線的控制點分別為A_1,i,A_2,i…A_p,i,Q_i和Q_i,B_p,i,…B_2,i,B_1,i，其中A_1,i，A_2,i…A_p,i與P_i-1P_i共線，B_p,i,…B_2,i,B_1,i與P_iP_i+1共線，Q_i為A_p,iB_p,i的中點；

③分G²和G³插值兩種情形給出保證橋接B樣條曲線處處G²或G³連續的控制點：

設

其中T_0,i為P_i-1P_i的單位向量；T_1,i為的A_p,iB_p,i單位向量；h_i為線段A_p,iP_i的長度；θ_i為向量T_0,i和T_1,i的夾角；

i)在G²插值時，G²橋接曲線的控制頂點滿足以下條件：

Q_i＝P_i-h_iT_0,i+h_icosθ_iT_1,i

A_3,i＝Q_i-h_icosθ_iT_1,i

A_2,i＝A_3,i-h_iT_0,i

A_1,i＝A_2,i-β_ih_iT_0,i

ii)在G³插值時，G³橋接曲線的控制頂點滿足以下條件：

Q_i＝P_i-h_iT_0,i+h_icosθ_iT_1,i

A_4,i＝Q_i-h_icosθ_iT_1,i

A_3,i＝A_4,i-α_ih_iT_0,i

A_1,i＝A_2,i-β_ih_iT_0,i

α_i≥0.7,0<β_i≤0.5

在G²插值時，h_i,β_i為兩個橋接曲線參數；在G³插值時,h_i,α_i,β_i為三個橋接曲線參數，橋接曲線參數決定了橋接曲線控制點的位置，在以下步驟⑤中通過目標函數最優化求解得到，B_1,i,B_2,i,…,B_p,i由橋接B樣條曲線的對稱性得到；

④以橋接曲線更平緩為目標建立優化目標函數，并給出保形和弦高誤差控制的約束條件：

兩條Bezier曲線構造的G²橋接曲線和G³橋接曲線在交點Q_i處的曲率最大，且最大曲率分別為和為得到更平緩的橋接曲線，要求k_g2max和k_g3max越小，即要求步驟③中橋接曲線參數h_i越大，因此將目標函數定義為求解橋接曲線參數h_i,i＝1,2,…N-1的和最大問題，即如下式(1)所示：

為實現保形約束，要求兩個軌跡點之間的橋接曲線不存在重合部分，其中G²連續和G³連續的橋接保形約束為：

|P_i-1B_1,i-1|+|A_1,iP_i|≤|P_i-1P_i|,i＝2,…N-1 (2)

|A_1,1P₁|≤|P₀P₁| (3)

|P_N-1B_1,N-1|≤|P_N-1P_N| (4)

為實現弦高誤差約束，只需控制P_iQ_i的距離不大于迭代弦高誤差閾值d_i即可，即：

|P_iQ_i|≤d_i (5)

⑤求解最優化問題即式(1)，同時要求橋接曲線參數滿足約束條件式(2)、(3)、(4)、(5)，求解出橋接曲線參數，即得到迭代軌跡點的橋接軌跡；

步驟三：計算橋接軌跡與原始軌跡點的軌跡點誤差最大值Δ_k，并判斷Δ_k是否大于軌跡點誤差閾值E，軌跡點誤差的計算方法為計算軌跡點與兩條Bezier曲線交點的距離，本步驟三中加k上標參數與步驟二中未加k上標參數等同，為描述第k次迭代的誤差檢測，未省略k：

其中位于的角平分線上，根據插值曲線的對稱性，通過計算的大小和角平分線向量得到，計算方法為：

若Δ_k>E，說明橋接曲線未滿足軌跡點誤差要求，進入步驟四更新迭代軌跡點和迭代弦高誤差閾值；否則，插值成功，退出迭代，導出插值軌跡；

步驟四：更新迭代參數，令迭代軌跡點的調整向量為：調整后的迭代軌跡點為：迭代弦高誤差閾值為：更新迭代次數：k＝k+1，轉步驟二。