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

Abstract

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

Description

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

技术领域

本发明涉及一种目标定位方法，尤其是涉及一种无线传感器网络中基于距离和多普勒测量的移动刚体(即为有固定形状的移动的物体)定位方法，其定位内容为估计刚体的旋转、位置、旋转角速度和位移速度。

背景技术

近些年来，随着无线传感器技术的快速发展，无线传感器网络(WSN)在与定位、监控、安防等相关的不同领域得到了广泛应用。在很多实际应用中，比如机器人、航空航天、水下探测器等方面，刚体并非总是静止不动的，即不仅需要估计刚体的旋转和位置，还需要对其旋转角速度和位移速度进行估计。因此，对无线传感器网络中移动刚体的定位方法的研究十分有必要。

目前，在无线传感器网络中移动刚体的定位方法中，使用较多的是基于距离和多普勒测量的方法，其优点是测量系统复杂度低，定位精度较高。但是，当无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，现有的基于距离和多普勒测量的方法的定位精度会明显下降。

发明内容

本发明所要解决的技术问题是提供一种无线传感器网络中基于距离和多普勒测量的移动刚体定位方法，其在无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，也能对刚体的旋转、位置、旋转角速度和位移速度进行精确估计。

本发明解决上述技术问题所采用的技术方案为：一种基于距离和多普勒测量的移动刚体定位方法，其特征在于包括以下步骤：

步骤一：设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的未知节点；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，并在刚体的内部设置一个空间坐标系作为局部参考坐标系；将M个锚节点在全局参考坐标系中的坐标位置对应记为a₁,…,a_m,...,a_M，将刚体运动前N个未知节点在局部参考坐标系中的坐标位置对应记为c₁,...,c_i,...,c_N；其中，M和N均为正整数，M≥4，N≥3，a₁表示第1个锚节点在全局参考坐标系中的坐标位置，m为正整数，1≤m≤M，a_m表示第m个锚节点在全局参考坐标系中的坐标位置，a_M表示第M个锚节点在全局参考坐标系中的坐标位置，c₁表示刚体运动前第1个未知节点在局部参考坐标系中的坐标位置，i为正整数，1≤i≤N，c_i表示刚体运动前第i个未知节点在局部参考坐标系中的坐标位置，c_N表示刚体运动前第N个未知节点在局部参考坐标系中的坐标位置；

步骤二：使刚体一直处于运动状态，在当前时刻，将当前时刻下N个未知节点在全局参考坐标系中的坐标位置对应记为s₁,...,s_i,…,s_N；然后获取每个未知节点到各个锚节点的距离测量值，将第i个未知节点到第m个锚节点的距离测量值记为r_mi；并获取每个未知节点相对于各个锚节点的多普勒测量值，，将第i个未知节点相对于第m个锚节点的多普勒测量值记为其中，s₁表示当前时刻下第1个未知节点在全局参考坐标系中的坐标位置，s_i表示当前时刻下第i个未知节点在全局参考坐标系中的坐标位置，s_N表示当前时刻下第N个未知节点在全局参考坐标系中的坐标位置；

步骤三：对当前时刻下每个未知节点在全局参考坐标系中的坐标位置以模型方式进行描述，将s_i的模型描述为：s_i＝Qc_i+t；然后对当前时刻下每个未知节点在全局参考坐标系中的坐标位置的模型求关于时间的导数，对于s_i＝Qc_i+t，对s_i＝Qc_i+t求关于时间的导数，得到s_i的导数，记为 接着对每个未知节点到各个锚节点的距离测量值以模型方式进行描述，将r_mi的模型描述为：

并对每个未知节点相对于各个锚节点的多普勒测量值以模型方式进行描述，将的模型描述为：之后对每个未知节点到各个锚节点的距离测量值的模型描述进行形式整理，对于将等式r_mi＝||a_m-Qc_i-t||+v_mi两边同时平方，忽略同时将||a_m-Qc_i-t||替换为r_mi，整理得到 并对每个未知节点相对于各个锚节点的多普勒测量值的模型描述进行形式整理，对于将等式两边同与r_mi相乘，忽略整理得到再将

i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：并将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：最后令和成立，并确定刚体定位问题的约束最小二乘表述形式，描述为：其中，Q表示旋转矩阵，Q的维数为3×3，t表示位置矢量，t代表当前时刻下局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，ω表示刚体的旋转角速度矢量，ω＝[ω₁,ω₂,ω₃]^T，符号“[]”为矢量表示符号，[ω₁,ω₂,ω₃]^T为[ω₁,ω₂,ω₃]的转置，ω₁表示刚体绕X轴旋转的角速度，ω₂表示刚体绕Y轴旋转的角速度，ω₃表示刚体绕Z轴旋转的角速度，[ω]×表示ω的叉乘矩阵，[ω]×的维数为3×3，表示局部参考坐标系的原点的移动速度，表示第i个未知节点到第m个锚节点的真实距离值，v_mi表示r_mi中存在的测量噪声，v_mi服从零均值的高斯分布 表示v_mi的功率，符号“|| ||”为求欧几里德范数符号，(s_i-a_m)^T为(s_i-a_m)的转置，表示中存在的测量噪声，服从零均值的高斯分布 表示的功率，为c_i的转置，为a_m的转置，符号为克罗内克积运算符号，vec(Q)表示对Q进行矩阵矢量化，Q^T为Q的转置，为s_i的转置，(t-a_m)^T为t-a_m的转置，(Qc_i+t-a_m)^T为Qc_i+t-a_m的转置，d₁＝[p₁₁,…,p_M1,p₁₂,…,p_M2,…,p_1N,…,p_MN]^T，[p₁₁,...,p_M1,p₁₂,…,p_M2,…,p_1N,…,p_MN]^T为[p₁₁,...,p_M1,p₁₂,…,p_M2,…,p_1N,…,p_MN]的转置，r₁₁表示第1个未知节点到第1个锚节点的距离测量值，r_M1表示第1个未知节点到第M个锚节点的距离测量值，r₁₂表示第2个未知节点到第1个锚节点的距离测量值，c₂表示刚体运动前第2个未知节点在局部参考坐标系中的坐标位置，r_M2表示第2个未知节点到第M个锚节点的距离测量值，r_1N表示第N个未知节点到第1个锚节点的距离测量值，r_MN表示第N个未知节点到第M个锚节点的距离测量值，H₁＝[h₁₁,...,h_M1,h₁₂,…,h_M2,…,h_1N,…,h_MN]^T，[h₁₁,...,h_M1,h₁₂,…,h_M2,…,h_1N,…,h_MN]^T为[h₁₁,...,h_M1,h₁₂,…,h_M2,…,h_1N,…,h_MN]的转置， 为的转置，为c₁的转置，为a₁的转置，为的转置，为a_M的转置， 为的转置，为c₂的转置，为的转置， 为的转置，为c_N的转置， 为的转置，f₁＝[(vec(Q))^T,t^T,(Q^Tt)^T,||t||²]^T，[(vec(Q))^T,t^T,(Q^Tt)^T,||t||²]^T为[(vec(Q))^T,t^T,(Q^Tt)^T,||t||²]的转置，(vec(Q))^T为vec(Q)的转置，t^T为t的转置，(Q^Tt)^T为Q^Tt的转置，

[2r₁₁v₁₁,...,2r_M1v_M1,2r₁₂v₁₂,…,2r_M2v_M2,…,2r_1Nv_1N,…,2r_MNv_MN]^T为[2r₁₁v₁₁,...,2r_M1v_M1,2r₁₂v₁₂,…,2r_M2v_M2,…,2r_1Nv_1N,…,2r_MNv_MN]的转置，v₁₁表示r₁₁中存在的测量噪声，v_M1表示r_M1中存在的测量噪声，v₁₂表示r₁₂中存在的测量噪声，v_M2表示r_M2中存在的测量噪声，v_1N表示r_1N中存在的测量噪声，v_MN表示r_MN中存在的测量噪声，

为

的转置，表示第1个未知节点相对于第1个锚节点的多普勒测量值，表示第1个未知节点相对于第M个锚节点的多普勒测量值，表示第2个未知节点相对于第1个锚节点的多普勒测量值，表示第2个未知节点相对于第M个锚节点的多普勒测量值，表示第N个未知节点相对于第1个锚节点的多普勒测量值，表示第N个未知节点相对于第M个锚节点的多普勒测量值，H₂＝[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]^T，[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]^T为[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]的转置， 为的转置，为的转置，为的转置，为的转置， 为的转置，为的转置，为的转置，P'＝[ω]^×Q，P'^T为P'的转置，(P'^Tt)^T为P'^Tt的转置，vec(P')表示对P'进行矩阵矢量化，(vec(P'))^T为vec(P')的转置，为的转置，为的转置，[n₁₁,...,n_M1,n₁₂,…,n_M2,…,n_1N,…,n_MN]^T为[n₁₁,…,n_M1,n₁₂,…,n_M2,…,n_1N,…,n_MN]的转置，

表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，min()为取最小值函数，“s.t.”表示“受约束于……”，为的转置，为d₁的转置，为d₂的转置，为的转置，为H₁的转置，为H₂的转置，为的转置，为f₁的转置，为f₂的转置，(d-Hf)^T为(d-Hf)的转置，为的逆， diag()为对角线矩阵表示形式，[r₁₁,...,r_M1,r₁₂,…,r_M2,…,r_1N,…,r_MN]^T为[r₁₁,...,r_M1,r₁₂,…,r_M2,…,r_1N,…,r_MN]的转置，

为的转置，K^T为K的转置，0_{(M×N)×(M×N)}表示元素全为0的矩阵，其维度为(M×N)×(M×N)，

表示v₁₁的功率，表示v_M1的功率，表示v₁₂的功率，表示v_M2的功率，表示v_1N的功率，表示v_MN的功率，表示的功率，表示的功率，表示多普勒测量噪声的功率，表示多普勒测量噪声的功率，表示的功率，表示的功率，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，Q^TQ＝I和det(Q)＝1为Q需要满足的条件；

步骤四：令F＝ff^T，使刚体定位问题的约束最小二乘表述形式中的约束条件Q^TQ＝I等价于并使f中的Q^Tt形成约束条件使f中的P'^T t形成约束条件使f中的形成约束条件使f中的形成约束条件f(32)＝tr(F(10:12,33:35))；根据(Q^Tt)^TQ^Tt＝t^Tt和||t||²＝tt^T，得到约束条件根据和得到约束条件根据(P'^Tt)^TQ^Tt＝0和(P'^Tt)^TQ^T＝-(P'(Q^Tt))^T，得到约束条件根据Q^TP'是反对称矩阵，得到约束条件然后舍掉刚体定位问题的约束最小二乘表述形式中的约束条件det(Q)＝1，将刚体定位问题的约束最小二乘表述形式转化为：

再根据F＝ff^T等价于去掉非凸的关于矩阵F的约束rank(F)＝1，将结合到刚体定位问题的约束最小二乘表述形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：

最后对刚体定位问题的半正定规划形式进行求解，直接得到Q、t、和P'各自的初步值，对应记为Q_sdp、t_sdp、和P'_sdp；进而根据等式[ω]×＝(P'Q^T-QP'^T)/2间接求得[ω]^×的初步值，记为[ω_sdp]^×，根据等式ω＝[[ω]^×(3,2),[ω]^×(1,3),[ω]^×(2,1)]^T，并结合[ω_sdp]^×，得到ω的初步值，记为ω_sdp，ω_sdp＝[[ω_sdp]^×(3,2),[ω_sdp]^×(1,3),[ω_sdp]^×(2,1)]^T；其中，H^T为H的转置，dT为d的转置，F为引入的矩阵，F的维数为35×35，f^T为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列所有元素形成的矩阵，为的转置，(P'^Tt)^T为P^'Tt的转置，(P'(Q^Tt))^T为P'(Q^Tt)的转置，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()表示求一个矩阵中的所有对角元素的值的和，符号“≥”表示一个矩阵是半正定的，rank()表示求一个矩阵的秩，为Q_sdp的转置，为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)]的转置；

步骤五：对Q_sdp进行正交化，将正交化后得到的值记为Q_ort，Q_ort满足且det(Q_ort)＝1；然后将Q_ort作为Q的估计值，将t_sdp作为t的估计值，将ω_sdp作为ω的估计值，将作为的估计值，将P'_sdp作为P'的估计值；其中，为Q_ort的转置，det(Q_ort)表示求Q_ort的行列式。

所述的步骤五执行完毕后，执行以下步骤六和步骤七，具体如下：

步骤六：令Q_fin表示Q的最优估计值，令t_fin表示t的最优估计值，令ω_fin表示ω的最优估计值，令表示的最优估计值；令Q_fin＝Q_ortQ_δ，t_fin＝t_sdp+Δt，ω_fin＝ω_sdp+Δω，假设Q_δ中的欧拉角都接近于则得到Q_δ的近似表达式为：然后对Q_δ的近似表达式和[ω_sdp]^×进行线性化，得到vec(Q_δ)＝γ+Lβ，vec([ω_fin]^×)＝Φω_fin；接着将Q_fin＝Q_ortQ_δ和t_fin＝t_sdp+Δt代入r_mi＝||a_m-Qc_i-t||+v_mi中，得到r_mi＝||a_m-Q_finc_i-t_fin||+v_mi＝||a_m-Q_ortQ_δc_i-t_sdp-Δt||+v_mi，将vec(Q_δ)＝γ+Lβ代入r_mi＝||a_m-Q_finc_i-t_fin||+v_mi＝||a_m-Q_ortQ_δc_i-t_sdp-Δt||+v_mi中，得到r_mi＝||e_mi-U_ig₁||+v_mi，对r_mi＝||e_mi-U_ig₁||+v_mi的等式右边进行一阶泰勒展开，得到对的两边同乘以||e_mi||，得到令则有并将Q_fin＝Q_ortQ_δ、t_fin＝t_sdp+Δt、ω_fin＝ω_sdp+Δω、vec(Q_δ)＝γ+Lβ和代入中，整理得到将改写为再将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：并将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：最后令和均成立，将和堆砌为求解中的g的线性加权最小二乘解，记为 其中，Q_δ表示Q的修正矩阵，Δt表示t的修正矢量，Δω表示ω的修正矢量，表示的修正矢量，θ、ψ和φ均为Q_δ中的欧拉角， k_θ＝sinθ，k_ψ＝sinψ，k_φ＝sinφ，cos为求余弦函数，sin为求正弦函数，vec(Q_δ)表示对Q_δ进行矩阵矢量化，γ＝[1 00 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为[φ θ ψ]的转置，vec([ω_fin]^×)表示对[ω_fin]^×进行矩阵矢量化，[ω_fin]^×表示ω_fin的叉乘矩阵，为的转置， g₁＝[β^T,Δt^T]^T，[β^T,Δt^T]^T为[β^T,Δt^T]的转置，β^T为β的转置，Δt^T为Δt的转置，为e_mi的转置，为引入的变量，为的转置，

为的转置，为U₁的转置，为U₂的转置，为U_N的转置， g₁＝[β^T,Δt^T]^T，[β^T,Δt^T]^T为[β^T,Δt^T]的转置，

[||e₁₁||v₁₁,...,||e_M1||v_M1,||e₁₂||v₁₂,…,||e_M2||v_M2,…,||e_1N||v_1N,…,||e_MN||v_MN]^T为

[||e₁₁||v₁₁,...,||e_M1||v_M1,||e₁₂||v₁₂,…,||e_M2||v_M2,…,||e_1N||v_1N,…,||e_MN||v_MN]的转置，为t_sdp的转置，为的转置，为的转置，(Qo_rtc_i)^T为Q_ortc_i的转置，(t_sdp-a_m)^T为t_sdp-a_m的转置，(Q_ortc_i+t_sdp-a_m)^T为Qo_rtc_i+t_sdp-a_m的转置，

表示中存在的测量噪声，

为的转置，

为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，ΔωT为Δω的转置，为的转置， 0_(M×N)×6表示元素全为0的矩阵，其维度为(M×N)×6， 为的转置，

[||e₁₁||,...,||e_M1||,||e₁₂||,…,||e_M2||,…,||e_1N||,…,||e_MN||]^T为

[||e₁₁||,...,||e_M1||,||e₁₂||,…,||e_M2||,…,||e_1N||,…,||e_MN||]的转置，为的转置，为的逆，为的逆；

步骤七：将代入中，得到进而根据得到β、Δt、Δω和的估计值，对应记为和然后将代入β＝[φ θ ψ]^T中，得到进而根据得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入中，得到Q_δ的估计值，记为最后将代入Q_fin＝Qo_rtQ_δ中，得到即得到Q_fin的值；将代入t_fin＝t_sdp+Δt中，得到即得到t_fin的值，将代入ω_fin＝ω_sdp+Δω中，得到即得到ω_fin t_fin的值，将代入中，得到即得到的值。

与现有技术相比，本发明的优点在于：

1)本发明方法充分利用了距离测量模型等式平方后估计参数和已知参数的线性关系，减少半正定规划问题中的优化变量，对刚体的旋转、位置、旋转角速和位移速度进行联合估计，算法性能稳健，即使在比较差的定位环境中，依然有比较高的定位精度。

2)本发明方法中的优化变量较少，且优化变量只与要估计的参数有关，减少算法的冗余度，估计结果精度较高。

3)本发明方法采用半正定规划来估计参数，充分发挥了凸优化方法全局收敛的优势，有效地提高无线网络中移动刚体定位的性能，即使在测量噪声功率比较大的情况下，依然有比较好的估计结果。

附图说明

图1为本发明方法的总体实现流程框图；

图2a为刚体运动前刚体的内部设置的未知节点在局部参考坐标系中的坐标位置；

图2b为锚节点在全局参考坐标系中的坐标位置和刚体运动后刚体的内部设置的未知节点在全局参考坐标系中的坐标位置；

图3为本发明方法与现有的分拆各个击破的方法的关于旋转矩阵Q的估计值与Q的真实值的均方根误差随测量噪声的标准差增加的变化图；

图4为本发明方法与现有的分拆各个击破的方法的关于位置矢量t的估计值与t的真实值的均方根误差随测量噪声的标准差增加的变化图；

图5为本发明方法与现有的分拆各个击破的方法的关于位置矢量ω的估计值与ω的真实值的均方根误差随测量噪声的标准差增加的变化图；

图6为本发明方法与现有的分拆各个击破的方法的关于位置矢量的估计值与的真实值的均方根误差随测量噪声的标准差增加的变化图。

具体实施方式

以下结合附图实施例对本发明作进一步详细描述。

本发明提出的一种基于距离和多普勒测量的移动刚体定位方法，其总体实现流程框图如图1所示，其包括以下步骤：

步骤一：设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的未知节点；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，并在刚体的内部设置一个空间坐标系作为局部参考坐标系；将M个锚节点在全局参考坐标系中的坐标位置对应记为a₁,...,a_m,...,a_M，将刚体运动前N个未知节点在局部参考坐标系中的坐标位置对应记为c₁,...,c_i,...,c_N；其中，M和N均为正整数，M≥4，如取M＝10，N≥3，如取N＝5，a₁表示第1个锚节点在全局参考坐标系中的坐标位置，m为正整数，1≤m≤M，a_m表示第m个锚节点在全局参考坐标系中的坐标位置，a_M表示第M个锚节点在全局参考坐标系中的坐标位置，c₁表示刚体运动前第1个未知节点在局部参考坐标系中的坐标位置，i为正整数，1≤i≤N，c_i表示刚体运动前第i个未知节点在局部参考坐标系中的坐标位置，c_N表示刚体运动前第N个未知节点在局部参考坐标系中的坐标位置，c₁,...,c_i,...,c_N已知，由人为设定，图2a给出了刚体运动前刚体的内部设置的未知节点在局部参考坐标系中的坐标位置，图2b给出了锚节点在全局参考坐标系中的坐标位置，刚体运动为刚体旋转或位移或旋转和位移。

步骤二：使刚体一直处于运动状态，在当前时刻，将当前时刻下N个未知节点在全局参考坐标系中的坐标位置对应记为s₁,...,s_i,...,s_N；然后采用现有技术获取每个未知节点到各个锚节点的距离测量值，将第i个未知节点到第m个锚节点的距离测量值记为r_mi(参见图2b)；并采用现有技术获取每个未知节点相对于各个锚节点的多普勒测量值，，将第i个未知节点相对于第m个锚节点的多普勒测量值记为 也即为第i个未知节点相对于第m个锚节点的移动速度；其中，s₁表示当前时刻下第1个未知节点在全局参考坐标系中的坐标位置，s_i表示当前时刻下第i个未知节点在全局参考坐标系中的坐标位置，s_N表示当前时刻下第N个未知节点在全局参考坐标系中的坐标位置，s₁,...,s_i,...,s_N未知，图2b同时给出了刚体运动后刚体的内部设置的未知节点在全局参考坐标系中的坐标位置。

步骤三：对当前时刻下每个未知节点在全局参考坐标系中的坐标位置以模型方式进行描述，将s_i的模型描述为：s_i＝Qc_i+t；然后对当前时刻下每个未知节点在全局参考坐标系中的坐标位置的模型求关于时间的导数，对于s_i＝Qc_i+t，对s_i＝Qc_i+t求关于时间的导数，得到s_i的导数，记为 接着对每个未知节点到各个锚节点的距离测量值以模型方式进行描述，将r_mi的模型描述为：

并对每个未知节点相对于各个锚节点的多普勒测量值以模型方式进行描述，将的模型描述为：之后对每个未知节点到各个锚节点的距离测量值的模型描述进行形式整理，对于将等式r_mi＝||a_m-Qc_i-t||+v_mi两边同时平方，忽略同时将||a_m-Qc_i-t||替换为r_mi，整理得到 并对每个未知节点相对于各个锚节点的多普勒测量值的模型描述进行形式整理，对于将等式两边同与r_mi相乘(即将测量距离值r_mi与多普勒测量值相乘)，忽略整理得到再将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：并将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：最后令和成立，并确定刚体定位问题的约束最小二乘表述形式，描述为：其中，Q表示旋转矩阵，Q的维数为3×3，t表示位置矢量，t代表当前时刻下局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，ω表示刚体的旋转角速度矢量，ω＝[ω₁,ω₂,ω₃]^T，符号“[]”为矢量表示符号，[ω₁,ω₂,ω₃]^T为[ω₁,ω₂,ω₃]的转置，ω₁表示刚体绕X轴旋转的角速度，ω₂表示刚体绕Y轴旋转的角速度，ω₃表示刚体绕Z轴旋转的角速度，[ω]^×表示ω的叉乘矩阵，[ω]^×的维数为3×3，表示局部参考坐标系的原点的移动速度，表示第i个未知节点到第m个锚节点的真实距离值，v_mi表示r_mi中存在的测量噪声，v_mi服从零均值的高斯分布 表示v_mi的功率，符号“|| ||”为求欧几里德范数符号，(s_i-a_m)^T为(s_i-a_m)的转置，表示中存在的测量噪声，服从零均值的高斯分布 表示的功率，为c_i的转置，为a_m的转置，符号为克罗内克积运算符号，vec(Q)表示对Q进行矩阵矢量化，Q^T为Q的转置，为s_i的转置，(t-a_m)^T为t-a_m的转置，(Qc_i+t-a_m)^T为Qc_i+t-a_m的转置，d₁＝[p₁₁,...,p_M1,p₁₂,…,p_M2,…,p_1N,…,p_MN]^T，[p₁₁,…,p_M1,p₁₂,…,p_M2,…,p_1N,…,p_MN]^T为[p₁₁,…,p_M1,p₁₂,…,p_M2,…,p_1N,…,p_MN]的转置，r₁₁表示第1个未知节点到第1个锚节点的距离测量值，r_M1表示第1个未知节点到第M个锚节点的距离测量值，r₁₂表示第2个未知节点到第1个锚节点的距离测量值，c₂表示刚体运动前第2个未知节点在局部参考坐标系中的坐标位置，r_M2表示第2个未知节点到第M个锚节点的距离测量值，r_1N表示第N个未知节点到第1个锚节点的距离测量值，r_MN表示第N个未知节点到第M个锚节点的距离测量值，H₁＝[h₁₁,...,h_M1,h₁₂,…,h_M2,…,h_1N,…,h_MN]^T，[h₁₁,...,h_M1,h₁₂,…,h_M2,…,h_1N,…,h_MN]^T为[h₁₁,...,h_M1,h₁₂,…,h_M2,…,h_1N,…,h_MN]的转置， 为的转置，为c₁的转置，为a₁的转置，为的转置，为a_M的转置， 为的转置，为c₂的转置， 为的转置， 为的转置，为c_N的转置，为的转置，f₁＝[(vec(Q))^T,t^T,(Q^Tt)^T,||t||²]^T，[(vec(Q))^T,t^T,(Q^Tt)^T,||t||²]^T为[(vec(Q))^T,t^T,(Q^Tt)^T,||t||²]的转置，(vec(Q))^T为vec(Q)的转置，t^T为t的转置，(Q^Tt)^T为Q^Tt的转置，

[2r₁₁v₁₁,...,2r_M1v_M1,2r₁₂v₁₂,…,2r_M2v_M2,…,2r_1Nv_1N,…,2r_MNv_MN]^T为[2r₁₁v₁₁,...,2r_M1v_M1,2r₁₂v₁₂,…,2r_M2v_M2,…,2r_1Nv_1N,…,2r_MNv_MN]的转置，v₁₁表示r₁₁中存在的测量噪声，v_M1表示r_M1中存在的测量噪声，v₁₂表示r₁₂中存在的测量噪声，v_M2表示r_M2中存在的测量噪声，v_1N表示r_1N中存在的测量噪声，v_MN表示r_MN中存在的测量噪声，

为 的转置，表示第1个未知节点相对于第1个锚节点的多普勒测量值，表示第1个未知节点相对于第M个锚节点的多普勒测量值，表示第2个未知节点相对于第1个锚节点的多普勒测量值，表示第2个未知节点相对于第M个锚节点的多普勒测量值，表示第N个未知节点相对于第1个锚节点的多普勒测量值，表示第N个未知节点相对于第M个锚节点的多普勒测量值，H₂＝[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]^T，[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]^T为[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]的转置， 为的转置，为的转置，为的转置，为的转置， 为的转置，为的转置，为的转置，P'＝[ω]^×Q，P'^T为P'的转置，(P'^Tt)^T为P'^Tt的转置，vec(P')表示对P'进行矩阵矢量化，(vec(P'))^T为vec(P')的转置，为的转置，为的转置，[n₁₁,...,n_M1,n₁₂,…,n_M2,…,n_1N,…,n_MN]^T为[n₁₁,…,n_M1,n₁₂,…,n_M2,…,n_1N,…,n_MN]的转置，

表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，min()为取最小值函数，“s.t.”表示“受约束于……”，为的转置，为d₁的转置，为d₂的转置，为的转置，为H₁的转置，为H₂的转置，为的转置，为f₁的转置，为f₂的转置，(d-Hf)^T为(d-Hf)的转置，为的逆， diag()为对角线矩阵表示形式，[r₁₁,…,r_M1,r₁₂,…,r_M2,…,r_1N,…,r_MN]^T为[r₁₁,...,r_M1,r₁₂,…,r_M2,…,r_1N,…,r_MN]的转置，

为的转置，K^T为K的转置，0_{(M×N)×(M×N)}表示元素全为0的矩阵，其维度为(M×N)×(M×N)，

表示v₁₁的功率，表示v_M1的功率，表示v₁₂的功率，表示v_M2的功率，表示v_1N的功率，表示v_MN的功率，表示的功率，表示的功率，表示多普勒测量噪声的功率，表示多普勒测量噪声的功率，表示的功率，表示的功率，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，Q^TQ＝I和det(Q)＝1为Q需要满足的条件。

步骤四：令F＝ff^T，使刚体定位问题的约束最小二乘表述形式中的约束条件Q^TQ＝I等价于并使f中的Q^Tt形成约束条件使f中的P'^T t形成约束条件使f中的形成约束条件使f中的形成约束条件f(32)＝tr(F(10:12,33:35))；根据(Q^Tt)^TQ^Tt＝t^Tt和||t||²＝tt^T，得到约束条件根据和得到约束条件根据(P'^Tt)^TQ^Tt＝0和(P'^Tt)^TQ^T＝-(P'(Q^Tt))^T，得到约束条件根据Q^TP'是反对称矩阵，得到约束条件然后舍掉刚体定位问题的约束最小二乘表述形式中的约束条件det(Q)＝1，将刚体定位问题的约束最小二乘表述形式转化为：

再根据F＝ff^T等价于去掉非凸的关于矩阵F的约束rank(F)＝1，将结合到刚体定位问题的约束最小二乘表述形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：

最后对刚体定位问题的半正定规划形式进行求解，直接得到Q、t、和P'各自的初步值，对应记为Q_sdp、t_sdp、和P'_sdp；进而根据等式[ω]^×＝(P'Q^T-QP'^T)/2间接求得[ω]^×的初步值，记为[ω_sdp]^×，根据等式ω＝[[ω]^×(3,2),[ω]^×(1,3),[ω]^×(2,1)]^T，并结合[ω_sdp]^×，得到ω的初步值，记为ω_sdp，ω_sdp＝[[ω_sdp]^×(3,2),[ω_sdp]^×(1,3),[ω_sdp]^×(2,1)]^T；其中，H^T为H的转置，d^T为d的转置，F为引入的矩阵，F的维数为35×35，f^T为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列所有元素形成的矩阵，为的转置，(P'^Tt)^T为P'^Tt的转置，(P'(Q^Tt))^T为P'(Q^Tt)的转置，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()表示求一个矩阵中的所有对角元素的值的和，符号“≥”表示一个矩阵是半正定的，rank()表示求一个矩阵的秩，为Q_sdp的转置，为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的求解结果Q_sdp并不准确，甚至不能满足旋转矩阵的性质，即可能出现或者det(Q_sdp)＜0的情况，为Q_sdp的转置，det(Q_sdp)表示求Q_sdp的行列式，故在本步骤中采用现有技术对Q_sdp进行正交化，将正交化后得到的值记为Q_ort，Q_ort满足且det(Q_ort)＝1；然后将Q_ort作为Q的估计值，将t_sdp作为t的估计值，将ω_sdp作为ω的估计值，将作为的估计值，将P'_sdp作为P'的估计值；其中，为Q_ort的转置，det(Q_ort)表示求Q_ort的行列式。

在此，对Q_sdp进行正交化所采用的双重迭代算法为：

当abs(det(X_n+1)-1)＜10^-4或者迭代次数达到设定上限时，迭代终止，其中，X₀表示迭代初始值，X_n表示第n迭代得到的值，X_n+1表示第n+1迭代得到的值，abs()为求绝对值函数。

步骤六：令Q_fin表示Q的最优估计值，令t_fin表示t的最优估计值，令ω_fin表示ω的最优估计值，令表示的最优估计值；令Q_fin＝Qo_rtQ_δ，t_fin＝t_sdp+Δt，ω_fin＝ω_sdp+Δω，如果Q_δ满足旋转矩阵的性质，则Q_fin也满足，因此在合理的假设Q_δ中的欧拉角都接近于0的前提下，使用近似等式cosx≈1,sinx≈x，x表示欧拉角，则得到Q_δ的近似表达式为：

然后对Q_δ的近似表达式和[ω_sdp]^×进行线性化，得到vec(Q_δ)＝γ+Lβ，vec([ω_fin]^×)＝Φω_fin；接着将Q_fin＝Q_ortQ_δ和t_fin＝t_sdp+Δt代入r_mi＝||a_m-Qc_i-t||+v_mi中，得到

r_mi＝||a_m-Q_finc_i-t_fin||+v_mi＝||a_m-Q_ortQ_δc_i-t_sdp-Δt||+v_mi，将vec(Q_δ)＝γ+Lβ代入r_mi＝||a_m-Q_finc_i-t_fin||+v_mi＝||a_m-Q_ortQ_δc_i-t_sdp-Δt||+v_mi中，得到r_mi＝||e_mi-U_ig₁||+v_mi，对r_mi＝||e_mi-U_ig₁||+v_mi的等式右边进行一阶泰勒展开，得到对的两边同乘以||e_mi||，得到令则有并将Q_fin＝Q_ortQ_δ、t_fin＝t_sdp+Δt、ω_fin＝ω_sdp+Δω、vec(Q_δ)＝γ+Lβ和vec([ω_fin]^×)＝Φω_fin代入中，整理得到将改写为再将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：并将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：最后令和均成立，将和堆砌为求解中的g的线性加权最小二乘解，记为 其中，Q_δ表示Q的修正矩阵，Δt表示t的修正矢量，Δω表示ω的修正矢量，表示的修正矢量，θ、ψ和φ均为Q_δ中的欧拉角， k_θ＝sinθ，k_ψ＝sinψ，k_φ＝sinφ，cos为求余弦函数，sin为求正弦函数，vec(Q_δ)表示对Q_δ进行矩阵矢量化，γ＝[1 00 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为[φ θ ψ]的转置，vec([ω_fin]^×)表示对[ω_fin]^×进行矩阵矢量化，[ω_fin]^×表示ω_fin的叉乘矩阵，为的转置， g₁＝[β^T,Δt^T]^T，[β^T,Δt^T]^T为[β^T,Δt^T]的转置，β^T为β的转置，Δt^T为Δt的转置，为e_mi的转置，为引入的变量，

为的转置，

为

的转置，为U₁的转置，为U₂的转置，为U_N的转置， g₁＝[β^T,Δt^T]^T，[β^T,Δt^T]^T为[β^T,Δt^T]的转置，

[||e₁₁||v₁₁,...,||e_M1||v_M1,||e₁₂||v₁₂,…,||e_M2||v_M2,…,||e_1N||v_1N,…,||e_MN||v_MN]^T为[||e₁₁||v₁₁,…,||e_M1||v_M1,||e₁₂||v₁₂,…,||e_M2||v_M2,…,||e_1N||v_1N,…,||e_MN||v_MN]的转置，为t_sdp的转置，为的转置，为的转置，(Q_ortc_i)^T为Q_ortc_i的转置，(t_sdp-a_m)^T为t_sdp-a_m的转置，(Q_ortc_i+t_sdp-a_m)^T为Q_ortc_i+t_sdp-a_m的转置，

表示中存在的测量噪声， 的转置，

为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，ΔωT为Δω的转置，为的转置， 0_(M×N)×6表示元素全为0的矩阵，其维度为(M×N)×6， 为的转置，

[||e₁₁||,...,||e_M1||,||e₁₂||,…,||e_M2||,…,||e_1N||,…,||e_MN||]^T为[||e₁₁||,...,||e_M1||,||e₁₂||,…,||e_M2||,…,||e_1N||,…,||e_MN||]的转置，为的转置，为的逆，为的逆。

步骤七：将代入中，得到进而根据得到β、Δt、Δω和的估计值，对应记为和然后将代入β＝[φ θ ψ]^T中，得到进而根据得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入中，得到Q_δ的估计值，记为最后将代入Q_fin＝Q_ortQ_δ中，得到即得到Q_fin的值；将代入t_fin＝t_sdp+Δt中，得到即得到t_fin的值，将代入ω_fin＝ω_sdp+Δω中，得到即得到ω_fin t_fin的值，将代入中，得到即得到的值。

上述步骤六和步骤七的过程是为了进一步提高定位精度，对已求得的Q_ort、t_sdp、ω_sdp和进行优化。

为了验证本发明方法的可行性和有效性，对本发明方法进行仿真试验。

假设刚体的内部放置了N＝5个未知节点，其相对于刚体的内部设置的局部参考坐标系的坐标位置分别为矩阵的各列。无线传感器网络中放置了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]的转置；刚体对应的相对移动速度为[1,1,1]^T为[1,1,1]的转置。假设同一个锚节点到所有未知节点的测量距离中存在的测量噪声的功率一致，不同的锚节点到未知节点的测量距离中存在的测量噪声的功率不同。设定未知节点到不同锚节点的距离测量值中存在的测量噪声的标准差分别为设定未知节点相对于不同锚节点的多普勒测量值中存在的测量噪声的标准差分别为σ表示仿真时参考的测量噪声的标准差，σ∈[0.01,10]。

测试本发明方法的性能随测量噪声的增加的变化情况。图3给出了本发明方法与现有的分拆各个击破的方法的关于旋转矩阵Q的估计值与Q的真实值的均方根误差随测量噪声的标准差增加的变化图；图4给出了本发明方法与现有的分拆各个击破的方法的关于位置矢量t的估计值与t的真实值的均方根误差随测量噪声的标准差增加的变化图；图5给出了本发明方法与现有的分拆各个击破的方法的关于位置矢量ω的估计值与ω的真实值的均方根误差随测量噪声的标准差增加的变化图；图6给出了本发明方法与现有的分拆各个击破的方法的关于位置矢量的估计值与的真实值的均方根误差随测量噪声的标准差增加的变化图。

从图3到图6中可以看出，在无线传感器网络中锚节点分布比较差和无线传感器网络中噪声功率在中等到较大的水平时，在Q、t、ω和的估计中，本发明方法明显优于现有的分拆各个击破的方法，足以说明本发明方法在移动刚体定位的精度方面有足够的优势。

Claims (2)

1.一种基于距离和多普勒测量的移动刚体定位方法，其特征在于包括以下步骤：

步骤一：设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的未知节点；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，并在刚体的内部设置一个空间坐标系作为局部参考坐标系；将M个锚节点在全局参考坐标系中的坐标位置对应记为a₁,...,a_m,...,a_M，将刚体运动前N个未知节点在局部参考坐标系中的坐标位置对应记为c₁,...,c_i,...,c_N；其中，M和N均为正整数，M≥4，N≥3，a₁表示第1个锚节点在全局参考坐标系中的坐标位置，m为正整数，1≤m≤M，a_m表示第m个锚节点在全局参考坐标系中的坐标位置，a_M表示第M个锚节点在全局参考坐标系中的坐标位置，c₁表示刚体运动前第1个未知节点在局部参考坐标系中的坐标位置，i为正整数，1≤i≤N，c_i表示刚体运动前第i个未知节点在局部参考坐标系中的坐标位置，c_N表示刚体运动前第N个未知节点在局部参考坐标系中的坐标位置；

步骤二：使刚体一直处于运动状态，在当前时刻，将当前时刻下N个未知节点在全局参考坐标系中的坐标位置对应记为s₁,...,s_i,...,s_N；然后获取每个未知节点到各个锚节点的距离测量值，将第i个未知节点到第m个锚节点的距离测量值记为r_mi；并获取每个未知节点相对于各个锚节点的多普勒测量值，，将第i个未知节点相对于第m个锚节点的多普勒测量值记为其中，s₁表示当前时刻下第1个未知节点在全局参考坐标系中的坐标位置，s_i表示当前时刻下第i个未知节点在全局参考坐标系中的坐标位置，s_N表示当前时刻下第N个未知节点在全局参考坐标系中的坐标位置；

步骤三：对当前时刻下每个未知节点在全局参考坐标系中的坐标位置以模型方式进行描述，将s_i的模型描述为：s_i＝Qc_i+t；然后对当前时刻下每个未知节点在全局参考坐标系中的坐标位置的模型求关于时间的导数，对于s_i＝Qc_i+t，对s_i＝Qc_i+t求关于时间的导数，得到s_i的导数，记为 接着对每个未知节点到各个锚节点的距离测量值以模型方式进行描述，将r_mi的模型描述为：

并对每个未知节点相对于各个锚节点的多普勒测量值以模型方式进行描述，将的模型描述为：之后对每个未知节点到各个锚节点的距离测量值的模型描述进行形式整理，对于将等式r_mi＝||a_m-Qc_i-t||+v_mi两边同时平方，忽略同时将||a_m-Qc_i-t||替换为r_mi，整理得到 并对每个未知节点相对于各个锚节点的多普勒测量值的模型描述进行形式整理，对于将等式两边同与r_mi相乘，忽略整理得到

再将

i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：并将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：最后令和成立，并确定刚体定位问题的约束最小二乘表述形式，描述为：其中，Q表示旋转矩阵，Q的维数为3×3，t表示位置矢量，t代表当前时刻下局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，ω表示刚体的旋转角速度矢量，ω＝[ω₁,ω₂,ω₃]^T，符号“[]”为矢量表示符号，[ω₁,ω₂,ω₃]^T为[ω₁,ω₂,ω₃]的转置，ω₁表示刚体绕X轴旋转的角速度，ω₂表示刚体绕Y轴旋转的角速度，ω₃表示刚体绕Z轴旋转的角速度，[ω]×表示ω的叉乘矩阵，[ω]×的维数为3×3，表示局部参考坐标系的原点的移动速度，表示第i个未知节点到第m个锚节点的真实距离值，v_mi表示r_mi中存在的测量噪声，v_mi服从零均值的高斯分布 表示v_mi的功率，符号“|| ||”为求欧几里德范数符号，(s_i-a_m)^T为(s_i-a_m)的转置，表示中存在的测量噪声，服从零均值的高斯分布 表示的功率，为c_i的转置，为a_m的转置，符号为克罗内克积运算符号，vec(Q)表示对Q进行矩阵矢量化，Q^T为Q的转置，为s_i的转置，(t-a_m)^T为t-a_m的转置，(Qc_i+t-a_m)^T为Qc_i+t-a_m的转置，d₁＝[p₁₁,...,p_M1,p₁₂,…,p_M2,…,p_1N,…,p_MN]^T，[p₁₁,...,p_M1,p₁₂,…,p_M2,…,p_1N,…,p_MN]^T为[p₁₁,...,p_M1,p₁₂,…,p_M2,…,p_1N,…,p_MN]的转置，r₁₁表示第1个未知节点到第1个锚节点的距离测量值，r_M1表示第1个未知节点到第M个锚节点的距离测量值，r₁₂表示第2个未知节点到第1个锚节点的距离测量值，c₂表示刚体运动前第2个未知节点在局部参考坐标系中的坐标位置，r_M2表示第2个未知节点到第M个锚节点的距离测量值，r_1N表示第N个未知节点到第1个锚节点的距离测量值，r_MN表示第N个未知节点到第M个锚节点的距离测量值，H₁＝[h₁₁,...,h_M1,h₁₂,…,h_M2,…,h_1N,…,h_MN]^T，[h₁₁,...,h_M1,h₁₂,…,h_M2,…,h_1N,…,h_MN]^T为[h₁₁,...,h_M1,h₁₂,…,h_M2,…,h_1N,…,h_MN]的转置， 为的转置，为c₁的转置，为a₁的转置，为的转置，为a_M的转置， 为的转置，为c₂的转置， 为的转置， 为的转置，为c_N的转置，为的转置，f₁＝[(vec(Q))^T,t^T,(Q^Tt)^T,||t||²]^T，[(vec(Q))^T,t^T,(Q^Tt)^T,||t||²]^T为[(vec(Q))^T,t^T,(Q^Tt)^T,||t||²]的转置，(vec(Q))^T为vec(Q)的转置，t^T为t的转置，(Q^Tt)^T为Q^Tt的转置，

[2r₁₁v₁₁,...,2r_M1v_M1,2r₁₂v₁₂,…,2r_M2v_M2,…,2r_1Nv_1N,…,2r_MNv_MN]^T为

[2r₁₁v₁₁,...,2r_M1v_M1,2r₁₂v₁₂,…,2r_M2v_M2,…,2r_1Nv_1N,…,2r_MNv_MN]的转置，v₁₁表示r₁₁中存在的测量噪声，v_M1表示r_M1中存在的测量噪声，v₁₂表示r₁₂中存在的测量噪声，v_M2表示r_M2中存在的测量噪声，v_1N表示r_1N中存在的测量噪声，v_MN表示r_MN中存在的测量噪声，

为

的转置，表示第1个未知节点相对于第1个锚节点的多普勒测量值，表示第1个未知节点相对于第M个锚节点的多普勒测量值，表示第2个未知节点相对于第1个锚节点的多普勒测量值，表示第2个未知节点相对于第M个锚节点的多普勒测量值，表示第N个未知节点相对于第1个锚节点的多普勒测量值，表示第N个未知节点相对于第M个锚节点的多普勒测量值，H₂＝[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]^T，[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]^T为[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]的转置， 为的转置，为的转置，为的转置，为的转置， 为的转置，为的转置，为的转置，P'＝[ω]^×Q，P'^T为P'的转置，(P'^Tt)^T为P'^Tt的转置，vec(P')表示对P'进行矩阵矢量化，(vec(P'))^T为vec(P')的转置，为的转置，为的转置，[n₁₁,...,n_M1,n₁₂,…,n_M2,…,n_1N,…,n_MN]^T为[n₁₁,...,n_M1,n₁₂,…,n_M2,…,n_1N,…,n_MN]的转置， 表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，min()为取最小值函数，“s.t.”表示“受约束于……”，为的转置，为d₁的转置，为d₂的转置，为的转置，为H₁的转置，为H₂的转置，为的转置，为f₁的转置，为f₂的转置，(d-Hf)^T为(d-Hf)的转置，为的逆， diag()为对角线矩阵表示形式，[r₁₁,...,r_M1,r₁₂,…,r_M2,…,r_1N,…,r_MN]^T为[r₁₁,...,r_M1,r₁₂,…,r_M2,…,r_1N,…,r_MN]的转置， 为的转置，K^T为K的转置，0_{(M×N)×(M×N)}表示元素全为0的矩阵，其维度为(M×N)×(M×N)， 表示v₁₁的功率，表示v_M1的功率，表示v₁₂的功率，表示v_M2的功率，表示v_1N的功率，表示v_MN的功率，表示的功率，表示的功率，表示多普勒测量噪声的功率，表示多普勒测量噪声的功率，表示的功率，表示的功率，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，Q^TQ＝I和det(Q)＝1为Q需要满足的条件；

步骤四：令F＝ff^T，使刚体定位问题的约束最小二乘表述形式中的约束条件Q^TQ＝I等价于并使f中的Q^Tt形成约束条件使f中的P'^Tt形成约束条件使f中的形成约束条件使f中的形成约束条件f(32)＝tr(F(10:12,33:35))；根据(Q^Tt)^TQ^Tt＝t^Tt和||t||²＝tt^T，得到约束条件根据和得到约束条件根据(P'^Tt)^TQ^Tt＝0和(P'^Tt)^TQ^T＝-(P'(Q^Tt))^T，得到约束条件根据Q^TP'是反对称矩阵，得到约束条件然后舍掉刚体定位问题的约束最小二乘表述形式中的约束条件det(Q)＝1，将刚体定位问题的约束最小二乘表述形式转化为：

再根据F＝ff^T等价于去掉非凸的关于矩阵F的约束rank(F)＝1，将结合到刚体定位问题的约束最小二乘表述形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：

最后对刚体定位问题的半正定规划形式进行求解，直接得到Q、t、和P'各自的初步值，对应记为Q_sdp、t_sdp、和P'_sdp；进而根据等式[ω]^×＝(P'Q^T-QP'^T)/2间接求得[ω]^×的初步值，记为[ω_sdp]^×，根据等式ω＝[[ω]^×(3,2),[ω]^×(1,3),[ω]^×(2,1)]^T，并结合[ω_sdp]^×，得到ω的初步值，记为ω_sdp，ω_sdp＝[[ω_sdp]^×(3,2),[ω_sdp]^×(1,3),[ω_sdp]^×(2,1)]^T；其中，H^T为H的转置，d^T为d的转置，F为引入的矩阵，F的维数为35×35，f^T为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列所有元素形成的矩阵，为的转置，(P'^Tt)^T为P'^Tt的转置，(P'(Q^Tt))^T为P'(Q^Tt)的转置，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()表示求一个矩阵中的所有对角元素的值的和，符号表示一个矩阵是半正定的，rank()表示求一个矩阵的秩，为Q_sdp的转置，为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)]的转置；

步骤五：对Q_sdp进行正交化，将正交化后得到的值记为Q_ort，Q_ort满足且det(Q_ort)＝1；然后将Q_ort作为Q的估计值，将t_sdp作为t的估计值，将ω_sdp作为ω的估计值，将作为的估计值，将P'_sdp作为P'的估计值；其中，为Q_ort的转置，det(Q_ort)表示求Q_ort的行列式。

2.根据权利要求1所述的一种基于距离和多普勒测量的移动刚体定位方法，其特征在于所述的步骤五执行完毕后，执行以下步骤六和步骤七，具体如下：

步骤六：令Q_fin表示Q的最优估计值，令t_fin表示t的最优估计值，令ω_fin表示ω的最优估计值，令表示的最优估计值；令Q_fin＝Q_ortQ_δ，t_fin＝t_sdp+Δt，ω_fin＝ω_sdp+Δω，假设Q_δ中的欧拉角都接近于则得到Q_δ的近似表达式为：然后对Q_δ的近似表达式和[ω_sdp]^×进行线性化，得到vec(Q_δ)＝γ+Lβ，vec([ω_fin]^×)＝Φω_fin；接着将Q_fin＝Q_ortQ_δ和t_fin＝t_sdp+Δt代入r_mi＝||a_m-Qc_i-t||+v_mi中，得到r_mi＝||a_m-Q_finc_i-t_fin||+v_mi＝||a_m-Q_ortQ_δc_i-t_sdp-Δt||+v_mi，将vec(Q_δ)＝γ+Lβ代入r_mi＝||a_m-Q_finc_i-t_fin||+v_mi＝||a_m-Q_ortQ_δc_i-t_sdp-Δt||+v_mi中，得到r_mi＝||e_mi-U_ig₁||+v_mi，对r_mi＝||e_mi-U_ig₁||+v_mi的等式右边进行一阶泰勒展开，得到对的两边同乘以||e_mi||，得到令则有并将Q_fin＝Q_ortQ_δ、t_fin＝t_sdp+Δt、ω_fin＝ω_sdp+Δω、vec(Q_δ)＝γ+Lβ和vec([ω_fin]^×)＝Φω_fin代入中，整理得到

将

改写为

再将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：并将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：最后令和均成立，将和堆砌为求解中的g的线性加权最小二乘解，记为 其中，Q_δ表示Q的修正矩阵，Δt表示t的修正矢量，Δω表示ω的修正矢量，表示的修正矢量，θ、ψ和φ均为Q_δ中的欧拉角， 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为[φ θ ψ]的转置，vec([ω_fin]^×)表示对[ω_fin]^×进行矩阵矢量化，[ω_fin]^×表示ω_fin的叉乘矩阵，

为

的转置，

g₁＝[β^T,Δt^T]^T，[β^T,Δt^T]^T为[β^T,Δt^T]的转置，β^T为β的转置，Δt^T为Δt的转置，为e_mi的转置，为引入的变量，

为

的转置，

为

的转置，为U₁的转置，为U₂的转置，为U_N的转置， g₁＝[β^T,Δt^T]^T，[β^T,Δt^T]^T为[β^T,Δt^T]的转置，

[||e₁₁||v₁₁,...,||e_M1||v_M1,||e₁₂||v₁₂,…,||e_M2||v_M2,…,||e_1N||v_1N,…,||e_MN||v_MN]^T为

[||e₁₁||v₁₁,…,||e_M1||v_M1,||e₁₂||v₁₂,…,||e_M2||v_M2,…,||e_1N||v_1N,…,||e_MN||v_MN]的转置，为t_sdp的转置，为的转置，为的转置，(Q_ortc_i)^T为Q_ortc_i的转置，(t_sdp-a_m)^T为t_sdp-a_m的转置，(Q_ortc_i+t_sdp-a_m)^T为Q_ortc_i+t_sdp-a_m的转置，

表示中存在的测量噪声， 为的转置，

为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，Δω^T为Δω的转置，为的转置， 0_(M×N)×6表示元素全为0的矩阵，其维度为(M×N)×6， 为的转置，

[||e₁₁||,...,||e_M1||,||e₁₂||,…,||e_M2||,…,||e_1N||,…,||e_MN||]^T为

[||e₁₁||,...,||e_M1||,||e₁₂||,…,||e_M2||,…,||e_1N||,…,||e_MN||]的转置，为的转置，为的逆，为的逆；

步骤七：将代入中，得到进而根据得到β、Δt、Δω和的估计值，对应记为和然后将代入β＝[φθψ]^T中，得到进而根据得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入

中，得到Q_δ的估计值，记为最后将代入Q_fin＝Qo_rtQ_δ中，得到即得到Q_fin的值；将代入t_fin＝t_sdp+Δt中，得到即得到t_fin的值，将代入ω_fin＝ω_sdp+Δω中，得到即得到ω_fint_fin的值，将代入中，得到即得到的值。