欢迎来到知嘟嘟! 联系电话:13336804447 卖家免费入驻,海量在线求购! 卖家免费入驻,海量在线求购!
知嘟嘟
我要发布
联系电话:13336804447
知嘟嘟经纪人
收藏
专利号: 2018105672107
申请人: 宁波大学
专利类型:发明专利
专利状态:已下证
专利领域: 测量;测试
更新日期:2026-11-16
缴费截止日期: 暂无
价格&联系人
年费信息
委托购买

摘要:

权利要求书:

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,符号T“[]”为矢量表示符号,[ω1,ω2,ω3]为[ω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)表示T T T对Q进行矩阵矢量化,Q为Q的转置, 为si的转置,(t-am)为t-am的转置,(Qci+t-am) 为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,T

2r12v12,…,2rM2vM2,…,2r1Nv1N,…,2rMNvMN]为[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'进行矩阵矢量化,(vecT(P')) 为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中T

的P ' t形成约束条件 使f中的 形成约束条件

使f中的 形成约束条件f(32)=tr(F(10:12,33:35));根据T T T T 2 T

(Qt)Q t=tt和||t||=tt,得到约束条件 根据

和 得到约束条件

根据(P'Tt)TQTt=0和(P'Tt)TQT=-(P'(QTt))T,T

得到约束条件 根据QP'是反

对称矩阵,得到约束条件 然后舍掉刚体定位问题的

约束最小二乘表述形式中的约束条件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的第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'(QTt))T为P'(QTt)的转置,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()表示求一个矩阵的秩, 为Qsdp的转置, 为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)]的转置;

步骤五:对Qsdp进行正交化,将正交化后得到的值记为Qort,Qort满足 且det(Qort)=1;然后将Qort作为Q的估计值,将tsdp作为t的估计值,将ωsdp作为ω的估计值,将作为 的估计值,将P'sdp作为P'的估计值;其中, 为Qort的转置,det(Qort)表示求Qort的行列式。

2.根据权利要求1所述的一种基于距离和多普勒测量的移动刚体定位方法,其特征在于所述的步骤五执行完毕后,执行以下步骤六和步骤七,具体如下:步骤六:令Qfin表示Q的最优估计值,令tfin表示t的最优估计值,令ωfin表示ω的最优估计值,令 表示 的最优估计值;令Qfin=QortQδ,tfin=tsdp+Δt,ωfin=ωsdp+Δω,假设Qδ中的欧拉角都接近于0的前提下,使用近似等式cosx≈1,sinx≈x,x表示欧拉角,则得到Qδ的近似表达式为:

然后对Qδ的近似表达式和

[ωsdp]×进行线性化,得到vec(Qδ)=γ+Lβ,vec([ωfin]×)=Φωfin;接着将Qfin=QortQδ和tfin=tsdp+Δt代入rmi=||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-Uig1||+vmi,对rmi=||emi-Uig1||+vmi的等式右边进行一阶泰勒展开,得到 对 的两边同乘以||emi||,得到令 则有

并将Qfin=QortQδ、tfin=tsdp+Δ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的叉乘矩阵,为 的转置,

g1=[βT,ΔtT]T,[βT,ΔtT]T为[βT,ΔtT]的转置,βT为β的转置,ΔtT为Δt的转置, 为emi的转置, 为引入的变量,为

的转置,

为

的转置, 为U1的转置,

为U2的转置, 为UN的转置,

g1=[βT,ΔtT]T,[βT,ΔtT]T为[βT,ΔtT]的转置,[||e11||v11,…,||

eM1||vM1,||e12||v12,…,||eM2||vM2,…,||e1N||v1N,…,||eMN||vMN]T为[||e11||v11,...,||eM1||vM1,||e12||v12,…,||eM2||vM2,…,||e1N||v1N,…,||eMN||vMN]的转置, 为tsdp的转置,为 的转置, 为 的转置,(Qortci)T为Qortci的转置,(tsdp-am)T为tsdp-am的转置,(Qortci+tsdp-am)T为Qortci+tsdp-am的转置,表示 中存在的测量噪声,

为 的转置,

为 的

转置, 为 的转置, 为 的转置, 为 的转置, 为 的转置, 为的转置, 为 的转置, 为

T

的转置,Δω 为Δω的转置, 为 的转置,

0(M×N)×6表示元素全为0的矩阵,其维度为(M×N)×6, 为的转置,

[||e11||,...,||eM1||,||e12||,…,||eM2||,…,||e1N||,…,||eMN||]T为||e11||,…,||eM1||,||e12||,…,||eM2||,…,||e1N||,…,||eMN||]的转置, 为 的转置, 为 的逆, 为 的逆;

步骤七:将 代入 中,得到 进而根据

得到β、Δt、Δω和 的估计值,对应记为 和 然

后将 代入β=[φ θ ψ]T中,得到 进而根据 得到φ、θ和ψ各自的值;接着将φ、θ和ψ各自的值代入 中,得到

Qδ的估计值,记为 最后将 代入Qfin=QortQδ中,得到 即得到Qfin的值;将代入tfin=tsdp+Δt中,得到 即得到tfin的值,将 代入ωfin=ωsdp+Δω中,得到 即得到ωfin的值,将 代入 中,得到

即得到 的值。