基于位置和速度观测的空间稳定系统快速对准

何虔恩，吴秋平

基于位置和速度观测的空间稳定系统快速对准

何虔恩，吴秋平

（清华大学 精密仪器系，北京 100084）

空间稳定系统是高精度长航时导航技术的关键，快速对准是其工程应用的重要功能之一。研究了基于位置和速度观测的系统快速对准方法。基于Wahba定姿原理设计平台姿态角的粗估计算法，研究了系统水平通道误差模型的短时可观测性，并据此设计一个可实时估计平台失准角初值、水平位置和速度误差的7维精对准Kalman滤波器。计算机仿真和动态试验结果表明，所述快速对准方法可估计较大的平台失准角（3°量级），同时适用于系泊和海上应急启动情况；在动态条件下精对准2 h，精度即满足指标要求，具有较强的工程应用价值。

空间稳定系统；快速对准；位置和速度观测；Kalman滤波

空间稳定平台式惯性导航系统（简称空间稳定系统）是一种航位推算系统，其精度依赖于初始条件设定精度。例如，为达到0.1 n mile/h的导航精度，初始方位和水平失准角应分别小于30"和6"，并且初始速度误差应小于0.1 kn。因此，为取得高精度导航性能，系统必须具有高精度的初始条件设定[1]。其中，初始速度和位置通过全球卫星定位系统等外部设备容易获得，所以系统初始化的主要任务在于“对准”，即减小方位和水平失准角。

一般情况下，系统在运载体停泊状态下进行正常启动，有足够长（至少1天）的时间进行对准并标定惯性仪表参数。但在实际应用中不可避免地会遇到执行突发紧急任务的情况，此时要求系统能够快速启动并具备规定的导航精度。另外，空间稳定系统是一复杂的光机电系统，零部件在长期工作过程和环境中各种冲击、振动的作用下不可避免地会出现一些故障，在故障修复后，仍然面临快速启动的问题。

惯性导航系统（简称惯导）有自对准法、传递对准法和光学对准法等对准方法[1]。其中，自对准法便捷、可操作性强，且同时适用于静基座和动基座，在实践中取得广泛应用。对于地球上的导航，惯导系统的自对准本质上是一个构建三维空间正交系作为要对准到的参考坐标系的过程，这里主要利用的是相互不平行的本地重力矢量和地球自转角速度矢量。目前，自对准法主要有两类实现方案：I）基于比力矢量一次积分观测和Wahba定姿原理的方案[2-3]。该方案概念形象直观，且适用于估计任意大小的初始失准角，但对各种随机干扰（如：发动机振动、风浪扰动等）比较敏感，因而对准精度较低。文献[3]给出其准静态条件下方位对准的仿真精度为4.8'（1σ）/120 s；文献[2]给出其摇摆条件下方位对准和水平对准的试验精度分别为3'/1000 s和14"/1000 s。这一方案主要用于低精度捷联惯导的自对准或高精度惯导的粗对准场合。II）基于导航误差（位置误差、速度误差和姿态误差）观测的方案[4-5]。根据对准误差传递到导航误差的特性，设计线性/非线性滤波器估计对准误差并予以校正来达到对准的目的。该方案抵抗各种随机干扰的能力比第一类方案强，对准精度高——在动态条件下方位和水平对准的仿真精度可分别达到4.2'/300 s和20"/300 s（基于姿态误差观测）[5]，因而常用于高精度惯导的精对准场合。不足的是，该方案要求初始失准角为小角度，一般需要借助于传递对准（有外参考基准时）或粗对准来满足这一条件。

对于上述第二类自对准方案，选择不同的观测量，对准的性能（精度和快速性）是不同的：按快速性排列，由快到慢依次是基于姿态误差、速度误差和位置误差观测，然而相应的对准精度却是依次提高的。对于捷联式惯导系统而言，快速性要求较高（对准时间通常不超过20 min），一般采用基于姿态误差/速度误差的自对准方案，这是公开发表的文献所集中讨论的。但对于空间稳定系统，即使在快速启动的情况下，对精度的要求也是较高的（对准精度应达到角秒量级），此时宜采用基于位置误差/速度误差的自对准方案，这正是本文研究的出发点。借鉴捷联式惯导系统自对准方法的研究经验，本文探讨适用于空间稳定系统的快速对准方案。

1 快速对准总体方案

快速对准分两步：I）粗对准，将平台坐标系（P系）对准到指定的参考坐标系（地球坐标系，e系），并粗略估计P系相对e系的姿态角；II）精对准，精确化第一步中的P系姿态角粗略估计值。

1.1 粗对准

① 在码头系泊条件下，利用方位参考基准和三轴加速度计输出的比力信息将P系对准到e系。

② 在海上动态条件下，利用另一套同类惯导输出的框架角或外部姿态参考基准将P系对准到e系。

③ 采用基于Wahba定姿原理的自对准方案粗略估计P系的姿态角，这一步主要考虑到初始失准角可能较大的情况（如海上动态环境下）。若能保证初始失准角较小（如在系泊和静止基座条件下），利用单点比力数据估计P系姿态角即可[1]。

实现以上第①和②两项内容已有比较成熟的技术和成功的经验，不再赘述。第③项内容的实现算法将在第2节讨论。

1.2 精对准

如图1所示，引入水平位置和速度的外参考信息，与惯导解算得到的相应信息作差，作为精对准滤波器的观测量。精对准滤波器运行一段时间达到平稳后，作出平台失准角的估计，并以之修正惯导解算回路中的P系姿态角。

图1 精对准方案框图Fig.1 Diagram of the fine alignment

2 P系姿态角粗估计

2.1 P系姿态角及导航解算动力学方程

P系姿态角定义为由e系依次绕-x、y和-z轴旋转到P系的三个Euler角，分别记为S1、σ1和γ2，则P系到e系的坐标转换阵可表示为

式中，cij表示的第i行第j列元素，i,j=1,2,3。

导航解算动力学方程可表示为[1]

式中：re——在e系中表示的系统位置矢量；

ve——在e系中表示的系统地速矢量；

fP——在P系中表示的比力矢量；

ge——在e系中表示的本地重力加速度矢量；

ωie——地球自转角速率；

ωx, ωy, ωz——P系运动角速度在P系中的投影。

2.2 粗估计算法

借鉴文献[2]中的捷联惯导对准方法，记初始时刻分别与e系和P系重合的惯性坐标系为e0系和P0系，则可分解为

xyzPxφy和φz的微分方程与方程(3)相似：

且初值φx(0)=φy(0)=φz(0)=0。给定P系运动角速度，由方程(5)可数值求解φx、φy和φz，进而可得0PPC。

将式(4)代入(3)的第二个微分方程，整理后可得

式中：

等式(6)两边同时右乘αT(t)，并对时间进行一次积分，则等式(6)给出P系初始姿态矩阵的最小二乘解：

文献[2]已经证明，只要在[0, t]区间上的所有α（τ）能够支起三维空间，则α（τ）αT（τ）dτ可逆，因而式(8)成立。对于地球上的低速载体导航，上述条件是容易满足的，因为在惯性系中观看，本地重力矢量形成的轨迹是一圆锥面，而重力又是比力α（τ）的主导分量，所以，α（τ）的轨迹也近乎一圆锥面，因而能支起三维空间。

将式(8)代入式(4)，再根据式(2)，即可估计出P系姿态角。由式(7)可知，α(t)和β(t)计算易受各种随机运动和加速度计固有噪声的干扰，加之积分时间有限，矩阵α（）αT（）dτ条件数较大。因此，通过上ττ述方法估计P系姿态角的精度往往较低，对于高精度惯导，还需要精对准对这一估计结果进行精化。

3 精对准

3.1 系统误差模型

引入高度阻尼后，惯导垂直通道的误差可忽略不计，此时，对于低速巡航载体，水平通道的误差方程可写为[1]

式中：δrN, δrE——位置误差的北向和东向分量；

δvN, δvE——速度误差的北向和东向分量；

ωD=-ωiesinL ，L为本地地理纬度；

s

ω——Schuler振荡角频率，1.24×10-3rad/s；

g——本地重力加速度值；

▽N, ▽E——比力误差的北向和东向分量；

ψN, ψE——平台失准角的北向和东向分量，满足如下关系式：

式中，λ——相对地理经度，初始时刻为0。

对式(3)中的平台运动微分方程取扰动，略去平台漂移误差及二阶误差量（经验表明，平台常值漂移重复性误差约1×10-4(°)/h，对快速（≤2 h）对准的影响小于1"），经推导，可得平台失准角的解析表达式为[1]3.2 平台失准角的可观测性

选择经纬度误差和水平速度误差为观测变量，并记观测向量为z=[δLδλcosLδvNδvE]T，则

式中，ra——地球平均半径。对于低速运载体，在精对准期间经纬度变化为小量。简单起见，考虑S1和σ1为小量的情况，此时式(10)可简化为

将式(11)代入式(13)，再与式(12)一同代入方程(9)，整理后可得

易见，常值项、sinωiet和cosωiet是相互正交的，当t足够大（大于1天）时，可分离ΔS1（0）、Δσ1（0）、∇N/g和Δγ2（0）+∇E/(gcosL)。然而，为满足快速性要求，t通常较小，此时式(14)可简化为

显然，等号左边均为关于t的一次多项式，各项可分解为两部分：I）误差源（平台失准角初值和比力误差北/东向分量或其线性组合）；II）放大系数。按照放大系数从大到小顺序排列各项，如表1所示。误差源的放大系数反映单位该误差量对观测值的贡献，在相同噪声环境下，放大系数越大，相应误差源越容易在观测值中反映出来（或者说它的可观测性越强），因而在相同时间内的估计精度越高。

表1 误差源及其放大系数Tab.1 Error sources and their amplifying coefficients

由表1可知，在短时间内，Δσ1（0）+∇N/g 可观测性最强，而欲达到同等可观测性（以放大系数大小表征），对于ΔS1（0）和Δγ2（0）+∇E/(gcosL)（后者的估计依赖于前者），额外需要3.8 h；对于Δσ1（0），额外需要更长时间。

在低（高）纬度地区，Δσ1（0）(Δγ2（0）+∇E/(gcosL))几乎是不可观测的，因此，为快速精确估计全部平台失准角，要求：I）在中低纬度地区进行对准；II）滤波器至少运行到ΔS1（0）收敛为止；III）比力误差的北向和东向分量尽可能小（经过标校，加速度计误差引起的比力误差可忽略不计，而重力垂线偏差引起的比力误差需视具体动态环境作相应的补偿）。

另外，从式(12)可知，事实上仅需观测水平位置误差或速度误差即可分离三个失准角，同时观测这两种误差主要是为了增加观测数据，以便在同样的对准时间内取得更高的滤波精度。

3.3 精对准Kalman滤波器设计

基于第3.2节讨论，选择增广状态向量为

将式(11)代入式(10)，再代入方程(9)可得4个时变状态量的动态方程；3个平台失准角初值为定常状态量，因此，可得如下增广状态方程：

式中：F(t)——7×7维系统矩阵，各元素根据(9)～(11)是明显的；

w(t)——7×1维状态噪声向量。

另外，根据第3.2节的讨论，观测方程为

式中：H——4×7维观测矩阵，

n(t)——4×1维观测噪声向量。令状态噪声w(t)和观测噪声n(t)为相互独立的零均值Gauss白噪声，且协方差阵满足

这里，δ(t)——Dirac函数。又令初始状态为x0，相应的协方差阵为P0，则根据状态方程(15)和观测方程(16)，容易设计7维精对准Kalman滤波器，对x进行实时估计。Kalman滤波器的具体公式可参阅文献[1]。

4 计算机仿真

假设系统所处位置的纬度为40°，高度为0 m；陀螺常值漂移为1×10-3(°)/h量级，加速度计附加幅值为1×10-3m/s2的零均值白噪声；采样周期为10 Hz；平台姿态角初值分别为S1(0)=3°、σ1(0)=0.2334°和γ2(0)=0°。根据上述快速对准算法和空间稳定系统的机械编排，依次进行粗对准、精对准和导航解算（陀螺漂移系数置为常规初始对准与标定结果）。图2摘录了某三组仿真实验对准期间的平台失准角曲线。

图2 对准期间的平台失准角曲线（仿真）Fig.2 Platform misalignment angles during alignments (simulation)

由图2易见：I）持续粗对准（A组曲线）对应的ΔS1和Δσ1需100 min才能收敛到3.3ρ以内（ρ表示指标要求），而Δγ2仅需30 min；II）粗对准进行12 min后（此时，平台失准角收敛到1200ρ以内）就开始精对准（B组曲线），平台失准角收敛速度慢，甚至发散；III）若粗对准进行47 min后（此时，平台失准角收敛到60ρ以内）再开始精对准（C组曲线），精对准分别进行11 min和56 min即可使.Δσ1和ΔS1（或Δγ2）收敛到ρ以内，这主要是由于初始失准角较小时，精对准滤波器所采用系统模型的线性化误差小，滤波精度较高，同时这也印证了保证精对准滤波器精度控制平台初始失准角小到一定范围内的必要性。

依次进行47 min粗对准和56 min精对准后开始导航解算，所得经纬度误差曲线如图3所示。易见，经度误差主要包含-ρ的偏置和幅值约ρ、周期约24 h的余弦分量；纬度误差包含同周期同幅值的正弦分量，根据空间稳定系统误差传播特性分析结果[1]知，这主要由初始失准角ΔS1(t0)和Δγ2(t0)引起，表明S1和γ2的对准误差约为ρ，而σ1的对准误差不显著。另外，经纬度误差中还包含趋势项和（或）发散振荡分量，这主要由陀螺常值漂移系数误差引起。

图3 导航经纬度误差（仿真）Fig.3 Longitude and latitude errors (simulation)

5 试验结果

某套空间稳定系统在纬度为9°～23°的区域进行了系泊状态和海上动态试验。利用在线记录的系统试验数据和外参考数据（如：计程仪速度和GPS经纬度），对前述快速对准算法进行离线验证（陀螺漂移系数置为常规初始对准与标定结果）。与计算机仿真情况不同，这里平台姿态角真值未知，因此，以平台失准角初值（平台姿态角初值的实时估计值减去相应稳态值）的收敛情况来衡量对准的性能。

下面摘录利用某两段数据进行算法验证的情况。两段数据分别来自系泊和海上环境，载体速度分别为0～0.07 m/s和0.2～0.6 m/s，加速度计随机噪声的幅值分别为0.005～0.015 m/s2和0.4～1 m/s2。

5.1 对准结果

持续进行粗对准24 h，可得系泊状态和海上动态条件下的粗对准曲线，其变化规律与图2的A组曲线相似。表2给出相应平台失准角初值分别收敛到1200ρ、600ρ、200ρ、60ρ和20ρ所需要的时间。

表2 粗对准过程平台失准角初值收敛特性Tab.2 Convergence features of initial platform misalignment angles during coarse alignment

由表2易见，在同等环境下，随着指定收敛范围缩小，所需粗对准时间急剧上升，尤其是对准到20ρ所需时间是对准到60ρ.的3倍以上。另外，收敛到相同范围，海上环境所需粗对准时间是系泊状态的1.6～2倍，其主要原因是在海上由风浪、洋流等引起的载体运动干扰和加速度计随机噪声比系泊状态大2倍以上，结合式(7)和(8)分析知，这将降低粗对准性能。

令粗对准进行47 min后（由表2知，此时平台失准角不超过600ρ）开始精对准并持续4 h，可得系泊状态和海上动态条件下的精对准曲线，其变化规律与图2的C组曲线相似。表3给出相应平台失准角初值分别收敛到20ρ、10ρ、6.6ρ、3.3ρ和ρ所需时间。

表3 精对准过程平台失准角初值收敛特性Tab.3 Convergence features of initial platform misalignment angles during fine alignment

由表3可见，收敛范围从20ρ.缩小到ρ，精对准所需时间的差异不超过一个量级；收敛到相同范围，海上环境所需精对准时间是系泊状态的1.2～2倍。将表2和表3对照可以看出，花费同等量级的时间，精对准精度比粗对准高一个数量级以上。

5.2 导航结果

令粗对准进行47 min，精对准在系泊和海上环境下分别进行1 h和2 h，然后进行导航解算，图4摘录了相应的导航经纬度误差归一化曲线。易见，经纬度误差中主要包含由随机干扰引起的Schuler周期振荡分量，且海上对应的分量比系泊大一倍以上；而周期为24 h的正余弦分量的幅值以及偏置均小于ρ，该结果表明，前述对准过程的精度优于ρ。另外，在导航初始阶段，海上环境对应的经纬误差出现显著的Schuler周期的超调振荡，这主要由速度初值误差引起，因为在精对准过程所用的速度参考信息来自电磁计程仪，此时，计程仪零偏和洋流等将不可避免地造成参考速度的偏置误差（可达0.3 m/s），从而导致速度的初值误差，而这对平台失准角初值估计的影响是可忽略不计的（根据文献[1]知，0.1 m/s的参考速度偏置误差引起的对准误差小于0.4"）。

图4 归一化导航经纬度误差（试验）Fig.4 Normalized longitude and latitude errors (experiments)

6 结论与展望

针对长航时高精度空间稳定系统的快速启动需求，研究了基于位置和速度观测的系统快速对准方法。基于Wahba定姿原理开发了一种可粗略估计较大平台失准角的粗对准算法。研究了系统水平通道误差模型的短时可观测性，并设计了一个可实时估计平台初始失准角、水平速度和位置误差的7维精对准Kalman滤波器。进行了计算机仿真和动态试验研究，结果表明，所述快速对准方法有效、实用，特别是在动态条件下精对准2 h精度即达到ρ，满足系统快速启动需求。

在快速启动时，平台常值漂移误差可达1×10-4(°)/h，如前所述，这对快速对准的影响可忽略不计，但对系统长时间导航精度的影响是难以容忍的，可利用精对准过程中位置和速度观测的残差序列对之进行建模，并在导航阶段预以实时补偿。这里，较为实用的是偏置分离滤波器[6]（偏置强可观）和考虑滤波器理论[7-8]（偏置弱可观），这些将在后续研究中进一步讨论。

（References）：

[1] 高钟毓. 惯性导航系统技术[M]. 北京: 清华大学出版社, 2012: 184-389.

[2] Wu M P, Wu Y X, Hu X P, et al. Optimization-based alignment for inertial navigation systems: Theory and algorithm[J]. Aerospace Science and Technology, 2011: 1-17.

[3] Peter M G. Coarse alignment of a ship’s strapdown inertial attitude reference system using velocity loci[J]. IEEE Transactions on Instrumentation and Measurement, 2011, 60(6): 1930-1941.

[4] Ali J, Mirza M R U. Initial orientation of inertial navigation system realized through nonlinear modeling and filtering[J]. Measurement, 2011, 44(5): 793-801.

[5] Cao S Y, Guo L. Multi-objective robust initial alignment algorithm for inertial navigation system with multiple disturbances[J]. Aerospace Science and Technology, 2012, 21(1): 1-6.

[6] Friedland B. Treatment of bias in recursive filtering[J]. IEEE Transactions on Automatic Control, 1969, 14(4): 359-367.

[7] Zanetti R, Bishop R H. Kalman filters with uncompensated biases[J]. Journal of Guidance, Control, and Dynamics, 2012, 35(1): 327-330.

[8] 何虔恩, 高钟毓, 吴秋平. 基于未补偿偏置滤波器的惯导动态初始对准[J]. 中国惯性技术学报, 2015, 23(2): 184-188.

He Q E, Gao Z Y, Wu Q P. Initial alignment based on uncompensated bias filter for inertial navigation systems under dynamic conditions[J]. Journal of Chinese Inertial Technology, 2015, 23(2): 184-188.

Fast alignment of space-stable systems based on position and velocity observations

HE Qian-en, WU Qiu-ping
(Department of Precision Instruments, Tsinghua University, Beijing 100084, China)

Space-stable systems are the key to high-precision long-term navigation technology, in which fast alignment is one of the most important functions in engineering applications. In this paper, a system fast alignment method based on position and velocity observations is investigated, and a coarse alignment algorithm based on Wahba’s attitude determination principle is developed. The short-time observability of system horizontal error model is analyzed, then a seven-dimension Kalman filter is designed to estimate the real-time initial platform non-alignment angles, horizontal position and velocity errors. Simulations results and dynamic experiments show that the presented fast alignment approach is efficient in estimating large platform non-alignment angles (a magnitude of 3°) and can be adopted in urgent starting situations whether in port or on the sea. The expected dynamic accuracy can be achieved in two hours for fine alignment, which is of great value in engineering applications.

space-stable systems; fast alignment; position and velocity observations; Kalman filtering

U666.1

A

2015-02-10；

2015-06-04

总装“十二五”预研项目（51309030401）

何虔恩（1985—），男，博士后，研究方向为导航系统与控制。E-mail：heqianen2005@126.com

联 系 人：吴秋平（1972—），男，副研究员，博士生导师。E-mail：wuqiuping@mail.tsinghua.edu.cn