[發明專利]一種基于人體下肢DH模型的行人慣性導航定位方法

在審

• 申請號: 202210181087.1
• 申請日: 2022-02-25
• 公開（公告）號: CN114623826A
• 公開（公告）日: 2022-06-14
• 發明（設計）人: 李曉東;熊智;丁一鳴;王婕;孫銀收;劉晨;張玲;張苗;崔巖;陳芷心;李婉玲;曹志國;王鉦淳
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C25/00;G01C1/00;G06F17/13;G06F17/15;G06F17/16
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 張弛

1.一種基于人體下肢DH模型的行人慣性導航定位方法，其特征在于，包括以下步驟：

(1)采集行人足綁式IMU數據并進行預處理；

(2)利用DH準則，建立人體下肢模型；在人體下肢靜止階段時，于左大腿、右大腿、左小腿、右小腿的位置進行傳感器的安裝和校準；

(3)實時解算左足踝關節節點、右足踝關節節點共兩節點的IMU數據并進行零速檢測，檢測結果處于零速狀態時進行零速修正，修正過程中獲得左右雙足的狀態量；

(4)通過步驟(2)的傳感器采集腿部四個節點數據并進行姿態的實時解算，獲得實時更新的姿態角，通過正向運動學實時求解雙足距離矢量；所述的四個節點分別為左腿髖關節節點、右腿髖關節節點、左腿膝關節節點、右腿膝關節節點；

(5)根據步驟(3)獲得的左右雙足的狀態量，結合步驟(4)解算得到的雙足距離矢量建立雙足距離等式約束，并結合濾波方法對位置、速度和姿態進行修正，得到修正后的速度、姿態和位置。

2.根據權利要求1所述的基于人體下肢DH模型的行人慣性導航定位方法，其特征在于，步驟(4)具體為：

(4.1)采集腿部四個節點數據，利用捷聯慣導算法實時解算腿部四個節點IMU姿態角，姿態角包括航向角ψ、俯仰角θ、滾轉角γ；

(4.2)利用正向運動學求解雙足距離矢量

(4.2.1)根據建立的DH模型參數，獲得單只腿部的關節之間的齊次變換矩陣所述的關節包括關節1、關節2、關節3、關節4，關節1、關節2、關節3對應髖關節的三個自由度，關節4對應膝關節的一個自由度；公式為：

其中，θ_i表示關節i的旋轉角，i取1、2、3、4；θ₁數值為單只腿部髖關節節點的IMU航向角；θ₂數值為單只腿部髖關節節點的IMU滾轉角，θ₃數值為單只腿部髖關節節點的IMU俯仰角；θ₄數值為單只腿部膝關節節點的IMU俯仰角，且θ₁、θ₂、θ₃、θ₄的數值均取自同一只腿的節點姿態角的數值；α_i表示關節i的扭角，d_i表示表示關節i與關節i-1之間連桿的偏置；

(4.2.2)獲取單只腿部節點中髖關節與該只腿部踝關節的齊次變換矩陣公式為：

(4.2.3)提取第四列中的前三個元素得到單只腿部節點中髖關節與該只腿部踝關節的位置相對關系

(4.2.4)足部兩節點IMU的位置相對關系表示為：

其中，表示左側足綁式IMU與左側髖關節的位置相對關系，表示右側足綁式IMU與右側髖關節的位置相對關系；T＝[-d 0 0]^T表示左右側髖關節的位置相對關系，d表示左右髖關節間距。

3.根據權利要求2所述的基于人體下肢DH模型的行人慣性導航定位方法，其特征在于，步驟(5)具體包括：

(5.1)聯立雙足狀態量，構建系統方程：

X＝[X^L X^R]

其中，X^L表示左腳IMU的導航誤差狀態量、X^R表示右腳IMU的導航誤差狀態量；

(5.2)根據X^L和X^R解算得到的雙足IMU相對距離模值為：

d_c＝f(L·X)

其中,f()為向量歐式距離求解函數,L＝[0_3×6 I_3×3 0_3×15 -I_3×3 0_3×9]；

(5.3)根據雙足距離矢量獲得足部兩節點IMU之間的相對距離模值為：

(5.4)建立雙足等式約束，獲得量測方程：

Z_dis＝δd＝d_c-d_DH＝f(L·X)-d_DH

(5.5)根據系統方程和量測方程應用擴展卡爾曼濾波獲得最優估計誤差狀態量后，對位置、速度、姿態和IMU器件誤差進行再次修正。