[發明專利]一種基于UWB移動節點的移動目標定位算法

有效

• 申請號: 202011362064.8
• 申請日: 2020-11-27
• 公開（公告）號: CN112533149B
• 公開（公告）日: 2022-06-07
• 發明（設計）人: 張烈平;吳俊康;譚銘揚;聶靖華;李智浩;匡貞伍
• 申請（專利權）人: 桂林理工大學
• 主分類號: H04W4/029
• 分類號: H04W4/029;H04W64/00;G01S11/02;G01S5/02
• 代理公司: 南寧新途專利代理事務所(普通合伙) 45119
• 代理人: 但玉梅

1.一種基于UWB移動節點的移動目標定位算法，其特征在于，包括以下步驟：

(1)建立UWB定位模型，在三維空間中，布置至少3個基站節點，在待測移動目標上設置標簽節點；

(2)利用UWB定位模型對基站節點和標簽節點間的距離進行測距，并經過預處理消除時鐘偏移和時鐘漂移帶來的誤差，得到消除誤差后的距離測試值，并得到移動目標的粗略位置坐標；

(3)將步驟(2)每次測距采集到CIR信號的接收信號強度指示RSSI、第一路徑功率強度FPPL、上升時間RT特征采用測距誤差緩解模糊推理定位算法得到NLOS測距誤差估計；

(4)將步驟(2)消除誤差后的距離測試值與步驟(3)模糊得到的NLOS測距誤差估計相減，得到模糊后的距離數據；

(5)將模糊后距離數據作為觀測值代入自適應抗NLOS卡爾曼算法，得到最終距離估計數據；

(6)將最終距離估計數據代入魯棒性更強LS定位算法，完成標簽節點的位置估計；

步驟(2)的具體步驟為：

設任一基站節點為節點A，待測移動目標為節點B，測試過程中，節點A向節點B發送測距信號，并記錄下發送信號時間T_a1,節點B在接收到測距信號時記錄下信號到達時間T_b1,并在間隔T_reply1后向節點A發送確認UWB信號，在發送時記錄下發送時刻T_b2,節點A收到節點B發送信號后，進行類似節點B操作，記錄下接收時間T_a2,延遲T_reply2的時間再次向節點B發送信號，并記錄發送時間T_a3，節點B再次在接收到信號后,記錄下信號到達時間T_b3；節點A與節點B經過三次通信后，得出節點A與節點B真實傳輸信號的時間T_tof的值：

其中，T_round1＝T_a2-T_a1，T_round2＝T_b3-T_b2，T_reply1＝T_b2-T_b1，T_reply2＝T_a3-T_a2；

根據每個基站節點與標簽節點之間距離計算公式d_i＝ct_i，其中c為信號傳輸的速率即光速，消除誤差后的距離測試值為：

d_i＝cT_tof (2)

根據消除誤差后的距離測試值，基于測距原理建立坐標方程，采用最小二乘法，計算UWB移動目標的坐標，獲取移動目標的粗略位置坐標；

步驟(3)所述測距誤差緩解模糊推理定位算法所用的模糊集和模糊規則的建立過程為：

3.1非視距實驗場景下，將基站節點放在固定位置，標簽節點位置從相距0.5m處開始，然后將障礙物相距基站10cm的位置開始，采集500次實驗數據為一組，每測一組就將障礙物到基站距離增加20cm，一直測到障礙物無法在標簽和基站之間為止，當所有障礙物測完后，增加標簽與基站距離，重復之前操作，一直測到3米為止；試驗過程中采集UWB的CIR信號特征RSSI、FPPL、RT以及計算NLOS測距誤差；

3.2視距實驗場景下，即無障礙物情況，將基站放在固定位置，標簽位置從相距0.5m處開始，每間隔0.5m作為一個測試點，一直測到3米為止；試驗過程中采集UWB的CIR信號特征RSSI、FPPL、RT以及計算NLOS測距誤差；

3.3對步驟3.1和3.2采集到的實驗數據進行分析，將輸入信號特征RSSI分成5個模糊集合，用非常大、大、中、小和非常小來劃分集合，將輸入信號特征FPPL分為6個模糊集合，用非常大、大、中、小、非常小以及非常非常小來劃分，對于輸入信號特征RT是與NLOS測距誤差影響成正相關的，分為非常非常小、非常小、小、中、大5個模糊集合，同時對于輸出NLOS誤差分為非常小、小、中、大、非常大5個模糊集合；結合實驗數據得到模糊規則；

將每一次測距采集到的CIR信號特征RSSI、FPPL、RT與所述模糊規則進行對應，得到對應的NLOS測距誤差的等級及NLOS測距誤差估計；

步驟(5)所述的自適應抗NLOS卡爾曼算法為，采用卡爾曼濾波算法對模糊后的距離數據進行濾波，利用3倍新息的方差與新息比較來判斷是否有NLOS存在，如果存在則通過對新息進行固定比例縮放來減小NLOS測距誤差，具體步驟為：

假設在短的時間內，標簽節點與基站節點之間距離是均勻變化的，標簽節點運動狀態為x(k)＝[d(k) v(k)]^T，其中：d(k)表示k時刻標簽節點到基站的距離，為除去噪聲后的估計值；v(k)表示k時刻標簽節點的速度；w(k-1)＝[w_d(k-1) w_v(k-1)]^T表示標簽在運動過程中必然受到過程噪聲干擾，其中：w_d(k-1)表示k-1時刻距離過程噪聲，w_v(k-1)表k-1時刻示速度過程噪聲，于是可以得到系統狀態方程：

其中：T代表采樣時間；將式(3)化為矩陣的形式，如公式(4)所示：

上式進一步簡化可得矩陣形式狀態方程模型，即式(5)，其中F_k是狀態轉移矩陣，w_k-1是k-1時刻過程噪聲，服從均值為0，方差矩陣為Q_k-1的高斯分布；

x_k＝F_kx_k-1+w_k-1 (5)

在基于濾波卡爾曼濾波系統中，設觀測量為模糊后的測距距離值，z(k)表示第k時刻模糊后距離數據，r(k)表示測距誤差，于是觀測方程可以寫成：

將(6)式寫成矩陣形式為：

z_k＝H_kx_k+v_k (7)

其中，H_k為觀測矩陣，v_k表示k時刻觀測噪聲，服從均值為0，方差矩陣為R_k的高斯分布；

式(5)和式(7)組成用于對測距數據濾波的卡爾曼濾波系統模型；假設k-1時刻卡爾曼濾波估計狀態為以及k時刻模糊后距離數據為z_k，則卡爾曼濾波計算k時刻估計狀態的步驟為：

5.1一步狀態預測

根據k-1時刻KF估計狀態預測k時刻狀態，即根據k-1時刻觀測值z_k-1對真實狀態x_k線性最小方差估計，即：

其中，表示k-1時刻最小均方估計；由于公式(5)可知，w_k-1只對x_k的值影響，因此E{w_k-1/z_k-1}的值為0于是可以得到一步預測狀態公式(9)為：

5.2誤差協方差矩陣預測

定義：

其中，表示一步預測估計代替真實狀態x_k引起誤差，表示將一步預測估計值代入觀測方程得到預測觀測值；表示預測觀測值與真實觀測值之間的誤差，也稱為新息Δ_k；設第k時刻新息Δ_k和其方差矩陣D_k為：

根據3倍方差理論并結合式(12)和式(13)可以得到一個測量值是否受到NLOS的判斷公式：

其中，表示D_k矩陣對角線上第i個元素，表達Δ_k的第i個元素，當式(14)不滿足時候，對其新息修正，如公式(15)所示：

其中，α表示新息修正系數，α根據新息與新息方差之間差變化來自動調節其取值，自動調節α的方法如下：

其中，b為修正系數α的最大縮小比例，取值范圍在0到1之間；

由公式(10)和(11)可知，新息中包含部分真實狀態的信息，于是對進行適當加權獲得信息，于是修正后狀態估計為：

其中，K_k為卡爾曼增益矩陣；假設修正后估計與真實狀態x_k之間估計誤差為：

于是估計誤差的協方差矩陣定義為：

一步預測誤差的協方差矩陣定義為：

并且已知其中R_k表示觀測噪聲方差，估計誤差與觀測噪聲互不相關，即代入式(20)可得：

P_k＝(I-K_kH_k)P_k|k-1(I-K_kH_k)^T+K_kR_kK^T (21)

要寫出遞推公式，還要分析從P_k-1到P_k|k-1的遞推公式；將公式(5)和公式(9)代入一步預測狀態估計誤差公式可得：

由于k-1時刻估計誤差與當前k時刻過程噪聲之間也互不相關，即:代入式(20)可得一步預測誤差的協方差矩陣：

P_k|k-1＝F_kP_k-1F_k^T+Q_k-1 (23)

在得到一步預測誤差協方差的遞推公式后，需要進一步討論增益矩陣K_k值的選擇，以使得估計誤差協方差P_k最小；

5.3增益矩陣K_k

為了使得估計誤差協方差P_k最小，通常矩陣的跡最小，即：

將式(23)代入上式，根據矩陣跡的求導公式，同時考慮到P_k|k-1、R_k為對稱矩陣，計算可得：

令公式(25)等于零，即可求得卡爾曼濾波增益矩陣為：

進一步將式(26)代入公式(23)，可將估計誤差協方差矩陣簡化為：

P_k＝(I-K_kH_k)P_k|k-1 (27)

5.4更新過程，根據式(14)更新狀態估計

5.5更新過程，根據式(24)更新估計誤差的協方差矩陣P_k；

5.6置k＝k+1，循環進行步驟5.1到步驟5.5，進行下一次濾波計算，得到最終狀態估計通過標簽節點的運動狀態方程x(k)＝[d(k)v(k)]^T得到最終距離估計數據。

2.根據權利要求1所述的一種基于UWB移動節點的移動目標定位算法，其特征在于：步驟(6)所述的將最終距離估計數據代入魯棒性更強LS定位算法，具體過程為：將得到三個基站距離數據d₁、d₂、d₃代入到TOA定位方程中，即：

在公式(28)中，從第二個方程到最后一個方程依次減去第一方程，得：

為了方便求解，可將其轉換成線性方程組矩陣形式為：

AX＝b (30)

其中，

對公式(30)用最小二乘原理可得到標簽的位置估計：

X＝(Α^TA)^-1A^Tb (31)。