一种基于多传感器融合的定位方法及装置与流程

1.本技术涉及物体定位技术领域，尤其涉及一种基于多传感器融合的定位方法及装置。


背景技术：

2.目前，随着自动驾驶技术、智能机器人和无人机技术的发展，如何保证自动驾驶车辆、智能机器人和无人机的精确定位成为了一个热点问题。在自动驾驶车辆、智能机器人、无人机等可移动物体上，一般可以搭载各类传感器，以便于对周围环境进行感知，并依靠这些感知信息进行定位。
3.然而，当前自动驾驶车辆、智能机器人和无人机的定位大多需要依赖于良好的gps信号以及良好的激光雷达感知来实现精准的定位。在gps信号丢失、跳变的场景(隧道，被高楼遮挡卫星信号的街道等)，在激光雷达观测严重退化的场景(空旷的跨海大桥,高速公路，以及隧道等)，如何对自动驾驶车辆、智能机器人以及无人机等进行准确的定位，成为了一个亟待解决的问题。


技术实现要素：

4.本技术的实施例提供一种基于多传感器融合的定位方法及装置，能够在gps信号丢失、跳变的场景，在激光雷达观测严重退化的场景，对可移动物体进行准确的定位。
5.为达到上述目的，本技术的实施例采用如下技术方案：
6.本技术实施例的第一方面，提供一种基于多传感器融合的定位方法，应用于一种搭载有多种传感器的可移动物体上，所述方法包括：
7.实时获得可移动物体上搭载的各种传感器采集的传感器数据；
8.对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；
9.将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；
10.获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；
11.根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
12.另外，本技术实施例的第二方面，提供一种基于多传感器融合的定位装置，应用于一种搭载有多种传感器的可移动物体上，所述装置，包括：
13.传感器数据获得单元，实时获得可移动物体上搭载的各种传感器采集的传感器数据；
14.时间空间同步单元，用于对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；
15.数据预处理单元，用于将进行时间空间同步后的各种传感器数据进行数据预处理
和数据关联，形成用于联合优化的传感器数据；
16.状态信息获得单元，用于获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；
17.联合优化单元，用于根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
18.另外，本技术实施例的第三方面，提供一种计算机可读存储介质，包括程序或指令，当所述程序或指令在计算机上运行时，实现上述第一方面所述的基于多传感器融合的定位方法。
19.另外，本技术实施例的第四方面，提供一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行如上述第一方面所述的基于多传感器融合的定位方法。
20.另外，本技术实施例的第五方面，提供一种计算机服务器，包括存储器，以及与所述存储器通信连接的一个或多个处理器；所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现如上述第一方面所述的基于多传感器融合的定位方法。
21.本技术实施例提供的一种基于多传感器融合的定位方法及装置，首先实时获得可移动物体上搭载的各种传感器采集的传感器数据；对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据；将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；之后，获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；从而根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。可见，本技术能够结合多种传感器数据，并且通过滑动窗口处理，实时更新当前时刻的状态信息，能够在gps信号丢失、跳变的场景，在激光雷达观测严重退化的场景，对可移动物体进行准确的定位。
附图说明
22.为了更清楚地说明本技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
23.图1为本技术实施例提供的一种基于多传感器融合的定位方法的流程图一；
24.图2为本技术实施例提供的一种基于多传感器融合的定位方法的流程图二；
25.图3为本技术实施例中的一子步骤示意图；
26.图4为本技术实施例中的联合优化的滑动窗口示意图；
27.图5为本技术实施例提供的一种基于多传感器融合的定位装置的结构示意图。
具体实施方式
28.下面将结合本技术实施例中的附图，对本技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本技术一部分实施例，而不是全部的实施例。基于
本技术中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本技术保护的范围。
29.需要说明的是，本技术的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本技术的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
30.为了使本领域的技术人员更好的了解本技术，下面先对本技术实施例中出现的部分技术术语进行解释如下：
31.可移动物体：是指车辆、移动机器人、飞行器等可进行地图采集的物体，可移动物体上可以搭载各类型传感器，如激光雷达、相机等。
32.gnss：global navigation satellite system，全球导航卫星系统。
33.gps：global positioning system，全球定位系统。
34.imu：inertial measurement unit，惯性测量单元，是测量物体三轴姿态角(或角速率)以及加速度的装置。
35.高精度地图：不同于传统的导航地图，高精度地图包含大量的驾驶辅助信息，最重要的信息是依托道路网的精确三维表征，例如交叉路口布局和路标位置等。另外，高精度地图还包含很多语义信息，地图上可能会报告通信交通灯上不同颜色的含义，它可能指示道路的速度限制，以及左转车道开始的位置等。高精地图最重要的特征之一是精度，高精度地图能使车辆达到厘米级的精度，这对确保自动驾驶汽车的安全至关重要。
36.位姿(pose)：位置和朝向的总称，包含6个自由度，其中包括3个位置自由度和3个朝向自由度。朝向的3个自由度通常用俯仰角(pitch)、翻滚角(roll)、偏航角(yaw)来表示。
37.帧(frame)：传感器完成一次观测所接收到的测量数据，如相机的一帧数据为一张图片，激光雷达的一帧数据为一组激光点云。
38.配准(registration)：对同一区域在不同时刻、不同位置的观测结果进行匹配，得到两个观测时刻间的相对位姿关系。
39.svd：singular value decomposition，奇异值分解，是线性代数中一种重要的矩阵分解。
40.rtk：real-time kinematic，实时动态载波相位差分技术，是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标。
41.msckf：multi-state constraint kalman filter，即多状态约束下的卡尔曼滤波器。
42.在本技术的一些实施例中，术语“车辆”广泛地解释为包括任何移动物体，包括例如飞行器、船只、航天器、汽车、卡车、厢式货车、半挂车、摩托车、高尔夫球车、越野车辆、仓库运输车辆或农用车以及行驶在轨道上的运输工具，例如电车或火车以及其它有轨车辆。本技术中的“车辆”通常可以包括：动力系统、传感器系统、控制系统、外围设备和计算机系统。在其它实施例中，车辆可以包括更多、更少或者不同的系统。
43.其中，动力系统是为车辆提供动力运动的系统，包括：引擎/马达、变速器和车轮/轮胎、能源单元。
44.控制系统可以包括控制车辆及其组件的装置的组合，例如转向单元、节气门、制动单元。
45.外围设备可以是允许车辆与外部传感器、其它车辆、外部计算设备和/或用户进行交互的设备，例如无线通信系统、触摸屏、麦克风和/或扬声器。
46.基于上述描述的车辆，自动驾驶车辆中还配置有传感器系统和自动驾驶控制装置。
47.传感器系统可以包括用于感测车辆所处环境的信息的多个传感器，以及改变传感器的位置和/或方向的一个或多个致动器。传感器系统可以包括全球定位系统传感器、惯性测量单元、无线电检测和测距(radar)单元、相机、激光测距仪、光检测和测距(lidar)单元和/或声学传感器等传感器的任何组合；传感器系统还可以包括监视车辆内部系统的传感器(例如o2监视器、燃油表、引擎温度计等)。
48.自动驾驶控制装置可以包括一个处理器和存储器，存储器中存储有至少一条机器可执行指令，处理器执行至少一条机器可执行指令实现包括地图引擎、定位模块、感知模块、导航或路径模块、以及自动控制模块等的功能。地图引擎和定位模块用于提供地图信息和定位信息。感知模块用于根据传感器系统获取到的信息和地图引擎提供的地图信息感知车辆所处环境中的事物。导航或路径模块用于根据地图引擎、定位模块和感知模块的处理结果，为车辆规划行驶路径。自动控制模块将导航或路径模块等模块的决策信息输入解析转换成对车辆控制系统的控制命令输出，并通过车载网(例如通过can总线、局域互联网络、多媒体定向系统传输等方式实现的车辆内部电子网络系统)将控制命令发送给车辆控制系统中的对应部件，实现对车辆的自动控制；自动控制模块还可以通过车载网来获取车辆中各部件的信息。
49.目前，可移动物体的定位方案大致有以下几种，这些定位方案大多对所定位的场景有较大依赖性。
50.例如，第
①
种：四旋翼类的无人机主要以相机和imu为主，来进行传感器融合定位；又例如，第
②
种：室内轮速机器人主要使用相机和轮速计为主，来进行传感器组合方式定位。然而，上述两种(
①
、
②
)方法对视觉观测的稳定数据关联要求很高，一旦遇到弱纹理以及大量重复纹理的场景，imu的状态变量将不能获得准确的视觉约束的更新，会导致状态变量估计不准，发生漂移。另外，在室外场景下，上述两种(
①
、
②
)方法还会受到太阳光线，曝光、遮挡等影响。并且对于大部分匀速或匀加速运动的情况下，imu得不到充分的激励，会导致原始观测(加速度计的加速度和陀螺仪角速度)噪声很大，这种情况会对visual-imu(视觉-惯性测量)紧耦合系统的可观性产生影响。
51.又例如，第
③
种：以机器视觉为主的无人车定位系统：通过图片上的语义信息和语义地图进行特征关联，并进行车辆定位，也有融合gps，以及多相机和imu的传感器融合方案进行车辆定位。对于该第
③
种方案，以视觉为主依赖语义信息进行状态估计和定位的传感器组合，在非结构化(没有车道线等语义路标的)场景下，长时间得不到稳定准确的数据关联，将会导致状态估计的准确性和鲁棒性受到影响。
52.又例如，第
④
种：以激光雷达为主的无人车定位系统：通过激光雷达的观测与全局
激光雷达的反射率地图或者特征地图进行特征关联，融合rtk等传感器进行联合状态估计，还有一些方案是融合激光雷达和imu进行无人车状态估计，也有用msckf的滤波框架融合visual-lidar-imu(视觉-激光雷达-惯性测量)的方案进行车辆状态估计。对于上述第
④
种方案，在空旷场景下，激光雷达的观测会发生严重退化，导致激光雷达和imu的数据更新不及时，传感器很难鲁棒的输出稳定的状态。
53.又例如，第
⑤
种：也有以gps为主的室外无人车定位系统。然而该第
⑤
种方案在大部分室外高动态环境，尤其是城市高楼林立的街道等场景，又或者是遮挡情况严重的隧道，gps的多路径反射的问题非常明显，这时候的gps会出现少则几米，多则几十米的定位误差，难以满足无人驾驶场景的定位需求。
54.本技术实施例旨在提出一种基于激光雷达点云地图的紧耦合的多传感器融合定位方案。旨在提高自动驾驶定位系统在复杂场景下的定位鲁棒性和准确性。使其能够适用于gps信号丢失、跳变的场景(隧道，被高楼遮挡卫星信号的街道等)，以及激光雷达观测严重退化的场景(空旷的跨海大桥,高速公路，以及隧道等)。
55.如图1所示，本技术实施例提供一种基于多传感器融合的定位方法，该方法应用于一种搭载有多种传感器的可移动物体上，该方法包括：
56.步骤101、实时获得可移动物体上搭载的各种传感器采集的传感器数据。
57.步骤102、对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据。
58.步骤103、将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据。
59.步骤104、获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息。
60.步骤105、根据用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
61.为了使本领域的技术人员更好的了解本发明，下面结合具体的实施例来对本技术实施例进行阐述，如图2所示，本技术实施例提供一种基于多传感器融合的定位方法，该方法应用于一种搭载有多种传感器的可移动物体上，该多种传感器可以包括惯性测量单元imu、轮速计、激光雷达等传感器，但不仅局限于此，例如一般情况下还可以包括相机、gps传感器等；其中，该imu包括加速度计和陀螺仪。
62.该方法包括：
63.步骤201、实时获得imu测量的imu数据、轮速计测量的可移动物体轮速数据和激光雷达测量的点云数据。
64.步骤202、根据imu、轮速计和激光雷达之间预先标定的外参，将可移动物体轮速数据和点云数据对齐到imu坐标系下，并根据imu、轮速计和激光雷达各自的数据采集帧率，将可移动物体轮速数据和点云数据的采集时的每帧与imu的时间戳对齐，形成imu坐标系下的，时间戳对齐的imu数据、可移动物体轮速数据和点云数据。
65.步骤203、将进行时间空间同步后的imu数据中处于连续两帧激光雷达时间戳之间的imu数据使用预先设置的积分算法进行传播，并通过预先设置的预积分方法进行处理，得到位移预积分量、速度预积分量和旋转增量预积分量。
66.值得说明的是，imu的原始数据一般为包含偏置(bias)和噪声(noise)的加速度和
角速度数据。因此，需要先进行上述步骤202的时间空间同步，然后将进行时间空间同步后的imu数据中处于连续两帧激光雷达时间戳(如第k帧和第k 1帧)之间的imu数据使用如rk4(4阶龙格库塔)积分算法、欧拉积分算法或者中值积分算法等积分算法进行传播，并通过预先设置的预积分方法(例如论文c.forster,l.carlone,f.dellaert,and d.scaramuzza,"imu preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation,"georgia institute of technology,2015.中的方法)进行处理，得到不依赖于绝对位姿的位移预积分量δp、速度预积分量δv和旋转增量预积分量δq。
67.步骤204、根据当前时刻的上一时刻在预先设置的滑动窗口中联合优化后的状态变量，以及进行时间空间同步后的imu数据进行下一时刻的状态变量预测，得到下一时刻的状态变量预测数据。
68.为了给后续的激光雷达的数据关联和后续的联合优化提供准确的初值，本技术实施例需要如步骤204这样的过程来进行下一时刻的状态变量预测。
69.具体过程是：通过滑动窗口中当前时刻的上一时刻的联合优化后的状态变量x
1:m
＝[x1,x2,..,xm]，以及当前时刻imu的观测输入作为输入。其中，x
1:m
为状态变量数组，xk为第k个状态变量；此处的t表示矩阵的转置；该状态变量包括平移向量pk、速度向量vk、旋转矩阵qk、imu中加速度计偏置b
ak
和imu中陀螺仪偏置b
gk
；为imu观测的第k个加速度，为imu观测的第k个角速度。
[0070]
这样，已知imu的观测噪声模型为如下公式(1)、(2)：
[0071][0072][0073]
其中，rk表示第k时刻imu相对于世界坐标系的旋转矩阵。
[0074]
从而，通过上述的输入，可以有效进行下一时刻(下一帧)的状态变量预测，公式如公式(3)、(4)所示：
[0075][0076][0077]
其中，为第k 1帧的平移向量预测值，为第k 1帧的旋转矩阵预测值，表示四元数的乘法运算，表示四元数的乘法运算，分别为滑动窗口前m帧状态变量中的两种偏置的平均值，因为陀螺仪和加速度计离散时刻的偏置是零均值随机游走的高斯白噪声，使用多帧偏置的均值能够减少异常偏置量的影响，使得预测的状态变量更加平滑。
[0078]
步骤205、将进行时间空间同步后的点云数据，采用下一时刻的状态变量预测数据
进行去畸变处理。
[0079]
即通过下一时刻的状态变量预测数据，可以对点云数据产生约束，从而能够对进行时间空间同步后的点云数据进行去畸变处理。
[0080]
步骤206、对去畸变处理后的点云数据进行线面特征提取，获得线特征数据和面特征数据。
[0081]
具体的对点云数据进行线面特征提取的过程，可见文章：j.zhang and s.singh,"loam:lidar odometry and mapping in real-time,"in robotics:science and systems,2014,vol.2.，此处不再赘述。
[0082]
步骤207、将线特征数据和面特征数据与预先生成的特征地图上的线特征和面特征分别进行配准，得到线特征约束关系数据和面特征约束关系数据。
[0083]
其中，线特征约束关系数据中包含线特征数据中的点及其对应的特征地图中的线特征中的点，面特征约束关系数据中包含面特征数据中的点及其对应的特征地图中的面特征中的点。
[0084]
步骤208、将进行时间空间同步后的可移动物体轮速数据，通过预先设置的车辆动力学模型，确定在imu坐标系下前进方向线速度、水平方向线速度和偏航角速度。
[0085]
例如，预先设置的车辆动力学模型可以采用阿克曼运动学模型，但不仅局限于此。
[0086]
步骤209、获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息。
[0087]
其中，状态信息可以为状态变量，该状态变量包括平移向量、速度向量、旋转矩阵、imu中加速度计偏置和imu中陀螺仪偏置。
[0088]
值得说明的是，在开始进行联合优化之前，可移动物体可能处于静止状态，也可能处于移动状态，当可移动物体处于静止状态，才开始移动时，联合优化之前需要进行初始化。具体的初始化过程主要针对初始速度，重力矢量g、陀螺仪偏置bg和加速度计偏置ba；考虑到重力加速度和加速度计偏置耦合在一起，短时间的初始化过程很难进行完全的解耦，所以此处的初始化方案，侧重估计初始速度、重力矢量g以及陀螺仪偏置bg。
[0089]
则整个步骤209其过程可以如图3所示，包括：
[0090]
步骤2091、在用于联合优化的传感器数据未经过初始化时，实时确定预先设置的滑动窗口中的数据是否满足初始化条件。
[0091]
其中，初始化条件包括所述滑动窗口中的数据观测时刻数大于等于预先设置的数量阈值，且所述滑动窗口中的imu数据充分激励。
[0092]
此处的imu数据充分激励是指判断一预设时间段的imu的观测数据的读数的方差，若方差大于等于预设阈值，则表示充分激励，否则若方差小于预设阈值，则表示未充分激励。
[0093]
步骤2092、在所述滑动窗口中的数据观测时刻数大于等于预先设置的数量阈值，且所述滑动窗口中的imu数据充分激励时，对状态变量进行初始化，形成初始化状态变量。
[0094]
另外，在滑动窗口中的数据观测时刻数小于预先设置的数量阈值，且所述滑动窗口中的imu数据充分激励时，则需要对当前滑动窗口中的点云数据和可移动物体轮速数据进行紧耦合帧间优化，得到当前滑动窗口中的当前时刻的初始化激光雷达位姿。所述初始化激光雷达位姿包括激光雷达旋转矩阵和激光雷达平移向量。其过程为：首先，基于轮速计结合匀速运动模型的假设，提供一个初始的帧间运动预测(轮速计结合匀速模型提供
粗略的线速度，imu提供带偏置的角速度)，对相邻帧的激光雷达线、面特征进行帧间配准和位姿优化。帧间位姿的优化目标包含基于线面特征的几何误差，以及基于轮速计的观测数据产生的位移约束，如下式(5)：
[0095][0096]
其中，表示滑动窗口中第k帧中，激光雷达线特征配准的几何残差；表示滑动窗口中第k帧中，激光雷达面特征配准的几何残差；rodom(vj，xk)表示基于轮速计的观测数据产生的位移约束，如公式(6)所示：
[0097][0098]
其中，表示相邻帧的相对位移，(rk，pk)表示第k帧位姿的旋转矩阵和平移向量，[vk]
4∶6
＝[v
x v
y 0]
t
表示增广速度向量vk的4到6列元素组成的子向量。
[0099]
这样，在通过上述公式(5)得到每帧的相对位姿后，转到滑动窗口的第一帧激光雷达坐标系之后，得到一系列的初始激光雷达位姿
[0100]
之后，该步骤2092可以采用如下方式实现：
[0101]
步骤(1)、根据当前时刻和当前时刻之前各时刻的初始化激光雷达位姿，采用激光雷达相邻帧间的旋转改变量以及激光雷达相邻帧间旋转增量预积分量的一致性约束，将连续多帧的一致性约束进行累加，求解得到初始化陀螺仪偏置。
[0102]
此处，可以采用激光雷达相邻帧间的旋转改变量δq
l
以及激光雷达相邻帧间旋转增量预积分量δq的一致性约束，构建优化问题，连续多帧的一致性约束累加在一起，即可求解初始化陀螺仪偏置。这样，使用求得的初始化陀螺仪偏置重新进行预积分的传递，可以减少由陀螺仪偏置引起的累计误差。求解得到初始化陀螺仪偏置δbg可通过如下公式(7)实现：
[0103][0104]
其中：表示滑动窗口中的帧数；为预积分中旋转增量关于陀螺仪偏置bg的雅克比(jacobian)矩阵，是imu坐标系到激光雷达坐标系的外参矩阵的旋转分量四元数形式，表示第k帧激光雷达的旋转矩阵四元数形式。
[0105]
步骤(2)、根据当前滑动窗口中的初始化激光雷达位姿以及所述位移预积分量的一致性约束，求解得到激光雷达坐标系下的重力加速度，将所述激光雷达坐标系下的重力加速度转换到世界坐标系下，得到初始化重力加速度。
[0106]
以下结合公式和具体推导过程，来详细阐述得到初始化重力加速度的过程：
[0107]
重力加速度的估计，主要利用激光雷达配准得到的激光雷达位姿与预积分的位移增量δp的尺度一致性构建约束；预积分量有如下公式(8)的关系：
[0108]
[0109]
为了推导过程更清晰，使用δtk表示k,k 1帧间的时间差，分别简记为pk，rk，vk，δvk以及δpk，则上述公式(8)可以记为公式(9)：
[0110][0111]
则连续三帧(下标分别使用1，2，3表示连续的三帧)有以下关系，如公式(10)所示：
[0112][0113]
则通过上述公式(9)、(10)可以得到如公式(11)的关系：
[0114][0115]
将上述公式(10)的上下两公式相减，结合上述公式(11)，可以得到下面公式的等价关系，如公式(12)：
[0116][0117]
其中，公式(12)中有如下公式(13)和公式(14)的关系：
[0118][0119][0120]
可见，上述公式(12)中，出了重力加速度其他变量均为已知量，rk,pk是通过上述已经求得的初始激光雷达位姿进行外参转换得到；结合公式(12)至(14)的等式关系，在连续m(m≥3)帧构成的滑动窗口中，可以构建出m-2组等式(12)的约束关系(k＝1,
…
,m-2)，将上述m-2组约束关系式，构建非线性方程(例如，方程形式为：a
3(m-2)
×3·
x3×1＝b
3(m-2)
×1)，使用svd分解，得到激光雷达坐标系下的重力加速度
[0121]
在得到激光雷达坐标系下的重力加速度之后，要将其转换到世界坐标系下。考虑到重力加速度g的模长g＝9.81约束，参考现有的重力参数化方式，可以令上述估计的激光雷达坐标系下的重力加速度进行模长归一化：定义imu坐标系(惯性系)下的归一化向量：分解重力矢量到惯性系，得到最终的重力加速度：作为初始化重力加速度；其中：exp表示指数映射。
[0122]
步骤(3)、根据初始化陀螺仪偏置和初始化重力加速度，进行初始速度向量估计，确定初始化速度。
[0123]
此处，在得到初始化陀螺仪偏置以及初始化重力加速度之后，可以继续求解得到初始化速度。其具体过程可以如下公式(15)所示：
[0124]
[0125]
即当k＜m时，可以由上述公式(15)得到初始化速度；当k＝m时，由于没有t
k 1
时刻的观测，只能通过上一帧的速度结合加速度模型进行两次积分来进行求解。
[0126]
步骤2093、在对状态变量进行初始化完成后，且当前时刻之前的上一时刻进行联合优化求解结束后，获得当前时刻之前的各时刻的联合优化后的当前状态变量。
[0127]
即通过上述步骤209，能够得到初始化状态变量或者滑动窗口内当前时刻之前的各时刻的联合优化后的当前状态变量。两者的区别在于，前者的滑动窗口内当前时刻之前的各时刻还没有进行联合优化，只能通过初始化来提供初始化状态变量；而后者的滑动窗口内当前时刻之前的各时刻已经进行联合优化，可以提供各时刻的联合优化后的当前状态变量。从而基于上述两者对应的情况，以下步骤210可以有两种方式。
[0128]
步骤210、根据用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
[0129]
如上述所述，该步骤210可以通过如下两种方式实现：
[0130]
例如：在滑动窗口中的当前时刻之前的各时刻未进行联合优化时，根据当前时刻之前的各时刻的初始化状态变量和所述用于联合优化的传感器数据，采用联合概率密度估计，确定滑动窗口中的各时刻的状态变量；其中滑动窗口中当前时刻的状态变量用于表示可移动物体的当前位姿状态。
[0131]
又例如，在滑动窗口中的当前时刻之前的各时刻已进行联合优化时，根据当前时刻之前的各时刻的联合优化后的当前状态变量和所述用于联合优化的传感器数据，采用联合概率密度估计，确定滑动窗口中的各时刻的状态变量；其中滑动窗口中当前时刻的状态变量用于表示可移动物体的当前位姿状态。
[0132]
此处的联合优化采用的优化框架是基于固定滞后平滑(fixed-lagsmoother)的滑动窗口结构。固定滞后平滑是一种迭代算法，交替进行新增观测值或状态变量的更新和边缘化操作，递归地维护对最近m个状态变量全概率密度的估计，是一种基于图模型的增量状态估计方法。从因子图的角度看，可以理解成将一个最大后验概率的求解，分解成对多个条件概率和似然概率的因子相乘，并且能够有效地进行增量求解，在维护更多观测信息的同时，降低增量目标优化的计算复杂度。
[0133]
如图4所示：此处以滑动窗口的尺寸为5为例(但不仅局限于此)，系统处于稳定状态时，在任何给定时间，5个最近时刻的状态变量的联合密度都可以随时获得；当添加了下一时刻的状态变量x
i 6
，可以将滑动窗口中滞后最久的变量(状态变量x
i 1
)边缘化，仅剩4个变量x
i 2
,x
i 3
,x
i 4
,x
i 5
同新的状态变量x
i 6
一起进行联合状态估计。其中hdmap表示预先设置的特征地图，此处为高清点云特征地图。
[0134]
其中，上述步骤210中的两种实现方式中，采用联合概率密度估计，确定滑动窗口中的各时刻的状态变量，可以采用如下方式实现：
[0135]
根据联合概率密度估计函数：
[0136]
[0137]
确定滑动窗口中的各时刻的状态变量xk；其中，k表示第k时刻及第k帧，r
p
(x)表示边缘化的先验项；表示滑动窗口中的总帧数；i表示第i个线特征；j表示第j个面特征；表示线特征约束关系数据的线特征对总数；表示面特征约束关系数据的面特征对总数；表示滑动窗口中第k帧中，激光雷达线特征配准的几何残差；zi表示第i个线特征观测；表示当前激光雷达线特征的观测协方差；表示滑动窗口中第k帧中，激光雷达面特征配准的几何残差；zj表示第j个面特征观测；表示当前激光雷达面特征的观测协方差；表示imu的残差；表示imu坐标系下第k帧到第k 1帧的imu观测；表示imu对应的预积分量的协方差矩阵；表示轮速计对部分状态变量约束的残差；表示轮速计坐标系下第k帧到第k 1帧的轮速计观测；表示轮速计观测的协方差；
[0138]
其中，表示为表示为表示为表示为表示为表示为表示为rv＝(v
i-rk·
[vi]
4：6
)；
[0139]
其中，pi表示待配准的线特征点；p1和p2表示特征地图上的线特征上的两个点；rk,pk分别表示第k帧状态变量xk中的旋转矩阵和平移向量；pj表示待配准的面特征点；n表示特征地图上的平面法向量；vk表示第k帧状态变量xk中的初始化速度；δt表示相邻两帧的时间间隔；gw表示初始化重力加速度；δp表示位移预积分量；δv表示速度预积分量；δq表示旋转增量预积分量；qk表示第k帧状态变量xk中的旋转矩阵rk的四元数表示形式；[
·
]
xyz
表示四元数的实部分量；b
ak
表示第k帧状态变量xk中的imu中加速度计偏置；b
gk
表示第k帧状态变量xk中的陀螺仪偏置；vi表示关于imu坐标系下前进方向线速度v
x
、水平方向线速度vy和偏航角速度θ的矩阵，vi＝[0 0 0 v
x v
y 0 0 0 θ 0 0 0 0 0 0]
t
。
[0140]
本技术实施例提供的基于多传感器融合的定位方法，能够最终确定可移动物体的当前位姿状态。而在研究本技术的过程中，发明人考虑到如下情况：尽管视觉传感器(相机)成本相对较低，但是在大部分自动驾驶的室外场景下，受曝光、光线影响，数据的观测误差以及数据关联的准确性很难保证，并且单目相机缺乏尺度信息。激光雷达传感器，受光线变
化等影响较少，有限范围内的数据精度高(200米以内的观测误差趋于厘米级别)，且包含三维结构信息，在自动驾驶领域的定位和建图上有很强的实用价值。imu是测量物体三轴角速率以及加速度的传感器，能够完整提供六自由度的位姿观测数据，其本身是一种主动传感器，不受限于场景的差异，在机器人、无人机以及无人车的应用上都是不可或缺的组成部分。但是，imu本身在一些特殊运动的情况下(匀速或者匀加速运动)会导致系统可观性受到影响的缺点，尤其在自动驾驶应用上，车辆的运动状态近似趋于平面上的匀速运动，这时候imu的可观性往往会受到影响。轮速计可以在低频下提供帧间的平面速度矢量，以及帧间的距离尺度测量。因而，本技术实施例采用轮速计与imu的融合，能够弥补imu在近似平面的匀速运动时，可观性受影响的缺陷。
[0141]
本技术实施例充分融合激光雷达，imu和轮速计，结合预生成的点云特征地图，能够准确、鲁棒地进行可移动物体(车辆、智能机器人等)的定位和状态估计。并且，整个联合优化框架能够很好的兼容新增传感器观测源，包括gps/rtk这些绝对的观测数据，以及相机提供的帧间运动和观测约束，即在增加新增传感器观测源后，本技术实施例同样可以进行联合优化。
[0142]
另外，如图5所示，本技术实施例提供一种基于多传感器融合的定位装置，应用于一种搭载有多种传感器的可移动物体上，该基于多传感器融合的定位装置，包括：
[0143]
传感器数据获得单元31，用于实时获得可移动物体上搭载的各种传感器采集的传感器数据。
[0144]
时间空间同步单元32，用于对各种传感器采集的传感器数据进行时间空间同步，形成时间空间同步后的各种传感器数据。
[0145]
数据预处理单元33，用于将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据。
[0146]
状态信息获得单元34，用于获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息。
[0147]
联合优化单元35，用于根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。
[0148]
另外，本技术实施例还提供一种计算机可读存储介质，包括程序或指令，当所述程序或指令在计算机上运行时，实现上述图1至图3所述的基于多传感器融合的定位方法。其具体实现过程请见上述方法实施例，此处不再赘述。
[0149]
另外，本技术实施例还提供一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行上述图1至图3所述的基于多传感器融合的定位方法。其具体实现过程请见上述方法实施例，此处不再赘述。
[0150]
另外，本技术实施例还提供一种计算机服务器，包括存储器，以及与所述存储器通信连接的一个或多个处理器；所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现上述图1至图3所述的基于多传感器融合的定位方法。其具体实现过程请见上述方法实施例，此处不再赘述。
[0151]
本技术实施例提供的一种基于多传感器融合的定位方法及装置，首先实时获得可移动物体上搭载的各种传感器采集的传感器数据；对各种传感器采集的传感器数据进行时
间空间同步，形成时间空间同步后的各种传感器数据；将进行时间空间同步后的各种传感器数据进行数据预处理和数据关联，形成用于联合优化的传感器数据；之后，获得预先设置的滑动窗口中在当前时刻之前的各时刻的状态信息；从而根据所述用于联合优化的传感器数据和滑动窗口中在当前时刻之前的各时刻的状态信息，进行联合优化求解，确定可移动物体的当前位姿状态。可见，本技术能够结合多种传感器数据，并且通过滑动窗口处理，实时更新当前时刻的状态信息，能够在gps信号丢失、跳变的场景，在激光雷达观测严重退化的场景，对可移动物体进行准确的定位。
[0152]
本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0153]
本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0154]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0155]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0156]
本技术中应用了具体实施例对本技术的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本技术的方法及其核心思想；同时，对于本领域的一般技术人员，依据本技术的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本技术的限制。