CN111829514B - 一种适用于车辆底盘集成控制的路面工况预瞄方法 - Google Patents

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

Info

Publication number
CN111829514B
CN111829514B CN202010606602.7A CN202010606602A CN111829514B CN 111829514 B CN111829514 B CN 111829514B CN 202010606602 A CN202010606602 A CN 202010606602A CN 111829514 B CN111829514 B CN 111829514B
Authority
CN
China
Prior art keywords
elevation
vehicle
map
road surface
point cloud
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010606602.7A
Other languages
English (en)
Other versions
CN111829514A (zh
Inventor
倪涛
张泮虹
赵丁选
孔志飞
巩明德
刘爽
王建涛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Yanshan University
Original Assignee
Yanshan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Yanshan University filed Critical Yanshan University
Priority to CN202010606602.7A priority Critical patent/CN111829514B/zh
Publication of CN111829514A publication Critical patent/CN111829514A/zh
Application granted granted Critical
Publication of CN111829514B publication Critical patent/CN111829514B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/40—Correcting position, velocity or attitude
    • G01S19/41—Differential correction, e.g. DGPS [differential GPS]
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

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

Description

一种适用于车辆底盘集成控制的路面工况预瞄方法
技术领域
本发明属于车前路面工况特征识别领域，具体涉及一种路面的高程、坡度、不平度等信息的在线实时扫描、预估和测量方法。
背景技术
车前路面高程特征的在线实时预瞄是车辆主动/半主动悬挂调控和底盘动力学集成控制的首要关键问题。车辆在行驶时，由于车前路面工况(包括车前路面的不平度、倾斜和坡度等)是随机的，因而需要利用传感器进行实时测量。目前国内外多通过差分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。
进一步地，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新，包括：
在所述地图更新感兴趣区域中，利用同点多次测量得到的车前路面点云数据进行地图更新感兴趣区域的更新。
进一步地，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新，包括：
在应用当前测量点云数据更新地图更新感兴趣区域时，基于极大似然估计，进行车辆位姿估计的误差、激光雷达测量的误差计算以及网格协方差传递模型计算，并根据系统误差传递模型获得每个点云数据的三维协方差并存储；
将点云数据网格化，若单元格中存在上一时刻测量的高程值，则基于马氏距离与设定阈值之间的大小关系来判定该单元格的高程值是否满足条件：
当上一时刻的高程值不满足判定条件时，将当前的测量结果作为该单元格的高程值并保存对应的方差；
当上一时刻的高程值满足判定条件时，通过卡尔曼滤波将地形的当前测量高程数据与已构建地形网格高程数据/>做数据融合，进而对高程值和方差进行存储，完成当前时刻局部地图的更新；
其中，滤波后的车前地形高程和方差计算公式为：
其中，和/>分别为滤波后车前地形高程和方差；/>和/>分别为滤波前车前地形高程和方差；/>为地形网格内当前测量的所有高程值的均值、/>为地形网格内当前测量的所有高程值的方差、/>为已构建地形网格内所有高程值的均值、/>为已构建地形网格内所有高程值的方差；
随着车辆的运动，应用各个时刻的测量点云数据更新地图更新感兴趣区域，构建每个时刻的以车辆为中心的局部高程地图。
进一步地，实时提取轮胎位置处的高程数据序列，包括：将轮胎与地面接触的部分近似为一个椭圆，取椭圆范围内地形网格坐标点Pj(xj,yj)的高程值hj(j＝1,...m)加权平均作为轮胎接地处坐标点Pi(xi,yi)的高程值hi，其计算公式为：
其中，wj为权重，选取时以椭圆范围内每个网格距离椭圆中心距离的远近作为权重，即距离越大，权重越小；
权重计算公式为：
wj＝|b-((xj-xi)2+(yj-yi)2)1/2|
其中，b为接地椭圆的长轴长度，xj、yj分别为椭圆范围内地形网格坐标点的横坐标和纵坐标，xi、yi分别为轮胎接地处坐标点的横坐标和纵坐标。
进一步地，基于所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列提取车前路面的不平度、倾斜和坡度，包括：
对所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列进行低通滤波，得到路面的倾斜和坡度；
对所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列进行高通滤波，得到路面的不平度。
本发明与现有技术相比具有如下有益效果：
(1)本发明通过安装多个传感器，实现了车前路面的高程信息提取和2.5D重构，设计了基于时间戳同步的点云运动补偿算法，将不同传感器的获取的数据转换至同一时刻下的数据，避免了因雷达同帧点云存在时空不匹配的问题导致点云数据畸变，提高了车前路面扫描的精度。
(2)本发明建立了局部地图和全局地图，并进行了特征点云的匹配，提高了车辆的定位精度，避免了某些车辆行驶环境下全球定位系统信号失效、反射、干涉等问题导致的车辆定位精度低的问题。
(3)本发明标记了感兴趣区域ROI，通过卡尔曼滤波的方式对感兴趣区域的多次测量值予以融合和更新，相比传统的基于卡尔曼滤波的点云测量值更新方法，缩小了运算量，增强了实时性。
(4)本发明实时提取了车辆轮胎位置处地形网格的高程数据序列，基于轮胎位置处地形网格的高程数据序列和车辆的运动状态提取车前路面的不平度、倾斜度和坡度，进一步提高了车前地形重建的精度，能够得到更准确的车前路面工况。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一个实施例中适用于车辆底盘集成控制的路面工况预瞄方法的流程示意图；
图2为本发明一个实施例中局部地图示意图；
图3为本发明一个实施例中激光点插值示意图；
图4为本发明一个实施例中全局地图示意图；
图5为本发明一个实施例中高程地图数据更新过程流程图；
图6为本发明一个实施例中轮胎接地处椭圆区域示意图。
具体实施方式
车前路面高程特征的在线实时预瞄是车辆主动/半主动悬挂调控和车辆底盘集成控制的关键问题。为此，本发明首先利用多个传感器数据获取车前路面点云数据，也就是车前路面高程信息；之后为了进一步提高精度，设计了时间戳同步的点云运动补偿，建立了局部地图和全局地图的特征点匹配；然后为了实时获取地图信息，利用卡尔曼滤波实现了局部地图感兴趣区域的融合和更新；最后，提取轮胎处地形网格的高程数据序列，完成了对车前路面不平度等信息的提取。解决了现有路面工况预瞄方法存在的精度低、实时性不好的问题。
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
在一个实施例中，如图1所示，其示出了一种适用于车辆底盘集成控制的路面工况预瞄方法的流程图，该方法包括以下步骤：
步骤1：利用激光雷达获得车前地面的扫描数据；利用卫星定位差分和/或惯性测量单元获得车辆的定位信息；基于车前地面的扫描数据，得到车前路面点云数据，并将车前路面点云数据按照预定的数据存储结构以地形网格形式存储，形成局部地图。
其中，得到车前路面点云数据也就是实现车前路面的高程信息提取；将车前路面点云数据和车辆定位信息按照预定的数据存储结构以地形网格形式存储，也就是完成了车体位姿序列和车前地形的2.5D地图重构；形成了初始的局部地图。
本发明中在车体安装如下传感器：卫星定位差分GPS：用于测量车辆的地理位置坐标、航向角、行驶速度；惯性测量单元IMU：用于测量三向姿态角、角速度及加速度；激光雷达：用以扫描车前地形，并获得路面的点云数据；通过车辆的CAN总线获取当前车速、各轮转向角、油门和方向盘及制动的操纵输入等信号。
采用激光雷达完成车前地面的实时点云扫描，结合GPS/IMU获得的车辆定位信息，建立如图2所示坐标系，W、M、B、S分别为世界坐标系、地图坐标系、车体坐标系、雷达坐标系，将激光雷达实时点云扫描获得的地形稠密点云数据SrSP装配到地图坐标系，并投影获得地形高程坐标p，其计算公式如式(1)：
其中，p为投影获得的地形高程坐标，SrSP为激光雷达获得的车前地面的扫描数据，和MrSM分别为激光雷达坐标系到世界坐标系的旋转矩阵和平移向量，q为车体欧拉角。
步骤2：结合车前路面点云数据的数据存储结构以及激光雷达、惯性测量单元各自的工作频率，设计基于时间戳同步的点云运动补偿算法，并基于所述点云运动补偿算法和所述车辆的定位信息对所述车前路面点云数据进行运动补偿，得到经过运动补偿的车前路面点云数据。
其中，运动补偿算法用于将不同传感器获得的数据同步至同一时刻的数据。
经过运动补偿的车前路面点云数据将形成新的局部地图。
考虑到车前地形扫描中点云数据畸变主要是由于车辆运动过程中雷达激光束发射和接收位置不一致造成的。本发明中根据点云的数据存储结构以及激光雷达、惯性测量单元IMU的工作频率，设计基于时间戳同步的点云运动补偿算法。
如图3所示，依据每个激光点的时间戳，从IMU中获取的位姿队列中进行查找，如果位姿的时间戳和激光的时间戳差值在2.304μ之内，那么查找的位姿即为该激光点时刻对应的真实位姿；如果没有找到，那么就通过差值运算获得该激光点的真实位姿。
本实施例中以Velodyn-16激光雷达为例，依据每帧点云中6460个激光点的各自时间戳，从车体位姿序列中找到距离该时间点最近的两个车体位姿(pi,qi)和(pi+1,qi+1)，通过线性插值，获取扫描此帧点云时的车体位姿(p,q)，计算公式如式(2)：
其中，t为激光点时间戳在惯导两个相邻位姿时刻的时间占比，t∈[0,1]；pi、pi+1为车体相邻时刻位置；qi、qi+1为四元数表示的车体相邻时刻姿态角，Δθ为四元数向量的夹角。
通过插值得到的车体位姿修正点云数据，以提高车前地形的预瞄精度。
步骤3：基于局部地图构建全局地图；从全局地图和局部地图中提取特征点，利用全局地图和局部地图进行特征点匹配，得到经过特征点匹配的局部地图。
本申请实施例中在基于车前路面的点云数据，也就是高程信息构建局部地图的同时，实时构建并更新基于八叉树Octree数据结构的全局地图。
如图2、图4所示，图2为局部地图的示意图，图4为全局地图的示意图，全局地图坐标系不随车体运动而变化，地图坐标系M和世界坐标系W位置相同，在z轴上相差初始时刻车体在世界坐标系下的偏航角。
其中，特征点指的是地图中具备明显特征的点，如电线杆的两个端点，经过全局地图与局部地图的特征点匹配之后，局部地图将更加完善和精准。特征点可以是直线特征点，也可以是平面特征点。
利用全局地图和局部地图进行特征点匹配的具体实施方式可以是：提取局部地图与全局地图中的直线特征点或平面特征点，构建特征约束方程。以多对局部地图特征点与全局地图特征点之间的距离和最小为优化目标函数J，实现局部地图与全局地图的配准和对齐，进而完成车体位姿的精确修正，提高地形的扫描精度。具体的数学模型如式(3)所示：
其中，特征约束方程为：优化目标函数J为：/>di为局部地图特征点与全局地图特征点之间的距离；/>为局部地图的特征点，/>为全局地图的特征点，/>为全局地图特征点与局部地图特征点之间的旋转关系，/>为平移关系。
di包括直线特征点之间的距离dε或平面特征点之间的距离dH,其计算公式如式(4)所示：
其中，为局部地图上的边线特征点i，/>分别为全局地图上的边线特征点j、l、m。
还需说明的是：步骤2和步骤3都是对车前路面点云数据进行不同方面的修正，使步骤1中重构出的原始局部地图更加完善和精确，在具体实施时，可以仅执行步骤2的修正，不执行步骤3的修正，也可以仅执行步骤3的修正，不执行步骤2的修正，还可以步骤2和步骤3的修正都执行。在步骤2和步骤3的修正都执行的情况下，可以先执行步骤2，再执行步骤3，此时步骤3中局部地图为经过步骤2中运动补偿之后的点云数据形成的局部地图。
步骤4：依据激光雷达的安装位置和所述经过特征点匹配的局部地图和/或经过运动补偿的车前路面点云数据在局部地图上的分布，标记感兴趣的地图更新感兴趣区域，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新。
标记感兴趣的地图更新感兴趣区域的具体方式可以是：对当前激光雷达扫描点对应的局部地图(地形网格)进行二值化标记，设当前有效的测量区域为ROI，并标记为1，其余区域标记为0。
在车辆行驶时，因为激光雷达扫描线比较密集，使车前地面同点获得了多次测量的机会。因此，为了兼顾实时性与准确性，在ROI区域中，利用同点多次测量得到的车前路面点云数据进行地图更新感兴趣区域的更新，以提高车前地形扫描的精度和稳定性。
如图5所示，其示出了高程地图数据更新的流程图。在应用当前测量点云数据更新地图更新感兴趣区域时，首先，随着高程图位置的更新，基于极大似然估计，进行车辆位姿估计的误差、激光雷达测量的误差计算以及网格协方差传递模型计算，并根据系统误差传递模型获得每个点云数据的三维协方差并存储；然后将点云数据网格化，若单元格中存在上一时刻测量的高程值，则需要基于马氏距离与设定阈值之间的大小关系来判定该单元格的高程值是否满足条件：当上一时刻的高程值不满足判定条件时，需将当前的测量结果作为该单元格的高程值并保存对应的方差；当上一时刻的高程值满足判定条件时，则通过卡尔曼滤波将地形网格的当前测量高程数据与已构建地形网格高程数据/>做数据融合，进而对高程值和方差进行存储，完成了当前时刻局部地图的更新，从而提高车前地形扫描的精度和稳定性。其中，地形网格的当前测量高程数据包括该地形网格内当前测量的所有高程值，/>为该地形网格内当前测量的所有高程值的均值、/>为该地形网格内当前测量的所有高程值的方差；已构建地形网格高程数据包括该已构建地形网格内所有高程值；为已构建地形网格内所有高程值的均值、/>为已构建地形网格内所有高程值的方差；
滤波后的车前地形高程和方差/>计算如式(5)和(6)所示：
其中，和/>分别为滤波后车前地形高程和方差；/>和/>分别为滤波前车前地形高程和方差。
随着车辆的运动，每个时刻的位姿数据以及点云数据均通过上述步骤进行数据更新，构建每个时刻的以车辆为中心的局部高程地图。
步骤5：结合经过更新的车前路面点云数据和车辆的运动学和动力学模型，预测未来时刻车辆的行驶轨迹，实时提取轮胎位置处的高程数据序列，基于所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列提取车前路面的不平度、倾斜和坡度。
其中，实时提取轮胎位置处的高程数据序列的一种可能实施方式为：将轮胎与地面接触的部分近似为一个椭圆，如图6所示，网格半径为r，取椭圆范围内坐标点Pj(xj,yj)处的高程值hj(j＝1,...m)加权平均作为轮胎接地处坐标点Pi(xi,yi)的高程值hi，其计算公式如式(7)所示。
其中，wj为权重，选取时以椭圆范围内每个网格距离椭圆中心距离的远近作为权重，即距离越大，权重越小。权重计算公式如式(8)所示：
wj＝|b-(xj-xi)2+(yj-yi)2)1/2| (8)
其中，b为接地椭圆的长轴长度，xj、yj分别为椭圆范围内地形网格坐标点的横坐标和纵坐标，xi、yi分别为轮胎接地处坐标点的横坐标和纵坐标。
在得到轮胎位置处的高程数据序列之后，对步骤4中经过更新的车前路面点云数据和该轮胎位置处的高程数据序列进行低通滤波，得到路面的倾斜和坡度；对步骤4中经过更新的车前路面点云数据和该轮胎位置处的高程数据序列进行高通滤波，得到路面的不平度。
本发明提出的一种适用于车辆底盘集成控制的路面工况精准预瞄方法，实时并精确地重构了车前路面的2.5D地形网格，解决了针对GPS信号失效、不稳定。更新慢的问题，提高了车前地形的扫描精度和稳定性，在线提取了路面的高程、不平度、坡度等信息，提高了车辆底盘集成控制对路面环境工况的适应性和匹配性。
本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (8)

1.一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于，该方法包括：
步骤1、利用激光雷达获得车前地面的扫描数据；利用卫星定位差分和/或惯性测量单元获得车辆的定位信息；基于所述车前地面的扫描数据，得到车前路面点云数据，并将所述车前路面点云数据和所述车辆定位信息按照预定的数据存储结构以地形网格形式存储，形成局部地图；
步骤2、结合所述数据存储结构以及激光雷达、惯性测量单元各自的工作频率，设计基于时间戳同步的点云运动补偿算法，并基于所述点云运动补偿算法和所述车辆的定位信息对车前路面点云数据进行运动补偿，得到经过运动补偿的车前路面点云数据；所述运动补偿算法用于将不同传感器获得的数据同步至同一时刻的数据；其中，所述基于所述点云运动补偿算法和所述车辆的定位信息对车前路面点云数据进行运动补偿，包括：
从惯性测量单元中获得的车体位姿序列中找到距离该时间戳最近的两个车体位姿(pi,qi)和(pi+1,qi+1)，通过线性插值，获取扫描此帧点云时的车体位姿(p,q)，计算公式为：
其中，t为激光点时间戳在惯导两个相邻位姿时刻的时间占比，t∈[0,1]；pi、pi+1为车体相邻时刻位置；qi、qi+1为四元数表示的车体相邻时刻姿态角，Δθ为四元数向量的夹角；
通过插值得到的车体位姿修正所述车前路面点云数据；
步骤3、基于局部地图构建全局地图；从全局地图和局部地图中提取特征点，利用全局地图和局部地图进行特征点匹配，得到经过特征点匹配的局部地图；
步骤4、依据激光雷达的安装位置和所述经过特征点匹配的局部地图和/或经过运动补偿的车前路面点云数据在局部地图上的分布，标记感兴趣的地图更新感兴趣区域，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新；
步骤5、结合经过更新的车前路面点云数据和车辆的运动学和动力学模型，预测未来时刻车辆的行驶轨迹，实时提取轮胎位置处的高程数据序列，基于所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列提取车前路面的不平度、倾斜和坡度。
2.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述基于所述车前地面的扫描数据，得到车前路面点云数据，包括：
将所述车前地面的扫描数据转换为世界坐标系下的数据；
对经过坐标系转换的数据进行水平面投影，得到车前路面点云数据，水平面投影的计算公式为：
其中，p为投影获得的地形高程坐标，SrSP为激光雷达获得的车前地面的扫描数据，和MrSM分别为激光雷达坐标系到世界坐标系的旋转矩阵和平移向量，q为车体欧拉角。
3.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述特征点包括直线特征点或平面特征点；
所述利用全局地图和局部地图进行特征点匹配，包括：
基于提取出的特征点，构建特征约束方程，所述特征约束方程为：
以多对局部地图特征点与全局地图特征点之间的距离和最小为优化目标函数，实现局部地图与全局地图的配准和对齐，所述优化目标函数为：
其中，J为优化目标函数；di为局部地图特征点与全局地图特征点之间的距离；为局部地图的特征点，/>为全局地图的特征点，/>为全局地图特征点与局部地图特征点之间的旋转关系，/>为平移关系；di包括直线特征点之间的距离dε或平面特征点之间的距离dH,其计算公式为：
其中，为局部地图上的边线特征点i，/>分别为全局地图上的边线特征点j、l、m。
4.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述标记感兴趣的地图更新感兴趣区域，包括：
对所述局部地图进行二值化标记，设当前有效的测量区域为感兴趣的地图更新感兴趣区域，并标记为1，其余区域标记为0。
5.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新，包括：
在所述地图更新感兴趣区域中，利用同点多次测量得到的车前路面点云数据进行地图更新感兴趣区域的更新。
6.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：通过卡尔曼滤波的方式对所述地图更新感兴趣区域进行更新，包括：
在应用当前测量得到的车前路面点云数据更新地图更新感兴趣区域时，基于极大似然估计，进行车辆位姿估计的误差、激光雷达测量的误差计算以及网格协方差传递模型计算，并根据系统误差传递模型获得每个点云数据的三维协方差并存储；
将点云数据网格化，若单元格中存在上一时刻测量的高程值，则基于马氏距离与设定阈值之间的大小关系来判定该单元格的高程值是否满足条件：
当上一时刻的高程值不满足判定条件时，将当前的测量结果作为该单元格的高程值并保存对应的方差；
当上一时刻的高程值满足判定条件时，通过卡尔曼滤波将地形的当前测量高程数据与已构建地形网格高程数据/>做数据融合，进而对高程值和方差进行存储，完成当前时刻局部地图的更新；
其中，滤波后的车前地形高程和方差计算公式为：
其中，和/>分别为滤波后车前地形高程和方差；/>和/>分别为滤波前车前地形高程和方差；/>为地形网格内当前测量的所有高程值的均值、/>为地形网格内当前测量的所有高程值的方差、/>为已构建地形网格内所有高程值的均值、/>为已构建地形网格内所有高程值的方差；
随着车辆的运动，应用各个时刻的测量点云数据更新地图更新感兴趣区域，构建每个时刻的以车辆为中心的局部高程地图。
7.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：所述实时提取轮胎位置处的高程数据序列，包括：
将轮胎与地面接触的部分近似为一个椭圆，取椭圆范围内地形网格坐标点Pj(xj,yj)的高程值hj(j＝1,...m)加权平均作为轮胎接地处坐标点Pi(xi,yi)的高程值hi，其计算公式为：
其中，wj为权重，选取时以椭圆范围内每个网格距离椭圆中心距离的远近作为权重，即距离越大，权重越小；权重计算公式为：
wj＝|b-((xj-xi)2+(yj-yi)2)1/2|
其中，b为接地椭圆的长轴长度，xj、yj分别为椭圆范围内地形网格坐标点的横坐标和纵坐标，xi、yi分别为轮胎接地处坐标点的横坐标和纵坐标。
8.根据权利要求1所述的一种适用于车辆底盘集成控制的路面工况预瞄方法，其特征在于：基于所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列提取车前路面的不平度、倾斜和坡度，包括：
对所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列进行低通滤波，得到路面的倾斜和坡度；
对所述经过更新的车前路面点云数据和所述轮胎位置处的高程数据序列进行高通滤波，得到路面的不平度。
CN202010606602.7A 2020-06-29 2020-06-29 一种适用于车辆底盘集成控制的路面工况预瞄方法 Active CN111829514B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010606602.7A CN111829514B (zh) 2020-06-29 2020-06-29 一种适用于车辆底盘集成控制的路面工况预瞄方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010606602.7A CN111829514B (zh) 2020-06-29 2020-06-29 一种适用于车辆底盘集成控制的路面工况预瞄方法

Publications (2)

Publication Number Publication Date
CN111829514A CN111829514A (zh) 2020-10-27
CN111829514B true CN111829514B (zh) 2023-08-18

Family

ID=72899473

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010606602.7A Active CN111829514B (zh) 2020-06-29 2020-06-29 一种适用于车辆底盘集成控制的路面工况预瞄方法

Country Status (1)

Country Link
CN (1) CN111829514B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114442616A (zh) * 2022-01-05 2022-05-06 中联重科土方机械有限公司 用于挖掘机的控制方法、装置、处理器及挖掘机
CN114416764A (zh) * 2022-02-24 2022-04-29 上海商汤临港智能科技有限公司 地图更新方法、装置、设备及存储介质
CN114897942B (zh) * 2022-07-15 2022-10-28 深圳元戎启行科技有限公司 点云地图的生成方法、设备及相关存储介质
CN115503417A (zh) * 2022-11-03 2022-12-23 北京理工大学 路面高程预瞄方法及基于该方法的主动油气悬架控制系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019023747A1 (en) * 2017-07-31 2019-02-07 Bellator Pty Ltd TERRESTRIAL VEHICLE WITHOUT DRIVER
CN109596078A (zh) * 2019-01-28 2019-04-09 吉林大学 多信息融合路面谱实时测试系统和测试方法
CN110967024A (zh) * 2019-12-23 2020-04-07 苏州智加科技有限公司 可行驶区域的检测方法、装置、设备及存储介质
CN111239757A (zh) * 2020-03-12 2020-06-05 湖南大学 一种路面特征参数自动提取方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DK180774B1 (en) * 2018-10-29 2022-03-04 Motional Ad Llc Automatic annotation of environmental features in a map during navigation of a vehicle

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019023747A1 (en) * 2017-07-31 2019-02-07 Bellator Pty Ltd TERRESTRIAL VEHICLE WITHOUT DRIVER
CN109596078A (zh) * 2019-01-28 2019-04-09 吉林大学 多信息融合路面谱实时测试系统和测试方法
CN110967024A (zh) * 2019-12-23 2020-04-07 苏州智加科技有限公司 可行驶区域的检测方法、装置、设备及存储介质
CN111239757A (zh) * 2020-03-12 2020-06-05 湖南大学 一种路面特征参数自动提取方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种融合密度聚类与区域生长算法的快速障碍物检测方法;李炯;赵凯;张志超;朱愿;;机器人(第01期);全文 *

Also Published As

Publication number Publication date
CN111829514A (zh) 2020-10-27

Similar Documents

Publication Publication Date Title
CN111829514B (zh) 一种适用于车辆底盘集成控制的路面工况预瞄方法
CN111551958B (zh) 一种面向矿区无人驾驶的高精地图制作方法
CN106840179B (zh) 一种基于多传感器信息融合的智能车定位方法
CN111142091B (zh) 一种融合车载信息的自动驾驶系统激光雷达在线标定方法
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
CN112923933A (zh) 一种激光雷达slam算法与惯导融合定位的方法
CN112162297B (zh) 一种剔除激光点云地图中动态障碍伪迹的方法
CN112965063B (zh) 一种机器人建图定位方法
US20230083965A1 (en) Map construction method, apparatus and storage medium
CN115372989A (zh) 基于激光雷达的越野自动小车长距离实时定位系统及方法
CN110208783B (zh) 基于环境轮廓的智能车辆定位方法
CN113947639A (zh) 基于多雷达点云线特征的自适应在线估计标定系统及方法
CN110412596A (zh) 一种基于图像信息和激光点云的机器人定位方法
CN113238251B (zh) 一种基于车载激光雷达的目标级语义定位方法
CN115993825A (zh) 一种基于空地协同的无人车集群控制系统
CN114485698A (zh) 一种交叉路口引导线生成方法及系统
CN111123953B (zh) 人工智能大数据下粒子化移动机器人组及其控制方法
CN113419235A (zh) 一种基于毫米波雷达的无人机定位方法
CN115540850A (zh) 一种激光雷达与加速度传感器结合的无人车建图方法
Parra-Tsunekawa et al. A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles
CN113777589B (zh) 一种基于点特征的lidar与gps/imu联合标定方法
CN117234203A (zh) 一种多源里程融合slam井下导航方法
CN116878542A (zh) 一种抑制里程计高度漂移的激光slam方法
CN114897967B (zh) 一种面向挖掘装备自主作业的物料形态识别方法
CN116520295A (zh) 一种基于自动建图的多激光雷达标定方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant