伞降平台扫描参数测量方法研究

田天齐，刘 宁,2，苏 中,2，戚文昊

(1.北京信息科技大学 高动态导航技术北京市重点实验室，北京 100192；2.北京信息科技大学 现代测控技术教育部重点实验室，北京 100192)

0 引言

伞降平台在军事武器[1]、空中探测、空投等方面有着广泛应用。伞降平台稳态扫描阶段的扫描角、扫描角速度是影响伞降平台空中探测性能和扫描效率的关键参数[2-3]。

伞降平台的扫描参数主要由其自身的结构参数和导旋降落伞的设计参数决定[4-5]。为保证扫描参数满足应用设计要求，在正式使用前往往需要通过多次试验对扫描参数进行测量，以及对扫描平台结构参数和导旋降落伞的设计参数进行调整。高精度、使用便捷、多参数测量的扫描参数测量方法能够极大地减少试验次数、节省人力成本和时间成本。目前，常用的伞降平台扫描参数测量方法主要有光测法[6]、传感器测量法等。光测法是通过高速摄像机等光测设备拍摄目标，提取目标上对应特征点计算目标姿态，但该方法对于相机架设要求较高，且测量精度极易受光线的影响。传感器测量法是将高精度陀螺仪等安装在被测目标上进行三维姿态测量，是目前三维姿态测量的重要方法。随着传感器件性能的提升和地磁理论的发展，传感器测量方法在姿态测量方面得到广泛应用。

在伞降平台扫描参数测量方面，国内外学者进行了相关研究。马凯成等[7]提出用三维角速度陀螺的数据直接计算伞降平台扫描角的方法，但该方法只是对扫描角进行测量，且无法获得每一采样时刻的扫描角。周连成等[8]提出了一种基于地磁传感器的扫描参数解算方法，在磁场恒定的假设下，解算出扫描角和扫描角速度，但其结果的准确性依赖于环境磁场恒定的假设，在周围磁场变化时无法保证测量结果的准确性。

为了解决现有伞降平台扫描参数测量方法存在的测量参数单一、前期准备工作繁杂严苛、抗干扰能力差的问题，本文提出采用磁强计和惯性器件组合对伞降平台进行扫描参数测量,以地磁解算姿态和惯性解算姿态的差值作为扩展卡尔曼滤波器的观测量，修正姿态误差，获得扫描参数。本文采用了自主设计的高精度伞降平台扫描参数采集装置获取数据，可有效降低测量误差。无人机空投实验的结果验证了该姿态测量方案的可行性和准确性。上述研究为空中伞降平台扫描参数的测量提供了一种更可靠、准确、便捷的方法。

1 坐标系及扫描运动

伞降平台通常由载体送至空中指定位置进行投放。为扩大其探测范围，其下落过程通常伴随旋转扫描运动。伞降平台通常由旋转降落伞、连接装置和载体3部分组成。如图1所示，旋转降落伞通过连接装置带动载体旋转扫描，在下落过程中，载体上的探测设备实现从外圈到内圈的旋转扫描探测。伞降平台的下落过程可分为自由坠落、减速减旋和稳态扫描3个阶段[9]。其中，伞降平台的使用性能取决于稳态扫描阶段的扫描角、扫描角速度以及这些参数的变化规律。本文主要针对伞降扫描平台在稳态扫描阶段的扫描角、扫描角速度的测量，提出一种使用便捷、测量精度高、具有一定抗干扰能力的扫描参数测量方法。

图1 伞降平台结构示意

为便于研究与分析，建立了如图2所示的坐标系。以伞降平台投放点的地理坐标系作为导航坐标系，以水平指向正北方向为X轴，Y轴水平指向正东，Z轴垂直于Y轴向下，建立导航坐标系O-XnYnZn；以平台轴向向上方向为Z轴，稳态扫描法向方向为X轴，建立载体坐标系O-XbYbZb；绕Xn轴旋转形成的角定义为滚转角φ，绕Yn轴旋转形成的角定义为俯仰角θ，绕Zn轴旋转形成的角为偏航角ψ；扫描角α为OnZn轴与OnZb轴的夹角，扫描角速度ω为平台绕OnZn轴旋转的角速率。

图2 导航坐标系与载体坐标系

2 扫描参数解算原理及算法设计

2.1 扫描参数解算原理

常用的姿态解算方法有欧拉角法、方向余弦法和四元数法。其中，基于四元数法变换的模型能够更好地反映伞降平台扫描的物理规律，可以很好地形成所需扫描轨迹，比欧拉角法和方向余弦法更适于描述伞降平台的大幅度姿态运动[10]。

扩展卡尔曼滤波器(EKF)是将卡尔曼滤波器(KF)局部线性化，其算法简单、计算量小，非常适用于伞降平台的弱非线性、高斯环境[11-12]。

伞降平台扫描参数的测量采用三轴陀螺仪、三轴加速度计和三轴磁强计进行组合测量。其算法主要流程：首先根据惯性器件的输出数据解算出扫描姿态；同时根据地磁信息解算出扫描姿态；然后利用扩展卡尔曼滤波器进行组合滤波和误差观测；最后根据修正后的姿态数据计算出扫描参数。图3中给出了姿态算法的流程。其中，ωx、ωy、ωz分别为陀螺仪输出数据；ax、ay、az分别为加速度计输出数据；mx、my、mz分别为地磁强计输出数据。

图3 姿态估计算法框图

2.2 基于四元数的EKF姿态估计算法

2.2.1 初始对准

惯性测量解算姿态首先需要进行初始对准[13]。伞降扫描平台通常是由飞行体运送至指定区域上方投放，在投放前，伞降扫描平台有短暂的静态平稳状态。选取该平稳状态下20 ms内加速度数据、地磁数据的均值，按照如下初始对准公式进行姿态角(初始欧拉角)初始对准。

(1)

(2)

式中，ax0、ay0、az0分别为无人机空投前20 ms(平稳状态)加速度计X、Y、Z轴输出值的平均值。

在滚转角和俯仰角已知的情况下，磁航向角可以表示为

ψ0=arctan 2(My,Mx)

(3)

式中

Mx=mx0cosθ+my0sinθsinφ+mz0cosθsinφ

My=my0cosφ-mz0sinφ

其中：mx0、my0、mz0分别为无人机空投前20 ms磁强计X、Y、Z轴输出值的平均值。

2.2.2 基于惯性传感器的姿态更新

获得初始姿态后，可以计算出从载体坐标系到导航坐标系的转换矩阵和初始四元数，通过转换矩阵将载体坐标系下的陀螺仪数据转换到导航系下，进行四元数更新，从而实现姿态更新。

(4)

2)利用初始方向余弦矩阵计算初始四元数：

(5)

(6)

3)利用毕卡求解法的定时采样增量法求解四元数。四元数更新公式如下：

(7)

T=0.001 s为采样周期。

4)根据3)中更新的四元数求欧拉角：

(8)

根据上述步骤，可以根据惯性传感器的数据更新伞降平台的姿态角数据。

2.2.3 基于地磁传感器的姿态更新

在下落过程中，伞降扫描平台的空间位移很小，地磁场的变化非常微弱，可以认为在该过程中，地磁场是恒定的。忽略铅锤方向的落速，建立如图4所示的地磁测姿模型。其中，γ为磁倾角，B为磁场强度大小，β为相角，ω为扫描角速度，α为扫描角。

图4 伞降扫描平台地磁测姿模型

扫描角速度ω和扫描角α的表达式为

(9)

2.2.4 EKF姿态估计滤波器

1)状态方程建立

本文组合滤波中选择下面的误差值作为扩展卡尔曼滤波器的状态变量。

δx=[δφ,δθ,δψ,δαx,δαy,δαz,δωx,δωy,δωz]T

式中：δφ、δθ、δψ分别为滚转角、俯仰角和航向角的误差；δαx、δαy、δαz为加速度计的零偏误差；δωx、δωy、δωz为陀螺仪的零偏误差。

根据状态变量建立系统的状态方程：

δxk+1/k=Fkδxk/k+Wk

(10)

(11)

(12)

2)观测方程建立

选择系统的观测量为姿态测量误差，姿态测量误差由地磁测量和惯性测量组合得到。建立观测方程：

zk=Hδxk/k+vk

(13)

(14)

式中：H=[I00]为观测矩阵；vk为观测的噪声；φM、θM、ψM分别为地磁滚转角、俯仰角、偏航姿角；φINS、θINS、ψINS分别为惯性测量得到的滚转角、俯仰角、偏航姿角。

3)状态更新过程

状态变量的时间更新公式：

δxk+1/k=Fkδxk/k+Wk

(15)

滤波状态误差矩阵更新公式：

(16)

式中：Fk=I+F(t)T,Qk=QkT。

4)观测更新过程

计算卡尔曼增益矩阵：

Kk+1=Pk+1/kHT[HPk+1/kHT+Rk+1]-1

(17)

更新状态估计：

δxk+1/k+1=δxk+1/k+Kk+1[yk+1-Hδxk+1/k]

(18)

更新协方差阵：

Pk+1/k+1=[I-Kk+1H]Pk+1/k[I-Kk+1H]T+

(19)

式中：Rk+1为观测方差矩阵；Pk+1/k+1为状态更新后的状态方差。

给定初值，根据陀螺、加速度计和磁强计的测量值便可由上述步骤计算各个时刻的姿态信息。

2.3 姿态误差修正与扫描参数获取

用估计的误差状态量δφ、δθ、δψ校正惯性测量解算结果，减小最终解算姿态的误差。姿态修正原理表示如下式所示：

(20)

获得伞降扫描平台的偏航角、滚转角、俯仰角姿态数据后，根据式(21)计算伞降平台的扫描角、扫描角速度。

(21)

式中ωz为陀螺仪Z轴方向输出的速率。

3 实验与结果分析

3.1 实验条件

通过无人机将载有扫描参数测量装置的伞降平台从300 m高空投下，测量装置采集和记录无人机从升起到载体落地的全过程中各传感器的数据。伞降扫描平台落地后，停止采集和记录数据，待回收读取数据。伞降装置的设计是按照转速4 r/s、扫描角30°设计的。

该高精度伞降平台扫描参数测量装置有1块主板和4块附板，该装置将多个磁阻传感器和多个陀螺仪垂直分布在测量装置主板四周，通过对采集的数据进行处理分析，对主板传感器采集的数据进行补偿，可有效降低测量误差。

3.2 实验结果

读取伞降平台上的扫描参数测量装置采集的数据，利用提出的姿态解算方法进行姿态信息的求解。图5～7分别是测量设备采集的三轴加速度计数据图、三轴地磁数据图、三轴陀螺数据图。

图5 三轴加速度计数据输出

图6 三轴地磁强度数据输出

图7 三轴陀螺仪数据输出

由图上的数据可知，伞降平台在大约5 s时进入稳定扫描阶段。伞降平台下落过程采用伞降装置控制扫描运动，其运动状态易受到风速等外界因素的影响而出现一定的波动，如17 s附近出现波动。上述各图的数据符合伞降平台的运动规律。

获得各传感器的数据后，经过算法解算得到的扫描角速度输出图、扫描角输出图和三维运动轨迹输出图分别如图8～10所示。

图9 扫描角输出

图10 三维扫描运动

根据实验结果可以看出，通过该算法解算得到的稳态扫描阶段的扫描角和扫描角速度分别稳定在30°和4 r/s附近，与设计参数基本相符。伞降平台的三维扫描运动图也符合伞降平台旋转扫描下落的运动规律。

3.3 误差分析

3.3.1 方差分析

伞降扫描平台姿态角方差输出曲线如图11所示。

图11 姿态角方差输出

根据姿态角方差图可知：在30 s内，偏航角方差约为0.01°，偏航角能够收敛；滚转角和俯仰角的方差也小于0.04°，其方差曲线有收敛趋势。因此，在伞降平台下落的几十秒过程中，本文解算出的俯仰角、滚转角和偏航角误差趋于稳定。

3.3.2 视频分析验证

本文实验的全过程采用高清摄像头进行记录，通过后期视频图像处理分析[14]，得到稳定阶段扫描角和扫描角速度均值。7～27 s内视频分析结果与算法解算结果数据如表1 所示。

表1 视频数据与解算数据对比

根据表中数据可得出以下结论：在误差允许的范围内，可以认为视频分析得到的扫描角、扫描角速度与解算得到的扫描角、扫描角速度相等。本文提出的地磁和惯性组合姿态测量方法能够实现伞降平台姿态的测量，测量结果具有较高的参考价值。

4 结束语

本文针对目前单一的地磁或惯性测量方法应用于伞降平台扫描参数测量时存在的抗干扰能力差、测量精度不够高、测量的扫描参数单一的问题，提出了采用地磁与惯性组合测量的方法。该方法通过地磁解算姿态辅助惯性测量姿态，采用扩展卡尔曼滤波器进行滤波和误差观测，很大程度上解决了上述单一测量方法存在的弊端。该方法不仅能够测量多个扫描参数，还提高了伞降平台扫描参数的测量精度，适应性更广，且具备一定的抗磁干扰能力。无人机空投试验结果表明，提出的姿态测量方法测得的姿态角具有较高的精度，解算出的扫描角、扫描角速度和三维扫描运动具有较高的参考价值。