CN108200547B - 基于测量距离的刚体定位方法 - Google Patents

基于测量距离的刚体定位方法 Download PDF

Info

Publication number
CN108200547B
CN108200547B CN201711234613.1A CN201711234613A CN108200547B CN 108200547 B CN108200547 B CN 108200547B CN 201711234613 A CN201711234613 A CN 201711234613A CN 108200547 B CN108200547 B CN 108200547B
Authority
CN
China
Prior art keywords
rigid body
transpose
coordinate system
fin
matrix
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
CN201711234613.1A
Other languages
English (en)
Other versions
CN108200547A (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 CN201711234613.1A priority Critical patent/CN108200547B/zh
Publication of CN108200547A publication Critical patent/CN108200547A/zh
Application granted granted Critical
Publication of CN108200547B publication Critical patent/CN108200547B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • 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
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/02—Systems for determining distance or velocity not using reflection or reradiation using radio waves
    • 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/0252—Radio frequency fingerprinting
    • 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/14—Determining absolute distances from a plurality of spaced points of known location
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W84/00—Network topologies
    • H04W84/18—Self-organising networks, e.g. ad-hoc networks or sensor networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

本发明公开了一种基于测量距离的刚体定位方法，其先建立每个目标源到各个锚节点的测量距离的模型；然后根据最小化噪声平方和建立带约束的最大似然估计模型，对最大似然估计模型中的目标函数和约束进行重构和松弛，得到半正定规划形式；接着对半正定规划形式求得的结果进行修正，得到刚体定位的最大似然估计问题的可行解；最后对修正后的结果进行优化，提高估计性能；优点是充分利用了关于旋转矩阵的性质约束，对旋转矩阵和位置矢量进行联合估计，从而能够提高在无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下的稳健性和定位精度。

Description

基于测量距离的刚体定位方法
技术领域
本发明涉及一种目标定位方法，尤其是涉及一种基于测量距离的刚体(即有固定形状的物体)定位方法，其通过对刚体的旋转和位置进行估计来实现定位。
背景技术
近些年来，无线传感器技术的进步使得无线传感器网络(WSN)在与定位导航、监控和控制相关的不同领域得到了广泛应用。在很多实际应用中，精确估计刚体的旋转和位置是非常重要的，如机器人、航天飞船、水下交通工具等等。因此，对无线传感器网络中刚体的定位方法的研究十分有必要。
目前，对于无线传感器网络中刚体的定位方法中，使用较多的是基于测量距离的方法，其优点是测量系统复杂度低，可实现高精度的定位结果。但是，当无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，现有的基于测量距离的方法的定位精度会明显下降。
发明内容
本发明所要解决的技术问题是提供一种基于测量距离的刚体定位方法，其在无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，也能对刚体的旋转和位置进行精确估计。
本发明解决上述技术问题所采用的技术方案为：一种基于测量距离的刚体定位方法，其特征在于包括以下步骤：
①设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的传感器；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，在刚体的内部设置一个空间坐标系作为局部参考坐标系；将M个锚节点在全局参考坐标系中的坐标位置对应记为a1,…,am,…,aM，将刚体运动前N个传感器在局部参考坐标系中的坐标位置对应记为c1,…,ci,…,cN；其中，M和N均为正整数，M≥4，N≥3，a1表示第1个锚节点在全局参考坐标系中的坐标位置，am表示第m个锚节点在全局参考坐标系中的坐标位置，1≤m≤M，aM表示第M个锚节点在全局参考坐标系中的坐标位置，c1,…,ci,…,cN已知，c1表示第1个传感器在局部参考坐标系中的坐标位置，ci表示第i个传感器在局部参考坐标系中的坐标位置，1≤i≤N，cN表示第N个传感器在局部参考坐标系中的坐标位置，刚体运动为刚体旋转或位移或旋转和位移；
②使刚体运动，将刚体运动后N个传感器在全局参考坐标系中的坐标位置对应记为s1,…,si,…,sN；然后获取每个传感器到各个锚节点的测量距离，将第i个传感器到第m个锚节点的测量距离记为rmi；其中，s1,…,si,...,sN未知，s1表示刚体运动后第1个传感器在全局参考坐标系中的坐标位置，si表示刚体运动后第i个传感器在全局参考坐标系中的坐标位置，sN表示刚体运动后第N个传感器在全局参考坐标系中的坐标位置；
③对刚体运动后每个传感器在全局参考坐标系中的坐标位置以模型方式进行描述，将si的模型描述为：si＝Qci+t；然后对每个传感器到各个锚节点的测量距离以模型方式进行描述，将rmi的模型描述为：rmi＝||am-si||+vmi＝||am-Qci-t||+vmi；接着根据所有传感器到各个锚节点的测量距离的模型，确定刚体定位问题的最大似然估计表述形式为：
Figure BDA0001488763080000021
其中，Q为旋转矩阵，Q的维数为3×3，t为位置矢量，t代表刚体运动后局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，符号“|| ||”为求欧几里德范数符号，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布
Figure BDA0001488763080000022
Figure BDA0001488763080000023
表示vmi的功率，min()为取最小值函数，“s.t.”表示“受约束于……”，QT为Q的转置，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ＝I和det(Q)＝1为Q需要满足的条件；
④将刚体定位问题的最大似然估计表述形式改写为其等价形式，描述为：
Figure BDA0001488763080000031
并根据矩阵矢量化的公式，将si＝Qci+t重写为：si＝Qci+t＝Bif；然后将si＝Qci+t＝Bif代入刚体定位问题的最大似然估计表述形式的等价形式中的约束条件di＝[||a1-si||,...,||aM-si||]T中，得到
di＝[||a1-Bif||,...,||aM-Bif||]T；接着令F＝ffT，使刚体定位问题的最大似然估计表述形式的等价形式中的约束条件QTQ＝I等价于
Figure BDA0001488763080000032
之后令Di＝didi T，并舍掉刚体定位问题的最大似然估计表述形式的等价形式中的约束条件det(Q)＝1，将刚体定位问题的最大似然估计表述形式的等价形式转化为：
Figure BDA0001488763080000033
再根据Di＝didi T等价于
Figure BDA0001488763080000041
F＝ffT等价于
Figure BDA0001488763080000042
去掉rank(Di)＝1和
rank(F)＝1，将
Figure BDA00014887630800000413
和
Figure BDA00014887630800000414
结合到刚体定位问题的最大似然估计表述形式的等价形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：
Figure BDA0001488763080000045
最后对刚体定位问题的半正定规划形式进行求解，得到Q和t各自的初步值，对应记为Qsdp和tsdp；
其中，ri＝[r1i,r2i,...,rMi]T，符号“[]”为矢量表示符号，[r1i,r2i,...,rMi]T为[r1i,r2i,...,rMi]的转置，(ri-di)T为ri-di的转置，
Figure BDA0001488763080000046
为Ri的逆，
Figure BDA0001488763080000047
diag()为对角矩阵表示形式，
Figure BDA0001488763080000048
表示第i个传感器到第1个锚节点的测量距离r1i中存在的测量噪声v1i的功率，
Figure BDA0001488763080000049
表示第i个传感器到第M个锚节点的测量距离rMi中存在的测量噪声vMi的功率，[||a1-si||,...,||aM-si||]T为[||a1-si||,...,||aM-si||]的转置，
Figure BDA00014887630800000410
f＝[(vec(Q))T,tT]T，
Figure BDA00014887630800000411
为ci的转置，符号
Figure BDA00014887630800000412
为克罗内克积运算符号，[(vec(Q))T,tT]T为[(vec(Q))T,tT]的转置，(vec(Q))T为vec(Q)的转置，tT为t的转置，vec(Q)表示对Q进行矩阵矢量化，[||a1-Bif||,...,||aM-Bif||]T为[||a1-Bif||,...,||aM-Bif||]的转置，F为引入的矩阵，F的维数为12×12，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列元素的值，tr()表示求一个矩阵中的所有对角元素的值的和，Di为引入的矩阵，Di的维数为M×M，di T为di的转置，ri T为ri的转置，Di(m,m)表示Di的第m行第m列元素的值，
Figure BDA0001488763080000051
为am的转置，
Figure BDA0001488763080000052
为Bi的转置，符号
Figure BDA0001488763080000053
表示一个矩阵是半正定的，rank()表示求一个矩阵的秩；
⑤对Qsdp进行正交化，将正交化后得到的值记为Qort，Qort满足
Figure BDA0001488763080000054
且det(Qort)＝1；
⑥令Qfin表示Q的最终估计值，令tfin表示t的最终估计值；令Qfin＝QortQδ，tfin＝tsdp+Δt，假设Qδ中的欧拉角都接近于0，则得到Qδ的近似表达式为：
Figure BDA0001488763080000055
然后对Qδ的近似表达式进行线性化，得到vec(Qδ)＝γ+Lβ；接着将Qfin＝QortQδ和tfin＝tsdp+Δt代入
rmi＝||am-si||+vmi＝||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-Uig||+vmi；之后对rmi＝||emi-Uig||+vmi的等式右边进行一阶泰勒展开，得到
Figure BDA0001488763080000061
对
Figure BDA0001488763080000062
两边同乘以||emi||，得到
Figure BDA0001488763080000063
令pmi＝(rmi-||emi||)||emi||，则有
Figure BDA0001488763080000064
再将
Figure BDA0001488763080000065
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure BDA0001488763080000066
最后令
Figure BDA0001488763080000067
成立，求解
Figure BDA0001488763080000068
中的g的线性加权最小二乘解，记为
Figure BDA0001488763080000069
Figure BDA00014887630800000610
其中，Qδ表示Q的修正矩阵，Δt表示t的修正矢量，θ、ψ和φ均为Qδ中的欧拉角，hθ＝cosθ，hψ＝cosψ，hφ＝cosφ，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 BDA00014887630800000611
Figure BDA00014887630800000612
为
Figure BDA00014887630800000613
的转置，
β＝[φ θ ψ]T，[φ θ ψ]T为[φ θ ψ]的转置，
Figure BDA00014887630800000614
pmi为引入的变量，
Figure BDA00014887630800000615
g＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，βT为β的转置，ΔtT为Δt的转置，
Figure BDA00014887630800000616
为emi的转置，
Figure BDA00014887630800000617
pM1＝(rM1-||eM1||)||eM1||，p12＝(r12-||e12||)||e12||，pM2＝(rM2-||eM2||)||eM2||，
p1N＝(r1N-||e1N||)||e1N||，pMN＝(rMN-||eMN||)||eMN||，
Figure BDA00014887630800000618
Figure BDA0001488763080000071
v11表示r11中存在的测量噪声，vM1表示rM1中存在的测量噪声，v12表示r12中存在的测量噪声，vM2表示rM2中存在的测量噪声，v1N表示r1N中存在的测量噪声，vMN表示rMN中存在的测量噪声，
Figure BDA0001488763080000072
为
Figure BDA0001488763080000073
的转置，
Figure BDA0001488763080000074
为
Figure BDA0001488763080000075
的逆，
Figure BDA0001488763080000076
Figure BDA0001488763080000077
为的逆；
⑦将
Figure BDA0001488763080000079
代入g＝[βT,ΔtT]T中，得到
Figure BDA00014887630800000710
进而根据
Figure BDA00014887630800000711
得到β和Δt的估计值，对应记为
Figure BDA00014887630800000712
和
Figure BDA00014887630800000713
然后将
Figure BDA00014887630800000714
代入β＝[φ θ ψ]T中，得到
Figure BDA00014887630800000715
进而根据
Figure BDA00014887630800000716
得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入
Figure BDA00014887630800000717
中，得到Qδ的估计值，记为
Figure BDA00014887630800000718
最后将
Figure BDA00014887630800000719
代入Qfin＝QortQδ中，得到
Figure BDA00014887630800000720
即得到Qfin的值；并将
Figure BDA00014887630800000721
代入tfin＝tsdp+Δt中，得到
Figure BDA00014887630800000722
即得到tfin的值。
与现有技术相比，本发明的优点在于：
1)本发明方法对旋转矩阵和位置矢量进行联合估计，充分考虑了估计参数的性质，性能稳健，即使在比较差的定位环境中，依然有比较高的定位精度。
2)本发明方法在对旋转矩阵的估计中，对半正定规划估计的结果进行正交化修正，然后再进行优化，使得最终的估计结果具有较高的精度。
3)本发明方法利用凸优化理论解决稳健规划问题，充分发挥了凸优化方法全局收敛的优势，有效地提高无线网络中刚体定位的性能，即使在测量噪声功率比较大的情况下，依然有比较好的估计结果。
附图说明
图1为本发明方法的总体流程框图；
图2a为刚体的内部的传感器在局部参考坐标系中的坐标位置的示意图；
图2b为锚节点和刚体运动后其内部的传感器在全局参考坐标系中的坐标位置的示意图；
图3为在本发明方法与现有的约束最小二乘方法和现有的分拆各个击破的方法的关于旋转矩阵Q的估计值与Q的真实值的均方根误差随测量噪声增加的变化图；
图4为在本发明方法与现有的约束最小二乘方法和现有的分拆各个击破的方法的关于位置矢量t的估计值与t的真实值的均方根误差随测量噪声增加的变化图。
具体实施方式
以下结合附图实施例对本发明作进一步详细描述。
本发明提出的一种基于测量距离的刚体定位方法，其总体流程框图如图1所示，其包括以下步骤：
①设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的传感器(即未知节点)；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，在刚体的内部设置一个空间坐标系作为局部参考坐标系；将M个锚节点在全局参考坐标系中的坐标位置对应记为a1,...,am,...,aM，将刚体运动前N个传感器在局部参考坐标系中的坐标位置对应记为c1,...,ci,...,cN；其中，M和N均为正整数，M≥4，如取M＝10，N≥3，如取N＝5，a1表示第1个锚节点在全局参考坐标系中的坐标位置，am表示第m个锚节点在全局参考坐标系中的坐标位置，1≤m≤M，aM表示第M个锚节点在全局参考坐标系中的坐标位置，c1,...,ci,...,cN已知，由人为设定，c1表示第1个传感器在局部参考坐标系中的坐标位置，ci表示第i个传感器在局部参考坐标系中的坐标位置，1≤i≤N，cN表示第N个传感器在局部参考坐标系中的坐标位置，刚体运动为刚体旋转或位移或旋转和位移。
②使刚体运动，将刚体运动后N个传感器在全局参考坐标系中的坐标位置对应记为s1,...,si,…,sN；然后采用现有技术获取每个传感器到各个锚节点的测量距离，将第i个传感器到第m个锚节点的测量距离记为rmi；其中，s1,…,si,…,sN未知，s1表示刚体运动后第1个传感器在全局参考坐标系中的坐标位置，si表示刚体运动后第i个传感器在全局参考坐标系中的坐标位置，sN表示刚体运动后第N个传感器在全局参考坐标系中的坐标位置。
图2a给出了刚体的内部的传感器在局部参考坐标系中的坐标位置的示意图，图2b给出了传感器和锚节点在全局参考坐标系中的坐标位置的示意图。
③对刚体运动后每个传感器在全局参考坐标系中的坐标位置以模型方式进行描述，将si的模型描述为：si＝Qci+t；然后对每个传感器到各个锚节点的测量距离以模型方式进行描述，将rmi的模型描述为：rmi＝||am-si||+vmi＝||am-Qci-t||+vmi；接着根据所有传感器到各个锚节点的测量距离的模型，确定刚体定位问题的最大似然估计表述形式为：
Figure BDA0001488763080000091
其中，Q为旋转矩阵，表示刚体的旋转，Q的维数为3×3，t为位置矢量，t代表刚体运动后局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，符号“|| ||”为求欧几里德范数符号，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布
Figure BDA0001488763080000092
Figure BDA0001488763080000093
表示vmi的功率，min()为取最小值函数，“s.t.”表示“受约束于……”，QT为Q的转置，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ＝I和det(Q)＝1为Q需要满足的条件。
④为了便于对约束进行松弛，将刚体定位问题的最大似然估计表述形式改写为其等价形式，描述为：
Figure BDA0001488763080000094
并根据矩阵矢量化的公式
Figure BDA0001488763080000095
将si＝Qci+t重写为：si＝Qci+t＝Bif；然后将si＝Qci+t＝Bif代入刚体定位问题的最大似然估计表述形式的等价形式中的约束条件di＝[||a1-si||,...,||aM-si||]T中，得到di＝[||a1-Bif||,...,||aM-Bif||]T；接着令F＝ffT，使刚体定位问题的最大似然估计表述形式的等价形式中的约束条件QTQ＝I等价于
Figure BDA0001488763080000101
之后令Di＝didi T，并舍掉刚体定位问题的最大似然估计表述形式的等价形式中的约束条件det(Q)＝1(考虑到关于旋转矩阵的约束条件
det(Q)＝1涉及到矩阵Q内部三个元素相乘，很难将其松弛成凸约束，故暂时先舍掉这一约束，在后面步骤中再补充这一约束)，将刚体定位问题的最大似然估计表述形式的等价形式转化为：
Figure BDA0001488763080000102
再根据Di＝didi T等价于
Figure BDA0001488763080000103
F＝ffT等价于
Figure BDA0001488763080000104
去掉非凸的关于矩阵秩的约束rank(Di)＝1和rank(F)＝1，将
Figure BDA0001488763080000105
和
Figure BDA0001488763080000106
结合到刚体定位问题的最大似然估计表述形式的等价形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：
Figure BDA0001488763080000111
最后对刚体定位问题的半正定规划形式进行求解，得到Q和t各自的初步值，对应记为Qsdp和tsdp。
其中，ri＝[r1i,r2i,...,rMi]T，符号“[]”为矢量表示符号，[r1i,r2i,...,rMi]T为[r1i,r2i,...,rMi]的转置，(ri-di)T为ri-di的转置，
Figure BDA0001488763080000112
为Ri的逆，
Figure BDA0001488763080000113
diag()为对角矩阵表示形式，
Figure BDA0001488763080000114
表示第i个传感器到第1个锚节点的测量距离r1i中存在的测量噪声v1i的功率，
Figure BDA0001488763080000115
表示第i个传感器到第M个锚节点的测量距离rMi中存在的测量噪声vMi的功率，[||a1-si||,...,||aM-si||]T为[||a1-si||,...,||aM-si||]的转置，
Figure BDA0001488763080000116
f＝[(vec(Q))T,tT]T，
Figure BDA0001488763080000117
为ci的转置，符号
Figure BDA0001488763080000118
为克罗内克积运算符号，[(vec(Q))T,tT]T为[(vec(Q))T,tT]的转置，(vec(Q))T为vec(Q)的转置，tT为t的转置，vec(Q)表示对Q进行矩阵矢量化，[||a1-Bif||,...,||aM-Bif||]T为[||a1-Bif||,...,||aM-Bif||]的转置，F为引入的矩阵，F的维数为12×12，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列元素的值，tr()表示求一个矩阵中的所有对角元素的值的和，Di为引入的矩阵，Di的维数为M×M，di T为di的转置，ri T为ri的转置，Di(m,m)表示Di的第m行第m列元素的值，
Figure BDA0001488763080000121
为am的转置，
Figure BDA0001488763080000122
为Bi的转置，符号
Figure BDA0001488763080000123
表示一个矩阵是半正定的，rank()表示求一个矩阵的秩。
⑤由于在步骤④中忽略了约束条件det(Q)＝1，因此刚体定位问题的半正定规划形式关于Q的求解结果Qsdp并不准确，甚至不能满足旋转矩阵的性质，即可能出现
Figure BDA0001488763080000124
或者det(Qsdp)＜0的情况，
Figure BDA0001488763080000125
为Qsdp的转置，故在本步骤中采用现有技术对Qsdp进行正交化，将正交化后得到的值记为Qort，Qort满足
Figure BDA0001488763080000126
且det(Qort)＝1。
在此，双重迭代的算法为：
Figure BDA0001488763080000127
当
abs(det(Xn+1)-1)＜10-4或者迭代次数达到设定上限时，迭代终止，其中，X0表示迭代初始值，Xn表示第n迭代得到的值，Xn+1表示第n+1迭代得到的值，abs()为求绝对值函数。
⑥令Qfin表示Q的最终估计值，令tfin表示t的最终估计值；令Qfin＝QortQδ，tfin＝tsdp+Δt，如果Qδ满足旋转矩阵的性质，则Qfin也满足，因此在合理的假设Qδ中的欧拉角都接近于0的前提下，使用近似等式cosx≈1,sinx≈x，x表示欧拉角，则可以得到Qδ的近似表达式为：
Figure BDA0001488763080000131
然后对Qδ的近似表达式进行线性化，得到vec(Qδ)＝γ+Lβ；接着将Qfin＝QortQδ和tfin＝tsdp+Δt代入rmi＝||am-si||+vmi＝||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-Uig||+vmi；
之后对rmi＝||emi-Uig||+vmi的等式右边进行一阶泰勒展开，得到
Figure BDA0001488763080000132
对
Figure BDA0001488763080000133
两边同乘以||emi||，得到
Figure BDA0001488763080000134
令pmi＝(rmi-||emi||)||emi||，则有
Figure BDA0001488763080000135
再将
Figure BDA0001488763080000136
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：
Figure BDA0001488763080000137
最后令
Figure BDA0001488763080000138
成立，求解
Figure BDA0001488763080000139
中的g的线性加权最小二乘解，记为
Figure BDA00014887630800001310
Figure BDA00014887630800001311
其中，Qδ表示Q的修正矩阵，Δt表示t的修正矢量，θ、ψ和φ均为Qδ中的欧拉角，hθ＝cosθ，hψ＝cosψ，hφ＝cosφ，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 BDA00014887630800001312
Figure BDA00014887630800001313
为
Figure BDA00014887630800001314
的转置，
β＝[φ θ ψ]T，[φ θ ψ]T为[φ θ ψ]的转置，
Figure BDA00014887630800001315
pmi为引入的变量，
Figure BDA0001488763080000141
g＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，βT为β的转置，ΔtT为Δt的转置，
Figure BDA0001488763080000142
为emi的转置，
Figure BDA0001488763080000143
pM1＝(rM1-||eM1||)||eM1||，p12＝(r12-||e12||)||e12||，pM2＝(rM2-||eM2||)||eM2||，
p1N＝(r1N-||e1N||)||e1N||，pMN＝(rMN-||eMN||)||eMN||，
Figure BDA0001488763080000144
Figure BDA0001488763080000145
v11表示r11中存在的测量噪声，vM1表示rM1中存在的测量噪声，v12表示r12中存在的测量噪声，vM2表示rM2中存在的测量噪声，v1N表示r1N中存在的测量噪声，vMN表示rMN中存在的测量噪声，
Figure BDA0001488763080000146
为
Figure BDA0001488763080000147
的转置，
Figure BDA0001488763080000148
为
Figure BDA0001488763080000149
的逆，
Figure BDA00014887630800001410
Figure BDA00014887630800001411
为的逆。
⑦将
Figure BDA00014887630800001413
代入g＝[βT,ΔtT]T中，得到
Figure BDA00014887630800001414
进而根据
Figure BDA00014887630800001415
得到β和Δt的估计值，对应记为
Figure BDA00014887630800001416
和
Figure BDA00014887630800001417
然后将
Figure BDA00014887630800001418
代入β＝[φ θ ψ]T中，得到
Figure BDA00014887630800001419
进而根据
Figure BDA00014887630800001420
得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入
Figure BDA00014887630800001421
中，得到Qδ的估计值，记为
Figure BDA00014887630800001422
最后将
Figure BDA00014887630800001423
代入Qfin＝QortQδ中，得到
Figure BDA00014887630800001424
即得到Qfin的值；并将
Figure BDA00014887630800001425
代入tfin＝tsdp+Δt中，得到
Figure BDA00014887630800001426
即得到tfin的值。
步骤⑥和步骤⑦的过程是为了进一步提高定位精度，对已求得的Qort和tsdp进行优化。
为了验证本发明方法的可行性和有效性，对本发明方法进行仿真试验。
假设刚体的内部放置了N＝5个传感器，其相对于刚体的内部设置的局部参考坐标系的坐标位置分别为矩阵
Figure BDA0001488763080000151
的各列。无线传感器网络中放置了M＝6个锚节点，其位置均随机分布在长、宽、高对应为200米、100米、30米的长方体内(锚节点分布比较差)，该长方体的中心位置的坐标在全局参考坐标系中为[0,-50,-85]T。刚体的旋转和位移设置如下：假设初始状态下局部参考坐标系和全局参考坐标系重合，即在全局参考坐标系中刚体的内部的传感器的初始位置的坐标就是其在局部参考坐标系下的坐标位置；刚体相对于X,Y,Z轴的旋转角度分别为20度、-25度和10度；刚体的位置矢量为t＝[50,50,20]T。假设同一个锚节点到所有传感器的测量距离中存在的测量噪声的功率一致，不同的锚节点到传感器的测量距离中存在的测量噪声的功率不同。设定传感器到不同锚节点的噪声功率分别为
Figure BDA0001488763080000152
测试本发明方法的性能随测量噪声的增加的变化情况。图3给出了本发明方法与现有的约束最小二乘方法和现有的分拆各个击破的方法的关于旋转矩阵Q的估计值与Q的真实值的均方根误差随测量噪声增加的变化图；图4给出了本发明方法与现有的约束最小二乘方法和现有的分拆各个击破的方法的关于位置矢量t的估计值与t的真实值的均方根误差随测量噪声增加的变化图。从图3和图4中可以看出，在无线传感器网络中锚节点分布比较差和无线传感器网络中噪声比较大的情况下，无论是关于Q，还是关于t的估计，本发明方法明显优于现有的约束最小二乘方法，当噪声功率在中等到较大的水平时，本发明方法也优于现有的分拆各个击破的方法，足以说明本发明方法在刚体定位的精度方面有足够的优势。

Claims (1)

1.一种基于测量距离的刚体定位方法，其特征在于包括以下步骤：
①设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的传感器；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，在刚体的内部设置一个空间坐标系作为局部参考坐标系；将M个锚节点在全局参考坐标系中的坐标位置对应记为a1,...,am,...,aM，将刚体运动前N个传感器在局部参考坐标系中的坐标位置对应记为c1,...,ci,...,cN；其中，M和N均为正整数，M≥4，N≥3，a1表示第1个锚节点在全局参考坐标系中的坐标位置，am表示第m个锚节点在全局参考坐标系中的坐标位置，1≤m≤M，aM表示第M个锚节点在全局参考坐标系中的坐标位置，c1,...,ci,...,cN已知，c1表示第1个传感器在局部参考坐标系中的坐标位置，ci表示第i个传感器在局部参考坐标系中的坐标位置，1≤i≤N，cN表示第N个传感器在局部参考坐标系中的坐标位置，刚体运动为刚体旋转或位移或旋转和位移；
②使刚体运动，将刚体运动后N个传感器在全局参考坐标系中的坐标位置对应记为s1,...,si,...,sN；然后获取每个传感器到各个锚节点的测量距离，将第i个传感器到第m个锚节点的测量距离记为rmi；其中，s1,...,si,...,sN未知，s1表示刚体运动后第1个传感器在全局参考坐标系中的坐标位置，si表示刚体运动后第i个传感器在全局参考坐标系中的坐标位置，sN表示刚体运动后第N个传感器在全局参考坐标系中的坐标位置；
③对刚体运动后每个传感器在全局参考坐标系中的坐标位置以模型方式进行描述，将si的模型描述为：si＝Qci+t；然后对每个传感器到各个锚节点的测量距离以模型方式进行描述，将rmi的模型描述为：rmi＝||am-si||+vmi＝||am-Qci-t||+vmi；接着根据所有传感器到各个锚节点的测量距离的模型，确定刚体定位问题的最大似然估计表述形式为：
Figure FDA0001488763070000021
其中，Q为旋转矩阵，Q的维数为3×3，t为位置矢量，t代表刚体运动后局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，符号“|| ||”为求欧几里德范数符号，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布
Figure FDA0001488763070000022
Figure FDA0001488763070000023
表示vmi的功率，min()为取最小值函数，“s.t.”表示“受约束于……”，QT为Q的转置，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ＝I和det(Q)＝1为Q需要满足的条件；
④将刚体定位问题的最大似然估计表述形式改写为其等价形式，描述为：
Figure FDA0001488763070000024
并根据矩阵矢量化的公式，将si＝Qci+t重写为：si＝Qci+t＝Bif；然后将si＝Qci+t＝Bif代入刚体定位问题的最大似然估计表述形式的等价形式中的约束条件di＝[||a1-si||,...,||aM-si||]T中，得到di＝[||a1-Bif||,...,||aM-Bif||]T；接着令F＝ffT，使刚体定位问题的最大似然估计表述形式的等价形式中的约束条件QTQ＝I等价于
Figure FDA0001488763070000025
之后令Di＝didi T，并舍掉刚体定位问题的最大似然估计表述形式的等价形式中的约束条件det(Q)＝1，将刚体定位问题的最大似然估计表述形式的等价形式转化为：
Figure FDA0001488763070000031
再根据Di＝didi T等价于
Figure FDA0001488763070000032
F＝ffT等价于
Figure FDA0001488763070000033
去掉rank(Di)＝1和rank(F)＝1，将
Figure FDA0001488763070000037
和
Figure FDA0001488763070000035
结合到刚体定位问题的最大似然估计表述形式的等价形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：
Figure FDA0001488763070000036
最后对刚体定位问题的半正定规划形式进行求解，得到Q和t各自的初步值，对应记为Qsdp和tsdp；
其中，ri＝[r1i,r2i,...,rMi]T，符号“[]”为矢量表示符号，[r1i,r2i,...,rMi]T为[r1i,r2i,...,rMi]的转置，(ri-di)T为ri-di的转置，
Figure FDA0001488763070000041
为Ri的逆，
Figure FDA0001488763070000042
diag()为对角矩阵表示形式，
Figure FDA0001488763070000043
表示第i个传感器到第1个锚节点的测量距离r1i中存在的测量噪声v1i的功率，
Figure FDA0001488763070000044
表示第i个传感器到第M个锚节点的测量距离rMi中存在的测量噪声vMi的功率，[||a1-si||,...,||aM-si||]T为[||a1-si||,...,||aM-si||]的转置，
Figure FDA0001488763070000045
f＝[(vec(Q))T,tT]T，
Figure FDA0001488763070000046
为ci的转置，符号
Figure FDA0001488763070000047
为克罗内克积运算符号，[(vec(Q))T,tT]T为[(vec(Q))T,tT]的转置，(vec(Q))T为vec(Q)的转置，tT为t的转置，vec(Q)表示对Q进行矩阵矢量化，[||a1-Bif||,...,||aM-Bif||]T为[||a1-Bif||,...,||aM-Bif||]的转置，F为引入的矩阵，F的维数为12×12，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列元素的值，tr()表示求一个矩阵中的所有对角元素的值的和，Di为引入的矩阵，Di的维数为M×M，di T为di的转置，ri T为ri的转置，Di(m,m)表示Di的第m行第m列元素的值，
Figure FDA0001488763070000048
为am的转置，
Figure FDA0001488763070000049
为Bi的转置，符号“≥”表示一个矩阵是半正定的，rank()表示求一个矩阵的秩；
⑤对Qsdp进行正交化，将正交化后得到的值记为Qort，Qort满足
Figure FDA00014887630700000410
且det(Qort)＝1；
⑥令Qfin表示Q的最终估计值，令tfin表示t的最终估计值；令Qfin＝QortQδ，tfin＝tsdp+Δt，假设Qδ中的欧拉角都接近于0，则得到Qδ的近似表达式为：
Figure FDA0001488763070000051
然后对Qδ的近似表达式进行线性化，得到vec(Qδ)＝γ+Lβ；接着将Qfin＝QortQδ和tfin＝tsdp+Δt代入
rmi＝||am-si||+vmi＝||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-Uig||+vmi；
之后对rmi＝||emi-Uig||+vmi的等式右边进行一阶泰勒展开，得到
Figure FDA0001488763070000052
对
Figure FDA0001488763070000053
两边同乘以||emi||，得到
Figure FDA0001488763070000054
令pmi＝(rmi-||emi||)||emi||，则有
Figure FDA0001488763070000055
再将
Figure FDA0001488763070000056
Figure FDA0001488763070000057
堆砌成向量的形式，描述为：
Figure FDA0001488763070000058
最后令
Figure FDA0001488763070000059
成立，求解
Figure FDA00014887630700000510
中的g的线性加权最小二乘解，记为
Figure FDA00014887630700000511
Figure FDA00014887630700000512
其中，Qδ表示Q的修正矩阵，Δt表示t的修正矢量，θ、ψ和φ均为Qδ中的欧拉角，hθ＝cosθ，hψ＝cosψ，hφ＝cosφ，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 FDA00014887630700000513
Figure FDA0001488763070000061
为
Figure FDA0001488763070000062
的转置，β＝[φ θ ψ]T，[φ θ ψ]T为[φ θ ψ]的转置，
Figure FDA0001488763070000063
pmi为引入的变量，
Figure FDA0001488763070000064
g＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，βT为β的转置，ΔtT为Δt的转置，
Figure FDA0001488763070000065
为emi的转置，
Figure FDA0001488763070000066
p11＝(r11-||e11||)||e11||，
pM1＝(rM1-||eM1||)||eM1||，p12＝(r12-||e12||)||e12||，pM2＝(rM2-||eM2||)||eM2||，
p1N＝(r1N-||e1N||)||e1N||，pMN＝(rMN-||eMN||)||eMN||，
Figure FDA0001488763070000067
Figure FDA0001488763070000068
v11表示r11中存在的测量噪声，vM1表示rM1中存在的测量噪声，v12表示r12中存在的测量噪声，vM2表示rM2中存在的测量噪声，v1N表示r1N中存在的测量噪声，vMN表示rMN中存在的测量噪声，
Figure FDA0001488763070000069
为
Figure FDA00014887630700000610
的转置，
Figure FDA00014887630700000611
为
Figure FDA00014887630700000612
的逆，
Figure FDA00014887630700000613
Figure FDA00014887630700000614
Rv＝diag(R1,...,RN)，
Figure FDA00014887630700000615
为
Figure FDA00014887630700000616
的逆；
⑦将
Figure FDA00014887630700000617
代入g＝[βT,ΔtT]T中，得到
Figure FDA00014887630700000618
进而根据
Figure FDA00014887630700000619
得到β和Δt的估计值，对应记为
Figure FDA00014887630700000620
和
Figure FDA00014887630700000621
然后将
Figure FDA00014887630700000622
代入β＝[φ θ ψ]T中，得到
Figure FDA00014887630700000623
进而根据
Figure FDA00014887630700000624
得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入
Figure FDA00014887630700000625
中，得到Qδ的估计值，记为
Figure FDA00014887630700000626
最后将
Figure FDA00014887630700000627
代入Qfin＝QortQδ中，得到
Figure FDA00014887630700000628
即得到Qfin的值；并将
Figure FDA00014887630700000629
代入tfin＝tsdp+Δt中，得到
Figure FDA0001488763070000071
即得到tfin的值。
CN201711234613.1A 2017-11-30 2017-11-30 基于测量距离的刚体定位方法 Active CN108200547B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711234613.1A CN108200547B (zh) 2017-11-30 2017-11-30 基于测量距离的刚体定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711234613.1A CN108200547B (zh) 2017-11-30 2017-11-30 基于测量距离的刚体定位方法

Publications (2)

Publication Number Publication Date
CN108200547A CN108200547A (zh) 2018-06-22
CN108200547B true CN108200547B (zh) 2020-07-14

Family

ID=62573451

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711234613.1A Active CN108200547B (zh) 2017-11-30 2017-11-30 基于测量距离的刚体定位方法

Country Status (1)

Country Link
CN (1) CN108200547B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109471061B (zh) * 2018-09-18 2020-07-14 宁波大学 一种鲁棒处理模型参数误差的接收信号强度差定位方法
CN109959918B (zh) * 2019-02-22 2021-11-09 西安电子科技大学 一种固态体定位的方法、装置及计算机存储介质
CN112834983B (zh) * 2021-01-06 2023-08-01 西安邮电大学 非视距环境下基于时能域联合的固态体定位方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102186242A (zh) * 2011-05-09 2011-09-14 江南大学 固定区域无线传感器网络移动节点定位方法
CN102890265A (zh) * 2012-08-09 2013-01-23 西北工业大学 一种基于水下声传感器网络的目标被动定位方法
CN104050483A (zh) * 2014-06-25 2014-09-17 北京大学 一种基于局部正交对齐的特征降维方法
CN106019217A (zh) * 2016-05-12 2016-10-12 天津大学 基于aoa的二维无线传感器网络半定规划定位方法
CN107271956A (zh) * 2017-04-24 2017-10-20 宁波大学 非视距环境中未知起始时间的基于到达时间的定位方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
PL2113786T3 (pl) * 2008-04-30 2011-04-29 Gmv Aerospace And Defence S A Sposób autonomicznego wyznaczania poziomów ochrony dla GNSS na podstawie resztowych błędów nawigacyjnych i izotropowego współczynnika ufności

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102186242A (zh) * 2011-05-09 2011-09-14 江南大学 固定区域无线传感器网络移动节点定位方法
CN102890265A (zh) * 2012-08-09 2013-01-23 西北工业大学 一种基于水下声传感器网络的目标被动定位方法
CN104050483A (zh) * 2014-06-25 2014-09-17 北京大学 一种基于局部正交对齐的特征降维方法
CN106019217A (zh) * 2016-05-12 2016-10-12 天津大学 基于aoa的二维无线传感器网络半定规划定位方法
CN107271956A (zh) * 2017-04-24 2017-10-20 宁波大学 非视距环境中未知起始时间的基于到达时间的定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
"Accurate Localization of a Rigid Body Using Multiple Sensors and Landmarks";Shanjie Chen等;《IEEE TRANSACTIONS ON SIGNAL PROCESSING》;20151215;第63卷(第24期);全文 *

Also Published As

Publication number Publication date
CN108200547A (zh) 2018-06-22

Similar Documents

Publication Publication Date Title
CN108200547B (zh) 基于测量距离的刚体定位方法
CN108668358B (zh) 一种应用于无线传感网络的基于到达时间的协作定位方法
CN101377812B (zh) 一种空间平面物体位姿识别方法
CN107871327A (zh) 基于特征点线的单目相机位姿估计和优化方法及系统
CN110673089B (zh) 未知视距和非视距分布情况下基于到达时间的定位方法
CN103862330A (zh) 基于机器视觉的弯管磁研磨自动导航方法
CN112986906B (zh) 一种半正定规划的rss-toa联合定位方法
CN104181513A (zh) 一种雷达天线阵元位置的校正方法
CN101907705B (zh) 通用的多源遥感影像几何校正模型联合平差方法
Dubbelman et al. Closed-form online pose-chain slam
Dubbelman et al. Efficient trajectory bending with applications to loop closure
Sun et al. Angular misalignment calibration method for ultra‐short baseline positioning system based on matrix decomposition
CN105469415A (zh) 多视角遥感图像融合方法
CN103995152A (zh) 一种外场环境三维测量加速度计误差无奇异估计方法
CN104517297A (zh) 一种基于粒子群优化的机器人标定方法
CN104866462B (zh) 一种地图综合相邻空间要素的拓扑关系纠正方法
Jiang et al. Modeling of unbounded long-range drift in visual odometry
CN111273302B (zh) 一种浅海匀速运动目标初始状态估计方法
CN103810747A (zh) 基于二维主流形的三维点云物体形状相似性比较方法
CN109031195B (zh) 一种基于距离和多普勒测量的移动刚体定位方法
CN110221245B (zh) 联合估计目标位置和非视距误差的鲁棒tdoa定位方法
CN116088303B (zh) 一种不确定复杂动态网络状态时变递推估计方法
CN112533284B (zh) 一种基于到达角的近远场统一定位方法
CN101718548A (zh) 基于平面标志物的位姿处理方法
CN108872935B (zh) 一种基于距离测量的静止刚体定位方法

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