CN111968179A - 地下停车场自动驾驶车辆定位方法 - Google Patents
地下停车场自动驾驶车辆定位方法 Download PDFInfo
- Publication number
- CN111968179A CN111968179A CN202010814965.XA CN202010814965A CN111968179A CN 111968179 A CN111968179 A CN 111968179A CN 202010814965 A CN202010814965 A CN 202010814965A CN 111968179 A CN111968179 A CN 111968179A
- Authority
- CN
- China
- Prior art keywords
- plane
- point cloud
- group
- normal vector
- primitive
- 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
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
- 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
-
- 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/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种地下停车场自动驾驶车辆定位方法及介质,其中方法包括:获取地下停车场的全局点云数据,并进行预处理,以过滤车辆三维点,得到初始点云数据;根据区域生长法对初始点云数据进行平面结构提取,以得到初始平面集合,并对初始平面集合进行优化,以得到最终平面集合;根据层次聚类法对最终平面集合进行划分,以生成多个第一平面基元组;获取自动驾驶车辆采集到的短期点云数据,并进行处理,以生成多个第二平面基元组;进行点云配准,并对配准结果进行优化,以生成自动驾驶车辆定位结果;能够在地下停车场等室内环境下对自动驾驶车辆进行精准定位,以保障自动驾驶车辆在无GPS定位的条件下安全有效运行。
Description
技术领域
本发明涉及无人驾驶技术领域,特别涉及一种地下停车场自动驾驶车辆定位方法和一种计算机可读存储介质。
背景技术
精准定位,是自动驾驶汽车在复杂动态环境下安全有效运行所必须要解决的问题。在室外,可以依靠GPS技术对自动驾驶汽车进行定位;然而,由于室内环境无GPS信号,使得室内环境中自动驾驶车辆的精准定位的难度较高。
相关技术中,在对自动驾驶汽车进行室内定位的过程中,多采用无线、测量传感器或者基于视觉的方法;然而,采用无线的方法存在多路径效应,定位结果不稳定;而基于测量传感器的方法具有较大的漂移误差和累积误差;基于视觉的方法受光线和障碍物影响大;同时,这些方法均需要大量的存储空间和车辆位置的先验信息。
并且,地下停车场中通常照明不佳,加上是人工建筑,其中有大量的平面结构,如墙面,地面,柱子的侧面等。因此,如何克服现有定位方法存在的缺陷,提高地下停车场中自动驾驶车辆的定位精确度具有很重要的现实意义。
发明内容
本发明旨在至少在一定程度上解决上述技术中的技术问题之一。为此,本发明的一个目的在于提出一种地下停车场自动驾驶车辆定位方法,能够在地下停车场等室内环境下对自动驾驶车辆进行精准定位,以保障自动驾驶车辆在无GPS定位的条件下安全有效运行。
本发明的第二个目的在于提出一种计算机可读存储介质。
为达到上述目的,本发明第一方面实施例提出了一种地下停车场自动驾驶车辆定位方法,包括以下步骤:获取地下停车场的全局点云数据,并对所述全局点云数据进行预处理,以过滤所述全局点云数据中的车辆三维点,得到初始点云数据;根据区域生长法对所述初始点云数据进行平面结构提取,以得到初始平面集合,并根据局部K均值聚类对所述初始平面集合进行优化,以得到最终平面集合;根据层次聚类法对最终平面集合进行划分,以生成内部存在平行关系的多个第一平面基元组;获取自动驾驶车辆采集到的短期点云数据,并对所述短期点云数据进行处理,以生成所述短期点云数据对应的多个第二平面基元组;根据所述第一平面基元组和所述第二平面基元组进行点云配准,并对配准结果进行优化,以生成自动驾驶车辆定位结果。
根据本发明实施例的地下停车场自动驾驶车辆定位方法,首先,获取地下停车场的全局点云数据,并对全局点云数据进行预处理,以过滤全局点云数据中的车辆三维点,得到初始点云数据;接着,根据区域生长法对初始点云数据进行平面结构提取,以得到初始平面集合,并根据局部K均值聚类对初始平面集合进行优化,以得到最终平面集合;然后,根据层次聚类法对最终平面集合进行划分,以生成内部存在平行关系的多个第一平面基元组;接着,获取自动驾驶车辆采集到的短期点云数据,并对短期点云数据进行处理,以生成短期点云数据对应的多个第二平面基元组;然后,根据第一平面基元组和第二平面基元组进行点云配准,并对配准结果进行优化,以生成自动驾驶车辆定位结果;从而实现在地下停车场等室内环境下对自动驾驶车辆进行精准定位,以保障自动驾驶车辆在无GPS定位的条件下安全有效运行。
另外,根据本发明上述实施例提出的地下停车场自动驾驶车辆定位方法还可以具有如下附加的技术特征:
可选地,对所述全局点云数据进行预处理,包括:根据预设尺寸对所述全局点云数据进行切分,以生成多个点云块,并对多个点云块进行水平分层,以及计算分层后每一层点云数据所对应的三维点点数;根据每一层点云数据对应的高度自底层向上依次判断该层点云数据所对应的三维点点数是否大于预设点数阈值,并在判断结果为是时,将该层点云数据作为地面层,以及将该层点云数据对应的高度作为地面高度值;根据八叉树索引结构对多个点云块进行划分,以生成空间上连续的多个点云体素,并根据多个点云体素进行向上生长,以确定局部高度值最大的点云体素Vh和Vh所对应的高度值h;判断h的值是否大于地面高度值且小于预设车辆高度阈值,并在判断结果为是时,确定Vh中的三维点为车辆点,以及滤除包含Vh的点云块中的所有三维点。
可选地,根据区域生长法对所述初始点云数据进行平面结构提取,以得到初始平面集合,并根据局部K均值聚类对所述初始平面集合进行优化,以得到最终平面集合,包括:寻找所述初始点云数据中每个三维点的K近邻点,并根据主成分分析法计算每个三维点所对应的最佳拟合初始切平面,以及根据迭代重加权的最小二乘法对所述最佳拟合初始切平面进行优化,以得到每个三维点对应的最终切平面和每个三维点对应的平滑度;对三维点对应的平滑度进行排序,并根据排序结果迭代地选择其中平滑度最高且未被访问过的三维点作为种子点;根据每个种子点进行区域生长,以生成每个种子点对应的初始平面,并根据每个种子点对应的初始平面生成初始平面集合,以及根据局部K均值聚类对所述初始平面集合进行优化,以得到最终平面集合。
可选地,在根据所述生成内部存在平行关系的多个第一平面基元组之后,还包括:计算每个第一平面基元组对应的平面法向量平均值,并计算第一平面基元组中平面法向量与对应平面法向量平均值之间的差值,以及将最小差值所对应的平面法向量作为该第一平面基元组的组法向量;计算任意两个第一平面基元组的组法向量之间的夹角。
可选地,根据所述第一平面基元组和所述第二平面基元组进行点云配准,包括:采用在旋转平移中具有不变性的二面角作为相似性测度,根据多个第一平面基元组中任意两个第一平面基元组的组法向量之间的夹角和多个第二平面基元组中任意两个第二平面基元组的组法向量之间的夹角寻找所有具有相似二面角的平面基元组对,以生成潜在的对应平面基元组对;计算所述平面基元组对所对应的旋转矩阵,并根据所述旋转矩阵获取对应平面对,以及根据所述对应平面对计算相应的平移向量;对所述平移向量和所述旋转矩阵进行几何一致性评估,并根据评估结果和迭代最近点算法确定最终配准结果。
可选地,根据以下公式进行最佳拟合初始切平面和最终切平面的计算:
可选地,根据以下公式寻找多个第一平面基元组中和多个第二平面基元组中具有相似二面角的平面基元组对:
其中,μ表示第一角度阈值,tθ表示第二角度阈值,表示多个第一平面基元组中第i个第一平面基元组的组法向量,表示多个第一平面基元组中第j个第一平面基元组的组法向量,表示多个第二平面基元组中第m个第二平面基元组的组法向量,表示多个第二平面基元组中第n个第二平面基元组的组法向量,θij表示第i个第一平面基元组的组法向量与第j个第一平面基元组的组法向量之间的夹角,θmn表示第m个第二平面基元组的组法向量与第n个第二平面基元组的组法向量之间的夹角。
可选地,根据以下公式进行旋转矩阵和平移向量的计算:
其中,axis1、axis2表示旋转轴,angle1、angle2表示旋转角,Ri1、Ri2、Ri表示旋转矩阵,toRotationMatrix()表示根据旋转轴和旋转角计算得到对应旋转矩阵的函数,cos-1()表示反余弦函数,表示第i个第一平面基元组的组法向量,表示第j个第一平面基元组的组法向量,表示第m个第二平面基元组的组法向量,表示第n个第二平面基元组的组法向量;
其中,表示第k个第一平面基元组的组法向量,Ri表示旋转矩阵,表示第l个第二平面基元组的组法向量,ξ表示一个值近似为1的标量,第k个第一平面基元组的组法向量所对应的平面与第l个第二平面基元组的组法向量所对应的平面所组成的平面对即为对应平面对;
ti=N-1D
其中,N和D为3×3矩阵,ti表示平移向量,表示第i个第一平面基元组的组法向量,表示第j个第一平面基元组的组法向量,表示第k个第一平面基元组的组法向量,表示原点到第i个第一平面基元组的组法向量所对应的平面的正交距离,表示原点到第j个第一平面基元组的组法向量所对应的平面的正交距离,表示原点到第k个第一平面基元组的组法向量所对应的平面的正交距离,表示原点到第m个第二平面基元组的组法向量所对应的平面的正交距离,表示原点到第n个第二平面基元组的组法向量所对应的平面的正交距离,表示原点到第l个第二平面基元组的组法向量所对应的平面的正交距离。
可选地,根据以下公式进行几何一致性评估:
其中,ξ表示一个值近似为1的标量,η表示标量阈值,表示第a个第一平面基元组的组法向量,表示第b个第二平面基元组的组法向量,Ri表示旋转矩阵,ti表示平移向量,表示原点到第a个第二平面基元组的组法向量所对应的平面的正交距离,表示原点到第b个第二平面基元组的组法向量所对应的平面的正交距离。
为达到上述目的,本发明第二方面实施例提出了一种计算机可读存储介质,其上存储有地下停车场自动驾驶车辆定位程序,该地下停车场自动驾驶车辆定位程序被处理器执行时实现如上述的地下停车场自动驾驶车辆定位方法。
根据本发明实施例的计算机可读存储介质,通过存储地下停车场自动驾驶车辆定位程序,以使得处理器在执行该地下停车场自动驾驶车辆定位程序时,实现如上述的地下停车场自动驾驶车辆定位方法;从而实现在地下停车场等室内环境下对自动驾驶车辆进行精准定位,以保障自动驾驶车辆在无GPS定位的条件下安全有效运行。
附图说明
图1为根据本发明实施例的地下停车场自动驾驶车辆定位方法的流程示意图;
图2为根据本发明实施例的初始点云数据示意图;
图3为根据本发明实施例的最终平面集合参考图,其中(a)为短期点云数据对应的最终平面集合参考图,(b)为全局点云数据对应的最终平面集合参考图;
图4为根据本发明实施例的点云配准局部结果示意图;
图5为根据本发明实施例的优化配准结果示意图;
图6为根据本发明实施例的ground truth验证结果示意图。
具体实施方式
下面详细描述本发明的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本发明,而不能理解为对本发明的限制。
相关技术中,难以在地下停车场等无GPS定位环境下对自动驾驶车辆进行稳定有效的精准定位,根据本发明实施例的地下停车场自动驾驶车辆定位方法,首先,获取地下停车场的全局点云数据,并对全局点云数据进行预处理,以过滤全局点云数据中的车辆三维点,得到初始点云数据;接着,根据区域生长法对初始点云数据进行平面结构提取,以得到初始平面集合,并根据局部K均值聚类对初始平面集合进行优化,以得到最终平面集合;然后,根据层次聚类法对最终平面集合进行划分,以生成内部存在平行关系的多个第一平面基元组;接着,获取自动驾驶车辆采集到的短期点云数据,并对短期点云数据进行处理,以生成短期点云数据对应的多个第二平面基元组;然后,根据第一平面基元组和第二平面基元组进行点云配准,并对配准结果进行优化,以生成自动驾驶车辆定位结果;从而实现在地下停车场等室内环境下对自动驾驶车辆进行精准定位,以保障自动驾驶车辆在无GPS定位的条件下安全有效运行。
为了更好的理解上述技术方案,下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例,然而应当理解,可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更透彻地理解本发明,并且能够将本发明的范围完整的传达给本领域的技术人员。
为了更好的理解上述技术方案,下面将结合说明书附图以及具体的实施方式对上述技术方案进行详细的说明。
图1为根据本发明实施例的地下停车场自动驾驶车辆定位方法,如图1所示,该地下停车场自动驾驶车辆定位方法包括以下步骤:
S101,获取地下停车场的全局点云数据,并对全局点云数据进行预处理,以过滤全局点云数据中的车辆三维点,得到初始点云数据。
也就是说,通过雷达传感器对地下停车场进行扫描,以获取整个地下停车场的完整三维点云模型(即全局点云数据);进而,对获取到的全局点云数据进行预处理,以过滤掉全局点云数据中的车辆三维点,得到用于后续运算的初始点云数据。
其中,对全局点云数据进行预处理的方式可以有多种。
在一些实施例中,对全局点云数据进行预处理,包括:根据预设尺寸对全局点云数据进行切分,以生成多个点云块,并对多个点云块进行水平分层,以及计算分层后每一层点云数据所对应的三维点点数;
根据每一层点云数据对应的高度自底层向上依次判断该层点云数据所对应的三维点点数是否大于预设点数阈值,并在判断结果为是时,将该层点云数据作为地面层,以及将该层点云数据对应的高度作为地面高度值;
根据八叉树索引结构对多个点云块进行划分,以生成空间上连续的多个点云体素,并根据多个点云体素进行向上生长,以确定局部高度值最大的点云体素Vh和Vh所对应的高度值h;
判断h的值是否大于地面高度值且小于预设车辆高度阈值,并在判断结果为是时,确定Vh中的三维点为车辆点,以及滤除包含Vh的点云块中的所有三维点。
作为一种示例,如图2所示,图2为根据本发明实施例的预处理方式处理后的初始点云数据示意图(为了便于展示,该图中去除了场景中的屋顶);首先,将全局点云数据中的三维点在XY平面内按照预设的长度和宽度进行垂直切分,以切分成多个点云块Blocki,i=1,2,...,Nb,其中,Nb为点云块的总数;
接着,根据每个点云块Blocki所对应的高度值对多个点云块进行水平分层,并计算每一层点云块所对应的三维点的数量;然后,从底层开始向上依次判断每一层点云块所对应的三维点的数量是否大于预设点数阈值nl,如果是,则将该层作为地面层;即言,将具有nl个以上点数且与底层最近邻的层作为地面层;并且,将该层所对应的高度作为地面高度值;
接着,对于每个点云块Blocki,利用八叉树索引结构将其重新划分为一系列空间上连续的多个点云体素;然后,根据多个点云体素进行向上增长,每个体素向上生长到其上的9个邻域;其邻域又以相同的方式继续向上生长,直至向上无法找到9个邻域用于生长为止;
然后,计算已生长区域中具有最大局部高度的点云体素Vh(即该生长区域的至高点)以及Vh对应的高度值h;接着,判断高度值h是否大于地面高度值且小于预设车辆高度阈值,如果是,则认为点云体素Vh中的三维点均为车辆三维点,同时,滤除包含有点云体素Vh的点云块中的所有三维点;而如果高度值h小于地面高度值,则将点云体素Vh中的点提取为地面点;如果高度值h大于预设车辆高度阈值,则将点云体素Vh中的点标记为非地面点。
S102,根据区域生长法对初始点云数据进行平面结构提取,以得到初始平面集合,并根据局部K均值聚类对初始平面集合进行优化,以得到最终平面集合。
即言,根据区域生长法对初始点云数据进行平面结构提取,以提取出初始点云数据中包含的平面结构(例如,墙面、地面、柱子的侧面等),并根据提取结果生成初始平面集合;进一步地,根据局部K均值聚类对初始平面集合进行优化,以输出最终平面集合。
在一些实施例中,根据区域生长法对初始点云数据进行平面结构提取,以得到初始平面集合,并根据局部K均值聚类对初始平面集合进行优化,以得到最终平面集合,包括:
寻找所述初始点云数据中每个三维点的K近邻点,并根据主成分分析法计算每个三维点所对应的最佳拟合初始切平面,以及根据迭代重加权的最小二乘法对所述最佳拟合初始切平面进行优化,从而得到每个三维点对应的最终切平面和每个三维点对应的平滑度;
对三维点对应的平滑度进行排序,并根据排序结果迭代地选择其中平滑度最高且未被访问过的三维点作为种子点;;
根据每个种子点进行区域生长,以生成每个种子点对应的初始平面,并根据每个种子点对应的初始平面生成初始平面集合,以及根据局部K均值聚类对初始平面集合进行优化,以得到最终平面集合。
作为一种示例,首先,将初始点云数据中的每个三维点pi所对应的初始切平面Tpi定义为:
Tpi=(oi,ni)
其中,
oi是初始切平面Tpi的中心点,ni是初始切平面Tpi的单位法向量;
接着,使用主成分分析法求解以下等式获得最小二乘意义下的拟合切平面:
然后,通过迭代重加权的最小二乘法对拟合切平面进行优化,以得到最终切平面和每个三维点对应的平滑度:
接着,当初始点云数据中所有三维点所对应的最终切平面均计算完毕后,对三维点对应的平滑度进行排序,并根据排序结果迭代地选择其中平滑度最高且未被访问过的三维点作为种子点;即言,根据每个三维点所对应的平滑度的值大小进行排序,并根据排序结果选择其中平滑度最高且未被访问过的三维点作为种子点,从而,可以通过迭代的方式得到多个种子点;;
然后,根据每个种子点进行区域生长,具体地,从种子点开始依据宽度优先的原则进行区域生长,为每个未访问过的种子点pi定义一个平面fi=(Bk(pi),oi,ni);然后,对fi进行区域生长;接着,对于每个候选点pj(在初始状态下,候选点为除种子点外的所有三维点,在迭代区域生长阶段,候选点为除种子点之外的所有未访问的三维点),设定选择条件:(1)pj到平面fi的正交距离小于σ/2;(2)nj与ni的夹角小于阈值θ;(3)pj与pi的距离小于Rseed;若pj同时满足以上三个条件,则将pj加入fi并标记为已访问。其中,θ是用来确定两个方向向量是否一致的角度阈值,Rseed是用来限制平面半径的阈值。当没有更多的点pj可以添加到平面fi时,区域生长被终止,即可得到该种子点所对应的初始平面;从而,根据上述方式进行区域生长,直至没有候选的种子点,即可得到所有种子点中每个种子点对应的初始平面,并根据区域生长结果生成初始平面集合。
接着,在初始平面集合已获得的情况下,进一步通过局部K均值聚类对初始平面集合进行优化,即迭代地将三维点分配给平面,以确保每个三维点到其所属平面的距离是其到所有平面距离中的最小值;进而输出最终平面集合;如图3所示,图3中(a)为短期点云数据对应的最终平面集合参考图,(b)为全局点云数据对应的最终平面集合参考图。
S103,根据层次聚类法对最终平面集合进行划分,以生成内部存在平行关系的多个第一平面基元组。
即言,根据层次聚类法对最终平面集合进行划分,以将存在平行关系的最终平面划分到同一基元组,以形成第一平面基元组。
在一些实施例中,在根据生成内部存在平行关系的多个第一平面基元组之后,还包括:计算每个第一平面基元组对应的平面法向量平均值,并计算第一平面基元组中平面法向量与对应平面法向量平均值之间的差值,以及将最小差值所对应的平面法向量作为该第一平面基元组的组法向量;计算任意两个第一平面基元组的组法向量之间的夹角。
S104,获取自动驾驶车辆采集到的短期点云数据,并对短期点云数据进行处理,以生成短期点云数据对应的多个第二平面基元组。
即言,在自动驾驶车辆的运行过程中,获取自动驾驶车辆采集到的短期点云数据(例如,自动驾驶车辆采集的点云数据中截取到的连续三秒中的点云数据),并对短期点云数据进行如上述对于全局点云数据的相同方式的处理,以生成短期点云数据所对应的多个第二平面基元组。
S105,根据第一平面基元组和第二平面基元组进行点云配准,并对配准结果进行优化,以生成自动驾驶车辆定位结果。
也就是说,根据第一平面基元组和第二平面基元组进行点云的配准,并对配准的结果进行优化,以输出自动驾驶车辆的定位信息。
在一些实施例中,根据第一平面基元组和第二平面基元组进行点云配准,包括:采用在旋转平移中具有不变性的二面角作为相似性测度,根据多个第一平面基元组中任意两个第一平面基元组的组法向量之间的夹角和多个第二平面基元组中任意两个第二平面基元组的组法向量之间的夹角寻找所有具有相似二面角的平面基元组对,以生成潜在的对应平面基元组对;计算第面基元组对所对应的旋转矩阵,并根据旋转矩阵获取对应平面对,以及根据对应平面对计算相应的平移向量;对平移向量和旋转矩阵进行几何一致性评估,并根据评估结果和迭代最近点算法确定最终配准结果。
作为一种示例,根据以下公式寻找多个第一平面基元组中和多个第二平面基元组中具有相似二面角的平面基元组对:
其中,μ表示第一角度阈值,tθ表示第二角度阈值,表示多个第一平面基元组中第i个第一平面基元组的组法向量,表示多个第一平面基元组中第j个第一平面基元组的组法向量,表示多个第二平面基元组中第m个第二平面基元组的组法向量,表示多个第二平面基元组中第n个第二平面基元组的组法向量,θij表示第i个第一平面基元组的组法向量与第j个第一平面基元组的组法向量之间的夹角,θmn表示第m个第二平面基元组的组法向量与第n个第二平面基元组的组法向量之间的夹角。
作为一种示例,根据以下公式进行旋转矩阵和平移向量的计算:
其中,axis1、axis2表示旋转轴,angle1、angle2表示旋转角,Ri1、Ri2、Ri表示旋转矩阵,toRotationMatrix()表示根据旋转轴和旋转角计算得到对应旋转矩阵的函数,cos-1()表示反余弦函数,表示第i个第一平面基元组的组法向量,表示第j个第一平面基元组的组法向量,表示第m个第二平面基元组的组法向量,表示第n个第二平面基元组的组法向量;
其中,表示第k个第一平面基元组的组法向量,Ri表示旋转矩阵,表示第l个第二平面基元组的组法向量,ξ表示一个值近似为1的标量,第k个第一平面基元组的组法向量所对应的平面与第l个第二平面基元组的组法向量所对应的平面所组成的平面对即为对应平面对;
ti=N-1D
其中,N和D为3×3矩阵,ti表示平移向量,表示第i个第一平面基元组的组法向量,表示第j个第一平面基元组的组法向量,表示第k个第一平面基元组的组法向量,表示原点到第i个第一平面基元组的组法向量所对应的平面的正交距离,表示原点到第j个第一平面基元组的组法向量所对应的平面的正交距离,表示原点到第k个第一平面基元组的组法向量所对应的平面的正交距离,表示原点到第m个第二平面基元组的组法向量所对应的平面的正交距离,表示原点到第n个第二平面基元组的组法向量所对应的平面的正交距离,表示原点到第1个第二平面基元组的组法向量所对应的平面的正交距离。
作为一种示例,根据以下公式进行几何一致性评估:
其中,ξ表示一个值近似为1的标量,η表示标量阈值,表示第a个第一平面基元组的组法向量,表示第b个第二平面基元组的组法向量,Ri表示旋转矩阵,ti表示平移向量,表示原点到第a个第二平面基元组的组法向量所对应的平面的正交距离,表示原点到第b个第二平面基元组的组法向量所对应的平面的正交距离。
如图4所示,图4为基于平面基元组的点云配准局部结果示意图。
需要说明的是,在对配准结果进行优化时,可以采用迭代最近点(ICP)算法进行精配准,以输出最终定位结果;如图5所示,图5为优化后的配准结果示意图。并且,在最终定位结果生成之后,可以根据自动驾驶车辆的ground truth对最终定位结果进行验证,如图6所示,图6为验证结果示意图,其中,点表示基于平面基元组的自动驾驶车辆定位结果,而线则是ground truth。
综上所述,根据本发明实施例的地下停车场自动驾驶车辆定位方法,首先,获取地下停车场的全局点云数据,并对全局点云数据进行预处理,以过滤全局点云数据中的车辆三维点,得到初始点云数据;接着,根据区域生长法对初始点云数据进行平面结构提取,以得到初始平面集合,并根据局部K均值聚类对初始平面集合进行优化,以得到最终平面集合;然后,根据层次聚类法对最终平面集合进行划分,以生成内部存在平行关系的多个第一平面基元组;接着,获取自动驾驶车辆采集到的短期点云数据,并对短期点云数据进行处理,以生成短期点云数据对应的多个第二平面基元组;然后,根据第一平面基元组和第二平面基元组进行点云配准,并对配准结果进行优化,以生成自动驾驶车辆定位结果;从而实现在地下停车场等室内环境下对自动驾驶车辆进行精准定位,以保障自动驾驶车辆在无GPS定位的条件下安全有效运行。
为了实现上述实施例,本发明实施例还提出了一种计算机可读存储介质,其上存储有地下停车场自动驾驶车辆定位程序,该地下停车场自动驾驶车辆定位程序被处理器执行时实现如上述的地下停车场自动驾驶车辆定位方法。
根据本发明实施例的计算机可读存储介质,通过存储地下停车场自动驾驶车辆定位程序,以使得处理器在执行该地下停车场自动驾驶车辆定位程序时,实现如上述的地下停车场自动驾驶车辆定位方法;从而实现在地下停车场等室内环境下对自动驾驶车辆进行精准定位,以保障自动驾驶车辆在无GPS定位的条件下安全有效运行。
本领域内的技术人员应明白,本发明的实施例可提供为方法、系统、或计算机程序产品。因此,本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
应当注意的是,在权利要求中,不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的单词“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中,这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。
尽管已描述了本发明的优选实施例,但本领域内的技术人员一旦得知了基本创造性概念,则可对这些实施例作出另外的变更和修改。所以,所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
显然,本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样,倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内,则本发明也意图包含这些改动和变型在内。
在本发明的描述中,需要理解的是,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中,“多个”的含义是两个或两个以上,除非另有明确具体的限定。
在本发明中,除非另有明确的规定和限定,术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或成一体;可以是机械连接,也可以是电连接;可以是直接相连,也可以通过中间媒介间接相连,可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言,可以根据具体情况理解上述术语在本发明中的具体含义。
在本发明中,除非另有明确的规定和限定,第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触,或第一和第二特征通过中间媒介间接接触。而且,第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方,或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方,或仅仅表示第一特征水平高度小于第二特征。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不应理解为必须针对的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管上面已经示出和描述了本发明的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本发明的限制,本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (10)
1.一种地下停车场自动驾驶车辆定位方法,其特征在于,包括以下步骤:
获取地下停车场的全局点云数据,并对所述全局点云数据进行预处理,以过滤所述全局点云数据中的车辆三维点,得到初始点云数据;
根据区域生长法对所述初始点云数据进行平面结构提取,以得到初始平面集合,并根据局部K均值聚类对所述初始平面集合进行优化,以得到最终平面集合;
根据层次聚类法对最终平面集合进行划分,以生成内部存在平行关系的多个第一平面基元组;
获取自动驾驶车辆采集到的短期点云数据,并对所述短期点云数据进行处理,以生成所述短期点云数据对应的多个第二平面基元组;
根据所述第一平面基元组和所述第二平面基元组进行点云配准,并对配准结果进行优化,以生成自动驾驶车辆定位结果。
2.如权利要求1所述的地下停车场自动驾驶车辆定位方法,其特征在于,对所述全局点云数据进行预处理,包括:
根据预设尺寸对所述全局点云数据进行切分,以生成多个点云块,并对多个点云块进行水平分层,以及计算分层后每一层点云数据所对应的三维点点数;
根据每一层点云数据对应的高度自底层向上依次判断该层点云数据所对应的三维点点数是否大于预设点数阈值,并在判断结果为是时,将该层点云数据作为地面层,以及将该层点云数据对应的高度作为地面高度值;
根据八叉树索引结构对多个点云块进行划分,以生成空间上连续的多个点云体素,并根据多个点云体素进行向上生长,以确定局部高度值最大的点云体素Vh和Vh所对应的高度值h;
判断h的值是否大于地面高度值且小于预设车辆高度阈值,并在判断结果为是时,确定Vh中的三维点为车辆点,以及滤除包含Vh的点云块中的所有三维点。
3.如权利要求1所述的地下停车场自动驾驶车辆定位方法,其特征在于,根据区域生长法对所述初始点云数据进行平面结构提取,以得到初始平面集合,并根据局部K均值聚类对所述初始平面集合进行优化,以得到最终平面集合,包括:
寻找所述初始点云数据中每个三维点的K近邻点,并根据主成分分析法计算每个三维点所对应的最佳拟合初始切平面,以及根据迭代重加权的最小二乘法对所述最佳拟合初始切平面进行优化,以得到每个三维点对应的最终切平面和每个三维点对应的平滑度;
对三维点对应的平滑度进行排序,并根据排序结果迭代地选择其中平滑度最高且未被访问过的三维点作为种子点;
根据每个种子点进行区域生长,以生成每个种子点对应的初始平面,并根据每个种子点对应的初始平面生成初始平面集合,以及根据局部K均值聚类对所述初始平面集合进行优化,以得到最终平面集合。
4.如权利要求1所述的地下停车场自动驾驶车辆定位方法,其特征在于,在根据所述生成内部存在平行关系的多个第一平面基元组之后,还包括:
计算每个第一平面基元组对应的平面法向量平均值,并计算第一平面基元组中平面法向量与对应平面法向量平均值之间的差值,以及将最小差值所对应的平面法向量作为该第一平面基元组的组法向量;
计算任意两个第一平面基元组的组法向量之间的夹角。
5.如权利要求4所述的地下停车场自动驾驶车辆定位方法,其特征在于,根据所述第一平面基元组和所述第二平面基元组进行点云配准,包括:
采用在旋转平移中具有不变性的二面角作为相似性测度,根据多个第一平面基元组中任意两个第一平面基元组的组法向量之间的夹角和多个第二平面基元组中任意两个第二平面基元组的组法向量之间的夹角寻找所有具有相似二面角的平面基元组对,以生成潜在的对应平面基元组对;
计算所述平面基元组对所对应的旋转矩阵,并根据所述旋转矩阵获取对应平面对,以及根据所述对应平面对计算相应的平移向量;
对所述平移向量和所述旋转矩阵进行几何一致性评估,并根据评估结果和迭代最近点算法确定最终配准结果。
8.如权利要求5所述的地下停车场自动驾驶车辆定位方法,其特征在于,根据以下公式进行旋转矩阵和平移向量的计算:
其中,axis1、axis2表示旋转轴,angle1、angle2表示旋转角,Ri1、Ri2、Ri表示旋转矩阵,toRotationMatrix()表示根据旋转轴和旋转角计算得到对应旋转矩阵的函数,cos-1()表示反余弦函数,表示第i个第一平面基元组的组法向量,表示第j个第一平面基元组的组法向量,表示第m个第二平面基元组的组法向量,表示第n个第二平面基元组的组法向量;
其中,表示第k个第一平面基元组的组法向量,Ri表示旋转矩阵,表示第l个第二平面基元组的组法向量,ξ表示一个值近似为1的标量,第k个第一平面基元组的组法向量所对应的平面与第l个第二平面基元组的组法向量所对应的平面所组成的平面对即为对应平面对;
ti=N-1D
10.一种计算机可读存储介质,其特征在于,其上存储有地下停车场自动驾驶车辆定位程序,该地下停车场自动驾驶车辆定位程序被处理器执行时实现如权利要求1-9中任一项所述的地下停车场自动驾驶车辆定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010814965.XA CN111968179B (zh) | 2020-08-13 | 2020-08-13 | 地下停车场自动驾驶车辆定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010814965.XA CN111968179B (zh) | 2020-08-13 | 2020-08-13 | 地下停车场自动驾驶车辆定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111968179A true CN111968179A (zh) | 2020-11-20 |
CN111968179B CN111968179B (zh) | 2022-07-19 |
Family
ID=73365612
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010814965.XA Active CN111968179B (zh) | 2020-08-13 | 2020-08-13 | 地下停车场自动驾驶车辆定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111968179B (zh) |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106296693A (zh) * | 2016-08-12 | 2017-01-04 | 浙江工业大学 | 基于3d点云fpfh特征实时三维空间定位方法 |
CN108765475A (zh) * | 2018-05-25 | 2018-11-06 | 厦门大学 | 一种基于深度学习的建筑物三维点云配准方法 |
CN108917761A (zh) * | 2018-05-07 | 2018-11-30 | 西安交通大学 | 一种无人车在地下车库中的精确定位方法 |
CN109253731A (zh) * | 2018-08-06 | 2019-01-22 | 百度在线网络技术(北京)有限公司 | 停车场地图生成方法、装置、设备及可读存储介质 |
CN109887028A (zh) * | 2019-01-09 | 2019-06-14 | 天津大学 | 一种基于点云数据配准的无人车辅助定位方法 |
CN110988894A (zh) * | 2019-12-25 | 2020-04-10 | 畅加风行(苏州)智能科技有限公司 | 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法 |
CN111505662A (zh) * | 2020-04-29 | 2020-08-07 | 北京理工大学 | 一种无人驾驶车辆定位方法及系统 |
-
2020
- 2020-08-13 CN CN202010814965.XA patent/CN111968179B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106296693A (zh) * | 2016-08-12 | 2017-01-04 | 浙江工业大学 | 基于3d点云fpfh特征实时三维空间定位方法 |
CN108917761A (zh) * | 2018-05-07 | 2018-11-30 | 西安交通大学 | 一种无人车在地下车库中的精确定位方法 |
CN108765475A (zh) * | 2018-05-25 | 2018-11-06 | 厦门大学 | 一种基于深度学习的建筑物三维点云配准方法 |
CN109253731A (zh) * | 2018-08-06 | 2019-01-22 | 百度在线网络技术(北京)有限公司 | 停车场地图生成方法、装置、设备及可读存储介质 |
CN109887028A (zh) * | 2019-01-09 | 2019-06-14 | 天津大学 | 一种基于点云数据配准的无人车辅助定位方法 |
CN110988894A (zh) * | 2019-12-25 | 2020-04-10 | 畅加风行(苏州)智能科技有限公司 | 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法 |
CN111505662A (zh) * | 2020-04-29 | 2020-08-07 | 北京理工大学 | 一种无人驾驶车辆定位方法及系统 |
Non-Patent Citations (3)
Title |
---|
DAWEIZAI ET AL.: "Pairwise registration of TLS point clouds using covariance descriptors and a non-cooperative game", 《ISPRS JOURNAL OF PHOTOGRAMMETRY & REMOTE SENSING》, vol. 134, no. 1, 31 December 2017 (2017-12-31), pages 15 - 29, XP085253929, DOI: 10.1016/j.isprsjprs.2017.10.001 * |
LILI LIN: "Planar Primitive Group-Based Point Cloud Registration for Autonomous Vehicle Localization in Underground Parking Lots", 《IEEE GEOSCIENCE AND REMOTE SENSING LETTERS》, 1 February 2021 (2021-02-01) * |
SONGLIN CHEN ET AL.: "PLADE: A Plane-Based Descriptor for Point Cloud Registration With Small Overlap", 《IEEE TRANSACTIONS ON GEOSCIENCE AND REMOTE SENSING》, vol. 58, no. 4, 18 December 2019 (2019-12-18), pages 2530 - 2540, XP011780033, DOI: 10.1109/TGRS.2019.2952086 * |
Also Published As
Publication number | Publication date |
---|---|
CN111968179B (zh) | 2022-07-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US20220028163A1 (en) | Computer Vision Systems and Methods for Detecting and Modeling Features of Structures in Images | |
CN110570428B (zh) | 一种从大规模影像密集匹配点云分割建筑物屋顶面片的方法及系统 | |
CN109165549B (zh) | 基于三维点云数据的道路标识获取方法、终端设备及装置 | |
CN108171131B (zh) | 基于改进MeanShift的Lidar点云数据道路标识线提取方法 | |
CN112347550B (zh) | 耦合式室内三维语义建图及建模方法 | |
US20180075319A1 (en) | 3d building extraction apparatus, method and system | |
CN112184736B (zh) | 一种基于欧式聚类的多平面提取方法 | |
US11430087B2 (en) | Using maps comprising covariances in multi-resolution voxels | |
US11288861B2 (en) | Maps comprising covariances in multi-resolution voxels | |
CN112508985B (zh) | 一种基于语义分割的slam回环检测改进方法 | |
CN112070870B (zh) | 点云地图评估方法、装置、计算机设备和存储介质 | |
CN116279592A (zh) | 一种用于无人物流车的可行驶区域划分方法 | |
CN114863376A (zh) | 一种基于车载激光点云数据的道路标线分割方法及系统 | |
WO2011085435A1 (en) | Classification process for an extracted object or terrain feature | |
CN114877838B (zh) | 一种基于车载激光扫描系统的道路几何特征检测方法 | |
CN114463338B (zh) | 一种基于图割与后处理的建筑物激光脚点自动提取方法 | |
CN111709988A (zh) | 一种物体的特征信息的确定方法、装置、电子设备及存储介质 | |
CN112435336B (zh) | 一种弯道类型识别方法、装置、电子设备及存储介质 | |
CN111968179B (zh) | 地下停车场自动驾驶车辆定位方法 | |
CN114419465B (zh) | 遥感图像变化检测方法、装置、设备及存储介质 | |
CN116628531A (zh) | 众包地图道路对象要素聚类方法、系统及存储介质 | |
CN116580160A (zh) | 一种建筑物单体分割和结构重建耦合处理方法及相关装置 | |
CN116206286A (zh) | 高速路况下的障碍物检测方法、装置、设备和介质 | |
CN111238524B (zh) | 视觉定位方法以及装置 | |
Kunyuan et al. | Automated object extraction from MLS data: A survey |
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 |