隨著運(yùn)動(dòng)控制系統(tǒng)的發(fā)展,現(xiàn)代控制系統(tǒng)的功能越來越強(qiáng)大,一些相對(duì)復(fù)雜的控制算法相繼在控制加工系統(tǒng)中得到應(yīng)用,使得軌跡運(yùn)動(dòng)的速度和精度不斷的提高。在高速運(yùn)行過程中,要求控制系統(tǒng)對(duì)軌跡運(yùn)動(dòng)進(jìn)行平滑的控制,以防止較大的沖擊影響加工質(zhì)量,本文提出了一種新型的具有“前瞻”準(zhǔn)直線功能的速度平滑算法,可以有效地減少?zèng)_擊,提高加工速度和精度。所謂“前瞻”就是在加工過程中向前觀察一段距離,通過拐彎點(diǎn)以及速度允許條件判斷其是否是準(zhǔn)直線,然后根據(jù)s型速度曲線進(jìn)行粗插補(bǔ)、精插補(bǔ)計(jì)算。
基本原理
拐點(diǎn)判別
[align=center]
圖1 曲率半徑[/align]
如圖1,假設(shè)p1、p2、p3為一條軌跡中的三個(gè)點(diǎn),這三個(gè)點(diǎn)可以確定一個(gè)圓,該圓的半徑作為p2點(diǎn)曲率半徑,可以根據(jù)三點(diǎn)的坐標(biāo)然后根據(jù)半徑和最大加速度(作為向心加速度),求該曲率半徑下的允許速度:
aa=(vc’/delt_t)2/r;v’c=sqrt(raa)delt_t
在拐點(diǎn)允許速度、vh、曲率半徑允許速度中取最小值,作為該點(diǎn)的速度。向心加速度也可以分解為x、y兩個(gè)方向上的分加速度。
θ是和之間的夾角,ci=1/ri是pi點(diǎn)的曲率,給出判斷高曲率點(diǎn)的標(biāo)準(zhǔn)如下,此處的高曲率點(diǎn)作為第一類速度拐點(diǎn)[1](在拐點(diǎn)的位置處需要降速):
(a)ci>ci-1和ci>ci+1;
(b)θ≤90°;
(c)θ>90°。
如果點(diǎn)pi同時(shí)滿足標(biāo)準(zhǔn)(a)和(b),可以判定此點(diǎn)是一個(gè)尖銳拐點(diǎn);如果pi同時(shí)滿足標(biāo)準(zhǔn)(a)和(c),可以判定此點(diǎn)是一個(gè)高曲率點(diǎn)但不是尖銳拐點(diǎn)。非尖銳拐點(diǎn)有可能包含在準(zhǔn)直線段內(nèi),而尖銳拐點(diǎn)一定是降速點(diǎn)。
[align=center]
圖2 拐點(diǎn)和準(zhǔn)直線[/align]
準(zhǔn)直線概念
準(zhǔn)直線段的定義:兩拐點(diǎn)(或起始點(diǎn)與拐點(diǎn))之間各線段的組合。在準(zhǔn)直線段內(nèi),所有兩條線段的交點(diǎn)速度都小于允許速度,因此,可以作為一條直線來看待和計(jì)算。描述準(zhǔn)直線的重要參數(shù):起始速度v0、末速度ve、長(zhǎng)度(直線內(nèi)各線段長(zhǎng)度之和),當(dāng)這三個(gè)參數(shù)確定后,可以唯一確定與其對(duì)應(yīng)的s型速度曲線。如圖2中p1~p4就是一條準(zhǔn)直線,其長(zhǎng)度l=l1+l2+l3。直線段是準(zhǔn)直線段的特例。
準(zhǔn)直線段的安全性:當(dāng)一條準(zhǔn)直線段的末速度為最小速度v0時(shí),則這條準(zhǔn)直線是安全的,不會(huì)因?yàn)楹罄m(xù)的準(zhǔn)直線銜接不上導(dǎo)致沖擊,至多會(huì)引起停頓。
準(zhǔn)直線段的合并:前一條準(zhǔn)直線段的末速度等于后一條準(zhǔn)直線段的初速度。對(duì)后一條準(zhǔn)直線段來說,若其長(zhǎng)度l不能滿足從其初速度v’o 按照降速模型降至其末速度v’e ,則改準(zhǔn)直線段必須與前一條準(zhǔn)直線段合并成一條新的準(zhǔn)直線段。若l4不能滿足從其初速度v’o 降速到其末速度v’e,則p4p5與前一條準(zhǔn)直線段p1~p4合并成一條新的準(zhǔn)直線段p1~p5,該準(zhǔn)直線段的初速度vo、末速度為v’e、長(zhǎng)度l’=l1+l2+l3+l4。
任意一條準(zhǔn)直線段,只要其末速度ve>v0,都有可能與后一條準(zhǔn)直線段合并。合并過程是一個(gè)倒推過程,直到滿足降速模型。該倒推過程是針對(duì)準(zhǔn)直線段,與以前的算法相比,大大提高效率。
計(jì)算vt,在pi點(diǎn)處速度大小不變,方向發(fā)生了變化即產(chǎn)生了δv,δv=vtsin(θ/2)2,可得:
=
將aa帶入at,求出vt,δt=1ms;
判斷:if vt≤vh,那么vt為該點(diǎn)的許用速度vc=vt,該點(diǎn)為拐點(diǎn);
else vt>vh該交點(diǎn)不是拐點(diǎn),通過曲率半徑計(jì)算出許用速度,兩者取最小值作為該點(diǎn)的許用速度vc。
當(dāng)兩直線段交點(diǎn)處的允許速度vc大于或等于最大速度vh,則該交點(diǎn)不是拐點(diǎn),如圖2中p2和p3;根據(jù)初速度vo(或當(dāng)前速度v1)、及兩直線段交點(diǎn)的允許速度(vc
插補(bǔ)算法
定時(shí)插補(bǔ)算法
定時(shí)插補(bǔ)(粗插補(bǔ))算法是通過對(duì)加速度,速度的遞推計(jì)算出當(dāng)前段的速度,根據(jù)定時(shí)delta_t,得出來delta_l=v×delta_t,對(duì)準(zhǔn)直線用剩余距離來驗(yàn)算降速觸發(fā)點(diǎn),降速點(diǎn)的觸發(fā)過程如圖3所示。
[align=center]
圖3 用剩余距離來驗(yàn)算降速觸發(fā)點(diǎn)[/align]
粗插補(bǔ)算法
粗插補(bǔ)算法即為等長(zhǎng)分割,就是處理通過定時(shí)分割后得到的距離li(δxi, δyi),準(zhǔn)直線長(zhǎng)度為l=σli,就是對(duì)li進(jìn)行m等分,每段定長(zhǎng)為l的話,(即對(duì)x方向進(jìn)行了mx等分,對(duì)y方向進(jìn)行了my等分)。li= vi δt,如圖4所示,定時(shí)分割完以后形成v1,v2,v3…的速度圖,定長(zhǎng)分割后將v1和v2之間也進(jìn)行了細(xì)分,細(xì)分后的速度記為v11,v12,…,v1m。根據(jù)公式,根據(jù)公式求出:
計(jì)算v2,v3之間的速度值。
[align=center]
圖4 粗插補(bǔ)速度圖[/align]
結(jié)語
實(shí)驗(yàn)結(jié)果如圖5所示,實(shí)驗(yàn)中采用的參考數(shù)據(jù)為aa = 1000;ad = -1000; vsa = 2000; vsd = 2000;delt_t = 0.001;vh = 5000;ja = aa×aa/(2×vsa) ;jd = ad×ad/(2×vsd) ;編程使用c/c++基于vc6.0的環(huán)境,通過軟件生成g代碼下載到mpc05卡。
[align=center]
圖5 速度曲線[/align]
從圖5中明顯看出速度達(dá)到了很好的平滑,圖5中有六條準(zhǔn)直線,因此就存在六條s型速度曲線。通過在電機(jī)齒輪上的測(cè)試,機(jī)械振動(dòng)有了很明顯的改善,無明顯沖擊。實(shí)驗(yàn)證明該算法能夠很好的減小機(jī)械振動(dòng),提高機(jī)械切割的精度和質(zhì)量,在算法的誤差分析方面還有待進(jìn)一步的研究。