CN112325872B - 一种基于多传感器耦合的移动设备的定位方法 - Google Patents

Abstract

本发明涉及一种基于多传感器耦合的移动设备的定位方法，包括步骤：S1、接收当前帧的信息，S2、生成图优化；S3、根据位姿来源对应的时钟偏移修正当前帧的时间，计算移动设备的位姿；S4、将移动设备的位姿和相应的时刻保存到移动设备过往位姿集合；获取移动设备的时间至位姿的函数，求解获得图优化分数；S5、优化时钟偏移和位姿；S6、根据移动设备的时间至位姿的函数，计算并输出当前时刻移动设备的位姿、位姿来源的类别对应的时钟偏移或位姿来源相对于所述移动设备的位姿。通过本发明，可以综合各传感器输出的位姿和小车动作特性进行准确地定位，并可将位姿至环境地图优化方法，以便建立或调整环境地图。

Description

一种基于多传感器耦合的移动设备的定位方法

技术领域

本发明涉及图像识别技术领域，特别涉及一种基于多传感器耦合的移动设备的定位方法。

背景技术

自主行走机器人要求稳定而精确的导航方法。由于使用单一种类传感器时，存在精度较低、稳定性不足等实际问题，同时使用多传感器定位的方法受到了较高的关注。常见的使用多传感器定位方法会结合各类传感器的优缺点，组合出一套实际可用的多传感器导航方法。比如：

(1)使用多个固定种类的传感器，如使用激光雷达和IMU的组合，使用视觉传感器和激光雷达的组合等。

(2)同时使用多个同类型传感器的耦合方法，典型的耦合方式如双目视觉导航、视觉和IMU同步导航、双激光同步导航、视觉和激光同步导航等方法，

(3)同时使用多传感器的自主行走机器人一般使用SLAM(同时定位与建图)算法建立环境地图或类似于地图的位置标定表格等。

但这些方法也有实际的问题，比如方法(1)在产品迭代升级时很难改变传感器的使用方案，缺少灵活性；方法(2)的成本较高，稳定性较低，且通常需要通过较为复杂的传感器同步机制进行采集时间的同步，要求传感器同时正常工作。这降低了多传感器导航方法的稳定性。方法(3)只考虑到如何使用环境地图或已生成的位置标定表格，而不考虑如何生成地图，这对于使用了多传感器定位方法的自主移动机器人的实际部署带来了一定困难。

同时，定位的方式多种多样，如何综合利用现有的定位方式和传感器数据，取长补短，提高定位的精度，成为一个解决问题的方向。

发明内容

本发明目的是全自动化地使用多传感器而不限其种类和数量，也不限制定位方式，从而进行移动定位。首先设定移动设备上位姿来源(包括传感器和各种定位方式(如slam)中传感器)的类型、误差特性和大致位姿，然后通过微扰法在运行时实时求解各传感器或位姿来源中的传感器的精确位姿和时钟偏移，并通过图优化方法，综合位姿来源输出的位姿和移动设备动作特性进行准确地定位，并可将移动设备的位姿输出至环境地图优化方法，以便建立或调整环境地图。

为实现上述目的，根据本发明提供了一种基于多传感器耦合的移动设备的定位方法，包括：

S1、接收当前帧的信息，包括但不限于当前帧的位姿、参照物的位姿、当前帧和参照物的位姿关系、产生当前帧的位姿来源的类别，所述位姿来源相对于所述移动设备的位姿；

S2、根据所述位姿来源的类别，将所述当前帧的位姿和参照物的位姿作为顶点，将所述当前帧和参照物的位姿关系作为条件边加入图优化；

S3、根据所述位姿来源的时钟偏移修正当前帧的时间，计算移动设备的位姿，然后将所述当前帧的位姿、移动设备的位姿作为顶点，将所述位姿来源相对移动设备的位姿作为边，加入所述图优化，将所述移动设备的位姿作为过往位姿和所述当前帧的时间保存到移动设备过往位姿集合；

S4、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，将过往各时刻的过往位姿和对应的理论位姿作为顶点，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对所述图优化进行迭代求解，获得图优化分数；

S5、优化所述位姿来源的时钟偏移和/或所述位姿来源相对于移动设备的位姿；

S6、根据移动设备的时间至位姿的函数，计算并输出当前时刻移动设备的位姿、所述位姿来源的时钟偏移或所述位姿来源相对于移动设备的位姿。

进一步的，在步骤S2中，如果所述当前帧和参照物的位姿为增量形式，则根据所述位姿来源的类别获取所述移动设备过往位姿集合中最近时刻的移动设备过往位姿作为初始值，计算所述当前帧的绝对位姿和参照物的绝对位姿，并将所述当前帧的绝对位姿和参照物的绝对位姿作为顶点，将所述当前帧的绝对位姿和参照物的绝对位姿的位姿关系作为条件边加入所述图优化。

进一步的，在步骤S4中，如果过往位姿和理论位姿的偏差大于预设的阈值，则撤销所述步骤S2-S4所作的修改，等待接收下一帧。

进一步的，在步骤S4中，所述拟合移动设备的时间至位姿的函数的方法为：对移动设备过往位姿集合，通过小车运动模型和扩展卡尔曼滤波方法，拟合最符合移动设备过往位姿的时间和位姿的函数。

进一步的，所述步骤S5中，优化所述位姿来源的时钟偏移的方法包括：

S51、修改所述位姿来源的时钟偏移；

S52、修改所述移动设备过往位姿集合中所有来源于所述位姿来源的过往位姿的时间；

S53、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，在所述图优化中修改各时刻过往位姿对应的理论位姿，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对图优化进行迭代求解，获得图优化分数；

S54、如果获得的图优化分数高于优化前获得的图优化分数，则保存图优化、所述位姿来源对应的时钟偏移、移动设备过往位姿集合、移动设备的时间至位姿的函数，并继续执行步骤S51～S54；

S55、如果获得的图优化分数低于修改前获得的图优化分数，则图优化、所述位姿来源对应的时钟偏移、移动设备过往位姿集合、移动设备的时间至位姿的函数恢复为优化前的数据。

进一步的，在所述步骤S51中，所述修改所述位姿来源的时钟偏移为：随机小幅修改时钟偏移，修改的幅度不超过预设的时间变化阈值。

进一步的，所述步骤S5中，优化所述位姿来源相对于移动设备的位姿的方法包括：

S511、修改所述位姿来源相对于所述移动设备的位姿；

S521、计算并修改所述移动设备过往位姿集合中和所述图优化中来源自所述位姿来源的移动设备的位姿；修改所述图优化中所述位姿来源相对移动设备的位姿；

S531、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，在所述图优化中修改各时刻过往位姿对应的理论位姿，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对图优化进行迭代求解，获得图优化分数；

S541、如果获得的图优化分数高于修改前获得的图优化分数，则保存图优化、所述位姿来源相对于所述移动设备的位姿、移动设备过往位姿集合、移动设备的时间至位姿的函数，并继续执行步骤S511～S541；

S551、如果获得的图优化分数低于修改前获得的图优化分数，则图优化、所述位姿来源相对于所述移动设备的位姿、移动设备过往位姿集合、移动设备的时间至位姿的函数恢复为优化前的数据。

进一步的，在所述步骤S511中，所述修改所述位姿来源相对于所述移动设备的位姿为：随机小幅修改，修改的幅度不超过预设的位姿变化阈值。

使用本发明的益处有：

(1)可以全自动地使用多传感器或多中定位方式同时计算位姿。在给定了初始传感器布局后，本方法能够全自动地计算传感器的时钟漂移和精确布局，从而降低了部署应用难度，提高了使用多传感器同时定位的普及程度。

(2)该多传感器同时计算位姿的方案中，各传感器间无需通信，没有相互之间的数据依赖，可以在一个或多个传感器失效或错解时仍然计算出正确的位姿。因此提高了定位的稳定性。

(3)能够优化调节传感器所使用的地图标定数据，能够关联不同种类的传感器所使用的地图标定数据，从而提高单项定位位姿来源传感器的定位精度和稳定性。

附图说明

为了更清楚地说明本公开的实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些图获得其他的附图。

图1为根据本发明一个实施例的一种基于多传感器耦合的移动设备的定位方法的流程示意图；

图2为根据本发明一个实施例的优化时钟偏移的方法的流程示意图；

图3为根据本发明一个实施例的优化位姿来源相对于移动设备的位姿的方法的流程示意图；

图4位根据本发明一个实施例的图优化所包含的边和顶点的示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明中，术语“帧”是传感器当前获取的数据，如拍照得到的图片、扫描的点云、测量的重力加速度等。位姿包括横纵坐标和角度，也可以包括垂直坐标和俯仰横滚。

下面结合附图和具体实施方式对本发明作进一步说明。

本发明提供一种基于多传感器耦合的移动设备的定位方法，如图1所示，包括如下步骤：

S1、接收当前帧的信息，包括但不限于当前帧的位姿、参照物的位姿、当前帧和参照物的位姿关系、产生当前帧的位姿来源的类别，位姿来源相对于移动设备的位姿；

S2、根据位姿来源的类别，将当前帧的位姿和参照物的位姿作为顶点，将当前帧和参照物的位姿关系作为条件边加入图优化；

S3、根据位姿来源的时钟偏移修正当前帧的时间，计算移动设备的位姿，然后将当前帧的位姿、移动设备的位姿作为顶点，将位姿来源相对移动设备的位姿作为边，加入图优化，将移动设备的位姿作为过往位姿和当前帧的时间保存到移动设备过往位姿集合；

S4、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，将过往各时刻的过往位姿和对应的理论位姿作为顶点，将过往各时刻的过往位姿和理论位姿重合作为条件边加入图优化；对图优化进行迭代求解，获得图优化分数；

S5、优化位姿来源的时钟偏移和/或位姿来源相对于移动设备的位姿；

S6、根据移动设备的时间至位姿的函数，计算并输出当前时刻移动设备的位姿、位姿来源的时钟偏移或位姿来源相对于移动设备的位姿。

在步骤S1中，本方法可耦合复合以下条件的任意传感器或定位方式(以下称为位姿来源)，即接收位姿来源发送的当前帧的信息。当前帧的信息包括但不限于当前帧的位姿、参照物的位姿、当前帧与参照物的位姿关系、产生当前帧的位姿来源的类别，位姿来源相对于移动设备的位姿。位姿来源的类别不同，参照物的内容不同，具体如下所述。

(1)位姿来源类别1

这种类别的位姿来源，其本身开机后立刻可根据自身观察数据做里程计。这类定位方式包括基于激光雷达的里程计、地面纹理导航、基于轮编码器的里程计、RGBD视觉里程计等。这类定位方式可连续地比较上一帧(即参照物)和当前帧获取的信息，确定两帧的位姿关系，从而进行里程计计算。

(2)位姿来源类别2

这种类别的位姿来源，其本身开机时无法直接根据自身观察数据做里程计，但经过初始化后，可根据自身观察数据做里程计。这类定位方式包括单目视觉导航、IMU导航等。这类定位方式需要一个初始信息，如初始速度、前若干秒的初始位移等后，随后可通过连续地比较参考帧(即参照物)和当前帧获取的信息，确定两帧的位姿关系，从而进行里程计计算。如：IMU只能观察到加速度，因此导航需要给出一个初始的速度大小，才能够通过累加观察到的加速度计算出当前的速度，并进一步推算出当前的位姿。

(3)位姿来源类别3

这种类别的位姿来源，其本身(如传感器)只能对位姿做有限的解算。这类定位方式包括磁条导航、色带导航、使用单轴IMU进行方向跟踪等。这类定位方式无法获取完整的位姿，如只有偏移量但不知道前进量等。这类定位方式可给出上一帧(即参照物)和当前帧的部分位姿关系：如单轴IMU能够得到上一帧和当前帧的角度关系，色带导航、磁条导航能够得到上一帧和当前帧的横向位移关系等。

(4)位姿来源类别4

这种类别的位姿来源，其本身可直接获得位姿信息。这类定位方式包括二维码定位、GNSS卫星定位、UWB微波定位、基于RFID的定位等。这类定位方式可直接获得位姿，即直接根据当前帧确定当前帧和一个参考点的位姿关系。

在步骤S2中，如果位姿来源是类型1：那么將位姿来源的参考帧和当前帧的位置关系作为一个条件边、参考帧位姿和当前帧位姿作为顶点，加入图优化。

比如，激光里程计使用上一帧扫描的点云和当前帧扫描的点云做配准，则将当前帧和上一帧的位置关系作为一个条件加入图优化。再比如，如果使用了激光SLAM方法进行定位，那么当前帧若能和某一地图上位置进行匹配，那么两者的位姿关系作为条件边、当前帧的位姿和被匹配上的地图上的点的位姿作为顶点，加入图优化。

如果位姿来源是类型2，将过往位置作为初始化类型2位姿来源的初始化信息，初始化并重新计算参考帧和当前帧的绝对位姿，然后将参考帧和当前帧的绝对位姿作为顶点，参考帧和当前帧的绝对位姿的位姿关系作为一个条件边加入图优化。比如：IMU导航使用重力加速度和角速度进行速度累加计算位姿，因此需要给出初始速度才能够进行位姿计算。在IMU位姿来源上报数据后，根据过往位姿计算出小车在这一段时间内的速度，从而初始化IMU并重新计算参考帧和当前帧的位姿，并将参考帧和当前帧的位姿关系加入图优化。

如果位姿来源是类型3，那么将该位姿来源的参照物和当前帧的位姿约束作为一个条件边，将参照物位姿和当前帧位姿作为顶点，加入图优化。比如：磁钉导航中，小车只知道磁传感器的某个位置接触到了磁钉，但并不知道方向如何，则对应的在图优化中增加一个约束，即只约束小车磁传感器和磁钉的位移关系，但不约束方向关系。

如果位姿来源是类型4，那么将位姿来源的参考点(即参照物)的位姿和当前帧的位姿作为顶点，两者的位置关系作为一个条件边加入图优化。比如：二维码定位中，当前帧能够和地图上某一个二维码匹配上，那么两者的地图上二维码和当前帧的位姿关系作为一个条件加入图优化。这样做之后，通过后面多次的图优化，可以对二维码地图进行精细地修正，从而解决人工张贴二维码不准确的问题。

图优化是一类算法。该算法将事物的位姿状态当作位姿顶点，位姿间变换关系约束当做条件边，然后调整各位姿顶点位置使其尽量满足条件边的约束。在本发明中，节点(顶点)可以是当前帧位姿、参考帧位姿、参考点位姿、小车过往位姿等。具体的图优化中的术语在此不一一介绍。

在步骤S3中，修正当前帧的时间：目的是将各传感器的时间统一，消除误差。各传感器之间采集的帧率是不同的，且采样所使用的时钟也是不同的：即不仅节拍不同，其时钟也或快或慢若干时间。本发明对每个位姿来源维护一个误差修正量(即时钟偏移)，并将该位姿来源报告的当前帧的时间加上该位姿来源误差修正量作为当前帧的真实时间。

根据当前帧的位姿和位姿来源相对于移动设备的位姿，通过几何关系换算获得当前帧真实时间下移动设备的位姿。具体的换算是现有技术，不再具体描述。然后将当前帧的位姿、移动设备的位姿作为顶点，将位姿来源相对移动设备的位姿作为边(也称为刚体边)加入图优化，将移动设备的位姿作为过往位姿和对应的当前帧的时间保存到移动设备过往位姿集合。

在步骤S4中，对移动设备过往位姿按照时间排序，然后对移动设备过往位姿应用小车运动模型(如，差速运动小车)和扩展卡尔曼滤波方法，拟合出最符合移动设备过往位姿的时间和位姿的函数：即在某时刻下最符合移动设备运动模型的位姿(称为理论位姿)。对该函数应用移动设备过往位姿中各位姿的时间，计算出各过往位姿时间下移动设备在此时间下的理论位姿(可能和观察到的移动设备过往位姿不同)。将理论位姿作为顶点，将过往各时刻的过往位姿和理论位姿重合作为条件边加入图优化(此时，图优化中已存在过往位姿，图优化中的顶点和边如图4所示)；对图优化进行迭代求解，获得图优化分数，该分数指示加入图优化的条件被满足的程度，可以以每个帧的平均位姿方差为标准。迭代求解可以使用现有的工具，如g2o。

优选的，如果计算出某个过往位姿和理论位姿偏差非常大(可以认为设定一个阈值)，表示对应的过位姿来源可能暂时失效了，那么撤销上面步骤S2-S4所作的修改(比如撤销步骤S2-S4中对图优化、移动设备过往位姿集合等做出的改变)，等待接收下一帧，即重新从步骤S1开始。

在步骤S5中，可以对该位姿来源的时钟偏移进行优化，如图2所示，具体包括如下步骤：

S51、修改位姿来源的时钟偏移；可以随机小幅修改时钟偏移，修改的幅度不超过预设的时间变化阈值，比如为该位姿来源的时钟偏移改变量设定时间变化阈值，然后随机产生一个绝对值在0～时间变化阈值内的数作为该位姿来源的时钟偏移：比如原来的时钟偏移是1s，阈值为0.1s，随机产生一个变化量0.05s，那么时钟偏移就变成了1.05s；

S52、修改移动设备过往位姿集合中所有来源于该位姿来源的过往位姿的时间；比如，源自该位姿来源的过往位姿的时间都增加0.05s；

S53、对移动设备过往位姿集合按时间排序，然后拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，在图优化中修改各时刻过往位姿对应的理论位姿，将过往各时刻的过往位姿和理论位姿重合作为条件边加入图优化；对图优化进行迭代求解，获得图优化分数；

S54、如果获得的图优化分数高于修改前获得的图优化分数，则保存图优化、所述位姿来源对应的时钟偏移、移动设备过往位姿集合、移动设备的时间至位姿的函数，并继续执行步骤S51～S54；

S55、如果获得的图优化分数低于修改前获得的图优化分数，则图优化、所述位姿来源对应的时钟偏移、移动设备过往位姿集合、移动设备的时间至位姿的函数恢复为优化前的数据。

也就是说，如果图优化分数有所提高，说明这样的调整是正确的，所以要保留所作的调整以及调整后计算得到的各种结果(比如图优化、函数)；如果分数没有提高，则没有必要做这样的调整，图优化、过往位姿集合等都无需改变，也就是需要恢复为调整前的状态和数值。移动设备运行过程中，每次获得位姿数据，就进行这种调整，这样位姿来源的时钟偏差的调整被分摊到整个定位环节中，虽然每次修正幅度较小，但仍然能够在经过一段时间的修正后达到最优解。此外由于修正分摊到整个定位环节中，能够降低计算力要求。

在步骤S5中，可以对该位姿来源的位姿进行优化，如图3所示，具体包括如下步骤：

S511、修改所述位姿来源相对于所述移动设备的位姿；比如为该位姿来源相对于移动设备的位姿都设定位姿变化阈值，然后随机产生一个处于位姿变化阈值内的位姿作为该位姿来源相对于移动设备的位姿调整量；

S521、计算并修改所述移动设备过往位姿集合中和图优化中来源自所述位姿来源的移动设备的位姿；修改图优化中所述位姿来源相对移动设备的位姿，即将所有来源自本位姿来源的过往位姿对应的刚体边作指定的小幅度的位姿关系调整。

S531、对移动设备过往位姿集合按时间排序，然后拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，在图优化中修改各时刻过往位姿对应的理论位姿，将过往各时刻的过往位姿和理论位姿重合作为条件边加入图优化；对图优化进行迭代求解，获得图优化分数；

S541、如果获得的图优化分数高于修改前获得的图优化分数，则保存图优化、所述位姿来源相对于所述移动设备的位姿、移动设备过往位姿集合、移动设备的时间至位姿的函数，并继续执行步骤S511～S541；

S551、如果获得的图优化分数低于修改前获得的图优化分数，则图优化、所述位姿来源相对于所述移动设备的位姿、移动设备过往位姿集合、移动设备的时间至位姿的函数恢复为优化前的数据。

同理，如果修改后获得的图优化分数高于修改前获得的图优化分数，则说明调整有效，保留修改以及得到的图优化、位姿来源相对于移动设备的位姿、移动设备过往位姿集合、移动设备的时间至位姿的函数。同样的，移动设备运行过程中，每次获得位姿数据，就进行这种调整，这样调整后，位姿来源相对移动设备的位姿的调整被分摊到整个定位环节中，虽然每次修正幅度较小，但仍然能够在经过一段时间的修正后达到最优解。此外由于修正分摊到整个定位环节中，能够降低计算力要求。

在一个实施例中，上面对位姿来源的时钟偏移的微调优化和对位姿来源相对于移动设备的位姿的微调优化，可以单独进行也可以交叉进行。

在步骤S6中，根据上面获得移动设备的时间至位姿的函数，计算并输出当前时刻移动设备的位姿、位姿来源对应的时钟偏移或位姿来源相对于所述移动设备的位姿。当前时刻移动设备的位姿可以用于移动设备的导航定位、运动控制、动作规划等，位姿来源对应的时钟偏移和位姿来源相对于所述移动设备的位姿，用于提高所输出的移动设备的位姿的精度，并提高移动设备位姿输出的稳定性。

根据本发明，可以全自动地使用多传感器同时计算位姿。比如，给定了初始传感器布局后，本方法能够全自动地计算传感器的时钟漂移和精确布局，从而降低了部署应用难度，提高多传感器同时定位的普及能力。

根据本发明，多传感器间无需通信，没有数据依赖。可以在一个或多个传感器失效或错解时仍然计算出正确的位姿。因此整体提高了定位的稳定性。

根据本发明，能够优化调节传感器所使用的地图标定数据，能够关联不同种类的传感器所使用的地图标定数据，从而提高单项定位来源传感器的定位精度和稳定性。如：通过本方法耦合的激光SLAM定位和二维码定位，可以将二维码标记点和激光SLAM地图通过图优化方法进行相互参考、关联计算，从而修正二维码标记点和激光SLAM地图的误差。具体过程如下：激光SLAM定位中某个地图位置(即参照物)在图优化中作为顶点，并通过一条边连接到一个小车过往位姿，小车过往位姿连接了一个理论位姿；同理，二维码标记点(即参照物)位姿连接到一个小车过往位姿，该小车过往位姿连接了一个理论位姿，理论位姿通过卡尔曼滤波相互优化，从而使得二维码标记点和激光SLAM定位中某个地图位置可通过图优化方式互相影响位姿，从而达到修正二维码标记点和激光SLAM地图的目的。如：人工铺贴的二维码具有一定误差。同时搭载了激光里程计和二维码导航的小车从一个二维码走到另一个二维码时，二维码所给出的位姿和激光里程计计算出来的位姿会有一定差异。本方法中，首先会通过应用小车运动模型和扩展卡尔曼滤波方法计算理论位姿、然后通过图优化修改观察位姿，使得观察位姿尽可能和理论位姿重合。由于观察位姿和二维码位姿也在图优化中作了关联，因此二维码位姿也会得到修正，从而达到修正人工铺贴二维码的位置误差的效果。

本领域普通技术人员可以理解：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明权利要求所限定的范围。

Claims (6)

1.一种基于多传感器耦合的移动设备的定位方法，其特征在于，所述方法包括：

S1、接收当前帧的信息，包括但不限于当前帧的位姿、参照物的位姿、当前帧和参照物的位姿关系、产生当前帧的位姿来源的类别，所述位姿来源相对于移动设备的位姿；

S2、根据所述位姿来源的类别，将所述当前帧的位姿和参照物的位姿作为顶点，将所述当前帧和参照物的位姿关系作为条件边加入图优化；

S3、根据所述位姿来源的时钟偏移修正当前帧的时间，计算移动设备的位姿，然后将所述当前帧的位姿、移动设备的位姿作为顶点，将所述位姿来源相对移动设备的位姿作为边，加入所述图优化，将所述移动设备的位姿作为过往位姿和所述当前帧的时间保存到移动设备过往位姿集合；

S4、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，将过往各时刻的过往位姿和对应的理论位姿作为顶点，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对所述图优化进行迭代求解，获得图优化分数；

S5、优化所述位姿来源的时钟偏移和/或所述位姿来源相对于移动设备的位姿；

S6、根据移动设备的时间至位姿的函数，计算并输出当前时刻移动设备的位姿、所述位姿来源的时钟偏移或所述位姿来源相对于移动设备的位姿；

所述拟合移动设备的时间至位姿的函数的方法为：对移动设备过往位姿集合，通过小车运动模型和扩展卡尔曼滤波方法，拟合最符合移动设备过往位姿的时间和位姿的函数；

优化所述位姿来源的时钟偏移的方法包括：

S51、修改所述位姿来源的时钟偏移；

S52、修改所述移动设备过往位姿集合中所有来源于所述位姿来源的过往位姿的时间；

S53、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，在所述图优化中修改各时刻过往位姿对应的理论位姿，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对图优化进行迭代求解，获得图优化分数；

S54、如果获得的图优化分数高于优化前获得的图优化分数，则保存图优化、所述位姿来源对应的时钟偏移、移动设备过往位姿集合、移动设备的时间至位姿的函数，并继续执行步骤S51～S54；

S55、如果获得的图优化分数低于修改前获得的图优化分数，则图优化、所述位姿来源对应的时钟偏移、移动设备过往位姿集合、移动设备的时间至位姿的函数恢复为优化前的数据。

2.根据权利要求1所述的定位方法，其特征在于，在步骤S2中，如果所述当前帧和参照物的位姿为增量形式，则根据所述位姿来源的类别获取所述移动设备过往位姿集合中最近时刻的移动设备过往位姿作为初始值，计算所述当前帧的绝对位姿和参照物的绝对位姿，并将所述当前帧的绝对位姿和参照物的绝对位姿作为顶点，将所述当前帧的绝对位姿和参照物的绝对位姿的位姿关系作为条件边加入所述图优化。

3.根据权利要求1所述的定位方法，其特征在于，在步骤S4中，如果过往位姿和理论位姿的偏差大于预设的阈值，则撤销所述步骤S2-S4所作的修改，等待接收下一帧。

4.根据权利要求1所述的定位方法，其特征在于，所述步骤S5中，优化所述位姿来源相对于移动设备的位姿的方法包括：

S511、修改所述位姿来源相对于所述移动设备的位姿；

S521、计算并修改所述移动设备过往位姿集合中和所述图优化中来源自所述位姿来源的移动设备的位姿；修改所述图优化中所述位姿来源相对移动设备的位姿；

S531、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，在所述图优化中修改各时刻过往位姿对应的理论位姿，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对图优化进行迭代求解，获得图优化分数；

S541、如果获得的图优化分数高于修改前获得的图优化分数，则保存图优化、所述位姿来源相对于所述移动设备的位姿、移动设备过往位姿集合、移动设备的时间至位姿的函数，并继续执行步骤S511～S541；

S551、如果获得的图优化分数低于修改前获得的图优化分数，则图优化、所述位姿来源相对于所述移动设备的位姿、移动设备过往位姿集合、移动设备的时间至位姿的函数恢复为优化前的数据。

5.根据权利要求1所述的定位方法，其特征在于，在所述步骤S51中，所述修改所述位姿来源的时钟偏移为：随机小幅修改时钟偏移，修改的幅度不超过预设的时间变化阈值。

6.根据权利要求4所述的定位方法，其特征在于，在所述步骤S511中，所述修改所述位姿来源相对于所述移动设备的位姿为：随机小幅修改，修改的幅度不超过预设的位姿变化阈值。

Images