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

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

Info

Publication number
CN108200547A
CN108200547A CN201711234613.1A CN201711234613A CN108200547A CN 108200547 A CN108200547 A CN 108200547A CN 201711234613 A CN201711234613 A CN 201711234613A CN 108200547 A CN108200547 A CN 108200547A
Authority
CN
China
Prior art keywords
row
represent
transposition
value
sensor
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.)
Granted
Application number
CN201711234613.1A
Other languages
English (en)
Other versions
CN108200547B (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

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；接着根据所有传感器到各个锚节点的测量距离的模型，确定刚体定位问题的最大似然估计表述形式为：
其中，Q为旋转矩阵，Q的维数为3×3，t为位置矢量，t代表刚体运动后局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，符号“|| ||”为求欧几里德范数符号，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布 表示vmi的功率，min()为取最小值函数，“s.t.”表示“受约束于……”，QT为Q的转置，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ＝I和det(Q)＝1为Q需要满足的条件；
④将刚体定位问题的最大似然估计表述形式改写为其等价形式，描述为：
并根据矩阵矢量化的公式，将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等价于之后令Di＝didi T，并舍掉刚体定位问题的最大似然估计表述形式的等价形式中的约束条件det(Q)＝1，将刚体定位问题的最大似然估计表述形式的等价形式转化为：
再根据Di＝didi T等价于
F＝ffT等价于去掉rank(Di)＝1和
rank(F)＝1，将和结合到刚体定位问题的最大似然估计表述形式的等价形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：
最后对刚体定位问题的半正定规划形式进行求解，得到Q和t各自的初步值，对应记为Qsdp和tsdp；
其中，ri＝[r1i,r2i,...,rMi]T，符号“[]”为矢量表示符号，[r1i,r2i,...,rMi]T为[r1i,r2i,...,rMi]的转置，(ri-di)T为ri-di的转置，为Ri的逆，diag()为对角矩阵表示形式，表示第i个传感器到第1个锚节点的测量距离r1i中存在的测量噪声v1i的功率，表示第i个传感器到第M个锚节点的测量距离rMi中存在的测量噪声vMi的功率，[||a1-si||,...,||aM-si||]T为[||a1-si||,...,||aM-si||]的转置，f＝[(vec(Q))T,tT]T，为ci的转置，符号为克罗内克积运算符号，[(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列元素的值，为am的转置，为Bi的转置，符号表示一个矩阵是半正定的，rank()表示求一个矩阵的秩；
⑤对Qsdp进行正交化，将正交化后得到的值记为Qort，Qort满足且det(Qort)＝1；
⑥令Qfin表示Q的最终估计值，令tfin表示t的最终估计值；令Qfin＝QortQδ，tfin＝tsdp+Δt，假设Qδ中的欧拉角都接近于0，则得到Qδ的近似表达式为：
然后对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的等式右边进行一阶泰勒展开，得到对两边同乘以||emi||，得到令pmi＝(rmi-||emi||)||emi||，则有再将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：最后令成立，求解中的g的线性加权最小二乘解，记为
其中，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]的转置，
为的转置，
β＝[φ θ ψ]T，[φ θ ψ]T为[φ θ ψ]的转置，pmi为引入的变量，g＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，βT为β的转置，ΔtT为Δt的转置，为emi的转置，
pM1＝(rM1-||eM1||)||eM1||，p12＝(r12-||e12||)||e12||，pM2＝(rM2-||eM2||)||eM2||，
p1N＝(r1N-||e1N||)||e1N||，pMN＝(rMN-||eMN||)||eMN||，
v11表示r11中存在的测量噪声，vM1表示rM1中存在的测量噪声，v12表示r12中存在的测量噪声，vM2表示rM2中存在的测量噪声，v1N表示r1N中存在的测量噪声，vMN表示rMN中存在的测量噪声，为的转置，为的逆，
⑦将代入g＝[βT,ΔtT]T中，得到进而根据得到β和Δt的估计值，对应记为和然后将代入β＝[φ θ ψ]T中，得到进而根据得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入中，得到Qδ的估计值，记为最后将代入Qfin＝QortQδ中，得到即得到Qfin的值；并将代入tfin＝tsdp+Δt中，得到即得到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；接着根据所有传感器到各个锚节点的测量距离的模型，确定刚体定位问题的最大似然估计表述形式为：其中，Q为旋转矩阵，表示刚体的旋转，Q的维数为3×3，t为位置矢量，t代表刚体运动后局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，符号“|| ||”为求欧几里德范数符号，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布 表示vmi的功率，min()为取最小值函数，“s.t.”表示“受约束于……”，QT为Q的转置，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ＝I和det(Q)＝1为Q需要满足的条件。
④为了便于对约束进行松弛，将刚体定位问题的最大似然估计表述形式改写为其等价形式，描述为：并根据矩阵矢量化的公式将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等价于
之后令Di＝didi T，并舍掉刚体定位问题的最大似然估计表述形式的等价形式中的约束条件det(Q)＝1(考虑到关于旋转矩阵的约束条件
det(Q)＝1涉及到矩阵Q内部三个元素相乘，很难将其松弛成凸约束，故暂时先舍掉这一约束，在后面步骤中再补充这一约束)，将刚体定位问题的最大似然估计表述形式的等价形式转化为：再根据Di＝didi T等价于
F＝ffT等价于去掉非凸的关于矩阵秩的约束rank(Di)＝1和rank(F)＝1，将和结合到刚体定位问题的最大似然估计表述形式的等价形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：最后对刚体定位问题的半正定规划形式进行求解，得到Q和t各自的初步值，对应记为Qsdp和tsdp。
其中，ri＝[r1i,r2i,...,rMi]T，符号“[]”为矢量表示符号，[r1i,r2i,...,rMi]T为[r1i,r2i,...,rMi]的转置，(ri-di)T为ri-di的转置，为Ri的逆，diag()为对角矩阵表示形式，表示第i个传感器到第1个锚节点的测量距离r1i中存在的测量噪声v1i的功率，表示第i个传感器到第M个锚节点的测量距离rMi中存在的测量噪声vMi的功率，[||a1-si||,...,||aM-si||]T为[||a1-si||,...,||aM-si||]的转置，f＝[(vec(Q))T,tT]T，为ci的转置，符号为克罗内克积运算符号，[(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列元素的值，为am的转置，为Bi的转置，符号表示一个矩阵是半正定的，rank()表示求一个矩阵的秩。
⑤由于在步骤④中忽略了约束条件det(Q)＝1，因此刚体定位问题的半正定规划形式关于Q的求解结果Qsdp并不准确，甚至不能满足旋转矩阵的性质，即可能出现或者det(Qsdp)＜0的情况，为Qsdp的转置，故在本步骤中采用现有技术对Qsdp进行正交化，将正交化后得到的值记为Qort，Qort满足且det(Qort)＝1。
在此，双重迭代的算法为：当
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δ的近似表达式为：然后对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的等式右边进行一阶泰勒展开，得到
对两边同乘以||emi||，得到令pmi＝(rmi-||emi||)||emi||，则有再将
i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：最后令成立，求解中的g的线性加权最小二乘解，记为
其中，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]的转置， 为的转置，
β＝[φ θ ψ]T，[φ θ ψ]T为[φ θ ψ]的转置，pmi为引入的变量，g＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，βT为β的转置，ΔtT为Δt的转置，为emi的转置，
pM1＝(rM1-||eM1||)||eM1||，p12＝(r12-||e12||)||e12||，pM2＝(rM2-||eM2||)||eM2||，
p1N＝(r1N-||e1N||)||e1N||，pMN＝(rMN-||eMN||)||eMN||，
v11表示r11中存在的测量噪声，vM1表示rM1中存在的测量噪声，v12表示r12中存在的测量噪声，vM2表示rM2中存在的测量噪声，v1N表示r1N中存在的测量噪声，vMN表示rMN中存在的测量噪声，为的转置，为的逆，
⑦将代入g＝[βT,ΔtT]T中，得到进而根据得到β和Δt的估计值，对应记为和然后将代入β＝[φ θ ψ]T中，得到进而根据得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入中，得到Qδ的估计值，记为最后将代入Qfin＝QortQδ中，得到即得到Qfin的值；并将代入tfin＝tsdp+Δt中，得到即得到tfin的值。
步骤⑥和步骤⑦的过程是为了进一步提高定位精度，对已求得的Qort和tsdp进行优化。
为了验证本发明方法的可行性和有效性，对本发明方法进行仿真试验。
假设刚体的内部放置了N＝5个传感器，其相对于刚体的内部设置的局部参考坐标系的坐标位置分别为矩阵的各列。无线传感器网络中放置了M＝6个锚节点，其位置均随机分布在长、宽、高对应为200米、100米、30米的长方体内(锚节点分布比较差)，该长方体的中心位置的坐标在全局参考坐标系中为[0,-50,-85]T。刚体的旋转和位移设置如下：假设初始状态下局部参考坐标系和全局参考坐标系重合，即在全局参考坐标系中刚体的内部的传感器的初始位置的坐标就是其在局部参考坐标系下的坐标位置；刚体相对于X,Y,Z轴的旋转角度分别为20度、-25度和10度；刚体的位置矢量为t＝[50,50,20]T。假设同一个锚节点到所有传感器的测量距离中存在的测量噪声的功率一致，不同的锚节点到传感器的测量距离中存在的测量噪声的功率不同。设定传感器到不同锚节点的噪声功率分别为
测试本发明方法的性能随测量噪声的增加的变化情况。图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；接着根据所有传感器到各个锚节点的测量距离的模型，确定刚体定位问题的最大似然估计表述形式为：其中，Q为旋转矩阵，Q的维数为3×3，t为位置矢量，t代表刚体运动后局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，符号“|| ||”为求欧几里德范数符号，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布 表示vmi的功率，min()为取最小值函数，“s.t.”表示“受约束于……”，QT为Q的转置，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ＝I和det(Q)＝1为Q需要满足的条件；
④将刚体定位问题的最大似然估计表述形式改写为其等价形式，描述为：并根据矩阵矢量化的公式，将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等价于之后令Di＝didi T，并舍掉刚体定位问题的最大似然估计表述形式的等价形式中的约束条件det(Q)＝1，将刚体定位问题的最大似然估计表述形式的等价形式转化为：再根据Di＝didi T等价于F＝ffT等价于去掉rank(Di)＝1和rank(F)＝1，将和结合到刚体定位问题的最大似然估计表述形式的等价形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：最后对刚体定位问题的半正定规划形式进行求解，得到Q和t各自的初步值，对应记为Qsdp和tsdp；
其中，ri＝[r1i,r2i,...,rMi]T，符号“[]”为矢量表示符号，[r1i,r2i,...,rMi]T为[r1i,r2i,...,rMi]的转置，(ri-di)T为ri-di的转置，为Ri的逆，diag()为对角矩阵表示形式，表示第i个传感器到第1个锚节点的测量距离r1i中存在的测量噪声v1i的功率，表示第i个传感器到第M个锚节点的测量距离rMi中存在的测量噪声vMi的功率，[||a1-si||,...,||aM-si||]T为[||a1-si||,...,||aM-si||]的转置，f＝[(vec(Q))T,tT]T，为ci的转置，符号为克罗内克积运算符号，[(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列元素的值，为am的转置，为Bi的转置，符号“≥”表示一个矩阵是半正定的，rank()表示求一个矩阵的秩；
⑤对Qsdp进行正交化，将正交化后得到的值记为Qort，Qort满足且det(Qort)＝1；
⑥令Qfin表示Q的最终估计值，令tfin表示t的最终估计值；令Qfin＝QortQδ，tfin＝tsdp+Δt，假设Qδ中的欧拉角都接近于0，则得到Qδ的近似表达式为：然后对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的等式右边进行一阶泰勒展开，得到
对两边同乘以||emi||，得到令pmi＝(rmi-||emi||)||emi||，则有再将 堆砌成向量的形式，描述为：最后令成立，求解中的g的线性加权最小二乘解，记为
其中，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]的转置， 为的转置，β＝[φ θ ψ]T，[φ θ ψ]T为[φ θ ψ]的转置，pmi为引入的变量，g＝[βT,ΔtT]T，[βT,ΔtT]T为[βT,ΔtT]的转置，βT为β的转置，ΔtT为Δt的转置，为emi的转置，
p11＝(r11-||e11||)||e11||，
pM1＝(rM1-||eM1||)||eM1||，p12＝(r12-||e12||)||e12||，pM2＝(rM2-||eM2||)||eM2||，
p1N＝(r1N-||e1N||)||e1N||，pMN＝(rMN-||eMN||)||eMN||，
v11表示r11中存在的测量噪声，vM1表示rM1中存在的测量噪声，v12表示r12中存在的测量噪声，vM2表示rM2中存在的测量噪声，v1N表示r1N中存在的测量噪声，vMN表示rMN中存在的测量噪声，为的转置，为的逆， Rv＝diag(R1,...,RN)，为的逆；
⑦将代入g＝[βT,ΔtT]T中，得到进而根据得到β和Δt的估计值，对应记为和然后将代入β＝[φ θ ψ]T中，得到进而根据得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入中，得到Qδ的估计值，记为最后将代入Qfin＝QortQδ中，得到即得到Qfin的值；并将代入tfin＝tsdp+Δt中，得到即得到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 true CN108200547A (zh) 2018-06-22
CN108200547B 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)

Cited By (3)

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

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090273515A1 (en) * 2008-04-30 2009-11-05 Miguel Azaola Saenz Method for autonomous determination of protection levels for gnss positioning based on navigation residuals and an isotropic confidence ratio
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 宁波大学 非视距环境中未知起始时间的基于到达时间的定位方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20090273515A1 (en) * 2008-04-30 2009-11-05 Miguel Azaola Saenz Method for autonomous determination of protection levels for gnss positioning based on navigation residuals and an isotropic confidence ratio
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
SHANJIE CHEN等: ""Accurate Localization of a Rigid Body Using Multiple Sensors and Landmarks"", 《IEEE TRANSACTIONS ON SIGNAL PROCESSING》 *

Cited By (5)

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

Also Published As

Publication number Publication date
CN108200547B (zh) 2020-07-14

Similar Documents

Publication Publication Date Title
CN110146839A (zh) 一种移动平台磁梯度张量系统校正方法
CN107870001B (zh) 一种基于椭球拟合的磁力计校正方法
CN108200547A (zh) 基于测量距离的刚体定位方法
CN107544042A (zh) 一种磁力计阵列校正方法
CN105676302A (zh) 一种基于改进最小二乘法的磁力仪随机噪声误差补偿方法
CN107290801B (zh) 基于函数链接型神经网络和场模平方差的捷联三轴磁强计误差一步校正方法
CN110361683A (zh) 基于双目标函数粒子群优化的磁力计校正方法
CN109725360A (zh) 基于磁梯度张量不变量的单点定位方法
CN104406610A (zh) 一种磁力计实时校正装置及方法
US10325005B2 (en) Apparatuses and methods for calibrating magnetometer attitude-independent parameters
CN112461224B (zh) 一种基于已知姿态角的磁力计标定方法
CN105737850B (zh) 基于粒子滤波的变尺度单方向重力采样矢量匹配定位方法
CN110736484B (zh) 基于陀螺仪及磁传感器融合的背景磁场标定方法
Sun et al. Angular misalignment calibration method for ultra‐short baseline positioning system based on matrix decomposition
CN115856744A (zh) 一种三轴磁通门传感器阵列线性不对准误差的校正方法
Yu et al. An effective in-field calibration method for triaxial magnetometers based on local magnetic inclination
CN105865444B (zh) 基于仿射变换的惯性/地磁匹配迭代定位方法
Chen et al. A novel calibration method for tri-axial magnetometers based on an expanded error model and a two-step total least square algorithm
CN110221245B (zh) 联合估计目标位置和非视距误差的鲁棒tdoa定位方法
Zhang et al. Improved ICCP algorithm considering scale error for underwater geomagnetic aided inertial navigation
Milovzorov et al. Angular installation options errors correction for three-component vector-measuring transducers with accelerometer at calibration phase
CN106772147A (zh) 一种中大磁场全象限测量方法
CN110954080A (zh) 一种消除载体磁干扰的磁罗盘校准方法
CN109031195B (zh) 一种基于距离和多普勒测量的移动刚体定位方法
US10935690B2 (en) Methods for inverting electromagnetic logging measurements

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