动态倾斜校准传感器的姿态角补偿优化算法

高 进，周大铮

(西北机电工程研究所 7室，陕西咸阳 712099)

在本测量系统中，有3个倾斜角敏感器，2个角速率敏感器分别固联于载体相应的坐标轴上，直接测量载体角运动，它要求较高的测量带宽。倾斜角敏感器输出载体姿态角静态初值，角速率敏感器提供积分时间内载体的角增量的累加值［1］。刚体的有限转动不是矢量，其转动次序不可交换。无限小转动是矢量。Bortz的转动向量［2］(Orientation Vector)微分方程为计算捷联系统的姿态矩阵建立了全面的理论基础［3］。转动向量的变化速率是惯性测量的角速率向量与计算得到的非互易速率向量二者之和。后者是影响捷联惯性系统姿态角精度的一个重要因素［4］。因此，在高角速率动态环境中，为防止姿态误差积累，必须对非互易速率向量进行补偿。对非互易速率向量进行补偿的计算一般称为圆锥补偿算法。要提高系统精度，可以有两种选择，一种是对角速率敏感器输出信号进行高速采样，使用简单的圆锥补偿算法;另一种是适当降低对角速率敏感器输出信号采样的速率，使用复杂但精确的圆锥补偿算法。

设计出补偿精度高、计算量少的圆锥补偿算法一直是人们不断追求得目标。Miller提出了在圆锥运动条件下优化的三子样算法［5］，Jang G.Lee等提出了四子样算法［6］，这两种算法没有利用前一个圆锥补偿周期的角速率敏感器输出信号，而且计算量较大。Yeon Fuh Jiang等提出了利用前一补偿周期的一类算法，包含了圆锥补偿周期内角速率敏感器输出信号的所有可能的叉乘项，并利用前一补偿周期的角速率敏感器输出累加角增量［7－9］。Musoff提出了圆锥算法的优化指标，分析了圆锥补偿后的算法误差与补偿周期的幂次r的关系［8］。本文提出的算法利用在经典圆锥运动下，相同时间间隔的角增量向量的叉乘对圆锥误差的贡献相等的特点，推导出一类计算量较少的圆锥补偿算法。同时利用前一圆锥补偿周期的角速率敏感器输出信号，提高了补偿精度，缩短了运算时间。

1 补偿算法

载体转动向量的微分方程为［3］

式中，Φ是转动向量;ω是机体角速率向量。方程右边第2项与第3项之和是非互易误差。对于小Φ，为简化计算，忽略右边第3项，并用式(2)代替第2项中的Φ。

这样，式(1)可简化为

β就是需要进行补偿的非互易向量τ。随着圆锥补偿周期内的采样次数不同，β可以有多种方式进行计算。

对于经典圆锥运动，它的转动向量为［7－9］

式中，Ω是圆锥运动角频率;a是圆锥运动的幅度。采用上标B表示机体坐标系，得到

假设在t～t+h间隔内，对角速率敏感器输出信号进行N次采样，θi表示第i次采样的角速率敏感器输出的角增量信号。

则在t～t+h间隔内，转动向量的估计值的计算公式为

根据式(4)，圆锥补偿可计算如下

右边共有N(N－1)/2项。由式(9)，得

由式(12)可以看出，在θi×θj中，x分量只与相对时间(i－j)h有关，而与绝对时间无关，y和z分量是绝对时间t的余弦函数。在圆锥运动下，能引起漂移的误差出现在x轴上。因此，具有相同时间间隔的两个角增量向量的叉乘对圆锥误差的贡献相等，而可以不考虑其与绝对时间的关系。利用这个性质，可以将圆锥误差补偿公式简化为［7］

比较式(13)和式(11)可以看出，简化后的算法，计算量大大减小。简化带来的缺陷是在一般机动条件下，圆锥误差补偿算法的误差会增大。但即使在非常大的机动条件下，算法误差也可以忽略，而且这种误差只是在机动期间产生。因此，对圆锥补偿算法的简化是合理的。

如果考虑到可以利用前一圆锥补偿周期的角增量输出，则可以得到以下算式

式中，θ'是前一圆锥补偿周期的累加值。对应采样为2～5等情况，可得到简化算法式，如(14)和式(15)的各种形式。

2 算法应用情况

为验证算法的有效性，把上述算法转化成动态倾斜校准传感器内的计算机程序，把动态倾斜校准传感器放在双轴摇摆台上对比输出曲线。图1中虚线是摇摆台输出曲线，实线是倾斜校准传感器输出曲线，可见在摆频2.5 Hz条件下，倾斜校准传感器输出的跟踪曲线已经比较完美，但仍有少量数据出现较大误差，这是因为摇摆台由曲柄连杆驱动，在其转动拐点处曲线可看到分明的棱角，在该处角速度和角加速度发生反向突变，高频分量丰富，远超出倾斜校准传感器的带宽范围。即便在苛刻的工作环境下，倾斜校准传感器输出的跟踪曲线仍能准确地描述转台曲线的棱角形状。

图1 输出曲线

3 结束语

选定的载体圆锥运动条件下的应用结果表明:(1)这类算法比相同精度其他算法的计算量可减少1/3。(2)所有算法的精度都随着对敏感器输出信号的采样频率提高而提高。(3)文中提出的采样算法漂移小，表明在对敏感器输出信号采样频率一定时，一个圆锥补偿周期内的采样数不是越多越好。(4)圆锥补偿周期的采样数≥4时，利用前一补偿周期的采样，对增加精度效果不明显。圆锥补偿周期的采样数＜4时，分别利用前一圆锥补偿周期敏感器输出信号的各次采样，而不是利用前一补偿周期各次采样输出的累加量，补偿精度将更高。(5)圆锥补偿周期的采样数＜4时，在圆锥补偿算法中，利用前一圆锥补偿周期对敏感器输出信号，相当于提高了对敏感器输出信号的采样频率。(6)使用内置DSP计算机，处理速度快，实时性好。

［1］陈哲.捷联惯性导航系统原理［M］.北京:宇航出版社，1986.

［2］BORTZ J E.A new mathematical formulation for strapdown inertial navigation［J］.IEEE Transacfions on Aerospace and Electronic Systems，1971，7(1):61 －66.

［3］IGNAGNI M B.On the orientation vector differential equation in strapdown inertial systems［J］.IEEE Transactions on Aerospace and Electronic Systerms，1994，30(4):1076 －1081.

［4］SAVAGE P G.Strapdown inertial navigation integration algorithm design，Part l:Attitude algorithm［J］.Journal of Guidance，Control，and Dynamics，1998，21(1):19 －28.

［5］MILLE R B.A new strapdown attitude algorithm［J］.Journal of Guidance，Control，and Dynamics，1983，6(4):287 －291.

［6］LEE J G，YOON Y J，MARK J G.Extension of strapdown attitude algorithm for highfrequency base motion［J］.Journal of Guidance，Control，and Dynamics，1990，13(4):738 －743.

［7］JIANG Y F，LIN Y P.Improved strapdown coning algorithm［J］.IEEE Transactions on Aerospace and Electronic Systems，1992，28(2):484 －490.

［8］MUSOFF H，MURPHY J H.Study of strapdown navigation attitude algorithms ［J］.Journal of Guidance，Control，and Dynamics，1995，18(2):287 －290.

［9］JIANG Y F，LIN Y P.On the rotation vector differential equation［J］.IEEE Transactions on Aerospace and Electronic Systems，1991，27(1):181－183.