蔣鴻翔，張　巍，盧小東，夏　剛

（北京航天控制儀器研究所，北京100039）

重力測(cè)量穩(wěn)定平臺(tái)控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

蔣鴻翔，張巍，盧小東，夏剛

（北京航天控制儀器研究所，北京100039）

介紹了重力測(cè)量慣性穩(wěn)定平臺(tái)的系統(tǒng)工作原理。設(shè)計(jì)了一種基于DSP加FPGA架構(gòu)的嵌入式控制系統(tǒng)。詳細(xì)描述了控制系統(tǒng)的功能劃分以及硬件實(shí)現(xiàn)。提出了電流、速度和位置三環(huán)回路控制方案。車載測(cè)試結(jié)果顯示穩(wěn)定平臺(tái)能實(shí)現(xiàn)地垂線跟蹤，驗(yàn)證了所設(shè)計(jì)控制系統(tǒng)的有效性。

慣性穩(wěn)定平臺(tái)；嵌入式控制系統(tǒng)；硬件實(shí)現(xiàn)；控制回路設(shè)計(jì)；車載試驗(yàn)

0 引言

動(dòng)基座重力測(cè)量是以飛機(jī)、船舶、車輛等運(yùn)動(dòng)物體為載體，結(jié)合重力傳感器、慣性穩(wěn)定平臺(tái)、GPS（全球定位系統(tǒng)）、INS（慣性導(dǎo)航系統(tǒng)）等技術(shù)測(cè)定近地或空中重力異常的一種重力測(cè)量方法。大型慣性穩(wěn)定平臺(tái)是高精度動(dòng)基座重力測(cè)量系統(tǒng)的重要組成部分，其承載的有效載荷主要是大型高精度重力傳感器如超導(dǎo)或冷原子式重力儀或梯度儀。

慣性導(dǎo)航平臺(tái)技術(shù)已經(jīng)在國(guó)外零長(zhǎng)彈簧式?？罩亓x和旋轉(zhuǎn)加速度計(jì)式重力梯度儀中得到了長(zhǎng)期推崇和廣泛應(yīng)用。LaCoste?？罩亓x采用水平阻尼穩(wěn)定平臺(tái)方案，利用陀螺與水平加速度計(jì)實(shí)現(xiàn)雙環(huán)控制，對(duì)當(dāng)?shù)氐卮咕€進(jìn)行跟蹤［1］。AIRGrav航空重力儀、貝爾宇航公司Air?FTG重力梯度儀都采用慣性導(dǎo)航平臺(tái)方案，使傳感器測(cè)量軸不受載體運(yùn)動(dòng)的干擾［2?3］。

隨著光學(xué)陀螺、MEMS陀螺等新型慣性器件精度的提高，GNSS、GPS等衛(wèi)星定位測(cè)量系統(tǒng)的廣泛應(yīng)用，嵌入式計(jì)算處理系統(tǒng)水平的提升，由INS／GNSS組合導(dǎo)航技術(shù)發(fā)展而來的POS（定位定向系統(tǒng)）技術(shù)近十年來也在一些大型高精度重力測(cè)量系統(tǒng)中得到了應(yīng)用，如ARkex公司的超導(dǎo)重力梯度儀EGG中結(jié)合了陀螺穩(wěn)定平臺(tái)與POS技術(shù)，目前已成為一種很有發(fā)展?jié)摿Φ母呔戎亓y(cè)量方案［4］。

本文設(shè)計(jì)了一種結(jié)合陀螺穩(wěn)定平臺(tái)和POS的重力測(cè)量慣性穩(wěn)定平臺(tái)，可以承載大型重力傳感器并實(shí)現(xiàn)較高的地垂線跟蹤精度。

1 穩(wěn)定平臺(tái)工作原理

由于飛機(jī)在飛行過程中易受氣流或者發(fā)動(dòng)機(jī)振動(dòng)等因素影響，產(chǎn)生機(jī)體低頻角運(yùn)動(dòng)與高頻線振動(dòng)，穩(wěn)定平臺(tái)可以隔離機(jī)體運(yùn)動(dòng)對(duì)載荷測(cè)量軸的干擾。此外，穩(wěn)定平臺(tái)可以在載機(jī)飛行過程中始終跟蹤當(dāng)?shù)氐仄矫?，?shí)現(xiàn)載荷測(cè)量軸對(duì)地垂直及方位保持。

本文所設(shè)計(jì)的穩(wěn)定平臺(tái)在工作時(shí)，利用POS敏感臺(tái)體的角速度，由控制及驅(qū)動(dòng)電路產(chǎn)生電機(jī)控制力矩，抑制或補(bǔ)償機(jī)體低頻角運(yùn)動(dòng)造成的載荷測(cè)量軸擾動(dòng)，保證載荷測(cè)量軸在慣性空間的穩(wěn)定；POS同時(shí)還完成地理坐標(biāo)系下的姿態(tài)解算，通過穩(wěn)定平臺(tái)的姿態(tài)跟蹤回路與角速度穩(wěn)定回路的復(fù)合作用，控制載荷測(cè)量軸跟蹤當(dāng)?shù)氐卮咕€。

穩(wěn)定平臺(tái)由俯仰、橫滾、基座、底座框架結(jié)構(gòu)、減振裝置、小型光纖POS、旋轉(zhuǎn)變壓器、力矩電機(jī)以及控制電路組成，如圖1所示。重力測(cè)量有效載荷與俯仰框架固連，作為被穩(wěn)定的臺(tái)體。基座框架通過減振器與底座框架連接，底座框架再固定于載體的安裝面上。

圖1　穩(wěn)定平臺(tái)系統(tǒng)構(gòu)成示意圖Fig.1 Component diagram of the stabilization platform system

光纖POS固定于俯仰框架，通過INS／GPS組合導(dǎo)航算法解算重力傳感器在當(dāng)?shù)氐乩碜鴺?biāo)系中的姿態(tài)角。POS解算姿態(tài)角的過程分為兩步：第一步完成捷聯(lián)慣性導(dǎo)航計(jì)算，將陀螺儀輸出的觀測(cè)信息直接送至導(dǎo)航計(jì)算中進(jìn)行實(shí)時(shí)的姿態(tài)矩陣解算，通過姿態(tài)矩陣把加速度計(jì)測(cè)量到的沿重力傳感器體坐標(biāo)系的線加速度信息，轉(zhuǎn)換到導(dǎo)航坐標(biāo)系中進(jìn)行導(dǎo)航計(jì)算，并從姿態(tài)矩陣的相關(guān)元素中提取重力傳感器的姿態(tài)角。第二步實(shí)現(xiàn)INS／ GPS組合導(dǎo)航最優(yōu)估計(jì)，基于擴(kuò)展卡爾曼濾波器對(duì)系統(tǒng)狀態(tài)進(jìn)行閉環(huán)狀態(tài)反饋，得到修正后的姿態(tài)角，其組合導(dǎo)航工作原理如圖2所示。POS同時(shí)還輸出角速度信號(hào)，應(yīng)用于穩(wěn)定回路控制計(jì)算。

圖2　POS實(shí)現(xiàn)慣性／GPS組合導(dǎo)航系統(tǒng)原理Fig.2 INS／GPS integrated navigation principle implementation of POS

POS采用小型化IMU、GPS板、計(jì)算機(jī)板和電源板的集成安裝方式，由高性能嵌入式計(jì)算機(jī)完成導(dǎo)航解算。POS轉(zhuǎn)臺(tái)標(biāo)定和車載試驗(yàn)結(jié)果表明，在GPS信號(hào)有效的情況下，水平姿態(tài)測(cè)量精度優(yōu)于0．05°，航向測(cè)量精度優(yōu)于0．1°。POS數(shù)據(jù)輸出采用422串行通信方式，波特率為230．4kbit／s，數(shù)據(jù)刷新率為100Hz，滿足穩(wěn)定平臺(tái)控制系統(tǒng)對(duì)控制帶寬的要求。

2 控制系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

2.1 控制電路

穩(wěn)定平臺(tái)控制系統(tǒng)采用以DSP芯片為控制運(yùn)算核心、FPGA芯片為信號(hào)處理核心的數(shù)字化設(shè)計(jì)方式，控制運(yùn)算和驅(qū)動(dòng)信號(hào)產(chǎn)生完全在DSP芯片里執(zhí)行，使得系統(tǒng)硬件構(gòu)成較為簡(jiǎn)潔，并且便于在軟件中調(diào)整控制回路參數(shù)和實(shí)現(xiàn)穩(wěn)定平臺(tái)復(fù)雜任務(wù)管理。

DSP主要負(fù)責(zé)完成速率穩(wěn)定回路和位置跟蹤回路的控制計(jì)算，輸出電機(jī)控制PWM信號(hào)，以及根據(jù)上位機(jī)指令進(jìn)行穩(wěn)定平臺(tái)工作狀態(tài)切換。FPGA主要完成如下任務(wù)：

（1）傳感器數(shù)據(jù)采集

光纖POS為數(shù)字量輸出，RS?422接口。FPGA模擬RS?422接口協(xié)議，完成POS角速度與姿態(tài)角數(shù)據(jù)采集。

旋轉(zhuǎn)變壓器的數(shù)據(jù)采集由FPGA利用IP核中的DDS模塊生成2KHz正弦波數(shù)字量，送給數(shù)模轉(zhuǎn)換器，再經(jīng)過放大器后生成旋轉(zhuǎn)變壓器激磁信號(hào)。FPGA生成RDC控制時(shí)序，輸出控制信號(hào)，完成RDC數(shù)據(jù)采集。

（2）控制指令解碼與數(shù)據(jù)輸出

為了降低DSP的任務(wù)開銷，外部指令的輸入與解碼都由FPGA來完成。穩(wěn)定平臺(tái)的上位機(jī)所發(fā)的控制指令中包含穩(wěn)定平臺(tái)的控制模式和具體的控制參數(shù)。FPGA接收到串行數(shù)據(jù)后，根據(jù)協(xié)議進(jìn)行解碼，并將相應(yīng)的指令重新編碼后由DSP主動(dòng)按時(shí)讀取。

穩(wěn)定平臺(tái)應(yīng)定時(shí)輸出其工作狀態(tài)、框架姿態(tài)角、POS姿態(tài)角、POS角速率、POS加速度、GPS信息等大量信息。FPGA負(fù)責(zé)采集和接收穩(wěn)定平臺(tái)各種狀態(tài)信息以及傳感器數(shù)據(jù)，按照技術(shù)協(xié)議進(jìn)行編碼，最后通過串行接口發(fā)給上位機(jī)。

（3）時(shí)序控制

FPGA根據(jù)系統(tǒng)通信協(xié)議，生成串行通信的波特率時(shí)鐘，并完成數(shù)據(jù)的收發(fā)控制。FPGA還需負(fù)責(zé)各模塊間數(shù)據(jù)交換的同步控制，以保證數(shù)據(jù)交換的可靠性。

DSP與FPGA之間采用并行總線通信，并通過DSP的系統(tǒng)時(shí)鐘進(jìn)行同步，以保證數(shù)據(jù)交換的可靠性。DSP每1ms從FPGA中讀取POS姿態(tài)、角速度、加速度數(shù)據(jù)、旋變數(shù)據(jù)、控制指令，完成速率穩(wěn)定回路、姿態(tài)跟蹤回路控制計(jì)算，并將穩(wěn)定平臺(tái)的工作狀態(tài)和姿態(tài)角等數(shù)據(jù)寫入FPGA中。控制回路硬件及信號(hào)連接原理圖如圖3所示。

圖3　控制系統(tǒng)硬件及信號(hào)連接圖Fig.3 Hardware and signal connection diagram of the control system

FPGA選擇XILINX公司的XC3S500E芯片，該芯片擁有50萬邏輯門，充分滿足設(shè)計(jì)任務(wù)要求，并為后續(xù)的功能擴(kuò)展預(yù)留了足夠的空間。DSP采用TMS320F2812，主頻可達(dá)150MHz，可以滿足系統(tǒng)對(duì)控制回路計(jì)算的任務(wù)需求。

由于FPGA完成的功能最終是由硬件實(shí)現(xiàn)，并且各功能模塊為并行執(zhí)行，與傳統(tǒng)單DSP方式相比，極大地節(jié)省了DSP的任務(wù)開銷，使DSP能夠?qū)Ｗ⒂诳刂苹芈酚?jì)算，提高了系統(tǒng)響應(yīng)速度。

2.2 控制回路方案

本文采用電流、速度和位置三環(huán)控制回路。由于穩(wěn)定平臺(tái)俯仰和橫滾控制通道相互獨(dú)立，單個(gè)通道的三環(huán)控制回路原理框圖如圖4所示。

圖4　穩(wěn)定平臺(tái)單通道三環(huán)控制原理框圖Fig.4 One channel tri?loop?control principle diagram of the stabilization platform

大型重力測(cè)量慣性穩(wěn)定平臺(tái)所用電機(jī)較大，帶來較大的力矩波動(dòng)和反電動(dòng)勢(shì)干擾，對(duì)速度控制回路的性能有不利影響。在電流環(huán)未加之前，電機(jī)的力矩波動(dòng)以及反電動(dòng)勢(shì)干擾由速度穩(wěn)定回路抵消，大型慣性穩(wěn)定平臺(tái)的速度回路帶寬較低，力矩波動(dòng)和反電勢(shì)干擾造成的穩(wěn)定誤差較大。加入電流環(huán)以后，通過電流反饋控制，可以更好地抑制力矩波動(dòng)以及反電動(dòng)勢(shì)干擾，有利于速度穩(wěn)定回路的工作。

速度控制回路的作用是抵抗作用在軸上的、由載體擾動(dòng)角運(yùn)動(dòng)所帶來的干擾力矩，保持載荷測(cè)量軸穩(wěn)定，并跟蹤位置控制回路輸出的控制量。

由于速度控制回路會(huì)在陀螺漂移、白噪聲、干擾力矩等干擾的共同作用下造成穩(wěn)定平臺(tái)內(nèi)環(huán)軸垂直指向的動(dòng)態(tài)變化，利用POS通過導(dǎo)航解算得到的平臺(tái)姿態(tài)作為位置反饋，可以抑制這一運(yùn)動(dòng)，最終實(shí)現(xiàn)垂直軸對(duì)地平線變化的跟蹤。

3 控制系統(tǒng)驗(yàn)證

本文進(jìn)行車載試驗(yàn)以驗(yàn)證穩(wěn)定平臺(tái)動(dòng)態(tài)性能。在穩(wěn)定平臺(tái)臺(tái)體的表面安裝1臺(tái)高精度激光陀螺慣性測(cè)量單元（IMU），激光IMU姿態(tài)測(cè)量精度可達(dá)0．002°，以10Hz頻率輸出姿態(tài)數(shù)據(jù)，為穩(wěn)定平臺(tái)控制系統(tǒng)的指向波動(dòng)提供基準(zhǔn)姿態(tài)數(shù)據(jù)。設(shè)每一時(shí)刻激光IMU測(cè)得的俯仰角和橫滾角分別為Pi和Ri，則此刻穩(wěn)定平臺(tái)指向誤差E（3σ）為：

載有穩(wěn)定平臺(tái)的試驗(yàn)車共進(jìn)行了2次測(cè)試。第1次測(cè)試是令試驗(yàn)車?yán)@著一個(gè)300m×300m的場(chǎng)地進(jìn)行低速行駛，直線行駛速度在20km／h～30km／h。第2次測(cè)試是令試驗(yàn)車沿著一條600m的直道進(jìn)行高速往返行駛，最高行駛速度在50km／h～60km／h。試驗(yàn)中激光IMU測(cè)得的穩(wěn)定平臺(tái)姿態(tài)角反映了姿態(tài)跟蹤誤差，部分結(jié)果如圖5、圖6所示。

圖5　第1次測(cè)試中IMU橫滾角、俯仰角和偏航角速度Fig.5 Roll rate，pitch rate and yaw rate of IMU in the 1sttest

圖6　第2次測(cè)試中IMU橫滾角、俯仰角和偏航角速度Fig.6 Roll rate，pitch rate and yaw rate of IMU in the 2ndtest

根據(jù)IMU測(cè)試數(shù)據(jù)得到的指向誤差如表1所示。

表1　車載試驗(yàn)結(jié)果列表Table 1 Vehicle experiment results of the stabilization platform

測(cè)試結(jié)果顯示，穩(wěn)定平臺(tái)實(shí)現(xiàn)了對(duì)地垂線的跟蹤，驗(yàn)證了本文所設(shè)計(jì)穩(wěn)定平臺(tái)控制系統(tǒng)的有效性。但是在車輛快速轉(zhuǎn)彎時(shí)，穩(wěn)定平臺(tái)臺(tái)體橫滾角和俯仰角出現(xiàn)了峰值，而當(dāng)車輛進(jìn)入直線行駛時(shí)，穩(wěn)定平臺(tái)臺(tái)體橫滾角和俯仰角不斷趨近于零。

其原因在于：穩(wěn)定平臺(tái)俯仰和橫滾通道對(duì)航向角速度的解耦不完全，當(dāng)車輛快速轉(zhuǎn)彎時(shí)，載體偏航角速度耦合到俯仰軸和橫滾軸，引起俯仰和橫滾控制通道的耦合角速度，由于框架姿態(tài)角誤差、陀螺標(biāo)度因數(shù)誤差等原因造成控制解耦不完全，殘余角速度使速度環(huán)先快速偏離水平位置，而POS姿態(tài)跟蹤環(huán)節(jié)屬于慢回路，因此POS姿態(tài)再進(jìn)行緩慢回調(diào)，從而使穩(wěn)定平臺(tái)的指向精度變差。

4 結(jié)論

本文介紹了重力測(cè)量慣性穩(wěn)定平臺(tái)的工作原理。設(shè)計(jì)了一種基于FPGA和DSP的嵌入式控制系統(tǒng)，詳細(xì)闡述了控制系統(tǒng)功能劃分、硬件設(shè)計(jì)以及基于電流、速度和位置三環(huán)控制的穩(wěn)定平臺(tái)控制方案。進(jìn)行了控制系統(tǒng)的車載試驗(yàn)，試驗(yàn)結(jié)果驗(yàn)證本文所設(shè)計(jì)控制系統(tǒng)的有效性。下一步的工作重點(diǎn)是實(shí)現(xiàn)橫滾軸和俯仰軸控制對(duì)偏航角速度的完全解耦。

［1］Air?sea systemIIgravitymeter［EB／OL］．http：／／www．microglacoste．com．

［2］Murthy B S R．AirbornegeophysicsandtheIndian scenario［J］．J．Ind．Geophys．Union，2007，11（1）：1?28．

［3］Rogers M M．An investigation into the feasibility of using a modern gravity gradient instrument for passive aircraft nav?igation and terrain avoidance［D］．Thesis of Degree of Master of Science in Aeronautical Engineering，Air Uni?versity，USAF，2009：24?180．

［4］Lumley J M，White J P，Barnes G，et al．A Superconduct?ing gravity gradiometer tool for exploration［J］．Airborne Gravity，2004：21?40．

［5］朱如意，丁祝順，夏剛．航空重力儀／梯度儀慣性穩(wěn)定平臺(tái)研制現(xiàn)狀［J］．導(dǎo)航與控制，2011，10（2）：69?73．ZHU Ru?yi，DING Zhu?shun，XIA Gang．Present state of airbornegravityandgradiometerinertialstabilized platforms［J］．Navigation and Control，2011，10（2）：69?73．

［6］張達(dá)昌．幾種新型的航空重力測(cè)量系統(tǒng)和航空重力梯度測(cè)量系統(tǒng)［J］．物探與化探，2005，29（6）：471?476．ZHANGDa?chang．Severalnewtypesofairborne gravimetric systems and airborne gravity gradiometric sys?tems［J］．Geophysical and Geochemical Exploration，2005，29（6）：471?476．

［7］韓豐田．TMS320F281xDSP原理及應(yīng)用技術(shù)［M］．北京：清華大學(xué)出版社，2009．HAN Feng?tian．Principle and application technology of TMS320F281xDSP［M］．Beijing：TsinghuaUniversity Press，2009．

［8］朱如意，丁祝順，蔣鴻翔．遙感平臺(tái)摩擦力矩的非線性校正補(bǔ)償方法［J］．導(dǎo)航與控制，2011，10（3）：21?25．ZHU Ru?yi，DING Zhu?shun，JIANG Hong?xiang．Method of friction compensation based on nonlinear revision in re?mote sensing platforms［J］．Navigation and Control，2011，10（3）：21?25．

［9］LH System Gmbh．Leica PAV80 Gyro?stabilized Sensor Mount User Manual［R］．2008．

［10］LH System Gmbh．Leica PAV30 Gyro?stabilized Camera Mount Technical Reference Manual［R］．2003．

Design and ImPlementation of Stabilization Platform Control System for Gravity Measurement

JIANG Hong?xiang，ZHANG Wei，LU Xiao?dong，XIA Gang
（Beijing Institute of Aerospace Control Devices，Beijing 100039）

The control systems principles of inertial stabilization platforms for gravity measurement are described．The embedded control system design utilizing the system architectures based on a digital signal processor（DSP）and a field pro?grammed gates array（FPGA）was described．Functional partition and hardware implementation of the control system are de?scribed particularly．A triple control loop scheme comprised of a current loop，a rate loop and a position loop are described．The actual vehicle based experiment result demonstrates that the inertial stabilization platform can track the local geograph?ical perpendicular line，and validates the design and implementation of the control system．

inertial stabilized platform；embedded control system；hardware implementation；control loop design；ve?hicle based experiment

U666．1

A

1674?5558（2016）01?01203

10．3969／j．issn．1674?5558．2016．06．009

2015?10?08

蔣鴻翔，男，高級(jí)工程師，研究方向?yàn)閼T性穩(wěn)定平臺(tái)控制。