CN110501736B - 利用视觉影像和gnss测距信号紧耦合定位系统与方法 - Google Patents

Abstract

本发明属于导航定位技术领域，公开了一种利用视觉影像和GNSS测距信号紧耦合定位系统与方法，利用相机拍摄3个及以上世界坐标已知的特征点来计算相机至特征点的距离；利用计算得到的近似距离与GNSS测距观测值进行紧耦合联合定位，求解用户坐标。本发明能够在复杂地理和电磁环境下提升GNSS定位的可用性，可靠性和抗干扰性；本发明能够利用GNSS测距信号反过来提升相机定位的精度；本发明将影像特征点视作“视觉基站”，无需基站布设成本，适用于手机等便携设备，用户终端成本低，可用于解决城市峡谷等复杂环境定位问题；本发明所述的方法能够在GNSS无法单独定位的条件下提供定位服务，从而提升定位服务的可用性。

Description

利用视觉影像和GNSS测距信号紧耦合定位系统与方法

技术领域

本发明属于导航定位技术领域，尤其涉及一种利用视觉影像和GNSS测距信号紧耦合定位系统与方法。

背景技术

目前，最接近的现有技术：

卫星导航技术能够在大部分户外情况下提供定位结果，但是受到遮挡和电磁干扰等情况下，仍旧无法获得可靠的定位结果。另一方面，随着电子站，导航战等电子对抗形式的出现，仅依赖卫星导航定位技术获得位置的可靠性逐渐降低，近年来开展GNSS干扰和GNSS欺骗的事件时有发生。虽然目前GPS,Beidou,GLONASS,Galileo等四大全球卫星导航系统已经投入运营或正在建设，在人类活动密集的城市区域受到建筑物的遮挡等，定位的可用性和可靠性都不高。另一方面，利用视觉影像进行定位的方法能够实现短距离，小范围的定位，典型的应用如视觉里程计，同步定位与地图构建(SLAM),光流技术等。视觉定位的定位精度主要受被摄物体距离相机远近的影响。如果被摄物体距离相机较远，则受分辨率和相机姿态确定精度等影响，相机的定位精度会显著下降，因此视觉定位主要应用与机器人，无人机导航，无人车避障等近距离场景的应用。目前也有一些视觉与GNSS融合的方案，主要是通过松耦合的方式，即GNSS和视觉分别提供定位结果，再利用互补滤波，联邦滤波等数据融合技术，对两种技术手段提供的定位结果进行融合。松耦合技术能够一定程度上提升定位性能，但是一旦其中一种技术失效，则无法继续利用松耦合进行数据融合。

综上所述，现有技术存在的问题是：

(1)卫星导航技术受到遮挡和电磁干扰等情况，无法获得可靠的定位结果；在人类活动密集的城市区域受到建筑物的遮挡等，定位的可用性和可靠性都不高。

(2)仅依赖卫星导航定位技术获得位置的可靠性逐渐降低，近年来开展GNSS干扰和GNSS欺骗的事件时有发生。

(3)目前视觉与GNSS融合通常是松耦合方案，一旦GPS或者相机无法单独定位，则无法继续利用松耦合进行数据融合，只能依赖有效工作的定位技术单独工作。

解决上述技术问题的难度：

卫星导航定位技术在复杂环境下的可用性和可靠性一直是其最大的技术弱点。为了弥补这一弱点提出了很多方法，主要是通过布设更多的导航信号源的方式来解决，而布设和维护这些导航信号源都需要很高的成本，目前还没有价格低廉，便于实现的方式解决复杂环境下定位服务可用性问题和抗干扰问题。

解决上述技术问题的意义：

目前卫星导航定位技术最大的瓶颈就是其可用性，脆弱性和可靠性问题。在开阔环境，卫星导航技术可以提供准确可靠的定位，但是在复杂环境下，卫星定位技术不佳，而与人类活动最频繁的城市等区域本来就属于复杂区域，因此单独卫星导航技术还不能满足准确可靠的定位需求。只能通过惯性传感器，或者布设其他导航信号源来提升导航性能，这些方法硬件成本和布设代价都比较高。本发明所述的GNSS与视觉紧耦合的定位方式不但能在复杂环境下提升定位服务的可用性和准确性，还无需额外布设信号源，摄像头传感器价格低廉，从效果和价格上都具有一定技术优势。

发明内容

针对GNSS被遮挡或者干扰导致定位性能和可用性下降的问题，本发明提供了一种利用视觉影像和GNSS测距信号紧耦合定位系统与方法。

本发明是这样实现的，一种利用视觉影像和GNSS测距信号紧耦合定位的方法，包括以下步骤：

步骤一，利用相机拍摄至少一张含有3个及以上世界坐标已知的特征点相片，对相片中的特征点进行识别，获得特征点在像平面坐标中的坐标(x,y)。

步骤二，利用求解的相机近似坐标和相片目标点的在世界坐标系下的已知坐标，计算相机至目标点的近似距离。

步骤三，利用步骤二计算得到的近似距离与GNSS测距观测值进行紧耦合联合定位，求解用户坐标。

进一步，步骤一中，定位的效果不受图像特征点世界坐标系坐标获取方式和图像特征点识别方式影响；图像特征点的世界坐标可以通过GNSS定位，全站仪定位，甚至三维城市模型提取；图像特征点识别的方式包括但不限于高斯拉普拉斯算子检测的方法(LOG)、Hessian矩阵行列式法(DOH)、尺度不变特征变换算法(SIFT)、加速鲁棒特征法(SURF)、Harris角点检测法、FAST角点特征提取法。

进一步，步骤二中，所述利用求解的相机近似坐标和相片目标点的准确坐标计算相机至目标点的距离，如下式所示：

式中ρ_i为相机到第i个视觉基站的距离。

进一步，所述联合相机计算得到的距离观测值和GNSS观测信号求解用户坐标的求解过程包括：

P_G＝ρ+δ_orb+c(δt^S-δt^R)+I_i+δ_trop+∈_Pi

P_C＝ρ+∈_C (2)

式中，P_G和P_C分别是GNSS测量的伪距和相机测量的距离，ρ是卫星或者视觉基站和接收机之间的几何距离(以米为单位)，δ_orb是GNSS卫星轨道误差,c是真空中的光速，δt^S和δt^R分别是卫星和接收机的时钟误差I_i和δ_trop分别是第i个频率信号的电离层延迟和对流层延迟。

其中GNSS信号观测精度与物方距离有关，距离越远，测距精度越差；GNSS信号采用标准定位的方法，忽略轨道误差，卫星钟差通过广播星历修正，电离层，对流层误差均利用经验模型修正，用户坐标和接收机钟差利用参数估计。

相机距离观测值仅和用户坐标有关，无需估计钟差参数，将几何距离ρ按泰勒级数展开，可得：

式中ρ₀是用户接收机到卫星或视觉基站的近似几何距离，ε是非线性误差；[x_i,y_i,z_i]是第i个卫星或视觉基站的坐标，[dx,dy,dz]是用户接收机坐标的增量；忽略掉上式的非线性误差项，上式表达为：

利用泰勒级数线性化后，GNSS观测信号和相机观测数据紧耦合的联合定位问题可近似地转化为一个线性问题，该线性化系统可表示为：

E(y)≈Ax (5)

式中设计矩阵A可表示为：

观测值向量可表示为：

式中前m个观测值为GNSS距离观测值，后n个观测值为相机距离观测值，用户坐标近似值的改正数最小二乘解可表示为：

利用坐标近似解修正用户的初始坐标，并根据改正数大小判断是否需要继续迭代计算。

进一步，所述GNSS测距信号不限于使用伪距观测值，也可以使用载波相位观测值；GNSS与视觉影像紧耦合的定位模型不限于标准的单点定位算法，也可使用实时伪距差分算法(RTD)，实时载波相位差分算法(RTK)，精密单点定位算法(PPP)；定位可以是实时模式，也可以是后处理模式。

本发明的另一目的在于提供一种所述利用视觉影像和GNSS测距信号紧耦合定位的方法在动态用户上的应用，所述应用包括：

在相机和GNSS接收机之间添加时间同步装置，用于相机拍照时刻和GNSS信号跟踪时刻的时间同步，获得正确的用户位置。

本发明的另一目的在于提供一种应用所述利用视觉影像和GNSS测距信号紧耦合定位的方法的利用视觉影像和GNSS测距信号紧耦合定位的系统，所述利用视觉影像和GNSS测距信号紧耦合定位的系统包括：

用户终端，可同时接收导航卫星的测距信号，并通过相机获取影像数据；

导航卫星，通过导航卫星的测距信号和通过相片量测的测距信号紧耦合进行用户终端自身定位；

视觉基站，指影像中容易辨认的特征点，并且其世界坐标已知。

本发明的另一目的在于提供一种计算机程序产品，所述计算机程序产品运行于计算机的处理器，运行时执行所述的利用视觉影像和GNSS测距信号紧耦合定位的方法。

本发明的另一目的在于提供一种终端，所述终端搭载实现所述利用视觉影像和GNSS测距信号紧耦合定位的方法的处理器。

本发明的另一目的在于提供一种计算机可读存储介质，包括指令，当其在计算机上运行时，使得计算机执行所述的利用视觉影像和GNSS测距信号紧耦合定位的方法。

综上所述，本发明的优点及积极效果为：

(1)视觉影像和GNSS测距信号紧耦合定位的方法能够提升定位服务在复杂电磁和地理环境下定位服务的可用性和可靠性。在GNSS无法单独定位的条件下，紧耦合定位仍能提供可靠的定位服务。

(2)“视觉基站”采用环境中的特征点，无需布设，无需维护，不消耗电力等资源，仅需事先获得其三维坐标即可提供定位服务。定位基础设施建设和维护成本低廉。

(3)“视觉基站”不受电磁干扰的影响，可以是人造或者自然地物特征点，随处可获得。视觉影像和GNSS测距信号紧耦合定位可提升定位服务的抗干扰能力。

(4)视觉影像和GNSS测距信号紧耦合定位的用户终端成本低廉，便于推广和应用。紧耦合定位的用户终端仅需要具备GNSS接收和拍照功能，两类传感器成本都不高，且常见移动设备，如智能手机，平板电脑等均可满足需求。

(5)视觉影像和GNSS测距信号紧耦合定位在物方距离较远时可有效提升基于单片视觉定位的精度。

(6)视觉影像和GNSS测距信号紧耦合定位方法可适用与静态和动态定位。仅需GNSS与相机之间保持时间同步，即可实现动态用户的紧耦合定位，解决于动态用户的定位问题。

本发明通过从视觉影像提取相机到特征点距离观测值的方式与GNSS测距信号实现紧耦合定位，不是通过分别计算的坐标或者坐标增量的形式进行融合定位。本发明所述的方法能够在GNSS无法单独定位的条件下提供定位服务，从而提升定位服务的可用性。

附图说明

图1是本发明实施例提供的利用视觉影像和GNSS测距信号紧耦合定位的方法流程图。

图2是本发明实施例提供的利用视觉影像和GNSS测距信号紧耦合定位的系统原理图。

图3是本发明实施例提供的通过相片测量获得相机至目标点距离的原理图；

图中：M为世界坐标系原点，S为相机的焦点位置，A为视觉基站，a为视觉基站在相机坐标系下的坐标。

图4是本发明实施例提供的利用相片测量获得相机至目标点距离的计算流程图。

图5是本发明实施例提供的仿真计算得到的定位效果对比图。

图中：(a)为不同物方距离条件下直接通过相机定位的精度；(b)为相同条件下，引入3个GPS测距观测值和相机测距观测值紧耦合定位的精度。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

卫星导航技术受到遮挡和电磁干扰等情况，无法获得可靠的定位结果；在人类活动密集的城市区域受到建筑物的遮挡等，定位的可用性和可靠性都不高。

仅依赖卫星导航定位技术获得位置的可靠性逐渐降低，近年来开展GNSS干扰和GNSS欺骗的事件时有发生。视觉与GNSS融合的方案，一旦其中一种技术失效，则无法继续利用松耦合进行数据融合。

针对现有技术存在的问题，本发明提供了一种利用视觉影像和GNSS测距信号紧耦合定位系统与方法，下面结合附图对本发明作详细的描述。

如图1所示，本发明实施例提供的利用视觉影像和GNSS测距信号紧耦合定位的方法，包括以下步骤：

S101：利用相机拍摄至少一张含有3个及以上世界坐标已知的特征点相片，对相片中的特征点进行识别，获得特征点在像平面坐标中的坐标(x,y)。

S102：利用求解的相机近似坐标和相片目标点的准确坐标，计算相机至目标点的近似距离。

S103：利用步骤二计算得到的近似距离与GNSS测距观测值进行紧耦合联合定位，求解用户坐标。

如图2所示，本发明实施例提供的利用视觉影像和GNSS测距信号紧耦合定位的系统，系统包括：用户终端、导航卫星、视觉基站。

用户终端，可同时接收导航卫星的测距信号，并通过相机获取影像数据。

导航卫星，通过导航卫星的测距信号和通过相片量测的测距信号紧耦合进行用户终端自身定位。

视觉基站，指影像中容易辨认的特征点，并且其世界坐标已知。

下面结合具体实施例对本发明作进一步描述。

实施例

根据影像覆盖范围内一定数量的分布合理的“视觉基站”(已知其像点和地面点的坐标)，利用共线条件方程求解像片外方位元素Xs,Ys,Zs,ψ,ω,κ。在求解外方位元素前，需要利用格网标志点对相机的内方位元素进行精确标定以消除镜头畸变带来的影响。在后续介绍中，假设相机已经事先完成了内参数的标定并对镜头参数进行了合理的校准。

第一步是对相片中的特征点进行识别，相片中特征点识别的方法可采用比较成熟的算法，例如高斯拉普拉斯算子检测的方法(LOG),Hessian矩阵行列式法(DOH),尺度不变特征变换算法(SIFT)，加速鲁棒特征法(SURF),Harris角点检测法，FAST角点特征提取法等。相片识别后，即可获得特征点在像平面坐标中的坐标(x,y)。

如图3所示，物点A和摄影中心S在地面摄影测量坐标系中的坐标依次是(X,Y,Z)、(Xs,Ys,Zs)；像点a在像空间坐标系中的坐标是(x,y,-f)。那么由共线条件方程知：

其中ai，bi，ci是只含三个独立参数ψ，ω，κ的九个方向余弦。

在方程中共有六个未知参数Xs,Ys,Zs,ψ,ω,κ，所以有三个不在一条直线上的已知地面点坐标就可以求出像片的这六个外方位元素。由于共线条件方程是非线性方程，为了便于迭代计算，需要把方程用泰勒级数展开，取一次项得到线型表达式，有

用新的符号表示各偏导数后为

x＝x₀+a₁₁dX_s+a₁₂dY_s+a₁₃dZ_s+a₁₄dφ+a₁₅dω+a₁₆dκ

y＝y₀+a₂₁dX_s+a₂₂dY_s+a₂₃dZ_s+a₂₄dφ+a₂₅dω+a₂₆dκ (12)

其中x0、y0是函数近似值，dω,dκ,dX_s,dY_s,dZ_s是外方位元素近似值的改正数，它们的系数为函数的偏导数。为了便于推导，令

那么有

对于系数，其严密算法(以为例)如下：

对于竖直摄影而言，像片的角方位元素都是小值，因而各系数的近似值为

为了提高精度和可靠性，通常需要测量四个或更多的地面控制点和对应的像点坐标，采用最小二乘平差方法解算。此时像点坐标(x,y)作为观测值，加入相应的偶然误差改正数v_x,v_y，可列出每个点的误差方程式

v_x＝a₁₁dX_s+a₁₂dY_s+a₁₃dZ_s+a₁₄dφ+a₁₅dω+a₁₆dκ-l_x

v_y＝a₂₁dX_s+a₂₂dY_s+a₂₃dZ_s+a₂₄dφ+a₂₅dω+a₂₆dκ-l_y (18)

那么列出误差方程为：

V＝Ax-l (19)

式中

根据最小二乘法求解外方位元素改正数：

dx＝(A^TA)^-1(A^Tl) (21)

计算外方位元素改正数后，利用其修正外方位元素的初值。判断外方位元素改正数的大小是否满足结束迭代的条件，如果满足，则结束迭代，否则，开始下一轮的迭代计算。

外方位元素的求解精度与物方距离远近有关。其中相机坐标的精度与物方距离成反比。物方距离越远，相机坐标求解越不准确。此时可利用求解的相机近似坐标和相片目标点的准确坐标计算相机至目标点的距离，如下式所示：

式中ρ_i为相机到第i个视觉基站的距离。这样就利用相片测量获得了相机到若干个目标点的距离观测值。

在可见卫星数不足的条件下，单独使用GNSS无法实现定位，此时可联合相机计算得到的距离观测值和GNSS观测信号求解用户坐标。求解过程如下：

P_G＝ρ+δ_orb+c(δt^S-δt^R)+I_i+δ_trop+∈_Pi

P_C＝ρ+∈_C (23)

式中，P_G和P_C分别是GNSS测量的伪距和相机测量的距离。ρ是卫星或者视觉基站和接收机之间的几何距离(以米为单位)，δ_orb是GNSS卫星轨道误差,c是真空中的光速，δt^S和δt^R分别是卫星和接收机的时钟误差I_i和δ_trop分别是第i个频率信号的电离层延迟和对流层延迟。

其中GNSS信号受到多种误差源的影响，但其测距精度与用户位置基本无关。相机通过视觉测距的方法得到的距离观测值基本不受其他误差的影响，但其观测精度与物方距离有关，距离越远，测距精度越差。

对GNSS信号而言，采用标准定位的方法，即忽略轨道误差，卫星钟差通过广播星历修正，电离层，对流层误差均利用经验模型修正，用户坐标和接收机钟差利用参数估计。通过这种方法，正常情况下可以获得米级的GNSS定位精度。

相机距离观测值仅和用户坐标有关，无需估计钟差参数，将几何距离ρ按泰勒级数展开，可得：

式中ρ₀是用户接收机到卫星或视觉基站的近似几何距离，ε是非线性误差；[x_i,y_i,z_i]是第i个卫星或视觉基站的坐标，[dx,dy,dz]是用户接收机坐标的增量；忽略掉上式的非线性误差项，上式表达为：

利用泰勒级数线性化后，GNSS观测信号和相机观测数据紧耦合的联合定位问题可近似地转化为一个线性问题，该线性化系统可表示为：

E(y)≈Ax (26)

式中设计矩阵A可表示为：

观测值向量可表示为：

式中前m个观测值为GNSS距离观测值，后n个观测值为相机距离观测值，用户坐标近似值的改正数最小二乘解可表示为：

利用坐标近似解修正用户的初始坐标，并根据改正数大小判断是否需要继续迭代计算。

由于相机测量的距离中不包含时间信息，因此对于静态用户而言，在仅耦合定位时，可以不需要保证拍照时刻与GNSS信号跟踪时刻严格同步。但是对于动态用户而言，由于用户位置随时间变化，此时需要在相机和GNSS接收机之间添加时间同步装置，保障相机拍照时刻和GNSS信号跟踪时刻的时间同步，才能获得正确的用户位置。

下面结合具体仿真对本发明作进一步描述。

在本发明实施例中，图4是利用相片测量获得相机至目标点距离的计算流程图。

图5是仿真计算得到的定位效果对比图。图中：(a)为不同物方距离条件下直接通过相机定位的精度；(b)为相同条件下，引入3个GPS测距观测值和相机测距观测值紧耦合定位的精度。图a显示定焦相机的定位误差随着目标点距离变远而导致定位误差显著增加。在图b中在只有3个GPS观测值条件下，单独使用GPS是无法提供定位结果的。因而也无法与相机定位结果进行松耦合定位。而本发明所述的紧耦合定位方式，通过将相机影响数据和GNSS信号进行紧耦合，不但可以提供定位结果，还提升了定位精度。因此本发明所述的GNSS与影像数据紧耦合的定位方法能够在复杂场景下提供定位。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种利用视觉影像和GNSS测距信号紧耦合定位的方法，其特征在于，所述利用视觉影像和GNSS测距信号紧耦合定位的方法包括以下步骤：

步骤一，对至少3个世界坐标已知特征点进行识别，获得所述特征点在像平面坐标中的坐标(x,y)；

步骤二，利用求解的相机近似坐标和相片目标点在世界坐标系下的已知坐标，计算相机至目标点的近似距离，所述目标点指影像中容易辨认的特征点，并且其世界坐标已知，也称为视觉基站；

步骤三，利用步骤二计算得到的近似距离与GNSS测距观测值进行紧耦合联合定位，求解用户坐标；

所述步骤三中求解用户坐标的求解过程包括：

P_G＝ρ+δ_orb+c(δt^S-δt^R)+I_i+δ_trop+∈_Pi

P_C＝ρ+∈_C；

式中，P_G和P_C分别是GNSS测量的伪距和相机测量的距离，ρ是卫星或者视觉基站和接收机之间的几何距离，δ_orb是GNSS卫星轨道误差,c是真空中的光速，δt^S和δt^R分别是卫星和接收机的时钟误差，I_i和δ_trop分别是第i个频率信号的电离层延迟和对流层延迟；

相机至目标点的近似距离和用户坐标有关，不估计钟差参数，将几何距离ρ按泰勒级数展开，得：

式中ρ₀是用户接收机到卫星或视觉基站的近似几何距离，ε是非线性误差；[x_i,y_i,z_i]是第i个卫星或视觉基站的坐标，[dx,dy,dz]是用户接收机坐标的增量；忽略掉非线性误差项，表达为：

利用泰勒级数线性化后，GNSS测距观测值和相机至目标点的近似距离紧耦合的联合定位问题可近似地转化为一个线性问题，该线性化系统表示为：

E(y)≈Ax；

式中设计矩阵A表示为：

观测值向量表示为：

式中前m个观测值为GNSS测距观测值，后n个观测值为相机至目标点的近似距离，用户坐标近似值的改正数最小二乘解表示为：

利用坐标近似解修正用户的初始坐标，并根据改正数大小判断是否继续迭代计算。

2.如权利要求1所述的利用视觉影像和GNSS测距信号紧耦合定位的方法，其特征在于，步骤一中，采用相机与GNSS接收机的组合进行图像特征点的世界坐标定位，图像特征点的世界坐标定位中采用GNSS定位、全站仪定位或三维城市模型提取方式；

步骤一中，对世界坐标已知特征点进行识别中，采用高斯拉普拉斯算子检测的方法、Hessian矩阵行列式法、尺度不变特征变换算法、加速鲁棒特征法、Harris角点检测法或FAST角点特征提取法进行识别。

3.如权利要求1所述的利用视觉影像和GNSS测距信号紧耦合定位的方法，其特征在于，所述步骤二利用求解的相机近似坐标和相片目标点的已知坐标计算相机至目标点的距离，如下式所示：

式中ρ_i为相机到第i个视觉基站的距离。

4.一种如权利要求1～3任意一项所述利用视觉影像和GNSS测距信号紧耦合定位的方法在静态用户上的应用，其特征在于，所述应用包括：拍照时刻与GNSS信号跟踪时刻在紧耦合定位时，采用同步方式或不同步方式。

5.一种如权利要求1～3任意一项所述利用视觉影像和GNSS测距信号紧耦合定位的方法在动态用户上的应用，其特征在于，所述应用包括：

在相机和GNSS接收机之间添加时间同步装置，用于相机拍照时刻和GNSS信号跟踪时刻的时间同步，获得正确的用户位置。

6.一种应用如权利要求1所述利用视觉影像和GNSS测距信号紧耦合定位的方法的利用视觉影像和GNSS测距信号紧耦合定位的系统，其特征在于，所述利用视觉影像和GNSS测距信号紧耦合定位的系统包括：

导航卫星；用户终端，可同时接收导航卫星的测距信号，并通过相机获取影像数据；通过导航卫星量测的所述GNSS测距观测值和通过相机量测的所述近似距离紧耦合进行用户终端自身定位

视觉基站，指影像中容易辨认的特征点，并且其世界坐标已知。

7.一种终端，其特征在于，所述终端搭载实现权利要求1～3任意一项所述利用视觉影像和GNSS测距信号紧耦合定位的方法的处理器。

8.一种计算机可读存储介质，包括指令，当其在计算机上运行时，使得计算机执行如权利要求1-3任意一项所述的利用视觉影像和GNSS测距信号紧耦合定位的方法。