CN110673115A - 雷达与组合导航系统的联合标定方法、装置、设备及介质 - Google Patents

Abstract

本申请实施例提供一种雷达与组合导航系统的联合标定方法、装置、设备及介质，其中，该方法通过获取自动驾驶车辆搭载的激光雷达在第一时刻和第二时刻采集得到的点云数据，以及组合导航系统在第一时刻和第二时刻采集得到的位置和姿态数据，将激光雷达采集获得的点云数据进行配准处理，得到第一齐次变换矩阵，根据组合导航系统采集得到的位置和姿态数据，计算得到第二齐次变换矩阵，根据第一齐次变换矩阵和第二齐次变换矩阵，来完成激光雷达与组合导航系统之间的联合标定。针对雷达与组合导航系统的联合标定，本申请实施例提供的方案对环境条件的要求较低，削弱了环境条件对标定准确性的影响，提高了标定的准确性。

Description

雷达与组合导航系统的联合标定方法、装置、设备及介质

技术领域

本申请实施例涉及自动驾驶技术领域，尤其涉及一种雷达与组合导航系统的联合标定方法、装置、设备及介质。

背景技术

在自动驾驶车辆中，激光雷达和INS/GPS组合导航系统是车辆的重要组成部分，其中激光雷达可以用于对车辆周围的障碍物进行检测，INS/GPS组合导航系统可以用于对车辆的位置以及姿态进行检测，在实际使用中，需要将激光雷达和INS/GPS组合导航系统进行联合标定，以使激光雷达检测到的障碍物的坐标能够从激光雷达坐标系转换到INS/GPS组合导航系统坐标系。

目前，用于实现激光雷达坐标系和INS/GPS组合导航系统坐标系之间联合标定的相关技术主要包括两种：一种是人工手动校准方法，该方法误差较大，准确性较低，同时无法很好的估计激光雷达坐标系与INS/GPS组合导航系统坐标系之间的旋转角度信息，另一种是基于手眼标定模型的三维激光雷达外参数标定方法，该方法要求同时具备上坡和转弯环境的采集数据，对环境条件要求较高，且校准模型对环境噪声比较敏感，容易受噪声影响，同样存在标定准确性较低的问题。

发明内容

本申请实施例提供一种雷达与组合导航系统的联合标定方法、装置、设备及介质，用以在实现激光雷达与组合导航系统之间的联合标定的同时，降低标定对环境条件的要求，减少环境条件对标定准确性的影响，提高标定的准确性。

本申请实施例第一方面提供一种雷达与组合导航系统的联合标定方法，该方法包括：获取自动驾驶车辆上搭载的激光雷达在第一时刻和第二时刻采集获得的点云数据，以及获取所述自动驾驶车辆上搭载的组合导航系统在所述第一时刻和所述第二时刻采集获得的位置和姿态数据，其中，在所述第一时刻和所述第二时刻采集获得的点云数据中包含相同物体的点云，所述自动驾驶车辆在所述第一时刻和所述第二时刻的位置和/或姿态不同；将所述激光雷达在所述第一时刻采集获得的点云数据与所述激光雷达在所述第二时刻采集获得的点云数据进行配准处理，获得所述激光雷达在所述第一时刻和所述第二时刻下激光雷达坐标系之间的第一齐次变换矩阵；根据所述组合导航系统在所述第一时刻和所述第二时刻采集得到的位置和姿态数据，计算得到所述组合导航系统在所述第一时刻和所述第二时刻下组合导航坐标系之间的第二齐次变换矩阵；基于所述第一齐次变换矩阵和所述第二齐次变换矩阵，确定所述激光雷达坐标系与所述组合导航坐标系之间的第三齐次变换矩阵，以根据所述第三齐次变换矩阵，确定所述激光雷达检测到的障碍物距离所述自动驾驶车辆的距离，并根据所述距离对所述自动驾驶车辆进行控制。

本申请实施例第二方面提供一种控制装置，该装置包括：

获取模块，用于获取自动驾驶车辆上搭载的激光雷达在第一时刻和第二时刻采集获得的点云数据，以及获取所述自动驾驶车辆上搭载的组合导航系统在所述第一时刻和所述第二时刻采集获得的位置和姿态数据，其中，在所述第一时刻和所述第二时刻采集获得的点云数据中包含相同物体的点云，所述自动驾驶车辆在所述第一时刻和所述第二时刻的位置和/或姿态不同。

配准处理模块，用于将所述激光雷达在所述第一时刻采集获得的点云数据与所述激光雷达在所述第二时刻采集获得的点云数据进行配准处理，获得所述激光雷达在所述第一时刻和所述第二时刻下激光雷达坐标系之间的第一齐次变换矩阵。

计算模块，用于根据所述组合导航系统在所述第一时刻和所述第二时刻采集得到的位置和姿态数据，计算得到所述组合导航系统在所述第一时刻和所述第二时刻下组合导航坐标系之间的第二齐次变换矩阵。

确定模块，用于基于所述第一齐次变换矩阵和所述第二齐次变换矩阵，确定所述激光雷达坐标系与所述组合导航坐标系之间的第三齐次变换矩阵。

控制模块，用于根据所述第三齐次变换矩阵，确定所述激光雷达检测到的障碍物距离所述自动驾驶车辆的距离，并根据所述距离对所述自动驾驶车辆进行控制。

本申请实施例第三方面提供一种自动驾驶车辆，该车辆包括激光雷达、组合导航系统以及处理器和存储器；其中激光雷达用于对所述自动驾驶车辆周围的障碍物进行检测，获得点云数据，组合导航系统用于采集所述自动驾驶车辆的位置和姿态数据，存储器中存储有指令，当所述处理器执行所述指令时用以执行上述第一方面所述的方法。

本申请实施例第四方面提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现上述第一方面所述的方法。

基于以上各方面，本申请实施例提供的雷达与组合导航系统的联合标定方法、装置、设备及介质，通过获取自动驾驶车辆搭载的激光雷达在第一时刻和第二时刻采集得到的点云数据，以及组合导航系统在第一时刻和第二时刻采集得到的位置和姿态数据，将激光雷达在第一时刻采集获得的点云数据与激光雷达在第二时刻采集获得的点云数据进行配准处理，得到第一齐次变换矩阵，并根据组合导航系统在第一时刻和第二时刻采集得到的位置和姿态数据，计算得到第二齐次变换矩阵，从而根据第一齐次变换矩阵和第二齐次变换矩阵，来完成激光雷达与组合导航系统之间的联合标定。一方面由于本申请实施例只要求用于联合标定的数据是在自动驾驶车辆位置不同和/或姿态不同的时刻采集得到的即可，对环境条件没有要求，因此本申请实施例能够降低联合标定对环境条件的要求，另一方面，本申请实施例是根据激光雷达坐标系在不同时刻之间的齐次转换矩阵以及组合导航系统在不同时刻之间的齐次转换矩阵，来确定激光雷达与组合导航系统之间的标定关系的，不需要借助于相关技术的手眼标定模型或者是类似的模型，因此环境噪声对本申请实施例的标定结果影响较小，能够提高标定的准确性。

应当理解，上述发明内容部分中所描述的内容并非旨在限定本申请的实施例的关键或重要特征，亦非用于限制本申请的范围。本公申请的其它特征将通过以下的描述变得容易理解。

附图说明

图1是本申请实施例提供的一种自动驾驶的场景示意图；

图2是本申请实施例提供的一种雷达与组合导航系统的联合标定方法的流程图；

图3a是本申请实施例提供的一种数据采集的示意图；

图3b是本申请实施例提供的另一种数据采集的示意图；

图4是本申请实施例提供的另一种雷达与组合导航系统的联合标定方法的流程图；

图5是本申请实施例提供的一种控制装置的结构示意图；

图6是本申请实施例提供的另一种控制装置的结构示意图；

图7是本申请实施例提供的一种自动驾驶车辆的结构示意图。

具体实施方式

下面将参照附图更详细地描述本申请的实施例。虽然附图中显示了本申请的某些实施例，然而应当理解的是，本申请可以通过各种形式来实现，而且不应该被解释为限于这里阐述的实施例，相反提供这些实施例是为了更加透彻和完整地理解本申请。应当理解的是，本申请的附图及实施例仅用于示例性作用，并非用于限制本申请的保护范围。

本申请实施例的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请实施例例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

图1是本申请实施例提供的一种自动驾驶的场景示意图，在图1中车辆10执行自动驾驶操作。其中，车辆10上搭载有激光雷达11、组合导航系统12和控制器14，激光雷达11用于对车辆10周围的信息进行采集，获得车辆周围(比如，车辆前方)的点云数据。组合导航系统12包括惯性测量单元(Inertial measurement unit,简称IMU)和定位设备(为了便于理解可示例性的理解为GPS定位设备)IMU用于检测车辆10的姿态，比如偏航角、俯仰角和横滚角等。定位设备用于检测车辆10的位置，比如当定位设备具体为GPS设备时，定位设备检测得到的车辆位置包括车辆的经纬度信息。控制器14用于根据激光雷达11和组合导航系统12采集到的数据，控制车辆10的运行状态，比如在遇到障碍物时，调整车辆10的移动轨迹，执行避障操作。如图1所示，在车辆10的移动过程中，若遇到障碍物13，激光雷达11获得障碍物13在其自身坐标系(即激光雷达坐标系)下的位置，并将该障碍物在激光雷达坐标系下的位置发送给处理器14，处理器14根据预先标定的激光雷达与联合导航系统之间的标定关系，将障碍物13在激光雷达坐标系下的位置映射到联合导航系统的坐标系中，并根据联合导航系统检测得到的车辆位置，确定障碍物与车辆10之间的相对位置，若障碍物13与车辆10之间的距离小于预设距离，则根据障碍物13与车辆10之间的相对位置执行相应的自动驾驶策略，以达到避障的目的。当然图1仅是一种可能的应用场景，并不是本申请实施例能够应用的唯一场景，比如在一些实施方式中，本申请实施例还可以应用在诸如自动倒车等场景。但是不管应用于何种应用场景，显然的，对于自动驾驶技术来说，激光雷达与联合导航系统之间的标定关系对于自动驾驶来说都是十分重要的，但是由背景技术部分介绍的相关技术可知，现有的标定方法都存在着这样或那样的缺陷，使得标定过程复杂，条件苛刻，准确性低。那么如何克服相关技术的缺陷，并提供一种方便、可靠、有效、准确的标定方法就显得十分重要。

针对相关技术存在的缺陷，本申请实施例提供了一种雷达与组合导航系统的联合标定方案，其中，图2是本申请实施例提供的一种雷达与组合导航系统的联合标定方法的流程图，该方法可以由自动驾驶设备上搭载的控制装置来执行，示例的，通过该方法标定后的激光雷达和组合导航系统，可应用于图1所示的场景，以实现自动驾驶设备的自动避障操作。如图2所示，该方法包括如下步骤：

步骤201、获取自动驾驶车辆上搭载的激光雷达在第一时刻和第二时刻采集获得的点云数据，以及获取自动驾驶车辆上搭载的组合导航系统在第一时刻和第二时刻采集获得的位置和姿态数据，其中，在第一时刻和所述第二时刻采集获得的点云数据中包含相同物体的点云，自动驾驶车辆在第一时刻和所述第二时刻的位置和/或姿态不同。

示例的，在本实施例中自动驾驶车辆(以下简称车辆)可以在任意的环境中沿任意行驶路线，以除静止之外的任意行驶状态行驶，比如，车辆可以原地进行转圈行驶，或者向前、后、左、右等方向中的至少一个方向行驶，使得车辆的位置和/姿态(比如，偏航角、俯仰角、横滚角等。)发生变化。在车辆行驶过程中车辆上搭载的激光雷达对车辆周围的环境进行检测，获得第一时刻和第二时刻的点云数据，与此同时车辆搭载的组合导航系统采集车辆在第一时刻和第二时刻的位置和姿态数据。

示例的，图3a是本申请实施例提供的一种数据采集的示意图，在图3a中，车辆上搭载的激光雷达和组合导航系统按照预设时间间隔采集数据，此时，若车速不是恒定不变的，那么相邻采样位置之间的距离可能不同。在完成数据后，对各时刻采集到的点云数据中的物体进行识别，并对各时刻的车辆位置和姿态进行判断，若在t1时刻和t3时刻的点云数据中包含相同物体A，同时车辆位置和/或姿态发生变化，则分别将t1和t3确定为第一时刻和第二时刻，并将在t1和t3采集到的数据作为本次联合标定的输入数据。其中，需要说明的是虽然在图3a中第一时刻和第二时刻不是相邻的两个数据采集时刻，但是在其他应用场景中，第一时刻和第二时刻可以是两个相邻的数据采集时刻。

另外，在一些实施方式中，为了保证相邻时刻或不同时刻的点云数据中包括相同物体的点云，可以根据需要将预设时间间隔设置的小一些，比如小于一个预设时长，其中，预设时长可以根据需要进行设定，比如，在一些示例中可以将预设时长设置为1.5秒，第一时刻和第二时刻可以为时间间隔为1秒的两个时刻，也就是说，本实施例中所称的第一时刻和第二时刻可以是时间间隔小于预设时间长的两个时刻。

图3b是本申请实施例提供的另一种数据采集的示意图，在图3b中，车辆上搭载的激光雷达和组合导航系统按照预设距离间隔采集数据，即车辆每移动预设距离激光雷达和组合导航系统就进行一次数据采集，采集位置分别为z1、z2、z3、z4。其中，若在位置z1、z2采集的点云数据中包括相同物体B的点云，则分别将车辆移动到位置z1、位置z2所对应的时刻确定为第一时刻和第二时刻，并将在位置z1和位置z2采集的数据作为联合标定的输入数据。

类似的，为了保证相邻时刻或不同时刻的点云数据中包括相同物体的点云，可以根据需要将预设距离间隔设置的小一些，比如小于一个预设距离，其中，预设时长可以根据需要进行设定，比如，在一些示例中可以将预设距离设置为10米，第一时刻和第二时刻可以为距离间隔小于10米的两个数据采集时刻，也就是说，车辆在第一时刻的位置与车辆在第二时刻的位置之间的距离小于预设距离，当然这里仅为示例说明并不是对预设距离的唯一限定。

步骤202、将激光雷达在第一时刻采集获得的点云数据与激光雷达在第二时刻采集获得的点云数据进行配准处理，获得激光雷达在第一时刻和第二时刻下激光雷达坐标系之间的第一齐次变换矩阵。

示例的，在本实施例中可以将第一时刻和第二时刻采集到的点云数据作为输入，使用ICP算法或者其他点云配准算法对两个时刻的点云数据进行配准处理，获得在激光雷达坐标系下由第一时刻到第二时刻的第一齐次变换矩阵。

步骤203、根据组合导航系统在第一时刻和第二时刻采集得到的位置和姿态数据，计算得到组合导航系统在第一时刻和第二时刻下组合导航坐标系之间的第二齐次变换矩阵。

在实际的实施方式中，可以将第一时刻和第二时刻采集到的车辆位置和姿态数据作为输入，对于两个时刻的数据，分别将组合导航系统在第一时刻采集到的位置(比如经纬度)转换为UTM坐标系下的第一位置(x1,y1,z1)，将组合导航系统在第二时刻采集到的位置转换为UTM坐标系下的第二位置(x2,y2,z2)。

进一步的，根据第一位置(x1,y1,z1)和组合导航系统在第一时刻采集到的姿态数据，计算得到组合导航坐标系到UTM坐标系的第一变换矩阵，示例的，假设组合导航系统在第一时刻采集到的偏航角、俯仰角和横滚角分别为α、β、γ，则由组合导航坐标系到UTM坐标系的第一变换矩阵可以表示为:

其中，

进一步的，根据第二位置(x2,y2,z2)以及第二时刻采集到的姿态数据，确定组合导航坐标系到UTM坐标系的第二变换矩阵，假设计作Pb，则组合导航系统在第一时刻和第二时刻下组合导航坐标系之间的第二齐次变换矩阵为：

P＝Pb^-1Pa

步骤204、基于第一齐次变换矩阵和第二齐次变换矩阵，确定激光雷达坐标系与组合导航坐标系之间的第三齐次变换矩阵，以根据第三齐次变换矩阵，确定激光雷达检测到的障碍物距离自动驾驶车辆的距离，并根据该距离对自动驾驶车辆进行控制。

具体的，假设根据步骤202得到的第一齐次变换矩阵矩阵为Q，根据步骤103得到的第二齐次变换矩阵为P，那么可以根据关系PX＝XQ,计算得到激光雷达坐标系与组合导航坐标系之间的第三齐次变换矩阵X。

进一步的，在得到第三齐次变换矩阵X之后，可以将第三齐次变换矩阵X应用到自动驾驶过程，车辆执行自动驾驶操作时，激光雷达对车辆周围环境进行检测，在检测到障碍物时，车辆上搭载的控制器根据第三齐次变换矩阵X将障碍物在激光雷达坐标系下的坐标映射到组合导航坐标系下的坐标，并根据组合导航检测到的车辆位置，确定车辆与障碍物之间的距离，从而根据该距离执行相应的自动驾驶策略。

本实施例，通过获取自动驾驶车辆搭载的激光雷达在第一时刻和第二时刻采集得到的点云数据，以及组合导航系统在第一时刻和第二时刻采集得到的位置和姿态数据，将激光雷达在第一时刻采集获得的点云数据与激光雷达在第二时刻采集获得的点云数据进行配准处理，得到第一齐次变换矩阵，并根据组合导航系统在第一时刻和第二时刻采集得到的位置和姿态数据，计算得到第二齐次变换矩阵，从而根据第一齐次变换矩阵和第二齐次变换矩阵，来完成激光雷达与组合导航系统之间的联合标定。一方面由于本实施例只要求用于联合标定的数据是在自动驾驶车辆位置不同和/或姿态不同的时刻采集得到的即可，对环境条件没有要求，因此能够降低联合标定对环境条件的要求，另一方面，本实施例是根据激光雷达坐标系在不同时刻之间的齐次转换矩阵以及组合导航系统在不同时刻之间的齐次转换矩阵，来确定激光雷达与组合导航系统之间的标定关系的，不需要借助于相关技术的手眼标定模型或者是类似的模型，因此环境噪声对本实施例的标定结果影响较小，能够提高标定的准确性。

图4是本申请实施例提供的另一种雷达与组合导航系统的联合标定方法的流程图，如图4所示，该方法包括：

步骤401、获取自动驾驶车辆上搭载的激光雷达和组合导航系统采集到的多组数据对，每组数据对中包括激光雷达和组合导航系统在两个时刻同时采集到的点云数据、车辆位置以及车辆姿态数据。

其中，对于每组数据来说，激光雷达在两个时刻采集到的点云数据中包括相同物体的点云，组合导航系统在两个时刻采集到的车辆的位置和/或姿态不同。

举例来说，假设t1时刻采集到的点云数据为d1，位置为w1，姿态数据为z1；t2时刻采集到的点云数据为d2，位置为w2，姿态数据为z2；t3时刻采集到的点云数据为d3，位置为w3，姿态数据为z3；t4时刻采集到的点云数据为d4，位置为w4，姿态数据为z4，若在t1时刻和t2时刻中，点云数据d1和d2包括相同物体的点云，位置w1和w2不同，则t1时刻和t2时刻的数据可以构成一组数据对G1((t1,d1,w1,z1),(t2,d2,w2,z2))，若在t3时刻和t4时刻中，点云数据d3和d4包括相同物体的点云，位置w3和w3相同，但z3和z4不同，则t3时刻和t4时刻的数据可以构成另一组数据对G2((t3,d3,w3,z3),(t4,d4,w4,z4))。当然这里仅为示例说明。

步骤402、分别基于获取到的每组数据对，计算激光雷达坐标系与组合导航坐标系之间的第三齐次变换矩阵。

在本实施例中基于数据对计算激光雷达坐标系与组合导航坐标系之间齐次变换矩阵的方法可以参考图2实施例的方法，在这里不再赘述。比如，获取到的数据对包括G1((t1,d1,w1,z1),(t2,d2,w2,z2))和G2((t3,d3,w3,z3),(t4,d4,w4,z4))，那么需要分别根据数据对G1((t1,d1,w1,z1),(t2,d2,w2,z2))和G2((t3,d3,w3,z3),(t4,d4,w4,z4))计算得到两个第三齐次变换矩阵C1和C2。

步骤403、根据获取到的每组数据对以及基于每组数据对计算得到的第三齐次变换矩阵，采用弗罗贝尼乌斯范数从计算得到的多个第三齐次变换矩阵中确定出误差最小的作为目标齐次变换矩阵，从而根据目标齐次变换矩阵，确定激光雷达检测到的障碍物距离自动驾驶车辆的距离，并根据距离对自动驾驶车辆进行控制。

示例的，本实施例可采用GPU从计算得到的多个第三齐次变换矩阵中加速搜索得到误差最小的齐次变换矩阵。比如，在搜索前可以通过预设的配置界面获取用户对标定参数的搜索范围，其中需要用户配置的标定参数的搜索范围包括：组合导航坐标系下的三个坐标轴方向的搜索范围、偏航方向上的搜索范围、俯仰方向上的搜索范围以及横滚方向上的搜索范围，比如在一些设置中，可将三个坐标轴的搜索范围设置为-5米到5米，将偏航方向、俯仰方向和横滚方向的搜索范围设置为-3到3弧度。

进一步的，在获得标定参数的搜索范围后，可将步骤402计算得到的多个第三齐次变换矩阵分别转换为相应的标定参数，比如在三个坐标轴上的位移量，在偏航方向、俯仰方向和横滚方向上的偏移角度等，再根据用户设置的搜索范围，从多个第三齐次变换矩阵中确定出符合该搜索范围的一个或多个第三齐次变换矩阵，通过将所述一个或多个第三齐次变换矩阵以及相应的数据对代入如下弗罗贝尼乌斯范数关系式，得到每个第三齐次变换矩阵对应的误差，并将误差最小的作为目标齐次变换矩阵:

其中，k为符合搜索范围的第三齐次变换矩阵的个数，X为符合搜索范围的第三齐次变换矩阵，P和Q为基于该第三齐次变换矩阵对应的数据对计算得到的第一齐次变换矩阵和第二齐次变换矩阵。

本实施例通过图2实施例的方法获取多个第三齐次变换矩阵，再采用采用弗罗贝尼乌斯范数的方法从多个第三齐次变换矩阵中确定目标齐次变换矩阵，并将目标齐次变换矩阵作为标定结果，能够进一步提高标定的准确性，且通过对搜索范围的设置，可以对标定结果的精度进行控制，以满足不同场景的不同需要。

图5是本申请实施例提供的一种控制装置的结构示意图，如图5所示，控制装置50包括：

获取模块51，用于获取自动驾驶车辆上搭载的激光雷达在第一时刻和第二时刻采集获得的点云数据，以及获取所述自动驾驶车辆上搭载的组合导航系统在所述第一时刻和所述第二时刻采集获得的位置和姿态数据，其中，在所述第一时刻和所述第二时刻采集获得的点云数据中包含相同物体的点云，所述自动驾驶车辆在所述第一时刻和所述第二时刻的位置和/或姿态不同。

配准处理模块52，用于将所述激光雷达在所述第一时刻采集获得的点云数据与所述激光雷达在所述第二时刻采集获得的点云数据进行配准处理，获得所述激光雷达在所述第一时刻和所述第二时刻下激光雷达坐标系之间的第一齐次变换矩阵。

计算模块53，用于根据所述组合导航系统在所述第一时刻和所述第二时刻采集得到的位置和姿态数据，计算得到所述组合导航系统在所述第一时刻和所述第二时刻下组合导航坐标系之间的第二齐次变换矩阵。

确定模块54，用于基于所述第一齐次变换矩阵和所述第二齐次变换矩阵，确定所述激光雷达坐标系与所述组合导航坐标系之间的第三齐次变换矩阵.

控制模块55，用于根据所述第三齐次变换矩阵，确定所述激光雷达检测到的障碍物距离所述自动驾驶车辆的距离，并根据所述距离对所述自动驾驶车辆进行控制。

在一种实施方式中，第一时刻和所述第二时刻之间的时间间隔小于预设时长，或者所述自动驾驶车辆在所述第一时刻的位置和所述自动驾驶车辆在所述第二时刻的位置之间的距离小于预设距离。

在一种实施方式中，所述计算模块53，包括：

坐标转换子模块，用于分别将所述组合导航系统在所述第一时刻采集到的位置转换为UTM坐标系下的第一位置，将在所述第二时刻采集到的位置转换为所述UTM坐标系下的第二位置。

确定子模块，用于根据所述第一位置以及第一时刻采集到的姿态数据，确定组合导航坐标系到所述UTM坐标系的第一变换矩阵，以及根据所述第二位置以及第二时刻采集到的姿态数据，确定组合导航坐标系到所述UTM坐标系的第二变换矩阵。

计算子模块，用于根据所述第一变换矩阵和所述第二变换矩阵，计算得到所述组合导航系统在所述第一时刻和所述第二时刻下组合导航坐标系之间的第二齐次变换矩阵。

本实施例提供的控制装置，其执行方式和有益效果与图2实施例类似，在这里不再赘述。

图6是本申请实施例提供的另一种控制装置的结构示意图，如图6所示，控制装置60包括：

获取模块61，用于获取自动驾驶车辆上搭载的激光雷达和组合导航系统采集到的多组数据对，每组数据对中包括激光雷达和组合导航系统在两个时刻同时采集到的点云数据、车辆位置以及车辆姿态数据。

其中，对于每组数据来说，激光雷达在两个时刻采集到的点云数据中包括相同物体的点云，组合导航系统在两个时刻采集到的车辆的位置和/或姿态不同。

计算模块62，用于分别基于获取到的每组数据对，计算激光雷达坐标系与组合导航坐标系之间的第三齐次变换矩阵。

以及确定模块63，用于根据获取到的每组数据对以及基于每组数据对计算得到的第三齐次变换矩阵，采用弗罗贝尼乌斯范数从计算得到的多个第三齐次变换矩阵中确定出误差最小的作为目标齐次变换矩阵，从而根据目标齐次变换矩阵，确定激光雷达检测到的障碍物距离自动驾驶车辆的距离，并根据距离对自动驾驶车辆进行控制。

本实施例提供的控制装置，其执行方式和有益效果与图4实施例类似，在这里不再赘述。

图7是本申请实施例提供的一种自动驾驶车辆的结构示意图，如图7所示，该自动驾驶车辆70包括激光雷达71、组合导航系统72以及处理器73和存储器74，其中，激光雷达71用于对所述自动驾驶车辆周围的障碍物进行检测，获得点云数据；所述组合导航系统72用于采集所述自动驾驶车辆的位置和姿态数据；所述存储器74中存储有指令，当所述处理器73执行所述指令时用以执行上述图2或图4实施例的方法。

本申请实施例还提供在一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述图2或图4实施例所示的方法，其执行方式和有益效果类似，在这里不再赘述。

本文中以上描述的功能可以至少部分地由一个或多个硬件逻辑部件来执行。例如，非限制性地，可以使用的示范类型的硬件逻辑部件包括：场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)等等。

用于实施本公开的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。

在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。

此外，虽然采用特定次序描绘了各操作，但是这应当理解为要求这样操作以所示出的特定次序或以顺序次序执行，或者要求所有图示的操作应被执行以取得期望的结果。在一定环境下，多任务和并行处理可能是有利的。同样地，虽然在上面论述中包含了若干具体实现细节，但是这些不应当被解释为对本公开的范围的限制。在单独的实施例的上下文中描述的某些特征还可以组合地实现在单个实现中。相反地，在单个实现的上下文中描述的各种特征也可以单独地或以任何合适的子组合的方式实现在多个实现中。

尽管已经采用特定于结构特征和/或方法逻辑动作的语言描述了本主题，但是应当理解所附权利要求书中所限定的主题未必局限于上面描述的特定特征或动作。相反，上面所描述的特定特征和动作仅仅是实现权利要求书的示例形式。

Claims (10)

1.一种雷达与组合导航系统的联合标定方法，其特征在于，所述方法包括：

获取自动驾驶车辆上搭载的激光雷达在第一时刻和第二时刻采集获得的点云数据，以及获取所述自动驾驶车辆上搭载的组合导航系统在所述第一时刻和所述第二时刻采集获得的位置和姿态数据，其中，在所述第一时刻和所述第二时刻采集获得的点云数据中包含相同物体的点云，所述自动驾驶车辆在所述第一时刻和所述第二时刻的位置和/或姿态不同；

将所述激光雷达在所述第一时刻采集获得的点云数据与所述激光雷达在所述第二时刻采集获得的点云数据进行配准处理，获得所述激光雷达在所述第一时刻和所述第二时刻下激光雷达坐标系之间的第一齐次变换矩阵；

根据所述组合导航系统在所述第一时刻和所述第二时刻采集得到的位置和姿态数据，计算得到所述组合导航系统在所述第一时刻和所述第二时刻下组合导航坐标系之间的第二齐次变换矩阵；

基于所述第一齐次变换矩阵和所述第二齐次变换矩阵，确定所述激光雷达坐标系与所述组合导航坐标系之间的第三齐次变换矩阵，以根据所述第三齐次变换矩阵，确定所述激光雷达检测到的障碍物距离所述自动驾驶车辆的距离，并根据所述距离对所述自动驾驶车辆进行控制。

2.根据权利要求1所述的方法，其特征在于，所述第一时刻和所述第二时刻之间的时间间隔小于预设时长。

3.根据权利要求1所述的方法，其特征在于，所述自动驾驶车辆在所述第一时刻的位置和所述自动驾驶车辆在所述第二时刻的位置之间的距离小于预设距离。

4.根据权利要求1-3中任一项所述的方法，其特征在于，所述根据所述组合导航系统在所述第一时刻和所述第二时刻采集得到的位置和姿态数据，计算得到所述组合导航系统在所述第一时刻和所述第二时刻下组合导航坐标系之间的第二齐次变换矩阵，包括：

分别将所述组合导航系统在所述第一时刻采集到的位置转换为UTM坐标系下的第一位置，将在所述第二时刻采集到的位置转换为所述UTM坐标系下的第二位置；

根据所述第一位置以及第一时刻采集到的姿态数据，确定组合导航坐标系到所述UTM坐标系的第一变换矩阵，以及根据所述第二位置以及第二时刻采集到的姿态数据，确定组合导航坐标系到所述UTM坐标系的第二变换矩阵；

根据所述第一变换矩阵和所述第二变换矩阵，计算得到所述组合导航系统在所述第一时刻和所述第二时刻下组合导航坐标系之间的第二齐次变换矩阵。

5.一种控制装置，其特征在于，包括：

获取模块，用于获取自动驾驶车辆上搭载的激光雷达在第一时刻和第二时刻采集获得的点云数据，以及获取所述自动驾驶车辆上搭载的组合导航系统在所述第一时刻和所述第二时刻采集获得的位置和姿态数据，其中，在所述第一时刻和所述第二时刻采集获得的点云数据中包含相同物体的点云，所述自动驾驶车辆在所述第一时刻和所述第二时刻的位置和/或姿态不同；

配准处理模块，用于将所述激光雷达在所述第一时刻采集获得的点云数据与所述激光雷达在所述第二时刻采集获得的点云数据进行配准处理，获得所述激光雷达在所述第一时刻和所述第二时刻下激光雷达坐标系之间的第一齐次变换矩阵；

计算模块，用于根据所述组合导航系统在所述第一时刻和所述第二时刻采集得到的位置和姿态数据，计算得到所述组合导航系统在所述第一时刻和所述第二时刻下组合导航坐标系之间的第二齐次变换矩阵；

确定模块，用于基于所述第一齐次变换矩阵和所述第二齐次变换矩阵，确定所述激光雷达坐标系与所述组合导航坐标系之间的第三齐次变换矩阵；

控制模块，用于根据所述第三齐次变换矩阵，确定所述激光雷达检测到的障碍物距离所述自动驾驶车辆的距离，并根据所述距离对所述自动驾驶车辆进行控制。

6.根据权利要求5所述的控制装置，其特征在于，所述第一时刻和所述第二时刻之间的时间间隔小于预设时长。

7.根据权利要求5所述的控制装置，其特征在于，所述自动驾驶车辆在所述第一时刻的位置和所述自动驾驶车辆在所述第二时刻的位置之间的距离小于预设距离。

8.根据权利要求5-7中任一项所述的控制装置，其特征在于，所述计算模块，包括：

坐标转换子模块，用于分别将所述组合导航系统在所述第一时刻采集到的位置转换为UTM坐标系下的第一位置，将在所述第二时刻采集到的位置转换为所述UTM坐标系下的第二位置；

确定子模块，用于根据所述第一位置以及第一时刻采集到的姿态数据，确定组合导航坐标系到所述UTM坐标系的第一变换矩阵，以及根据所述第二位置以及第二时刻采集到的姿态数据，确定组合导航坐标系到所述UTM坐标系的第二变换矩阵；

计算子模块，用于根据所述第一变换矩阵和所述第二变换矩阵，计算得到所述组合导航系统在所述第一时刻和所述第二时刻下组合导航坐标系之间的第二齐次变换矩阵。

9.一种自动驾驶车辆，其特征在于，所述自动驾驶车辆包括激光雷达、组合导航系统以及处理器和存储器；

所述激光雷达用于对所述自动驾驶车辆周围的障碍物进行检测，获得点云数据；

所述组合导航系统用于采集所述自动驾驶车辆的位置和姿态数据；

所述存储器中存储有指令，当所述处理器执行所述指令时用以执行权利要求1-4中任一项所述的方法。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-4中任一项所述的方法。

Images