本發(fā)明屬于衛(wèi)星定位與導(dǎo)航技術(shù)領(lǐng)域，涉及一種低成本雙天線GNSS/AHRS組合測(cè)姿方法。

背景技術(shù)：

全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)是我國(guó)自主建設(shè)的全球衛(wèi)星導(dǎo)航系統(tǒng)，能夠?yàn)榈厍虮砻婧徒乜臻g的廣大用戶提供全天時(shí)、全天候、高精度的定位、導(dǎo)航和授時(shí)服務(wù)，已廣泛應(yīng)用于國(guó)防、海陸空交通運(yùn)輸、測(cè)繪、移動(dòng)通信、電力、電子金融、精細(xì)農(nóng)業(yè)和減災(zāi)救災(zāi)等領(lǐng)域，是拓展人類活動(dòng)和促進(jìn)社會(huì)發(fā)展的重要空間基礎(chǔ)設(shè)施。

GNSS精密測(cè)向技術(shù)是全球衛(wèi)星導(dǎo)航系統(tǒng)提供精密位置服務(wù)的關(guān)鍵技術(shù)之一，目前已廣泛應(yīng)用于駕考、精準(zhǔn)農(nóng)業(yè)、無(wú)人機(jī)等機(jī)械控制。它利用GNSS測(cè)向技術(shù)，以載體上的兩個(gè)接收機(jī)采集的載波相位和偽距數(shù)據(jù)作為主要觀測(cè)值來(lái)進(jìn)行差分解算，并估計(jì)載波相位的整周模糊度，可以實(shí)時(shí)獲得高精度的航向信息，極大的提高了作業(yè)效率，降低作業(yè)成本。但是基于目前的GNSS測(cè)向產(chǎn)品一般基于采用雙頻方案，其硬件成本相對(duì)較高，從而限制了其應(yīng)用范圍以及產(chǎn)業(yè)化推廣。

技術(shù)實(shí)現(xiàn)要素：

有鑒于此，本發(fā)明的目的在于提供一種低成本雙天線GNSS/AHRS組合測(cè)姿方法，該方法能夠?qū)崟r(shí)矯正AHRS傳感器系統(tǒng)性偏差，并提高系統(tǒng)的穩(wěn)定性與可靠性。

為達(dá)到上述目的，本發(fā)明提供如下技術(shù)方案：

一種低成本雙天線GNSS/AHRS組合測(cè)姿方法，該方法基于低成本GNSS模塊以及9軸AHRS傳感器，利用GNSS單頻偽距與載波相位觀測(cè)值，實(shí)時(shí)解算載體的位置與速度信息，結(jié)合AHRS加速度、角速度以及磁傳感器數(shù)據(jù)，采用Kalman濾波技術(shù)實(shí)時(shí)估計(jì)載體的姿態(tài)信息，并基于虛擬觀測(cè)值的最小二乘平差，實(shí)時(shí)解算載波相位的模糊度，修正載體的航向。

進(jìn)一步，該方法具體包括以下步驟：

步驟一：采用單點(diǎn)定位估計(jì)主天線位置；

原始的偽距觀測(cè)值為：

其中，為原始的偽距觀測(cè)值，ρ^s為接收機(jī)到衛(wèi)星的距離(包含誤差源，如相對(duì)論、地 球固體潮、衛(wèi)星相位中心、衛(wèi)星硬件延遲等)，t_r，g為接收機(jī)鐘差，g表示不同得衛(wèi)星導(dǎo)航系統(tǒng)，I^s為第一頻段載波的電離層延遲，τ^s為對(duì)流層延遲，ε表示偽距的觀測(cè)噪聲；

利用模型改正對(duì)流層，電離層誤差影響，則觀測(cè)方程可以表示為：

對(duì)上式線性化，采用最小二乘法估計(jì)即可估計(jì)接收機(jī)位置；

步驟二：原始的多普勒觀測(cè)值估計(jì)主天線速度；

原始的多普勒觀測(cè)值為：

其中，為原始的偽距觀測(cè)值，λ_g為衛(wèi)星導(dǎo)航系統(tǒng)對(duì)應(yīng)的載波相位波長(zhǎng)，為接收機(jī)到衛(wèi)星的距離變化率，為接收機(jī)鐘差漂移，為衛(wèi)星鐘差漂移，為第一頻段載波的電離層延遲變化，為對(duì)流層延遲變化，∈表示多普勒觀測(cè)噪聲；

對(duì)距離變化率展開可得：

式中v^s為衛(wèi)星的速度，v_r為接收機(jī)速度，I^s為衛(wèi)星到接收機(jī)的方向矢量；

則可得：

式中G^s為觀測(cè)系數(shù)矩陣，則采用最小二乘解算即可獲得速度信息；

步驟三：基于Kalman濾波技術(shù)AHRS姿態(tài)解算；

基于步驟一與步驟二獲得的位置與速度，以及AHRS提供的加速度、角速度、磁強(qiáng)度數(shù)據(jù)，建立GNSS/AHRS融合Kalman濾波方程：

狀態(tài)方程與過程方程如下：

X_k＝φX_k-1+Γw_k-1

z_k＝HX_k+v_k

采用Kalman濾波技術(shù)實(shí)時(shí)估計(jì)載體的姿態(tài)信息；

步驟四：基線約束模糊度解算；

基于步驟三解算的三維姿態(tài)信息以及已知的基線長(zhǎng)度，實(shí)時(shí)解算基線分量：

式中l(wèi)為兩個(gè)天線的基線長(zhǎng)度，γ為俯仰角，ε為偏航角，為當(dāng)?shù)貣|北天坐標(biāo)系基線分量；

利用解算的基線分量，基于虛擬觀測(cè)值建立雙差載波相位與偽距觀測(cè)方程為：

式為雙差偽距觀測(cè)值，為雙差載波相位觀測(cè)值，A為觀測(cè)值系數(shù)矩陣，m＝(e，n，u)^T，b為載波相位整周模糊度矢量；采用最小二乘技術(shù)，即可計(jì)算模糊度浮點(diǎn)解及其方差協(xié)方差陣采用LAMBDA方法解算模糊度，獲得模糊度的固定解；

式中b為整數(shù)模糊度候選矢量，為最優(yōu)模糊度解算結(jié)果；

步驟五：固定模糊度更新姿態(tài)信息；

采用經(jīng)典的Ratio檢驗(yàn)方法，如果模糊度通過檢驗(yàn)，則更新基線向量：

式中為基線向量與模糊度的協(xié)方差，為浮點(diǎn)解基線向量，為固定解基線向量；

計(jì)算更新后的航向與俯仰信息：

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

本發(fā)明的有益效果在于：本發(fā)明提供的一種低成本雙天線GNSS/AHRS組合測(cè)姿方法基于AHRS實(shí)時(shí)解算三維姿態(tài)信息，利用虛擬觀測(cè)值的最小二乘方法，實(shí)時(shí)解算模糊度浮點(diǎn)解，實(shí)現(xiàn)穩(wěn)健的高精度GNSS測(cè)向；該方法能夠?qū)崟r(shí)矯正AHRS傳感器系統(tǒng)性偏差，并提高系統(tǒng)的穩(wěn)定性與可靠性。

附圖說(shuō)明

為了使本發(fā)明的目的、技術(shù)方案和有益效果更加清楚，本發(fā)明提供如下附圖進(jìn)行說(shuō)明：

圖1為本發(fā)明所述方法流程圖。

具體實(shí)施方式

下面將結(jié)合附圖，對(duì)本發(fā)明的優(yōu)選實(shí)施例進(jìn)行詳細(xì)的描述。

圖1為本發(fā)明所述方法流程圖，如圖所示，本發(fā)明提供的低成本雙天線GNSS/AHRS測(cè)向方法具體包括以下步驟：

步驟一：采用單點(diǎn)定位估計(jì)主天線位置；

原始的偽距觀測(cè)值為：

其中，為原始的偽距觀測(cè)值，ρ^s為接收機(jī)到衛(wèi)星的距離(包含誤差源，如相對(duì)論、地球固體潮、衛(wèi)星相位中心、衛(wèi)星硬件延遲等)，t_r，g為接收機(jī)鐘差，g表示不同得衛(wèi)星導(dǎo)航系統(tǒng)，I^s為第一頻段載波的電離層延遲，τ^s為對(duì)流層延遲，ε表示偽距的觀測(cè)噪聲；

利用模型改正對(duì)流層，電離層誤差影響，則觀測(cè)方程可以表示為：

對(duì)上式線性化，采用最小二乘法估計(jì)即可估計(jì)接收機(jī)位置。

步驟二：原始的多普勒觀測(cè)值估計(jì)主天線速度；

原始的多普勒觀測(cè)值為：

其中，為原始的偽距觀測(cè)值，λ_g為衛(wèi)星導(dǎo)航系統(tǒng)對(duì)應(yīng)的載波相位波長(zhǎng)，為接收機(jī)到衛(wèi)星的距離變化率，為接收機(jī)鐘差漂移，為衛(wèi)星鐘差漂移，為第一頻段載波的電離層延遲變化，為對(duì)流層延遲變化，∈表示多普勒觀測(cè)噪聲；

對(duì)距離變化率展開可得：

式中v^s為衛(wèi)星的速度，v_r為接收機(jī)速度，I^s為衛(wèi)星到接收機(jī)的方向矢量。

則可得：

式中G^s為觀測(cè)系數(shù)矩陣，則采用最小二乘解算即可獲得速度信息。

步驟三：建立AHRS Kalman濾波模型；

忽略導(dǎo)航坐標(biāo)系相對(duì)于慣性坐標(biāo)系和導(dǎo)航坐標(biāo)系相對(duì)于地球坐標(biāo)系的旋轉(zhuǎn)加速度影響，加速度計(jì)與陀螺儀的測(cè)量方程為：

式中ω_m＝[ω_mx ω_my ω_mz]^T為載體坐標(biāo)系陀螺儀測(cè)量值；

式中a_m＝[a_mx a_my a_mz]^T為載體坐標(biāo)系加速度計(jì)測(cè)量值；

式中ω＝[ω_x ω_y ω_z]^T為載體坐標(biāo)系實(shí)際旋轉(zhuǎn)角速度；

式中a＝[a_x a_y a_z]^T為載體坐標(biāo)系實(shí)際加速度；

式中w_ω，w_a分別為陀螺儀與加速度計(jì)的測(cè)量噪聲；

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

式中為載體坐標(biāo)系到地固系轉(zhuǎn)換矩陣；

式中g(shù)＝[0 O g]^T為北東地重力分量；

選取狀態(tài)向量：

其中P＝[P_n P_e P_d]^T為地固坐標(biāo)系北東地坐標(biāo)；

V＝[V_n V_e V_d]^T為地固坐標(biāo)系北東地速度；

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

b_ω＝[b_ωx b_ωy b_ωz]^T為陀螺儀偏差。

然后建立系統(tǒng)的狀態(tài)方程為：

式中：

式中w_b為隨機(jī)游走過程噪聲。

對(duì)狀態(tài)方程線性化可得：

式中：

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₁)

對(duì)系統(tǒng)偏差求導(dǎo)：

建立系統(tǒng)的觀測(cè)方程為：

Kalman濾波的觀測(cè)量y包含三維位置、速度與磁強(qiáng)度數(shù)據(jù)。

對(duì)觀測(cè)方程線性化：

式中：

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濾波的狀態(tài)方程與過程方程為：

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

z_k＝HX_k+v_k (16)

式中：

φ＝I+Ft (17)

Γ＝Gt (18)

基于以上線性化展開，即可采用擴(kuò)展Kalman濾波估計(jì)載體姿態(tài)，其中一步預(yù)測(cè)方程為：

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

計(jì)算公式(19)時(shí)，先采用4階龍格庫(kù)塔數(shù)值積分算法對(duì)公式(8)進(jìn)行積分，然后加上上一個(gè)歷元的狀態(tài)向量，狀態(tài)方差陣預(yù)測(cè)，采用擴(kuò)展Kalman濾波一步預(yù)測(cè)方法。

狀態(tài)更新方程如下：

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)

步驟四：基線約束模糊度解算；

基于步驟三解算的三維姿態(tài)信息以及已知的基線長(zhǎng)度，實(shí)時(shí)解算基線分量：

式中l(wèi)為兩個(gè)天線的基線長(zhǎng)度，γ為俯仰角，ε為偏航角，為當(dāng)?shù)貣|北天坐標(biāo)系基線分量。

利用解算的基線分量，建立雙差載波相位與偽距觀測(cè)方程為：

式為雙差偽距觀測(cè)值，為雙差載波相位觀測(cè)值，A為觀測(cè)值系數(shù)矩陣，m＝(e，n，u）^T，為基線分量的虛擬觀測(cè)值，b為載波相位整周模糊度矢量。

采用最小二乘技術(shù)，即可計(jì)算模糊度浮點(diǎn)解及其方差協(xié)方差陣采用LAMBDA方法解算模糊度，獲得模糊度的固定解。

式中b為整數(shù)模糊度候選矢量，為最優(yōu)模糊度解算結(jié)果。

步驟五：固定模糊度更新姿態(tài)信息。

采用經(jīng)典的Ratio檢驗(yàn)方法，如果模糊度通過檢驗(yàn)，則更新基線向量：

式中為基線向量與模糊度的協(xié)方差，為浮點(diǎn)解基線向量，為固定解基線向量。

計(jì)算更新后的航向與俯仰信息：

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

最后說(shuō)明的是，以上優(yōu)選實(shí)施例僅用以說(shuō)明本發(fā)明的技術(shù)方案而非限制，盡管通過上述優(yōu)選實(shí)施例已經(jīng)對(duì)本發(fā)明進(jìn)行了詳細(xì)的描述，但本領(lǐng)域技術(shù)人員應(yīng)當(dāng)理解，可以在形式上和細(xì)節(jié)上對(duì)其作出各種各樣的改變，而不偏離本發(fā)明權(quán)利要求書所限定的范圍。