[發(fā)明專利]一種基于余弦二階的機(jī)器人軌跡規(guī)劃方法

無(wú)效

• 申請(qǐng)?zhí)枺?/td>: 200910071524.9
• 申請(qǐng)日: 2009-03-11
• 公開(kāi)（公告）號(hào): CN101508113A
• 公開(kāi)（公告）日: 2009-08-19
• 發(fā)明（設(shè)計(jì)）人: 黃劍斌;王再明;熊根良;謝宗武;劉宏
• 申請(qǐng)（專利權(quán)）人: 哈爾濱工業(yè)大學(xué)
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 哈爾濱市松花江專利商標(biāo)事務(wù)所
• 代理人: 張果瑞

1.一種基于余弦二階的機(jī)器人軌跡規(guī)劃方法，其特征在于實(shí)現(xiàn)該方法的步驟包括：

步驟一、建立基于余弦二階的機(jī)器人機(jī)械臂軌跡曲線的位置的數(shù)學(xué)模型、速度的數(shù)學(xué)模型、加速度的數(shù)學(xué)模型和加速度的導(dǎo)數(shù)的數(shù)學(xué)模型；

所述基于余弦二階的機(jī)器人機(jī)械臂軌跡曲線的位置x(t)的數(shù)學(xué)模型為：

x(t)=b3cos(2πtT)+b2t2+b1t+b0,]]>

其中：b₀，b₁，b₂，b₃為系數(shù)，T為所述余弦的周期；

所述基于余弦二階的機(jī)器人機(jī)械臂軌跡曲線的速度v(t)的數(shù)學(xué)模型為：

v(t)=-2πTb3sin(2πtT)+2b2t+b1;]]>

所述基于余弦二階的機(jī)器人機(jī)械臂軌跡曲線的加速度a(t)的數(shù)學(xué)模型為：

a(t)=-4π2T2b3cos(2πtT)+2b2;]]>

所述基于余弦二階的機(jī)器人機(jī)械臂軌跡曲線的加速度的導(dǎo)數(shù)Jerk(t)的數(shù)學(xué)模型為：

Jerk(t)=8π3T3b3sin(2πtT),]]>

步驟二、給定機(jī)器人機(jī)械臂運(yùn)動(dòng)的起始期望點(diǎn)位置S₁、起始期望點(diǎn)速度v₁、末端期望點(diǎn)位置S₂和末端期望點(diǎn)速度v₂，按最大加速度a_max和最大加速度的導(dǎo)數(shù)J_max進(jìn)行基于余弦二階的機(jī)器人機(jī)械臂軌跡曲線的規(guī)劃；

所述的最大加速度a_max和最大加速度的導(dǎo)數(shù)J_max之間存在如下關(guān)系：

Jmax=πamaxT,]]>

步驟三、依據(jù)最短時(shí)間原則將機(jī)器人機(jī)械臂從起始期望點(diǎn)到末端期望點(diǎn)的軌跡分成四段，分別為加速度傾斜上升段、加速度平穩(wěn)段、加速度傾斜下降段和速度平穩(wěn)段；

所述的最短時(shí)間原則為：在加速度傾斜上升段以T/2時(shí)間達(dá)到加速度最大值a_max，

所述加速度傾斜上升段、加速度平穩(wěn)段、加速度傾斜下降段和速度平穩(wěn)段分別為：

加速度傾斜上升段的時(shí)間區(qū)間為[0，t1]，速度由v₁增加到v_a，運(yùn)行距離為D1，加速度達(dá)到最大值a_max，

加速度平穩(wěn)段的時(shí)間區(qū)間為[t1，t2]，速度由v_a增加到v_b，運(yùn)行距離為D2，加速度保持最大值a_max，加速度的導(dǎo)數(shù)為0，

加速度傾斜下降段的時(shí)間區(qū)間為[t2，t3]，速度由v_b增加到v₂，運(yùn)行距離為D3，加速度為由a_max下降到0，

速度平穩(wěn)段的時(shí)間區(qū)間為[t3，t4]，速度保持v₂，運(yùn)行距離為D4，加速度為0，加速度的導(dǎo)數(shù)為0，

步驟四、將加速度傾斜上升段的邊界條件的值代入步驟一所述數(shù)學(xué)模型中，列出方程組并求解出所述數(shù)學(xué)模型的參數(shù)；

求解數(shù)學(xué)模型參數(shù)的方法為：

加速度傾斜上升段滿足在T/2時(shí)間達(dá)到加速度最大值a_max，所述基于余弦二階的機(jī)器人機(jī)械臂軌跡曲線在t＝0時(shí)位置x(0)、速度v(0)、加速度a(0)和加速度的導(dǎo)數(shù)Jerk(0)滿足的約束條件為：

x(0)=S1v(0)=v1a(0)=0Jerk(0)=0,]]>

所述基于余弦二階的機(jī)器人機(jī)械臂軌跡曲線在t＝T/2時(shí)位置x(T/2)、速度v(T/2)、加速度a(T/2)和加速度的導(dǎo)數(shù)Jerk(T/2)滿足的約束條件為：

x(T/2)=S1+D1v(T/2)=vaa(T/2)=amaxJerk(T/2)=0,]]>

將上述兩組邊界約束條件方程組代入步驟一建立的四個(gè)數(shù)學(xué)模型中，獲得有關(guān)四個(gè)系數(shù)b₀，b₁，b₂，b₃的值、加速度傾斜上升段末端速度v_a和加速度傾斜上升段運(yùn)行距離D1：

b0=S1-amaxT28π2b1=v1b2=amax4b3=amaxT28π2,]]>va=v1+amaxT4D1=(116-14π2)amaxT2+v1T2,]]>

則加速度平穩(wěn)段末端速度v_b為：

vb=v2-amaxT4,]]>

加速度平穩(wěn)段的運(yùn)行距離D2和加速度傾斜下降段的運(yùn)行距離D3為：

D2=vb2-va22amaxD3=(116-14π2)amaxT2+v1T2,]]>

步驟五、判斷是否滿足條件：S₂-S₁≥D1+D2+D3，如果判斷結(jié)果為是，執(zhí)行步驟六，如果判斷結(jié)果為否，執(zhí)行步驟九，

步驟六、判斷是否滿足條件：v₂≥v_2min，

如果判斷結(jié)果為是，執(zhí)行步驟七，如果判斷結(jié)果為否，執(zhí)行步驟八，

其中，v_2min為基于余弦二階的軌跡只包括加速度傾斜上升段和加速度傾斜下降段時(shí)的最小的末端速度值；

步驟七、基于余弦二階的軌跡按最大加速度a_max和最大加速度的導(dǎo)數(shù)J_max進(jìn)行規(guī)劃，獲得機(jī)器人機(jī)械臂期望的輸出軌跡，基于余弦二階的機(jī)器人軌跡規(guī)劃方法完成，

本步驟所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段、加速度平穩(wěn)段、加速度傾斜下降段和速度平穩(wěn)段四段，則：

D4＝S₂-S₁-D1-D2-D3

既而獲得機(jī)器人機(jī)械臂期望的輸出軌跡，

步驟八、基于余弦二階的軌跡按加速度閾值a_f和最大加速度的導(dǎo)數(shù)J_max進(jìn)行規(guī)劃，獲得機(jī)器人機(jī)械臂期望的輸出軌跡，基于余弦二階的機(jī)器人軌跡規(guī)劃方法完成，

本步驟所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段、加速度傾斜下降段和速度平穩(wěn)段三段，且滿足以下方程組：

v2=v1+afT2T=πafJmax,]]>

獲得加速度閾值a_f和余弦周期T，既而獲得機(jī)器人機(jī)械臂期望的輸出軌跡，

步驟九、基于余弦二階的軌跡按加速度閾值a_f和加速度的導(dǎo)數(shù)閾值J_f進(jìn)行規(guī)劃，獲得機(jī)器人機(jī)械臂期望的輸出軌跡，基于余弦二階的機(jī)器人軌跡規(guī)劃方法完成，

本步驟所述的基于余弦二階的軌跡規(guī)劃包括加速度傾斜上升段和加速度傾斜下降段兩段，且滿足以下方程組：

S2-S1=(v1+v2)T2v2=v1+afT2Jf=πafT,]]>

獲得余弦周期T、加速度閾值a_f和加速度的導(dǎo)數(shù)閾值J_f：

T=2(S2-S1)v1+v2af=2(v2-v1)TJf=2π(v2-v1)T2,]]>

既而獲得機(jī)器人機(jī)械臂期望的輸出軌跡。