[發(fā)明專利]應(yīng)用全天域中性點輔助定向的車輛自主導(dǎo)航方法

在審

• 申請?zhí)枺?/td>: 201510093668.X
• 申請日: 2015-03-03
• 公開（公告）號: CN104713555A
• 公開（公告）日: 2015-06-17
• 發(fā)明（設(shè)計）人: 熊劍;王吉旭;楊祖華;郭杭;馬號;林舟杰;周先贊;徐江穎;楊歡
• 申請（專利權(quán)）人: 南昌大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 南昌市平凡知識產(chǎn)權(quán)代理事務(wù)所 36122
• 代理人: 夏材祥

1.一種應(yīng)用全天域中性點輔助定向的車輛自主導(dǎo)航方法，其特征在于：

1)建立車載中性點導(dǎo)航定向模型：

大氣偏振模式具有圍繞中性點偏振態(tài)表征，以太陽子午線穩(wěn)定對稱分布的特性，建立車輛偏振光導(dǎo)航定向模型，定向原理具體流程為：

a)利用大氣偏振探測裝置，采用宏觀性好易測量的Stokes矢量表征成像全天域光的偏振態(tài)，實現(xiàn)偏振圖像數(shù)據(jù)的獲取、傳輸和預(yù)處理、偏振圖像Stokes參數(shù)、偏振度和偏振方位角信息的計算；

b)結(jié)合中性點在偏振圖像中同偏振度分布與中性線分布間的關(guān)系特征，利用圖像處理算法連續(xù)對圖像中偏振像素單元偏振度為零的點進行跟蹤檢測和識別，確定載體坐標系下偏振圖像中的對稱軸即太陽子午線位置，根據(jù)偏振模式的分布特性，進一步區(qū)分太陽子午線與逆太陽子午線方向，從而獲得載體朝向與太陽子午線之間的夾角；

c)根據(jù)天文歷相關(guān)理論公式，由載體的經(jīng)緯度、觀測年份、日期及時間來確定當?shù)靥柗轿唤牵Y(jié)合載體朝向與太陽子午線之間的夾角，得到與地理正北方向夾角即航向信息；

2)建立改進的車輛動態(tài)數(shù)學(xué)模型：

車輛在道路行駛是一個較為復(fù)雜的運動學(xué)過程，一般情況下，假設(shè)路面是平坦和水平的，車輛轉(zhuǎn)向時簡化的運動學(xué)模型：車輛任意t時刻的狀態(tài)以{x_t?y_t?z_t?ψ_t}來表征，其中，t取非負整數(shù)，{x_t?y_t?z_t}定義為t時刻載體坐標系下的位置值，載體坐標系定義為坐標原點位于載體重心、x軸指向載體橫軸方向、y軸指向載體縱軸方向、z軸指向載體豎軸方向，通常用表示為載體坐標系(b)系，ψ_t表示t時刻載體縱軸方向相對于x軸的方位角，結(jié)合車輛四輪轉(zhuǎn)向模型，Δ表示連續(xù)時刻后輪軸心行駛的距離，ω表示橫擺角速度，ρ表示車體轉(zhuǎn)彎半徑，e表示輪距的一半，L表示軸距，θ表示虛前輪的轉(zhuǎn)向角，這個角度可以近似的代替車輛的前輪擺角，并且假設(shè)在兩個采樣點之間車輛沒有滑動，e和L保持常值，根據(jù)運動學(xué)理論，由各變量之間的幾何關(guān)系可得到以下表征車輛運動學(xué)關(guān)系的方程：

xt+1=xt+((ΔRR+ΔRL)/2)·cos(θt+(ΔRR-ΔRL)/4e)yt+1=yt+((ΔRR+ΔRL)/2)·sin(θt+(ΔRR-ΔRL)/4e)zt+1=ztθt+1=θt+(ΔRR-ΔRL)/2e---(1)]]>

式中，cos表示數(shù)學(xué)中的余弦函數(shù),sin表示數(shù)學(xué)中的正弦函數(shù)，Δ_RR表示t至t+1兩個連續(xù)時刻右后輪行使距離，Δ_RL表示t至t+1兩個連續(xù)時刻左后輪行使距離，且：Δ_RR＝ω×(ρ+e)，Δ_RL＝ω×(ρ-e)；

但是，由于差動里程表在實際工作過程中存在車輪半徑及車體結(jié)構(gòu)尺寸的誤差、運動中的滑動及地面的起伏影響，由以上運動學(xué)方程得到的航向角精度會越來越差，其誤差將交聯(lián)影響到運動學(xué)模型中位置和速度的解算精度，如不加控制，運動學(xué)模型的輸出將可能成為野值，進而失去對慣導(dǎo)的校正作用；

因此，本發(fā)明采用基于車載偏振光傳感器實時給出的高精度航向信息，和慣導(dǎo)輸出一起經(jīng)過組合系統(tǒng)濾波器修正后的航向角來實時校正車輛運動學(xué)模型，改進后的運動學(xué)模型如下：

xt+1=xt+((ΔRR+ΔRL)/2)·cos(ψ~t+(ΔRR-ΔRL)/4e)yt+1=yt+((ΔRR+ΔRL)/2)·sin(ψ~t+(ΔRR-ΔRL)/4e)zt+1=zt---(2)]]>

車輛運動學(xué)模型可以提供車輛位置和行駛方向上的速度信息，但是無法提供與車輛行駛方向垂直方向上的速度信息，理想情況下，車輛在地面上運動且不發(fā)生側(cè)滑的條件下，載體坐標系(b)中y和z軸方向上速度應(yīng)為零，實際應(yīng)用中可用高斯白噪聲表示：

Vby-vy=0Vbz-vz=0---(3)]]>

式中，{V_by?V_bz}是車輛在載體坐標系(b)中y和z軸方向上的速度分量，{v_y?v_z}是均值為零，方差分別為的高斯白噪聲，結(jié)合車輛前行方向速度為V_b，運動學(xué)模型輔助慣性導(dǎo)航系統(tǒng)的速度觀測量在導(dǎo)航坐標系中可表示為V_VDM，導(dǎo)航坐標系定義為坐標原點位于載體重心、x軸指向東、y軸指向北、z軸指向天，即東北天坐標系，通常表示為導(dǎo)航坐標系(n)，下式中的表示載體坐標系(b)至導(dǎo)航坐標系(n)的轉(zhuǎn)換矩陣：

VVDM=CbnVbvyvz---(4)]]>

3)建立車載偏振光導(dǎo)航/動態(tài)數(shù)學(xué)模型/慣性導(dǎo)航組合導(dǎo)航系統(tǒng)：

車載組合導(dǎo)航系統(tǒng)信息源包括慣性導(dǎo)航系統(tǒng)、車載偏振光二維定向傳感系統(tǒng)和車輛動態(tài)數(shù)學(xué)模型組成，系統(tǒng)作用機制為：首先，對導(dǎo)航系統(tǒng)進行初始化設(shè)置，然后，慣性導(dǎo)航系統(tǒng)直接參與濾波，車載偏振光二維導(dǎo)航定向系統(tǒng)和車輛動態(tài)數(shù)學(xué)模型解算出的導(dǎo)航信息經(jīng)過慣性導(dǎo)航系統(tǒng)提供的平臺轉(zhuǎn)換矩陣轉(zhuǎn)換到導(dǎo)航坐標系中的值，與慣性導(dǎo)航系統(tǒng)的輸出一起，再根據(jù)多源信息融合技術(shù)進行系統(tǒng)最優(yōu)融合估計，利用濾波值一方面對車輛動態(tài)數(shù)學(xué)模型中的航向角信息進行實時修正，另一方面對慣性導(dǎo)航系統(tǒng)進行閉環(huán)反饋校正，最后，輸出高精度導(dǎo)航數(shù)據(jù)；

4)建立車載組合導(dǎo)航系統(tǒng)狀態(tài)方程：

組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型—狀態(tài)方程如下：

X.=FX+GW---(5)]]>

式中，X為系統(tǒng)狀態(tài)矢量：

其中，{δL?δλ?δh}為緯、經(jīng)、高位置誤差，{δV_E?δV_N?δV_U}為東、北、天速度誤差，為東、北、天平臺誤差角，{ε_bx?ε_by?ε_bz?ε_rx?ε_ry?ε_rz}為陀螺儀漂移沿載體坐標系分量，為加速度計偏置沿載體坐標系分量，因為陸地車輛導(dǎo)航時姿態(tài)誤差角為小角，可以不考慮高度通道；

W為系統(tǒng)白噪聲隨機誤差矢量：

W＝[ω_gx?ω_gy?ω_gz?ω_rx?ω_ry?ω_rz?ω_ax?ω_ay?ω_az]^T???(7)

其中，{ω_gx?ω_gy?ω_gz}為陀螺儀白噪聲，{ω_rx?ω_ry?ω_rz}為陀螺儀一階馬爾可夫過程隨機噪聲，{ω_ax?ω_ay?ω_az}為加速度計一階馬爾可夫過程；

F和G分別為系統(tǒng)方程狀態(tài)系數(shù)矩陣和誤差系數(shù)矩陣：

F=FNFS0FM,G=Cbn03×303×309×309×309×303×3I3×303×303×303×3I3×3---(8)]]>

其中，0_m×n為m×n維零矩陣：m和n取正整數(shù)，I_m×n為m×n維單位矩陣：m和n取正整數(shù)，F(xiàn)_N為位置、速度及平臺角度誤差參數(shù)對應(yīng)的系統(tǒng)陣，F(xiàn)_S和F_M分別為：

FS=CbnCbn03×303×303×3Cbn03×303×303×3]]>

其中，{T_gx?T_gy?T_gz}與{T_ax?T_ay?T_az}為相關(guān)時間參數(shù)；

按以上公式，系統(tǒng)狀態(tài)方程取線性方程；

5)建立車載組合導(dǎo)航系統(tǒng)量測方程：

組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型—量測方程：

組合框架中，量測輸入值有三組：位置、速度及航向角誤差觀測量{δP?δV?δψ}，即由車輛運動學(xué)模型得到的位置P_VDM和前向速度，約束條件得到的另外兩個方向上的速度信息V_VDM與慣性導(dǎo)航系統(tǒng)輸出的位置P_SINS、速度V_SINS之差作為位置、速度誤差觀測量；由車載偏振光測角傳感器得到航向角信息ψ_PL與慣性導(dǎo)航系統(tǒng)輸出的航向角ψ_SINS之差作為航向角誤差觀測量，構(gòu)建量測方程如下：

Zp,v,ψ=δPδVδψ=PSINS-PVDMVSINS-VVDMψSINS-ψPL=Hp,v,ψX+ηp,v,ψ---(10)]]>

式中，H_p,v,ψ為量測轉(zhuǎn)換矩陣；

Hp,v,ψ=03×6diagRMRNcosL103×903×3diag11103×12-1cosθ000000sinψsinθcosψθcos-cosθ03×303×12---(11)]]>

其中，cos表示數(shù)學(xué)中的余弦函數(shù),sin表示數(shù)學(xué)中的正弦函數(shù)，0_m×n為m×n維零矩陣：m和n取正整數(shù)，diag[1×n]為n維對角陣：n取正整數(shù)，R_M和R_N分別為子午圈和卯酉圈曲率半徑，L為緯度，ψ為航向角，θ為俯仰角，η_p,v,ψ為量測噪聲，其值與器件的仿真精度有關(guān)，設(shè)置為隨機白噪聲，測噪聲協(xié)方差陣為R＝diag[R_p?R_v?R_ψ]，其中，{R_p?R_v?R_ψ}分別是位置、速度及航向觀測量的噪聲方差陣，由以上兩式可知，量測方程為線性方程；

故用于組合系統(tǒng)濾波的系統(tǒng)狀態(tài)方程與量測方程的離散形式為：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

????????????????????????????????????????(12)

Z_k＝H_kX_k+V_k

式中，Φk/k-1=Σn=0∞[FkT]n/n!,Γk-1={Σn=1∞[1n!(FkT)n-1]}GkT,]]>其中，k/(k-1)代表前一時刻到當前時刻的遞推預(yù)測，k取非負整數(shù)的時間參數(shù)，T為迭代周期取非負整數(shù)，n為方程離散化程度參數(shù)取正整數(shù)，F(xiàn)_k和G_k分別為時刻k的系統(tǒng)方程狀態(tài)系數(shù)矩陣和誤差系數(shù)矩陣，W_k-1為k-1時刻的系統(tǒng)白噪聲隨機誤差矢量，H_k為時刻k的系統(tǒng)量測轉(zhuǎn)換矩陣，V_k為k時刻的量測噪聲矩陣；

最后，對以上系統(tǒng)狀態(tài)方程與量測方程采用標準線性卡爾曼濾波算法過程，進行系統(tǒng)狀態(tài)的遞推估計。