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

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

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

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

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 控制電路

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

DSP主要負責(zé)完成速率穩(wěn)定回路和位置跟蹤回路的控制計算，輸出電機控制PWM信號，以及根據(jù)上位機指令進行穩(wěn)定平臺工作狀態(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)變壓器激磁信號。FPGA生成RDC控制時序，輸出控制信號，完成RDC數(shù)據(jù)采集。

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

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

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

（3）時序控制

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

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

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

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

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

2.2 控制回路方案

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)論

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

［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)定平臺研制現(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］張達昌．幾種新型的航空重力測量系統(tǒng)和航空重力梯度測量系統(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］朱如意，丁祝順，蔣鴻翔．遙感平臺摩擦力矩的非線性校正補償方法［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

蔣鴻翔，男，高級工程師，研究方向為慣性穩(wěn)定平臺控制。