本發(fā)明屬于工業(yè)數(shù)控設(shè)備自動(dòng)化技術(shù)領(lǐng)域，涉及一種應(yīng)用于定插補(bǔ)周期的多軸數(shù)控設(shè)備的速度規(guī)劃方法。

背景技術(shù)：

在當(dāng)今的工業(yè)領(lǐng)域中，隨著工業(yè)自動(dòng)化進(jìn)程的加速，以及日益激烈的市場(chǎng)競(jìng)爭(zhēng)，使得大量的生產(chǎn)制造商不得不尋求更高效的生產(chǎn)方式，以求得生存。并且伴隨著人們物質(zhì)生活水平的提高，人工勞動(dòng)的成本也隨之上漲，這使得傳統(tǒng)的勞動(dòng)密集型的生產(chǎn)方式難以維持。所以為了在競(jìng)爭(zhēng)激烈的市場(chǎng)大環(huán)境下更好的發(fā)展，提高生產(chǎn)效率，降低生產(chǎn)成本，機(jī)器換人成為了必然的趨勢(shì)。隨著國(guó)內(nèi)中小企業(yè)對(duì)中低端運(yùn)動(dòng)控制設(shè)備的需求越來(lái)越大，多軸機(jī)械運(yùn)動(dòng)控制平臺(tái)備受青睞，如模板切割機(jī)、噴墨機(jī)、裁床等。

如今，所述運(yùn)動(dòng)控制平臺(tái)的控制系統(tǒng)也日趨成熟，目前許多運(yùn)動(dòng)控制平臺(tái)采用梯形速度曲線規(guī)劃方法，但在實(shí)際計(jì)算中，軌跡插補(bǔ)周期往往不是整數(shù)倍，這對(duì)控制速度及精度會(huì)帶來(lái)較大的影響，尤其是在定插補(bǔ)周期的前提下，每段軌跡中必須保證是整數(shù)插補(bǔ)周期，且不能有較大的速度突變，因此更難進(jìn)行速度規(guī)劃。針對(duì)上述問(wèn)題，本發(fā)明提出一種定插補(bǔ)周期的梯型曲線速度規(guī)劃方法，可有效實(shí)現(xiàn)機(jī)械運(yùn)動(dòng)控制平臺(tái)速度的平穩(wěn)控制；很好地解決了對(duì)于速度規(guī)劃插補(bǔ)計(jì)算中最后一個(gè)插補(bǔ)周期內(nèi)速度突變的問(wèn)題，實(shí)現(xiàn)了速度的平穩(wěn)過(guò)渡，提高了控制精度。

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

為了克服現(xiàn)有在定插補(bǔ)周期的前提下,對(duì)梯形速度曲線進(jìn)行速度規(guī)劃在最后速度突變問(wèn)題的不足，本發(fā)明提供了一種定插補(bǔ)周期的速度規(guī)劃方法，在保證整數(shù)個(gè)插補(bǔ)周期的前提下，對(duì)最后兩個(gè)插補(bǔ)周期速度突變的問(wèn)題提出了較好的解決方案。

本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案是：

一種定插補(bǔ)周期的速度規(guī)劃方法，包括如下步驟：

第一步，以中間點(diǎn)作為拐點(diǎn)對(duì)軌跡進(jìn)行分段，進(jìn)行速度規(guī)劃；設(shè)置速度規(guī)劃過(guò)程中的限定參數(shù)：每一個(gè)軌跡輪廓的起始速度vstart、運(yùn)動(dòng)過(guò)程中的最大速度vmax、最大加速度amax以及插補(bǔ)周期t；

第二步，根據(jù)第一步設(shè)定好的限定參數(shù)，根據(jù)每個(gè)拐點(diǎn)處的前后三個(gè)點(diǎn)的坐標(biāo)pi-1(xi-1,yi-1)、pi(xi,yi)、pi+1(xi+1,yi+1)，起點(diǎn)和終點(diǎn)除外，計(jì)算對(duì)應(yīng)拐點(diǎn)處的夾角θi，然后根據(jù)拐點(diǎn)處速度與夾角的關(guān)系式，確定該拐點(diǎn)的最大允許速度vcormax_i，i＝2、3···n-1。再采用倒推法，從軌跡的最后一點(diǎn)開始計(jì)算，將最后一點(diǎn)的速度vcor_n設(shè)置成啟動(dòng)速度，即vcor_n＝vstart，然后從最后一點(diǎn)開始向倒數(shù)第二點(diǎn)，以最大加速度做勻加速運(yùn)動(dòng)，計(jì)算運(yùn)動(dòng)到倒數(shù)第二點(diǎn)時(shí)的速度vcor_(n-1)，n為軌跡上的拐點(diǎn)數(shù)，將其與該點(diǎn)允許的最大拐點(diǎn)速度vcormax_(n-1)比較，取較小的為該拐點(diǎn)速度；以此類推計(jì)算其他拐點(diǎn)的速度vcor_i，i＝1、2···n；

第三步，根據(jù)起始點(diǎn)和各拐點(diǎn)處的速度對(duì)每段軌跡進(jìn)行速度規(guī)劃；

根據(jù)第二步計(jì)算所得軌跡上各拐點(diǎn)處的速度vcor_i，以及根據(jù)每段軌跡的起始點(diǎn)的x、y坐標(biāo)值，定義其中差值較大的軸作為長(zhǎng)軸，則另一個(gè)軸就為短軸，以長(zhǎng)軸作為參考軸來(lái)計(jì)算；從起點(diǎn)開始，根據(jù)梯形速度曲線對(duì)每段軌跡作加減速規(guī)劃，計(jì)算每個(gè)插補(bǔ)周期t時(shí)間內(nèi)的脈沖數(shù)；再根據(jù)每段軌跡的起點(diǎn)對(duì)應(yīng)的長(zhǎng)軸坐標(biāo)值，計(jì)算每個(gè)插補(bǔ)周期后的末點(diǎn)對(duì)應(yīng)軸的坐標(biāo)值，結(jié)合長(zhǎng)短軸之間的關(guān)系，計(jì)算出相應(yīng)短軸的坐標(biāo)值，實(shí)現(xiàn)兩周同步；再根據(jù)實(shí)際規(guī)劃出的每段的末速度vi_n與之前的對(duì)應(yīng)拐點(diǎn)速度vcor_i比較，取較小者為下一段的起始速度；

第四步，根據(jù)以上計(jì)算，每段軌跡插補(bǔ)周期都是非整數(shù)倍；

比較最后兩個(gè)插補(bǔ)周期的脈沖數(shù)，通過(guò)將之前的各個(gè)插補(bǔ)周期內(nèi)的脈沖數(shù)減小，使最后兩個(gè)插補(bǔ)周期的差值變小，最后兩個(gè)插補(bǔ)周期的脈沖數(shù)spn-1和spn，n為當(dāng)前段軌跡的插補(bǔ)周期數(shù)。第一次通過(guò)使前n-1個(gè)插補(bǔ)周期內(nèi)的脈沖數(shù)都減1，從而使spn增大，再比較spn-1和spn，如果spn-1>spn，則再?gòu)牡?個(gè)插補(bǔ)周期到第n-1個(gè)插補(bǔ)周期內(nèi)的脈沖數(shù)都減1，以此類推，直到spn-1＝spn。

進(jìn)一步，所述第二步中，根據(jù)每個(gè)拐點(diǎn)處的前后三個(gè)點(diǎn)的坐標(biāo)pi-1(xi-1,yi-1)、pi(xi,yi)、pi+1(xi+1,yi+1)，計(jì)算出當(dāng)前拐點(diǎn)夾角θi：

通過(guò)反三角函數(shù)求得θi的值，再根據(jù)拐點(diǎn)處速度公式：

計(jì)算出每個(gè)拐點(diǎn)的限制速度vcormax_i；根據(jù)軌跡上各拐點(diǎn)的坐標(biāo)值計(jì)算出每一段軌跡的長(zhǎng)度si：

各拐點(diǎn)速度值vcor_i：

其中，n為軌跡上拐點(diǎn)數(shù)，再將所計(jì)算得到的拐點(diǎn)速度vcor_i與拐點(diǎn)處的最大限制速度vcormax_i比較，如果vcor_i>vcormax_i，則將vcormax_i賦值給vcor_i，否則就取計(jì)算所得到的拐點(diǎn)速度值。

再進(jìn)一步，所述第三步中，先根據(jù)每段軌跡的起始位置點(diǎn)的x軸、y軸坐標(biāo)差值，取差值較大的軸為長(zhǎng)軸，另一個(gè)軸就為短軸；再根據(jù)梯形速度曲線的特點(diǎn)，在長(zhǎng)軸上做速度規(guī)劃，每個(gè)插補(bǔ)周期t內(nèi)作勻速運(yùn)動(dòng)，每個(gè)插補(bǔ)周期段內(nèi)的起始速度vj

如果處于加速段，則

vj＝vj-1+amax*t

如果處于減速段，則

vj＝vj-1-amax*t

如果處于勻速段，則

vj＝vmax

那么，一個(gè)插補(bǔ)周期內(nèi)電機(jī)運(yùn)動(dòng)的脈沖數(shù)spj為：

spj＝vj*t

再根據(jù)當(dāng)前軌跡的斜率計(jì)算出相應(yīng)短軸的脈沖數(shù)，以t時(shí)間進(jìn)行速度規(guī)劃，得到當(dāng)前軌跡的末速度vn，將vn與之前該拐點(diǎn)的速度vcor_i進(jìn)行比較，取較小的速度值作為下一段軌跡的起始速度。

本發(fā)明的有益效果主要表現(xiàn)在：在對(duì)初始限定參數(shù)設(shè)定后，每一段最后不足一個(gè)插補(bǔ)周期內(nèi)的速度突變的問(wèn)題得到了很好的解決，使每一段軌跡之間都平緩的過(guò)渡。

附圖說(shuō)明

圖1為定插補(bǔ)周期速度規(guī)劃方法的程序流程圖。

圖2為某一個(gè)拐點(diǎn)夾角示意圖。

圖3為某一個(gè)輪廓軌跡示意圖。

圖4為輪廓軌跡其中一段的加減速規(guī)劃及優(yōu)化對(duì)比示意圖。

具體實(shí)施方案

下面結(jié)合附圖以及具體實(shí)例對(duì)本發(fā)明作進(jìn)一步描述。

結(jié)合圖1～圖4，一種定插補(bǔ)周期的速度規(guī)劃方法，包括以下步驟：

第一步，設(shè)置一些規(guī)劃過(guò)程中的限定參數(shù)：每一個(gè)軌跡輪廓的起始速度vstart、運(yùn)動(dòng)過(guò)程中的最大速度vmax、最大加速度amax以及插補(bǔ)周期t；

第二步，根據(jù)第一步設(shè)定好的限定參數(shù)，根據(jù)第一步設(shè)定好的限定參數(shù)，根據(jù)每個(gè)拐點(diǎn)出的前后三個(gè)點(diǎn)的坐標(biāo)pi-1(xi-1,yi-1)、pi(xi,yi)、pi+1(xi+1,yi+1)，起點(diǎn)和終點(diǎn)除外，計(jì)算對(duì)應(yīng)拐點(diǎn)處的夾角θi，然后根據(jù)拐點(diǎn)處速度與夾角的關(guān)系式，確定該拐點(diǎn)的最大允許速度vcormax_i，i＝2、3···n-1。采用倒推法，從軌跡的最后一點(diǎn)開始計(jì)算，將最后一點(diǎn)的速度vcor_n設(shè)置成啟動(dòng)速度，即vcor_n＝vstart，然后從最后一點(diǎn)開始向倒數(shù)第二點(diǎn)，以最大加速度做勻加速運(yùn)動(dòng)，計(jì)算運(yùn)動(dòng)到倒數(shù)第二點(diǎn)時(shí)的速度vcor_(n-1)，將其與該點(diǎn)允許的最大拐點(diǎn)速度vcormax_(n-1)比較，取較小的為該拐點(diǎn)速度。以此類推計(jì)算其他拐點(diǎn)的速度；

如圖2、圖3所示，根據(jù)夾角公式，分別計(jì)算出第2、3、4、5點(diǎn)處的夾角θ2、θ3、θ4、θ5，再根據(jù)拐點(diǎn)速度公式分別確定各拐點(diǎn)處的最大限制速度：

根據(jù)軌跡上各拐點(diǎn)的坐標(biāo)值計(jì)算出每一段軌跡的長(zhǎng)度si：

各拐點(diǎn)速度值vcor_i：

vcor_1＝vcor_6＝vstart

再將所計(jì)算得到的拐點(diǎn)速度vcor_i與拐點(diǎn)處的最大限制速度vcormax_i比較，如果vcor_i>vcormax_i，則將vcormax_i賦值給vcor_i，否則就取計(jì)算所得到的拐點(diǎn)速度值；

第三步，根據(jù)起始點(diǎn)和各拐點(diǎn)處的速度對(duì)每段軌跡進(jìn)行速度規(guī)劃。

如圖3、圖4所示，根據(jù)第二步計(jì)算所得軌跡上各拐點(diǎn)處的速度vcor_i(i＝1、2···6)，選取1，2兩點(diǎn)為例進(jìn)行說(shuō)明，先根據(jù)1、2兩點(diǎn)的x、y軸坐標(biāo)差值，取差值較大的x軸為長(zhǎng)軸。再根據(jù)梯形速度曲線的特點(diǎn)，在長(zhǎng)軸上做速度規(guī)劃，每個(gè)插補(bǔ)周期t內(nèi)作勻速運(yùn)動(dòng)，每個(gè)插補(bǔ)周期段內(nèi)的起始速度vj為：

v0＝vstart

v1＝v0+amax*t

v2＝v1+amax*t

···

v8＝v7+amax*t

v9＝v8-amax*t

v10＝v9-amax*t

vend1＝v10-amax*t

如果有勻速段

vj＝vmax

一個(gè)插補(bǔ)周期內(nèi)電機(jī)在長(zhǎng)軸x軸上運(yùn)動(dòng)的脈沖數(shù)spj

spj＝vj*t

再根據(jù)當(dāng)前軌跡的斜率計(jì)算出相應(yīng)y軸的脈沖數(shù)，實(shí)現(xiàn)兩軸同步。一直以t時(shí)間進(jìn)行速度規(guī)劃，可以得到當(dāng)前軌跡的末速度vend1，將vend1與之前對(duì)應(yīng)拐點(diǎn)的速度vcor_i進(jìn)行比較，取較小的速度值作為下一段軌跡的起始速度。

第四步，根據(jù)以上計(jì)算，往往每段軌跡插補(bǔ)周期都是非整數(shù)倍，根據(jù)實(shí)際計(jì)算結(jié)果，解決每段軌跡的最后一小段不足一個(gè)插補(bǔ)周期的速度突變問(wèn)題。如圖4上部所示，假設(shè)第一段有10個(gè)完整的插補(bǔ)周期，最后剩余0.6t。所以在每一段的最后不足一個(gè)插補(bǔ)周期的時(shí)間內(nèi)，速度容易發(fā)生較大的突變，使得電機(jī)出現(xiàn)抖動(dòng)。

所以通過(guò)減小前面插補(bǔ)周期內(nèi)的脈沖數(shù)，以減小最后兩個(gè)插補(bǔ)周期的脈沖數(shù)的差值，從而防止電機(jī)出現(xiàn)抖動(dòng)。最后兩個(gè)插補(bǔ)周期的脈沖數(shù)sp10和sp11，第一次通過(guò)使sp1～sp10的值減1，相當(dāng)于sp11增加了1，再比較sp10和sp11，如果sp10>sp11，則再?gòu)牡?個(gè)插補(bǔ)周期到第10個(gè)插補(bǔ)周期內(nèi)的脈沖數(shù)sp2～sp10的值減1，以此類推，直到sp10＝sp11，如圖4下部所示。