CN111402339A - 一种实时定位方法、装置、系统及存储介质 - Google Patents

一种实时定位方法、装置、系统及存储介质 Download PDF

Info

Publication number
CN111402339A
CN111402339A CN202010482416.7A CN202010482416A CN111402339A CN 111402339 A CN111402339 A CN 111402339A CN 202010482416 A CN202010482416 A CN 202010482416A CN 111402339 A CN111402339 A CN 111402339A
Authority
CN
China
Prior art keywords
laser point
point cloud
cloud data
frame
frames
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
CN202010482416.7A
Other languages
English (en)
Other versions
CN111402339B (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.)
Shenzhen Smart Mapping Tech Co ltd
Original Assignee
Shenzhen Smart Mapping Tech 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 Shenzhen Smart Mapping Tech Co ltd filed Critical Shenzhen Smart Mapping Tech Co ltd
Priority to CN202010482416.7A priority Critical patent/CN111402339B/zh
Publication of CN111402339A publication Critical patent/CN111402339A/zh
Application granted granted Critical
Publication of CN111402339B publication Critical patent/CN111402339B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • 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

Abstract

本发明实施例涉及一种实时定位方法、装置、系统及存储介质，该方法包括：获取惯性数据集合以及第一激光点云数据集合；对第一激光点云数据集合提取特征点，获取第二激光点云数据集合；根据第二激光点云数据集合中前N帧激光点云数据的特征点之间的结构位置关系、首帧惯性数据、相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量；将前N帧作为关键帧，对当前滑动窗口中的关键帧进行迭代更新；对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，获取优化后的状态量；将优化后的状态量与惯性数据集合进行融合，获取最终位姿量，并完成实时定位。

Description

一种实时定位方法、装置、系统及存储介质
技术领域
本发明实施例涉及计算机技术领域，尤其涉及一种实时定位方法、装置、系统及存储介质。
背景技术
在智能无人化系统中，定位作为系统的基础模块，负责提供智能无人系统的实时位置信息，在系统中缺一不可。在一些应用如室内服务机器人、高压电场巡检机器人、园区物流机器人等中，场景的外部环境复杂多样，有源定位的方式如GNSS（Global NavigationSatellite System）系统等存在由于遮挡所造成的信号不稳定，甚至定位失败的情况，而基于WIFI、蓝牙、UWB（Ultra Wideband）等有源定位方式则存在架设信号发射装置密集且昂贵、大场景下无法实施的问题，这些有源定位的缺点制约了场景的多样性应用。
无源定位包括惯性导航和同步定位与地图构建（Simultaneous localizationand mapping，简称SLAM ）等，希望机器人从未知环境的未知地点出发，在运动过程中通过重复观测到的地图特征（比如，墙角，柱子等）定位自身位置和姿态，再根据自身位置增量式的构建地图，从而达到同时定位和地图构建的目的，实现增强定位。惯性导航依赖高精度惯性器材进行自身姿态推导结算，需要依赖高精度微机电系统（Microelectro MechanicalSystems，简称MEMS）传感器，价格可至几十万甚至上百万，制约了该方法的商业化应用；而SLAM增强定位可只依赖价格低廉、市场上常见的传感器如消费级惯性测量单元（Inertialmeasurement unit，简称IMU）、相机、激光雷达等，通过传感器自身的感知组件获取外部环境信息对自身的姿态进行推导计算进行获取，是目前市面上已工业化的无源定位方法。在SLAM增强定位中，不同传感器的融合可以有效克服不同传感器的缺点，并充分利用不同传感器的优点。
近年来，国内外学者对SLAM增强定位进行了广泛的研究。有基于相机的SLAM方法如双目SLAM、视觉惯性里程计VIO等，有基于激光雷达的方法如三维激光SLAM等。在这些方法中，基于视觉的方法容易受相机传感器本身的缺点的影响，无法在室外多时空的场景下进行有效应用，如在晚上，相机作为被动光源传感器则无法感知环境，整个系统失效，但无人智能系统在很多应用中就要求24小时作业，无法满足要求；三维激光SLAM中，由于三维激光雷达的频率普遍比较低，一般为10HZ，而目前市面上比较常见的16线激光雷达垂直视场角仅为30度，无法应对系统短时间的剧烈运动，在一些结构比较简单的场景里由于获取的三维激光点云数据特征少，也容易出现定位丢失的情况。
那么，如何才能提供一种定位方案，能够尽量实现在高频运动、结构较少甚至缺失的室内外场景下的高精度定位成为本申请亟待解决的技术问题。
发明内容
鉴于此，为解决现有技术中上述技术问题，本发明实施例提供一种实时定位方法、装置、系统及存储介质。
第一方面，本发明实施例提供一种实时定位方法，该方法包括：
获取惯性数据集合以及第一激光点云数据集合；
对第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合；
根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量；
将前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新；
按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，直至第二激光点云数据集合中每一帧激光点云数据均在滑动窗口滑动过程中完成局部优化，获取与每一帧激光点云数据分别对应的优化后的状态量；
将与每一帧激光点云数据分别对应的优化后的状态量，与惯性数据集合进行融合，获取最终位姿量；
根据最终位姿量，完成实时定位。
在一个可能的实施方式中，对第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合，具体包括：
从第一激光点云数据集合中提取与第一激光点同属同一环数，且与第一激光点的最近邻的预设数量激光点，与第一激光点共同构成局部点集合；
根据局部点集合中每一个激光点，计算第一激光点的曲率；
当确定第一激光点的曲率大于预设阈值时，确定第一激光点为角点；否则，确定第一激光点为平面点；
根据所有角点构成第一角点集合，根据所有平面点，构成第一平面点集合；
第一角点集合和第一平面点集合共同构成第二激光点云数据集合，其中第一激光点为激光点云数据中任一帧激光点云数据上的一个激光点。
在一个可能的实施方式中，根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量，具体包括：
根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系，确定前N帧激光点云数据中每一帧激光点云数据的初始位姿量；
根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
在一个可能的实施方式中，根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系，确定前N帧激光点云数据中每一帧激光点云数据的初始位姿量，具体包括：
将前N帧激光点云数据中除第一帧激光点云数据以外的其他N-1帧激光点云数据中包含的所有平面点构成第二平面点集合，所有角点构成第二角点集合；
在第二平面点集合中寻找与第三激光点满足面状特征的预设个数的第一类最近邻点；获取第三激光点分别与第一类最近邻点中每两点之间的第一组匹配关系；
在第二角点集合中寻找与第四激光点满足线状特征的预设个数的第二类最近邻点；获取第四激光点分别与第二类最近邻点中每两点之间的第二组匹配关系；
根据第一组匹配关系，获取点到面距离残差；
根据第二组匹配关系，获取点到线距离残差；
根据点到面距离残差和点到线距离残差，构建第一代价函数；
对第一代价函数进行一次优化求解，获取第一帧激光点云数据对应的初始位姿量，其中第一帧激光点云数据为前N帧激光点云数据中的任一帧激光点云数据。
在一个可能的实施方式中，根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量，具体包括：
根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量，前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系；
根据惯性数据集合中首帧惯性数据、惯性数据与激光点云数据之间的转换关系，确定首帧激光点云数据对应的状态量；
根据首帧激光点云数据对应的状态量，以及前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
在一个可能的实施方式中，将前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新，具体包括：
获取与当前滑动窗口最邻近的，滑动窗口以外的第二帧激光点云数据；
并确定与第二帧激光点云数据最邻近的关键帧；
根据第二帧激光点云数据对应的状态量，与第二帧激光点云数据最邻近的关键帧对应的状态量，确定第二帧激光点云数据是否为关键帧；
当第二帧激光点云数据为关键帧时，删除当前滑动窗口中最旧的关键帧，将第二帧激光点云数据加入到当前滑动窗口中，作为最新的一帧关键帧；
将加入第二帧激光点云数据的滑动窗口按照预设步长滑动，并对新生成的滑动窗口中的关键帧进行迭代更新；
或者，当第二帧激光点云数据并非为关键帧时，舍弃第二帧激光点云数据，按照预设步长滑动滑动窗口，并继续对初始滑动窗口中的关键帧进行迭代更新。
在一个可能的实施方式中，按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，具体包括：
以当前经过更新后的滑动窗口中每一个关键帧对应的状态量，以及每一个关键帧中所包含的平面特征点，构建以预设关键帧为参考的基准坐标系；
将除预设关键帧之外的每一个关键帧所包含的平面特征点对应的第一类位置数据均转换为基准坐标系下的第二类位置数据；
根据第二类位置数据以及基准坐标系中的极坐标，对每一个关键帧中的平面特征点进行筛选；
获取筛选特征点之后N-1关键帧中的第一关键帧，与相邻的第二关键帧之间的第一相对位姿关系；并根据第一相对位姿关系，生成第一残差方程；
获取第一关键帧与预设关键帧之间的第二相对位姿关系；并根据第二相对位姿关系，生成第二残差方程；
获取当前滑动窗口最近已删除的第三关键帧的先验信息；
根据第一残差方程、第二残差方程以及先验信息，构成联合优化模型代价函数；
对联合优化模型代价函数进行一次优化，以此完成对当前滑动窗口中的关键帧对应的状态量进行一次优化，获取当前滑动窗口中每一帧关键帧的最优状态量，其中，第一关键帧为经过筛选特征点之后N-1关键帧中的任一关键帧。
在一个可能的实施方式中，将与每一帧激光点云数据分别对应的优化后的状态量，与惯性数据集合进行融合，获取最终位姿量之前，方法还包括：
对激光点云数据中每一帧激光点云数据对应的优化后的状态量进行二次优化，以便根据经过二次优化后的状态量和惯性数据进行融合，获取最终位姿量。
第二方面，本发明实施例提供一种实时定位装置，该装置包括：
获取单元，用于获取惯性数据集合以及第一激光点云数据集合；
特征提取单元，用于对第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合；
处理单元，用于根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量；
迭代更新单元，用于将前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新；
优化单元，用于按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，直至第二激光点云数据集合中每一帧激光点云数据均在滑动窗口滑动过程中完成局部优化，获取与每一帧激光点云数据分别对应的优化后的状态量；
融合单元，用于将与每一帧激光点云数据分别对应的优化后的状态量，与惯性数据集合进行融合，获取最终位姿量；
定位单元，用于根据最终位姿量，完成实时定位。
第三方面，本发明实施例提供一种实时定位系统，该系统包括：至少一个处理器和存储器；
处理器用于执行存储器中存储的实时定位程序，以实现如第一方面任一实施方式所介绍的实时定位方法。
第四方面，本发明实施例提供一种计算机存储介质，该计算机存储介质存储有一个或者多个程序，一个或者多个程序可被如第三方面所介绍的实时定位系统执行，以实现如第一方面任一实施方式所介绍的实时定位方法。
本发明实施例提供的一种实时定位方法，获取惯性数据集合和第一激光点云数据集合。然后对第一激光点云数据进行特征点提取，获取第二激光点云数据集合。根据第二激光点云数据中的前N帧数据的特征点之间的结构位置关系、惯性数据中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。然后将前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新。按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，直至第二激光点云数据集合中每一帧激光点云数据均在滑动窗口滑动过程中完成局部优化，获得经过优化后的状态量。将经过优化后的状态量和惯性数据集合进行融合，获取最终位姿量，并根据最终位姿量完成实时定位。
通过该种方式，结合了惯性数据的高频特定和积分特性，以及激光点云数据的离散特性，在原始状态量的层面将两者进行融合，充分利用了这两类数据优势，可以实现在高速运动、结构少的场景，甚至缺失的室内外场景下的高精度定位，并且在长时间里保持高精度的状态最优估计，有效地提高了整体的定位精度，还能够保证在不同场景下具有很好的稳定性和鲁棒性。
附图说明
图1为本发明实施例提供的一种实时定位方法流程示意图；
图2为本发明提供的IMU数据和激光雷达数据之间的对应关系示意图；
图3为本发明提供的滑动窗口中包含N帧关键帧的结构示意图；
图4为本发明提供的筛选激光雷达点云数据中特征点的原理示意图；
图5为本发明提供的实时定位方法流程简易框图；
图6为本发明实施例提供的一种实时定位装置结构示意图；
图7为本发明实施例提供一种实时定位系统结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为便于对本发明实施例的理解，下面将结合附图以具体实施例做进一步的解释说明，实施例并不构成对本发明实施例的限定。
图1为本发明实施例提供的一种实时定位方法流程示意图，如图1所示，该方法包括：
步骤110，获取惯性数据集合以及第一激光点云数据集合。
具体的，可以采用惯性测量单元采集惯性数据，构成惯性数据集合。例如采用六轴惯性测量单元采集惯性数据。
采用激光雷达采集激光点云数据，构成第一激光点云数据集合。具体的，可以采用多线激光雷达如16线激光雷达采集激光点云数据。
步骤120，对第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合。
具体的，第一激光点云数据（即原始激光点云数据）为一般三维空间点云，是外部环境在三维空间的离散取样，一方面单帧点云数据量比较大，另一方面数据是离散的，无法反应空间的结构化信息，顾需对原始单帧激光点云数据进行特征提取，以减小计算量，满足实时性要求，同时对结构化信息进行特征表达。
可选的，在具体进行特征点提取时，可以采用如下方式：
从第一激光点云数据集合中提取与第一激光点同属同一环数，且与第一激光点的最近邻的预设数量激光点，与第一激光点共同构成局部点集合；
根据局部点集合中每一个激光点，计算第一激光点的曲率；
当确定第一激光点的曲率大于预设阈值时，确定第一激光点为角点；否则，确定第一激光点为平面点；
根据所有角点构成第一角点集合，根据所有平面点，构成第一平面点集合；
第一角点集合和第一平面点集合共同构成第二激光点云数据集合，其中第一激光点为激光点云数据中任一帧激光点云数据上的一个激光点。
在一个具体的例子中，设第一激光点云数据集合为P，
Figure 955613DEST_PATH_IMAGE001
为第一激光点云数据集合 P中的任一个激光点，记为第一激光点。取
Figure 659127DEST_PATH_IMAGE001
同一环数最近临的K个激光点组成的局部点集 合为S。集合S中的点
Figure 751848DEST_PATH_IMAGE001
的曲率可以采用如下公式计算获取：
Figure 224418DEST_PATH_IMAGE002
（公式1）
取经验值
Figure 869026DEST_PATH_IMAGE003
，当
Figure 935202DEST_PATH_IMAGE004
大于
Figure 690668DEST_PATH_IMAGE003
时，将
Figure 545973DEST_PATH_IMAGE001
标记为角点，否则将
Figure 33586DEST_PATH_IMAGE001
标记为平面点，其中
Figure 711692DEST_PATH_IMAGE005
为集 合S中除
Figure 146216DEST_PATH_IMAGE001
以外的激光点云数据。通过上述方式，获取第一激光点云数据集合中的所有角点 构成第一角点集合，获取第一激光点云数据集合中的所有平面点构成第一平面点集合。第 一角点集合和第一平面点集合共同构成第二激光点云数据集合。
步骤130，根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
可选的，在执行时，可以首先根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系，确定前N帧激光点云数据中每一帧激光点云数据的初始位姿量；
然后，根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
进一步可选的，在确定前N帧激光点云数据中每一帧激光点云数据的初始位姿量时，首先将前N帧激光点云数据中除第一帧激光点云数据以外的其他N-1帧激光点云数据中包含的所有平面点构成第二平面点集合，所有角点构成第二角点集合；
在第二平面点集合中寻找与第三激光点满足面状特征的预设个数的第一类最近邻点；获取第三激光点分别与第一类最近邻点中每两点之间的第一组匹配关系；
在第二角点集合中寻找与第四激光点满足线状特征的预设个数的第二类最近邻点；获取第四激光点分别与第二类最近邻点中每两点之间的第二组匹配关系；
根据第一组匹配关系，获取点到面距离残差；
根据第二组匹配关系，获取点到线距离残差；
根据点到面距离残差和点到线距离残差，构建第一代价函数；
对第一代价函数进行一次优化求解，获取第一帧激光点云数据对应的初始位姿量，其中第一帧激光点云数据为前N帧激光点云数据中的任一帧激光点云数据。
在一个具体的例子中，可以设置第一帧激光点云数据对应的状态量为
Figure 858957DEST_PATH_IMAGE006
。设滑动窗口中当前共具有N帧激光点云数据，假 设第一帧激光点云数据为第N帧激光点云数据。那么，取前N-1帧组成一个局部地图，该局部 地图中角点组成第二角点集合为
Figure 452225DEST_PATH_IMAGE007
，第二平面点集合为
Figure 883206DEST_PATH_IMAGE008
，设第N帧激光点云数据中的角 点集合和平面点集合分别为
Figure 714896DEST_PATH_IMAGE009
和
Figure 157510DEST_PATH_IMAGE010
，对于集合
Figure 49242DEST_PATH_IMAGE009
中的第四激光点
Figure 577307DEST_PATH_IMAGE011
在
Figure 478266DEST_PATH_IMAGE007
中寻找最近邻
Figure 40966DEST_PATH_IMAGE012
个点，如果该
Figure 369179DEST_PATH_IMAGE012
个点符合线状特征则取线段中任意两个点
Figure 384540DEST_PATH_IMAGE013
，
Figure 558032DEST_PATH_IMAGE014
组成点到线关系
Figure 834293DEST_PATH_IMAGE015
，通过该种方式可以获取第二组匹配关系。同理，在集合
Figure 67828DEST_PATH_IMAGE010
中的第三激光 点
Figure 304905DEST_PATH_IMAGE016
可在
Figure 547668DEST_PATH_IMAGE008
中寻找满足面状特征的
Figure 944014DEST_PATH_IMAGE017
个最邻近点，组成点到面的关系
Figure 223817DEST_PATH_IMAGE018
，
Figure 807245DEST_PATH_IMAGE019
为空间平面方程参数即
Figure 588119DEST_PATH_IMAGE020
，通过该种方式可以获取第一组匹配关 系。将第一组匹配关系个数定义为
Figure 838972DEST_PATH_IMAGE021
个。第二组匹配关系个数定义为
Figure 289676DEST_PATH_IMAGE022
个。
由点到面的关系，即第一组匹配关系，可组成点到面的距离残差
Figure 360400DEST_PATH_IMAGE023
；
由点到线的关系，即第二组匹配关系，可组成点到线的距离残差
Figure 679386DEST_PATH_IMAGE024
，分别参见公式2 和公式3：
Figure 784745DEST_PATH_IMAGE025
（公式2）
Figure 239358DEST_PATH_IMAGE026
（公式3）
其中，
Figure 62958DEST_PATH_IMAGE027
为
Figure 920055DEST_PATH_IMAGE028
所对应的旋转矩阵。利用残差构建ICP匹配代价函数
Figure 286446DEST_PATH_IMAGE029
，然 后通过最小二乘原理求解该代价函数即可对第N帧的位置和姿态
Figure 203586DEST_PATH_IMAGE030
进行优化求解， 在本实施例中，将
Figure 248903DEST_PATH_IMAGE030
定义为初始位姿量。
Figure 909691DEST_PATH_IMAGE031
（公式4）
其中，
Figure 865009DEST_PATH_IMAGE032
为鲁棒核函数。
在获取初始位姿量后，可以根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
具体实现方式可以参见如下方案：
首先，根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量，前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系。
然后，根据惯性数据集合中首帧惯性数据、惯性数据与激光点云数据之间的转换关系，确定首帧激光点云数据对应的状态量；
根据首帧激光点云数据对应的状态量，以及前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
具体的，惯性测量单元IMU具有高频的惯性信号输出，一般能达到200Hz甚至更高的频率。由于IMU的频率大于激光雷达，在激光雷达帧与帧，关键帧与关键帧之间可通过IMU数据的预积分获取状态量之间的关系如图2所示，IMU的频率一般超过200HZ，而激光雷达的频率一般为10HZ，在连续两帧LiDAR之间具备多个IMU数据，可利用该间隔时间段内的IMU数据进行位置、速度、姿态积分获取两帧雷达数据之间的位置、速度和姿态相对量，如第i时刻和第j时刻之间具有多帧IMU数据，利用这些IMU数据可进行预积分获得两个时刻之间状态量的关系，也即是相对位姿关系。
相对位姿关系的公式可以参见如下：
Figure 953050DEST_PATH_IMAGE033
（公式5）
Figure 485663DEST_PATH_IMAGE034
（公式6）
Figure 950142DEST_PATH_IMAGE035
（公式7）
其中，
Figure 759967DEST_PATH_IMAGE036
，
Figure 18910DEST_PATH_IMAGE037
以及
Figure 38818DEST_PATH_IMAGE038
分别为第j时刻的位置数据、速度数据以及旋转数据；
Figure 306988DEST_PATH_IMAGE039
、
Figure 236898DEST_PATH_IMAGE040
以及
Figure 666743DEST_PATH_IMAGE028
分别为第i时刻的位置数据、速度数据以及旋转数据。
Figure 173947DEST_PATH_IMAGE041
为第t时刻的加速度，
Figure 855596DEST_PATH_IMAGE042
为重力加速度，
Figure 499067DEST_PATH_IMAGE043
为加速度计偏置，
Figure 365391DEST_PATH_IMAGE044
为陀螺仪偏置。
在公式5-7中存在几个比较关键的未知参数，
Figure 94313DEST_PATH_IMAGE042
为重力加速度，
Figure 579652DEST_PATH_IMAGE043
为加速度计偏 置，
Figure 343209DEST_PATH_IMAGE044
为陀螺仪偏置。如果不确定这几个参数的情况下，即使知道第i时刻的激光点云数据 的状态量，也不能够求取第j时刻的激光点云数据的状态量。因此需要求取重力加速度、加 速度计偏置和陀螺仪偏置等参数。具体求取方式参见如下：
由公式7对第
Figure 380435DEST_PATH_IMAGE045
帧和第
Figure 7770DEST_PATH_IMAGE046
帧进行IMU积分计算可得两帧之间的旋转相对量
Figure 968904DEST_PATH_IMAGE047
， 采用上文中介绍获取初始位姿量的方式，可以获取第
Figure 233574DEST_PATH_IMAGE045
帧和第
Figure 848226DEST_PATH_IMAGE046
帧对应的旋转量分别 为
Figure 489423DEST_PATH_IMAGE048
和
Figure 441198DEST_PATH_IMAGE049
，则这三个量满足一下关系：
Figure 54713DEST_PATH_IMAGE050
（公式8）
设陀螺仪偏置变化量为
Figure 433742DEST_PATH_IMAGE051
，有：
Figure 624552DEST_PATH_IMAGE052
（公式9）
其中
Figure 114439DEST_PATH_IMAGE053
为
Figure 582461DEST_PATH_IMAGE047
对
Figure 866811DEST_PATH_IMAGE044
的雅克比矩阵。对公式9取虚部进行最小二乘可得：
Figure 810497DEST_PATH_IMAGE054
（公式10）
求解以上方程获取偏置变化量为
Figure 976511DEST_PATH_IMAGE051
，则滑动窗口中第
Figure 423673DEST_PATH_IMAGE045
帧对应的偏置为：
Figure 144504DEST_PATH_IMAGE055
（公式11）
同理，利用预积分量和已求解的位置和位姿量可求得加速度偏置和重力加速度。
通过上文方式可以获取第i时刻对应的点云数据的初始位姿量，例如第i时刻为初 始时刻。即，首先获取到初始时刻的激光点云数据的初始位姿量。初始时刻激光点云数据和 初始时刻的惯性数据之间存在一定的转换关系。根据这个转换关系和初始时刻的惯性数 据，也即是首帧惯性数据，可以得到初始时刻的激光点云数据。然后，将初始时刻的激光点 云数据代入到公式5-7中，在
Figure 44327DEST_PATH_IMAGE042
为重力加速度，
Figure 16963DEST_PATH_IMAGE043
为加速度计偏置，
Figure 849789DEST_PATH_IMAGE044
为陀螺仪偏置均为 已知的情况下，就可以获知下一时刻的激光点云数据的状态量。即，获取第j时刻的激光点 云数据状态量。在获知第j时刻的激光点云数据对应的状态量后，代入公式5-7，自然可以获 知第j+1时刻的激光点云数据的状态量，通过该种方式，就可以确定述前N帧激光点云数据 中每一帧激光点云数据的状态量。
步骤140，将前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新。
具体的，在通过步骤130获取每一帧激光点云数据对应的状态量后，可以将这N帧激光点云数据均作为初始滑动窗口中的关键帧。然后，获取与当前滑动窗口最邻近的，滑动窗口以外的第二帧激光点云数据；并确定与第二帧激光点云数据最邻近的关键帧；根据第二帧激光点云数据对应的状态量，与第二帧激光点云数据最邻近的关键帧对应的状态量，确定第二帧激光点云数据是否为关键帧；当第二帧激光点云数据为关键帧时，删除当前滑动窗口中最旧的关键帧，将第二帧激光点云数据加入到当前滑动窗口中，作为最新的一帧关键帧；将加入第二帧激光点云数据的滑动窗口按照预设步长滑动，并对新生成的滑动窗口中的关键帧进行迭代更新；或者，当第二帧激光点云数据并非为关键帧时，舍弃第二帧激光点云数据，按照预设步长滑动滑动窗口，并继续对初始滑动窗口中的关键帧进行迭代更新。
在一个具体的例子中，具体参见图3所示，设滑动窗口中的关键帧就是第一帧（
Figure 882467DEST_PATH_IMAGE056
） 到第N帧（
Figure 535166DEST_PATH_IMAGE057
）。那么，距离滑动窗口最近的，滑动窗口以外的第激光点云数据，也即是第N +1帧激光点云数据（
Figure 45913DEST_PATH_IMAGE058
）。设与
Figure 733246DEST_PATH_IMAGE058
最邻近的关键帧是
Figure 795880DEST_PATH_IMAGE059
，
Figure 545661DEST_PATH_IMAGE059
从滑动窗口中选取。
Figure 719153DEST_PATH_IMAGE058
对 应的状态量以及
Figure 995414DEST_PATH_IMAGE059
的对应状态量均可以通过步骤130获取。假设
Figure 228949DEST_PATH_IMAGE059
对应的状态量为
Figure 466027DEST_PATH_IMAGE060
，
Figure 443210DEST_PATH_IMAGE058
对应的状态量
Figure 573977DEST_PATH_IMAGE061
。
那么，可以通过如下条件判断
Figure 978413DEST_PATH_IMAGE058
是否为关键帧：
当
Figure 702787DEST_PATH_IMAGE062
或
Figure 483661DEST_PATH_IMAGE063
时，
Figure 468935DEST_PATH_IMAGE058
为关键帧，否 则，
Figure 44273DEST_PATH_IMAGE058
并非为关键帧。其中，
Figure 114997DEST_PATH_IMAGE064
为距离阈值，
Figure 309349DEST_PATH_IMAGE065
为角度阈值，
Figure 414708DEST_PATH_IMAGE066
为
Figure 160947DEST_PATH_IMAGE059
与
Figure 984547DEST_PATH_IMAGE058
的旋转相对量。
当确定
Figure 714081DEST_PATH_IMAGE058
为关键帧时，则需要删除当前滑动窗口中最旧的关键帧，例如当前滑动 窗口中的
Figure 673947DEST_PATH_IMAGE056
，然后将
Figure 856666DEST_PATH_IMAGE058
加入到当前的滑动窗口中。如果，确定
Figure 901983DEST_PATH_IMAGE058
并非为关键帧，则可以舍 弃
Figure 438137DEST_PATH_IMAGE058
。不管是上述哪种情况，都可以在执行上述操作后，哪找预设滑动步长滑动当前的滑 动窗口，然后继续对当前滑动窗口中的关键帧进行迭代更新。
进一步的，在每更新一次滑动窗口后，还需要对滑动窗口中的关键帧对应的状态量进行一次优化，具体参见步骤150。
步骤150，按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，直至第二激光点云数据集合中每一帧激光点云数据均在滑动窗口滑动过程中完成局部优化，获取与每一帧激光点云数据分别对应的优化后的状态量。
具体的，通过如下方式实现一次优化：
以当前经过更新后的滑动窗口中每一个关键帧对应的状态量，以及每一个关键帧中所包含的平面特征点，构建以预设关键帧为参考的基准坐标系；
将除预设关键帧之外的每一个关键帧所包含的平面特征点对应的第一类位置数据均转换为基准坐标系下的第二类位置数据；
根据第二类位置数据以及基准坐标系中的极坐标，对每一个关键帧中的平面特征点进行筛选；
获取筛选特征点之后N-1关键帧中的第一关键帧，与相邻的第二关键帧之间的第一相对位姿关系；并根据第一相对位姿关系，生成第一残差方程；
获取第一关键帧与预设关键帧之间的第二相对位姿关系；并根据第二相对位姿关系，生成第二残差方程；
获取当前滑动窗口最近已删除的第三关键帧的先验信息；
根据第一残差方程、第二残差方程以及先验信息，构成联合优化模型代价函数；
对联合优化模型代价函数进行一次优化，以此完成对当前滑动窗口中的关键帧对应的状态量进行一次优化，获取当前滑动窗口中每一帧关键帧的最优状态量，其中，第一关键帧为经过筛选特征点之后N-1关键帧中的任一关键帧。
在一个具体的例子中，设帧
Figure 518089DEST_PATH_IMAGE067
的状态量为
Figure 606130DEST_PATH_IMAGE068
， 需将第0帧到第
Figure 873164DEST_PATH_IMAGE069
帧中平面特征点转换到以帧
Figure 337643DEST_PATH_IMAGE056
（预设关键帧）为参考的基准坐标系 下。设
Figure 147467DEST_PATH_IMAGE070
为帧
Figure 406410DEST_PATH_IMAGE071
则该点在该基准坐标系中的坐标为：
Figure 426319DEST_PATH_IMAGE072
(公式12)
然后，通过步骤130中所介绍的点到面对应关系的方式，在基准坐标系中所有平面特征 点中寻找与
Figure 428910DEST_PATH_IMAGE070
符合点到面的对应关系的点。然后，对基准坐标系中与
Figure 93241DEST_PATH_IMAGE070
符合点到面的对应 关系的点进行筛选。
由于激光雷达点云所获取的特征点比较散乱，容易使得特征点分布不均匀使得过于密集的特征点所形成的约束等效，造成计算冗余，顾需要对特征点进行筛选以减少特征点数目，加快后续的优化速度。
具体参见图4所示，以帧
Figure 788664DEST_PATH_IMAGE056
坐标系为基准坐标系，计算特征点
Figure 295869DEST_PATH_IMAGE073
相 对于该参考系下的极坐标
Figure 836572DEST_PATH_IMAGE074
以及与坐标原点的距离
Figure 965196DEST_PATH_IMAGE075
，由于三维激光雷达在平面视角 上是按照360度等距扫描，故将参考系所组成的极坐标进行8等切分，并按照距离
Figure 969536DEST_PATH_IMAGE076
、
Figure 229616DEST_PATH_IMAGE077
以及
Figure 714956DEST_PATH_IMAGE078
切分，共切分为24个区域，根据特征点的极坐标依次划分到每个 区域同时满足每个区域的点个数不大于
Figure 744091DEST_PATH_IMAGE079
。通过该种方式，实现对特征点进行筛选以 减少特征点数目。
在对除帧
Figure 656684DEST_PATH_IMAGE056
以外的N-1帧关键帧进行特征点筛选后，获取N-1关键帧中的第一关键 帧
Figure 872901DEST_PATH_IMAGE080
和相邻的第二关键帧
Figure 20986DEST_PATH_IMAGE081
(
Figure 639049DEST_PATH_IMAGE082
)之间的第一相对位置关系，包括位置、速 度和姿态等状态相对量。利用这些状态相对量可组成对于状态量
Figure 722543DEST_PATH_IMAGE083
与
Figure 426057DEST_PATH_IMAGE084
之间的第一 残差方程
Figure 846674DEST_PATH_IMAGE085
。
类似的道理，利用匹配关系的构建（即基本坐标系中各特征点之间的位置关系）可 获取滑动窗口中
Figure 584823DEST_PATH_IMAGE056
与帧
Figure 963851DEST_PATH_IMAGE081
之间的位置、姿态等状态相对量，利用状态相对量可组成对于状 态量
Figure 30027DEST_PATH_IMAGE086
与
Figure 519915DEST_PATH_IMAGE087
的第二残差方程
Figure 846991DEST_PATH_IMAGE088
。
此外，滑动窗口中已剔除的最新一帧的状态量作为已估计量不再更新，其与滑动 窗口中现有帧存在的约束关系不能直接忽略掉，而是需要作为先验信息通过边缘化操作加 入到联合优化模型中以提高模型的稳定性和精度，设构建的先验残差为
Figure 396921DEST_PATH_IMAGE089
，则联合优 化模型的代价函数为：
Figure 950393DEST_PATH_IMAGE090
（公式13）
通过最小二乘方法求解以上非线性优化问题可对滑动窗口中的所有状态进行最优估计，输出当前帧的状态量，包括位置和姿态。以此完成对当前滑动窗口中的关键帧对应的状态量进行一次优化，获取当前滑动窗口中每一帧关键帧的最优状态量。
通过上述步骤所获取的所有帧输出的姿态频率都是与三维激光雷达的频率一致的，一般多线三维激光雷达的输出频率为10Hz，但在一些无人智能系统的应用中，如高速驾驶的自动驾驶车，速度可达80Km/h，1秒的行驶时间可达20米以上，而10Hz的姿态输出使得位置更新的频率也只能维持在10Hz，无法满足高速应用场景下的需求，顾需要对输出的姿态进行高频输出。因此，还需要执行步骤160。
步骤160，将与每一帧激光点云数据分别对应的优化后的状态量，与惯性数据集合进行融合，获取最终位姿量。
具体的，可利用IMU的高频惯性数据，利用积分原理对当前获取的每一帧的姿态进 行积分以获取IMU频率下的姿态信息，即可利用公式5和公式7，类似求取
Figure 243971DEST_PATH_IMAGE036
和
Figure 691133DEST_PATH_IMAGE038
的方式 获取位置数据和旋转数据，然后以当前获取的位置数据和旋转数据作为最新姿态信息，当 下一激光帧的数据输出时，重复以上过程，即可获取高频的姿态输出信息。
通过以上步骤，利用三维激光雷达和惯性测量数据的特性，将两种数据在状态量层面进行融合，构建一个完整的非线性模型，并对该模型进行优化以获取最优的状态信息。
可选的，在上述步骤150中都是以滑动窗口形式对每一关键帧的状态量进行优化的。为了使得获取的位姿量更加优化，可以在步骤150的基础上，考虑从整体角度度对所有的关键帧进行二次优化。以使得获取的位姿量更加优化，然后再执行步骤160。具体的，进行二次优化的步骤参见步骤155。
步骤155，对激光点云数据中每一帧激光点云数据对应的优化后的状态量进行二次优化，以便根据经过二次优化后的状态量和惯性数据进行融合，获取最终位姿量。
在一个具体的例子中，可以利用利用关键帧构建局部地图，并对滑动窗口输出的当前帧（步骤150输出的当前帧）进行精匹配以获取当前帧中的点与局部地图之间的对应关系，通过该关系构建非线性优化模型对当前帧的位置位姿量进行优化以获取高精度位姿量。
具体的，局部地图中的点由关键帧中的特征点组成，关键帧判断原则与同上文步 骤130介绍的类似，这里不再过多赘述。在初始创建时，则将最开始
Figure 146385DEST_PATH_IMAGE091
个帧作为关键帧以维 持局部地图点的数量，当达到N个帧后，则判断当前待判定帧是否为关键帧，如果是则插入 该帧并删除最旧的一帧，否则不加入到局部地图中。
然后执行类似步骤130中的操作，通过选择当前帧中的特征点与局部地图中的点 寻找到点到线与点到面的对应关系，通过构建距离残差创建非线性优化模型对当前状态量 进行优化，获取最优化的位姿信息。设当前帧从滑动窗口中输出的状态量为
Figure 187153DEST_PATH_IMAGE092
，上一帧通过局部地图匹配优化的状态量为
Figure 18843DEST_PATH_IMAGE093
，上一帧从滑 动窗口中输出的状态量为
Figure 851670DEST_PATH_IMAGE094
，则当前帧提供给局部地图匹配所构建的非线性 模型的状态初值为
Figure 353190DEST_PATH_IMAGE095
：
Figure 5888DEST_PATH_IMAGE096
（公式14）
Figure 906848DEST_PATH_IMAGE097
（公式15）
其中
Figure 466618DEST_PATH_IMAGE098
为
Figure 529251DEST_PATH_IMAGE099
所对应的旋转矩阵。
即，通过公式14获取
Figure 279033DEST_PATH_IMAGE099
，通过公式15，求取
Figure 718104DEST_PATH_IMAGE100
，由此获取当前帧提供给局部地图 匹配所构建的非线性模型的状态初值为
Figure 994365DEST_PATH_IMAGE095
。
通过上述方式，获取经过优化后的每一个关键帧对应的姿态量，作为步骤160中的输入数据。与IMU数据集合中对应的惯性数据进行积分计算，获取高频的位置和姿态量，该输出结果即为最终位姿量。
图5示出了上述步骤实现的一个简易流程框图，具体参见图5所示。包括对激光雷达数据进行特征提取，完成初始位姿量初始化，对滑动窗口中的关键帧进行一次优化，以局部地图精匹配的方式实现对步骤150获取的每一个关键帧都进行二次优化，最终与惯性数据集合在状态层面融合，即利用IMU积分计算，获取最终的位姿量。
步骤170，根据最终位姿量，完成实时定位。
本发明实施例提供的实时定位方法，结合了惯性数据的高频特定和积分特性，以及激光点云数据的离散特性，在原始状态量的层面将两者进行融合，充分利用了这两类数据优势，可以实现在高速运动、结构少的场景，甚至缺失的室内外场景下的高精度定位，并且在长时间里保持高精度的状态最优估计，有效地提高了整体的定位精度。利用相对参考系的原理构建基于基准坐标系的局部地图，通过极坐标对特征进行筛选，提高了系统的稳定性与运行效率。利用关键帧之间的相对位姿量信息进行关键帧的提取并构建局部地图，通过局部地图与当前帧的匹配提高当前帧姿态的估计精度，同时利用局部图策略降低了整体的计算量。保证在不同场景下具有很好的稳定性和鲁棒性。
图6为本发明实施例提供的一种实时定位装置，该装置包括：获取单元601、特征提取单元602、处理单元603、迭代更新单元604、优化单元605、融合单元606和定位单元607。
获取单元601，用于获取惯性数据集合以及第一激光点云数据集合；
特征提取单元602，用于对第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合；
处理单元603，用于根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量；
迭代更新单元604，用于将前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新；
优化单元605，用于按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，直至第二激光点云数据集合中每一帧激光点云数据均在滑动窗口滑动过程中完成局部优化，获取与每一帧激光点云数据分别对应的优化后的状态量；
融合单元606，用于将与每一帧激光点云数据分别对应的优化后的状态量，与惯性数据集合进行融合，获取最终位姿量；
定位单元607，用于根据最终位姿量，完成实时定位。
可选的，特征提取单元602具体用于，从第一激光点云数据集合中提取与第一激光点同属同一环数，且与第一激光点的最近邻的预设数量激光点，与第一激光点共同构成局部点集合；
根据局部点集合中每一个激光点，计算第一激光点的曲率；
当确定第一激光点的曲率大于预设阈值时，确定第一激光点为角点；否则，确定第一激光点为平面点；
根据所有角点构成第一角点集合，根据所有平面点，构成第一平面点集合；
第一角点集合和第一平面点集合共同构成第二激光点云数据集合，其中第一激光点为激光点云数据中任一帧激光点云数据上的一个激光点。
可选的，处理单元603具体用于，根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系，确定前N帧激光点云数据中每一帧激光点云数据的初始位姿量；
根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
可选的，处理单元603具体用于，将前N帧激光点云数据中除第一帧激光点云数据以外的其他N-1帧激光点云数据中包含的所有平面点构成第二平面点集合，所有角点构成第二角点集合；
在第二平面点集合中寻找与第三激光点满足面状特征的预设个数的第一类最近邻点；获取第三激光点分别与第一类最近邻点中每两点之间的第一组匹配关系；
在第二角点集合中寻找与第四激光点满足线状特征的预设个数的第二类最近邻点；获取第四激光点分别与第二类最近邻点中每两点之间的第二组匹配关系；
根据第一组匹配关系，获取点到面距离残差；
根据第二组匹配关系，获取点到线距离残差；
根据点到面距离残差和点到线距离残差，构建第一代价函数；
对第一代价函数进行一次优化求解，获取第一帧激光点云数据对应的初始位姿量，其中第一帧激光点云数据为前N帧激光点云数据中的任一帧激光点云数据。
可选的，处理单元603具体用于，根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量，前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系；
根据惯性数据集合中首帧惯性数据、惯性数据与激光点云数据之间的转换关系，确定首帧激光点云数据对应的状态量；
根据首帧激光点云数据对应的状态量，以及前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
可选的，迭代更新单元604具体用于，获取与当前滑动窗口最邻近的，滑动窗口以外的第二帧激光点云数据；
并确定与第二帧激光点云数据最邻近的关键帧；
根据第二帧激光点云数据对应的状态量，与第二帧激光点云数据最邻近的关键帧对应的状态量，确定第二帧激光点云数据是否为关键帧；
当第二帧激光点云数据为关键帧时，删除当前滑动窗口中最旧的关键帧，将第二帧激光点云数据加入到当前滑动窗口中，作为最新的一帧关键帧；
将加入第二帧激光点云数据的滑动窗口按照预设步长滑动，并对新生成的滑动窗口中的关键帧进行迭代更新；
或者，当第二帧激光点云数据并非为关键帧时，舍弃第二帧激光点云数据，按照预设步长滑动滑动窗口，并继续对初始滑动窗口中的关键帧进行迭代更新。
可选的，优化单元605具体用于，以当前经过更新后的滑动窗口中每一个关键帧对应的状态量，以及每一个关键帧中所包含的平面特征点，构建以预设关键帧为参考的基准坐标系；
将除预设关键帧之外的每一个关键帧所包含的平面特征点对应的第一类位置数据均转换为基准坐标系下的第二类位置数据；
根据第二类位置数据以及基准坐标系中的极坐标，对每一个关键帧中的平面特征点进行筛选；
获取筛选特征点之后N-1关键帧中的第一关键帧，与相邻的第二关键帧之间的第一相对位姿关系；并根据第一相对位姿关系，生成第一残差方程；
获取第一关键帧与预设关键帧之间的第二相对位姿关系；并根据第二相对位姿关系，生成第二残差方程；
获取当前滑动窗口最近已删除的第三关键帧的先验信息；
根据第一残差方程、第二残差方程以及先验信息，构成联合优化模型代价函数；
对联合优化模型代价函数进行一次优化，以此完成对当前滑动窗口中的关键帧对应的状态量进行一次优化，获取当前滑动窗口中每一帧关键帧的最优状态量，其中，第一关键帧为经过筛选特征点之后N-1关键帧中的任一关键帧。
可选的，优化单元605还用于，对激光点云数据中每一帧激光点云数据对应的优化后的状态量进行二次优化，以便根据经过二次优化后的状态量和惯性数据进行融合，获取最终位姿量。
本实施例提供的实时定位装置中各功能部件所执行的功能均已在图1对应的实施例中做了详细介绍，因此这里不再赘述。
本发明实施例提供的一种实时定位装置，结合了惯性数据的高频特定和积分特性，以及激光点云数据的离散特性，在原始状态量的层面将两者进行融合，充分利用了这两类数据优势，可以实现在高速运动、结构少的场景，甚至缺失的室内外场景下的高精度定位，并且在长时间里保持高精度的状态最优估计，有效地提高了整体的定位精度。利用相对参考系的原理构建基于基准坐标系的局部地图，通过极坐标对特征进行筛选，提高了系统的稳定性与运行效率。利用关键帧之间的相对位姿量信息进行关键帧的提取并构建局部地图，通过局部地图与当前帧的匹配提高当前帧姿态的估计精度，同时利用局部图策略降低了整体的计算量。保证在不同场景下具有很好的稳定性和鲁棒性。
图7为本发明实施例提供的一种实时定位系统的结构示意图，图7所示的实时定位系统700包括：至少一个处理器701、存储器702、至少一个网络接口703和其他用户接口704。实时定位实时定位系统700中的各个组件通过总线系统705耦合在一起。可理解，总线系统705用于实现这些组件之间的连接通信。总线系统705除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图7中将各种总线都标为总线系统705。
其中，用户接口704可以包括显示器、键盘或者点击设备(例如，鼠标，轨迹球(trackball)、触感板或者触摸屏等。
可以理解，本发明实施例中的存储器702可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(Read-Only Memory，ROM)、可编程只读存储器 (Programmable ROM，PROM)、可擦除可编程只读存储器(Erasable PROM，EPROM)、电可擦除可编程只读存储器(Electrically EPROM，EEPROM)或闪存。易失性存储器可以是随机存取存储器(Random Access Memory，RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(Static RAM，SRAM)、动态随机存取存储器 (Dynamic RAM，DRAM)、同步动态随机存取存储器(Synchronous DRAM，SDRAM)、双倍数据速率同步动态随机存取存储器(Double DataRate SDRAM，DDRSDRAM)、增强型同步动态随机存取存储器(Enhanced SDRAM，ESDRAM)、同步连接动态随机存取存储器(Synch link DRAM，SLDRAM)和直接内存总线随机存取存储器(Direct Rambus RAM，DRRAM)。 本文描述的存储器702旨在包括但不限于这些和任意其它适合类型的存储器。
在一些实施方式中，存储器702存储了如下的元素，可执行单元或者数据结构，或者他们的子集，或者他们的扩展集：操作系统7021和应用程序 7022。
其中，操作系统7021，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序7022，包含各种应用程序，例如媒体播放器(Media Player)、浏览器(Browser)等，用于实现各种应用业务。实现本发明实施例方法的程序可以包含在应用程序7022中。
在本发明实施例中，通过调用存储器702存储的程序或指令，具体的，可以是应用程序7022中存储的程序或指令，处理器701用于执行各方法实施例所提供的方法步骤，例如包括：
获取惯性数据集合以及第一激光点云数据集合；
对第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合；
根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量；
将前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新；
按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，直至第二激光点云数据集合中每一帧激光点云数据均在滑动窗口滑动过程中完成局部优化，获取与每一帧激光点云数据分别对应的优化后的状态量；
将与每一帧激光点云数据分别对应的优化后的状态量，与惯性数据集合进行融合，获取最终位姿量；
根据最终位姿量，完成实时定位。
可选的，从第一激光点云数据集合中提取与第一激光点同属同一环数，且与第一激光点的最近邻的预设数量激光点，与第一激光点共同构成局部点集合；
根据局部点集合中每一个激光点，计算第一激光点的曲率；
当确定第一激光点的曲率大于预设阈值时，确定第一激光点为角点；否则，确定第一激光点为平面点；
根据所有角点构成第一角点集合，根据所有平面点，构成第一平面点集合；
第一角点集合和第一平面点集合共同构成第二激光点云数据集合，其中第一激光点为激光点云数据中任一帧激光点云数据上的一个激光点。
可选的，根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系，确定前N帧激光点云数据中每一帧激光点云数据的初始位姿量；
根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
可选的，将前N帧激光点云数据中除第一帧激光点云数据以外的其他N-1帧激光点云数据中包含的所有平面点构成第二平面点集合，所有角点构成第二角点集合；
在第二平面点集合中寻找与第三激光点满足面状特征的预设个数的第一类最近邻点；获取第三激光点分别与第一类最近邻点中每两点之间的第一组匹配关系；
在第二角点集合中寻找与第四激光点满足线状特征的预设个数的第二类最近邻点；获取第四激光点分别与第二类最近邻点中每两点之间的第二组匹配关系；
根据第一组匹配关系，获取点到面距离残差；
根据第二组匹配关系，获取点到线距离残差；
根据点到面距离残差和点到线距离残差，构建第一代价函数；
对第一代价函数进行一次优化求解，获取第一帧激光点云数据对应的初始位姿量，其中第一帧激光点云数据为前N帧激光点云数据中的任一帧激光点云数据。
可选的，根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量，前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系；
根据惯性数据集合中首帧惯性数据、惯性数据与激光点云数据之间的转换关系，确定首帧激光点云数据对应的状态量；
根据首帧激光点云数据对应的状态量，以及前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
可选的，获取与当前滑动窗口最邻近的，滑动窗口以外的第二帧激光点云数据；
并确定与第二帧激光点云数据最邻近的关键帧；
根据第二帧激光点云数据对应的状态量，与第二帧激光点云数据最邻近的关键帧对应的状态量，确定第二帧激光点云数据是否为关键帧；
当第二帧激光点云数据为关键帧时，删除当前滑动窗口中最旧的关键帧，将第二帧激光点云数据加入到当前滑动窗口中，作为最新的一帧关键帧；
将加入第二帧激光点云数据的滑动窗口按照预设步长滑动，并对新生成的滑动窗口中的关键帧进行迭代更新；
或者，当第二帧激光点云数据并非为关键帧时，舍弃第二帧激光点云数据，按照预设步长滑动滑动窗口，并继续对初始滑动窗口中的关键帧进行迭代更新。
可选的，以当前经过更新后的滑动窗口中每一个关键帧对应的状态量，以及每一个关键帧中所包含的平面特征点，构建以预设关键帧为参考的基准坐标系；
将除预设关键帧之外的每一个关键帧所包含的平面特征点对应的第一类位置数据均转换为基准坐标系下的第二类位置数据；
根据第二类位置数据以及基准坐标系中的极坐标，对每一个关键帧中的平面特征点进行筛选；
获取筛选特征点之后N-1关键帧中的第一关键帧，与相邻的第二关键帧之间的第一相对位姿关系；并根据第一相对位姿关系，生成第一残差方程；
获取第一关键帧与预设关键帧之间的第二相对位姿关系；并根据第二相对位姿关系，生成第二残差方程；
获取当前滑动窗口最近已删除的第三关键帧的先验信息；
根据第一残差方程、第二残差方程以及先验信息，构成联合优化模型代价函数；
对联合优化模型代价函数进行一次优化，以此完成对当前滑动窗口中的关键帧对应的状态量进行一次优化，获取当前滑动窗口中每一帧关键帧的最优状态量，其中，第一关键帧为经过筛选特征点之后N-1关键帧中的任一关键帧。
可选的，对激光点云数据中每一帧激光点云数据对应的优化后的状态量进行二次优化，以便根据经过二次优化后的状态量和惯性数据进行融合，获取最终位姿量。
上述本发明实施例揭示的方法可以应用于处理器701中，或者由处理器701实现。处理器701可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器701中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器701可以是通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(FieldProgrammable Gate Array， FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件单元组合执行完成。软件单元可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程 存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器702，处理器701读取存储器702中的信息，结合其硬件完成上述方法的步骤。
可以理解的是，本文描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现，处理单元603701可以实现在一个或多个专用集成电路(Application Specific Integrated Circuits，ASIC)、数字信号处理器(Digital SignalProcessing，DSP)、数字信号处理设备(DSPDevice，DSPD)、可编程逻辑设备(ProgrammableLogic Device，PLD)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)、通用处理器、控制器、微控制器、微处理器、用于执行本申请功能的其它电子单元或其组合中。
对于软件实现，可通过执行本文功能的单元来实现本文的技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。
本实施例提供的实时定位系统可以是如图7中所示的实时定位系统，可执行如图1中实时定位方法的所有步骤，进而实现图1所示实时定位方法的技术效果，具体请参照图1相关描述，为简洁描述，在此不作赘述。
本发明实施例还提供了一种存储介质（计算机可读存储介质）。这里的存储介质存储有一个或者多个程序。其中，存储介质可以包括易失性存储器，例如随机存取存储器；存储器也可以包括非易失性存储器，例如只读存储器、快闪存储器、硬盘或固态硬盘；存储器还可以包括上述种类的存储器的组合。
当存储介质中一个或者多个程序可被一个或者多个处理器执行，以实现上述在实时定位系统侧执行的实时定位方法。
处理器用于执行存储器中存储的实时定位程序，以实现以下在实时定位系统侧执行的实时定位方法的步骤：
获取惯性数据集合以及第一激光点云数据集合；
对第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合；
根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量；
将前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新；
按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，直至第二激光点云数据集合中每一帧激光点云数据均在滑动窗口滑动过程中完成局部优化，获取与每一帧激光点云数据分别对应的优化后的状态量；
将与每一帧激光点云数据分别对应的优化后的状态量，与惯性数据集合进行融合，获取最终位姿量；
根据最终位姿量，完成实时定位。
可选的，从第一激光点云数据集合中提取与第一激光点同属同一环数，且与第一激光点的最近邻的预设数量激光点，与第一激光点共同构成局部点集合；
根据局部点集合中每一个激光点，计算第一激光点的曲率；
当确定第一激光点的曲率大于预设阈值时，确定第一激光点为角点；否则，确定第一激光点为平面点；
根据所有角点构成第一角点集合，根据所有平面点，构成第一平面点集合；
第一角点集合和第一平面点集合共同构成第二激光点云数据集合，其中第一激光点为激光点云数据中任一帧激光点云数据上的一个激光点。
可选的，根据第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系，确定前N帧激光点云数据中每一帧激光点云数据的初始位姿量；
根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量、惯性数据集合中的首帧惯性数据、前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
可选的，将前N帧激光点云数据中除第一帧激光点云数据以外的其他N-1帧激光点云数据中包含的所有平面点构成第二平面点集合，所有角点构成第二角点集合；
在第二平面点集合中寻找与第三激光点满足面状特征的预设个数的第一类最近邻点；获取第三激光点分别与第一类最近邻点中每两点之间的第一组匹配关系；
在第二角点集合中寻找与第四激光点满足线状特征的预设个数的第二类最近邻点；获取第四激光点分别与第二类最近邻点中每两点之间的第二组匹配关系；
根据第一组匹配关系，获取点到面距离残差；
根据第二组匹配关系，获取点到线距离残差；
根据点到面距离残差和点到线距离残差，构建第一代价函数；
对第一代价函数进行一次优化求解，获取第一帧激光点云数据对应的初始位姿量，其中第一帧激光点云数据为前N帧激光点云数据中的任一帧激光点云数据。
可选的，根据前N帧激光点云数据中每一帧激光点云数据的初始位姿量，前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系；
根据惯性数据集合中首帧惯性数据、惯性数据与激光点云数据之间的转换关系，确定首帧激光点云数据对应的状态量；
根据首帧激光点云数据对应的状态量，以及前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系，确定前N帧激光点云数据中每一帧激光点云数据的状态量。
可选的，获取与当前滑动窗口最邻近的，滑动窗口以外的第二帧激光点云数据；
并确定与第二帧激光点云数据最邻近的关键帧；
根据第二帧激光点云数据对应的状态量，与第二帧激光点云数据最邻近的关键帧对应的状态量，确定第二帧激光点云数据是否为关键帧；
当第二帧激光点云数据为关键帧时，删除当前滑动窗口中最旧的关键帧，将第二帧激光点云数据加入到当前滑动窗口中，作为最新的一帧关键帧；
将加入第二帧激光点云数据的滑动窗口按照预设步长滑动，并对新生成的滑动窗口中的关键帧进行迭代更新；
或者，当第二帧激光点云数据并非为关键帧时，舍弃第二帧激光点云数据，按照预设步长滑动滑动窗口，并继续对初始滑动窗口中的关键帧进行迭代更新。
可选的，以当前经过更新后的滑动窗口中每一个关键帧对应的状态量，以及每一个关键帧中所包含的平面特征点，构建以预设关键帧为参考的基准坐标系；
将除预设关键帧之外的每一个关键帧所包含的平面特征点对应的第一类位置数据均转换为基准坐标系下的第二类位置数据；
根据第二类位置数据以及基准坐标系中的极坐标，对每一个关键帧中的平面特征点进行筛选；
获取筛选特征点之后N-1关键帧中的第一关键帧，与相邻的第二关键帧之间的第一相对位姿关系；并根据第一相对位姿关系，生成第一残差方程；
获取第一关键帧与预设关键帧之间的第二相对位姿关系；并根据第二相对位姿关系，生成第二残差方程；
获取当前滑动窗口最近已删除的第三关键帧的先验信息；
根据第一残差方程、第二残差方程以及先验信息，构成联合优化模型代价函数；
对联合优化模型代价函数进行一次优化，以此完成对当前滑动窗口中的关键帧对应的状态量进行一次优化，获取当前滑动窗口中每一帧关键帧的最优状态量，其中，第一关键帧为经过筛选特征点之后N-1关键帧中的任一关键帧。
可选的，对激光点云数据中每一帧激光点云数据对应的优化后的状态量进行二次优化，以便根据经过二次优化后的状态量和惯性数据进行融合，获取最终位姿量。
专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器（RAM）、内存、只读存储器（ROM）、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (11)

1.一种实时定位方法，其特征在于，所述方法包括：
获取惯性数据集合以及第一激光点云数据集合；
对所述第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合；
根据所述第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、所述惯性数据集合中的首帧惯性数据、所述前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定所述前N帧激光点云数据中每一帧激光点云数据的状态量；
将所述前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新；
按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，直至所述第二激光点云数据集合中每一帧激光点云数据均在滑动窗口滑动过程中完成局部优化，获取与每一帧激光点云数据分别对应的优化后的状态量；
将所述与每一帧激光点云数据分别对应的优化后的状态量，与所述惯性数据集合进行融合，获取最终位姿量；
根据所述最终位姿量，完成实时定位。
2.根据权利要求1所述的方法，其特征在于，所述对所述第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合，具体包括：
从所述第一激光点云数据集合中提取与第一激光点同属同一环数，且与所述第一激光点的最近邻的预设数量激光点，与所述第一激光点共同构成局部点集合；
根据所述局部点集合中每一个激光点，计算所述第一激光点的曲率；
当确定所述第一激光点的曲率大于预设阈值时，确定所述第一激光点为角点；否则，确定所述第一激光点为平面点；
根据所有角点构成第一角点集合，根据所有平面点，构成第一平面点集合；
所述第一角点集合和所述第一平面点集合共同构成所述第二激光点云数据集合，其中所述第一激光点为所述激光点云数据中任一帧激光点云数据上的一个激光点。
3.根据权利要求2所述的方法，其特征在于，所述根据所述第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、所述惯性数据集合中的首帧惯性数据、所述前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及所述惯性数据与激光点云数据之间的转换关系，确定所述前N帧激光点云数据中每一帧激光点云数据的状态量，具体包括：
根据所述第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系，确定所述前N帧激光点云数据中每一帧激光点云数据的初始位姿量；
根据所述前N帧激光点云数据中每一帧激光点云数据的初始位姿量、所述惯性数据集合中的首帧惯性数据、所述前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及所述惯性数据与激光点云数据之间的转换关系，确定所述前N帧激光点云数据中每一帧激光点云数据的状态量。
4.根据权利要求3所述的方法，其特征在于，所述根据所述第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系，确定所述前N帧激光点云数据中每一帧激光点云数据的初始位姿量，具体包括：
将所述前N帧激光点云数据中除第一帧激光点云数据以外的其他N-1帧激光点云数据中包含的所有平面点构成第二平面点集合，所有角点构成第二角点集合；
在所述第二平面点集合中寻找与第三激光点满足面状特征的预设个数的第一类最近邻点；获取所述第三激光点分别与所述第一类最近邻点中每两点之间的第一组匹配关系；
在所述第二角点集合中寻找与第四激光点满足线状特征的预设个数的第二类最近邻点；获取所述第四激光点分别与所述第二类最近邻点中每两点之间的第二组匹配关系；
根据所述第一组匹配关系，获取点到面距离残差；
根据所述第二组匹配关系，获取点到线距离残差；
根据所述点到面距离残差和所述点到线距离残差，构建第一代价函数；
对所述第一代价函数进行一次优化求解，获取所述第一帧激光点云数据对应的初始位姿量，其中所述第一帧激光点云数据为所述前N帧激光点云数据中的任一帧激光点云数据。
5.根据权利要求3或4所述的方法，其特征在于，所述根据所述前N帧激光点云数据中每一帧激光点云数据的初始位姿量、所述惯性数据集合中的首帧惯性数据、所述前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及所述惯性数据与激光点云数据之间的转换关系，确定所述前N帧激光点云数据中每一帧激光点云数据的状态量，具体包括：
根据所述前N帧激光点云数据中每一帧激光点云数据的初始位姿量，所述前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及所述惯性数据与激光点云数据之间的转换关系，确定所述前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系；
根据所述惯性数据集合中首帧惯性数据、所述惯性数据与激光点云数据之间的转换关系，确定所述首帧激光点云数据对应的状态量；
根据所述首帧激光点云数据对应的状态量，以及所述前N帧激光点云数据中相邻两帧激光点云数据之间的相对位姿关系，确定所述前N帧激光点云数据中每一帧激光点云数据的状态量。
6.根据权利要求1-4任一项所述的方法，其特征在于，所述将所述前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新，具体包括：
获取与当前滑动窗口最邻近的，滑动窗口以外的第二帧激光点云数据；
并确定与所述第二帧激光点云数据最邻近的关键帧；
根据所述第二帧激光点云数据对应的状态量，与所述第二帧激光点云数据最邻近的关键帧对应的状态量，确定所述第二帧激光点云数据是否为关键帧；
当所述第二帧激光点云数据为关键帧时，删除当前滑动窗口中最旧的关键帧，将所述第二帧激光点云数据加入到当前滑动窗口中，作为最新的一帧关键帧；
将加入所述第二帧激光点云数据的滑动窗口按照预设步长滑动，并对新生成的滑动窗口中的关键帧进行迭代更新；
或者，当所述第二帧激光点云数据并非为关键帧时，舍弃所述第二帧激光点云数据，按照预设步长滑动滑动窗口，并继续对所述初始滑动窗口中的关键帧进行迭代更新。
7.根据权利要求6所述的方法，其特征在于，按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，具体包括：
以所述当前经过更新后的滑动窗口中每一个关键帧对应的状态量，以及每一个关键帧中所包含的平面特征点，构建以预设关键帧为参考的基准坐标系；
将除所述预设关键帧之外的每一个关键帧所包含的平面特征点对应的第一类位置数据均转换为所述基准坐标系下的第二类位置数据；
根据所述第二类位置数据以及所述基准坐标系中的极坐标，对每一个关键帧中的平面特征点进行筛选；
获取筛选特征点之后N-1关键帧中的第一关键帧，与相邻的第二关键帧之间的第一相对位姿关系；并根据所述第一相对位姿关系，生成第一残差方程；
获取所述第一关键帧与所述预设关键帧之间的第二相对位姿关系；并根据所述第二相对位姿关系，生成第二残差方程；
获取所述当前滑动窗口最近已删除的第三关键帧的先验信息；
根据所述第一残差方程、第二残差方程以及所述先验信息，构成联合优化模型代价函数；
对所述联合优化模型代价函数进行一次优化，以此完成对当前滑动窗口中的关键帧对应的状态量进行一次优化，获取所述当前滑动窗口中每一帧关键帧的最优状态量，其中，所述第一关键帧为经过筛选特征点之后N-1关键帧中的任一关键帧。
8.根据权利要求1-4或7任一项所述的方法，其特征在于，所述将所述与每一帧激光点云数据分别对应的优化后的状态量，与所述惯性数据集合进行融合，获取最终位姿量之前，所述方法还包括：
对所述激光点云数据中每一帧激光点云数据对应的优化后的状态量进行二次优化，以便根据经过二次优化后的状态量和所述惯性数据进行融合，获取最终位姿量。
9.一种实时定位装置，其特征在于，所述装置包括：
获取单元，用于获取惯性数据集合以及第一激光点云数据集合；
特征提取单元，用于对所述第一激光点云数据集合进行特征点提取，获取第二激光点云数据集合；
处理单元，用于根据所述第二激光点云数据集合中的前N帧激光点云数据的特征点之间的结构位置关系、所述惯性数据集合中的首帧惯性数据、所述前N帧激光点云数据中相邻两帧激光点云数据之间的惯性数据，以及惯性数据与激光点云数据之间的转换关系，确定所述前N帧激光点云数据中每一帧激光点云数据的状态量；
迭代更新单元，用于将所述前N帧激光点云数据作为初始滑动窗口中的关键帧，按照预设步长滑动滑动窗口，并对当前滑动窗口中的关键帧进行迭代更新；
优化单元，用于按照第一预设规则对当前经过更新后的滑动窗口中的关键帧对应的状态量进行一次优化，直至所述第二激光点云数据集合中每一帧激光点云数据均在滑动窗口滑动过程中完成局部优化，获取与每一帧激光点云数据分别对应的优化后的状态量；
融合单元，用于将所述与每一帧激光点云数据分别对应的优化后的状态量，与所述惯性数据集合进行融合，获取最终位姿量；
定位单元，用于根据所述最终位姿量，完成实时定位。
10.一种实时定位系统，其特征在于，所述系统包括：至少一个处理器和存储器；
所述处理器用于执行所述存储器中存储的实时定位程序，以实现权利要求1~8中任一项所述的实时定位方法。
11.一种计算机存储介质，其特征在于，所述计算机存储介质存储有一个或者多个程序，所述一个或者多个程序可被如权利要求10所述的实时定位系统执行，以实现权利要求1~8中任一项所述的实时定位方法。
CN202010482416.7A 2020-06-01 2020-06-01 一种实时定位方法、装置、系统及存储介质 Active CN111402339B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010482416.7A CN111402339B (zh) 2020-06-01 2020-06-01 一种实时定位方法、装置、系统及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010482416.7A CN111402339B (zh) 2020-06-01 2020-06-01 一种实时定位方法、装置、系统及存储介质

Publications (2)

Publication Number Publication Date
CN111402339A true CN111402339A (zh) 2020-07-10
CN111402339B CN111402339B (zh) 2020-10-09

Family

ID=71433828

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010482416.7A Active CN111402339B (zh) 2020-06-01 2020-06-01 一种实时定位方法、装置、系统及存储介质

Country Status (1)

Country Link
CN (1) CN111402339B (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112067007A (zh) * 2020-11-12 2020-12-11 湖北亿咖通科技有限公司 地图生成方法、计算机存储介质及电子设备
CN112269187A (zh) * 2020-09-28 2021-01-26 广州视源电子科技股份有限公司 机器人状态检测方法、装置及设备
CN112506200A (zh) * 2020-12-14 2021-03-16 广州视源电子科技股份有限公司 机器人定位方法、装置、机器人及存储介质
CN113147738A (zh) * 2021-02-26 2021-07-23 重庆智行者信息科技有限公司 一种自动泊车定位方法和装置
CN113253293A (zh) * 2021-06-03 2021-08-13 中国人民解放军国防科技大学 一种激光点云畸变的消除方法和计算机可读存储介质
CN113311452A (zh) * 2021-05-26 2021-08-27 上海新纪元机器人有限公司 一种基于多传感器的定位方法及其系统
CN113671530A (zh) * 2021-08-06 2021-11-19 北京京东乾石科技有限公司 位姿确定方法、装置及存储介质和电子设备
CN115685223A (zh) * 2022-12-15 2023-02-03 深圳市智绘科技有限公司 位置识别方法、装置、电子设备及可读存储介质
CN115937383A (zh) * 2022-09-21 2023-04-07 北京字跳网络技术有限公司 渲染图像的方法、装置、电子设备及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105354875A (zh) * 2015-09-25 2016-02-24 厦门大学 一种室内环境二维与三维联合模型的构建方法和系统
CN106228596A (zh) * 2016-08-05 2016-12-14 吉鸥信息技术（杭州）有限公司 三维激光点云生成方法及装置
CN106446815A (zh) * 2016-09-14 2017-02-22 浙江大学 一种同时定位与地图构建方法
CN108053473A (zh) * 2017-12-29 2018-05-18 北京领航视觉科技有限公司 一种室内三维模型数据的处理方法
US20190146062A1 (en) * 2017-11-15 2019-05-16 Baidu Online Network Technology (Beijing) Co., Ltd Laser point cloud positioning method and system
CN110211228A (zh) * 2019-04-30 2019-09-06 北京云迹科技有限公司 用于建图的数据处理方法及装置
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN111024066A (zh) * 2019-12-10 2020-04-17 中国航空无线电电子研究所 一种无人机视觉-惯性融合室内定位方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105354875A (zh) * 2015-09-25 2016-02-24 厦门大学 一种室内环境二维与三维联合模型的构建方法和系统
CN106228596A (zh) * 2016-08-05 2016-12-14 吉鸥信息技术（杭州）有限公司 三维激光点云生成方法及装置
CN106446815A (zh) * 2016-09-14 2017-02-22 浙江大学 一种同时定位与地图构建方法
US20190146062A1 (en) * 2017-11-15 2019-05-16 Baidu Online Network Technology (Beijing) Co., Ltd Laser point cloud positioning method and system
CN108053473A (zh) * 2017-12-29 2018-05-18 北京领航视觉科技有限公司 一种室内三维模型数据的处理方法
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110211228A (zh) * 2019-04-30 2019-09-06 北京云迹科技有限公司 用于建图的数据处理方法及装置
CN111024066A (zh) * 2019-12-10 2020-04-17 中国航空无线电电子研究所 一种无人机视觉-惯性融合室内定位方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
JAEHOON JUNG ET AL: "Development of Kinematic 3D Laser Scanning System for Indoor Mapping and As-Built BIM Using Constrained SLAM", 《SENSORS》 *
张天喜 等: "基于三维激光的图优化即时定位与建图策略", 《激光与光电子学进展》 *
熊伟成 等: "面向车载激光扫描数据的道路目标精细化鲁棒提取", 《地球信息科学学报》 *

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111983639B (zh) * 2020-08-25 2023-06-02 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112269187A (zh) * 2020-09-28 2021-01-26 广州视源电子科技股份有限公司 机器人状态检测方法、装置及设备
CN112067007A (zh) * 2020-11-12 2020-12-11 湖北亿咖通科技有限公司 地图生成方法、计算机存储介质及电子设备
CN112067007B (zh) * 2020-11-12 2021-01-29 湖北亿咖通科技有限公司 地图生成方法、计算机存储介质及电子设备
CN112506200A (zh) * 2020-12-14 2021-03-16 广州视源电子科技股份有限公司 机器人定位方法、装置、机器人及存储介质
CN112506200B (zh) * 2020-12-14 2023-12-08 广州视源电子科技股份有限公司 机器人定位方法、装置、机器人及存储介质
CN113147738A (zh) * 2021-02-26 2021-07-23 重庆智行者信息科技有限公司 一种自动泊车定位方法和装置
CN113311452A (zh) * 2021-05-26 2021-08-27 上海新纪元机器人有限公司 一种基于多传感器的定位方法及其系统
CN113253293A (zh) * 2021-06-03 2021-08-13 中国人民解放军国防科技大学 一种激光点云畸变的消除方法和计算机可读存储介质
CN113671530A (zh) * 2021-08-06 2021-11-19 北京京东乾石科技有限公司 位姿确定方法、装置及存储介质和电子设备
CN113671530B (zh) * 2021-08-06 2024-01-12 北京京东乾石科技有限公司 位姿确定方法、装置及存储介质和电子设备
CN115937383A (zh) * 2022-09-21 2023-04-07 北京字跳网络技术有限公司 渲染图像的方法、装置、电子设备及存储介质
CN115937383B (zh) * 2022-09-21 2023-10-10 北京字跳网络技术有限公司 渲染图像的方法、装置、电子设备及存储介质
CN115685223A (zh) * 2022-12-15 2023-02-03 深圳市智绘科技有限公司 位置识别方法、装置、电子设备及可读存储介质

Also Published As

Publication number Publication date
CN111402339B (zh) 2020-10-09

Similar Documents

Publication Publication Date Title
CN111402339A (zh) 一种实时定位方法、装置、系统及存储介质
JP7086111B2 (ja) 自動運転車のｌｉｄａｒ測位に用いられるディープラーニングに基づく特徴抽出方法
US11313684B2 (en) Collaborative navigation and mapping
CN108444482B (zh) 一种无人机自主寻路避障方法及系统
CN108717710B (zh) 室内环境下的定位方法、装置及系统
Li et al. Multi-sensor fusion for navigation and mapping in autonomous vehicles: Accurate localization in urban environments
Mutz et al. Large-scale mapping in complex field scenarios using an autonomous car
JP7060625B2 (ja) 自動運転車において３ｄｃｎｎネットワークを用いてソリューション推断を行うｌｉｄａｒ測位
EP3427008B1 (en) Laser scanner with real-time, online ego-motion estimation
US10380890B2 (en) Autonomous vehicle localization based on walsh kernel projection technique
JP2021152662A (ja) リアルタイムのマッピングと位置確認のための方法及び装置
CN112639502A (zh) 机器人位姿估计
CN110386142A (zh) 用于自动驾驶车辆的俯仰角校准方法
CN111968229A (zh) 高精度地图制图方法及装置
WO2021207999A1 (zh) 车辆定位的方法和装置、定位图层生成的方法和装置
CN112700479B (zh) 一种基于cnn点云目标检测的配准方法
CN113286985A (zh) 一种路径规划方法和路径规划装置
CN112070870B (zh) 点云地图评估方法、装置、计算机设备和存储介质
CN113514843A (zh) 多子图激光雷达定位方法、系统以及终端
CN114964212A (zh) 面向未知空间探索的多机协同融合定位与建图方法
CN114061611A (zh) 目标对象定位方法、装置、存储介质和计算机程序产品
CN115639823A (zh) 崎岖起伏地形下机器人地形感知与移动控制方法及系统
CN112950710A (zh) 位姿确定方法、装置、电子设备和计算机可读存储介质
Wen 3D LiDAR aided GNSS and its tightly coupled integration with INS via factor graph optimization
Jiang et al. 3D SLAM based on NDT matching and ground constraints for ground robots in complex environments

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
PE01 Entry into force of the registration of the contract for pledge of patent right
PE01 Entry into force of the registration of the contract for pledge of patent right

Denomination of invention: A real-time positioning method, device, system, and storage medium

Effective date of registration: 20231213

Granted publication date: 20201009

Pledgee: Shenzhen SME financing Company limited by guarantee

Pledgor: Shenzhen Smart Mapping Tech. Co.,Ltd.

Registration number: Y2023980071273