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

摘要

本发明公开了一种基于距离测量的静止刚体定位方法，其先确定刚体定位问题的约束最小二乘表述形式，其包含有待估计的旋转矩阵和位置矢量；然后通过改变约束条件，得到刚体定位问题的约束最小二乘表述形式的转化形式，进而得到刚体定位问题的半正定规划形式；再对刚体定位问题的半正定规划形式进行求解，得到旋转矩阵和位置矢量各自的初步值；最后对旋转矩阵的初步值进行正交化，将正交化后的值作为旋转矩阵的估计值，且直接将位置矢量的初步值作为其估计值；优点是其在无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，也能对刚体的旋转和位置进行精确估计，且计算复杂度低。

主权项

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个未知节点在全局参考坐标系中的坐标位置对应记为s1,...,si,...,sN；然后获取每个未知节点到各个锚节点的测量距离，将第i个未知节点到第m个锚节点的测量距离记为rmi；其中，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；接着对每个未知节点到各个锚节点的测量距离的模型描述进行形式整理，对于r_mi＝||a_m‑s_i||+v_mi＝||a_m‑Qc_i‑t||+v_mi，将等式r_mi＝||a_m‑Qc_i‑t||+v_mi两边同时平方，忽略同时将||a_m‑Qc_i‑t||替换为r_mi，整理得到再将堆砌成向量的形式，描述为：最后令成立，并确定刚体定位问题的约束最小二乘表述形式，描述为：其中，Q表示旋转矩阵，Q的维数为3×3，t表示位置矢量，t代表刚体运动后局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，符号“|| ||”为求欧几里德范数符号，v_mi表示r_mi中存在的测量噪声，v_mi服从零均值的高斯分布表示v_mi的功率，为c_i的转置，为a_m的转置，符号为克罗内克积运算符号，vec(Q)表示对Q进行矩阵矢量化，Q^T为Q的转置，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中存在的测量噪声，min()为取最小值函数，(d‑Hf)^T为(d‑Hf)的转置，为的逆，[r₁₁,…,r_M1,r₁₂,…,r_M2,…,r_1N,...,r_MN]^T为[r₁₁,...,r_M1,r₁₂,…,r_M2,…,r_1N,…,r_MN]的转置，diag()为对角线矩阵表示形式，表示v₁₁的功率，表示v_M1的功率，表示v₁₂的功率，表示v_M2的功率，表示v_1N的功率，表示v_MN的功率，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，Q^TQ＝I和det(Q)＝1为Q需要满足的条件，“s.t.”表示“受约束于……”；步骤四：令F＝ff^T，使刚体定位问题的约束最小二乘表述形式中的约束条件Q^TQ＝I等价于并使f中的Q^Tt形成约束条件根据(Q^Tt)^TQ^Tt＝t^Tt和||t||²＝t^Tt，得到约束条件然后舍掉刚体定位问题的约束最小二乘表述形式中的约束条件det(Q)＝1，将刚体定位问题的约束最小二乘表述形式转化为：再根据F＝ff^T等价于去掉非凸的关于矩阵F的约束rank(F)＝1，将结合到刚体定位问题的约束最小二乘表述形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：最后对刚体定位问题的半正定规划形式进行求解，得到Q和t各自的初步值，对应记为Q_sdp和t_sdp；其中，H^T为H的转置，d^T为d的转置，F为引入的矩阵，F的维数为16×16，f^T为f的转置，tr()表示求一个矩阵中的所有对角元素的值的和，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(10:12,10:12)表示由F的第10行至第12行、第10列至第12列所有元素形成的矩阵，F(13:15,13:15)表示由F的第13行至第15行、第13列至第15列所有元素形成的矩阵，f(16)表示f中的第16个元素的值，符号表示一个矩阵是半正定的，rank()表示求一个矩阵的秩；步骤五：对Q_sdp进行正交化，将正交化后得到的值记为Q_ort，Q_ort满足且det(Q_ort)＝1；然后将Q_ort作为Q的估计值，将t_sdp作为t的估计值；其中，为Q_ort的转置，det(Q_ort)表示求Q_ort的行列式。