一种多传感器融合定位系统的位姿优化方法和装置与流程

1.本发明涉及高精定位技术领域，特别涉及一种多传感器融合定位系统的位姿优化方法和装置。


背景技术：

2.多传感器融合定位系统一般是指包括视觉传感器、惯性测量单元(inertial measurement unit，imu，简称“惯导”)和全球导航卫星系统((global navigation satellite system，gnss)等的定位系统，兼具定位精度高和成本低的优势，所以多传感器融合是高精度定位的重要选择。而通过多传感器融合提供全场景、高精度和多源融合的精准定位信息过程中，各传感器位置和姿态的优化精度对后续定位的精度起到至关重要的作用。
3.多传感器融合定位系统的位姿优化的现有技术一般包括：
4.(1)vins_mono算法
5.仅加入回环检测的累积误差校正，存在定位误差累计漂移的问题。
6.(2)vi-orb视觉惯导初始化算法
7.在位姿图优化pose graph的基础上进行局部的bundle adjustment，首先未加入全球定位系统的绝对位置信息，且如果加入绝对信息，需要足够的窗口大小，此时进行bundle adjustment优化，会消耗过大的计算资源。
8.(3)传统的pose graph方案
9.仅优化相对位姿信息和全球定位系统的绝对位置信息，世界坐标系旋转矩阵是通过最新帧恢复的，当最新帧的融合位姿或者原始观测值不准时，容易导致世界坐标系旋转矩阵不准，进而导致优化不稳定；且通过不准确的世界系旋转矩阵解算的初始地图位姿也会不准，容易使算法陷入局部优化。
10.综上所述，现有技术中多传感器融合定位系统的位姿优化存在误差累计、局部优化等精度低和优化耗时长的问题。


技术实现要素：

11.鉴于上述问题，提出了本发明以便提供一种克服上述问题或者至少部分地解决上述问题的一种多传感器融合定位系统的位姿优化方法和装置。
12.第一方面，本发明实施例提供一种多传感器融合定位系统的位姿优化方法，包括：
13.将获取到的预设时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿；
14.基于所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到所述第二位姿的优化值和旋转外参的优化值，所述旋转外参的优化值用于下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合。
15.在一些可选的实施例中，所述对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到所述第二位姿的优化值和旋转外参的优化值，具体包括：
16.迭代优化所述第二位姿中的第二位置和第二航向角及所述旋转外参，得到所述第二位置的优化值、第二航向角的优化值和旋转外参的优化值。
17.在一些可选的实施例中，所述基于所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，具体包括：
18.根据所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，建立下述至少一项残差，将建立的各残差组合成目标残差：
19.所述第一位姿中的第一位置与所述第一位姿转换到绝对世界坐标系下的第二位姿中的第二位置的相对位置变化残差；
20.所述第一位姿中的第一姿态与所述第一位姿转换到绝对世界坐标系下的第二位姿中的第二姿态的相对姿态变化残差；
21.所述第一姿态的绝对姿态转换残差；
22.全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差；
23.全球定位系统在绝对世界坐标系下的观测航向的绝对姿态残差。
24.在一些可选的实施例中，所述相对位置变化残差，通过下述步骤建立：
25.将相邻两帧图像拍摄时刻的第一位置的差值，根据所述两帧中的前一帧图像拍摄时刻惯导坐标系与相对世界坐标系的旋转外参，转换成所述前一帧图像拍摄时刻惯导坐标系下的第一差值；
26.根据所述两帧图像拍摄时刻的待优化的第二位姿中的第二位置的差值与所述前一帧图像拍摄时刻第二位姿中的第二姿态，确定所述前一帧图像拍摄时刻惯导坐标系下的第二差值；
27.将所述第一差值与所述第二差值间的差值确定为相对位置变化残差。
28.在一些可选的实施例中，所述相对姿态变化残差，通过下述步骤建立：
29.根据相邻两帧图像拍摄时刻的待优化的第二位姿中的第二姿态，和分别在所述两帧图像拍摄时刻惯导坐标系与相对世界坐标系的旋转外参，确定相对姿态变化残差。
30.在一些可选的实施例中，所述第一姿态的绝对姿态变化残差，通过下述步骤建立：
31.根据惯导坐标系与相对世界坐标系的旋转外参、待优化的第二位姿中的第二姿态和相对世界坐标系与绝对世界坐标系间的旋转外参，确定所述第一姿态的绝对姿态变化残差。
32.在一些可选的实施例中，全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差，通过下述步骤建立：
33.根据全球定位系统在绝对世界坐标系下的观测位置、待优化的第二位姿中的第二位置和第二姿态，及惯导坐标系与绝对世界坐标系间的旋转外参，确定全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差。
34.在一些可选的实施例中，全球定位系统在绝对世界坐标系下的观测航向的绝对姿态残差，通过下述步骤建立：
35.根据全球定位系统在绝对世界坐标系下的观测航向和待优化的第二位姿中的第二姿态，确定全球定位系统在绝对世界坐标系下的观测航向的绝对姿态残差。
36.在一些可选的实施例中，所述基于所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化前，还包括：
37.判断视觉传感器拍摄的图像数据与惯导测量数据融合的次数是否达到预设次数；或，
38.判断当前获取到的第一位姿对应的帧数是否达到预设帧数。
39.第二方面，本发明实施例提供一种多传感器融合定位系统的位姿优化装置，包括：
40.融合模块，用于将获取到的预设时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿；
41.优化模块，用于根据所述融合模块得到的第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到所述第二位姿的优化值和旋转外参的优化值，所述旋转外参的优化值用于下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合。
42.第三方面，本发明实施例提供一种计算机可读存储介质，其上存储有计算机指令，当该指令被处理器执行时实现上述多传感器融合定位系统的位姿优化方法。
43.第四方面，本发明实施例提供一种服务器，包括：存储器、处理器及存储于存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述多传感器融合定位系统的位姿优化方法。
44.发明实施例提供的多传感器融合定位系统的位姿优化方法，将获取到的预设时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿；基于述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到第二位姿的优化值和旋转外参的优化值，旋转外参的优化值用于下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合。该技术方案的有益效果至少包括：
45.(1)内层视觉惯性里程计(visual-inertial odometry，vio)进行视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿，外层引入全球定位系统在世界坐标系下的观测位置和观测航向为约束，在第一位姿的基础上优化得到第二位姿和旋转外参的优化值，解决了局部优化导致的精度不准的问题；将旋转外参的优化值输入下一轮的内层vio融合，提升了内层vio输出的第一位姿的精度，解决了相对世界坐标系与绝对世界坐标系间的旋转外参估计不准导致的优化不稳定的问题；并且外层引入全球定位系统在世界坐标系下的观测位置和观测航向为约束可解决内层因为漂移引起的误差累积问题，从而提升第二位姿精度。
46.(2)内层vio融合为紧耦合，优化的参数和残差多，计算量大；外层的位姿优化为松耦合，每帧优化的参数少残差相对也少，计算量小。故内外层分开的双层优化方案，较所有优化工作都在内层完成减小了计算量。
47.本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
48.下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
49.附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
50.图1为本发明实施例一中多传感器融合定位系统的位姿优化方法流程图；
51.图2为本发明实施例一中多传感器融合定位系统的位姿优化方法示例图；
52.图3为本发明实施例二中目标残差建立方法的具体实现流程图；
53.图4为图3中步骤s31的具体实现流程图；
54.图5为本发明实施例中因子图优化原理的示意图；
55.图6为本发明实施例中多传感器融合定位系统的位姿优化装置结构示意图。
具体实施方式
56.下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。
57.为了解决现有技术中存在的多传感器融合定位系统的位姿优化精度低且耗时长的问题，本发明实施例提供一种多传感器融合定位系统的位姿优化方法和装置，能够快速准确的对多传感器融合定位系统进行位姿优化，且计算量小。
58.实施例一
59.本发明实施例一提供一种多传感器融合定位系统的位姿优化方法，其流程如图1所示，包括如下步骤：
60.步骤s11：将获取到的预设时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿。
61.根据获取到的视觉传感器拍摄的每帧图像的拍摄时间，将惯导测量数据与图像数据进行时间差分对齐后，将惯导测量数据与图像数据进行融合得到相对世界坐标系下的第一位姿，具体的，第一位姿可以是惯导的位姿，也可以是视觉传感器的位姿，二者可以通过惯导与觉传感器间的旋转外参灵活转换。
62.具体的，第一位置包括相对世界坐标系下的x、y和z坐标的值；第一姿态包括航向角yaw、翻滚角roll和俯仰角pitch。
63.图像数据与惯导测量数据的融合可以是采用现有技术，具体融合方法本实施例不做限定。
64.步骤s12：基于第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到第二位姿的优化值和旋转外参的优化值。
65.上述相对世界坐标系也可以理解为初始世界坐标系，是姿态优化前的世界坐标系，更具体的，可以是航向角优化前的坐标系；绝对世界坐标系为姿态优化后的坐标系。
66.在一个实施例中，可以包括，迭代优化第二位姿中的第二位置和第二航向角及旋转外参，得到第二位置的优化值、第二航向角的优化值和旋转外参的优化值。
67.可以是，根据第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，建立下述至少一项残差，将建立的各残差组合成目标残差。
68.(1)第一位姿中的第一位置与第一位姿转换到绝对世界坐标系下的第二位姿中的第二位置的相对位置变化残差；
69.(2)第一位姿中的第一姿态与第一位姿转换到绝对世界坐标系下的第二位姿中的第二姿态的相对姿态变化残差；
70.(3)第一姿态的绝对姿态转换残差；
71.(4)全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差；
72.(5)全球定位系统在绝对世界坐标系下的观测航向的绝对姿态残差。
73.上述全球定位系统，具体为全球导航卫星系统，可以是任一卫星导航系统，例如gps或北斗卫星导航系统等。
74.建立的目标残差包括第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参。
75.每项残差的具体确定方法，后续实施例二中详细介绍。
76.优化得到的旋转外参的优化值用于下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，使得第一位姿的优化更为准确。
77.具体的，上述优化的终止条件可以是目标残差达到设定条件，可以是目标残差小于设定残差阈值；也可以是目标残差小于设定残差阈值且迭代次数达到预设次数；可选的，也可以是预先设定的其他的迭代优化终止条件。
78.发明实施例提供的多传感器融合定位系统的位姿优化方法，将获取到的预设时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿；基于述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到第二位姿的优化值和旋转外参的优化值，旋转外参的优化值用于下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合。该技术方案的有益效果至少包括：
79.(1)内层视觉惯性里程计(visual-inertial odometry，vio)进行视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿，外层引入全球定位系统在世界坐标系下的观测位置和观测航向为约束，在第一位姿的基础上优化得到第二位姿和旋转外参的优化值，解决了局部优化导致的精度不准的问题；将旋转外参的优化值输入下一轮的内层vio融合，提升了内层vio输出的第一位姿的精度，解决了相对世界坐标系与绝对世界坐标系间的旋转外参估计不准导致的优化不稳定的问题；并且外层引入全球定位系统在世界坐标系下的观测位置和观测航向为约束可解决内层因为漂移引起的误差累积问题，从而提升第二位姿精度。
80.(2)内层vio融合为紧耦合，优化的参数和残差多，计算量大；外层的位姿优化为松
耦合，每帧优化的参数少残差相对也少，计算量小。故内外层分开的双层优化方案，较所有优化工作都在内层完成减小了计算量。
81.参照图2所示，上述步骤可以概述为：在vio内层融合基础上，即将视觉传感器(如camera)的测量数据和惯性测量单元(inertial measurement unit，imu，简称惯导)的测量数据，结合全球定位系统(例如gps)测量的速度和经纬度信息，进行相对世界坐标系下的第一位姿估计(local estimator)；再以第一位姿(local position)为输入，结合全球定位系统(例如gps)测量的经纬度信息，进行外层的绝对世界坐标系下的位姿优化(global pose graph优化)，输出绝对世界坐标系下的位姿(global position)，同时将相对世界坐标系与绝对世界坐标系间的旋转外参(word frame r)返回给local estimator，用于下一时间段的vio融合。其中，全球定位系统(例如gps)测量数据的约束，解决了内层vio漂移的问题，实现了全局优化，提升了位姿的精度。
82.在一个实施例中，执行步骤s12前，还可以包括，判断视觉传感器拍摄的图像数据与惯导测量数据融合的次数是否达到预设次数；或，判断当前获取到的第一位姿对应的帧数是否达到预设帧数。
83.视觉传感器拍摄的图像数据与惯导测量数据融合即vio融合为紧耦合，优化的参数和残差多，计算量大，故内层vio融合为小窗口，每次优化的帧数少；外层的位姿优化即第二位姿的优化为松耦合，每帧优化的参数和残差都相对较少，计算量小，故外层为大窗口，每次优化的帧数较多。故可以在进行多次内层融合后，根据多次内层融合的结果再进行一次外层的优化。
84.当上述步骤判断为是时，执行步骤s12；若判断为否，继续获取下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据，执行步骤s11。
85.实施例二
86.本发明实施例二提供一种多传感器融合定位系统的位姿优化过程中，待优化的目标残差建立方法的具体实现，其流程如图3所示，包括如下步骤：
87.步骤s31：建立第一位姿中的第一位置与第二位姿中的第二位置的相对位置变化残差。
88.参照图4所示，包括下述步骤：
89.步骤s311：将相邻两帧图像拍摄时刻的第一位置的差值，根据两帧中的前一帧图像拍摄时刻惯导坐标系与相对世界坐标系的旋转外参，转换成前一帧图像拍摄时刻惯导坐标系下的第一差值。
90.步骤s312：根据两帧图像拍摄时刻的待优化的第二位姿中的第二位置的差值与前一帧图像拍摄时刻的第二位姿中的第二姿态，确定前一帧图像拍摄时刻惯导坐标系下的第二差值。
91.步骤s313：将第一差值与第二差值间的差值确定为相对位置变化残差。
92.具体的，第一位姿中的第一位置与第二位姿中的第二位置的相对位置变化残差，可以为下式所示：
[0093][0094]
其中，为第k帧时刻与第k 1帧时刻的第一位置的相对位置变化残差；
为第k帧时刻惯导坐标系与相对世界坐标系的旋转外参；为第k帧时刻的第一位置，即惯导在相对世界坐标系下的位置；为第k帧时刻的第二位置，即待优化的惯导在绝对世界坐标系下的位置；为第k帧时刻的第二姿态，即待优化的惯导在绝对世界坐标系下的姿态；k＝0,1
……
n，n 1为图像的总帧数。
[0095]
上述及后续中的第k帧时刻为视觉传感器拍摄第k帧图像的拍摄时刻。
[0096]
步骤s32：建立第一位姿中的第一姿态与第二位姿中的第二姿态的相对姿态变化残差。
[0097]
在一个实施例中，可以包括，根据相邻两帧图像拍摄时刻的待优化的第二位姿中的第二姿态，和分别在两帧图像拍摄时刻惯导坐标系与相对世界坐标系的旋转外参，确定相对姿态变化残差。具体建立的第一位姿中的第一姿态与第二位姿中的第二姿态的相对姿态变化残差可以为下式：
[0098]
参照下式所示：
[0099][0100]
步骤s33：建立第一姿态的绝对姿态转换残差。
[0101]
在一个实施例中，可以包括，根据惯导坐标系与相对世界坐标系的旋转外参、待优化的第二位姿中的第二姿态和相对世界坐标系与绝对世界坐标系间的旋转外参，确定第一姿态的绝对姿态变化残差。
[0102]
参照下式所示：
[0103][0104]
其中，为第k帧时刻的第一姿态的绝对姿态变化残差；为待优化的相对世界坐标系与绝对世界坐标系间的旋转外参。
[0105]
步骤s34：建立全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差。
[0106]
在一个实施例中，可以包括，根据全球定位系统在绝对世界坐标系下的观测位置、待优化的第二位姿中的第二位置和第二姿态，及惯导坐标系与绝对世界坐标系间的旋转外参，确定全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差。
[0107]
参照下式所示：
[0108][0109]
其中，为全球定位系统在绝对世界坐标系下的第k帧时刻的观测位置的绝对位置残差；为绝对世界坐标系与惯导坐标系间的旋转外参，即全球定位系统的天线与惯导间的相对位置外参；为全球定位系统在绝对世界坐标系下的第k帧时刻的观测位置。
[0110]
步骤s35：建立全球定位系统在绝对世界坐标系下的观测航向的绝对姿态残差。
[0111]
在一个实施例中，可以包括，根据全球定位系统在绝对世界坐标系下的观测航向和待优化的第二位姿中的第二姿态，确定全球定位系统在绝对世界坐标系下的观测航向的
绝对姿态残差。
[0112]
由于存在vio的z值优化不准和全球定位系统观测z值不平滑等问题，而z值容易与翻滚角roll和俯仰角pitch产生耦合，导致roll和pitch估计不准。故可以将整体的旋转优化进行在线分解，只对姿态中的航向角yaw进行优化，再结合vio结果中的俯仰角pitch和翻滚角roll进行姿态合并，解决了误差耦合导致roll和pitch估计不准的问题，提升姿态优化精度。故，全球定位系统在绝对世界坐标系下的观测姿态的绝对姿态残差，可以建立为下式：
[0113][0114]
其中，为全球定位系统在绝对世界坐标系下的第k帧时刻的观测航向的绝对姿态残差；为绝对世界坐标系下的第二姿态中第k帧时刻的航向角；为全球定位系统在绝对世界坐标系下的第k帧时刻的观测航向(角)。
[0115]
综上所述，上述残差中待优化的参数为：
[0116][0117][0118]
上述残差中每帧的第二姿态，例如第k帧的第二姿态只需优化
[0119]
步骤s31～步骤s35没有先后顺序，可以先执行其中的任何一步或多步，也可以同时执行。
[0120]
步骤s36：将建立的各残差组合成目标残差。
[0121]
组合成的目标残差x*可以为：
[0122][0123]
其中，ωpk为第k帧时刻的第一位置协方差；ωφk为第k帧时刻的第一姿态协方差；ωφvk为第k帧时刻的第一姿态的绝对姿态协方差；ωφwk为第k帧时刻全球定位系统的绝对姿态协方差，即全球定位系统在绝对世界坐标系下的观测航向的绝对姿态协方差；ωpwk为第k帧时刻全球定位系统的绝对位置协方差，即全球定位系统在绝对世界坐标系下的观测位置的绝对位置协方差；ρ(x)表示对x中的野值进行剔除。
[0124]
本发明实施例提供了基于4dof和绝对姿态约束的因子图优化确定位姿的方法，具体的，4dof指位置中的x、y和z及姿态中的航向角，绝对姿态约束指利用全球定位系统在绝对世界坐标系下的观测数据为约束，因子图优化原理参照图5所示，其中，x1～x6表示内层视觉传感器拍摄的图像数据与惯导测量数据融合得到的相对世界坐标系下的每帧的第一位姿(下标表示帧数)，rww0为待优化的相对世界坐标系与绝对世界坐标系间的旋转外参，因
子
①
表示相邻两帧间的第一位姿中的第一位置的相对位置变化残差和第一位姿中的第一姿态的相对姿态变化残差，因子
②
表示第一姿态的绝对姿态转换残差，因子
③
表示全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差和观测航向的绝对姿态残差。迭代优化上述因子组成的总因子，可以实现相对世界坐标系(local coordination)下的位姿到绝对世界坐标系(global coordination)下的位姿的转换。
[0125]
基于本发明的发明构思，本发明实施例还提供一种多传感器融合定位系统的位姿优化装置，其结构如图6所示，包括：
[0126]
融合模块61，用于将获取到的预设时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合，得到相对世界坐标系下的第一位姿；
[0127]
优化模块62，用于根据融合模块61得到的第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到所述第二位姿的优化值和旋转外参的优化值，所述旋转外参的优化值用于下一时间段内的视觉传感器拍摄的图像数据与惯导测量数据融合。
[0128]
在一个实施例中，优化模块62，所述对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，得到所述第二位姿的优化值和旋转外参的优化值，具体用于：
[0129]
迭代优化所述第二位姿中的第二位置和第二航向角及所述旋转外参，得到所述第二位置的优化值、第二航向角的优化值和旋转外参的优化值。
[0130]
一个实施例中，优化模块62，所述基于所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化，具体用于：
[0131]
根据所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，建立下述至少一项残差，将建立的各残差组合成目标残差：
[0132]
所述第一位姿中的第一位置与所述第一位姿转换到绝对世界坐标系下的第二位姿中的第二位置的相对位置变化残差；所述第一位姿中的第一姿态与所述第一位姿转换到绝对世界坐标系下的第二位姿中的第二姿态的相对姿态变化残差；所述第一姿态的绝对姿态转换残差；全球定位系统在绝对世界坐标系下的观测位置的绝对位置残差；全球定位系统在绝对世界坐标系下的观测航向的绝对姿态残差。
[0133]
一个实施例中，上述装置还包括判断模块63，优化模块62基于所述第一位姿和全球定位系统在绝对世界坐标系下的观测位置和观测航向，对所述第一位姿转换到绝对世界坐标系下的第二位姿和相对世界坐标系与绝对世界坐标系间的旋转外参进行迭代优化前，判断模块63用于：
[0134]
判断视觉传感器拍摄的图像数据与惯导测量数据融合的次数是否达到预设次数；或，判断当前获取到的第一位姿对应的帧数是否达到预设帧数。
[0135]
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
[0136]
基于本发明的发明构思，本发明实施例还提供一种计算机可读存储介质，其上存储有计算机指令，当该指令被处理器执行时实现上述多传感器融合定位系统的位姿优化方
法。
[0137]
基于本发明的发明构思，本发明实施例还提供一种服务器，包括：存储器、处理器及存储于存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述多传感器融合定位系统的位姿优化方法。
[0138]
除非另外具体陈述，术语比如处理、计算、运算、确定、显示等等可以指一个或更多个处理或者计算系统、或类似设备的动作和/或过程，所述动作和/或过程将表示为处理系统的寄存器或存储器内的物理(如电子)量的数据操作和转换成为类似地表示为处理系统的存储器、寄存器或者其他此类信息存储、发射或者显示设备内的物理量的其他数据。信息和信号可以使用多种不同的技术和方法中的任何一种来表示。例如，在贯穿上面的描述中提及的数据、指令、命令、信息、信号、比特、符号和码片可以用电压、电流、电磁波、磁场或粒子、光场或粒子或者其任意组合来表示。
[0139]
应该明白，公开的过程中的步骤的特定顺序或层次是示例性方法的实例。基于设计偏好，应该理解，过程中的步骤的特定顺序或层次可以在不脱离本公开的保护范围的情况下得到重新安排。所附的方法权利要求以示例性的顺序给出了各种步骤的要素，并且不是要限于所述的特定顺序或层次。
[0140]
在上述的详细描述中，各种特征一起组合在单个的实施方案中，以简化本公开。不应该将这种公开方法解释为反映了这样的意图，即，所要求保护的主题的实施方案需要清楚地在每个权利要求中所陈述的特征更多的特征。相反，如所附的权利要求书所反映的那样，本发明处于比所公开的单个实施方案的全部特征少的状态。因此，所附的权利要求书特此清楚地被并入详细描述中，其中每项权利要求独自作为本发明单独的优选实施方案。
[0141]
本领域技术人员还应当理解，结合本文的实施例描述的各种说明性的逻辑框、模块、电路和算法步骤均可以实现成电子硬件、计算机软件或其组合。为了清楚地说明硬件和软件之间的可交换性，上面对各种说明性的部件、框、模块、电路和步骤均围绕其功能进行了一般地描述。至于这种功能是实现成硬件还是实现成软件，取决于特定的应用和对整个系统所施加的设计约束条件。熟练的技术人员可以针对每个特定应用，以变通的方式实现所描述的功能，但是，这种实现决策不应解释为背离本公开的保护范围。
[0142]
结合本文的实施例所描述的方法或者算法的步骤可直接体现为硬件、由处理器执行的软件模块或其组合。软件模块可以位于ram存储器、闪存、rom存储器、eprom存储器、eeprom存储器、寄存器、硬盘、移动磁盘、cd-rom或者本领域熟知的任何其它形式的存储介质中。一种示例性的存储介质连接至处理器，从而使处理器能够从该存储介质读取信息，且可向该存储介质写入信息。当然，存储介质也可以是处理器的组成部分。处理器和存储介质可以位于asic中。该asic可以位于用户终端中。当然，处理器和存储介质也可以作为分立组件存在于用户终端中。
[0143]
对于软件实现，本技术中描述的技术可用执行本技术所述功能的模块(例如，过程、函数等)来实现。这些软件代码可以存储在存储器单元并由处理器执行。存储器单元可以实现在处理器内，也可以实现在处理器外，在后一种情况下，它经由各种手段以通信方式耦合到处理器，这些都是本领域中所公知的。
[0144]
上文的描述包括一个或多个实施例的举例。当然，为了描述上述实施例而描述部件或方法的所有可能的结合是不可能的，但是本领域普通技术人员应该认识到，各个实施
例可以做进一步的组合和排列。因此，本文中描述的实施例旨在涵盖落入所附权利要求书的保护范围内的所有这样的改变、修改和变型。此外，就说明书或权利要求书中使用的术语“包含”，该词的涵盖方式类似于术语“包括”，就如同“包括，”在权利要求中用作衔接词所解释的那样。此外，使用在权利要求书的说明书中的任何一个术语“或者”是要表示“非排它性的或者”。术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。