基于同源增量优化的车辆定位位姿精细化修正方法及装置与流程

本公开涉及自动驾驶定位，具体而言，涉及一种基于同源增量优化的车辆定位位姿精细化修正方法及装置。

背景技术：

1、在自动驾驶系统中，车辆定位位姿用于表征车辆在预设坐标系中的位置和姿态，是环境感知、行驶规划及车辆控制等处理的重要数据基础。目前车辆定位通常基于全球导航卫星系统、惯性测量单元等定位传感器采集的数据，对车辆的位置和姿态进行估计。与此同时，为保证相机采集数据能够用于车辆周围环境分析，还需要确定相机与车辆之间的空间关系。

2、对于相关技术中的自动驾驶系统，车辆定位处理与相机标定处理通常分别设计和执行。车辆定位处理主要依据定位传感器数据确定车辆位姿，相机标定处理则主要依据图像数据或几何约束确定相机相关参数。即使部分方案能够同时对车辆位姿和相机相关参数进行处理，二者通常仍采用各自独立的处理流程，相关计算过程之间缺少有效衔接。

3、由于上述处理过程相互独立，系统需要分别维护对应的计算流程，容易造成算法实现冗余和重复计算。此外，不同处理结果之间难以及时协调，车辆定位结果在进一步修正过程中可能受到不同处理过程之间差异的影响，进而影响车辆定位位姿优化的稳定性和准确性。因此，相关技术中的车辆定位位姿优化方式仍有待改进。

4、需要说明的是，在上述背景技术部分公开的信息仅用于加强对本公开的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

技术实现思路

1、本公开实施例的目的在于提供一种基于同源增量优化的车辆定位位姿精细化修正方法及装置，通过将相机外参标定处理采用的增量参数化、损失计算及参数更新方式复用于车辆定位位姿优化，使二者的优化逻辑保持一致，进而减少算法冗余和重复计算，并提高定位优化的稳定性和定位精度。

2、根据本公开实施例的第一方面，提供了一种基于同源增量优化的车辆定位位姿精细化修正方法，包括：

3、获取车辆的多源定位数据，并根据所述多源定位数据确定所述车辆的初始定位位姿；

4、采用与相机外参标定处理相同的增量参数化方式设置位姿增量参数，并根据所述初始定位位姿和所述位姿增量参数确定所述车辆的当前定位位姿；

5、按照与所述相机外参标定处理相同的损失函数形式，基于所述当前定位位姿和所述多源定位数据确定定位一致性损失；

6、按照与所述相机外参标定处理相同的参数更新方式，根据所述定位一致性损失更新所述位姿增量参数；

7、根据所述初始定位位姿和更新后的所述位姿增量参数，确定所述车辆的目标定位位姿。

8、在本公开的一种示例性实施例中，所述采用与相机外参标定处理相同的增量参数化方式设置位姿增量参数，包括：

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、目标定位位姿确定模块，用于根据所述初始定位位姿和更新后的所述位姿增量参数，确定所述车辆的目标定位位姿。

46、本公开实施例提供的技术方案可以包括以下有益效果：

47、本公开示例实施例中提供的基于同源增量优化的车辆定位位姿精细化修正方法，通过根据多源定位数据确定车辆的初始定位位姿，并在初始定位位姿的基础上设置位姿增量参数，使后续优化围绕已有定位结果对车辆的位置和姿态进行增量调整，避免重新求解完整定位位姿；由于位姿增量参数采用与相机外参标定处理相同的增量参数化方式，且定位一致性损失的计算和位姿增量参数的更新分别采用与相机外参标定处理相同的损失函数形式和参数更新方式，因而能够使车辆定位位姿优化与相机外参标定在参数表达、误差计算及参数调整过程上保持一致，减少分别构建和维护不同计算流程所产生的算法冗余及重复计算，并降低不同参数表示方式、损失计算标准和更新规则对位姿调整方向造成的干扰；进一步通过定位一致性损失持续更新位姿增量参数，并将更新后的位姿增量参数与初始定位位姿结合确定目标定位位姿，能够逐步减小当前定位位姿与多源定位数据之间的不一致，从而提高定位优化的稳定性和定位精度。

48、应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。