CN111707272B - 一种地下车库自动驾驶激光定位系统 - Google Patents
一种地下车库自动驾驶激光定位系统 Download PDFInfo
- Publication number
- CN111707272B CN111707272B CN202010594763.9A CN202010594763A CN111707272B CN 111707272 B CN111707272 B CN 111707272B CN 202010594763 A CN202010594763 A CN 202010594763A CN 111707272 B CN111707272 B CN 111707272B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- module
- laser
- point cloud
- matching
- 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
Links
Images
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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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/89—Lidar systems specially adapted for specific applications for mapping or imaging
- G01S17/894—3D imaging with simultaneous measurement of time-of-flight at a 2D array of receiver pixels, e.g. time-of-flight cameras or flash lidar
-
- 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)
- Traffic Control Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种地下车库自动驾驶激光定位系统,包括:输入模块,该输入模块包括激光雷达、轮速传感器和方向盘转角传感器;计算模块,耦接于输入模块,包括车辆运动学模块、激光里程计模块、激光回环检测模块和联合优化模块;输出模块,耦接于计算模块,用于输出准确的自动驾驶车辆位姿信息并将位姿传递给计算模块用于下一次车辆位姿计算。本发明的地下车库自动驾驶激光定位系统,通过输入模块、计算模块和输出模块的配合设置,便可有效的实现输入车辆状态信息,然后进行计算输出的效果,如此有效的实现了激光定位。
Description
技术领域
本发明涉及一种自动驾驶车辆定位系统,特别是关于一种地下车库自动驾驶激光定位系统。
背景技术
近年来,随着人工智能技术的兴起,自动驾驶车辆作为人工智能技术的重要算法验证平台应运而生,代表高新科技水平,同时满足人们对于汽车技术发展的迫切需求。车辆定位技术在自动驾驶领域具有关键作用,其涉及环境感知、路径规划和决策控制功能的准确实现。
目前,自动驾驶车辆常用定位技术包括GNSS定位、航迹推算和SLAM算法。GNSS定位精度较高,但容易受到使用环境的遮蔽影响而失效,而地下车库属于室内封闭空间,因此,GNSS定位技术无法为地下车辆提供位置信息。航迹推算定位算法在短时间内能提供较高精度的车辆定位信息,但是其误差会随时间不断累积,不适合长时间的单独定位。视觉SLAM算法并不适用于光线不足的地下车库环境。激光SLAM算法直接估计激光雷达的无约束六自由度运动,并未考虑激光雷达会受安装平台的约束影响,这可能会导致估计的车辆位姿与实际运动不符。自动驾驶车辆在地下车库的平面运动受三个自由度约束。因此,利用激光SLAM算法实现地下车库自动驾驶车辆的准确定位需要增加额外约束条件。本专利利用基于平面运动假设的车辆运动学模型为激光SLAM算法提供约束条件,提高自动驾驶激光定位算法收敛速度并降低车辆位姿估计陷入局部最优的概率。
发明内容
针对现有技术存在的不足,本发明的目的在于提供一种地下车库自动驾驶激光定位方法,通过紧耦合方式完成车辆运动学模型和激光SLAM算法的数据融合,实现地下车库自动驾驶车辆准确定位,从而确保自动驾驶车辆顺利出入库以及安全、稳定行驶。
为实现上述目的,本发明提供了如下技术方案:一种地下车库自动驾驶激光定位系统,包括:
输入模块,该输入模块包括激光雷达、轮速传感器和方向盘转角传感器,所述激光雷达用于提供点云匹配所需的特征点云,轮速传感器用于提供车辆速度信息,方向盘转角传感器用于提供角速度计算所需的方向盘转角;
计算模块,耦接于输入模块,包括车辆运动学模块、激光里程计模块、激光回环检测模块和联合优化模块,所述车辆运动学模型预测车辆运动状态并构建用于联合优化的车辆运动学模型约束,激光里程计模块利用局部曲率提取特征点云并进行帧与局部地图匹配,实现激光里程计的残差构建,激光回环检测模块基于局部曲率构建全局描述符,利用描述符匹配提取回环帧并进行匹配,为后续位姿优化提供激光回环残差,联合优化模块利用梯度下降法共同优化车辆运动学模型、激光里程计和激光回环三者提供的运动约束;
输出模块,耦接于计算模块,用于输出准确的自动驾驶车辆位姿信息并将位姿传递给计算模块用于下一次车辆位姿计算。
作为本发明的进一步改进,所述车辆运动学模块包括:
车辆状态预测模块,用于在未获取新一帧激光点云数据前,基于轮速传感器和方向盘转角传感器的数据,通过车辆运动学模型预测车辆运动状态;
模型约束构建模块,基于车辆预测状态构建车辆运动学模型约束,利用车辆运动学模型限制车辆位姿的优化方向。
作为本发明的进一步改进,所述车辆状态预测模块的预测步骤如下:
步骤1,通过轮速传感器获取车辆纵向速度v,方向盘转角传感器获取方向盘转角δs;
步骤2,根据速度v、方向盘转角δs、转向器角传动比K以及轴距h计算车辆横摆角速度ω:
步骤3,基于上一时刻已优化的车辆状态,利用车辆运动学方程积分{i,…,j}时间段内的状态量,获得相对于上一时刻车辆坐标系自动驾驶车辆相对运动状态:
作为本发明的进一步改进,所述模型约束构建模块利用车辆运动学模型提供预测值,通过约束同一时刻系统状态量构建车辆运动学模型约束:
作为本发明的进一步改进,所述激光里程计模块包括:
点云畸变矫正模块,接收最新时刻激光点云数据,根据车辆预测状态矫正点云的运动畸变;
点云特征提取模块,基于密度自适应策略实现局部曲率计算,克服基于固定邻域特征提取算法的局限性,提取边缘点和平面点特征用于点云匹配;
局部地图更新模块,基于上一时刻已优化的车辆位姿,更新定尺寸局部点云地图;
帧与地图匹配模块,基于车辆位姿初始估计,利用帧与局部地图匹配算法构建用于联合优化的激光里程计残差。
作为本发明的进一步改进,所述激光回环检测模块包括:
描述符构建模块,利用局部曲率构建全局描述符;
相似性计算模块,利用卡方检验计算相似性;
特征点验证模块,利用特征点匹配数量验证回环的正确性;
回环帧匹配模块,利用回环帧对应位姿构建局部地图,进行当前帧与局部地图的匹配。
本发明的有益效果,1)本发明提出一种地下车库自动驾驶激光定位方法,通过实现车辆运动学模型和激光SLAM算法的数据融合,可充分发挥各传感器的数据优势,提高地下车库环境下自动驾驶车辆定位算法的精度和鲁棒性。2)本发明基于车辆运动学模型的状态预测,利用线性插值方法矫正点云运动畸变,无畸变点云数据有助于实现准确数据关联。3)本发明基于密度自适应策略实现局部曲率计算,通过量化点云密度对曲率计算的影响,提高边缘点和平面点检测精度和鲁棒性。4)本发明通过车辆运动学模型状态预测提供车辆位姿初始估计,避免点云匹配算法陷入局部极值,提高激光里程计和激光回环检测的效率与准确性。5)本发明基于局部曲率直方图的识别能保证较高精度回环检测,同时,局部曲率的利用极大程度降低计算量,局部曲率的一物两用提高特征利用效率。因此,较小计算量能保证实时重定位的实现。6)本发明利用车辆运动学模型为地下车库自动驾驶车辆位姿优化提供平面运动约束,符合地下车库大多为平面的地形特点,可在梯度下降的优化过程中引导梯度方向,缩小优化空间,实现激光定位算法收敛速度和准确性的提高。7)本发明使用地下车库的地形大多为平面,故在选择车辆状态量时使用3自由度平面运动状态量,即车辆的平移(2自由度)和旋转(1自由度)。该方法可带来3方面优势:1)减小算法复杂度,利于工程实践。2)减小计算量,利于嵌入式实现。3)减小后续位姿优化过程中的搜索空间,利于提升精度和鲁棒性。8)本发明实现自车传感器共用,无需增加昂贵的额外传感器,实现成本节约和复杂度降低的同时提升定位系统可靠性,从而使得定位算法易于工程实践的同时更利于通过车规级测试,9)采用紧耦合方式实现车辆运动学模型和激光SLAM算法的数据融合,可充分发挥各传感器的数据优势,提升定位精度和鲁棒性,10)共用原车传感器,无需增加额外传感器,实现成本节约和复杂度降低的同时提升定位系统可靠性;使用少量传感器完成数据融合,无需使用IMU、GNSS等复杂、昂贵传感器,易于工程实践的同时更利于通过车规级测试。
附图说明
图1是本发明地下车库自动驾驶激光定位系统架构图;
图2是本发明地下车库自动驾驶激光定位算法架构图;
图3是本发明车辆运动学模型示意图。
具体实施方式
下面将结合附图所给出的实施例对本发明做进一步的详述。
如图1所示,地下车库自动驾驶激光定位系统架构图包含三个模块:输入模块、计算模块和输出模块。
1、输入模块包含感知环境和车辆状态的主要传感器:激光雷达、轮速传感器和方向盘转角传感器。1)激光雷达用于提供点云匹配所需的特征点云。2)轮速传感器用于提供车辆速度信息。3)方向盘转角传感器用于提供角速度计算所需的方向盘转角。
2、计算模块主要完成四个方面的任务:车辆运动学模型、激光里程计、激光回环检测和联合优化。1)车辆运动学模型预测车辆运动状态并构建用于联合优化的车辆运动学模型约束。2)激光里程计利用局部曲率提取特征点云并进行帧与局部地图匹配,实现激光里程计的残差构建。3)激光回环检测基于局部曲率构建全局描述符,利用描述符匹配提取回环帧并进行匹配,为后续位姿优化提供激光回环残差。4)联合优化利用梯度下降法共同优化车辆运动学模型、激光里程计和激光回环三者提供的运动约束。
3、输出模块用于输出准确的自动驾驶车辆位姿信息并将位姿传递给计算模块用于下一次车辆位姿计算。
如图2所示,地下车库自动驾驶激光定位算法架构图包含四个模块:车辆运动学模块、激光里程计模块、激光回环检测模块和联合优化模块。
基于上一时刻车辆优化位姿,利用激光点云数据和两帧点云对应时刻间的轮速传感器和方向盘转角传感器数据优化车辆位姿。本专利面向结构化地下停车场,其地形大多为平面,在更新车辆状态量时使用3自由度平面运动状态量,即车辆的平移(2自由度)和旋转(1自由度)。在j时刻,待优化系统状态量定义如下:
1、车辆运动学模块
车辆运动学模块包含两个部分:车辆状态预测和模型约束构建。这两个部分分别完成:1)未获取新一帧激光点云数据前,基于轮速传感器和方向盘转角传感器的数据,通过车辆运动学模型预测车辆运动状态。2)基于车辆预测状态构建车辆运动学模型约束,利用车辆运动学模型限制车辆位姿的优化方向,以期提升车辆位姿估计精度。
(1)车辆状态预测
车辆运动学模型包含两个输入:1)轮速传感器直接提供车辆纵向速度v。2)方向盘转角传感器提供的方向盘转角δs。
车辆横摆角速度ω由速度v、方向盘转角δs、转向器角传动比K以及轴距h共同决定,即:
车辆运动学模块利用车辆底层数据进行车辆状态预测,该预测仅考虑自动驾驶车辆的平面运动。基于上一时刻(i时刻)已优化的车辆状态,利用车辆运动学方程积分{i,…,j}时间段内的状态量,获得相对于上一时刻车辆坐标系自动驾驶车辆相对运动状态:
其中,由车辆模型预测的车辆位置表示为二维平移向量的增广形式为 自动驾驶车辆的垂直方向位移始终为零,即,表示仅考虑水平面上自动驾驶车辆运动。表示上一时刻已优化车辆坐标系到世界坐标系的旋转变换,其维度为2×2。表示两时刻间车辆坐标系的旋转变换。平面上的旋转矩阵计算公式为:
基于车辆运动学模型的状态预测为后续车辆位姿优化提供模型约束,同时兼作优化求解问题中的初值。
(2)模型约束构建
利用车辆运动学模型提供预测值,通过约束同一时刻系统状态量构建车辆运动学模型约束:
通过车辆运动学模型约束对系统状态量求偏导构建雅克比矩阵Jb:
其中,φ~表示增广旋转矩阵对应的李代数,旋转矩阵和李代数间的关系表达如下:
R=exp(φ~∧)
推导可得:
其中,θa表示增广旋转矩阵R~对应的旋转向量,θ表示旋转角度,a表示旋转轴。∧为反对称符号,∨为解反对称符号。
2、激光里程计模块
激光里程计模块包含四个部分:点云畸变矫正、点云特征提取、局部地图更新和帧与地图匹配。这四个部分分别完成:1)接收最新时刻激光点云数据,根据车辆预测状态矫正点云的运动畸变。2)基于密度自适应策略实现局部曲率计算,提取边缘点和平面点特征用于点云匹配。3)基于上一时刻已优化的车辆位姿,更新定尺寸局部点云地图。4)基于车辆位姿初始估计,利用帧与局部地图匹配算法构建用于联合优化的激光里程计残差。
(1)点云畸变矫正
为保证点云匹配准确性需矫正点云畸变。基于匀速运动假设,通过线性插值操作实现激光点云的运动畸变矫正。去畸变点云用于后续局部曲率计算和特征提取。
(2)点云特征提取
利用去畸变点云的角度信息进行点云线束划分。考虑密度大小对特征提取的影响,基于密度自适应策略计算局部曲率。对于每条扫描线上点云的局部曲率计算,如下所示:
其中,a=0.1,b=0.06。在扫描线上寻找满足距离阈值dj的邻域点组成集合Sj。
本实施例中的局部曲率阈值定为0.1。排序点云曲率值并通过曲率值和邻域点分布提取两类特征点:1)边缘点:曲率值大于阈值的同时邻域点没有发生突变。2)平面点:曲率值小于阈值的同时邻域点没有发生突变。
为实现特征点的均匀分布,将每条线束划分为6个独立区域。每个区域最多提供15个边缘点和30个平面点,构成边缘点集合和平面点集合在选择特征点时希望避免选取以下几类点:1)可能被遮挡的点。2)已经选过点周围的点。3)位于激光线接近平行的平面点。
(3)局部地图更新
为兼顾计算效率和定位精度,本专利使用定尺寸局部地图,即,在算法中保持地图尺寸为500×500×150m。局部地图为栅格化地图且随车辆运动不断更新。为保证地图的尺寸以及点云匹配的准确性,算法将不断删除位于地图边缘的特征点云,并利用上一时刻车辆优化位姿投影上一帧特征点云(边缘点和平面点)到局部地图。为保证特征点云规模以及匹配搜索效率,在更新局部地图时进行必要的点云降采样操作。
(4)帧与地图匹配
基于已更新局部地图和车辆位姿初始估计,采用帧与局部地图匹配构建激光里程计残差。当前时刻特征点云集合和中的点云根据相对位姿关系实现点云投影,即,特征点云转换到世界坐标系。基于局部地图中的特征点云,构建KD树并寻找边缘点对应的特征线和平面点对应的特征面,即,1)点线ICP:使用kd树算法快速寻找每个边缘点的两个最近点,利用最近点构造直线并计算点到直线的垂足坐标Lw。2)点面ICP:使用kd树算法快速寻找每个平面点的三个最近点,利用最近点构造平面并计算点到平面的垂足坐标Lw。
对于j时刻激光点云中的特征点,将其投影到世界坐标系下的值为:
其中,l表示激光坐标系,表示j时刻激光测量点在激光坐标系下的三维坐标。表示车辆坐标系到世界坐标系的位姿变换矩阵。Tbl表示激光坐标系到车辆坐标系的位姿变换矩阵,该变换矩阵根据激光雷达安装位置和车辆后轴中心位置测量可得。变换矩阵T是由旋转矩阵和平移向量组成,即:
因此,三维坐标形式可转换为:
通过约束同一时刻激光雷达的测量值构建激光里程计残差。利用点到直线和点到平面的距离表示激光里程计残差rl:
rl=L′w-Lw
其中,L′w表示将j时刻激光测量点转换到世界坐标系下的投影点。Lw表示投影点在世界坐标系中的对应点。
通过激光里程计残差对系统状态量求偏导构建雅克比矩阵Jl:
推导可得:
3、激光回环检测模块
激光回环检测模块包含四个部分:描述符构建、相似性计算、特征点验证和回环帧匹配。这四个部分分别完成:1)利用局部曲率构建全局描述符。2)利用卡方检验计算相似性。3)利用特征点匹配数量验证回环的正确性。4)利用回环帧对应位姿构建局部地图,进行当前帧与局部地图的匹配。
(1)描述符构建
使用主成分分析法进行坐标轴划分。获得参考框架后,对齐边界和坐标轴,从而获取对应的划分坐标轴。基于局部曲率的全局描述符由m个不互相重叠区域组成,即,以激光雷达为中心,定义外球半径和内球半径,球体划分为上下两部分,每个半球体均分为四个区域,由此,m=16。每个区域进行相应直方图描述,即,落入该区域的k个激光点对应局部曲率值被计算成直方图,该直方图具有n个划分区域。
(2)相似性计算
以车辆位姿初始估计作为中心点,设定候选区域尺寸为50×50×15m。在满足时间间隔条件下,利用描述符匹配寻找满足相似度阈值且相似度最低的激光帧。利用卡方检验定义描述符A第i个区域与描述符B第i个区域的相似性度量,即:
利用区域相似性计算两个描述符的相似性,如下所示:
利用主成分分析法确定坐标轴存在歧义,为消除该过程造成的干扰,需通过分类讨论,实现坐标轴正确对齐。根据x轴和y轴的划分,共有4种情况。使用上式计算A和B的相似性时,出现四个不同的值,取最小值作为描述符匹配结果。即:
SAB=min{SAB1,SAB2,SAB3,SAB4}
(3)特征点验证
选择相似度最低且满足阈值的激光帧作为回环候选帧。为保证回环检测准确性,需利用特征点进行进一步验证。基于回环候选帧的对应车辆位姿,利用k近邻算法寻找k个历史帧,将历史帧对应特征点云投影到回环候选帧所对应激光雷达的坐标系,实现局部地图构建。基于车辆位姿初始估计,投影当前帧中的特征点云到回环候选帧。利用k近邻算法寻找边缘点对应直线和平面点对应平面,计算对应的坐标计算点到线以及点到面的距离。满足最小距离阈值的点云为匹配点,统计匹配点数量并计算匹配点与特征点数量的比值。若比值满足设定阈值,则认为回环候选帧为正确回环。否则,认为此次回环检测不正确,不进行激光回环的残差构建。
(4)回环帧匹配
对于j时刻激光点云中的特征点,将其投影到o时刻(回环帧对应时刻)激光坐标系下的值为:
三维坐标形式为:
通过约束同一时刻激光雷达的测量值构建激光回环残差。利用点到直线和点到平面的距离表示激光回环残差ro:
通过激光回环残差对系统状态量求偏导构建雅克比矩阵Jo:
推导可得:
4、位姿联合优化模块
位姿联合优化模块根据车辆运动学模型约束、激光里程计残差和激光回环残差构建系统代价函数,利用梯度下降法进行联合非线性优化。在梯度下降法执行过程中需要用到代价函数的雅克比矩阵,在前述内容中已经进行相关推导和描述,此处不再赘述。自动驾驶车辆的联合优化位姿,即车辆准确位姿,用于下一时刻局部地图更新和车辆状态预测。
通过计算系统代价函数最小值获取待优化系统状态量X的最大后验估计。地下车库自动驾驶激光定位系统代价函数构建如下:
其中,rb(z,X)表示车辆运动学模型约束,z表示轮速传感器和方向盘转角的测量数据。rl(c,X)表示激光里程计残差,c表示通过帧与局部地图匹配确定的特征点云对应关系。ro(e,X)表示激光回环残差,e表示通过帧与局部地图匹配确定的特征点云对应关系。三种残差均用马氏距离表示。协方差矩阵由传感器精度决定。利用Ceres Solver实现代价函数求解。
图3为车辆运动学模型示意图。车辆运动学模型简化为两自由度自行车模型,前后轮均用单轮替代。以车辆后轴中心O为原点建立车辆坐标系,沿车辆前进方向为X轴方向,垂直X轴指向车体左侧为Y轴方向。表示相邻时刻间车辆横摆角增量,h表示轴距,δf为前轮转角。为保证安全,自动驾驶系统通常不会进入极限工况,因此质心侧偏角较小,可忽略不计。通常情况下,汽车后轮不可控,故可认为自行车模型中后轮转角控制输入δr=0。利用方向盘转角δs和转向器角传动比K可获得前轮转角δf,即:
车辆运动学模型建立原则是在保证模型简单的同时应尽可能反映车辆真实运动特性。过于严格的车辆运动学模型不利于理论推导和求解。针对地下车库工况,自行车模型采取以下假设:1)不考虑车辆在Z轴方向的运动,只考虑XY水平面的运动。2)左右侧车轮转角一致,将左右侧轮胎合并为一个轮胎。3)车辆的转向只由前轮控制。
车辆运动学模型有两个输入:轮速传感器提供的车辆速度v和方向盘转角传感器提供的前轮转角δf。以前一时刻车辆坐标系作为参考坐标系,当前时刻车辆运动学模型表达式如下:
其中,vx和vy分别表示自动驾驶车辆在参考坐标系下X轴方向的速度、Y轴方向的速度。
以上所述仅是本发明的优选实施方式,本发明的保护范围并不仅局限于上述实施例,凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理前提下的若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。
Claims (4)
1.一种地下车库自动驾驶激光定位系统,其特征在于:包括:
输入模块,该输入模块包括激光雷达、轮速传感器和方向盘转角传感器,所述激光雷达用于提供点云匹配所需的特征点云,轮速传感器用于提供车辆速度信息,方向盘转角传感器用于提供角速度计算所需的方向盘转角;
计算模块,耦接于输入模块,包括车辆运动学模块、激光里程计模块、激光回环检测模块和联合优化模块,所述车辆运动学模块预测车辆运动状态并构建用于联合优化的车辆运动学模型约束,激光里程计模块利用局部曲率提取特征点云并进行帧与局部地图匹配,实现激光里程计的残差构建,激光回环检测模块基于局部曲率构建全局描述符,利用描述符匹配提取回环帧并进行匹配,为后续位姿优化提供激光回环残差,联合优化模块利用梯度下降法共同优化车辆运动学模型、激光里程计和激光回环三者提供的运动约束;
输出模块,耦接于计算模块,用于输出准确的自动驾驶车辆位姿信息并将位姿传递给计算模块用于下一次车辆位姿计算;所述车辆运动学模块包括:
车辆状态预测模块,用于在未获取新一帧激光点云数据前,基于轮速传感器和方向盘转角传感器的数据,通过车辆运动学模型预测车辆运动状态;
模型约束构建模块,基于车辆预测状态构建车辆运动学模型约束,利用车辆运动学模型限制车辆位姿的优化方向;所述车辆状态预测模块的预测步骤如下:
步骤1,通过轮速传感器获取车辆纵向速度v,方向盘转角传感器获取方向盘转角δs;
步骤2,根据速度v、方向盘转角δs、转向器角传动比K以及轴距h计算车辆横摆角速度ω:
步骤3,基于上一时刻已优化的车辆状态,利用车辆运动学方程积分{i,…,j}时间段内的状态量,获得相对于上一时刻车辆坐标系自动驾驶车辆相对运动状态:
3.根据权利要求1或2所述的地下车库自动驾驶激光定位系统,其特征在于:所述激光里程计模块包括:
点云畸变矫正模块,接收最新时刻激光点云数据,根据车辆预测状态矫正点云的运动畸变;
点云特征提取模块,基于密度自适应策略实现局部曲率计算,克服基于固定邻域特征提取算法的局限性,提取边缘点和平面点特征用于点云匹配;
局部地图更新模块,基于上一时刻已优化的车辆位姿,更新定尺寸局部点云地图;
帧与地图匹配模块,基于车辆位姿初始估计,利用帧与局部地图匹配算法构建用于联合优化的激光里程计残差。
4.根据权利要求1或2所述的地下车库自动驾驶激光定位系统,其特征在于:所述激光回环检测模块包括:
描述符构建模块,利用局部曲率构建全局描述符;
相似性计算模块,利用卡方检验计算相似性;
特征点验证模块,利用特征点匹配数量验证回环的正确性;
回环帧匹配模块,利用回环帧对应位姿构建局部地图,进行当前帧与局部地图的匹配。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010594763.9A CN111707272B (zh) | 2020-06-28 | 2020-06-28 | 一种地下车库自动驾驶激光定位系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010594763.9A CN111707272B (zh) | 2020-06-28 | 2020-06-28 | 一种地下车库自动驾驶激光定位系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111707272A CN111707272A (zh) | 2020-09-25 |
CN111707272B true CN111707272B (zh) | 2022-10-14 |
Family
ID=72542782
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010594763.9A Active CN111707272B (zh) | 2020-06-28 | 2020-06-28 | 一种地下车库自动驾驶激光定位系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111707272B (zh) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP4258078A4 (en) * | 2021-01-13 | 2023-12-27 | Huawei Technologies Co., Ltd. | POSITIONING METHOD AND DEVICE AND VEHICLE |
CN112907491B (zh) * | 2021-03-18 | 2023-08-22 | 中煤科工集团上海有限公司 | 一种适用于井下巷道的激光点云回环检测方法及系统 |
CN113447949B (zh) * | 2021-06-11 | 2022-12-09 | 天津大学 | 一种基于激光雷达和先验地图的实时定位系统及方法 |
CN113740875A (zh) * | 2021-08-03 | 2021-12-03 | 上海大学 | 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法 |
CN113639782A (zh) * | 2021-08-13 | 2021-11-12 | 北京地平线信息技术有限公司 | 车载传感器的外参标定方法和装置、设备和介质 |
CN114018284B (zh) * | 2021-10-13 | 2024-01-23 | 上海师范大学 | 一种基于视觉的轮速里程计校正方法 |
CN113870316B (zh) * | 2021-10-19 | 2023-08-15 | 青岛德智汽车科技有限公司 | 一种无gps跟车场景下前车路径重建方法 |
CN114353799B (zh) * | 2021-12-30 | 2023-09-05 | 武汉大学 | 搭载多线激光雷达的无人驾驶平台室内快速全局定位方法 |
CN114820749A (zh) * | 2022-04-27 | 2022-07-29 | 西安优迈智慧矿山研究院有限公司 | 无人车井下定位方法、系统、设备及介质 |
CN115655302B (zh) * | 2022-12-08 | 2023-03-21 | 安徽蔚来智驾科技有限公司 | 激光里程计的实现方法、计算机设备、存储介质及车辆 |
CN117584989A (zh) * | 2023-11-23 | 2024-02-23 | 昆明理工大学 | 一种激光雷达/imu/车辆运动学约束紧耦合slam系统及算法 |
CN118031983B (zh) * | 2024-04-11 | 2024-06-25 | 江苏集萃清联智控科技有限公司 | 自动驾驶融合定位方法及系统 |
Family Cites Families (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB2270438B (en) * | 1992-09-08 | 1996-06-26 | Caterpillar Inc | Apparatus and method for determining the location of a vehicle |
US7298319B2 (en) * | 2004-04-19 | 2007-11-20 | Magellan Navigation, Inc. | Automatic decorrelation and parameter tuning real-time kinematic method and apparatus |
CN106153048A (zh) * | 2016-08-11 | 2016-11-23 | 广东技术师范学院 | 一种基于多传感器的机器人室内定位及制图系统 |
CN107015238A (zh) * | 2017-04-27 | 2017-08-04 | 睿舆自动化(上海)有限公司 | 基于三维激光雷达的无人车自主定位方法 |
US10807236B2 (en) * | 2018-04-30 | 2020-10-20 | Beijing Jingdong Shangke Information Technology Co., Ltd. | System and method for multimodal mapping and localization |
CN109443351B (zh) * | 2019-01-02 | 2020-08-11 | 亿嘉和科技股份有限公司 | 一种稀疏环境下的机器人三维激光定位方法 |
CN110261870B (zh) * | 2019-04-15 | 2021-04-06 | 浙江工业大学 | 一种用于视觉-惯性-激光融合的同步定位与建图方法 |
CN110243358B (zh) * | 2019-04-29 | 2023-01-03 | 武汉理工大学 | 多源融合的无人车室内外定位方法及系统 |
CN110296698B (zh) * | 2019-07-12 | 2023-04-28 | 贵州电网有限责任公司 | 一种以激光扫描为约束的无人机路径规划方法 |
CN111337018B (zh) * | 2020-05-21 | 2020-09-01 | 上海高仙自动化科技发展有限公司 | 定位方法及装置、智能机器人及计算机可读存储介质 |
-
2020
- 2020-06-28 CN CN202010594763.9A patent/CN111707272B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN111707272A (zh) | 2020-09-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111707272B (zh) | 一种地下车库自动驾驶激光定位系统 | |
CN112083725B (zh) | 一种面向自动驾驶车辆的结构共用多传感器融合定位系统 | |
CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位系统 | |
KR102581263B1 (ko) | 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN112484725A (zh) | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 | |
CN110930495A (zh) | 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质 | |
US11158065B2 (en) | Localization of a mobile unit by means of a multi hypothesis kalman filter method | |
CN114018248B (zh) | 一种融合码盘和激光雷达的里程计方法与建图方法 | |
CN113223161B (zh) | 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法 | |
CN111487960A (zh) | 一种基于定位能力估计的移动机器人路径规划方法 | |
Han et al. | Robust ego-motion estimation and map matching technique for autonomous vehicle localization with high definition digital map | |
CN115902930A (zh) | 一种面向船舶检测的无人机室内建图与定位方法 | |
CN113129377B (zh) | 一种三维激光雷达快速鲁棒slam方法和装置 | |
CN111257853A (zh) | 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法 | |
Campa et al. | A comparison of pose estimation algorithms for machine vision based aerial refueling for UAVs | |
Zeng et al. | Monocular visual odometry using template matching and IMU | |
EP4148599A1 (en) | Systems and methods for providing and using confidence estimations for semantic labeling | |
Emter et al. | Simultaneous localization and mapping for exploration with stochastic cloning EKF | |
Cheng et al. | Graph-based proprioceptive localization using a discrete heading-length feature sequence matching approach | |
CN110749327B (zh) | 一种合作环境下的车辆导航方法 | |
Fan et al. | GCV-SLAM: Ground Constrained Visual SLAM Through Local Ground Planes | |
Nguyen et al. | Likelihood-based iterated cubature multi-state-constraint Kalman filter for visual inertial navigation system | |
CN116503482B (zh) | 车辆位置的获取方法、装置及电子设备 | |
CN116299542A (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 |