一種工業機器人門型軌跡插補方法與流程
2023-05-29 17:24:16
發明涉及工業機器人應用領域，尤其涉及一種高速高精度的工業機器人門型軌跡插補方法。
背景技術：
門型軌跡是工業機器人應用領域中使用的比較廣的固定運動軌跡，通常應用於3c裝配行業，對機器人運動的速度和精度要求比較高。在工業機器人高速插補時，如果不對門型軌跡進行過渡軌跡處理，容易造成機器人頻繁啟停，影響機器人控制的速度和精度。同時，相比於s型和梯形速度規劃方法，採用五次樣條曲線對各段軌跡進行插值，其加速度變化分布在插補的各個區間，且變化平滑，這樣可以把機器人各關節的驅動力矩，分配到機器人插補的各個區間，可以防止高速插補時起始或結束段各關節的驅動力矩過大，造成電機報警，導致插補無法完成。
技術實現要素：
本發明的目的在於克服現有技術的不足，提供一種計算簡單方便，滿足高速高精度控制的工業機器人門型軌跡插補方法。
上述目的是通過以下技術方案實現的：
一種工業機器人門型軌跡插補方法，包括步驟：
(1)根據目標點和用戶要求，確定門型軌跡的過渡方案和對整段軌跡進行分段處理；
(2)使用五次樣條曲線，根據各段邊界條件，對各段軌跡進行插值，確定各段軌跡的參數方程，最後根據五次樣條曲線，分別對各段軌跡進行插補。
進一步地，所述步驟(1)具體包括步驟：
(11)根據工業機器人的常用的門型運動軌跡，機器人運動的路徑為p3→p1→p2→p4，然後從p4→p2→p1→p3，這裡以p3→p1→p2→p4的軌跡進行插補分析；
(12)在工業機器人門型運動軌跡的插補時，為了保證高速插補的要求，將p3→p1段及p2→p4段和平面段的p1→p2段進行速度合成，得到合成後的插補路徑；
(13)設定過渡係數為ratio，整段運動軌跡的期望運動時間為ttotal，根據ratio參數的值將整段軌跡分為五段：上升段、上過渡段、平面段、下過渡段、下降段，其中上過渡段軌跡由上升過渡段插補和平面過渡段插補合成而成，下過渡段軌跡由平面過渡段和下降過渡段插補合成而成，各段的插補時間為：
從p3→p1→p2→p4的插補總時間為：
上升段和上升過渡段的插補總時間為：
下降段和下降過渡段的插補總時間為：
平面段的插補時間為：
上升段和下降段的插補時間為：
上升過渡段和下降過渡段的插補時間為：
上升過渡段和直線過渡段插補同時進行，並進行速度合成，合成軌跡稱為上過渡段；下降過渡段和直線過渡段插補同時進行，並進行速度合成，合成軌跡稱為下過渡段。
這樣，根據目標點和用戶要求，確定了門型軌跡的過渡方案和對整段軌跡進行分段處理。
進一步地，所述步驟(2)具體包括步驟：
(21)將速度規劃和插補結合起來，建立以下插補表達式：
pm＝ps+u(pe-ps)；
式中，ps為插補路徑起點位姿，pe為插補路徑終點位姿，u為插補歸一化參數，pm為當前計算插補點。
(22)將插補時間t換算到0-1之間，設定當段路徑的總插補時間為tm，則插補序列n處對應的插補時間為：
t＝n/tm；
(23)插補歸一化參數u和插補時間t的函數關係為：
u＝a1t5+a2t4+a3t3+a4t2+a5t+a6
a1、a2、a3、a4、a5、a6為速度規劃係數；
(24)根據速度規劃和插補的特點，可以確定五次樣條曲線的六個邊界條件：
通過邊界條件，即可求得各速度規劃係數：
代入插補表達式，即可完成各段軌跡的插補過程；
則u和t的函數關係為：
u＝6t5-15t4+10t3
在t∈[0，1]時，有
u′＝30t4-60t3+30t2＝30t2(t-1)2≥0。
這表明，使用這一函數關係進行速度規劃和插補時，可以保證插補歸一化參數u的變化是單調的，不會出現插補速度和位移的抖動，滿足速度規劃和插補的要求。
本發明有如下有益效果：相比於s型和梯形速度規劃方法，採用五次樣條曲線對各段軌跡進行插值，其加速度變化分布在插補的各個區間，且變化平滑，這樣可以把機器人各關節的驅動力矩，分配到機器人插補的各個區間，可以防止高速插補時起始或結束段各關節的驅動力矩過大，造成電機報警，導致插補無法完成。
附圖說明
圖1是工業機器人的門型軌跡示意圖。
圖2是門型軌跡過渡和分段後的示意圖。
圖3是五次樣條曲線的速度和加速度變化示意圖。
具體實施方式
下面結合附圖和具體實施例對本發明做進一步的說明。
一種工業機器人門型軌跡插補方法，包括步驟：
(1)根據目標點和用戶要求，確定門型軌跡的過渡方案和對整段軌跡進行分段處理；
(2)使用五次樣條曲線，根據各段邊界條件，對各段軌跡進行插值，確定各段軌跡的參數方程，最後根據五次樣條曲線，分別對各段軌跡進行插補。
具體而言，所述步驟(1)具體包括步驟：
(11)根據工業機器人的常用的門型運動軌跡(見圖1)，機器人運動的路徑為p3→p1→p2→p4，然後從p4→p2→p1→p3，這裡以p3→p1→p2→p4的軌跡進行插補分析；
(12)在工業機器人門型運動軌跡的插補時，為了保證高速插補的要求，將p3→p1段及p2→p4段和平面段的p1→p2段進行速度合成，得到合成後的插補路徑(見圖2)；
(13)設定過渡係數為ratio，整段運動軌跡的期望運動時間為ttotal，根據ratio參數的值將整段軌跡分為五段：上升段、上過渡段、平面段、下過渡段、下降段，其中上過渡段軌跡由上升過渡段插補和平面過渡段插補合成而成，下過渡段軌跡由平面過渡段和下降過渡段插補合成而成，各段的插補時間為：
從p3→p1→p2→p4的插補總時間為：
上升段和上升過渡段的插補總時間為：
下降段和下降過渡段的插補總時間為：
平面段的插補時間為：
上升段和下降段的插補時間為：
上升過渡段和下降過渡段的插補時間為：
上升過渡段和直線過渡段插補同時進行，並進行速度合成，合成軌跡稱為上過渡段；下降過渡段和直線過渡段插補同時進行，並進行速度合成，合成軌跡稱為下過渡段。
這樣，根據目標點和用戶要求，確定了門型軌跡的過渡方案和對整段軌跡進行分段處理。
具體而言，所述步驟(2)具體包括步驟：
(21)將速度規劃和插補結合起來，建立以下插補表達式：
pm＝ps+u(pe-ps)
式中，ps為插補路徑起點位姿，pe為插補路徑終點位姿，u為插補歸一化參數，pm為當前計算插補點。
(22)將插補時間t換算到0-1之間，設定當段路徑的總插補時間為tm，則插補序列n處對應的插補時間為：
t＝n/tm；
(23)插補歸一化參數u和插補時間t的函數關係為：
u＝a1t5+a2t4+a3t3+a4t2+a5t+a6
a1、a2、a3、a4、a5、a6為速度規劃係數；
(24)根據速度規劃和插補的特點，可以確定五次樣條曲線的六個邊界條件：
通過邊界條件，即可求得各速度規劃係數：
代入插補表達式，即可完成各段軌跡的插補過程；
則u和t的函數關係為：
u＝6t5-15t4+10t3
在t∈[0，1]時，有
u′＝30t4-60t3+30t2＝30t2(t-1)2≥0
u′和u″即為機器人的速度和加速度歸一化表達式，兩者在t∈[0，1]時的變化曲線如下附圖3所示。這表明，使用這一函數關係進行速度規劃和插補時，可以保證插補歸一化參數u的變化是單調的，不會出現插補速度和位移的抖動，滿足速度規劃和插補的要求。同時，在圖3可以看出，相比於s型和梯形速度規劃方法，採用五次樣條曲線對各段軌跡進行插值，其加速度變化分布在插補的各個區間，且變化平滑，這樣可以把機器人各關節的驅動力矩，分配到機器人插補的各個區間，可以防止高速插補時起始或結束段各關節的驅動力矩過大，造成電機報警，導致插補無法完成。
本發明的上述實施例僅僅是為清楚地說明本發明所作的舉例，而並非是對本發明的實施方式的限定。對於所屬領域的普通技術人員來說，在上述說明的基礎上還可以做出其它不同形式的變化或變動。這裡無需也無法對所有的實施方式予以窮舉。凡在本發明的精神和原則之內所作的任何修改、等同替換和改進等，均應包含在本發明權利要求的保護範圍之內。