[发明专利]一种车辆位姿的修正方法和装置

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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