CN117671008A - 位姿估计方法、可读存储介质及智能设备 - Google Patents
位姿估计方法、可读存储介质及智能设备 Download PDFInfo
- Publication number
- CN117671008A CN117671008A CN202311728395.2A CN202311728395A CN117671008A CN 117671008 A CN117671008 A CN 117671008A CN 202311728395 A CN202311728395 A CN 202311728395A CN 117671008 A CN117671008 A CN 117671008A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- pose
- map
- historical
- frame
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 238000005457 optimization Methods 0.000 claims description 26
- 238000004590 computer program Methods 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 9
- 230000001815 facial effect Effects 0.000 claims description 4
- 230000008569 process Effects 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000033228 biological regulation Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000013475 authorization Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
本申请涉及自动驾驶技术领域,具体提供一种位姿估计方法、可读存储介质及智能设备,旨在解决如何获得点云帧更为准确的位姿,从而进一步提升激光雷达里程计的精度的问题。为此目的,本申请获取智能设备上设置的激光雷达采集的当前点云帧,将当前点云帧与局部点云地图进行配准,获得当前点云帧的位姿估计结果。局部点云地图是基于滑动窗口内的历史点云帧拼接,并应用因子图进行优化获得的,因子图的优化变量为历史点云帧的位姿和面元特征,能够同时对加入滑动窗口内历史点云帧的位姿和面元特征进行优化,能够有效减少点云配准时的累计误差,获得更为准确的位姿估计结果,提升激光雷达里程计的精度。
Description
技术领域
本申请涉及自动驾驶技术领域,具体提供一种位姿估计方法、可读存储介质及智能设备。
背景技术
在自动驾驶技术中,激光雷达里程计应用越来越广泛。激光雷达里程计通过估计连续点云帧之间的变换,获得点云帧的姿态。
但是现有的激光雷达里程计主要是利用当前时刻观测对点云特征值估计当前时刻的运动状态,进行当前帧点云与局部点云地图的配准,没有考虑观测的误差,并且固定了历史时刻的状态,容易产生累计误差,针对一些退化场景不鲁棒,存在着激光雷达里程计精度差,容易产生漂移的问题。
相应地,本领域需要一种新的位姿估计方案来解决上述问题。
发明内容
为了克服上述缺陷,提出了本申请,以提供解决或至少部分地解决如何获得点云帧更为准确的位姿,从而进一步提升激光雷达里程计的精度的问题。
在第一方面,本申请提供一种位姿估计方法,所述方法包括:
获取智能设备上设置的激光雷达采集的当前点云帧;
将所述当前点云帧与局部点云地图进行配准,获取所述当前点云帧的位姿估计结果;
其中,所述局部点云地图是基于预设窗口大小的滑动窗口内的历史点云帧拼接,并应用因子图进行优化后获得的;
所述因子图的优化变量包括所述历史点云帧的位姿和面元特征。
在上述位姿估计方法的一个技术方案中,所述方法还包括根据以下步骤获得所述局部点云地图:
获取所述滑动窗口内的所述历史点云帧的位姿;
根据所述位姿和所述历史点云帧,获得初始点云地图;
应用所述因子图,对所述初始点云地图中包含的所述历史点云帧的位姿和面元特征进行优化,获得所述局部点云地图。
在上述位姿估计方法的一个技术方案中,所述方法还包括根据以下步骤构建所述因子图:
设置所述因子图的变量节点;所述变量节点包括与所述历史点云帧的位姿对应的位姿变量节点和所述面元特征对应的面元特征变量节点;
设置所述变量节点的约束项;每个所述位姿变量节点分别设置有至少一个用于对所述位姿进行约束的约束项;每个所述面元特征变量节点分别设置有至少一个用于对所述面元特征进行约束的约束项;
根据所述变量节点和所述约束项,构建所述因子图。
在上述位姿估计方法的一个技术方案中,所述应用所述因子图,对所述初始点云地图中包含的所述历史点云帧的位姿和与面元特征进行优化,包括:
基于所述因子图,对所述初始点云地图中包含的所述历史点云帧的位姿和所述面元特征进行滑动窗口优化,并在滑动优化之后分别对每个约束项进行优化,以形成优化因子图,并在进行下一次滑动窗口优化时能够基于所述优化因子图对所述滑动窗口内的所述位姿和所述面元特征进行优化。
在上述位姿估计方法的一个技术方案中,
所述分别对每个约束项进行优化,以形成优化因子图,包括:
获取每个约束项的残差;
根据每个约束项的残差,分别对每个约束项进行优化,以形成所述优化因子图。
在上述位姿估计方法的一个技术方案中,所述根据每个约束项的残差,分别对每个约束项进行优化,包括:
根据所述每个约束项的残差,获取所述残差关于所述优化变量的雅可比矩阵;
根据所述残差和所述雅可比矩阵,对所述每个约束项进行优化。
在上述位姿估计方法的一个技术方案中,所述根据所述残差和所述雅可比矩阵,对所述每个约束项进行优化,包括:
根据所述残差和所述雅可比矩阵,应用最小二乘法,对所述每个约束项进行优化。
在上述位姿估计方法的一个技术方案中,所述根据所述位姿和所述历史点云帧,获得初始点云地图,包括:
根据所述位姿将所述历史点云帧进行拼接,获得拼接点云地图;
将所述拼接点云地图进行体素化,获得体素化点云地图;
根据所述体素化点云地图构建八叉树,以获得所述初始点云地图。
在上述位姿估计方法的一个技术方案中,所述获取所述滑动窗口内的所述历史点云帧的位姿,包括:
根据所述智能设备的其他里程计,获得所述历史点云帧的初始位姿;
根据所述初始位姿,将所述历史点云帧与局部点云地图进行配准,获取所述历史点云帧与所述局部点云地图之间的面元特征之间的距离残差;
根据所述距离残差优化所述初始位姿,以获得所述历史点云帧的位姿,并根据所述位姿将所述历史点云帧加入至所述滑动窗口。
在上述位姿估计方法的一个技术方案中,所述方法还包括根据以下步骤获取所述面元特征:
将面元上距离当前坐标系原点的最近点的3D坐标,作为所述面元的面元特征。
在上述位姿估计方法的一个技术方案中,所述方法还包括:
当所述滑动窗口中的历史点云帧的数量大于所述预设窗口大小时,将最老的一个历史点云帧删除。
在第二方面,提供一种计算机可读存储介质,该计算机可读存储介质其中存储有多条程序代码,所述程序代码适于由处理器加载并运行以执行上述位姿估计方法的技术方案中任一项技术方案所述的位姿估计方法。
在第三方面,提供一种智能设备,其特征在于,包括:
至少一个处理器;
以及,与所述至少一个处理器通信连接的存储器;
其中,所述存储器中存储有计算机程序,所述计算机程序被所述至少一个处理器执行时实现位姿估计方法技术方案中所述的位姿估计方法。。
本申请上述一个或多个技术方案,至少具有如下一种或多种有益效果:
在实施本申请的技术方案中,本申请获取智能设备上设置的激光雷达采集的当前点云帧,将当前点云帧与局部点云地图进行配准,获得当前点云帧的位姿估计结果。局部点云地图是基于滑动窗口内的历史点云帧拼接,并应用因子图进行优化获得的,因子图的优化变量为历史点云帧的位姿和面元特征。通过上述配置方式,本申请能够同时对加入滑动窗口内历史点云帧的位姿和面元特征同时进行优化,从而在进行当前点云帧与局部点云地图进行配准时,能够有效减少累计误差,获得更为准确的位姿估计结果,提升激光雷达里程计的精度。
附图说明
参照附图,本申请的公开内容将变得更易理解。本领域技术人员容易理解的是:这些附图仅仅用于说明的目的,而并非意在对本申请的保护范围组成限制。其中:
图1是根据本申请的一个实施例的位姿估计方法的主要步骤流程示意图;
图2是根据本申请实施例的一个实施方式的面元参数化示意图;
图3是根据本申请实施例的一个实施方式的获取局部点云地图的主要步骤流程示意图;
图4是根据本申请的一个实施例的智能设备中的存储器和处理器的连接关系示意图。
具体实施方式
下面参照附图来描述本申请的一些实施方式。本领域技术人员应当理解的是,这些实施方式仅仅用于解释本申请的技术原理,并非旨在限制本申请的保护范围。
在本申请的描述中,“模块”、“处理器”可以包括硬件、软件或者两者的组合。一个模块可以包括硬件电路,各种合适的感应器,通信端口,存储器,也可以包括软件部分,比如程序代码,也可以是软件和硬件的组合。处理器可以是中央处理器、微处理器、图像处理器、数字信号处理器或者其他任何合适的处理器。处理器具有数据和/或信号处理功能。处理器可以以软件方式实现、硬件方式实现或者二者结合方式实现。非暂时性的计算机可读存储介质包括任何合适的可存储程序代码的介质,比如磁碟、硬盘、光碟、闪存、只读存储器、随机存取存储器等等。术语“A和/或B”表示所有可能的A与B的组合,比如只是A、只是B或者A和B。术语“至少一个A或B”或者“A和B中的至少一个”含义与“A和/或B”类似,可以包括只是A、只是B或者A和B。单数形式的术语“一个”、“这个”也可以包含复数形式。
自动驾驶系统(Automated Driving Systems,ADS),其是指系统在自身的设计运行范围(Operational Domain Design,ODD)内将持续执行全部的动态驾驶任务(DynamicDriving Task,DDT)。即,在规定的适当驾驶场景条件下才允许机器系统全部承担车辆自动操控的任务,即车辆满足ODD条件时系统被激活,该系统代替人类驾驶员作为车辆的驾驶主体。其中,动态驾驶任务DDT是指对车辆进行持续的横向(左、右转向)和纵向运动控制(加速,减速,匀速)以及对车辆行驶环境中的目标和事件进行探测和响应。设计运行范围ODD是指自动驾驶系统可以安全运行的条件,其设置的条件可以包括地理位置、道路类型、速度范围、天气、时间、国家和地方性交通法律法规等。
参阅附图1,图1是根据本申请的一个实施例的位姿估计方法的主要步骤流程示意图。如图1所示,本申请实施例中的位姿估计方法主要包括下列步骤S101-步骤S102。
步骤S101:获取智能设备上设置的激光雷达采集的当前点云帧。
在本实施例中,可以获取智能设备上设置的激光雷达采集的当前点云帧。
一个实施方式中,智能设备可以为驾驶设备、智能车、机器人等设备。
步骤S102:将当前点云帧与局部点云地图进行配准,获取当前点云帧的位姿估计结果;其中,局部点云地图是基于预设窗口大小的滑动窗口内的历史点云帧拼接,并应用因子图进行优化后获得的;因子图的优化变量包括历史点云帧的位姿和面元特征。
在本实施例中,激光雷达里程计可以将当前点云帧与局部点云地图进行配准获得当前点云帧的位姿估计结果。其中,局部点云地图根据激光雷达采集的历史帧点云和历史帧点云的位姿进行拼接,且基于滑动窗口应用因子图进行优化获得的,因子图的优化变量为历史点云帧的位姿和面元特征。即,可以将历史点云帧建模为多个面元,将表示每个面元的参数作为历史点云帧的面元特征。本领域技术人员可以根据实际应用的需要设置预设窗口大小的取值。
一个实施方式中,面元特征可以为每个面元的平面方程。
一个实施方式中,可以参阅附图2,图2是根据本申请实施例的一个实施方式的面元参数化示意图。如图2所示,可以将将面元中距离当前坐标系原点的最近点的3D坐标,即图2中的作为面元的面元特征,以实现面元参数化。这样能够在确保面元特征具有真实物理意义的前提下,最小化参数形式,减少运算过程的运算量。
基于上述步骤S101-步骤S102,本申请实施例获取智能设备上设置的激光雷达采集的当前点云帧,将当前点云帧与局部点云地图进行配准,获得当前点云帧的位姿估计结果。局部点云地图是基于滑动窗口内的历史点云帧拼接,并应用因子图进行优化获得的,因子图的优化变量为历史点云帧的位姿和面元特征。通过上述配置方式,本申请实施例能够同时对加入滑动窗口内历史点云帧的位姿和面元特征同时进行优化,从而在进行当前点云帧与局部点云地图进行配准时,能够有效减少累计误差,获得更为准确的位姿估计结果,提升激光雷达里程计的精度。
下面对获取局部点云地图的过程作进一步地说明。
在本申请实施例的一个实施方式中,可以根据以下步骤S201至步骤S203获取局部点云地图:
步骤S201:获取滑动窗口内的历史点云帧的位姿。
在本实施方式中,步骤S201可以进一步包括以下步骤S2011值至步骤S2013:
步骤S2011:根据智能设备的其他里程计,获得历史点云帧的初始位姿。
在本实施方式中,可以根据智能设备其他里程计来获取历史点云帧的初始位姿。其中,其他里程计可以为视觉里程计、轮式里程计等。
步骤S2012:根据初始位姿,将历史点云帧与局部点云地图进行配准,获取历史点云帧与局部点云地图之间的面元特征之间的距离残差。
在本实施方式中,可以根据初始位姿,将当前历史点云帧与局部点云地图进行匹配(scan to map),获得当前历史点云帧的面元特征与局部点云地图的面元特征之间的距离残差。
步骤S2013:根据距离残差优化初始位姿,以获得历史点云帧的位姿,并根据位姿将历史点云帧加入至滑动窗口。
在本实施方式中,可以应用距离残差来优化初始位姿,从而获得历史点云帧的位姿,并将历史点云帧加入至滑动窗口中。
步骤S202:根据位姿和历史点云帧,获得初始点云地图。
在本实施方式中,步骤S202可以进一步包括以下步骤S2021至步骤S2023:
步骤S2021:根据位姿将历史点云帧进行拼接,获得拼接点云地图。
步骤S2022:将拼接点云地图进行体素化,获得体素化点云地图。
步骤S2023:根据体素化点云地图构建八叉树,以获得初始点云地图。
在本实施方式中,可以根据位姿将历史点云帧拼接起来,并对拼接点云地图进行体素化,从而获得体素化点云地图,并根据体素化点云地图,构建八叉树,以方便后续进行查找索引,从而获得初始点云地图。
步骤S203:应用因子图,对初始点云地图中包含的历史点云帧的位姿和面元特征进行优化,获得局部点云地图。
在本实施方式中,可以应用因子图对初始点云地图中历史点云帧的位姿和面元特征进行优化,获得局部点云地图。可以采用自动驾驶技术领域中常规的基于因子图进行优化的方法,基于上述因子图对滑动窗口内的每个历史点云帧的位姿和面元特征进行优化,本申请实施例对此不进行具体限定。
一个实施方式中,可以根据以下步骤301至步骤S303构建因子图:
步骤301:设置因子图的变量节点;变量节点包括与历史点云帧的位姿对应的位姿变量节点和面元特征对应的面元特征变量节点。
步骤302:设置变量节点的约束项;每个位姿变量节点分别设置有至少一个用于对位姿进行约束的约束项;每个面元特征变量节点分别设置有至少一个用于对面元特征进行约束的约束项。
步骤303:根据变量节点和约束项,构建因子图。
在本实施方式中,因子图的变量节点可以为历史点云帧的位姿对应的位姿变量节点和面元特征对应的面元特征变量节点,并为每个变量节点设置至少一个约束项,从而根据变量节点和约束项来构建因子图。
一个实施方式中,步骤S203可以进一步配置为:
基于因子图,对初始点云地图中包含的历史点云帧的位姿和面元特征进行滑动窗口优化,并在滑动优化之后分别对每个约束项进行优化,以形成优化因子图,并在进行下一次滑动窗口优化时能够基于优化因子图对滑动窗口内的位姿和面元特征进行优化。
在本实施方式中,可以对滑动窗口内的位姿和面元特征进行优化,并对每个约束项进行优化,从而形成优化因子图,并在下一次滑动窗口优化时基于优化因子图对滑动窗口内的位姿和面元特征进行优化。
一个实施方式中,可以获取每个约束项的残差,根据对约束项进行优化,从而形成优化因子图。
一个实施方式中,可以计算残差关于优化变量的雅可比矩阵,根据残差和雅可比矩阵进行每个约束项的优化。其中,可以应用本领域常用的基于残差和雅可比矩阵进行优化的方法实现约束项的优化,本申请对此不进行限定。
一个实施方式中,可以应用最小二乘法,根据残差和雅可比矩阵进行每个约束项的优化,进而优化滑动窗口内的位姿和面元特征。
一个实施方式中,当滑动窗口内的历史点云帧的数量大于预设窗口大小时,可以将最老的一个历史点云帧删除,并加入新的历史点云帧,以重复进行上述局部点云地图获取过程。
一个实施方式中,可以参阅附图3,图3是根据本申请实施例的一个实施方式的获取局部点云地图的主要步骤流程示意图。如图3所示,获取局部点云地图时,可以先基于其他来源的里程计获得历史点云帧的初始位姿,并基于当前历史点云帧与地图之间的距离残差来优化当前帧(当前历史点云帧)的位姿,并将优化后的当前帧加入至滑动窗口。判断滑动窗口内的帧数是否达到阈值(预设滑动窗口大小),若否,则继续获取新的历史点云帧,若是则将滑动窗口内的历史点云帧拼接为局部地图,并构建八叉树地图,构建同时优化滑动窗口内的多帧历史点云帧的位姿和面元特征的因子图,进行因子图优化,删除最老的一帧历史点云帧,继续获取新的历史点云帧。
需要指出的是,尽管上述实施例中将各个步骤按照特定的先后顺序进行了描述,但是本领域技术人员可以理解,为了实现本申请的效果,不同的步骤之间并非必须按照这样的顺序执行,其可以同时(并行)执行或以其他顺序执行,这些变化都在本申请的保护范围之内。
本领域技术人员能够理解的是,本申请实现上述一实施例的方法中的全部或部分流程,也可以通过计算机程序来指令相关的硬件来完成,所述的计算机程序可存储于一计算机可读存储介质中,该计算机程序在被处理器执行时,可实现上述各个方法实施例的步骤。其中,所述计算机程序包括计算机程序代码,所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读存储介质可以包括:能够携带所述计算机程序代码的任何实体或装置、介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器、随机存取存储器、电载波信号、电信信号以及软件分发介质等。需要说明的是,所述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减,例如在某些司法管辖区,根据立法和专利实践,计算机可读存储介质不包括电载波信号和电信信号。
进一步,本申请还提供了一种计算机可读存储介质。在根据本申请的一个计算机可读存储介质实施例中,计算机可读存储介质可以被配置成存储执行上述方法实施例的位姿估计方法的程序,该程序可以由处理器加载并运行以实现上述位姿估计方法。为了便于说明,仅示出了与本申请实施例相关的部分,具体技术细节未揭示的,请参照本申请实施例方法部分。该计算机可读存储介质可以是包括各种电子设备形成的存储装置设备,可选的,本申请实施例中计算机可读存储介质是非暂时性的计算机可读存储介质。
进一步,本申请还提供了一种智能设备,智能设备可以包括至少一个处理器;以及,与至少一个处理器通信连接的存储器;其中,存储器中存储有计算机程序,计算机程序被至少一个处理器执行时实现上述任一实施例所述的方法。本申请的智能设备,可以包括驾驶设备、智能车、机器人等设备。参阅附图4,图4是根据本申请的一个实施例的智能设备中的存储器和处理器的连接关系示意图。图4中示例性的示出了存储器和处理器通过总线通信连接。
在本申请的一些实施例中,智能设备还包括至少一个传感器,传感器用于感知信息。传感器与本申请提到的任何一种类型的处理器通信连接。可选地,智能设备还包括自动驾驶系统,自动驾驶系统用于引导智能设备自行行驶或辅助驾驶。处理器与传感器和/或自动驾驶系统通信,用于完成上述任一实施例所述的方法。
进一步,应该理解的是,由于各个模块的设定仅仅是为了说明本申请的装置的功能单元,这些模块对应的物理器件可以是处理器本身,或者处理器中软件的一部分,硬件的一部分,或者软件和硬件结合的一部分。因此,图中的各个模块的数量仅仅是示意性的。
本领域技术人员能够理解的是,可以对装置中的各个模块进行适应性地拆分或合并。对具体模块的这种拆分或合并并不会导致技术方案偏离本申请的原理,因此,拆分或合并之后的技术方案都将落入本申请的保护范围内。
本申请各实施例中可能涉及的相关用户个人信息,均为严格按照法律法规的要求,遵循合法、正当、必要的原则,基于业务场景的合理目的,处理用户在使用产品/服务过程中主动提供或因使用产品/服务而产生的,以及经用户授权获取的个人信息。
本申请处理的用户个人信息会因具体产品/服务场景而有所不同,需以用户使用产品/服务的具体场景为准,可能会涉及用户的账号信息、设备信息、驾驶信息、车辆信息或其他相关信息。申请人会以高度的勤勉义务对待用户的个人信息及其处理。
本申请非常重视用户个人信息的安全,已采取符合业界标准、合理可行的安全防护措施保护用户的信息,防止个人信息遭到未经授权访问、公开披露、使用、修改、损坏或丢失。
至此,已经结合附图所示的优选实施方式描述了本申请的技术方案,但是,本领域技术人员容易理解的是,本申请的保护范围显然不局限于这些具体实施方式。在不偏离本申请的原理的前提下,本领域技术人员可以对相关技术特征作出等同的更改或替换,这些更改或替换之后的技术方案都将落入本申请的保护范围之内。
Claims (13)
1.一种位姿估计方法,其特征在于,所述方法包括:
获取智能设备上设置的激光雷达采集的当前点云帧;
将所述当前点云帧与局部点云地图进行配准,获取所述当前点云帧的位姿估计结果;
其中,所述局部点云地图是基于预设窗口大小的滑动窗口内的历史点云帧拼接,并应用因子图进行优化后获得的;
所述因子图的优化变量包括所述历史点云帧的位姿和面元特征。
2.根据权利要求1所述的位姿估计方法,其特征在于,所述方法还包括根据以下步骤获得所述局部点云地图:
获取所述滑动窗口内的所述历史点云帧的位姿;
根据所述位姿和所述历史点云帧,获得初始点云地图;
应用所述因子图,对所述初始点云地图中包含的所述历史点云帧的位姿和面元特征进行优化,获得所述局部点云地图。
3.根据权利要求2所述的位姿估计方法,其特征在于,所述方法还包括根据以下步骤构建所述因子图:
设置所述因子图的变量节点;所述变量节点包括与所述历史点云帧的位姿对应的位姿变量节点和所述面元特征对应的面元特征变量节点;
设置所述变量节点的约束项;每个所述位姿变量节点分别设置有至少一个用于对所述位姿进行约束的约束项;每个所述面元特征变量节点分别设置有至少一个用于对所述面元特征进行约束的约束项;
根据所述变量节点和所述约束项,构建所述因子图。
4.根据权利要求3所述的位姿估计方法,其特征在于,
所述应用所述因子图,对所述初始点云地图中包含的所述历史点云帧的位姿和与面元特征进行优化,包括:
基于所述因子图,对所述初始点云地图中包含的所述历史点云帧的位姿和所述面元特征进行滑动窗口优化,并在滑动优化之后分别对每个约束项进行优化,以形成优化因子图,并在进行下一次滑动窗口优化时能够基于所述优化因子图对所述滑动窗口内的所述位姿和所述面元特征进行优化。
5.根据权利要求4所述的位姿估计方法,其特征在于,
所述分别对每个约束项进行优化,以形成优化因子图,包括:
获取每个约束项的残差;
根据每个约束项的残差,分别对每个约束项进行优化,以形成所述优化因子图。
6.根据权利要求5所述的位姿估计方法,其特征在于,
所述根据每个约束项的残差,分别对每个约束项进行优化,包括:
根据所述每个约束项的残差,获取所述残差关于所述优化变量的雅可比矩阵;
根据所述残差和所述雅可比矩阵,对所述每个约束项进行优化。
7.根据权利要求6所述的位姿估计方法,其特征在于,
所述根据所述残差和所述雅可比矩阵,对所述每个约束项进行优化,包括:
根据所述残差和所述雅可比矩阵,应用最小二乘法,对所述每个约束项进行优化。
8.根据权利要求2所述的位姿估计方法,其特征在于,
所述根据所述位姿和所述历史点云帧,获得初始点云地图,包括:
根据所述位姿将所述历史点云帧进行拼接,获得拼接点云地图;
将所述拼接点云地图进行体素化,获得体素化点云地图;
根据所述体素化点云地图构建八叉树,以获得所述初始点云地图。
9.根据权利要求2所述的位姿估计方法,其特征在于,
所述获取所述滑动窗口内的所述历史点云帧的位姿,包括:
根据所述智能设备的其他里程计,获得所述历史点云帧的初始位姿;
根据所述初始位姿,将所述历史点云帧与局部点云地图进行配准,获取所述历史点云帧与所述局部点云地图之间的面元特征之间的距离残差;
根据所述距离残差优化所述初始位姿,以获得所述历史点云帧的位姿,并根据所述位姿将所述历史点云帧加入至所述滑动窗口。
10.根据权利要求1至9中任一项所述的位姿估计方法,其特征在于,所述方法还包括根据以下步骤获取所述面元特征:
将面元上距离当前坐标系原点的最近点的3D坐标,作为所述面元的面元特征。
11.根据权利要求1至10中任一项所述的位姿估计方法,其特征在于,所述方法还包括:
当所述滑动窗口中的历史点云帧的数量大于所述预设窗口大小时,将最老的一个历史点云帧删除。
12.一种计算机可读存储介质,其中存储有多条程序代码,其特征在于,所述程序代码适于由处理器加载并运行以执行权利要求1至11中任一项所述的位姿估计方法。
13.一种智能设备,其特征在于,包括:
至少一个处理器;
以及,与所述至少一个处理器通信连接的存储器;
其中,所述存储器中存储有计算机程序,所述计算机程序被所述至少一个处理器执行时实现权利要求1至11中任一项所述的位姿估计方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311728395.2A CN117671008A (zh) | 2023-12-14 | 2023-12-14 | 位姿估计方法、可读存储介质及智能设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311728395.2A CN117671008A (zh) | 2023-12-14 | 2023-12-14 | 位姿估计方法、可读存储介质及智能设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117671008A true CN117671008A (zh) | 2024-03-08 |
Family
ID=90064052
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311728395.2A Pending CN117671008A (zh) | 2023-12-14 | 2023-12-14 | 位姿估计方法、可读存储介质及智能设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117671008A (zh) |
-
2023
- 2023-12-14 CN CN202311728395.2A patent/CN117671008A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111860493B (zh) | 一种基于点云数据的目标检测方法及装置 | |
EP3875907B1 (en) | Method, apparatus, computing device and computer-readable storage medium for positioning | |
CN110654381B (zh) | 用于控制车辆的方法和装置 | |
CN111007534A (zh) | 一种利用十六线激光雷达的障碍物检测方法及系统 | |
CN114926809A (zh) | 可通行区域检测方法及装置、移动工具、存储介质 | |
CN112712608B (zh) | 用于通过车辆收集性能数据的系统和方法 | |
CN117671013A (zh) | 点云定位方法、智能设备及计算机可读存储介质 | |
CN111476062A (zh) | 车道线检测方法、装置、电子设备及驾驶系统 | |
CN110502601B (zh) | 查找仓库的方法和装置 | |
CN117671008A (zh) | 位姿估计方法、可读存储介质及智能设备 | |
CN116244902A (zh) | 车路云融合的道路环境场景仿真方法、电子设备及介质 | |
DE102022121602A1 (de) | Objektbewegungsbahnprognose | |
CN113920490A (zh) | 车辆障碍检测方法、装置及设备 | |
CN117705125B (zh) | 定位方法、可读存储介质及智能设备 | |
CN117629254A (zh) | 融合定位偏差预警方法、存储介质及智能设备 | |
CN117788597A (zh) | 点云定位方法、可读存储介质及智能设备 | |
CN117495847B (zh) | 路口检测方法、可读存储介质及智能设备 | |
CN117496476B (zh) | 路网生成方法、可读存储介质及智能设备 | |
CN117475397B (zh) | 基于多模态传感器的目标标注数据获取方法、介质及设备 | |
CN110930714B (zh) | 位置匹配方法及装置 | |
CN112572440B (zh) | 控制车辆调头的方法、装置、设备、存储介质和程序产品 | |
CN113836964B (zh) | 车道线角点检测方法及装置 | |
CN114754778B (zh) | 一种车辆定位方法以及装置、电子设备、存储介质 | |
CN117632944A (zh) | 地图分布式闭环检测方法、装置及移动工具 | |
CN114675274A (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 |