[发明专利]一种基于角度参数化的航向初始对准方法

权利要求书

1.一种基于角度参数化的航向初始对准方法，其特征在于：包括以下步骤：

步骤1)，建立航向角初始对准模型；

步骤2)，建立EKF滤波器框架；

步骤3)，航向角滤波器的初始化；

步骤4)，将Mahony滤波器作为姿态航向参考系统，该滤波器通过基于比例积PI控制器的误差向量来校正旋转矢量；

步骤5)，扩展卡尔曼滤波器构建；

步骤6)，构建高斯和滤波器。

2.根据权利要求1所述的一种基于角度参数化的航向初始对准方法，其特征在于：步骤1)所述建立航向角初始对准模型，采用的地理坐标系取东北天，航向角正方向取北偏东为正，SINS的姿态、速度和位置的动力学方程如下：

其中，n为导航坐标系，b为载体坐标系，N为地理坐标系，i为惯性坐标系，表示b系到n系的旋转矩阵，表示旋转矩阵的微分，表示b系相对于n系在b系下的角速度，Vⁿ表示载体在导航坐标系下的速度，f^b表示载体坐标系下的比力，表示n系相对于i系在n系下的角速度，表示n系相对于e系在n系下的角速度，gⁿ表示重力n系下分量，L,h,R分别表示载体所在的经度，高度，半径，表示N系相对n系的相对速度。

3.根据权利要求1所述的一种基于角度参数化的航向初始对准方法，其特征在于：步骤2)所述建立EKF滤波器框架是将SINS/GPS以组合方式应用于扩展卡尔曼滤波，提出航向角估计方法，该方法架构包含3个模块：GPS接收模块，基于IMU的航向角积分模块，二者的数据通过一个扩展卡尔曼滤波算法进行滤波处理得到融合航向角。

4.根据权利要求1所述的一种基于角度参数化的航向初始对准方法，其特征在于：步骤3)所述航向角滤波器初始化流程是在运行APEKF算法之前，对该算法中的航向角滤波器进行初始化，

步骤41：假设航向角由xy水平面向北的增量速度ΔV_x，向东的增量速度ΔV_y和绕z轴旋转的增量角度Δψ所驱动，偏航角的测量误差ψ_error和这3个值的测量噪声相关，

式中3个值和3个噪声由IMU提供，算法在xy水平面上预置多个偏航角，每个偏航角由这3个值和3个噪声驱动演化，独立进行时间更新，

步骤42：在xy水平面上预置多个平均间隔、平均权重的偏航角，在测量阶段GPS观测所提供水平面的速度，根据残差进行运算，偏航角向真实航向角收敛。

5.根据权利要求1所述的一种基于角度参数化的航向初始对准方法，其特征在于：步骤4)所述Mahony滤波器作为姿态航向参考系统，该滤波器通过基于比例积PI控制器的误差向量来校正旋转矢量，

步骤51：误差向量的定义如下：

e＝a×d (5)

式中a是加速度计向量，d是由估计姿态得到的重力向量方向，e是误差向量，

步骤52：从四元数估计重力矢量d，q是四元数，q₁表示实部，q₂，q₃，q₄表示虚部：

步骤53：计算向量积分，l_n表示n时刻的误差积分，Ki表示积分系数，Δt表示时间间隔：

l_n＝l_n-1+eKiΔt (7)

步骤54：计算比例向量，ω表示角速度，K_p表示比例系数：

ω′＝ω+K_pe+l_n (8)

步骤55：使用四元数微分方程进行积分运算：

步骤56：计算航向角Yaw：