[发明专利]一种结合IMU与单组UWB的旋转式目标定位方法

权利要求书

1.一种结合IMU与单组UWB的旋转式目标定位方法，其特征在于，包括以下步骤：

S1、设置旋转臂，将IMU传感器和UWB基站分别固定在所述旋转臂的两个端点处；

S2、通过IMU传感器采集UWB基站旋转的角速度和磁场强度，通过UWB基站采集定位目标与UWB基站之间的距离信息；

S3、基于采集的角速度和磁场强度，通过互补滤波的IMU角度估计算法，计算出多组不同时刻的UWB基站的旋转角度；

S4、通过对UWB基站的旋转角度进行几何坐标变换，生成多组不同时刻不同角度的UWB基站的坐标信息；

S5、基于多组UWB基站的坐标信息和对应坐标下UWB基站采集的距离信息，通过滑动窗口结合非线性优化算法，计算出定位目标的位置，完成目标的定位。

2.根据权利要求1所述的一种结合IMU与单组UWB的旋转式目标定位方法，其特征在于，所述步骤S2中角速度和磁场强度的采集方法为：

以所述IMU传感器所在的旋转臂端点为轴心进行旋转，通过IMU传感器中的三轴陀螺仪和三轴磁力计分别采集UWB基站旋转的角速度和磁场强度。

3.根据权利要求1所述的一种结合IMU与单组UWB的旋转式目标定位方法，其特征在于，所述步骤S2中距离信息的采集方法为：

将UWB标签设置在定位目标上，通过UWB基站采集UWB标签与UWB基站之间的距离信息。

4.根据权利要求1所述的一种结合IMU与单组UWB的旋转式目标定位方法，其特征在于，所述步骤S3中互补滤波的IMU角度估计算法具体为：

以空间笛卡尔直角坐标系作为IMU传感器的参考坐标系，将UWB基站旋转平面相垂直的旋转轴定义为空间笛卡尔直角坐标系中的z轴，将IMU传感器与UWB基站分别固定在旋转臂的两个端点处，以IMU传感器所在端点为圆心，将相交于该圆心且相互垂直的两条直线定义为空间笛卡尔直角坐标系中的X轴和Y轴，则将IMU传感器绕z轴的旋转角度作为UWB基站的旋转角度，其旋转方向遵循右手定则，其中，UWB基站的旋转角度的起始边为X轴，终止边为旋转臂；则互补滤波的IMU角度估计算的表达式为：

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

其中，θ(t)为t时刻的UWB基站的旋转角度，θ(t-1)为t-1时刻的UWB基站的旋转角度，g_z(t)为t时刻的陀螺仪测得的绕z轴旋转角速度，Δt为陀螺仪的采样周期，k为互补滤波的系数，θ_m(t)为t时刻磁力计测算获得的绕z轴旋转角度。

5.根据权利要求4所述的一种结合IMU与单组UWB的旋转式目标定位方法，其特征在于，所述磁力计测算获得的绕z轴旋转角度的计算公式具体为：

其中，M_y(t)为t时刻磁力计测得的y轴磁场强度，M_x(t)为t时刻磁力计测得的x轴磁场强度。

6.根据权利要求4所述的一种结合IMU与单组UWB的旋转式目标定位方法，其特征在于，所述步骤S4具体为：

选取多组不同时刻的UWB基站的旋转角度，结合旋转臂的长度，并通过几何坐标变换的公式获得UWB基站的坐标信息；其中几何坐标变换的公式为：

其中，L为旋转臂的长度；(x(t),y(t))为t时刻变换得到的UWB基站坐标信息。

7.根据权利要求1所述的一种结合IMU与单组UWB的旋转式目标定位方法，其特征在于，所述步骤S5包括以下分步骤：

S5-1、将包括多组UWB基站坐标信息的数据集作为一个时间窗口W；

S5-2、基于时间窗口W和对应时间UWB基站所测的距离信息，通过滑动窗口结合非线性优化算法来估计定位目标的位置，完成目标的定位。