[發明專利]一種工業機器人應用的過渡軌跡規劃方法

有效

• 申請號: 201610075346.7
• 申請日: 2016-02-02
• 公開（公告）號: CN105500354B
• 公開（公告）日: 2017-05-17
• 發明（設計）人: 夏正仙;馮日月;臧秀娟;王繼虎;王明昕
• 申請（專利權）人: 南京埃斯頓機器人工程有限公司
• 主分類號: B25J9/02
• 分類號: B25J9/02;B25J9/16
• 代理公司: 江蘇圣典律師事務所32237
• 代理人: 程化銘

1.一種工業機器人應用的過渡軌跡規劃方法，包括以下步驟：

步驟1：將過渡軌跡規劃需要的運動參數導入機器人過渡軌跡規劃模塊

機器人末端點位姿由位置矢量(x,y,z)和RPY姿態矢量(α,β,γ)共同描述，組合成一個6自由度的復合矢量(x,y,z,α,β,γ)，過渡軌跡規劃需要的運動參數有：第一條軌跡起點位姿P₀、終點位姿P₁，第二條軌跡終點位姿P₂，過渡參數百分比a；工程約束條件包括：系統最大速度V_max、系統最大加速度A_max、系統允許的最大弓高誤差E_max；

步驟2：確定軌跡P₀ P₁與軌跡P₁ P₂之間過渡軌跡的起點和終點的位姿

當軌跡P₀ P₁是笛卡爾空間的直線軌跡時，過渡起點P_s到拐點P₁的直線長度是軌跡P₀ P₁直線長度的一半乘以過渡參數百分比a；當軌跡P₀ P₁是笛卡爾空間的圓弧軌跡時，過渡起點P_s到拐點P₁的弧長是軌跡P₀ P₁弧長的一半乘以過渡參數百分比a；設定過渡起點P_s到拐點P₁的RPY姿態矢量變化對應軌跡P₀ P₁RPY姿態矢量變化的一半乘以過渡參數百分比a；

當軌跡P₀ P₁是關節空間軌跡時，設定過渡起點P_s到拐點P₁的各關節位置變化對應軌跡P₀ P₁各關節位置變化的一半乘以過渡參數百分比a，進一步根據正向運動學計算出過渡起點P_s的位姿；

當軌跡P₁ P₂是笛卡爾空間的直線軌跡時，拐點P₁到過渡終點P_e的直線長度是軌跡P₁ P₂直線長度的一半乘以過渡參數百分比a；當軌跡P₁ P₂是笛卡爾空間的圓弧軌跡時，拐點P₁到過渡終點P_e的弧長是軌跡P₁ P₂弧長的一半乘以過渡參數百分比a；設定拐點P₁到過渡終點P_e的RPY姿態矢量變化對應軌跡P₁ P₂的RPY姿態矢量變化的一半乘以過渡參數百分比a；

當軌跡P₁ P₂是關節空間軌跡時，設定拐點P₁到過渡終點P_e的各關節位置變化對應軌跡P₁ P₂各關節位置變化的一半乘以過渡參數百分比a，進一步根據正向運動學計算出過渡終點P_e的位姿；

步驟3：確定過渡軌跡的邊界速度

由外部軟件模塊機器人的加減速軌跡規劃算法，輸入軌跡P₀ P₁在過渡起點P_s處的速度，軌跡P₁ P₂在過渡終點P_e處的速度，為簡化計算，約定過渡軌跡起、終點的邊界速度相等；

笛卡爾空間軌跡對應末端點的路徑速度V_path、末端點的姿態旋轉速度V_ori，關節空間軌跡對應各關節的速度，此情況下，借助雅可比矩陣，由各關節的速度直接計算出機器人末端點位姿6個自由度的速度；

步驟4：利用弓高誤差來約束過渡軌跡的邊界速度

假定軌跡P₀ P₁與軌跡P₁ P₂之間有個假想的小圓弧，通過弓高誤差來調整曲率半徑r，然后利用r對軌跡銜接速度進行約束，保證過渡軌跡邊界速度V_path和V_ori不超過允許的軌跡銜接速度；關節空間軌跡用過渡點與拐點之間的直線連線替代原軌跡計算拐點處的夾角；軌跡P₀ P₁與軌跡P₁ P₂在拐點P₁處的切向速度矢量的夾角為θ，對路徑速度V_path的約束公式如下：

$V_{path}\≤\{\begin{array}{c}{V}_{\max }\\ \sqrt{A_{\max }r}\end{array}---\left(1\right)$

其中，V_max為系統最大速度，A_max為系統最大加速度，E_max系統允許的弓高誤差參數，

定義一個角度到毫米的量綱轉換系數λ，將姿態旋轉速度等效為一個長度上的速度矢量λV_ori；

軌跡P₀ P₁與軌跡P₁ P₂在拐點P₁處的切向姿態旋轉速度矢量的夾角為θ_ori，對姿態旋轉速度V_ori的約束公式如下：

$V_{ori}\≤\left\{\begin{array}{c}\frac{1}{\mathrm{\λ}}{V}_{max}\\ \frac{1}{\mathrm{\λ}}\sqrt{A_{max}{r}_{ori}}\end{array}\right.---\left(2\right)$

其中，

步驟5：將笛卡爾空間軌跡在過渡軌跡邊界點的速度V_path和V_ori分解到位姿6個自由度上

若軌跡P₀P₁是笛卡爾空間軌跡，將過渡起點P_s處的速度V_path和V_ori分解到位姿6個自由度上；若軌跡P₁P₂是笛卡爾空間軌跡，將過渡終點P_e處的速度V_path和V_ori分解到位姿6個自由度上；

步驟6：計算過渡軌跡起點P_s和終點P_e的位姿6個自由度上的加速度

假定過渡起點P_s處的瞬間是勻速，過渡終點P_e處的瞬間是勻速，通過速度差分計算得到過渡軌跡起點P_s和終點P_e的位姿6個自由度上的加速度；

步驟7：計算過渡軌跡運行時間T

假定過渡軌跡的執行時間與從過渡起點P_s勻速運動到拐點P₁再勻速運動到過渡終點P_e的直線段時間相等，依此設定，分別計算位置過渡的運行時間T₁和姿態過渡的運行時間T₂，T選取較長的時間：

$T=\left\{\begin{array}{cc}{T}_1,& T\≥T_2\\ T_2,& T_1\<T_2\end{array}\right.---\left(3\right)$

若其中存在位置或姿態不過渡，則將對應的運行時間置為零；

步驟8：對機器人過渡軌跡末端點位姿的6個自由度分別構造過渡曲線方程P(σ)，采用兩條拋物線運動的疊加融合為過渡軌跡的運動，矩陣方程如下：

P(σ)＝P₁(σ)+η(σ)[P₂(σ)-P₁(σ)](4)

其中，σ是過渡時間t的參數化變量；P₁(σ)是與過渡起點P_s相連的拋物線，是關于變量σ的二次函數，與第一段軌跡相切于過渡起點P_s；P₂(σ)是與過渡終點P_e相連的拋物線，是關于變量σ的二次函數，與第二段軌跡相切于過渡終點P_e；η(σ)是保證曲線P₁(σ)過渡到曲線P₂(σ)的過渡函數，η(σ)＝6σ⁵-15σ⁴+10σ³；

將過渡軌跡起點P_s和終點P_e的邊界條件：位姿、速度、加速度，帶入公式(4)的矩陣方程，確定過渡軌跡的位姿6個自由度的曲線方程。