CN117761663B - 外参标定方法、装置及自动驾驶车辆 - Google Patents
外参标定方法、装置及自动驾驶车辆 Download PDFInfo
- Publication number
- CN117761663B CN117761663B CN202311434328.XA CN202311434328A CN117761663B CN 117761663 B CN117761663 B CN 117761663B CN 202311434328 A CN202311434328 A CN 202311434328A CN 117761663 B CN117761663 B CN 117761663B
- Authority
- CN
- China
- Prior art keywords
- pose
- data
- external parameter
- pose data
- residual error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 71
- 239000011159 matrix material Substances 0.000 claims description 23
- 238000001514 detection method Methods 0.000 claims description 21
- 238000012545 processing Methods 0.000 claims description 8
- 230000015654 memory Effects 0.000 description 17
- 238000005457 optimization Methods 0.000 description 13
- 230000008569 process Effects 0.000 description 7
- 238000004891 communication Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 5
- 238000013459 approach Methods 0.000 description 4
- 238000004590 computer program Methods 0.000 description 4
- 238000009795 derivation Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000010365 information processing Effects 0.000 description 3
- 230000003993 interaction Effects 0.000 description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002146 bilateral effect Effects 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000005284 excitation Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 238000004804 winding Methods 0.000 description 1
Landscapes
- Navigation (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本申请公开了一种外参标定方法、装置及自动驾驶车辆，涉及自动驾驶技术领域。该方法的一具体实施方式包括：获取待标定的雷达位姿数据，以及组合位姿数据，其中，组合位姿数据包括惯性位姿数据和/或导航位姿数据；根据雷达位姿数据和对应的位姿真实值，确定第一残差；根据位姿真实值和组合位姿数据，确定外参数据；根据外参数据和对应的外参预估值，确定第二残差；基于预设的图模型，调整第一残差和第二残差，得到激光雷达与组合导航系统之间的外参标定结果。
Description
技术领域
本申请涉及自动驾驶技术领域，尤其涉及一种外参标定方法、装置及自动驾驶车辆。
背景技术
在自动驾驶领域，通过融合采集车配置的多个传感器采集位姿数据，以构建高精地图。为了将多个传感器采集的多源位姿数据进行准确融合，需对多个传感器之间的外参进行标定，以提高多个传感器之间的外参的标定精度。
目前，在外参标定过程，采集车的标定环境很受限，它往往只能通过室外平面运动进行地面标定。
发明内容
本申请的实施例提出了一种外参标定方法、装置及自动驾驶车辆。
第一方面，本申请的实施例提供了一种外参标定方法，该方法包括：获取待标定的雷达位姿数据，以及组合位姿数据，其中，组合位姿数据包括惯性位姿数据和/或导航位姿数据；根据雷达位姿数据和对应的位姿真实值，确定第一残差；根据位姿真实值和组合位姿数据，确定外参数据；根据外参数据和对应的外参预估值，确定第二残差；基于预设的图模型，调整第一残差和第二残差，得到激光雷达与组合导航系统之间的外参标定结果。
第二方面，本申请的实施例提供了一种生成图模型的方法，该方法包括：获取雷达位姿数据，以及激光雷达与组合导航系统之间的外参；基于GTSAM算法，分别以雷达位姿数据中的每帧位姿和外参为节点，以至少一个约束条件为边，生成预设的图模型；其中，至少一个约束条件包括：位姿约束条件、先验约束条件、外参约束条件。
第三方面，本申请的实施例提供了一种外参标定装置，该装置包括：数据获取模块，被配置成获取待标定的雷达位姿数据，以及组合位姿数据，其中，组合位姿数据包括惯性位姿数据和/或导航位姿数据；第一确定模块，被配置成根据雷达位姿数据和对应的位姿真实值，确定第一残差；第二确定模块，被配置成根据位姿真实值和组合位姿数据，确定外参数据；第三确定模块，被配置成根据外参数据和对应的外参预估值，确定第二残差；外参标定模块，被配置成基于预设的图模型，调整第一残差和第二残差，得到激光雷达与组合导航系统之间的外参标定结果。
第四方面，本申请的实施例提供了一种生成图模型的装置，该装置包括：数据获取模块，被配置成获取雷达位姿数据，以及激光雷达与组合导航系统之间的外参；模型生成模块，被配置成基于GTSAM算法，分别以雷达位姿数据中的每帧位姿和外参为节点，以至少一个约束条件为边，生成预设的图模型；其中，至少一个约束条件包括：位姿约束条件、先验约束条件、外参约束条件。
第五方面，本申请的实施例提供了一种电子设备，包括至少一个处理器；以及与至少一个处理器通信连接的存储器；其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够执行如第一方面或第二方面描述的方法。
第六方面，本申请的实施例提供了一种自动驾驶车辆，包括：激光雷达、组合导航系统和如第五方面描述的电子设备。
第七方面，本申请的实施例提供了一种存储有计算机指令的非瞬时计算机可读存储介质，该计算机指令用于使计算机执行如第一方面描述的方法。
本申请的实施例提供的外参标定方法、装置及自动驾驶车辆，根据雷达位姿数据和对应的位姿真实值，确定第一残差；然后，根据位姿真实值和组合位姿数据，确定外参数据，该组合位姿数据包括惯性位姿数据和/或导航位姿数据；然后，根据外参数据和对应的外参预估值，确定第二残差；基于预设的图模型，调整第一残差和第二残差，得到激光雷达与组合导航系统之间的外参标定结果，一方面借助于精度高的组合导航系统采集的组合位姿数据得到的第二残差；然后，将第一残差和第二残差均输入到预设的图模型中进行优化，以得到精度高的外参标定结果；一方面，充分借助高精度的组合导航系统采集的组合位姿数据，能够提高外参标定结果的精度；另一方面，不依赖于标定环境，即在平面特征缺失且只能进行地面标定的场景中依然能够保持高精度的外参标定结果。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：
图1是本申请可以应用于其中的示例性系统架构图；
图2是根据本申请的外参标定方法的一个实施例的流程图；
图3是根据本申请的外参标定方法的一个实施例的流程图；
图4是根据本申请的生成图模型的方法的一个实施例的流程图；
图5是图模型的示意图；
图6是根据本申请的外参标定方法的一个应用场景的示意图；
图7a～7g是根据本申请的外参标定装置的实施例的结构示意图；
图8是根据本申请的外参标定装置的实施例的结构示意图；
图9是根据本申请的生成图模型的装置的实施例的结构示意图；
图10是适于用来实现本申请的实施例的电子设备的结构示意图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
图1示出了可以应用本申请的外参标定方法和装置或生成图模型的方法和装置的实施例的示例性系统架构100。
如图1所示，系统架构100可以包括激光雷达101、组合导航系统102，网络103和电子设备104。网络103用以在激光雷达101、组合导航系统102和电子设备104之间提供通信链路的介质。网络103可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
激光雷达101和组合导航导航系统102可以安装在采集车上，并在采集车行驶过程中采集与位姿相关的数据。上述采集车可以是自动驾驶车辆(Autonomous vehicles；Self-driving automobile)又称无人驾驶车辆、电子设备驾驶车辆或轮式移动机器人，是一种通过电子设备实现无人驾驶的智能车辆。激光雷达可以采集得到原始点云数据，组合导航系统可以采集得到组合位姿数据。激光雷达可以将采集到的原始点云数据，组合导航系统可以将采集到的组合位姿数据发送给电子设备，以供进行外参标定，以为采集车提供高精度的地图。
通过组合导航系统采集组合位姿数据对激光雷达采集原始点云数据，以及激光雷达与组合导航系统之间的外参进行标定，以给自动驾驶车辆提供高精度地图，从而能够为自动驾驶车感知、定位、规划、决策提供重要支持。
电子设备104可以提供各种服务。例如，电子设备104可以获取待标定的雷达位姿数据，以及组合位姿数据，其中，组合位姿数据包括惯性位姿数据和/或导航位姿数据；根据雷达位姿数据和对应的位姿真实值，确定第一残差；根据位姿真实值和组合位姿数据，确定外参数据；根据外参数据和对应的外参预估值，确定第二残差；基于预设的图模型，调整第一残差和第二残差，得到激光雷达与组合导航系统之间的外参标定结果。
需要说明的是，电子设备104可以是硬件，也可以是软件。当电子设备104为硬件时，可以实现成分布式集群。当电子设备104为软件时，可以实现成多个软件或软件模块(例如用来提供分布式服务)，也可以实现成单个软件或软件模块。在此不做具体限定。
需要说明的是，本申请实施例所提供的外参标定方法或生成图模型的方法一般由电子设备104执行，相应地，外参标定装置或生成图模型的装置一般设置于电子设备104中。
应该理解，图1中的激光雷达、组合导航系统、网络和电子设备的数目仅仅是示意性的。根据实现需要，可以具有任意数目的激光雷达、组合导航系统、网络和电子设备。
如图2，示出了根据本申请的外参标定方法的一个实施例的流程图200，该外参标定方法可以包括以下步骤：
步骤201，获取待标定的雷达位姿数据，以及组合位姿数据，其中，组合位姿数据包括惯性位姿数据和/或导航位姿数据。
在本实施例中，外参标定方法的执行主体(例如图1所示的电子设备104)可以通过激光雷达采集待标定的雷达位姿数据，以及通过组合导航系统采集的组合位姿数据。
其中，待标定的雷达位姿数据可以为从原始点云数据中获取的、与位姿相关的数据，该原始点云数据是激光雷达采集得到的，该与位姿相关的数据可以包括与偏移量和/或旋转量相关的数据。该偏移量可以为目标坐标系下在坐标轴方向上的偏移量，例如，在x轴、y轴、z轴方向上的偏移量；该旋转量可以为在目标坐标系下的绕坐标轴的旋转量，例如，翻滚角roll为绕x轴旋转、俯仰角pitch为绕y轴旋转、偏航角yaw为绕z轴旋转。
在这里，该待标定的雷达位姿数据可以为噪声误差的干扰下，所采集得到的预估值。
其中，组合位姿数据可以为通过组合导航系统(Inertial Navigation System，INS)采集的、与位姿相关的数据，该组合导航系统可以包括多种传感器，该多种传感器可以用于采集与位姿相关的数据，例如，IMU(Inertial Measurement Unit)和GNSS(GlobalNavigation Satellite System)。
在这里，IMU可以用于获取与惯性位姿相关的惯性位姿数据，该GNSS可以用于获取与导航位姿相关的导航位姿数据。
对应地，在该示例中，该组合位姿数据可以包括IMU采集的惯性位姿数据和GNSS采集的导航位姿数据。
在本实施例中，组合导航系统相较激光雷达，其精度更高；在外参标定过程中，充分利用高精度的组合导航系统所采集的组合位姿数据来进行外参标定，可以提高外参标定的精度。
步骤202，根据待标定的雷达位姿数据和对应的位姿真实值，确定第一残差。
在本实施例中，上述执行主体可以先获取待标定的雷达位姿数对应的位姿真实值；根据待标定的雷达位姿数据和对应的位姿预估值，确定第一残差。
在本实施例中，由于存在噪声误差，使得待标定的雷达位姿数据与对应的位姿真实值之间存在残存。
在这里，位姿真实值可以为不存在噪声误差的情况下，所采集到的位姿理论值。
在一个示例中，该待标定的雷达位姿数据可以基于以下步骤确定：获取通过激光雷达采集原始点云数据；通过激光雷达里程计，从原始点云数据中获取待标定的雷达位姿数据。
对应地，在该示例中，里程计可以包括过帧间ICP(Iterative Closest Point，迭代最近点)匹配的里程计、Fast-LIO里程计或Fast-LIO2里程计。
在实际使用中，激光雷达里程计用于采集相邻帧之间的相对位姿的位姿预估值。
对应地，在该示例中，该待标定的雷达位姿数据可以包括相邻帧之间的相对位姿的位姿预估值。
在一个示例中，根据待标定的雷达位姿数据和对应的位姿真实值，确定第一残差，可以包括：
获取相邻帧(即，Ta1,Ta2)之间的相对位姿的位姿真实值Treal：
根据待标定的雷达位姿数据中相邻帧之间的相对位姿的位姿预估值Ta-m，以及相对位姿的位姿真实值Ta-real，确定第一残差ea：
需要说明的是，上述相邻帧之间的相对位姿可以为任意相邻帧之间的相对位姿，该相邻帧可以为相邻几帧，例如，相邻两帧，相邻三帧等。
在本实施例中，在外参标定的过程中，对待标定的雷达位姿数据进行参数标定，以使待标定的雷达位姿数据与对应的位姿真实值趋于一致。
步骤203，根据位姿真实值和组合位姿数据，确定外参数据。
在本实施例中，上述执行主体可以根据位姿真实值，以及组合位姿数据，确定激光雷达与组合导航系统之间的变换关系(即，外参数据)。
在一个示例中，以位于UTM(Universal Transverse Mercator Grid System，通用横墨卡托格网系统)坐标系下的激光雷达(Lidar)和组合导航系统(INS)为示例。
根据Lidar在UTM坐标系下的位姿真实值，以及INS在UTM坐标系下的组合位姿数据，确定Lidar与INS之间的外参数据：
其中，为Lidar与INS之间的外参数据，为INS在UTM坐标系下的组合位姿数据(即，步骤201中的组合位姿数据)，为Lidar在UTM坐标系下的位姿真实值。
步骤204，根据外参数据和对应的外参预估值，确定第二残差。
在本实施例中，上述执行主体可以根据步骤203所得的外参数据(即，外参真实值)，以及外参预估值，确定第二残差。
在本实施例中，由于噪声存在误差，使得步骤203中所得的外参数据与外参预估值之间也存在第二残差。
在这里，该外参预估值可以为在噪声误差的情况下，基于激光雷达的位姿预估值和组合导航系统的位姿预估值所确定。
步骤205，基于预设的图模型，调整第一残差和第二残差，得到激光雷达与组合导航系统之间的外参标定结果。
在本实施例中，上述执行主体可以基于预先生成的图模型，对第一残差和第二残差进行调整，以获得激光雷达与组合导航系统之间的外参标定结果。
在本实施例中，通过基于图模型迭代调整第一残差和第二残差进行，以逼近最优解，并将该最优解作为激光雷达与组合导航系统之间的外参标定结果。
本实施例提供的外参标定方法，根据雷达位姿数据和对应的位姿真实值，确定第一残差；然后，根据位姿真实值和组合位姿数据，确定外参数据，该组合位姿数据包括惯性位姿数据和/或导航位姿数据；然后，根据外参数据和对应的外参预估值，确定第二残差；基于预设的图模型，调整第一残差和第二残差，得到激光雷达与组合导航系统之间的外参标定结果，一方面借助于精度高的组合导航系统采集的组合位姿数据得到的第二残差；然后，将第一残差和第二残差均输入到预设的图模型中进行优化，以得到精度高的外参标定结果；一方面，充分借助高精度的组合导航系统采集的组合位姿数据，能够提高外参标定结果的精度；另一方面，不依赖于标定环境，即在平面特征缺失且只能进行地面标定的场景中依然能够保持高精度的外参标定结果。
如图3，示出了根据本申请的外参标定方法的一个实施例的流程图300，该外参标定方法可以包括以下步骤：
步骤301，获取待标定的雷达位姿数据，以及组合位姿数据，其中，组合位姿数据包括惯性位姿数据和/或导航位姿数据。
在本实施例中，外参标定方法的执行主体(例如图1所示的电子设备104)可以通过激光雷达采集待标定的雷达位姿数据，以及通过组合导航系统采集的组合位姿数据。
其中，待标定的雷达位姿数据可以为从原始点云数据中获取的、与位姿相关的数据，该原始点云数据是激光雷达采集得到的，该与位姿相关的数据可以包括与偏移量和/或旋转量相关的数据。该偏移量可以为目标坐标系下在坐标轴方向上的偏移量，例如，在x轴、y轴、z轴方向上的偏移量；该旋转量可以为在目标坐标系下的绕坐标轴的旋转量，例如，翻滚角roll为绕x轴旋转、俯仰角pitch为绕y轴旋转、偏航角yaw为绕z轴旋转。
其中，组合位姿数据可以为通过组合导航系统(Inertial Navigation System，INS)采集的、与位姿相关的数据，该组合导航系统可以包括多种传感器，该多种传感器可以用于采集与位姿相关的数据，例如，IMU(Inertial Measurement Unit)和GNSS(GlobalNavigation Satellite System)。
步骤302，根据待标定的雷达位姿数据和对应的位姿真实值，确定第一残差。
在本实施例中，上述执行主体可以先获取待标定的雷达位姿数对应的位姿真实值；根据待标定的雷达位姿数据和对应的位姿预估值，确定第一残差。
步骤303，根据位姿真实值和组合位姿数据，确定外参数据。
在本实施例中，上述执行主体可以根据位姿真实值，以及组合位姿数据，确定激光雷达与组合导航系统之间的外参数据。
步骤304，根据外参数据和对应的外参预估值，确定第二残差。
在本实施例中，上述执行主体可以根据步骤303所得的外参数据(即，外参真实值)，以及外参预估值，确定第二残差。
步骤305，分别对第一残差和第二残差求导，得到与第一残差对应的第一雅可比矩阵，以及与第二残差对应的第二雅可比矩阵。
在本实施例中，上述执行主体可以分别对第一残差和第二残差进行求导，以得到与第一残差对应的第一雅可比矩阵，以及与第二残差对应的第二雅可比矩阵。
在一个示例中，对第一残差进行求导，得到第一雅可比矩阵，可以包括：
采用残差方程分别对Ta1,Ta2进行求导，分别得到矩阵
为了求导便利性，分别用x，y表示Ta1、Ta2，并采用扰动模型对式(2)和(3)进行求导，得到第一雅可比矩阵：
在一个示例中，对第二残差进行求导，得到第二雅可比矩阵，可以包括：
其中，为组合位姿数据对应的位姿预估值，Tb-real为组合位姿数据对应的位姿真实值；为Lidar在UTM坐标系下的待标定的激光雷达数据的位姿真实值；为激光雷达与INS之间的外参数据；
采用残差方程分别对进行求导，得到矩阵
为了求导便利性，分别用x，y表示使用扰动模型对式(7)和(8)进行求导，得到第二雅可比矩阵:
步骤306，采用GTSAM算法中的优化器，基于预设的图模型中的至少一个约束条件，迭代调整第一雅可比矩阵和第二雅可比矩阵，得到激光雷达与组合导航系统之间的外参标定结果，其中，至少一个约束条件包括：位姿约束条件、先验约束条件、外参约束条件。
在本实施例中，上述执行主体将预设的图模型中的至少一个约束条件加入到GTSAM算法中的优化器中，以迭代调整第一雅可比矩阵和第二雅可比矩阵，以逼近最优解，并将该最优解作为激光雷达与组合导航系统之间的外参标定结果。
其中，位姿约束条件、先验约束条件和外参约束条件分别用于约束第一残差和第二残差迭代，以快速地逼近最优解。
在一个示例中，将位姿约束条件、先验约束条件、外参约束条件中的至少一个约束条件加入到GTSAM算法中的优化器中，以对式(4)、(5)、(9)、(10)进行迭代调整，以得到激光雷达与组合导航系统之间的外参标定结果。
在本实施例中，步骤301、302、303和304的具体操作与图2所示的实施例中的步骤201、202、203和204的操作基本相同，在此不再赘述。
在本实施例中，与图2相比，突出了采用GTSAM算法中的优化器，基于预设的图模型包括位姿约束条件、先验约束条件和外参约束条件中的任一项，迭代调整与第一残差对应的第一雅可比矩阵和与第二残差对应的第二雅可比矩阵，得到激光雷达与组合导航系统之间的外参标定结果；从而可以基于GTSAM算法中的优化器，充分利用组合导航系统输出的UTM坐标下组合位姿数据，并同待标定的雷达位姿数据，一并输入到预设的图模型中优化，最终得到位姿的精度在0.1°，偏移量在厘米级的外参标定结果。
在本实施例的一些可选的实现方式中，外参标定结果包括第一偏移量和第二偏移量，其中，第一偏移量的置信度小于第二偏移量的置信度小，第一偏移量为目标坐标系下在z轴方向上的偏移量，第二偏移量为目标坐标系下除z轴方向上的其他偏移量。
在本实现方式中，目标坐标系包括6个自由度，即，沿x轴方向上的自由度、沿y轴方向上的自由度、沿z轴方向上的自由度，以及绕x轴旋转的自由度、绕y轴旋转的自由度、绕z轴旋转的自由度。
在本实现方式中，采集车标定场景一般在地面上，在通过平面运动进行外参校准时，由于z轴激励的不够充分，使得外参标定结果在z轴方向上的偏移量存在较大误差。
为了解决在z轴方向上的偏移量存在较大误差，可以将在z轴方向上偏移量的置信度设置得比其他5个自由度的偏移量的置信度更低，使得在z轴方向上的偏移量更信赖于初始值，从而在基于图模型的优化调整的过程中不会干扰z轴方向上的偏移量。
在一个示例中，在z轴方向上的偏移量的置信度与其他五个自由度的置信度之间相差1*10个数量级。
在本实现方式中，通过将在z轴方向上的第一偏移量的置信度设置得比第二偏移量的置信度更低，从而解决了外参标定结果在z轴方向上的偏移量存在较大误差的问题。
在本实施例的一些可选的实现方式中，根据外参数据和对应的外参预估值，确定第二残差，可以包括：
在本实现方式中，上述外参预估值可以为基于雷达位姿数据采集的位姿预估值和组合导航系统采集的位姿预估值所确定。
以UTM(Universal Transverse Mercator Grid System)坐标系下的激光雷达(Lidar)和组合导航系统(INS)为示例。
其中，为Lidar与INS之间的外参数据(即，外参真实值)，为INS在UTM坐标系下的组合位姿数据(即，组合导航系统对应的位姿预估值)，为Lidar在UTM坐标系下的位姿真实值(即，雷达位姿数据对应的位姿预估值)；
通过INS采集得到INS在UTM坐标系下的位姿预估值(即，)；根据外参数据(即，)、位姿预估值(即，Lidar在UTM坐标系下的位姿真实值)，以及待标定的雷达位姿数据，确定第二残差eb：
在本实现方式中，可以通过组合导航系统采集的位姿预估值和位姿真实值(即，组合位姿数据)，以及通过激光雷达采集的位姿预估值(即，雷达位姿数据)和位姿真实值，得到第二残差；充分借助高精度的组合导航系统采集的组合位姿数据，能够提高外参标定结果的精度。
如图4，示出了根据本申请的生成图像模型的一个实施例的流程图400，该生成图像模型可以包括以下步骤：
步骤401，获取雷达位姿数据，以及激光雷达与组合导航系统之间的外参。
在本实施例中，生成图像模型的执行主体(例如，图1所示的电子设备104)可以获取雷达位姿数据，以及激光雷达与组合导航系统之间的外参。
步骤402，基于GTSAM算法，分别以雷达位姿数据中的每帧位姿和外参为节点，以至少一个约束条件为边，生成预设的图模型；其中，至少一个约束条件包括：位姿约束条件、先验约束条件、外参约束条件。
在本实施例中，上述主体可以基于GTSAM算法，分别以雷达位姿数据中的每帧位姿和外参为图模型的节点，以至少一个约束条件为图模型的边，以生成预设的图像模型，其中，至少一个约束条件用于描述各节点之间的约束关系，该至少一个约束条件可以包括：位姿约束条件，先验约束条件、外参约束条件。
在本实施例中，基于GTSAM算法生成图模型，建立最小化残差的图模型，从而可以基于该图模型，提高激光雷达与组合导航系统之间的外参标定结果的精度。
在本实施例的一些可选的实现方式中，先验约束条件基于以下步骤确定：
获取外参的初始值，以及组合位姿数据；
根据组合位姿数据中的第一帧组合位姿，以及外参的初始值，确定第一帧雷达位姿；
获取与第一帧雷达位姿对应的先验约束条件。
在本实现方式中，上述执行主体可以通过外参的初始值和组合位姿数据中的第一帧位姿，确定第一帧雷达位姿；然后，将该第一帧雷达位姿作为上述图模型的先验因子(例如，图5中的“prior factor(单边先验因子)”)，输入到该图模型中，以对步骤205中的第一残差和第二残差进行约束调整，以逼近全局最优解，并将该最优解作为外参标定结果。
在本实现方式中，通过高精度的组合位姿数据和外参初始值来确定高精度的第一帧雷达位姿，从而能够从第一帧就可以实现对激光雷达数据所采集的位姿数据的标定，进而可以提高该激光雷达与组合导航系统之间的外参标定结果的精度。
在本实施例的一些可选的实现方式中，位姿约束条件基于以下步骤确定：
根据雷达位姿数据中相邻帧之间的相对位姿的位姿真实值，以及对应的位姿预估值，确定激光雷达残差；
将满足第一预设残差阈值的激光雷达残差，确定为相对位姿的回环检测结果；
获取相对位姿的回环检测结果对应的位姿约束条件。
在本实现方式中，上述执行主体可以针对雷达位姿数据中的任意相邻帧，获取该相邻帧之间的相对位姿的位姿真实值和对应的位姿预估值，确定该相邻帧对应的激光雷达残差；之后，将小于第一预设残差阈值的激光雷达残差，确定为相对位姿对应的回环检测结果。该第一预设残差阈值可以基于外参标定结果的精度进行设定。
在图5中，位姿约束条件对应“X1与X2之间的Between factor、X2与X3之间的Between factor、X3…Xk之间的Between factor”。
在本实现方式中，通过回环检测对雷达位姿数据中的相邻帧之间的相对位姿进行检测，以判断采集车是否驶过历史位置，如果相邻帧之间的相对位姿的残差小于第一预设残差阈值，可以确定采集车驶过该历史位置，也即此处有回环。
在本实现方式中，通过将满足预设第一残差阈值的激光雷达残差作为回环检测结果；并将与该回环检测结果对应的位姿约束条件作为图模型的约束条件，从而可以提高激光雷达与组合导航系统之间的外参标定结果的精度。
在本实施例的一些可选的实现方式中，外参约束条件基于以下步骤确定：
根据雷达位姿数据和组合位姿数据，确定外参数据；
根据外参数据和对应的外参预估值，确定外参残差；
将满足第二预设残差阈值的外参残差，确定为外参的回环检测结果；
获取与外参的回环检测结果对应的外参约束条件。
在本实现方式中，上述执行主体可以根据雷达位姿数据和组合位姿数据，确定外参数据(即，外参真实值)；之后，根据外参数据和对应的外参预估值，确定外参残差；之后，将满足第二预设残差阈值的残差作为外参的回环检测结果；之后，获取与该外参的回环检测结果对应的外参约束条件。该第二预设残差阈值可以基于外参标定结果的精度进行设定。
在图5中，外参约束条件对应“T与每帧位姿之间的Compose factor”。
在本实现方式中，通过回环检测对组合位姿数据中的相邻帧之间的相对位姿进行检测，以判断采集车是否驶过历史位置，如果相邻帧之间的相对位姿的残差小于第二预设残差阈值，可以确定采集车驶过该历史位置，也即此处有回环。
在本实现方式中，通过将满足预设第二残差阈值的外参残差作为回环检测结果；并将与该回环检测结果对应的位姿约束条件作为图模型的约束条件，从而可以提高激光雷达与组合导航系统之间的外参标定结果的精度。
在本实施例的一些可选的实现方式中，获取雷达位姿数据，可以包括：获取原始点云数据；通过里程计，从原始点云数据中获取在目标坐标系下的雷达位姿数据；
获取组合位姿数据，包括：获取在目标坐标系下的原始位姿数据；对原始位姿数据进行插值处理，得到组合位姿数据，其中，雷达位姿数据与组合位姿数据时空对应。
在本实施例中，采集车离线采集绕八字数据，绕八字可以最大限度激励组合导航系统，从而可以采集得到高精度的原始位姿数据。之后，对原始位姿数据进行插值处理，得到组合位姿数据。之后，获取绕八字过程中的原始点云数据；然后，通过里程计，从原始点云数据中获取目标坐标系下的激光雷达数据。
需要说明的是，在对原始位姿数据进行插值处理之后，将组合位姿数据进行记录，以备后续进行外参标定时，获取该记录的组合位姿数据以执行图2所示的外参标定方法。
在这里，上述雷达位姿数据可以为无噪声误差的情况下，所得到的理论值。
在一个示例中，图2中所涉及的组合位姿数据可以为对原始位姿数据进行插值之后得到的数据。
在这里，雷达位姿数据和组合位姿数据为同一坐标系下的位姿数据，例如，目标坐标系下。该目标坐标系可以为UTM、雷达坐标系等。
在本实现方式中，可以通过GPS(Global Positioning System，全球定位系统)(详见图6中的GPS)获取采集车的坐标信息，并将该坐标信息转化为目标坐标系下的坐标信息；进而通过里程计输出目标坐标系下的雷达位姿数据，以及组合导航系统输出目标坐标系下的原始位姿数据。
在一个示例中，上述执行主体可以通过帧间ICP匹配的里程计或Fast-LIO里程计或Fast-LIO2里程计，从原始点云数据中获取匹配的雷达位姿数据；
对应地，在该示例中，上述执行主体可以通过Fast-LIO2里程计，从原始点云数据中获取匹配的雷达位姿数据。
在本实现方式中，可以通过精度高的里程计从原始点云数据中获取精度更高的雷达位姿数据。
在本实现方式中，对原始位姿数据进行插值处理，以使雷达位姿数据与组合位姿数据的时间窗口对应，使得组合位姿数据和雷达位姿数据帧同步，从而能够基于高精度的组合位姿数据实现对每帧雷达位姿的标定，进而可以提高激光雷达与组合导航系统之间的外参标定结果的准确性。
如图6，示出了根据本申请的外参标定方法的一个应用场景的示意图，在该应用场景中，该外参标定方法可以包括以下步骤：
第一步，初始化节点：雷达位姿数据中的每帧位姿和外参赋予初始值，该里程计初始值是由里程计在UTM坐标系下的位姿提供，该外参初始值可以为激光雷达与组合导航系统(INS)之间的设计结构值。
第二步，加入单边先验因子(Prior factor)，如图5中的Prior factor。将第一帧激光位姿作为先验因子加入图模型中。
第三步，加入双边约束因子，即Between factor和Compose factor。其中，Betweenfactor包括里程计约束和回环约束，分别对应图5中的X1与X2之间的Between factor、X2与X3之间的Between factor、X3…Xk之间的Between factor和X1与Xk之间的Betweenfactor，组合位姿数据则作为Compose factor，对应图5中的T与每帧位姿之间的Composefactor。
第四步，基于GTSAM算法，生成图模型(对应，优化模型)。
在本实施例中，GTSAM(Graphical Models for Sequential Estimation)优化库是一个用于非线性优化的开源库，它的原理最小化二乘优化算法，该图模型中的节点代表变量，边代表变量之间的关系。
在迭代的过程中，使用最小二乘优化算法对图模型进行求解，目标是最小化代价函数，使得预估值和真实值趋于一致。
在本实施例中，雷达位姿数据中的每帧位姿和外参都是待优化变量，其中，X1,X2,X3......Xk是雷达位姿数据中的每帧位姿，T是组合导航系统与激光雷达之间的外参，Priorfactor，X1与X2之间的Between factor、X2与X3之间的Between factor、X3…Xk之间的Between factor，X1与Xk之间的Between factor和T与每帧位姿之间的Compose factor分别对应约束条件。
第五步，使用GTSAM算法中的ISAM(simultaneous localization and mapping)迭代优化器，基于第四步中的图模型中的至少一个约束条件进行迭代优化，得到激光雷达与组合导航系统之间的外参标定结果。
在本实施例中，采用GTSAM算法中的ISAM迭代优化器，对第一残差和第二残差进行迭代调整，得到激光雷达与组合导航系统之间的外参标定结果。其中，第一残差和第二残差可以基于图2所示的外参标定方法所确定，在此不再赘述。至少一个约束条件可以基于图4所示的生成图模型的方法所确定，在此不再赘述。
在这里，GNSS/INS组合导航系统输出UTM系下的INS位姿对应组合位姿数据；UTM系下激光里程计位姿为雷达位姿数据；外参标定结果可以包括优化后的外参(即，Lidar与INS之间优化后的外参)和优化后的UTM下位姿(即，Lidar在UTM系下的、优化后的雷达位姿数据)。
在本实施例中，通过5次迭代调整，就可以得到外参标定结果，该外参标定结果可以包括激光雷达和组合导航系统之间的外参以及每帧雷达位姿。
在本实施例中，为了验证图模型的稳定性，采集车在全国不同城市进行了大量标定实验，标定效果详见附图7a～7g，外参标定结果中的姿态精度保持在0.1°，偏移精度控制在厘米级。同时能对偏移在±30cm，姿态偏差在±5°度误差的外参初始值进行矫正，具有极高的鲁棒性。
其中，图7a～图7c是对偏移量误差的优化效果图：图7a是外参X方向上30cm偏差的点云图，可以看出红框中有重影；图7b是图模型进行外参标定之后生成的点云图；图7c是图7a与图7b的对比图。图7d为图7c中红框的放大图，在该图中，黄色对应图7a，红色对应图7b，在该图7c中在X轴方向的30cm偏差被消除，重影消失。
图7f～图7g是对位姿误差的优化效果图：图7f是外参yaw添加5°误差后生成的点云图；图7f是基于图模型进行外参标定后的点云图；图7g为图7f和图7f的对比图，黄色对应图7f，红色对应图7f。在该图7g中，在纠正偏移量误差的同时，还纠结姿态在5°以内的误差。
在本实施例中，使用Lidar传感器通过帧间匹配得到里程计，再结合INS组合导航系统输出的UTM系下组合位姿数据，最后将激光雷达与组合导航系统(INS)之间的设计结构值作为外参的初始值；然后，采用开源GTSAM图优化库，建立最小化位姿残差的图模型对第一残差和第二残差进行优化迭代，得到精度在0.1°的外参标定结果。
在本实施例中，一方面不依赖里程计的类型，高精度的里程计(例如，Fast-LIO2)可以进一步提高待标定的雷达位姿数据的精度，从而为后续进行外参标定打好基础。
进一步参考图8，作为对上述各图所示方法的实现，本申请公开了外参标定装置的一个实施例，该装置实施例与图2所示的方法实施例相对应，该装置具体可以应用于各种电子设备中。
如图8所示，本申请的实施例提供了一种外参标定装置800，该装置800包括：数据获取模块801、第一确定模块802、第二确定模块803、第三确定模块804和外参标定模块805。其中，数据获取模块801，被配置成获取待标定的雷达位姿数据，以及组合位姿数据，其中，组合位姿数据包括惯性位姿数据和/或导航位姿数据；第一确定模块802，被配置成根据雷达位姿数据和对应的位姿真实值，确定第一残差；第二确定模块803，被配置成根据位姿真实值和组合位姿数据，确定外参数据；第三确定模块804，被配置成根据外参数据和对应的外参预估值，确定第二残差；外参标定模块805，被配置成基于预设的图模型，调整第一残差和第二残差，得到激光雷达与组合导航系统之间的外参标定结果。
在本实施例中，外参标定装置800中，数据获取模块801、第一确定模块802、第二确定模块803、第三确定模块804和外参标定模块805的具体处理及其所带来的技术效果可分别参考图2对应实施例中的步骤201～步骤205。
在一些实施例中，外参标定模块805，进一步被配置成：分别对第一残差和第二残差求导，得到与第一残差对应的第一雅可比矩阵，以及与第二残差对应的第二雅可比矩阵；采用GTSAM算法中的优化器，基于预设的图模型中的至少一个约束条件，迭代调整第一雅可比矩阵和第二雅可比矩阵，得到激光雷达与组合导航系统之间的外参标定结果，其中，至少一个约束条件包括：位姿约束条件、先验约束条件、外参约束条件。
在一些实施例中，第三确定模块804，进一步被配置成：获取与组合位姿数据对应的位姿预估值；根据外参数据、位姿预估值，以及雷达位姿数据，确定第二残差。
在一些实施例中，外参标定结果包括：第一偏移量和第二偏移量，其中，第一偏移量的置信度小于第二偏移量的置信度，第一偏移量为目标坐标系下在z轴方向上的偏移量，第二偏移量为目标坐标系下除z轴方向上的其他偏移量。
进一步参考图9，作为对上述各图所示方法的实现，本申请公开了生成图模型的装置的一个实施例，该装置实施例与图4所示的方法实施例相对应，该装置具体可以应用于各种电子设备中。
如图9所示，本申请的实施例提供了一种生成图模型的装置900，该装置900包括：数据获取模块901和模型生成模块902。其中，数据获取模块901，被配置成获取雷达位姿数据，以及激光雷达与组合导航系统之间的外参；模型生成模块902，被配置成基于GTSAM算法，分别以雷达位姿数据中的每帧位姿和外参为节点，以至少一个约束条件为边，生成预设的图模型；其中，至少一个约束条件包括：位姿约束条件、先验约束条件、外参约束条件。
在本实施例中，生成图模型的装置900中，数据获取模块901和模型生成模块902的具体处理及其所带来的技术效果可分别参考图4对应实施例中的步骤401～步骤402。
在一些实施例中，该生成图模型的装置900还包括：第二获取模块，被配置成：获取外参的初始值，以及组合位姿数据；根据组合位姿数据中的第一帧组合位姿，以及外参的初始值，确定第一帧雷达位姿；获取与第一帧雷达位姿对应的先验约束条件。
在一些实施例中，该生成图模型的装置900还包括：第三获取模块，被配置成：根据雷达位姿数据中相邻帧之间的相对位姿，以及对应的位姿预估值，确定激光雷达残差；将满足第一预设残差阈值的激光雷达残差，确定为相对位姿的回环检测结果；获取与相对位姿的回环检测结果对应的位姿约束条件。
在一些实施例中，该生成图模型的装置900还包括：第四获取模块，被配置成：根据雷达位姿数据和组合位姿数据，确定外参数据；根据外参数据和对应的外参预估值，确定外参残差；将满足第二预设残差阈值的外参残差，确定为外参的回环检测结果；获取与外参的回环检测结果对应的外参约束条件。
在一些实施例中，数据获取模块901，进一步被配置成：获取原始点云数据；通过里程计，从原始点云数据中获取在目标坐标系下的雷达位姿数据；
第二获取模块，进一步被配置成：获取在目标坐标系下的原始位姿数据；对原始位姿数据进行插值处理，得到组合位姿数据，其中，雷达位姿数据与组合位姿数据时空对应。
如图10所示，是根据本申请实施例的外参标定方法或生成图模型的方法的电子设备的框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本申请的实现。
如图10所示，该电子设备包括：一个或多个处理器1001、存储器1002，以及用于连接各部件的接口，包括高速接口和低速接口。各个部件利用不同的总线互相连接，并且可以被安装在公共主板上或者根据需要以其它方式安装。处理器可以对在电子设备内执行的指令进行处理，包括存储在存储器中或者存储器上以在外部输入/输出装置(诸如，耦合至接口的显示设备)上显示GUI的图形信息的指令。在其它实施方式中，若需要，可以将多个处理器和/或多条总线与多个存储器和多个存储器一起使用。同样，可以连接多个电子设备，各个设备提供部分必要的操作(例如，作为服务器阵列、一组刀片式服务器、或者多处理器系统)。图10中以一个处理器1001为例。
存储器1002即为本申请所提供的非瞬时计算机可读存储介质。其中，存储器存储有可由至少一个处理器执行的指令，以使至少一个处理器执行本申请所提供的外参标定方法或生成图模型的方法。本申请的非瞬时计算机可读存储介质存储计算机指令，该计算机指令用于使计算机执行本申请所提供的外参标定方法或生成图模型的方法。
存储器1002作为一种非瞬时计算机可读存储介质，可用于存储非瞬时软件程序、非瞬时计算机可执行程序以及模块，如本申请实施例中的外参标定方法或生成图模型的方法对应的程序指令/模块(例如，附图8所示的数据获取模块801、第一确定模块802、第二确定模块803、第三确定模块804和外参标定模块805；或，附图9所示的数据获取模块901和模型生成模块902)。处理器1001通过运行存储在存储器1002中的非瞬时软件程序、指令以及模块，从而执行服务器的各种功能应用以及数据处理，即实现上述方法实施例中的外参标定方法或生成图模型的方法。
存储器1002可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据基于区块链的信息处理电子设备的使用所创建的数据等。此外，存储器1002可以包括高速随机存取存储器，还可以包括非瞬时存储器，例如至少一个磁盘存储器件、闪存器件、或其他非瞬时固态存储器件。在一些实施例中，存储器1002可选包括相对于处理器1001远程设置的存储器，这些远程存储器可以通过网络连接至基于区块链的信息处理电子设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
外参标定方法或生成图模型的方法的电子设备还可以包括：输入装置1003和输出装置1004。处理器1001、存储器1002、输入装置1003和输出装置1004可以通过总线或者其他方式连接，图10中以通过总线连接为例。
输入装置1003可接收输入的数字或字符信息，以及产生与基于区块链的信息处理电子设备的用户设置以及功能控制有关的键信号输入，例如触摸屏、小键盘、鼠标、轨迹板、触摸板、指示杆、一个或者多个鼠标按钮、轨迹球、操纵杆等输入装置。输出装置1004可以包括显示设备、辅助照明装置(例如，LED)和触觉反馈装置(例如，振动电机)等。该显示设备可以包括但不限于，液晶显示器(LCD)、发光二极管(LED)显示器和等离子体显示器。在一些实施方式中，显示设备可以是触摸屏。
此处描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、专用ASIC(专用集成电路)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
这些计算程序(也称作程序、软件、软件应用、或者代码)包括可编程处理器的机器指令，并且可以利用高级过程和/或面向对象的编程语言、和/或汇编/机器语言来实施这些计算程序。如本文使用的，术语“机器可读介质”和“计算机可读介质”指的是用于将机器指令和/或数据提供给可编程处理器的任何计算机程序产品、设备、和/或装置(例如，磁盘、光盘、存储器、可编程逻辑装置(PLD))，包括，接收作为机器可读信号的机器指令的机器可读介质。术语“机器可读信号”指的是用于将机器指令和/或数据提供给可编程处理器的任何信号。
根据本申请的实施例，本申请还提供了一种自动驾驶车辆，该自动驾驶车辆包括激光雷达、组合导航系统，以及上述图10所示的电子设备。
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)和互联网。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本申请中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本申请公开的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本申请保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本申请的精神和原则之内所作的修改、等同替换和改进等，均应包含在本申请保护范围之内。
Claims (11)
1.一种外参标定方法，包括：
获取待标定的激光雷达位姿数据，以及组合位姿数据，其中，所述组合位姿数据包括惯性位姿数据和/或GNSS位姿数据；
根据所述激光雷达位姿数据和对应的位姿真实值，确定第一残差；
根据所述位姿真实值和所述组合位姿数据，确定外参数据；
根据所述外参数据和对应的外参预估值，确定第二残差；
基于预设的图模型，调整所述第一残差和所述第二残差，得到激光雷达与组合导航系统之间的外参标定结果。
2.根据权利要求1所述的方法，其中，所述基于预设的图模型，调整所述第一残差和所述第二残差，得到激光雷达与组合导航系统之间的外参标定结果，包括：
分别对所述第一残差和所述第二残差求导，得到与所述第一残差对应的第一雅可比矩阵，以及与所述第二残差对应的第二雅可比矩阵；
采用GTSAM算法中的优化器，基于预设的图模型中的至少一个约束条件，迭代调整所述第一雅可比矩阵和所述第二雅可比矩阵，得到激光雷达与组合导航系统之间的外参标定结果，其中，至少一个约束条件包括：位姿约束条件、先验约束条件、外参约束条件。
3.根据权利要求1所述的方法，其中，所述根据所述外参数据和对应的外参预估值，确定第二残差，包括：
获取与所述组合位姿数据对应的位姿预估值；
根据所述外参数据、所述位姿预估值，以及所述激光雷达位姿数据，确定所述第二残差。
4.根据权利要求1-3任一项所述的方法，其中，所述外参标定结果包括：第一偏移量和第二偏移量，其中，所述第一偏移量的置信度小于所述第二偏移量的置信度，所述第一偏移量为目标坐标系下z轴方向上的偏移量，所述第二偏移量为所述目标坐标系下除z轴方向上的其他偏移量。
5.根据权利要求1所述的方法，其中，所述图模型通过以下步骤生成：
获取激光雷达位姿数据，以及激光雷达与组合导航系统之间的外参；
基于GTSAM算法，分别以所述激光雷达位姿数据中的每帧位姿和所述外参为节点，以至少一个约束条件为边，生成预设的图模型；
其中，至少一个约束条件包括：位姿约束条件、先验约束条件、外参约束条件。
6.根据权利要求5所述的方法，其中，所述先验约束条件基于以下步骤确定：
获取所述外参的初始值，以及组合位姿数据；
根据所述组合位姿数据中的第一帧组合位姿，以及所述外参的初始值，确定第一帧雷达位姿；
获取与所述第一帧雷达位姿对应的先验约束条件。
7.根据权利要求5所述的方法，其中，所述位姿约束条件基于以下步骤确定：
根据所述激光雷达位姿数据中相邻帧之间的相对位姿的位姿真实值，以及对应的位姿预估值，确定激光雷达残差；
将满足第一预设残差阈值的激光雷达残差，确定为相对位姿的回环检测结果；
获取与所述相对位姿的回环检测结果对应的位姿约束条件。
8.根据权利要求5所述的方法，其中，所述外参约束条件基于以下步骤确定：
根据所述激光雷达位姿数据和所述组合位姿数据，确定所述外参数据；
根据所述外参数据和对应的外参预估值，确定外参残差；
将满足第二预设残差阈值的外参残差，确定为所述外参的回环检测结果；
获取与所述外参的回环检测结果对应的外参约束条件。
9.根据权利要求6-8任一项所述的方法，其中，所述获取激光雷达位姿数据，包括：
获取原始点云数据；
通过里程计，从所述原始点云数据中获取在目标坐标系下的激光雷达位姿数据；
获取组合位姿数据，包括：
获取在目标坐标系下的原始位姿数据；
对所述原始位姿数据进行插值处理，得到所述组合位姿数据，其中，所述激光雷达位姿数据与所述组合位姿数据时空对应。
10.一种外参标定装置，包括：
数据获取模块，被配置成获取待标定的激光雷达位姿数据，以及组合位姿数据，其中，所述组合位姿数据包括惯性位姿数据和/或GNSS位姿数据；
第一确定模块，被配置成根据所述激光雷达位姿数据和对应的位姿真实值，确定第一残差；
第二确定模块，被配置成根据所述位姿真实值和所述组合位姿数据，确定外参数据；
第三确定模块，被配置成根据所述外参数据和对应的外参预估值，确定第二残差；
外参标定模块，被配置成基于预设的图模型，调整所述第一残差和所述第二残差，得到激光雷达与组合导航系统之间的外参标定结果。
11.一种自动驾驶车辆，包括激光雷达、组合导航系统和执行如权利要求1-9任一项所述的外参标定方法的电子设备。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311434328.XA CN117761663B (zh) | 2023-10-31 | 2023-10-31 | 外参标定方法、装置及自动驾驶车辆 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311434328.XA CN117761663B (zh) | 2023-10-31 | 2023-10-31 | 外参标定方法、装置及自动驾驶车辆 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117761663A CN117761663A (zh) | 2024-03-26 |
| CN117761663B true CN117761663B (zh) | 2024-10-18 |
Family
ID=90309248
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311434328.XA Active CN117761663B (zh) | 2023-10-31 | 2023-10-31 | 外参标定方法、装置及自动驾驶车辆 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117761663B (zh) |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116608873A (zh) * | 2023-04-18 | 2023-08-18 | 华南理工大学 | 一种自动驾驶车辆的多传感器融合定位建图方法 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021031157A1 (zh) * | 2019-08-21 | 2021-02-25 | 深圳市速腾聚创科技有限公司 | 外参标定方法、装置、计算设备以及计算机存储介质 |
| CN113933818A (zh) * | 2021-11-11 | 2022-01-14 | 阿波罗智能技术(北京)有限公司 | 激光雷达外参的标定的方法、设备、存储介质及程序产品 |
| CN114296058B (zh) * | 2021-12-14 | 2025-10-10 | 华人运通(上海)自动驾驶科技有限公司 | 场端激光雷达的标定方法、装置、存储介质及终端设备 |
| CN114397642B (zh) * | 2022-01-13 | 2024-10-29 | 北京工业大学 | 一种基于图优化的三维激光雷达与imu外参标定方法 |
| CN115876222B (zh) * | 2022-06-15 | 2025-11-18 | 东南大学 | 基于图优化的imu/相机/激光雷达联合标定方法 |
| CN116106870A (zh) * | 2023-01-31 | 2023-05-12 | 新石器慧通（北京）科技有限公司 | 车辆激光雷达外参的标定方法和装置 |
-
2023
- 2023-10-31 CN CN202311434328.XA patent/CN117761663B/zh active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116608873A (zh) * | 2023-04-18 | 2023-08-18 | 华南理工大学 | 一种自动驾驶车辆的多传感器融合定位建图方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117761663A (zh) | 2024-03-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10732647B2 (en) | Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (MAV) | |
| Heo et al. | EKF-based visual inertial navigation using sliding window nonlinear optimization | |
| Liu et al. | Stereo visual-inertial odometry with multiple Kalman filters ensemble | |
| Achtelik et al. | Stereo vision and laser odometry for autonomous helicopters in GPS-denied indoor environments | |
| CN111721289A (zh) | 车辆定位方法、装置、设备、存储介质及车辆 | |
| CN111338383B (zh) | 基于gaas的自主飞行方法及系统、存储介质 | |
| Li et al. | Two-step LiDAR/camera/IMU spatial and temporal calibration based on continuous-time trajectory estimation | |
| CN113763548A (zh) | 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统 | |
| US9513130B1 (en) | Variable environment high integrity registration transformation system and related method | |
| CN110895408A (zh) | 一种自主定位方法、装置及移动机器人 | |
| Irfan et al. | Lgvins: Lidar-gps-visual and inertial system based multi-sensor fusion for smooth and reliable uav state estimation | |
| Barrau et al. | Invariant filtering for pose ekf-slam aided by an imu | |
| Nubert et al. | Holistic fusion: Task-and setup-agnostic robot localization and state estimation with factor graphs | |
| CN117687006A (zh) | 激光雷达到惯性测量单元的外参标定方法及其装置 | |
| CN117191023A (zh) | 基于扩展卡尔曼滤波的无人系统集群相对定位方法 | |
| CN116642484A (zh) | 基于矩阵李群和因子图的无人机视觉定位导航方法及装置 | |
| CN118999559B (zh) | 巡检无人机的定位方法、装置、计算机设备及存储介质 | |
| CN117761663B (zh) | 外参标定方法、装置及自动驾驶车辆 | |
| Tsai et al. | Cooperative SLAM using fuzzy Kalman filtering for a collaborative air-ground robotic system | |
| Cucci et al. | On the development of a generic multi-sensor fusion framework for robust odometry estimation | |
| CN112880664B (zh) | 一种行驶设备的定位方法、装置、设备及存储介质 | |
| Cai et al. | GPS-Aided Stereo Inertial Navigation Localization Algorithm for Outdoor Scenarios | |
| Thalagala | Comparison of state marginalization techniques in visual inertial navigation filters | |
| CN119935144B (zh) | 一种基于相对位姿量测的分布式协同定位方法及装置 | |
| Wang et al. | A new algorithm for robot localization using monocular vision and inertia/odometry sensors |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |