重力测量稳定平台控制系统设计与实现

蒋鸿翔，张　巍，卢小东，夏　刚

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

重力测量稳定平台控制系统设计与实现

蒋鸿翔，张巍，卢小东，夏刚

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

介绍了重力测量惯性稳定平台的系统工作原理。设计了一种基于DSP加FPGA架构的嵌入式控制系统。详细描述了控制系统的功能划分以及硬件实现。提出了电流、速度和位置三环回路控制方案。车载测试结果显示稳定平台能实现地垂线跟踪，验证了所设计控制系统的有效性。

惯性稳定平台；嵌入式控制系统；硬件实现；控制回路设计；车载试验

0 引言

动基座重力测量是以飞机、船舶、车辆等运动物体为载体，结合重力传感器、惯性稳定平台、GPS（全球定位系统）、INS（惯性导航系统）等技术测定近地或空中重力异常的一种重力测量方法。大型惯性稳定平台是高精度动基座重力测量系统的重要组成部分，其承载的有效载荷主要是大型高精度重力传感器如超导或冷原子式重力仪或梯度仪。

惯性导航平台技术已经在国外零长弹簧式海空重力仪和旋转加速度计式重力梯度仪中得到了长期推崇和广泛应用。LaCoste海空重力仪采用水平阻尼稳定平台方案，利用陀螺与水平加速度计实现双环控制，对当地地垂线进行跟踪［1］。AIRGrav航空重力仪、贝尔宇航公司Air⁃FTG重力梯度仪都采用惯性导航平台方案，使传感器测量轴不受载体运动的干扰［2⁃3］。

随着光学陀螺、MEMS陀螺等新型惯性器件精度的提高，GNSS、GPS等卫星定位测量系统的广泛应用，嵌入式计算处理系统水平的提升，由INS／GNSS组合导航技术发展而来的POS（定位定向系统）技术近十年来也在一些大型高精度重力测量系统中得到了应用，如ARkex公司的超导重力梯度仪EGG中结合了陀螺稳定平台与POS技术，目前已成为一种很有发展潜力的高精度重力测量方案［4］。

本文设计了一种结合陀螺稳定平台和POS的重力测量惯性稳定平台，可以承载大型重力传感器并实现较高的地垂线跟踪精度。

1 稳定平台工作原理

由于飞机在飞行过程中易受气流或者发动机振动等因素影响，产生机体低频角运动与高频线振动，稳定平台可以隔离机体运动对载荷测量轴的干扰。此外，稳定平台可以在载机飞行过程中始终跟踪当地地平面，实现载荷测量轴对地垂直及方位保持。

本文所设计的稳定平台在工作时，利用POS敏感台体的角速度，由控制及驱动电路产生电机控制力矩，抑制或补偿机体低频角运动造成的载荷测量轴扰动，保证载荷测量轴在惯性空间的稳定；POS同时还完成地理坐标系下的姿态解算，通过稳定平台的姿态跟踪回路与角速度稳定回路的复合作用，控制载荷测量轴跟踪当地地垂线。

稳定平台由俯仰、横滚、基座、底座框架结构、减振装置、小型光纤POS、旋转变压器、力矩电机以及控制电路组成，如图1所示。重力测量有效载荷与俯仰框架固连，作为被稳定的台体。基座框架通过减振器与底座框架连接，底座框架再固定于载体的安装面上。

图1　稳定平台系统构成示意图Fig.1 Component diagram of the stabilization platform system

光纤POS固定于俯仰框架，通过INS／GPS组合导航算法解算重力传感器在当地地理坐标系中的姿态角。POS解算姿态角的过程分为两步：第一步完成捷联惯性导航计算，将陀螺仪输出的观测信息直接送至导航计算中进行实时的姿态矩阵解算，通过姿态矩阵把加速度计测量到的沿重力传感器体坐标系的线加速度信息，转换到导航坐标系中进行导航计算，并从姿态矩阵的相关元素中提取重力传感器的姿态角。第二步实现INS／ GPS组合导航最优估计，基于扩展卡尔曼滤波器对系统状态进行闭环状态反馈，得到修正后的姿态角，其组合导航工作原理如图2所示。POS同时还输出角速度信号，应用于稳定回路控制计算。

图2　POS实现惯性／GPS组合导航系统原理Fig.2 INS／GPS integrated navigation principle implementation of POS

POS采用小型化IMU、GPS板、计算机板和电源板的集成安装方式，由高性能嵌入式计算机完成导航解算。POS转台标定和车载试验结果表明，在GPS信号有效的情况下，水平姿态测量精度优于0．05°，航向测量精度优于0．1°。POS数据输出采用422串行通信方式，波特率为230．4kbit／s，数据刷新率为100Hz，满足稳定平台控制系统对控制带宽的要求。

2 控制系统设计与实现

2.1 控制电路

稳定平台控制系统采用以DSP芯片为控制运算核心、FPGA芯片为信号处理核心的数字化设计方式，控制运算和驱动信号产生完全在DSP芯片里执行，使得系统硬件构成较为简洁，并且便于在软件中调整控制回路参数和实现稳定平台复杂任务管理。

DSP主要负责完成速率稳定回路和位置跟踪回路的控制计算，输出电机控制PWM信号，以及根据上位机指令进行稳定平台工作状态切换。FPGA主要完成如下任务：

（1）传感器数据采集

光纤POS为数字量输出，RS⁃422接口。FPGA模拟RS⁃422接口协议，完成POS角速度与姿态角数据采集。

旋转变压器的数据采集由FPGA利用IP核中的DDS模块生成2KHz正弦波数字量，送给数模转换器，再经过放大器后生成旋转变压器激磁信号。FPGA生成RDC控制时序，输出控制信号，完成RDC数据采集。

（2）控制指令解码与数据输出

为了降低DSP的任务开销，外部指令的输入与解码都由FPGA来完成。稳定平台的上位机所发的控制指令中包含稳定平台的控制模式和具体的控制参数。FPGA接收到串行数据后，根据协议进行解码，并将相应的指令重新编码后由DSP主动按时读取。

稳定平台应定时输出其工作状态、框架姿态角、POS姿态角、POS角速率、POS加速度、GPS信息等大量信息。FPGA负责采集和接收稳定平台各种状态信息以及传感器数据，按照技术协议进行编码，最后通过串行接口发给上位机。

（3）时序控制

FPGA根据系统通信协议，生成串行通信的波特率时钟，并完成数据的收发控制。FPGA还需负责各模块间数据交换的同步控制，以保证数据交换的可靠性。

DSP与FPGA之间采用并行总线通信，并通过DSP的系统时钟进行同步，以保证数据交换的可靠性。DSP每1ms从FPGA中读取POS姿态、角速度、加速度数据、旋变数据、控制指令，完成速率稳定回路、姿态跟踪回路控制计算，并将稳定平台的工作状态和姿态角等数据写入FPGA中。控制回路硬件及信号连接原理图如图3所示。

图3　控制系统硬件及信号连接图Fig.3 Hardware and signal connection diagram of the control system

FPGA选择XILINX公司的XC3S500E芯片，该芯片拥有50万逻辑门，充分满足设计任务要求，并为后续的功能扩展预留了足够的空间。DSP采用TMS320F2812，主频可达150MHz，可以满足系统对控制回路计算的任务需求。

由于FPGA完成的功能最终是由硬件实现，并且各功能模块为并行执行，与传统单DSP方式相比，极大地节省了DSP的任务开销，使DSP能够专注于控制回路计算，提高了系统响应速度。

2.2 控制回路方案

本文采用电流、速度和位置三环控制回路。由于稳定平台俯仰和横滚控制通道相互独立，单个通道的三环控制回路原理框图如图4所示。

图4　稳定平台单通道三环控制原理框图Fig.4 One channel tri⁃loop⁃control principle diagram of the stabilization platform

大型重力测量惯性稳定平台所用电机较大，带来较大的力矩波动和反电动势干扰，对速度控制回路的性能有不利影响。在电流环未加之前，电机的力矩波动以及反电动势干扰由速度稳定回路抵消，大型惯性稳定平台的速度回路带宽较低，力矩波动和反电势干扰造成的稳定误差较大。加入电流环以后，通过电流反馈控制，可以更好地抑制力矩波动以及反电动势干扰，有利于速度稳定回路的工作。

速度控制回路的作用是抵抗作用在轴上的、由载体扰动角运动所带来的干扰力矩，保持载荷测量轴稳定，并跟踪位置控制回路输出的控制量。

由于速度控制回路会在陀螺漂移、白噪声、干扰力矩等干扰的共同作用下造成稳定平台内环轴垂直指向的动态变化，利用POS通过导航解算得到的平台姿态作为位置反馈，可以抑制这一运动，最终实现垂直轴对地平线变化的跟踪。

3 控制系统验证

本文进行车载试验以验证稳定平台动态性能。在稳定平台台体的表面安装1台高精度激光陀螺惯性测量单元（IMU），激光IMU姿态测量精度可达0．002°，以10Hz频率输出姿态数据，为稳定平台控制系统的指向波动提供基准姿态数据。设每一时刻激光IMU测得的俯仰角和横滚角分别为Pi和Ri，则此刻稳定平台指向误差E（3σ）为：

载有稳定平台的试验车共进行了2次测试。第1次测试是令试验车绕着一个300m×300m的场地进行低速行驶，直线行驶速度在20km／h～30km／h。第2次测试是令试验车沿着一条600m的直道进行高速往返行驶，最高行驶速度在50km／h～60km／h。试验中激光IMU测得的稳定平台姿态角反映了姿态跟踪误差，部分结果如图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

根据IMU测试数据得到的指向误差如表1所示。

表1　车载试验结果列表Table 1 Vehicle experiment results of the stabilization platform

测试结果显示，稳定平台实现了对地垂线的跟踪，验证了本文所设计稳定平台控制系统的有效性。但是在车辆快速转弯时，稳定平台台体横滚角和俯仰角出现了峰值，而当车辆进入直线行驶时，稳定平台台体横滚角和俯仰角不断趋近于零。

其原因在于：稳定平台俯仰和横滚通道对航向角速度的解耦不完全，当车辆快速转弯时，载体偏航角速度耦合到俯仰轴和横滚轴，引起俯仰和横滚控制通道的耦合角速度，由于框架姿态角误差、陀螺标度因数误差等原因造成控制解耦不完全，残余角速度使速度环先快速偏离水平位置，而POS姿态跟踪环节属于慢回路，因此POS姿态再进行缓慢回调，从而使稳定平台的指向精度变差。

4 结论

本文介绍了重力测量惯性稳定平台的工作原理。设计了一种基于FPGA和DSP的嵌入式控制系统，详细阐述了控制系统功能划分、硬件设计以及基于电流、速度和位置三环控制的稳定平台控制方案。进行了控制系统的车载试验，试验结果验证本文所设计控制系统的有效性。下一步的工作重点是实现横滚轴和俯仰轴控制对偏航角速度的完全解耦。

［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］朱如意，丁祝顺，夏刚．航空重力仪／梯度仪惯性稳定平台研制现状［J］．导航与控制，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］张达昌．几种新型的航空重力测量系统和航空重力梯度测量系统［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原理及应用技术［M］．北京：清华大学出版社，2009．HAN Feng⁃tian．Principle and application technology of TMS320F281xDSP［M］．Beijing：TsinghuaUniversity Press，2009．

［8］朱如意，丁祝顺，蒋鸿翔．遥感平台摩擦力矩的非线性校正补偿方法［J］．导航与控制，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

蒋鸿翔，男，高级工程师，研究方向为惯性稳定平台控制。