CN109031195B - 一种基于距离和多普勒测量的移动刚体定位方法 - Google Patents

一种基于距离和多普勒测量的移动刚体定位方法 Download PDF

Info

Publication number
CN109031195B
CN109031195B CN201810567210.7A CN201810567210A CN109031195B CN 109031195 B CN109031195 B CN 109031195B CN 201810567210 A CN201810567210 A CN 201810567210A CN 109031195 B CN109031195 B CN 109031195B
Authority
CN
China
Prior art keywords
transpose
denotes
column
row
sdp
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810567210.7A
Other languages
English (en)
Other versions
CN109031195A (zh
Inventor
王刚
蒋坚
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ningbo University
Original Assignee
Ningbo University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ningbo University filed Critical Ningbo University
Priority to CN201810567210.7A priority Critical patent/CN109031195B/zh
Publication of CN109031195A publication Critical patent/CN109031195A/zh
Application granted granted Critical
Publication of CN109031195B publication Critical patent/CN109031195B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/06—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明公开了一种基于距离和多普勒测量的移动刚体定位方法，其充分利用了距离测量模型等式平方后估计参数和已知参数的线性关系，减少半正定规划问题中的优化变量，对刚体的旋转、位置、旋转角速和位移速度进行联合估计，算法性能稳健，即使在比较差的定位环境中，依然有比较高的定位精度；使用的优化变量较少，且优化变量只与要估计的参数有关，减少算法的冗余度，估计结果精度较高；采用半正定规划来估计参数，充分发挥了凸优化方法全局收敛的优势，有效地提高无线网络中移动刚体定位的性能，即使在测量噪声功率比较大的情况下，依然有比较好的估计结果。

Description

一种基于距离和多普勒测量的移动刚体定位方法
技术领域
本发明涉及一种目标定位方法，尤其是涉及一种无线传感器网络中基于距离和多普勒测量的移动刚体(即为有固定形状的移动的物体)定位方法，其定位内容为估计刚体的旋转、位置、旋转角速度和位移速度。
背景技术
近些年来，随着无线传感器技术的快速发展，无线传感器网络(WSN)在与定位、监控、安防等相关的不同领域得到了广泛应用。在很多实际应用中，比如机器人、航空航天、水下探测器等方面，刚体并非总是静止不动的，即不仅需要估计刚体的旋转和位置，还需要对其旋转角速度和位移速度进行估计。因此，对无线传感器网络中移动刚体的定位方法的研究十分有必要。
目前，在无线传感器网络中移动刚体的定位方法中，使用较多的是基于距离和多普勒测量的方法，其优点是测量系统复杂度低，定位精度较高。但是，当无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，现有的基于距离和多普勒测量的方法的定位精度会明显下降。
发明内容
本发明所要解决的技术问题是提供一种无线传感器网络中基于距离和多普勒测量的移动刚体定位方法，其在无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，也能对刚体的旋转、位置、旋转角速度和位移速度进行精确估计。
本发明解决上述技术问题所采用的技术方案为：一种基于距离和多普勒测量的移动刚体定位方法，其特征在于包括以下步骤：
步骤一：设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的未知节点；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，并在刚体的内部设置一个空间坐标系作为局部参考坐标系；将M个锚节点在全局参考坐标系中的坐标位置对应记为a1,…,am,…,aM，将刚体运动前N个未知节点在局部参考坐标系中的坐标位置对应记为c1,…,ci,…,cN；其中，M和N均为正整数，M≥4，N≥3，a1表示第1个锚节点在全局参考坐标系中的坐标位置，m为正整数，1≤m≤M，am表示第m个锚节点在全局参考坐标系中的坐标位置，aM表示第M个锚节点在全局参考坐标系中的坐标位置，c1表示刚体运动前第1个未知节点在局部参考坐标系中的坐标位置，i为正整数，1≤i≤N，ci表示刚体运动前第i个未知节点在局部参考坐标系中的坐标位置，cN表示刚体运动前第N个未知节点在局部参考坐标系中的坐标位置；
步骤二：使刚体一直处于运动状态，在当前时刻，将当前时刻下N个未知节点在全局参考坐标系中的坐标位置对应记为s1,...,si,...,sN；然后获取每个未知节点到各个锚节点的距离测量值，将第i个未知节点到第m个锚节点的距离测量值记为rmi；并获取每个未知节点相对于各个锚节点的多普勒测量值，将第i个未知节点相对于第m个锚节点的多普勒测量值记为
Figure GDA0002428083450000021
其中，s1表示当前时刻下第1个未知节点在全局参考坐标系中的坐标位置，si表示当前时刻下第i个未知节点在全局参考坐标系中的坐标位置，sN表示当前时刻下第N个未知节点在全局参考坐标系中的坐标位置；
步骤三：对当前时刻下每个未知节点在全局参考坐标系中的坐标位置以模型方式进行描述，将si的模型描述为：si＝Qci+t；然后对当前时刻下每个未知节点在全局参考坐标系中的坐标位置的模型求关于时间的导数，对于si＝Qci+t，对si＝Qci+t求关于时间的导数，得到si的导数，记为
Figure GDA0002428083450000031
接着对每个未知节点到各个锚节点的距离测量值以模型方式进行描述，将rmi的模型描述为：
Figure GDA0002428083450000032
并对每个未知节点相对于各个锚节点的多普勒测量值以模型方式进行描述，将
Figure GDA00024280834500000313
的模型描述为：
Figure GDA0002428083450000033
之后对每个未知节点到各个锚节点的距离测量值的模型描述进行形式整理，对于
Figure GDA0002428083450000034
将等式rmi＝||am-Qci-t||+vmi两边同时平方，忽略
Figure GDA0002428083450000035
同时将||am-Qci-t||替换为rmi，整理得到
Figure GDA00024280834500000312
并对每个未知节点相对于各个锚节点的多普勒测量值的模型描述进行形式整理，对于
Figure GDA0002428083450000036
将等式
Figure GDA0002428083450000037
两边同与rmi相乘，忽略
Figure GDA0002428083450000038
整理得到
Figure GDA0002428083450000039
再将
Figure GDA00024280834500000310
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure GDA00024280834500000311
并将
Figure GDA0002428083450000041
堆砌成向量的形式，描述为：
Figure GDA0002428083450000042
最后令
Figure GDA0002428083450000043
和
Figure GDA0002428083450000044
成立，并确定刚体定位问题的约束最小二乘表述形式，描述为：
Figure GDA0002428083450000045
其中，Q表示旋转矩阵，Q的维数为3×3，t表示位置矢量，t代表当前时刻下局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，ω表示刚体的旋转角速度矢量，ω＝[ω1,ω2,ω3]T，符号“[]”为矢量表示符号，[ω1,ω2,ω3]T为[ω1,ω2,ω3]的转置，ω1表示刚体绕X轴旋转的角速度，ω2表示刚体绕Y轴旋转的角速度，ω3表示刚体绕Z轴旋转的角速度，[ω]×表示ω的叉乘矩阵，[ω]×的维数为3×3，
Figure GDA0002428083450000046
表示局部参考坐标系的原点的移动速度，
Figure GDA0002428083450000047
表示第i个未知节点到第m个锚节点的真实距离值，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布
Figure GDA0002428083450000048
表示vmi的功率，符号“|| ||”为求欧几里德范数符号，(si-am)T为(si-am)的转置，
Figure GDA0002428083450000049
表示
Figure GDA00024280834500000410
中存在的测量噪声，
Figure GDA00024280834500000411
服从零均值的高斯分布
Figure GDA00024280834500000412
表示
Figure GDA00024280834500000413
的功率，
Figure GDA00024280834500000414
为ci的转置，
Figure GDA00024280834500000415
为am的转置，符号
Figure GDA00024280834500000416
为克罗内克积运算符号，vec(Q)表示对Q进行矩阵矢量化，QT为Q的转置，
Figure GDA00024280834500000417
为si的转置，(t-am)T为t-am的转置，(Qci+t-am)T为Qci+t-am的转置，d1＝[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]T，[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]T为[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]的转置，
Figure GDA00024280834500000418
r11表示第1个未知节点到第1个锚节点的距离测量值，
Figure GDA00024280834500000419
rM1表示第1个未知节点到第M个锚节点的距离测量值，
Figure GDA00024280834500000420
r12表示第2个未知节点到第1个锚节点的距离测量值，c2表示刚体运动前第2个未知节点在局部参考坐标系中的坐标位置，
Figure GDA00024280834500000421
rM2表示第2个未知节点到第M个锚节点的距离测量值，
Figure GDA00024280834500000422
r1N表示第N个未知节点到第1个锚节点的距离测量值，
Figure GDA0002428083450000051
rMN表示第N个未知节点到第M个锚节点的距离测量值，
Figure GDA0002428083450000052
为[h11,...,hM1,h12,…,hM2,…,h1N,…,hMN]的转置，
Figure GDA0002428083450000053
Figure GDA0002428083450000054
的转置，
Figure GDA0002428083450000055
为c1的转置，
Figure GDA0002428083450000056
为a1的转置，
Figure GDA0002428083450000057
为
Figure GDA0002428083450000058
的转置，
Figure GDA00024280834500000526
为aM的转置，
Figure GDA0002428083450000059
Figure GDA00024280834500000510
为
Figure GDA00024280834500000511
的转置，
Figure GDA00024280834500000512
为c2的转置，
Figure GDA00024280834500000513
为
Figure GDA00024280834500000514
的转置，
Figure GDA00024280834500000515
Figure GDA00024280834500000516
为
Figure GDA00024280834500000517
的转置，
Figure GDA00024280834500000518
为cN的转置，
Figure GDA00024280834500000519
为
Figure GDA00024280834500000520
的转置，
Figure GDA00024280834500000521
[(vec(Q))T,tT,(QTt)T,||t||2]T为
Figure GDA00024280834500000522
的转置，(vec(Q))T为vec(Q)的转置，tT为t的转置，(QTt)T为QTt的转置，
Figure GDA00024280834500000523
[2r11v11,...,2rM1vM1,2r12v12,…,2rM2vM2,…,2r1Nv1N,…,2rMNvMN]T为[2r11v11,...,2rM1vM1,2r12v12,…,2rM2vM2,…,2r1Nv1N,…,2rMNvMN]的转置，v11表示r11中存在的测量噪声，vM1表示rM1中存在的测量噪声，v12表示r12中存在的测量噪声，vM2表示rM2中存在的测量噪声，v1N表示r1N中存在的测量噪声，vMN表示rMN中存在的测量噪声，
Figure GDA00024280834500000524
Figure GDA00024280834500000525
为
Figure GDA0002428083450000061
的转置，
Figure GDA0002428083450000062
表示第1个未知节点相对于第1个锚节点的多普勒测量值，
Figure GDA0002428083450000063
表示第1个未知节点相对于第M个锚节点的多普勒测量值，
Figure GDA0002428083450000064
表示第2个未知节点相对于第1个锚节点的多普勒测量值，
Figure GDA0002428083450000065
表示第2个未知节点相对于第M个锚节点的多普勒测量值，
Figure GDA0002428083450000066
表示第N个未知节点相对于第1个锚节点的多普勒测量值，
Figure GDA0002428083450000067
表示第N个未知节点相对于第M个锚节点的多普勒测量值，
Figure GDA0002428083450000068
为[q11,…,qM1,q12,…,qM2,…,q1N,…,qMN]的转置，
Figure GDA0002428083450000069
Figure GDA00024280834500000610
为
Figure GDA00024280834500000611
的转置，
Figure GDA00024280834500000612
为
Figure GDA00024280834500000613
的转置，
Figure GDA00024280834500000614
为
Figure GDA00024280834500000615
的转置，
Figure GDA00024280834500000616
为
Figure GDA00024280834500000617
的转置，
Figure GDA00024280834500000618
Figure GDA00024280834500000619
为
Figure GDA00024280834500000620
的转置，
Figure GDA00024280834500000621
为
Figure GDA00024280834500000622
的转置，
Figure GDA00024280834500000623
为
Figure GDA00024280834500000624
的转置，P'＝[ω]×Q，P'T为P'的转置，(P'Tt)T为P'Tt的转置，vec(P')表示对P'进行矩阵矢量化，(vec(P'))T为vec(P')的转置，
Figure GDA00024280834500000634
为
Figure GDA00024280834500000625
的转置，
Figure GDA00024280834500000626
为
Figure GDA00024280834500000627
的转置，
Figure GDA00024280834500000628
[n11,...,nM1,n12,…,nM2,…,n1N,…,nMN]T为[n11,...,nM1,n12,…,nM2,…,n1N,…,nMN]的转置，
Figure GDA00024280834500000629
Figure GDA00024280834500000630
表示
Figure GDA00024280834500000631
中存在的测量噪声，
Figure GDA00024280834500000632
表示
Figure GDA00024280834500000633
中存在的测量噪声，
Figure GDA0002428083450000071
表示
Figure GDA0002428083450000072
中存在的测量噪声，
Figure GDA0002428083450000073
表示
Figure GDA0002428083450000074
中存在的测量噪声，
Figure GDA0002428083450000075
表示
Figure GDA0002428083450000076
中存在的测量噪声，
Figure GDA0002428083450000077
表示
Figure GDA0002428083450000078
中存在的测量噪声，min()为取最小值函数，“s.t.”表示“受约束于……”，
Figure GDA0002428083450000079
为
Figure GDA00024280834500000710
的转置，
Figure GDA00024280834500000711
为d1的转置，
Figure GDA00024280834500000712
为d2的转置，
Figure GDA00024280834500000713
为
Figure GDA00024280834500000714
的转置，
Figure GDA00024280834500000715
为H1的转置，
Figure GDA00024280834500000716
为H2的转置，
Figure GDA00024280834500000717
为
Figure GDA00024280834500000718
的转置，
Figure GDA00024280834500000719
为f1的转置，
Figure GDA00024280834500000720
为f2的转置，(d-Hf)T为(d-Hf)的转置，
Figure GDA00024280834500000721
为
Figure GDA00024280834500000722
的逆，
Figure GDA00024280834500000723
Figure GDA00024280834500000724
diag()为对角线矩阵表示形式，
Figure GDA00024280834500000725
[r11,...,rM1,r12,…,rM2,…,r1N,…,rMN]T为[r11,…,rM1,r12,…,rM2,…,r1N,…,rMN]的转置，
Figure GDA00024280834500000726
Figure GDA00024280834500000727
为
Figure GDA00024280834500000728
的转置，KT为K的转置，0(M×N)×(M×N)表示元素全为0的矩阵，其维度为(M×N)×(M×N)，
Figure GDA00024280834500000729
Figure GDA00024280834500000730
表示v11的功率，
Figure GDA00024280834500000731
表示vM1的功率，
Figure GDA00024280834500000732
表示v12的功率，
Figure GDA00024280834500000733
表示vM2的功率，
Figure GDA00024280834500000734
表示v1N的功率，
Figure GDA00024280834500000735
表示vMN的功率，
Figure GDA00024280834500000736
表示
Figure GDA00024280834500000737
的功率，
Figure GDA00024280834500000738
表示
Figure GDA00024280834500000739
的功率，
Figure GDA00024280834500000740
表示多普勒测量噪声
Figure GDA00024280834500000741
的功率，
Figure GDA00024280834500000742
表示多普勒测量噪声
Figure GDA00024280834500000743
的功率，
Figure GDA00024280834500000744
表示
Figure GDA00024280834500000745
的功率，
Figure GDA00024280834500000746
表示
Figure GDA00024280834500000747
的功率，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ＝I和det(Q)＝1为Q需要满足的条件；
步骤四：令F＝ffT，使刚体定位问题的约束最小二乘表述形式中的约束条件QTQ＝I等价于
Figure GDA0002428083450000081
并使f中的QTt形成约束条件
Figure GDA0002428083450000082
使f中的P'Tt形成约束条件
Figure GDA0002428083450000083
使f中的
Figure GDA0002428083450000084
形成约束条件
Figure GDA0002428083450000085
使f中的
Figure GDA0002428083450000086
形成约束条件f(32)＝tr(F(10:12,33:35))；根据(QTt)TQTt＝tTt和||t||2＝ttT，得到约束条件
Figure GDA0002428083450000087
根据
Figure GDA0002428083450000088
和
Figure GDA0002428083450000089
得到约束条件
Figure GDA00024280834500000810
根据(P'Tt)TQTt＝0和
Figure GDA00024280834500000811
得到约束条件
Figure GDA00024280834500000812
根据QTP'是反对称矩阵，得到约束条件
Figure GDA00024280834500000813
然后舍掉刚体定位问题的约束最小二乘表述形式中的约束条件det(Q)＝1，将刚体定位问题的约束最小二乘表述形式转化为：
Figure GDA0002428083450000091
再根据F＝ffT等价于
Figure GDA0002428083450000092
去掉非凸的关于矩阵F的约束rank(F)＝1，将
Figure GDA0002428083450000093
结合到刚体定位问题的约束最小二乘表述形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：
Figure GDA0002428083450000101
最后对刚体定位问题的半正定规划形式进行求解，直接得到Q、t、
Figure GDA0002428083450000102
和P'各自的初步值，对应记为Qsdp、tsdp、
Figure GDA0002428083450000103
和P'sdp；进而根据等式[ω]×＝(P'QT-QP'T)/2间接求得[ω]×的初步值，记为[ωsdp]×，
Figure GDA0002428083450000104
根据等式ω＝[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]T，并结合[ωsdp]×，得到ω的初步值，记为ωsdp，ωsdp＝[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]T；其中，HT为H的转置，dT为d的转置，F为引入的矩阵，F的维数为35×35，fT为f的转置，F(1:3,1:3)表示由F的第1行至第3行、第1列至第3列所有元素形成的矩阵，F(4:6,4:6)表示由F的第4行至第6行、第4列至第6列所有元素形成的矩阵，F(7:9,7:9)表示由F的第7行至第9行、第7列至第9列所有元素形成的矩阵，F(1,4)表示F的第1行第4列元素的值，F(2,5)表示F的第2行第5列元素的值，F(3,6)表示F的第3行第6列元素的值，F(1,7)表示F的第1行第7列元素的值，F(2,8)表示F的第2行第8列元素的值，F(3,9)表示F的第3行第9列元素的值，F(4,7)表示F的第4行第7列元素的值，F(5,8)表示F的第5行第8列元素的值，F(6,9)表示F的第6行第9列元素的值，f(13)表示f中的第13个元素的值，f(14)表示f中的第14个元素的值，f(15)表示f中的第15个元素的值，F(1:3,10:12)表示由F的第1行至第3行、第10列至第12列所有元素形成的矩阵，F(4:6,10:12)表示由F的第4行至第6行、第10列至第12列所有元素形成的矩阵，F(7:9,10:12)表示由F的第7行至第9行、第10列至第12列所有元素形成的矩阵，f(17)表示f中的第17个元素的值，f(18)表示f中的第18个元素的值，f(19)表示f中的第19个元素的值，F(20:22,10:12)表示由F的第20行至第22行、第10列至第12列所有元素形成的矩阵，F(23:25,10:12)表示由F的第23行至第25行、第10列至第12列所有元素形成的矩阵，F(26:28,10:12)表示由F的第26行至第28行、第10列至第12列所有元素形成的矩阵，f(29)表示f中的第29个元素的值，f(30)表示f中的第30个元素的值，f(31)表示f中的第31个元素的值，F(1:3,33:35)表示由F的第1行至第3行、第33列至第35列所有元素形成的矩阵，F(4:6,33:35)表示由F的第4行至第6行、第33列至第35列所有元素形成的矩阵，F(7:9,33:35)表示由F的第7行至第9行、第33列至第35列所有元素形成的矩阵，f(32)表示f中的第32个元素的值，F(10:12,33:35)表示由F的第10行至第12行、第33列至第35列所有元素形成的矩阵，F(10:12,10:12)表示由F的第10行至第12行、第10列至第12列所有元素形成的矩阵，F(13:15,13:15)表示由F的第13行至第15行、第13列至第15列所有元素形成的矩阵，f(16)表示f中的第16个元素的值，F(29:31,29:31)表示由F的第29行至第31行、第29列至第31列所有元素形成的矩阵，F(33:35,33:35)表示由F的第33行至第35行、第33列至第35列所有元素形成的矩阵，F(10:12,33:35)表示由F的第10行至第12行、第33列至第35列所有元素形成的矩阵，F(29:31,13:15)表示由F的第29行至第31行、第13列至第15列所有元素形成的矩阵，
Figure GDA0002428083450000121
为
Figure GDA0002428083450000122
的转置，(P'Tt)T为P'Tt的转置，(P'(QTt))T为P'(QTt)的转置，F(17:19,13:15)表示由F的第17行至第19行、第13列至第15列所有元素形成的矩阵，F(20,13)表示F的第20行第13列的元素，F(23,14)表示F的第23行第14列的元素，F(26,15)表示F的第26行第15列的元素，F(17,1)表示F的第17行第1列的元素，F(18,4)表示F的第18行第4列的元素，F(19,7)表示F的第19行第7列的元素，F(21,13)表示F的第21行第13列的元素，F(24,14)表示F的第24行第14列的元素，F(27,15)表示F的第27行第15列的元素，F(17,2)表示F的第17行第2列的元素，F(18,5)表示F的第18行第5列的元素，F(19,8)表示F的第19行第8列的元素，F(22,13)表示F的第22行第13列的元素，F(25,14)表示F的第25行第14列的元素，F(28,15)表示F的第28行第15列的元素，F(17,3)表示F的第17行第3列的元素，F(18,6)表示F的第18行第6列的元素，F(19,9)表示F的第19行第9列的元素，F(1:3,20:22)表示由F的第1行至第3行、第20列至第22列所有元素形成的矩阵，F(4:6,23:25)表示由F的第4行至第6行、第23列至第25列所有元素形成的矩阵，F(7:9,26:28)表示由F的第7行至第9行、第26列至第28列所有元素形成的矩阵，F(1:3,23:25)表示由F的第1行至第3行、第23列至第25列所有元素形成的矩阵，F(4:6,20:22)表示由F的第4行至第6行、第20列至第22列所有元素形成的矩阵，F(7:9,20:22)表示由F的第7行至第9行、第20列至第22列所有元素形成的矩阵，F(1:3,26:28)表示由F的第1行至第3行、第26列至第28列所有元素形成的矩阵，F(7:9,23:25)表示由F的第7行至第9行、第23列至第25列所有元素形成的矩阵，F(4:6,26:28)表示由F的第4行至第6行、第26列至第28列所有元素形成的矩阵，tr()表示求一个矩阵中的所有对角元素的值的和，符号
Figure GDA0002428083450000139
表示一个矩阵是半正定的，rank()表示求一个矩阵的秩，
Figure GDA0002428083450000131
为Qsdp的转置，
Figure GDA00024280834500001310
为P'sdp的转置，[ω]×(3,2)表示[ω]×中第3行第2列的元素，[ω]×(1,3)表示[ω]×中第1行第3列的元素，[ω]×(2,1)表示[ω]×中第2行第1列的元素，[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]T为[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]的转置，[ωsdp]×表示ωsdp的叉乘矩阵，[ωsdp]×(3,2)表示[ωsdp]×中第3行第2列的元素，[ωsdp]×(1,3)表示[ωsdp]×中第1行第3列的元素，[ωsdp]×(2,1)表示[ωsdp]×中第2行第1列的元素，[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]T为[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]的转置；
步骤五：对Qsdp进行正交化，将正交化后得到的值记为Qort，Qort满足
Figure GDA0002428083450000135
且det(Qort)＝1；然后将Qort作为Q的估计值，将tsdp作为t的估计值，将ωsdp作为ω的估计值，将
Figure GDA0002428083450000137
作为
Figure GDA0002428083450000138
的估计值，将P'sdp作为P'的估计值；其中，
Figure GDA0002428083450000136
为Qort的转置，det(Qort)表示求Qort的行列式。
所述的步骤五执行完毕后，执行以下步骤六和步骤七，具体如下：
步骤六：令Qfin表示Q的最优估计值，令tfin表示t的最优估计值，令ωfin表示ω的最优估计值，令
Figure GDA0002428083450000132
表示
Figure GDA0002428083450000133
的最优估计值；令Qfin＝QortQδ，tfin＝tsdp+Δt，ωfin＝ωsdp+Δω，
Figure GDA0002428083450000134
假设Qδ中的欧拉角都接近于0的前提下，使用近似等式cosx≈1,sinx≈x，x表示欧拉角，则得到Qδ的近似表达式为：
Figure GDA0002428083450000141
然后对Qδ的近似表达式和[ωsdp]×进行线性化，得到vec(Qδ)＝γ+Lβ，vec([ωfin]×)＝Φωfin；接着将Qfin＝QortQδ和tfin＝tsdp+Δt代入rmi＝||am-Qci-t||+vmi中，得到rmi＝||am-Qfinci-tfin||+vmi＝||am-QortQδci-tsdp-Δt||+vmi，将vec(Qδ)＝γ+Lβ代入rmi＝||am-Qfinci-tfin||+vmi＝||am-QortQδci-tsdp-Δt||+vmi中，得到rmi＝||emi-Uig1||+vmi，对rmi＝||emi-Uig1||+vmi的等式右边进行一阶泰勒展开，得到
Figure GDA0002428083450000142
对
Figure GDA0002428083450000143
的两边同乘以||emi||，得到
Figure GDA0002428083450000144
令
Figure GDA0002428083450000145
则有
Figure GDA0002428083450000146
并将Qfin＝QortQδ、tfin＝tsdp+Δt、ωfin＝ωsdp+Δω、
Figure GDA0002428083450000147
vec(Qδ)＝γ+Lβ和vec([ωfin]×)＝Φωfin代入
Figure GDA0002428083450000148
中，整理得到
Figure GDA0002428083450000149
将
Figure GDA00024280834500001410
改写为
Figure GDA00024280834500001411
再将
Figure GDA00024280834500001412
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure GDA00024280834500001413
并将
Figure GDA00024280834500001414
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure GDA00024280834500001415
最后令
Figure GDA00024280834500001416
和
Figure GDA00024280834500001417
均成立，将
Figure GDA00024280834500001418
和
Figure GDA0002428083450000151
堆砌为
Figure GDA0002428083450000152
求解
Figure GDA0002428083450000153
中的g的线性加权最小二乘解，记为
Figure GDA0002428083450000154
Figure GDA0002428083450000155
其中，Qδ表示Q的修正矩阵，Δt表示t的修正矢量，Δω表示ω的修正矢量，
Figure GDA0002428083450000156
表示
Figure GDA0002428083450000157
的修正矢量，θ、ψ和φ均为Qδ中的欧拉角，
Figure GDA0002428083450000158
Figure GDA0002428083450000159
kθ＝sinθ，kψ＝sinψ，kφ＝sinφ，cos为求余弦函数，sin为求正弦函数，vec(Qδ)表示对Qδ进行矩阵矢量化，γ＝[1 0 0 0 1 0 0 0 1]T，[1 0 0 0 1 0 0 0 1]T为[1 0 0 0 1 0 0 0 1]的转置，
Figure GDA00024280834500001510
为
Figure GDA00024280834500001511
的转置，β＝[φ θ ψ]T，[φ θ ψ]T为[φ θ ψ]的转置，vec([ωfin]×)表示对[ωfin]×进行矩阵矢量化，[ωfin]×表示ωfin的叉乘矩阵，
Figure GDA00024280834500001512
为
Figure GDA00024280834500001513
的转置，
Figure GDA00024280834500001514
Figure GDA00024280834500001515
g1=[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，βT为β的转置，ΔtT为Δt的转置，
Figure GDA00024280834500001516
为emi的转置，
Figure GDA00024280834500001517
为引入的变量，
Figure GDA00024280834500001518
为
Figure GDA00024280834500001519
的转置，
Figure GDA00024280834500001520
Figure GDA00024280834500001521
Figure GDA00024280834500001522
Figure GDA0002428083450000161
Figure GDA0002428083450000162
Figure GDA0002428083450000163
Figure GDA0002428083450000164
为
Figure GDA0002428083450000165
的转置，
Figure GDA0002428083450000166
为U1的转置，
Figure GDA0002428083450000167
为U2的转置，
Figure GDA0002428083450000168
为UN的转置，
Figure GDA0002428083450000169
Figure GDA00024280834500001610
g1=[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，
Figure GDA00024280834500001611
[||e11||v11,...,||eM1||vM1,||e12||v12,…,||eM2||vM2,…,||e1N||v1N,…,||eMN||vMN]T为[||e11||v11,...,||eM1||vM1,||e12||v12,…,||eM2||vM2,…,||e1N||v1N,…,||eMN||vMN]的转置，
Figure GDA00024280834500001612
为tsdp的转置，
Figure GDA00024280834500001613
为
Figure GDA00024280834500001614
的转置，
Figure GDA00024280834500001615
为
Figure GDA00024280834500001616
的转置，(Qortci)T为Qortci的转置，(tsdp-am)T为tsdp-am的转置，(Qortci+tsdp-am)T为Qortci+tsdp-am的转置，
Figure GDA00024280834500001617
Figure GDA00024280834500001618
Figure GDA00024280834500001619
表示
Figure GDA00024280834500001620
中存在的测量噪声，
Figure GDA00024280834500001621
Figure GDA00024280834500001622
为
Figure GDA00024280834500001623
的转置，
Figure GDA00024280834500001624
Figure GDA00024280834500001625
为
Figure GDA00024280834500001626
的转置，
Figure GDA0002428083450000171
为
Figure GDA0002428083450000172
的转置，
Figure GDA0002428083450000173
为
Figure GDA0002428083450000174
的转置，
Figure GDA0002428083450000175
为
Figure GDA0002428083450000176
的转置，
Figure GDA0002428083450000177
为
Figure GDA0002428083450000178
的转置，
Figure GDA0002428083450000179
为
Figure GDA00024280834500001710
的转置，
Figure GDA00024280834500001711
为
Figure GDA00024280834500001712
的转置，
Figure GDA00024280834500001713
为
Figure GDA00024280834500001751
的转置，ΔωT为Δω的转置，
Figure GDA00024280834500001714
为
Figure GDA00024280834500001715
的转置，
Figure GDA00024280834500001716
Figure GDA00024280834500001717
0(M×N)×6表示元素全为0的矩阵，其维度为(M×N)×6，
Figure GDA00024280834500001718
Figure GDA00024280834500001719
为
Figure GDA00024280834500001720
的转置，
Figure GDA00024280834500001721
Figure GDA00024280834500001722
[||e11||,…,||eM1||,||e12||,…,||eM2||,…,||e1N||,…,||eMN||]T为[||e11||,...,||eM1||,||e12||,…,||eM2||,…,||e1N||,…,||eMN||]的转置，
Figure GDA00024280834500001723
为
Figure GDA00024280834500001724
的转置，
Figure GDA00024280834500001725
为
Figure GDA00024280834500001726
的逆，
Figure GDA00024280834500001727
为
Figure GDA00024280834500001728
的逆；
步骤七：将
Figure GDA00024280834500001729
代入
Figure GDA00024280834500001730
中，得到
Figure GDA00024280834500001731
进而根据
Figure GDA00024280834500001732
得到β、Δt、Δω和
Figure GDA00024280834500001733
的估计值，对应记为
Figure GDA00024280834500001734
和
Figure GDA00024280834500001735
然后将
Figure GDA00024280834500001736
代入β=[φθψ]T中，得到
Figure GDA00024280834500001737
进而根据
Figure GDA00024280834500001738
得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入
Figure GDA00024280834500001739
中，得到Qδ的估计值，记为
Figure GDA00024280834500001740
最后将
Figure GDA00024280834500001741
代入Qfin＝QortQδ中，得到
Figure GDA00024280834500001742
即得到Qfin的值；将
Figure GDA00024280834500001743
代入tfin＝tsdp+Δt中，得到
Figure GDA00024280834500001744
即得到tfin的值，将
Figure GDA00024280834500001745
代入ωfin＝ωsdp+Δω中，得到
Figure GDA00024280834500001746
即得到ωfin的值，将
Figure GDA00024280834500001747
代入
Figure GDA00024280834500001748
中，得到
Figure GDA00024280834500001749
即得到
Figure GDA00024280834500001750
的值。
与现有技术相比，本发明的优点在于：
1)本发明方法充分利用了距离测量模型等式平方后估计参数和已知参数的线性关系，减少半正定规划问题中的优化变量，对刚体的旋转、位置、旋转角速和位移速度进行联合估计，算法性能稳健，即使在比较差的定位环境中，依然有比较高的定位精度。
2)本发明方法中的优化变量较少，且优化变量只与要估计的参数有关，减少算法的冗余度，估计结果精度较高。
3)本发明方法采用半正定规划来估计参数，充分发挥了凸优化方法全局收敛的优势，有效地提高无线网络中移动刚体定位的性能，即使在测量噪声功率比较大的情况下，依然有比较好的估计结果。
附图说明
图1为本发明方法的总体实现流程框图；
图2a为刚体运动前刚体的内部设置的未知节点在局部参考坐标系中的坐标位置；
图2b为锚节点在全局参考坐标系中的坐标位置和刚体运动后刚体的内部设置的未知节点在全局参考坐标系中的坐标位置；
图3为本发明方法与现有的分拆各个击破的方法的关于旋转矩阵Q的估计值与Q的真实值的均方根误差随测量噪声的标准差增加的变化图；
图4为本发明方法与现有的分拆各个击破的方法的关于位置矢量t的估计值与t的真实值的均方根误差随测量噪声的标准差增加的变化图；
图5为本发明方法与现有的分拆各个击破的方法的关于位置矢量ω的估计值与ω的真实值的均方根误差随测量噪声的标准差增加的变化图；
图6为本发明方法与现有的分拆各个击破的方法的关于位置矢量
Figure GDA0002428083450000181
的估计值与
Figure GDA0002428083450000182
的真实值的均方根误差随测量噪声的标准差增加的变化图。
具体实施方式
以下结合附图实施例对本发明作进一步详细描述。
本发明提出的一种基于距离和多普勒测量的移动刚体定位方法，其总体实现流程框图如图1所示，其包括以下步骤：
步骤一：设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的未知节点；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，并在刚体的内部设置一个空间坐标系作为局部参考坐标系；将M个锚节点在全局参考坐标系中的坐标位置对应记为a1,…,am,…,aM，将刚体运动前N个未知节点在局部参考坐标系中的坐标位置对应记为c1,…,ci,...,cN；其中，M和N均为正整数，M≥4，如取M＝10，N≥3，如取N＝5，a1表示第1个锚节点在全局参考坐标系中的坐标位置，m为正整数，1≤m≤M，am表示第m个锚节点在全局参考坐标系中的坐标位置，aM表示第M个锚节点在全局参考坐标系中的坐标位置，c1表示刚体运动前第1个未知节点在局部参考坐标系中的坐标位置，i为正整数，1≤i≤N，ci表示刚体运动前第i个未知节点在局部参考坐标系中的坐标位置，cN表示刚体运动前第N个未知节点在局部参考坐标系中的坐标位置，c1,...,ci,...,cN已知，由人为设定，图2a给出了刚体运动前刚体的内部设置的未知节点在局部参考坐标系中的坐标位置，图2b给出了锚节点在全局参考坐标系中的坐标位置，刚体运动为刚体旋转或位移或旋转和位移。
步骤二：使刚体一直处于运动状态，在当前时刻，将当前时刻下N个未知节点在全局参考坐标系中的坐标位置对应记为s1,...,si,…,sN；然后采用现有技术获取每个未知节点到各个锚节点的距离测量值，将第i个未知节点到第m个锚节点的距离测量值记为rmi(参见图2b)；并采用现有技术获取每个未知节点相对于各个锚节点的多普勒测量值，将第i个未知节点相对于第m个锚节点的多普勒测量值记为
Figure GDA0002428083450000201
也即为第i个未知节点相对于第m个锚节点的移动速度；其中，s1表示当前时刻下第1个未知节点在全局参考坐标系中的坐标位置，si表示当前时刻下第i个未知节点在全局参考坐标系中的坐标位置，sN表示当前时刻下第N个未知节点在全局参考坐标系中的坐标位置，s1,…,si,…,sN未知，图2b同时给出了刚体运动后刚体的内部设置的未知节点在全局参考坐标系中的坐标位置。
步骤三：对当前时刻下每个未知节点在全局参考坐标系中的坐标位置以模型方式进行描述，将si的模型描述为：si＝Qci+t；然后对当前时刻下每个未知节点在全局参考坐标系中的坐标位置的模型求关于时间的导数，对于si＝Qci+t，对si＝Qci+t求关于时间的导数，得到si的导数，记为
Figure GDA0002428083450000202
接着对每个未知节点到各个锚节点的距离测量值以模型方式进行描述，将rmi的模型描述为：
Figure GDA0002428083450000203
并对每个未知节点相对于各个锚节点的多普勒测量值以模型方式进行描述，将
Figure GDA0002428083450000204
的模型描述为：
Figure GDA0002428083450000205
之后对每个未知节点到各个锚节点的距离测量值的模型描述进行形式整理，对于
Figure GDA0002428083450000206
将等式rmi＝||am-Qci-t||+vmi两边同时平方，忽略
Figure GDA0002428083450000207
同时将||am-Qci-t||替换为rmi，整理得到
Figure GDA0002428083450000208
并对每个未知节点相对于各个锚节点的多普勒测量值的模型描述进行形式整理，对于
Figure GDA0002428083450000209
将等式
Figure GDA00024280834500002010
两边同与rmi相乘(即将测量距离值rmi与多普勒测量值
Figure GDA00024280834500002011
相乘)，忽略
Figure GDA0002428083450000211
整理得到
Figure GDA0002428083450000212
再将
Figure GDA0002428083450000213
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure GDA0002428083450000214
并将
Figure GDA0002428083450000215
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure GDA0002428083450000216
最后令
Figure GDA0002428083450000217
和
Figure GDA0002428083450000218
成立，并确定刚体定位问题的约束最小二乘表述形式，描述为：
Figure GDA0002428083450000219
其中，Q表示旋转矩阵，Q的维数为3×3，t表示位置矢量，t代表当前时刻下局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，ω表示刚体的旋转角速度矢量，ω＝[ω1,ω2,ω3]T，符号“[]”为矢量表示符号，[ω1,ω2,ω3]T为[ω1,ω2,ω3]的转置，ω1表示刚体绕X轴旋转的角速度，ω2表示刚体绕Y轴旋转的角速度，ω3表示刚体绕Z轴旋转的角速度，[ω]×表示ω的叉乘矩阵，[ω]×的维数为3×3，t表示局部参考坐标系的原点的移动速度，
Figure GDA00024280834500002110
表示第i个未知节点到第m个锚节点的真实距离值，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布
Figure GDA00024280834500002111
表示vmi的功率，符号“||||”为求欧几里德范数符号，(si-am)T为(si-am)的转置，
Figure GDA00024280834500002112
表示
Figure GDA00024280834500002113
中存在的测量噪声，
Figure GDA00024280834500002114
服从零均值的高斯分布
Figure GDA00024280834500002115
表示
Figure GDA00024280834500002116
的功率，
Figure GDA00024280834500002117
为ci的转置，
Figure GDA00024280834500002118
为am的转置，符号
Figure GDA00024280834500002120
为克罗内克积运算符号，vec(Q)表示对Q进行矩阵矢量化，QT为Q的转置，si T为si的转置，(t-am)T为t-am的转置，(Qci+t-am)T为Qci+t-am的转置，d1＝[p11,…,pM1,p12,…,pM2,…,p1N,…,pMN]T，[p11,…,pM1,p12,…,pM2,…,p1N,…,pMN]T为[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]的转置，
Figure GDA00024280834500002119
r11表示第1个未知节点到第1个锚节点的距离测量值，
Figure GDA0002428083450000221
rM1表示第1个未知节点到第M个锚节点的距离测量值，
Figure GDA0002428083450000222
r12表示第2个未知节点到第1个锚节点的距离测量值，c2表示刚体运动前第2个未知节点在局部参考坐标系中的坐标位置，
Figure GDA0002428083450000223
rM2表示第2个未知节点到第M个锚节点的距离测量值，
Figure GDA0002428083450000224
r1N表示第N个未知节点到第1个锚节点的距离测量值，
Figure GDA0002428083450000225
rMN表示第N个未知节点到第M个锚节点的距离测量值，H1=[h11,...,hM1,h12,…,hM2,…,h1N,…,hMN]T，[h11,…,hM1,h12,…,hM2,…,h1N,…,hMN]T为[h11,...,hM1,h12,…,hM2,…,h1N,…,hMN]的转置，
Figure GDA0002428083450000226
Figure GDA0002428083450000227
为
Figure GDA0002428083450000228
的转置，
Figure GDA0002428083450000229
为c1的转置，
Figure GDA00024280834500002210
为a1的转置，
Figure GDA00024280834500002211
为
Figure GDA00024280834500002212
的转置，
Figure GDA00024280834500002213
为aM的转置，
Figure GDA00024280834500002214
Figure GDA00024280834500002215
为
Figure GDA00024280834500002216
的转置，
Figure GDA00024280834500002217
为c2的转置，
Figure GDA00024280834500002218
为
Figure GDA00024280834500002219
的转置，
Figure GDA00024280834500002220
Figure GDA00024280834500002221
为
Figure GDA00024280834500002222
的转置，
Figure GDA00024280834500002227
为cN的转置，
Figure GDA00024280834500002223
为
Figure GDA00024280834500002224
的转置，
Figure GDA00024280834500002225
[(vec(Q))T,tT,(QTt)T,t2]T为[(vec(Q))T,tT,(QTt)T,||t||2]的转置，(vec(Q))T为vec(Q)的转置，tT为t的转置，(QTt)T为QTt的转置，
Figure GDA00024280834500002226
[2r11v11,...,2rM1vM1,2r12v12,…,2rM2vM2,…,2r1Nv1N,…,2rMNvMN]T为[2r11v11,...,2rM1vM1,2r12v12,…,2rM2vM2,…,2r1Nv1N,…,2rMNvMN]的转置，v11表示r11中存在的测量噪声，vM1表示rM1中存在的测量噪声，v12表示r12中存在的测量噪声，vM2表示rM2中存在的测量噪声，v1N表示r1N中存在的测量噪声，vMN表示rMN中存在的测量噪声，
Figure GDA0002428083450000231
Figure GDA0002428083450000232
为
Figure GDA0002428083450000233
的转置，
Figure GDA0002428083450000234
表示第1个未知节点相对于第1个锚节点的多普勒测量值，
Figure GDA0002428083450000235
表示第1个未知节点相对于第M个锚节点的多普勒测量值，
Figure GDA0002428083450000236
表示第2个未知节点相对于第1个锚节点的多普勒测量值，
Figure GDA0002428083450000237
表示第2个未知节点相对于第M个锚节点的多普勒测量值，
Figure GDA0002428083450000238
表示第N个未知节点相对于第1个锚节点的多普勒测量值，
Figure GDA0002428083450000239
表示第N个未知节点相对于第M个锚节点的多普勒测量值，H2＝[q11,…,qM1,q12,…,qM2,…,q1N,…,qMN]T，[q11,…,qM1,q12,…,qM2,…,q1N,…,qMN]T为[q11,…,qM1,q12,…,qM2,…,q1N,…,qMN]的转置，
Figure GDA00024280834500002310
Figure GDA00024280834500002311
为
Figure GDA00024280834500002312
的转置，
Figure GDA00024280834500002313
为
Figure GDA00024280834500002314
的转置，
Figure GDA00024280834500002315
为
Figure GDA00024280834500002316
的转置，
Figure GDA00024280834500002317
为
Figure GDA00024280834500002318
的转置，
Figure GDA00024280834500002319
Figure GDA00024280834500002320
为
Figure GDA00024280834500002321
的转置，
Figure GDA00024280834500002322
为
Figure GDA00024280834500002323
的转置，
Figure GDA00024280834500002324
为
Figure GDA00024280834500002325
的转置，P'＝[ω]×Q，P'T为P'的转置，(P'Tt)T为P'Tt的转置，vec(P')表示对P'进行矩阵矢量化，(vec(P'))T为vec(P')的转置，
Figure GDA0002428083450000241
为
Figure GDA0002428083450000242
的转置，
Figure GDA0002428083450000243
为
Figure GDA0002428083450000244
的转置，
Figure GDA0002428083450000245
[n11,...,nM1,n12,…,nM2,…,n1N,…,nMN]T为[n11,...,nM1,n12,…,nM2,…,n1N,…,nMN]的转置，
Figure GDA0002428083450000246
Figure GDA0002428083450000247
表示
Figure GDA0002428083450000248
中存在的测量噪声，
Figure GDA0002428083450000249
表示
Figure GDA00024280834500002410
中存在的测量噪声，
Figure GDA00024280834500002411
表示
Figure GDA00024280834500002412
中存在的测量噪声，
Figure GDA00024280834500002413
表示
Figure GDA00024280834500002414
中存在的测量噪声，
Figure GDA00024280834500002415
表示
Figure GDA00024280834500002416
中存在的测量噪声，
Figure GDA00024280834500002417
表示
Figure GDA00024280834500002418
中存在的测量噪声，min()为取最小值函数，“s.t.”表示“受约束于……”，
Figure GDA00024280834500002419
为
Figure GDA00024280834500002420
的转置，
Figure GDA00024280834500002421
为d1的转置，
Figure GDA00024280834500002422
为d2的转置，
Figure GDA00024280834500002423
为
Figure GDA00024280834500002424
的转置，
Figure GDA00024280834500002425
为H1的转置，
Figure GDA00024280834500002426
为H2的转置，
Figure GDA00024280834500002427
为
Figure GDA00024280834500002428
的转置，
Figure GDA00024280834500002429
为f1的转置，
Figure GDA00024280834500002430
为f2的转置，
(d-Hf)T为(d-Hf)的转置，
Figure GDA00024280834500002431
为
Figure GDA00024280834500002432
的逆，
Figure GDA00024280834500002433
Figure GDA00024280834500002434
diag()为对角线矩阵表示形式，
Figure GDA00024280834500002435
[r11,…,rM1,r12,…,rM2,…,r1N,…,rMN]T为[r11,…,rM1,r12,…,rM2,…,r1N,…,rMN]的转置，
Figure GDA00024280834500002436
Figure GDA00024280834500002437
为
Figure GDA00024280834500002438
的转置，KT为K的转置，0(M×N)×(M×N)表示元素全为0的矩阵，其维度为(M×N)×(M×N)，
Figure GDA00024280834500002439
Figure GDA00024280834500002440
表示v11的功率，
Figure GDA00024280834500002441
表示vM1的功率，
Figure GDA00024280834500002442
表示v12的功率，
Figure GDA00024280834500002443
表示vM2的功率，
Figure GDA00024280834500002444
表示v1N的功率，
Figure GDA00024280834500002445
表示vMN的功率，
Figure GDA00024280834500002446
表示
Figure GDA00024280834500002447
的功率，
Figure GDA00024280834500002448
表示
Figure GDA00024280834500002449
的功率，
Figure GDA00024280834500002450
表示多普勒测量噪声
Figure GDA00024280834500002451
的功率，
Figure GDA00024280834500002452
表示多普勒测量噪声
Figure GDA00024280834500002453
的功率，
Figure GDA00024280834500002454
表示
Figure GDA00024280834500002455
的功率，
Figure GDA00024280834500002456
表示
Figure GDA00024280834500002457
的功率，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ=I和det(Q)=1为Q需要满足的条件。
步骤四：令F=ffT，使刚体定位问题的约束最小二乘表述形式中的约束条件QTQ＝I等价于
Figure GDA0002428083450000251
并使f中的QTt形成约束条件
Figure GDA0002428083450000252
使f中的P'T t形成约束条件
Figure GDA0002428083450000253
使f中的
Figure GDA0002428083450000254
形成约束条件
Figure GDA0002428083450000255
使f中的
Figure GDA0002428083450000256
形成约束条件f(32)＝tr(F(10:12,33:35))；根据(QTt)TQTt＝tTt和||t||2＝ttT，得到约束条件
Figure GDA0002428083450000257
根据
Figure GDA0002428083450000258
和
Figure GDA0002428083450000259
得到约束条件
Figure GDA00024280834500002510
根据(P'Tt)TQTt＝0和(P'Tt)TQT＝-(P'(QTt))T，得到约束条件
Figure GDA00024280834500002511
根据QTP'是反对称矩阵，得到约束条件
Figure GDA00024280834500002512
然后舍掉刚体定位问题的约束最小二乘表述形式中的约束条件det(Q)＝1，将刚体定位问题的约束最小二乘表述形式转化为：
Figure GDA0002428083450000261
再根据F＝ffT等价于
Figure GDA0002428083450000262
去掉非凸的关于矩阵F的约束rank(F)＝1，将
Figure GDA0002428083450000263
结合到刚体定位问题的约束最小二乘表述形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：
Figure GDA0002428083450000271
最后对刚体定位问题的半正定规划形式进行求解，直接得到Q、t、
Figure GDA0002428083450000272
和P'各自的初步值，对应记为Qsdp、tsdp、
Figure GDA0002428083450000273
和P'sdp；进而根据等式[ω]×＝(P'QT-QP'T)/2间接求得[ω]×的初步值，记为[ωsdp]×，
Figure GDA0002428083450000274
根据等式ω＝[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]T，并结合[ωsdp]×，得到ω的初步值，记为ωsdp，ωsdp＝[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]T；其中，HT为H的转置，dT为d的转置，F为引入的矩阵，F的维数为35×35，fT为f的转置，F(1:3,1:3)表示由F的第1行至第3行、第1列至第3列所有元素形成的矩阵，F(4:6,4:6)表示由F的第4行至第6行、第4列至第6列所有元素形成的矩阵，F(7:9,7:9)表示由F的第7行至第9行、第7列至第9列所有元素形成的矩阵，F(1,4)表示F的第1行第4列元素的值，F(2,5)表示F的第2行第5列元素的值，F(3,6)表示F的第3行第6列元素的值，F(1,7)表示F的第1行第7列元素的值，F(2,8)表示F的第2行第8列元素的值，F(3,9)表示F的第3行第9列元素的值，F(4,7)表示F的第4行第7列元素的值，F(5,8)表示F的第5行第8列元素的值，F(6,9)表示F的第6行第9列元素的值，f(13)表示f中的第13个元素的值，f(14)表示f中的第14个元素的值，f(15)表示f中的第15个元素的值，F(1:3,10:12)表示由F的第1行至第3行、第10列至第12列所有元素形成的矩阵，F(4:6,10:12)表示由F的第4行至第6行、第10列至第12列所有元素形成的矩阵，F(7:9,10:12)表示由F的第7行至第9行、第10列至第12列所有元素形成的矩阵，f(17)表示f中的第17个元素的值，f(18)表示f中的第18个元素的值，f(19)表示f中的第19个元素的值，F(20:22,10:12)表示由F的第20行至第22行、第10列至第12列所有元素形成的矩阵，F(23:25,10:12)表示由F的第23行至第25行、第10列至第12列所有元素形成的矩阵，F(26:28,10:12)表示由F的第26行至第28行、第10列至第12列所有元素形成的矩阵，f(29)表示f中的第29个元素的值，f(30)表示f中的第30个元素的值，f(31)表示f中的第31个元素的值，F(1:3,33:35)表示由F的第1行至第3行、第33列至第35列所有元素形成的矩阵，F(4:6,33:35)表示由F的第4行至第6行、第33列至第35列所有元素形成的矩阵，F(7:9,33:35)表示由F的第7行至第9行、第33列至第35列所有元素形成的矩阵，f(32)表示f中的第32个元素的值，F(10:12,33:35)表示由F的第10行至第12行、第33列至第35列所有元素形成的矩阵，F(10:12,10:12)表示由F的第10行至第12行、第10列至第12列所有元素形成的矩阵，F(13:15,13:15)表示由F的第13行至第15行、第13列至第15列所有元素形成的矩阵，f(16)表示f中的第16个元素的值，F(29:31,29:31)表示由F的第29行至第31行、第29列至第31列所有元素形成的矩阵，F(33:35,33:35)表示由F的第33行至第35行、第33列至第35列所有元素形成的矩阵，F(10:12,33:35)表示由F的第10行至第12行、第33列至第35列所有元素形成的矩阵，F(29:31,13:15)表示由F的第29行至第31行、第13列至第15列所有元素形成的矩阵，
Figure GDA0002428083450000291
为
Figure GDA0002428083450000292
的转置，(P'Tt)T为P'Tt的转置，(P'(QTt))T为P'(QTt)的转置，F(17:19,13:15)表示由F的第17行至第19行、第13列至第15列所有元素形成的矩阵，F(20,13)表示F的第20行第13列的元素，F(23,14)表示F的第23行第14列的元素，F(26,15)表示F的第26行第15列的元素，F(17,1)表示F的第17行第1列的元素，F(18,4)表示F的第18行第4列的元素，F(19,7)表示F的第19行第7列的元素，F(21,13)表示F的第21行第13列的元素，F(24,14)表示F的第24行第14列的元素，F(27,15)表示F的第27行第15列的元素，F(17,2)表示F的第17行第2列的元素，F(18,5)表示F的第18行第5列的元素，F(19,8)表示F的第19行第8列的元素，F(22,13)表示F的第22行第13列的元素，F(25,14)表示F的第25行第14列的元素，F(28,15)表示F的第28行第15列的元素，F(17,3)表示F的第17行第3列的元素，F(18,6)表示F的第18行第6列的元素，F(19,9)表示F的第19行第9列的元素，F(1:3,20:22)表示由F的第1行至第3行、第20列至第22列所有元素形成的矩阵，F(4:6,23:25)表示由F的第4行至第6行、第23列至第25列所有元素形成的矩阵，F(7:9,26:28)表示由F的第7行至第9行、第26列至第28列所有元素形成的矩阵，F(1:3,23:25)表示由F的第1行至第3行、第23列至第25列所有元素形成的矩阵，F(4:6,20:22)表示由F的第4行至第6行、第20列至第22列所有元素形成的矩阵，F(7:9,20:22)表示由F的第7行至第9行、第20列至第22列所有元素形成的矩阵，F(1:3,26:28)表示由F的第1行至第3行、第26列至第28列所有元素形成的矩阵，F(7:9,23:25)表示由F的第7行至第9行、第23列至第25列所有元素形成的矩阵，F(4:6,26:28)表示由F的第4行至第6行、第26列至第28列所有元素形成的矩阵，tr()表示求一个矩阵中的所有对角元素的值的和，符号
Figure GDA0002428083450000309
表示一个矩阵是半正定的，rank()表示求一个矩阵的秩，
Figure GDA0002428083450000301
为Qsdp的转置，
Figure GDA0002428083450000302
为P'sdp的转置，[ω]×(3,2)表示[ω]×中第3行第2列的元素，[ω]×(1,3)表示[ω]×中第1行第3列的元素，[ω]×(2,1)表示[ω]×中第2行第1列的元素，[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]T为[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]的转置，[ωsdp]×表示ωsdp的叉乘矩阵，[ωsdp]×(3,2)表示[ωsdp]×中第3行第2列的元素，[ωsdp]×(1,3)表示[ωsdp]×中第1行第3列的元素，[ωsdp]×(2,1)表示[ωsdp]×中第2行第1列的元素，[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]T为[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]的转置。
步骤五：由于在步骤四中忽略了约束条件det(Q)＝1，因此刚体定位问题的半正定规划形式关于Q的求解结果Qsdp并不准确，甚至不能满足旋转矩阵的性质，即可能出现
Figure GDA0002428083450000303
或者det(Qsdp)＜0的情况，
Figure GDA0002428083450000304
为Qsdp的转置，det(Qsdp)表示求Qsdp的行列式，故在本步骤中采用现有技术对Qsdp进行正交化，将正交化后得到的值记为Qort，Qort满足
Figure GDA0002428083450000305
且det(Qort)＝1；然后将Qort作为Q的估计值，将tsdp作为t的估计值，将ωsdp作为ω的估计值，将
Figure GDA0002428083450000306
作为
Figure GDA0002428083450000307
的估计值，将P'sdp作为P'的估计值；其中，
Figure GDA0002428083450000308
为Qort的转置，det(Qort)表示求Qort的行列式。
在此，对Qsdp进行正交化所采用的双重迭代算法为：
Figure GDA0002428083450000311
当abs(det(Xn+1)-1)＜10-4或者迭代次数达到设定上限时，迭代终止，其中，X0表示迭代初始值，Xn表示第n迭代得到的值，Xn+1表示第n+1迭代得到的值，abs()为求绝对值函数。
步骤六：令Qfin表示Q的最优估计值，令tfin表示t的最优估计值，令ωfin表示ω的最优估计值，令
Figure GDA0002428083450000312
表示
Figure GDA0002428083450000313
的最优估计值；令Qfin＝QortQδ，tfin＝tsdp+Δt，ωfin=ωsdp+Δω，
Figure GDA0002428083450000314
如果Qδ满足旋转矩阵的性质，则Qfin也满足，因此在合理的假设Qδ中的欧拉角都接近于0的前提下，使用近似等式cosx≈1,sinx≈x，x表示欧拉角，则得到Qδ的近似表达式为：
Figure GDA0002428083450000315
然后对Qδ的近似表达式和[ωsdp]×进行线性化，得到vec(Qδ)=γ+Lβ，vec([ωfin]×)=Φωfin；接着将Qfin＝QortQδ和tfin＝tsdp+Δt代入rmi＝||am-Qci-t||+vmi中，得到rmi＝||am-Qfinci-tfin||+vmi＝||am-QortQδci-tsdp-Δt||+vmi，将vec(Qδ)＝γ+Lβ代入rmi=||am-Qfinci-tfin||+vmi=||am-QortQδci-tsdp-Δt||+vmi中，得到rmi＝||emi-Uig1||+vmi，对rmi＝||emi-Uig1||+vmi的等式右边进行一阶泰勒展开，得到
Figure GDA0002428083450000316
对
Figure GDA0002428083450000317
的两边同乘以||emi||，得到
Figure GDA0002428083450000318
令
Figure GDA0002428083450000319
则有
Figure GDA00024280834500003110
并将Qfin＝QortQδ、tfin＝tsdp+Δt、ωfin＝ωsdp+Δω、
Figure GDA0002428083450000321
vec(Qδ)＝γ+Lβ和vec([ωfin]×)=Φωfin代入
Figure GDA0002428083450000322
中，整理得到
Figure GDA0002428083450000323
将
Figure GDA0002428083450000324
改写为
Figure GDA0002428083450000325
再将
Figure GDA0002428083450000326
i=1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure GDA0002428083450000327
并将
Figure GDA0002428083450000328
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure GDA0002428083450000329
最后令
Figure GDA00024280834500003210
和
Figure GDA00024280834500003211
均成立，将
Figure GDA00024280834500003212
和
Figure GDA00024280834500003213
堆砌为
Figure GDA00024280834500003214
求解
Figure GDA00024280834500003215
中的g的线性加权最小二乘解，记为
Figure GDA00024280834500003216
Figure GDA00024280834500003217
其中，Qδ表示Q的修正矩阵，Δt表示t的修正矢量，Δω表示ω的修正矢量，
Figure GDA00024280834500003218
表示
Figure GDA00024280834500003219
的修正矢量，θ、ψ和φ均为Qδ中的欧拉角，
Figure GDA00024280834500003220
Figure GDA00024280834500003221
kθ＝sinθ，kψ＝sinψ，kφ＝sinφ，cos为求余弦函数，sin为求正弦函数，vec(Qδ)表示对Qδ进行矩阵矢量化，γ＝[1 0 0 0 1 0 0 0 1]T，[1 0 0 0 1 0 0 0 1]T为[1 0 0 0 1 0 0 0 1]的转置，
Figure GDA00024280834500003222
为
Figure GDA00024280834500003223
的转置，β＝[φ θ ψ]T，[φ θ ψ]T为[φ θ ψ]的转置，vec([ωfin]×)表示对[ωfin]×进行矩阵矢量化，[ωfin]×表示ωfin的叉乘矩阵，
Figure GDA0002428083450000331
为
Figure GDA0002428083450000332
的转置，
Figure GDA0002428083450000333
Figure GDA0002428083450000334
g1＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，βT为β的转置，ΔtT为Δt的转置，
Figure GDA0002428083450000335
为emi的转置，
Figure GDA0002428083450000336
为引入的变量，
Figure GDA0002428083450000337
为
Figure GDA0002428083450000338
的转置，
Figure GDA0002428083450000339
Figure GDA00024280834500003310
Figure GDA00024280834500003311
Figure GDA00024280834500003312
Figure GDA00024280834500003313
Figure GDA00024280834500003314
Figure GDA00024280834500003315
为
Figure GDA00024280834500003316
的转置，
Figure GDA00024280834500003317
为U1的转置，
Figure GDA00024280834500003318
为U2的转置，
Figure GDA00024280834500003319
为UN的转置，
Figure GDA00024280834500003320
Figure GDA00024280834500003321
g1＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，
Figure GDA00024280834500003322
[||e11||v11,…,||eM1||vM1,||e12||v12,…,||eM2||vM2,…,||e1N||v1N,…,||eMN||vMN]T为[||e11||v11,…,||eM1||vM1,||e12||v12,…,||eM2||vM2,…,||e1N||v1N,…,||eMN||vMN]的转置，
Figure GDA00024280834500003323
为tsdp的转置，
Figure GDA0002428083450000341
为
Figure GDA0002428083450000342
的转置，
Figure GDA0002428083450000343
为
Figure GDA0002428083450000344
的转置，(Qortci)T为Qortci的转置，(tsdp-am)T为tsdp-am的转置，(Qortci+tsdp-am)T为Qortci+tsdp-am的转置，
Figure GDA0002428083450000345
Figure GDA0002428083450000346
表示
Figure GDA0002428083450000347
中存在的测量噪声，
Figure GDA0002428083450000348
Figure GDA0002428083450000349
为
Figure GDA00024280834500003410
的转置，
Figure GDA00024280834500003411
Figure GDA00024280834500003412
为
Figure GDA00024280834500003413
的转置，
Figure GDA00024280834500003414
为
Figure GDA00024280834500003415
的转置，
Figure GDA00024280834500003416
为
Figure GDA00024280834500003417
的转置，
Figure GDA00024280834500003418
为
Figure GDA00024280834500003419
的转置，
Figure GDA00024280834500003420
为
Figure GDA00024280834500003421
的转置，
Figure GDA00024280834500003422
为
Figure GDA00024280834500003423
的转置，
Figure GDA00024280834500003424
为
Figure GDA00024280834500003425
的转置，
Figure GDA00024280834500003426
为
Figure GDA00024280834500003427
的转置，ΔωT为Δω的转置，
Figure GDA00024280834500003428
为
Figure GDA00024280834500003429
的转置，
Figure GDA00024280834500003430
Figure GDA00024280834500003431
0(M×N)×6表示元素全为0的矩阵，其维度为(M×N)×6，
Figure GDA00024280834500003432
为
Figure GDA00024280834500003433
的转置，
Figure GDA00024280834500003434
Figure GDA00024280834500003435
Figure GDA00024280834500003436
Figure GDA00024280834500003437
[||e11||,...,||eM1||,||e12||,…,||eM2||,…,||e1N||,…,||eMN||]T为[||e11||,...,||eM1||,||e12||,…,||eM2||,…,||e1N||,…,||eMN||]的转置，
Figure GDA00024280834500003438
为
Figure GDA00024280834500003439
的转置，
Figure GDA00024280834500003440
为
Figure GDA00024280834500003441
的逆，
Figure GDA00024280834500003442
为
Figure GDA00024280834500003443
的逆。
步骤七：将
Figure GDA0002428083450000351
代入
Figure GDA0002428083450000352
中，得到
Figure GDA0002428083450000353
进而根据
Figure GDA0002428083450000354
得到β、Δt、Δω和
Figure GDA0002428083450000355
的估计值，对应记为
Figure GDA0002428083450000356
和
Figure GDA0002428083450000357
然后将
Figure GDA0002428083450000358
代入β＝[φθψ]T中，得到
Figure GDA0002428083450000359
进而根据
Figure GDA00024280834500003510
得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入
Figure GDA00024280834500003511
中，得到Qδ的估计值，记为
Figure GDA00024280834500003512
最后将
Figure GDA00024280834500003513
代入Qfin=QortQδ中，得到
Figure GDA00024280834500003514
即得到Qfin的值；将
Figure GDA00024280834500003515
代入tfin＝tsdp+Δt中，得到
Figure GDA00024280834500003516
即得到tfin的值，将
Figure GDA00024280834500003517
代入ωfin=ωsdp+Δω中，得到
Figure GDA00024280834500003518
即得到ωfin的值，将
Figure GDA00024280834500003519
代入
Figure GDA00024280834500003520
中，得到
Figure GDA00024280834500003521
即得到
Figure GDA00024280834500003522
的值。
上述步骤六和步骤七的过程是为了进一步提高定位精度，对已求得的Qort、tsdp、ωsdp和
Figure GDA00024280834500003523
进行优化。
为了验证本发明方法的可行性和有效性，对本发明方法进行仿真试验。
假设刚体的内部放置了N＝5个未知节点，其相对于刚体的内部设置的局部参考坐标系的坐标位置分别为矩阵
Figure GDA00024280834500003524
的各列。无线传感器网络中放置了M＝6个锚节点，其位置均随机分布在长、宽、高对应为200米、100米、30米的长方体内(锚节点分布比较差)，该长方体的中心位置的坐标在全局参考坐标系中为[0,-50,-85]T，[0,-50,-85]T为[0,-50,-85]的转置。刚体的旋转和位移设置如下：假设初始状态下局部参考坐标系和全局参考坐标系重合，即在全局参考坐标系中刚体的内部的未知节点的初始位置的坐标就是其在局部参考坐标系下的坐标位置；刚体相对于X,Y,Z轴的旋转角度分别为20度、-25度和10度；位置矢量t＝[50,50,20]T，[50,50,20]T为[50,50,20]的转置；刚体的旋转角速度ω＝[0.1,0.2,0.3]T，[0.1,0.2,0.3]T为[0.1,0.2,0.3]的转置；刚体对应的相对移动速度为
Figure GDA0002428083450000361
[1,1,1]T为[1,1,1]的转置。假设同一个锚节点到所有未知节点的测量距离中存在的测量噪声的功率一致，不同的锚节点到未知节点的测量距离中存在的测量噪声的功率不同。设定未知节点到不同锚节点的距离测量值中存在的测量噪声的标准差分别为
Figure GDA0002428083450000362
设定未知节点相对于不同锚节点的多普勒测量值中存在的测量噪声的标准差分别为
Figure GDA0002428083450000363
σ表示仿真时参考的测量噪声的标准差，σ∈[0.01,10]。
测试本发明方法的性能随测量噪声的增加的变化情况。图3给出了本发明方法与现有的分拆各个击破的方法的关于旋转矩阵Q的估计值与Q的真实值的均方根误差随测量噪声的标准差增加的变化图；图4给出了本发明方法与现有的分拆各个击破的方法的关于位置矢量t的估计值与t的真实值的均方根误差随测量噪声的标准差增加的变化图；图5给出了本发明方法与现有的分拆各个击破的方法的关于位置矢量ω的估计值与ω的真实值的均方根误差随测量噪声的标准差增加的变化图；图6给出了本发明方法与现有的分拆各个击破的方法的关于位置矢量
Figure GDA0002428083450000365
的估计值与
Figure GDA0002428083450000366
的真实值的均方根误差随测量噪声的标准差增加的变化图。
从图3到图6中可以看出，在无线传感器网络中锚节点分布比较差和无线传感器网络中噪声功率在中等到较大的水平时，在Q、t、ω和
Figure GDA0002428083450000364
的估计中，本发明方法明显优于现有的分拆各个击破的方法，足以说明本发明方法在移动刚体定位的精度方面有足够的优势。

Claims (2)

1.一种基于距离和多普勒测量的移动刚体定位方法，其特征在于包括以下步骤：
步骤一：设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的未知节点；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，并在刚体的内部设置一个空间坐标系作为局部参考坐标系；将M个锚节点在全局参考坐标系中的坐标位置对应记为a1,...,am,...,aM，将刚体运动前N个未知节点在局部参考坐标系中的坐标位置对应记为c1,...,ci,...,cN；其中，M和N均为正整数，M≥4，N≥3，a1表示第1个锚节点在全局参考坐标系中的坐标位置，m为正整数，1≤m≤M，am表示第m个锚节点在全局参考坐标系中的坐标位置，aM表示第M个锚节点在全局参考坐标系中的坐标位置，c1表示刚体运动前第1个未知节点在局部参考坐标系中的坐标位置，i为正整数，1≤i≤N，ci表示刚体运动前第i个未知节点在局部参考坐标系中的坐标位置，cN表示刚体运动前第N个未知节点在局部参考坐标系中的坐标位置；
步骤二：使刚体一直处于运动状态，在当前时刻，将当前时刻下N个未知节点在全局参考坐标系中的坐标位置对应记为s1,...,si,...,sN；然后获取每个未知节点到各个锚节点的距离测量值，将第i个未知节点到第m个锚节点的距离测量值记为rmi；并获取每个未知节点相对于各个锚节点的多普勒测量值，将第i个未知节点相对于第m个锚节点的多普勒测量值记为
Figure FDA0002428083440000011
其中，s1表示当前时刻下第1个未知节点在全局参考坐标系中的坐标位置，si表示当前时刻下第i个未知节点在全局参考坐标系中的坐标位置，sN表示当前时刻下第N个未知节点在全局参考坐标系中的坐标位置；
步骤三：对当前时刻下每个未知节点在全局参考坐标系中的坐标位置以模型方式进行描述，将si的模型描述为：si＝Qci+t；然后对当前时刻下每个未知节点在全局参考坐标系中的坐标位置的模型求关于时间的导数，对于si＝Qci+t，对si＝Qci+t求关于时间的导数，得到si的导数，记为
Figure FDA0002428083440000021
Figure FDA0002428083440000022
接着对每个未知节点到各个锚节点的距离测量值以模型方式进行描述，将rmi的模型描述为：
Figure FDA0002428083440000023
并对每个未知节点相对于各个锚节点的多普勒测量值以模型方式进行描述，将
Figure FDA0002428083440000024
的模型描述为：
Figure FDA0002428083440000025
之后对每个未知节点到各个锚节点的距离测量值的模型描述进行形式整理，对于
Figure FDA0002428083440000026
将等式rmi＝||am-Qci-t||+vmi两边同时平方，忽略
Figure FDA0002428083440000027
同时将||am-Qci-t||替换为rmi，整理得到
Figure FDA0002428083440000028
并对每个未知节点相对于各个锚节点的多普勒测量值的模型描述进行形式整理，对于
Figure FDA0002428083440000029
将等式
Figure FDA00024280834400000210
两边同与rmi相乘，忽略
Figure FDA00024280834400000211
整理得到
Figure FDA00024280834400000212
再将
Figure FDA00024280834400000213
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure FDA00024280834400000214
并将
Figure FDA00024280834400000215
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure FDA00024280834400000216
最后令
Figure FDA00024280834400000217
和
Figure FDA00024280834400000218
成立，并确定刚体定位问题的约束最小二乘表述形式，描述为：
Figure FDA00024280834400000219
其中，Q表示旋转矩阵，Q的维数为3×3，t表示位置矢量，t代表当前时刻下局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，ω表示刚体的旋转角速度矢量，ω＝[ω1,ω2,ω3]T，符号“[]”为矢量表示符号，[ω1,ω2,ω3]T为[ω1,ω2,ω3]的转置，ω1表示刚体绕X轴旋转的角速度，ω2表示刚体绕Y轴旋转的角速度，ω3表示刚体绕Z轴旋转的角速度，[ω]×表示ω的叉乘矩阵，[ω]×的维数为3×3，
Figure FDA0002428083440000031
表示局部参考坐标系的原点的移动速度，
Figure FDA0002428083440000032
表示第i个未知节点到第m个锚节点的真实距离值，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布
Figure FDA0002428083440000033
Figure FDA0002428083440000034
表示vmi的功率，符号“|| ||”为求欧几里德范数符号，(si-am)T为(si-am)的转置，
Figure FDA0002428083440000035
表示
Figure FDA0002428083440000036
中存在的测量噪声，
Figure FDA0002428083440000037
服从零均值的高斯分布
Figure FDA0002428083440000038
Figure FDA0002428083440000039
表示
Figure FDA00024280834400000310
的功率，
Figure FDA00024280834400000311
为ci的转置，
Figure FDA00024280834400000312
为am的转置，符号
Figure FDA00024280834400000313
为克罗内克积运算符号，vec(Q)表示对Q进行矩阵矢量化，QT为Q的转置，
Figure FDA00024280834400000314
为si的转置，(t-am)T为t-am的转置，(Qci+t-am)T为Qci+t-am的转置，d1＝[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]T，[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]T为[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]的转置，
Figure FDA00024280834400000315
r11表示第1个未知节点到第1个锚节点的距离测量值，
Figure FDA00024280834400000316
rM1表示第1个未知节点到第M个锚节点的距离测量值，
Figure FDA00024280834400000317
r12表示第2个未知节点到第1个锚节点的距离测量值，c2表示刚体运动前第2个未知节点在局部参考坐标系中的坐标位置，
Figure FDA00024280834400000318
rM2表示第2个未知节点到第M个锚节点的距离测量值，
Figure FDA00024280834400000319
r1N表示第N个未知节点到第1个锚节点的距离测量值，
Figure FDA00024280834400000320
rMN表示第N个未知节点到第M个锚节点的距离测量值，H1＝[h11,...,hM1,h12,…,hM2,…,h1N,…,hMN]T，[h11,...,hM1,h12,…,hM2,…,h1N,…,hMN]T为[h11,...,hM1,h12,…,hM2,…,h1N,…,hMN]的转置，
Figure FDA00024280834400000321
Figure FDA0002428083440000041
为
Figure FDA0002428083440000042
的转置，
Figure FDA0002428083440000043
为c1的转置，
Figure FDA0002428083440000044
为a1的转置，
Figure FDA0002428083440000045
Figure FDA0002428083440000046
为
Figure FDA0002428083440000047
的转置，
Figure FDA0002428083440000048
为aM的转置，
Figure FDA0002428083440000049
Figure FDA00024280834400000410
为
Figure FDA00024280834400000411
的转置，
Figure FDA00024280834400000412
为c2的转置，
Figure FDA00024280834400000413
Figure FDA00024280834400000414
为
Figure FDA00024280834400000415
的转置，
Figure FDA00024280834400000416
Figure FDA00024280834400000417
为
Figure FDA00024280834400000418
的转置，
Figure FDA00024280834400000419
为cN的转置，
Figure FDA00024280834400000420
Figure FDA00024280834400000421
为
Figure FDA00024280834400000422
的转置，f1＝[(vec(Q))T,tT,(QTt)T,||t||2]T，[(vec(Q))T,tT,(QTt)T,||t||2]T为[(vec(Q))T,tT,(QTt)T,||t||2]的转置，(vec(Q))T为vec(Q)的转置，tT为t的转置，(QTt)T为QTt的转置，
Figure FDA00024280834400000423
[2r11v11,…,2rM1vM1,2r12v12,…,2rM2vM2,…,2r1Nv1N,…,2rMNvMN]T为[2r11v11,…,2rM1vM1,2r12v12,…,2rM2vM2,…,2r1Nv1N,…,2rMNvMN]的转置，v11表示r11中存在的测量噪声，vM1表示rM1中存在的测量噪声，v12表示r12中存在的测量噪声，vM2表示rM2中存在的测量噪声，v1N表示r1N中存在的测量噪声，vMN表示rMN中存在的测量噪声，
Figure FDA00024280834400000424
Figure FDA00024280834400000425
为
Figure FDA00024280834400000426
的转置，
Figure FDA00024280834400000427
表示第1个未知节点相对于第1个锚节点的多普勒测量值，
Figure FDA00024280834400000428
表示第1个未知节点相对于第M个锚节点的多普勒测量值，
Figure FDA00024280834400000429
表示第2个未知节点相对于第1个锚节点的多普勒测量值，
Figure FDA00024280834400000430
表示第2个未知节点相对于第M个锚节点的多普勒测量值，
Figure FDA0002428083440000051
表示第N个未知节点相对于第1个锚节点的多普勒测量值，
Figure FDA0002428083440000052
表示第N个未知节点相对于第M个锚节点的多普勒测量值，H2＝[q11,...,qM1,q12,…,qM2,…,q1N,…,qMN]T，[q11,...,qM1,q12,…,qM2,…,q1N,…,qMN]T为[q11,...,qM1,q12,…,qM2,…,q1N,…,qMN]的转置，
Figure FDA0002428083440000053
Figure FDA0002428083440000054
为
Figure FDA0002428083440000055
的转置，
Figure FDA0002428083440000056
Figure FDA0002428083440000057
为
Figure FDA0002428083440000058
的转置，
Figure FDA0002428083440000059
Figure FDA00024280834400000510
为
Figure FDA00024280834400000511
的转置，
Figure FDA00024280834400000512
Figure FDA00024280834400000513
为
Figure FDA00024280834400000514
的转置，
Figure FDA00024280834400000515
Figure FDA00024280834400000516
为
Figure FDA00024280834400000517
的转置，
Figure FDA00024280834400000518
Figure FDA00024280834400000519
为
Figure FDA00024280834400000520
的转置，
Figure FDA00024280834400000521
Figure FDA00024280834400000522
为
Figure FDA00024280834400000523
的转置，P'＝[ω]×Q，P'T为P'的转置，(P'Tt)T为P'Tt的转置，vec(P')表示对P'进行矩阵矢量化，(vec(P'))T为vec(P')的转置，
Figure FDA00024280834400000524
为
Figure FDA00024280834400000525
的转置，
Figure FDA00024280834400000526
为
Figure FDA00024280834400000527
的转置，
Figure FDA00024280834400000528
[n11,...,nM1,n12,…,nM2,…,n1N,…,nMN]T为[n11,...,nM1,n12,…,nM2,…,n1N,…,nMN]的转置，
Figure FDA00024280834400000529
Figure FDA00024280834400000530
Figure FDA00024280834400000531
表示
Figure FDA00024280834400000532
中存在的测量噪声，
Figure FDA00024280834400000533
表示
Figure FDA00024280834400000534
中存在的测量噪声，
Figure FDA00024280834400000535
表示
Figure FDA00024280834400000536
中存在的测量噪声，
Figure FDA00024280834400000537
表示
Figure FDA00024280834400000538
中存在的测量噪声，
Figure FDA00024280834400000539
表示
Figure FDA00024280834400000540
中存在的测量噪声，
Figure FDA00024280834400000541
表示
Figure FDA00024280834400000542
中存在的测量噪声，min()为取最小值函数，“s.t.”表示“受约束于……”，
Figure FDA00024280834400000543
Figure FDA00024280834400000544
为
Figure FDA00024280834400000545
的转置，
Figure FDA00024280834400000546
为d1的转置，
Figure FDA00024280834400000547
为d2的转置，
Figure FDA0002428083440000061
Figure FDA0002428083440000062
为
Figure FDA0002428083440000063
的转置，
Figure FDA0002428083440000064
为H1的转置，
Figure FDA0002428083440000065
为H2的转置，
Figure FDA0002428083440000066
Figure FDA0002428083440000067
为
Figure FDA0002428083440000068
的转置，
Figure FDA0002428083440000069
为f1的转置，
Figure FDA00024280834400000610
为f2的转置，(d-Hf)T为(d-Hf)的转置，
Figure FDA00024280834400000611
为
Figure FDA00024280834400000612
的逆，
Figure FDA00024280834400000613
Figure FDA00024280834400000614
diag()为对角线矩阵表示形式，
Figure FDA00024280834400000615
[r11,…,rM1,r12,…,rM2,…,r1N,…,rMN]T为[r11,…,rM1,r12,…,rM2,…,r1N,…,rMN]的转置，
Figure FDA00024280834400000616
Figure FDA00024280834400000617
为
Figure FDA00024280834400000618
的转置，KT为K的转置，0(M×N)×(M×N)表示元素全为0的矩阵，其维度为(M×N)×(M×N)，
Figure FDA00024280834400000619
Figure FDA00024280834400000620
表示v11的功率，
Figure FDA00024280834400000621
表示vM1的功率，
Figure FDA00024280834400000622
表示v12的功率，
Figure FDA00024280834400000623
表示vM2的功率，
Figure FDA00024280834400000624
表示v1N的功率，
Figure FDA00024280834400000625
表示vMN的功率，
Figure FDA00024280834400000626
表示的功率，
Figure FDA00024280834400000628
表示
Figure FDA00024280834400000629
的功率，
Figure FDA00024280834400000630
表示多普勒测量噪声
Figure FDA00024280834400000631
的功率，
Figure FDA00024280834400000632
表示多普勒测量噪声
Figure FDA00024280834400000633
的功率，
Figure FDA00024280834400000634
表示
Figure FDA00024280834400000635
的功率，
Figure FDA00024280834400000636
表示
Figure FDA00024280834400000637
的功率，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ＝I和det(Q)＝1为Q需要满足的条件；
步骤四：令F＝ffT，使刚体定位问题的约束最小二乘表述形式中的约束条件QTQ＝I等价于
Figure FDA00024280834400000638
并使f中的QTt形成约束条件
Figure FDA00024280834400000639
使f中的P'Tt形成约束条件
Figure FDA00024280834400000640
使f中的
Figure FDA0002428083440000071
形成约束条件
Figure FDA0002428083440000072
使f中的
Figure FDA0002428083440000073
形成约束条件f(32)＝tr(F(10:12,33:35))；根据(QTt)TQTt＝tTt和||t||2＝ttT，得到约束条件
Figure FDA0002428083440000074
根据
Figure FDA0002428083440000075
和
Figure FDA0002428083440000076
得到约束条件
Figure FDA0002428083440000077
根据(P'Tt)TQTt＝0和(P'Tt)TQT＝-(P'(QTt))T，得到约束条件
Figure FDA0002428083440000078
根据QTP'是反对称矩阵，得到约束条件
Figure FDA0002428083440000079
然后舍掉刚体定位问题的约束最小二乘表述形式中的约束条件det(Q)＝1，将刚体定位问题的约束最小二乘表述形式转化为：
Figure FDA0002428083440000081
再根据F＝ffT等价于
Figure FDA0002428083440000082
去掉非凸的关于矩阵F的约束rank(F)＝1，将
Figure FDA0002428083440000083
结合到刚体定位问题的约束最小二乘表述形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：
Figure FDA0002428083440000091
最后对刚体定位问题的半正定规划形式进行求解，直接得到Q、t、
Figure FDA0002428083440000092
和P'各自的初步值，对应记为Qsdp、tsdp、
Figure FDA0002428083440000093
和P'sdp；进而根据等式[ω]×＝(P'QT-QP'T)/2间接求得[ω]×的初步值，记为[ωsdp]×，
Figure FDA0002428083440000094
根据等式ω＝[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]T，并结合[ωsdp]×，得到ω的初步值，记为ωsdp，ωsdp＝[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]T；其中，HT为H的转置，dT为d的转置，F为引入的矩阵，F的维数为35×35，fT为f的转置，F(1:3,1:3)表示由F的第1行至第3行、第1列至第3列所有元素形成的矩阵，F(4:6,4:6)表示由F的第4行至第6行、第4列至第6列所有元素形成的矩阵，F(7:9,7:9)表示由F的第7行至第9行、第7列至第9列所有元素形成的矩阵，F(1,4)表示F的第1行第4列元素的值，F(2,5)表示F的第2行第5列元素的值，F(3,6)表示F的第3行第6列元素的值，F(1,7)表示F的第1行第7列元素的值，F(2,8)表示F的第2行第8列元素的值，F(3,9)表示F的第3行第9列元素的值，F(4,7)表示F的第4行第7列元素的值，F(5,8)表示F的第5行第8列元素的值，F(6,9)表示F的第6行第9列元素的值，f(13)表示f中的第13个元素的值，f(14)表示f中的第14个元素的值，f(15)表示f中的第15个元素的值，F(1:3,10:12)表示由F的第1行至第3行、第10列至第12列所有元素形成的矩阵，F(4:6,10:12)表示由F的第4行至第6行、第10列至第12列所有元素形成的矩阵，F(7:9,10:12)表示由F的第7行至第9行、第10列至第12列所有元素形成的矩阵，f(17)表示f中的第17个元素的值，f(18)表示f中的第18个元素的值，f(19)表示f中的第19个元素的值，F(20:22,10:12)表示由F的第20行至第22行、第10列至第12列所有元素形成的矩阵，F(23:25,10:12)表示由F的第23行至第25行、第10列至第12列所有元素形成的矩阵，F(26:28,10:12)表示由F的第26行至第28行、第10列至第12列所有元素形成的矩阵，f(29)表示f中的第29个元素的值，f(30)表示f中的第30个元素的值，f(31)表示f中的第31个元素的值，F(1:3,33:35)表示由F的第1行至第3行、第33列至第35列所有元素形成的矩阵，F(4:6,33:35)表示由F的第4行至第6行、第33列至第35列所有元素形成的矩阵，F(7:9,33:35)表示由F的第7行至第9行、第33列至第35列所有元素形成的矩阵，f(32)表示f中的第32个元素的值，F(10:12,33:35)表示由F的第10行至第12行、第33列至第35列所有元素形成的矩阵，F(10:12,10:12)表示由F的第10行至第12行、第10列至第12列所有元素形成的矩阵，F(13:15,13:15)表示由F的第13行至第15行、第13列至第15列所有元素形成的矩阵，f(16)表示f中的第16个元素的值，F(29:31,29:31)表示由F的第29行至第31行、第29列至第31列所有元素形成的矩阵，F(33:35,33:35)表示由F的第33行至第35行、第33列至第35列所有元素形成的矩阵，F(10:12,33:35)表示由F的第10行至第12行、第33列至第35列所有元素形成的矩阵，F(29:31,13:15)表示由F的第29行至第31行、第13列至第15列所有元素形成的矩阵，
Figure FDA0002428083440000111
为
Figure FDA0002428083440000112
的转置，(P'Tt)T为P'Tt的转置，(P'(QTt))T为P'(QTt)的转置，F(17:19,13:15)表示由F的第17行至第19行、第13列至第15列所有元素形成的矩阵，F(20,13)表示F的第20行第13列的元素，F(23,14)表示F的第23行第14列的元素，F(26,15)表示F的第26行第15列的元素，F(17,1)表示F的第17行第1列的元素，F(18,4)表示F的第18行第4列的元素，F(19,7)表示F的第19行第7列的元素，F(21,13)表示F的第21行第13列的元素，F(24,14)表示F的第24行第14列的元素，F(27,15)表示F的第27行第15列的元素，F(17,2)表示F的第17行第2列的元素，F(18,5)表示F的第18行第5列的元素，F(19,8)表示F的第19行第8列的元素，F(22,13)表示F的第22行第13列的元素，F(25,14)表示F的第25行第14列的元素，F(28,15)表示F的第28行第15列的元素，F(17,3)表示F的第17行第3列的元素，F(18,6)表示F的第18行第6列的元素，F(19,9)表示F的第19行第9列的元素，F(1:3,20:22)表示由F的第1行至第3行、第20列至第22列所有元素形成的矩阵，F(4:6,23:25)表示由F的第4行至第6行、第23列至第25列所有元素形成的矩阵，F(7:9,26:28)表示由F的第7行至第9行、第26列至第28列所有元素形成的矩阵，F(1:3,23:25)表示由F的第1行至第3行、第23列至第25列所有元素形成的矩阵，F(4:6,20:22)表示由F的第4行至第6行、第20列至第22列所有元素形成的矩阵，F(7:9,20:22)表示由F的第7行至第9行、第20列至第22列所有元素形成的矩阵，F(1:3,26:28)表示由F的第1行至第3行、第26列至第28列所有元素形成的矩阵，F(7:9,23:25)表示由F的第7行至第9行、第23列至第25列所有元素形成的矩阵，F(4:6,26:28)表示由F的第4行至第6行、第26列至第28列所有元素形成的矩阵，tr()表示求一个矩阵中的所有对角元素的值的和，符号
Figure FDA00024280834400001210
表示一个矩阵是半正定的，rank()表示求一个矩阵的秩，
Figure FDA0002428083440000121
为Qsdp的转置，
Figure FDA0002428083440000122
为P'sdp的转置，[ω]×(3,2)表示[ω]×中第3行第2列的元素，[ω]×(1,3)表示[ω]×中第1行第3列的元素，[ω]×(2,1)表示[ω]×中第2行第1列的元素，[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]T为[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]的转置，[ωsdp]×表示ωsdp的叉乘矩阵，[ωsdp]×(3,2)表示[ωsdp]×中第3行第2列的元素，[ωsdp]×(1,3)表示[ωsdp]×中第1行第3列的元素，[ωsdp]×(2,1)表示[ωsdp]×中第2行第1列的元素，[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]T为[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]的转置；
步骤五：对Qsdp进行正交化，将正交化后得到的值记为Qort，Qort满足
Figure FDA0002428083440000123
且det(Qort)＝1；然后将Qort作为Q的估计值，将tsdp作为t的估计值，将ωsdp作为ω的估计值，将
Figure FDA0002428083440000124
作为
Figure FDA0002428083440000125
的估计值，将P'sdp作为P'的估计值；其中，
Figure FDA0002428083440000126
为Qort的转置，det(Qort)表示求Qort的行列式。
2.根据权利要求1所述的一种基于距离和多普勒测量的移动刚体定位方法，其特征在于所述的步骤五执行完毕后，执行以下步骤六和步骤七，具体如下：
步骤六：令Qfin表示Q的最优估计值，令tfin表示t的最优估计值，令ωfin表示ω的最优估计值，令
Figure FDA0002428083440000127
表示
Figure FDA0002428083440000128
的最优估计值；令Qfin＝QortQδ，tfin＝tsdp+Δt，ωfin＝ωsdp+Δω，
Figure FDA0002428083440000129
假设Qδ中的欧拉角都接近于0的前提下，使用近似等式cosx≈1,sinx≈x，x表示欧拉角，则得到Qδ的近似表达式为：
Figure FDA0002428083440000131
然后对Qδ的近似表达式和[ωsdp]×进行线性化，得到vec(Qδ)＝γ+Lβ，vec([ωfin]×)＝Φωfin；接着将Qfin＝QortQδ和tfin＝tsdp+Δt代入rmi＝||am-Qci-t||+vmi中，得到rmi＝||am-Qfinci-tfin||+vmi＝||am-QortQδci-tsdp-Δt||+vmi，将vec(Qδ)＝γ+Lβ代入rmi＝||am-Qfinci-tfin||+vmi＝||am-QortQδci-tsdp-Δt||+vmi中，得到rmi＝||emi-Uig1||+vmi，对rmi=||emi-Uig1||+vmi的等式右边进行一阶泰勒展开，得到
Figure FDA0002428083440000132
对
Figure FDA0002428083440000133
的两边同乘以||emi||，得到
Figure FDA0002428083440000134
令
Figure FDA0002428083440000135
则有
Figure FDA0002428083440000136
并将Qfin=QortQδ、tfin=tsdp+Δt、ωfin＝ωsdp+Δω、
Figure FDA0002428083440000137
vec(Qδ)＝γ+Lβ和vec([ωfin]×)=Φωfin代入
Figure FDA0002428083440000138
中，整理得到
Figure FDA0002428083440000139
将
Figure FDA00024280834400001310
改写为
Figure FDA00024280834400001311
再将
Figure FDA00024280834400001312
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure FDA00024280834400001313
并将
Figure FDA00024280834400001314
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure FDA0002428083440000141
最后令
Figure FDA0002428083440000142
和
Figure FDA0002428083440000143
均成立，将
Figure FDA0002428083440000144
和
Figure FDA0002428083440000145
堆砌为
Figure FDA0002428083440000146
求解
Figure FDA0002428083440000147
中的g的线性加权最小二乘解，记为
Figure FDA0002428083440000148
Figure FDA0002428083440000149
其中，Qδ表示Q的修正矩阵，Δt表示t的修正矢量，Δω表示ω的修正矢量，
Figure FDA00024280834400001410
表示
Figure FDA00024280834400001411
的修正矢量，θ、ψ和φ均为Qδ中的欧拉角，
Figure FDA00024280834400001412
Figure FDA00024280834400001413
kθ=sinθ，kψ＝sinψ，kφ＝sinφ，cos为求余弦函数，sin为求正弦函数，vec(Qδ)表示对Qδ进行矩阵矢量化，γ=[1 0 0 0 1 0 0 0 1]T，[1 0 0 0 1 0 0 0 1]T为[1 0 0 0 1 0 0 0 1]的转置，
Figure FDA00024280834400001414
Figure FDA00024280834400001415
为
Figure FDA00024280834400001416
的转置，β=[φ θψ]T，[φ θ ψ]T为[φ θ ψ]的转置，vec([ωfin]×)表示对[ωfin]×进行矩阵矢量化，[ωfin]×表示ωfin的叉乘矩阵，
Figure FDA00024280834400001417
Figure FDA00024280834400001418
为
Figure FDA00024280834400001419
的转置，
Figure FDA00024280834400001420
Figure FDA00024280834400001421
g1＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，βT为β的转置，ΔtT为Δt的转置，
Figure FDA00024280834400001422
为emi的转置，
Figure FDA00024280834400001423
为引入的变量，
Figure FDA00024280834400001424
Figure FDA00024280834400001425
为
Figure FDA00024280834400001426
的转置，
Figure FDA00024280834400001427
Figure FDA00024280834400001428
Figure FDA0002428083440000151
Figure FDA0002428083440000152
Figure FDA0002428083440000153
Figure FDA0002428083440000154
Figure FDA0002428083440000155
为
Figure FDA0002428083440000156
的转置，
Figure FDA0002428083440000157
为U1的转置，
Figure FDA0002428083440000158
为U2的转置，
Figure FDA0002428083440000159
为UN的转置，
Figure FDA00024280834400001510
Figure FDA00024280834400001511
g1＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，
Figure FDA00024280834400001512
[||e11||v11,…,||eM1||vM1,||e12||v12,…,||eM2||vM2,…,||e1N||v1N,…,||eMN||vMN]T为[||e11||v11,...,||eM1||vM1,||e12||v12,…,||eM2||vM2,…,||e1N||v1N,…,||eMN||vMN]的转置，
Figure FDA00024280834400001513
为tsdp的转置，
Figure FDA00024280834400001514
为
Figure FDA00024280834400001515
的转置，
Figure FDA00024280834400001516
为
Figure FDA00024280834400001517
的转置，(Qortci)T为Qortci的转置，(tsdp-am)T为tsdp-am的转置，(Qortci+tsdp-am)T为Qortci+tsdp-am的转置，
Figure FDA00024280834400001518
Figure FDA00024280834400001519
Figure FDA00024280834400001520
表示
Figure FDA00024280834400001521
中存在的测量噪声，
Figure FDA00024280834400001522
Figure FDA00024280834400001523
为
Figure FDA00024280834400001524
的转置，
Figure FDA00024280834400001525
Figure FDA0002428083440000161
为
Figure FDA0002428083440000162
的转置，
Figure FDA0002428083440000163
为
Figure FDA0002428083440000164
的转置，
Figure FDA0002428083440000165
为
Figure FDA0002428083440000166
的转置，
Figure FDA0002428083440000167
为
Figure FDA0002428083440000168
的转置，
Figure FDA0002428083440000169
为
Figure FDA00024280834400001610
的转置，
Figure FDA00024280834400001611
为
Figure FDA00024280834400001612
的转置，
Figure FDA00024280834400001613
为
Figure FDA00024280834400001614
的转置，
Figure FDA00024280834400001615
Figure FDA00024280834400001616
为
Figure FDA00024280834400001617
的转置，ΔωT为Δω的转置，
Figure FDA00024280834400001618
为
Figure FDA00024280834400001619
的转置，
Figure FDA00024280834400001620
Figure FDA00024280834400001621
0(M×N)×6表示元素全为0的矩阵，其维度为(M×N)×6，
Figure FDA00024280834400001622
Figure FDA00024280834400001623
为
Figure FDA00024280834400001624
的转置，
Figure FDA00024280834400001625
Figure FDA00024280834400001626
[||e11||,...,||eM1||,||e12||,…,||eM2||,…,||e1N||,…,||eMN||]T为||e11||,…,||eM1||,||e12||,…,||eM2||,…,||e1N||,…,||eMN||]的转置，
Figure FDA00024280834400001627
为
Figure FDA00024280834400001628
的转置，
Figure FDA00024280834400001629
为
Figure FDA00024280834400001630
的逆，
Figure FDA00024280834400001631
为
Figure FDA00024280834400001632
的逆；
步骤七：将
Figure FDA00024280834400001633
代入
Figure FDA00024280834400001634
中，得到
Figure FDA00024280834400001635
进而根据
Figure FDA00024280834400001636
得到β、Δt、Δω和
Figure FDA00024280834400001637
的估计值，对应记为
Figure FDA00024280834400001638
和
Figure FDA00024280834400001639
然后将
Figure FDA00024280834400001640
代入β＝[φ θ ψ]T中，得到
Figure FDA00024280834400001641
进而根据
Figure FDA00024280834400001642
得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入
Figure FDA00024280834400001643
中，得到Qδ的估计值，记为
Figure FDA00024280834400001644
最后将
Figure FDA00024280834400001645
代入Qfin＝QortQδ中，得到
Figure FDA00024280834400001646
即得到Qfin的值；将
Figure FDA00024280834400001647
代入tfin＝tsdp+Δt中，得到
Figure FDA00024280834400001648
即得到tfin的值，将
Figure FDA00024280834400001649
代入ωfin＝ωsdp+Δω中，得到
Figure FDA00024280834400001650
即得到ωfin的值，将
Figure FDA00024280834400001651
代入
Figure FDA00024280834400001652
中，得到
Figure FDA00024280834400001653
即得到
Figure FDA00024280834400001654
的值。
CN201810567210.7A 2018-06-05 2018-06-05 一种基于距离和多普勒测量的移动刚体定位方法 Active CN109031195B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810567210.7A CN109031195B (zh) 2018-06-05 2018-06-05 一种基于距离和多普勒测量的移动刚体定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810567210.7A CN109031195B (zh) 2018-06-05 2018-06-05 一种基于距离和多普勒测量的移动刚体定位方法

Publications (2)

Publication Number Publication Date
CN109031195A CN109031195A (zh) 2018-12-18
CN109031195B true CN109031195B (zh) 2020-07-14

Family

ID=64612158

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810567210.7A Active CN109031195B (zh) 2018-06-05 2018-06-05 一种基于距离和多普勒测量的移动刚体定位方法

Country Status (1)

Country Link
CN (1) CN109031195B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114878145B (zh) * 2022-05-05 2023-01-03 中国科学院长春光学精密机械与物理研究所 一种基于温度畸变值评价光学传函影响的方法和系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1246924A (zh) * 1997-02-03 2000-03-08 诺基亚电信公司 多普勒定向仪以及用多普勒定向仪定向的方法
CN101004445A (zh) * 2003-08-04 2007-07-25 洛克达公司 用于创建合成多普勒效应的系统和方法
CN102004244A (zh) * 2010-08-12 2011-04-06 中国航空无线电电子研究所 多普勒直接测距法
CN107015259A (zh) * 2016-01-27 2017-08-04 北京中联星通投资管理有限公司 采用多普勒测速仪计算伪距/伪距率的紧组合方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100922937B1 (ko) * 2003-02-12 2009-10-22 삼성전자주식회사 이동통신 단말기의 위치를 측정하기 위한 위성 획득 정보계산 장치 및 방법

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1246924A (zh) * 1997-02-03 2000-03-08 诺基亚电信公司 多普勒定向仪以及用多普勒定向仪定向的方法
CN101004445A (zh) * 2003-08-04 2007-07-25 洛克达公司 用于创建合成多普勒效应的系统和方法
CN102004244A (zh) * 2010-08-12 2011-04-06 中国航空无线电电子研究所 多普勒直接测距法
CN107015259A (zh) * 2016-01-27 2017-08-04 北京中联星通投资管理有限公司 采用多普勒测速仪计算伪距/伪距率的紧组合方法

Also Published As

Publication number Publication date
CN109031195A (zh) 2018-12-18

Similar Documents

Publication Publication Date Title
Zhou et al. Robot-to-robot relative pose estimation from range measurements
Huang et al. On the complexity and consistency of UKF-based SLAM
Sweeney et al. Solving for relative pose with a partially known rotation is a quadratic eigenvalue problem
CN108896047B (zh) 分布式传感器网络协同融合与传感器位置修正方法
CN109342993B (zh) 基于RSS-AoA混合测量的无线传感器网络目标定位方法
CN108200547B (zh) 基于测量距离的刚体定位方法
CN104181513A (zh) 一种雷达天线阵元位置的校正方法
CN110662163A (zh) 基于rss和aoa的三维无线传感网络协作定位方法
CN112130111A (zh) 一种大规模均匀十字阵列中单快拍二维doa估计方法
CN110673196A (zh) 一种基于多维标定和多项式求根的时差定位方法
CN109031195B (zh) 一种基于距离和多普勒测量的移动刚体定位方法
CN106154225B (zh) 基于波达方向歧义消除的定位方法及装置
Dai et al. Localisation algorithm for large-scale and low-density wireless sensor networks
CN109764876A (zh) 无人平台的多模态融合定位方法
Zhou et al. Determining the robot-to-robot 3d relative pose using combinations of range and bearing measurements (part II)
CN110221245B (zh) 联合估计目标位置和非视距误差的鲁棒tdoa定位方法
CN109856619B (zh) 一种雷达测向相对系统误差修正方法
CN108872935B (zh) 一种基于距离测量的静止刚体定位方法
CN109633531B (zh) 一种复合噪声条件下的无线传感器网络节点定位系统
CN116299163A (zh) 无人机航迹规划方法、装置、设备及介质
CN112533284B (zh) 一种基于到达角的近远场统一定位方法
CN104750977A (zh) 一种复合位置度误差评定的方法及装置
Pizzo et al. Towards multi-rigid body localization
Zhou et al. Determining the robot-to-robot relative pose using range-only measurements
Ge et al. Relative sensor registration with two‐step method for state estimation

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant