CN109166140A - 一种基于多线激光雷达的车辆运动轨迹估计方法及系统 - Google Patents

一种基于多线激光雷达的车辆运动轨迹估计方法及系统 Download PDF

Info

Publication number
CN109166140A
CN109166140A CN201810852477.0A CN201810852477A CN109166140A CN 109166140 A CN109166140 A CN 109166140A CN 201810852477 A CN201810852477 A CN 201810852477A CN 109166140 A CN109166140 A CN 109166140A
Authority
CN
China
Prior art keywords
characteristic
point
vehicle
characteristic matching
roughing
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.)
Granted
Application number
CN201810852477.0A
Other languages
English (en)
Other versions
CN109166140B (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.)
Shaanxi Heavy Duty Automobile Co Ltd
Original Assignee
Changan 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 Changan University filed Critical Changan University
Priority to CN201810852477.0A priority Critical patent/CN109166140B/zh
Publication of CN109166140A publication Critical patent/CN109166140A/zh
Application granted granted Critical
Publication of CN109166140B publication Critical patent/CN109166140B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • 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
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory

Abstract

本发明属于智能汽车定位技术领域，具体涉及一种基于多线激光雷达的车辆运动轨迹估计方法及系统，其中方法包括，根据车辆的初始位姿和不同时刻惯性传感器的输出值得到粗选运动轨迹；获取多线激光雷达不同时刻的三维点云数据并进行栅格化处理得到灰度图；对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对；根据特征匹配点对得到相邻时刻的点云数据之间的变换关系；根据所述变换关系得到车辆的精确运动轨迹，基于粗选的轨迹进行特定范围内的SURF匹配，这样就减少了SURF特征匹配的运算量和运算时间。该车辆轨迹估计方法无须依赖高精度地图、GPS和UWB等辅助定位技术，环境适用性强且应用范围广。

Description

一种基于多线激光雷达的车辆运动轨迹估计方法及系统
技术领域
本发明属于智能汽车定位技术领域，具体涉及一种基于多线激光雷达的车辆运动轨迹估计方法及系统。
背景技术
随着全球经济的全面深入发展，城市化进程得到了进一步的推进，为大众的出行、教育、医疗、休闲等带来了高度的便利。但是密集的城市化现状也带来了交通拥堵、交通事故高发、出行成本提高、环境污染急速加剧等一系列的问题。为了解决上述问题，提升道路的通行效率并且降低出现成本，众多的研究机构和汽车厂商逐步加大了对于智能汽车的研究。
智能汽车采用最前沿的定位与导航技术、环境感知技术、智能决策技术和高精度的运动控制技术。可以将驾驶员从繁重的驾驶任务中解脱出来，同时与车联网技术和前沿通信技术相结合，实现了车-车协同驾驶和车-路协同驾驶，利用前沿的通信技术和车辆决策技术，从根本上缓解交通拥堵难题。
智能汽车的实现需要多项关键技术的支撑，从智能车系统的工作流程入手可以将其关键技术概括为：高精度定位与导航技术、高实时环境感知与理解技术、智能化自主决策和优化技术、高可靠运动控制技术。其中，高精度定位与导航技术中要求智能汽车可以在多种复杂的道路环境中实现车辆的高精度定位和高精度运动轨迹估计。
目前，智能汽车的高精度定位与导航主要有如下几种方式：基于高精度地图匹配的车辆定位方式、基于组合导航与拓扑地图的车辆定位、基于UWB等局部定位方式的车辆高精度定位。上述这些定位方法都要求地图信息作为车辆定位的先验信息。而在智能汽车实际运行的过程中，因为道路维护、交通事故等原因，经常会遇到车辆行驶到高精度地图之外的区域。此时，上述方法就不能够稳定可靠地工作。针对这一现状，需对研究者纷纷对未知环境下的车辆自主定位与运动轨迹估计展开了研究。
发明内容
为了解决现有技术中车辆在高精度地图之外或未知环境下的运动轨迹估计的技术问题，本申请提供一种基于多线激光雷达的车辆运动轨迹估计方法及系统。
根据第一方面，本申请提供一种基于多线激光雷达的车辆运动轨迹估计方法，包括：
一种基于多线激光雷达的车辆运动轨迹估计方法，包括：
获取车辆的初始位姿和惯性传感器的测量值，根据所述初始位姿和测量值得到车辆的粗选运动轨迹；
获取多线激光雷达不同时刻的三维点云数据，分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图；
将所述粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对；根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系；
根据所述变换关系，得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。
其中，根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系的步骤，包括：
从所述特征匹配点对中任取三对不同线的特征匹配点对，通过该三对特征匹配点对计算出所述相邻时刻的三维点云数据之间的变换关系。
其中，所述分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图的步骤，包括：
建立目标栅格平面，分别将多线激光雷达不同时刻的三维点云数据投射到所述目标栅格平面上，标记出所述三维点云数据与每个栅格之间的映射关系，并根据所述映射关系将三维点云数据映射到所述灰度图中作为灰度图中的特征点。
其中，所述标记出所述三维点云数据与每个栅格之间的映射关系，并根据所述映射关系将三维点云数据映射到所述灰度图中作为灰度图中的特征点的步骤，包括：
在目标栅格平面上形成与三维点云数据对应的点阵；点阵中的点与三维点云数据中的点对应；计算目标栅格平面上每个栅格内所有点的灰度值的平均值，将有灰度的栅格作为特征点得到灰度图；其中，所述点的灰度值由三维点云数据的回波强度得到。
其中，其特征在于，所述在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对的步骤，包括：
在粗选范围内通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对，再采用随机抽样一致性算法从所述候选特征匹配点对中去除无效的特征匹配点对，得到所述特征匹配点对。
其中，所述通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对的步骤，包括：
对于任意相邻时刻的两帧灰度图，取其后一帧灰度图中的每一个特征点，计算该特征点与前一帧灰度图中每个特征点之间的欧氏距离，再计算出最小欧氏距离和次小欧氏距离的比值，如果所述比值小于预设的阈值，则认为最小欧式距离对应的这一对特征点为候选特征匹配点对。
一种基于多线激光雷达的车辆运动轨迹估计系统，包括：
粗选运动轨迹获取模块，用于获取车辆的初始位姿和惯性传感器的测量值，根据所述初始位姿和测量值得到车辆的粗选运动轨迹；
栅格化处理模块，用于获取多线激光雷达不同时刻的三维点云数据，分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图；
初始配准模块，用于将所述粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对；根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系；
精确运动轨迹获取模块，用于根据所述变换关系，得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。
其中，所述在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对，包括：
在粗选范围内通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对，再采用随机抽样一致性算法从所述候选特征匹配点对中去除无效的特征匹配点对，得到所述特征匹配点对。
其中，所述通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对的步骤，包括：
对于任意相邻时刻的两帧灰度图，取其后一帧灰度图中的每一个特征点，计算该特征点与前一帧灰度图中每个特征点之间的欧氏距离，再计算出最小欧氏距离和次小欧氏距离的比值，如果所述比值小于预设的阈值，则认为最小欧式距离对应的这一对特征点为候选特征匹配点对。
其中，根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系，包括：
从所述特征匹配点对中任取三对不同线的特征匹配点对，通过该三对特征匹配点对计算出所述相邻时刻的三维点云数据之间的变换关系。
本申请提供的基于多线激光雷达的车辆运动轨迹估计方法及系统，采用惯性传感器及车辆的初始值得到车辆的粗选运动轨迹，再通过多线激光雷达获取周围环境的三维点云数据，通过对三维点云数据进行特征匹配得到车辆的精确运动轨迹，该方法基于粗选的轨迹进行特定范围内的SURF匹配，这样就减少了SURF特征匹配的运算量和运算时间，同时该车辆轨迹估计方法无须依赖高精度地图、GPS和UWB等辅助定位技术，同时也无须在环境中设置明显的地标，环境适用性强且应用范围广。
附图说明
图1为本申请实施例1中车辆运动轨迹估计方法流程图；
图2为本申请实施例2中车辆运动轨迹估计系统结构框图；
图3为本申请实施例1中建立的车辆运动学模型示意图。
具体实施方式
下面通过具体实施方式结合附图对本发明作进一步详细说明。
实施例1
本实施例提供一种基于多线激光雷达的车辆运动轨迹估计方法，如图1，该方法包括：
步骤101，获取车辆的初始位姿和当前时刻的惯性传感器的测量值，根据初始位姿和测量值得到车辆的粗选运动轨迹；
步骤102，获取多线激光雷达不同时刻的三维点云数据，分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图；
步骤103，将粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对；根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系；
步骤104，根据所述变换关系，得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。
上述方法基于多线激光雷达的车辆运动轨迹估计方法，采用惯性传感器及车辆的初始值得到车辆的粗选运动轨迹，再通过多线激光雷达获取周围环境的三维点云数据，在粗选运动轨迹范围内通过对三维点云数据进行特征匹配得到车辆的精确运动轨迹。
其中，步骤101中首先建立车辆的运动学模型，如图3，即建立全局坐标系OwXwYwZw和局部坐标系OvXvYvZv，则车辆在全局坐标系某时刻的位姿表示为在局部坐标系下某时刻的位姿表示为则车辆在全局坐标系下的速度表示为和在当前局部坐标系下的速度表示为
在全局坐标系下车辆在三维平面运动的运动学方程与局部坐标下车辆在三维平面上的运动学方程之间的变换关系为：
其中，
其中，R1和R3旋转矩阵，s表示sin；c表示cos；
其中，采用惯性传感器可以实时测量出在局部坐标下，车辆沿相对坐标系的三个坐标轴的线加速度和沿三个坐标轴的角速度,通过上述的运动学方程变换关系可以得到在全局坐标系下车辆沿三个坐标轴的线加速度和沿三个坐标轴的角速度，在车辆初始速度值已知的情况下，再采用航迹推算的方法可以估算出车辆各个时刻的位移信息。其中在估算车辆的位移信息时，将上述的机器人运动模型离散化，设Δt为相邻两个采样时刻的时间差，车辆的运动加速度为在车辆运动模型的建立中采用恒加速模型，认为短时间内车辆平台的加速度是恒定的，建立的车辆横加速度模型为：
上式中vk和ak分别是车辆在第k采样时刻的速度与加速度，vk+1和ak+1分别为车辆在第k+1个采样时刻的速度与加速度，式中的I6表示任意同一个自由度，由于此处基于惯导的恒加速模型是对三维空间的运动进行估计，所以有六个自由度，I6表示的是6个自由度中的任意一个，在全局坐标系下通过上述恒加速度模型，对已知速度初始值的车辆进行速度的积分得到车辆的位移信息。同理对惯性传感器输出的沿三个轴的角速度进行积分运算可以估算出每个时刻的姿态角信息。
进一步的，将各个时刻的位移信息和姿态角信息融合，得到车辆不同时刻的位姿，将各个时刻的位姿连接得到车辆的粗选运动轨迹。
其中，在步骤102中，首先获取多线激光雷达不同时刻的三维点云数据，建立目标栅格平面，建立目标栅格平面时要求目标栅格平面与激光雷达的扫描方向垂直，在xoz平面建立一个目标栅格平面，横轴是X轴，其范围是0-(max(x)-min(z))，其中max(x),min(z)分别是点云Z轴的最大值和最小值，在完成了目标栅格平面的数据初始化之后，将车载多线激光雷达的点云数据投射到目标栅格平面上，在目标栅格平面上形成与三维点云数据对应的点阵；点阵中的点与三维点云数据中的点对应，计算目标栅格平面上每个栅格内所有点的灰度值的平均值。其中，本实施例中栅格尺寸为5cm*5cm，栅格的大小决定了灰度图的分辨率，当栅格的间距大于点云的间距时，一个栅格内就会出现多个点云，灰度图的分辨率就比较低。
在完成了三维点云数据的映射后，根据下式计算每一个栅格的灰度值得平均值：
其中，V(i,j)表示第(i,j)个栅格内的灰度值，其范围在0-255之间；I(i,j)表示第(i,j)个栅格内的回波强度信息；Imax和Imin分别表示点云数据中的最大回波强度和最小回波强度。其中，当栅格内没有点云数据映射时，该栅格灰度值为255，当栅格内有多个点云数据映射时，其灰度值为所有点云数据中回波强度最大的那个点云数据的灰度值。最后，将有灰度的栅格作为特征点得到灰度图，还灰度图中包含多个特征点。
其中，在步骤103中，将粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对，包括：步骤101中得到的粗选运动轨迹划定了车辆运动的粗选范围，在粗选范围内通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对，再采用随机抽样一致性算法从候选特征匹配点对中去除无效的特征匹配点对得到特征匹配点对，其中去掉无效的特征匹配点对的方法具体为，在进行特征数据配对的过程中，采用迭代的方法进行匹配，目标就是全局最优，保证最多的特征之间能够配对起来。当所有特征数据中的大多数数据都已经配对完成之后，剩余的点就认为是无效特征点对，需要剔除掉；得到的已经匹配成功的特征匹配点对。
其中，通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对的步骤，包括：对于任意相邻时刻的两帧灰度图，取其后一帧灰度图中的每一个特征点，计算该特征点与前一帧灰度图中每个特征点之间的欧氏距离，再计算出最小欧氏距离和次小欧氏距离的比值，如果比值小于预设的阈值，则认为最小欧式距离对应的这一对特征点为候选特征匹配点对。
其中，根据特征匹配点对在相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系的步骤，包括：从特征匹配点对中任取三对不同线的特征匹配点对，通过该三对特征匹配点对计算出相邻时刻的三维点云数据之间的变换关系。其计算方法为：为了方便描述，将相邻时刻的两个三维点云数据分别记为第一帧和第二帧，第一帧点云数据的特征点1坐标为(x1,y1,z1)，其对应的第二帧点云数据的特征点1坐标为则相应的变换公式为：
上式中，(l,m,n)表示第一帧和第二帧之间的平移关系，R表示第一帧和第二帧之间的旋转矩阵
上式中θ、β、γ分别表示该特征点与x轴、y轴、z轴之间的夹角。
在上式的基础上，将三对特征点对代入进行运算，就可以求解出平移位置(l,m,n)和旋转矩阵R，从而求出了相邻两帧点云数据之间的变换矩阵即变换关系，根据变换矩阵对车载激光雷达的三维点云数据进行初始变换，即完成了点云数据的初始配准。
根据配准的结果得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。其中，初始得到的粗选运动轨迹为车辆初始运动的粗选范围，在粗选范围通过SURF特征配准后得到精确运动轨迹。这样通过在粗选运动轨迹的基础上进行更加精细的匹配运算最后得到车辆的精确运动轨迹，有了粗选运动轨迹的粗选范围，在获取精确运动轨迹时，在粗选运动轨迹的粗选范围内进行配准，加快了运算速度和配准的准确性。
以上基于多线激光雷达的车辆运动轨迹估计方法及系统，采用惯性传感器及车辆的初始值得到车辆的粗选运动轨迹，再通过多线激光雷达获取周围环境的三维点云数据，通过对在粗选运动轨迹范围内三维点云数据进行特征匹配得到车辆的精确运动轨迹，，该车辆轨迹估计方法无须依赖高精度地图、GPS和UWB等辅助定位技术，同时也无须在环境中设置明显的地标，环境适用性强且应用范围广。
实施例2
本实施例提供一种基于多线激光雷达的车辆运动轨迹估计系统，该系统设置在车辆上或者其他车辆监控平台上用于实时获取车辆位置和运动轨迹，如图2，包括粗选运动轨迹获取模块201、栅格化处理模块202、初始配准模块203、精确运动轨迹获取模块204。
其中，粗选运动轨迹获取模块201用于获取车辆的初始位姿和惯性传感器的测量值，根据初始位姿和测量值得到车辆的粗选运动轨迹。
栅格化处理模块202用于获取多线激光雷达不同时刻的三维点云数据，分别对不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图。
初始配准模块203用于将粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对；根据特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系。
精确运动轨迹获取模块204用于根据所述变换关系，得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。
其中，粗选运动轨迹获取模块201与车辆上的速度传感器和惯性传感器通过信号线连接，用于获取车辆的初始位姿和惯性传感器的测量值，根据该初始位姿和当前时刻惯性传感器的测量值进行积分运算得到车辆的粗选运动轨迹，该粗选运动轨迹划定了车辆运动的粗选范围。
其中，惯性传感器的测量值包括在局部坐标系下车辆沿相对坐标系的三个坐标轴的线加速度和沿所述三个坐标轴的角速度。
其中，获取车辆的初始位姿和惯性传感器的测量值，得到车辆不同时刻的位姿，根据各个位姿得到车辆的粗选运动轨迹,包括:
根据车辆的初始位姿和各个时刻的所述线加速度估算车辆各个时刻的位移信息，根据各个时刻的角速度估算出各个时刻的姿态角信息；
将各个时刻的位移信息和姿态角信息融合，得到车辆不同时刻的位姿，根据各个位姿得到车辆的粗选运动轨迹，具体方法与实施例1中相同，此处不再赘述。
其中，栅格化处理模块202，分别对不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图，包括：首先建立目标栅格平面，分别将多线激光雷达不同时刻的三维点云数据投射到目标栅格平面上，标记出所述三维点云数据与每个栅格之间的映射关系，并根据所述映射关系将三维点云数据映射到灰度图中作为灰度图中的特征点。
其中，标记出三维点云数据与每个栅格之间的映射关系，并根据所述映射关系将三维点云数据映射到所述灰度图中作为灰度图中的特征点，包括：
在目标栅格平面上形成与三维点云数据对应的点阵；点阵中的点与三维点云数据中的点对应；计算目标栅格平面上每个栅格内所有点的灰度值的平均值，将有灰度的栅格作为特征点得到灰度图；其中，所述点的灰度值由三维点云数据的回波强度得到。此处计算每个栅格内每个点得灰度值得平均值的方法和实施例1中相同，此处不再赘述。
其中，初始配准模块203将粗选运动轨迹获取模块201得到的粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对，包括：
在粗选范围内通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对，再采用随机抽样一致性算法从候选特征匹配点对中去除无效的特征匹配点对，得到特征匹配点对，其中去掉无效的特征匹配点对的方法与实施例1中所述的相同，此处不再赘述。
进一步的，初始配准模块在通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对，包括：对于任意相邻时刻的两帧灰度图，取其后一帧灰度图中的每一个特征点，计算该特征点与前一帧灰度图中每个特征点之间的欧氏距离，再计算出最小欧氏距离和次小欧氏距离的比值，如果所述比值小于预设的阈值，则认为最小欧式距离对应的这一对特征点为候选特征匹配点对。
其中，初始配准模块203在根据特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系，包括：
从特征匹配点对中任取三对不同线的特征匹配点对，通过该三对特征匹配点对计算出所述相邻时刻的三维点云数据之间的变换关系。其中变换关系即相邻两帧点云数据之间的变换矩阵，该变换矩阵的计算方法与实施例1中相同，此处不再赘述。
其中，精确运动轨迹获取模块204根据变换矩阵进行配准的结果得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。其中配准方法与实施例1中相同，此处不再赘述。
上述基于多线激光雷达的车辆运动轨迹估计系统，粗选运动轨迹获取模块201获取车辆本身的速度传感器及惯性传感器的输出值估计出车辆的粗选运动轨迹，栅格化处理模块202获取多线激光雷达获取周围环境的三维点云数据进行栅格化处理后得到灰度图，初始配准模块203在粗选范围内根据得到的灰度图得到相邻时刻的三维点云数据之间的变换关系，精确运动轨迹获取模块204根据变换关系对三维点云数据进行特征匹配得到车辆的精确运动轨迹。该系统通过惯导获得了粗选的运动轨迹，然后在进行SURF特征匹配的时候，不再是原来的那种大范围迭代了，而是基于粗选的轨迹进行特定范围内的SURF匹配，这样就减少了SURF特征匹配的运算量和运算时间。SURF匹配完成之后就得到了精确的运动轨迹。在每一次运动轨迹估算中，都是先进行粗选轨迹，然后提取特征、特征配对得到最终的精准轨迹，然后再开始下一次循环。该车辆轨迹估计系统无须依赖高精度地图、GPS和UWB等辅助定位技术，同时也无须在环境中设置明显的地标，环境适用性强，可以广泛应用与多种环境下车辆的定位与估计估计。
以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。

Claims (10)

1.一种基于多线激光雷达的车辆运动轨迹估计方法，其特征在于，包括：
获取车辆的初始位姿和惯性传感器的测量值，根据所述初始位姿和测量值得到车辆的粗选运动轨迹；
获取多线激光雷达不同时刻的三维点云数据，分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图；
将所述粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对；根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系；
根据所述变换关系，得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。
2.如权利要求1所述的车辆运动轨迹估计方法，其特征在于，根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系的步骤，包括：
从所述特征匹配点对中任取三对不同线的特征匹配点对，通过该三对特征匹配点对计算出所述相邻时刻的三维点云数据之间的变换关系。
3.如权利要求1所述的车辆运动轨迹估计方法，其特征在于，所述分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图的步骤，包括：
建立目标栅格平面，分别将多线激光雷达不同时刻的三维点云数据投射到所述目标栅格平面上，标记出所述三维点云数据与每个栅格之间的映射关系，并根据所述映射关系将三维点云数据映射到所述灰度图中作为灰度图中的特征点。
4.如权利要求3所述的车辆运动轨迹估计方法，其特征在于，所述标记出所述三维点云数据与每个栅格之间的映射关系，并根据所述映射关系将三维点云数据映射到所述灰度图中作为灰度图中的特征点的步骤，包括：
在目标栅格平面上形成与三维点云数据对应的点阵；点阵中的点与三维点云数据中的点对应；计算目标栅格平面上每个栅格内所有点的灰度值的平均值，将有灰度的栅格作为特征点得到灰度图；其中，所述点的灰度值由三维点云数据的回波强度得到。
5.如权利要求1所述的车辆运动轨迹估计方法，其特征在于，所述在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对的步骤，包括：
在粗选范围内通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对，再采用随机抽样一致性算法从所述候选特征匹配点对中去除无效的特征匹配点对，得到所述特征匹配点对。
6.如权利要求5所述的车辆运动轨迹估计方法，其特征在于，所述通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对的步骤，包括：
对于任意相邻时刻的两帧灰度图，取其后一帧灰度图中的每一个特征点，计算该特征点与前一帧灰度图中每个特征点之间的欧氏距离，再计算出最小欧氏距离和次小欧氏距离的比值，如果所述比值小于预设的阈值，则认为最小欧式距离对应的这一对特征点为候选特征匹配点对。
7.一种基于多线激光雷达的车辆运动轨迹估计系统，其特征在于，包括：
粗选运动轨迹获取模块，用于获取车辆的初始位姿和惯性传感器的测量值，根据所述初始位姿和测量值得到车辆的粗选运动轨迹；
栅格化处理模块，用于获取多线激光雷达不同时刻的三维点云数据，分别对所述不同时刻的三维点云数据进行栅格化处理得到对应的包含有多个特征点的灰度图；
初始配准模块，用于将所述粗选运动轨迹作为粗选范围，在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对；根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系；
精确运动轨迹获取模块，用于根据所述变换关系，得到相邻时刻车辆的相对位姿，根据初始位姿和各个相邻时刻的相对位姿得到车辆的精确运动轨迹。
8.如权利要求7所述的运动轨迹估计系统，其特征在于，所述在粗选范围内对相邻时刻的两帧灰度图之间的特征点进行特征匹配，得到特征匹配点对，包括：
在粗选范围内通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对，再采用随机抽样一致性算法从所述候选特征匹配点对中去除无效的特征匹配点对，得到所述特征匹配点对。
9.如权利要求8所述的运动轨迹估计系统，其特征在于，所述通过SURF算法找到相邻时刻的两帧灰度图之间的候选特征匹配点对的步骤，包括：
对于任意相邻时刻的两帧灰度图，取其后一帧灰度图中的每一个特征点，计算该特征点与前一帧灰度图中每个特征点之间的欧氏距离，再计算出最小欧氏距离和次小欧氏距离的比值，如果所述比值小于预设的阈值，则认为最小欧式距离对应的这一对特征点为候选特征匹配点对。
10.如权利要求7所述的运动轨迹估计系统，其特征在于，根据所述特征匹配点对在所述相邻时刻的两帧灰度图中的位置，得到相邻时刻的三维点云数据之间的变换关系，包括：
从所述特征匹配点对中任取三对不同线的特征匹配点对，通过该三对特征匹配点对计算出所述相邻时刻的三维点云数据之间的变换关系。
CN201810852477.0A 2018-07-27 2018-07-27 一种基于多线激光雷达的车辆运动轨迹估计方法及系统 Active CN109166140B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810852477.0A CN109166140B (zh) 2018-07-27 2018-07-27 一种基于多线激光雷达的车辆运动轨迹估计方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810852477.0A CN109166140B (zh) 2018-07-27 2018-07-27 一种基于多线激光雷达的车辆运动轨迹估计方法及系统

Publications (2)

Publication Number Publication Date
CN109166140A true CN109166140A (zh) 2019-01-08
CN109166140B CN109166140B (zh) 2021-10-01

Family

ID=64898662

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810852477.0A Active CN109166140B (zh) 2018-07-27 2018-07-27 一种基于多线激光雷达的车辆运动轨迹估计方法及系统

Country Status (1)

Country Link
CN (1) CN109166140B (zh)

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110789528A (zh) * 2019-08-29 2020-02-14 腾讯科技（深圳）有限公司 一种车辆行驶轨迹预测方法、装置、设备及存储介质
CN111443337A (zh) * 2020-03-27 2020-07-24 北京航空航天大学 一种基于手眼标定的雷达-imu标定方法
CN111866305A (zh) * 2020-08-11 2020-10-30 普达迪泰（天津）智能装备科技有限公司 室内外特定条件下的图像增强与环境适应性方法
CN111971574A (zh) * 2019-01-30 2020-11-20 百度时代网络技术（北京）有限公司 用于自动驾驶车辆的lidar定位的基于深度学习的特征提取
CN112070800A (zh) * 2020-07-23 2020-12-11 武汉理工大学 一种基于三维点云极化地图表征的智能车定位方法及系统
CN112204344A (zh) * 2019-08-30 2021-01-08 深圳市大疆创新科技有限公司 位姿获取方法、系统和可移动平台
CN112445210A (zh) * 2019-08-15 2021-03-05 纳恩博（北京）科技有限公司 运动轨迹的确定方法及装置、存储介质、电子装置
CN112649815A (zh) * 2019-10-10 2021-04-13 华为技术有限公司 处理数据的方法和装置
CN112781586A (zh) * 2020-12-29 2021-05-11 上海商汤临港智能科技有限公司 一种位姿数据的确定方法、装置、电子设备及车辆
CN112837424A (zh) * 2021-02-04 2021-05-25 脸萌有限公司 图像处理方法、装置、设备和计算机可读存储介质
CN113711273A (zh) * 2019-04-25 2021-11-26 三菱电机株式会社 移动量估计装置、移动量估计方法及移动量估计程序
CN113760908A (zh) * 2021-08-23 2021-12-07 北京易航远智科技有限公司 基于时间序列的既定路径下不同地图间的位姿映射方法
CN114119885A (zh) * 2020-08-11 2022-03-01 中国电信股份有限公司 图像特征点匹配方法、装置及系统、地图构建方法及系统
CN117112867A (zh) * 2023-10-24 2023-11-24 四川国蓝中天环境科技集团有限公司 一种基于gps轨迹数据的重型车辆排放计算和可视化方法
CN117471513A (zh) * 2023-12-26 2024-01-30 合众新能源汽车股份有限公司 一种车辆定位方法、定位装置、电子设备及存储介质

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060238411A1 (en) * 2004-05-28 2006-10-26 Time Domain Corporation System and method for spatially diverse radar signal processing
CN103473774A (zh) * 2013-09-09 2013-12-25 长安大学 一种基于路面图像特征匹配的车辆定位方法
CN103499350A (zh) * 2013-09-28 2014-01-08 长安大学 Gps盲区下融合多源信息的车辆高精度定位方法及装置
CN104764457A (zh) * 2015-04-21 2015-07-08 北京理工大学 一种用于无人车的城市环境构图方法
CN106407315A (zh) * 2016-08-30 2017-02-15 长安大学 一种基于街景图像数据库的车辆自主定位方法
CN106541945A (zh) * 2016-11-15 2017-03-29 广州大学 一种基于icp算法的无人车自动泊车方法
CN106872963A (zh) * 2017-03-31 2017-06-20 厦门大学 一种多组多线激光雷达的自动标定算法
CN106932780A (zh) * 2017-03-14 2017-07-07 北京京东尚科信息技术有限公司 物体定位方法、装置和系统
CN107015238A (zh) * 2017-04-27 2017-08-04 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法
CN107590827A (zh) * 2017-09-15 2018-01-16 重庆邮电大学 一种基于Kinect的室内移动机器人视觉SLAM方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060238411A1 (en) * 2004-05-28 2006-10-26 Time Domain Corporation System and method for spatially diverse radar signal processing
CN103473774A (zh) * 2013-09-09 2013-12-25 长安大学 一种基于路面图像特征匹配的车辆定位方法
CN103499350A (zh) * 2013-09-28 2014-01-08 长安大学 Gps盲区下融合多源信息的车辆高精度定位方法及装置
CN104764457A (zh) * 2015-04-21 2015-07-08 北京理工大学 一种用于无人车的城市环境构图方法
CN106407315A (zh) * 2016-08-30 2017-02-15 长安大学 一种基于街景图像数据库的车辆自主定位方法
CN106541945A (zh) * 2016-11-15 2017-03-29 广州大学 一种基于icp算法的无人车自动泊车方法
CN106932780A (zh) * 2017-03-14 2017-07-07 北京京东尚科信息技术有限公司 物体定位方法、装置和系统
CN106872963A (zh) * 2017-03-31 2017-06-20 厦门大学 一种多组多线激光雷达的自动标定算法
CN107015238A (zh) * 2017-04-27 2017-08-04 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法
CN107590827A (zh) * 2017-09-15 2018-01-16 重庆邮电大学 一种基于Kinect的室内移动机器人视觉SLAM方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
ELLIOTTZHENG: "基于点云集合特征匹配的配准概述", 《博客园》 *
M.BUENO ET.AL: "Automatic point cloud coarse registration using geometric keypoint descriptors for indoor scenes", 《AUTOMATION IN CONSTRUCTION》 *
杨长强等: "车载激光点云数据的栅格化处理", 《测绘科学》 *

Cited By (27)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111971574A (zh) * 2019-01-30 2020-11-20 百度时代网络技术（北京）有限公司 用于自动驾驶车辆的lidar定位的基于深度学习的特征提取
CN111971574B (zh) * 2019-01-30 2022-07-22 百度时代网络技术（北京）有限公司 用于自动驾驶车辆的lidar定位的基于深度学习的特征提取
CN113711273A (zh) * 2019-04-25 2021-11-26 三菱电机株式会社 移动量估计装置、移动量估计方法及移动量估计程序
CN112445210B (zh) * 2019-08-15 2023-10-27 纳恩博（北京）科技有限公司 运动轨迹的确定方法及装置、存储介质、电子装置
CN112445210A (zh) * 2019-08-15 2021-03-05 纳恩博（北京）科技有限公司 运动轨迹的确定方法及装置、存储介质、电子装置
CN110789528B (zh) * 2019-08-29 2022-03-25 腾讯科技（深圳）有限公司 一种车辆行驶轨迹预测方法、装置、设备及存储介质
CN110789528A (zh) * 2019-08-29 2020-02-14 腾讯科技（深圳）有限公司 一种车辆行驶轨迹预测方法、装置、设备及存储介质
CN112204344A (zh) * 2019-08-30 2021-01-08 深圳市大疆创新科技有限公司 位姿获取方法、系统和可移动平台
WO2021035748A1 (zh) * 2019-08-30 2021-03-04 深圳市大疆创新科技有限公司 位姿获取方法、系统和可移动平台
CN112649815A (zh) * 2019-10-10 2021-04-13 华为技术有限公司 处理数据的方法和装置
CN111443337B (zh) * 2020-03-27 2022-03-08 北京航空航天大学 一种基于手眼标定的雷达-imu标定方法
CN111443337A (zh) * 2020-03-27 2020-07-24 北京航空航天大学 一种基于手眼标定的雷达-imu标定方法
CN112070800B (zh) * 2020-07-23 2024-03-26 武汉理工大学 一种基于三维点云极化地图表征的智能车定位方法及系统
CN112070800A (zh) * 2020-07-23 2020-12-11 武汉理工大学 一种基于三维点云极化地图表征的智能车定位方法及系统
CN111866305A (zh) * 2020-08-11 2020-10-30 普达迪泰（天津）智能装备科技有限公司 室内外特定条件下的图像增强与环境适应性方法
CN114119885A (zh) * 2020-08-11 2022-03-01 中国电信股份有限公司 图像特征点匹配方法、装置及系统、地图构建方法及系统
CN112781586B (zh) * 2020-12-29 2022-11-04 上海商汤临港智能科技有限公司 一种位姿数据的确定方法、装置、电子设备及车辆
WO2022142185A1 (zh) * 2020-12-29 2022-07-07 上海商汤临港智能科技有限公司 位姿数据的确定方法、装置、电子设备及车辆
CN112781586A (zh) * 2020-12-29 2021-05-11 上海商汤临港智能科技有限公司 一种位姿数据的确定方法、装置、电子设备及车辆
CN112837424A (zh) * 2021-02-04 2021-05-25 脸萌有限公司 图像处理方法、装置、设备和计算机可读存储介质
CN112837424B (zh) * 2021-02-04 2024-02-06 脸萌有限公司 图像处理方法、装置、设备和计算机可读存储介质
CN113760908A (zh) * 2021-08-23 2021-12-07 北京易航远智科技有限公司 基于时间序列的既定路径下不同地图间的位姿映射方法
CN113760908B (zh) * 2021-08-23 2023-07-07 北京易航远智科技有限公司 基于时间序列的既定路径下不同地图间的位姿映射方法
CN117112867A (zh) * 2023-10-24 2023-11-24 四川国蓝中天环境科技集团有限公司 一种基于gps轨迹数据的重型车辆排放计算和可视化方法
CN117112867B (zh) * 2023-10-24 2024-01-09 四川国蓝中天环境科技集团有限公司 一种基于gps轨迹数据的重型车辆排放计算和可视化方法
CN117471513A (zh) * 2023-12-26 2024-01-30 合众新能源汽车股份有限公司 一种车辆定位方法、定位装置、电子设备及存储介质
CN117471513B (zh) * 2023-12-26 2024-03-15 合众新能源汽车股份有限公司 一种车辆定位方法、定位装置、电子设备及存储介质

Also Published As

Publication number Publication date
CN109166140B (zh) 2021-10-01

Similar Documents

Publication Publication Date Title
CN109166140A (zh) 一种基于多线激光雷达的车辆运动轨迹估计方法及系统
CN110008851B (zh) 一种车道线检测的方法及设备
Alonso et al. Accurate global localization using visual odometry and digital maps on urban environments
Ruchti et al. Localization on openstreetmap data using a 3d laser scanner
US11199850B2 (en) Estimation device, control method, program and storage medium
CN110361027A (zh) 基于单线激光雷达与双目相机数据融合的机器人路径规划方法
CN106441275A (zh) 一种机器人规划路径的更新方法及装置
CN105953796A (zh) 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN106199626A (zh) 基于摆动激光雷达的室内三维点云地图生成系统及方法
EP3686775B1 (en) Method for detecting pseudo-3d bounding box based on cnn capable of converting modes according to poses of objects using instance segmentation
CN103413352A (zh) 基于rgbd多传感器融合的场景三维重建方法
CN110146910A (zh) 一种基于gps与激光雷达数据融合的定位方法及装置
CN104833354A (zh) 一种多基多模组网融合室内人员导航定位系统及其实施方法
CN110243380A (zh) 一种基于多传感器数据与角度特征识别的地图匹配方法
CN112965063B (zh) 一种机器人建图定位方法
CN113189613B (zh) 一种基于粒子滤波的机器人定位方法
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN105333869A (zh) 一种基于自适应ekf的无人侦察机同步定位与构图方法
CN108387236A (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN112833892B (zh) 一种基于轨迹对齐的语义建图方法
CN111060099A (zh) 一种无人驾驶汽车实时定位方法
JP2023164553A (ja) 位置推定装置、推定装置、制御方法、プログラム及び記憶媒体
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN116429116A (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
TR01 Transfer of patent right

Effective date of registration: 20240207

Address after: 710200 No. 1, Shaanqi Avenue, Jingwei Industrial Park, economic development zone, Xi'an City, Shaanxi Province

Patentee after: SHAANXI HEAVY DUTY AUTOMOBILE Co.,Ltd.

Country or region after: China

Address before: 710000 central section of south two ring road, Yanta District, Xi'an, Shaanxi

Patentee before: CHANG'AN University

Country or region before: China

TR01 Transfer of patent right