本發(fā)明涉及數(shù)控系統(tǒng)
技術(shù)領(lǐng)域：
，具體是涉及一種初加速度不為零的S型曲線速度控制方法。
背景技術(shù)：
：目前，用于數(shù)控系統(tǒng)、工業(yè)機(jī)器人操作作業(yè)的運(yùn)動(dòng)速度曲線有很多種，如梯形圖曲線、S型速度曲線和正弦速度曲線。速度曲線的作用是保證在啟動(dòng)、停止、調(diào)速或者兩段曲線之間過(guò)渡時(shí)不產(chǎn)生震蕩、沖擊，從而提高運(yùn)動(dòng)控制的精度。大多數(shù)的速度規(guī)劃都能夠?qū)崿F(xiàn)在給定首末速度、位置、最大速度、最大加速度和最大加加速度的情況下規(guī)劃出相應(yīng)的速度曲線，使得被控制的曲線軌跡的加速度連續(xù)。目前大多數(shù)的速度規(guī)劃算法要求起點(diǎn)加速度為零，這就使得一旦規(guī)劃完成，運(yùn)動(dòng)過(guò)程中不能隨意調(diào)整給定最大速度和給定末點(diǎn)速度，從而使得算法難以實(shí)時(shí)實(shí)現(xiàn)調(diào)速、暫停等功能，并為實(shí)現(xiàn)自動(dòng)運(yùn)行，調(diào)速、暫停之間切換，需要重新開(kāi)發(fā)一系列函數(shù)，并保存程序運(yùn)行過(guò)程中的大量現(xiàn)場(chǎng)數(shù)據(jù)和狀態(tài)。這就要付出大量時(shí)間來(lái)測(cè)試，并且當(dāng)某個(gè)現(xiàn)場(chǎng)數(shù)據(jù)或狀態(tài)更新出錯(cuò)時(shí)也不易發(fā)現(xiàn)和修正。目前S型曲線規(guī)劃在計(jì)算最大速度時(shí)由于公式過(guò)于復(fù)雜，一般需要采用數(shù)值法來(lái)求解，使得計(jì)算量大增，需要使用更加快速的芯片來(lái)完成，增加硬件成本。技術(shù)實(shí)現(xiàn)要素：有鑒于此，本發(fā)明提出了一種初始加速度不為零的S型曲線速度控制方法，該方法在每步運(yùn)行過(guò)程中都根據(jù)最新的給定最大速度，給定末點(diǎn)速度重新規(guī)劃，從而能夠通過(guò)修改最大速度和給定末點(diǎn)速度快速實(shí)時(shí)實(shí)現(xiàn)調(diào)速、暫停以及調(diào)速、暫停、重啟等過(guò)程之間的切換。由于在每步速度規(guī)劃過(guò)程中都按照運(yùn)行到最大速度預(yù)測(cè)一步，并為保證達(dá)到末點(diǎn)速度而實(shí)時(shí)規(guī)劃，因而無(wú)需耗費(fèi)大量時(shí)間來(lái)計(jì)算最大速度，故能夠快速實(shí)現(xiàn)算法，降低硬件要求和成本。本發(fā)明的目的是以下述方式實(shí)現(xiàn)的：一種初加速度不為零的S型曲線速度控制方法，所述方法分解為三個(gè)層面，即速度規(guī)劃層、步進(jìn)規(guī)劃層和位置規(guī)劃層；其中速度規(guī)劃層根據(jù)給定的起始速度v0、末點(diǎn)速度ve、起始加速度a0、最大加加速度jmax和最大加速度amax，進(jìn)行變速規(guī)劃，得到變速過(guò)程的加加速截止時(shí)間t1、勻加速截止時(shí)間t2、減加速截止時(shí)間t3以及當(dāng)前的加加速度j；步進(jìn)規(guī)劃層根據(jù)速度規(guī)劃層計(jì)算結(jié)果，計(jì)算任意時(shí)刻t的位置xr、速度vrx和加速度arx；位置規(guī)劃層根據(jù)速度規(guī)劃層和步進(jìn)規(guī)劃層的計(jì)算結(jié)果、起始點(diǎn)位置x0、末點(diǎn)位置xe以及最大速度vmax，計(jì)算當(dāng)前起始點(diǎn)的下一步運(yùn)動(dòng)位置、速度和加速度，并保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度不大于ve；當(dāng)當(dāng)前段鏈規(guī)劃長(zhǎng)度不夠時(shí)，將當(dāng)前段鏈鏈接下一條段鏈，并根據(jù)鏈接后段鏈總長(zhǎng)度重新計(jì)算，保證段鏈長(zhǎng)度足夠規(guī)劃。優(yōu)選地，設(shè)向量y0＝[x0v0a0]表示起始點(diǎn)位置、速度、加速度，設(shè)向量ye＝[xeve0]表示末點(diǎn)位置、速度、加速度，設(shè)向量y1＝[x1v1a1]表示下一步運(yùn)動(dòng)位置、速度、加速度，設(shè)向量y＝[xva]為預(yù)測(cè)的運(yùn)動(dòng)位置、速度、加速度，設(shè)給定的最大速度為vmax，所述位置規(guī)劃層包括以下步驟：步驟1：針對(duì)一條段鏈，如果末點(diǎn)位置xe與起始點(diǎn)位置x0之差ΔL小于一常量Ⅰδ1，即ΔL＝xe-x0＜δ1，且末點(diǎn)速度ve與起始點(diǎn)速度v0之差的絕對(duì)值Δv小于一常量Ⅱδ2，即|Δv|＝|ve-v0|＜δ2，則設(shè)置下一步運(yùn)動(dòng)向量y1為末點(diǎn)向量ye，即y1＝y(tǒng)e，返回參數(shù)y1；否則進(jìn)入步驟2；步驟2：以v0為起始速度，a0為起始加速度，vmax為末點(diǎn)速度調(diào)用速度規(guī)劃層函數(shù)，并按照計(jì)算結(jié)果調(diào)用步進(jìn)規(guī)劃層函數(shù)往前預(yù)測(cè)一步，得y1；接著以y1為起始點(diǎn)向量，ye為末點(diǎn)向量調(diào)用速度規(guī)劃層函數(shù)和步進(jìn)規(guī)劃層函數(shù)往前預(yù)測(cè)達(dá)到末點(diǎn)速度ve所需要運(yùn)行到的位置x，如果x≤xe，表明下一步運(yùn)動(dòng)到y(tǒng)1可以保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度不大于ve，因此y1即為下一步運(yùn)動(dòng)目標(biāo)，返回參數(shù)y1；否則進(jìn)入步驟3；步驟3：如果步驟2往前預(yù)測(cè)一步使用的加加速度j＝j(luò)max，則轉(zhuǎn)到步驟4，否則轉(zhuǎn)到步驟5；步驟4：以勻加速即j＝0調(diào)用步進(jìn)規(guī)劃層函數(shù)往前預(yù)測(cè)一步，得y1；接著以y1為起始點(diǎn)向量，ye為末點(diǎn)向量調(diào)用速度規(guī)劃層函數(shù)和步進(jìn)規(guī)劃層函數(shù)往前預(yù)測(cè)達(dá)到末點(diǎn)速度ve所需要達(dá)到的位置x，如果x≤xe，表明下一步運(yùn)動(dòng)到y(tǒng)1可以保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度不大于ve，因此y1即為下一步運(yùn)動(dòng)目標(biāo)，返回參數(shù)y1；否則進(jìn)入步驟5；步驟5：以y0為起始點(diǎn)向量，ye為末點(diǎn)向量調(diào)用速度規(guī)劃層函數(shù)和步進(jìn)規(guī)劃層函數(shù)往前預(yù)測(cè)一步，得y1，并往前預(yù)測(cè)達(dá)到末點(diǎn)速度ve所需要達(dá)到的位置x，如果x≤xe，表明下一步運(yùn)動(dòng)到y(tǒng)1可以保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度等于ve，因此y1即為下一步運(yùn)動(dòng)目標(biāo)，返回參數(shù)y1；否則進(jìn)入步驟6；步驟6：當(dāng)前段鏈規(guī)劃長(zhǎng)度不夠，將當(dāng)前段鏈鏈接下一條段鏈，并返回步驟2。優(yōu)選地，設(shè)置常量Ⅰδ1＝0.5(amax-a0)T2，設(shè)置常量Ⅱδ2＝JmaxT2。優(yōu)選地，設(shè)v0為起始速度，ve為S型速度規(guī)劃結(jié)束速度，首末速度差Δv＝v0-ve，a0為起始加速度，jmax為給定最大加加速度，amax為給定最大加速度，標(biāo)志速度常量vx＝Δv×flag，加速度常量ax＝a0×flag；所述速度規(guī)劃層函數(shù)包括以下步驟，其中t1、t2、t3和j分別為加加速截止時(shí)間、勻加速截止時(shí)間、減加速截止時(shí)間和當(dāng)前的加加速度：步驟A：當(dāng)ax＜0，且時(shí)，取j=jmax,t1=-ax+12ax2-jmaxvxjmax,t2=t1,t3=t1+t2+axj;]]>步驟B：當(dāng)ax≥0或時(shí)，(1)當(dāng)時(shí)，取j＝-jmax，t1=axjmax+12ax2+jmaxvxjmax,t2=t1,t3=t1+t2+axj;]]>(2)當(dāng)時(shí)，取j＝-jmax,t1=ax+amaxjmax,t2=vxamax+12ax2amaxjmax+axjmax,t3=t1+t2+axj;]]>步驟C：更新給定加加速度，即取j＝j(luò)×flag。優(yōu)選地，設(shè)t為當(dāng)前時(shí)刻，所述步進(jìn)規(guī)劃層函數(shù)根據(jù)以下公式計(jì)算當(dāng)前時(shí)刻t時(shí)的位置xr、速度vrx和加速度arx：(1)當(dāng)t≤t1時(shí)arx＝a0+jtvrx=v0+a0t+12jt2]]>xr=x0+v0t+12a0t2+16jt3]]>(2)當(dāng)t1＜t≤t2時(shí)arx＝a0+jt1vrx=v0+a0t-12jt12+jt1t]]>xr=x0+v0t+12a0t2+16jt13-12jt12t+12jt1t2]]>(3)當(dāng)t2＜t≤t3arx＝a0+jt1-j(t-t2)vrx=v0+a0t+jt1t-12jt12-12j(t-t2)2]]>xr=x0+v0t+12a0t2-16jt3+16j(t13+t23)+12j(t1+t2)t2-12j(t12+t22)t]]>(4)當(dāng)t≥t3時(shí)arx=a0+jt1-j(t3-t2)vrx=v0+a0t3+jt1t3-12jt12-12j(t3-t2)2xr=x0+v0t3+12a0t32-16jt33+16j(t13+t23)+12j(t1+t2)t32-12j(t12+t22)t3.]]>對(duì)初始加速度不為零的速度規(guī)劃3段時(shí)間點(diǎn)t1,t2,t3的計(jì)算，具體見(jiàn)速度規(guī)劃層設(shè)計(jì)，該部分以速度增量方向?yàn)檎?，按照加速度方向和速度增量大小，分?個(gè)條件獨(dú)立計(jì)算加加速度，勻加速度和減加速度時(shí)間結(jié)束點(diǎn)。步進(jìn)規(guī)劃層：對(duì)初始加速度不為零的任意時(shí)刻加速度，速度，位置計(jì)算，計(jì)算量小，易于實(shí)時(shí)實(shí)現(xiàn)。位置規(guī)劃層:采用按照達(dá)到最大速度預(yù)測(cè)一步，再測(cè)試當(dāng)前段鏈達(dá)到末點(diǎn)速度是否夠長(zhǎng)的策略，保證最終規(guī)劃速度不會(huì)大于最大速度(最大速度大于當(dāng)前速度)。相對(duì)現(xiàn)有技術(shù)，本發(fā)明的優(yōu)點(diǎn)是：1.只需要簡(jiǎn)單調(diào)整最大速度和末點(diǎn)速度參數(shù)就能實(shí)時(shí)實(shí)現(xiàn)S型調(diào)速、暫停、步進(jìn)等功能。2.本發(fā)明計(jì)算量少，能夠在較低成本的硬件上實(shí)現(xiàn)。3.本發(fā)明函數(shù)少，代碼量少，執(zhí)行時(shí)間短，可以滿足數(shù)控系統(tǒng)或機(jī)器人的運(yùn)動(dòng)控制的高實(shí)時(shí)性。附圖說(shuō)明圖1為本發(fā)明的速度規(guī)劃層的流程圖；圖2、圖3分別為本發(fā)明不同起始速度、起始加速度、末點(diǎn)速度情況下的速度和加速度規(guī)劃示意圖；圖4為本發(fā)明的位置規(guī)劃層的流程圖。圖5、圖6和圖7分別為本發(fā)明的S型曲線速度控制方法的仿真位置、速度和加速度結(jié)果示意圖。具體實(shí)施方式本發(fā)明采用下述技術(shù)方案：把規(guī)劃分解為三個(gè)層面進(jìn)行處理，即速度規(guī)劃層，步進(jìn)規(guī)劃層，位置規(guī)劃層，并分別采用3個(gè)函數(shù)實(shí)現(xiàn)。速度規(guī)劃層和步進(jìn)規(guī)劃層作為底層實(shí)現(xiàn)基本的S型速度規(guī)劃和步進(jìn)距離求解。其中速度規(guī)劃層根據(jù)給定的首末速度和首加速度進(jìn)行變速規(guī)劃，得到變速過(guò)程的加加速、勻加速、減加速的截止時(shí)間以及加加速度方向；步進(jìn)規(guī)劃層根據(jù)速度規(guī)劃層規(guī)劃結(jié)果計(jì)算一步或任意時(shí)間運(yùn)行后的位置，速度，加速度。位置規(guī)劃層作為外層，根據(jù)給定位置、最大速度、起始速度、起始加速度、末點(diǎn)速度計(jì)算一步運(yùn)行后的位置，速度，加速度，該函數(shù)可由實(shí)時(shí)運(yùn)行線程調(diào)用，能夠保證整個(gè)段鏈在初始加速度不為零時(shí)的S型曲線速度規(guī)劃的自動(dòng)實(shí)現(xiàn)。1)速度規(guī)劃層設(shè)Δv＝v0-ve，v0為起始速度，ve為S型速度規(guī)劃結(jié)束速度，a0為起始加速度，jmax為給定最大加加速度，amax為給定最大加速度，vx＝Δv×flag，ax＝a0×flag。步驟1：當(dāng)ax＜0，且時(shí)，取j=jmax,t1=-ax+12ax2-jmaxvxjmax,t2=t1,t3=t1+t2+axj;]]>步驟2：當(dāng)ax≥0或時(shí)，(3)當(dāng)時(shí)，取j＝-jmax，t1=axjmax+12ax2+jmaxvxjmax,t2=t1,t3=t1+t2+axj;]]>(4)當(dāng)時(shí)，取j＝-jmaxt1=ax+amaxjmax,t2=vxamax+12ax2amaxjmax+axjmax,t3=t1+t2+axj;]]>步驟3：更新給定加加速度，即取j＝j(luò)×flag。該規(guī)劃層以函數(shù)形式實(shí)現(xiàn)，記為VelocityPlan，其實(shí)現(xiàn)流程圖如圖1所示。2)步進(jìn)規(guī)劃層設(shè)v0為起始速度，a0為起始加速度，x0為起始位置。設(shè)t為當(dāng)前時(shí)刻，所述步進(jìn)規(guī)劃層根據(jù)以下公式計(jì)算當(dāng)前時(shí)刻t的位置xr、速度vrx和加速度arx：(1)當(dāng)t≤t1時(shí)arx＝a0+jtvrx=v0+a0t+12jt2]]>xr=x0+v0t+12a0t2+16jt3]]>(2)當(dāng)t1＜t≤t2時(shí)arx＝a0+jt1vrx=v0+a0t-12jt12+jt1t]]>xr=x0+v0t+12a0t2+16jt13-12jt12t+12jt1t2]]>(3)當(dāng)t2＜t≤t3arx＝a0+jt1-j(t-t2)vrx=v0+a0t+jt1t-12jt12-12j(t-t2)2]]>xr=x0+v0t+12a0t2-16jt3+16j(t13+t23)+12j(t1+t2)t2-12j(t12+t22)t]]>(4)當(dāng)t≥t3時(shí)arx＝a0+jt1-j(t3-t2)vrx=v0+a0t3+jt1t3-12jt12-12j(t3-t2)2]]>xr=x0+v0t3+12a0t32-16jt33+16j(t13+t23)+12j(t1+t2)t32-12j(t12+t22)t3]]>該規(guī)劃層以函數(shù)形式實(shí)現(xiàn)，記為GetStepPVAG，該函數(shù)返回yr＝[xrvrxarx]。不同起始速度、起始加速度、末點(diǎn)速度情況下的速度和加速度規(guī)劃效果如圖2和圖3所示。3)位置規(guī)劃層記向量y0＝[x0v0a0]表示當(dāng)前起始點(diǎn)位置、速度、加速度，ye＝[xeve0]為末點(diǎn)向量，y1＝[x1v1a1]為下一步給定向量，y＝[xva]為預(yù)測(cè)向量。步驟1：針對(duì)一條段鏈，如果末點(diǎn)給定位置xe與起始點(diǎn)位置x0之差小于一小量δ1，即ΔL＝xe-x0＜δ1，且末點(diǎn)速度ve與起始點(diǎn)速度v0之差的絕對(duì)值小于一小量δ2，即|Δv|＝|ve-v0|＜δ2，則取下一步給定向量y1為末點(diǎn)給定向量ye，即y1＝y(tǒng)e，否則進(jìn)入步驟2。一般可取δ1＝0.5(amax-a0)T2，δ2＝JmaxT2。步驟2：以當(dāng)前起始速度v0為起始速度，當(dāng)前加速度a0為起始加速度，最大速度vmax為末點(diǎn)速度，調(diào)用速度規(guī)劃函數(shù)VelocityPlan，并按照規(guī)劃結(jié)果調(diào)用步進(jìn)規(guī)劃函數(shù)GetStepPVAG往前預(yù)測(cè)一步，得y1。接著以y1為起始點(diǎn)向量，ye為末點(diǎn)向量調(diào)用函數(shù)VelocityPlan和函數(shù)GetStepPVAG往前預(yù)測(cè)達(dá)到末點(diǎn)速度ve所需要運(yùn)行到的位置x，如果x≤xe，表明下一步運(yùn)動(dòng)到y(tǒng)1可以保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度不大于ve，因此y1即為下一步運(yùn)動(dòng)目標(biāo)，否則進(jìn)入步驟3。步驟3：如果步驟2往前預(yù)測(cè)一步使用的加加速度j＝j(luò)max，則進(jìn)入步驟4，否則進(jìn)入步驟5。步驟4：以勻加速預(yù)測(cè)一步，即j＝0，得y1。接著以y1為起始點(diǎn)向量，ye為末點(diǎn)向量調(diào)用函數(shù)VelocityPlan和函數(shù)GetStepPVAG往前預(yù)測(cè)達(dá)到末點(diǎn)速度ve所需要達(dá)到的位置x，如果x≤xe，表明下一步運(yùn)動(dòng)到y(tǒng)1可以保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度不大于ve，因此y1即為下一步運(yùn)動(dòng)目標(biāo)，否則進(jìn)入步驟5。步驟5：以y0為起始點(diǎn)向量，ye為末點(diǎn)向量調(diào)用函數(shù)VelocityPlan和函數(shù)GetStepPVAG往前預(yù)測(cè)一步，得y1，并往前預(yù)測(cè)達(dá)到末點(diǎn)速度ve所需要達(dá)到的位置x，如果x≤xe，表明下一步運(yùn)動(dòng)到y(tǒng)1可以保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度等于ve，因此y1即為下一步運(yùn)動(dòng)目標(biāo)，否則進(jìn)入步驟6。步驟6：當(dāng)前段鏈規(guī)劃長(zhǎng)度不夠，將當(dāng)前段鏈鏈接下一條段鏈，并返回步驟2。該規(guī)劃層以函數(shù)形式實(shí)現(xiàn)，記為AutoVelocityPlan，具體實(shí)現(xiàn)流程圖如圖4所示。通過(guò)改變最大速度實(shí)現(xiàn)調(diào)速、暫停、重啟的S型規(guī)劃仿真結(jié)果如圖5至圖7所示。在0.25s時(shí)，設(shè)置vmax＝0；在1.2s時(shí)，設(shè)置vmax＝100；在1.5s時(shí)，設(shè)置vmax＝0；在2s時(shí)，設(shè)置vmax＝100；在0.25至3s內(nèi)，設(shè)置vmax＝25*rand(1)+25，rand(1)為程序產(chǎn)生的隨機(jī)數(shù)。本申請(qǐng)中的線段隊(duì)列表示按序存放的一定數(shù)量的數(shù)控代碼所代表的線段。段鏈由多段線段連接在一起組成，是為保證從起點(diǎn)速度運(yùn)行到末點(diǎn)速度長(zhǎng)度足夠長(zhǎng)而人為鏈接在一起的多段線段。以上所述的僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出，對(duì)于本領(lǐng)域的技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明整體構(gòu)思前提下，還可以作出若干改變和改進(jìn)，這些也應(yīng)該視為本發(fā)明的保護(hù)范圍。當(dāng)前第1頁(yè)1 2 3