[發(fā)明專利]基于UWB雙標(biāo)簽的航向計(jì)算及其性能分析方法

在審

• 申請?zhí)枺?/td>: 201810701416.4
• 申請日: 2018-06-29
• 公開（公告）號: CN109341683A
• 公開（公告）日: 2019-02-15
• 發(fā)明（設(shè)計(jì)）人: 傅軍;高端陽;李安
• 申請（專利權(quán)）人: 中國人民解放軍海軍工程大學(xué)
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 武漢開元知識產(chǎn)權(quán)代理有限公司 42104
• 代理人: 潘杰;胡藝

1.一種基于UWB雙標(biāo)簽的航向計(jì)算及其性能分析方法，其特征在于，包括：

步驟1，對雙標(biāo)簽到至少三個基站之間的距離分別進(jìn)行測算；

步驟2，基于UKF算法建立UWB系統(tǒng)的定位解算模型；

步驟3，依據(jù)步驟1所得的各個距離，結(jié)合步驟2所得的定位結(jié)算模型獲取所述雙標(biāo)簽道位置；

步驟4，建立基于UWB雙標(biāo)簽道航向解算模型，利用雙標(biāo)簽的位置信息進(jìn)行運(yùn)動載體的航向解算。

2.根據(jù)權(quán)利要求1所述的一種基于UWB雙標(biāo)簽的航向計(jì)算及其性能分析方法，其特征在于，所述步驟1對雙標(biāo)簽到至少三個基站之間的距離分別進(jìn)行測算的具體方法包括：

步驟11，標(biāo)簽向基站發(fā)送一個帶有標(biāo)識和時間戳的數(shù)據(jù)包，基站接收數(shù)據(jù)包并記錄當(dāng)前時刻；

步驟12，基站向標(biāo)簽發(fā)送一個帶有標(biāo)識和時間戳的數(shù)據(jù)包，標(biāo)簽接收數(shù)據(jù)包并記錄當(dāng)前時刻；

步驟13，標(biāo)簽再向基站發(fā)送一個帶有標(biāo)識和時間戳的數(shù)據(jù)包，基站接收數(shù)據(jù)包之后進(jìn)行信號飛行時間T_prop計(jì)算：

其中T_round1為標(biāo)簽信號往返時間，T_round2為基站信號往返時間，T_reply1為基站信號回復(fù)時間，T_reply2為標(biāo)簽信號回復(fù)時間，

步驟14，標(biāo)簽到基站之間的距離r為：

r＝c×T_prop

其中，c為光速。

3.根據(jù)權(quán)利要求1所述的一種基于UWB雙標(biāo)簽的航向計(jì)算及其性能分析方法，其特征在于，所述步驟2基于UKF算法建立UWB系統(tǒng)的定位解算模型的具體方法包括：

步驟21，建立系統(tǒng)狀態(tài)量方程

狀態(tài)量為X＝[x，v_x，y，v_y]，其中(x，y)表示標(biāo)簽的位置坐標(biāo)，(v_x，v_y)表示標(biāo)簽的運(yùn)動速度，T為采樣時間間隔；

步驟22，建立系統(tǒng)觀測方程如下：

觀測量Z＝[r₁，r₂，r₃，r₄]表示標(biāo)簽至各個基站的距離，UKF傳播流程首先求解誤差協(xié)方差矩陣的均方根采用Cholesky分解求解：

計(jì)算sigma點(diǎn)：

式中下標(biāo)：，i表示矩陣第i列；

每個sigma點(diǎn)可通過系統(tǒng)模型進(jìn)行傳播：

傳播過后的狀態(tài)預(yù)測及其誤差協(xié)方差為：

UKF的觀測更新流程由下式產(chǎn)生新的sigma點(diǎn)：

sigma點(diǎn)和平均觀測新息可由下式解算：

則觀測新息的協(xié)方差為：

步驟23，更新UKF卡爾曼增益、狀態(tài)向量更新和誤差協(xié)方差得到UWB系統(tǒng)的定位解算模型：

式中：為k時刻狀態(tài)估計(jì)值，為k時刻狀態(tài)預(yù)測值，K_k為k時刻卡爾曼增益值，為k時刻平均觀測新息。

4.根據(jù)權(quán)利要求3所述的一種基于UWB雙標(biāo)簽的航向計(jì)算及其性能分析方法，其特征在于，所述步驟3依據(jù)步驟1所得的各個距離，結(jié)合步驟2所得的定位結(jié)算模型獲取所述雙標(biāo)簽道位置的具體方法包括：

在UWB定位區(qū)域內(nèi)，2個標(biāo)簽實(shí)時測量至少到3個基站之間的距離，利用該距離信息，按照步驟2進(jìn)行狀態(tài)估計(jì)，分別得到各個時刻2個標(biāo)簽的狀態(tài)估計(jì)值，根據(jù)步驟2中狀態(tài)量的選取，提取2個標(biāo)簽狀態(tài)量中的位置信息，得到2個標(biāo)簽的坐標(biāo)Tag1(x_Tag1，y_Tag1)和Tag2(x_Tag2，y_Tag2)。

5.根據(jù)權(quán)利要求4所述的一種基于UWB雙標(biāo)簽的航向計(jì)算及其性能分析方法，其特征在于，所述步驟4建立基于UWB雙標(biāo)簽道航向解算模型，利用雙標(biāo)簽的位置信息進(jìn)行運(yùn)動載體的航向解算的具體方法包括：

根據(jù)步驟3得到2個標(biāo)簽的位置信息分別為Tag1(x_Tag1，y_Tag1)和Tag2(x_Tag2，y_Tag2)，利用雙標(biāo)簽的位置信息進(jìn)行運(yùn)動載體的航向解算，航向