技术领域
[0001] 本
发明涉及组合导航领域，尤其涉及一种环境信标支持下的GNSS/SINS/视觉紧组合方法。
背景技术
[0002] 经过几十年的研究发展，GNSS(Global Navigation Satellite System,GNSS)/SINS(Strap-down inertial navigation system,SINS)组合导航的基本理论与方法已较为成熟，在开阔观测环境下，可达到稳定的厘米级
定位。加拿大NovAtel公司提供的
硬件、
软件一体化的GNSS/SINS解决方案由于其优越的性能被广泛采用。近年来，自动驾驶这一高新产业也发展的如火如荼，而精准定位作为自动驾驶的
基础模
块，也引起了业界的重视。Tesla、通用、proPILOT等自动驾驶企业中，也都采用了以GNSS/惯性为核心的组合导航定位系统。但在复杂环境下，尤其是高楼林立，高架隧道密布的典型城市环境下，由于GNSS观测
质量受到
信号遮挡、干扰的影响，质量急剧降低，导致GNSS/SINS组合系统的性能显著下降。
随着美国GPS、俄罗斯GLONASS、欧盟Galileo以及中国北斗的建设发展，GNSS正朝多频多系统的方向发展，多频多模的GNSS/SINS组合在一定程度上提高了复杂环境下的模糊度固定率；惯导短时间内高
精度的
位姿递推功能也在GNSS数据预处理方向得到了有效运用；学界也提出了许多不同类型的抗差自适应滤波方法以试图解决状态信息和观测信息质量差的问题。上述方法在GNSS偶尔发生短时间(30s)中断时，能够起到很好的修复效果，但在典型的城市环境中，GNSS一旦发生较长时间的中断，SINS的机械编排误差会呈指数次方发散，该问题依然是GNSS/SINS组合的一大难题，也是在车载实时定位中的一大技术
瓶颈。
[0003] 视觉
传感器通过
感知、识别、
跟踪周围环境的特征信息来实现自身载体位姿的估计。视觉传感器硬件技术成熟、体积小、价格便宜、普遍集成于智能终端，并且具有语义分割、场景理解、大范围建图等
计算机视觉能
力，具有广阔的应用前景和研究价值。以Tesla为代表的AutoPilot自动驾驶系统采用的就是以视觉为主导的多传感器融合方案。根据是否存在先验环境地图信息的辅助，视觉定位可分为相对定位和绝对定位两种方式，相对定位目前研究已较为成熟，其代表
算法有基于纯视觉的ORB-SLAM2、LSD-SLAM、DSO、SVO等和基于视觉/惯导融合的MSCKF、VINS-Mono、OKVIS等，目前在移动测图设备、无人机、扫地
机器人等设备上已有不少成熟应用，其定位精度约为1％。由于相对定位在无闭环修正情况下，位姿误差会逐渐累积，而实际载体运行环境又往往无闭环。所以高德、四维图新、TomTom、Here等图商，都在积极布局高精度地图，以实现基于先验地图支持下的视觉匹配绝对定位。目前高精度地图还没有统一的标准，所以还没有成熟的落地产品。
[0004] GNSS、SINS、视觉传感器具有很强的优势互补性，GNSS建立起了全球范围唯一统一的
时空基准，可提供全球、全天候的高精度绝对定位系统；SINS独立自主、不受环境影像，
采样率高，且稳定连续，可作为多源融合的主
滤波器；视觉传感器即可进行航位推算，也可在有先验地图支持下，实现绝对定位，其性能介于GNSS/SINS之间，是GNSS/SINS组合系统的有效补充。由此可见，GNSS/SINS/视觉传感器是未来多源融合的一个重要方向。
[0005] 针对上述问题，本发明给出了一种环境信标支持下的GNSS/SINS/视觉紧组合方法，整个定位系统以差分GNSS/SINS紧组合为基础，在特征点
云地图辅助下，对采集到的影像进行动态干扰去除、特征提取和环境信标匹配，并利用匹配的
像素与信标点对的共线关系构建观测约束方程参与紧组合解算，以实现连续、高精度的定位。
发明内容
[0006] 本发明要解决的技术问题在于针对
现有技术中的
缺陷，提供一种环境信标支持下的GNSS/SINS/视觉紧组合方法。
[0007] 本发明解决其技术问题所采用的技术方案是：
[0008] 本发明提供一种环境信标支持下的GNSS/SINS/视觉紧组合方法，该方法中，通过视觉传感器采集图像，离线生成包含特征描述子的视觉特征点云地图作为先验环境信标
数据库；在载体进行实时解算导航定位时，以差分GNSS/SINS紧组合为基础，当GNSS信号受到遮挡时，通过载体上安装的视觉传感器拍摄影像，对影像进行动态干扰去除和特征提取，并将提取到的特征与视觉特征点云地图中的环境信标进行特征匹配；当匹配到大于等于1个路标点时，将匹配到的像素点与信标点组成的后方交会方程作为观测信息，加入到紧组合中为SINS提供辅助定位。
[0009] 进一步地，本发明的该方法具体包括以下步骤：
[0010] 步骤1、对实时采集的影像预处理得到矫正畸变后的影像，利用
深度学习构建
卷积神经网络进行语义分割，将动态物体对应的像素块进行标记；
[0011] 步骤2、对影像进行网格划分，对各个网格分别利用基于GPU
加速的SIFT特征算子提取特征点，判断特征点是否落入动态物体标记区域，若是则将该特征点删除，并将剩余特征点按响应强度进行排序筛选，取出每个网格的响应强度前二的特征点；
[0012] 步骤3、将特征点云地图利用特征十叉数辅助存储，并加速特征匹配，采用特征描述子的欧氏几何距离法得到初步匹配，当匹配到的环境信标点多于4点时，利用RANSAC算法和EPnP算法进行外点干扰排除；
[0013] 步骤4、对匹配到的像素点和信标点构建后方交会方程，根据惯导与相机的外参关系，将方程转换为惯导位姿误差与像素观测值之间的关系，并将其作为观测方程加入到GNSS/INS紧组合中辅助定位；
[0014] 步骤5、在卫星观测数量大于一定
阈值时，屏蔽
视觉反馈的观测值信息，直接采用GNSS/SINS紧组合定位；反之，若卫星观测数量小于阈值，对其进行筛选后采用GNSS/SINS/视觉共同参与解算；若无可见卫星或筛选后无剩余卫星，则仅采用视觉观测和SINS进行紧组合定位。
[0015] 进一步地，本发明的步骤1的具体方法为：
[0016] 在影像采集之前，采用张正友相机标定法，利用MATLAB的标定工具箱对相机的内参进行标定获得相机的焦距、像主点偏移以及径向、切向畸变参数；在获得实时采集的影像后，利用标定的内参和畸变参数进行畸变校正；
[0017] 校正后的影像首先利用CNN卷积得到最后一个卷积图层的特征映射，采用金字塔
池化模块得到4个不同尺度的特征提取效果，并对每层金字塔均进行1*1卷积，以降低维度和维持全局特征权重，利用双线性插值进行上采样以形成包含局部和全局信息的特征表征，最后将表征反馈回卷积层以实现像素级的语义分割；得到图像的语义分割结果后，将属于动态物体的像素块进行标记。
[0018] 进一步地，本发明的步骤2中特征点提取的具体方法为：
[0019] 步骤2.1、SIFT特征提取：采用SIFT算子进行特征点提取，并采用GPU对SIFT特征提取进行加速；
[0020] 步骤2.2、动态区域特征点剔除：将SIFT提取到的特征点依次进行标记判断，若特征点落在动态标记区域，则将该特征点删去；
[0021] 步骤2.3、特征点选取：将清理后的特征点按照特征响应程度进行从大到小的排序，取响应程度最大和次大的两个特征点作为最后的候选特征点；若网格中的特征点少于两个，则将其全部选为最后的候选特征点；每个网格的候选特征点都筛选完毕后，对各个网格的特征点进行汇总。
[0022] 进一步地，本发明的步骤3进行特征匹配的方法具体为：
[0023] 步骤3.1、构建十叉树特征索引：对视觉特征点云地图的所有数据构建十叉树特征索引，建立L层的十叉树，最多搜索L次找到叶子
节点；
[0024] 步骤3.2、最邻近信标点搜索：将提取到的特征点的特征描述子先与十叉树特征索引的第一层所有节点的
特征向量进行比较，选择欧氏距离最短的节点继续往下搜索，直到第L层；L层的
子节点的所有特征描述子进行欧式距离计算，找到欧氏距离最短dmin和次短dsmin的叶子节点作为候选节点，若此时的最短欧式距离小于设定的最小距离阈值且经过ratio检验后， 则将该特征点和信标点加入到候选匹配点对中；
[0025] 步骤3.3、RANSAC算法和EPnP算法外点剔除：若候选匹配点对不小于8对，采用RANSAC+EPnP算法剔除外点，随机选取四组匹配点对，利用EPnP计算相机位姿，根据相机位姿计算反投影误差，将反投影误差小于2个像素的匹配点对标记为内点，并记录该内点子集；对上述操作循环K次，取出K次循环内点子集中数量最多的为最优子集，并将最优子集对应的相机位姿作为最终估算的相机位姿，若估算的相机
位置和惯导预报位置的几何距离差小于10m，则认为RANSAC+EPnP算法成功，将最优子集外的匹配点对判断为外点，并将其剔除。
[0026] 进一步地，本发明的步骤4的具体方法为：
[0027] 步骤4.1、后方交会方程的构建：已知环境信标点的位置以及匹配的像素点的像素坐标，求得相机
光心的位置和
姿态；将式相机位置和姿态表示成误差形式，通过链式法则求观测值对状态的偏导，得到对于每对匹配点可构造后方交会观测方程为：
[0028]
[0029] 其中，[u v]为像素坐标，[x y z]为相机
坐标系下的信标点坐标，P为信标点在ECEF系下的坐标，待估状态为 Pc是相机光心在ECEF系下的坐标， 是ECEF系到相机坐标系的旋转矩阵，失准
角为 为相机姿态、位置的观测值，δPc为位置误差，ωu,v为像素观测值的噪声；
[0030] 步骤4.2、视觉/惯导融合观测方程：根据惯导与相机的外参关系，将像素观测值与相机位姿误差的关系转换为像素观测值与惯导位姿误差的关系，得到观测方程为：
[0031]
[0032] 其中，u1、un为像素观测值， 为信标点反投影得到的像素值，εu为像素误差，δre、δve、φ、ab、εb、δN分别表示惯导的位置、速度、姿态误差、
加速度计零偏、陀螺零偏以及双差模糊度参数， 表示像素观测值对相机位姿误差的偏导数。
[0033] 进一步地，本发明的步骤5的具体方法为：
[0034] 当GNSS卫星数大于阈值，且PDOP值小于阈值时，说明GNSS定位几何构型满足要求，此时，屏蔽视觉观测信息，只采用GNSS/SINS紧组合，其观测更新方程：
[0035]
[0036] 其中， 为利用SINS递推的位置计算的战星几何双差距离， 和分别表示利用基准站和流动站接收机输出的伪距和载波
相位计算的站星双差观
测值， 为ECEF系下的GNSS杆臂改正量，为各个卫星的双差方向余弦，εP和εL为伪距、相位的观测噪声；
[0037] 当GNSS卫星数小于阈值，且视觉传感器成功匹配到若干信标点时，则加入视觉像素约束观测值，得到观测方程：
[0038]
[0039] 当GNSS卫星完全被遮挡，但视觉可成功提取匹配到信标点时，只采用视觉的像素约束观测值，其观测方程采用步骤4.2得到的视觉/惯导融合观测方程。
[0040] 本发明产生的有益效果是：本发明的环境信标支持下的GNSS/SINS/视觉紧组合方法，1)本发明只增加了视觉传感器，该传感器大多数车辆载体已具备，且该传感器成本较低，所以该方法具备一定的可落地性。2)本发明充分利用GNSS、SINS、视觉三者定位手段的互补特性，GNSS受遮挡的环境往往视觉特征较为丰富，视觉后方交会绝对定位可极大抑制惯导在各方向上的误差发散。3)本发明只需匹配一个路标点，就可对惯导的位置误差漂移起到抑制，且可在GNSS长时间失
锁情况下，持续稳定地提供分米乃至厘米级的定位结果。4)根据载体行驶的环境的不同特性，可动态选择是否加入视觉信息的辅助约束，定位手段较为灵活。
附图说明
[0041] 下面将结合附图及
实施例对本发明作进一步说明，附图中：
[0042] 图1为本发明实例的GNSS/SINS/视觉紧组合定位算法总
流程图；
[0043] 图2为本发明实例的ECEF系下的SINS机械编排流程图；
[0044] 图3为本发明实例的视觉约束观测方程获取流程图；
[0045] 图4为本发明实例的影像去动态干扰示意图；
[0046] 图5为本发明实例的影像特征提取流程图；
[0047] 图6为本发明实例的影像特征匹配流程图。
具体实施方式
[0048] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
[0049] 由于GNSS/SINS组合系统为非线性，所以本发明的整体滤波
框架采用
扩展卡尔曼滤波，其总流程图如图1所示，导航坐标系为ECEF系，相应的机械编排也在ECEF系下进行，如图2所示。本发明采用站间星间双差分，消去接收机钟差，所以GNSS/SINS紧组合模型中和GNSS相关的误差状态双差模糊度，除此之外紧组合误差状态还包括导航状态(位置、速度和姿态误差)以及IMU相关的误差状态(陀螺零偏和加速度计零偏)。滤波采用闭环修正技术，对SINS系统误差进行反馈矫正。紧组合的滤波方程介绍如下：
[0050] 由于模糊度参数δN不随时间变化(假设没有周跳发生)，所以其对应的误差
状态方程为：
[0051]
[0052] 根据ECEF系下的惯导误差模型以及IMU传感器的误差模型，可得GNSS/SINS紧组合的连续误差状态方程为：
[0053]
[0054] 上式中， 和 分别表示惯导位置、速度和姿态误差向量，δbg和δba分别为陀螺和加速度计零偏误差向量。 为b系相对于e系的旋转矩阵， 为
陀螺仪仪表误差项、fb为加速度计的输出值，δfb为加速度计仪表误差项，δge为重力矢量误差， 为地球自转
角速度。 分别为对应的一阶高斯-
马尔科夫过程的相关时间，ωa和ωg分别表示陀螺和加速度计的驱动白噪声。GNSS/SINS紧组合的误差状态向量为：
[0055]
[0056] 为了得到离散时间系统误差状态方程，可将连续误差状态方程离散化，如下：
[0057] δxk+1＝Φk+1,kδxk+ωk (4)
[0058] 其中，Φk+1,k为状态转移矩阵，ωk为驱动白噪声。根据下式可进行一步状态预测：
[0059]
[0060] Xk,k-1为预报状态，Q为过程噪声，Δtk为两个历元间的时间间隔，Pk,k-1为预报方差。紧组合滤波更新的量测方程可表示如下：
[0061] δz＝HδX+η (6)
[0062] 式(6)中，δz为观测值残差，H为设计矩阵，η为观测噪声。当有GNSS观测值时，经过数据预处理后，作为滤波的观测信息进行观测更新：
[0063]
[0064] 式(7)中，Kk为增益系数，Rk为观测噪声，Xk和Pk为滤波状态及其方差。
[0065] 以上紧组合Kalman滤波模型是本发明算法的基础，下面将结合图1所示的技术路线，对本发明各模块的关键技术及其实施方法展开详细叙述。
[0066] 一、影像动态干扰去除
[0067] 本发明中所采用的视觉特征点云地图是只包含建筑、公共设施、交通标志等静止物体的环境信标数据库，其排除了行人、植被、车辆等不可靠信息。只有这样才能保证环境信标点不随时间、天气等原因而发生明显变化。所以在对影像进行处理时，也同样要保证提取的特征点不在动态物体上，如车辆、行人、随季节更替易发生变化的植被等。
[0068] 在影像采集之前，需要采用张正友相机标定法，利用MATLAB的标定工具箱对相机的内参进行标定获得相机的焦距fx、fy，像主点偏移cx、cy以及径向、切向畸变参数。
[0069] 如图4所示，在获得实时采集的影像后，首先需要利用标定的内参和畸变参数进行畸变校正。校正后的图像首先利用CNN卷积得到最后一个卷积图层的特征映射，采用金字塔池化模块得到4个不同尺度的特征提取效果，并对每层金字塔均进行1*1卷积，以降低维度和维持全局特征权重，利用双线性插值进行上采样以形成包含局部和全局信息的特征表征，最后将表征反馈回卷积层以实现像素级的语义分割。得到图像的语义分割结果后，将属于动态物体(如车辆、行人、植被等)的像素块进行标记，后续的环境特征提取和匹配将不再考虑这部分像素，以有效避免环境动态干扰。
[0070] 二、影像特征提取
[0071] 由于特征点的几何分布直接影响了影像后方交会的精度，几何分布越好，影像后方交会的精度越高。所以为了确保影像的特征点均匀分布，需要对校正后的影像网格划分后再对各个网格进行特征提取。
[0072] 整个特征提取的流程图如图5所示，首先将校正后的影像划分成M×N的网格，M，N的数值取决于影像本身的大小，每个网格的大小控制在10*10像素左右即可。划分好网格后，对每个网格分别进行如下并行操作：
[0073] 1)SIFT特征提取
[0074] 本发明采用SIFT算子进行特征点提取，因为SIFT(Scale Invariant Feature Transform)算子对旋转、尺度缩放、
亮度变化保持不变，对视角变化、放射变换和噪声也有一定的鲁棒性，在后续视觉定位中，能够实现较为可靠的特征匹配。为了加快处理速度，我们采用GPU对SIFT特征提取进行加速。
[0075] 2)动态区域特征点剔除
[0076] 步骤1中已经采用深度学习的方法对影像进行了语义分割，识别出了行人、车辆、植被等动态物体，并且进行了标记。将SIFT提取到的特征点依次进行标记判断，若特征点落在动态标记区域，则将该特征点删去。
[0077] 3)特征点选取
[0078] 将清理后的特征点按照特征响应程度进行从大到小的排序，取响应程度最大和次大的两个特征点作为最后的候选特征点。若网格中的特征点少于两个，则将其全部选为最后的候选特征点。每个网格的候选特征点都筛选完毕后，对各个网格的特征点进行汇总。
[0079] 三、影像特征匹配
[0080] 视觉特征点云地图通常包含几百万甚至上千万的特征点及其描述子，如果每次特征点匹配都需要与这上千万的特征点一一比较，是非常耗时的。本发明采用基于点云特征描述子进行聚类、划分的数据结构——词袋库辅助匹配，其本质是一个根据特征描述子聚类的十叉树。十叉树的每个节点都是一个根据K均值
聚类算法得到的128维的SIFT特征描述子。有了十叉树的辅助，可极大的加速匹配速度。整个特征匹配的关键步骤如下：
[0081] 1)构建十叉树特征索引
[0082] 对视觉特征点云地图的所有数据构建十叉树特征索引。首先第0层的节点，其特征向量就是视觉特征点云地图中所有点云特征向量的均值，这一层可以忽略，从第1层开始使用K均值聚类算法，根据特征向量的欧式距离大小分成10类，每一类的中心(特征均值)即为该节点的特征向量，10类中的每一类，再继续使用K均值聚类算法分成10类，直到不可再分割为止。总共建立L层的十叉树(忽略第0层)，最多搜索L次就可以找到叶子节点。
[0083] 2)最邻近信标点搜索
[0084] 将提取到的特征点的特征描述子先与十叉树特征索引的第一层所有节点的特征向量进行比较，选择欧氏距离最短的节点继续往下搜索，直到第L层。和L层的子节点的所有特征描述子进行欧式距离计算，找到欧氏距离最短dmin和次短dsmin的叶子节点作为候选节点，若此时的最短欧式距离小于设定的最小距离阈值且经过ratio检验后， 则将该特征点和信标点加入到候选匹配点对中。
[0085] 3)RANSAC+EPnP外点剔除
[0086] 根据n个三维点坐标及其对应的像素点坐标，在已知相机内参的情况下可求得相机的位姿，该求解问题一般称为PnP(Perspective-n-Point)问题，而EPnP是目前最有效的PnP解法。RANSAC算法则可用于在一组包含异常数据的样本数据集中得到有效样本数据。两者结合可以有效的识别除部分匹配错误的点对。
[0087] 由于RANSAC算法对样本数量由一定要求，若候选匹配点对小于8对，则跳过该操作，反之则采用RANSAC+EPnP算法剔除外点，具体操作如图6所示，随机选取四组匹配点对，利用EPnP计算相机位姿，根据相机位姿计算反投影误差，将反投影误差小于2个像素的匹配点对标记为内点，并记录该内点子集。对上述操作循环K次，取出K次循环内点子集中数量最多的为最优子集，并将最优子集对应的相机位姿作为最终估算的相机位姿，若估算的相机位置和惯导预报位置的几何距离差小于10m，则认为RANSAC+EPnP成功，将最优子集外的匹配点对判断为外点，并将其剔除。
[0088] 四、环境信标支持下的视觉/惯性融合定位
[0089] 由步骤1，2，3，已经得到了较为可靠的影像像素点和环境信标点的匹配点对，则根据后方交会的原理可反算出相机的位置和姿态。由于视觉共线方程的非线性程度较大，故其必须要较为准确的初始值才能够进行最小二乘解算。若没有任何外部信息辅助，至少需要4对有效匹配点，由EPnP提供初始位置。但有了惯导预报信息的辅助，则只需1对匹配点即可进行辅助定位，其具体流程如下：
[0090] 1)后方交会方程的构建
[0091] 已知环境信标点的位置以及匹配的像素点的像素坐标，求相机光心的位置和姿态，其基本方程如下：
[0092]
[0093] 其中，[u v]为像素坐标，[x y z]为相机坐标系下的信标点坐标，P为信标点在ECEF系下的坐标，待估状态为 Pc是相机光心在ECEF系下的坐标， 是ECEF系到相机坐标系的旋转矩阵。由于旋转矩阵无法直接加减和求导，所以将其表达为失准角 形式，即：
[0094]
[0095] 将式(8)的相机位置和姿态表示成误差形式为：
[0096]
[0097] 上式中， 为相机姿态、位置的观测值，δPc为位置误差。通过链式法则求观测值对状态的偏导，得到下式：
[0098]
[0099] 对于每对匹配点可构造后方交会观测方程，其
整理形式如下：
[0100]
[0101] ωu,v为像素观测值的噪声。
[0102] 2)视觉/惯导融合观测方程
[0103] 由于相机位姿和惯导位姿之间可通过外参建立联系，所以GNSS/SINS紧组合中加入视觉并不会增加额外的状态量，因此，其滤波的状态仍为惯导误差模型建立的导航参数(位置、速度和姿态)、SINS系统误差以及GNSS相关参数。只需根据惯导与相机的外参关系，将式(12)中表示的像素观测值与相机位姿误差的关系转换为像素观测值与惯导位姿误差的关系即可。
[0104] 惯导和相机光心经过空间标定后，可以表示如下：
[0105]
[0106] 式中，PSINS是惯导中心在ECEF系下的坐标， 是惯导坐标系到ECEF系的旋转矩阵，lb是相机光心在惯导坐标下的杆臂分量， 是相机坐标系到ECEF系的旋转矩阵， 是相机坐标系到惯导坐标系的旋转矩阵。
[0107] 根据式(13)可将式(10)转换为：
[0108]
[0109] 又因为 可以表示为：
[0110]
[0111] 因此可以得到p对惯导位姿的偏导数：
[0112]
[0113] 因此，式(12)可以转换为：
[0114]
[0115] 其最终完整的观测方程为：
[0116]
[0117] 式中，u1、un为像素观测值， 为信标点反投影得到的像素值，即式(17)中的εu为像素误差。
[0118] 五、GNSS/SINS/视觉紧组合
[0119] 上述步骤中，我们已经得到了在环境信标支持下，视觉和惯导融合的定位观测方程，如式。
[0120] 当GNSS卫星数较多以及PDOP值较小时，说明GNSS定位几何构型满足要求，此时，屏蔽视觉观测信息，只采用GNSS/SINS紧组合，其观测更新方程如下：
[0121]
[0122] 上式中， 为利用SINS递推的位置计算的战星几何双差距离， 和分别表示利用基准站和流动站接收机输出的伪距和载波相位计算的站星双差观
测值。 为ECEF系下的GNSS杆臂改正量，为各个卫星的双差方向余弦，εP和εL为伪距、相位的观测噪声。
[0123] 当GNSS卫星数较少，且视觉传感器成功匹配到若干信标点时，则在上式观测更新方程的基础上加入视觉像素约束观测值，得到观测方程如下：
[0124]
[0125] 当GNSS卫星完全被遮挡，但视觉可成功提取匹配到信标点时，只采用视觉的像素约束观测值，其观测方程如式(18)。
[0126] 应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附
权利要求的保护范围。
