CN106443746A - 一种低成本双天线gnss/ahrs组合测姿方法 - Google Patents

Abstract

本发明涉及一种低成本双天线GNSS/AHRS组合测姿方法，属于卫星定位与导航技术领域。该方法基于低成本GNSS模块以及9轴AHRS传感器，利用GNSS单频伪距与载波相位观测值，实时解算载体的位置与速度信息，结合AHRS加速度、角速度以及磁传感器数据，采用Kalman滤波技术实时估计载体的姿态信息，并基于虚拟观测值的最小二乘平差，实时解算载波相位的模糊度，修正载体的航向。本方法能够实时矫正AHRS传感器系统性偏差，并提高系统的稳定性与可靠性。

Description

一种低成本双天线GNSS/AHRS组合测姿方法

技术领域

本发明属于卫星定位与导航技术领域，涉及一种低成本双天线GNSS/AHRS组合测姿方法。

背景技术

全球卫星导航系统(GNSS)是我国自主建设的全球卫星导航系统，能够为地球表面和近地空间的广大用户提供全天时、全天候、高精度的定位、导航和授时服务，已广泛应用于国防、海陆空交通运输、测绘、移动通信、电力、电子金融、精细农业和减灾救灾等领域，是拓展人类活动和促进社会发展的重要空间基础设施。

GNSS精密测向技术是全球卫星导航系统提供精密位置服务的关键技术之一，目前已广泛应用于驾考、精准农业、无人机等机械控制。它利用GNSS测向技术，以载体上的两个接收机采集的载波相位和伪距数据作为主要观测值来进行差分解算，并估计载波相位的整周模糊度，可以实时获得高精度的航向信息，极大的提高了作业效率，降低作业成本。但是基于目前的GNSS测向产品一般基于采用双频方案，其硬件成本相对较高，从而限制了其应用范围以及产业化推广。

发明内容

有鉴于此，本发明的目的在于提供一种低成本双天线GNSS/AHRS组合测姿方法，该方法能够实时矫正AHRS传感器系统性偏差，并提高系统的稳定性与可靠性。

为达到上述目的，本发明提供如下技术方案：

一种低成本双天线GNSS/AHRS组合测姿方法，该方法基于低成本GNSS模块以及9轴AHRS传感器，利用GNSS单频伪距与载波相位观测值，实时解算载体的位置与速度信息，结合AHRS加速度、角速度以及磁传感器数据，采用Kalman滤波技术实时估计载体的姿态信息，并基于虚拟观测值的最小二乘平差，实时解算载波相位的模糊度，修正载体的航向。

进一步，该方法具体包括以下步骤：

步骤一：采用单点定位估计主天线位置；

原始的伪距观测值为：

其中，为原始的伪距观测值，ρ^s为接收机到卫星的距离(包含误差源，如相对论、地 球固体潮、卫星相位中心、卫星硬件延迟等)，t_r，g为接收机钟差，g表示不同得卫星导航系统，I^s为第一频段载波的电离层延迟，τ^s为对流层延迟，ε表示伪距的观测噪声；

利用模型改正对流层，电离层误差影响，则观测方程可以表示为：

对上式线性化，采用最小二乘法估计即可估计接收机位置；

步骤二：原始的多普勒观测值估计主天线速度；

原始的多普勒观测值为：

其中，为原始的伪距观测值，λ_g为卫星导航系统对应的载波相位波长，为接收机到卫星的距离变化率，为接收机钟差漂移，为卫星钟差漂移，为第一频段载波的电离层延迟变化，为对流层延迟变化，∈表示多普勒观测噪声；

对距离变化率展开可得：

式中v^s为卫星的速度，v_r为接收机速度，I^s为卫星到接收机的方向矢量；

则可得：

式中G^s为观测系数矩阵，则采用最小二乘解算即可获得速度信息；

步骤三：基于Kalman滤波技术AHRS姿态解算；

基于步骤一与步骤二获得的位置与速度，以及AHRS提供的加速度、角速度、磁强度数据，建立GNSS/AHRS融合Kalman滤波方程：

状态方程与过程方程如下：

X_k＝φX_k-1+Γw_k-1

z_k＝HX_k+v_k

采用Kalman滤波技术实时估计载体的姿态信息；

步骤四：基线约束模糊度解算；

基于步骤三解算的三维姿态信息以及已知的基线长度，实时解算基线分量：

式中l为两个天线的基线长度，γ为俯仰角，ε为偏航角，为当地东北天坐标系基线分量；

利用解算的基线分量，基于虚拟观测值建立双差载波相位与伪距观测方程为：

式为双差伪距观测值，为双差载波相位观测值，A为观测值系数矩阵，m＝(e，n，u)^T，b为载波相位整周模糊度矢量；采用最小二乘技术，即可计算模糊度浮点解及其方差协方差阵采用LAMBDA方法解算模糊度，获得模糊度的固定解；

式中b为整数模糊度候选矢量，为最优模糊度解算结果；

步骤五：固定模糊度更新姿态信息；

采用经典的Ratio检验方法，如果模糊度通过检验，则更新基线向量：

式中为基线向量与模糊度的协方差，为浮点解基线向量，为固定解基线向量；

计算更新后的航向与俯仰信息：

利用更新后的航向信息修正磁传感器的地磁航向，获得地理北指向。

本发明的有益效果在于：本发明提供的一种低成本双天线GNSS/AHRS组合测姿方法基于AHRS实时解算三维姿态信息，利用虚拟观测值的最小二乘方法，实时解算模糊度浮点解，实现稳健的高精度GNSS测向；该方法能够实时矫正AHRS传感器系统性偏差，并提高系统的稳定性与可靠性。

附图说明

为了使本发明的目的、技术方案和有益效果更加清楚，本发明提供如下附图进行说明：

图1为本发明所述方法流程图。

具体实施方式

下面将结合附图，对本发明的优选实施例进行详细的描述。

图1为本发明所述方法流程图，如图所示，本发明提供的低成本双天线GNSS/AHRS测向方法具体包括以下步骤：

步骤一：采用单点定位估计主天线位置；

原始的伪距观测值为：

其中，为原始的伪距观测值，ρ^s为接收机到卫星的距离(包含误差源，如相对论、地球固体潮、卫星相位中心、卫星硬件延迟等)，t_r，g为接收机钟差，g表示不同得卫星导航系统，I^s为第一频段载波的电离层延迟，τ^s为对流层延迟，ε表示伪距的观测噪声；

利用模型改正对流层，电离层误差影响，则观测方程可以表示为：

对上式线性化，采用最小二乘法估计即可估计接收机位置。

步骤二：原始的多普勒观测值估计主天线速度；

原始的多普勒观测值为：

其中，为原始的伪距观测值，λ_g为卫星导航系统对应的载波相位波长，为接收机到卫星的距离变化率，为接收机钟差漂移，为卫星钟差漂移，为第一频段载波的电离层延迟变化，为对流层延迟变化，∈表示多普勒观测噪声；

对距离变化率展开可得：

式中v^s为卫星的速度，v_r为接收机速度，I^s为卫星到接收机的方向矢量。

则可得：

式中G^s为观测系数矩阵，则采用最小二乘解算即可获得速度信息。

步骤三：建立AHRS Kalman滤波模型；

忽略导航坐标系相对于惯性坐标系和导航坐标系相对于地球坐标系的旋转加速度影响，加速度计与陀螺仪的测量方程为：

式中ω_m＝[ω_mx ω_my ω_mz]^T为载体坐标系陀螺仪测量值；

式中a_m＝[a_mx a_my a_mz]^T为载体坐标系加速度计测量值；

式中ω＝[ω_x ω_y ω_z]^T为载体坐标系实际旋转角速度；

式中a＝[a_x a_y a_z]^T为载体坐标系实际加速度；

式中w_ω，w_a分别为陀螺仪与加速度计的测量噪声；

式中b_ω＝[b_ωx b_ωy b_ωz]^T为陀螺仪的零偏；

式中为载体坐标系到地固系转换矩阵；

式中g＝[0 O g]^T为北东地重力分量；

选取状态向量：

其中P＝[P_n P_e P_d]^T为地固坐标系北东地坐标；

V＝[V_n V_e V_d]^T为地固坐标系北东地速度；

q＝[q₀ q₁ q₂ q₃]^T为四元素；

b_ω＝[b_ωx b_ωy b_ωz]^T为陀螺仪偏差。

然后建立系统的状态方程为：

式中：

式中w_b为随机游走过程噪声。

对状态方程线性化可得：

式中：

A＝[O_3×3 I_3×3]

F_Vq0＝2(q₀a_mx-q₃a_my+q₂a_mz)

F_Vq1＝2(q₁a_mx+q₂a_my+q₃a_mz)

F_Vq2＝2(-q₂a_mx+q₁a_my+q₀a_mz)

F_Vq3＝2(-q₃a_mx-q₀a_my+q₁)

对系统偏差求导：

建立系统的观测方程为：

Kalman滤波的观测量y包含三维位置、速度与磁强度数据。

对观测方程线性化：

式中：

H_Bq0＝2(q₀B_ex+q₃B_ey-q₂B_ez)

H_Bq1＝2(q₁B_ex+q₂B_ey+q₃B_ez)

H_Bq2＝2(-q₂B_ex+q₁B_ey-q₀B_ez)

H_Bq3＝2(-q₃B_ex+q₀B_ey+q₁B_ez)

建立Kalman滤波的状态方程与过程方程为：

X_k＝φX_k-1+Γw_k-1 (15)

z_k＝HX_k+v_k (16)

式中：

φ＝I+Ft (17)

Γ＝Gt (18)

基于以上线性化展开，即可采用扩展Kalman滤波估计载体姿态，其中一步预测方程为：

P_k＝(I+Ft)P_k-1(I+Ft)^T+t²GQG^T (20)

计算公式(19)时，先采用4阶龙格库塔数值积分算法对公式(8)进行积分，然后加上上一个历元的状态向量，状态方差阵预测，采用扩展Kalman滤波一步预测方法。

状态更新方程如下：

K＝P_kH^T(HP_kH^T+R)^-1 (21)

X_k＝X_k+K(z_k-y_k) (22)

P_k＝P_k-KHP_k (23)

步骤四：基线约束模糊度解算；

基于步骤三解算的三维姿态信息以及已知的基线长度，实时解算基线分量：

式中l为两个天线的基线长度，γ为俯仰角，ε为偏航角，为当地东北天坐标系基线分量。

利用解算的基线分量，建立双差载波相位与伪距观测方程为：

式为双差伪距观测值，为双差载波相位观测值，A为观测值系数矩阵，m＝(e，n，u）^T，为基线分量的虚拟观测值，b为载波相位整周模糊度矢量。

采用最小二乘技术，即可计算模糊度浮点解及其方差协方差阵采用LAMBDA方法解算模糊度，获得模糊度的固定解。

式中b为整数模糊度候选矢量，为最优模糊度解算结果。

步骤五：固定模糊度更新姿态信息。

采用经典的Ratio检验方法，如果模糊度通过检验，则更新基线向量：

式中为基线向量与模糊度的协方差，为浮点解基线向量，为固定解基线向量。

计算更新后的航向与俯仰信息：

利用更新后的航向信息修正磁传感器的地磁航向，获得地理北指向。

最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明权利要求书所限定的范围。

Claims (2)

1.一种低成本双天线GNSS/AHRS组合测姿方法，其特征在于：该方法基于低成本GNSS模块以及9轴AHRS传感器，利用GNSS单频伪距与载波相位观测值，实时解算载体的位置与速度信息，结合AHRS加速度、角速度以及磁传感器数据，采用Kalman滤波技术实时估计载体的姿态信息，并基于虚拟观测值的最小二乘平差，实时解算载波相位的模糊度，修正载体的航向。

2.根据权利要求1所述的一种低成本双天线GNSS/AHRS组合测姿方法，其特征在于：该方法包括以下步骤：

步骤一：采用单点定位估计主天线位置；

原始的伪距观测值为：

$P_g^s={\mathrm{\ρ}}^s+t_{r,g}+I^s+{\mathrm{\τ}}^s+\mathrm{\ϵ}$

其中，为原始的伪距观测值，ρ^s为接收机到卫星的距离，t_r，g为接收机钟差，g表示不同得卫星导航系统，I^s为第一频段载波的电离层延迟，τ^s为对流层延迟，ε表示伪距的观测噪声；

利用模型改正对流层，电离层误差影响，则观测方程可以表示为：

$P_g^s=\sqrt{{(x^s-x)}^2+{(y^s-y)}^2+{(z^s-z)}^2}+t_{r,g}$

对上式线性化，采用最小二乘法估计即可估计接收机位置；

步骤二：原始的多普勒观测值估计主天线速度；

原始的多普勒观测值为：

${\mathrm{\λ}}_g{D}_g^s={\stackrel{\·}{\mathrm{\ρ}}}^s+{\stackrel{\·}{t}}_{r,g}-{\stackrel{\·}{t}}^s+{\stackrel{\·}{I}}^s+{\stackrel{\·}{\mathrm{\τ}}}^s+\∈$

其中，为原始的伪距观测值，λ_g为卫星导航系统对应的载波相位波长，为接收机到卫星的距离变化率，为接收机钟差漂移，为卫星钟差漂移，为第一频段载被的电离层延迟变化，为对流层延迟变化，∈表示多普勒观测噪声；

对距离变化率展开可得：

$D_g^s=(v^s-v_r)\·I^s+{\stackrel{\·}{t}}_{r,g}-{\stackrel{\·}{t}}^s+\∈$

式中v^s为卫星的速度，v_r为接收机速度，I^s为卫星到接收机的方向矢量；

则可得：

$D_g^s+{\stackrel{\·}{t}}^s-v^s{I}^s=G^s\left[\begin{array}{c}{v}_r\\ {\stackrel{\·}{t}}_{r,g}\end{array}\right]+\∈$

式中G^s为观测系数矩阵，则采用最小二乘解算即可获得速度信息；

步骤三：基于Kalman滤波技术AHRS姿态解算；

基于步骤一与步骤二获得的位置与速度，以及AHRS提供的加速度、角速度、磁强度数据，建立GNSS/AHRS融合Kalman滤波方程：

状态方程与过程方程如下：

z_k＝HX_k+v_k

式中下标k-1，k为上个历元和当前历元标记，X为Kalman滤波的状态向量，φ为状态一步预测矩阵，为过程噪声系数矩阵，w为过程噪声，z为观测值，H为观测值系数矩阵，v为观测值噪声；

采用Kalman滤被技术实时估计载体的姿态信息；

步骤四：基线约束模糊度解算；

基于步骤三解算的三维姿态信息以及已知的基线长度，实时解算基线分量：

$\tilde{u}=l\×\sin \left(\mathrm{\γ}\right)$

$\tilde{n}=l\×cos\left(\mathrm{\γ}\right)\×\sin \left(\mathrm{\ϵ}\right)$

$\tilde{e}=l\×cos\left(\mathrm{\γ}\right)\×cos\left(\mathrm{\ϵ}\right)$

式中l为两个天线的基线长度，γ为俯仰角，ε为偏航角，为当地东北天坐标系基线分量；

利用解算的基线分量，基于虚拟观测值建立双差载波相位与伪距观测方程为：

式为双差伪距观测值，为双差载被相位观测值，A为观测值系数矩阵，m＝(e，n，u)^T，b为载波相位整周模糊度矢量；采用最小二乘技术，即可计算模糊度浮点解及其方差协方差阵采用方法解算模糊度，获得模糊度的固定解；

$\stackrel{\‾}{b}=\arg \underset{b\∈Z}{\min }(b-\hat{b})Q_{\hat{b}\hat{b}}^{-1}(b-\hat{b})$

式中b为整数模糊度候选矢量，为最优模糊度解算结果；

步骤五：固定模糊度更新姿态信息；

采用经典的Ratio检验方法，如果模糊度通过检验，则更新基线向量：

$\stackrel{\‾}{m}=\hat{m}+Q_{\hat{m}\hat{b}}{Q}_{\hat{b}\hat{b}}^{-1}\×(\stackrel{\‾}{b}-\hat{b})$

式中为基线向量与模糊度的协方差，为浮点解基线向量，为固定解基线向量；

计算更新后的航向与俯仰信息：

$\stackrel{\‾}{\mathrm{\ϵ}}=atan(\stackrel{\‾}{e}/\stackrel{\‾}{u})$

式中为计算获得的航向角；利用更新后的航向信息修正磁传感器的地磁航向，获得地理北指向。