CN113709662B - 一种基于超宽带的自主式三维反演定位方法 - Google Patents

Abstract

本发明公开了一种基于超宽带的自主式三维反演定位方法。在标签智能体移动过程中，以预定周期采集与各目标超宽带节点间的测距信息，以及标签智能体的运动信息；构建系统状态方程和系统观测方程，利用无迹卡尔曼滤波方法对系统状态空间模型进行解算，反演定位出各所述目标超宽带节点的位置，根据反演定位结果对标签智能体的运动轨迹进行校正。本发明可根据一个已知超宽带标签的运动信息和测量的距离信息，对多个未知目标超宽带节点进行反演定位，并可根据反演定位结果对智能体的运动轨迹进行预测校正。

Description

一种基于超宽带的自主式三维反演定位方法

技术领域

本发明涉及室内定位领域，尤其是一种基于超宽带的对三维空间中未知节点进行反演定位的解决方案。

背景技术

目前的大部分超宽带应用环境是已知室内环境，例如建筑内部、工厂仓库等，可以预先人为架设锚点，在此基础上通过多锚点对进入的标签进行定位。此类方案需要知晓室内的各锚点的位置，在不知晓各锚点位置的情况下，无法对标签进行定位。

CN113137967A公开的一种机器人定位方法、装置、机器人和可读存储介质，其机器人上的通信模块接收各个定位标签发送的脉冲信号，获取机器人和各个定位标签之间的相对距离和相对角度；获取所述机器人的惯性数据；根据惯性数据估计所述机器人在当前时刻的估计位姿；根据所述估计位姿中的估计角度和各个所述相对角度的比较结果从所述各个定位标签中确定预定第一数目个角度可靠标签；根据所述预定第一数目个角度可靠标签对应的相对距离和相对角度确定所述机器人在当前时刻的实际位姿。该方案即为典型的通过已知锚点对未知标签进行室内定位的方案，在锚点定位信息丢失时，无法实现对标签的定位。另外，也有通过在智能体上设置里程计/惯性导航模块来获得智能体的运动信息的应用，但里程计/惯性导航模块随着智能体运行时间的推移，会产生累计误差，即越往后，定位的偏差越大。

如何通过已知标签搜寻位置锚点，目前少见有相应的方案，但其对于灾害搜寻工作又有着重要的应用价值，因此，是值得研究的方向。

发明内容

本发明的发明目的在于：针对上述存在的问题，提供一种基于超宽带的自主式三维反演定位方法，以实现在已知携带超宽带标签的智能体位置以及运动信息的情况下，对三维空间中未知的多个超宽带锚点的位置进行反演推算。

本发明采用的技术方案如下：

一种基于超宽带的自主式三维反演定位方法，包括：

移动标签智能体，在所述标签智能体移动过程中，获取多个时刻下所述标签智能体与各目标超宽带节点间的测距信息，以及所述标签智能体的运动信息；

基于多个时刻下的所述测距信息和所述运动信息，反演定位出各所述目标超宽带节点的位置。

进一步的，方法还包括：根据各所述目标超宽带节点的位置，对所述标签智能体的运动轨迹进行校正。

进一步的，在所述标签智能体移动过程中，以固定周期获取所述测距信息和所述运动信息。

进一步的，所述基于多个时刻下的所述测距信息和所述运动信息，反演定位出各所述目标超宽带节点的位置，包括：

基于所述测距信息和运动信息构建系统状态空间模型；

通过无迹卡尔曼滤波方法对所述系统状态空间模型进行解算，反演定位出各所述目标超宽带节点的位置。

进一步的，所述通过无迹卡尔曼滤波方法对所述系统状态空间模型进行解算，包括：

计算系统状态空间模型中状态的采样sigma点；

根据计算的sigma点对状态进行一步预测；

对状态的一步预测值做UT变化；

根据UT变换值对系统状态空间模型的观测值进行更新；

根据更新值确定卡尔曼增益矩阵并更新状态估计值及其协方差。

进一步的，所述计算系统状态空间模型中状态的采样sigma点，包括：

初始化系统状态及协方差矩阵P，计算采样sigma点；

sigma点权重配置为：

其中，n是状态向量的维数，κ、α、β均为待配置的待选参数，λ是缩放比例参数，λ与待选参数κ和α的关系为：

λ＝α²(n+κ)-n

κ的选取要保证矩阵(n+κ)P恒为半正定矩阵，待选参数β是一个非负数。

综上所述，由于采用了上述技术方案，本发明的有益效果是：

本发明相较于市面上常用的锚点-标签定位方法，可以在室内通过不断移动已知超宽带标签位置实现对多个未知目标超宽带节点进行反演定位。此外，本发明还可根据反演定位结果，对标签的运动轨迹进行预测校正。

附图说明

本发明将通过例子并参照附图的方式说明，其中：

图1是本发明应用场景的一个实施例。

图2是图1实施例中超宽带传感器的网络拓扑示意图。

图3是发明方法执行的流程示意图。

图4(包括图4-1～图4-4)是搜寻智能体运动轨迹对比图。

图5是目标超宽带节点反演定位误差对比图。

图6是搜寻智能体运动轨迹误差对比图。

具体实施方式

本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。

本说明书(包括任何附加权利要求、摘要)中公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换。即，除非特别叙述，每个特征只是一系列等效或类似特征中的一个例子而已。

实施例一

定义P₀为标签智能体节点，此节点搭载了超宽带(Ultra Wide Band，UWB)模块、里程计/惯性导航模块(或其他能够实现三维空间定位的装置)。智能体进入未知三维空间内，其运动信息由里程计/惯性导航模块提供，运动信息包括位置信息。定义智能体在t时刻位置为T_0，t＝[T_0，x，t T_0，y，t T_0，z，t]^T，在t时刻速度为v_0，t＝[v_0，x，t v_0，y，t v_0，z，t]^T。定义未知三维空间内若干超宽带节点(即目标超宽带节点)为T_n，n＝1，2，...，N，N为超宽带节点数量，节点位置信息未知，定义t时刻的节点坐标为T_n，t＝[T_n，x，t T_n，y，t T_n，z，t]^T，n＝1，2，...，N。上述任意节点T_i，i＝0，1，...，N可与其他节点T_j，j＝0，1，...，N，i≠j进行通信，获得相互间的测距值d_ij。

智能体通过不断运动，生成大量的运动数据点以及对应的测距信息，通过无迹卡尔曼滤波方法，对三维空间内未知节点位置进行反演推算。

当智能体连续运动一段时间后，由于传感器特性，其运动信息会出现累计误差。此时利用未知节点的推算位置和测距信息，对智能体运动轨迹进行预测与校正。最终实现未知超宽带节点的反演定位以及智能体的运动轨迹校正。

基于上述定义，基于超宽带的自主式三维反演定位方法包括：

(1)建立系统状态方程

定义系统状态方程为：

X_t+1＝AX_t+Bu_t+W

式中X_t为t时刻状态向量：

状态转移矩阵A＝I_N+1，输入矩阵B为：

上式中T为采样周期。

u_t为t时刻输入向量：

u_t＝[v_0，t]

W为过程噪声，其协方差为Q。

(2)建立系统观测方程

定义系统观测方程为：

Z_t+1＝H(X_t+1)+V

Z_t表示t时刻的观测向量，观测方程H(X_t)为：

式中i＝0，1，...，N，j＝1，...，N，i≠j。d_ij，t为t时刻节点T_i与T_j的欧氏距离：

d_ij，t＝||T_i，t-T_j，t||

V为观测噪声，其协方差为R。

(3)基于无迹卡尔曼滤波进行反演定位以及对智能体运动轨迹进行预测与校正

系统状态空间模型由系统状态方程和系统观测方程构成，其定义为：

显然，观测方程是非线性的，因此，本实施例采用无迹卡尔曼滤波(UnscentedKalman Filter，UKF)方法对目标超宽带节点进行反演定位以及动态轨迹跟踪。所以，在对智能体运动特性进行分析后，进行UKF滤波解算，具体过程包括：

1)对系统状态X及协方差矩阵P进行初始化，计算采样Sigma点。

采样点的权重如下：

其中，n是状态向量的维数，n为自然数，我们共采集了2n+1个Sigma点，式中的λ是缩放比例参数，它的值与两个待选参数κ和α有关，三者关系式如下：

λ＝α²(n+κ)-n

参数的选取控制了采样点的分布状态，同时参数κ的选取要保证矩阵(n+κ)P恒为半正定矩阵。此外，待选参数β是一个非负数，它可以通过合并方程的高阶项误差将高阶项影响消除。

2)根据选取的Sigma点进行一步预测。计算出经状态转移矩阵A变换后的Sigma点以及状态的一步预测和方差的一步预测：

3)对状态的一步预测值做UT变换：

4)量测更新。

计算出经过非线性的观测方程H(·)处理后的Sigma点，并计算观测估计值及其协方差、状态估计值与量测估计值间的互协方差：

5)确定卡尔曼增益矩阵并更新状态估计值及其协方差：

智能体的运动是一个动态过程，随着智能体的运动，系统状态不断更新。重复上述1)-5)的步骤，即可对系统进行UKF滤波解算。

未知目标超宽带节点的坐标即为系统状态方程中的变量，随着系统状态不断更新，预测值与真实值之间的误差不断减小，最终预测的点位收敛于真实的点位，该预测的点位的坐标(即反演定位的坐标)即为目标超宽带节点的坐标。智能体的坐标同理进行预测更新，实现运动轨迹的校正。经上述UKF滤波解算，即可实现三维空间内未知超宽带节点的反演定位，以及对智能体运动轨迹的预测校正。

实施例二

本实施例以三个未知超宽带目标节点为例，说明根据智能体节点进行三维空间内对三个未知超宽带目标节点的反演定位的方法。

三个超宽带节点因受损(如在建筑物内执行任务的节点因地震等原因受损)或其他原因出现失控的情形，派遣新的智能体进入目标场景中搜寻受损的目标超宽带节点，并回收智能体。需要说明的是，目标超宽带节点与智能体均携带超宽带模块，目标超宽带节点的受损仅是无法受控，即无法确定在三维空间中的位置，其仍然可以实现节点间的相互通信，该假设的应用场景如图1所示智能体进入建筑后，与受损目标超宽带节点建立通信，可获得智能体与各目标超宽带节点间的相互的距离。

智能体在受灾三维空间内不断移动，通过自身搭载的里程计/惯性导航模块不断生成运动信息。同时，通过超宽带模块获取与各目标超宽带节点间的测距信息。基于上述UKF方法，实现在三维空间中对各目标超宽带节点的定位，以实现目标超宽带节点的搜寻以及回收。

此外，由于智能体运动距离的增加，里程计/惯性导航模块会出现累计误差增大的现象，而上述反演定位的结果能够对智能体运动轨迹进行预测与矫正，由此减小累计误差带来的影响。

实施例三

本实施例对上述实施例二假设场景进行了准确性验证，过程如图3所示。

S1.对三维空间内的搜寻智能体以及受损智能体进行建模。如图2所示，搜寻智能体节点定义为T₀，其t时刻位置为T_0，t＝[T_0，x，t T_0，y，t T_0，z，t]^T，智能体速度定义为v_0，t＝[v_0，x，t v_0，y，t v_0，z，t]^T。三架受损智能体节点分别定义为T₁、T₂、T₃，t时刻位置信息为T_1，t＝[T_1，x，t T_1，y，t T_1，z，t]^T、T_2，t＝[T_2，x，t T_2，y，t T_2，z，t]^T、T_3，t＝[T_3，x，t T_3，y，t T_3，z，t]^T。以搜寻智能体的初始位置为原点建立三维坐标系，智能体传感器标定的姿态确定空间中x、y、z轴正方向，受损超宽带节点的反演定位结果是基于搜寻智能体位置的相对坐标。

本实例中搜寻智能体初始位置(m)与速度(m/s)为：

T_0，t＝0＝[1 1 0]^T

v_0，t＝0＝[0 0 0.03]^T

受损智能体真实初始相对坐标(m)为：

在本实例中，任何能够通讯的节点间都会利用UWB信号进行分布式测距。在仿真中，可以通过距离公式计算任意两个节点T_i、T_j间的距离作为测距值使用，距离公式如下所示。

d_ij＝||T_i-T_j||

可以看出，这一公式计算出的测距值是一种理想的情况，不包含噪声和任意扰动。对于实际情况的测距，需要考虑对测距的扰动。对于任意一对能够通讯的节点T_i、T_j，根据对UWB传感器实物的误差标定结果，在距离公式上增加期望为0，标准差为v的高斯白噪声v_ij作为实测距离

S2.根据三维受灾场景内状态信息进行状态方程建立。

建立状态方程如下：

X_t+1＝AX_t+Bu_t+W

式中初始化状态向量X_t为：

状态转移矩阵A＝I₄，输入矩阵B为：

上式中T为采样周期，且T＝1s。

u_t为t时刻输入向量：

u_t＝[v_0，t]

W为过程噪声，其均值为0，协方差Q为：

S3.建立系统观测方程。

定义系统观测方程为：

Z_t+1＝H(X_t+1)+V

式中Z_t为t时刻观测向量，观测方程H(X_t)为：

式。d_ij，t为t时刻节点T_i与T_j的欧氏距离：

d_ij，t＝||T_i，t-T_j，t||

V为观测噪声，其均值为零，协方差R为：

R＝0.03²×I₆

S4.基于UFK方法进行目标超宽带节点的反演定位，以及对搜寻智能体运动轨迹进行预测与校正。

选择无迹卡尔曼滤波参数，其中，

n＝12

α＝0.5

β＝2

λ＝3×α²-n＝-11.25

初始化协方差矩阵P：

按照上述UKF方法，对系统状态进行预测更新，最终实现三维空间内的目标超宽带节点的反演定位，以及对搜寻智能体的运动轨迹校正。

本实例中搜救回收智能体的运动轨迹为(单位：m)，从(1 1 0)点起飞，以速度(单位：m/s)v₀＝(0 0 0.03)上升100s，到达(1 1 3)点。

以速度v₀＝(0.05 0 0.03)运动100s至(6 1 6)点。

以速度v₀＝(0 0.05 -0.03)运动100s至(6 6 3)点。

以速度v₀＝(0 0.05 0.03)运动100s至(11 6 6)点。

以速度v₀＝(0 0.05 -0.03)运动100s至(11 11 3)点。

以速度v₀＝(-0.05 0 0.02)运动200s至(1 11 7)点。

以速度v₀＝(0 0.05 -0.02)运动200s至(1 1 3)点。

最终以速度v₀＝(0 0 -0.03)运动100s返回至初始点(1 1 0)点。其轨迹如图4所示。

直接用节点真实位置的坐标与定位坐标T_i＝[T_i，x T_i，T_i]^T之间的距离表示距离绝对误差E：

该数值实施例中通过模拟真实传感器参数，观察本发明提出方法在实际场景中的有效性。本实施例在图2中的连线测距分别叠加了误差在0-12cm的高斯白噪声。

图4为上述假设场景下搜寻智能体的真实轨迹、里程计/惯性导航模块下轨迹以及UKF预测校正下的运动轨迹对比图，以及受损智能体真实位置与反演定位结果的对比图。图5显示了搜寻过程中目标超宽带节点的反演定位结果的误差变化。图6显示了搜寻过程中搜寻智能体的里程计/惯性导航模块定位误差，及UKF方法执行后运动轨迹预测校正结果误差。

如图4所示，里程计/惯性导航模块定位坐标下搜寻智能体会随着运动产生累计误差，导致轨迹产生偏离。而经UKF方法校正后的轨迹在滤波器收敛后轨迹跟踪效果良好，大致与真实运动轨迹重合且更更加平滑。智能体搜寻任务结束后，UKF反演定位结果收敛于目标超宽带节点真实位置。

如图5所示，目标超宽带节点反演定位误差经过无迹卡尔曼滤波后不断减小，最终收敛于真实位置。本实施例中节点最小误差为0.2171m，总体节点平均误差为0.3986m。

如图6所示，里程计/惯性导航模块定位结果会随着运动距离的增加，产生累计误差，且误差不断增加。而UKF校正后的轨迹误差得到了控制，且轨迹更加平滑。最终里程计误差为0.6674m，而UKF校正轨迹误差为0.2078m。

相较于市场上常用的锚点-标签定位方法，本发明提出的方法可以在室内通过不断移动已知超宽带标签位置，对空间内的未知锚点进行反演定位，并且可以基于反演定位结果对标签的运动轨迹进行预测矫正。

本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合，以及披露的任一新的方法或过程的步骤或任何新的组合。

Claims (3)

1.一种基于超宽带的自主式三维反演定位方法，其特征在于，包括：

移动标签智能体，在所述标签智能体移动过程中，获取多个时刻下所述标签智能体与各目标超宽带节点间的测距信息，以及所述标签智能体的运动信息；

基于多个时刻下的所述测距信息和所述运动信息，反演定位出各所述目标超宽带节点的位置，包括：

基于所述测距信息和运动信息构建系统状态空间模型；

通过无迹卡尔曼滤波方法对所述系统状态空间模型进行解算，反演定位出各所述目标超宽带节点的位置，包括：计算系统状态空间模型中状态的采样sigma点；根据计算的sigma点对状态进行一步预测；对状态的一步预测值做UT变化；根据UT变换值对系统状态空间模型的观测值进行更新；根据更新值确定卡尔曼增益矩阵并更新状态估计值及其协方差。

2.如权利要求1所述的基于超宽带的自主式三维反演定位方法，其特征在于，还包括：根据各所述目标超宽带节点的位置，对所述标签智能体的运动轨迹进行校正。

3.如权利要求1或2所述的基于超宽带的自主式三维反演定位方法，其特征在于，在所述标签智能体移动过程中，以固定周期获取所述测距信息和所述运动信息。