[发明专利]一种基于航迹匹配的初始对准方法

权利要求书

1.一种基于航迹匹配的初始对准方法，其特征在于，所述方法包括以下步骤：

步骤S1、通过重力加速度和地球自转角速度在导航坐标系上的投影与它们在载体坐标上的投影之间的坐标转换关系，计算初始姿态矩阵；

步骤S2、设定初始数据，包括初始位置、初始速度、初始游动方位角；

步骤S3、通过得到的所述初始姿态矩阵和所述初始数据计算捷联矩阵初始值、初始姿态角和初始四元数；

步骤S4、载体行进，通过捷联矩阵的实时更新，用惯导算法得到水平姿态角及经纬度位置；

步骤S5、记录载体行进轨迹并作为基准值；

步骤S6、将所述轨迹上各点位置经纬度经过差分过程得到方位角变化量；

步骤S7、将所述方位角变化量分别进行存储，建立匹配数据库。

2.根据权利要求1所述的一种基于航迹匹配的初始对准方法，其特征在于，所述步骤S1的具体过程如下：

重力加速度g在导航坐标系上的投影为：gⁿ＝[0 0 -g]^T；地球自转角速度在导航坐标系上的投影为：其中，为当地的纬度，ω_ie为地球自转角速率，设为15°/h。重力加速度和地球自转角速度在载体坐标系上的投影g^b、分别通过捷联惯导系统的加速度计与陀螺仪测量值与得到；

利用g、ω_ie以及g×ω_ie计算姿态矩阵计算公式为：

3.根据权利要求1所述的一种基于航迹匹配的初始对准方法，其特征在于，所述步骤S2的具体过程如下：

设定初始数据，包括初始位置、初始速度、初始游动方位角。其中，初始位置包括起点的经度λ₀和起点的纬度由于从静止状态开始行驶，因此初始速度V_x0＝V_y0＝V_z0＝0；初始游动方位角α₀＝0。

4.根据权利要求1所述的一种基于航迹匹配的初始对准方法，其特征在于，所述步骤S3的具体过程如下：

步骤S3.1、计算捷联矩阵初始值：

步骤S3.2、根据捷联矩阵初始值进行反三角函数计算得到初始姿态角：

步骤S3.3、求解四元数约束方程得到初始四元数Q，其中所述四元数约束方程如下：

5.根据权利要求1所述的一种基于航迹匹配的初始对准方法，其特征在于，所述步骤S4的具体过程如下：

载体按照预先设计好的轨迹运动，通过捷联矩阵的实时更新，用惯导算法解算得到水平姿态角及经纬度位置；

步骤S4.1、设机体坐标系相对平台坐标系的转动四元数Q＝q₀+q₁i_b+q₂j_b+q₃k_b，即时修正转动四元数Q：

步骤S4.2、通过q₀、q₁、q₂、q₃计算捷联矩阵T：

步骤S4.3、四元数Q的最佳归一化：

步骤S4.4、比力的坐标转换：加速度计测量的比力通过矩阵T可转换为即

步骤S4.5、速度的即时修正：

步骤S4.6、位置矩阵的即时修正：

步骤S4.7、位置速率的计算：

对于游动方位系统，由于因此

其中，代表扭转挠率、Rxp代表x轴自由曲率半径、Ryp代表y轴自由曲率半径；

步骤S4.8、地球速率经矩阵C转换为

步骤S4.9、姿态速率的计算：

步骤S4.10、将姿态矩阵T表示为ψ_G、θ、γ的关系，并求得姿态角θ、γ和格网航向角ψ_G；

步骤S4.11、将位置矩阵C表示成λ，α的关系，并求得位置经度纬度λ和方位角α；