CN111707272A - 一种地下车库自动驾驶激光定位*** - Google Patents

一种地下车库自动驾驶激光定位*** Download PDF

Info

Publication number
CN111707272A
CN111707272A CN202010594763.9A CN202010594763A CN111707272A CN 111707272 A CN111707272 A CN 111707272A CN 202010594763 A CN202010594763 A CN 202010594763A CN 111707272 A CN111707272 A CN 111707272A
Authority
CN
China
Prior art keywords
vehicle
module
laser
underground garage
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.)
Granted
Application number
CN202010594763.9A
Other languages
English (en)
Other versions
CN111707272B (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.)
Hunan University
Original Assignee
Hunan 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 Hunan University filed Critical Hunan University
Priority to CN202010594763.9A priority Critical patent/CN111707272B/zh
Publication of CN111707272A publication Critical patent/CN111707272A/zh
Application granted granted Critical
Publication of CN111707272B publication Critical patent/CN111707272B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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计算车辆横摆角速度ω：
Figure BDA0002557123690000031
步骤3，基于上一时刻已优化的车辆状态，利用车辆运动学方程积分{i,…,j}时间段内的状态量，获得相对于上一时刻车辆坐标系自动驾驶车辆相对运动状态：
Figure BDA0002557123690000032
Figure BDA0002557123690000033
Figure BDA0002557123690000034
步骤4，基于上一时刻已优化的车辆位姿
Figure BDA0002557123690000035
计算j时刻自动驾驶车辆在世界坐标系下的位姿
Figure BDA0002557123690000036
Figure BDA0002557123690000037
Figure BDA0002557123690000038
其中，
Figure BDA0002557123690000039
Figure BDA00025571236900000310
表示上一时刻已优化车辆坐标系到世界坐标系的旋转变换，其维度为2×2，
Figure BDA00025571236900000311
表示两时刻间车辆坐标系的旋转变换。
作为本发明的进一步改进，所述模型约束构建模块利用车辆运动学模型提供预测值，通过约束同一时刻***状态量构建车辆运动学模型约束：
Figure BDA00025571236900000312
Figure BDA00025571236900000313
Figure BDA00025571236900000314
其中，上标～表示向量或旋转矩阵的增广形式，
Figure BDA00025571236900000315
的初始值由
Figure BDA00025571236900000316
给定，
Figure BDA00025571236900000317
表示由车辆运动学模型预测的两时刻间车辆坐标系旋转变换，
Figure BDA0002557123690000041
表示j时刻世界坐标系到车辆坐标系的旋转，其初始值由
Figure BDA0002557123690000042
的逆矩阵给定。
作为本发明的进一步改进，所述激光里程计模块包括：
点云畸变矫正模块，接收最新时刻激光点云数据，根据车辆预测状态矫正点云的运动畸变；
点云特征提取模块，基于密度自适应策略实现局部曲率计算，克服基于固定邻域特征提取算法的局限性，提取边缘点和平面点特征用于点云匹配；
局部地图更新模块，基于上一时刻已优化的车辆位姿，更新定尺寸局部点云地图；
帧与地图匹配模块，基于车辆位姿初始估计，利用帧与局部地图匹配算法构建用于联合优化的激光里程计残差。
作为本发明的进一步改进，所述激光回环检测模块包括：
描述符构建模块，利用局部曲率构建全局描述符；
相似性计算模块，利用卡方检验计算相似性；
特征点验证模块，利用特征点匹配数量验证回环的正确性；
回环帧匹配模块，利用回环帧对应位姿构建局部地图，进行当前帧与局部地图的匹配。
本发明的有益效果，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时刻，待优化***状态量定义如下：
Figure BDA0002557123690000071
Figure BDA0002557123690000072
其中，下标w表示世界坐标系，b表示车辆坐标系。该***状态量仅由二维状态表示。
Figure BDA0002557123690000073
表示j时刻车辆坐标系到世界坐标系的平移变换。
Figure BDA0002557123690000074
表示j时刻车辆横摆角。
1、车辆运动学模块
车辆运动学模块包含两个部分：车辆状态预测和模型约束构建。这两个部分分别完成：1)未获取新一帧激光点云数据前，基于轮速传感器和方向盘转角传感器的数据，通过车辆运动学模型预测车辆运动状态。2)基于车辆预测状态构建车辆运动学模型约束，利用车辆运动学模型限制车辆位姿的优化方向，以期提升车辆位姿估计精度。
(1)车辆状态预测
车辆运动学模型包含两个输入：1)轮速传感器直接提供车辆纵向速度v。2)方向盘转角传感器提供的方向盘转角δs。
车辆横摆角速度ω由速度v、方向盘转角δs、转向器角传动比K以及轴距h共同决定，即：
Figure BDA0002557123690000075
车辆运动学模块利用车辆底层数据进行车辆状态预测，该预测仅考虑自动驾驶车辆的平面运动。基于上一时刻(i时刻)已优化的车辆状态，利用车辆运动学方程积分{i,…,j}时间段内的状态量，获得相对于上一时刻车辆坐标系自动驾驶车辆相对运动状态：
Figure BDA0002557123690000076
Figure BDA0002557123690000081
Figure BDA0002557123690000082
基于上一时刻已优化的车辆位姿
Figure BDA0002557123690000083
计算j时刻自动驾驶车辆在世界坐标系下的位姿
Figure BDA0002557123690000084
Figure BDA0002557123690000085
Figure BDA0002557123690000086
其中，
Figure BDA0002557123690000087
由车辆模型预测的车辆位置表示为
Figure BDA0002557123690000088
二维平移向量的增广形式为
Figure BDA0002557123690000089
Figure BDA00025571236900000810
自动驾驶车辆的垂直方向位移始终为零，即，
Figure BDA00025571236900000811
表示仅考虑水平面上自动驾驶车辆运动。
Figure BDA00025571236900000812
表示上一时刻已优化车辆坐标系到世界坐标系的旋转变换，其维度为2×2。
Figure BDA00025571236900000813
表示两时刻间车辆坐标系的旋转变换。平面上的旋转矩阵
Figure BDA00025571236900000814
计算公式为：
Figure BDA00025571236900000815
其中，两时刻间角度的变化量：
Figure BDA00025571236900000816
旋转矩阵
Figure BDA00025571236900000817
的增广形式为：
Figure BDA00025571236900000818
基于车辆运动学模型的状态预测为后续车辆位姿优化提供模型约束，同时兼作优化求解问题中的初值。
(2)模型约束构建
利用车辆运动学模型提供预测值，通过约束同一时刻***状态量构建车辆运动学模型约束：
Figure BDA00025571236900000819
Figure BDA0002557123690000091
Figure BDA0002557123690000092
其中，上标～表示向量或旋转矩阵的增广形式。
Figure BDA0002557123690000093
的初始值由
Figure BDA0002557123690000094
给定。
Figure BDA0002557123690000095
表示由车辆运动学模型预测的两时刻间车辆坐标系旋转变换。
Figure BDA0002557123690000096
表示j时刻世界坐标系到车辆坐标系的旋转。
通过车辆运动学模型约束对***状态量求偏导构建雅克比矩阵Jb：
Figure BDA0002557123690000097
其中，φ～表示增广旋转矩阵对应的李代数，旋转矩阵和李代数间的关系表达如下：
R＝exp(φ～∧)
推导可得：
Figure BDA0002557123690000098
其中，右乘BCH近似雅可比矩阵的逆
Figure BDA0002557123690000099
按下式计算：
Figure BDA00025571236900000910
其中，θa表示增广旋转矩阵R～对应的旋转向量，θ表示旋转角度，a表示旋转轴。∧为反对称符号，∨为解反对称符号。
2、激光里程计模块
激光里程计模块包含四个部分：点云畸变矫正、点云特征提取、局部地图更新和帧与地图匹配。这四个部分分别完成：1)接收最新时刻激光点云数据，根据车辆预测状态矫正点云的运动畸变。2)基于密度自适应策略实现局部曲率计算，提取边缘点和平面点特征用于点云匹配。3)基于上一时刻已优化的车辆位姿，更新定尺寸局部点云地图。4)基于车辆位姿初始估计，利用帧与局部地图匹配算法构建用于联合优化的激光里程计残差。
(1)点云畸变矫正
为保证点云匹配准确性需矫正点云畸变。基于匀速运动假设，通过线性插值操作实现激光点云的运动畸变矫正。去畸变点云用于后续局部曲率计算和特征提取。
(2)点云特征提取
利用去畸变点云的角度信息进行点云线束划分。考虑密度大小对特征提取的影响，基于密度自适应策略计算局部曲率。对于每条扫描线上点云的局部曲率计算，如下所示：
Figure BDA0002557123690000101
其中，cj表示点云的局部曲率值。
Figure BDA0002557123690000102
表示点云的测量值。S表示邻域点集合。该集合并非固定不变，而是根据点云密度确定，即：
Figure BDA0002557123690000103
其中，a＝0.1，b＝0.06。在扫描线上寻找满足距离阈值dj的邻域点组成集合Sj。
本实施例中的局部曲率阈值定为0.1。排序点云曲率值并通过曲率值和邻域点分布提取两类特征点：1)边缘点：曲率值大于阈值的同时邻域点没有发生突变。2)平面点：曲率值小于阈值的同时邻域点没有发生突变。
为实现特征点的均匀分布，将每条线束划分为6个独立区域。每个区域最多提供15个边缘点和30个平面点，构成边缘点集合
Figure BDA0002557123690000104
和平面点集合
Figure BDA0002557123690000105
在选择特征点时希望避免选取以下几类点：1)可能被遮挡的点。2)已经选过点周围的点。3)位于激光线接***行的平面点。
(3)局部地图更新
为兼顾计算效率和定位精度，本专利使用定尺寸局部地图，即，在算法中保持地图尺寸为500×500×150m。局部地图为栅格化地图且随车辆运动不断更新。为保证地图的尺寸以及点云匹配的准确性，算法将不断删除位于地图边缘的特征点云，并利用上一时刻车辆优化位姿投影上一帧特征点云(边缘点和平面点)到局部地图。为保证特征点云规模以及匹配搜索效率，在更新局部地图时进行必要的点云降采样操作。
(4)帧与地图匹配
基于已更新局部地图和车辆位姿初始估计，采用帧与局部地图匹配构建激光里程计残差。当前时刻特征点云集合
Figure BDA0002557123690000111
和
Figure BDA0002557123690000112
中的点云根据相对位姿关系实现点云投影，即，特征点云转换到世界坐标系。基于局部地图中的特征点云，构建KD树并寻找边缘点对应的特征线和平面点对应的特征面，即，1)点线ICP：使用kd树算法快速寻找每个边缘点的两个最近点，利用最近点构造直线并计算点到直线的垂足坐标Lw。2)点面ICP：使用kd树算法快速寻找每个平面点的三个最近点，利用最近点构造平面并计算点到平面的垂足坐标Lw。
对于j时刻激光点云中的特征点，将其投影到世界坐标系下的值为：
Figure BDA0002557123690000113
其中，l表示激光坐标系，
Figure BDA0002557123690000114
表示j时刻激光测量点在激光坐标系下的三维坐标。
Figure BDA0002557123690000115
表示车辆坐标系到世界坐标系的位姿变换矩阵。Tbl表示激光坐标系到车辆坐标系的位姿变换矩阵，该变换矩阵根据激光雷达安装位置和车辆后轴中心位置测量可得。变换矩阵T是由旋转矩阵和平移向量组成，即：
Figure BDA0002557123690000116
因此，三维坐标形式可转换为：
Figure BDA0002557123690000121
其中，Rbl表示激光坐标系到车辆坐标系的旋转矩阵，其维度为3×3，pbl表示激光坐标系到车辆坐标系的平移向量，其维度为3×1。
Figure BDA0002557123690000122
表示j时刻增广旋转矩阵，其表达形式如下：
Figure BDA0002557123690000123
通过约束同一时刻激光雷达的测量值构建激光里程计残差。利用点到直线和点到平面的距离表示激光里程计残差rl：
rl＝L′w-Lw
其中，L′w表示将j时刻激光测量点转换到世界坐标系下的投影点。Lw表示投影点在世界坐标系中的对应点。
通过激光里程计残差对***状态量求偏导构建雅克比矩阵Jl：
Figure BDA0002557123690000124
推导可得：
Figure BDA0002557123690000125
3、激光回环检测模块
激光回环检测模块包含四个部分：描述符构建、相似性计算、特征点验证和回环帧匹配。这四个部分分别完成：1)利用局部曲率构建全局描述符。2)利用卡方检验计算相似性。3)利用特征点匹配数量验证回环的正确性。4)利用回环帧对应位姿构建局部地图，进行当前帧与局部地图的匹配。
(1)描述符构建
使用主成分分析法进行坐标轴划分。获得参考框架后，对齐边界和坐标轴，从而获取对应的划分坐标轴。基于局部曲率的全局描述符由m个不互相重叠区域组成，即，以激光雷达为中心，定义外球半径和内球半径，球体划分为上下两部分，每个半球体均分为四个区域，由此，m＝16。每个区域进行相应直方图描述，即，落入该区域的k个激光点对应局部曲率值被计算成直方图，该直方图具有n个划分区域。
(2)相似性计算
以车辆位姿初始估计作为中心点，设定候选区域尺寸为50×50×15m。在满足时间间隔条件下，利用描述符匹配寻找满足相似度阈值且相似度最低的激光帧。利用卡方检验定义描述符A第i个区域与描述符B第i个区域的相似性度量，即：
Figure BDA0002557123690000131
利用区域相似性计算两个描述符的相似性，如下所示：
Figure BDA0002557123690000132
利用主成分分析法确定坐标轴存在歧义，为消除该过程造成的干扰，需通过分类讨论，实现坐标轴正确对齐。根据x轴和y轴的划分，共有4种情况。使用上式计算A和B的相似性时，出现四个不同的值，取最小值作为描述符匹配结果。即：
SAB＝min{SAB1,SAB2,SAB3,SAB4}
(3)特征点验证
选择相似度最低且满足阈值的激光帧作为回环候选帧。为保证回环检测准确性，需利用特征点进行进一步验证。基于回环候选帧的对应车辆位姿，利用k近邻算法寻找k个历史帧，将历史帧对应特征点云投影到回环候选帧所对应激光雷达的坐标系，实现局部地图构建。基于车辆位姿初始估计，投影当前帧中的特征点云到回环候选帧。利用k近邻算法寻找边缘点对应直线和平面点对应平面，计算对应的坐标
Figure BDA0002557123690000141
计算点到线以及点到面的距离。满足最小距离阈值的点云为匹配点，统计匹配点数量并计算匹配点与特征点数量的比值。若比值满足设定阈值，则认为回环候选帧为正确回环。否则，认为此次回环检测不正确，不进行激光回环的残差构建。
(4)回环帧匹配
对于j时刻激光点云中的特征点，将其投影到o时刻(回环帧对应时刻)激光坐标系下的值为：
Figure BDA0002557123690000142
三维坐标形式为：
Figure BDA0002557123690000143
通过约束同一时刻激光雷达的测量值构建激光回环残差。利用点到直线和点到平面的距离表示激光回环残差ro：
Figure BDA0002557123690000144
其中，
Figure BDA0002557123690000145
表示将j时刻激光测量点转换到o时刻激光坐标系下的投影点。
Figure BDA0002557123690000146
表示投影点在o时刻激光坐标系下的对应点。
通过激光回环残差对***状态量求偏导构建雅克比矩阵Jo：
Figure BDA0002557123690000147
推导可得：
Figure BDA0002557123690000148
4、位姿联合优化模块
位姿联合优化模块根据车辆运动学模型约束、激光里程计残差和激光回环残差构建***代价函数，利用梯度下降法进行联合非线性优化。在梯度下降法执行过程中需要用到代价函数的雅克比矩阵，在前述内容中已经进行相关推导和描述，此处不再赘述。自动驾驶车辆的联合优化位姿，即车辆准确位姿，用于下一时刻局部地图更新和车辆状态预测。
通过计算***代价函数最小值获取待优化***状态量X的最大后验估计。地下车库自动驾驶激光定位***代价函数构建如下：
Figure BDA0002557123690000151
其中，rb(z,X)表示车辆运动学模型约束，z表示轮速传感器和方向盘转角的测量数据。rl(c,X)表示激光里程计残差，c表示通过帧与局部地图匹配确定的特征点云对应关系。ro(e,X)表示激光回环残差，e表示通过帧与局部地图匹配确定的特征点云对应关系。三种残差均用马氏距离表示。协方差矩阵由传感器精度决定。利用Ceres Solver实现代价函数求解。
根据车辆优化位姿的增广形式获取车辆准确位姿。
Figure BDA0002557123690000152
由
Figure BDA0002557123690000153
前两项构成，
Figure BDA0002557123690000154
根据
Figure BDA0002557123690000155
计算获取，即：
Figure BDA0002557123690000156
Figure BDA0002557123690000157
其中，车辆位姿
Figure BDA0002557123690000158
和
Figure BDA0002557123690000159
将用于下一时刻车辆的位姿估计。
图3为车辆运动学模型示意图。车辆运动学模型简化为两自由度自行车模型，前后轮均用单轮替代。以车辆后轴中心O为原点建立车辆坐标系，沿车辆前进方向为X轴方向，垂直X轴指向车体左侧为Y轴方向。
Figure BDA00025571236900001510
表示相邻时刻间车辆横摆角增量，h表示轴距，δf为前轮转角。为保证安全，自动驾驶***通常不会进入极限工况，因此质心侧偏角较小，可忽略不计。通常情况下，汽车后轮不可控，故可认为自行车模型中后轮转角控制输入δr＝0。利用方向盘转角δs和转向器角传动比K可获得前轮转角δf，即：
Figure BDA00025571236900001511
车辆运动学模型建立原则是在保证模型简单的同时应尽可能反映车辆真实运动特性。过于严格的车辆运动学模型不利于理论推导和求解。针对地下车库工况，自行车模型采取以下假设：1)不考虑车辆在Z轴方向的运动，只考虑XY水平面的运动。2)左右侧车轮转角一致，将左右侧轮胎合并为一个轮胎。3)车辆的转向只由前轮控制。
车辆运动学模型有两个输入：轮速传感器提供的车辆速度v和方向盘转角传感器提供的前轮转角δf。以前一时刻车辆坐标系作为参考坐标系，当前时刻车辆运动学模型表达式如下：
Figure BDA0002557123690000161
其中，vx和vy分别表示自动驾驶车辆在参考坐标系下X轴方向的速度、Y轴方向的速度。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种地下车库自动驾驶激光定位***，其特征在于：包括：
输入模块，该输入模块包括激光雷达、轮速传感器和方向盘转角传感器，所述激光雷达用于提供点云匹配所需的特征点云，轮速传感器用于提供车辆速度信息，方向盘转角传感器用于提供角速度计算所需的方向盘转角；
计算模块，耦接于输入模块，包括车辆运动学模块、激光里程计模块、激光回环检测模块和联合优化模块，所述车辆运动学模型预测车辆运动状态并构建用于联合优化的车辆运动学模型约束，激光里程计模块利用局部曲率提取特征点云并进行帧与局部地图匹配，实现激光里程计的残差构建，激光回环检测模块基于局部曲率构建全局描述符，利用描述符匹配提取回环帧并进行匹配，为后续位姿优化提供激光回环残差，联合优化模块利用梯度下降法共同优化车辆运动学模型、激光里程计和激光回环三者提供的运动约束；
输出模块，耦接于计算模块，用于输出准确的自动驾驶车辆位姿信息并将位姿传递给计算模块用于下一次车辆位姿计算。
2.根据权利要求1所述的地下车库自动驾驶激光定位***，其特征在于：所述车辆运动学模块包括：
车辆状态预测模块，用于在未获取新一帧激光点云数据前，基于轮速传感器和方向盘转角传感器的数据，通过车辆运动学模型预测车辆运动状态；
模型约束构建模块，基于车辆预测状态构建车辆运动学模型约束，利用车辆运动学模型限制车辆位姿的优化方向。
3.根据权利要求2所述的地下车库自动驾驶激光定位***，其特征在于：所述车辆状态预测模块的预测步骤如下：
步骤1，通过轮速传感器获取车辆纵向速度v，方向盘转角传感器获取方向盘转角δs；
步骤2，根据速度v、方向盘转角δs、转向器角传动比K以及轴距h计算车辆横摆角速度ω：
Figure FDA0002557123680000021
步骤3，基于上一时刻已优化的车辆状态，利用车辆运动学方程积分*i,…,j}时间段内的状态量，获得相对于上一时刻车辆坐标系自动驾驶车辆相对运动状态：
Figure FDA0002557123680000022
Figure FDA0002557123680000023
Figure FDA0002557123680000024
步骤4，基于上一时刻已优化的车辆位姿
Figure FDA0002557123680000025
计算j时刻自动驾驶车辆在世界坐标系下的位姿
Figure FDA0002557123680000026
Figure FDA0002557123680000027
Figure FDA0002557123680000028
其中，
Figure FDA0002557123680000029
Figure FDA00025571236800000210
表示上一时刻已优化车辆坐标系到世界坐标系的旋转变换，其维度为2×2，
Figure FDA00025571236800000211
表示两时刻间车辆坐标系的旋转变换，其初始值由
Figure FDA00025571236800000212
的逆矩阵给定。
4.根据权利要求2或3所述的地下车库自动驾驶激光定位***，其特征在于：所述模型约束构建模块利用车辆运动学模型提供预测值，通过约束同一时刻***状态量构建车辆运动学模型约束：
Figure FDA00025571236800000213
Figure FDA00025571236800000214
Figure FDA00025571236800000215
其中，上标～表示向量或旋转矩阵的增广形式，
Figure FDA00025571236800000216
的初始值由
Figure FDA00025571236800000217
给定，
Figure FDA00025571236800000218
表示由车辆运动学模型预测的两时刻间车辆坐标系旋转变换，
Figure FDA0002557123680000031
表示j时刻世界坐标系到车辆坐标系的旋转。
5.根据权利要求1或2或3所述的地下车库自动驾驶激光定位***，其特征在于：所述激光里程计模块包括：
点云畸变矫正模块，接收最新时刻激光点云数据，根据车辆预测状态矫正点云的运动畸变；
点云特征提取模块，基于密度自适应策略实现局部曲率计算，克服基于固定邻域特征提取算法的局限性，提取边缘点和平面点特征用于点云匹配；
局部地图更新模块，基于上一时刻已优化的车辆位姿，更新定尺寸局部点云地图；
帧与地图匹配模块，基于车辆位姿初始估计，利用帧与局部地图匹配算法构建用于联合优化的激光里程计残差。
6.根据权利要求1或2或3所述的地下车库自动驾驶激光定位***，其特征在于：所述激光回环检测模块包括：
描述符构建模块，利用局部曲率构建全局描述符；
相似性计算模块，利用卡方检验计算相似性；
特征点验证模块，利用特征点匹配数量验证回环的正确性；
回环帧匹配模块，利用回环帧对应位姿构建局部地图，进行当前帧与局部地图的匹配。
基于局部曲率直方图的识别能保证较高精度回环检测，同时，局部曲率的利用极大程度降低计算量，局部曲率的一物两用提高特征利用效率。因此，较小计算量能保证实时重定位的实现。
7.根据权利要求1或2或3所述的地下车库自动驾驶激光定位***，其特征在于：地下车库的地形大多为平面，故在选择车辆状态量时使用3自由度平面运动状态量，即车辆的平移(2自由度)和旋转(1自由度)。该方法可带来3方面优势：1)减小算法复杂度，利于工程实践。2)减小计算量，利于嵌入式实现。3)减小后续位姿优化过程中的搜索空间，利于提升精度和鲁棒性。
8.根据权利要求1或2或3所述的地下车库自动驾驶激光定位***，其特征在于：采用紧耦合方式实现车辆运动学模型和激光SLAM算法的数据融合，可充分发挥各传感器的数据优势，提升定位精度和鲁棒性。
9.根据权利要求1或2或3所述的地下车库自动驾驶激光定位***，其特征在于：共用原车传感器，无需增加额外传感器，实现成本节约和复杂度降低的同时提升定位***可靠性；使用少量传感器完成数据融合，无需使用IMU、GNSS等复杂、昂贵传感器，易于工程实践的同时更利于通过车规级测试。
CN202010594763.9A 2020-06-28 2020-06-28 一种地下车库自动驾驶激光定位*** Active CN111707272B (zh)

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 true CN111707272A (zh) 2020-09-25
CN111707272B 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)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112867977A (zh) * 2021-01-13 2021-05-28 华为技术有限公司 一种定位方法、装置和车辆
CN112907491A (zh) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 一种适用于井下巷道的激光点云回环检测方法及***
CN113447949A (zh) * 2021-06-11 2021-09-28 天津大学 一种基于激光雷达和先验地图的实时定位***及方法
CN113639782A (zh) * 2021-08-13 2021-11-12 北京地平线信息技术有限公司 车载传感器的外参标定方法和装置、设备和介质
CN113740875A (zh) * 2021-08-03 2021-12-03 上海大学 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法
CN113870316A (zh) * 2021-10-19 2021-12-31 青岛德智汽车科技有限公司 一种无gps跟车场景下前车路径重建方法
CN114018284A (zh) * 2021-10-13 2022-02-08 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114353799A (zh) * 2021-12-30 2022-04-15 武汉大学 搭载多线激光雷达的无人驾驶平台室内快速全局定位方法
CN114820749A (zh) * 2022-04-27 2022-07-29 西安优迈智慧矿山研究院有限公司 无人车井下定位方法、***、设备及介质
CN115655302A (zh) * 2022-12-08 2023-01-31 安徽蔚来智驾科技有限公司 激光里程计的实现方法、计算机设备、存储介质及车辆
CN117584989A (zh) * 2023-11-23 2024-02-23 昆明理工大学 一种激光雷达/imu/车辆运动学约束紧耦合slam***及算法
CN118031983A (zh) * 2024-04-11 2024-05-14 江苏集萃清联智控科技有限公司 自动驾驶融合定位方法及***

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5483455A (en) * 1992-09-08 1996-01-09 Caterpillar Inc. Method and apparatus for determining the location of a vehicle
US20050231423A1 (en) * 2004-04-19 2005-10-20 Thales 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 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法
CN109443351A (zh) * 2019-01-02 2019-03-08 亿嘉和科技股份有限公司 一种稀疏环境下的机器人三维激光定位方法
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及***
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110296698A (zh) * 2019-07-12 2019-10-01 贵州电网有限责任公司 一种以激光扫描为约束的无人机路径规划方法
US20190329407A1 (en) * 2018-04-30 2019-10-31 Beijing Jingdong Shangke Information Technology Co., Ltd. System and method for multimodal mapping and localization
CN111337018A (zh) * 2020-05-21 2020-06-26 上海高仙自动化科技发展有限公司 定位方法及装置、智能机器人及计算机可读存储介质

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5483455A (en) * 1992-09-08 1996-01-09 Caterpillar Inc. Method and apparatus for determining the location of a vehicle
US20050231423A1 (en) * 2004-04-19 2005-10-20 Thales 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 睿舆自动化（上海）有限公司 基于三维激光雷达的无人车自主定位方法
US20190329407A1 (en) * 2018-04-30 2019-10-31 Beijing Jingdong Shangke Information Technology Co., Ltd. System and method for multimodal mapping and localization
CN109443351A (zh) * 2019-01-02 2019-03-08 亿嘉和科技股份有限公司 一种稀疏环境下的机器人三维激光定位方法
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110243358A (zh) * 2019-04-29 2019-09-17 武汉理工大学 多源融合的无人车室内外定位方法及***
CN110296698A (zh) * 2019-07-12 2019-10-01 贵州电网有限责任公司 一种以激光扫描为约束的无人机路径规划方法
CN111337018A (zh) * 2020-05-21 2020-06-26 上海高仙自动化科技发展有限公司 定位方法及装置、智能机器人及计算机可读存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
CHANDRA SEKHAR GATLA 等: "An Automated Method to Calibrate Industrial Robots Using a Virtual Closed Kinematic Chain", 《IEEE TRANSACTIONS ON ROBOTICS》 *
王彦 等: "基于重采样技术的激光SLAM***优化设计", 《中国科技论文》 *

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112867977A (zh) * 2021-01-13 2021-05-28 华为技术有限公司 一种定位方法、装置和车辆
CN112907491A (zh) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 一种适用于井下巷道的激光点云回环检测方法及***
CN112907491B (zh) * 2021-03-18 2023-08-22 中煤科工集团上海有限公司 一种适用于井下巷道的激光点云回环检测方法及***
CN113447949B (zh) * 2021-06-11 2022-12-09 天津大学 一种基于激光雷达和先验地图的实时定位***及方法
CN113447949A (zh) * 2021-06-11 2021-09-28 天津大学 一种基于激光雷达和先验地图的实时定位***及方法
CN113740875A (zh) * 2021-08-03 2021-12-03 上海大学 一种基于激光里程计和点云描述符匹配的自动驾驶车辆定位方法
CN113639782A (zh) * 2021-08-13 2021-11-12 北京地平线信息技术有限公司 车载传感器的外参标定方法和装置、设备和介质
CN114018284A (zh) * 2021-10-13 2022-02-08 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114018284B (zh) * 2021-10-13 2024-01-23 上海师范大学 一种基于视觉的轮速里程计校正方法
CN113870316A (zh) * 2021-10-19 2021-12-31 青岛德智汽车科技有限公司 一种无gps跟车场景下前车路径重建方法
CN113870316B (zh) * 2021-10-19 2023-08-15 青岛德智汽车科技有限公司 一种无gps跟车场景下前车路径重建方法
CN114353799A (zh) * 2021-12-30 2022-04-15 武汉大学 搭载多线激光雷达的无人驾驶平台室内快速全局定位方法
CN114353799B (zh) * 2021-12-30 2023-09-05 武汉大学 搭载多线激光雷达的无人驾驶平台室内快速全局定位方法
CN114820749A (zh) * 2022-04-27 2022-07-29 西安优迈智慧矿山研究院有限公司 无人车井下定位方法、***、设备及介质
CN115655302A (zh) * 2022-12-08 2023-01-31 安徽蔚来智驾科技有限公司 激光里程计的实现方法、计算机设备、存储介质及车辆
CN117584989A (zh) * 2023-11-23 2024-02-23 昆明理工大学 一种激光雷达/imu/车辆运动学约束紧耦合slam***及算法
CN118031983A (zh) * 2024-04-11 2024-05-14 江苏集萃清联智控科技有限公司 自动驾驶融合定位方法及***

Also Published As

Publication number Publication date
CN111707272B (zh) 2022-10-14

Similar Documents

Publication Publication Date Title
CN111707272B (zh) 一种地下车库自动驾驶激光定位***
CN112083725B (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位***
CN111739063B (zh) 一种基于多传感器融合的电力巡检机器人定位方法
CN111126269B (zh) 三维目标检测方法、装置以及存储介质
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位***
CN110717927A (zh) 基于深度学习和视惯融合的室内机器人运动估计方法
CN112484725A (zh) 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
US11158065B2 (en) Localization of a mobile unit by means of a multi hypothesis kalman filter method
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN112101160B (zh) 一种面向自动驾驶场景的双目语义slam方法
CN114966734A (zh) 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
Han et al. Robust ego-motion estimation and map matching technique for autonomous vehicle localization with high definition digital map
CN113129377B (zh) 一种三维激光雷达快速鲁棒slam方法和装置
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
CN116774247A (zh) 基于ekf的多源信息融合的slam前端策略
Emter et al. Simultaneous localization and mapping for exploration with stochastic cloning EKF
Nguyen Computationally-efficient visual inertial odometry for autonomous vehicle
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