基于CORDIC算法的半球諧振陀螺信號相位跟蹤和解調(diào)

夏 語，李建勛，張明舉，趙萬良，王 偉

（1.上海交通大學 機械與動力工程學院，上海 200240；2.上海航天控制技術(shù)研究所，上海 200233）

0 引言

半球諧振陀螺是新一代慣性敏感元件，其機械結(jié)構(gòu)中無經(jīng)典陀螺中的高速轉(zhuǎn)子和活動支承，利用半球諧振子產(chǎn)生的哥氏效應引起振型移動實現(xiàn)角度或角速率的測量，壽命可達15年以上，具有測量精度高、穩(wěn)定性強、抗輻射、長壽命等優(yōu)點，特別適于空間領(lǐng)域應用。從1996年NASA將第一顆裝有半球諧振陀螺空間慣性參考單元的衛(wèi)星送上太空至今，美國在軌工作的半球諧振陀螺慣性基準已有91套，累計工作時間超過1 200萬小時，在軌工作無一失效［1］。近年來，我國自主研發(fā)的半球諧振陀螺慣性基準也取得了較大的進展［2］。2012年10月發(fā)射的實踐9號衛(wèi)星首次搭載了中國電科26所研制的半球諧振陀螺慣性組合，成功切入衛(wèi)星姿態(tài)控制系統(tǒng)，參與衛(wèi)星姿態(tài)控制，在軌運行穩(wěn)定，但其精度較國外產(chǎn)品差距較大［3］。國內(nèi)的半球諧振陀螺控制回路主要以模擬電路為主，回路中相位跟蹤器用鎖相環(huán)的方式實現(xiàn)，鎖相環(huán)電路由壓控振蕩器（VCO）和鎖相回路（PLL）組成，VCO輸出的信號分成兩路，一路通過分頻和鎖相回路產(chǎn)生本振信號，另一路信號與本振信號相位比較，若相位差為定值，則說明相位已被跟蹤，若相位差在變化，則鎖相回路輸出將變化，控制VCO，使相位差穩(wěn)定，說明鎖相環(huán)的輸入與輸出頻率與相位保持特定關(guān)系，完成鎖相的閉環(huán)。但模擬電路用于陀螺控制回路中缺點有：模擬器件溫度偏移較嚴重，溫度變化時參數(shù)會隨之改變，影響陀螺性能穩(wěn)定；模擬電路各模塊易引入噪聲，前后器件會互相影響，調(diào)試過程復雜；性能優(yōu)良的模擬電路濾波器設(shè)計復雜，階數(shù)一般較高，占用印制板面積大。數(shù)字電路精度高、可靠性強、設(shè)計簡便、受溫度影響小，非常適于陀螺控制回路。數(shù)字控制回路中，用現(xiàn)場可編程邏輯陣列（FPGA）或數(shù)字信號處理器（DSP）為基礎(chǔ)，設(shè)計相位跟蹤器中采用直接數(shù)字頻率合成（DDS）技術(shù)。以查找表為核心的相位跟蹤器雖然原理簡單，但實現(xiàn)較困難，以CORDIC算法為核心的相位跟蹤器易實現(xiàn)，且能同時完成陀螺信號解調(diào)任務。為此，本文研究了兩種基于FPGA的HRG的相位和解調(diào)。

1 HRG螺數(shù)字控制回路原理介紹

HRG數(shù)字控制回路從陀螺的讀出電極獲取x、y兩路輸入信號，經(jīng)A／D采樣，進入FPGA完成相位跟蹤和三路信號處理，信號處理過程分為解調(diào)、低通濾波、PID控制和調(diào)制，通過執(zhí)行機構(gòu)作用于陀螺。其原理如圖1所示。

相位跟蹤器是回路中重要組成部分。它通過兩次過零信號的間隔估計載波頻率，并計算出對應的相位累加步進，再進行步進累加，最后輸出與輸入信號相位相同的本地載波。本地載波的精度直接關(guān)系解調(diào)陀螺信號和調(diào)制信號的精度，當本地載波精度不足時，會造成整個系統(tǒng)性能下降。因此，相位跟蹤器設(shè)計尤為重要，一種以查找表法為核心相位跟蹤器如圖2所示。

圖1 HRG數(shù)字控制回路Fig.1 Digital control loop of HRG

圖2 以查找表法為核心的相位跟蹤器Fig.2 Phase tracker based on lookup table

過零比較信號由計數(shù)器得到計數(shù)N，利用360／N模塊得到360°與N的商，即相位累加步進Cp。Cp自身累加后得到所需的正余弦地址，然后在SRAM中的查找表中讀取對應的正余弦值，即生成本地載波Vrc，Vrs并送回FPGA。通過計算得出查找表需約16MB儲存空間，該查找表無法置于FPGA中，只能存放于SRAM中。此外還需增加FLASH芯片，用于斷電儲存查找表。

該方案的特點是：軟件需增加FPGA與SRAM、FLASH的通信模塊程序；讀寫SRAM需消耗一定的FPGA資源；硬件需增加FLASH和SRAM等芯片，這增加了印制板面積，增加了未來HRG慣性組合小型化的難度。因此，有必要更換低硬件復雜度、低資源消耗的方案。

本文利用CORDIC算法特點，設(shè)計了一種算法簡單、無需增加硬件的相位跟蹤器，且跟蹤器可在生成本地載波同時完成陀螺信號的解調(diào)任務，極大節(jié)約FPGA資源。

2 基于CORDIC算法的相位跟蹤器方案設(shè)計

CORDIC算法即坐標旋轉(zhuǎn)數(shù)字計算方法，該算法通過多次迭代加法和移位運算產(chǎn)生高精度的正余弦波形。

已知向量（xi，yi），以及旋轉(zhuǎn)量θi，可計算出旋轉(zhuǎn)后的向量值（xi＋1，yi＋1），有

式中：0≤i≤n。此處：n為迭代次數(shù)。n越大，xn，yn就越逼近真實目標值。

用于CORDIC算法中每個迭代i的旋轉(zhuǎn)角度見表1。

表1 迭代旋轉(zhuǎn)角度Tab.1 Iterative rotation angle

提取cosθi，將tanθi＝2－i代入式（1），可得

式（2）中：2－i在二進制中實際操作為小數(shù)點向右移動i位，以此可證明CORDIC算法僅需簡單的移位和相加運算就能進行三角運算；zi用于累加每次迭代過程中追蹤累加的旋轉(zhuǎn)角度，迭代i次后zi將逼近于0，而xi，yi將逼近真實的目標值；di為±1，由zi的符號位決定，當zi＜0時，di＝1，否則為－1。

CORDIC算法基本邏輯如圖3所示。

由以上CORDIC算法原理可實現(xiàn)相位跟蹤器功能。對CORDIC邏輯模塊，式（1）累加n次后得

式中：K為在迭代n次后xn，yn的被伸縮倍數(shù)（即每個迭代方程中cosθi的積），且

圖3 CORDIC算法基本邏輯Fig.3 CORDIC algorithm basic logic

故當x0＝K，y0＝0，z0為控制回路中的相位累加值時，由式（3）可得本地載波

以CORDIC算法為核心的相位跟蹤器原理如圖4所示。以CORDIC算法為核心的相位跟蹤器能在FPGA中完成所有的本地載波生成任務，無需與SRAM通信讀寫外置查找表，且算法中只有加法和移位，消耗資源少并能減少系統(tǒng)延時。

圖4 以CORDIC算法為核心的相位跟蹤器Fig.4 Phase tracker based on CORDIC algorithm

3 用CORDIC邏輯模塊完成信號解調(diào)

傳統(tǒng)半球諧振陀螺信號解調(diào)方案是：相位跟蹤器生成的本地載波Vrc，Vrs與x，y相乘進行解調(diào)，需要3個乘法器解調(diào)出3路控制信號，如圖5所示。

圖5 信號解調(diào)Fig.5 Signal demodulation

以CORDIC算法為核心的相位跟蹤器除能便利地得到本地載波外，還可利用其原理完成控制回路中的解調(diào)過程（如圖6所示），能節(jié)約3個乘法運算。

以x線路為例，令式（5）的初始值：x0為輸入陀螺信號x，y0＝0，z0為相位累加值，則

由此可見，可通過CORDIC自身算法直接輸出x、y信號與本地載波的乘積，完成3路信號的解調(diào)，節(jié)省3個乘法器，且整個系統(tǒng)的輸出也無變化。用CORDIC解調(diào)后的數(shù)字控制回路如圖7所示。

圖6 用CORDIC內(nèi)部邏輯完成信號解調(diào)過程Fig.6 Signal demodulate process with CORDIC logic

4 方案仿真驗證

分別對以查找表法為核心的相位跟蹤器及解調(diào)過程（A方案，如圖8所示）和以CORDIC算法為核心的相位跟蹤器及解調(diào)過程（B方案，如圖9所示）進行建模（兩個方案的輸出均通過低通濾波器，否則信號會被高頻淹沒）。

圖7 CORDIC解調(diào)的數(shù)字控制回路Fig.7 Digital control loop with CORDIC demodulation

圖8 查找表法為核心的相位跟蹤器及解調(diào)過程（方案A）Fig.8 Phase tracker and demodulation based on lookup table（scheme A）

圖9 CORDIC邏輯為核心的相位跟蹤器及解調(diào)過程（方案B）Fig.9 Phase tracker and demodulation based on CORDIC algorithm （scheme B）

仿真條件如下：仿真輸入（模擬過零比較信號）4.5kHz方波信號，高電平＋1V，低電平0V，占空比50%；仿真輸出幅值控制解調(diào)信號xVrc，力反饋控制解調(diào)信號yVrc，正交控制解調(diào)信號yVrs。

4.1 仿真結(jié)果對比

方案A、B分別在Simulink中進行仿真，結(jié)果如圖10～12所示。由圖可知：初始化期間（前0.4×10－3s內(nèi)）波形不同，0.4×10－3s后波形相同（嚴格說兩波形是倍數(shù)關(guān)系，該倍數(shù)與上文推算的伸縮因子K≈0.607 3相同），而K可在之后的PID環(huán)節(jié)吸收，故方案B可從功能和性能上替代方案A。

圖10 兩種方案幅值解調(diào)信號（xVrc）輸出Fig.10 Amplitude demodulation signal output of two schemes

4.2 FPGA資源評估

宇航級的FPGA的邏輯單元僅為工業(yè)級的FPGA約1／10倍，節(jié)約FPGA資源對航天產(chǎn)品是必須考慮的。評估FPGA資源使用率時，選用的是ANTEL的型號為A3P250FPGA，該FPGA有邏輯單元25萬門，用Modelsim對方案A、B對應的Verilog程序進行評估。方案A資源利用率可達128.91%，已超出硬件的極限，方案B資源利用率為80.47%，方案B較方案A少3個乘法器，節(jié)約FPGA資源近50%。

圖11 兩種方案力反饋解調(diào)信號（yVrc）輸出Fig.11 Force feedback demodulation signal output of two schemes

圖12 兩種方案正交解調(diào)信號（yVrs）輸出Fig.12 Quadrature demodulation signal output of two schemes

以上算法驗證和仿真結(jié)果表明，方案A、B的輸出結(jié)果完全一致，但方案B的軟件減少了通信模塊，硬件減少了SRAM，F(xiàn)LASH，并可節(jié)約大量的FPGA資源。由此可認為以CORDIC算法為核心的相位跟蹤器更優(yōu)。

5 結(jié)束語

本文設(shè)計了半球諧振陀螺數(shù)字控制回路中兩種相位跟蹤器。與以查找表法為核心的相位跟蹤器相比，以CORDIC算法為核心的相位跟蹤器只用加法和移位實現(xiàn)了相位跟蹤和陀螺信號解調(diào)的功能，其軟硬件實現(xiàn)簡單、計算快速、節(jié)約資源，算法推算和仿真試驗結(jié)果表明該法的可靠性高，能保證整個回路的數(shù)據(jù)精度要求。本文研究為陀螺數(shù)字信號系統(tǒng)的設(shè)計提供了一定的參考依據(jù)。

［1］ ROZELLE D M.The hemispherical resonator gyro：from wineglass to the planets［R］.Northrop Grumman Corporation，2004.

［2］ 高勝利.半球諧振陀螺的分析與設(shè)計［D］.哈爾濱：哈爾濱工程大學，2008.

［3］ 中國電子科技集團公司.中國電科研制的半球諧振陀螺完成衛(wèi)星閉環(huán)控制試驗［EB／R］.［2012－12－21］.http：／／www.sasac.gov.cn／n1180／n1226／n2410／n314319／15024297.html.