[發(fā)明專利]一種室內自校準導航定位方法

在審

• 申請?zhí)枺?/td>: 202110387718.0
• 申請日: 2021-04-12
• 公開（公告）號: CN113091748A
• 公開（公告）日: 2021-07-09
• 發(fā)明（設計）人: 傅惠民;崔軼
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 北京慧泉知識產權代理有限公司 11232
• 代理人: 王順榮;唐愛華

1.一種室內自校準導航定位方法，它包含兩個方面，即室內自校準定位方法和室內自校準導航方法，其特征在于：

室內自校準定位方法包括：

1)室內信號強度路徑損耗自校準模型及方法

設在位置(x,y)處能夠接收到n個信號，信號強度與距離之間的關系用式(1)描述：

S_i＝S_0i-10η_ilg(r_i/r_0i)+d_si+v_si (1)

式中，S_i為第i個熱點AP在距離r_i處的信號強度，r_0i為參考距離，S_0i為參考距離處的信號強度，η_i為信號強度的路徑損耗系數(shù)，v_si為零均值的正態(tài)隨機變量，即d_si為因墻壁障礙物引起的信號損耗而帶來的未知輸入；設前n₀個信號是在視距環(huán)境中傳播，n₀≥3，此時取d_si＝0；后n-n₀個信號是在非視距環(huán)境中傳播；

設第i個AP的位置坐標為(X_i,Y_i)；首先，對信號強度進行零批次測量，得到測量值S_i,0；由于當i≤n₀時，d_si＝0，所以根據式(1)求得距離r_i的初步估計為

進而得到位置(x,y)的初步估計為

式中：H₀為量測矩陣，Z₀為量測向量；

然后，由下式計算式(1)后n-n₀個方程中距離r_i的估計值

并將r_i的估計值代入式(1)，求得未知輸入d_si的估計值為

2)基于接收信號的強度指示RSSI的室內自校準定位方法

由式(1)得到，距離越遠，距離估計值的分散性就越大，于是建立距離量測方程為

式中v_ri為零均值的正態(tài)隨機變量，即方差由式(9)給出：

式中：σ_si是式(1)中v_si的方差，為距離r_i的估計，由式(10)給出：

將式(8)在處進行泰勒展開并保留一階項，整理后得到方程組為

采用加權最小二乘原理，求解方程組式(11)，最終得到位置坐標(x,y)的估計為：

式中：Z為量測向量，H為量測矩陣，R為噪聲協(xié)方差矩陣；

Z＝(Z₁,Z₂,…,Z_n)^T (13)

H＝(H_ij)_n×2 (14)

室內自校準導航方法包括：

1)新步長估計模型及行人航位自校準推算方法

新步長估計模型如下：

式中：L為步長，a_xmax和a_xmin分別為步態(tài)周期內X軸加速度的最大值和最小值，a_ymax和a_ymin分別為步態(tài)周期內Y軸加速度的最大值和最小值，K為步長參數(shù)，或者通過訓練得到，或者通過自校準濾波實時估計；

在行人航位自校準推算方法中，通過對陀螺儀和加速度計量測數(shù)據進行自校準濾波處理，實時計算行人的航向角；濾波的狀態(tài)方程如下式所示：

式中：ψ_k為航向角，θ_k為俯仰角，γ_k為滾轉角，ω_x,k,ω_y,k,ω_z,k為導航坐標系下的三軸角速度，Δt_k-1是相鄰兩次采樣的時間間隔；

w_ψ,k-1,w_θ,k-1,w_γ,k-1,w_ωx,k-1,w_ωy,k-1,w_ωz,k-1為狀態(tài)噪聲，b_ψ,k-1,b_θ,k-1,b_γ,k-1,b_ωx,k-1,b_ωy,k-1,b_ωz,k-1為狀態(tài)方程中的未知輸入；

濾波的量測方程如下式所示：

式中：a_cx,k,a_cy,k,a_cz,k為第k次采樣時加速度計的量測數(shù)據，ω_gx,k,ω_gy,k,ω_gz,k為第k次采樣時陀螺儀的量測數(shù)據，v_ax,k,v_ay,k,v_az,k,v_ωx,k,v_ωy,k,v_ωz,k為量測噪聲，d_ax,k,d_ay,k,d_az,k為量測方程中的未知輸入，g為當?shù)氐闹亓铀俣龋?/p>

采用非線性系統(tǒng)雙未知輸入自校準濾波方法進行濾波，或者采用兩步自校準濾波方法來進一步提高濾波的魯棒性和精度；與此同時，進行步態(tài)探測并由式(19)估計步長；最后，由式(22)和式(23)估計行人所處的位置：

式中：(x_j,y_j)為第j步終點的位置坐標，L_j為第j步步長，為第j步內航向角的平均值；

2)基于RSSI的室內自校準導航方法

首先，由行人航位自校準推算式(22)和式(23)，得到基于RSSI的室內自校準導航的狀態(tài)方程為：

式中：K_j-1為狀態(tài)方程中的未知輸入(待定參數(shù))，w_x,j-1,w_y,j-1為狀態(tài)噪聲，Δ_j由下式計算：

式中：a_xmax,j和a_xmin,j分別為第j步步態(tài)周期內X軸加速度的最大值和最小值，a_ymax,j和a_ymin,j分別為第j步步態(tài)周期內Y軸加速度的最大值和最小值；

然后，根據式(1)，建立基于RSSI的室內自校準導航的量測方程為：

式中：S_i,j為在第j步終點處來自第i個AP的信號強度，d_si,j為量測方程中的未知輸入，v_si,j為量測噪聲；

最后，采用非線性系統(tǒng)雙未知輸入自校準濾波方法對式(24)和式(26)進行導航濾波。