王 慧，黃 帥

(中國電子科技集團(tuán)公司第四十五研究所，北京 100176)

插補(bǔ)模式或者PVT 通常被應(yīng)用于伺服控制系統(tǒng)的軌跡規(guī)劃。伺服驅(qū)動(dòng)器通過CAN 總線接收一系列的PVT點(diǎn)，每一個(gè)PVT點(diǎn)都由位置、速度、時(shí)間組成，伺服驅(qū)動(dòng)器在這些點(diǎn)之間進(jìn)行插補(bǔ)，以得到所需要的運(yùn)動(dòng)軌跡。PVT 控制最為關(guān)鍵的是分割時(shí)間的選擇，和分割時(shí)間內(nèi)相應(yīng)的速度設(shè)置，以及如何確定軌跡規(guī)劃時(shí)間。

1 PVT 分割時(shí)間的選擇

對(duì)于PVTPoints 要仔細(xì)考慮所使用的PVT點(diǎn)的數(shù)量，太少的點(diǎn)電機(jī)無法準(zhǔn)確跟隨速度曲線規(guī)劃，太多的點(diǎn)會(huì)導(dǎo)致CAN 總線網(wǎng)絡(luò)阻塞。最大數(shù)量的PVT 間隔取決于所應(yīng)用的網(wǎng)絡(luò)類型，對(duì)于CanOpen，間隔不能超過255 ms。

關(guān)于PVT 時(shí)間分割，較小的分段時(shí)間可以減少誤差，但是，整個(gè)過程必然會(huì)有一個(gè)誤差最小的分割間隔，如果想通過計(jì)算獲得系統(tǒng)誤差允許范圍內(nèi)最小的時(shí)間分割是非常困難的。通常最好的辦法就是系統(tǒng)最高頻率（位置環(huán)）的1/10～1/20（由數(shù)字采樣系統(tǒng)理論推出），此外，當(dāng)速度較小時(shí)，即位移變化較慢時(shí)，PVT點(diǎn)的時(shí)間間隔較長一點(diǎn)比較好，相反，當(dāng)速度較大時(shí)，即位移變化較快時(shí)，PVT點(diǎn)的時(shí)間間隔較短一點(diǎn)比較好。

針對(duì)不同的控制器，他們都有自己的PVT點(diǎn)數(shù)設(shè)置方法，用戶只是應(yīng)用已存在的設(shè)置方法來完成自己的需要。

2 速度的重要性

事實(shí)上，比分割時(shí)間段更為重要的就是在設(shè)定的分割時(shí)間內(nèi)，給出滿足其設(shè)定要求的準(zhǔn)確速度。

以T曲線為例，速度線性變化至目標(biāo)值，T型曲線軌跡規(guī)劃可分為三段，如圖1所示。

圖1 T型曲線軌跡

A1段加速度為確定值，即速度的變化規(guī)律。A2段加速度為0，速度達(dá)到目標(biāo)值；A3段為勻減速段。以此類推，當(dāng)給出確定的速度、加速度、減速度，就可以演化出S曲線波形，典型的S曲線如圖2所示。

圖2 S曲線位置軌跡

3 如何確定軌跡規(guī)劃時(shí)間和位移

3.1 確定軌跡規(guī)劃時(shí)間

當(dāng)給出確定的速度、加速度、減速度，則完成該運(yùn)動(dòng)所需要的時(shí)間就可以計(jì)算出來。將速度分解成3 段，加速段、勻速段、減速段，完成運(yùn)動(dòng)所需要的時(shí)間為T總。

Ta：加速段時(shí)間，Td：加速段時(shí)間，Tc：加速段時(shí)間，Aa為加速段的加速度，Ad為減速段的減速度。由圖1 可得：

同理：

勻速段加速度為0，則：

其中：Vt為目標(biāo)速度，Pt為目標(biāo)位置，Aa為加速度，Ad為減速度。

運(yùn)動(dòng)所需的時(shí)間是用于確定PVT點(diǎn)的間隔，所以：

其中：I為PVT點(diǎn)的時(shí)間間隔，Np為PVT 分割點(diǎn)數(shù)目。

3.2 軌跡規(guī)劃的位移

由圖1 可得加速段的位移是對(duì)速度的積分獲得。

其中：A1同為加速度段的位移，V0為初始速度。

當(dāng)其時(shí)速度為0，加速度不變時(shí)，該式可演化為：

即：

此刻的速度計(jì)算為：

勻速段的位移A2為：

此處的初始位移P0：

由于勻速段加速度為0，所以

減速段的位移A3計(jì)算公式為：

由此三個(gè)階段的總位移Pt：

將上式簡化為：

其中目標(biāo)位置就是將目標(biāo)速度對(duì)時(shí)間積分所得與加速度（或者減速度）對(duì)時(shí)間的積分所得和初始位置累加而得來的。積分的過程就是N個(gè)這樣時(shí)間點(diǎn)對(duì)應(yīng)的位置的累加，每個(gè)軸在設(shè)置上都采用同樣的速度，加速度，相同的目標(biāo)位置，從而在每個(gè)相同時(shí)間點(diǎn)上達(dá)到相同的位置，繼而實(shí)現(xiàn)理論上的同步。

4 PVT 應(yīng)用實(shí)例分析

4.1 實(shí)例硬件結(jié)構(gòu)

下圖是某設(shè)備的爐內(nèi)機(jī)械手，其機(jī)械結(jié)構(gòu)圖3所示。

在圖3 中，機(jī)械手前爪1、2 在機(jī)械結(jié)構(gòu)上由一個(gè)電機(jī)驅(qū)動(dòng)，機(jī)械手后爪1、2 在機(jī)械結(jié)構(gòu)上由另一個(gè)電機(jī)驅(qū)動(dòng)，兩個(gè)電機(jī)的可控行程完全一致，通過控制兩個(gè)電機(jī)的行走位置來完成具體的抓片動(dòng)作。在硅片抓取過程中不允許硅片晃動(dòng)，如果兩個(gè)電機(jī)速度不同步都會(huì)引起夾碎硅片，所以同步性非常重要。

圖3 機(jī)械手機(jī)構(gòu)圖

4.2 實(shí)例軟件執(zhí)行過程

Copley CanOpen 直接集成了PVT的軌跡算法，下面簡要介紹一下PVT 函數(shù)的設(shè)置方法。

首先引入SDO（主從對(duì)象字典，master →slave，一對(duì)一），PDO（過程對(duì)象字典master→master，master →slave，一對(duì)多）。

對(duì)于Copley 驅(qū)動(dòng)器來說，它的PVT 使用方法較為簡單，主要是通過Link 類來實(shí)現(xiàn)的，該函數(shù)主要適用于多軸聯(lián)動(dòng)。

該函數(shù)中，目標(biāo)位置的維數(shù)必須等于Linkage所控制的軸數(shù)，即作N 軸插補(bǔ)，則PVT點(diǎn)的坐標(biāo)維數(shù)為N。

這種方法使Link 函數(shù)規(guī)劃出一種N 維的點(diǎn)對(duì)點(diǎn)直線運(yùn)動(dòng)，運(yùn)動(dòng)中的速度、加減速度、和加加速度都將被限定在設(shè)定的范圍內(nèi)。MoveTo 函數(shù)得到聯(lián)動(dòng)指令會(huì)立即觸發(fā)運(yùn)動(dòng)。

該函數(shù)觸發(fā)一系列經(jīng)過計(jì)算的PVT點(diǎn)組成的運(yùn)動(dòng)軌跡，該計(jì)算值是浮點(diǎn)型的，所以在定義變量庫時(shí)要進(jìn)行浮點(diǎn)型定義。

其中：

參數(shù)pN 維位置坐標(biāo)點(diǎn)

參數(shù)maxvel 速度最大限制

參數(shù)maxacc 加速度最大限制

參數(shù)maxdec 減速度最大限制

參數(shù)maxjrk 加加速最大限制

參數(shù)start 如果為真，link 類中的軸開始聯(lián)動(dòng)，如果為假，規(guī)劃點(diǎn)將上傳，直到得到聯(lián)動(dòng)指令。

我們對(duì)基本函數(shù)進(jìn)行封裝后，得到多軸聯(lián)動(dòng)所需的模塊函數(shù)組：

long MC_LinkInit(int NodeID)；Link 類初始化

long MC_LinkHome(int NodeID)；同步回初始位

long MC_LinkEnable (int NodeID，bool bEnable)；同步驅(qū)動(dòng)器初始化

long MC_LinkAmpAbsMove (int NodeID，double Pos)；Link 絕對(duì)位移運(yùn)動(dòng)

longMC_SetLinkParam(doublevelLimit，double accelLimit，double decelLimit，double jerkLimit)；

long MC_LinkAbsMoveDone(double Pos1，double Pos2)；

long MC_LinkWaitMoveDone(int NodeID)；

long MC_LinkSetSoftLimit (int NodeID，double dPos，double dNeg，double dAcc)；

當(dāng)執(zhí)行兩個(gè)軸的同步運(yùn)動(dòng)時(shí)，直接調(diào)用以上函數(shù)就可達(dá)到控制所需的運(yùn)動(dòng)效果。在Copley 驅(qū)動(dòng)器的軟件介紹中，這種PVT 控制的同步誤差幾乎只有1～2個(gè)脈沖。

5 結(jié)論

PVT 是一種簡單的適用于任意位置軌跡規(guī)劃的計(jì)算規(guī)則，在不大量丟失精確數(shù)據(jù)的情況下，分段時(shí)間可以相應(yīng)的大一些，PVT 數(shù)據(jù)點(diǎn)既可以儲(chǔ)存在本地工作表中，也可以通過網(wǎng)絡(luò)通訊的方式下傳至各個(gè)軸。PVT 模式主要應(yīng)用于非高速網(wǎng)絡(luò)通訊的多軸控制系統(tǒng)。

[1]Calle Tecate.ADVANCED Motion Controls[Z].March 2001.

[2]Doxygen.Copley MotionLibrary Reference Manual[Z].Sep 2006.