[發(fā)明專利]一種用于目標(biāo)同步的機(jī)器人動態(tài)軌跡規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202111318376.3
• 申請日: 2021-11-09
• 公開（公告）號: CN113858213B
• 公開（公告）日: 2023-07-25
• 發(fā)明（設(shè)計(jì)）人: 趙臻暉;夏正仙;馮日月;潘婷婷;涂孔;王繼虎
• 申請（專利權(quán)）人: 南京埃斯頓機(jī)器人工程有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16
• 代理公司: 江蘇圣典律師事務(wù)所 32237
• 代理人: 程化銘;陶得天

1.一種用于目標(biāo)同步的機(jī)器人動態(tài)軌跡規(guī)劃方法，其特征在于，按以下步驟進(jìn)行：

步驟1、參數(shù)定義及軌跡參數(shù)給定；

輸入用戶設(shè)置的傳送帶參考速度Vcvy；輸入機(jī)器人跟隨加速度a_m以及插補(bǔ)周期dt；

定義期望加速a_i、位移同步允差S_ε、速度同步允差V_ε；定義同步階段跳轉(zhuǎn)系數(shù)k_s；定義加速同步階段實(shí)際時(shí)間t_a、加速同步階段參考時(shí)間T_a、加速同步階段影響系數(shù)k_a、減速同步階段實(shí)際時(shí)間t_d、減速同步階段參考時(shí)間T_d、減速同步階段影響系數(shù)k_d；

步驟2、劃分階段；

將整個(gè)追蹤與同步過程按順序劃分為三個(gè)階段，分別為加速同步階段、減速同步階段、同步階段；

步驟3、進(jìn)行加速同步階段的動態(tài)規(guī)劃；

設(shè)置三個(gè)同步階段跳轉(zhuǎn)條件，條件一：當(dāng)前插補(bǔ)周期中機(jī)器人的當(dāng)前位置Scur_i小于目標(biāo)位置Strgt_i；條件二：當(dāng)前插補(bǔ)周期中機(jī)器人當(dāng)前速度Vcur_i大于目標(biāo)速度Vtrgt_i；條件三：當(dāng)前插補(bǔ)周期中機(jī)器人當(dāng)前位置Scur_i與目標(biāo)位置Strgt_i之間的位移差ds_i小于機(jī)器人當(dāng)前位置Scur_i和同步階段跳轉(zhuǎn)系數(shù)k_s的乘積；

若當(dāng)前各項(xiàng)參數(shù)同時(shí)滿足條件一、條件二、條件三，則跳轉(zhuǎn)至步驟4進(jìn)入減速同步階段的動態(tài)規(guī)劃，若有一個(gè)條件不滿足，則先執(zhí)行插補(bǔ)運(yùn)動再進(jìn)行超時(shí)校驗(yàn)；

若超時(shí)，則機(jī)器人結(jié)束本次追蹤同步運(yùn)動并退出至安全位置；若執(zhí)行插補(bǔ)運(yùn)動后不超時(shí)，則重新進(jìn)行步驟3；

步驟4、進(jìn)行減速同步階段的動態(tài)規(guī)劃；

設(shè)置兩個(gè)同步條件，條件四：當(dāng)前插補(bǔ)周期中機(jī)器人當(dāng)前位置Scur_i與目標(biāo)位置Strgt_i之間的位移差ds_i小于位移同步允差S_ε；條件五：即當(dāng)前插補(bǔ)周期中目標(biāo)速度Vtrgt_i與機(jī)器人當(dāng)前速度Vcur_i的速度差小于速度同步允差V_ε，滿足速度同步允差要求；

若當(dāng)前各項(xiàng)參數(shù)同時(shí)滿足條件四和條件五，則跳轉(zhuǎn)至步驟5；若有一個(gè)條件不滿足，則先執(zhí)行插補(bǔ)運(yùn)動再進(jìn)行超時(shí)校驗(yàn)；

若超時(shí)，則機(jī)器人結(jié)束本次追蹤同步運(yùn)動并退出至安全位置；若執(zhí)行插補(bǔ)運(yùn)動后不超時(shí)，則重新進(jìn)行步驟4；

步驟5、完成追蹤進(jìn)入同步階段；

執(zhí)行插補(bǔ)運(yùn)動直至接收到同步終止命令或超出行程范圍時(shí)，結(jié)束并退出至安全位置；

在步驟3的當(dāng)前插補(bǔ)周期中，采用以下方式進(jìn)行動態(tài)規(guī)劃：

步驟3.1、通過用戶配置的傳送帶編碼器單元獲取當(dāng)前插補(bǔ)周期中的目標(biāo)位置Strgt_i與目標(biāo)速度Vtrgt_i，通過機(jī)器人控制器獲取機(jī)器人的當(dāng)前位置Scur_i與當(dāng)前速度Vcur_i；

步驟3.2、求解當(dāng)前插補(bǔ)周期中機(jī)器人當(dāng)前位置Scur_i與目標(biāo)位置Strgt_i之間的位移差ds_i＝|Strgt_i-Scur_i|；

步驟3.3、同步階段跳轉(zhuǎn)條件判斷：若當(dāng)前各項(xiàng)參數(shù)同時(shí)滿足條件一、條件二、條件三，則跳轉(zhuǎn)至步驟4進(jìn)入減速同步階段的動態(tài)規(guī)劃，若有一個(gè)條件不滿足，則進(jìn)入步驟3.4繼續(xù)本階段的動態(tài)規(guī)劃；

步驟3.4、執(zhí)行插補(bǔ)運(yùn)動：在當(dāng)前插補(bǔ)周期中，求解本周期的插補(bǔ)輸出位置Scur_i+1與插補(bǔ)輸出速度Vcur_i+1，同時(shí)記錄本階段實(shí)際時(shí)間t_a，將其數(shù)值遞增插補(bǔ)周期dt以備校驗(yàn)，具體的表達(dá)式如下：

步驟3.5、超時(shí)校驗(yàn)：在執(zhí)行插補(bǔ)過程中實(shí)時(shí)記錄本階段的實(shí)際時(shí)間t_a，該值由影響系數(shù)k_a和傳送帶無速度波動情況下的參考時(shí)間T_a共同約束，其中影響系數(shù)k_a設(shè)置相應(yīng)的取值范圍，可根據(jù)傳送帶速度波動情況設(shè)置；求解參考時(shí)間T_a時(shí)可建立如下關(guān)系式：

進(jìn)一步求解可得當(dāng)t_ak_a·T_a時(shí)，則認(rèn)為加速同步失敗，此時(shí)不再進(jìn)行同步階段跳轉(zhuǎn)，機(jī)器人結(jié)束本次追蹤同步運(yùn)動并退出至安全位置；

步驟3.6、若本周期中無同步階段跳轉(zhuǎn)且未因超時(shí)校驗(yàn)退出，則在下一個(gè)插補(bǔ)周期中重復(fù)步驟3.1至3.5進(jìn)行動態(tài)規(guī)劃；

在步驟4的當(dāng)前插補(bǔ)周期中，采用以下方式進(jìn)行動態(tài)規(guī)劃：

步驟4.1、通過用戶配置的傳送帶編碼器單元獲取當(dāng)前插補(bǔ)周期中的目標(biāo)位置Strgt_i與目標(biāo)速度Vtrgt_i，通過機(jī)器人控制器獲取機(jī)器人的當(dāng)前位置Scur_i與當(dāng)前速度Vcur_i；

步驟4.2、求解當(dāng)前插補(bǔ)周期中機(jī)器人當(dāng)前位置Scur_i與目標(biāo)位置Strgt_i之間的位移差ds_i，其表達(dá)式為ds_i＝|Strgt_i-Scur_i|；

步驟4.3、同步條件判斷：若當(dāng)前各項(xiàng)參數(shù)同時(shí)滿足條件四和條件五，則跳轉(zhuǎn)至步驟5；若有一個(gè)條件不滿足，則進(jìn)入步驟4.4繼續(xù)本階段的動態(tài)規(guī)劃；

步驟4.4、求解期望加速度a_i：定義期望加速度a_i為機(jī)器人在當(dāng)前狀態(tài)下能夠與目標(biāo)物體實(shí)現(xiàn)位置與速度同步的加速度，其表達(dá)式為

步驟4.5、執(zhí)行插補(bǔ)運(yùn)動：在當(dāng)前插補(bǔ)周期中，求解本周期的插補(bǔ)輸出位置Scur_i+1與插補(bǔ)輸出速度Vcur_i+1，同時(shí)記錄本階段實(shí)際時(shí)間t_d，將其數(shù)值遞增插補(bǔ)周期dt以備校驗(yàn)，具體的表達(dá)式如下：

步驟4.6、超時(shí)校驗(yàn)：在執(zhí)行步驟4.5的插補(bǔ)過程中實(shí)時(shí)記錄本階段的實(shí)際時(shí)間t_d，該值由影響系數(shù)k_d和傳送帶無速度波動情況下的參考時(shí)間T_d共同約束，其中影響系數(shù)k_d設(shè)置相應(yīng)的取值范圍，可根據(jù)傳送帶速度波動情況設(shè)置；參考時(shí)間T_d可通過聯(lián)立下式求解：

進(jìn)一步求解可得當(dāng)t_dk_d·T_d時(shí)，則認(rèn)為減速同步失敗，此時(shí)機(jī)器人結(jié)束本次追蹤同步運(yùn)動并退出至安全位置；

步驟4.7、若本周期中未進(jìn)入同步階段且未因超時(shí)校驗(yàn)退出，則在下一個(gè)插補(bǔ)周期中重復(fù)步驟4.1至4.6進(jìn)行動態(tài)規(guī)劃；

步驟5具體為：

步驟5.1、通過用戶配置的傳送帶編碼器單元獲取當(dāng)前插補(bǔ)周期中的目標(biāo)位置Strgt_i與目標(biāo)速度Vtrgt_i，通過機(jī)器人控制器獲取機(jī)器人的當(dāng)前位置Scur_i與當(dāng)前速度Vcur_i；

步驟5.2、執(zhí)行插補(bǔ)運(yùn)動：由于此時(shí)位置差與速度差均滿足同步允差要求，將目標(biāo)位置Strgt_i與目標(biāo)速度Vtrgt_i直接作為當(dāng)前周期的插補(bǔ)輸出，具體表達(dá)式如下：

步驟5.3、根據(jù)實(shí)際工藝需求執(zhí)行軌跡運(yùn)動：此時(shí)機(jī)器人已實(shí)現(xiàn)與目標(biāo)物體的同步，在同步運(yùn)動的基礎(chǔ)上進(jìn)一步疊加機(jī)器人常規(guī)的軌跡運(yùn)動以滿足實(shí)際工藝需求，若無特殊需求，則本步驟中不疊加運(yùn)動軌跡，機(jī)器人僅與目標(biāo)物體同步運(yùn)動；

步驟5.4、若本周期中機(jī)器人未接收到同步終止命令且未超出行程范圍，則在下一個(gè)插補(bǔ)周期中重復(fù)步驟5.1至5.3繼續(xù)執(zhí)行同步階段的運(yùn)動，否則機(jī)器人結(jié)束本次追蹤同步運(yùn)動并退出至安全位置。