[发明专利]一种基于多传感器耦合的移动设备的定位方法

权利要求书

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