CN111854727B - 一种车辆位姿的修正方法和装置 - Google Patents

Abstract

本发明实施例公开了一种车辆位姿的修正方法和装置，该方法包括：根据预设定位装置提供的车辆当前位置，筛选出车辆当前所在车道中与当前位置满足设定距离范围的第一车道线；对所述第一车道线进行离散化，得到多个车道线离散点，并基于多个车道线离散点拟合出当前位置车辆所在的地面平面；将地面平面的法向量作为车体的实际法向量，利用实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。通过采用上述技术方案，解决了当感知模型信息缺失时无法对自动驾驶车辆进行定位的问题，提升了定位***的鲁棒性。

Description

一种车辆位姿的修正方法和装置

技术领域

本发明涉及自动驾驶技术领域，具体涉及一种车辆位姿的修正方法和装置。

背景技术

在自动驾驶领域，高精度定位至关重要。近年来，深度学习等技术的成果，极大促进了图像语义分割、图像识别领域的发展，这为高精度地图及高精度定位提供了坚实的基础。

在通常的无人驾驶定位方案中，使用高精度地图一般依赖于深度学***面并与深度学习感知模型提供的信息作对比，将得到的误差作为定位过程中的修正量，对车辆的位置姿态进行修正。因此车辆的定位精度也在一定程度上依赖于感知模型的准确程度。

但是，当车辆所处环境光照条件较差，车载相机设备异常等不能正确提供感知模型信息的情况下，定位则会失败，自动驾驶车辆将无法进行正常的行驶。

发明内容

本发明实施例公开一种车辆位姿的修正方法和装置，解决了当感知模型信息缺失时无法对自动驾驶车辆进行定位的问题，提升了定位***的鲁棒性。

第一方面，本发明实施例公开了一种车辆位姿的修正方法，该方法包括：

根据预设定位装置提供的车辆当前位置，筛选出车辆当前所在车道中与所述当前位置满足设定距离范围的第一车道线；

对所述第一车道线进行离散化，得到多个车道线离散点，并基于所述多个车道线离散点拟合出当前位置车辆所在的地面平面；

将所述地面平面的法向量作为车体的实际法向量，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。

可选的，所述基于所述多个车道线离散点拟合出当前位置车辆所在的地面平面，包括：

从所述多个车道线离散点中任意选择预设个数的目标离散点；

利用所述目标离散点拟合出当前位置车辆所在的多个拟合地平面；

对于任意一个拟合地平面，判断所述目标离散点之外的其他离散点到该拟合地平面之间的距离之和；

从多个距离之和中选择和值最小的距离所对应的拟合地平面作为当前位置车辆所在的地面平面。

可选的，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正，包括：

基于卡尔曼滤波算法，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。

可选的，所述方法还包括：

从所述第一车道线中筛选出方向与车辆行驶方向一致的第二车道线；

在所述第二车道线中，如果存在根数达到设定数量且方向向量一致的多个目标车道线，则将该多个目标车道线方向向量的均值作为车体的实际方向向量；

根据所述实际方向向量与车体当前方向向量之间的差值，对所述当前位置处车辆的航向进行修正。

可选的，所述方法还包括：

基于预设导航地图，确定第一车道线的高程的平均值；

将所述第一车道线的高程的平均值作为当前位置处车辆的高程，以对所述当前位置下车辆的高程进行修正。

第二方面，本发明实施例还提供了一种车辆位姿的修正装置，该装置包括：

第一车道线筛选模块，被配置为根据预设定位装置提供的车辆当前位置，筛选出车辆当前所在车道中与所述当前位置满足设定距离范围的第一车道线；

地面平面拟合模块，被配置为对所述第一车道线进行离散化，得到多个车道线离散点，并基于所述多个车道线离散点拟合出当前位置车辆所在的地面平面；

角度修正模块，被配置为将所述地面平面的法向量作为车体的实际法向量，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。

可选的，所述地面平面拟合模块，具体用于：

对所述第一车道线进行离散化，得到多个车道线离散点，并从所述多个车道线离散点中任意选择预设个数的目标离散点；

利用所述目标离散点拟合出当前位置车辆所在的多个拟合地平面；

对于任意一个拟合地平面，判断所述目标离散点之外的其他离散点到该拟合地平面之间的距离之和；

从多个距离之和中选择和值最小的距离所对应的拟合地平面作为当前位置车辆所在的地面平面。

可选的，所述角度修正模块，具体用于：

将所述地面平面的法向量作为车体的实际法向量，基于卡尔曼滤波算法，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。

可选的，所述装置还包括：

第二车道线筛选模块，被配置为从所述第一车道线中筛选出方向与车辆行驶方向一致的第二车道线；

实际方向向量确定模块，被配置为在所述第二车道线中，如果存在根数达到设定数量且方向向量一致的多个目标车道线，则将该多个目标车道线方向向量的均值作为车体的实际方向向量；

朝向修正模块，被配置为根据所述实际方向向量与车体当前方向向量之间的差值，对所述当前位置处车辆的航向进行修正。

可选的，所述装置还包括：

车道线高程确定模块，被配置为基于预设导航地图，确定第一车道线的高程的平均值；

车辆高程修正模块，被配置为将所述第一车道线的高程的平均值作为当前位置处车辆的高程，以对所述当前位置下车辆的高程进行修正。

第三方面，本发明实施例还提供了一种车载终端，包括：

存储有可执行程序代码的存储器；

与所述存储器耦合的处理器；

所述处理器调用所述存储器中存储的所述可执行程序代码，执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤。

第四方面，本发明实施例还提供了一种计算机可读存储介质，其存储计算机程序，所述计算机程序包括用于执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤的指令。

第五方面，本发明实施例还提供了一种计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤。

本实施例提供的技术方案，当感知模型信息缺失无法对车辆进行定位时，通过将第一车道线离散化，并利用多个车道线离散点拟合出当前位置车辆所在的地面平面，可将地面平面的法向量作为车体的实际法向量，利用实际法向量与当前车体法向量之差可对车体的横滚角和俯仰角进行修正，解决了当感知模型信息缺失时无法对自动驾驶车辆进行定位的问题，提升了定位***的鲁棒性。

本发明的发明点包括：

1、本发明实施例的方案，当感知信息缺失时仍能够提供车辆的高程，俯仰角，横滚角以及航向角的四自由度信息，解决了在感知模型信息缺失的情况下无法对车辆进行定位的问题，保证车辆在感知模型信息缺失的情况下仍能保持正常的行驶状态，是本发明的发明点之一。

2、利用第一车道线离散点拟合出当前位置车辆所在的地面平面，通过将地面平面的法向量作为车体的实际法向量，并利用实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正，解决了在感知模型缺失的情况下无法对自动驾驶车辆的横滚角和俯仰角进行定位的问题，使得自动驾驶车辆在感知模型信息缺失的情况下能够正常行驶，提升了定位***的鲁棒性，是本发明的发明点之一。

3、通过从与车辆行驶方向一致的第二车道线中，筛选出根数达到设定数量且方向向量一致的多个目标车道线。利用多个目标车道线方向向量的均值与车体当前方向向量之间的差值，对车辆的航向进行修正，解决了在感知模型信息缺失的情况下无法对自动驾驶车辆的航向进行定位的问题，使得自动驾驶车辆在感知模型信息缺失的情况下仍然能够正常行驶，提升了定位***的鲁棒性，是本发明的发明点之一。

4、通过确定导航地图中车辆当前位置周围几米范围内的车道线高程，可将确定出的车道线高程作为车辆当前位置的高程。在感知模型信息缺失的情况下，可将车辆高程拉低到车道线位置，同时也可以提高后续车辆姿态修正的准确性，是本发明的发明点之一。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1a为本发明实施例提供的一种车辆位姿的修正方法的流程示意图；

图1b为本发明实施例提供的一种车体的横滚角和俯仰角修正方法的流程示意图；

图1c为本发明实施例提供的一种地面平面的拟合示意图；

图1d为本发明实施例提供的一种车辆位置示意图；

图1e为本发明实施例提供的一种对车辆横滚角和俯仰角进行修正的示意图；

图2a是本发明实施例提供的一种车体航向的修正方法的流程示意图；

图2b为本发明实施例提供的一种对车体进行航向修正前的示意图。

图2c为本发明实施例提供的一种对车体进行航向修正后的示意图；

图3a为本发明是实施例提供的一种车体高程修正方法的流程示意图；

图3b为本发明实施例提供的一种对车体进行高程修正的示意图；

图4为本发明实施例提供的一种车辆位姿的修正装置的结构示意图；

图5为本发明实施例提供的一种车载终端的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、***、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

实施例一

请参阅图1a，图1a为本发明实施例提供的一种车辆位姿的修正方法的流程示意图，该方法应用于自动驾驶中，典型的是应用于由于感知模型信息缺失而无法对自动驾驶车辆进行定位的场景下，该方法可由车辆位姿的修正方法装置来执行，该装置可通过软件和/或硬件的方式实现，一般可集成在车载电脑、车载工业控制计算机(Industrialpersonal Computer，IPC)等车载终端中，本发明实施例不做限定。如图1a所示，本发明实施例提供的位姿修正过程发生在高精度地图初始化后，从车辆进入地图定位算法到离开地图定位算法的过程中，具体车辆位姿的修正包括利用车道线信息对车辆高程、俯仰角、横滚角和航向的分别修正，上述各个姿态角的修正过程不存在执行顺序的先后之分。下面，本实施例先对车辆的横滚角和俯仰角的修正过程进行详细介绍：

图1b为本发明实施例提供的一种车体的横滚角和俯仰角修正方法的流程示意图，如图1b所示，该方法具体包括：

110、根据预设定位装置提供的车辆当前位置，筛选出车辆当前所在车道中与当前位置满足设定距离范围的第一车道线。

其中，预设定位装置为单点GPS或低精度的消费级定位设备。该预设定位装置可为车辆提供一个粗略的位置，即车辆当前位置。根据当前位置，可从误差级别为厘米级的高精度地图中确定出当前位置处多条车道线的属性，该属性包括车道线的类别、所在位置以及方向等。基于车道线的属性，可筛选出在当前车辆所在车道中的车道线。通过对筛选出的车道线进行裁剪，可得到与车辆当前位置满足设定距离范围(例如50米范围内)的第一车道线。

120、对第一车道线进行离散化，得到多个车道线离散点，并基于多个车道线离散点拟合出当前位置车辆所在的地面平面。

由于车道线是连续的曲线，难以表达出车道线中的某个位置，因此可将车道线离散为多个离散点，例如可将连续的第一车道线离散为以1米为单位的10个离散点。

本实施例中，可利用多个车道线离散点拟合出当前车辆所在的地面平面。示例性的，具体拟合方法可以为：

从多个车道线离散点中任意选择预设个数的目标离散点，其中，目标离散点的个数至少为3个。利用目标离散点拟合出当前位置车辆所在的多个拟合地平面。对于任意一个拟合地平面，判断目标离散点之外的其他离散点到该拟合地平面之间的距离之和，并从多个距离之和中选择和值最小的距离所对应的拟合地平面作为当前位置车辆所在的地面平面。

具体的，图1c为本发明实施例提供的一种地面平面的拟合示意图，如图1c所示，1表示利用任意目标离散点拟合出的拟合地平面，通过利用上述实施例方式，可从多个拟合地平面中选择出当前位置车辆所在的地面平面，如图1c中的2所表示的平面。

示例性的，每次可随机选择三个目标离散点进行平面拟合，并将每次拟合得到的拟合地平面作为候选地平面，并计算除目标离散点之外的其他离散点到该候选地平面之间的距离之和。如果下次拟合得到的地平面对应的距离和大于候选地平面所对应的距离和，则保持候选地平面不变，直到某次拟合得到的地平面所对应的距离之和小于候选地平面所对应的距离之和时，将当前拟合得到的地平面作为新的候选地平面，以代替之前的候选地平面。例如，如果第一次拟合得到的候选地平面对应的距离和值是10厘米，第二次拟合得到的地平面所对应的距离和值是12厘米，则仍保持第一次拟合得到的候选地平面不变。如果第三次拟合得到的地平面所对应的距离和值是8厘米，则将第三次拟合得到的地平面代替第一次拟合得到的候选地平面作为新的候选地平面。通过按照上述方式依次迭代求解，可确定到对应和值最小的拟合地平面作为车辆所在的地面平面。

130、将地面平面的法向量作为车体的实际法向量，利用实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。

其中，当前车体法向量即为车体航向所在的轴向。车体的航向可根据当前位置和IMU(Inertial measurement unit，惯性测量单元)提供的车体的加速度和角速度来确定。在修正车体的横滚角和俯仰角时，可利用卡尔曼滤波算法来修正，具体可通过不断调整车体的横滚角和俯仰角，使得实际法向量与当前车体法向量之差不断逼近于零。

具体的，图1d为本发明实施例提供的一种车辆位置示意图，图1e为本发明实施例提供的一种对车辆横滚角和俯仰角进行修正的示意图。如图1d所示，1表示车体的实际法向量，2表示车体的当前车体法向量，1和2之间存在偏差。如图1e所示，3表示对车体的横滚角进行修正，4表示对车体的俯仰角进行修正。通过利用卡尔曼滤波算法，可使得实际法向量与当前车体法向量之差趋近于为零。

本实施例提供的技术方案，在感知模型信息缺失无法对车辆进行定位时，通过将第一车道线离散化，并利用多个车道线离散点拟合出当前位置车辆所在的地面平面，可将地面平面的法向量作为车体的实际法向量，利用实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正，解决了当感知模型信息缺失时无法对车辆进行定位的问题，提升了定位***的鲁棒性。

实施例二

请参阅图2a，图2a是本发明实施例提供的一种车体航向的修正方法的流程示意图。本实施例在上述实施例的基础上进行了优化，对车辆的航向进行了进一步的修正。如图2a所示，该方法包括：

210、根据预设定位装置提供的车辆当前位置，筛选出车辆当前所在车道中与当前位置满足设定距离范围的第一车道线。

220、从第一车道线中筛选出方向与车辆行驶方向一致的第二车道线。

本实施例中，当车辆行驶在高精度地图路段时，通常沿着车道线行驶，因此可以使用车道线的轨迹方向对车辆的航向进行修正。一般的，当车辆进行转向或变道等动作时，车辆行驶方向与当前车道线轨迹方向不同，因此一般无法利用车道线对车辆进行航向修正，即在对车辆进行航向修正时，需识别出与车辆行驶方向一致的车道线。

示例性的，根据第一车道线的属性，可过滤掉与车辆行驶方向不一致的汇入线，阻止线，潮汐线等车道线，保留与车辆行驶方向一致的第二车道线。其中，车辆的行驶方向可通过车载陀螺仪进行判断。

230、在第二车道线中，如果存在根数达到设定数量且方向向量一致的多个目标车道线，则将该多个目标车道线方向向量的均值作为车体的实际方向向量。

其中，车辆线的方向向量为过车道线离散点的切线向量。对于多个第二车道线，可统计每个第二车道线的切线向量，如果绝大多数(例如90％)的第二车道线的方向向量一致，则将所有方向向量一致的多个目标车道线方向向量的均值作为车体的实际方向向量。本实施例中，计算车体的实际方向向量是为了利用车道线的轨迹方向对车辆的航向进行修正。

240、根据实际方向向量与车体当前方向向量之间的差值，对当前位置处车辆的航向进行修正。

其中，车体当前方向向量可通过GPS提供的车辆位置、IMU提供的车辆的加速度和角速度得到。

示例性的，可采用卡尔曼滤波算法根据实际方向向量与车体当前方向向量之间的差值，对当前位置处车辆的航向进行修正，具体修正过程可参照车体横滚角和俯仰角的修正方式，本实施例不再赘述。

具体的，图2b为本发明实施例提供的一种对车体进行航向修正前的示意图，图2c为本发明实施例提供的一种对车体进行航向修正后的示意图。如图2b所示，1表示车体的实际方向向量，2表示车体的当前方向向量，3表示车体的实际法向量，4表示当前车体法向量。在对车体的航向修正前，车体的实际方向向量与当前方向向量之间存在偏差。如图2c所示，通过对车体的航向进行修正，可使得车体的当前方向向量2不断逼近于车体的实际方向向量1。

本实施例提供的技术方案，解决当感知模型信息缺失时无法对车辆航向进行定位的问题，提高了定位***的鲁棒性。

实施例三

图3a为本发明是实施例提供的一种车体高程修正方法的流程示意图，如图3a所示，车体高程的修正方法包括：

310、根据预设定位装置提供的车辆当前位置，筛选出车辆当前所在车道中与当前位置满足设定距离范围的第一车道线。

当车辆在经过主辅路，高架路等特殊情况下，同一位置处可能存在多层路段，因此需要加以区分，过滤出车辆所处的当前路段，并裁剪出车辆当前位置处一定范围内的第一车道线。

320、基于预设导航地图，确定第一车道线的高程的平均值。

330、将第一车道线的高程的平均值作为当前位置处车辆的高程，以对当前位置下车辆的高程进行修正。

本实施例中，车辆的高程是指消费级定位设备提供的车辆的海拔高度。在利用消费级定位设备对车辆进行定位时，定位精度较低，有时会有几米甚至十几米的误差，但在该误差范围内同一路段的高度变化较小。因此，可根据消费级定位设备提供的车辆当前位置，在导航地图中搜索当前位置附近几米的车道线信息，从而利用该车道线信息对车辆的高程进行修正，以将车辆拉到车道线高度。

具体的，可在导航地图中搜索车辆当前位置周围几米范围内车道线，并计算车道线的平均高度，即车道线的高程。将计算出的车道线的高程作为车辆当前位置的高程，以完成对车辆高程的修正。图3b为本发明实施例提供的一种对车体进行高程修正的示意图，如图3b所示，利用第一车道线对车体进行高程修正后，可将车辆拉低到车道线高度。

综上所述，上述实施例提供的技术方案，在当感知信息缺失时仍能够提供车辆的高程，俯仰角，横滚角以及航向角的四自由度信息，保持车辆的正常行驶，解决了当感知模型信息缺失时无法对车辆进行定位的问题，提高了定位***的鲁棒性。

需要说明的是，本发明实施例对车辆高程、俯仰角、横滚角和航向的修正顺序不存在先后之分，可同步进行，也可先后执行，本发明实施例对此不做具体限定。本发明实施例中，可优选先对车辆的高程进行修正，以提高后续姿态修正的准确性。在高程修正完成后，可对车辆的横滚角、俯仰角和航向进行依次修正，以保证车辆能够在感知模型缺失无法定位的情况下能够正常行驶。

还需要说明的是，当感知模型正常时，也可以利用本发明实施例的技术方案对车辆位姿进行修正，并将修正后的结果与利用感知模型得到的车辆位姿信息进行融合，以提高车辆的定位精度。

实施例四

请参阅图4，图4为本发明实施例提供的一种车辆位姿的修正装置的结构示意图。如图4所示，该装置包括：第一车道线筛选模块410、地面平面拟合模块420和角度修正模块430；

其中，第一车道线筛选模块410，被配置为根据预设定位装置提供的车辆当前位置，筛选出车辆当前所在车道中与所述当前位置满足设定距离范围的第一车道线；

地面平面拟合模块420，被配置为对所述第一车道线进行离散化，得到多个车道线离散点，并基于所述多个车道线离散点拟合出当前位置车辆所在的地面平面；

角度修正模块430，被配置为将所述地面平面的法向量作为车体的实际法向量，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。

本实施例提供的技术方案，在感知模型信息缺失无法对车辆进行定位时，通过将第一车道线离散化，并利用多个车道线离散点拟合出当前位置车辆所在的地面平面，可将地面平面的法向量作为车体的实际法向量，利用实际法向量与当前车体法向量之差可对车体的横滚角和俯仰角进行修正，解决了当感知模型信息缺失时无法对车辆进行定位的问题，提升了定位***的鲁棒性。

可选的，所述地面平面拟合模块，具体用于：

对所述第一车道线进行离散化，得到多个车道线离散点，并从所述多个车道线离散点中任意选择预设个数的目标离散点；

利用所述目标离散点拟合出当前位置车辆所在的多个拟合地平面；

对于任意一个拟合地平面，判断所述目标离散点之外的其他离散点到该拟合地平面之间的距离之和；

从多个距离之和中选择和值最小的距离所对应的拟合地平面作为当前位置车辆所在的地面平面。

可选的，所述角度修正模块，具体用于：

将所述地面平面的法向量作为车体的实际法向量，基于卡尔曼滤波算法，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。

可选的，所述装置还包括：

第二车道线筛选模块，被配置为从所述第一车道线中筛选出方向与车辆行驶方向一致的第二车道线；

实际方向向量确定模块，被配置为在所述第二车道线中，如果存在根数达到设定数量且方向向量一致的多个目标车道线，则将该多个目标车道线方向向量的均值作为车体的实际方向向量；

朝向修正模块，被配置为根据所述实际方向向量与车体当前方向向量之间的差值，对所述当前位置处车辆的航向进行修正。

可选的，所述装置还包括：

车道线高程确定模块，被配置为基于预设导航地图，确定第一车道线的高程的平均值；

车辆高程修正模块，被配置为将所述第一车道线的高程的平均值作为当前位置处车辆的高程，以对所述当前位置下车辆的高程进行修正。

本发明实施例所提供的车辆位姿的修正装置可执行本发明任意实施例所提供的车辆位姿的修正方法，具备执行方法相应的功能模块和有益效果。未在上述实施例中详尽描述的技术细节，可参见本发明任意实施例所提供的车辆位姿的修正方法。

实施例五

请参阅图5，图5为本发明实施例提供的一种车载终端的结构示意图。如图5所示，该车载终端可以包括：

存储有可执行程序代码的存储器701；

与存储器701耦合的处理器702；

其中，处理器702调用存储器701中存储的可执行程序代码，执行本发明任意实施例所提供的车辆位姿的修正方法。

本发明实施例公开一种计算机可读存储介质，其存储计算机程序，其中，该计算机程序使得计算机执行本发明任意实施例所提供的车辆位姿的修正方法。

本发明实施例公开一种计算机程序产品，其中，当计算机程序产品在计算机上运行时，使得计算机执行本发明任意实施例所提供的车辆位姿的修正方法的部分或全部步骤。

在本发明的各种实施例中，应理解，上述各过程的序号的大小并不意味着执行顺序的必然先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

在本发明所提供的实施例中，应理解，“与A相应的B”表示B与A相关联，根据A可以确定B。但还应理解，根据A确定B并不意味着仅仅根据A确定B，还可以根据A和/或其他信息确定B。

另外，在本发明各实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

上述集成的单元若以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可获取的存储器中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或者部分，可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干请求用以使得一台计算机设备(可以为个人计算机、服务器或者网络设备等，具体可以是计算机设备中的处理器)执行本发明的各个实施例上述方法的部分或全部步骤。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质包括只读存储器(Read-Only Memory，ROM)、随机存储器(Random Access Memory，RAM)、可编程只读存储器(Programmable Read-only Memory，PROM)、可擦除可编程只读存储器(Erasable Programmable Read Only Memory，EPROM)、一次可编程只读存储器(One-time Programmable Read-Only Memory，OTPROM)、电子抹除式可复写只读存储器(Electrically-Erasable Programmable Read-Only Memory，EEPROM)、只读光盘(CompactDisc Read-Only Memory，CD-ROM)或其他光盘存储器、磁盘存储器、磁带存储器、或者能够用于携带或存储数据的计算机可读的任何其他介质。

以上对本发明实施例公开的一种车辆位姿的修正方法和装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种车辆位姿的修正方法，其特征在于，包括：

根据预设定位装置提供的车辆当前位置，筛选出车辆当前所在车道中与所述当前位置满足设定距离范围的第一车道线；

对所述第一车道线进行离散化，得到多个车道线离散点，并基于所述多个车道线离散点拟合出当前位置车辆所在的地面平面；

将所述地面平面的法向量作为车体的实际法向量，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正；

其中，所述基于所述多个车道线离散点拟合出当前位置车辆所在的地面平面，包括：

从所述多个车道线离散点中任意选择预设个数的目标离散点；

利用所述目标离散点拟合出当前位置车辆所在的多个拟合地平面；

对于任意一个拟合地平面，判断所述目标离散点之外的其他离散点到该拟合地平面之间的距离之和；

从多个距离之和中选择和值最小的距离所对应的拟合地平面作为当前位置车辆所在的地面平面。

2.根据权利要求1所述的方法，其特征在于，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正，包括：

基于卡尔曼滤波算法，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。

3.根据权利要求1-2任一所述的方法，其特征在于，所述方法还包括：

从所述第一车道线中筛选出方向与车辆行驶方向一致的第二车道线；

在所述第二车道线中，如果存在根数达到设定数量且方向向量一致的多个目标车道线，则将该多个目标车道线方向向量的均值作为车体的实际方向向量；

根据所述实际方向向量与车体当前方向向量之间的差值，对所述当前位置处车辆的航向进行修正。

4.根据权利要求1所述的方法，其特征在于，所述方法还包括：

基于预设导航地图，确定第一车道线的高程的平均值；

将所述第一车道线的高程的平均值作为当前位置处车辆的高程，以对所述当前位置下车辆的高程进行修正。

5.一种车辆位姿的修正装置，应用于自动驾驶，其特征在于，包括：

第一车道线筛选模块，被配置为根据预设定位装置提供的车辆当前位置，筛选出车辆当前所在车道中与所述当前位置满足设定距离范围的第一车道线；

地面平面拟合模块，被配置为对所述第一车道线进行离散化，得到多个车道线离散点，并基于所述多个车道线离散点拟合出当前位置车辆所在的地面平面；

角度修正模块，被配置为将所述地面平面的法向量作为车体的实际法向量，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正；

其中，所述地面平面拟合模块，具体用于：

对所述第一车道线进行离散化，得到多个车道线离散点，并从所述多个车道线离散点中任意选择预设个数的目标离散点；

利用所述目标离散点拟合出当前位置车辆所在的多个拟合地平面；

对于任意一个拟合地平面，判断所述目标离散点之外的其他离散点到该拟合地平面之间的距离之和；

从多个距离之和中选择和值最小的距离所对应的拟合地平面作为当前位置车辆所在的地面平面。

6.根据权利要求5所述的装置，其特征在于，所述角度修正模块，具体用于：

将所述地面平面的法向量作为车体的实际法向量，基于卡尔曼滤波算法，利用所述实际法向量与当前车体法向量之差对车体的横滚角和俯仰角进行修正。

7.根据权利要求5所述的装置，其特征在于，所述装置还包括：

第二车道线筛选模块，被配置为从所述第一车道线中筛选出方向与车辆行驶方向一致的第二车道线；

实际方向向量确定模块，被配置为在所述第二车道线中，如果存在根数达到设定数量且方向向量一致的多个目标车道线，则将该多个目标车道线方向向量的均值作为车体的实际方向向量；

朝向修正模块，被配置为根据所述实际方向向量与车体当前方向向量之间的差值，对所述当前位置处车辆的航向进行修正。

8.根据权利要求5所述的装置，其特征在于，所述装置还包括：

车道线高程确定模块，被配置为基于预设导航地图，确定第一车道线的高程的平均值；

车辆高程修正模块，被配置为将所述第一车道线的高程的平均值作为当前位置处车辆的高程，以对所述当前位置下车辆的高程进行修正。

Images