[發(fā)明專利]一種運(yùn)動(dòng)路徑的規(guī)劃方法、系統(tǒng)、主機(jī)及存儲(chǔ)介質(zhì)

有效

• 申請(qǐng)?zhí)枺?/td>: 202011114914.2
• 申請(qǐng)日: 2020-10-16
• 公開（公告）號(hào): CN112207833B
• 公開（公告）日: 2021-08-17
• 發(fā)明（設(shè)計(jì)）人: 梁旺;石建軍;程國(guó)醒;苗立曉;范玉魁;李超
• 申請(qǐng)（專利權(quán)）人: 深圳市華成工業(yè)控制股份有限公司
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16
• 代理公司: 北京維正專利代理有限公司 11508
• 代理人: 任志龍

1.一種運(yùn)動(dòng)路徑的規(guī)劃方法，其特征在于，包括：

錄入步驟：

選取示教樣本，基于采樣策略在示教樣本上有序選取若干個(gè)控制點(diǎn)，并作為有序點(diǎn)集錄入存儲(chǔ)區(qū)；

在緩存區(qū)中建立執(zhí)行區(qū)和暫存區(qū)，其中，執(zhí)行區(qū)具有固定數(shù)量的存儲(chǔ)位，每個(gè)存儲(chǔ)位至多容納一個(gè)控制點(diǎn)；

基于調(diào)整策略向存儲(chǔ)區(qū)申請(qǐng)有序點(diǎn)集并錄入暫存區(qū)；

執(zhí)行步驟：

S1.從暫存區(qū)中的有序點(diǎn)集中移出一組控制點(diǎn)，并將該組控制點(diǎn)逐點(diǎn)寫入執(zhí)行區(qū)的存儲(chǔ)位中；

S2.對(duì)執(zhí)行區(qū)中的各控制點(diǎn)按寫入順序進(jìn)行B樣條擬合，以更新擬合軌跡；

S3.基于插補(bǔ)策略對(duì)擬合軌跡進(jìn)行插補(bǔ)以更新插補(bǔ)軌跡；

S4.驅(qū)動(dòng)執(zhí)行件沿插補(bǔ)軌跡移動(dòng)，檢測(cè)執(zhí)行件與插補(bǔ)軌跡節(jié)點(diǎn)的相對(duì)位置關(guān)系，并查詢執(zhí)行區(qū)以檢測(cè)剩余數(shù)量，其中，剩余數(shù)量為執(zhí)行件最新經(jīng)過的節(jié)點(diǎn)對(duì)應(yīng)的控制點(diǎn)按寫入順序排列在后的控制點(diǎn)數(shù)量；判斷剩余數(shù)量是否小于警戒值，若是，則清除執(zhí)行區(qū)中執(zhí)行件經(jīng)過插補(bǔ)軌跡節(jié)點(diǎn)對(duì)應(yīng)的控制點(diǎn)，并返回S1，直至?xí)捍鎱^(qū)中有序點(diǎn)集中的控制點(diǎn)被全部取出；其中，警戒值為小于存儲(chǔ)位數(shù)量的預(yù)設(shè)值；

S5.銷毀執(zhí)行區(qū)。

2.根據(jù)權(quán)利要求1所述的運(yùn)動(dòng)路徑的規(guī)劃方法，其特征在于，所述錄入步驟還包括：錄入預(yù)設(shè)擬合階數(shù)，設(shè)定B樣條擬合的階數(shù)為預(yù)設(shè)擬合階數(shù)；所述存儲(chǔ)位排序設(shè)置，并設(shè)置警戒節(jié)點(diǎn)，其中警戒節(jié)點(diǎn)為執(zhí)行區(qū)中倒數(shù)第N個(gè)存儲(chǔ)位，N為警戒值；

所述S1中控制點(diǎn)從暫存區(qū)中有序點(diǎn)集中定量移出，并依序?qū)懭雸?zhí)行區(qū)；

所述S4包括以下步驟：

S41.驅(qū)動(dòng)執(zhí)行件沿插補(bǔ)軌跡移動(dòng)；

S42.檢測(cè)執(zhí)行件與插補(bǔ)軌跡節(jié)點(diǎn)的相對(duì)位置關(guān)系，并查詢執(zhí)行區(qū)以檢測(cè)剩余數(shù)量，判斷剩余數(shù)量是否小于警戒值，若否，則返回S41；若是，則清除執(zhí)行區(qū)中位于警戒節(jié)點(diǎn)之前的存儲(chǔ)位數(shù)據(jù)；

S43.遷移執(zhí)行區(qū)中剩余的控制點(diǎn)至執(zhí)行區(qū)中序次靠前的存儲(chǔ)位，并按原有次序排列；

S44.返回S1，直至?xí)捍鎱^(qū)中有序點(diǎn)集中的控制點(diǎn)被全部取出。

3.根據(jù)權(quán)利要求2所述的運(yùn)動(dòng)路徑的規(guī)劃方法，其特征在于，

所述錄入步驟還包括：在緩存區(qū)中劃出用于容納中間數(shù)據(jù)的衍生數(shù)據(jù)區(qū)；

所述S2還包括：將擬合計(jì)算產(chǎn)生的中間數(shù)據(jù)寫入衍生數(shù)據(jù)區(qū)；

所述S3還包括：將插補(bǔ)計(jì)算產(chǎn)生的中間數(shù)據(jù)寫入衍生數(shù)據(jù)區(qū)中，其中插補(bǔ)計(jì)算產(chǎn)生的中間數(shù)據(jù)與控制點(diǎn)相對(duì)應(yīng)；

所述S42還包括：清除衍生數(shù)據(jù)區(qū)內(nèi)警戒節(jié)點(diǎn)之前數(shù)據(jù)位存儲(chǔ)的控制點(diǎn)相對(duì)應(yīng)的中間數(shù)據(jù)。

4.根據(jù)權(quán)利要求1所述的運(yùn)動(dòng)路徑的規(guī)劃方法，其特征在于，所述采樣策略包括以下步驟：

在輸入上均勻或非均勻地選取控制點(diǎn)；

將控制點(diǎn)按照選取順序排序并作為有序點(diǎn)集。

5.根據(jù)權(quán)利要求2所述的運(yùn)動(dòng)路徑的規(guī)劃方法，其特征在于，所述調(diào)整策略包括以下步驟：

將暫存區(qū)中有序點(diǎn)集的首位復(fù)制為至少k個(gè)重合點(diǎn)并并入暫存區(qū)中有序點(diǎn)集的頭部；將暫存區(qū)中有序點(diǎn)集的末位復(fù)制為至少k個(gè)重合點(diǎn)并并入暫存區(qū)中有序點(diǎn)集的尾部，其中，k=預(yù)設(shè)擬合階數(shù)-1。

6.根據(jù)權(quán)利要求1或5所述的運(yùn)動(dòng)路徑的規(guī)劃方法，其特征在于，所述采樣策略包括以下步驟：

錄入示教軌跡圖像；

將示教軌跡劃分為若干段局部示教軌跡，并計(jì)算各段局部示教軌跡的平均曲率半徑；

在各段局部示教軌跡上分別以等間距選取控制點(diǎn)，其中，各段局部示教軌跡上控制點(diǎn)的間距與各段局部示教軌跡的平均曲率半徑正相關(guān)。