[发明专利]一种惯导和双天线GNSS外参标定方法

权利要求书

1.一种惯导和双天线GNSS外参在线标定方法，其特征在于，包括以下步骤：

步骤1，周期采集k时刻车载传感器信息，包括惯性传感器信息和双天线GNSS信息；

步骤2，根据k时刻的惯性传感器数据，预测k时刻载体的姿态、速度、位置信息；

步骤3，若k时刻采集到双天线GNSS数据，则通过卡尔曼滤波器估计k时刻惯导和双天线GNSS之间的外参；否则，跳转至步骤1；

步骤4，跳转至步骤1。

2.根据权利要求1所述的一种惯导和双天线GNSS外参在线标定方法，其特征在于，所述步骤2的具体过程如下：

1)采用如下公式预测载体的姿态：

其中：

q(k)＝[q₀(k) q₁(k) q₂(k) q₃(k)]^T为k时刻的姿态四元数，上标T表示矩阵的转置，其中q₀(k)、q₁(k)、q₂(k)、q₃(k)为表示k时刻载体姿态的4个数字；

q(k-1)＝[q₀(k-1) q₁(k-1) q₂(k-1) q₃(k-1)]^T为k-1时刻的姿态四元数，其中q₀(k-1)、q₁(k-1)、q₂(k-1)、q₃(k-1)为表示k-1时刻载体姿态的4个数字；

ΔT为离散采样周期；

其中为k时刻机体系相对于导航系的角速率在机体系X、Y、Z轴上的分量；

为k时刻采集到的三轴陀螺数据；

为k-1时刻的导航系到机体系的姿态转移矩阵；

为k-1时刻地球自转角速率在导航系上的分量，ω_ie为地球自转角速率，L(k-1)为k-1时刻的纬度；

为k-1时刻导航系相对于地球系的角速度在导航系上的分量，为k-1时刻的速度在导航系东向、北向上的分量，L(k-1)、h(k-1)为k-1时刻的纬度和高度，R_M、R_N为地球的子午圈、卯酉圈半径；

2)采用如下公式预测载体的速度：

其中：

为k时刻的速度，为k时刻的速度在导航系东向、北向、天向上的分量；

为k-1时刻的速度，为k-1时刻的速度在导航系东向、北向、天向上的分量；

为k时刻的机体系到导航系的姿态转移矩阵；

f^b(k)为k时刻采集到的三轴加速度计数据；

gⁿ为地球重力加速度在导航系上的分量；

3)采用如下公式预测载体的位置：

其中：

λ(k)、L(k)、h(k)为k时刻的经度、纬度和高度；

λ(k-1)、L(k-1)、h(k-1)为k-1时刻的经度、纬度和高度。

3.根据权利要求1所述的一种惯导和双天线GNSS外参在线标定方法，其特征在于，所述步骤3的具体过程如下：

1)计算状态量的一步预测值

式中，

Φ_M＝diag{0,0,0,0,0,0,0,0,0,0}

为载体的速度在导航系东向、北向、天向上的分量，L为载体的纬度，h为载体的高度，R为地球半径，为三轴加速度计的输出在导航系东北天方向上的分量，即为机体系到导航系的姿态转移矩阵，0_m×n为m×n的零矩阵，Φ_k,k-1为滤波器k-1时刻到k时刻的一步转移矩阵，为k-1时刻到k时刻的状态量一步预测值，为k-1时刻滤波器状态量估计值，φ_E、φ_N、φ_U为东向、北向、天向平台误差角，δv_E、δv_N、δv_U为东向、北向、天向速度误差，δL、δλ、δh为纬度、经度、高度误差，为陀螺三轴零偏，为加速度计三轴零偏，ψ为双天线GNSS基线之间的航向与惯导轴向之间的偏差角，为GNSS主天线与惯导之间的三轴杆臂；

2)计算一步预测均方误差P_k|k-1

式中，

P_k|k-1为k-1时刻到k时刻的一步预测均方误差，P_k-1为k-1时刻的状态估计均方误差，上标T表示矩阵转置；

Γ_k-1为滤波器k-1时刻的系统噪声矩阵；

Q_k-1＝diag{ε_gx²,ε_gy²,ε_gz²,ε_ax²,ε_ay²,ε_az²}

Q_k-1为k-1时刻的系统噪声，diag表示矩阵对角化，其中ε_gx、ε_gy、ε_gz分别为的模型噪声，ε_ax、ε_ay、ε_az分别为的模型噪声；

3)计算k时刻卡尔曼滤波器的滤波增益K_k

式中，

H_p＝[0_3×6 diag{R_M+h,(R_N+h)cosL,1} 0_3×7 H_l]

H_v＝[0_3×3 diag{1,1,1} 0_3×13]

H_k为k时刻的量测矩阵，由位置量测矩阵H_p、速度量测矩阵H_v、航向量测矩阵组成，表示取矩阵的第m行，θ、分别为载体的俯仰角、航向角，可通过下式计算：

其中：q₀、q₁、q₂、q₃为表示载体姿态的四元数；

R_k为k时刻的量测噪声，diag表示矩阵对角化，其中ε_p为位置量测的噪声，ε_v为速度量测的噪声，为角度量测的噪声，上标-1表示矩阵求逆；

4)计算k时刻卡尔曼滤波器的状态估计值

其中，为k时刻滤波器状态量的估计值，为k-1时刻到k时刻的状态量一步预测值，Z_k为k时刻的量测值，Z_p、Z_v、为位置、速度、航向量测值，L_I、λ_I、h_I为k时刻惯导预测的纬度、经度、高度，L_G、λ_G、h_G为k时刻双天线GNSS输出的纬度、经度、高度，V_IE、V_IN、V_IU为k时刻惯导预测的速度在导航系东北天方向上的分量，V_GE、V_GN、V_GU为k时刻双天线GNSS输出的速度在导航系东北天方向上的分量，为k时刻惯导预测的航向，为双天线GNSS输出的航向；

5)计算k时刻卡尔曼滤波器的估计均方误差P_k|k

P_k|k＝(I-K_kH_k)P_k|k-1

其中，P_k|k为k时刻的估计均方误差，I为单位矩阵；

6)基于卡尔曼滤波器，通过量测值Z_k对惯导和双天线GNSS之间的外参进行估计。