[发明专利]一种适用于车辆底盘集成控制的路面工况预瞄方法

权利要求书

1.一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于，该方法包括：

步骤1、利用激光雷达获得车前地面的扫描数据；利用卫星定位差分和/或惯性测量单元获得车辆的定位信息；基于所述车前地面的扫描数据，得到车前路面点云数据，并将所述车前路面点云数据和所述车辆定位信息按照预定的数据存储结构以地形网格形式存储，形成局部地图；

步骤2、结合所述数据存储结构以及激光雷达、惯性测量单元各自的工作频率，设计基于时间戳同步的点云运动补偿算法，并基于所述点云运动补偿算法和所述车辆的定位信息对车前路面点云数据进行运动补偿，得到经过运动补偿的车前路面点云数据；所述运动补偿算法用于将不同传感器获得的数据同步至同一时刻的数据；其中，所述基于所述点云运动补偿算法和所述车辆的定位信息对车前路面点云数据进行运动补偿，包括：

从惯性测量单元中获得的车体位姿序列中找到距离该时间戳最近的两个车体位姿(p_i,q_i)和(p_i+1,q_i+1)，通过线性插值，获取扫描此帧点云时的车体位姿(p,q)，计算公式为：

其中，t为激光点时间戳在惯导两个相邻位姿时刻的时间占比，t∈[0,1]；p_i、p_i+1为车体相邻时刻位置；q_i、q_i+1为四元数表示的车体相邻时刻姿态角，Δθ为四元数向量的夹角；

通过插值得到的车体位姿修正所述车前路面点云数据；

步骤3、基于局部地图构建全局地图；从全局地图和局部地图中提取特征点，利用全局地图和局部地图进行特征点匹配，得到经过特征点匹配的局部地图；

步骤4、依据激光雷达的安装位置和所述经过特征点匹配的局部地图和/或经过运动补偿的车前路面点云数据在局部地图上的分布，标记感兴趣的地图更新感兴趣区域，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新；

步骤5、结合经过更新的车前路面点云数据和车辆的运动学和动力学模型，预测未来时刻车辆的行驶轨迹，实时提取轮胎位置处的高程数据序列，基于所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列提取车前路面的不平度、倾斜和坡度。

2.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述基于所述车前地面的扫描数据，得到车前路面点云数据，包括：

将所述车前地面的扫描数据转换为世界坐标系下的数据；

对经过坐标系转换的数据进行水平面投影，得到车前路面点云数据，水平面投影的计算公式为：

其中，p为投影获得的地形高程坐标，_Sr_SP为激光雷达获得的车前地面的扫描数据，和_Mr_SM分别为激光雷达坐标系到世界坐标系的旋转矩阵和平移向量，q为车体欧拉角。

3.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述特征点包括直线特征点或平面特征点；

所述利用全局地图和局部地图进行特征点匹配，包括：

基于提取出的特征点，构建特征约束方程，所述特征约束方程为：

以多对局部地图特征点与全局地图特征点之间的距离和最小为优化目标函数，实现局部地图与全局地图的配准和对齐，所述优化目标函数为：

其中，J为优化目标函数；d_i为局部地图特征点与全局地图特征点之间的距离；为局部地图的特征点，为全局地图的特征点，为全局地图特征点与局部地图特征点之间的旋转关系，为平移关系；d_i包括直线特征点之间的距离d_ε或平面特征点之间的距离d_H,其计算公式为：

其中，为局部地图上的边线特征点i，分别为全局地图上的边线特征点j、l、m。

4.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述标记感兴趣的地图更新感兴趣区域，包括：

对所述局部地图进行二值化标记，设当前有效的测量区域为感兴趣的地图更新感兴趣区域，并标记为1，其余区域标记为0。