QEP-CORDIC 演算法

第二章弦波編碼器解碼演算法

2.3 精細位置解碼

2.3.3 QEP-CORDIC 演算法

從圖 2-15 中得知原始 CORDIC 的輸出週期與線性化輸出一樣是

π

，而且範圍在

~ 2 2

π

−

π

之間，不適合與粗略位置資訊做結合，因此在這提出一個專用於弦波編碼器解

碼的修正法則。首先釐清目前的兩個問題及想要達到的目標，一是要把原本週期是

π

的輸出轉為週期為

π

輸出；二是把原本輸出的範圍由

~ 2 2

π

−

π

轉為

0 ~ 1

之間。

就第一個問題而言，本文提出一個輸入信號象限對應轉換的方法，將所有的輸入信號轉換到相對應的第一象限位置上，如此一來便可使輸出周期變為

π

，而輸出範圍則變

為0~

π

之間。

表 2-3 象限對應對照表

原初始位置修正後初始位置修正後起始軸第一象限

( x ₀

, y

₀ ) ( x ₀

, y

₀ )

+x 第二象限

( x ₀

, y

₀ ) ( y ₀

, x−

₀ )

+y 第三象限

( x 0

, y

0 ) (

−

x 0

, y−

0 )

-x

第四象限

( x ₀

, y

₀ ) (

−

y ₀

, x

₀ )

-y

圖 2-18 象限對應圖

利用表 2-3 很快的便可知道新舊輸入的對應關係，如下圖 2-19 顯示週期確實降為

π

，就如同把

~ 2 0

π

這段輸出複製到其他象限一樣的效果。並且與圖 2-20 相比看出經過轉換後的輸出與理想波形都齊集大小都相當符合。

針對第二個問題最直接的方法則是由 ROM 中的

θ _i

去做改變，如 2.3.2 中所做的第

二個假設，我們在 ROM 中需要存著

θ _i

=tan

⁻ ¹

⁻ ⁱ

的數值以供轉角累加方程式做累加用，

Original CORDIC Output

Position(rad)

Modify CORDIC Output

Position Coarse Position Output (A phase)

0 1 2 3 4 5 6

0 0.5 1

Position(rad) Coarse Position Output (B phase)

0 1 2 3 4 5 6

Linearizae output After Modify

Position(rad)

圖 2-20 理想精細位置與粗略位置對照波形

第3章第三章弦波編碼器解碼實現

3.1 DSP-Based 線性化法解碼實驗

本節首先介紹 DSP-Based 線性化解碼法的各項硬體，包含了實驗用的弦波編碼器及測試用的伺服馬達連接裝置及 DSP 實驗版，在 3.1.2 中介紹了 ADC 校準法以提供更精確的 ADC 取樣信號，增加解碼後的精度，最後實際測試 DSP-Based 線性化解碼後的速度回授結果。

3.1.1 硬體介紹

弦波編碼器

本論文所用的弦波編碼器是由 HPB 公司所生產的弦波編碼器，可以提供 2048 PPR 的正弦/餘弦信號，同時也提供 1 PPR 的正弦/餘弦信號作為絕對位置的判斷，除此之外傳統編碼器所提供的 Z 相信號也以弦波的方式輸出。實體如下圖

圖 3-1 弦波編碼器實體圖

編碼器需要供給 5V 電源，信號輸出的方式是以差動輸出以減少雜訊影響，所有的弦波電壓範圍都是在 2~3V 之間，並且額外提供 2.5V 直流電壓以供判斷使用，下是弦波編碼器各信號線功能說明

表 3-1 弦波編碼器各線功能表線色功能輸入/出電壓

藍

A ⁺ 2 . 5 + sin ( 2048 × θ )

藍黑

A ⁻ 2 . 5 − sin ( 2048 ×

)

綠

B ⁺ 2 . 5 + cos ( 2048 ×

)

綠黑

B ⁻ 2 . 5 − cos ( 2048 ×

)

黃

Z ⁺

Z index 黃黑

Z ⁻

Z index 褐

A

′

⁺ 2 . 5 + sin ( )

θ 褐黑

A

′

⁻ 2 . 5 − sin ( )

θ 白

B

′

⁺ 2 . 5 + cos ( )

θ 白黑

B

′

⁻ 2 . 5 − cos ( )

灰 Med 2.5

灰黑 Gnd 0

紅

V cc

黑 Gnd 0

DSP 解碼核心

近年來由於超大型積體電路(VLSI)的技術迅速發展，使得 DSP 數位訊號微控器的運算速度及周邊處理功能有大幅的提升，取代了傳統的微處理器(microprocessor)，在許多應用領域如行動通訊、資訊家電與馬達驅動等都被廣泛的使用。因此本研究採用由德州儀器(Texas Instrument，TI)公司研發的 TMS320F2812 這顆 DSP 作為控制核心。

利用 DSP 強大的數學運算能力，可以最少的硬體周邊電路實現馬達控制所需要的功能。而在德州儀器公司所生產的 DSP 中，’C2000 系列的硬體發展卡是專為馬達控制而設計的，其中本論文所使用的是此系列中的 F2812 晶片控制器，其擁有 150 MHz (6.67 ns cycle time)的高速運算處理能力，比 TI 早期出產的 DSP ‘C240 快 7 倍，比傳統 8051 單晶片快約 150 倍。為了提升運算效能，核心部份的算數邏輯單元(ALU)、累積器(ACC) 均採用 32 位元定點運算，並以硬體方式實現乘法器、位移器等，如此僅需一個指令週期(instruction cycle)即可完成乘加運算。在整數計算方面，為減少數值計算所衍生的問題如溢位(overflow)等，採用硬體式的位移器來提高軟體執行的精確度。

F2812 主要擁有的記憶體有 28K × 16 Flash EEPROM、兩組 4K × 16 Single-Access RAM (SARAM)與一組 8K × 16 SARAM，並採用哈佛匯流排(Harvard bus)架構。舊型的 DSP 大部分是採用分離式的程式記憶體、資料記憶體與 I/O 記憶體，而新型的 DSP 如 F2812 則是使用包含上述三種記憶體的單獨記憶體空間，使用起來更為有彈性，也可以減少不同記憶體之間搬動資料的指令集。

Spectrum Digital 公司利用 F2812 晶片結合自行開發的周邊模組，發展出 eZdspTM F2812 初學板(DSP Started Kit，DSK)，其週邊包含 16 個通道的 12 位元類比數位轉換器 (ADC)，串列傳輸支援四種常用的型式：SPI (serial peripheral interface)、SCI (serial communications interface)、eCAN (enhanced controller area network)與 McBSP

(multi-channel buffered serial port)，另外最高可支援 56 個 GPIO (general-purpose I/O)。為 TMS320F2812 功能方塊圖。

圖 3-2 TMS320F2812 功能方塊圖

由於弦波編碼器的解碼中類比數位轉換器占有相當重要的地位，所以特別加以說明內建之 12 位元的類比數位轉換器的各項功能如下：

z 內建兩組取樣及維持(Sample-and-hold)模組的 12 位元 ADC z 兩組取樣及維持模組共有 16 的輸入通道

z 取樣及維持模組可分為同步取樣模式或是依序取樣模式 z 類比輸入範圍 0~3V

z 快速的轉換能力，最大取樣率為 12.5 Msps

z 自動取樣轉換功能，可在一次中斷中執行 16 次的 ADC 轉換，每次的轉換可任意指定 16 個通道中的任一個

z 16 個轉換結果暫存器，最高可儲存 16 次的轉換結果 z 擁有四種不同的方法啟動 ADC 轉換

z 可設定連續轉換模式

介面電路

圖 3-4 DSP-Based 弦波編碼器解碼硬體圖

上圖 3-4 是由 eZdsp 實驗發展版與介面電路所組合而成的 DSP-Based 弦波編碼器解碼硬體圖，上層是介面電路板，除了電位的提升功能之外，還包含下節所使用到的 ADC 校準電路及伺服馬達控制所需的其他功能，在此便不多做說明，而下層則是 eZdsp 實驗發展版。為了將雜訊影響降到最低，兩者採上下兩層重疊方式組合而成，減少線路連接所造成的雜訊干擾。

3.1.2 即時線上 ADC 校準

弦波編碼器的解碼過程中，ADC 取樣弦波信號用來解碼精細位置資訊，所以 ADC 的取樣信號可信度是影響弦波解碼後精度的最大因素之ㄧ。DSP-Based 的弦波編碼器解碼是以線性化解碼法為主，使用的 ADC 是 TI 公司 TMS320F2812 數位信號處理器(DSP) 所內建的 12-bit ADC，由於所使用的實驗發展版 eZdsp 上並沒有設置校準 ADC 的周邊硬體電路，而弦波取樣需要極為精確可信的取樣結果，所以我們使用一個 ADC 校準方

法，利用外部電路提供兩個穩定已知的電壓信號作為參考電壓，強制將這兩個取樣結果指定到我們所要的輸出位置上，搭配上 DSP 本身所具有的運算功能，便可算出實際的增益值及偏移量，在每次取樣時都能進行電壓的校準及補償。

上圖 3-5 是 ADC 的校準示意圖，我們假設理想的 ADC 方程式為

mi

x

y

= ×

(3.1)

其中輸入為 x，輸出為 y，mi 是理想的增益值，以實驗用的 12-bit 對應到 0~3V 的 ADC 來說，增益值應該為 1365

0 3

1 2

¹²

− =

− 。而實際的 ADC 輸出方程式則是

b ma x

y

= × +

(3.2)

ma 是實際的增益，b 則是實際偏移量(offset)，這兩個數值在每次進行校準運算的時

候都會從新運算。

校準的流程是在每次 ADC 取樣後，我們都會得到兩組輸入與三組輸出數據，分別 是已知輸入的高電壓輸出 y

_H

、已知輸入的低電壓輸出 y

_L

及未知輸入的電壓輸出 y，在加 上我們原本做為基準的輸入 x

H

與 x

L

共五個參數。

首先求出實際增益量 ma

L H

x x

y ma y

−

= − (3.3)

接著求出實際偏移量 b

圖 3-5 ADC 校準示意圖 [9]

ma x y

b = _L − _L × (3.4)

估測出未知輸出 y 所應對應到的輸入

xˆ

ma b x y

−

(3.5)

最後將估測出的輸入

xˆ

經過理想的 ADC 輸出方程式得到理想輸出值

mi

x

y

ˆ= ˆ×

(3.6)

即時線上 ADC 校準流程如下：

1. 先以方程式(3.3)算出實際增益量 ma 2. 再以方程式(3.4)求出實際偏移量 b

3. 利用 ADC 取樣結果 y 與方程式(3.5)估測出目前輸入電壓

xˆ

4. 最後以方程式(3.6)將估測輸出電壓對應到理想輸出 yˆ

經過上述四個步驟便可將原本未經校準補償的輸出 y 補償成理想輸出 yˆ ，而且在每 次 ADC 取樣時都執行這樣校準，以得到最佳的 ADC 取樣效果。實驗中所使用提供穩定電壓的 IC 分別為 LD1117 與 LM317 兩種，LD1117 提供 2.5V 左右的穩定電壓，實際測量後實驗所用的 LD1117 應為 2.485V，而 LM317 則是一顆可調整輸出的穩壓 IC，其最低輸出電壓約為 1.25V，在實際測量後實驗用的 LM317 輸出為 1.260V。

以下實驗是在不同時間裡用同樣的電壓測試 ADC 輸出，經過實驗證實內建 ADC 在不同時間下取樣可能會有時變問題，所以每次取樣都應該進行即時線上校準，不適合以離線方式先行算出再補償。

0 200 400 600 800 1000 1200

1925 1930 1935 1940 1945 1950 1955

ADC Before Calibration in 1.352V

0 200 400 600 800 1000 1200

1925 1930 1935 1940 1945 1950 1955

ADC Before Calibration in 1.352V

(a) 第一次取樣 (b) 第二次取樣

0 200 400 600 800 1000 1200

ADC Before Calibration in 1.352V

0 200 400 600 800 1000 1200

1925

ADC Before Calibration in 1.352V

(c) 第三次取樣 (d) 第四次取樣

0 0.005 0.01 0.015 0.02 0.025 0.03

180

Output value

ADC Original output (0.136V)

0 0.005 0.01 0.015 0.02 0.025 0.03

180

Output value

ADC after Calibration and Average output (0.136V)

(a) 0.136V 校準前 (b) 0.136V 校準後

0 0.005 0.01 0.015 0.02 0.025 0.03

Output value

ADC Original output (0.471V)

0 0.005 0.01 0.015 0.02 0.025 0.03

600

Output value

ADC after Calibration and Average output (0.471V)

(c) 0.471V 校準前 (d) 0.471V 校準後

0 0.005 0.01 0.015 0.02 0.025 0.03

1930

Output value

ADC Original output (1.417V)

0 0.005 0.01 0.015 0.02 0.025 0.03

1930

Output value

ADC after Calibration and Average output (1.417V)

(e) 1.417V 校準前 (f) 1.417V 校準後

0 0.005 0.01 0.015 0.02 0.025 0.03

2200

Output value

ADC Original output (1.622V)

0 0.005 0.01 0.015 0.02 0.025 0.03

2200

Output value

ADC after Calibration and Average output (1.622V)

(g) 1.622V 校準前 (h) 1.622V 校準後

0 0.005 0.01 0.015 0.02 0.025 0.03

Output value

ADC Original output (2.53V)

0 0.005 0.01 0.015 0.02 0.025 0.03

3440

Output value

ADC after Calibration and Average output (2.53V)

(i) 2.53V 校準前 (j) 2.53V 校準後

0 0.005 0.01 0.015 0.02 0.025 0.03

3950 4000 4050 4100

Time(s)

Output value

ADC Original output (2.90V)

0 0.005 0.01 0.015 0.02 0.025 0.03

3950 4000 4050 4100

Time(s)

Output value

ADC after Calibration and Average output (2.90V)

(k) 2.90V 校準前 (l) 2.90V 校準後

3.1.3 實驗結果

圖 3-8 弦波編碼器解碼實驗硬體圖

實驗中以附有 10000 PPR(此指 pulse pre revolution)的傳統編碼器的 400 W 伺服馬達以套管帶動弦波編碼器，如上圖 3-8。用弦波編碼器位置回授後的速度估測來看解碼的 效果，取樣頻率為 7500 Hz。因為 DSP-Based 的解碼方法花費的時間比較多，所以無法 以一個 DSP 同時完成 DSP-Based 的解碼以及馬達控制兩個功能。實驗中分別以一個 DSP 為馬達控制器，另一個 DSP 為弦波編碼器解碼用，所以僅紀錄 DSP-Based 弦波解碼的速度輸出。

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

19bit encoder in 300rpm

Time(s)

19bit encoder in 30rpm

Time(s)

19bit encoder in 10rpm

Time(s)

19bit encoder in 5rpm

Time(s)

19bit encoder in 3rpm

Time(s)

19bit encoder in 1rpm

Time(s)

Velocity(rpm)

(e) 3rpm (f) 1rpm

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0

0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

19bit encoder in 0.5rpm

Time(s)

Velocity(rpm)

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7

19bit encoder in 0.3rpm

Time(s)

Velocity(rpm)

(g) 0.5rpm (h) 0.3rpm 圖 3-9 不同轉速下的 DSP-Based 解碼實驗

由上面八種不同轉速下的實驗結果顯示，在 1rpm 以上時 DSP-Based 的解碼都能夠 有良好的效果，並且平均值也與速度命令將近，但在 0.5 及 0.3rpm 時平均值似乎低於速 度命令。

由實驗得知 DSP-Based 弦波編碼器解碼效果確實可靠，能夠提供馬達控制器所需的位置資訊，但由於線性化法所需的計算較為複雜，所花費的計算時間較多，下一節將改用 FPGA 為實驗硬體，搭配上不同的解碼技巧以大大縮短運算時間。

3.2 FPGA-Based QEP-CORDIC 演算法實驗

本節實際將 QEP-CORDIC 演算法實現於 FPGA 上，利用此演算法計算弦波編碼器的精細位置，除了基本的硬體介紹之外，在 3.2.2 中進一步討論了錯位問題的發生，並且提出另一種新的粗略位置解碼方法來改善這個問題，最後實際測試 FPGA-Based QEP-CORDIC 演算法解碼後的速度回授。

3.2.1 硬體介紹

FPGA-Based 的實驗硬體與前節所用的弦波編碼器是相同的，介面電路也是相似，

僅提升的範圍有所不同，唯一的差異在於解碼本體的 FPGA 及外部 ADC 的使用兩部份

現場可程式化邏輯閘(Field programmable gate array, FPGA) [10]

實驗所使用的 FPGA 是 Xilinx 公司所發展的 Spartan-3AN 系列，是專為空間有限的環境下所設計，為業界第一片內建 Flash RAM 且具有多重開機模式的非失真性 FPGA。

在文檔中發展QEP-CORDIC之伺服馬達19-bit弦波解碼器及電路實現 (頁 31-0)

第二章 弦波編碼器解碼演算法

2.3 精細位置解碼

2.3.3 QEP-CORDIC 演算法

π

π

π

π

π

π

π

0 ~ 1

π

π

表 2-3 象限對應對照表

( x 0

0 ) ( x 0

0 )

( x 0

0 ) ( y 0

0 )

( x 0

0 ) (

x 0

0 )

( x 0

0 ) (

y 0

0 )

圖 2-18 象限對應圖

π

π

θ i

θ i

− 1

− i

圖 2-20 理想精細位置與粗略位置對照波形

第3章第三章 弦波編碼器解碼實現

3.1 DSP-Based 線性化法解碼實驗

圖 3-1 弦波編碼器實體圖

A + 2 . 5 + sin ( 2048 × θ )

A − 2 . 5 − sin ( 2048 ×

)

B + 2 . 5 + cos ( 2048 ×

)

B − 2 . 5 − cos ( 2048 ×

)

Z +

Z −

A

+ 2 . 5 + sin ( )

A

− 2 . 5 − sin ( )

B

+ 2 . 5 + cos ( )

B

− 2 . 5 − cos ( )

V cc

圖 3-2 TMS320F2812 功能方塊圖

圖 3-4 DSP-Based 弦波編碼器解碼硬體圖

mi

x

y

(3.1)

12

b ma x

y

(3.2)

ma 是實際的增益，b 則是實際偏移量(offset)，這兩個數值在每次進行校準運算的時

H

L

H

L

L H

L H

x x

y ma y

−

= − (3.3)

圖 3-5 ADC 校準示意圖 [9]

第二章弦波編碼器解碼演算法

( x ₀

₀ ) ( x ₀

₀ )

( x ₀

₀ ) ( y ₀

₀ )

( x ₀

₀ ) (

y ₀

₀ )

θ _i

θ _i

⁻ ¹

⁻ ⁱ

第3章第三章弦波編碼器解碼實現

A ⁺ 2 . 5 + sin ( 2048 × θ )

A ⁻ 2 . 5 − sin ( 2048 ×

B ⁺ 2 . 5 + cos ( 2048 ×

B ⁻ 2 . 5 − cos ( 2048 ×

Z ⁺

Z ⁻

⁺ 2 . 5 + sin ( )

⁻ 2 . 5 − sin ( )

⁺ 2 . 5 + cos ( )

⁻ 2 . 5 − cos ( )

¹²

_H

_L

b = _L − _L × (3.4)