[发明专利]基于测量距离的刚体定位方法

摘要

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

主权项

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个传感器在全局参考坐标系中的坐标位置；③对刚体运动后每个传感器在全局参考坐标系中的坐标位置以模型方式进行描述，将s_i的模型描述为：s_i＝Qc_i+t；然后对每个传感器到各个锚节点的测量距离以模型方式进行描述，将r_mi的模型描述为：r_mi＝||a_m‑s_i||+v_mi＝||a_m‑Qc_i‑t||+v_mi；接着根据所有传感器到各个锚节点的测量距离的模型，确定刚体定位问题的最大似然估计表述形式为：其中，Q为旋转矩阵，Q的维数为3×3，t为位置矢量，t代表刚体运动后局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，符号“|| ||”为求欧几里德范数符号，v_mi表示r_mi中存在的测量噪声，v_mi服从零均值的高斯分布表示v_mi的功率，min()为取最小值函数，“s.t.”表示“受约束于……”，Q^T为Q的转置，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，Q^TQ＝I和det(Q)＝1为Q需要满足的条件；④将刚体定位问题的最大似然估计表述形式改写为其等价形式，描述为：并根据矩阵矢量化的公式，将s_i＝Qc_i+t重写为：s_i＝Qc_i+t＝B_if；然后将s_i＝Qc_i+t＝B_if代入刚体定位问题的最大似然估计表述形式的等价形式中的约束条件d_i＝[||a₁‑s_i||,...,||a_M‑s_i||]^T中，得到d_i＝[||a₁‑B_if||,...,||a_M‑B_if||]^T；接着令F＝ff^T，使刚体定位问题的最大似然估计表述形式的等价形式中的约束条件Q^TQ＝I等价于之后令D_i＝d_id_i^T，并舍掉刚体定位问题的最大似然估计表述形式的等价形式中的约束条件det(Q)＝1，将刚体定位问题的最大似然估计表述形式的等价形式转化为：再根据D_i＝d_id_i^T等价于F＝ff^T等价于去掉rank(D_i)＝1和rank(F)＝1，将和结合到刚体定位问题的最大似然估计表述形式的等价形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：最后对刚体定位问题的半正定规划形式进行求解，得到Q和t各自的初步值，对应记为Q_sdp和t_sdp；其中，r_i＝[r_1i,r_2i,...,r_Mi]^T，符号“[]”为矢量表示符号，[r_1i,r_2i,...,r_Mi]^T为[r_1i,r_2i,...,r_Mi]的转置，(r_i‑d_i)^T为r_i‑d_i的转置，为R_i的逆，diag()为对角矩阵表示形式，表示第i个传感器到第1个锚节点的测量距离r_1i中存在的测量噪声v_1i的功率，表示第i个传感器到第M个锚节点的测量距离r_Mi中存在的测量噪声v_Mi的功率，[||a₁‑s_i||,...,||a_M‑s_i||]^T为[||a₁‑s_i||,...,||a_M‑s_i||]的转置，f＝[(vec(Q))^T,t^T]^T，为c_i的转置，符号为克罗内克积运算符号，[(vec(Q))^T,t^T]^T为[(vec(Q))^T,t^T]的转置，(vec(Q))^T为vec(Q)的转置，t^T为t的转置，vec(Q)表示对Q进行矩阵矢量化，[||a₁‑B_if||,...,||a_M‑B_if||]^T为[||a₁‑B_if||,...,||a_M‑B_if||]的转置，F为引入的矩阵，F的维数为12×12，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列元素的值，tr()表示求一个矩阵中的所有对角元素的值的和，D_i为引入的矩阵，D_i的维数为M×M，d_i^T为d_i的转置，r_i^T为r_i的转置，D_i(m,m)表示D_i的第m行第m列元素的值，为a_m的转置，为B_i的转置，符号“≥”表示一个矩阵是半正定的，rank()表示求一个矩阵的秩；⑤对Q_sdp进行正交化，将正交化后得到的值记为Q_ort，Q_ort满足且det(Q_ort)＝1；⑥令Q_fin表示Q的最终估计值，令t_fin表示t的最终估计值；令Q_fin＝Q_ortQ_δ，t_fin＝t_sdp+Δt，假设Q_δ中的欧拉角都接近于0，则得到Q_δ的近似表达式为：然后对Q_δ的近似表达式进行线性化，得到vec(Q_δ)＝γ+Lβ；接着将Q_fin＝Q_ortQ_δ和t_fin＝t_sdp+Δ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；之后对r_mi＝||e_mi‑U_ig||+v_mi的等式右边进行一阶泰勒展开，得到对两边同乘以||e_mi||，得到令p_mi＝(r_mi‑||e_mi||)||e_mi||，则有再将堆砌成向量的形式，描述为：最后令成立，求解中的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为[φ θ ψ]的转置，p_mi为引入的变量，g＝[β^T,Δt^T]^T，[β^T,Δt^T]^T为[β^T,Δt^T]的转置，β^T为β的转置，Δt^T为Δt的转置，为emi的转置，p₁₁＝(r₁₁‑||e₁₁||)||e₁₁||，pM1＝(rM1‑||eM1||)||eM1||，p12＝(r12‑||e12||)||e12||，pM2＝(rM2‑||eM2||)||eM2||，p1N＝(r1N‑||e1N||)||e1N||，pMN＝(rMN‑||eMN||)||eMN||，v₁₁表示r₁₁中存在的测量噪声，v_M1表示r_M1中存在的测量噪声，v₁₂表示r₁₂中存在的测量噪声，v_M2表示r_M2中存在的测量噪声，v_1N表示r_1N中存在的测量噪声，v_MN表示r_MN中存在的测量噪声，为的转置，为的逆，R_v＝diag(R₁,...,R_N)，为的逆；⑦将代入g＝[β^T,Δt^T]^T中，得到进而根据得到β和Δt的估计值，对应记为和然后将代入β＝[φ θ ψ]^T中，得到进而根据得到φ、θ和ψ各自的值；接着将φ、θ和ψ各自的值代入中，得到Q_δ的估计值，记为最后将代入Q_fin＝Q_ortQ_δ中，得到即得到Q_fin的值；并将代入t_fin＝t_sdp+Δt中，得到即得到t_fin的值。