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

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

【技术实现步骤摘要】
一种基于多传感器耦合的移动设备的定位方法


[0001]本专利技术涉及图像识别
，特别涉及一种基于多传感器耦合的移动设备的定位方法。

技术介绍

[0002]自主行走机器人要求稳定而精确的导航方法。由于使用单一种类传感器时，存在精度较低、稳定性不足等实际问题，同时使用多传感器定位的方法受到了较高的关注。常见的使用多传感器定位方法会结合各类传感器的优缺点，组合出一套实际可用的多传感器导航方法。比如：
[0003](1)使用多个固定种类的传感器，如使用激光雷达和IMU的组合，使用视觉传感器和激光雷达的组合等。
[0004](2)同时使用多个同类型传感器的耦合方法，典型的耦合方式如双目视觉导航、视觉和IMU同步导航、双激光同步导航、视觉和激光同步导航等方法，
[0005](3)同时使用多传感器的自主行走机器人一般使用SLAM(同时定位与建图)算法建立环境地图或类似于地图的位置标定表格等。
[0006]但这些方法也有实际的问题，比如方法(1)在产品迭代升级时很难改变传感器的使用方案，缺少灵活性；方法(2)的成本较高，稳定性较低，且通常需要通过较为复杂的传感器同步机制进行采集时间的同步，要求传感器同时正常工作。这降低了多传感器导航方法的稳定性。方法(3)只考虑到如何使用环境地图或已生成的位置标定表格，而不考虑如何生成地图，这对于使用了多传感器定位方法的自主移动机器人的实际部署带来了一定困难。
[0007]同时，定位的方式多种多样，如何综合利用现有的定位方式和传感器数据，取长补短，提高定位的精度，成为一个解决问题的方向。

技术实现思路

[0008]本专利技术目的是全自动化地使用多传感器而不限其种类和数量，也不限制定位方式，从而进行移动定位。首先设定移动设备上位姿来源(包括传感器和各种定位方式(如slam)中传感器)的类型、误差特性和大致位姿，然后通过微扰法在运行时实时求解各传感器或位姿来源中的传感器的精确位姿和时钟偏移，并通过图优化方法，综合位姿来源输出的位姿和移动设备动作特性进行准确地定位，并可将移动设备的位姿输出至环境地图优化方法，以便建立或调整环境地图。
[0009]为实现上述目的，根据本专利技术提供了一种基于多传感器耦合的移动设备的定位方法，包括：
[0010]S1、接收当前帧的信息，包括但不限于当前帧的位姿、参照物的位姿、当前帧和参照物的位姿关系、产生当前帧的位姿来源的类别，所述位姿来源相对于所述移动设备的位姿；
[0011]S2、根据所述位姿来源的类别，将所述当前帧的位姿和参照物的位姿作为顶点，将
所述当前帧和参照物的位姿关系作为条件边加入图优化；
[0012]S3、根据所述位姿来源的时钟偏移修正当前帧的时间，计算移动设备的位姿，然后将所述当前帧的位姿、移动设备的位姿作为顶点，将所述位姿来源相对移动设备的位姿作为边，加入所述图优化，将所述移动设备的位姿作为过往位姿和所述当前帧的时间保存到移动设备过往位姿集合；
[0013]S4、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，将过往各时刻的过往位姿和对应的理论位姿作为顶点，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对所述图优化进行迭代求解，获得图优化分数；
[0014]S5、优化所述位姿来源的时钟偏移和/或所述位姿来源相对于移动设备的位姿；
[0015]S6、根据移动设备的时间至位姿的函数，计算并输出当前时刻移动设备的位姿、所述位姿来源的时钟偏移或所述位姿来源相对于移动设备的位姿。
[0016]进一步的，在步骤S2中，如果所述当前帧和参照物的位姿为增量形式，则根据所述位姿来源的类别获取所述移动设备过往位姿集合中最近时刻的移动设备过往位姿作为初始值，计算所述当前帧的绝对位姿和参照物的绝对位姿，并将所述当前帧的绝对位姿和参照物的绝对位姿作为顶点，将所述当前帧的绝对位姿和参照物的绝对位姿的位姿关系作为条件边加入所述图优化。
[0017]进一步的，在步骤S4中，如果过往位姿和理论位姿的偏差大于预设的阈值，则撤销所述步骤S2-S4所作的修改，等待接收下一帧。
[0018]进一步的，在步骤S4中，所述拟合移动设备的时间至位姿的函数的方法为：对移动设备过往位姿集合，通过小车运动模型和扩展卡尔曼滤波方法，拟合最符合移动设备过往位姿的时间和位姿的函数。
[0019]进一步的，所述步骤S5中，优化所述位姿来源的时钟偏移的方法包括：
[0020]S51、修改所述位姿来源的时钟偏移；
[0021]S52、修改所述移动设备过往位姿集合中所有来源于所述位姿来源的过往位姿的时间；
[0022]S53、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，在所述图优化中修改各时刻过往位姿对应的理论位姿，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对图优化进行迭代求解，获得图优化分数；
[0023]S54、如果获得的图优化分数高于优化前获得的图优化分数，则保存图优化、所述位姿来源对应的时钟偏移、移动设备过往位姿集合、移动设备的时间至位姿的函数，并继续执行步骤S51～S54；
[0024]S55、如果获得的图优化分数低于修改前获得的图优化分数，则图优化、所述位姿来源对应的时钟偏移、移动设备过往位姿集合、移动设备的时间至位姿的函数恢复为优化前的数据。
[0025]进一步的，在所述步骤S51中，所述修改所述位姿来源的时钟偏移为：随机小幅修改时钟偏移，修改的幅度不超过预设的时间变化阈值。
[0026]进一步的，所述步骤S5中，优化所述位姿来源相对于移动设备的位姿的方法包括：
[0027]S511、修改所述位姿来源相对于所述移动设备的位姿；
[0028]S521、计算并修改所述移动设备过往位姿集合中和所述图优化中来源自所述位姿来源的移动设备的位姿；修改所述图优化中所述位姿来源相对移动设备的位姿；
[0029]S531、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，在所述图优化中修改各时刻过往位姿对应的理论位姿，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对图优化进行迭代求解，获得图优化分数；
[0030]S541、如果获得的图优化分数高于修改前获得的图优化分数，则保存图优化、所述位姿来源相对于所述移动设备的位姿、移动设备过往位姿集合、移动设备的时间至位姿的函数，并继续执行步骤S511～S541；
[0031]S551、如果获得的图优化分数低于修改前获得的图优化分数，则图优化、所述位姿来源相对于所述移动设备的位姿、移动设备过往位姿集合、移动设备的时间至位姿的函数恢复为优化前的数据。
[0032]进一步的，在所述步骤S511中，所述修改所述位姿来源相对于所述移动设备的位姿为：随机小幅修改，修改的本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于多传感器耦合的移动设备的定位方法，其特征在于，所述方法包括：S1、接收当前帧的信息，包括但不限于当前帧的位姿、参照物的位姿、当前帧和参照物的位姿关系、产生当前帧的位姿来源的类别，所述位姿来源相对于移动设备的位姿；S2、根据所述位姿来源的类别，将所述当前帧的位姿和参照物的位姿作为顶点，将所述当前帧和参照物的位姿关系作为条件边加入图优化；S3、根据所述位姿来源的时钟偏移修正当前帧的时间，计算移动设备的位姿，然后将所述当前帧的位姿、移动设备的位姿作为顶点，将所述位姿来源相对移动设备的位姿作为边，加入所述图优化，将所述移动设备的位姿作为过往位姿和所述当前帧的时间保存到移动设备过往位姿集合；S4、对移动设备过往位姿集合按时间排序，拟合移动设备的时间至位姿的函数，并计算过往各时刻移动设备的理论位姿，将过往各时刻的过往位姿和对应的理论位姿作为顶点，将过往各时刻的过往位姿和理论位姿重合作为条件边加入所述图优化；对所述图优化进行迭代求解，获得图优化分数；S5、优化所述位姿来源的时钟偏移和/或所述位姿来源相对于移动设备的位姿；S6、根据移动设备的时间至位姿的函数，计算并输出当前时刻移动设备的位姿、所述位姿来源的时钟偏移或所述位姿来源相对于移动设备的位姿。2.根据权利要求1所述的定位方法，其特征在于，在步骤S2中，如果所述当前帧和参照物的位姿为增量形式，则根据所述位姿来源的类别获取所述移动设备过往位姿集合中最近时刻的移动设备过往位姿作为初始值，计算所述当前帧的绝对位姿和参照物的绝对位姿，并将所述当前帧的绝对位姿和参照物的绝对位姿作为顶点，将所述当前帧的绝对位姿和参照物的绝对位姿的位姿关系作为条件边加入所述图优化。3.根据权利要求1所述的定位方法，其特征在于，在步骤S4中，如果过往位姿和理论位姿的偏差大于预设的阈值，则撤销所述步骤S2-S4所作的修改，等待接收下一帧。4.根据权利要求1所述的定位方法，其特征在于，在步骤S4中，所述拟合移动设备的时间至位姿的函数的方法为：对移动设备过往位姿集合，通过小车运动模型和扩展卡尔曼滤波方法，拟合最符合移动设备过往位姿的时间和位姿的函数。5.根据权利要求1所述的定位方法，其特征在于，所述步骤S5中，优化所述位姿来源的时钟偏移的方法包括：S51、修改所述位姿来源的时钟偏移；S52、修改所述...

【专利技术属性】
技术研发人员：刘诗聪，
申请(专利权)人：上海懒书智能科技有限公司，
类型：发明
国别省市：