CN111121755B - 一种多传感器的融合定位方法、装置、设备及存储介质 - Google Patents

Abstract

本发明实施例公开了一种多传感器的融合定位方法、装置、设备及存储介质。该方法包括：获取至少两个定位传感器输出的待定位物体的定位数据；将所述各定位传感器的定位数据进行时间戳同步，确定所述各定位传感器的时间定位数据；根据所述各定位传感器的时间定位数据，构建非线性优化函数，并生成非线性优化函数组；基于所述非线性优化函数组，确定所述待定位物体的目标定位数据。本发明实施例通过构建非线性优化函数，解决了不同定位传感器之间数据融合的问题，降低了不同定位传感器组合使用的难度，进而提高了融合定位系统的普及性和鲁棒性。

Description

一种多传感器的融合定位方法、装置、设备及存储介质

技术领域

本发明实施例涉及导航定位技术领域，尤其涉及一种多传感器的融合定位方法、装置、设备及存储介质。

背景技术

建筑工地具有面积范围广泛、环境复杂多样和不易移动等特点。为了对建筑工地内的作业信息进行采集，辅助完成作业任务，定位导航是建筑场景中的关键技术之一。

现阶段的导航定位技术有很多，例如，全球定位系统(Global PositioningSystem,GPS)、双目视觉里程计、惯性导航系统(Inertial Navigation System,INS)、轮式里程计和激光测距仪(Laser range finder)等等，其中，GPS是一种以空中卫星为基础的高精度无线电导航定位系统，具有高精度、全天候、全球覆盖和使用灵活等特点。双目视觉里程计采用两个相机采集场景内图像信息，利用三角测量法求取场景内的三维深度信息，使用于非结构化的环境。INS利用三个陀螺仪和三个加速计器件来检测并输出运载体在运动过程中的角速度信息和加速度信息。

在实际应用过程中，通常会采用不同误差特性的导航子系统或子设备进行组合，利用优势互补的方式，实现融合定位系统，再利用数据融合算法解算各个子系统采集到的信息。其中，最常用的数据融合算法是卡尔曼滤波算法。但基于卡尔曼滤波的融合算法的导航定位系统，需要将不同的定位传感器之间实现实时时间对齐和坐标对齐，因而会增加不同定位传感器的组合难度，减少不同定位传感器的组合形式，进而影响融合定位系统的普及性和鲁棒性。

发明内容

本发明实施例提供了一种多传感器的融合定位方法、装置、设备及存储介质，以实现降低不同定位传感器组合使用时数据融合的难度，进而提高融合定位系统的普及性和鲁棒性。

第一方面，本发明实施例提供了一种多传感器的融合定位方法，该方法包括：

获取至少两个定位传感器输出的待定位物体的定位数据；

将所述各定位传感器的定位数据进行时间戳同步，确定所述各定位传感器的时间定位数据；

根据所述各定位传感器的时间定位数据，构建非线性优化函数，并生成非线性优化函数组；

基于所述非线性优化函数组，确定所述待定位物体的目标定位数据。

第二方面，本发明实施例还提供了一种多传感器的融合定位装置，该装置包括：定位数据获取模块，用于获取至少两个定位传感器输出的待定位物体的定位数据；

时间定位数据确定模块，用于将所述各定位传感器的定位数据进行时间戳同步，确定所述各定位传感器的时间定位数据；

非线性优化函数构建模块，用于根据所述各定位传感器的时间定位数据，构建非线性优化函数，并生成非线性优化函数组；

目标定位数据确定模块，用于基于所述非线性优化函数组，确定所述待定位物体的目标定位数据。

第三方面，本发明实施例还提供了一种设备，该设备包括：

一个或多个处理器；

存储器，用于存储一个或多个程序；

当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现上述所涉及的任一所述的多传感器的融合定位方法。

第四方面，本发明实施例还提供了一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行上述所涉及的任一所述的多传感器的融合定位方法。

本发明实施例通过构建非线性优化函数，解决了不同定位传感器之间数据融合的问题，降低了不同定位传感器组合使用的难度，进而提高了融合定位系统的普及性和鲁棒性。

附图说明

图1是本发明实施例一提供的一种多传感器的融合定位方法的流程图。

图2是本发明实施例二提供的一种多传感器的融合定位方法的流程图。

图3是本发明实施例二提供的一种多传感器的融合定位方法的具体实例流程图。

图4是本发明实施例二提供的一种多传感器的融合定位方法的算法程序流程图。

图5是本发明实施例三提供的一种多传感器的融合定位装置的示意图。

图6是本发明实施例四提供的一种设备的结构示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。

实施例一

图1是本发明实施例一提供的一种多传感器的融合定位方法的流程图，本实施例可适用于采用多种定位传感器获取定位信息的情况，该方法可以由多传感器的融合定位装置来执行，该装置可采用软件和/或硬件的方式实现，该装置可以配置于终端设备中。具体包括如下步骤：

S110、获取至少两个定位传感器输出的待定位物体的定位数据。

建筑工地具有面积范围广泛、环境复杂多样和不易移动等特点。GPS是目前应用最广泛的导航定位技术，但是建筑场景中高楼耸立，各种塔架、棚帐、塔吊都会严重遮挡GPS信号，导致定位精度误差大甚至丢失位置。双目是视觉里程计的定位特点是特征点丰富，场景越脏乱差，相机定位往往越准确，相反过分空旷双目相机定位误差较大。同时，建筑场景还有一个特点是工人多，灰尘多容易对相机成像造成干扰，从而影响精度。而轮式里程计不易受外部环境的干扰。在一个实施例中，可选的，采用GPS、双目视觉里程计和轮式里程计对待定位物体进行定位。

其中，示例性的，定位传感器包括但不限于3D激光里程计、2D激光里程计、超宽带(Ultra Wide Band，UWB)定位系统和多普勒测速传感器等等。

在一个实施例中，可选的，获取已保存的至少两个定位传感器输出的待定位物体的定位数据。现有技术中的卡尔曼滤波算法也需要各定位传感器的定位数据输出时间相同，但由于卡尔曼滤波算法是实时融合的算法，因此要求各定位传感器的定位数据实时时间同步，通常在使用时需要加一个定时器以控制各定位传感器在同一时间点输出定位数据，实现各定位传感器的时间同步。但这样的技术方法会增加各定位传感器的定位数据的融合难度。本实施例中，可以在安装定位传感器后，将各定位传感器的输出的定位数据进行同步存储，在需要进行数据融合时，读取已保存的各定位传感器的定位数据再进行时间同步。其中，示例性的，可以将定位传感器输出的定位数据缓存在不同的存储区域。本实施例的技术方法允许各定位传感器之间的时钟延迟，在建筑工地复杂场景下实现持续稳定高精度定位。

S120、将各定位传感器的定位数据进行时间戳同步，确定各定位传感器的时间定位数据。

其中，时间戳同步是指根据预设时间同步规则，将各定位传感器的定位数据进行时间上的同步，以使各时间定位数据的时间点相同或相近。在一个实施例中，可选的，将与预设时间点相同和/或在预设时间点的预设误差范围内的参考时间点对应的各定位传感器的定位数据作为各定位传感器的时间定位数据。

其中，示例性的，预设时间点可以是用户输入的时间点，当然，也可以是以任一定位传感器的时间点为时间基准确定的时间点。示例性的，预设时间点最小时间单位可以是秒。

其中，示例性的，预设误差范围可以是±0.1s。此处对预设误差范围不作限定，可以根据各传感器的采样频率确定。举例而言，定位传感器A的采样频率为10Hz，定位传感器B的采样频率为20Hz，则定位传感器A和定位传感器B相对于预设时间点的时间误差为0.05s，则可以将预设误差范围设置为±0.05s。

S130、根据各定位传感器的时间定位数据，构建非线性优化函数，并生成非线性优化函数组。

其中，非线性优化函数组包括至少两个非线性优化函数。

在一个实施例中，可选的，非线性优化函数包括时间优化函数，相应的，根据各定位传感器的时间定位数据，构建非线性优化函数，包括：

基于每个定位传感器的时间定位数据，通过如下公式构建定位传感器的时间优化函数：

其中，x₀、y₀和z₀表示定位传感器分别在x方向、y方向和z方向上的时间定位数据；x、y和z表示对定位传感器的时间定位数据进行优化后得到的在x方向、y方向和z方向上的目标定位数据。

在一个实施例中，可选的，时间定位数据包括二维时间定位数据。示例性的，当定位传感器包括轮式里程计时，轮式里程计输出的定位数据为(x,y,ω)，其中，x，y分别为x方向和y方向上的水平速度，ω为航向角。如果直接将二维时间定位数据和三维时间定位数据进行融合，则在遇到载体爬坡时，融合后得到的目标定位数据会存在较大的误差。其中，载体是指安装各定位传感器的硬件结构。

在本实施例中，二维时间定位数据的时间优化函数满足公式：

本实施例中构建的时间优化函数，解决了二维时间定位数据和三维时间定位数据融合的问题，提高了数据融合后的准确度和鲁棒性。由于时间定位数据在融合时不会受到数据空间维度的影响，进而提高了融合定位系统的普及性。

S140、基于非线性优化函数组，确定待定位物体的目标定位数据。

在一个实施例中，可选的，将非线性优化函数组中的非线性优化函数进行求和计算，得到非线性求和函数；基于非线性求和函数，构建最小二乘法误差函数，并基于最小二乘法误差函数对非线性函数组进行优化计算，得到待定位物体的目标定位数据。

举例而言，在一个实施例中，当定位传感器包括GPS、双目视觉里程计和轮式里程计时，则非线性优化函数组包括3个非线性优化函数。将GPS、双目视觉里程计和轮式里程计的非线性优化函数分别为E_GPS、E_视觉和E_轮式表示，则非线性求和函数为E_求和＝E_GPS+E_视觉+E_轮式。

在一个实施例中，可选的，获取非线性优化的初始值，并基于非线性求和函数，构建最小二乘法误差函数，采用LevenbergMarquardt算法计算得到待定位物体的目标定位数据。其中，初始值可以是任一三维的时间定位数据，示例性的，可以将视觉里程计的6维位姿作为非线性优化的初始值。具体的，最小二乘法误差函数为argmin(E_求和)。

本实施例的技术方案，通过构建非线性优化函数，解决了不同定位传感器之间数据融合的问题，降低了不同定位传感器组合使用的难度，进而提高了融合定位系统的普及性和鲁棒性。

实施例二

图2是本发明实施例二提供的一种多传感器的融合定位方法的流程图，本实施例的技术方案是上述实施例的基础上的进一步细化。可选的，所述非线性优化函数还包括坐标优化函数，相应的，所述根据所述各定位传感器的时间定位数据，构建非线性优化函数，包括：根据所述各定位传感器的时间定位数据、各时间定位数据的空间维度和所属的坐标系，构建坐标优化函数。

本实施例的具体实施步骤包括：

S210、获取至少两个定位传感器输出的待定位物体的定位数据。

S220、将各定位传感器的定位数据进行时间戳同步，确定各定位传感器的时间定位数据。

S230、根据各定位传感器的时间定位数据，构建时间优化函数。

S240、根据各定位传感器的时间定位数据、各时间定位数据的空间维度和所属的坐标系，构建坐标优化函数。

在一个实施例中，可选的，根据各时间定位数据的空间维度和所属的坐标系，对各时间定位数据进行筛选，确定时间定位数据中是否存在至少两种坐标系对应的三维时间定位数据；如果是，则生成坐标系信息，并根据坐标系信息和三维时间定位数据，构建坐标优化函数。

其中，不同的定位传感器输出的定位数据所属的坐标系可能不同。示例性的，GPS的时间定位数据所属的坐标系包括地球坐标系，地球坐标系包括经度、纬度和高度等时间定位数据。双目视觉里程计的时间定位数据所属的坐标系包括三维直角坐标系。其中，示例性的，坐标系信息包括至少两种坐标系和各坐标系对应的时间定位数据。

在一个实施例中，可选的，如果时间定位数据中不存在至少两种坐标系对应的三维时间定位数据，则只构建时间优化函数。基于时间优化函数生成的非线性优化函数组，确定待定位物体的目标定位数据。

在一个实施例中，可选的，基于每种坐标系，根据坐标系对应的三维时间定位数据和除坐标系之外的所有坐标系对应的三维时间定位数据，构建坐标系对应的至少一个坐标优化函数；其中，通过如下公式构建坐标优化函数：

其中，A表示定位传感器A，

表示定位传感器A从第i帧到第j帧的旋转变换四元数，/>

表示定位传感器A在T时刻第j帧的时间定位数据，/>

表示定位传感器A在T时刻第i帧的时间定位数据；B表示定位传感器B，/>

表示定位传感器B从第i帧到第j帧的旋转变换四元数，/>

表示定位传感器B在T时刻第j帧的时间定位数据，/>

表示定位传感器B在T时刻第i帧的时间定位数据。

其中，示例性的，假设定位传感器B为双目视觉里程计，则

表示第T时刻，双目视觉里程计坐标系下第j帧采集到的唯一。

举例而言，现有3种坐标系分别为坐标系A、坐标系B和坐标系C，若以坐标系A为参考坐标系，则坐标优化函数有坐标系A和坐标系B的坐标优化函数和坐标系A和坐标系C的坐标优化函数。此处对参考坐标系的选择不作限定。

S250、基于时间优化函数和坐标优化函数，生成非线性优化函数组。

S260、基于非线性优化函数组，确定待定位物体的目标定位数据。

举例而言，图3是本发明实施例二提供的一种多传感器的融合定位方法的具体实例流程图。图3以定位传感器包括GPS、双目视觉里程计和轮式里程计为例。对3种定位传感器输出的定位数据进行时间同步后，构建非线性优化函数，并进行非线性优化计算，得到待定位物体的目标定位数据。将GPS、双目视觉里程计和轮式里程计的定位数据进行数据融合，在建筑场景中，目标定位数据的单点定位精度稳定在正负10cm左右；

图4是本发明实施例二提供的一种多传感器的融合定位方法的算法程序流程图。图4以定位传感器包括GPS、双目视觉里程计和轮式里程计为例。将某一时间点的GPS、双目视觉里程计和轮式里程计输出的定位数据输入到算法程序中，并对其进行时间戳同步，将生成的各传感器的时间定位数据保存到缓存空间中，其中，各传感器的时间定位数据的缓存空间可以相同也可以不同。获取下一时间点的各定位传感器的定位数据，重复上述过程。在算法程序实现的过程中，在获取输入数据的同时，运行构建优化函数的算法程序，获取缓存空间中的时间定位数据，构建优化函数，进行非线性优化计算并输出结果，得到待定位物体的目标定位数据并保存。

本实施例的技术方案，通过将不同定位传感器在同一时间的定位数据的相对变化值进行融合，构建坐标优化函数，解决了不同定位传感器输出的定位数据坐标系不同的问题，降低了不同定位传感器组合使用的难度，安全要求低，进而提高了融合定位系统的普及性。由于不同的定位传感器在组合使用时不会受到坐标系不同的限制，进而可以更好的发挥各定位传感器的定位优势，提高融合定位系统的鲁棒性。

实施例三

图5是本发明实施例三提供的一种多传感器的融合定位装置的示意图。本实施例可适用于采用多种定位传感器获取定位信息的情况，该装置可采用软件和/或硬件的方式实现，该装置可以配置于终端设备中。该多传感器的融合定位装置包括：定位数据获取模块310、时间定位数据确定模块320、非线性优化函数构建模块330和目标定位数据确定模块340。

其中，定位数据获取模块310，用于获取至少两个定位传感器输出的待定位物体的定位数据；

时间定位数据确定模块320，用于将各定位传感器的定位数据进行时间戳同步，确定各定位传感器的时间定位数据；

非线性优化函数构建模块330，用于根据各定位传感器的时间定位数据，构建非线性优化函数，并生成非线性优化函数组；

目标定位数据确定模块340，用于基于非线性优化函数组，确定待定位物体的目标定位数据。

本实施例的技术方案，通过构建非线性优化函数，解决了不同定位传感器之间数据融合的问题，降低了不同定位传感器组合使用的难度，进而提高了融合定位系统的普及性和鲁棒性。

在上述技术方案的基础上，可选的，非线性优化函数包括时间优化函数，相应的，非线性优化函数构建模块330包括：

时间优化函数构建单元，用于基于每个定位传感器的时间定位数据，通过如下公式构建定位传感器的时间优化函数：

其中，x₀、y₀和z₀表示定位传感器分别在x方向、y方向和z方向上的时间定位数据；x、y和z表示对定位传感器的时间定位数据进行优化后得到的在x方向、y方向和z方向上的目标定位数据。

可选的，非线性优化函数还包括坐标优化函数，相应的，相应的，非线性优化函数构建模块330包括：

坐标优化函数构建单元，用于根据各定位传感器的时间定位数据、各时间定位数据的空间维度和所属的坐标系，构建坐标优化函数。

可选的，坐标优化函数构建单元包括：

时间定位数据筛选子单元，用于根据各时间定位数据的空间维度和所属的坐标系，对各时间定位数据进行筛选，确定时间定位数据中是否存在至少两种坐标系对应的三维时间定位数据；

坐标优化函数构建子单元，用于如果是，则生成坐标系信息，并根据坐标系信息和三维时间定位数据，构建坐标优化函数。

可选的，坐标优化函数构建子单元具体用于：

基于每种坐标系，根据坐标系对应的三维时间定位数据和除坐标系之外的所有坐标系对应的三维时间定位数据，构建坐标系对应的至少一个坐标优化函数；

其中，通过如下公式构建坐标优化函数：

其中，A表示定位传感器A，

表示定位传感器A从第i帧到第j帧的旋转变换四元数，/>

表示定位传感器A在T时刻第j帧的时间定位数据，/>

表示定位传感器A在T时刻第i帧的时间定位数据；B表示定位传感器B，/>

表示定位传感器B从第i帧到第j帧的旋转变换四元数，/>

表示定位传感器B在T时刻第j帧的时间定位数据，/>

表示定位传感器B在T时刻第i帧的时间定位数据。

可选的，目标定位数据确定模块340具体用于：

将非线性优化函数组中的非线性优化函数进行求和计算，得到非线性求和函数；

基于非线性求和函数，构建最小二乘法误差函数，并基于最小二乘法误差函数对非线性函数组进行优化计算，得到待定位物体的目标定位数据。

可选的，时间定位数据确定模块320具体用于：

将与预设时间点相同和/或在预设时间点的预设误差范围内的参考时间点对应的各定位传感器的定位数据作为各定位传感器的时间定位数据。

本发明实施例所提供的多传感器的融合定位装置可以用于执行本发明实施例所提供的多传感器的融合定位方法，具备执行方法相应的功能和有益效果。

值得注意的是，上述多传感器的融合定位装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。

实施例四

图6是本发明实施例四提供的一种设备的结构示意图，本发明实施例四为本发明上述实施例的多传感器的融合定位方法的实现提供服务，可配置上述实施例中的多传感器的融合定位装置。图6示出了适于用来实现本发明实施方式的示例性设备12的框图。图6显示的设备12仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。

如图6所示，设备12以通用计算设备的形式表现。设备12的组件可以包括但不限于：一个或者多个处理器或者处理单元16，系统存储器28，连接不同系统组件(包括系统存储器28和处理单元16)的总线18。

总线18表示几类总线结构中的一种或多种，包括存储器总线或者存储器控制器，外围总线，图形加速端口，处理器或者使用多种总线结构中的任意总线结构的局域总线。举例来说，这些体系结构包括但不限于工业标准体系结构(ISA)总线，微通道体系结构(MAC)总线，增强型ISA总线、视频电子标准协会(VESA)局域总线以及外围组件互连(PCI)总线。

设备12典型地包括多种计算机系统可读介质。这些介质可以是任何能够被设备12访问的可用介质，包括易失性和非易失性介质，可移动的和不可移动的介质。

系统存储器28可以包括易失性存储器形式的计算机系统可读介质，例如随机存取存储器(RAM)30和/或高速缓存存储器32。设备12可以进一步包括其它可移动/不可移动的、易失性/非易失性计算机系统存储介质。仅作为举例，存储系统34可以用于读写不可移动的、非易失性磁介质(图6未显示，通常称为“硬盘驱动器”)。尽管图6中未示出，可以提供用于对可移动非易失性磁盘(例如“软盘”)读写的磁盘驱动器，以及对可移动非易失性光盘(例如CD-ROM,DVD-ROM或者其它光介质)读写的光盘驱动器。在这些情况下，每个驱动器可以通过一个或者多个数据介质接口与总线18相连。存储器28可以包括至少一个程序产品，该程序产品具有一组(例如至少一个)程序模块，这些程序模块被配置以执行本发明各实施例的功能。

具有一组(至少一个)程序模块42的程序/实用工具40，可以存储在例如存储器28中，这样的程序模块42包括但不限于操作系统、一个或者多个应用程序、其它程序模块以及程序数据，这些示例中的每一个或某种组合中可能包括网络环境的实现。程序模块42通常执行本发明所描述的实施例中的功能和/或方法。

设备12也可以与一个或多个外部设备14(例如键盘、指向设备、显示器24等)通信，还可与一个或者多个使得用户能与该设备12交互的设备通信，和/或与使得该设备12能与一个或多个其它计算设备进行通信的任何设备(例如网卡，调制解调器等等)通信。这种通信可以通过输入/输出(I/O)接口22进行。并且，设备12还可以通过网络适配器20与一个或者多个网络(例如局域网(LAN)，广域网(WAN)和/或公共网络，例如因特网)通信。如图6所示，网络适配器20通过总线18与设备12的其它模块通信。应当明白，尽管图中未示出，可以结合设备12使用其它硬件和/或软件模块，包括但不限于：微代码、设备驱动器、冗余处理单元、外部磁盘驱动阵列、RAID系统、磁带驱动器以及数据备份存储系统等。

处理单元16通过运行存储在系统存储器28中的程序，从而执行各种功能应用以及数据处理，例如实现本发明实施例所提供的多传感器的融合定位方法。

通过上述终端设备，解决了不同定位传感器之间数据融合的问题，降低了不同定位传感器组合使用的难度，进而提高了融合定位系统的普及性和鲁棒性。

实施例五

本发明实施例五还提供了一种包含计算机可执行指令的存储介质，计算机可执行指令在由计算机处理器执行时用于执行一种多传感器的融合定位方法，该方法包括：

获取至少两个定位传感器输出的待定位物体的定位数据；

将各定位传感器的定位数据进行时间戳同步，确定各定位传感器的时间定位数据；

根据各定位传感器的时间定位数据，构建非线性优化函数，并生成非线性优化函数组；

基于非线性优化函数组，确定待定位物体的目标定位数据。

本发明实施例的计算机存储介质，可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。

计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。

计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于无线、电线、光缆、RF等等，或者上述的任意合适的组合。

可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码，程序设计语言包括面向对象的程序设计语言，诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络包括局域网(LAN)或广域网(WAN)，连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

当然，本发明实施例所提供的一种包含计算机可执行指令的存储介质,其计算机可执行指令不限于如上的方法操作，还可以执行本发明任意实施例所提供的多传感器的融合定位方法中的相关操作。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (9)

1.一种多传感器的融合定位方法，其特征在于，包括：

获取至少两个定位传感器输出的待定位物体的定位数据；

将所述各定位传感器的定位数据进行时间戳同步，确定所述各定位传感器的时间定位数据；

根据所述各定位传感器的时间定位数据，构建非线性优化函数，并生成非线性优化函数组；

基于所述非线性优化函数组，确定所述待定位物体的目标定位数据；

所述非线性优化函数包括时间优化函数，相应的，所述根据所述各定位传感器的时间定位数据，构建非线性优化函数，包括：

基于每个定位传感器的时间定位数据，通过如下公式构建所述定位传感器的时间优化函数：

其中，x₀、y₀和z₀表示所述定位传感器分别在x方向、y方向和z方向上的时间定位数据；x、y和z表示对所述定位传感器的时间定位数据进行优化后得到的在x方向、y方向和z方向上的目标定位数据。

2.根据权利要求1所述的方法，其特征在于，所述非线性优化函数还包括坐标优化函数，相应的，所述根据所述各定位传感器的时间定位数据，构建非线性优化函数，包括：

根据所述各定位传感器的时间定位数据、各时间定位数据的空间维度和所属的坐标系，构建坐标优化函数。

3.根据权利要求2所述的方法，其特征在于，所述根据所述各定位传感器的时间定位数据、各时间定位数据的空间维度和所属的坐标系，构建坐标优化函数，包括：

根据所述各时间定位数据的空间维度和所属的坐标系，对所述各时间定位数据进行筛选，确定所述时间定位数据中是否存在至少两种坐标系对应的三维时间定位数据；

如果是，则生成坐标系信息，并根据所述坐标系信息和所述三维时间定位数据，构建坐标优化函数。

4.根据权利要求3所述的方法，其特征在于，所述根据所述坐标系信息和所述三维时间定位数据，构建坐标优化函数，包括：

基于每种坐标系，根据所述坐标系对应的三维时间定位数据和除所述坐标系之外的所有坐标系对应的三维时间定位数据，构建所述坐标系对应的至少一个坐标优化函数；

其中，通过如下公式构建坐标优化函数：

其中，A表示定位传感器A，

表示定位传感器A从第i帧到第j帧的旋转变换四元数，/>

表示定位传感器A在T时刻第j帧的时间定位数据，/>

表示定位传感器A在T时刻第i帧的时间定位数据；B表示定位传感器B，/>

表示定位传感器B从第i帧到第j帧的旋转变换四元数，/>

表示定位传感器B在T时刻第j帧的时间定位数据，/>

表示定位传感器B在T时刻第i帧的时间定位数据。

5.根据权利要求1-4任一所述的方法，其特征在于，所述基于所述非线性优化函数组，确定所述待定位物体的目标定位数据，包括：

将所述非线性优化函数组中的非线性优化函数进行求和计算，得到非线性求和函数；

基于所述非线性求和函数，构建最小二乘法误差函数，并基于所述最小二乘法误差函数对所述非线性优化函数组进行优化计算，得到所述待定位物体的目标定位数据。

6.根据权利要求1所述的方法，其特征在于，所述将所述各定位传感器的定位数据进行时间戳同步，确定所述各定位传感器的时间定位数据，包括：

将与预设时间点相同和/或在预设时间点的预设误差范围内的参考时间点对应的各定位传感器的定位数据作为所述各定位传感器的时间定位数据。

7.一种多传感器的融合定位装置，其特征在于，包括：

定位数据获取模块，用于获取至少两个定位传感器输出的待定位物体的定位数据；

时间定位数据确定模块，用于将所述各定位传感器的定位数据进行时间戳同步，确定所述各定位传感器的时间定位数据；

非线性优化函数构建模块，用于根据所述各定位传感器的时间定位数据，构建非线性优化函数，并生成非线性优化函数组；

目标定位数据确定模块，用于基于所述非线性优化函数组，确定所述待定位物体的目标定位数据；

所述非线性优化函数包括时间优化函数，相应的，所述非线性优化函数构建模块包括：

时间优化函数构建单元，用于基于每个定位传感器的时间定位数据，通过如下公式构建定位传感器的时间优化函数：

其中，x₀、y₀和z₀表示定位传感器分别在x方向、y方向和z方向上的时间定位数据；x、y和z表示对定位传感器的时间定位数据进行优化后得到的在x方向、y方向和z方向上的目标定位数据。

8.一种电子设备，其特征在于，包括：

一个或多个处理器；

存储器，用于存储一个或多个程序；

当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-6中任一所述的多传感器的融合定位方法。

9.一种包含计算机可执行指令的存储介质，其特征在于，所述计算机可执行指令在由计算机处理器执行时用于执行如权利要求1-6中任一所述的多传感器的融合定位方法。

Images