[發(fā)明專利]一種結(jié)合IMU與單組UWB的旋轉(zhuǎn)式目標(biāo)定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110706410.8
• 申請(qǐng)日: 2021-06-24
• 公開(kāi)（公告）號(hào): CN113640738B
• 公開(kāi)（公告）日: 2023-05-09
• 發(fā)明（設(shè)計(jì)）人: 劉冉;鄧忠元;張華;肖宇峰;陳凱翔;曹志強(qiáng);秦正泓;張靜;劉滿祿
• 申請(qǐng)（專利權(quán)）人: 西南科技大學(xué)
• 主分類號(hào): G01S5/02
• 分類號(hào): G01S5/02;G01C21/16
• 代理公司: 北京正華智誠(chéng)專利代理事務(wù)所(普通合伙) 11870
• 代理人: 代維凡

1.一種結(jié)合IMU與單組UWB的旋轉(zhuǎn)式目標(biāo)定位方法，其特征在于，包括以下步驟：

S1、設(shè)置旋轉(zhuǎn)臂，將IMU傳感器和UWB基站分別固定在所述旋轉(zhuǎn)臂的兩個(gè)端點(diǎn)處；

S2、通過(guò)IMU傳感器采集UWB基站旋轉(zhuǎn)的角速度和磁場(chǎng)強(qiáng)度，通過(guò)UWB基站采集定位目標(biāo)與UWB基站之間的距離信息；

S3、基于采集的角速度和磁場(chǎng)強(qiáng)度，通過(guò)互補(bǔ)濾波的IMU角度估計(jì)算法，計(jì)算出多組不同時(shí)刻的UWB基站的旋轉(zhuǎn)角度；

S4、通過(guò)對(duì)UWB基站的旋轉(zhuǎn)角度進(jìn)行幾何坐標(biāo)變換，生成多組不同時(shí)刻不同角度的UWB基站的坐標(biāo)信息；

S5、基于多組UWB基站的坐標(biāo)信息和對(duì)應(yīng)坐標(biāo)下UWB基站采集的距離信息，通過(guò)滑動(dòng)窗口結(jié)合非線性優(yōu)化算法，計(jì)算出定位目標(biāo)的位置，完成目標(biāo)的定位。

2.根據(jù)權(quán)利要求1所述的一種結(jié)合IMU與單組UWB的旋轉(zhuǎn)式目標(biāo)定位方法，其特征在于，所述步驟S2中角速度和磁場(chǎng)強(qiáng)度的采集方法為：

以所述IMU傳感器所在的旋轉(zhuǎn)臂端點(diǎn)為軸心進(jìn)行旋轉(zhuǎn)，通過(guò)IMU傳感器中的三軸陀螺儀和三軸磁力計(jì)分別采集UWB基站旋轉(zhuǎn)的角速度和磁場(chǎng)強(qiáng)度。

3.根據(jù)權(quán)利要求1所述的一種結(jié)合IMU與單組UWB的旋轉(zhuǎn)式目標(biāo)定位方法，其特征在于，所述步驟S2中距離信息的采集方法為：

將UWB標(biāo)簽設(shè)置在定位目標(biāo)上，通過(guò)UWB基站采集UWB標(biāo)簽與UWB基站之間的距離信息。

4.根據(jù)權(quán)利要求1所述的一種結(jié)合IMU與單組UWB的旋轉(zhuǎn)式目標(biāo)定位方法，其特征在于，所述步驟S3中互補(bǔ)濾波的IMU角度估計(jì)算法具體為：

以空間笛卡爾直角坐標(biāo)系作為IMU傳感器的參考坐標(biāo)系，將UWB基站旋轉(zhuǎn)平面相垂直的旋轉(zhuǎn)軸定義為空間笛卡爾直角坐標(biāo)系中的z軸，將IMU傳感器與UWB基站分別固定在旋轉(zhuǎn)臂的兩個(gè)端點(diǎn)處，以IMU傳感器所在端點(diǎn)為圓心，將相交于該圓心且相互垂直的兩條直線定義為空間笛卡爾直角坐標(biāo)系中的X軸和Y軸，則將IMU傳感器繞z軸的旋轉(zhuǎn)角度作為UWB基站的旋轉(zhuǎn)角度，其旋轉(zhuǎn)方向遵循右手定則，其中，UWB基站的旋轉(zhuǎn)角度的起始邊為X軸，終止邊為旋轉(zhuǎn)臂；則互補(bǔ)濾波的IMU角度估計(jì)算的表達(dá)式為：

θ(t)＝k·(θ(t-1)+g_z(t)·Δt)+(1-k)·θ_m(t)(1)

其中，θ(t)為t時(shí)刻的UWB基站的旋轉(zhuǎn)角度，θ(t-1)為t-1時(shí)刻的UWB基站的旋轉(zhuǎn)角度，g_z(t)為t時(shí)刻的陀螺儀測(cè)得的繞z軸旋轉(zhuǎn)角速度，Δt為陀螺儀的采樣周期，k為互補(bǔ)濾波的系數(shù)，θ_m(t)為t時(shí)刻磁力計(jì)測(cè)算獲得的繞z軸旋轉(zhuǎn)角度。

5.根據(jù)權(quán)利要求4所述的一種結(jié)合IMU與單組UWB的旋轉(zhuǎn)式目標(biāo)定位方法，其特征在于，所述磁力計(jì)測(cè)算獲得的繞z軸旋轉(zhuǎn)角度的計(jì)算公式具體為：

其中，M_y(t)為t時(shí)刻磁力計(jì)測(cè)得的y軸磁場(chǎng)強(qiáng)度，M_x(t)為t時(shí)刻磁力計(jì)測(cè)得的x軸磁場(chǎng)強(qiáng)度。

6.根據(jù)權(quán)利要求4所述的一種結(jié)合IMU與單組UWB的旋轉(zhuǎn)式目標(biāo)定位方法，其特征在于，所述步驟S4具體為：

選取多組不同時(shí)刻的UWB基站的旋轉(zhuǎn)角度，結(jié)合旋轉(zhuǎn)臂的長(zhǎng)度，并通過(guò)幾何坐標(biāo)變換的公式獲得UWB基站的坐標(biāo)信息；其中幾何坐標(biāo)變換的公式為：

其中，L為旋轉(zhuǎn)臂的長(zhǎng)度；(x(t),y(t))為t時(shí)刻變換得到的UWB基站坐標(biāo)信息。

7.根據(jù)權(quán)利要求1所述的一種結(jié)合IMU與單組UWB的旋轉(zhuǎn)式目標(biāo)定位方法，其特征在于，所述步驟S5包括以下分步驟：

S5-1、將包括多組UWB基站坐標(biāo)信息的數(shù)據(jù)集作為一個(gè)時(shí)間窗口W；

S5-2、基于時(shí)間窗口W和對(duì)應(yīng)時(shí)間UWB基站所測(cè)的距離信息，通過(guò)滑動(dòng)窗口結(jié)合非線性優(yōu)化算法來(lái)估計(jì)定位目標(biāo)的位置，完成目標(biāo)的定位。