[發明專利]基于毫米波雷達及慣性測量單元的一種行人自主導航方法

有效

• 申請號: 202010016854.4
• 申請日: 2020-01-08
• 公開（公告）號: CN111157984B
• 公開（公告）日: 2021-10-22
• 發明（設計）人: 閻波;李柯蒙;魏震益;肖卓凌
• 申請（專利權）人: 電子科技大學
• 主分類號: G01S13/86
• 分類號: G01S13/86;G01S13/88;G01C21/16;G01C21/20
• 代理公司: 成都正華專利代理事務所(普通合伙) 51229
• 代理人: 陳選中

1.一種基于毫米波雷達及慣性測量單元的一種行人自主導航方法，其特征在于，包括以下步驟：

S1、在每只腳上安裝一個慣性測量單元，并在任一只腳上安裝毫米波雷達；

S2、通過毫米波雷達獲取行人的步長信息；以當前位置信息為基準，通過慣性測量單元獲取行人每只腳的模糊位置信息；

S3、將步長信息作為多傳感器融合的卡爾曼濾波器的水平觀測量，對行人雙腳的模糊位置信息進行修正與融合，得到融合后的位置信息；

S4、將融合后的信息作為行人的具體位置信息，判斷是否繼續導航，若是則返回步驟S2，否則結束導航；

步驟S2中通過毫米波雷達獲取行人的步長信息的具體方法包括以下子步驟：

S2-1-1、通過毫米波雷達發射信號并接收反射回來的信號；

S2-1-2、根據反射回來的信號遍歷每個所檢測到的目標，獲取每個檢測到的目標的速度、距離和回波功率；

S2-1-3、去除速度為0的目標、去除距離大于第一閾值的目標，以及去除回波功率小于第二閾值的目標；

S2-1-4、在保留下來的目標中，根據公式：

將歸一化的回波功率作為權重，對當前每一個探測到的目標進行加權平均，得到行人的步長信息R；其中為保留下來的第i個目標的距離；為保留下來的第i個目標的回波功率。

2.根據權利要求1所述的基于毫米波雷達及慣性測量單元的一種行人自主導航方法，其特征在于，所述S2-1-2中獲取每個檢測到的目標的速度和距離的具體方法為：

將反射回來的信號進行混頻后得到中頻信號的頻率Fs，根據公式：

獲取目標距離雷達的距離d；將獲取的若干距離d按行排列，將排列好的矩陣的每一列進行快速傅里葉變換，將變換后得到的矩陣的行作為距離數值，將變換后得到的矩陣的列作為速度數值；其中c為光速；S為信號頻率隨時間的變化率。

3.根據權利要求1所述的基于毫米波雷達及慣性測量單元的一種行人自主導航方法，其特征在于，所述第一閾值為行人身高的0.8倍；所述第二閾值為毫米波雷達發射功率的千分之一。

4.根據權利要求1所述的基于毫米波雷達及慣性測量單元的一種行人自主導航方法，其特征在于，步驟S2中通過慣性測量單元獲取行人每只腳的模糊位置信息的具體方法包括以下子步驟：

S2-2-1、以每只腳中安裝慣性測量單元的位置為坐標原點建立該腳的本體坐標系，讀取慣性測量單元中陀螺儀的數據，并根據公式：

C_t+1＝C_t(2I_3×3+Ω_tΔt)(2I_3×3-Ω_tΔt)^-1

獲取t+1時刻每只腳的姿態矩陣；其中C_t為t時刻該腳的姿態矩陣；C_t+1為t+1時刻該腳的姿態矩陣；I_3×3為3×3單位矩陣；Ω_t為t時刻該腳角速率的反對稱矩陣；Δt為相鄰時刻的間隔時長；ω_x、ω_y和ω_z分別為t時刻該腳上陀螺儀讀取的x軸方向、y軸方向和z軸方向的角速率；

S2-2-2、以行人的人體中心為坐標原點建立慣性導航坐標系，獲取慣性測量單元中加速度計的數據，并根據公式：

p_t+1＝p_t+v_t+1Δt

獲取t+1時刻每只腳的模糊位置信息；其中為慣性導航坐標系下t時刻該腳的加速度；為本體系下t時刻該腳在該本體坐標系中的加速度；v_t為t時刻該腳的速度；v_t+1為t+1時刻該腳的速度；p_t為t時刻該腳的位置信息；p_t+1為t+1時刻該腳的模糊位置信息。

5.根據權利要求4所述的基于毫米波雷達及慣性測量單元的一種行人自主導航方法，其特征在于，步驟S3的具體方法包括以下子步驟：

S3-1、根據公式

X_a0＝(C_a0,p_a0,v_a0)^T,X_b0＝(C_b0,p_b0,v_b0)^T

分別獲取行人兩只腳t+1時刻的狀態向量X_a0和X_b0；其中C_a0、p_a0和v_a0分別為未設置毫米波雷達的腳t+1時刻對應的姿態矩陣、位置信息和速度；C_b0、p_b0和v_b0分別為設置了毫米波雷達的腳t+1時刻對應的姿態矩陣、位置信息和速度；

S3-2、根據公式：

P_t+1＝F_tP_tF_t^T+Q

更新每只腳t+1時刻對應的誤差協方差矩陣；其中P_t+1為t+1時刻該腳的誤差協方差矩陣；P_t為t時刻該腳的誤差協方差矩陣；F_t為t時刻的狀態轉移矩陣；Q為狀態轉移噪聲；(·)^T為矩陣的轉置；S_t為t時刻慣性測量單元的加速度；為t時刻慣性測量單元x軸方向的加速度；為t時刻慣性測量單元y軸方向的加速度；為t時刻慣性測量單元z軸方向的加速度；0_3×3為3×3的零矩陣；

S3-3、判斷當前速度是否為0，若是則進入步驟S3-4，否則進入步驟S3-7；

S3-4、根據公式：

K_t+1＝P_t+1H^T(HP_t+1H^T+M)^-1

H＝{0_3×3 0_3×3 I_3×3}

分別獲取t+1時刻兩只腳的卡爾曼增益K_t+1；其中H為觀測矩陣，H中的兩個零矩陣的位置分別對應觀測到的該腳t+1時刻的狀態向量中姿態矩陣和位置信息的位置，H中的單位矩陣對應觀測到的該腳t+1時刻的狀態向量中速度的位置信息；M為觀測噪聲；

S3-5、根據公式：

P′_t+1＝(I_9×9-K_t+1H)P_t+1

修正t+1時刻每只腳的誤差協方差矩陣，得到修正后的誤差協方差矩陣P'；其中P′_t+1為t+1時刻該腳對應的修正后的誤差協方差矩陣；I_9×9為9×9的單位矩陣；

S3-6、根據公式：

ε＝(ε_c,ε_p,ε_v)^T＝K_t+1·v_t+1

C'_t+1＝(2I_3×3+ε′_cΔt)(2I_3×3-ε′_cΔt)^-1C_t+1

p′_t+1＝p_t+1-ε_p

v′_t+1＝v_t+1-ε_v

X_a1＝(C_a1,p_a1,v_a1)^T,X_b1＝(C_b1,p_b1,v_b1)^T

修正t+1時刻慣性測量單元所在腳的姿態矩陣C_t+1、慣性測量單元所在腳的位置信息p_t+1和慣性測量單元所在腳的速度v_t+1，并更新該腳t+1時刻的狀態向量，分別得到修正后的慣性測量單元所在腳的姿態矩陣C'_t+1、修正后的慣性測量單元所在腳的位置信息p′_t+1和修正后的慣性測量單元所在腳的速度v′_t+1，以及更新后的狀態向量X_a1和X_b1；其中ε′_c為參數ε_c的反對稱矩陣；ε為修正數據；ε_p和ε_v均為中間參數；參數ε_c、ε_p和ε_v的值由K_t+1·v_t+1決定；C_a1、p_a1和v_a1分別為未設置毫米波雷達的腳t+1時刻對應的修正后的姿態矩陣、位置信息和速度；C_b1、p_b1和v_b1分別為設置了毫米波雷達的腳t+1時刻對應的修正后的姿態矩陣、位置信息和速度；

S3-7、根據公式：

Z_radar＝C_rn·(R+ρ_r2i)+C_ln·ρ_a2i

獲取t+1時刻兩個本體坐標系間的相對位置信息Z_radar；其中C_rn為t+1時刻設置有毫米波雷達的腳對應的本體坐標系到慣性導航坐標系的旋轉矩陣；C_ln為t+1時刻另一只腳對應的本體坐標系到慣性導航坐標系的旋轉矩陣；R為步長信息；ρ_r2i為毫米波雷達到所在腳的慣性測量單元的方向向量；ρ_a2i為毫米波雷達的反射點到反射點所在腳的慣性測量單元的方向向量；

S3-8、根據公式：

X＝(C_a,p_a,v_a,C_b,p_b,v_b)^T

將左右兩只腳分別對應的最新誤差協方差矩陣拼接成協方差矩陣P_fusion，并將左右兩只腳對應的最新狀態向量拼接成狀態向量X；其中P_a為未設置毫米波雷達的腳對應的最新誤差協方差矩陣；P_b為設置了毫米波雷達的腳對應的最新誤差協方差矩陣；C_a、p_a和v_a分別為未設置毫米波雷達的腳對應的最新姿態矩陣、位置信息和速度；C_b、p_b和v_b分別為設置了毫米波雷達的腳對應的最新姿態矩陣、位置信息和速度；

S3-9、根據公式：

獲取融合卡爾曼增益K_fusion；其中觀測矩陣H_fusion＝(0_3×3 I_3×3 0_3×3 0_3×3 -I_3×3 0_3×3)；R_fusion為當前的觀測噪聲；

S3-10、根據公式：

P'_fusion＝(I_18×18-K_fusionH_fusion)P_fusion

X'＝X+K_fusion(Z_radar-H_fusionX)

融合更新協方差矩陣P_fusion和狀態向量X，得到融合更新后的協方差矩陣P'_fusion和融合后的狀態向量X'，即行人在t+1時刻真實的協方差矩陣和狀態向量；其中融合后的狀態向量X'中包含了融合后的位置信息。