[发明专利]一种基于距离和多普勒测量的移动刚体定位方法

摘要

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

主权项

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个未知节点在全局参考坐标系中的坐标位置对应记为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的转置，[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中存在的测量噪声，为的转置，表示第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的行列式。