[发明专利]一种弹道环境下的多矢量空中对准方法

权利要求书

1.一种弹道环境下的多矢量空中对准方法，其特征在于，包括以下步骤：

S1、坐标系的明确：导航坐标系为东北天地理坐标系OX_nY_nZ_n，载体坐标系采用以载体质心为原点的右前上坐标系OX_bY_bZ_b，其中Y轴为载体自转轴，初始时刻载体惯性系OX_b0Y_b0Z_b0与开始瞬时的载体坐标系重合，随后相对于惯性空间无转动,初始时刻导航坐标系OX_n0Y_n0Z_n0与初始对准开始瞬时的导航坐标系重合，随后相对于惯性空间无转动；

S2、空中对准方法所需数据信息的获取：包含卫星导航系统GNSS信息的获取与惯性导航系统INS数据的获取；

S2.1、GNSS信息的获取：包括载体的速度vel与位置pos，并设T_GPS为GNSS信息输出的采样时间；

S2.2、INS数据的获取：包括陀螺数据与加速度计数据f^b，并设T_INS为INS数据输出的采样时间；

S3、当载体在空中上电，卫星接收机接收到第一次的速度信息vel0,第一次的位置信息pos0，以此来初始化空中对准算法，同时设定此时刻为对准初始时刻T₀；

S4、根据INS输出数据，通过式构造矢量α(t)；

S4.1、通过式求得

S5、根据GNSS输出数据，建立质点弹道动力学模型，完成基于扩展卡尔曼滤波器EKF的弹道运动估计算法，输出质点弹道动力学模型约束下的载体速度信息vel，位置信息pos，通过式构造矢量β(t)；

S5.1、通过下式建立质点弹道动力学模型：

式中，表示弹道系数；H(z)＝ρ/ρ_0N表示空气密度函数；v_x表示东向速度，w_x表示东向风速；v_y表示北向速度，w_y表示北向风速；v_z表示天向速度，表示合速度；g表示当地重力加速度，L表示纬度，λ表示经度，h表示高度，R_M表示当地子午圈的曲率半径，R_N表示当地东西圈的主曲率半径；Ma＝v_r/c_s为空气阻力系数的计算公式；

S5.2、选取卡尔曼滤波状态量为:

x＝[v_x,v_y,v_z,L,λ,h]^T＝[x₁,x₂,x₃,x₄,x₅,x₆]^T；

S5.3、选取卡尔曼滤波量测矩阵和量测量为：

H_k＝I；Z_k＝[v_e,v_n,v_u,L,λ,h]^T；

S5.3、对步骤S5.1的质点动力学模型进行离散化，得到f(X_k)：

S5.4、通过式得到状态转移矩阵，式中T为扩展卡尔曼滤波器的步长，设置T＝T_INS；

S5.5、通过式完成扩展卡尔曼滤波更新：

S5.6、通过扩展卡尔曼滤波器中的状态量得到vel＝[v_x,v_y,v_z]^T和pos＝[L,λ,h]^T，进而求得其中ω_ie代表地球自转角速率，是一个常值；

S5.7、通过式求取

S6、在S4、S5获取到的矢量的基础上，通过Request矢量定姿算法求解式完成初始时刻T₀的姿态矩阵的对准；

S6.1、构建矩阵其中B_k＝α(t_k)'*β(t_k)，S_k＝B_k+B_k'，z_k＝α(t_k)×β(t_k)，σ_k＝trace(B_k)；

S6.2、通过K_k+1＝K_k+δK迭代求得每个时刻的K矩阵；

S6.3、求取K矩阵的特征值与特征向量，并取最大特征值所对应的特征向量，即为初始时刻T₀的姿态矩阵的四元数形式；

S6.4、将四元数形式的姿态矩阵转化为欧拉角形式，便可以输出对准后的三个姿态角。