CN114339595A - 一种基于多模型预测的超宽带动态反演定位方法 - Google Patents

Abstract

本发明公开了一种基于多模型预测的超宽带动态反演定位方法，该方法包括：获取主智能体的运动信息和测距信息以及子智能体的测距信息；基于主智能体的运动信息和测距信息以及子智能体的测距信息，利用基于无迹卡尔曼滤波的交互式多模型预测算法对动态的子智能体进行运动模型预测以及动态反演定位。本发明在已知携带超宽带标签的智能体位置以及运动信息时，可以基于超宽带测距信息利用基于无迹卡尔曼滤波的交互式多模型算法对未知空间内动态超宽带锚点(即子智能体)进行运动模型预测并进行反演定位推算。根据未知动态锚点的反演推算结果构建相对定位网络，实现多智能体协同定位。

Description

一种基于多模型预测的超宽带动态反演定位方法

技术领域

本发明涉及技术领域，特别是一种基于多模型预测的超宽带动态反演定位方法。

背景技术

现有存在一种通过超宽带标签来定位超宽带基站的研究方案，其通过已知标签在移动过程中的位置以及测距值来实现，该方法称为反演定位技术。目前大多反演定位技术针对动态标签、静态基站进行研究，而针对超宽带节点完全动态环境研究较少，具有局限性。已有的动态反演定位研究大多要求已知运动节点的运动模型，而针对未知运动模型的节点无法进行反演定位。

发明内容

有鉴于此，本发明提供一种基于多模型预测的超宽带动态反演定位方法，基于超宽带测距信息利用基于无迹卡尔曼滤波的交互式多模型算法对未知空间内动态超宽带锚点(即子智能体)进行运动模型预测并进行反演定位推算。

本发明提供一种基于多模型预测的超宽带动态反演定位方法，包括：

获取主智能体的运动信息和测距信息以及子智能体的测距信息；

基于主智能体的运动信息和测距信息以及子智能体的测距信息，利用基于无迹卡尔曼滤波的交互式多模型预测算法对动态的子智能体进行运动模型预测以及动态反演定位。

优选地，所述基于主智能体的运动信息和测距信息以及子智能体的测距信息，利用基于无迹卡尔曼滤波的交互式多模型预测算法对动态的子智能体进行运动模型预测以及动态反演定位，包括以下步骤：

1)、根据主智能体的运动信息和测距信息以及子智能体的测距信息，建立系统状态空间模型；

2)、通过交互式多模型算法对所述系统状态空间模型进行计算，得到系统中每个模型的状态空间模型的初始状态估计值及相应的协方差矩阵；

3)、根据每个模型的状态空间模型的初始状态估计值及相应的协方差矩阵，对所述每个模型的状态空间模型进行无迹卡尔曼滤波并行滤波，得到更新后的状态估计值及其协方差；

4)、更新系统中每个模型的状态空间模型概率，并基于所述更新后的状态估计值及其协方差进行数据融合。

优选地，当多个智能体分别作为主智能体运行步骤1)至步骤4)时，即可构建协同定位网络，实现多个智能体之间的协同定位。

优选地，还包括：

根据动态反演定位推算，对主智能体的运动轨迹进行预测与校正。

优选地，所述主智能体和子智能体均搭载超宽带模块、里程计或惯性导航模块；所述运动信息由里程计或惯性导航模块提供，所述测距信息由超宽带模块提供。

优选地，所述建立系统状态空间模型包括：

建立系统多模型状态方程；

建立系统观测方程。

优选地，所述建立系统多模型状态方程包括建立匀速模型和建立匀速圆周运动模型。

优选地，所述匀速模型的建立过程为：

输入矩阵B₁为：

其中，T为采样周期，I₃为三维的单位矩阵；

t时刻输入向量u_t：

其中，v_N，t为第N个智能体的在t时刻的速度；

所述匀速圆周运动模型的建立过程为：

输入矩阵B₂为：

τ_i为：

其中，T为采样周期，ω为节点匀速圆周运动角速度；所述主智能体基于所述匀速模型持续匀速运动，所述子智能体基于匀速圆周运动模型做匀速圆周运动。

由于采用了上述技术方案，本发明具有如下的优点：在已知携带超宽带标签的智能体位置以及运动信息时，可以基于超宽带测距信息利用基于无迹卡尔曼滤波的交互式多模型算法对未知空间内动态超宽带锚点(即子智能体)进行运动模型预测并进行反演定位推算。根据未知动态锚点的反演推算结果构建相对定位网络，实现多智能体协同定位。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明实施例中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。

图1为本发明实施例的应用场景示意图；

图2为本发明实施例的传感器网络拓扑示意图；

图3为本发明实施例的交互式多模型预测算法示意图；

图4为本发明实施例的一种基于多模型预测的超宽带动态反演方法的流程示意图；

图5为本发明实施例的主智能体真实轨迹以及子智能体真实位置与动态反演定位图；

图6为本发明实施例的子智能体反演定位误差对比图。

具体实施方式

结合附图和实施例对本发明作进一步说明，显然，所描述的实施例仅是本发明实施例一部分实施例，而不是全部的实施例。本领域普通技术人员所获得的所有其他实施例，都应当属于本发明实施例保护的范围。

实施例一：

本实施例中使用的超宽带设备可以实现分布式测距，不特别区分标签基站，传感器节点之间均可测距。定义P_i，i＝0，1，...，N为智能体节点，N+1为超宽带节点数量。节点均搭载超宽带模块、里程计或惯性导航模块，其中P₀为运行动态反演定位算法的主节点。多智能体进入未知三维空间内，其运动信息由里程计或惯性导航模块提供。定义智能体在t时刻位置为T_i，t＝[T_i，x，t T_i，y，t T_i，z，t]^T，在t时刻速度为v_i，t＝[v_i，x，t v_i，y，t v_i，z，t]^T。上述任意节点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_3(N+1)。

1)匀速(CV)模型。其输入矩阵B₁为：

其中，T为采样周期，I₃为三维的单位矩阵；

u_t为t时刻输入向量：

2)匀速圆周(CTRV)运动模型。其输入矩阵B₂为：

每个智能体的运动模型τ_i为：

其中，T为采样周期，ω为节点匀速圆周运动角速度；所述主智能体基于所述匀速模型持续匀速运动，所述子智能体基于匀速圆周运动模型做匀速圆周运动。

u_t为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)交互式多模型算法

假设系统模式由模型集M＝{m₁，m₂，…，m_r}表示，其中r是子模型个数，那么系统第j(j＝1，...，r)个模型的状态空间模型如下所示：

交互性多模型算法如图3所示。

假设在t时刻第j个模型的状态估计值与相应的协方差矩阵分别是

P_j，t，第j个模型的概率是μ_j，t，模型转移概率矩阵如下所示：

其中，P_ij，i，j＝1，2，...，r表示从模型i转移到模型j的转移概率，且为马尔可夫过程。

计算输入交互概率，有

其中，

为归一化常数。

计算初始状态估计，有

其中，

P_0j，t依次表示第j个模型的初始状态估计值和相应的协方差矩阵；

为第i个模型在t时刻的估计值。

(4)无迹卡尔曼滤波并行滤波

显然，观测方程是非线性的，可以采用UKF方法对目标进行反演定位以及动态轨迹跟踪。所以在对目标运动特性分析后，我们进行UKF滤波解算，具体过程如下：

1、对系统状态

及协方差矩阵P_j，t进行初始化，计算采样Sigma点。

采样点的权重如下：

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

λ＝α²(n+κ)-n

参数

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

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

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

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

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

重复上述1-5的步骤，即可对不同的模型分别进行UKF滤波解算。

(5)概率模型更新并进行数据融合

计算似然函数，有

更新模型概率，有

其中，

为归一化常数。

进行数据融合：

其中

为t+1时刻的状态估计；P_t+1为t+1时刻的状态误差协方差矩阵。

主智能体节点重复以上步骤(1)-(5)，即可通过超宽带测距实现对未知动态智能体节点进行运动模型预测以及动态反演定位。多个智能体分别作为主节点运行此算法，即可构建协同定位网络，实现多机协同定位。

在基于超宽带自主式三维反演定位方法上进行改进，可以在室内通过不断移动已知超宽带标签位置，对空间内未知动态锚点进行运动模型预测以及反演定位，并且可以通过构建相对定位网络实现多智能体协同定位。当系统中某一智能体自身定位系统故障时，可以通过其他智能体动态反演定位结果继续进行导航，提高了系统的鲁棒性。

在地震后为提高搜救效率，保障人员安全，使用多个智能体进入受损建筑内协同执行探索任务。由于室内无GPS信号，且环境复杂影响智能体相互通信，使得难以获取智能体之间的运动模式和相对位置。而超宽带模块具有抗干扰能力强的优点，复杂环境下能保证一定的测距精度，拟仅采用超宽带测距值通过本专利提出算法获取智能体间相对位置，最终实现多机协同定位。

智能体均携带超宽带模块、惯性导航模块，其应用场景如图1所示，其包括1个主智能体和4个智能体。

当1个主智能体和4个智能体进入受灾建筑内后，智能体之间建立通讯，获得相互的距离。主智能体在受灾三维空间内不断移动，通过自身搭载的里程计或惯性导航系统不断生成运动信息，同时结合超宽带测距信息，基于交互式多模型预测方法，实现受灾三维环境内的智能体间相对定位。以每一个智能体为主智能体运行此算法构建相对定位网络，实现多智能体协同定位。

在本实例中应用提供的基于多模型预测的超宽带动态反演定位方法，主智能体通过里程计或惯性导航模块提供的运动信息以及超宽带节点之间的测距与通讯，实现三维空间内的未知动态智能体节点的反演定位。同时，智能体运动距离的增加，会出现累计误差增加的现象，而反演定位结果能够对智能体运动轨迹进行预测与校正，减小累计误差带来的影响。以下将通过具体实例来实施和验证其准确性，技术方案如图4所示，具体过程如下：

步骤一：对三维场景内的多智能体进行建模。运行算法的主智能体节点定义为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 v0_，z，t]^T。四架子智能体节点分别定义为T₁、T₂、T₃、T₄，t时刻位置信息为T_i，t＝[T_i，x，t T_i，y，t T_i，z，t]^T，i＝1，2，3，4，v_i，t＝[v_i，x，t v_i，y，t v_i，z，t]^T，i＝1，2，3，4。以主智能体的初始位置为原点建立三维坐标系，智能体传感器标定的姿态确定空间中x、y、z轴正方向，其余智能体的反演定位结果是基于主智能体位置的相对坐标。本实例中主智能体初始位置(m)与速度(m/s)为：

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

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

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

子智能体初始速度(m/s)为：

v_1，t＝0＝[0 0 0.03×0.9]^T

v_2，t＝0＝[0 0 0.03×0.9]^T

v_3，t＝0＝[0 0 0.03×0.9]^T

v_4，t＝0＝[0 0 0.03×0.9]^T

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

d_ij＝||T_i-T_j||

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

步骤二：根据三维受灾场景内状态信息进行状态方程建立。

建立状态方程如下：

X_t+1＝AX_t+Bu_t+W

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

状态转移矩阵A＝I₁₅，输入矩阵B以模型选择而变化。

1.匀速(CV)模型。其输入矩阵B₁为：

上式中T为采样周期。

u_t为t时刻输入向量：

2.匀速圆周(CTRV)运动模型。其输入矩阵B₂为：

上式中T为采样周期，

为节点匀速圆周运动角速度，

其中主节点持续匀速运动，其余节点做匀速圆周运动。

u_t为t时刻输入向量：

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

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

Q＝[(1×10^-6)²，(4.5×10^-3)²，(1×10^-3)²，(4.5×10^-3)²，(4.5×10^-3)²]I₅

步骤二：建立系统观测方程。

定义系统观测方程为：

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₁₀

步骤三：基于多模型预测的超宽带动态反演定位方法进行子智能体模型预测以及运动轨迹进行预测与校正。

模型转移概率矩阵设置如下：

模型初始概率如下所示：

μ_1，t＝0.5

μ_2，t＝0.5

选择无迹卡尔曼滤波参数其中n＝12：

α＝0.5

β＝2

λ＝3×α²-n＝-11.25

初始化协方差矩阵P：

按照基于多模型预测的超宽带动态反演定位方法对系统状态进行预测更新，最终实现三维空间内的未知动态智能体反演定位，以及对智能体的运动轨迹校正。主智能体实在运行算法中区别于其他智能体名称，并不具有特异性，且主智能体同时也作为其他智能体的反演定位目标，主智能体与子智能体之间可相互转化。所有智能体都运行此算法，构建相对定位网络，实现多机协同定位。

本实例中主智能体的运动轨迹为(单位：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)点。其轨迹如图5所示。

子智能体轨迹形状与主智能体相同，线速度为其0.9倍。

直接用节点真实位置的坐标

与定位坐标T_i＝[T_i，x T_i，T_i]^T之间的距离表示距离绝对误差E：

本数值实例通过模拟真实传感器参数，观察本发明提出方法在实际场景中的有效性。本实例在图2中的连线测距分别叠加了误差在0-12cm的高斯白噪声。图5为三维受灾场景下主智能体真实轨迹以及子智能体真实位置与动态反演定位结果的对比图。图6显示了任务过程中子智能体的反演定位结果的误差变化。

如图5所示，多智能体搜寻任务中，基于多模型预测的动态反演定位结果收敛于受损智能体真实位置。

如图6所示，子智能体节点反演定位误差经过交互式多模型预测算法(如图3所示)滤波后不断减小，最终收敛于真实位置。本发明中节点最小误差为0.0219m，总体节点平均误差为0.0443m。

通过仿真可以说明，相比于市面上常用的锚点-标签定位方案，本实例提出的基于多模型预测的超宽带动态反演定位方法可以在室内通过不断移动已知超宽带标签位置，对空间内未知动态锚点进行反演定位，并且可以对所有超宽节点的运动轨迹进行预测校正。在多智能体上运行此算法，可以实现相对定位网络构建，实现多机协同定位。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，所述计算机可读记录介质包括用于以计算机(例如计算机)可读的形式存储或传送信息的任何机制。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

Claims (8)

1.一种基于多模型预测的超宽带动态反演定位方法，其特征在于，包括：

获取主智能体的运动信息和测距信息以及子智能体的测距信息；

基于主智能体的运动信息和测距信息以及子智能体的测距信息，利用基于无迹卡尔曼滤波的交互式多模型预测算法对动态的子智能体进行运动模型预测以及动态反演定位。

2.根据权利要求1所述的方法，其特征在于，所述基于主智能体的运动信息和测距信息以及子智能体的测距信息，利用基于无迹卡尔曼滤波的交互式多模型预测算法对动态的子智能体进行运动模型预测以及动态反演定位，包括以下步骤：

1)、根据主智能体的运动信息和测距信息以及子智能体的测距信息，建立系统状态空间模型；

2)、通过交互式多模型算法对所述系统状态空间模型进行计算，得到系统中每个模型的状态空间模型的初始状态估计值及相应的协方差矩阵；

3)、根据每个模型的状态空间模型的初始状态估计值及相应的协方差矩阵，对所述每个模型的状态空间模型进行无迹卡尔曼滤波并行滤波，得到更新后的状态估计值及其协方差；

4)、更新系统中每个模型的状态空间模型概率，并基于所述更新后的状态估计值及其协方差进行数据融合。

3.根据权利要求2所述的方法，其特征在于，当多个智能体分别作为主智能体运行步骤1)至步骤4)时，即可构建协同定位网络，实现多个智能体之间的协同定位。

4.根据权利要求1所述的方法，其特征在于，还包括：

根据动态反演定位推算，对主智能体的运动轨迹进行预测与校正。

5.根据权利要求1所述的方法，其特征在于，所述主智能体和子智能体均搭载超宽带模块、里程计或惯性导航模块；所述运动信息由里程计或惯性导航模块提供，所述测距信息由超宽带模块提供。

6.根据权利要求2所述的方法，其特征在于，所述建立系统状态空间模型包括：

建立系统多模型状态方程；

建立系统观测方程。

7.根据权利要求6所述的方法，其特征在于，所述建立系统多模型状态方程包括建立匀速模型和建立匀速圆周运动模型。

8.根据权利要求7所述的方法，其特征在于，所述匀速模型的建立过程为：

输入矩阵B₁为：

其中，T为采样周期，I₃为三维的单位矩阵；

t时刻输入向量u_t：

其中，v_N,t为第N个智能体的在t时刻的速度；

所述匀速圆周运动模型的建立过程为：

输入矩阵B₂为：

τ_i为：

其中，T为采样周期，ω为节点匀速圆周运动角速度；所述主智能体基于所述匀速模型持续匀速运动，所述子智能体基于所述匀速圆周运动模型做匀速圆周运动。

Images