一种基于无线电测距和人造光源角测量的融合定位系统

1.本发明提出一种基于无线电测距和人造光源角测量的融合定位系统，它涉及一种新型融合定位方法的系统组成，属于导航定位定姿技术领域。


背景技术：

2.室内导航与定位系统是指在各种室内空间中采用不同技术来实现人员室内导航以及对人员、物体的定位与跟踪。随着物联网技术的发展，人员、物体在经济行为、个人活动、军事等应用领域的定位需求越来越多。室外比较成熟的定位系统有全球卫星定位系统gnss；但是在室内空间越来越庞大的今天，各个工厂车间、大型购物商城、办公楼、地铁站的不断建设，基于室内的导航与定位对人员、物体的安全与监测是必不可少的，人们在室内环境中的位置服务需求已经日趋显著。然而在室内环境，gnss的信号往往会比较差，无法承担精确的定位功能。因此，亟需研制不依赖gnss的室内导航定位系统。
3.目前室内定位技术有：红外定位、超声波定位、惯性导航定位、视觉定位、基于射频信号定位。其中红外定位系统由于红外线穿透力差，不能穿透固体墙壁，它们只能提供房间级的位置传感功能，且传播只有几米的有效范围，容易受到光照或者荧光灯的影响而产生盲区，定位效果不理想；超声波定位系统虽然定位精度较高，但是对硬件布设方面技术要求较高，而且成本很高；惯性导航定位的定位结果会随时间而发散，因此依靠自身无法进行长时间定位；视觉定位系统相对而言信号探测范围宽、获取信息完整，但是对处理器要求更高；基于射频信号定位系统虽然都具有功耗低、运用广泛等优点，但同时稳定性较差、受环境干扰大。由此可见单一的室内定位系统优缺点分明，无法同时满足室内导航硬件要求低、稳定性强、定位精度高的要求，所以需要对不同传感器进行位置信息融合，即融合定位。融合定位技术把多种技术的优势结合以达到更高精度从而提高定位的鲁棒性。目前融合定位的研究方案有多种，例如wifi测量结合pdr定位系统、蓝牙测量结合加速度计和气压计融合定位系统、地磁场定位和惯性传感器融合定位系统，但是总的来说，融合定位系统目前还不完善，有很大的发展空间。
4.综上，基于无线电测距以及人造光源角测量的定位系统具有巨大的研究与应用价值，本发明提出了一种基于无线电测距以及人造光源角测量的融合定位系统。


技术实现要素：

5.(一)发明目的
6.本发明为解决gnss拒止环境下的定位问题，提出了一种基于无线电测距以及人造光源角测量的融合定位系统。本发明具有完全自主性、可以实现无gnss的全天时位姿确定，适用于室内环境定位、地下空间定位等的应用场景。
7.(二)技术方案
8.本发明的定位系统由三个模块组成，分别是无线电测距模块、角测量测向模块和导航解算模块。
9.无线电测距模块
10.首先将测量出的电磁波信号从导航用户节点到已知位置的携带无线电接收装置的人造光源的传播时间与已知的传播速度c相乘，得出导航用户与人造光源之间的估计距离|r|，则距离|r|可表述为式(1)：
11.|r|＝c(t
n
‑
t
m
)
ꢀꢀ
(1)
12.式中：t
n
为到达时间，t
m
为发射时间。
13.然后通过距离和位置矢量的关系，构建在在导航坐标系下位置矢量与光源到导航用户距离的联系方程：
[0014][0015]
式中：[x
n
,y
n
,z
n
]
t
为光源在导航坐标系下的位置，为已知量。
[0016]
[x
n
,y
n
,z
n
]
t
为导航用户在导航坐标系下的位置。
[0017]
角测量测向模块
[0018]
首先定义以下3个坐标系：
[0019]
相机坐标系：原点o
c
建立在镜头的光学中心，x
c
‑
o
c
‑
y
c
平面与成像平面平行，图像传感器一般采用矩形结构，x
c
轴和y
c
轴分别与图像传感器的行平行向右和列平行向下，z
c
轴沿光轴指向前方。
[0020]
图像坐标系：二维坐标系，原点o’位于成像平面的中心，也被称为图像主点，是透镜光轴与成像平面的交点，x’轴与x轴平行，y’轴和y轴平行。
[0021]
像素坐标系定义成：原点i位于图像左上角，u轴与x轴平行，v轴和y轴平行，单位用像素表示，u，v分别表示像素点的行号和列号。
[0022]
根据相机成像模型，投影点s’像素坐标[u,v]
t
与光源s的相机坐标系下的三维坐标[x
c
,y
c
,z
c
]
t
满足下述条件：
[0023][0024]
式中，图像坐标系与像素坐标系之间的坐标尺度缩放因子为α和β，这两个参数由传感器尺寸与像元分辨率决定，镜头焦距为f，定义式中f
x
＝αf,f
y
＝βf为等效焦距。
[0025]
式(3)写成矩阵形式为：
[0026][0027]
式中：矩阵k为相机的内参数矩阵。相机的内参数矩阵是相机的固有参数，对于定焦相机来说是出厂后不变的，可以通过相机标定来确定相机的内参，即内参数矩阵k为已知量。
[0028]
则光源s在相机坐标系下的坐标可由观测量[u,v]
t
表示：
[0029][0030]
在式(5)中，使用的是光源s在相机坐标系下的坐标，但是实际运动中由于相机在运动，所以光源s的相机坐标应该是它在导航坐标系下的位置根据相机的当前位姿变换到相机坐标系下的结果。
[0031]
相机的位姿由其的旋转矩阵和平移向量t
c
来描述，根据欧式刚体变换公式，得出光源s在导航坐标系和相机坐标系两者间的变换关系：
[0032][0033]
式中：表示相机本体坐标系相对于导航坐标系的姿态变换矩阵，t
c
表示导航坐标系原点o在相机坐标系下的坐标。
[0034]
将式(6)改写成齐次坐标下的线性形式为：
[0035][0036]
式中：e表示相机的外参数矩阵。
[0037]
将式(7)带入式(5)中可得角测量方程式(8)，它描述了光源s的世界坐标到像素坐标的投影关系：
[0038][0039]
由于导航用户本体系与相机坐标系并不重合，将式(8)改写为：
[0040][0041]
式中：是相机的安装矩阵，δr
b
是相机光心在导航用户本体系下的坐标，z
c
是相机到光源的矢量在相机坐标系下的z轴坐标，表示光源在导航坐标系的坐标。以上各量均可视为常量。
[0042]
为导航用户本体系到导航坐标系的旋转矩阵，其中姿态角变换为ψ、θ、γ，变换顺序为zyx。本发明考虑的室内环境，地面视为水平，因此θ、γ可视为不随时间变化的常量，导航用户只做偏航运动，即ψ为需求解量。可由ψ、θ、γ进行表述为式(10)：
[0043][0044]
r
n
＝[x
n
,y
n
,z
n
]
t
为用户在导航坐标系下的位置矢量，本发明考虑的室内环境，地面视为水平，因此z
n
可视为已知量，导航用户只进行xy平面的平动，待求解量为x
n
,y
n
。
[0045]
导航解算模块
[0046]
根据无线电测距模块和角测量测向模块的观测方程，综合式(1)～(10)，联立得到导航方程组可以表述为式(11)：
[0047][0048]
其中：u,v,|r|为观测量，ψ,x
n
,y
n
为待求解量，其余均为已知量。
[0049]
因此导航用户的位置可通过牛顿迭代法求解定位方程组得到。
[0050]
至此完成对导航用户位置的解算。
[0051]
(三)优点
[0052]
本发明提供的一种基于无线电测距以及人造光源角测量的融合定位系统的优点在于：
[0053]
①
本发明提出的融合定位系统自主性好，无需依赖gnss等其他观测信息。
[0054]
②
本发明提出的融合定位系统能对两种观测信息进行融合，较单一观测定位系统定位精度更高。
[0055]
③
本发明提出的融合定位系统主要针对室内，地下环境，解决了此类环境下定位困难的问题。
[0056]
④
本发明提出的融合定位系统的硬件设备技术要求低，只需要一颗光源即可实现定位。
附图说明
[0057]
图1是本发明所述刚体变换模型示意图：o
c
‑
x
c
y
c
z
c
为相机坐标系、o'
‑
x'y'为图像坐标系、i
‑
uv为像素坐标系。
[0058]
图2是本发明所述的相机成像模型示意图：s为光源在导航坐标系下的位置、s’为光源在像素坐标下的位置。
[0059]
图3是本发明的实施步骤流程图。
具体实施方式
[0060]
下面描述为了便于本领域普通技术人员更好理解和实现本发明。以下实例仅作为举例，不应理解为本发明局限于这里描述的示例性实例。
[0061]
步骤一：构建距离
‑
位置矢量方程
[0062]
导航用户携带的无线电测距装置可以测得无线电发射时间与接收时间的时间差，然后根据式(1)可以求解得到光源到用户本体坐标系原点o的距离|r|，然后构建距离与位置矢量联系方程式：
[0063][0064]
本步骤对应附图3中的第一个方框的无线电测距模块。
[0065]
步骤二：构建测向观测方程
[0066]
同时光接受机能在图像上生成光源，根据图像信息可以得到光源在图像坐标系下的位置坐标[u,v]
t
。根据相机成像模型式(4)将光源在图像坐标系下的位置坐标转换为光源在相机坐标系下的坐标[x
c
,y
c
,z
c
]：
[0067][0068]
其中k为相机的内参数矩阵，z
c
是相机到光源的矢量在相机坐标系下的z轴坐标，均为已知量。
[0069]
再结合欧式刚体变换公式(6)构建光源在相机坐标系下的位置与在导航坐标系下的位置[x
n
,y
n
,z
n
]关系式：
[0070][0071]
考虑到相机坐标系与导航用户本体坐标系由于安装问题并不重合，因此将旋转矩阵与平移向量进行分解：
[0072][0073]
其中是相机的安装矩阵，δr
b
是相机光心在导航用户本体系下的坐标，表示光源在导航坐标系的坐标，以上均为已知量。
[0074]
然后将式(14)和(15)计算结果带入式(13)中，构建测向观测方程(16)：
[0075]
[0076]
其中：由于场景为室内环境，导航用户可只进行oxy平面的运动，即俯仰和滚转角以及导航坐标系下的z轴坐标均为已知量；
[0077]
为绕y轴的旋转矩阵，[x
n
,y
n
]
t
为导航用户在导航坐标系下的位置，均为待求解量。
[0078]
本步骤对应附图3中的第一个方框的角测量测向模块。
[0079]
步骤三：位置解算
[0080]
首先根据步骤一中的式(12)和步骤二中的式(16)构建定位解算方程组：
[0081]
其中观测量为观测量为u,v,|r|，待求解量为ψ,x
n
,y
n
，其余均为已知量。
[0082]
之后利用牛顿迭代法求解定位方程组即可得到导航用户的位置和偏航角。至此完成对导航用户位置的导航解算。
[0083]
本步骤对应附图3中的第3个方框。
[0084]
通过以上步骤，可以获得基于无线电测距以及人造光源角测量的导航用户定位结果。从而可以满足导航用户在完全自主的情况下，仅利用无线电测距信息以及人造光源角测量测向信息，结合光接收机相关硬件信息，完成定位的需求。