CN113640738B - 一种结合imu与单组uwb的旋转式目标定位方法 - Google Patents

Abstract

本发明公开了一种结合IMU与单组UWB的旋转式目标定位方法，采用旋转臂的结构，使一个UWB基站通过旋转的方式获得多组UWB基站的坐标信息，解决现有多UWB基站定位方法和定位环境部署困难、适用性差的问题；通过互补滤波的IMU角度估计算法削减三轴陀螺仪自身的累计误差，弥补三轴磁力计动态性能差的缺点；通过滑动窗口结合非线性优化算法获得精确的目标位置信息，解决了传统UWB定位需要多个UWB基站，适用性不高的问题。

Description

一种结合IMU与单组UWB的旋转式目标定位方法

技术领域

本发明属于室内目标定位技术领域，具体涉及一种结合IMU(惯性传感器)与单组UWB(超宽带)的旋转式目标定位方法。

背景技术

近年来，低成本高精度的定位解决方案已成为物联网应用的关键，如智能制造和智能家居。

在物联网和移动机器人领域中，定位一直是热门的研究课题，它能够为控制平台提供实时精确的目标位置。在物联网领域中可以利用这些位置对目标进行精准的控制，如远程指定目标遥控、场景内目标查找等。在机器人领域中可以通过得到的位置数据进行自主导航和地图构建。因此，目标定位在这些领域中占有重要的位置。

超宽带定位技术，相比于其他定位方式如，WiFi、RFID等，拥有更好的定位精度，同时基于在带宽上的优势其具有更强抗干扰能力、更低功耗和更快的数据传输速度。UWB的定位精度可达分米级，适合于目标的精确定位。但是，传统UWB定位系统常需要至少三个UWB基站和一个UWB标签才能完成对目标的精确定位，这样的系统结构大大增加了系统搭建的难度和适用性，很难适配一些不方便建立基站系统的场景。

发明内容

针对现有技术中的上述不足，本发明提供的一种结合IMU与单组UWB的旋转式目标定位方法解决了现有多基站UWB定位方法定位环境部署困难、适用性差的问题。

为了达到上述发明目的，本发明采用的技术方案为：一种结合IMU与单组UWB的旋转式目标定位方法，包括以下步骤：

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

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

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

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

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

进一步地：所述步骤S2中角速度和磁场强度的采集方法为：

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

上述进一步方案的有益效果为：通过IMU传感器采集的角速度和磁场强度，可以更加精确地计算出UWB基站的旋转角度，减小测量误差。

进一步地：所述步骤S2中距离信息的采集方法为：

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

上述进一步方案的有益效果为：通过UWB基站采集多组不同角度不同时刻的距离信息，将其作为滑动窗口结合非线性优化算法的输入，以便于计算出更精确的定位信息。

进一步地：所述步骤S3中互补滤波的IMU角度估计算法具体为：

以空间笛卡尔直角坐标系作为IMU传感器的参考坐标系，将UWB基站旋转平面相垂直的旋转轴定义为空间笛卡尔直角坐标系中的z轴，则将IMU传感器绕z轴的旋转角度作为UWB基站的旋转角度，其旋转方向遵循右手定则；则互补滤波的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轴旋转角度。

上述进一步方案的有益效果为：互补滤波的IMU角度估计算法融合了三轴陀螺仪传感器良好的动态特性和三轴磁力计的静态特性，可以获得精确的旋转角度估计。

进一步地：所述磁力计测算获得的绕z轴旋转角度的计算公式具体为：

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

上述进一步方案的有益效果为：三轴陀螺仪具有良好的动态响应特性，三轴磁力计在长时间测量中不存在累计误差，结合这两种方法测量IMU基站的角度精确度高。

进一步地：所述步骤S4具体为：

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

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

上述进一步方案的有益效果为：这种采用旋转臂的结构可以使用一个UWB基站通过旋转的方式获得多组不同时刻的UWB基站信息，节省了资源消耗。

进一步地：所述步骤S5包括以下分步骤：

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

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

上述进一步方案的有益效果为：本发明通过滑动窗口结合非线性优化算法获得精确的目标位置信息。解决了传统UWB定位需要多个UWB基站，适用性不高的问题。

进一步地：所述步骤S5-2中，所述滑动窗口结合非线性优化算法的具体公式为：

其中，(x,y)为定位目标的位置估计参数，W为设定的时间窗口大小，d_i为UWB基站在i时刻采集的距离信息，t为当前的时间，θ_i为IMU传感器在i时刻测算获得的UWB基站旋转角度，η_i为i时刻计算的权重系数；η_i的计算公式如下：

其中，Δθ_i为当前IMU传感器测量角度θ(t)与i时刻测算获得的θ_i的差值，σ²为IMU传感器的测量精度方差。

上述进一步方案的有益效果为：本发明采用指数衰减的权重系数，当累计误差大时其计算的估计误差权重系数低，从而矫正IMU传感器的累计误差对系统的影响，使得整个优化更加准确。

进一步地：所述滑动窗口结合非线性优化算法中滑动时间窗口的方式具体为：

取当前时刻的前W个UWB基站的坐标信息和距离信息；作为一个时间窗口，当有新的坐标信息时丢弃掉当前时间窗口最末的坐标信息，通过图优化的方式，使W个UWB基站坐标信息和距离信息构成图的W条约束，再将待估计位置(x,y)作为图的顶点，采用高斯牛顿算法迭代出最优位置估计。

上述进一步方案的有益效果为：通过滑动窗口的方式可以减少IMU传感器在长时间测量过程中存在的累计误差。

本发明的有益效果为：

(1)利用IMU传感器采集UWB基站的角速度和磁场强度信息，通过结合旋转臂旋转可以得到多组UWB基站的坐标信息。

(2)融合多UWB基站的坐标信息和对应坐标的距离信息，通过滑动窗口结合非线性优化算法可以获得精确的目标位置信息，解决了传统UWB基站定位需要多个UWB基站，适用性不高的问题。

(3)通过互补滤波的IMU角度估计算法可以消除陀螺仪自身的累计误差，弥补磁力计动态性能差的缺点，计算出更准确的UWB基站的角度信息，帮助完成目标精确定位。

附图说明

图1为一种结合IMU与单组UWB的旋转式目标定位方法的概念图。

图2为一种结合IMU与单组UWB的旋转式目标定位方法的流程图。

具体实施方式

下面对本发明的具体实施方式进行描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

如图1所示，在本发明的一个实施例中，通过将IMU传感器与UWB基站分别固定在一个旋转臂的两个端点处，以IMU传感器所在端点为轴心进行旋转，IMU传感器在旋转过程中可测得旋转臂旋转的磁场强度、角速度信息，UWB基站通过测量设置在定位目标的UWB标签与UWB基站之间的距离，采集不同时刻的距离信息d_i。通过互补滤波的IMU角度估计算法，将测得的磁场强度和角速度信息进行计算，得到精确的UWB基站的旋转角度θ_i。根据旋转角度θ_i和已知的旋转臂的长度L，可获得UWB旋转过程中采集的每个不同时间不同角度的UWB基站的坐标信息(x(i),y(i))，再结合UWB基站采集的距离信息d_i，采用滑动窗口结合非线性优化算法可以实现目标的精确定位。

基于此，本发明提供了如图2所示的一种结合IMU与单组UWB的旋转式目标定位方法，包括以下步骤：

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

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

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

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

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

本实施例中的步骤S2中角速度、磁场强度和距离信息的采集方法为：

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

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

通过IMU传感器采集的角速度和磁场强度，可以更加精确地计算出UWB基站的旋转角度，减小测量误差。通过UWB基站采集多组不同角度不同时刻的距离信息，将其作为滑动窗口结合非线性优化算法的输入，以便于计算出更精确的定位信息。

本实施例中的步骤S3中互补滤波的IMU角度估计算法具体为：

以空间笛卡尔直角坐标系作为IMU传感器的参考坐标系，将UWB基站旋转平面相垂直的旋转轴定义为空间笛卡尔直角坐标系中的z轴，则将IMU传感器绕z轴的旋转角度作为UWB基站的旋转角度，其旋转方向遵循右手定则；则互补滤波的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轴旋转角度。

所述磁力计测算获得的绕z轴旋转角度的计算公式具体为：

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

IMU传感器中的三轴陀螺仪具有良好的动态响应特性，在短时间内能够精确的反应旋转角度变化，但长时间运行会产生累计误差，而IMU传感器中的三轴磁力计在长时间测量中不存在累计误差，但是动态响应较差。因此通过调节互补滤波的系数k可以改变陀螺仪所测角度与磁力计所测角度的可信度，它们之间呈互补性，当动态测量时三轴陀螺仪所测角度可信度高，则增大k值，当静态测量时三轴磁力计可信度高，则减小k值。

本实施例中的步骤S4具体为：

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

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

基于互补滤波的IMU角度估计算法得到的UWB基站的旋转角度，通过几何坐标变换后，可以得到多组UWB基站的坐标信息。

本实施例中的步骤S5包括以下分步骤：

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

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

所述步骤S5-2中，滑动窗口结合非线性优化算法的具体公式为：

其中，(x,y)为定位目标的位置估计参数，W为设定的时间窗口大小，d_i为UWB基站在i时刻采集的距离信息，t为当前的时间，θ_i为IMU传感器在i时刻测算获得的UWB基站旋转角度，η_i为i时刻计算的权重系数；η_i的计算公式如下：

其中，Δθ_i为当前IMU传感器测量角度θ(t)与i时刻测算获得的θ_i的差值，σ²为IMU传感器的测量精度方差。

在计算滑动窗口结合非线性优化算法过程中，随着时间的增长IMU传感器的累计误差将逐渐增大，本发明采用指数衰减的权重系数η_i，当累计误差大时其计算的估计残差权重系数低，从而矫正IMU传感器的累计误差对系统的影响，使得整个计算更加准确。

所述滑动窗口结合非线性优化算法中滑动时间窗口的方式具体为：

取当前时刻的前W个UWB基站的坐标信息和距离信息；作为一个时间窗口，当有新的坐标信息时丢弃掉当前时间窗口最末的坐标信息，通过图优化的方式，使W个UWB基站坐标信息和距离信息构成图的W条约束，再将待估计位置(x,y)作为图的顶点，采用高斯牛顿算法迭代出最优位置估计。

本发明的有益效果为：通过互补滤波的IMU角度估计算法可以消除三轴陀螺仪自身的累计误差，弥补三轴磁力计动态性能差的缺点，计算出更准确的角度信息。通过滑动窗口结合非线性优化算法获得精确的目标位置信息。解决了传统UWB定位需要多个UWB基站，适用性不高的问题。

在本发明的描述中，需要理解的是，术语“中心”、“厚度”、“上”、“下”、“水平”、“顶”、“底”、“内”、“外”、“径向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的设备或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性或隐含指明的技术特征的数量。因此，限定由“第一”、“第二”、“第三”的特征可以明示或隐含地包括一个或者更多个该特征。

Claims (9)

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基站所测的距离信息，通过滑动窗口结合非线性优化算法来估计定位目标的位置，完成目标的定位。

8.根据权利要求7所述的一种结合IMU与单组UWB的旋转式目标定位方法，其特征在于，所述步骤S5-2中，所述滑动窗口结合非线性优化算法的具体公式为：

其中，(x,y)为定位目标的位置估计参数，W为设定的时间窗口大小，d_i为UWB基站在i时刻采集的距离信息，t为当前的时间，θ_i为IMU传感器在i时刻测算获得的UWB基站旋转角度，η_i为i时刻计算的权重系数；η_i的计算公式如下：

其中，Δθ_i为当前IMU传感器测量角度θ(t)与i时刻测算获得的θ_i的差值，σ²为IMU传感器的测量精度方差。

9.根据权利要求8所述的一种结合IMU与单组UWB的旋转式目标定位方法，其特征在于，所述滑动窗口结合非线性优化算法中滑动时间窗口的方式具体为：

取当前时刻的前W个UWB基站的坐标信息和距离信息；作为一个时间窗口，当有新的坐标信息时丢弃掉当前时间窗口最末的坐标信息，通过图优化的方式，使W个UWB基站坐标信息和距离信息构成图的W条约束，再将待估计位置(x,y)作为图的顶点，采用高斯牛顿算法迭代出最优位置估计。

Images