一种基于定位子系统的融合定位方法及系统与流程

1.本发明属于定位技术领域，具体涉及一种基于定位子系统的融合定位方法及系统。


背景技术：

2.多传感器融合定位或多传感系统通常是指包括视觉传感器(如相机)、惯性测量单元(inertialmeasurementunit，imu，简称“惯导”)和全球导航卫星系统(globalnavigationsatellite system，gnss)定位等的定位系统，该系统兼具定位精度高和成本低的优势，成为了需要高精度(厘米级)定位结果的应用场景主要选择的定位方案。通过传感器融合定位获得高精度定位结果的过程中，需要确定各传感器的位置和姿态等参数，以及确定这些参数的协方差，以便对融合定位的定位精度半径进行估计和对相关性进行评估等。
3.现有技术一般采用海森矩阵求逆的方法进行协方差估计，但多传感器融合定位的参数的维数较多，直接矩阵求逆的算力消耗巨大，导致系统成本升高，而为了降低成本，进行近似处理又会影响估计出的协方差的准确性。因此，需要提供新的协方差确定方案，以满足成本和准确性的要求。


技术实现要素：

4.为解决多定位系统融合过程中的算力消耗大、成本高的问题，在本发明的第一方面提供了一种基于定位子系统的融合定位方法，包括：根据卫星定位系统获取目标实时的第一位置信息；基于所述第一位置信息，利用匹配定位系统实时计算目标的第二位置信息；基于多个第二位置信息和航迹推算系统实时计算目标的第三位置信息；根据所述第一位置信息、第二位置信息和第三位置信息各自协方差覆盖的位置区域的相互交并关系，对所述第一位置信息、第二位置信息和第三位置信息进行融合。
5.在本发明的一些实施例中，所述基于所述第一位置信息，利用匹配定位系统实时计算目标的第二位置信息包括：根据卫星定位系统获取目标实时的第一位置信息o1(x1，y1)～(0，r1)；提取所述第一位置信息o1(x1，y1)～(0，r1)预设范围内的局部高精地图，通过视觉识别提取地图的固定要素并将其与第一位置信息o1(x1，y1)～(0，r1)进行匹配，得到位置信息o2(x2，y2)～(0，r2)。
6.进一步的，所述基于多个第二位置信息和航迹推算系统实时计算目标的第三位置信息包括：基于多帧第一位置信息或第二位置信息，利用航迹推算对输出的位置信息o2(x2，y2)～(0，r2)进行解算，求解位置信息o3(x3，y3)～(0，r3)。
7.在本发明的一些实施例中，所述根据所述第一位置信息、第二位置信息和第三位置信息各自协方差覆盖的位置区域的相互交并关系，对所述第一位置信息、第二位置信息和第三位置信息进行融合包括：分别以第一位置信息、第二位置信息和第三位置信息中的坐标为圆心，每个位置信息的协方差为半径，构建每个位置信息的覆盖的位置区域；根据每
个位置信息的覆盖的位置区域与余下两个位置信息的覆盖的位置区域的相互交并关系，并通过最小二乘法对其融合。
8.进一步的，所述根据每个位置信息的覆盖的位置区域与余下两个位置信息的覆盖的位置区域的相互交并关系，并通过最小二乘法对其融合包括：若三个位置信息的覆盖的位置区域存在一个交点，则将交点的对应的位置信息作为融合后的位置信息；若三个位置信息的覆盖的位置区域至少存在两个交点，则将交点共同确定的圆的圆心对应的位置信息作为融合后的位置信息。
9.在上述的实施例中，还包括：若第二位置信息确定的位置与第一位置信息确定的位置距离低于阈值，则将第三位置信息设置为与第二位置信息等同。
10.本发明的第二方面，提供了一种基于定位子系统的融合定位系统，包括：确定模块，用于根据卫星定位系统获取目标实时的第一位置信息；计算模块，用于基于所述第一位置信息，利用匹配定位系统实时计算目标的第二位置信息；基于多个第二位置信息和航迹推算系统实时计算目标的第三位置信息；融合模块，用于根据所述第一位置信息、第二位置信息和第三位置信息各自协方差覆盖的位置区域的相互交并关系，对所述第一位置信息、第二位置信息和第三位置信息进行融合。
11.本发明的第三方面，提供了一种电子设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明在第一方面提供的基于定位子系统的融合定位方法。
12.本发明的第四方面，提供了一种计算机可读介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现本发明在第一方面提供的基于定位子系统的融合定位方法。
13.本发明的有益效果是：
14.1.基于传统的惯性导航会存在累计误差、基于视觉或高精度地图受制于地图自身和传感器的精度的影响，卫星定位系统在一些像立交桥下、停车场等场景中，无法准确定位且对算力要求高；本发明通过多种定位系统的融合，提高了单一定位系统的定位精度、鲁棒性；
15.2.依据定位子系统的协方差进行数据融合由于无需参数设置，具有更好的实时性；通过不同精度的位置信息融合，综合了惯性导航系统、视觉匹配系统、卫星定位系统各自的成本和算力优势，从而解决了多定位系统融合过程中的算力消耗大、成本高的问题。
附图说明
16.图1为本发明的一些实施例中的基于定位子系统的融合定位方法的基本流程示意图；
17.图2为本发明的一些实施例中的第一位置信息、第二位置信息和第三位置信息各自协方差覆盖的位置区域存在一个交点时的定位信息融合示意图；
18.图3为本发明的一些实施例中第一位置信息、第二位置信息和第三位置信息各自协方差覆盖的位置区域存在多个交点时的定位信息融合示意图；
19.图4为本发明的一些实施例中的基于定位子系统的融合定位系统的结构示意图；
20.图5为本发明的一些实施例中的电子设备的结构示意图。
具体实施方式
21.以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
22.参考图1与图2，在本发明的第一方面，提供了一种基于定位子系统的融合定位方法，包括：s100.根据卫星定位系统获取目标实时的第一位置信息；s200.基于所述第一位置信息，利用匹配定位系统实时计算目标的第二位置信息；基于多个第二位置信息和航迹推算系统实时计算目标的第三位置信息；s300.根据所述第一位置信息、第二位置信息和第三位置信息各自协方差覆盖的位置区域的相互交并关系，对所述第一位置信息、第二位置信息和第三位置信息进行融合。
23.可以理解，本发明的实施例中的定位子系统，通常包括视觉传感器、惯导和全球导航卫星系统，利用多传感器定位需要根据多传感器的测量数据通过迭代优化的方式确定传感器参数，即定位过程中所需要优化的与传感器相关的定位参数，也可简称为定位参数或位置信息。
24.具体的，传感器参数可以是直接的位姿参数，也可以是用于确定位姿参数过程中的其他待优化参数，例如，初始化时迭代优化的传感器参数包括：视觉传感器的位姿(每一帧相对于第一帧的位置和姿态)、视觉传感器的尺度、视觉传感器的速度、相机坐标系与世界坐标系间的旋转外参和陀螺仪零偏等；定位过程中迭代优化的传感器参数包括：相对世界坐标系下的位姿、绝对世界坐标系下的位姿、相对世界坐标系与绝对世界坐标系间的旋转外参和全球定位系统的天线与惯导间的相对位置外参等。迭代优化过程中需要建立包含各传感器的测量数据即观测数据和待优化的传感器参数的多个残差因子，每个残差因子可以是包含部分的观测数据和部分的传感器参数。
25.参考图2与图3，位置信息可以通过定位参数转换后得到的坐标及其精度(误差)表达，例如，o1(x1，y1)～(0，r1)表示为由卫星定位系统获取的第一位置信息的坐标为(x1，y1)，以协方差r1为圆心的圆所覆盖的位置区域；o2(x2，y2)～(0，r2)或o3(x3，y3)～(0，r3)所表示的位置区域以此类推；位置区域通常是平面的，在某些特殊场景中，可以包含三维信息。据gps_rtk定位系统可以直接给出位置信息o1(x1，y1)～(0，r1)。
26.在本发明的一些实施例的步骤s200中，所述基于所述第一位置信息，利用匹配定位系统实时计算目标的第二位置信息包括：s201.根据卫星定位系统获取目标实时的第一位置信息o1(x1，y1)～(0，r1)；s202.提取所述第一位置信息o1(x1，y1)～(0，r1)预设范围内的局部高精地图，通过视觉识别提取地图的固定要素并将其与第一位置信息o1(x1，y1)～(0，r1)进行匹配，得到位置信息o2(x2，y2)～(0，r2)。
27.具体地，根据gps_rtk定位系统可以直接给出位置信息o1(x1，y1)～(0，r1)；然后根据o1(x1，y1)～(0，r1)提取该位置的局部高精地图，通过视觉方法(视觉识别或语义识别等手段)提取的车道线、路标等信息进行地图匹配，匹配输出位置信息o2(x2，y2)～(0，r2)；例如，视觉传感器(摄像头或激光雷达)中获取或识别出目标当前位置可视范围内包含了某条道路的提示牌或地标建筑的图像，其记载了当前位置的具体位置信息，将其与高精度地图匹配后得出位置信息o2(x2，y2)～(0，r2)。
28.在一些实施例中，由于航迹推算具有短时高精度，需要多帧历史位置信息进行轨迹推算，因此，基于上述步骤s201和s202，所述基于多个第二位置信息和航迹推算系统实时
计算目标的第三位置信息包括：s203.基于多帧第一位置信息或第二位置信息，利用航迹推算对输出的位置信息o2(x2，y2)～(0，r2)进行解算，求解位置信息o3(x3，y3)～(0，r3)。
29.具体地，第一帧数据时无融合定位，航迹推算根据地图匹配输出的位置信息o2(x2，y2)～(0，r2)，进行解算，求解位置信息o3(x3，y3)～(0，r3)，航迹推算具有短时高精，因此设置r3＝r2；第一帧后航迹推算根据融合定位信息进行求解，求解位置信息o3(x3，y3)～(0，r3)，此时航迹推算精度r3＝r。
30.在本发明的一些实施例的s300中，所述根据所述第一位置信息、第二位置信息和第三位置信息各自协方差覆盖的位置区域的相互交并关系，对所述第一位置信息、第二位置信息和第三位置信息进行融合包括：s301.分别以第一位置信息、第二位置信息和第三位置信息中的坐标为圆心，每个位置信息的协方差为半径，构建每个位置信息的覆盖的位置区域；s302.根据每个位置信息的覆盖的位置区域与余下两个位置信息的覆盖的位置区域的相互交并关系，并通过最小二乘法对其融合。
31.参考图2与图3，可以理解，以定位点为圆心，以定位点的协方差为半径，根据步骤s100中三个子系统的空间位置信息，可以将其给出位置信息抽象为三个圆相交的问题；而三个圆相交的问题主要根据交点进行划分或处理，因此：所述根据每个位置信息的覆盖的位置区域与余下两个位置信息的覆盖的位置区域的相互交并关系，并通过最小二乘法对其融合包括：s3021.若三个位置信息的覆盖的位置区域存在一个交点，则将交点的对应的位置信息作为融合后的位置信息；s3022.若三个位置信息的覆盖的位置区域至少存在两个交点，则将交点共同确定的圆的圆心对应的位置信息作为融合后的位置信息。
32.具体地，图2示出了三个圆存在同一个点(交点)的情形，设该交点坐标为o(x，y)～(0，r)，则该点满足以下方程组：
[0033][0034]
其中，(xi，yi)表示第i个子系统的坐标，(0，ri)表示子系统坐标满足均值为0，协方差为ri高斯分布。由于真实值存在以每个子系统坐标为圆心，以协方差为半径的圆内。
[0035]
图3示出了三个圆存在同一交集(多个交点)的情形，设交集区域三个点分别为a1、b1、c1，设三点坐标形成的圆，圆心为o(x，y)，半径为r。则交点a1、b1、c1满足方程：
[0036][0037]
点a1满足以下方程：
[0038][0039]
点b1满足以下方程：
[0040][0041]
点c1满足以下方程：
[0042][0043]
其他情况，利用等式(6)求解定位子系统中最小协方差以确定最终融合后的定位信息：
[0044][0045]
可以理解，针对步骤200中的方程，均可以采用最小二乘或高斯牛顿法求解方程组：等式(1)可展开成式(7)：
[0046][0047]
等式整理得：
[0048][0049]
即：
[0050][0051]
上式为经典ax＝b问题，令
[0052][0053][0054][0055]
其中a、b矩阵为已知矩阵，利用最小二乘求解
[0056]
x＝(a
t
*a)*a
t
*b
ꢀꢀ
(13)；
[0057]
输出融合定位位置信息(x,y)～(0,r＝0)。
[0058]
特别地，将上述求解方式按“三圆交点问题”改写为如下等式：
[0059][0060]
利用最小二乘求解x＝(a
t
*a)*a
t
*b可得(x,y)～(0,r＝r)。
[0061]
根据上述步骤求解的三个子系统的最小协方差，确定融合定位位置信息，如下式：
[0062][0063]
在一些实施例中，由于航迹推算具有短时高精度，需要多帧历史位置信息(由第一定位信息或第二定位信息给出)进行轨迹推算，即在第一位置信息或第二位置信息定位不准，或误差超过阈值时，历史位置信息失效或目标保持静止，此时，需要对第三位置信息进行校正，即在上述的实施例中，还包括：s400.若第二位置信息确定的位置与第一位置信息确定的位置距离低于阈值，则将第三位置信息设置为与第二位置信息等同。
[0064]
实施例2
[0065]
参考图4，本发明的第二方面，提供了一种基于定位子系统的融合定位系统1，包括：确定模块11，用于根据卫星定位系统获取目标实时的第一位置信息；计算模块12，用于基于所述第一位置信息，利用匹配定位系统实时计算目标的第二位置信息；基于多个第二位置信息和航迹推算系统实时计算目标的第三位置信息；融合模块13，用于根据所述第一位置信息、第二位置信息和第三位置信息各自协方差覆盖的位置区域的相互交并关系，对所述第一位置信息、第二位置信息和第三位置信息进行融合。
[0066]
进一步地，所述计算模块12包括：第一计算单元，用于基于所述第一位置信息，利用匹配定位系统实时计算目标的第二位置信息；第二计算单元，用于基于多个第二位置信息和航迹推算系统实时计算目标的第三位置信息。
[0067]
实施例3
[0068]
参考图5，本发明的第三方面，提供了一种电子设备，包括：一个或多个处理器；存储装置，用于存储一个或多个程序，当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明在第一方面的方法。
[0069]
电子设备500可以包括处理装置(例如中央处理器、图形处理器等)501，其可以根据存储在只读存储器(rom)502中的程序或者从存储装置508加载到随机访问存储器(ram)503中的程序而执行各种适当的动作和处理。在ram503中，还存储有电子设备500操作所需的各种程序和数据。处理装置501、rom 502以及ram 503通过总线504彼此相连。输入/输出(i/o)接口505也连接至总线504。
[0070]
通常以下装置可以连接至i/o接口505：包括例如触摸屏、触摸板、键盘、鼠标、摄像头、麦克风、加速度计、陀螺仪等的输入装置506；包括例如液晶显示器(lcd)、扬声器、振动器等的输出装置507；包括例如硬盘等的存储装置508；以及通信装置509。通信装置509可以允许电子设备500与其他设备进行无线或有线通信以交换数据。虽然图5示出了具有各种装
置的电子设备500，但是应理解的是，并不要求实施或具备所有示出的装置。可以替代地实施或具备更多或更少的装置。图5中示出的每个方框可以代表一个装置，也可以根据需要代表多个装置。
[0071]
特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信装置509从网络上被下载和安装，或者从存储装置508被安装，或者从rom502被安装。在该计算机程序被处理装置501执行时，执行本公开的实施例的方法中限定的上述功能。需要说明的是，本公开的实施例所描述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑磁盘只读存储器(cd－rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开的实施例中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开的实施例中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、rf(射频)等等，或者上述的任意合适的组合。
[0072]
上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。上述计算机可读介质承载有一个或者多个计算机程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备：
[0073]
可以以一种或多种程序设计语言或其组合来编写用于执行本公开的实施例的操作的计算机程序代码，程序设计语言包括面向对象的程序设计语言—诸如java、smalltalk、c 、python，还包括常规的过程式程序设计语言—诸如“c”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(lan)或广域网(wan)——连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
[0074]
附图中的流程图和框图，图示了按照本公开各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上
可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。需要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0075]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。