一种适用于车辆底盘集成控制的路面工况预瞄方法技术

本发明专利技术公开了一种适用于车辆底盘集成控制的路面工况预瞄方法，对路面的高程、坡度、倾斜、不平度等信息进行在线实时扫描、预估和测量，该方法包括：首先，本发明专利技术通过安装多个传感器，实现了车前路面高程信息提取和2.5D重构。其次，设计了时间戳同步的点云运动补偿，建立了局部地图和全局地图的特征点匹配，进而提高地形扫描的精度。接下来，利用卡尔曼滤波实现了地图融合和更新。最后，提取轮胎处地形网格的高程数据序列，结合该高程数据序列和车前路面高程信息完成对坡度、倾斜、不平度的提取。本发明专利技术提高了车辆底盘集成控制对路面工况的适应性和匹配性，解决了针对GPS信号失效、不稳定、更新慢的问题，提高了车前地形的扫描精度和稳定性。

【技术实现步骤摘要】
一种适用于车辆底盘集成控制的路面工况预瞄方法
本专利技术属于车前路面工况特征识别领域，具体涉及一种路面的高程、坡度、不平度等信息的在线实时扫描、预估和测量方法。
技术介绍
车前路面高程特征的在线实时预瞄是车辆主动/半主动悬挂调控和底盘动力学集成控制的首要关键问题。车辆在行驶时，由于车前路面工况(包括车前路面的不平度、倾斜和坡度等)是随机的，因而需要利用传感器进行实时测量。目前国内外多通过差分GPS和IMU(惯性测量单元)组合的方式测量车身位置和姿态，并将激光雷达测量车前路面的点云信息装配到车辆行驶的世界坐标系下，在此基础上通过点云的区域分割和重构即可获得车前的路面工况。现有预瞄方法提取的路面高程特征虽可用于无人车辆的行驶路线规划和自主避障，但其精度、准确性和实时性远不能满足车辆底盘的悬挂调控和底盘动力学集成控制的需求，主要体现在以下几个方面：(1)因车前路面扫描是在车辆运动过程中进行的，由于雷达同帧点云存在时空不匹配的问题，直接导致了点云数据畸变，且车速越快，引发的点云畸变越严重。(2)车辆在野外空旷或城市建筑物遮挡的环境中行驶时，会存在GPS信号失效、反射、干涉等问题，会对车辆定位的精度产生较大的影响，该定位误差也会传递至路面高程的点云数据序列中。(3)由于激光雷达通常安装于车辆驾驶室位置，方向朝前，为了获得车辆各轮胎位置处的路面高程，所建地图尺寸需超过车身长度。传统基于卡尔曼滤波的点云测量值更新方法是对所有地形网格的高程值予以更新，运算量会随着地图分辨率的提高呈平方级数增加，实时性不好。
技术实现思路
为了解决现有路面工况预瞄方法存在的精度低、实时性不好的问题，本专利技术提出了一种基于多传感器信息融合和同步修正技术的车前路面工况预瞄方法，通过行车环境的智能感知来实现车辆底盘集成控制系统对特定路面的适应能力。本专利技术为了达到上述目的，采取如下技术方案：一种适用于车辆底盘集成控制的路面工况预瞄方法，该方法是基于已安装差分GPS、IMU和激光雷达的实验车实现的，该方法包括以下步骤：步骤1、利用激光雷达获得车前地面的扫描数据；利用卫星定位差分和/或惯性测量单元获得车辆的定位信息；基于所述车前地面的扫描数据，得到车前路面点云数据，并将所述车前路面点云数据和车辆的定位信息按照预定的数据存储结构以地形网格形式存储，形成局部地图；步骤2、结合所述数据存储结构以及激光雷达、惯性测量单元各自的工作频率，设计基于时间戳同步的点云运动补偿算法，所述运动补偿算法用于将不同传感器获得的数据同步至同一时刻的数据；基于所述点云运动补偿算法和所述车辆的定位信息对所述车前路面点云数据进行运动补偿，得到经过运动补偿的车前路面点云数据；步骤3、基于局部地图构建全局地图；从全局地图和局部地图中提取特征点，利用全局地图和局部地图进行特征点匹配，得到经过特征点匹配的局部地图；步骤4、依据激光雷达的安装位置和经过特征点匹配的局部地图和/或经过运动补偿的车前路面点云数据在局部地图上的分布，标记感兴趣的地图更新感兴趣区域，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新；步骤5、结合所述经过更新的车前路面点云数据和车辆的运动学和动力学模型，预测未来时刻车辆的行驶轨迹，实时提取轮胎位置处的高程数据序列，基于所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列提取车前路面的不平度、倾斜和坡度。进一步地，所述基于所述车前地面的扫描数据，得到车前路面点云数据，包括：将所述车前地面的扫描数据转换为世界坐标系下的数据；对经过坐标系转换的数据进行水平面投影，得到车前路面点云数据，水平面投影的计算公式为：其中，p为投影获得的地形高程坐标，SrSP为激光雷达获得的车前地面的扫描数据，和MrSM分别为激光雷达坐标系到世界坐标系的旋转矩阵和平移向量，q为车体欧拉角。进一步地，所述基于所述点云运动补偿算法和所述车辆的定位信息对车前路面点云数据进行运动补偿，包括：从惯性测量单元中获得的车体位姿序列中找到距离该时间点最近的两个车体位姿(pi,qi)和(pi+1,qi+1)，通过线性插值，获取扫描此帧点云时的车体位姿(p,q)，计算公式为：其中，t为激光点时间戳在惯导两个相邻位姿时刻的时间占比，t∈[0,1]；pi、pi+1为车体相邻时刻位置；qi、qi+1为四元数表示的车体相邻时刻姿态角，Δθ为四元数向量的夹角；相应地，所述基于所述点云运动补偿算法对所述车前路面点云数据进行运动补偿，包括：通过插值得到的车体位姿修正所述车前路面点云数据。进一步地，所述特征点包括直线特征点或平面特征点；所述利用全局地图和局部地图进行特征点匹配，包括：基于提取出的特征点，构建特征约束方程；所述特征约束方程为：以多对局部地图特征点与全局地图特征点之间的距离和最小为优化目标函数J，实现局部地图与全局地图的配准和对齐，所述优化目标函数为：其中，J为优化目标函数；di为局部地图特征点与全局地图特征点之间的距离；为局部地图的特征点，为全局地图的特征点，为全局地图特征点与局部地图特征点之间的旋转关系，为平移关系；di包括直线特征点之间的距离dε或平面特征点之间的距离dH,其计算公式为：其中，为局部地图上的边线特征点i，分别为全局地图上的边线特征点j,l,m。进一步地，标记感兴趣的地图更新感兴趣区域，包括：对所述局部地图进行二值化标记，设当前有效的测量区域为感兴趣的地图更新感兴趣区域，并标记为1，其余区域标记为0。进一步地，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新，包括：在所述地图更新感兴趣区域中，利用同点多次测量得到的车前路面点云数据进行地图更新感兴趣区域的更新。进一步地，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新，包括：在应用当前测量点云数据更新地图更新感兴趣区域时，基于极大似然估计，进行车辆位姿估计的误差、激光雷达测量的误差计算以及网格协方差传递模型计算，并根据系统误差传递模型获得每个点云数据的三维协方差并存储；将点云数据网格化，若单元格中存在上一时刻测量的高程值，则基于马氏距离与设定阈值之间的大小关系来判定该单元格的高程值是否满足条件：当上一时刻的高程值不满足判定条件时，将当前的测量结果作为该单元格的高程值并保存对应的方差；当上一时刻的高程值满足判定条件时，通过卡尔曼滤波将地形的当前测量高程数据与已构建地形网格高程数据做数据融合，进而对高程值和方差进行存储，完成当前时刻局部地图的更新；其中，滤波后的车前地形高程和方差计算公式为：其中，和分别为滤波后车前地形高程和方差；和分别为滤波前车前地形高程和方差；为地形网格内当前测量的所有高程值的均值、为地形网格内当前测量的所有高程值的方差、为已构建地形网格内所有高程值的均值、为已构建地形网格内所有高程值的方差；随着车辆的运动，应用各个时刻本文档来自技高网...

【技术保护点】
1.一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于，该方法包括：/n步骤1、利用激光雷达获得车前地面的扫描数据；利用卫星定位差分和/或惯性测量单元获得车辆的定位信息；基于所述车前地面的扫描数据，得到车前路面点云数据，并将所述车前路面点云数据和所述车辆定位信息按照预定的数据存储结构以地形网格形式存储，形成局部地图；/n步骤2、结合所述数据存储结构以及激光雷达、惯性测量单元各自的工作频率，设计基于时间戳同步的点云运动补偿算法，并基于所述点云运动补偿算法和所述车辆的定位信息对车前路面点云数据进行运动补偿，得到经过运动补偿的车前路面点云数据；所述运动补偿算法用于将不同传感器获得的数据同步至同一时刻的数据；/n步骤3、基于局部地图构建全局地图；从全局地图和局部地图中提取特征点，利用全局地图和局部地图进行特征点匹配，得到经过特征点匹配的局部地图；/n步骤4、依据激光雷达的安装位置和所述经过特征点匹配的局部地图和/或经过运动补偿的车前路面点云数据在局部地图上的分布，标记感兴趣的地图更新感兴趣区域，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新；/n步骤5、结合所述经过更新的车前路面点云数据和车辆的运动学和动力学模型，预测未来时刻车辆的行驶轨迹，实时提取轮胎位置处的高程数据序列，基于所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列提取车前路面的不平度、倾斜和坡度。/n...

【技术特征摘要】
1.一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于，该方法包括：
步骤1、利用激光雷达获得车前地面的扫描数据；利用卫星定位差分和/或惯性测量单元获得车辆的定位信息；基于所述车前地面的扫描数据，得到车前路面点云数据，并将所述车前路面点云数据和所述车辆定位信息按照预定的数据存储结构以地形网格形式存储，形成局部地图；
步骤2、结合所述数据存储结构以及激光雷达、惯性测量单元各自的工作频率，设计基于时间戳同步的点云运动补偿算法，并基于所述点云运动补偿算法和所述车辆的定位信息对车前路面点云数据进行运动补偿，得到经过运动补偿的车前路面点云数据；所述运动补偿算法用于将不同传感器获得的数据同步至同一时刻的数据；
步骤3、基于局部地图构建全局地图；从全局地图和局部地图中提取特征点，利用全局地图和局部地图进行特征点匹配，得到经过特征点匹配的局部地图；
步骤4、依据激光雷达的安装位置和所述经过特征点匹配的局部地图和/或经过运动补偿的车前路面点云数据在局部地图上的分布，标记感兴趣的地图更新感兴趣区域，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新；
步骤5、结合所述经过更新的车前路面点云数据和车辆的运动学和动力学模型，预测未来时刻车辆的行驶轨迹，实时提取轮胎位置处的高程数据序列，基于所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列提取车前路面的不平度、倾斜和坡度。


2.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述基于所述车前地面的扫描数据，得到车前路面点云数据，包括：
将所述车前地面的扫描数据转换为世界坐标系下的数据；
对经过坐标系转换的数据进行水平面投影，得到车前路面点云数据，水平面投影的计算公式为：



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


3.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述基于所述点云运动补偿算法和所述车辆的定位信息对车前路面点云数据进行运动补偿，包括：
从惯性测量单元中获得的车体位姿序列中找到距离该时间点最近的两个车体位姿(pi,qi)和(pi+1,qi+1)，通过线性插值，获取扫描此帧点云时的车体位姿(p,q)，计算公式为：



其中，t为激光点时间戳在惯导两个相邻位姿时刻的时间占比，t∈[0,1]；pi、pi+1为车体相邻时刻位置；qi、qi+1为四元数表示的车体相邻时刻姿态角，Δθ为四元数向量的夹角；
通过插值得到的车体位姿修正所述车前路面点云数据。


4.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述特征点包括直线特征点或平面特征点；
所述利用全局地图和局部地图进行特征点匹配，包括：
基于提取出的特征点，构建特征约束方程，所述特征约束方程为：
以多对局部地图特征点与全局地图特征点之间的距离和最小为优化目标函数，实现局部地图与全局地图的配准和对齐，所述优化目标函数为：
其中，J为优化目标函数；di为局部地图特征点与全局地图特征点之间的距离；为局部地图的特征点，为全局地图的特征点，为全局地图特征点与局部地图特征点之间的旋转关系，为平移关系；di包括直线特征点之间的距离dε或平面特征点之间的距离dH,其计...

【专利技术属性】
技术研发人员：倪涛，张泮虹，赵丁选，孔志飞，巩明德，刘爽，王建涛，
申请(专利权)人：燕山大学，
类型：发明
国别省市：河北;13