1.本公开涉及智能交通技术领域，尤其涉及自动驾驶领域，更具体地，涉及一种针对自动驾驶车辆的数据处理方法、装置、电子设备、介质、程序产品和自动驾驶车辆。


背景技术：

2.目前针对视觉/lidar里程计问题已经有很多解决方案，现有的方法虽然可以满足在线实时应用的需求，但一般需要消耗大量的cpu计算资源。


技术实现要素：

3.本公开提供了一种针对自动驾驶车辆的数据处理方法、装置、电子设备、存储介质、程序产品和自动驾驶车辆。
4.根据本公开的一方面，提供了一种针对自动驾驶车辆的数据处理方法，包括：获取针对所述自动驾驶车辆的传感器数据，所述传感器数据包括惯性测量数据、激光雷达数据和视觉图像数据；根据所述惯性测量数据和所述视觉图像数据，确定针对所述惯性测量数据的第一约束因子；根据所述惯性测量数据和所述激光雷达数据，确定针对所述激光雷达数据的第二约束因子；根据所述惯性测量数据、所述视觉图像数据和所述激光雷达数据，确定针对所述视觉图像数据的第三约束因子；基于所述第一约束因子、所述第二约束因子和所述第三约束因子，处理所述传感器数据，得到用于对所述自动驾驶车辆进行定位的定位数据。
5.根据本公开的另一方面，提供了一种针对自动驾驶车辆的数据处理装置，包括：获取模块、第一确定模块、第二确定模块、第三确定模块以及处理模块。获取模块，用于获取针对所述自动驾驶车辆的传感器数据，所述传感器数据包括惯性测量数据、激光雷达数据和视觉图像数据；第一确定模块，用于根据所述惯性测量数据和所述视觉图像数据，确定针对所述惯性测量数据的第一约束因子；第二确定模块，用于根据所述惯性测量数据和所述激光雷达数据，确定针对所述激光雷达数据的第二约束因子；第三确定模块，用于根据所述惯性测量数据、所述视觉图像数据和所述激光雷达数据，确定针对所述视觉图像数据的第三约束因子；处理模块，用于基于所述第一约束因子、所述第二约束因子和所述第三约束因子，处理所述传感器数据，得到用于对所述自动驾驶车辆进行定位的定位数据。
6.根据本公开的另一方面，提供了一种电子设备，包括：至少一个处理器和与所述至少一个处理器通信连接的存储器。其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述的针对自动驾驶车辆的数据处理方法。
7.根据本公开的另一方面，提供了一种存储有计算机指令的非瞬时计算机可读存储介质，所述计算机指令用于使所述计算机执行上述的针对自动驾驶车辆的数据处理方法。
8.根据本公开的另一方面，提供了一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现上述的针对自动驾驶车辆的数据处理方法。
9.根据本公开的另一方面，提供了一种自动驾驶车辆，包括上述电子设备。
10.应当理解，本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征，也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。
附图说明
11.附图用于更好地理解本方案，不构成对本公开的限定。其中：
12.图1示意性示出了根据本公开一实施例的针对自动驾驶车辆的数据处理方法的流程图；
13.图2示意性示出了根据本公开一实施例的针对自动驾驶车辆的数据处理方法的原理图；
14.图3示意性示出了根据本公开一实施例的插值计算的原理示意图；
15.图4示意性示出了根据本公开一实施例的因子图的示意图；
16.图5示意性示出了根据本公开一实施例的针对自动驾驶车辆的数据处理装置的框图；以及
17.图6是用来实现本公开实施例的用于执行针对自动驾驶车辆的数据处理的电子设备的框图。
具体实施方式
18.以下结合附图对本公开的示范性实施例做出说明，其中包括本公开实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本公开的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
19.在此使用的术语仅仅是为了描述具体实施例，而并非意在限制本公开。在此使用的术语“包括”、“包含”等表明了所述特征、步骤、操作和/或部件的存在，但是并不排除存在或添加一个或多个其他特征、步骤、操作或部件。
20.在此使用的所有术语(包括技术和科学术语)具有本领域技术人员通常所理解的含义，除非另外定义。应注意，这里使用的术语应解释为具有与本说明书的上下文相一致的含义，而不应以理想化或过于刻板的方式来解释。
21.在使用类似于“a、b和c等中至少一个”这样的表述的情况下，一般来说应该按照本领域技术人员通常理解该表述的含义来予以解释(例如，“具有a、b和c中至少一个的系统”应包括但不限于单独具有a、单独具有b、单独具有c、具有a和b、具有a和c、具有b和c、和/或具有a、b、c的系统等)。
22.由于利用视觉/lidar里程计在进行定位计算过程中一般需要消耗大量的cpu计算资源。因此搭建一个精度高、cpu资源占用少、gpu资源利用合理的视觉、激光雷达、惯性测量单元的里程计是有实用价值的。本公开的实施例提供了一种搭建基于视觉、激光雷达、惯性测量单元里程计的方法，并利用基于视觉、激光雷达、惯性测量单元里程计进行自动驾驶车辆的定位。
23.为了便于描述，对本公开中所使用的符号进行说明。使用(
·
)w来表示全局坐标系，即系统初始关键帧所对应的imu(惯性测量单元)坐标系。使用(
·
)b表示局部坐标系，即
imu坐标系。使用(
·
)c表示视觉坐标系。在文中使用旋转矩阵r和hamilton风格的单位四元数q来表达旋转。是局部坐标系到全局坐标系的旋转和平移值。表示车体坐标系到局部坐标系的外参关系。bk是处理第k帧数据时的局部坐标系。ck表示第k帧视觉图像数据。表示在第k帧imu的速度在全局坐标系中的值。表示在第k帧imu的速度在相应帧局部坐标系中的值。使用b
ω
表示imu陀螺仪的零偏，ba表示imu加速度计的零偏。gw表示重力加速度在全局坐标系中的值，由初始化建立。使用表示带有噪声的测量数据或者具有一定不确定性的估计数据。表示两个四元数相乘，
×
表示两个3维向量的矢量积。
24.本公开的实施例提供了一种针对自动驾驶车辆的数据处理方法，包括：获取针对自动驾驶车辆的传感器数据，传感器数据包括惯性测量数据、激光雷达数据和视觉图像数据。然后，根据惯性测量数据和视觉图像数据，确定针对惯性测量数据的第一约束因子，根据惯性测量数据和激光雷达数据，确定针对激光雷达数据的第二约束因子，根据惯性测量数据、视觉图像数据和激光雷达数据，确定针对视觉图像数据的第三约束因子。接下来，基于第一约束因子、第二约束因子和第三约束因子，处理传感器数据，得到用于对自动驾驶车辆进行定位的定位数据。
25.以下参考图1～图4来描述根据本公开示例性实施方式的针对自动驾驶车辆的数据处理方法。
26.图1示意性示出了根据本公开一实施例的针对自动驾驶车辆的数据处理方法的流程图。
27.如图1所示，本公开实施例的针对自动驾驶车辆的数据处理方法100例如可以包括操作s110～操作s150。
28.在操作s110，获取针对自动驾驶车辆的传感器数据，传感器数据包括惯性测量数据、激光雷达数据和视觉图像数据。
29.在操作s120，根据惯性测量数据和视觉图像数据，确定针对惯性测量数据的第一约束因子。
30.在操作s130，根据惯性测量数据和激光雷达数据，确定针对激光雷达数据的第二约束因子。
31.在操作s140，根据惯性测量数据、视觉图像数据和激光雷达数据，确定针对视觉图像数据的第三约束因子。
32.在操作s150，基于第一约束因子、第二约束因子和第三约束因子，处理传感器数据，得到用于对自动驾驶车辆进行定位的定位数据。
33.示例性地，自动驾驶车辆在驾驶过程中，可以通过多种类型的传感器采集数据，多种类型的传感器例如包括但不仅限于惯性测量单元、激光雷达、相机。惯性测量单元例如包括但不仅限于imu陀螺仪和轮速计。
34.在采集得到传感器数据之后，可以基于传感器数据得到第一约束因子、第二约束因子和第三约束因子。然后基于第一约束因子、第二约束因子和第三约束因子处理传感器数据得到定位数据，以便于自动驾驶车辆可以基于定位数据进行定位。
35.可以理解，本公开的实施例通过惯性测量单元、激光雷达、相机采集传感器数据，
并基于传感器数据进行数据处理实现自动驾驶车辆的定位，提高了自动驾驶车辆的定位精度。另外，基于传感器数据进行数据处理的过程中合理利用gpu资源，从而降低了cpu资源的占用率，在节约资源的同时提高了计算速度。
36.图2示意性示出了根据本公开一实施例的针对自动驾驶车辆的数据处理方法的原理图。
37.如图2所示，根据本公开的实施例，首先获取激光雷达数据210、视觉图像数据220和惯性测量数据230。然后，基于icp cuda(iterative closet point compute unified device architecture)算法处理激光雷达数据210得到点云帧间相对位姿约束信息；利用光流法特征跟踪算法处理视觉图像数据220得到视觉特征，并利用深度信息估计算法基于视觉特征和激光雷达数据210得到深度信息；对惯性测量数据230进行数值积分，得到帧间运动预测和状态约束信息，惯性测量数据230例如包括imu数据和轮速计数据。
38.接下来，基于点云帧间相对位姿约束信息、深度信息、帧间运动预测和状态约束信息进行融合计算得到多个约束因子240，多个约束因子240例如包括第一约束因子、第二约束因子和第三约束因子。
39.在得到多个约束因子240之后，可以以多个约束因子240中的至少一个作为状态输出，以校正惯性测量单元，校正过程例如包括基于惯性测量数据进行数值积分时，以多个约束因子240中的至少一个作为约束。
40.对于惯性测量数据，假设有两帧惯性测量数据对应的时间分别为tk，t
k+1
，这两帧时刻之间有多次imu和轮速计测量。忽略地球自转因素的影响，可以直接根据imu陀螺仪和轮速计的测量值计算得到这两帧所对应时刻的局部坐标系的相对位姿关系。
41.例如，可以直接对imu输出的角速度信息进行积分可得到旋转部分。旋转部分的一种示例计算方法为：
[0042][0043]
通过根据轮速计输出和外参关系直接计算出imu在局部坐标系的速度，进而对速度进行积分而得到位移部分。例如，imu在时刻t的速度计算方法为：
[0044][0045]
其中wheel_speed是轮速计的速度测量值，为标量。通过对该速度测量值进行积分即可得到帧间的相对位移。帧间的相对位移的计算方法为：
[0046][0047]
对于视觉图像数据，可以采用非直接法，即提取视觉图像数据的特征，并得到视觉图像帧(即，视觉帧)之间的运动约束。
[0048]
对于第二约束因子，可以利用激光雷达数据得到视觉图像数据的深度信息，并利用该深度信息确定针对激光雷达数据的第二约束因子。
[0049]
例如，激光雷达数据可以包括多帧激光点云数据。根据惯性测量数据和激光雷达数据，确定针对激光雷达数据的第二约束因子例如包括如下过程。
[0050]
首先，根据惯性测量数据、多帧激光点云数据中的第一帧激光点云数据和第二帧激光点云数据，确定第一帧激光点云数据与第二帧激光点云数据之间的初始相对位姿。
[0051]
然后，分别确定第一帧激光点云数据的第一投影数据和第二帧激光点云数据的第二投影数据。
[0052]
接下来，根据第一投影数据和第二投影数据，调整初始相对位姿，得到点云帧间相对位姿，并根据点云帧间相对位姿，确定第二约束因子。
[0053]
示例性地，第一投影数据和第二投影数据例如均包括n个体像素，n为大于1的整数。可以根据第一投影数据和第二投影数据，调整初始相对位姿，得到点云帧间相对位姿。例如，根据n个体像素中的每一个体像素和初始相对位姿，执行n次迭代操作，得到第n相对位姿作为点云帧间相对位姿。
[0054]
示例性地，第一投影数据例如包括第一投影图像和第一法向量，第二投影数据例如包括第二投影图像和第二法向量。通过执行n次迭代操作，得到第n相对位姿。
[0055]
例如，n次迭代中的第i次迭代包括：根据i确定距离阈值和角度阈值，i大于等于1且小于等于n。在确定第一投影图像中的第i第一像素与第二投影图像中的第i第二像素之间的距离小于等于距离阈值且第i像素的对应第一法向量与对应第二法向量之间的夹角小于等于角度阈值的情况下，利用第i-1次迭代得到的第i-1相对位姿、第i第一像素、第i第二像素、第i像素的对应第一法向量和第i像素的对应第二法向量，确定第i相对位姿。例如，第i像素例如包括第i第一像素和第i第二像素，第i第一像素对应第一法向量，第i第二像素对应第二法向量。
[0056]
根据本公开实施例，lidar(激光雷达)数据可以包括激光点云数据，可以采用两帧点云数据进行配准。因此理论上可以完成两帧点云配准的任何方法都适用于本公开实施例。优点在于实施方式更易扩展。
[0057]
通过对不同帧点云数据进行配准，可以得到相应时刻的imu坐标系的相对位姿关系。通过gpu执行的基于icp(iterative closet point，迭代最近点)方法，降低了两帧点云配准的耗时，进而可快速完成多对关键帧点云的配准。基于icp方法进行退化因子改进的方法，提高了原方法在lidar退化场景的精度。
[0058]
在一个示例中，根据两帧经过运动补偿的激光点云数据：a＝{ai}，b＝{bi}以及两帧点云的初始相对位姿：t0，得到修正后的两帧点云数据的相对位姿t。一个示例过程如下。
[0059]
将点云a和b中的点根据水平方向和竖直方向的角度投影到二维平面，并计算各点的法向量，生成第一投影图像a
uvmap
和第二投影图像b
uvmap
。
[0060]
t的初始值为t0，iteration_time表示迭代次数，初始值为0。对于n个点，执行以下操作，直到输出结果收敛。
[0061]
对于第i次迭代，
[0062]
根据iteration_time设置角度阈值angle_thresh，以及根据iteration_time设置距离阈值dis_thresh。在一个示例中，视觉图像数据的帧率为15hz，激光点云数据的帧率为10hz，距离阈值为max(0.5，4.0-0.5*iteration_time)m，角度阈值为max(10.0，30.0-5.0*iteration_time)
°
。
[0063]
根据t计算bi在a
uvmap
的2d投影位置，并根据2d距离查找匹配点在
且的情况下，否则ri←
0，ji←
0。
[0064]
接下来，即，执行并归求和。然后，利用退化因子h求解hδt＝g，得到δt。符号
“←”
表示赋值。
[0065]
对于第i+1次迭代，令iteration_time+1，重复执行上述计算过程。
[0066]
在每轮迭代中，距离阈值和角度阈值随着迭代次数而减小。这有利于随着迭代收敛排除掉动态物体之类的外点，提高配准精度。在一个示例中，迭代可以采用高斯-牛顿法，并利用退化因子保证只在非退化的自由度迭代更新t。退化因子定义为h的最小特征值，其所对应的特征向量即为特征退化自由度。
[0067]
应注意，以上算法可以在gpu上并行执行，由此可以减小cpu的资源占用，提高处理效率。
[0068]
本领域技术人员可以理解，对于视觉图像数据，可以基于多个视觉图像帧间构建一致的观测，用于后续图像特征点重投影误差的计算。
[0069]
对于第三约束因子，可以根据惯性测量数据、视觉图像数据和激光雷达数据，确定针对视觉图像数据的第三约束因子。
[0070]
例如，根据视觉图像数据和激光雷达数据，确定视觉图像数据的深度信息。然后，根据视觉图像数据和惯性测量数据，确定视觉图像数据和惯性测量数据之间的映射关系，并根据深度信息和映射关系，确定第三约束因子。
[0071]
关于深度信息，可以通过确定在视觉图像数据的视觉图像采集时刻相对于相邻雷达数据的雷达数据采集时刻t
l
的初始惯性测量相对位姿。然后，根据视觉图像采集时刻tc、雷达数据采集时刻和初始惯性测量相对位姿确定激光雷达数据到图像数据平面的第一投影位置接下来，根据第一投影位置确定图像投影时刻t
′c以及更新的惯性测量相对位姿并根据更新的惯性测量相对位姿确定视觉图像数据的深度信息λ
l
。
[0072]
示例性地，根据第一投影位置确定图像投影时刻以及更新的惯性测量相对位姿例如包括以下过程。首先，确定在视觉图像采集时刻相对于图像投影时刻的第一惯性测量相对位姿，并确定在图像投影时刻相对于激光数据采集时刻的第二惯性测量相对位姿。然后，利用第一惯性测量相对位姿和第二惯性测量相对位姿对初始惯性测量相对位姿进行插值，得到更新的惯性测量相对位姿。
[0073]
对于利用激光点云辅助计算深度的方法参考了limo(lidar monocular visual odometry)的实现。对于新的一帧视觉图像数据，会根据视觉图像数据的采集时刻tc得到相邻的雷达数据采集时刻t
l
，由此得到雷达数据采集时刻t
l
的激光点云数据。利用上文imu加轮速计直接积分的方法或者只对imu数据积分的方法可以得到时刻tc和时刻t
l
之间，imu坐标系的相对位姿关系
[0074]
利用该相对位姿关系和相机内参模型可以把点云数据投影到视觉图像平面上：
[0075][0076]
其中πc是相机投影模型，是imu坐标系到相机坐标系的外参，是激光点云中的点在imu坐标系下的坐标。
[0077]
对于rolling shutter快门的相机，上述点云到视觉图像平面的投影关系是有一定误差的。为了减小rolling shutter快门对投影误差的影响，实际计算时可以采用两次投影法。第一次投影利用上式(4)得到激光点投影后大概所在的行，第二次投影根据行号更新图像投影时间t
′c和所使用的相对位姿关系更新后的相对位姿采用线性插值的方法计算。以下图3示出了一种线性插值示例。
[0078]
图3示意性示出了根据本公开一实施例的插值计算的原理示意图。
[0079]
如图3所示，根据本公开的实施例，根据视觉图像数据的采集时刻tc得到相邻的雷达数据采集时刻t
l1
、t
l2
，基于点云数据在时刻t
l1
、t
l2
的深度信息d1和d2，将点云数据第一次投影到图像平面得到激光点投影后所在的行t
l-c
范围。然后，通过线性插值的方式进行第二次投影，例如基于深度信息d1+δd和d1+2δd和所在的行t
l-c
范围，将点云数据投影至与采集时刻t
c1
和t
c2
对应的视觉图像平面上。由此，可以通过插值方式得到采集时刻t
c1
和t
c2
的深度信息。
[0080]
通过投影的方法将激光雷达数据和视觉图像数据融合后，可以利用视觉特征点周围点云数据patch来计算视觉特征点的深度，示例方法与limo类似，不再赘述。
[0081]
本领域技术人员可以理解，除了以上根据本公开利用激光点云数据计算深度值的方法以外，还可以利用多帧视觉图像三角化的方法计算深度值，本公开不对此进行限制。由于激光雷达垂直视角往往比相机的视觉图像采集装置视角小，或者激光雷达的有效观测距离存在限制，会存在一些视觉图像区域无法被激光点云数据覆盖的情况。因此不能通过激光点云数据计算出部分视觉特征点的深度信息。对于这部分视觉特征点，可以通过多帧图像观测数据三角化的方法来计算特征点深度，即只对稳定跟踪超过一定帧数的特征点进行三角化。与两帧三角化相比，这种方法对特征点深度的估计更准确，进而降低了特征点深度估计误差对运动估计精度的负面影响。
[0082]
根据本公开实施例，可以采用基于滑动窗口的图优化方法。每个视觉或者激光雷达关键帧的状态可以用图的节点来表示，每个节点的状态定义为：
[0083][0084]
系统整体状态为：
[0085]
x＝[x0，x1，...，x
n-1
]
ꢀꢀꢀꢀ
(6)
[0086]
x为定位数据，自动驾驶车辆可以基于该定位数据进行定位。例如，可以将该定位数据用于环境感知、运动控制、相对定位、地图构建等领域。
[0087]
图4示意性示出了根据本公开一实施例的因子图的示意图。
[0088]
如图4所示，根据本公开的实施例，空心圆形表示lidar节点，空心正方形表示视觉节点，空心五角星表示视觉特征，点划线表示imu预积分因子，虚线表示icp因子，线段表示视觉因子。对于因子图中的边，在滑动窗口的长度达到最大值后，插入新的关键帧节点，并
删除最旧的关键帧节点。也可以删除不再有关联观测帧的图像数据特征，以保证计算复杂度不会随着时间推移而增大。在一个示例中，滑动窗口的长度最大值例如为40。图4中的imu预积分因子例如为第一约束因子，icp因子例如为第二约束因子，视觉因子例如为第三约束因子。
[0089]
为了优化定位数据x，可以基于第一约束因子、第二约束因子和第三约束因子，处理传感器数据，得到用于对自动驾驶车辆进行定位的定位数据。例如，可以将满足以下公式的x作为定位数据：
[0090][0091]
其中，为第一约束因子，为第三约束因子，为第二约束因子；为惯性测量数据，为视觉图像数据，为激光雷达数据。
[0092][0093]
imu预积分因子r
imu
，即根据imu预积分结果对相邻关键帧间状态创建的约束关系。根据本公开实施例，对视觉图像关键帧和激光点云关键帧创建了统一的串行imu预积分因子。采用imu预积分因子可以有效利用imu测量值对节点的所有维度的状态进行约束，进而为有效估计出节点的速度提供帮助。
[0094]
视觉因子r
vision
，即对视觉关键帧创建的约束关系。根据本公开实施例，采用重投影误差作为视觉因子，另外在计算重投影误差的时候消除了rolling shutter相机快门对平面投影坐标的影响。
[0095][0096][0097][0098][0099]
其中，分别是第i，j帧对视觉特征点l的观测；δti、δtj是在两帧视觉数据生成过程中imu坐标系的相对运动，使用预测的运动先验值，在优化过程中设为常量；是根据特征点所在相应成像平面行坐标进行运动线性插值后得到的对应时
刻imu坐标系的状态，这是对rolling shutter快门图像传感器成像时刻状态的修正；πc是相机投影模型，是相机投影模型的逆过程；λ
l
是视觉特征l在第i帧观测的逆深度值。例如，可以利用公式(4)将视觉图像数据与点云数据进行融合，得到融合图像，并根据融合图像中图像数据特征点周围的点云数据patch来计算逆深度值λ
l
，本公开不对此进行限制。
[0100]
icp因子r
icp
，即针对激光雷达关键帧创建的状态约束。
[0101][0102]
其中，为激光雷达前端输出的激光点云帧间相对位姿，例如，可以是上文中基于icp方法得到的t；log是3维刚体变换的对数映射，结果为6维向量。
[0103]
根据本公开的实施例，惯性测量数据是由惯性传感器采集的，可以根据激光雷达数据和第一约束因子，对惯性传感器的速度和加速度进行初始化。
[0104]
示例性地，需要对惯性传感器进行初始化，初始化的目的是通过构建初始关键帧序列，对关键帧节点的状态进行估计，包括全局位姿、速度、加速度计零偏、陀螺仪零偏等，并对重力方向进行估计，以便建立准确的系统初始状态，初始化是系统正常运行的前提。对于有视觉图像传感器的系统，在初始化过程中还需要建立视觉地图的尺度信息。如果初始化精度不高甚至不进行初始化，系统状态则很难收敛，从而降低了系统精度。
[0105]
根据本公开实施例，可以采用激光雷达数据对惯性传感器进行初始化。此外，还可以采用视觉图像数据和轮速计数据对惯性传感器进行初始化。
[0106]
在采用激光雷达数据对惯性传感器进行初始化的情况下，例如，在非退化场景通过lidar前端可以计算帧间相对运动。之后的初始化过程可以包括：陀螺仪零偏估计阶段以及速度和重力加速度估计阶段。
[0107]
对于陀螺仪零偏估计阶段，可以使用例如vins-mono(versatile monocular visual inertial state estimator，单目视觉惯性系统，香港科技大学开发的一种开源单目惯性视觉里程计vio)来实现。在陀螺仪零偏估计阶段，可以根据关键帧之间的旋转运动估计并结合imu对旋转运动的预积分结果来计算imu的陀螺仪零偏。更新imu的陀螺仪零偏后需要重新进行预积分，以保证预积分结果的准确性。具体过程例如可以参考相关文献(tong q，peiliang l，shaojie s.vins-mono：a robust and versatile monocular visual-inertial state estimator[j].ieee transactions on robotics，2018，pp(99)：1-17)。为了简洁，本公开的实施例不再赘述。
[0108]
对于速度和重力加速度估计阶段，要估计的状态变量为：
[0109][0110]
在速度和重力加速度估计阶段，可以结合运动估计结果和imu预积分中的速度和位移结果，构建如下初始化问题：
[0111][0112]
其中为滑动窗口内关键帧的集合。
[0113]
在该优化问题中，一种优化目标函数是imu预积分残差，表示imu预积分结果速度和位移分量与估计值的差异，从而提供对系统节点的速度状态和重力加速状态的约束：
[0114][0115]
其中是已知量，由激光雷达数据计算得到，不进行优化。
[0116]
另一种优化目标函数在于利用地球表面重力加速的模近似为常量9.8m/s2的事实。由此可以保证计算的重力加速度更为准确。
[0117]
根据本公开实施例，无需对可以直接观测到的旋转分量和平移分量进行优化，而只优化速度分量、imu陀螺仪零偏和重力加速度矢量。可以将imu加速度计的零偏初始值设置为零。上述方法使用范围广，即对于通过其他传感器得到帧间相对运动方法也适用。
[0118]
例如，可以利用视觉图像数据得到帧间约束对imu进行初始化，由视觉图像数据计算得到公式(16)中的即可。
[0119]
在没有激光雷达数据的情况下，可以采用视觉图像数据和轮速计数据进行初始化。由于使用imu和轮速计计算帧间运动时，假设陀螺仪零偏为零，因此这种初始化方法中省略了对陀螺仪零偏的估计，而直接假设为零。
[0120]
采用轮速计数据辅助初始化的优点在于实现简单，对初始运动条件要求低，且一般情况下精度足够，由此避免了单纯imu加视觉初始化对车辆机动性的要求导致尺寸过大。
[0121]
对包括视觉图像传感器的系统初始化，除需要对节点状态进行估计之外，还需要建立起视觉特征点的深度信息。在一个示例中，可以通过ba(bundle adjustment，光束平差法)来实现。
[0122]
根据本公开实施例的技术方案，由于需要处理视觉和激光雷达数据，因此对处理效率的要求更高。尤其是数据处理计算量更大，因此有必要对视觉图像数据和激光雷达帧数据进行并行处理。但由于在数据融合部分，将视觉关键帧节点和激光雷达关键帧节点利用同一个因子图进行状态优化，且之前通过imu预积分因子，按照时间顺序依次连接视觉关键帧节点和激光雷达关键帧节点，因此需要imu预积分的操作为串行处理。类似地，对于数据融合部分，由于整个系统共享同一个滑动窗口，且滑动窗口中包含视觉图像关键帧和激光雷达关键帧，因此滑动窗口更新和融合优化操作需要进行线程同步，即，并行处理。有鉴于此，可以使用gpu执行并行处理，因此能够减少cpu资源占用，合理利用gpu资源。
[0123]
根据一种实现方式，可以分别创建独立的线程来处理视觉图像数据和激光雷达数据，从而实现并行处理。而对于imu预积分等串行处理，可以通过线程同步来保证时序的正确性。
[0124]
图5示意性示出了根据本公开一实施例的针对自动驾驶车辆的数据处理装置的框图。
[0125]
如图5所示，本公开实施例的针对自动驾驶车辆的数据处理装置500例如包括获取模块510、第一确定模块520、第二确定模块530、第三确定模块540以及处理模块550。
[0126]
获取模块510可以用于获取针对自动驾驶车辆的传感器数据，传感器数据包括惯性测量数据、激光雷达数据和视觉图像数据。根据本公开实施例，获取模块510例如可以执行上文参考图1描述的操作s110，在此不再赘述。
[0127]
第一确定模块520可以用于根据惯性测量数据和视觉图像数据，确定针对惯性测
量数据的第一约束因子。根据本公开实施例，第一确定模块520例如可以执行上文参考图1描述的操作s120，在此不再赘述。
[0128]
第二确定模块530可以用于根据惯性测量数据和激光雷达数据，确定针对激光雷达数据的第二约束因子。根据本公开实施例，第二确定模块530例如可以执行上文参考图1描述的操作s130，在此不再赘述。
[0129]
第三确定模块540可以用于根据惯性测量数据、视觉图像数据和激光雷达数据，确定针对视觉图像数据的第三约束因子。根据本公开实施例，第三确定模块540例如可以执行上文参考图1描述的操作s140，在此不再赘述。
[0130]
处理模块550可以用于基于第一约束因子、第二约束因子和第三约束因子，处理传感器数据，得到用于对自动驾驶车辆进行定位的定位数据。根据本公开实施例，处理模块550例如可以执行上文参考图1描述的操作s150，在此不再赘述。
[0131]
根据本公开的实施例，激光雷达数据包括多帧激光点云数据；第二确定模块530包括：第一确定子模块、第二确定子模块、调整子模块和第三确定子模块。第一确定子模块，用于根据惯性测量数据、多帧激光点云数据中的第一帧激光点云数据和第二帧激光点云数据，确定第一帧激光点云数据与第二帧激光点云数据之间的初始相对位姿；第二确定子模块，用于分别确定第一帧激光点云数据的第一投影数据和第二帧激光点云数据的第二投影数据；调整子模块，用于根据第一投影数据和第二投影数据，调整初始相对位姿，得到点云帧间相对位姿；第三确定子模块，用于根据点云帧间相对位姿，确定第二约束因子。
[0132]
根据本公开的实施例，第一投影数据和第二投影数据均包括n个体像素，n为大于1的整数；根调整子模块还用于：根据n个体像素中的每一个体像素和初始相对位姿，执行n次迭代操作，得到第n相对位姿，作为点云帧间相对位姿。
[0133]
根据本公开的实施例，第一投影数据包括第一投影图像和第一法向量，第二投影数据包括第二投影图像和第二法向量；调整子模块还用于：n次迭代中的第i次迭代包括：根据i确定距离阈值和角度阈值，i大于等于1且小于等于n；在确定第一投影图像中的第i第一像素与第二投影图像中的第i第二像素之间的距离小于等于距离阈值且第i像素的对应第一法向量与对应第二法向量之间的夹角小于等于角度阈值的情况下，利用第i-1次迭代得到的第i-1相对位姿、第i第一像素、第i第二像素、第i像素的对应第一法向量和第i像素的对应第二法向量，确定第i相对位姿。
[0134]
根据本公开的实施例，第三确定模块540包括：第四确定子模块、第五确定子模块和第六确定子模块。第四确定子模块，用于根据视觉图像数据和激光雷达数据，确定视觉图像数据的深度信息；第五确定子模块，用于根据视觉图像数据和惯性测量数据，确定视觉图像数据和惯性测量数据之间的映射关系；第六确定子模块，用于根据深度信息和映射关系，确定第三约束因子。
[0135]
根据本公开的实施例，第四确定子模块包括：第一确定单元、第二确定单元、第三确定单元和第四确定单元。第一确定单元，用于确定在视觉图像数据的视觉图像采集时刻相对于相邻雷达数据的雷达数据采集时刻的初始惯性测量相对位姿；第二确定单元，用于根据视觉图像采集时刻、雷达数据采集时刻和初始惯性测量相对位姿，确定激光雷达数据到视觉图像数据平面的第一投影位置；第三确定单元，用于根据第一投影位置确定图像投影时刻以及更新的惯性测量相对位姿；第四确定单元，用于根据更新的惯性测量相对位姿
确定视觉图像数据的深度信息。
[0136]
根据本公开的实施例，第三确定单元包括：第一确定子单元、第二确定子单元和插值子单元。第一确定子单元，用于确定在视觉图像采集时刻相对于图像投影时刻的第一惯性测量相对位姿；第二确定子单元，用于确定在图像投影时刻相对于激光数据采集时刻的第二惯性测量相对位姿；插值子单元，用于利用第一惯性测量相对位姿和第二惯性测量相对位姿对初始惯性测量相对位姿进行插值，得到更新的惯性测量相对位姿。
[0137]
根据本公开的实施例，处理模块550用于将满足以下公式的x作为定位数据：其中，为第一约束因子，为第三约束因子，为第二约束因子；为惯性测量数据，为视觉图像数据，为激光雷达数据。
[0138]
根据本公开的实施例，惯性测量数据是由惯性传感器采集的；装置500还可以包括：初始化模块，用于根据激光雷达数据和第一约束因子，对惯性传感器的速度和加速度进行初始化。
[0139]
本公开的技术方案中，所涉及的用户个人信息的收集、存储、使用、加工、传输、提供和公开等处理，均符合相关法律法规的规定，且不违背公序良俗。
[0140]
根据本公开的实施例，本公开还提供了一种电子设备、一种可读存储介质和一种计算机程序产品。
[0141]
根据本公开的实施例，提供了一种自动驾驶车辆，包括图6的电子设备。
[0142]
图6是用来实现本公开实施例的用于执行针对自动驾驶车辆的数据处理的电子设备的框图。
[0143]
图6示出了可以用来实施本公开实施例的示例电子设备600的示意性框图。电子设备600旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本公开的实现。
[0144]
如图6所示，设备600包括计算单元601，其可以根据存储在只读存储器(rom)602中的计算机程序或者从存储单元608加载到随机访问存储器(ram)603中的计算机程序，来执行各种适当的动作和处理。在ram 603中，还可存储设备600操作所需的各种程序和数据。计算单元601、rom 602以及ram 603通过总线604彼此相连。输入/输出(i/o)接口605也连接至总线604。
[0145]
设备600中的多个部件连接至i/o接口605，包括：输入单元606，例如键盘、鼠标等；输出单元607，例如各种类型的显示器、扬声器等；存储单元608，例如磁盘、光盘等；以及通信单元609，例如网卡、调制解调器、无线通信收发机等。通信单元609允许设备600通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
[0146]
计算单元601可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元601的一些示例包括但不限于中央处理单元(cpu)、图形处理单元(gpu)、各种专用的人工
智能(ai)计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(dsp)、以及任何适当的处理器、控制器、微控制器等。计算单元601执行上文所描述的各个方法和处理，例如针对自动驾驶车辆的数据处理方法。例如，在一些实施例中，针对自动驾驶车辆的数据处理方法可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元608。在一些实施例中，计算机程序的部分或者全部可以经由rom 602和/或通信单元609而被载入和/或安装到设备600上。当计算机程序加载到ram 603并由计算单元601执行时，可以执行上文描述的针对自动驾驶车辆的数据处理方法的一个或多个步骤。备选地，在其他实施例中，计算单元601可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行针对自动驾驶车辆的数据处理方法。
[0147]
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、现场可编程门阵列(fpga)、专用集成电路(asic)、专用标准产品(assp)、芯片上系统的系统(soc)、负载可编程逻辑设备(cpld)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
[0148]
用于实施本公开的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程针对自动驾驶车辆的数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
[0149]
在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(ram)、只读存储器(rom)、可擦除可编程只读存储器(eprom或快闪存储器)、光纤、便捷式紧凑盘只读存储器(cd-rom)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
[0150]
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，crt(阴极射线管)或者lcd(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
[0151]
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算
系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(lan)、广域网(wan)和互联网。
[0152]
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，也可以为分布式系统的服务器，或者是结合了区块链的服务器。
[0153]
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本公开中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本公开公开的技术方案所期望的结果，本文在此不进行限制。
[0154]
上述具体实施方式，并不构成对本公开保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本公开的精神和原则之内所作的修改、等同替换和改进等，均应包含在本公开保护范围之内。