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

权利要求书

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_δ中的欧拉角都接近于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 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＝Q_ortQ_δ中，得到即得到Q_fin的值；将代入t_fin＝t_sdp+Δt中，得到即得到t_fin的值，将代入ω_fin＝ω_sdp+Δω中，得到即得到ω_fin的值，将代入中，得到即得到的值。