CN110827403B - 一种矿山三维点云地图的构建方法及装置 - Google Patents
一种矿山三维点云地图的构建方法及装置 Download PDFInfo
- Publication number
- CN110827403B CN110827403B CN201911068605.3A CN201911068605A CN110827403B CN 110827403 B CN110827403 B CN 110827403B CN 201911068605 A CN201911068605 A CN 201911068605A CN 110827403 B CN110827403 B CN 110827403B
- Authority
- CN
- China
- Prior art keywords
- point
- frame
- point cloud
- pos
- current
- 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.)
- Active
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Geometry (AREA)
- Remote Sensing (AREA)
- Theoretical Computer Science (AREA)
- Software Systems (AREA)
- Electromagnetism (AREA)
- Radar, Positioning & Navigation (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Computer Networks & Wireless Communication (AREA)
- Computer Graphics (AREA)
- Processing Or Creating Images (AREA)
Abstract
本公开提供了一种矿山三维点云地图的构建方法,包括:S1,进行点云预处理;S2,根据距离选取点云关键帧,先行插入地面点八叉树和全局八叉树中;S3,通过RTK输出的位姿点,计算车轮与地面接触的假设地面点;S4,对当前帧点云及所述假设地面点进行配准并坐标转换;搜索地面点云的邻近点集,并进行配准与坐标转换,将转换后的地面点云插入地面点八叉树;以及在全局点八叉树中搜索当前帧点云的邻近点集,并进行配准与坐标转换,将转换后的当前点云插入全局八叉树;S5,判断本次建图中经历的建图旋转平移矩阵相较上次的改变量是否大于旋转平移矩阵阈值,若不大于则完成建图。本公开能够保证矿区特殊环境的云地图的精度要求。
Description
技术领域
本公开涉及无人驾驶领域,尤其涉及一种矿山三维点云地图的构建方法及装置。
背景技术
在无人驾驶技术的不断推进过程中,对车辆行驶环境进行感知并生成点云地图已经成为了不可或缺的一环。点云地图可以用于自身定位以及制作矢量地图以供路径规划等等,可见点云地图的质量对无人驾驶技术路线的重要意义。矿山无人驾驶是无人驾驶技术重要的落地场景之一,其环境相较其他无人驾驶落地场景诸如城市RoboTaxi、封闭园区通勤等主要有如下重大不同:1)矿区环境更加复杂多变,结构性特征少;2)矿区道路质量较差,车辆行驶过程中较为颠簸,感知难度增大。
现有的一种三维点云地图通过标记地面点和非地面点中的角点和平面点,对其进行帧间匹配得到相邻帧的旋转矩阵和位移矩阵,对相邻帧做基于该旋转和位移矩阵的坐标转换,不断累加拼接点云生成点云地图;筛选点云关键帧,融合GPS和IMU数据得到路径点数据集,利用线性插值计算关键帧基于路径点的假设位姿,对从上一个关键帧到当前关键帧之间的每一帧点云位姿进行后端优化。但是,该方法并不适用于矿区环境,这是由于:
1)矿区场景结构性特征很少,现有技术中所使用的非地面点中的角点和平面点很少或是不明显,无法利用上述特征进行帧间匹配来计算出误差可接受的前后两帧点云之间的旋转矩阵和位移矩阵;
2)由于激光雷达扫描某一点云的时刻与GPS和IMU对应时刻的输出不能够做到丝毫不差,每帧点云所对应的位姿和其实际位姿存在微小误差,而矿区道路崎岖颠簸,激光雷达位姿变化剧烈,放大了这一误差带来的影响,对关键帧基于路径点进行位姿优化也会存在较大误差,导致点云地图精度不达标。
由此可知,现有的三维点云地图构建方法不适用于满足矿区环境,因此需要一种能够保证矿区特殊环境的云地图的精度要求的三维点云地图的构建方法。
发明内容
(一)要解决的技术问题
本公开提供了一种矿山三维点云地图的构建方法及装置,以至少部分解决以上所提出的技术问题。
(二)技术方案
根据本公开的一个方面,提供了一种矿山三维点云地图的构建方法,包括:
S1,进行点云预处理,计算当前点云帧每个采样点相对于当前点云帧第一个点的位姿偏移,依据所述位姿偏移进行位姿刚体变换,并标记出当前点云帧的地面点;
S2,根据距离选取点云关键帧,先行插入地面点八叉树和全局八叉树中;
S3,通过RTK输出的位姿点,计算车轮与地面接触的假设地面点;
S4,对当前帧点云及所述假设地面点进行配准并坐标转换;在地面点八叉树中搜索当前帧点云标记的地面点云的邻近点集,并进行配准与坐标转换,将转换后的地面点云插入地面点八叉树;以及在全局点八叉树中搜索当前帧点云的邻近点集,并进行配准与坐标转换,将转换后的当前点云插入全局八叉树;
S5,判断是否为第一次建图,若非第一次建图,则进一步判断每帧点云在本次建图中经历的建图旋转平移矩阵ΔT相较上次建图的改变量是否大于旋转平移矩阵阈值,若不大于所述旋转平移矩阵阈值,则完成建图。
进一步的,所述步骤S1包括:
S11,对于当前点云帧的任意采样点p,使用其时间戳p.timestamp对RTK输出的采样时间戳序列{pos1.t,pos2.t,…,posn.t}进行差值,计算出该采样点对应的假设位姿pos_estimate;取当前点云帧的第一个采样点的假设位姿作为当前点云帧基准假设位姿每个采样点的假设位姿pos_estimate与当前点云帧基准假设位姿作差,计算出该点相对于当前点云帧基准假设位姿的位姿偏移pos_diff;对该点依据位姿偏移pos_diff进行位姿刚体变换;
S12,标记出当前点云帧的地面点
进一步的,所述步骤S2包括:
S21,设定距离阈值m;
S22,对于RTK输出的采样序列{pos1pos2,…,posn},从起始位置采样pos1开始计算累计距离,搜索到累计距离大于距离阈值m的最近采样输出作为第一个关键RTK采样点位,设其为posm1;
S23,从第一个关键RTK采样点位posm1开始重新计算累计距离,再次搜索到累计距离大于m的最近采样输出作为第二个关键RTK采样点位,设其为posm2,依次类推,直至将RTK采样序列搜索完毕,得到关键RTK采样点位序列{posm1,posm2,…,posmk};
S24,对于关键RTK采样点位序列{posm1,posm2,…,posmk}中每个采样点位posmi,1<i≤k,根据其时间戳pos.t在所有点云帧时间戳序列{frame1.timestamp,frame2.timestamp,…,framel.timestamp}中寻找时间间隔最近的点云帧时间戳,设为framer.timestamp,则将其对应的点云帧确定为一个点云关键帧framer;得到关键RTK采样点位序列{posm1,posm2,…,Posmk}对应的点云关键帧序列{framer1,framer2,…,framerk};
S25,对点云关键帧序列{framer1,framer2,…,framerk}中的每一个点云关键帧framer,使用其时间戳framer.timestamp对RTK输出的采样时间戳序列{pos1.t,pos2.t,…,posn.t}进行差值,计算出该点云关键帧framer的假设位姿pos_framer,并根据pos_framer对framer进行刚体变换,将framer变换到投影坐标系中;将刚体变换后的framer先行插入全局八叉树octree_entire中,并将变换后的framer中被标记出的地面点云framer_ground线性插入地面点云八叉树octree_ground中。
进一步的,所述步骤S3包括:
S31,在车辆坐标系下,标定RTK输出位置与两个前车轮与地面接触点的相对距离:获取前左车轮对于RTK输出位置的相对位置坐标l1={x1,y1,z1},前右车轮对于RTK输出位置的相对位置坐标为l2={x2,y2,z2};
S32,对于RTK输出的采样序列{pos1pos2,…,posn}的每个采样pos,将l1和l2根据pos的投影坐标向量{pos.x,posy,pos.z}和姿态角向量{pos.pitchpos.rollpos.yaw}作刚体旋转平移变换,得到l1`={x1`,y1`,z1`}和l2`={x2`,y2`,z2`},作为单次采样的假设地面点;对RTK输出的采样序列{pos1,pos2,…,posn}中全部采样进行假设地面点计算,得到假设地面点集合{l1`1,l2`1,l1`2,l2`2,…,l1`n,l2`n}。
进一步的,所述步骤S4包括:
S41,对当前帧点云及通过所述假设地面点进行配准并坐标转换;
S42,在地面点八叉树中对当前帧点云标记的地面点云进行搜索第一邻近点集,使用标记的地面点云与第一邻近点集进行配准,并进行坐标转换,将转换后的地面点云插入地面点八叉树;
S43,在全局点八叉树中对当前帧点云进行搜索第二邻近点集,使用当前帧点云点云与第二邻近点集进行配准,并进行坐标转换,将转换后的当前点云插入全局八叉树。
进一步的,所述步骤S41包括:
S411,对于当前帧点云frame,使用其时间戳frametimestamp对RTK输出的采样时间戳序列{pos1.t,pos2.t,…,posn.t}进行差值,计算出该点云帧的假设位姿pos_frame,并根据pos_frame构建第一旋转平移矩阵T,对frame进行刚体变换,将frame变换到投影坐标系中;
S412,对于转换后的当前帧点云frame中的每一条线的采样点集{p1,p2,…,ph},在假设地面点集{l1`1,l2`1,l1`2,l2`2,…,l1`n,l2`n}中搜索最近的假设地面点,得到最近的采样点和地面点点对,设为pair={p,l},从每条线的最近点对集中筛选出点对距离小于阈值r的点对集{pair1,pair2,…,pairh};使用点对集中的点云{pair1.p,pair2.p,…,pairh.p}作为输入源,使用点对集中的假设地面点{pair1.l,pair2.l,...,pairh.l}作为输入目标,进行ICP配准,得到第二旋转平移矩阵;将当前帧点云根据第二旋转平移矩阵作坐标转换。
进一步的,所述步骤S42包括:
在地面点八叉树octree_ground中对采用第二旋转平移矩阵转换后的当前帧点云所标记的地面点云frame.gromd中的每个点进行搜索第一邻近点集,搜索方式为搜索当前点周围半径radius内不多于最大数目max cnt的点集,所述半径radius与当前搜索点与激光雷达坐标系原点的距离dis成正比,记为一个系数a与距原点距离dis的乘积a*dis;对于frame.ground中的每个点进行搜索,输出第一邻近点集neighbors_ground;使用当前帧标记的地面点云frame.ground作为输入源,使用其所对应的第一邻近点集neighbors_ground为输入目标,进行ICP配准,得到第三旋转平移矩阵;将当前帧点云根据第三旋转平移矩阵作坐标转换,将转换后的当前帧点云中所标记的地面点云frame.gromd插入地面点八叉树octree_ground中。
进一步的,所述步骤S43包括:
在全局八叉树octree_entire中对采用第二旋转平移矩阵转换后的当前帧点云frame中的每个点进行搜索第二邻近点集,搜索方式为搜索当前点附近的最近点;于frame中的每个点进行搜索,输出第二邻近点集neighbors_entire;使用当前帧点云frame作为输入源,使用其所对应的第二邻近点集neighbors_entire为输入目标,进行ICP配准,得到第四旋转平移矩阵;将当前帧点云根据第四旋转平移矩阵作坐标转换,将转换后的当前帧点云中所标记的地面点云插入地面点八叉树octree_entire中。
进一步的,所述步骤S5包括:
S51,当每帧点云都按照所述S4步骤建图后,判断当前是否为第一次建图,若此次建图是第一次建图,则将地面点八叉树octree_ground和全局八叉树octree_entire内的点云更新为当次建图的点云结果;然后返回S4步骤重新建图;若此次建图不是第一次建图,则转至步骤S52;
S52,获取本次所述步骤S4中经历的建图旋转平移矩阵,判断每帧点云在本次步骤S4中经历的建图旋转平移矩阵ΔT相较于上一次步骤S4建图的改变量是否大于旋转平移矩阵阈值,若改变量不大于所述旋转平移矩阵阈值则建图完成。
进一步的,所述步骤S52包括:
S521,获取当前帧点云frame在本次所述步骤S4中经历的总旋转平移矩阵Tfinal,记录当前帧在步骤S412、S42、S43的算法处理过程中经历的建图旋转平移矩阵ΔT=Tfinal-T;其中,若当前为第num次建图,建图完成后,全部点云帧{frame1,frame2,…,framel}在本次步骤S4的S412、S42、S43建图算法过程中经历的建图旋转平移矩阵序列为
S522,设定旋转平移矩阵阈值T_trh;计算当次建图每帧点云在算法过程中经历的建图旋转平移矩阵序列与上次建图对应帧点云在算法过程中经历的建图旋转平移矩阵序列/>之差/>即,
其中num>1,计算其平均值得到若diff_avg大于旋转平移矩阵阈值T_trh,则将地面点八叉树octree_ground和全局八叉树octree_entire内的点云更新为当次建图的点云结果,然后重新开始重复S4步骤建图;若dif_avg小于等于旋转平移矩阵阈值T_trh,则建图精度满足要求,建图结束。
根据本公开的另一个方面,提供了一种矿山三维点云地图的构建装置,所述矿山三维点云地图的构建装置包括:
可读存储介质,所述可读存储介质用于存储可执行指令;
一个或多个处理器,所述一个或多个处理器根据所述可执行指令执行如前所述的矿山三维点云地图的构建方法。
(三)有益效果
从上述技术方案可以看出,本公开矿山三维点云地图的构建方法及装置至少具有以下有益效果其中之一:
(1)通过根据距离选取点云关键帧,先行插入地面点八叉树和全局八叉树中再进行建图,消除了由于ICP配准造成的累计误差,提升了定位精度;
(2)通过点云和假设地面点进行配准,很大程度上保证了点云中的路面与真实路面距离差在同一平面,消除了车辆剧烈抖动造成的建图精度误差;
(3)通过单帧点云标定的路面与地面点八叉树进行配准,提高了点云地图中路面的精度,满足了后续绘制高精矢量地图对路面点云地图精度的高要求;
(4)通过单帧点云与全局八叉树进行配准,提高了点云地图的整体精度;
(5)通过对地面点八叉树和全局八叉树进行迭代、重复建图,最终达到误差收敛,实现了既能保证点云的局部精度,又能保证消除累计误差的定位精度。
附图说明
图1为本公开实施例矿山三维点云地图的构建方法的流程图。
具体实施方式
本公开针对矿区环境复杂多变,结构性特征少,以及矿区道路质量较差,车辆行驶过程中较为颠簸,感知难度大的特点,提出了一种矿山三维点云地图的构建方法及装置,从而保证矿区点云地图的精度要求。
在描述问题的解决方案之前,先定义一些特定词汇的定义是有帮助的。
点云帧frame由若干个激光发射器在一个扫描周期内采样的点集共同组成,称每个激光发射器在一个扫描周期内采样的点集为一条线。点云帧frame包括若干个激光雷达测距采样点p,p至少包括采样点时间戳p.timestamp和坐标p.x、p.y、p.z,frame的时间戳frame.timestamp为其第一个采样点p1的时间戳p1.timestamp。
RTK为实时载波相位差分仪,RTK的一个采样输出pos,包括采样时刻的时间戳pos.t,输出位置所在的投影坐标向量{pos.x,posy,pos.z},以及姿态角向量{pos.pttchpos.rollpos.yaw}。
ICP配准为最近点迭代算法,用于计算一个三维点集到另一个三维点集的最优旋转平移矩阵。
为使本公开的目的、技术方案和优点更加清楚明白,以下结合具体实施例,并参照附图,对本公开进一步详细说明。
本公开某些实施例于后方将参照所附附图做更全面性地描述,其中一些但并非全部的实施例将被示出。实际上,本公开的各种实施例可以由许多不同形式实现,而不应被解释为限于此处所阐述的实施例;相对地,提供这些实施例使得本公开满足适用的法律要求。
在本公开的一个示例性实施例中,提供了一种矿山三维点云地图的构建方法。
图1为本公开实施例矿山三维点云地图的构建方法的流程图。如图1所示,本公开矿山三维点云地图的构建方法包括:
S1,进行点云预处理,计算当前点云帧每个采样点相对于当前点云帧第一个点的位姿偏移,依据所述位姿偏移进行位姿刚体变换,并标记出当前点云帧的地面点。具体地,所述步骤S1进一步包括:
S11,对于当前点云帧的任意采样点p,使用其时间戳p.timestamp对RTK输出的采样时间戳序列{pos1·t,pos2.t,…,posn.t}进行差值,计算出该采样点对应的假设位姿pos_estimate;取当前点云帧的第一个采样点的假设位姿作为当前点云帧基准假设位姿每个采样点的假设位姿pos_estimate与当前点云帧基准假设位姿作差,计算出该点相对于当前点云帧基准假设位姿的位姿偏移pos_diff;对该点依据位姿偏移pos_diff进行位姿刚体变换,实现消除运动畸变。
S12,标记出当前点云帧的地面点frame.ground;
S2,根据距离选取点云关键帧,先行插入地面点八叉树和全局八叉树中。具体地,所述步骤S2进一步包括:
S21,设定距离阈值m;
S22,对于RTK输出的采样序列{pos1pos2,…,posn},从起始位置采样pos1开始计算累计距离,搜索到累计距离大于距离阈值m的最近采样输出作为第一个关键RTK采样点位,不妨设其为posm1;
S23,从第一个关键RTK采样点位posm1开始重新计算累计距离,再次搜索到累计距离大于m的最近采样输出作为第二个关键RTK采样点位,不妨设其为posm2,依次类推,直至将RTK采样序列搜索完毕,得到关键RTK采样点位序列{posm1,posm2,…,posmk};
S24,对于关键RTK采样点位序列{posm1,posm2,…,posmk}中每个采样点位posmi,1<i≤k,根据其时间戳pos.t在所有点云帧时间戳序列{frame1.timestamp,frame2.timestamp,…,framel.timestamp}中寻找时间间隔最近的点云帧时间戳,不妨设为framer.timestamp,则将其对应的点云帧确定为一个点云关键帧framer。关键RTK采样点位序列{posm1,posm2,…,posmk}对应的点云关键帧序列为{framer1,framer2,…,framerk};
S25,对点云关键帧序列{framer1,framer2,…,framerk}中的每一个点云关键帧framer,使用其时间戳framer.timestamp对RTK输出的采样时间戳序列{pos1.t,pos2.t,…,posn.t}进行差值,计算出该点云关键帧framer的假设位姿pos_framer,并根据pos_framer对framer进行刚体变换,将framer变换到投影坐标系中;将刚体变换后的framer先行插入全局八叉树octree_entire中,并将变换后的framer中被标记出的地面点云framer_ground线性插入地面点云八叉树octree_ground中。
由此,通过根据距离选取点云关键帧,先行插入地面点八叉树和全局八叉树中再进行建图,能够消除由于ICP配准造成的累计误差,提升定位精度。
S3,通过RTK输出的位姿点,计算车轮与地面接触的假设地面点。
具体地,所述步骤S3进一步包括:
S31,在车辆坐标系下,标定RTK输出位置与两个前车轮与地面接触点的相对距离:获取前左车轮对于RTK输出位置的相对位置坐标为l1={x1,y1,z1},前右车轮对于RTK输出位置的相对位置坐标为l2={x2,y2,z2};
S32,对于RTK输出的采样序列{pos1pos2,…,posn}的每个采样pos,将l1和l2根据pos的投影坐标向量{pos.x,posy,pos.z}和姿态角向量{pos.pitchpos.rollpos.yaw}作刚体旋转平移变换,得到l1`={x1`,yl`,z1`}和l2`={x2`,.y2`,z2`},作为单次采样的假设地面点;对RTK输出的采样序列{pos1pos2,…,posn}中全部采样进行假设地面点计算,得到假设地面点集合{l1`1,l2`1,l1`2,l2`2,…,l1`n,l2`n}。
S4,对当前帧点云及通过所述步骤S3计算出的所述假设地面点进行配准并坐标转换;在地面点八叉树中搜索当前帧点云标记的地面点云的邻近点集,并进行配准与坐标转换,将转换后的地面点云插入地面点八叉树;以及在全局点八叉树中搜索当前帧点云的邻近点集,并进行配准与坐标转换,将转换后的当前点云插入全局八叉树。
该步骤S4为单次建图过程,描述建图过程中对每一帧点云的操作过程。所述步骤S4进一步包括:
S41,对当前帧点云及通过所述步骤S3计算出的所述假设地面点进行配准并坐标转换,包括:
S411,对于当前帧点云frame,使用其时间戳frametimestamp对RTK输出的采样时间戳序列{pos1.t,pos2.t,…,posn.t}进行差值,计算出该点云帧的假设位姿pos_frame,并根据pos_frame构建第一旋转平移矩阵T,对frsme进行刚体变换,将frame变换到投影坐标系中;
S412,对于采用第一旋转平移矩阵转换后的当前帧点云frame中的每一条线的采样点集{p1,p2,…,ph},在假设地面点集{l1`1,l2`1,l1`2,l2`2,…,l1`n,l2`n}中搜索最近的假设地面点,得到最近的采样点和地面点点对,不妨设为pair={p,l},从每条线的最近点对集中筛选出点对距离小于阈值r的点对集{pair1,pair2,…,pairh};使用点对集中的点云{pair1.p,pair2.p,…,pairh.p}作为输入源,使用点对集中的假设地面点{pair1.l,pair2.l,…,pairh.l}作为输入目标,进行ICP配准,得到第二旋转平移矩阵;将采用第一旋转平移矩阵转换后的当前帧点云根据上述第二旋转平移矩阵作坐标转换。
通过点云和假设地面点进行配准,很大程度上保证了点云中的路面与真实路面距离差在同一平面,从而消除了车辆剧烈抖动造成的建图精度误差。
S42,在地面点八叉树中对当前帧点云标记的地面点云进行搜索第一邻近点集,使用标记的地面点云与第一邻近点集进行配准,并进行坐标转换,将转换后的地面点云插入地面点八叉树。具体地,所述步骤S42包括:
在地面点八叉树octree_ground中对上述转换后的当前帧点云所标记的地面点云frame.gromd中的每个点进行搜索第一邻近点集,搜索方式为搜索当前点周围半径radius内不多于最大数目max cnt的点集,所述半径radius与当前搜索点与激光雷达坐标系原点的距离dis成正比,记为一个系数a与所述距原点距离dis的乘积a*dis;对于中的每个点进行上述搜索,输出第一邻近点集neighbors_ground;使用当前帧标记的地面点云frame.gromd作为输入源,使用其所对应的第一邻近点集neighbors_ground为输入目标,进行ICP配准,得到第三旋转平移矩阵;将当前帧点云根据上述第三旋转平移矩阵作坐标转换,将转换后的当前帧点云中所标记的地面点云frame,gromd插入地面点八叉树octree_ground中。
通过单帧点云标定的路面与地面点八叉树进行配准,能够提高点云地图中路面的精度,满足后续绘制高精矢量地图对路面点云地图精度的高要求。
S43,在全局点八叉树中对当前帧点云进行搜索第二邻近点集,使用当前帧点云点云与第二邻近点集进行配准,并进行坐标转换,将转换后的当前点云插入全局八叉树。具体地,所述步骤S43包括:
在全局八叉树octree_entire中对上述转换后的当前帧点云frame中的每个点进行搜索第二邻近点集,搜索方式为搜索当前点附近的最近点;于frame中的每个点进行上述搜索,输出第二邻近点集neighbors_entire;使用当前帧点云frame作为输入源,使用其所对应的第二邻近点集neighbors_entire为输入目标,进行ICP配准,得到第四旋转平移矩阵;将当前帧点云根据上述第四旋转平移矩阵作坐标转换,将转换后的当前帧点云中所标记的地面点云插入地面点八叉树octree_entire中。
由此,通过采用单帧点云与全局八叉树进行配准,提高了点云地图的整体精度。
步骤S5,判断是否为第一次建图,若非第一次建图,则判断相较于上次建图,每帧点云在步骤S4中经历的建图旋转平移矩阵的改变量是否大于旋转平移矩阵阈值,若不大于旋转平移矩阵阈值,则完成建图。
具体地,所述步骤S5进一步包括:
S51,当每帧点云都按照S4步骤建图后,判断当前是否为第一次建图,若此次建图是第一次建图,则将地面点八叉树octree_ground和全局八叉树octree_entire内的点云更新为当次建图的点云结果;然后重复S4步骤重新建图;若此次建图不是第一次建图,转至步骤S52;
S52,获取所述本次步骤S4中经历的建图旋转平移矩阵,判断每帧点云在本次步骤S4中经历的建图旋转平移矩阵相较于上一次步骤S4建图的改变量是否大于旋转平移矩阵阈值,若改变量小于等于所述旋转平移矩阵阈值则建图完成。
具体地,所述步骤S52包括:
S521,获取当前帧点云frame在本次步骤S4中经历的总旋转平移矩阵Tfinal,记录当前帧在步骤S412、S42、S43的算法处理过程中经历的建图旋转平移矩阵ΔT=Tfinal-T;其中,若当前为第num次建图,建图完成后,全部点云帧{frame1,frame2,…,framel}在本次步骤S4的S412、S42、S43建图算法过程中经历的建图旋转平移矩阵序列为
S522,设定旋转平移矩阵阈值T_trh;计算当次建图每帧点云在算法过程中经历的建图旋转平移矩阵序列与上次建图对应帧点云在算法过程中经历的建图旋转平移矩阵序列/>之差/>即,
其中num>1,计算其平均值得到若diff_avg大于旋转平移矩阵阈值T_trh,则将地面点八叉树octree_ground和全局八叉树octree_entire内的点云更新为当次建图的点云结果;然后重新开始重复S4步骤建图;若diff_avg小于等于旋转平移矩阵阈值T_trh,则建图精度满足要求,建图结束。
由此,通过对地面点八叉树和全局八叉树进行迭代、重复建图,最终达到误差收敛,实现了既能保证点云的局部精度,又能保证消除累计误差的定位精度。
在本公开的第二个示例性实施例中,提供了一种矿山三维点云地图的构建装置。所述矿山三维点云地图的构建装置包括可读存储介质及一个或多个处理器,其中所述可读存储介质用于存储可执行指令;所述一个或多个处理器根据所述可执行指令执行如前述实施例所述的矿山三维点云地图的构建方法。
为了达到简要说明的目的,上述实施例1中任何可作相同应用的技术特征叙述皆并于此,无需再重复相同叙述。
至此,已经结合附图对本公开实施例进行了详细描述。需要说明的是,在附图或说明书正文中,未绘示或描述的实现方式,均为所属技术领域中普通技术人员所知的形式,并未进行详细说明。此外,上述对各元件和方法的定义并不仅限于实施例中提到的各种具体结构、形状或方式,本领域普通技术人员可对其进行简单地更改或替换。
综上所述,本公开提供一种可利用微加工技术制备的、可实现电场三维测量的旋转谐振式三维电场传感器。即使待测电场强度的方向与传感器表面不垂直,本公开旋转谐振式三维传感器也能实现对待测电场强度的精确测量,从而可以广泛应用于气象、智能电网、资源探测等诸多领域。
说明书与权利要求中所使用的序数例如“第一”、“第二”、“第三”等的用词,以修饰相应的元件,其本身并不意味着该元件有任何的序数,也不代表某一元件与另一元件的顺序、或是制造方法上的顺序,该些序数的使用仅用来使具有某命名的一元件得以和另一具有相同命名的元件能做出清楚区分。
此外,除非特别描述或必须依序发生的步骤,上述步骤的顺序并无限制于以上所列,且可根据所需设计而变化或重新安排。并且上述实施例可基于设计及可靠度的考虑,彼此混合搭配使用或与其他实施例混合搭配使用,即不同实施例中的技术特征可以自由组合形成更多的实施例。
在此提供的算法和显示不与任何特定计算机、虚拟系统或者其它设备固有相关。各种通用系统也可以与基于在此的示教一起使用。根据上面的描述,构造这类系统所要求的结构是显而易见的。此外,本公开也不针对任何特定编程语言。应当明白,可以利用各种编程语言实现在此描述的本公开的内容,并且上面对特定语言所做的描述是为了披露本公开的最佳实施方式。
本公开可以借助于包括有若干不同元件的硬件以及借助于适当编程的计算机来实现。本公开的各个部件实施例可以以硬件实现,或者以在一个或者多个处理器上运行的软件模块实现,或者以它们的组合实现。本领域的技术人员应当理解,可以在实践中使用微处理器或者数字信号处理器(DSP)来实现根据本公开实施例的相关设备中的一些或者全部部件的一些或者全部功能。本公开还可以实现为用于执行这里所描述的方法的一部分或者全部的设备或者装置程序(例如,计算机程序和计算机程序产品)。这样的实现本公开的程序可以存储在计算机可读介质上,或者可以具有一个或者多个信号的形式。这样的信号可以从因特网网站上下载得到,或者在载体信号上提供,或者以任何其他形式提供。
本领域那些技术人员可以理解,可以对实施例中的设备中的模块进行自适应性地改变并且把它们设置在与该实施例不同的一个或多个设备中。可以把实施例中的模块或单元或组件组合成一个模块或单元或组件,以及此外可以把它们分成多个子模块或子单元或子组件。除了这样的特征和/或过程或者单元中的至少一些是相互排斥之外,可以采用任何组合对本说明书(包括伴随的权利要求、摘要和附图)中公开的所有特征以及如此公开的任何方法或者设备的所有过程或单元进行组合。除非另外明确陈述,本说明书(包括伴随的权利要求、摘要和附图)中公开的每个特征可以由提供相同、等同或相似目的的替代特征来代替。并且,在列举了若干装置的单元权利要求中,这些装置中的若干个可以是通过同一个硬件项来具体体现。
类似地,应当理解,为了精简本公开并帮助理解各个公开方面中的一个或多个,在上面对本公开的示例性实施例的描述中,本公开的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而,并不应将该公开的方法解释成反映如下意图:即所要求保护的本公开要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说,如下面的权利要求书所反映的那样,公开方面在于少于前面公开的单个实施例的所有特征。因此,遵循具体实施方式的权利要求书由此明确地并入该具体实施方式,其中每个权利要求本身都作为本公开的单独实施例。
以上所述的具体实施例,对本公开的目的、技术方案和有益效果进行了进一步详细说明,所应理解的是,以上所述仅为本公开的具体实施例而已,并不用于限制本公开,凡在本公开的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本公开的保护范围之内。
Claims (11)
1.一种矿山三维点云地图的构建方法,其特征在于,包括:
S1,进行点云预处理,计算当前点云帧每个采样点相对于当前点云帧第一个点的位姿偏移,依据所述位姿偏移进行位姿刚体变换,并标记出当前点云帧的地面点;
S2,根据距离选取点云关键帧,先行插入地面点八叉树和全局八叉树中;
S3,通过RTK输出的位姿点,计算车轮与地面接触的假设地面点,其中,所述RTK为实时载波相位差分仪;
S4,对当前帧点云及所述假设地面点进行配准并坐标转换;在地面点八又树中搜索当前帧点云标记的地面点云的邻近点集,并进行配准与坐标转换,将转换后的地面点云插入地面点八又树;以及在全局点八叉树中搜索当前帧点云的邻近点集,并进行配准与坐标转换,将转换后的当前点云插入全局八叉树;
S5,判断是否为第一次建图,若非第一次建图,则进一步判断每帧点云在本次建图中经历的建图旋转平移矩阵△T相较上次建图的改变量是否大于旋转平移矩阵阈值,若不大于所述旋转平移矩阵阈值,则完成建图。
2.根据权利要求1所述的矿山三维点云地图的构建方法,其特征在于,所述步骤S1包括:
S11,对于当前点云帧的任意采样点p,使用其时间戳p.timestamp对RTK输出的采样时间戳序列{pos1.t,pos2.t,…,posn.t}进行差值,其中n为任意采样点序号,t为任意采样点所对应的时刻,计算出该采样点对应的假设位姿pos_estimate;取当前点云帧的第一个采样点的假设位姿作为当前点云帧基准假设位姿每个采样点的假设位姿pos_estimate与当前点云帧基准假设位姿/>作差,计算出该点相对于当前点云帧基准假设位姿的位姿偏移pos_diff;对该点依据位姿偏移pos_diff进行位姿刚体变换;
S12,标记出当前点云帧的地面点frame.gromd。
3.根据权利要求2所述的矿山三维点云地图的构建方法,其特征在于,所述步骤S2包括:
S21,设定距离阈值m;
S22,对于RTK输出的采样序列{pos1,pos2,…,posn},从起始位置采样pos1开始计算累计距离,搜索到累计距离大于距离阈值m的最近采样输出作为第一个关键RTK采样点位,设其为posm1;
S23,从第一个关键RTK采样点位posm1开始重新计算累计距离,再次搜索到累计距离大于m的最近采样输出作为第二个关键RTK采样点位,设其为posm2,依次类推,直至将RTK采样序列搜索完毕,得到关键RTK采样点位序列{posm1,posm2,…,posmk},其中k为搜索到累计距离大于距离阈值m的最近采样输出关键采样点位的序号;
S24,对于关键RTK采样点位序列{posm1,posm2,…,posmk}中每个采样点位posmi,1<i≤k,根据其时间戳pos.t在所有点云帧时间戳序列fframe1.timestamp,frame2.timestamp,…,framel.timestamp}中寻找时间间隔最近的点云帧时间戳,设为framer.timestamp,则将其对应的点云帧确定为一个点云关键帧fiamer;得到关键RTK采样点位序列{posm1,posm2,…,posmk}对应的点云关键帧序列{framer1,framer2,…,fiamerk},其中r为时间间隔最近的点云帧时间戳的序号;;
S25,对点云关键帧序列{framer1,framer2,…,framerk}中的每一个点云关键帧framer,使用其时间戳framer.timestamp对RTK输出的采样时间戳序列{pos1.t,pos2.t,…,posn.t}进行差值,计算出该点云关键帧fiamer的假设位姿pos_.framer,并根据pos_.framer对framer进行刚体变换,将framer变换到投影坐标系中;将刚体变换后的framer先行插入全局八叉树octree_entire中,并将变换后的framer中被标记出的地面点云framer_ground线性插入地面点云八叉树octree_ground中。
4.根据权利要求3所述的矿山三维点云地图的构建方法,其特征在于,所述步骤S3包括:
S31,在车辆坐标系下,标定RTK输出位置与两个前车轮与地面接触点的相对距离:获取前左车轮对于RTK输出位置的相对位置坐标l1={x1,y1,z1},前右车轮对于RTK输出位置的相对位置坐标为l2={x2,y2,z2};
S32,对于RTK输出的采样序列{pos1,pos2,…,posn}的每个采样pos,将l1和l2根据pos的投影坐标向量{pos.x,posy,pos.z}和姿态角向量{pos.pitchpos.rollpos.yaw}作刚体旋转平移变换,得到l1`={x1`,yl,z1`}和l2`={x2`,y2`,z2`},作为单次采样的假设地面点;对RTK输出的采样序列{pos1,pos2,…,posn}中全部采样进行假设地面点计算,得到假设地面点集合{l1`1,l2`1,l1`2,l2`2,…,l1`n,l2`n},其中n为任意采样点序号。
5.根据权利要求4所述的矿山三维点云地图的构建方法,其特征在于,所述步骤S4包括:
S41,对当前帧点云及通过所述假设地面点进行配准并坐标转换;
S42,在地面点八叉树中对当前帧点云标记的地面点云进行搜索第一邻近点集,使用标记的地面点云与第一邻近点集进行配准,并进行坐标转换,将转换后的地面点云插入地面点八叉树;
S43,在全局点八又树中对当前帧点云进行搜索第二邻近点集,使用当前帧点云点云与第二邻近点集进行配准,并进行坐标转换,将转换后的当前点云插入全局八叉树。
6.根据权利要求5所述的矿山三维点云地图的构建方法,其特征在于,所述步骤S41包括:
S411,对于当前帧点云frame,使用其时间戳frametimestamp对RTK输出的采样时间戳序列{pos1.t,pos2.t,…,posn.t}进行差值,计算出该点云帧的假设位姿pos_.frame,并根据pos_.frame构建第一旋转平移矩阵T,对frame进行刚体变换,将frame变换到投影坐标系中;
S412,对于采用第一旋转平移矩阵转换后的当前帧点云frame中的每一条线的采样点集{p1,p2,…,ph},在假设地面点集{l1`1,12`1,l1`2,l2`2,…,l1`n,l2`n}中搜索最近的假设地面点,得到最近的采样点和地面点点对,设为pair={p,l},其中,p为采样点集合,从每条线的最近点对集中筛选出点对距离小于阈值r的点对集{pair1,pair2,…,pairh};使用点对集中的点云{pair1.p,pair2.p,…,pairh.p}作为输入源,使用点对集中的假设地面点{pair1.l,pair2.l,...,pairh.l}作为输入目标,进行ICP配准,得到第二旋转平移矩阵;将采用第一旋转平移矩阵转换后的当前帧点云根据第二旋转平移矩阵作坐标转换。
7.根据权利要求6所述的矿山三维点云地图的构建方法,其特征在于,所述步骤S42包括:
在地面点八叉树octree_ground中对采用第二旋转平移矩阵转换后的当前帧点云所标记的地面点云frame.gromd中的每个点进行搜索第一邻近点集,搜索方式为搜索当前点周围半径radius内不多于最大数目maxcnt的点集,所述半径radius与当前搜索点与激光雷达坐标系原点的距离dis成正比,记为一个系数a与距原点距离dis的乘积a*dis;对于frame.gromd中的每个点进行搜索,输出第一邻近点集neighbors_ground;使用当前帧标记的地面点云frame.gromd作为输入源,使用其所对应的第一邻近点集neighbors_ground为输入目标,进行ICP配准,得到第三旋转平移矩阵;将当前帧点云根据第三旋转平移矩阵作坐标转换,将转换后的当前帧点云中所标记的地面点云frame.ground插入地面点八叉树octree_ground中。
8.根据权利要求7所述的矿山三维点云地图的构建方法,其特征在于,所述步骤S43包括:
在全局八叉树octree_entire中对采用第三旋转平移矩阵转换后的当前帧点云frame中的每个点进行搜索第二邻近点集,搜索方式为搜索当前点附近的最近点;于frame中的每个点进行搜索,输出第二邻近点集neighbors_entire;使用当前帧点云frame作为输入源,使用其所对应的第二邻近点集neighbors_entire为输入目标,进行ICP配准,得到第四旋转平移矩阵;将当前帧点云根据第四旋转平移矩阵作坐标转换,将转换后的当前帧点云中所标记的地面点云插入地面点八又树octree_entire中。
9.根据权利要求8所述的矿山三维点云地图的构建方法,其特征在于,所述步骤S5包括:
S51,当每帧点云完成所述S4步骤后,判断当前是否为第一次建图,若此次建图是第一次建图,则将地面点八叉树octree_ground和全局八叉树octree_entire内的点云更新为当次建图的点云结果;然后返回S4步骤重新建图;若此次建图不是第一次建图,则转至步骤S52;
S52,获取本次所述步骤S4中经历的建图旋转平移矩阵,判断每帧点云在本次步骤S4中经历的建图旋转平移矩阵ΔT相较于上一次步骤S4建图的改变量是否大于旋转平移矩阵阈值,若改变量不大于所述旋转平移矩阵阈值则建图完成。
10.根据权利要求9所述的矿山三维点云地图的构建方法,其特征在于,所述步骤S52包括:
S521,获取当前帧点云frame在本次所述步骤S4中经历的总旋转平移矩阵Tfinal,记录当前帧在步骤S412、S42、S43的算法处理过程中经历的建图旋转平移矩阵ΔT=Tfinal-T,其中,T为旋转平移矩阵;其中,若当前为第num次建图,建图完成后,全部点云帧{frame1,frame2,…,framel}在本次步骤S4的S412、S42、S43建图算法过程中经历的建图旋转平移矩阵序列为
S522,设定旋转平移矩阵阈值T_trh;计算当次建图每帧点云在算法过程中经历的建图旋转平移矩阵序列与上次建图对应帧点云在算法过程中经历的建图旋转平移矩阵序列/>之差/>即,
其中num>1,计算其平均值得到若diff_avg大于旋转平移矩阵阈值T_trh,则将地面点八叉树octree_ground和全局八叉树octree_entire内的点云更新为当次建图的点云结果,然后重新开始重复S4步骤建图;若diff_avg小于等于旋转平移矩阵阈值T_trh,则判定建图精度满足要求,建图结束。
11.一种矿山三维点云地图的构建装置,其特征在于,所述矿山三维点云地图的构建装置包括:
可读存储介质,所述可读存储介质用于存储可执行指令;
一个或多个处理器,所述一个或多个处理器根据所述可执行指令执行如权利要求1-10所述的矿山三维点云地图的构建方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911068605.3A CN110827403B (zh) | 2019-11-04 | 2019-11-04 | 一种矿山三维点云地图的构建方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911068605.3A CN110827403B (zh) | 2019-11-04 | 2019-11-04 | 一种矿山三维点云地图的构建方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110827403A CN110827403A (zh) | 2020-02-21 |
CN110827403B true CN110827403B (zh) | 2023-08-25 |
Family
ID=69552791
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911068605.3A Active CN110827403B (zh) | 2019-11-04 | 2019-11-04 | 一种矿山三维点云地图的构建方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110827403B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111508021B (zh) * | 2020-03-24 | 2023-08-18 | 广州视源电子科技股份有限公司 | 一种位姿确定方法、装置、存储介质及电子设备 |
CN111311743B (zh) * | 2020-03-27 | 2023-04-07 | 北京百度网讯科技有限公司 | 三维重建精度测试方法、测试装置和电子设备 |
CN111539999A (zh) * | 2020-04-27 | 2020-08-14 | 深圳南方德尔汽车电子有限公司 | 基于点云配准的3d地图构建方法、装置、计算机设备及存储介质 |
CN112861674A (zh) * | 2021-01-28 | 2021-05-28 | 中振同辂(江苏)机器人有限公司 | 一种基于地面特征的点云优化方法及计算机可读存储介质 |
CN115661189A (zh) * | 2022-12-27 | 2023-01-31 | 上海仙工智能科技有限公司 | 一种场景动态信息检测方法及系统 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018048353A1 (en) * | 2016-09-09 | 2018-03-15 | Nanyang Technological University | Simultaneous localization and mapping methods and apparatus |
CN109100730A (zh) * | 2018-05-18 | 2018-12-28 | 北京师范大学-香港浸会大学联合国际学院 | 一种多车协同快速建图方法 |
CN109934920A (zh) * | 2019-05-20 | 2019-06-25 | 奥特酷智能科技(南京)有限公司 | 基于低成本设备的高精度三维点云地图构建方法 |
CN110264563A (zh) * | 2019-05-23 | 2019-09-20 | 武汉科技大学 | 一种基于orbslam2的八叉树建图方法 |
-
2019
- 2019-11-04 CN CN201911068605.3A patent/CN110827403B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2018048353A1 (en) * | 2016-09-09 | 2018-03-15 | Nanyang Technological University | Simultaneous localization and mapping methods and apparatus |
CN109100730A (zh) * | 2018-05-18 | 2018-12-28 | 北京师范大学-香港浸会大学联合国际学院 | 一种多车协同快速建图方法 |
CN109934920A (zh) * | 2019-05-20 | 2019-06-25 | 奥特酷智能科技(南京)有限公司 | 基于低成本设备的高精度三维点云地图构建方法 |
CN110264563A (zh) * | 2019-05-23 | 2019-09-20 | 武汉科技大学 | 一种基于orbslam2的八叉树建图方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110827403A (zh) | 2020-02-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110827403B (zh) | 一种矿山三维点云地图的构建方法及装置 | |
Dellenbach et al. | Ct-icp: Real-time elastic lidar odometry with loop closure | |
CN108765487B (zh) | 重建三维场景的方法、装置、设备和计算机可读存储介质 | |
CN109934920B (zh) | 基于低成本设备的高精度三维点云地图构建方法 | |
CN112268559B (zh) | 复杂环境下融合slam技术的移动测量方法 | |
CN108759833B (zh) | 一种基于先验地图的智能车辆定位方法 | |
CN111007530B (zh) | 激光点云数据处理方法、装置及系统 | |
CN112230242B (zh) | 位姿估计系统和方法 | |
CN110703268B (zh) | 一种自主定位导航的航线规划方法和装置 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN110187375A (zh) | 一种基于slam定位结果提高定位精度的方法及装置 | |
CN113358112A (zh) | 一种地图构建方法及一种激光惯性里程计 | |
Wen et al. | TM 3 Loc: Tightly-coupled monocular map matching for high precision vehicle localization | |
CN117541655B (zh) | 融合视觉语义消除radar建图z轴累积误差的方法 | |
CN117824667A (zh) | 一种基于二维码和激光的融合定位方法及介质 | |
CN114442133A (zh) | 无人机定位方法、装置、设备及存储介质 | |
Chiang et al. | High-Definition-Map-Based LiDAR Localization Through Dynamic Time-Synchronized Normal Distribution Transform Scan Matching | |
CN112098926B (zh) | 一种利用无人机平台的智能测角训练样本生成方法 | |
CN112424568A (zh) | 构建高清地图的系统和方法 | |
CN113418527A (zh) | 一种强实时双构连续景象融合匹配导航定位方法及系统 | |
CN110927765A (zh) | 激光雷达与卫星导航融合的目标在线定位方法 | |
CN112184906B (zh) | 一种三维模型的构建方法及装置 | |
CN115775242A (zh) | 一种基于匹配的点云地图质量评估方法 | |
CN115390088A (zh) | 点云地图建立方法、车道标注数据获取方法、设备及介质 | |
Mounier et al. | High-precision positioning in GNSS-challenged environments: a LiDAR-based multi-sensor fusion approach with 3D digital maps registration |
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 |