基于校准源的运动固态体定位方法技术

本发明专利技术提出了一种基于校准源地运动固态体定位方，实现步骤为：(1)构建运动固态体的定位场景并初始化参数；(2)构建测量向量；(3)构造估计向量并获取估计向量的最大似然解；(4)获取运动固态体的定位结果。本发明专利技术通过在定位场景中引入了一个校准源，在构建测量向量时使用了校准源与锚节点之间的距离测量信息，通过每次迭代过程中对测量向量更新的过程，不断消除锚节点的位置误差，有效抑制了由锚节点位置误差带来的定位精度的恶化问题，实现了对运动固态体定位精度的提升。固态体定位精度的提升。固态体定位精度的提升。

【技术实现步骤摘要】
基于校准源的运动固态体定位方法


[0001]本专利技术属于信号处理
，涉及一种固态体定位方法，具体涉及一种基于校准源的运动固态体定位方法，可应用于无人机驾驶汽车、水下救援等系统。

技术介绍

[0002]固态体是指在运动过程中或受到外力的作用后具有相同形状和尺寸，并且内部点的相对位置恒定的物体，绝对固态体实际上并不存在，只是一个理想的模型，因为任何物体在施加外力后都会产生形变，如果变形程度相对于物体本身的几何尺寸可忽略不计，就可将该物体视为固态体。根据运动状态固态体可分为静止固态体和运动固态体。运动固态体定位是指使用固态体上安装的多个传感器与固态体周围多个锚节点之间的距离测量信息、时间测量信息或角度测量信息等确定运动固态体的位置信息、方向信息、角速度信息及速度信息，例如在无人驾驶汽车应用中，不仅有汽车位置移动，还有汽车的转弯等操作，因此除汽车的位置信息及方向信息外，还需获取处于运动状态下汽车的角速度信息以及速度信息，保障对汽车的基本控制和实时调整，降低操作风险；在水下救援系统中，获取救援设备如机器人、无人机等的位置信息、方向信息来保证救援设备的精确部署，同时获取救援设备的角速度信息及速度信息实现远程自动操作，为救援任务的有序开展打下基础。
[0003]运动固态体定位重点在于如何提高定位精度，而影响定位精度主要因素在于固态体周围锚节点的位置误差、固态体上安装的传感器的位置误差、传感器与锚节点之间的距离测量误差及多普勒测量误差等。例如，S.Chen和Ho在2015年在其发表的论文"Accurate Localization of a Rigid Body Using Multiple Sensors and Landmarks,"in IEEE Transactions on Signal Processing,vol.63,no.24,pp.6459
‑
6472,Dec.15 2015，提出了一种误差消除的运动固态体定位方法，该方法首先构建一个包括安装有多个传感器的运动固态体、分布在运动固态体周围处于静止状态的多个锚节点的三维定位场景，并在局部参考系下获取所有传感器的位置信息，在全球参考系下获取所有锚节点的位置信息及固态体带有误差的参数信息，包括位置信息、方向信息、角速度信息及速度信息；其次通过局部参考系下所有传感器的位置信息以及全球参考系下所有锚节点的位置信息、运动固态体带有误差的参数信息，获取每个锚节点与每个传感器之间的距离测量信息及多普勒测量信息，并基于此构建测量向量；然后通过测量向量构建以运动固态体无误差的位置信息、方向信息、角速度信息及速度信息为待估计参数的最大似然模型，基于高斯
‑
牛顿迭代法则，通过每次迭代过程中对测量向量更新的过程，消除了传感器与锚节点之间的距离测量误差及多普勒测量误差，实现对最大似然模型的优化，最终得到待估计参数的最大似然解，并通过数学转换获取运动固态体位置信息、方向信息、角速度信息及速度信息的估计结果。该方法有效抑制了由传感器与锚节点之间的距离测量误差及多普勒测量误差带来的定位精度的恶化，但其存在的不足之处在于，该方法构建的定位场景为理想场景，其内部所有的锚节点均不含有位置误差，但在实际应用中，不含有误差的锚节点位置信息无从获取，可得的只有带有误差的锚节点位置信息，将会对基于所有锚节点位置信息构建的测量向量引入额外的
误差，使得基于测量向量求得的最大似然解偏离正确解，最终造成定位精度的降低。

技术实现思路

[0004]本专利技术的目的在于克服上述现有技术存在的缺陷，提出了一种基于校准源的运动固态体定位方法，用于解决现有技术中存在的因锚节点位置存在误差导致的定位精度下降的问题。
[0005]为实现上述目的，本专利技术采取的技术方案包括如下步骤：
[0006](1)构建运动固态体的定位场景并初始化参数：
[0007]构建包括安装有N个传感器的运动固态体、分布在运动固态体周围处于静止状态的M个锚节点和一个校准源的运动固态体的三维定位场景；初始化所述N个传感器在以运动固态体几何中心为原点的局部参考系Oq1q2q3下的位置坐标为c＝[c1,c2,
…
,c
n
,
…
,c
N
]T
；所述M个锚节点、校准源在全球参考系Oxyz下的位置坐标分别为a＝[a1,a2,
…
,a
m
,
…
,a
M
]T
、p＝[p
x
,p
y
,p
z
]T
，a、p对应的位置误差分别为Δa＝[Δa1,Δa2,
…
,Δa
m
,
…
,Δa
M
]T
、Δp＝[Δp
x
,Δp
y
,Δp
z
]T
，Δa、Δp的标准差分别为σ
a
、σ
p
；所述运动固态体在全球参考系下含有误差的方向信息、位置信息、角速度信息、速度信息分别为q＝[α,β,γ]T
、t＝[t
x
,t
y
,t
z
]T
、w＝[w
x
,w
y
,w
z
]T
、其中，N≥5，M≥6，c
n
表示第n个传感器在局部参考系下的位置坐标，a
m
表示第m个锚节点在全球参考系下位置坐标，a
m
＝[a
mx
,a
my
,a
mz
],Δa
m
表示a
m
对应的位置误差，Δa
m
＝[Δa
mx
,Δa
my
,Δa
mz
]，α、β、γ表示所述固态体在全球参考系中的偏航角、翻滚角、俯仰角，
·
T
表示转置操作；
[0008](2)构建测量向量：
[0009]通过每个传感器在局部参考系下的位置坐标c
n
、全球参考系下每个锚节点的位置坐标a
m
和运动固态体含有误差的方向信息q、位置信息t、角速度信息w和速度信息计算每个传感器和每个锚节点之间的距离测量值r
mn
及多普勒测量值得到距离测量向量r＝[r
11
,r
21
,
…
,r
mn
,
…
,r
MN
]T
和多普勒测量向量同时通过在全球坐标系下每个锚节点的位置信息a
m
和校准源的位置信息p，计算每个锚节点与校准源之间的距离测量值d
m
，得到距离测量向量d＝[d1,d2,...,d
m
,
…
,d
M
...

【技术保护点】

【技术特征摘要】
1.一种基于校准源的运动固态体定位方法，其特征在于，包括如下步骤：(1)构建运动固态体的定位场景并初始化参数：构建包括安装有N个传感器的运动固态体、分布在运动固态体周围处于静止状态的M个锚节点和一个校准源的运动固态体的三维定位场景；初始化所述N个传感器在以运动固态体几何中心为原点的局部参考系Oq1q2q3下的位置坐标为c＝[c1,c2,
…
,c
n
,
…
,c
N
]
T
；所述M个锚节点、校准源在全球参考系Oxyz下的位置坐标分别为a＝[a1,a2,
…
,a
m
,
…
,a
M
]
T
、p＝[p
x
,p
y
,p
z
]
T
，a、p对应的位置误差分别为Δa＝[Δa1,Δa2,
…
,Δa
m
,
…
,Δa
M
]
T
、Δp＝[Δp
x
,Δp
y
,Δp
z
]
T
，Δa、Δp的标准差分别为σ
a
、σ
p
；所述运动固态体在全球参考系下含有误差的方向信息、位置信息、角速度信息、速度信息分别为q＝[α,β,γ]
T
、t＝[t
x
,t
y
,t
z
]
T
、w＝[w
x
,w
y
,w
z
]
T
、其中，N≥5，M≥6，c
n
表示第n个传感器在局部参考系下的位置坐标，a
m
表示第m个锚节点在全球参考系下位置坐标，a
m
＝[a
mx
,a
my
,a
mz
],Δa
m
表示a
m
对应的位置误差，Δa
m
＝[Δa
mx
,Δa
my
,Δa
mz
]，α、β、γ表示所述固态体在全球参考系中的偏航角、翻滚角、俯仰角，
·
T
表示转置操作；(2)构建测量向量：通过每个传感器在局部参考系下的位置坐标c
n
、全球参考系下每个锚节点的位置坐标a
m
和运动固态体含有误差的方向信息q、位置信息t、角速度信息w和速度信息计算每个传感器和每个锚节点之间的距离测量值r
mn
及多普勒测量值得到距离测量向量r＝[r
11
,r
21
,
…
,r
mn
,
…
,r
MN
]
T
和多普勒测量向量同时通过在全球坐标系下每个锚节点的位置信息a
m
和校准源的位置信息p，计算每个锚节点与校准源之间的距离测量值d
m
，得到距离测量向量d＝[d1,d2,...,d
m
,...,d
M
]
T
，并通过r、d以及步骤(1)中校准源的位置信息p和M个锚节点的位置坐标a，构建测量向量(3)构造估计向量并获取估计向量的最大似然解：(3a)通过全球参考系下固态体的方向估计量、位置估计量、角速度估计量、速度估计量q
o
、t
o
、w
o
、以及M个锚节点位置坐标a的估计量a
o
和校准源位置坐标p的估计量p
o
，构造估计向量同时通过M个锚节点与N个传感器之间距离测量向量r、多普勒测量向量的误差协方差矩阵R
v
、M...

【专利技术属性】
技术研发人员：郝本建，程帅，李赞，陈小军，赵越，李思文，黄淑冰，赵曼，
申请(专利权)人：西安电子科技大学，
类型：发明
国别省市：