CN108872935A - 一种基于距离测量的静止刚体定位方法 - 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；其中，s₁表示刚体运动后第1个未知节点在全局参考坐标系中的坐标位置，s_i表示刚体运动后第i个未知节点在全局参考坐标系中的坐标位置，s_N表示刚体运动后第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的行列式。

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

步骤六：对Q_ort和t_sdp进行优化，得到Q_ort和t_sdp各自的优化值，对应记为Q_fin和t_fin；将Q_fin作为Q的最优估计值，将t_fin作为t的最优估计值。

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

1)本发明方法充分利用了测量距离等式平方后估计参数和已知参数的线性关系，减少了半正定规划问题中的优化变量，降低了计算复杂度，缩短了计算时间。

2)本发明方法中的优化变量较少，且优化变量只与旋转矩阵和位置矢量有关，因此估计结果精度较高，且在无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，也能对刚体的旋转和位置进行精确估计。

3)本发明方法中半正定规划问题中的优化变量个数是固定的，不会随锚节点和刚体内未知节点的个数变化而改变，因此本发明方法的稳健性较高，在不同场景中均有较高的定位精度。

附图说明

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

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

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

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

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

具体实施方式

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

本发明提出的一种基于距离测量的静止刚体定位方法，其总体实现流程框图如图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)；其中，s₁表示刚体运动后第1个未知节点在全局参考坐标系中的坐标位置，s_i表示刚体运动后第i个未知节点在全局参考坐标系中的坐标位置，s_N表示刚体运动后第N个未知节点在全局参考坐标系中的坐标位置，s₁,...,s_i,...,s_N未知，图2b同时给出了刚体运动后刚体的内部设置的未知节点在全局参考坐标系中的坐标位置。

步骤三：对刚体运动后每个未知节点在全局参考坐标系中的坐标位置以模型方式进行描述，将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||2]的转置，(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()表示求一个矩阵的秩。

步骤五：由于在步骤四中忽略了约束条件det(Q)＝1，因此刚体定位问题的半正定规划形式关于Q的求解结果Q_sdp并不准确，甚至不能满足旋转矩阵的性质，即可能出现或者det(Q_sdp)＜0的情况，为Q_sdp的转置，故在本步骤中采用现有技术对Q_sdp进行正交化，将正交化后得到的值记为Q_ort，Q_ort满足且det(Q_ort)＝1；然后将Q_ort作为Q的估计值，将t_sdp作为t的估计值；其中，为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_ort和t_sdp进行优化，得到Q_ort和t_sdp各自的优化值，对应记为Q_fin和t_fin；将Q_fin作为Q的最优估计值，将t_fin作为t的最优估计值。

在本实施例中，步骤六中对Q_ort和t_sdp进行优化采用的优化方法为论文“J.Jiang,G.Wang,and K.C.Ho,``Accurate localization of a rigid body via semidefiniterelaxation,”IEEE Signal Process.Lett.,vol.25,no.3,pp,378-382,Mar.2018.”(蒋坚，王刚，K.C.Ho，“基于半正定规划的精确刚体定位，”IEEE Signal Process.Lett，第25卷，第3期，页码，378-382，三月，2018年)中公开的优化方法，步骤六的具体过程为：

1)令Q_fin表示Q的最优估计值，令t_fin表示t的最优估计值；令Q_fin＝Q_ortQ_δ，t_fin＝t_sdp+Δt；如果Q_δ满足旋转矩阵的性质，则Q_fin也满足，因此在合理的假设Q_δ中的欧拉角都接近于0的前提下，使用近似等式cosx≈1,sinx≈x，x为欧拉角，则得到Q_δ的近似表达式为：然后对Q_δ的近似表达式进行线性化，得到vec(Q_δ)＝γ+Lβ；并将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_mi＝(r_mi-||e_mi||)||e_mi||，则有再将 堆砌成向量的形式，描述为：最后令成立，求解中的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 01]的转置，为的转置，β＝[φ θ ψ]^T，[φ θ ψ]^T为[φ θ ψ]的转置， 为c_i的转置，

g＝[β^T,Δt^T]^T，β^T为β的转置，Δt^T为Δt的转置，[β^T,Δt^T]^T为[β^T,Δt^T]的转置，为e_mi的转置，qmi为引入的变量，[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]^T为[q₁₁,...,q_M1,q₁₂,…,q_M2,…,q_1N,…,q_MN]的转置，q₁₁＝(r₁₁-||e₁₁||)||e₁₁||，q_M1＝(r_M1-||e_M1||)||e_M1||，q₁₂＝(r₁₂-||e₁₂||)||e₁₂||，q_M2＝(r_M2-||e_M2||)||e_M2||，q_1N＝(r_1N-||e_1N||)||e_1N||，q_MN＝(r_MN-||e_MN||)||e_MN||， 为的转置，为U₁的转置， 为U₂的转置， 为U_N的转置，

[||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]的转置，为的转置，为的逆， [||e₁₁||，...，||e_M1||，||e₁₂||，…，||e_M2||，…，||e_1N||，…，||e_MN||]^T为[||e₁₁||，...，||e_M1||，||e₁₂||，…，||e_M2||，…，||e_1N||，…，||e_MN||]的转置， 为的逆。

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

上述步骤六的过程是为了进一步提高定位精度，对已求得的Q_ort和t_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.01,10]。

测试本发明方法的性能随测量噪声标准差增加的变化情况。图3给出了本发明方法与现有的分拆各个击破的方法和现有的半正定规划方法的关于旋转矩阵Q的估计值与旋转矩阵Q的真实值的均方根误差随测量噪声标准差增加的变化图；图4给出了本发明方法与现有的分拆各个击破的方法和现有的半正定规划方法的关于位置矢量t的估计值与位置矢量t的真实值的均方根误差随测量噪声标准差增加的变化图。从图3和图4中可以看出，在无线传感器网络中锚节点分布比较差和无线传感器网络中噪声比较大的情况下，无论是关于Q，还是关于t的估计，本发明方法明显优于现有的分拆各个击破的方法，当测量噪声的功率在中等到较大的水平时，本发明方法对t的估计也优于现有的半正定规划方法，对Q的估计与现有的半正定规划方法性能接近，同时本发明方法的复杂度比现有的半正定规划方法低，足以说明本发明方法在刚体定位的精度方面有足够的优势。

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；其中，s₁表示刚体运动后第1个未知节点在全局参考坐标系中的坐标位置，s_i表示刚体运动后第i个未知节点在全局参考坐标系中的坐标位置，s_N表示刚体运动后第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的行列式。

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

步骤六：对Q_ort和t_sdp进行优化，得到Q_ort和t_sdp各自的优化值，对应记为Q_fin和t_fin；将Q_fin作为Q的最优估计值，将t_fin作为t的最优估计值。