CN116295391A - 建图定位方法及相关产品 - Google Patents

建图定位方法及相关产品 Download PDF

Info

Publication number
CN116295391A
CN116295391A CN202211088753.3A CN202211088753A CN116295391A CN 116295391 A CN116295391 A CN 116295391A CN 202211088753 A CN202211088753 A CN 202211088753A CN 116295391 A CN116295391 A CN 116295391A
Authority
CN
China
Prior art keywords
point cloud
laser
gallery
pose
scene
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
CN202211088753.3A
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 Idriverplus Technologies Co Ltd
Original Assignee
Beijing Idriverplus Technologies 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 Idriverplus Technologies Co Ltd filed Critical Beijing Idriverplus Technologies Co Ltd
Priority to CN202211088753.3A priority Critical patent/CN116295391A/zh
Publication of CN116295391A publication Critical patent/CN116295391A/zh
Pending legal-status Critical Current

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
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开建图定位方法及相关产品，其中，一种建图定位方法，包括：判断当前场景是否是长廊场景；若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算。通过在长廊场景是对长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算，从而实现了DR与激光的融合，进而提高了建图定位的精度。

Description

建图定位方法及相关产品
技术领域
本发明属于自动驾驶技术领域，尤其涉及建图定位方法及相关产品。
背景技术
无人驾驶技术是近年的热点话题，在提高产业效率、产业安全以及降低企业成本等方面，无人驾驶技术带来飞速的发展与改变。
随着无人驾驶技术在室内外环境清扫行业的不断推进，利用无人清扫车替代人工清扫已然成为未来趋势。
在室内外的环境中，经常出现长廊场景，导致无人清扫车的多线激光建图和定位精度下降，直接影响无人清扫车的清扫能力和决策规划能力。因而，如何提高无人车在长廊场景的建图和定位精度至关重要。
目前无人驾驶行业，多线激光建图与定位一般都使用激光、imu、轮速多传感器融合的方案，imu与轮速相互融合做航位推算(简称DR)，DR主要用于激光匹配的初值预测中。
目前的多传感器融合方案，不能准确应对长廊场景，在长廊场景中，激光仅仅在墙面法向存在约束，在平行墙面方向缺乏约束，造成在约束缺失的方向求解出的位姿不能反应车辆真实的位姿变化，在建图中表现为长廊变长或者变短，在定位方面表现为车辆位置与实际位置在平行长廊方向存在较大误差，而DR虽然在平行长廊方向没有类似激光的退化问题，如果完全不用激光匹配，只用DR则DR较大的累计误差会造成建图和定位的精度较差。缺陷主要在于不能解决长廊场景激光与DR的互相结合，以实现较好的建图定位精度。
发明内容
本发明实施例提供一种建图定位方法及相关产品，以解决上述技术问题中的任一。
第一方面，本发明实施例提供一种建图定位方法，包括：判断当前场景是否是长廊场景；若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算。
第二方面，本发明实施例提供一种建图定位设备，包括：判断模块，配置为判断当前场景是否是长廊场景；优化模块，配置为若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算。
第三方面，提供一种控制器，其包括：至少一个处理器，以及与所述至少一个处理器通信连接的存储器，其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行本发明任一实施例的建图定位方法的步骤。
第四方面，本发明实施例还提供一种计算机程序产品，所述计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，使所述计算机执行本发明任一实施例的建图定位方法的步骤。
第五方面，本发明实施例还提供一种移动工具，所述移动工具包括第三方面所述的控制器。
本申请，通过在长廊场景是对长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算，从而实现了DR与激光的融合，进而提高了建图定位的精度，可以很好的应对激光匹配在长廊场景中发生的退化问题，确保了在长廊环境下车辆的高效安全运行。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明一实施例提供的一种建图定位方法的流程图；
图2为本发明一实施例提供的另一种建图定位方法的流程图；
图3为本发明一实施例提供的又一种建图定位方法的流程图；
图4为本发明一实施例提供的再一种建图定位方法的流程图；
图5为本发明一实施例提供的又一种建图定位方法的流程图
图6为本发明一实施例提供的再一种建图定位方法的流程图
图7为本发明一实施例提供的建图定位方法的一个具体示例的建图定位流程图；
图8为本发明一实施例提供的建图定位执行设备的结构示意图；
图9是本发明一实施例提供的控制器的结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参考图1，其示出了本申请的建图定位方法一实施例的流程图，本实施例的建图定位方法可以适用于国际自动机工程师学会(Society of Automotive EngineersInternational，SAE International)或中国国家标准《汽车驾驶自动化分级》制定的L0-L5自动驾驶技术等级的车辆，例如可以是具有如下各种功能的车辆设备或机器人设备(以下简称“移动工具”)：
(1)载人功能，如家用轿车、公共汽车等；
(2)载货功能，如普通货车、厢式货车、甩挂车、封闭货车、罐式货车、平板货车、集装厢车、自卸货车、特殊结构货车等；
(3)工具功能，如物流配送车、自动导引运输车AGV、巡逻车、起重机、吊车、挖掘机、推土机、铲车、压路机、装载机、越野工程车、装甲工程车、污水处理车、环卫车、吸尘车、洗地车、洒水车、扫地机器人、送餐机器人、导购机器人、割草机、高尔夫球车等；
(4)娱乐功能，如娱乐车、游乐场自动驾驶装置、平衡车等；
(5)特殊救援功能，如消防车、救护车、电力抢修车、工程抢险车等。
如图1所示，在步骤101中，判断当前场景是否是长廊场景；
在步骤102中，若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算。
在本实施例中，对于步骤101中，建图定位装置判断当前场景是否是长廊场景，例如，先对获取的激光原始点云做PCA(principal components analysis，主成分分析)检测，若PCA检测出的点云最大特征值与最小特征值的比值超过预设阈值则开启墙面拟合功能，评估是否属于长廊场景。其中，PCA即主成分分析技术，又称主分量分析，旨在利用降维的思想，把多指标转化为少数几个综合指标。在统计学中，主成分分析PCA是一种简化数据集的技术。它是一个线性变换。这个变换把数据变换到一个新的坐标系统中，使得任何数据投影的第一大方差在第一个坐标(称为第一主成分)上，第二大方差在第二个坐标(第二主成分)上，依次类推。主成分分析经常用于减少数据集的维数，同时保持数据集的对方差贡献最大的特征。这是通过保留低阶主成分，忽略高阶主成分做到的。这样低阶成分往往能够保留住数据的最重要方面。
然后，对于步骤102，若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算，例如，在长廊墙面的法向使用激光约束进行特征点提取和点云残差计算得到激光点云残差项，然后在垂直于长廊墙面的法向的方向使用DR约束计算DR分量作为DR残差项，最后基于DR残差项和激光点云残差项进行位姿优化求解得到优化后的位姿。
本实施例的方法通过在长廊场景是对长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算，从而实现了DR与激光的融合，进而提高了建图定位的精度。
进一步参考图2，其示出了本申请一实施例提供的另一种建图定位方法的流程图。该流程图主要是对流程图1“判断当前环境是否是长廊环境”的流程进一步限定的步骤的流程图。
如图2所示，在步骤201中，对激光原始点云做主成分分析检测；
在步骤202中，若所述主成分分析检测出的点云的最大特征值与最小特征值的比值超过预设比值阈值，则开启墙面拟合评估是否属于长廊环境。
在本实施例中，对于步骤201，建图定位装置对激光原始点云做主成分分析检测，例如，对当前帧激光原始点云做PCA检测，判断PCA检测出的点云最大特征值与最小特征值的比值超过预设阈值。
然后，对于步骤202，若激光原始点云的主成分分析检测出的点云的最大特征值与最小特征值的比值超过预设比值阈值，则开启墙面拟合评估是否属于长廊环境，例如，长廊场景中，激光仅仅在墙面法向存在约束，在平行墙面方向缺乏约束，在建图中表现为长廊变长或者变短，在定位方面表现为车辆位置与实际位置在平行长廊方向存在较大误差，而DR虽然在平行长廊方向没有类似激光的退化问题，如果完全不用激光匹配，只用DR则DR较大的累计误差会造成建图和定位的精度较差。
本实施例的方法通过评估当前环境是否属于长廊环境，从而可以针对长廊环境很好的应对激光匹配在长廊场景中发生的退化问题，确保了长廊环境下车辆的高效安全运行。
进一步参考图3，其示出了本申请一实施例提供的又一种建图定位方法的流程图。该流程图主要是对流程图2“开启墙面拟合评估是否属于长廊环境”的流程进一步限定的步骤的流程图。
如图3所示，在步骤301中，对所述激光原始点云进行点云投影与分割得到两部分点云，对所述两部分点云分别计算第一直线方程并进行点云合并；
在步骤302中，基于合并后的点云重新进行主成分分析、点云分割和分别计算第二直线方程；
在步骤303中，基于所述第二直线方程，分别计算两个墙面的夹角和距离、以及激光雷达到所述两个墙面的距离之和；
在步骤304中，当所述夹角小于预设夹角，且所述激光雷达到所述两个墙面的距离之和与两个墙面之间的距离在预设范围内时，确定当前场景属于长廊场景。
在本实施例中，对于步骤301，建图定位装置对激光原始点云进行点云投影与分割得到左侧墙面点云和右侧墙面点云，之后，对两部分点云分别计算做直线拟合，得到第一直线方程并进行点云合并。
然后，对于步骤302，建图定位装置基于合并后的点云重新进行主成分分析、点云分割和分别计算第二直线方程，例如，使用直线方程做杂点剔除，只保留距离直线在预设阈值以内的点云认为是墙面点云，把保留下来的两个点云合并在一起，重新计算PCA，再次分割点云与拟合墙面投影线。
之后，对于步骤303，建图定位装置基于第二直线方程，分别计算两个墙面的夹角和距离、以及激光雷达到两个墙面的距离之和。
最后，对于步骤304，当夹角小于预设夹角，且激光雷达到两个墙面的距离之和与两个墙面之间的距离大致相等，其差值在预设范围内时，确定当前场景属于长廊场景，当夹角大于等于预设夹角，且激光雷达到两个墙面的距离之和与两个墙面之间的距离差距较大，其差值不在预设范围内时，确定当前场景不属于长廊场景。其中，在一个实例中，激光雷达到所述两个墙面的距离之和为d1，两个墙面之间的距离为d2，其差值在预设范围内指的是d1-d2在预设范围内，该预设范围可以设置为一个较小的值，从而在d1和d2大致相等时确定当前场景属于长廊场景，在此不再赘述。
本实施例的方法通过对点云进行墙面拟合以及二次墙面拟合，从而可以实现准确的判断当前环境是否属于长廊环境，确保了长廊环境下车辆的高效安全运行。
进一步参考图4，其示出了本申请一实施例提供的再一种建图定位方法的流程图。该流程图主要是对流程图3“对所述激光原始点云进行点云投影与分割得到两部分点云，对所述两部分点云分别计算第一直线方程并进行点云合并”的流程进一步限定的步骤的流程图。
如图4所示，在步骤401中，使用所述激光原始点云的点云坐标种的高度值去除地面点云得到剩余点云；
在步骤402中，将剩余点云投影至XY平面，计算投影后的XY平面上的点云中心P；
在步骤403中，将主成分分析检测出的点云的最大特征值对应的特征向量投影至XY平面上的点记为向量V，基于所述P和所述向量V确定XY平面上的分割直线，基于所述分割直线将XY平面上的点云分割为两部分点云；
在步骤404中，对所述两部分点云分别进行直线拟合，得到所述两部分点云分别对应的第一直线方程；
在步骤405中，基于两个第一直线方程分别保留距离任一第一直线方程在预设距离阈值范围内的点云并合并所述点云。
在本实施例中，对于步骤401，建图定位装置使用激光原始点云的点云坐标种的高度值去除地面点云得到剩余点云，例如，高度值小于预设阈值的认为是地面点云，把这部分点云去掉。
然后，对于步骤402，建图定位装置将剩余点云投影至XY平面，计算投影后的XY平面上的点云中心P，也就是将投影后的XY平面上的点云中心记为P，其中，车辆的前向为X，车辆的左向为Y，车辆的高度为Z，投影至车辆系的意思就是去掉了高度Z。
之后，对于步骤403，建图定位装置将主成分分析检测出的点云的最大特征值对应的特征向量投影至XY平面上的点记为向量V，基于点云中心P和向量V确定XY平面上的分割直线，基于分割直线将XY平面上的点云分割为两部分点云，这两部分点云分别是左侧墙面点云和右侧墙面点云在XY平面上的投影。
然后，对于步骤404，建图定位装置对两部分点云分别进行直线拟合，得到两部分点云分别对应的第一直线方程，例如，使用随机抽样一致性(RANSAC，Random SampleConsensus)对投影到XY平面上的两个点云做分别做直线拟合，得到直线方程，需要设置RANSAC的相关参数，要拟合模型是直线方程，其中，RANSAC是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。RANSAC算法的基本假设是样本中包含正确数据(inliers，可以被模型描述的数据)，也包含异常数据(outliers，偏离正常范围很远、无法适应数学模型的数据)，即数据集中含有噪声。这些异常数据可能是由于错误的测量、错误的假设、错误的计算等产生的。同时RANSAC也假设，给定一组正确的数据，存在可以计算出符合这些数据的模型参数的方法。
最后，对于步骤405，建图定位装置基于两个第一直线方程分别保留距离任一第一直线方程在预设距离阈值范围内的点云并合并点云，例如，使用直线方程对点云做杂点剔除，只保留距离直线在预设阈值以内的点云认为是墙面点云，把保留下来的两个点云合并在一起。
本实施例通过对激光原始点云进行点云投影与分割得到两部分点云，对两部分点云分别计算第一直线方程并进行点云合并，从而可以完成墙面拟合，准确的评估当前环境是否属于长廊环境。
进一步参考图5，其示出了本申请一实施例提供的又一种建图定位方法的流程图。该流程图主要是对流程图4“在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束”的进行位姿求解计算进一步限定的步骤的流程图。
如图5所示，在步骤501中，使用激光约束进行特征点提取和点云残差计算得到激光点云残差项；
在步骤502中，在垂直所述长廊墙面的法向使用DR约束计算DR分量作为DR残差项；
在步骤503中，基于所述DR残差项和所述激光点云残差项进行位姿优化求解得到优化后的位姿。
在本实施例中，对于步骤501，建图定位装置使用激光约束进行特征点提取和点云残差计算得到激光点云残差项。具体的，对激光点云进行特征点提取，并计算点云残差，得到激光点云残差项，之后使用最小化残差项来做优化，也可以认为是残差项提供了约束。例如，对当前帧激光点云做特征点提取，使用待优化求解的目标位姿Pose把特征点转到全局坐标系下，遍历当前帧所有的特征点，找到某点在上一帧中最近的5个点，并计算点线或点面残差，也就是找最近点时注意区分平面点、边角点，每一项残差都是图优化中的一条边。
然后，对于步骤502，建图定位装置在垂直长廊墙面的法向使用DR约束计算DR分量作为DR残差项，例如，使用图优化的方式进行优化求解，当前的位姿是待优化求解的目标位姿Pose，也是图优化中的顶点，DR递推出来的位姿与待优化求解的目标位姿Pose的差异构成DR的残差项，即图优化中的一条边，称为DR边。
最后，对于步骤503，建图定位装置基于DR残差项和激光点云残差项进行位姿优化求解得到优化后的位姿，例如，可以对DR的残差项和激光点云的残差项的信息矩阵进行加权得到优化后的位姿，协方差矩阵的逆，协方差矩阵能够由使用者自己设置。
本实施例的方法通过DR残差项和激光点云残差项进行位姿优化求解得到优化后的位姿，从而实现了DR与激光的融合，大幅度提高建图定位的精度。
在上述实施例方法中，所述基于所述DR残差项和所述激光点云残差项进行位姿优化求解得到优化后的位姿包括：
使用图优化的方式进行位姿优化求解，其中，每一残差项都是图优化中的一条边。
进一步参考图6，其示出了本申请一实施例提供的再一种建图定位方法的流程图。该流程图主要是对流程图4“基于所述DR残差项和所述激光点云残差项进行位姿优化求解得到优化后的位姿”的流程进一步限定的步骤的流程图。
如图6所示，在步骤601中，将所述向量V转到全局坐标系下记为DR权值；
在步骤602中，将所述向量V的法向转到全局坐标系下记为激光权值；
在步骤603中，基于所述DR权值对所述DR残差项的信息矩阵进行加权，基于所述激光权值对所述激光点云残差的信息矩阵进行加权得到优化后的位姿。
在本实施例中，对于步骤601，建图定位装置将向量V转到全局坐标系下记为DR权值，例如，可以使用待优化求解的目标Pose把向量V转到全局坐标系下记为Vw，此处要把V从2维转为3维度,第3维Z方向补0。
然后，对于步骤602，建图定位装置将向量V的法向转到全局坐标系下记为激光权值，例如，在XY平面上取向量V的法向计为Vn,用Pose把向量Vn转到全局坐标系下记为Vnw，此处需要把Vn从2维转为3维度,第3维Z方向补0。
最后，对于步骤603，建图定位装置基于DR权值对DR残差项的信息矩阵进行加权，基于激光权值对激光点云残差的信息矩阵进行加权得到优化后的位姿，例如，使用Vw对DR信息矩阵MDRinfo，公式如下：
MDRinfo＝diag(Vw)*MDRinfo
使用Vnw对激光点云信息矩阵MLinfo加权，公式如下：
MLinfo＝diag(Vnw)*MLinfo
在一具体实施例中，可以使用第三方优化库，能够使用自动求导，省去了手动求解残差项对应的雅克比。
本实施例的方法通过将向量V的法向转到全局坐标系下记为激光权值，然后将向量V转到全局坐标系下记为DR权值，之后基于DR权值对DR残差项的信息矩阵进行加权，最后基于激光权值对激光点云残差的信息矩阵进行加权得到优化后的位姿，从而实现了DR与激光的融合，进而提高了建图定位的精度。
请参考图7，其示出了本发明一实施例提供的建图定位方法的一个具体示例的建图定位流程图。
如图7所示，第一步，对当前帧激光原始点云做PCA检测(主成分分析)，若PCA检测出的点云最大特征值与最小特征值的比值超过阈值3则开启墙面拟合功能，评估是否属于长廊场景。
第二步，点云投影与分割，使用点云坐标中的高度值(Z值)去除地面点云，高度值小于阈值0.1m的认为是地面点云，把这部分点云去掉。剩下的点云投影到XY平面。PCA检测出来的最大特征值对应的特征向量也投影到XY平面上计为V，投影后的XY平面上的点云中心记为P，P和V确定XY平面上一条直线，该直线把XY平面上的点云分割为两部分，分别是左侧墙面点云和右侧墙面点云在XY平面上的投影。
第三步，墙面拟合，使用随机抽样一致性(RANSAC)对投影到XY平面上的两个点云做分别做直线拟合，得到直线方程，需要设置RANSAC的相关参数，要拟合模型是直线方程，拟合成功率设.0999，最大外点比例.05，外点阈值0.03m。
第四步，二次墙面拟合，使用直线方程做杂点剔除，只保留距离直线在阈值0.03m以内的点云认为是墙面点云，把保留下来的两个点云合并在一起，重新计算PCA,再次分割点云、拟合墙面投影线。根据墙面在XY平面的投影线方程，计算这两个墙面的夹角、距离，以及激光原点到两个墙面的距离。当墙面夹角较小，激光雷达到两个墙面的距离之和约等于两个墙面距离时，认为是长廊场景。
第五步，当符合长廊场景时：
使用图优化的方式进行优化求解，当前的位姿是待优化求解的目标Pose，也是图优化中的顶点，DR递推出来的位姿态与Pose的差异构成DR的残差项，即图优化中的一条边，称为DR边。对当前帧点云做特征点提取，使用Pose把特征点转到全局坐标系下，遍历当前帧所有特征点，找到某的点在上一帧中最近的5个点,并计算点线或点面残差(找最近点时注意区分平面点、边角点)每一项残差都是图优化中的一条边。关键是对DR的残差项、激光点云的残差项的信息矩阵(协方差矩阵的逆，协方差矩阵需要使用者自己设置)进行加权，使用Pose把向量V(此处需要把V从2维转为3维度，第3维Z方向补0)转到全局坐标系下记为Vw，使用Vw对DR信息矩阵MDRinfo加权，在XY平面上取向量V的法向计为Vn，用Pose把向量Vn(此处需要把Vn从2维转为3维度，第3维Z方向补0)转到全局坐标系下记为Vnw，使用Vnw对激光点云信息矩阵MLinfo加权。
请参考图8，其示出了本发明一实施例提供的一种建图定位执行设备的结构示意图。
如图8所示，建图定位设备800，包括判断模块810和优化模块820。
其中，判断模块810，配置为判断当前场景是否是长廊场景；优化模块820，配置为若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算。
应当理解，图8中记载的诸模块与参考图1、图2、图3、图4、图5和图6中描述的方法中的各个步骤相对应。由此，上文针对方法描述的操作和特征以及相应的技术效果同样适用于图8中的诸模块，在此不再赘述。
值得注意的是，本公开的实施例中的模块并不用于限制本公开的方案，例如判断模块可以描述为判断当前场景是否是长廊场景的模块。另外，还可以通过硬件处理器来实现相关功能模块，例如判断模块也可以用处理器实现，在此不再赘述。
在另一些实施例中，本发明实施例还提供了一种非易失性计算机存储介质，计算机存储介质存储有计算机可执行指令，该计算机可执行指令可执行上述任意方法实施例中的建图定位方法；
作为一种实施方式，本发明的非易失性计算机存储介质存储有计算机可执行指令，计算机可执行指令设置为：
判断当前场景是否是长廊场景；
若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算。
非易失性计算机可读存储介质可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据建图定位装置的使用所创建的数据等。此外，非易失性计算机可读存储介质可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，非易失性计算机可读存储介质可选包括相对于处理器远程设置的存储器，这些远程存储器可以通过网络连接至建图定位装置。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
本发明实施例还提供一种计算机程序产品，计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，计算机程序包括程序指令，当程序指令被计算机执行时，使计算机执行上述任一项建图定位方法。
图9是本发明实施例提供的控制器的结构示意图，如图9所示，该设备包括：一个或多个处理器910以及存储器920，图9中以一个处理器910为例。实现建图定位方法的控制器还可以包括：输入装置930和输出装置940。处理器910、存储器920、输入装置930和输出装置940可以通过总线或者其他方式连接，图9中以通过总线连接为例。存储器920为上述的非易失性计算机可读存储介质。处理器910通过运行存储在存储器920中的非易失性软件程序、指令以及模块，从而执行服务器的各种功能应用以及数据处理，即实现上述方法实施例建图定位方法。输入装置930可接收输入的数字或字符信息，以及产生与控制器的用户设置以及功能控制有关的键信号输入。输出装置940可包括显示屏等显示设备。
本发明实施例提供的控制器可实现为应用于移动工具上的自动驾驶域控制器，本发明实施例提供的建图定位方法可以由移动工具的自动驾驶域控制器执行，其中，自动驾驶域控制器与移动工具上搭载的各种传感器(包括但不限于摄像机、激光雷达等)通信连接，通过这些传感器获取环境感知数据和车身速度信息，并根据获取的环境感知数据和车身速度信息等提取障碍物信息及生成移动工具的位置信息，以及根据位置信息和障碍物信息进行路径规划。
作为一种实施方式，上述控制器应用于建图定位设备中，包括：至少一个处理器；以及，与至少一个处理器通信连接的存储器；其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够：
判断当前场景是否是长廊场景；
若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算。
本申请实施例的控制器还可以以其他多种形式存在，包括但不限于：
(1)移动通信设备：这类设备的特点是具备移动通信功能，并且以提供话音、数据通信为主要目标。这类终端包括:智能手机(例如iPhone)、多媒体手机、功能性手机，以及低端手机等。
(2)超移动个人计算机设备：这类设备属于个人计算机的范畴，有计算和处理功能，一般也具备移动上网特性。这类终端包括：PDA、MID和UMPC设备等，例如iPad。
(3)便携式娱乐设备：这类设备可以显示和播放多媒体内容。该类设备包括:音频、视频播放器(例如iPod)，掌上游戏机，电子书，以及智能玩具和便携式车载导航设备。
(4)服务器:提供计算服务的设备，服务器的构成包括处理器、硬盘、内存、系统总线等，服务器和通用的计算机架构类似，但是由于需要提供高可靠的服务，因此在处理能力、稳定性、可靠性、安全性、可扩展性、可管理性等方面要求较高。
(5)其他具有数据交互功能的电子装置。
以上所描述的装置实施例仅仅是示意性的，其中作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (11)

1.一种建图定位方法，包括：
判断当前场景是否是长廊场景；
若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算。
2.根据权利要求1所述的方法，其特征在于，所述判断当前环境是否是长廊环境包括：
对激光原始点云做主成分分析检测；
若所述主成分分析检测出的点云的最大特征值与最小特征值的比值超过预设比值阈值，则开启墙面拟合评估是否属于长廊环境。
3.根据权利要求2所述的方法，其特征在于，所述开启墙面拟合评估是否属于长廊环境包括：
对所述激光原始点云进行点云投影与分割得到两部分点云，对所述两部分点云分别计算第一直线方程并进行点云合并；
基于合并后的点云重新进行主成分分析、点云分割和分别计算第二直线方程；
基于所述第二直线方程，分别计算两个墙面的夹角和距离、以及激光雷达到所述两个墙面的距离之和；
当所述夹角小于预设夹角，且所述激光雷达到所述两个墙面的距离之和与两个墙面之间的距离在预设范围内时，确定当前场景属于长廊场景。
4.根据权利要求3所述的方法，其特征在于，所述对所述激光原始点云进行点云投影与分割得到两部分点云，对所述两部分点云分别计算第一直线方程并进行点云合并包括：
使用所述激光原始点云的点云坐标种的高度值去除地面点云得到剩余点云；
将剩余点云投影至XY平面，计算投影后的XY平面上的点云中心P；
将主成分分析检测出的点云的最大特征值对应的特征向量投影至XY平面上的点记为向量V，基于所述P和所述向量V确定XY平面上的分割直线，基于所述分割直线将XY平面上的点云分割为两部分点云；
对所述两部分点云分别进行直线拟合，得到所述两部分点云分别对应的第一直线方程；
基于两个第一直线方程分别保留距离任一第一直线方程在预设距离阈值范围内的点云并合并所述点云。
5.根据权利要求1所述的方法，其特征在于，所述在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算包括：
使用激光约束进行特征点提取和点云残差计算得到激光点云残差项；
在垂直于长廊墙面的法向的方向使用DR约束计算DR分量作为DR残差项；
基于所述DR残差项和所述激光点云残差项进行位姿优化求解得到优化后的位姿。
6.根据权利要求5所述的方法，其特征在于，所述基于所述DR残差项和所述激光点云残差项进行位姿优化求解得到优化后的位姿包括：
使用图优化的方式进行位姿优化求解，其中，每一残差项都是图优化中的一条边。
7.根据权利要求5所述的方法，其特征在于，所述基于所述DR残差项和所述激光点云残差项进行位姿优化求解得到优化后的位姿包括：
将所述向量V转到全局坐标系下记为DR权值；
将所述向量V的法向转到全局坐标系下记为激光权值；
基于所述DR权值对所述DR残差项的信息矩阵进行加权，基于所述激光权值对所述激光点云残差的信息矩阵进行加权得到优化后的位姿。
8.一种建图定位设备，包括：
判断模块，配置为判断当前场景是否是长廊场景；
优化模块，配置为若当前场景是长廊场景，在长廊墙面的法向使用激光约束、以及在垂直于长廊墙面的法向的方向使用DR约束，进行位姿求解计算。
9.一种控制器，其包括：至少一个处理器，以及与所述至少一个处理器通信连接的存储器，其中，所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1至7任一项所述方法的步骤。
10.一种存储介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现权利要求1至7任一项所述方法的步骤。
11.一种移动工具，其特征在于，所述移动工具包括权利要求9所述的控制器。
CN202211088753.3A 2022-09-07 2022-09-07 建图定位方法及相关产品 Pending CN116295391A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211088753.3A CN116295391A (zh) 2022-09-07 2022-09-07 建图定位方法及相关产品

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211088753.3A CN116295391A (zh) 2022-09-07 2022-09-07 建图定位方法及相关产品

Publications (1)

Publication Number Publication Date
CN116295391A true CN116295391A (zh) 2023-06-23

Family

ID=86813637

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211088753.3A Pending CN116295391A (zh) 2022-09-07 2022-09-07 建图定位方法及相关产品

Country Status (1)

Country Link
CN (1) CN116295391A (zh)

Similar Documents

Publication Publication Date Title
EP3660618B1 (en) Map building and positioning of robot
CN115235500B (zh) 基于车道线约束的位姿校正方法及装置、全工况静态环境建模方法及装置
CN112179353B (zh) 自移动机器人的定位方法、装置、机器人及可读存储介质
CN112154449A (zh) 车道线融合方法、车道线融合装置、车辆和存储介质
CN112880694B (zh) 确定车辆的位置的方法
WO2021017072A1 (zh) 基于激光雷达的 slam 闭环检测方法及检测系统
CN114926809A (zh) 可通行区域检测方法及装置、移动工具、存储介质
CN115143996B (zh) 定位信息修正方法及电子设备和存储介质
Berrio et al. Identifying robust landmarks in feature-based maps
WO2022078342A1 (zh) 动态占据栅格估计方法及装置
CN114387576A (zh) 一种车道线识别方法、系统、介质、设备及信息处理终端
US20210398014A1 (en) Reinforcement learning based control of imitative policies for autonomous driving
CN116295391A (zh) 建图定位方法及相关产品
CN112987709A (zh) 一种路径规划方法、系统及作业机器人
CN116358528A (zh) 地图更新方法、地图更新装置、自移动设备及存储介质
CN117408935A (zh) 障碍物检测方法、电子设备和存储介质
CN115269763B (zh) 局部点云地图更新维护方法、装置、移动工具及存储介质
CN117804421A (zh) 栅格地图更新方法及相关产品
CN117908035A (zh) 用于动态场景的单线激光定位方法、移动装置和存储介质
CN117804434A (zh) 用于室内建图的前端里程计确定方法及设备和存储介质
WO2024036984A1 (zh) 目标定位方法及相关系统、存储介质
CN117109602A (zh) 实时回环检测方法、装置、计算设备、移动工具、存储介质、实时地图构建方法及装置
CN117456498A (zh) 物体的动静态估计方法、设备、移动装置和存储介质
CN114739411A (zh) 一种室内定位方法、系统及移动工具
CN117853851A (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