CN115930977A - 特征退化场景的定位方法、系统、电子设备和可读存介质 - Google Patents

特征退化场景的定位方法、系统、电子设备和可读存介质 Download PDF

Info

Publication number
CN115930977A
CN115930977A CN202211569480.4A CN202211569480A CN115930977A CN 115930977 A CN115930977 A CN 115930977A CN 202211569480 A CN202211569480 A CN 202211569480A CN 115930977 A CN115930977 A CN 115930977A
Authority
CN
China
Prior art keywords
point cloud
data
points
matching
curvature
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.)
Pending
Application number
CN202211569480.4A
Other languages
English (en)
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.)
Beijing Tage Idriver Technology Co Ltd
Original Assignee
Beijing Tage Idriver Technology Co Ltd
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 Beijing Tage Idriver Technology Co Ltd filed Critical Beijing Tage Idriver Technology Co Ltd
Priority to CN202211569480.4A priority Critical patent/CN115930977A/zh
Publication of CN115930977A publication Critical patent/CN115930977A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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

Abstract

本发明公开了涉及多传感器融合技术领域，特别涉及特征退化场景的定位方法、系统、电子设备和可读存介质，本发明的车辆定位方法包括：获取GNSS传感器、激光雷达传感器、惯性传感器和毫米波雷达传感器数据并进行时空同步；基于GNSS位姿信息作为先验位姿和地图进行匹配，得到初步定位结果；构建惯性传感器和毫米波雷达测速的预积分因子；将匹配结果、惯性预积分因子、毫米波雷达测速因子进行融合优化，实时求解残差模型，得到定位；具有以下优点：实现在特征退化场景的定位，具有更好的可靠性和鲁棒性。

Description

特征退化场景的定位方法、系统、电子设备和可读存介质
技术领域
本发明涉及多传感器融合建图与定位技术领域，具体而言，涉及特征退化场景的定位方法、系统、电子设备和可读存介质。
背景技术
在自动驾驶领域中，对自动驾驶车辆进行准确定位是后续的路径规划的前提，也是自动驾驶车辆安全驾驶的关键。目前的自动驾驶车辆定位常使用全球定位系统(GNSS)和实时运行学定位系统(RTK)。但在实际环境中存在许多弱GNSS信号或无GNSS信号的区域，比如隧道、高架、车库等，在这些区域中，GNSS和RTK定位系统无法提供的定位结果。因此，目前常采用多传感器融合的综合车辆定位方法，保证实时定位的鲁棒性和可靠性。
目前，多源融合定位常常是采用VIO(视觉-惯性里程计)与GNSS融合的方法和LIO(激光-惯性里程计)与GNSS融合的方法，甚至是LVIO(激光-视觉-惯性)里程计和GNSS融合的方法，这些方法主要是面对特征丰富场景。在特征退化场景的，如矿区、隧道等，由于出现不可观测的自由度，定位精度会显著下降，并且这些场景中RTK信号不稳定，定位精度进一步下降。因此，如何在这些特征退化场景的进行高精度定位是一个重要问题。
中国专利CN112415557A，名称为《一种基于云平台的伪卫星室内多源融合车辆定位方法》提供了一种基于云平台的伪卫星室内多源融合车辆定位方法。它在传统的伪卫星融合定位基础上，借助云平台的计算能力，采用室内三维空间信息和多传感器定位信息约束，针对廊道和空旷区域分别采用位置搜索和位置推算方法，解决了传统伪卫星搜索方法存在局部最优和场景适用性差的问题，在此基础上，设计WIFI、蓝牙和惯性导航辅助的格网点异常判别和惯性导航方位的合理性判别，解决惯性导航方位的实时校准和室内融合定位可靠性评估问题。但是该方法主要还是基于GPS信息为主的车辆定位方法，在GPS信号较差的矿区等场景定位效果未知。
中国专利CN114719843 A，名称为《复杂环境下的车辆定位方法》提供了一种复杂环境下的车辆定位方法，通过获取GNSS传感器、VISON视觉传感器、SINS惯性传感器和WHEEL轮式里程计的原始观测数据并预处理和同步，利用视觉传感器数据、惯性传感器数据、轮式里程计数据与GNSS传感器数据完成复杂环境下的定位。该方法运用到的视觉传感受天气影响较大，不能满足自动驾驶全天候的工作要求。
为此提出特征退化场景的定位方法、系统、电子设备和可读存介质，以解决上述提出的问题。
发明内容
本发明旨在提供一种面向特征退化场景的的定位系统和方法，以解决或改善上述技术问题中的至少之一。
有鉴于此，本发明的第一方面在于提供特征退化场景的定位方法。
本发明的第二方面在于提供特征退化场景的定位系统。
本发明的第三方面在于提供种电子设备。
本发明的第四方面在于提供可读存介质。
本发明的第一方面提供了特征退化场景的定位方法，包括如下步骤：S1：当无人驾驶车辆处于行驶状态时，采集所述车辆行驶时的传感器数据，包括：激光雷达点云数据，组合导航GNSS数据，惯性导航IMU数据，毫米波雷达点云数据；S2：根据所述激光雷达点云数据，对其它所述传感器数据进行时间同步，以获得同步后的传感器数据；S3：根据同步后的所述激光雷达点云数据和同步后的所述GNSS数据构建点云地图；S4：对同步后的所述激光雷达点云数据提取特征，并根据与激光雷达的距离对所述特征添加可信度；将所述特征与点云地图进行匹配，根据所述特征的可信度赋予匹配权重，得到匹配位姿；
S5：基于同步后的所述IMU数据和所述毫米波雷达点云数据建立关键帧之间的IMU预积分因子和毫米波速度预积分因子；S6：将所述匹配位姿、所述IMU预积分因子和所述毫米波测速因子进行融合优化，以构建因子图，并进行优化得到定位信息。
本发明提供的特征退化场景的定位方法，选择曲率突变点作为特征点，能够有效提取出环境特征信息；
利用激光雷达点云数据到激光雷达的距离信息作为可信度，符合激光雷达点云近密远疏的特点，能够更好的利用激光雷达点云特征信息；
基于激光雷达、毫米波雷达、惯性导航和GNSS定位系统的多传感器融合定位系统，能够提高定位系统面对不同场景的鲁棒性，避免了基于GPS信息为主的车辆定位方法，在GPS信号较差的矿区等场景定位效果未知的问题。
另外，根据本发明的实施例提供的技术方案还可以具有如下附加技术特征：
上述任一技术方案中，所述S4的步骤具体为：S401，计算实时帧中所述点云数据的每个点的曲率，将所述曲率的突变点作为特征；S402，采用ICP匹配进行匹配，对于匹配优化函数中加入可信度，以使可信度大的特征点先与地图匹配。
在该技术方案中，利用激光雷达点云数据到激光雷达的距离信息作为可信度，符合激光雷达点云近密远疏的特点，能够更好的利用激光雷达点云特征信息。
上述任一技术方案中，所述S401中所述计算的步骤具体为：S4011，利用kd-tree寻找实时帧中所述激光雷达点云数据中每个点周围的多个最近邻点，并根据所述最近邻点计算该点曲率；S4012，选择曲率突变点作为所述特征，并保留所述特征与激光雷达的距离信息作为可信度依据。
在该技术方案中，利用KDtree树快速查找实时帧点云每一个点的最近邻一定区域的所有点云。依据找出的周围点通过最小二乘法进行平面拟合，求解平面参数，即根据邻域内的所有点云，使
Figure BDA0003987408310000041
取最小值，在对通过式中的系数求导，求解平面参数后写成曲面参数方程，基于单条曲线的表达式r＝r(x(t),y(t))，得到曲率计算公式，进而计算出曲率；得出曲率后基于KDtree搜索出的最近邻点，判断曲率突变方向，最终将所有提取的特征划分为3类特征点。
上述任一技术方案中，所述的计算所述最近邻点的曲率，采用下述公式：
Figure BDA0003987408310000051
L＝rxxn,M＝rxyn,N＝ryyn,E＝rxrx,F＝rxry,G＝ryry；
其中，H为曲率，n为该点的单位法向量，rx、ry、rxx、rxy、ryy为偏微分。
在该技术方案中，采用具体的公式对曲率进行计算，保证了将众多获得参数集中和处理，以便得出需要的曲率，并进一步结合曲率和最近邻点，进行曲率突变方向的判断。
上述任一技术方案中，所述突变点包括：纵向凹凸点、横向凹凸点和孤立特征点；其中，所述纵向凹凸点为在X方向曲率发送较大突变的最近邻点，所述横向凹凸点为在Y方向曲率发生突变的最近邻点，所述孤立特征点为在X和Y方向曲率都发送突变的最近邻点。
在该技术方案中，判断曲率突变方向中，分别考虑在X方向曲率发送较大突变、在Y方向曲率发生突变，以及在X和Y方向曲率都发送突变的三种情况，并进行分别的标注和分类，可对突变点的种类考虑更加全面，以便后续步骤的准确计算。
上述任一技术方案中，S501，基于IMU数据构建两个关键帧之间的惯性预积分因子；S502，基于毫米波雷达测速算法，对自身速度进行计算，构建两个关键帧之间的毫米波雷达测速因子。
在该技术方案中，基于IMU数据对激光雷达点云的关键帧之间构建观测模型，具体为：
Figure BDA0003987408310000061
Figure BDA0003987408310000062
其中
Figure BDA0003987408310000063
同时利用毫米波雷达的测速能力构建运动方程，具体为：
Figure BDA0003987408310000064
上述任一技术方案中，所述S6的步骤具体为：S601，根据所述匹配位姿、所述惯性预积分因子和所述毫米波雷达测速因子的观测量构建因子图；S602，构建优化目标模型，并通过所述因子图得到优化后的定位信息。
在该技术方案中，根据距离和时间选择前端匹配的关键帧，在两个关键帧之间加入IMU预积分作为新的约束，重新优化关键帧位置；当优化后位置与低精度GPS的位姿误差高于一定阈值时，加入毫米波雷达测速因子，进行优化，消除其他传感器退化时产生的误差。
具体地，其中的优化目标模型为：
Figure BDA0003987408310000065
其中，
Figure BDA0003987408310000066
eij分别为激光里程计因子，IMU预积分因子，车速积分因子，Ipre表示IMU预积分状态节点，Vradar为毫米波测速状态节点，LO为激光里程计状态节点，
Figure BDA0003987408310000067
和
Figure BDA0003987408310000068
为激光雷达第i个和第j个关键帧在世界坐标系下的状态位姿估计，ΔTij为各个因子在两个状态节点之间的相对位姿变换。
本发明的第二方面提供了特征退化场景的定位系统，包括：包括：数据采集模块，用于采集自动驾驶车辆的多传感器数据；时间同步模块，用于将所述多传感器数据进行时间同步；地图构建模块，用于基于激光雷达点云数据和GNSS数据进行点云地图构建，其中所述GNSS数据包含RTK定位信息；特征提取模块，用于将实时帧点云数据进行特征点提取，并计算特征点的可靠性；特征匹配模块用于将提取的特征点和地图点云进行匹配，同时依据距离信息建立关键帧点云；运动约束模块，用于基于匹配结果和观测因子，根据不同情况对多传感器信息进行融合，求解最优定位参数；后端优化模块，用于将多传感器信息进行融合；其中，所述的系统用于实施如第一方面中任一项技术方案所述的方法。
本发明提供的特征退化场景的定位系统，所述系统用于实施上述技术方案中的方法。因此，本发明提出的系统具备上述技术方案中的方法的全部有益效果，在此不再赘述。
本发明的第三方面提供了电子设备，包括：存储器、处理器和存储在所述存储器上并可在所述处理器上运行的程序，所述处理器执行所述程序，以实现上述方法。
本发明提供的一种电子设备，所述电子设备搭载上述技术方案中的方法。因此，本发明提出的电子设备具备上述技术方案中的方法的全部有益效果，在此不再赘述。
本发明的第四方面提供了可读存介质，所述可读存介质上存储有程序或指令，所述程序或指令被处理器执行时实现如上述任一技术方案中的方法。因此，本发明所提出的可读存介质具备上述任一技术方案中的方法的全部有益效果，在此不再赘述。
本发明提供的可读存介质，可读存介质包括能够存储或传输信息的任何可读存介质。可读存介质的例子包括电子电路、半导体存储器电子设备、只读存储器(Read-0nlyMemory，ROND、随机存取存储器(Random Access Memory，RAMD、光盘只读存储器(CompactDisc Read-0nly Memory，CD-ROMD、闪存、可擦除ROM(EROMD、磁带、软盘、光盘、硬盘、光纤可读存介质、射频(RF)链路、光数据存储电子设备等。代码段可以经由诸如因特网、内联网等的计算机网络被下载。
本发明与现有技术相比所具有的有益效果：
根据特征退化场景的的环境特点，提出一种特征点提取方法，选择曲率突变点作为特征点，能够有效提取出环境特征信息；
提出一种基于可信度的特征匹配方法，利用激光雷达点云到激光雷达的距离信息作为可信度，符合激光雷达点云近密远疏的特点，能够更好的利用激光雷达点云特征信息；
提出了一种基于激光雷达、毫米波雷达、惯性导航和GNSS定位系统的多传感器融合定位系统，能够提高定位系统面对不同场景的鲁棒性。
根据本发明的实施例的附加方面和优点将在下面的描述部分中变得明显，或通过根据本发明的实施例的实践了解到。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制。
图1为本发明的多传感器融合的车辆定位方法流程图；
图2为本发明的多传感器融合的定模块图；
图3为本发明的多传感器融合的定模块图。
具体实施方式
为了可以更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。
实施例一
参照图1，本实施例的面向特征退化场景的的车辆定位方法包括如下步骤：
步骤1：在无人驾驶车辆安装激光雷达传感器、毫米波雷达传感器、组合惯导传感器，标定其它各传感器到组合惯导的安装位置，运行车辆进行多种传感器数据采集。
步骤2：将各传感器数据进行时空同步。依靠步骤1的标定参数，旋转变换各个传感器数据，将各传感器数据标定到组合惯导坐标系中。同时，以点云数据的时间戳为基准，将各传感器数据进行线性插值同步，得到点云数据同时刻的其它传感器数据。
具体的，已知激光雷达点云时间戳为t1，通过比较不同传感器数据的时间戳，当一条数据的时间戳小于t1且下一条数据的时间戳t1，此时进行插值计算得到对应的数据。某时刻的GPS数据g1的时间戳为tg1，下一时刻的GPS数据g2的时间戳为tg2，则对应点云的GPS数据g为：
Figure BDA0003987408310000101
步骤3：基于同步的激光雷达点云和GNSS数据，依靠GNSS数据得到的位置信息和姿态信息作为激光雷达点云的里程计信息进行地图构建。在构建点云地图时，必须具备的GNSS信息，同时，只保留激光雷达点云的前向3米范围内的点云进行拼接。即根据GNSS信息每隔3米确定一个关键帧，每个关键帧只保留前向3米范围内的点云。将所有关键帧以及GNSS位姿进行旋转变换并拼接，最终得到点云地图：
步骤4：利用KDtree树快速查找实时帧点云每一个点的最近邻一定区域的所有点云。依据找出的周围点通过最小二乘法进行平面拟合，求解平面参数，即根据邻域内的所有点云，使如下公式取最小值，
Figure BDA0003987408310000102
其中，j为邻域内的点、x,y,z为邻域内点的坐标值，a,b,c为拟合二次曲面的参数，Q为最小二乘法优化函数。对系数求导，如下：
Figure BDA0003987408310000103
Figure BDA0003987408310000104
Figure BDA0003987408310000105
求解平面参数后写成曲面参数方程形式：
Figure BDA0003987408310000111
其中，r(u,v)为曲面参数方程、x,y,z为曲面上点坐标，u，v为满足曲面条件的平面区域内的点，曲面上的一条曲线的表达式为：
r＝r(x,y)
其中，所述曲率计算公式为：
Figure BDA0003987408310000112
其中，n为曲面的单位法向量、H为平均曲率、E,F,G为曲面第一基本不变量，L,M,N为曲面的第二基本不变量，L＝rxxn,M＝rxyn,N＝ryyn,E＝rxrx,F＝rxry,G＝ryry且rx、ry、rxx、rxy和ryy为偏微分。
计算出曲率H后，基于KDtree搜索出的最近邻点，判断曲率突变方向，如果在X方向曲率发送较大突变，则认为是纵向凹凸点，如果是Y方向曲率发生突变，则认为是横向凹凸点，如果XY方向曲率都发送突变，则认为是孤立特征点。最终将所有提取的特征点划分为3类特征点。
步骤5：采用改进的ICP匹配方法，对提取的特征点和地图点云进行匹配，计算出其在地图中的位置参数。具体过程如下：
基于低精度的GNSS信息进行初始定位。通过此时获得的GNSS信息，能够获取其在地图上的大概位置范围，作为ICP匹配的初始信息。而后对基于提取的特征点，根据其离激光雷达的距离信息d，通过与检测距离D的比值作为其可信度，具体为下述公式：
Figure BDA0003987408310000121
修改ICP匹配的优化函数，在ICP匹配的目标函数中加入可信度因子，可信度高的特征点优先满足其匹配要求。即：
Figure BDA0003987408310000122
其中，i为点云中点的序号、R*为最优旋转矩阵、t*为最优变换矩阵、
Figure BDA0003987408310000123
为第i个目标点、R为旋转矩阵、
Figure BDA0003987408310000124
为源点云中对应点、argminR,t代表最小误差，ηi为第i个点的可信度。
步骤6：基于IMU数据对激光雷达点云的关键帧之间构建观测模型，同时利用毫米波雷达的测速能力构建运动方程，如下所示：
IMU预积分因子:
Figure BDA0003987408310000125
Figure BDA0003987408310000126
Figure BDA0003987408310000127
其中，t k为k时刻、
Figure BDA0003987408310000128
为位置预积分量、
Figure BDA0003987408310000129
为速度预积分量，
Figure BDA00039874083100001210
为姿态预积分量、
Figure BDA00039874083100001211
为t时刻相对于i时刻的姿态变化量、
Figure BDA00039874083100001212
为t时刻加速度计的输出值、bat为加速度计的零漂、na为加速度计的高斯白噪声、
Figure BDA00039874083100001213
为角速度计的输出值、bwt为角速度计的零漂、nw为角速度计的高斯白噪声、
Figure BDA00039874083100001214
为四元数乘法。
毫米波雷达测速因子：
Figure BDA0003987408310000131
Figure BDA0003987408310000132
其中，
Figure BDA0003987408310000133
为k时刻速度、
Figure BDA0003987408310000134
为k时刻测得的速度、
Figure BDA0003987408310000135
为k时刻的位置、vt为t时刻速度。
步骤7：根据距离和时间选择前端匹配的关键帧，在两个关键帧之间加入IMU预积分作为新的约束，重新优化关键帧位置。当优化后位置与低精度GPS的位姿误差高于一定阈值时，加入毫米波雷达测速因子，进行优化，消除其他传感器退化时产生的误差。
实施例二
如图2所示，本实施例的面向特征退化场景的的定位装置包括采集模块、标定模块、特征提取模块、观测模型模块、特征匹配模块和优化模块。其中：
数据采集模块，用于采集无人驾驶车辆行驶时激光雷达传感器、毫米波雷达传感器、组合惯导传感器等多传感器数据，采集过程需要采集无人驾驶车辆正常行驶的完整路径周围环境信息。
时间同步模块，用于标定多传感器之间的安装参数，并将多传感器数据统一到组合惯导坐标系中，同时对采集到的数据，以激光雷达数据的时间戳为基准进行时间同步。
特征提取模块，用于提取实时帧激光雷达点云特征，通过用KDtree树快速查找实时帧点云每一个点的最近邻一定区域的所有点云。依据找出的周围点通过最小二乘法进行平面拟合，然后根据平面参数求解出该点曲率，选择曲率突变点作为特征点。
特征匹配模块，用于将特征点和地图进行匹配。根据特征点和激光雷达的距离信息，确定特征点的可信度，并改进ICP匹配的优化函数。基于先验GPS信息，将特征点和地图点云进行匹配。
运动约束模块，用于构建IMU预积分因子和毫米波雷达测速因子。
基于IMU数据对激光雷达点云的关键帧之间构建观测模型，同时利用毫米波雷达的测速能力构建运动方程，如下所示：
Figure BDA0003987408310000141
Figure BDA0003987408310000142
Figure BDA0003987408310000143
式中，t k为k时刻、
Figure BDA0003987408310000144
为位置预积分量、
Figure BDA0003987408310000145
为速度预积分量，
Figure BDA0003987408310000146
为姿态预积分量、
Figure BDA0003987408310000147
为t时刻相对于i时刻的姿态变化量、
Figure BDA0003987408310000148
为t时刻加速度计的输出值、bat为加速度计的零漂、na为加速度计的高斯白噪声、
Figure BDA0003987408310000149
为角速度计的输出值、bwt为角速度计的零漂、nw为角速度计的高斯白噪声、
Figure BDA00039874083100001410
为四元数乘法。
毫米波雷达测速：
Figure BDA00039874083100001411
Figure BDA00039874083100001412
后端优化模块，用于将多传感器信息进行融合。根据距离和时间选择前端匹配的关键帧，在两个关键帧之间加入IMU预积分作为新的约束，重新优化关键帧位置。当优化后位置与低精度GPS的位姿误差高于一定阈值时，加入毫米波雷达测速因子，进行优化，消除其他传感器退化时产生的误差。
在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (10)

1.特征退化场景的定位方法，其特征在于，包括如下步骤：
S1：当无人驾驶车辆处于行驶状态时，采集所述车辆行驶时的传感器数据，包括：激光雷达点云数据，组合导航GNSS数据，惯性导航IMU数据，毫米波雷达点云数据；
S2：根据所述激光雷达点云数据，对其它所述传感器数据进行时间同步，以获得同步后的传感器数据；
S3：根据同步后的所述激光雷达点云数据和所述GNSS数据构建点云地图；
S4：对同步后的所述激光雷达点云数据提取特征，并根据与激光雷达的距离对所述特征添加可信度；将所述特征与点云地图进行匹配，根据所述特征的可信度赋予匹配权重，得到匹配位姿；
S5：基于同步后的所述IMU数据和所述毫米波雷达点云数据建立关键帧之间的IMU预积分因子和毫米波速度预积分因子；
S6：将所述匹配位姿、所述IMU预积分因子和所述毫米波测速因子进行融合优化，以构建因子图，并进行优化得到定位信息。
2.根据权利要求1所述的特征退化场景的定位方法，其特征在于，所述S4的步骤具体为：
S401，计算实时帧中所述点云数据的每个点的曲率，将所述曲率的突变点作为特征；
S402，采用ICP匹配进行匹配，对于匹配优化函数中加入可信度，以使可信度大的特征点先与地图匹配。
3.根据权利要求2所述的特征退化场景的定位方法，其特征在于，所述S401中所述计算的步骤具体为：
S4011，利用kd-tree寻找实时帧中所述激光雷达点云数据中每个点周围的多个最近邻点，并根据所述最近邻点计算该点曲率；
S4012，选择曲率突变点作为所述特征，并保留所述特征与激光雷达的距离信息作为可信度依据。
4.根据权利要求3所述的特征退化场景的定位方法，其特征在于，所述的计算所述最近邻点的曲率，采用下述公式：
Figure FDA0003987408300000021
L＝rxxn,M＝rxyn,N＝ryyn,E＝rxrx,F＝rxry,G＝ryry；
其中，H为曲率，n为该点的单位法向量，rx、ry、rxx、rxy、ryy为偏微分。
5.根据权利要求3所述的特征退化场景的定位方法，其特征在于，所述突变点包括：纵向凹凸点、横向凹凸点和孤立特征点；
其中，所述纵向凹凸点为在X方向曲率发送较大突变的最近邻点，所述横向凹凸点为在Y方向曲率发生突变的最近邻点，所述孤立特征点为在X和Y方向曲率都发送突变的最近邻点。
6.根据权利要求1所述的特征退化场景的定位方法，其特征在于，所述S5的步骤具体为：
S501，基于IMU数据构建两个关键帧之间的惯性预积分因子；
S502，基于毫米波雷达测速算法，对自身速度进行计算，构建两个关键帧之间的毫米波雷达测速因子。
7.根据权利要求6所述的特征退化场景的定位方法，其特征在于，所述S6的步骤具体为：
S601，根据所述匹配位姿、所述惯性预积分因子和所述毫米波雷达测速因子的观测量构建因子图；
S602，构建优化目标模型，并通过所述因子图得到优化后的定位信息。
8.特征退化场景的定位系统，其特征在于，包括：
数据采集模块，用于采集自动驾驶车辆的多传感器数据；
时间同步模块，用于将所述多传感器数据进行时间同步；
地图构建模块，用于基于激光雷达点云数据和GNSS数据进行点云地图构建，其中所述GNSS数据包含RTK定位信息；
特征提取模块，用于将实时帧点云数据进行特征点提取，并计算特征点的可靠性；
特征匹配模块用于将提取的特征点和地图点云进行匹配，同时依据距离信息建立关键帧点云；
运动约束模块，用于基于匹配结果和观测因子，根据不同情况对多传感器信息进行融合，求解最优定位参数；
后端优化模块，用于将多传感器信息进行融合；
其中，所述的系统用于实施如权利要求1-7中任一项所述的方法。
9.电子设备，其特征在于，包括：
存储器，储存有程序或指令；
处理器，所述处理器执行所述程序或指令，以实现如权利要求1-7中任一项所述的方法。
10.可读存介质，其特征在于，所述可读存介质上存储有程序或指令，所述程序或指令被所述处理器执行，以实现如权利要求1-7中任一项所述的方法。
CN202211569480.4A 2022-12-08 2022-12-08 特征退化场景的定位方法、系统、电子设备和可读存介质 Pending CN115930977A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211569480.4A CN115930977A (zh) 2022-12-08 2022-12-08 特征退化场景的定位方法、系统、电子设备和可读存介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211569480.4A CN115930977A (zh) 2022-12-08 2022-12-08 特征退化场景的定位方法、系统、电子设备和可读存介质

Publications (1)

Publication Number Publication Date
CN115930977A true CN115930977A (zh) 2023-04-07

Family

ID=86700321

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211569480.4A Pending CN115930977A (zh) 2022-12-08 2022-12-08 特征退化场景的定位方法、系统、电子设备和可读存介质

Country Status (1)

Country Link
CN (1) CN115930977A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116518992A (zh) * 2023-04-14 2023-08-01 之江实验室 一种退化场景下的无人车定位方法和装置
CN117451033A (zh) * 2023-12-21 2024-01-26 广东石油化工学院 一种同步定位与地图构建方法、装置、终端及介质

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116518992A (zh) * 2023-04-14 2023-08-01 之江实验室 一种退化场景下的无人车定位方法和装置
CN116518992B (zh) * 2023-04-14 2023-09-08 之江实验室 一种退化场景下的无人车定位方法和装置
CN117451033A (zh) * 2023-12-21 2024-01-26 广东石油化工学院 一种同步定位与地图构建方法、装置、终端及介质

Similar Documents

Publication Publication Date Title
CN112484725B (zh) 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
Chiang et al. Performance enhancement of INS/GNSS/refreshed-SLAM integration for acceptable lane-level navigation accuracy
CN110411462B (zh) 一种gnss/惯导/车道线约束/里程计多源融合方法
Tardif et al. A new approach to vision-aided inertial navigation
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN112639502A (zh) 机器人位姿估计
CN115930977A (zh) 特征退化场景的定位方法、系统、电子设备和可读存介质
Jean et al. Accurate vehicle positioning on a numerical map
CN113654555A (zh) 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
Cai et al. Mobile robot localization using gps, imu and visual odometry
Zheng et al. An optimization-based UWB-IMU fusion framework for UGV
CN115407357A (zh) 基于大场景的低线束激光雷达-imu-rtk定位建图算法
CN115371665A (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN115183762A (zh) 一种机场仓库内外建图方法、系统、电子设备及介质
CN112923934A (zh) 一种适用于非结构化场景中结合惯导的激光slam技术
CN115930948A (zh) 一种果园机器人融合定位方法
CN116380070A (zh) 一种基于时间戳优化的视觉惯性定位方法
CN115344033A (zh) 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法
CN110849349A (zh) 一种基于磁传感器与轮式里程计融合定位方法
Hu et al. Efficient Visual-Inertial navigation with point-plane map
CN114440892B (zh) 一种基于拓扑地图和里程计的自定位方法
CN116222556B (zh) 一种基于多源传感器融合的室内定位方法及系统
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法

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