基于到达时间和到达角测量值的刚体定位方法技术

本发明专利技术公开了一种基于到达时间和到达角测量值的刚体定位方法，其对距离测量模型和到达角度测量模型进行转化，并将两者的转化形式一同简化为矩阵形式；根据矩阵形式和旋转矩阵需满足的特殊的正交组构建约束加权最小二乘问题，进而松弛为凸的半正定规划问题；添加更多的约束得到收紧后的半正定规划问题；对收紧后的半正定规划问题进行求解，得到刚体旋转矩阵变量、刚体的位置向量变量、距离偏差变量构成的向量各自的最优估计值；应用正交化操作来保证求解的刚体旋转矩阵变量的最优估计值满足特殊的正交组，得到刚体的旋转角度向量的最优估计值；优点是在锚点和传感器时钟不同步时联合估计刚体的位置、方向和时钟偏移，且在小噪声下定位精度较高。噪声下定位精度较高。噪声下定位精度较高。

【技术实现步骤摘要】
基于到达时间和到达角测量值的刚体定位方法


[0001]本专利技术涉及一种目标定位技术，尤其是涉及一种非同步传感器网络中的基于到达时间和到达角测量值的刚体定位方法。

技术介绍

[0002]随着无线通信技术的迅速发展，无线传感器网络中的目标定位问题受到了广泛关注。目前，目标定位技术已广泛应用于国防建设、国民经济建设、科学研究等领域。传统的目标定位方法感兴趣的目标是点源，只需要定位点源在空间中的位置，点源定位在无线通信、目标跟踪等方面发挥了极其重要的作用。然而，许多新兴的应用，如自动驾驶、宇宙飞船、机器人等，定位过程中不仅要求对目标的位置进行定位，而且还需要定位目标的方向信息，在这种情况下，不能只将目标视为点源，而应将其视为刚体。刚体的形状、大小及其内部点的相对位置在刚体经历平移旋转后保持不变，刚体定位比点源定位更具挑战性。刚体定位的实现方法是：首先采集锚点与安装在刚体上的传感器之间的测量值，然后根据锚点的已知全局位置和安装在刚体上的传感器的已知局部位置来进一步估计刚体的位置和方向信息(即旋转角度信息)，典型的测量值包括到达时间(Time of Arrival，TOA)和到达角度(Angle of Arrival，AOA)。刚体的未知位置和旋转角度信息之间存在强耦合，此外，在测量模型中，刚体的旋转角度以三角函数的形式出现，这些因素使得刚体定位问题成为一个非常难以解决的问题。
[0003]基于TOA的刚体定位方法需要锚点和传感器之间的时钟同步。在实践中，时钟同步可能不容易实现，特别是当刚体上安装了小且低成本的传感器时，在没有时钟同步的情况下，可以采用基于AOA的刚体定位方法，其中AOA测量值可以通过传感器中的一个小的天线阵列或通过一个方向敏感传感器获得，最近的蓝牙v5.1协议提供了一种获取AOA测量值的方法，但基于AOA的刚体定位方法易受到外界环境影响，且测量AOA需要额外的硬件，在硬件尺寸和功耗上不适用于大规模的传感器网络。

技术实现思路

[0004]本专利技术所要解决的技术问题是提供一种非同步传感器网络中的基于到达时间和到达角测量值的刚体定位方法，其在锚点和安装在刚体上的已知局部位置的传感器的时钟不同步的情况下联合估计刚体的位置、方向和时钟偏移导致的距离偏差，避免了由于时钟不同步对定位性能造成的影响，定位精度高。
[0005]本专利技术解决上述技术问题所采用的技术方案为：一种基于到达时间和到达角测量值的刚体定位方法，其特征在于包括以下步骤：
[0006]步骤1：在一个非同步传感器网络中，建立一个三维的坐标系作为参考坐标系，并设定存在M个已知真实的全局位置坐标的锚点和N个分布在刚体上的传感器，N个传感器相对于刚体的局部位置坐标已知，将第m个锚点的已知全局位置坐标记为a
m
，将第i个传感器相对于刚体的已知局部位置坐标记为c
i
，将刚体的位置向量的真实值记为t
o
，t
o
＝[x
o
,y
o
,
z
o
]T
，将刚体的旋转角度向量的真实值记为ω
o
，ω
o
＝[α
o
,β
o
,γ
o
]T
，将第i个传感器在刚体平移和旋转后的全局位置坐标的真实值记为移和旋转后的全局位置坐标的真实值记为其中，M＞1，N＞1，1≤m≤M，1≤i≤N，上标“T”表示向量或矩阵的转置，x
o
,y
o
,z
o
对应表示t
o
的北分量即X轴分量、东分量即Y轴分量和垂直分量即Z轴分量，α
o
,β
o
,γ
o
为ω
o
包含的三个不同方向的旋转角度，α
o
,β
o
,γ
o
对应表示刚体绕着X轴旋转的俯仰角、绕着Y轴旋转的偏航角、绕着Z轴旋转的翻滚角，Q
o
表示由三个不同方向的旋转角度构成的刚体旋转矩阵，表示由三个不同方向的旋转角度构成的刚体旋转矩阵，f
o
＝[(vec(Q
o
))
T
,(t
o
)
T
]T
，符号为克罗内克积符号，I3表示维度为3
×
3的单位矩阵，对应表示中的第1列向量、第2列向量、第3列向量，vec()表示将矩阵转化为向量形式；
[0007]步骤2：收集第i个传感器发射的信号被第m个锚点接收所经历的信号传输时间，即收集第i个传感器发射的信号被第m个锚点接收到的到达时间测量值，记为τ
im
；收集第i个传感器发射的信号被第m个锚点接收到的到达角度测量值，在参考坐标系中该到达角度测量值包括方位角测量值和俯仰角测量值，对应记为θ
im
和φ
im
；然后根据第i个传感器发射的信号被第m个锚点接收到的到达时间测量值τ
im
，计算第i个传感器到第m个锚点的距离测量值，记为d
im
，d
im
为τ
im
与信号传播速度的乘积；其中，1≤m≤M，1≤i≤N；
[0008]步骤3：根据d
im
，构建距离测量模型，描述为：其中，表示第i个传感器到第m个锚点之间不受噪声污染且包含时钟偏移量的距离的真实值，表示第i个传感器和锚点间的时钟偏移导致的距离偏差，n
im
表示d
im
中的测量噪声，n
im
服从均值为零且方差为的高斯分布，即符号“|| ||”为二范数符号，表示第i个传感器与第m个锚点之间不受噪声污染且不包含时钟偏移量的真实距离；
[0009]根据θ
im
和φ
im
，构建到达角度测量模型，描述为：其中，表示第i个传感器发射的信号被第m个锚点接收到的不受噪声污染的方位角的真实值，表示第i个传感器发射的信号被第m个锚点接收到的不受噪声污染的俯仰角的真实值，表示θ
im
中的测量噪声，服从均值为零且方差为的高斯分布，即的高斯分布，即表示φ
im
中的测量噪声，服从均值为零且方差为的高斯分布，即的高斯分布，即对
应表示的北分量即X轴分量、东分量即Y轴分量和垂直分量即Z轴分量，a
mx
、a
my
、a
mz
对应表示a
m
的北分量即X轴分量、东分量即Y轴分量和垂直分量即Z轴分量，arctan2表示四象限反正切函数，arcsin表示反正弦函数；
[0010]步骤4：对步骤3构建的距离测量模型和到达角度测量模型进行转化，距离测量模型的具体转化过程如下：
[0011]步骤4_1a：将中的项移到等式左边，然后等式两边同时平方，等价地转化为：
[0012]步骤4_2本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于到达时间和到达角测量值的刚体定位方法，其特征在于包括以下步骤：步骤1：在一个非同步传感器网络中，建立一个三维的坐标系作为参考坐标系，并设定存在M个已知真实的全局位置坐标的锚点和N个分布在刚体上的传感器，N个传感器相对于刚体的局部位置坐标已知，将第m个锚点的已知全局位置坐标记为a
m
，将第i个传感器相对于刚体的已知局部位置坐标记为c
i
，将刚体的位置向量的真实值记为t
o
，t
o
＝[x
o
,y
o
,z
o
]
T
，将刚体的旋转角度向量的真实值记为ω
o
，ω
o
＝[α
o
,β
o
,γ
o
]
T
，将第i个传感器在刚体平移和旋转后的全局位置坐标的真实值记为旋转后的全局位置坐标的真实值记为其中，M＞1，N＞1，1≤m≤M，1≤i≤N，上标“T”表示向量或矩阵的转置，x
o
,y
o
,z
o
对应表示t
o
的北分量即X轴分量、东分量即Y轴分量和垂直分量即Z轴分量，α
o
,β
o
,γ
o
为ω
o
包含的三个不同方向的旋转角度，α
o
,β
o
,γ
o
对应表示刚体绕着X轴旋转的俯仰角、绕着Y轴旋转的偏航角、绕着Z轴旋转的翻滚角，Q
o
表示由三个不同方向的旋转角度构成的刚体旋转矩阵，表示由三个不同方向的旋转角度构成的刚体旋转矩阵，表示由三个不同方向的旋转角度构成的刚体旋转矩阵，f
o
＝[(vec(Q
o
))
T
,(t
o
)
T
]
T
，符号为克罗内克积符号，I3表示维度为3
×
3的单位矩阵，对应表示中的第1列向量、第2列向量、第3列向量，vec()表示将矩阵转化为向量形式；步骤2：收集第i个传感器发射的信号被第m个锚点接收所经历的信号传输时间，即收集第i个传感器发射的信号被第m个锚点接收到的到达时间测量值，记为τ
im
；收集第i个传感器发射的信号被第m个锚点接收到的到达角度测量值，在参考坐标系中该到达角度测量值包括方位角测量值和俯仰角测量值，对应记为θ
im
和φ
im
；然后根据第i个传感器发射的信号被第m个锚点接收到的到达时间测量值τ
im
，计算第i个传感器到第m个锚点的距离测量值，记为d
im
，d
im
为τ
im
与信号传播速度的乘积；其中，1≤m≤M，1≤i≤N；步骤3：根据d
im
，构建距离测量模型，描述为：其中，表示第i个传感器到第m个锚点之间不受噪声污染且包含时钟偏移量的距离的真实值，∈
io
表示第i个传感器和锚点间的时钟偏移导致的距离偏差，n
im
表示d
im
中的测量噪声，n
im
服从均值为零且方差为的高斯分布，即符号“|| ||”为二范数符号，表示第i个传感器与第m个锚点之间不受噪声污染且不包含时钟偏移量的真实距离；根据θ
im
和φ
im
，构建到达角度测量模型，描述为：其中，表示第i个传感器发射的信号被第m个锚点接收到的不受噪声污染的方位角的真实值，表示第i个
传感器发射的信号被第m个锚点接收到的不受噪声污染的俯仰角的真实值，表示θ
im
中的测量噪声，服从均值为零且方差为的高斯分布，即的高斯分布，即表示φ
im
中的测量噪声，服从均值为零且方差为的高斯分布，即的高斯分布，即对应表示的北分量即X轴分量、东分量即Y轴分量和垂直分量即Z轴分量，a
mx
、a
my
、a
mz
对应表示a
m
的北分量即X轴分量、东分量即Y轴分量和垂直分量即Z轴分量，arctan2表示四象限反正切函数，arcsin表示反正弦函数；步骤4：对步骤3构建的距离测量模型和到达角度测量模型进行转化，距离测量模型的具体转化过程如下：步骤4_1a：将中的项移到等式左边，然后等式两边同时平方，等价地转化为：步骤4_2a：根据矩阵矢量化公式将转化为其中，vec()表示将矩阵转化为向量形式，符号为克罗内克积符号；到达角度测量模型的具体转化过程如下：步骤4_1b：将转化为然后将代入中，得到
步骤4_2b：将转化为然后将一阶泰勒展开式勒展开式代入中，并忽略二阶噪声项和得到其中，E＝[I2,02×1]，I2表示维度为2
×
2的单位矩阵，02×1表示维度为2
×
1的元素全为0的列向量；步骤5：将距离测量模型的转化形式和到达角度测量模型的转化形式一同简化为矩阵形式，表示为：其中，其中，A
11
(m+M
×
(i
‑
1),:)表示矩阵A
11
中的第m+M
×
(i
‑
1)行对应的元素，I
N
表示维度为N
×
N的单位矩阵，1
M
表示维度为M
×
1的元素全为1的列向量，c1表示第1个传感器相对于刚体的已知局部位置坐标，c
N
表示第N个传感器相对于刚体的已知局部位置坐标，diag()为元素对角操作函数，d
11
表示第1个传感器到第1个锚点之间的距离测量值，d
1M
表示第1个传感器到第M个锚点之间的距离测量值，d
N1
表示第N个传感器到第1个锚点之间的距离测量值，d
NM
表示第N个传感器到第M个锚点之间的距离测量值，点之间的距离测量值，A
21
(m+M
×
(i
‑
1),:)表示矩阵A
21
中的第m+M
×
(i
‑
1)行对应的元素，
φ
11
表示第1个传感器发射的信号被第1个锚点接收到的俯仰角测量值，φ
1M
表示第1个传感器发射的信号被第M个锚点接收到的俯仰角测量值，φ
N1
表示第N个传感器发射的信号被第1个锚点接收到的俯仰角测量值，φ
NM
表示第N个传感器发射的信号被第M个锚点接收到的俯仰角测量值，A、A
11
、A
12
、A
13
、A
14
、A
21
、A
31
、A
33
均为引入的系数矩阵，表示由未知量组成的向量，∈
1o
表示第1个传感器和锚点间的时钟偏移导致的距离偏差，表示第N个传感器和锚点间的时钟偏移导致的距离偏差，ε
o
表示由所有传感器和锚点间的时钟偏移导致的距离偏差构成的向量，表示由所有传感器和锚点间的时钟偏移导致的距离偏差构成的向量，b1(m+M
×
(i
‑
1),1)表示向量b1中的第m+M
×
(i
‑
1)行对应的元素，b2(m+M
×
(i
‑
1),1)＝a
mx
sinθ
im
‑
a
my
cosθ
im
，b2(m+M
×
(i
‑
1),1)表示向量b2中的第m+M
×
(i
‑
1)行对应的元素，b3(m+M
×
(i
‑
1),1)＝
‑
d
im sinφ
im
‑
a
...

【专利技术属性】
技术研发人员：王刚，董晓萌，陈华，
申请(专利权)人：宁波大学，
类型：发明
国别省市：