【技术实现步骤摘要】
一种基于距离和多普勒测量的移动刚体定位方法
本专利技术涉及一种目标定位方法，尤其是涉及一种无线传感器网络中基于距离和多普勒测量的移动刚体(即为有固定形状的移动的物体)定位方法，其定位内容为估计刚体的旋转、位置、旋转角速度和位移速度。
技术介绍
近些年来，随着无线传感器技术的快速发展，无线传感器网络(WSN)在与定位、监控、安防等相关的不同领域得到了广泛应用。在很多实际应用中，比如机器人、航空航天、水下探测器等方面，刚体并非总是静止不动的，即不仅需要估计刚体的旋转和位置，还需要对其旋转角速度和位移速度进行估计。因此，对无线传感器网络中移动刚体的定位方法的研究十分有必要。目前，在无线传感器网络中移动刚体的定位方法中，使用较多的是基于距离和多普勒测量的方法，其优点是测量系统复杂度低，定位精度较高。但是，当无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，现有的基于距离和多普勒测量的方法的定位精度会明显下降。
技术实现思路
本专利技术所要解决的技术问题是提供一种无线传感器网络中基于距离和多普勒测量的移动刚体定位方法，其在无线传感器网络中锚节点分布比较差或无线传感器网络中噪声比较大的情况下，也能对刚体的旋转、位置、旋转角速度和位移速度进行精确估计。本专利技术解决上述技术问题所采用的技术方案为：一种基于距离和多普勒测量的移动刚体定位方法，其特征在于包括以下步骤：步骤一：设定无线传感器网络中存在M个用于接收测量信号的锚节点和一个刚体，并设定刚体的内部放置有N个用于发射测量信号的未知节点；在无线传感器网络中建立一个空间坐标系作为全局参考坐标系，并在刚体的内...
【技术保护点】
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个未知节点在全局参考坐标系中的坐标位...
【技术特征摘要】
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；并获取每个未知节点相对于各个锚节点的多普勒测量值，，将第i个未知节点相对于第m个锚节点的多普勒测量值记为其中，s1表示当前时刻下第1个未知节点在全局参考坐标系中的坐标位置，si表示当前时刻下第i个未知节点在全局参考坐标系中的坐标位置，sN表示当前时刻下第N个未知节点在全局参考坐标系中的坐标位置；步骤三：对当前时刻下每个未知节点在全局参考坐标系中的坐标位置以模型方式进行描述，将si的模型描述为：si＝Qci+t；然后对当前时刻下每个未知节点在全局参考坐标系中的坐标位置的模型求关于时间的导数，对于si＝Qci+t，对si＝Qci+t求关于时间的导数，得到si的导数，记为接着对每个未知节点到各个锚节点的距离测量值以模型方式进行描述，将rmi的模型描述为：并对每个未知节点相对于各个锚节点的多普勒测量值以模型方式进行描述，将的模型描述为：之后对每个未知节点到各个锚节点的距离测量值的模型描述进行形式整理，对于将等式rmi＝||am-Qci-t||+vmi两边同时平方，忽略同时将||am-Qci-t||替换为rmi，整理得到并对每个未知节点相对于各个锚节点的多普勒测量值的模型描述进行形式整理，对于将等式两边同与rmi相乘，忽略整理得到再将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：并将i＝1,...,N,m＝1,...,M堆砌成向量的形式，描述为：最后令和成立，并确定刚体定位问题的约束最小二乘表述形式，描述为：其中，Q表示旋转矩阵，Q的维数为3×3，t表示位置矢量，t代表当前时刻下局部参考坐标系的原点在全局参考坐标系中的坐标位置，t的维数为3×1，ω表示刚体的旋转角速度矢量，ω＝[ω1,ω2,ω3]T，符号“[]”为矢量表示符号，[ω1,ω2,ω3]T为[ω1,ω2,ω3]的转置，ω1表示刚体绕X轴旋转的角速度，ω2表示刚体绕Y轴旋转的角速度，ω3表示刚体绕Z轴旋转的角速度，[ω]×表示ω的叉乘矩阵，[ω]×的维数为3×3，表示局部参考坐标系的原点的移动速度，表示第i个未知节点到第m个锚节点的真实距离值，vmi表示rmi中存在的测量噪声，vmi服从零均值的高斯分布表示vmi的功率，符号“||||”为求欧几里德范数符号，(si-am)T为(si-am)的转置，表示中存在的测量噪声，服从零均值的高斯分布表示的功率，为ci的转置，为am的转置，符号为克罗内克积运算符号，vec(Q)表示对Q进行矩阵矢量化，QT为Q的转置，为si的转置，(t-am)T为t-am的转置，(Qci+t-am)T为Qci+t-am的转置，d1＝[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]T，[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]T为[p11,...,pM1,p12,…,pM2,…,p1N,…,pMN]的转置，r11表示第1个未知节点到第1个锚节点的距离测量值，rM1表示第1个未知节点到第M个锚节点的距离测量值，r12表示第2个未知节点到第1个锚节点的距离测量值，c2表示刚体运动前第2个未知节点在局部参考坐标系中的坐标位置，rM2表示第2个未知节点到第M个锚节点的距离测量值，r1N表示第N个未知节点到第1个锚节点的距离测量值，rMN表示第N个未知节点到第M个锚节点的距离测量值，H1＝[h11,...,hM1,h12,…,hM2,…,h1N,…,hMN]T，[h11,...,hM1,h12,…,hM2,…,h1N,…,hMN]T为[h11,...,hM1,h12,…,hM2,…,h1N,…,hMN]的转置，为的转置，为c1的转置，为a1的转置，为的转置，为aM的转置，为的转置，为c2的转置，为的转置，为的转置，为cN的转置，为的转置，f1＝[(vec(Q))T,tT,(QTt)T,||t||2]T，[(vec(Q))T,tT,(QTt)T,||t||2]T为[(vec(Q))T,tT,(QTt)T,||t||2]的转置，(vec(Q))T为vec(Q)的转置，tT为t的转置，(QTt)T为QTt的转置，[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个锚节点的多普勒测量值，H2＝[q11,...,qM1,q12,…,qM2,…,q1N,…,qMN]T，[q11,...,qM1,q12,…,qM2,…,q1N,…,qMN]T为[q11,...,qM1,q12,…,qM2,…,q1N,…,qMN]的转置，为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，为的转置，P'＝[ω]×Q，P'T为P'的转置，(P'Tt)T为P'Tt的转置，vec(P')表示对P'进行矩阵矢量化，(vec(P'))T为vec(P')的转置，为的转置，为的转置，[n11,...,nM1,n12,…,nM2,…,n1N,…,nMN]T为[n11,...,nM1,n12,…,nM2,…,n1N,…,nMN]的转置，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，表示中存在的测量噪声，min()为取最小值函数，“s.t.”表示“受约束于……”，为的转置，为d1的转置，为d2的转置，为的转置，为H1的转置，为H2的转置，为的转置，为f1的转置，为f2的转置，(d-Hf)T为(d-Hf)的转置，为的逆，diag()为对角线矩阵表示形式，[r11,...,rM1,r12,…,rM2,…,r1N,…,rMN]T为[r11,...,rM1,r12,…,rM2,…,r1N,…,rMN]的转置，为的转置，KT为K的转置，0(M×N)×(M×N)表示元素全为0的矩阵，其维度为(M×N)×(M×N)，表示v11的功率，表示vM1的功率，表示v12的功率，表示vM2的功率，表示v1N的功率，表示vMN的功率，表示的功率，表示的功率，表示多普勒测量噪声的功率，表示多普勒测量噪声的功率，表示的功率，表示的功率，I为单位矩阵，I的维数为3×3，det(Q)表示求Q的行列式，QTQ＝I和det(Q)＝1为Q需要满足的条件；步骤四：令F＝ffT，使刚体定位问题的约束最小二乘表述形式中的约束条件QTQ＝I等价于并使f中的QTt形成约束条件使f中的P'Tt形成约束条件使f中的形成约束条件使f中的形成约束条件f(32)＝tr(F(10:12,33:35))；根据(QTt)TQTt＝tTt和||t||2＝ttT，得到约束条件根据和得到约束条件根据(P'Tt)TQTt＝0和(P'Tt)TQT＝-(P'(QTt))T，得到约束条件根据QTP'是反对称矩阵，得到约束条件然后舍掉刚体定位问题的约束最小二乘表述形式中的约束条件det(Q)＝1，将刚体定位问题的约束最小二乘表述形式转化为：再根据F＝ffT等价于去掉非凸的关于矩阵F的约束rank(F)＝1，将结合到刚体定位问题的约束最小二乘表述形式的转化形式中，得到刚体定位问题的半正定规划形式，描述为：最后对刚体定位问题的半正定规划形式进行求解，直接得到Q、t、和P'各自的初步值，对应记为Qsdp、tsdp、和P'sdp；进而根据等式[ω]×＝(P'QT-QP'T)/2间接求得[ω]×的初步值，记为[ωsdp]×，根据等式ω＝[[ω]×(3,2),[ω]×(1,3),[ω]×(2,1)]T，并结合[ωsdp]×，得到ω的初步值，记为ωsdp，ωsdp＝[[ωsdp]×(3,2),[ωsdp]×(1,3),[ωsdp]×(2,1)]T；其中，HT为H的转置，dT为d的转置，F为引入的矩阵，F的维数为35×35，fT为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的第...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。