CN112835020B - 面向非视距参数估计的刚体定位方法 - Google Patents

Abstract

一种面向非视距参数估计的刚体定位方法，由提取接收信号强度信息、确定刚体定位的能域参数、构建刚体定位目标函数、确定全局坐标系中刚体节点位置、确定非视距偏差平均值、确定刚体位姿参数组成。本发明根据非视距环境下的刚体定位模型，采用无线信号中接收信号强度信息确定非视距环境中的刚体位姿参数，避免了基于距离测量的刚体定位中所需的时钟同步问题，解决了非视距环境中的刚体定位，同时能确定非视距偏差平均值，保证了非视距环境中刚体定位的鲁棒性。

Description

面向非视距参数估计的刚体定位方法

技术领域

本发明属于通信技术领域，具体涉及到在非视距环境下无线信号的刚体定位。

背景技术

近年来，随着传感器网络以及物联网技术的快速发展，如机器人系统、雷达、声呐等各种智能设备及高科技应用不仅需要获取目标的位置信息，而且需掌控其姿态。然而在实际应用场景中，障碍物的存在对刚体定位性能的影响已经很普遍。由于障碍物的位置和运动状态等存在不确定性，从而引起定位信号的折射、反射和散射等，使得信号传输过程中出现多径效应，导致信号强度测量误差增大，进而使得刚体目标定位精度下降。因此，研究非视距环境下的高精度刚体定位算法就显得极其重要。

目前，刚体定位方法多采用分治法与半正定松弛法，如S.Chen和K.C.Ho等人的《Accurate localization of a rigid body using multiple sensors and landmarks》针对静止与运动两种场景中的刚体定位问题引入分治法，利用欧拉角和加权最小二乘法初始估计出刚体目标的旋转矩阵及位移矢量，再对其进行修正得到刚体位姿参数的最优精度解。J.Jiang和G.Wang等人的《Accurate Rigid Body Localization via SemidefiniteRelaxation Using Range Measurements》基于无线信号的距离测量参数，将半正定松弛法应用于最大似然估计实现了刚体目标的精确定位。

上述现有技术的缺点如下：

(1)基于距离测量的刚体定位技术需要严格的时钟同步问题，难以满足复杂的实际应用环境。

(2)在提取无线信号测量参数方面，现有技术仅研究了视距环境下的刚体定位，未考虑非视距传输对刚体定位性能的影响。

发明内容

本发明所要解决的技术问题在于克服上述现有技术的缺点，提出一种应用在非视距环境下，面向非视距偏差估计的刚体定位方法。

解决上述技术问题所采用的技术方案是由下述步骤组成：

(1)提取接收信号强度信息

根据非视距传输环境下的刚体定位模型，利用锚节点对刚体目标进行定位，定位方法如下：

第m个锚节点从第i个刚体节点发射的无线信号中提取刚体定位中能域的接收信号强度信息P_mi：

其中a_m表示锚节点在三维全局坐标系中的位置坐标，a_m为[x_m,y_m,z_m]^T，m为1,2,…,M，M是锚节点个数，M取值为≥4的正整数；s_i表示刚体节点在三维全局坐标系中的位置坐标，s_i为[x_i,y_i,z_i]^T，i为1,2,…,N，N是刚体节点个数，N取值为≥3的正整数；||*||为欧几里得范数；r₀为单位距离，r₀取值为1m；P_i为第i个刚体节点在单位距离的发射信号强度；k为传输路径损耗，k取值为3；l_mi为接收信号强度中所包含的对数型阴影衰落，服从均值为零、方差为的高斯分布；β_mi为非视距传输偏差。

(2)确定刚体定位的能域参数

按下式确定接收信号强度测量值

其中表示第i个刚体节点至M个锚节点的非视距偏差平均值，/>为/>

(3)构建刚体定位目标函数

采用最小二乘法按下式构建能域内刚体目标定位的最小化函数：

其中ω_mi为能域加权权重。

(4)确定全局坐标系中刚体节点位置

1)将非视距偏差平均值初始化为

2)按下式得到广义信赖域子问题：

其中||W_i(A_iy_i-Z_i)||²为最小化的目标函数，为约束条件，y_i为含有全局坐标系中刚体节点位置的辅助变量，加权矩阵W_i表示为：

目标函数中的矩阵A_i、Z_i分别为：

约束条件中的矩阵B、D分别为：

其中I表示单位矩阵，0表示全零矩阵，K表示矩阵维度，K取值为3。

3)用二分法确定刚体节点位置：

其中是利用二分法所确定的第i个刚体节点在全局坐标系中的位置，/>为

(5)确定非视距偏差平均值

1)用最大似然估计法确定刚体定位中的非视距偏差平均值

2)将得到的非视距偏差平均值代回步骤(4)的步骤1)中，替换已初始化为零的非视距偏差平均值，确定N个刚体节点的定位精度是否满足/>当满足时停止迭代，确定最终得到的非视距偏差平均值和刚体节点位置，否则重复上述操作。

(6)确定刚体位姿参数

1)刚体节点在局部坐标系和全局坐标系的位置关系按下式确定：

其中c_i为第i个刚体节点在局部坐标系的位置，c_i是[x_i,y_i,z_i]^T，Q为刚体旋转矩阵，t为刚体位移矢量。

2)采用最小二乘法确定刚体旋转矩阵Q：

Q＝Vdiag([1^T,det(VU^T)]^T)U^T (7)

q_i＝R_iI^-1

其中，SVD表示奇异值分解，R_i为协方差矩阵的逆矩阵，/>为全局坐标系中N个刚体节点位置坐标的加权平均，/>为局部坐标系中N个刚体节点位置坐标的加权平均。

3)确定刚体位移矢量t：

确定了面向非视距参数估计的刚体位置和方向。

在本发明的提取接收信号强度信息步骤(1)中，所述的M取值为4～10，N取值为3～10，σ_mi取值为0.1～0.6dBm，β_mi取值为0～2dBm。

在本发明的提取接收信号强度信息步骤(1)中，所述的M取值最佳为5，N取值最佳为5，取值最佳为0.1～0.6dBm，β_mi取值最佳为0～2dBm。

本发明采用了提取无线信号中接收信号强度信息，对测量信息中的非视距偏差取平均值确定刚体目标的能域定位参数；根据非视距环境下的刚体定位模型，利用最小二乘法构建刚体定位目标函数；对非视距偏差平均值初始化，将目标函数转变为广义信赖域子问题确定刚体节点位置及非视距偏差平均值；将已确定的刚体节点位置结合刚体定位模型，确定非视距环境中的刚体位姿参数。解决了非视距环境中的刚体定位问题，同时能确定非视距偏差平均值，保证了非视距环境中刚体定位的鲁棒性。

本发明具有以下优点：

(1)采用了无线信号传输的接收信号强度信息进行刚体定位，避免了基于距离测量的刚体定位中所需的时钟同步问题。

(2)考虑实际应用场景中的非视距偏差先验信息未知时，结合刚体定位模型确定非视距偏差信息，有效地应对非视距环境下的刚体定位问题。

附图说明

图1是本发明实施例1的流程图。

图2是在不同测量误差σ_mi情况下，旋转矩阵Q的均方根误差性能曲线。

图3是在不同测量误差σ_mi情况下，位移矢量t的均方根误差性能曲线。

图4是在不同非视距偏差β_mi情况下，旋转矩阵Q的均方根误差性能曲线。

图5是在不同非视距偏差β_mi情况下，位移矢量t的均方根误差性能曲线。

具体实施方式

下面结合附图和实施例对本发明进一步详细说明，但本发明不限于下述的实施例。

实施例1

本实施例面向非视距参数估计的刚体定位方法由下述步骤组成(参见图1)：

(1)提取接收信号强度信息

根据非视距传输环境下的刚体定位模型，利用锚节点对刚体目标进行定位，定位方法如下。

第m个锚节点从第i个刚体节点发射的无线信号中提取刚体定位中能域的接收信号强度信息P_mi：

其中a_m表示锚节点在三维全局坐标系中的位置坐标，a_m为[x_m,y_m,z_m]^T，m为1,2,…,M，M是锚节点个数，M取值为≥4的正整数，本实施例的M取值为5；s_i表示刚体节点在三维全局坐标系中的位置坐标，s_i为[x_i,y_i,z_i]^T，i为1,2,…,N，N是刚体节点个数，N取值为≥3的正整数，本实施例的N取值为5；||*||为欧几里得范数；r₀为单位距离，r₀取值为1m；P_i为第i个刚体节点在单位距离的发射信号强度；k为传输路径损耗，k取值为3；l_mi为接收信号强度中所包含的对数型阴影衰落，服从均值为零、方差为的高斯分布，本实施例的σ_mi取值为0.1～0.6dBm；β_mi为非视距传输偏差，本实施例的β_mi取值为1dBm。

在该步骤中，采用无线信号传输的接收信号强度信息进行刚体定位，避免了基于距离测量的刚体定位中所需的时钟同步问题。

(2)确定刚体定位的能域参数

按下式确定接收信号强度测量值

其中表示第i个刚体节点至M个锚节点的非视距偏差平均值，/>为/>M取值与步骤(1)相同。

(3)构建刚体定位目标函数

采用最小二乘法按下式构建能域内刚体目标定位的最小化函数：

其中ω_mi为能域加权权重。

(4)确定全局坐标系中刚体节点位置

1)将非视距偏差平均值初始化为

2)按下式得到广义信赖域子问题：

其中||W_i(A_iy_i-Z_i)||²为最小化的目标函数，为约束条件，y_i为含有刚体节点位置的辅助变量，加权矩阵W_i表示为：

目标函数中的矩阵A_i、Z_i分别为：

约束条件中的矩阵B、D分别为：

其中I表示单位矩阵，0表示全零矩阵，K表示矩阵维度，K取值为3。

3)用二分法确定刚体节点位置：

其中是利用二分法所确定的第i个刚体节点在全局坐标系中的位置，/>为

(5)确定非视距偏差平均值

1)用最大似然估计法确定刚体定位中的非视距偏差平均值

2)将得到的非视距偏差平均值代回步骤(4)的步骤1)中，替换已初始化为零的非视距偏差平均值，确定N个刚体节点的定位精度是否满足/>当满足时停止迭代，确定最终得到的非视距偏差平均值和刚体节点位置，否则重复上述操作。

在该步骤中，考虑实际应用场景中的非视距偏差先验信息未知时，结合刚体定位模型确定非视距偏差信息，有效地应对非视距环境下的刚体定位问题。

(6)确定刚体位姿参数

1)刚体节点在局部坐标系和全局坐标系的位置关系按下式确定：

其中c_i为第i个刚体节点在局部坐标系的位置，c_i是[x_i,y_i,z_i]^T，Q为刚体的旋转矩阵，t为刚体位移矢量。

2)采用最小二乘法确定刚体旋转矩阵Q：

Q＝Vdiag([1^T,det(VU^T)]^T)U^T (7)

q_i＝R_iI^-1

其中，SVD表示奇异值分解，R_i为协方差矩阵的逆矩阵，/>为全局坐标系中N个刚体节点位置坐标的加权平均，/>为局部坐标系中N个刚体节点位置坐标的加权平均。

3)确定刚体位移矢量t：

确定了面向非视距参数估计的刚体位置和方向。

实施例2

本实施例面向非视距参数估计的刚体定位方法由下述步骤组成：

(1)提取接收信号强度信息

根据非视距传输环境下的刚体定位模型，利用锚节点对刚体目标进行定位，定位方法如下。

第m个锚节点从第i个刚体节点发射的无线信号中提取刚体定位中能域的接收信号强度信息P_mi：

其中a_m表示锚节点在三维全局坐标系中的位置坐标，a_m为[x_m,y_m,z_m]^T，m为1,2,…,M，M是锚节点个数，M取值为≥4的正整数，本实施例的M取值为5；s_i表示刚体节点在三维全局坐标系中的位置坐标，s_i为[x_i,y_i,z_i]^T，i为1,2,…,N，N是刚体节点个数，N取值为≥3的正整数，本实施例的N取值为5；||*||为欧几里得范数；r₀为单位距离，r₀取值为1m；P_i为第i个刚体节点在单位距离的发射信号强度；k为传输路径损耗，k取值为3；l_mi为接收信号强度中所包含的对数型阴影衰落，服从均值为零、方差为的高斯分布，本实施例的σ_mi取值为0.2dBm；β_mi为非视距传输偏差，本实施例的β_mi取值为0.1～0.6dBm。

其它步骤与实施例1相同。确定了面向非视距参数估计的刚体位置和方向。

实施例3

本实施例面向非视距参数估计的刚体定位方法由下述步骤组成：

(1)提取接收信号强度信息

根据非视距传输环境下的刚体定位模型，利用锚节点对刚体目标进行定位，定位方法如下：

第m个锚节点从第i个刚体节点发射的无线信号中提取刚体定位中能域的接收信号强度信息P_mi：

其中a_m表示锚节点在三维全局坐标系中的位置坐标，a_m为[x_m,y_m,z_m]^T，m为1,2,…,M，M是锚节点个数，M取值为≥4的正整数，本实施例的M取值为4；s_i表示刚体节点在三维全局坐标系中的位置坐标，s_i为[x_i,y_i,z_i]^T，i为1,2,…,N，N是刚体节点个数，N取值为≥3的正整数，本实施例的N取值为3；||*||为欧几里得范数；r₀为单位距离，r₀取值为1m；P_i为第i个刚体节点在单位距离的发射信号强度；k为传输路径损耗，k取值为3；l_mi为接收信号强度中所包含的对数型阴影衰落，服从均值为零、方差为的高斯分布，本实施例的σ_mi取值为0.1dBm；β_mi为非视距传输偏差，本实施例的β_mi取值为0dBm。

其它步骤与实施例1相同。确定了面向非视距参数估计的刚体位置和方向。

实施例4

本实施例面向非视距参数估计的刚体定位方法由下述步骤组成：

(1)提取接收信号强度信息

根据非视距传输环境下的刚体定位模型，采用锚节点对刚体目标进行定位，定位方法如下：

第m个锚节点从第i个刚体节点发射的无线信号中提取刚体定位中能域的接收信号强度信息P_mi：

其中a_m表示锚节点在三维全局坐标系中的位置坐标，a_m为[x_m,y_m,z_m]^T，m为1,2,…,M，M是锚节点个数，M取值为≥4的正整数，本实施例的M取值为10；s_i表示刚体节点在三维全局坐标系中的位置坐标，s_i为[x_i,y_i,z_i]^T，i为1,2,…,N，N是刚体节点个数，N取值为≥3的正整数，本实施例的N取值为10；||*||为欧几里得范数；r₀为单位距离，r₀取值为1m；P_i为第i个刚体节点在单位距离的发射信号强度；k为传输路径损耗，k取值为3；l_mi为接收信号强度中所包含的对数型阴影衰落，服从均值为零、方差为的高斯分布，本实施例的σ_mi取值为0.6dBm；β_mi为非视距传输偏差，本实施例的β_mi取值为2dBm。

其它步骤与实施例1相同。确定了面向非视距参数估计的刚体位置和方向。

为了验证本发明的有益效果，发明人采用本发明实施例1、2的面向非视距参数估计的刚体定位方法与分治法(DAC)、半正定松弛法(SDR)进行了仿真对比实验，实验结果如下：

1、实验条件

每次仿真中M个锚节点被随机均匀放置于B×B×B的三维区域内，B取值为30m，刚体节点在局部坐标系中的位置由矩阵C确定：

其中矩阵C中第i列所构成的向量表示第i个刚体节点在局部坐标系中的位置。蒙特卡洛仿真次数为L，其余参数设定：L＝1000、t＝[10,-10,10]^Tm、P_i＝20dBm、k＝3、r₀＝1m、M＝4、N＝5。刚体在全局坐标系中旋转矩阵由Q确定：

该方法的定位性能评价指标为均方误差，其定义为：

其中表示在第l次蒙特卡洛仿真中旋转矩阵的估计值，

其中表示在第l次蒙特卡洛仿真中位移矢量的估计值。

2、仿真实验

(1)仿真实验1

本实验采用实施例1的方法进行仿真实验。非视距偏差β_mi取值为1dBm，对各种方法在不同测量误差σ_mi情况下进行仿真，仿真结果如图2、3所示，图2是刚体目标旋转矩阵Q的均方根误差性能曲线，图3是刚体目标位移矢量t的均方根误差性能曲线，在图2、3中，RSS曲线表示实施例1的方法，DAC曲线表示分治法，SDR曲线表示半正定松弛法，当σ_mi增大时，所有方法对刚体目标旋转矩阵和位移矢量的定位性能都出现了恶化，但仍能证明在非视距环境下，实施例1的方法对于刚体定位性能更优。

(2)仿真实验2

本实验采用实施例2的方法进行仿真实验。测量误差σ_mi取值为0.2dBm，对各种方法在不同非视距偏差β_mi情况下进行仿真，仿真结果如图4、5所示，图4是刚体目标旋转矩阵Q的均方根误差性能曲线，图5是刚体目标位移矢量t的均方根误差性能曲线，在图4、5中，RSS曲线表示实施例1的方法，DAC曲线表示分治法，SDR曲线表示半正定松弛法，当非视距偏差β_mi增大时，所有方法对刚体目标旋转矩阵和位移矢量的定位精度都有所下降，但仍能证明在非视距环境下，实施例2的方法对于刚体定位性能更优。

3、仿真实验结果

综合上述仿真结果和分析，通过对比不同刚体定位方法在非视距环境下的刚体定位性能，验证了本发明方法的有效性、可靠性与鲁棒性，在非视距环境下使用该定位方法，能够有效地确定刚体节点位置及非视距偏差平均值，保证了非视距环境下的刚体定位精度。

Claims (3)

1.一种面向非视距参数估计的刚体定位方法，其特征在于由下述步骤组成：

(1)提取接收信号强度信息

根据非视距传输环境下的刚体定位模型，利用锚节点对刚体目标进行定位，定位方法如下：

第m个锚节点从第i个刚体节点发射的无线信号中提取刚体定位中能域的接收信号强度信息P_mi：

其中a_m表示锚节点在三维全局坐标系中的位置坐标，a_m为[x_m,y_m,z_m]^T，m为1,2,…,M，M是锚节点个数，M取值为≥4的正整数；s_i表示刚体节点在三维全局坐标系中的位置坐标，s_i为[x_i,y_i,z_i]^T，i为1,2,…,N，N是刚体节点个数，N取值为≥3的正整数；||*||为欧几里得范数；r₀为单位距离，r₀取值为1m；P_i为第i个刚体节点在单位距离的发射信号强度；k为传输路径损耗，k取值为3；l_mi为接收信号强度中所包含的对数型阴影衰落，服从均值为零、方差为的高斯分布；β_mi为非视距传输偏差；

(2)确定刚体定位的能域参数

按下式确定接收信号强度测量值

其中表示第i个刚体节点至M个锚节点的非视距偏差平均值，/>为/>

(3)构建刚体定位目标函数

采用最小二乘法按下式构建能域内刚体目标定位的最小化函数：

其中ω_mi为能域加权权重；

(4)确定全局坐标系中刚体节点位置

1)将非视距偏差平均值初始化为

2)按下式得到广义信赖域子问题：

其中||W_i(A_iy_i-Z_i)||²为最小化的目标函数，为约束条件，y_i为含有全局坐标系中刚体节点位置的辅助变量，加权矩阵W_i表示为：

目标函数中的矩阵A_i、Z_i分别为

约束条件中的矩阵B、D分别为

其中I表示单位矩阵，0表示全零矩阵，K表示矩阵维度，K取值为3；

3)用二分法确定刚体节点位置：

其中是利用二分法所确定的第i个刚体节点在全局坐标系中的位置，/>为/>

(5)确定非视距偏差平均值

1)用最大似然估计法确定刚体定位中的非视距偏差平均值

2)将得到的非视距偏差平均值代回步骤(4)的步骤1)中，替换已初始化为零的非视距偏差平均值，确定N个刚体节点的定位精度是否满足/>当满足时停止迭代，确定最终得到的非视距偏差平均值和刚体节点位置，否则重复上述操作；

(6)确定刚体位姿参数

1)刚体节点在局部坐标系和全局坐标系的位置关系按下式确定：

其中c_i为第i个刚体节点在局部坐标系的位置，c_i是[x_i,y_i,z_i]^T，Q为刚体旋转矩阵，t为刚体位移矢量；

2)采用最小二乘法确定刚体旋转矩阵Q：

Q＝Vdiag([1^T,det(VU^T)]^T)U^T (7)

q_i＝R_iI^-1

其中，SVD表示奇异值分解，R_i为协方差矩阵的逆矩阵，/>为全局坐标系中N个刚体节点位置坐标的加权平均，/>为局部坐标系中N个刚体节点位置坐标的加权平均；

3)确定刚体位移矢量t：

确定了面向非视距参数估计的刚体位置和方向。

2.根据权利要求1所述的面向非视距参数估计的刚体定位方法，其特征在于：在提取接收信号强度信息步骤(1)中，所述的M取值为4～10，N取值为3～10，σ_mi取值为0.1～0.6dBm，β_mi取值为0～2dBm。

3.根据权利要求1或2所述的面向非视距参数估计的刚体定位方法，其特征在于：在提取接收信号强度信息步骤(1)中，所述的M取值为5，N取值为5，取值为0.1～0.6dBm，β_mi取值为0～2dBm。