發(fā)明涉及工業(yè)機(jī)器人應(yīng)用領(lǐng)域，尤其涉及一種高速高精度的工業(yè)機(jī)器人門型軌跡插補(bǔ)方法。

背景技術(shù)：

門型軌跡是工業(yè)機(jī)器人應(yīng)用領(lǐng)域中使用的比較廣的固定運動軌跡，通常應(yīng)用于3c裝配行業(yè)，對機(jī)器人運動的速度和精度要求比較高。在工業(yè)機(jī)器人高速插補(bǔ)時，如果不對門型軌跡進(jìn)行過渡軌跡處理，容易造成機(jī)器人頻繁啟停，影響機(jī)器人控制的速度和精度。同時，相比于s型和梯形速度規(guī)劃方法，采用五次樣條曲線對各段軌跡進(jìn)行插值，其加速度變化分布在插補(bǔ)的各個區(qū)間，且變化平滑，這樣可以把機(jī)器人各關(guān)節(jié)的驅(qū)動力矩，分配到機(jī)器人插補(bǔ)的各個區(qū)間，可以防止高速插補(bǔ)時起始或結(jié)束段各關(guān)節(jié)的驅(qū)動力矩過大，造成電機(jī)報警，導(dǎo)致插補(bǔ)無法完成。

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

本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，提供一種計算簡單方便，滿足高速高精度控制的工業(yè)機(jī)器人門型軌跡插補(bǔ)方法。

上述目的是通過以下技術(shù)方案實現(xiàn)的：

一種工業(yè)機(jī)器人門型軌跡插補(bǔ)方法，包括步驟：

(1)根據(jù)目標(biāo)點和用戶要求，確定門型軌跡的過渡方案和對整段軌跡進(jìn)行分段處理；

(2)使用五次樣條曲線，根據(jù)各段邊界條件，對各段軌跡進(jìn)行插值，確定各段軌跡的參數(shù)方程，最后根據(jù)五次樣條曲線，分別對各段軌跡進(jìn)行插補(bǔ)。

進(jìn)一步地，所述步驟(1)具體包括步驟：

(11)根據(jù)工業(yè)機(jī)器人的常用的門型運動軌跡，機(jī)器人運動的路徑為p3→p1→p2→p4，然后從p4→p2→p1→p3，這里以p3→p1→p2→p4的軌跡進(jìn)行插補(bǔ)分析；

(12)在工業(yè)機(jī)器人門型運動軌跡的插補(bǔ)時，為了保證高速插補(bǔ)的要求，將p3→p1段及p2→p4段和平面段的p1→p2段進(jìn)行速度合成，得到合成后的插補(bǔ)路徑；

(13)設(shè)定過渡系數(shù)為ratio，整段運動軌跡的期望運動時間為ttotal，根據(jù)ratio參數(shù)的值將整段軌跡分為五段：上升段、上過渡段、平面段、下過渡段、下降段，其中上過渡段軌跡由上升過渡段插補(bǔ)和平面過渡段插補(bǔ)合成而成，下過渡段軌跡由平面過渡段和下降過渡段插補(bǔ)合成而成，各段的插補(bǔ)時間為：

從p3→p1→p2→p4的插補(bǔ)總時間為：

上升段和上升過渡段的插補(bǔ)總時間為：

下降段和下降過渡段的插補(bǔ)總時間為：

平面段的插補(bǔ)時間為：

上升段和下降段的插補(bǔ)時間為：

上升過渡段和下降過渡段的插補(bǔ)時間為：

上升過渡段和直線過渡段插補(bǔ)同時進(jìn)行，并進(jìn)行速度合成，合成軌跡稱為上過渡段；下降過渡段和直線過渡段插補(bǔ)同時進(jìn)行，并進(jìn)行速度合成，合成軌跡稱為下過渡段。

這樣，根據(jù)目標(biāo)點和用戶要求，確定了門型軌跡的過渡方案和對整段軌跡進(jìn)行分段處理。

進(jìn)一步地，所述步驟(2)具體包括步驟：

(21)將速度規(guī)劃和插補(bǔ)結(jié)合起來，建立以下插補(bǔ)表達(dá)式：

pm＝ps+u(pe-ps)；

式中，ps為插補(bǔ)路徑起點位姿，pe為插補(bǔ)路徑終點位姿，u為插補(bǔ)歸一化參數(shù)，pm為當(dāng)前計算插補(bǔ)點。

(22)將插補(bǔ)時間t換算到0-1之間，設(shè)定當(dāng)段路徑的總插補(bǔ)時間為tm，則插補(bǔ)序列n處對應(yīng)的插補(bǔ)時間為：

t＝n/tm；

(23)插補(bǔ)歸一化參數(shù)u和插補(bǔ)時間t的函數(shù)關(guān)系為：

u＝a1t⁵+a2t⁴+a3t³+a4t²+a5t+a6

a1、a2、a3、a4、a5、a6為速度規(guī)劃系數(shù)；

(24)根據(jù)速度規(guī)劃和插補(bǔ)的特點，可以確定五次樣條曲線的六個邊界條件：

通過邊界條件，即可求得各速度規(guī)劃系數(shù)：

代入插補(bǔ)表達(dá)式，即可完成各段軌跡的插補(bǔ)過程；

則u和t的函數(shù)關(guān)系為：

u＝6t⁵-15t⁴+10t³

在t∈[0，1]時，有

u′＝30t⁴-60t³+30t²＝30t²(t-1)²≥0。

這表明，使用這一函數(shù)關(guān)系進(jìn)行速度規(guī)劃和插補(bǔ)時，可以保證插補(bǔ)歸一化參數(shù)u的變化是單調(diào)的，不會出現(xiàn)插補(bǔ)速度和位移的抖動，滿足速度規(guī)劃和插補(bǔ)的要求。

本發(fā)明有如下有益效果：相比于s型和梯形速度規(guī)劃方法，采用五次樣條曲線對各段軌跡進(jìn)行插值，其加速度變化分布在插補(bǔ)的各個區(qū)間，且變化平滑，這樣可以把機(jī)器人各關(guān)節(jié)的驅(qū)動力矩，分配到機(jī)器人插補(bǔ)的各個區(qū)間，可以防止高速插補(bǔ)時起始或結(jié)束段各關(guān)節(jié)的驅(qū)動力矩過大，造成電機(jī)報警，導(dǎo)致插補(bǔ)無法完成。

附圖說明

圖1是工業(yè)機(jī)器人的門型軌跡示意圖。

圖2是門型軌跡過渡和分段后的示意圖。

圖3是五次樣條曲線的速度和加速度變化示意圖。

具體實施方式

下面結(jié)合附圖和具體實施例對本發(fā)明做進(jìn)一步的說明。

一種工業(yè)機(jī)器人門型軌跡插補(bǔ)方法，包括步驟：

(1)根據(jù)目標(biāo)點和用戶要求，確定門型軌跡的過渡方案和對整段軌跡進(jìn)行分段處理；

(2)使用五次樣條曲線，根據(jù)各段邊界條件，對各段軌跡進(jìn)行插值，確定各段軌跡的參數(shù)方程，最后根據(jù)五次樣條曲線，分別對各段軌跡進(jìn)行插補(bǔ)。

具體而言，所述步驟(1)具體包括步驟：

(11)根據(jù)工業(yè)機(jī)器人的常用的門型運動軌跡(見圖1)，機(jī)器人運動的路徑為p3→p1→p2→p4，然后從p4→p2→p1→p3，這里以p3→p1→p2→p4的軌跡進(jìn)行插補(bǔ)分析；

(12)在工業(yè)機(jī)器人門型運動軌跡的插補(bǔ)時，為了保證高速插補(bǔ)的要求，將p3→p1段及p2→p4段和平面段的p1→p2段進(jìn)行速度合成，得到合成后的插補(bǔ)路徑(見圖2)；

(13)設(shè)定過渡系數(shù)為ratio，整段運動軌跡的期望運動時間為ttotal，根據(jù)ratio參數(shù)的值將整段軌跡分為五段：上升段、上過渡段、平面段、下過渡段、下降段，其中上過渡段軌跡由上升過渡段插補(bǔ)和平面過渡段插補(bǔ)合成而成，下過渡段軌跡由平面過渡段和下降過渡段插補(bǔ)合成而成，各段的插補(bǔ)時間為：

從p3→p1→p2→p4的插補(bǔ)總時間為：

上升段和上升過渡段的插補(bǔ)總時間為：

下降段和下降過渡段的插補(bǔ)總時間為：

平面段的插補(bǔ)時間為：

上升段和下降段的插補(bǔ)時間為：

上升過渡段和下降過渡段的插補(bǔ)時間為：

上升過渡段和直線過渡段插補(bǔ)同時進(jìn)行，并進(jìn)行速度合成，合成軌跡稱為上過渡段；下降過渡段和直線過渡段插補(bǔ)同時進(jìn)行，并進(jìn)行速度合成，合成軌跡稱為下過渡段。

這樣，根據(jù)目標(biāo)點和用戶要求，確定了門型軌跡的過渡方案和對整段軌跡進(jìn)行分段處理。

具體而言，所述步驟(2)具體包括步驟：

(21)將速度規(guī)劃和插補(bǔ)結(jié)合起來，建立以下插補(bǔ)表達(dá)式：

pm＝ps+u(pe-ps)

式中，ps為插補(bǔ)路徑起點位姿，pe為插補(bǔ)路徑終點位姿，u為插補(bǔ)歸一化參數(shù)，pm為當(dāng)前計算插補(bǔ)點。

(22)將插補(bǔ)時間t換算到0-1之間，設(shè)定當(dāng)段路徑的總插補(bǔ)時間為tm，則插補(bǔ)序列n處對應(yīng)的插補(bǔ)時間為：

t＝n/tm；

(23)插補(bǔ)歸一化參數(shù)u和插補(bǔ)時間t的函數(shù)關(guān)系為：

u＝a1t⁵+a2t⁴+a3t³+a4t²+a5t+a6

a1、a2、a3、a4、a5、a6為速度規(guī)劃系數(shù)；

(24)根據(jù)速度規(guī)劃和插補(bǔ)的特點，可以確定五次樣條曲線的六個邊界條件：

通過邊界條件，即可求得各速度規(guī)劃系數(shù)：

代入插補(bǔ)表達(dá)式，即可完成各段軌跡的插補(bǔ)過程；

則u和t的函數(shù)關(guān)系為：

u＝6t⁵-15t⁴+10t³

在t∈[0，1]時，有

u′＝30t⁴-60t³+30t²＝30t²(t-1)²≥0

u′和u″即為機(jī)器人的速度和加速度歸一化表達(dá)式，兩者在t∈[0，1]時的變化曲線如下附圖3所示。這表明，使用這一函數(shù)關(guān)系進(jìn)行速度規(guī)劃和插補(bǔ)時，可以保證插補(bǔ)歸一化參數(shù)u的變化是單調(diào)的，不會出現(xiàn)插補(bǔ)速度和位移的抖動，滿足速度規(guī)劃和插補(bǔ)的要求。同時，在圖3可以看出，相比于s型和梯形速度規(guī)劃方法，采用五次樣條曲線對各段軌跡進(jìn)行插值，其加速度變化分布在插補(bǔ)的各個區(qū)間，且變化平滑，這樣可以把機(jī)器人各關(guān)節(jié)的驅(qū)動力矩，分配到機(jī)器人插補(bǔ)的各個區(qū)間，可以防止高速插補(bǔ)時起始或結(jié)束段各關(guān)節(jié)的驅(qū)動力矩過大，造成電機(jī)報警，導(dǎo)致插補(bǔ)無法完成。

本發(fā)明的上述實施例僅僅是為清楚地說明本發(fā)明所作的舉例，而并非是對本發(fā)明的實施方式的限定。對于所屬領(lǐng)域的普通技術(shù)人員來說，在上述說明的基礎(chǔ)上還可以做出其它不同形式的變化或變動。這里無需也無法對所有的實施方式予以窮舉。凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明權(quán)利要求的保護(hù)范圍之內(nèi)。