徐張旗，陶家園，王克逸，簡(jiǎn)慧杰

(中國(guó)科學(xué)技術(shù)大學(xué) 精密機(jī)械與精密儀器系，安徽 合肥 230026)

增量式光電編碼器因其體積小、抗干擾性強(qiáng)、分辨率高、輸出穩(wěn)和成本低等特點(diǎn)廣泛應(yīng)用于伺服控制系統(tǒng)。其工作原理是將角度位移信息轉(zhuǎn)換成周期性脈沖信號(hào)，通過(guò)測(cè)量增量式編碼器輸出脈沖信號(hào)的頻率或周期即可得到電動(dòng)機(jī)的轉(zhuǎn)速。而測(cè)速的精度正是伺服控制系統(tǒng)精度得以保證的關(guān)鍵。

1 常見(jiàn)的測(cè)速方法

目前常見(jiàn)的測(cè)速方法[1-3]包括如下3種。

1)定時(shí)測(cè)角法(“M”法)。該方法通過(guò)測(cè)量給定時(shí)間間隔內(nèi)的脈沖數(shù)來(lái)計(jì)算轉(zhuǎn)速，以高頻時(shí)鐘脈沖時(shí)間為基準(zhǔn)，采樣受編碼器輸出脈沖信號(hào)量化誤差較大。而低速階段中輸出脈沖周期較大，對(duì)高頻時(shí)鐘固定時(shí)間間隔要求更大，不利于轉(zhuǎn)速的實(shí)時(shí)測(cè)量，因此“M”法在實(shí)際應(yīng)用中不適用于低速階段測(cè)量。定時(shí)測(cè)角法原理與誤差來(lái)源圖如圖1所示。

圖1 定時(shí)測(cè)角法原理與誤差來(lái)源圖

2)定角測(cè)時(shí)法(“T”法)。該方法通過(guò)測(cè)量2個(gè)相鄰脈沖之間的時(shí)間間隔來(lái)計(jì)算轉(zhuǎn)速，以編碼器輸出脈沖周期為基準(zhǔn)，采樣受高頻時(shí)鐘脈沖信號(hào)量化誤差影響較大。而中高速階段編碼器輸出脈沖周期較小，此時(shí)要想達(dá)到較高的精度，必須采用更高的采樣頻率，因此“T”法在實(shí)際應(yīng)用中不適用于中高速階段測(cè)量。定角測(cè)時(shí)法原理與誤差來(lái)源圖如圖2所示。

圖2 定角測(cè)時(shí)法原理與誤差來(lái)源圖

3)“M/T”法。該方法是將“M”法與“T”法結(jié)合，同時(shí)測(cè)量一定數(shù)量的脈沖和產(chǎn)生這些脈沖所用的時(shí)間，可有效抑制采樣時(shí)間間隔量化誤差的影響，得到在整個(gè)速度范圍內(nèi)精度較高的測(cè)量結(jié)果；但在低速運(yùn)行時(shí)，編碼器輸出的脈沖間隔較大，即采樣周期較大，使得檢測(cè)時(shí)間過(guò)長(zhǎng)，從而導(dǎo)致后級(jí)的伺服控制系統(tǒng)無(wú)法及時(shí)得到最新的編碼器狀態(tài)信息。

卡爾曼濾波是一種利用線性系統(tǒng)狀態(tài)方程，采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻的估計(jì)值和現(xiàn)時(shí)刻的觀測(cè)值來(lái)更新對(duì)狀態(tài)變量的估計(jì)，求出現(xiàn)時(shí)刻的估計(jì)值，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的方法。本文在“M”法和“T”法的基礎(chǔ)上，結(jié)合卡爾曼濾波，提出一種新型的變“M/T”高精度實(shí)時(shí)動(dòng)態(tài)測(cè)速方法。該方法能夠根據(jù)速度范圍，在“M”法和“T”法中擇優(yōu)選用，能夠在整個(gè)速度范圍內(nèi)有效地抑制采樣量化誤差，輸出相對(duì)實(shí)時(shí)、準(zhǔn)確的速度值。

2 新型變“M/T”動(dòng)態(tài)測(cè)速方法原理

(1)

式中，N為編碼器輸出分辨率。故可以將測(cè)速過(guò)程描述為一個(gè)離散時(shí)間的隨機(jī)過(guò)程。對(duì)測(cè)量過(guò)程做如下建模，假設(shè)線性離散系統(tǒng)的方程如下。

系統(tǒng)方程：

Xk=Ak·Xk-1+Bk·Uk+Wk

(2)

觀測(cè)方程：

Yk=Hk·Xk+Vk

(3)

圖3 新型變“M/T”算法原理圖

卡爾曼濾波算法具體計(jì)算流程[8]如下。

(4)

式中，Xk-1是上一狀態(tài)最優(yōu)的結(jié)果或給定的初值。

(5)

式中，Pk-1是上一狀態(tài)最優(yōu)估計(jì)值Xk-1的誤差協(xié)方差陣。

3)結(jié)合預(yù)測(cè)值和測(cè)量值，得到k狀態(tài)的最優(yōu)估計(jì)值Xk為：

(6)

4)更新Xk對(duì)應(yīng)的估計(jì)誤差協(xié)方差陣Pk為：

(7)

當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)，Pk就是式5中Pk-1?？梢?jiàn)，只要給定初始值X1和P1，算法可以自回歸運(yùn)算下去，結(jié)合時(shí)刻測(cè)量值Yk，就可遞推得到k時(shí)刻最優(yōu)轉(zhuǎn)速估計(jì)值Xk，從而實(shí)現(xiàn)整個(gè)速度段的精確測(cè)量。

3 試驗(yàn)與結(jié)果分析

3.1 硬件設(shè)計(jì)

試驗(yàn)采用CKD-AX4009T高速轉(zhuǎn)臺(tái)，編碼器輸出分辨率N為33 792脈沖/圈，試驗(yàn)時(shí)轉(zhuǎn)臺(tái)的速度通過(guò)信號(hào)發(fā)生器與CKD驅(qū)動(dòng)器來(lái)精確控制。驅(qū)動(dòng)器與測(cè)試轉(zhuǎn)臺(tái)如圖4所示。

圖4 CKD驅(qū)動(dòng)器與測(cè)試轉(zhuǎn)臺(tái)

轉(zhuǎn)臺(tái)所帶的增量式編碼器的輸出為2路正交的差分方波信號(hào)，這里采用德州儀器的DS26C32差分信號(hào)接收器，將差分信號(hào)轉(zhuǎn)換為普通數(shù)字電路可以接受的單端信號(hào)。新型變“M/T”高精度實(shí)時(shí)動(dòng)態(tài)測(cè)速算法在STM32F303RET6數(shù)字信號(hào)控制器中實(shí)現(xiàn)。該型號(hào)數(shù)字信號(hào)控制器包含了主頻高達(dá)72 MHz的ARM Cortex-M4內(nèi)核和浮點(diǎn)協(xié)處理器(FPU)，滿足新型變“M/T”測(cè)速方法對(duì)硬件的要求。測(cè)速系統(tǒng)的硬件實(shí)物圖如圖5所示，測(cè)速系統(tǒng)的硬件框圖如圖6所示。

圖5 測(cè)速系統(tǒng)硬件實(shí)物圖

圖6 測(cè)速系統(tǒng)硬件框圖

單片機(jī)的定時(shí)器4配置成正交編碼器輸入模式，同時(shí)開(kāi)啟輸入管腳的中斷觸發(fā)功能，觸發(fā)方式為上升沿與下降沿均觸發(fā)。單片機(jī)的定時(shí)器3每隔一段時(shí)間產(chǎn)生定時(shí)中斷，在輸入管腳的外部中斷例程里計(jì)算定時(shí)器3產(chǎn)生中斷的時(shí)間間隔。根據(jù)判斷的速度段，選擇相應(yīng)的計(jì)數(shù)中斷(即“T”法)或定時(shí)中斷(即“M”法)。CPU在中斷中讀取定時(shí)器4的計(jì)數(shù)值與定時(shí)器3對(duì)應(yīng)中斷的時(shí)間間隔來(lái)測(cè)量角速度；然后由卡爾曼濾波算法計(jì)算出角速度最優(yōu)估計(jì)值；最后通過(guò)USB2.0 FullSpeed接口，將新型的變“M/T”法的計(jì)算結(jié)果上傳至計(jì)算機(jī)。

3.2 試驗(yàn)結(jié)果及分析

選擇CKD轉(zhuǎn)臺(tái)在脈沖串輸入模式下，分別做低速段、高速段和勻變速段等3組試驗(yàn)。

編碼器給定理論輸出轉(zhuǎn)速ω=6.725°/s，在低速段對(duì)比“T”法與新型變“M/T”法，結(jié)果如圖7所示。由圖7可知，“T”法曲線受高頻時(shí)鐘脈沖信號(hào)量化誤差影響跳動(dòng)較大，新型變“M/T”法穩(wěn)定階段測(cè)量誤差在0.015°/s以內(nèi)，相對(duì)誤差在0.223%以內(nèi)，起到了很好的濾波作用，有效地抑制了量化噪聲。

圖7 低速段“T”法與卡爾曼算法對(duì)比圖

編碼器給定理論輸出轉(zhuǎn)速ω=399.103 3°/s，在高速段對(duì)比“M”法與新型變“M/T”法，結(jié)果如圖8所示。由圖8可知，“M”法曲線受編碼器輸出脈沖信號(hào)量化誤差影響跳動(dòng)較大，新型變“M/T”法穩(wěn)定階段測(cè)量誤差在0.3°/s以內(nèi)，相對(duì)誤差在0.751%以內(nèi)，同樣起到了很好的濾波和抑制量化噪聲的作用。

圖8 高速段“M”法與卡爾曼算法對(duì)比圖

信號(hào)發(fā)生器選擇掃頻脈沖輸入，即給定編碼器角加速度，1 s內(nèi)由靜止勻加速至ω=266.335°/s，循環(huán)往復(fù)，在勻加速段對(duì)比“M/T”法與新型變“M/T”法，結(jié)果如圖9和圖10所示。由圖9和圖10可知，新型變“M/T”法既能在整個(gè)速度段精確、實(shí)時(shí)地對(duì)速度進(jìn)行測(cè)量，也能很好地抑制噪聲，減少量化誤差[9-10]。

圖9 勻加速段“M/T”法與卡爾曼算法對(duì)比圖

圖10 “M/T”法與卡爾曼算法對(duì)比區(qū)間放大圖

4 結(jié)語(yǔ)

針對(duì)增量式編碼器測(cè)速問(wèn)題，提出了一種基于卡爾曼濾波的新型變“M/T”高精度動(dòng)態(tài)測(cè)速算法，根據(jù)速度預(yù)判，在“M”法、“T”法中進(jìn)行擇優(yōu)選用，既保證了測(cè)量的實(shí)時(shí)性，也提高了測(cè)量精度，擴(kuò)大了測(cè)速范圍，并在數(shù)字信號(hào)控制器上實(shí)現(xiàn)了這一算法。試驗(yàn)結(jié)果表明，采用該測(cè)速方法實(shí)現(xiàn)了在整個(gè)速度段的實(shí)時(shí)、準(zhǔn)確地對(duì)轉(zhuǎn)速進(jìn)行測(cè)量，能夠同時(shí)抑制和消除采樣時(shí)間間隔量化誤差的影響，對(duì)提高整個(gè)伺服控制系統(tǒng)的性能具有一定的應(yīng)用價(jià)值。