CN113985429B - 基于三维激光雷达的无人机环境扫描与重构方法 - Google Patents
基于三维激光雷达的无人机环境扫描与重构方法 Download PDFInfo
- Publication number
- CN113985429B CN113985429B CN202111114995.0A CN202111114995A CN113985429B CN 113985429 B CN113985429 B CN 113985429B CN 202111114995 A CN202111114995 A CN 202111114995A CN 113985429 B CN113985429 B CN 113985429B
- Authority
- CN
- China
- Prior art keywords
- point
- points
- laser
- pose
- dimensional
- 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
- 238000000034 method Methods 0.000 title claims abstract description 77
- 238000013507 mapping Methods 0.000 claims abstract description 21
- 238000005457 optimization Methods 0.000 claims description 62
- 230000033001 locomotion Effects 0.000 claims description 26
- 230000009466 transformation Effects 0.000 claims description 22
- 238000004364 calculation method Methods 0.000 claims description 20
- 239000011159 matrix material Substances 0.000 claims description 20
- 230000011218 segmentation Effects 0.000 claims description 16
- 230000008859 change Effects 0.000 claims description 15
- 238000010586 diagram Methods 0.000 claims description 12
- 239000013598 vector Substances 0.000 claims description 12
- 238000010276 construction Methods 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 9
- 238000000605 extraction Methods 0.000 claims description 6
- 238000013461 design Methods 0.000 claims description 5
- JXASPPWQHFOWPL-UHFFFAOYSA-N Tamarixin Natural products C1=C(O)C(OC)=CC=C1C1=C(OC2C(C(O)C(O)C(CO)O2)O)C(=O)C2=C(O)C=C(O)C=C2O1 JXASPPWQHFOWPL-UHFFFAOYSA-N 0.000 claims description 4
- 238000001514 detection method Methods 0.000 claims description 4
- 230000010354 integration Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 238000000638 solvent extraction Methods 0.000 claims description 4
- 238000002474 experimental method Methods 0.000 description 7
- 238000005259 measurement Methods 0.000 description 4
- 101100391182 Dictyostelium discoideum forI gene Proteins 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 230000007547 defect Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 241000196324 Embryophyta Species 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000005286 illumination Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 241000714197 Avian myeloblastosis-associated virus Species 0.000 description 1
- 101150037717 Mavs gene Proteins 0.000 description 1
- 238000013480 data collection Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
Landscapes
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明涉及一种利用四旋翼无人机机载三维激光雷达对室外环境进行扫描与重构的方法，为提出一种基于四旋翼无人机机载三维激光雷达的环境扫描与重构方法，实现四旋翼无人机利用机载三维激光雷达进行实时自主定位与建图。本发明，基于三维激光雷达的环境扫描与重构方法，在设置有机载三维激光雷达的四旋翼无人机上实现，步骤是，对三维激光雷达采集到的实时三维点云图进行点云分割，之后在点云图中提取特征点进行特征匹配，然后进行位姿图优化和回环检测得到较高精度的三维定位数据和三维点云地图，最后利用qPCV(Portion of Visible Sky)方法进行环境重构。本发明主要应用于四旋翼无人机自主定位场合。
Description
技术领域
本发明涉及一种利用四旋翼无人机机载三维激光雷达对室外环境进行扫描与重构的方法，特别是涉及四旋翼无人机有限机载计算资源的在线环境扫描与重构的方法。
背景技术
四旋翼无人机是一种多旋翼结构的飞行器。由于四旋翼无人机便利、廉价、机动性能强的优势，四旋翼无人机在科研、民用及军用等领域得到了广泛应用。目前复杂多变的飞行环境需要四旋翼无人机实现自主飞行。自主飞行的四旋翼无人机系统需要解决“定位”和“建图”问题。在无运动捕捉系统、GPS(Global Positioning System，全球定位系统)信号拒止等环境下，无人机则需要借助携带在机体上的传感器进行自主定位和环境感知。无人机利用机载传感器进行同时定位与建图的问题即为SLAM(Simultaneous Localization andMapping)问题。得益于雷达采集数据的方式，其在建筑物内、夜晚等无纹理、低光照环境下依旧能正常工作，且可提供具有高信噪比的测量结果，采用三维激光雷达SLAM算法进行四旋翼无人机的自主定位与地图构建是当前的研究热点之一。
2014年，卡内基梅隆大学的Ji Zhang团队(会议：InRobotics:Science andSystems Conference；著者：Zhang J,Singh S；出版年月：2014年7月；文章题目：LOAM:Lidar odometry and mapping in real-time[C]；页码：1-9)提出了名为LOAM的激光雷达SLAM算法。该算法在不需要高精度的距离测量和惯导测量数据的同时实现了低漂移和低计算复杂度。2017年，该团队进行了基于机载二维激光雷达的四旋翼无人机三维点云地图构建试验(期刊：AutonomousRobots；著者：Zhang J,Singh S；出版年月：2017年；文章题目：Low-drift and real-time lidar odometry and mapping；页码：401–416)。研究人员使用一架搭载了二维激光雷达和惯性测量单元的八旋翼无人机对一座小型桥梁进行了三维点云地图的构建。2015年，该团队提出了V-LOAM算法，涉及到的传感器为一个点头式二维激光雷达和一个单目摄像头。V-LOAM算法综合了这两种传感器的优点，并弥补了双方的缺点，在性能上比基于单个传感器的SLAM算法有了进一步的提高(会议：In 2015 IEEEInternational Conference on Robotics and Automation；著者：Zhang J,Singh S；出版年月：2015年5月；文章题目：Visual-lidar odometry and mapping:low-drift,robust,andfast[C]；页码：2174–2181)。2017年该团队提出了一种利用三维激光雷达、单目相机和IMU进行自主定位与建图的新型SLAM算法(会议：In 2017 IEEE InternationalConference on Robotics andAutomation；著者：Zhang J,Singh S；出版年月：2017年5月；文章题目：Enabling aggressive motion estimation at low-drift andaccuratemapping in real-time[C]；页码：5051–5058)。该算法可以进行高频率的传感器位姿估计，同时在各种光照条件和环境结构下构建稠密且准确的三维地图。
2017年，宾夕法尼亚大学的Vijay Kumar团队提出了一种以三维激光雷达、多个摄像头和IMU为主要传感器的SLAM算法(期刊：IEEE Robotics and AutomationLetters；著者：Ozaslan T,Loianno G,Keller J,et al；出版年月：2017年；文章题目：Autonomousnavigation and mapping for inspectionof penstocks and tunnels with MAVs；页码：1740–1747)。该团队在隧道环境下利用四旋翼无人机搭载激光雷达和摄像头进行了环境扫描与建图。
2016年，来自新加坡国立大学的研究人员提出了一种导航系统，该系统可以使小型四旋翼无人机在无GPS的树林环境中使用二维激光雷达进行自主导航(期刊：Journal ofIntelligent&Robotic Systems；著者：Cui J,Lai S,Dong X,et al；出版年月：2016年；文章题目：utonomous navigation of UAV in foliage environment；页码：259–276)。该系统包括无人机双环控制、导航时的位姿估计和在线路径规划。
发明内容
为克服现有技术的不足，本发明旨在提出一种基于四旋翼无人机机载三维激光雷达的环境扫描与重构方法，实现四旋翼无人机利用机载三维激光雷达进行实时自主定位与建图。本发明采用的技术方案是，基于三维激光雷达的环境扫描与重构方法，在设置有机载三维激光雷达的四旋翼无人机上实现，步骤是，对三维激光雷达采集到的实时三维点云图进行点云分割，之后在点云图中提取特征点进行特征匹配，然后进行位姿图优化和回环检测得到较高精度的三维定位数据和三维点云地图，最后利用qPCV(Portion of VisibleSky)方法进行环境重构。
具体步骤如下：
首先定义激光雷达里的激光发射器旋转一周为一个扫描周期，一个扫描周期内获得的点云数据为一帧点云图，记为第k帧点云图，表示正整数，每帧点云图中的所有激光点记为P，Pk代表了第k帧点云图中的所有激光点，Pk＝{pk1,pk2,…,pkn}，pki是k时刻的点云Pk中的一个激光点，将Pk投影到一幅二维图像上，其分辨率为M×N，M的大小由雷达的水平分辨率决定，N的大小由雷达的竖直分辨率决定，进行重投影后，三维点云中每个激光点变为二维图像中的像素点，其中每一个像素点带有坐标信息和深度信息，其坐标pki＝[xki,yki,zki]T表示该激光点在k时刻的点云图中，以激光雷达中心为原点的坐标系中的位置，深度信息rki则表示pki到k时刻激光雷达中心点的欧氏距离，重投影后形成的图像为二维深度图像；
然后对深度图像中的估计为地面的像素点进行标记：通过将x-y平面表示为半径R＝∞无限大的圆并将其分成数个独立的扇区来实现数据的有效分区，参数△α用于描述每个扇区覆盖的角度，由此得到M段扇区Si，点pki映射到的扇区号由sector(pki)表示，具体计算如下
其中atan2(yki,xki)给出平面的正x轴与点(xki,yki)之间的夹角，并确保其在[0,2π)之内,用Ps表示映射到相同扇区Ss的所有点的集合，
Ps＝{pi∈P|sector(pi)＝ss}#(2)
接着，根据同一扇区中的点(xki,yki,zki)到(0,0,zki)的距离的不同分散到多个距离范围不同的集合j＝1,2,…,B中，B表示最大的合集个数，分别用和表示集合覆盖的最小和最大距离，如果一个点pki∈Ps的距离满足则表示该点落在集合中，使用表示所有落在该集合中的点，由此，将三维点集合映射为二维点集合：
对于每一个非空集合都计算一个代表点选择z轴坐标值最低的点为代表点随后，利用代表点和增量算法进行线性拟合，最后根据拟合出的直线的斜率判断该直线是否可以代表地面，对于每个点，找到其所属扇区中与其距离最近的直线，如果直线被判断可以代表地面，且点到直线的距离小于一个阈值，则将该点标记为地面，否则不做标记；
然后对二维深度图像中的非地面点进行分割并标记，扫描到的激光点A和B是由相邻激光束OA和OB得到的相邻激光点，O表示激光雷达的中心点，存在一个以O为原点的坐标系，且该坐标系的y轴与OA和OB中较长的激光束重合，这里假定OA更长，定义角β为y轴和穿过A和B的直线之间的夹角，由于二维深度图像带有每个点的深度信息，即每个激光点到激光雷达中心的距离，则A和B到原点O的距离，分别定义为rA和rB，由此得到β的计算公式：
其中α为激光雷达两束相邻激光束之间的夹角，由激光雷达的分辨率决定，是固定值；
然后根据之前已得到的二维深度图像进行点云分割，如果两个激光点在深度图像中是相邻的，并且它们之间的角度β大于一个阈值θ，则将他们视为源自同一物体，对于每幅二维深度图像，从左上角的激光点开始计算，从左至右、从上至下，对于每一个没有标记过的特征点，先对其进行广度优先搜索，由此将所有与该点源自同一物体的激光点标记为相同的标签，对当前点的广度优先搜索结束后，按顺序找到下一个未标记点，重复上述操作，由此保证整幅二维深度图像在O(n)时间内完成标记；
对分割后的数据再进行如下处理：1)如果某个标签下的激光点个数超过30个，则该标签有效；2)如果某个标签下的激光点个数小于30大于等于5，统计其竖直方向上的激光点个数；3)如果竖直方向上的激光点个数大于3个，该标签有效；4)其余的标签均标记为不可用的类，即该标签无效；
如此处理后，每个激光点都具有三个信息：1)分割之后的标签值，用Lki表示；2)在二维深度图像中的行号和列号；3)深度信息rki，之后，对这些点进行特征提取；
进行特征提取的第一步为定义二维深度图像中每个激光点的光滑度c
其中，集合S由点pki以及和其在同一行的10个临近点组成，左右各5个，故|S|＝10，设定阈值cth，c>cth的点为边缘特征点，c<cth的点为平面特征点，为了在所有方向上都提取特征，将整幅二维深度图像在水平方向上分割成若干个子图，对于每个子图，在每一行上对每个点进行光滑度计算并排序，接着，从每一行的非地面点中，选取c值最大的个边缘特征点，组成边缘特征点集合从每一行的地面点或带有有效标签的激光点中，选取c值最小的个平面特征点，组成平面特征点集合为方便后续特征匹配，从集合中，选择个c值最大的边缘特征点组成一个更小的边缘特征点集合Fe；从集合中，选择个c值最小且属于地面点的平面特征点组成一个更小的平面特征点集合Fp，显然有如下关系：和
之后利用提取到的特征估计两个连续扫描帧之间的位姿变化，设定当前时刻为k时刻，则当前时刻较小的边缘特征点集合和平面特征点集合分别为和上一时刻中较大的边缘特征点集合和平面特征点集合分别为和令tk-1表示第k-1次扫描的开始时间，在每次扫描结束时，在扫描过程中接收到的点云Pk-1将重新投影到时间戳tk，重新投影得到的点云图为在第k次扫描运动期间，和Pk一起用于估计雷达的运动，因为 故将和均投影到tk时刻，分别得到和接下来，需要从和中找到和的关联点；
对于集合中的一个边缘特征点i，在中寻找与i的标签相同，且距离其最近的边缘特征点j，由于边缘特征点的对应特征是一条直线，且两点确定一条直线，故在中寻找与j的标签相同，且距离其最近的边缘特征点l，由此构建点到直线的几何约束关系，将点i，j和l的坐标分别记为和由此把姿态变换转化为点到直线的距离计算
边缘特征点的优化方程得以构建；
对于集合中的一个平面特征点i，在中寻找与i的标签相同，且距离其最近的平面特征点j，由于平面特征点的对应特征是一个平面，且三点确定一个平面，故在中寻找与j的标签相同，且距离其最近的两个平面特征点l和m，由此构建点到平面的几何约束关系，将点i，j，l和m的坐标分别记为和由此把姿态变换转化为点到平面的距离计算
平面特征点的优化方程得以建立。以上寻找最近点的算法使用k-d树(K-Dimensional Tree,K-D Tree)的最近邻搜索法，下面进行运动估计：
在一次扫描过程中，雷达的角速度和线速度都是均匀的，令t为当前时刻，tk为第k次扫描开始的时间，令Tk为[tk,t]时刻之间的雷达位姿变换，Tk包括了雷达在六自由度空间里的刚体运动，Tk＝[tx,ty,tz,θx,θy,θz]T，对于激光点pki，pki∈Pk，令ti为其时间戳，T(k,i)为[tk,ti]时刻之间的雷达位姿变换，则T(k,i)通过线性插值得到：
为解决雷达的运动估计问题，需要分别建立和与和之间的几何关系，根据式(8)可得
其中V(k,i)为或中得一个激光点i的坐标，为与其相对应的点坐标，T(k,i)(1:3)为T(k,i)的第一项到第三项，即(tx,ty,tz)。R表示由罗德里格斯公式定义的旋转矩阵
其中，θ是旋转角：
θ＝||T(k,i)(4:6)||,#(11)
表示旋转方向的单位向量：
表示由向量张成的反对称阵，即对于向量有：
根据式(6)和式(8)-(12)可以得到中的激光点和其对应点之间的几何关系：
同理可得中的激光点和其对应点之间的几何关系：
以式(14)为例，进行列文伯格-马夸尔特L-M(Levenberg-Marquardt)优化，fe中的每一行代表一个边缘特征点，he包含了边缘特征点与对应边缘线之间的距离，计算相对于Tk的fe的雅可比矩阵，记为J，然后，L-M方法利用求解优化问题式(14)，得到Tk的迭代式：
Tk←Tk-(JTJ+λdiag(JTJ))-1JThe.#(16)
其中λ是L-M方法中的系数因子。由此得到位姿估计Tk＝[tx,ty,tz,θx,θy,θz]T，基于其中的[tz,θx,θy]和式(15)进行第二次L-M优化，得到Tk＝[tx,ty,tz,θx,θy,θz]T选择其中的[tx,ty,θz]和第一次优化得到的[tz,θx,θy]组成最终的位姿估计；
进行两次L-M优化的前提条件是，激光雷达在运动过程中地面变化不大，如果地面变化剧烈，或者无人机在高空飞行，扫描到的地面点少于7200时，平面特征则从非地面点中得到，且在进行位姿估计时，不采用两次L-M优化，而是将式(14)-(15)联合起来得到：
f(Tk)＝h,#(17)
f中的每一行代表一个平面特征点或边缘特征点，h则包含了其对应的平面特征或边缘特征之间的距离，记f相对于Tk的雅可比矩阵为通过一次L-M优化得到六个自由度的位姿估计，迭代式如下：
Tk←Tk-(JTJ+λdiag(JTJ))-1JTh.#(18)
接下来进行位姿图优化设计，定义k时刻之前所有扫描到的点云在惯性坐标系下的投影为Qk-1，那么Qk-1就是k时刻时已经建立完成的三维点云地图，后端优化的目的就在于将k时刻获得的特征点匹配到点云地图Qk-1中，由此得到一组位姿估计结果；
每一时刻点云地图的存储方式为存储每次扫描得到的特征点集令 为存储k时刻之前的所有特征点集的集合，Qk-1中的每个集合都与雷达扫描时的位姿相关联，，因为经过位姿图优化的传感器位姿估计漂移较低，假设在短时间内没有漂移，令s定义为的大小。以中的每一项为位姿图中的一个节点，为该节点上的观测数据，作为对该节点的约束，将作为新节点加入位姿图中，由此得到位姿图优化的目标函数
其中V表示位姿图中所有位姿节点的位姿估计，∈为位姿图中所有边的集合，eij表示边(i,j)代表的位姿估计的误差，∑ij表示位姿估计的协方差矩阵，至此，优化问题变成了一个最小二乘问题，利用L-M优化求解，得到更为精确的位姿变换估计；
对于新的特征点集使用迭代最近点(Iterative Closest Point,ICP)法将其与之前时刻的特征点集进行匹配，若匹配成功，则表示检测到了回环，由此得到新的位姿估计约束，并将其加入到位姿图中，再使用iSAM2方法进行优化计算，更新位姿估计；如果匹配失败，则表示无人机当前位置并非已经过位置，不会加入新的约束；
地图构建模块根据位姿图优化以2赫兹发布的位姿估计，将每一帧局部点云图投影到惯性坐标系下，由此得到三维点云地图，且点云地图的发布频率也为2赫兹；局部点云地图的原点为当前时刻的传感器位置，惯性坐标系的原点则为算法开始时传感器的位置；变换整合模块将雷达里程计发布的10赫兹位姿估计和位姿图优化得到的2赫兹位姿估计进行插值，发布频率为10赫兹的最终的位姿估计；由此，实现了同时定位与建图；
最后利用CloudCompare软件中集成的qPCV算法对三维点云地图进行处理，得到重构过后的三维点云地图。
本发明的特点及有益效果是：
本发明设计基于三维激光雷达的环境扫描与重构方法，对搭载三维激光雷达的四旋翼无人机进行实时自主定位及建图具有良好的效果。
附图说明
图1是本发明的技术路线图。
图2是地面点云分割示意图。
图3是判断相邻激光点是否来自同一物体示意图。
图4是点云增长示意图。
图5是构建特征点几何约束关系示意图，其中图5(a)是边缘特征点构建几何约束关系的示意图，图5(b)是平面特征点构建几何约束关系的示意图。
图6是进行室外定点实验无人机位置变化曲线图。
图7是进行室外建图实验得到的天津大学北洋广场三维点云地图。
图8是经过重构处理之后的天津大学北洋广场三维点云地图。
具体实施方式
为克服现有技术的不足，本发明旨在提出一种基于四旋翼无人机机载三维激光雷达的环境扫描与重构方法，实现四旋翼无人机利用机载三维激光雷达进行实时自主定位与建图。本发明采用的技术方案是，基于三维激光雷达的环境扫描与重构方法，在设置有机载三维激光雷达的四旋翼无人机上实现，步骤是，对三维激光雷达采集到的实时三维点云图进行点云分割，之后在点云图中提取特征点进行特征匹配，然后进行位姿图优化和回环检测得到较高精度的三维定位数据和三维点云地图，最后利用qPCV(Portion of VisibleSky)方法进行环境重构。
进一步具体步骤是，首先定义激光雷达里的激光发射器旋转一周为一个扫描周期，一个扫描周期内获得的点云数据为一帧点云图，记为第k帧点云图，每帧点云图中的所有激光点记为P，Pk代表了第k帧点云图中的所有激光点。Pk＝{pk1,pk2,…,pkn}，pki是k时刻的点云Pk中的一个激光点。将Pk投影到一幅二维图像上，其分辨率为M×N，M的大小由雷达的水平分辨率决定，N的大小由雷达的竖直分辨率决定。本方法以Velodyne公司的VLP-16三维激光雷达为例，VLP-16激光雷达中的激光发射器旋转一周为360°，水平方向的角度分辨率为0.2°，故M＝360/0.2＝1800，且VLP-16在竖直方向上有16根扫描线，故N＝16。进行重投影后，三维点云中每个激光点变为二维图像中的像素点，其中每一个像素点带有坐标信息和深度信息。其坐标pki＝[xki,yki,zki]T表示该激光点在k时刻的点云图中，以激光雷达中心为原点的坐标系中的位置。深度信息rki则表示pki到k时刻激光雷达中心点的欧氏距离。这幅图像，下文称之为深度图像。
然后对深度图像中的估计为地面的像素点进行标记。在室外环境下，不可能所有的地面都是平坦的，因此地平面模型必须适合于描述平坦和倾斜地形以及介于两者之间的地形。为解决该问题，可以通过将x-y平面表示为半径R＝∞无限大的圆并将其分成数个独立的扇区来实现数据的有效分区，参数△α用于描述每个扇区覆盖的角度，由此得到M段扇区Si，点pki映射到的扇区号由sector(pki)表示，具体计算如下
其中atan2(yki,xki)给出平面的正x轴与点(xki,yki)之间的夹角，并确保其在[0,2π)之内。本方法用Ps表示映射到相同扇区Ss的所有点的集合，
Ps＝{pi∈P|sector(pi)＝ss}#(2)
接着，根据同一扇区中的点(xki,yki,zki)到(0,0,zki)的距离的不同分散到多个距离范围不同的集合j＝1,2,…,B中。本方法分别用和表示集合覆盖的最小和最大距离。如果一个点pki∈Ps的距离满足则表示该点落在集合中，使用表示所有落在该集合中的点。由此，将三维点集合映射为二维点集合
显然这种映射方式是一对多的，一个可能包括多个点，也可能一个点都不包括。因此，对于每一个非空集合都可以计算一个代表点使用z轴坐标最低的点会产生比较好的结果，因为它最有可能位于地平面上。随后，利用代表点和增量算法进行线性拟合。最后根据拟合出的直线的斜率判断该直线是否可以代表地面。对于每个点，找到其所属扇区中与其距离最近的直线，如果直线被判断可以代表地面，且点到直线的距离小于一个阈值，则将该点标记为地面，否则不做标记。
对于VLP-16激光雷达扫描得到的k时刻点云图，取△α＝0.2°，则共有M＝1800个扇区，等于二维深度图像的列数。VLP-16激光雷达的竖直扫描范围为[-15°,15°]，扫描到地面的激光点必定出现在范围为[-15°,-1°]的8根扫描线上，对应着二维深度图像的下面8行数据。令B＝8，将每个扇区中的8个点分别落在8个中，则每个集合恰好为一个点，代表点也随之确定。从最下面一排开始拟合直线，因为最下面一排的点最有可能扫描到地面。如果拟合后直线斜率的绝对值小于等于10°，则判定其为地面点。
然后本方法对二维深度图像中的非地面点进行分割并标记。本方法采用的分割方法的关键在于判断任意两束激光束得到的激光点，是否来自对同一物体的扫描。假定扫描到的激光点A和B是由相邻激光束OA和OB得到的相邻激光点，O表示激光雷达的中心点。不失一般性，假定存在一个以O为原点的坐标系，且该坐标系的y轴与OA和OB中较长的激光束重合，这里假定OA更长。定义角β为y轴和穿过A和B的直线之间的夹角。由于二维深度图像带有每个点的深度信息，即每个激光点到激光雷达中心的距离，我们可以知道A和B到原点O的距离，分别定义为rA和rB。由此可以得到β的计算公式，
其中α为激光雷达两束相邻激光束之间的夹角，由激光雷达的分辨率决定，是固定值。对于VLP-16激光雷达，当A和B为水平方向相邻点时，α＝0.2°，当A和B为竖直方向相邻点时，α＝2°。
然后根据之前已得到的二维深度图像进行点云分割。如果两个激光点在深度图像中是相邻的，并且它们之间的角度β大于一个阈值θ，则将他们视为源自同一物体。对于每幅二维深度图像，从左上角的激光点开始计算，从左至右、从上至下。对于每一个没有标记过的特征点，先对其进行广度优先搜索，由此可以将所有与该点源自同一物体的激光点标记为相同的标签。对当前点的广度优先搜索结束后，按顺序找到下一个未标记点，重复上述操作，由此可以保证整幅二维深度图像在O(n)时间内完成标记。
需要注意的是，这种判断方法存在一个失败的场景：当激光雷达非常靠近一堵墙时。对于远离激光雷达但仍在墙壁上的激光点，β将很小，因此墙壁远离激光雷达的部分可能会分成多个部分。从本质上讲，这意味着如果β小于θ，则很难推断出这两个点是起源于两个不同物体还是仅位于几乎平行于激光线方向的墙壁上的点。而且，当激光雷达在草地环境中时，地面上的杂草将会被分割成多个部分，但这些部分对于特征匹配几乎没有帮助甚至会降低匹配精度。考虑到这两种情况，对分割后的数据再进行如下处理：1)如果某个标签下的激光点个数超过30个，则该标签有效；2)如果某个标签下的激光点个数小于30大于等于5，统计其竖直方向上的激光点个数；3)如果竖直方向上的激光点个数大于3个，该标签有效；4)其余的标签均标记为不可用的类，即该标签无效。
如此处理后，每个激光点都具有三个信息：1)分割之后的标签值，用Lki表示；2)在二维深度图像中的行号和列号；3)深度信息rki。之后，对这些点进行特征提取。
进行特征提取的第一步为定义二维深度图像中每个激光点的光滑度c
其中，集合S由点pki以及和其在同一行的10个临近点组成，左右各5个，故|S|＝10。设定阈值cth，c>cth的点为边缘特征点，c<cth的点为平面特征点。为了在所有方向上都提取特征，将整幅二维深度图像在水平方向上分割成若干个子图。对于每个子图，在每一行上对每个点进行光滑度计算并排序。接着，从每一行的非地面点中，选取c值最大的个边缘特征点，组成边缘特征点集合从每一行的地面点或带有有效标签的激光点中，选取c值最小的个平面特征点，组成平面特征点集合为方便后续特征匹配，从集合中，选择个c值最大的边缘特征点组成一个更小的边缘特征点集合Fe；从集合中，选择个c值最小且属于地面点的平面特征点组成一个更小的平面特征点集合Fp。显然有如下关系：和在本文中，每幅二维深度图像被分为6幅子图，每幅子图的分辨率为300×16。
之后利用提取到的特征估计两个连续扫描帧之间的位姿变化。设定当前时刻为k时刻，则当前时刻较小的边缘特征点集合和平面特征点集合分别为和上一时刻中较大的边缘特征点集合和平面特征点集合分别为和令tk-1表示第k-1次扫描的开始时间，在每次扫描结束时，在扫描过程中接收到的点云Pk-1将重新投影到时间戳tk，重新投影得到的点云图为在第k次扫描运动期间，和Pk一起用于估计雷达的运动。因为 故将和均投影到tk时刻，分别得到和接下来，需要从和中找到和的关联点。
对于集合中的一个边缘特征点i，在中寻找与i的标签相同，且距离其最近的边缘特征点j，由于边缘特征点的对应特征是一条直线，且两点确定一条直线，故在中寻找与j的标签相同，且距离其最近的边缘特征点l。由此构建点到直线的几何约束关系。将点i，j和l的坐标分别记为和由此把姿态变换转化为点到直线的距离计算
边缘特征点的优化方程得以构建。
对于集合中的一个平面特征点i，在中寻找与i的标签相同，且距离其最近的平面特征点j，由于平面特征点的对应特征是一个平面，且三点确定一个平面，故在中寻找与j的标签相同，且距离其最近的两个平面特征点l和m。由此构建点到平面的几何约束关系。将点i，j，l和m的坐标分别记为和由此把姿态变换转化为点到平面的距离计算
平面特征点的优化方程得以建立。以上寻找最近点的算法使用k-d树(K-Dimensional Tree,K-D Tree)的最近邻搜索法。下面进行运动估计。
假设在一次扫描过程中，雷达的角速度和线速度都是均匀的。令t为当前时刻，tk为第k次扫描开始的时间。令Tk为[tk,t]时刻之间的雷达位姿变换。Tk包括了雷达在六自由度空间里的刚体运动，Tk＝[tx,ty,tz,θx,θ,θz]T。对于激光点pki，pki∈Pk，令ti为其时间戳，T(k,i)为[tk,ti]时刻之间的雷达位姿变换。则T(k,i)可以通过线性插值得到：
为解决雷达的运动估计问题，需要分别建立和与和之间的几何关系。根据式(8)可得
其中V(k,i)为或中得一个激光点i的坐标，为与其相对应的点坐标，T(k,i)(1:3)为T(k,i)的第一项到第三项，即(tx,ty,tz)。R表示由罗德里格斯公式定义的旋转矩阵
其中，θ是旋转角：
θ＝||T(k,i)(4:6)||,#(11)
表示旋转方向的单位向量：
表示由向量张成的反对称阵，即对于向量有：
根据式(6)和式(8)-(12)可以得到中的激光点和其对应点之间的几何关系：
同理可得中的激光点和其对应点之间的几何关系：
以式(14)为例，进行列文伯格-马夸尔特(Levenberg-Marquardt，L-M)优化。fe中的每一行代表一个边缘特征点，he包含了边缘特征点与对应边缘线之间的距离。计算相对于Tk的fe的雅可比矩阵，记为J，然后，L-M方法利用求解优化问题式(14)，得到Tk的迭代式：
Tk←Tk-(JTJ+λdiag(JTJ))-1JThe.#(16)
其中λ是L-M方法中的系数因子。由此得到位姿估计Tk＝[tx,ty,tz,θx,θy,θz]T，基于其中的[tz,θx,θy]和式(15)进行第二次L-M优化，得到Tk＝[tx,ty,tz,θx,θy,θz]T选择其中的[tx,ty,θz]和第一次优化得到的[tz,θx,θy]组成最终的位姿估计。
需要注意的是，进行两次L-M优化的前提条件是，激光雷达在运动过程中地面变化不大，如果地面变化剧烈，或者无人机在高空飞行，扫描到的地面点少于7200时，平面特征则从非地面点中得到，且在进行位姿估计时，不采用两次L-M优化，而是将式(14)-(15)联合起来得到：
f(Tk)＝h,#(17)
f中的每一行代表一个平面特征点或边缘特征点，h则包含了其对应的平面特征或边缘特征之间的距离。记f相对于Tk的雅可比矩阵为通过一次L-M优化得到六个自由度的位姿估计。迭代式如下：
Tk←Tk-(JTJ+λdiag(JTJ))-1JTh.#(18)
接下来进行位姿图优化设计。定义k时刻之前所有扫描到的点云在惯性坐标系下的投影为Qk-1，那么Qk-1就是k时刻时已经建立完成的三维点云地图。后端优化的目的就在于将k时刻获得的特征点匹配到点云地图Qk-1中，由此得到一组比特征匹配得到的位姿估计频率更低、精度更高的位姿估计结果。
每一时刻点云地图的存储方式为存储每次扫描得到的特征点集令 为存储k时刻之前的所有特征点集的集合。Qk-1中的每个集合都与雷达扫描时的位姿相关联。因为经过位姿图优化的传感器位姿估计漂移较低，假设在短时间内没有漂移，可以令s定义为的大小。以中的每一项为位姿图中的一个节点，为该节点上的观测数据，作为对该节点的约束。将作为新节点加入位姿图中，由此得到位姿图优化的目标函数
其中V表示位姿图中所有位姿节点的位姿估计，∈为位姿图中所有边的集合，eij表示边(i,j)代表的位姿估计的误差，∑ij表示位姿估计的协方差矩阵。至此，优化问题变成了一个最小二乘问题，利用L-M优化求解，得到更为精确的位姿变换估计。
对于新的特征点集使用迭代最近点(Iterative Closest Point,ICP)法将其与之前时刻的特征点集进行匹配，若匹配成功，则表示检测到了回环，由此得到新的位姿估计约束，并将其加入到位姿图中，再使用iSAM2方法进行优化计算，更新位姿估计。如果匹配失败，则表示无人机当前位置并非已经过位置，不会加入新的约束。
地图构建模块根据位姿图优化以2赫兹发布的位姿估计，将每一帧局部点云图投影到惯性坐标系下，由此得到三维点云地图，且点云地图的发布频率也为2赫兹。局部点云地图的原点为当前时刻的传感器位置，惯性坐标系的原点则为算法开始时传感器的位置。变换整合模块将雷达里程计发布的10赫兹位姿估计和位姿图优化得到的2赫兹位姿估计进行插值，发布频率为10赫兹的最终的位姿估计。由此，实现了同时定位与建图。
最后利用CloudCompare软件中集成的qPCV算法对三维点云地图进行处理，得到重构过后的三维点云地图。
本发明要解决的技术问题是，提出一种基于四旋翼无人机机载三维激光雷达的环境扫描与重构方法，实现四旋翼无人机利用机载三维激光雷达进行实时自主定位与建图。
本发明采用的技术方案是，基于三维激光雷达的环境扫描与重构方法，在设置有机载三维激光雷达的四旋翼无人机上实现，步骤是，对三维激光雷达采集到的实时三维点云图进行点云分割，之后在点云图中提取特征点进行特征匹配，然后进行位姿图优化和回环检测得到较高精度的三维定位数据和三维点云地图，最后利用qPCV(Portion ofVisible Sky)方法进行环境重构。包括如下步骤：
首先定义激光雷达里的激光发射器旋转一周为一个扫描周期，一个扫描周期内获得的点云数据为一帧点云图，记为第k帧点云图，每帧点云图中的所有激光点记为P，Pk代表了第k帧点云图中的所有激光点。Pk＝{pk1,pk2,…,pkn}，pki是k时刻的点云Pk中的一个激光点。将Pk投影到一幅二维图像上，其分辨率为M×N，M的大小由雷达的水平分辨率决定，N的大小由雷达的竖直分辨率决定。本方法以Velodyne公司的VLP-16三维激光雷达为例，VLP-16激光雷达中的激光发射器旋转一周为360°，水平方向的角度分辨率为0.2°，故M＝360/0.2＝1800，且VLP-16在竖直方向上有16根扫描线，故N＝16。进行重投影后，三维点云中每个激光点变为二维图像中的像素点，其中每一个像素点带有坐标信息和深度信息。其坐标pki＝[xki,yki,zki]T表示该激光点在k时刻的点云图中，以激光雷达中心为原点的坐标系中的位置。深度信息rki则表示pki到k时刻激光雷达中心点的欧氏距离。这幅图像，下文称之为深度图像。
然后对深度图像中的估计为地面的像素点进行标记。在室外环境下，不可能所有的地面都是平坦的，因此地平面模型必须适合于描述平坦和倾斜地形以及介于两者之间的地形。为解决该问题，可以通过将x-y平面表示为半径R＝∞无限大的圆并将其分成数个独立的扇区来实现数据的有效分区，参数△α用于描述每个扇区覆盖的角度，由此得到M段扇区Si，点pki映射到的扇区号由sector(pki)表示，具体计算如下
其中atan2(yki,xki)给出平面的正x轴与点(xki,yki)之间的夹角，并确保其在[0,2π)之内。本方法用Ps表示映射到相同扇区Ss的所有点的集合，
Ps＝{pi∈P|sector(pi)＝ss}#(2)
接着，根据同一扇区中的点(xki,yki,zki)到(0,0,zki)的距离的不同分散到多个距离范围不同的集合j＝1,2,…,B中。本方法分别用和表示集合覆盖的最小和最大距离。如果一个点pki∈Ps的距离满足则表示该点落在集合中，使用表示所有落在该集合中的点。由此，将三维点集合映射为二维点集合
显然这种映射方式是一对多的，一个可能包括多个点，也可能一个点都不包括。因此，对于每一个非空集合都可以计算一个代表点使用z轴坐标最低的点会产生比较好的结果，因为它最有可能位于地平面上。随后，利用代表点和增量算法进行线性拟合。最后根据拟合出的直线的斜率判断该直线是否可以代表地面。对于每个点，找到其所属扇区中与其距离最近的直线，如果直线被判断可以代表地面，且点到直线的距离小于一个阈值，则将该点标记为地面，否则不做标记。
对于VLP-16激光雷达扫描得到的k时刻点云图，取△α＝0.2°，则共有M＝1800个扇区，等于二维深度图像的列数。VLP-16激光雷达的竖直扫描范围为[-15°,15°]，扫描到地面的激光点必定出现在范围为[-15°,-1°]的8根扫描线上，对应着二维深度图像的下面8行数据。令B＝8，将每个扇区中的8个点分别落在8个中，则每个集合恰好为一个点，代表点也随之确定。从最下面一排开始拟合直线，因为最下面一排的点最有可能扫描到地面。如果拟合后直线斜率的绝对值小于等于10°，则判定其为地面点。
然后本方法对二维深度图像中的非地面点进行分割并标记。本方法采用的分割方法的关键在于判断任意两束激光束得到的激光点，是否来自对同一物体的扫描。假定扫描到的激光点A和B是由相邻激光束OA和OB得到的相邻激光点，O表示激光雷达的中心点。不失一般性，假定存在一个以O为原点的坐标系，且该坐标系的y轴与OA和OB中较长的激光束重合，这里假定OA更长。定义角β为y轴和穿过A和B的直线之间的夹角。由于二维深度图像带有每个点的深度信息，即每个激光点到激光雷达中心的距离，我们可以知道A和B到原点O的距离，分别定义为rA和rB。由此可以得到β的计算公式，
其中α为激光雷达两束相邻激光束之间的夹角，由激光雷达的分辨率决定，是固定值。对于VLP-16激光雷达，当A和B为水平方向相邻点时，α＝0.2°，当A和B为竖直方向相邻点时，α＝2°。
然后根据之前已得到的二维深度图像进行点云分割。如果两个激光点在深度图像中是相邻的，并且它们之间的角度β大于一个阈值θ，则将他们视为源自同一物体。对于每幅二维深度图像，从左上角的激光点开始计算，从左至右、从上至下。对于每一个没有标记过的特征点，先对其进行广度优先搜索，由此可以将所有与该点源自同一物体的激光点标记为相同的标签。对当前点的广度优先搜索结束后，按顺序找到下一个未标记点，重复上述操作，由此可以保证整幅二维深度图像在O(n)时间内完成标记。
需要注意的是，这种判断方法存在一个失败的场景：当激光雷达非常靠近一堵墙时。对于远离激光雷达但仍在墙壁上的激光点，β将很小，因此墙壁远离激光雷达的部分可能会分成多个部分。从本质上讲，这意味着如果β小于θ，则很难推断出这两个点是起源于两个不同物体还是仅位于几乎平行于激光线方向的墙壁上的点。而且，当激光雷达在草地环境中时，地面上的杂草将会被分割成多个部分，但这些部分对于特征匹配几乎没有帮助甚至会降低匹配精度。考虑到这两种情况，对分割后的数据再进行如下处理：1)如果某个标签下的激光点个数超过30个，则该标签有效；2)如果某个标签下的激光点个数小于30大于等于5，统计其竖直方向上的激光点个数；3)如果竖直方向上的激光点个数大于3个，该标签有效；4)其余的标签均标记为不可用的类，即该标签无效。
如此处理后，每个激光点都具有三个信息：1)分割之后的标签值，用Lki表示；2)在二维深度图像中的行号和列号；3)深度信息rki。之后，对这些点进行特征提取。
进行特征提取的第一步为定义二维深度图像中每个激光点的光滑度c
其中，集合S由点pki以及和其在同一行的10个临近点组成，左右各5个，故|S|＝10。设定阈值cth，c>cth的点为边缘特征点，c<cth的点为平面特征点。为了在所有方向上都提取特征，将整幅二维深度图像在水平方向上分割成若干个子图。对于每个子图，在每一行上对每个点进行光滑度计算并排序。接着，从每一行的非地面点中，选取c值最大的个边缘特征点，组成边缘特征点集合从每一行的地面点或带有有效标签的激光点中，选取c值最小的个平面特征点，组成平面特征点集合为方便后续特征匹配，从集合中，选择个c值最大的边缘特征点组成一个更小的边缘特征点集合Fe；从集合中，选择个c值最小且属于地面点的平面特征点组成一个更小的平面特征点集合Fp。显然有如下关系：和在本文中，每幅二维深度图像被分为6幅子图，每幅子图的分辨率为300×16。
之后利用提取到的特征估计两个连续扫描帧之间的位姿变化。设定当前时刻为k时刻，则当前时刻较小的边缘特征点集合和平面特征点集合分别为和上一时刻中较大的边缘特征点集合和平面特征点集合分别为和令tk-1表示第k-1次扫描的开始时间，在每次扫描结束时，在扫描过程中接收到的点云Pk-1将重新投影到时间戳tk，重新投影得到的点云图为在第k次扫描运动期间，和Pk一起用于估计雷达的运动。因为 故将和均投影到tk时刻，分别得到和接下来，需要从和中找到和的关联点。
对于集合中的一个边缘特征点i，在中寻找与i的标签相同，且距离其最近的边缘特征点j，由于边缘特征点的对应特征是一条直线，且两点确定一条直线，故在中寻找与j的标签相同，且距离其最近的边缘特征点l。由此构建点到直线的几何约束关系。将点i，j和l的坐标分别记为和由此把姿态变换转化为点到直线的距离计算
边缘特征点的优化方程得以构建。
对于集合中的一个平面特征点i，在中寻找与i的标签相同，且距离其最近的平面特征点j，由于平面特征点的对应特征是一个平面，且三点确定一个平面，故在中寻找与j的标签相同，且距离其最近的两个平面特征点l和m。由此构建点到平面的几何约束关系。将点i，j，l和m的坐标分别记为和由此把姿态变换转化为点到平面的距离计算
平面特征点的优化方程得以建立。以上寻找最近点的算法使用k-d树(K-Dimensional Tree,K-D Tree)的最近邻搜索法。下面进行运动估计。
假设在一次扫描过程中，雷达的角速度和线速度都是均匀的。令t为当前时刻，tk为第k次扫描开始的时间。令Tk为[tk,t]时刻之间的雷达位姿变换。Tk包括了雷达在六自由度空间里的刚体运动，Tk＝[tx,ty,z,θx,θy,θz]T。对于激光点pki，pki∈Pk，令ti为其时间戳，T(k,i)为[tk,ti]时刻之间的雷达位姿变换。则T(k,i)可以通过线性插值得到：
为解决雷达的运动估计问题，需要分别建立和与和之间的几何关系。根据式(8)可得
其中V(k,i)为或中得一个激光点i的坐标，为与其相对应的点坐标，T(k,i)(1:3)为T(k,i)的第一项到第三项，即(tx,ty,tz)。R表示由罗德里格斯公式定义的旋转矩阵
其中，θ是旋转角：
θ＝||T(k,i)(4:6)||,#(11)
表示旋转方向的单位向量：
表示由向量张成的反对称阵，即对于向量有：
根据式(6)和式(8)-(12)可以得到中的激光点和其对应点之间的几何关系：
同理可得中的激光点和其对应点之间的几何关系：
以式(14)为例，进行列文伯格-马夸尔特(Levenberg-Marquardt，L-M)优化。fe中的每一行代表一个边缘特征点，he包含了边缘特征点与对应边缘线之间的距离。计算相对于Tk的fe的雅可比矩阵，记为J，然后，L-M方法利用求解优化问题式(14)，得到Tk的迭代式：
Tk←T-(JTJ+λdiag(JTJ))-1JThe.#(16)
其中λ是L-M方法中的系数因子。由此得到位姿估计Tk＝[tx,ty,tz,θx,θy,θz]T，基于其中的[tz,θx,θy]和式(15)进行第二次L-M优化，得到Tk＝[tx,ty,tz,θx,θy,θz]T选择其中的[tx,ty,θz]和第一次优化得到的[tz,θx,θy]组成最终的位姿估计。
需要注意的是，进行两次L-M优化的前提条件是，激光雷达在运动过程中地面变化不大，如果地面变化剧烈，或者无人机在高空飞行，扫描到的地面点少于7200时，平面特征则从非地面点中得到，且在进行位姿估计时，不采用两次L-M优化，而是将式(14)-(15)联合起来得到：
f(Tk)＝h,#(17)
f中的每一行代表一个平面特征点或边缘特征点，h则包含了其对应的平面特征或边缘特征之间的距离。记f相对于Tk的雅可比矩阵为通过一次L-M优化得到六个自由度的位姿估计。迭代式如下：
Tk←Tk-(JTJ+λdiag(JTJ))-1JTh.#(18)
接下来进行位姿图优化设计。定义k时刻之前所有扫描到的点云在惯性坐标系下的投影为Qk-1，那么Qk-1就是k时刻时已经建立完成的三维点云地图。后端优化的目的就在于将k时刻获得的特征点匹配到点云地图Qk-1中，由此得到一组比特征匹配得到的位姿估计频率更低、精度更高的位姿估计结果。
每一时刻点云地图的存储方式为存储每次扫描得到的特征点集令 为存储k时刻之前的所有特征点集的集合。Qk-1中的每个集合都与雷达扫描时的位姿相关联。因为经过位姿图优化的传感器位姿估计漂移较低，假设在短时间内没有漂移，可以令s定义为的大小。以中的每一项为位姿图中的一个节点，为该节点上的观测数据，作为对该节点的约束。将作为新节点加入位姿图中，由此得到位姿图优化的目标函数
其中V表示位姿图中所有位姿节点的位姿估计，∈为位姿图中所有边的集合，eij表示边(i,j)代表的位姿估计的误差，∑ij表示位姿估计的协方差矩阵。至此，优化问题变成了一个最小二乘问题，利用L-M优化求解，得到更为精确的位姿变换估计。
对于新的特征点集使用迭代最近点(Iterative Closest Point,ICP)法将其与之前时刻的特征点集进行匹配，若匹配成功，则表示检测到了回环，由此得到新的位姿估计约束，并将其加入到位姿图中，再使用iSAM2方法进行优化计算，更新位姿估计。如果匹配失败，则表示无人机当前位置并非已经过位置，不会加入新的约束。
地图构建模块根据位姿图优化以2赫兹发布的位姿估计，将每一帧局部点云图投影到惯性坐标系下，由此得到三维点云地图，且点云地图的发布频率也为2赫兹。局部点云地图的原点为当前时刻的传感器位置，惯性坐标系的原点则为算法开始时传感器的位置。变换整合模块将雷达里程计发布的10赫兹位姿估计和位姿图优化得到的2赫兹位姿估计进行插值，发布频率为10赫兹的最终的位姿估计。由此，实现了同时定位与建图。
最后利用CloudCompare软件中集成的qPCV算法对三维点云地图进行处理，得到重构过后的三维点云地图。
为验证本发明的基于三维激光雷达的环境扫描与重构的有效性，进行了实际实验验证。
一、实验平台介绍
实验平台由以一架轴距0.65米的四旋翼无人机为主体，无人机上搭载了一个三维激光雷达进行数据采集以及一块x86架构的计算板用于环境扫描与重构算法的实现，机载差分GPS传感器提供无人机定位数据用于和本方法中的定位算法部分进行定位精度的比较。
二、定位精度比较实验
图6描述了进行室外定点实验时，采用本方法进行无人机位置估计的位置变化曲线和采用差分GPS传感器进行无人机位置估计的位置变化曲线。可以看到，本方法的定位精度在z方向上为5厘米，这得益于激光雷达SLAM算法中的点云分割过程。在x-y方向上的定位精度为20厘米。
经过上述分析，证明了本发明中定位算法部分的有效性。
三、建图和环境重构实验
图7描述了进行室外建图实验的天津大学北洋广场三维点云地图效果图，该点云地图无明显重影，效果良好。图8描述了对图7进行重构处理后得到的三维点云地图，进行重构后的地图时物体看起来更加立体，点云地图中的物体也更易辨别。
经过上述分析，证明了本发明所提算法的有效性。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (1)
1.一种基于三维激光雷达的环境扫描与重构方法，其特征是，在设置有机载三维激光雷达的四旋翼无人机上实现，步骤是，对三维激光雷达采集到的实时三维点云图进行点云分割，之后在点云图中提取特征点进行特征匹配，然后进行位姿图优化和回环检测得到较高精度的三维定位数据和三维点云地图，最后利用qPCV(Portion of Visible Sky)方法进行环境重构；具体步骤如下：
首先定义激光雷达里的激光发射器旋转一周为一个扫描周期，一个扫描周期内获得的点云数据为一帧点云图，记为第k帧点云图， 表示正整数，每帧点云图中的所有激光点记为P，Pk代表了第k帧点云图中的所有激光点，Pk＝{pk1,pk2,…,pkn}，pki是k时刻的点云Pk中的一个激光点，将Pk投影到一幅二维图像上，其分辨率为M×N，M的大小由雷达的水平分辨率决定，N的大小由雷达的竖直分辨率决定，进行重投影后，三维点云中每个激光点变为二维图像中的像素点，其中每一个像素点带有坐标信息和深度信息，其坐标pki＝[xki,yki,zki]T表示该激光点在k时刻的点云图中，以激光雷达中心为原点的坐标系中的位置，深度信息rki则表示pki到k时刻激光雷达中心点的欧氏距离，重投影后形成的图像为二维深度图像；
然后对深度图像中的估计为地面的像素点进行标记：通过将x-y平面表示为半径R＝∞无限大的圆并将其分成数个独立的扇区来实现数据的有效分区，参数△α用于描述每个扇区覆盖的角度，由此得到M段扇区Si，点pki映射到的扇区号由sector(pki)表示，具体计算如下
其中atan2(yki,xki)给出平面的正x轴与点(xki,yki)之间的夹角，并确保其在[0,2π)之内,用Ps表示映射到相同扇区Ss的所有点的集合，
Ps＝{pi∈P|sector(pi)＝ss} (2)
接着，根据同一扇区中的点(xki,yki,zki)到(0,0,zki)的距离的不同分散到多个距离范围不同的集合中，B表示最大的合集个数，分别用和表示集合覆盖的最小和最大距离，如果一个点pki∈Ps的距离满足则表示该点落在集合中，使用表示所有落在该集合中的点，由此，将三维点集合映射为二维点集合：
对于每一个非空集合都计算一个代表点选择z轴坐标值最低的点为代表点随后，利用代表点和增量算法进行线性拟合，最后根据拟合出的直线的斜率判断该直线是否可以代表地面，对于每个点，找到其所属扇区中与其距离最近的直线，如果直线被判断可以代表地面，且点到直线的距离小于一个阈值，则将该点标记为地面，否则不做标记；
然后对二维深度图像中的非地面点进行分割并标记，扫描到的激光点A和B是由相邻激光束OA和OB得到的相邻激光点，O表示激光雷达的中心点，存在一个以O为原点的坐标系，且该坐标系的y轴与OA和OB中较长的激光束重合，这里假定OA更长，定义角β为y轴和穿过A和B的直线之间的夹角，由于二维深度图像带有每个点的深度信息，即每个激光点到激光雷达中心的距离，则A和B到原点O的距离，分别定义为rA和rB，由此得到β的计算公式：
其中α为激光雷达两束相邻激光束之间的夹角，由激光雷达的分辨率决定，是固定值；
然后根据之前已得到的二维深度图像进行点云分割，如果两个激光点在深度图像中是相邻的，并且它们之间的角度β大于一个阈值θ，则将他们视为源自同一物体，对于每幅二维深度图像，从左上角的激光点开始计算，从左至右、从上至下，对于每一个没有标记过的特征点，先对其进行广度优先搜索，由此将所有与该点源自同一物体的激光点标记为相同的标签，对当前点的广度优先搜索结束后，按顺序找到下一个未标记点，重复上述操作，由此保证整幅二维深度图像在O(n)时间内完成标记；
对分割后的数据再进行如下处理：1)如果某个标签下的激光点个数超过30个，则该标签有效；2)如果某个标签下的激光点个数小于30大于等于5，统计其竖直方向上的激光点个数；3)如果竖直方向上的激光点个数大于3个，该标签有效；4)其余的标签均标记为不可用的类，即该标签无效；
如此处理后，每个激光点都具有三个信息：1)分割之后的标签值，用Lki表示；2)在二维深度图像中的行号和列号；3)深度信息rki，之后，对这些点进行特征提取；
进行特征提取的第一步为定义二维深度图像中每个激光点的光滑度c
其中，集合S由点pki以及和其在同一行的10个临近点组成，左右各5个，故|S|＝10，设定阈值cth，c>cth的点为边缘特征点，c<cth的点为平面特征点，为了在所有方向上都提取特征，将整幅二维深度图像在水平方向上分割成若干个子图，对于每个子图，在每一行上对每个点进行光滑度计算并排序，接着，从每一行的非地面点中，选取c值最大的个边缘特征点，组成边缘特征点集合从每一行的地面点或带有有效标签的激光点中，选取c值最小的个平面特征点，组成平面特征点集合为方便后续特征匹配，从集合中，选择个c值最大的边缘特征点组成一个更小的边缘特征点集合Fe；从集合中，选择个c值最小且属于地面点的平面特征点组成一个更小的平面特征点集合Fp，显然有如下关系：和
之后利用提取到的特征估计两个连续扫描帧之间的位姿变化，设定当前时刻为k时刻，则当前时刻较小的边缘特征点集合和平面特征点集合分别为和上一时刻中较大的边缘特征点集合和平面特征点集合分别为和令tk-1表示第k-1次扫描的开始时间，在每次扫描结束时，在扫描过程中接收到的点云Pk-1将重新投影到时间戳tk，重新投影得到的点云图为在第k次扫描运动期间，和Pk一起用于估计雷达的运动，因为故将和均投影到tk时刻，分别得到和接下来，需要从和中找到和的关联点；
对于集合中的一个边缘特征点i，在中寻找与i的标签相同，且距离其最近的边缘特征点j，由于边缘特征点的对应特征是一条直线，且两点确定一条直线，故在中寻找与j的标签相同，且距离其最近的边缘特征点l，由此构建点到直线的几何约束关系，将点i，j和l的坐标分别记为和由此把姿态变换转化为点到直线的距离计算
边缘特征点的优化方程得以构建；
对于集合中的一个平面特征点i，在中寻找与i的标签相同，且距离其最近的平面特征点j，由于平面特征点的对应特征是一个平面，且三点确定一个平面，故在中寻找与j的标签相同，且距离其最近的两个平面特征点l和m，由此构建点到平面的几何约束关系，将点i，j，l和m的坐标分别记为和由此把姿态变换转化为点到平面的距离计算
平面特征点的优化方程得以建立，以上寻找最近点的算法使用k-d树(K-DimensionalTree,K-D Tree)的最近邻搜索法，下面进行运动估计：
在一次扫描过程中，雷达的角速度和线速度都是均匀的，令t为当前时刻，tk为第k次扫描开始的时间，令Tk为[tk,t]时刻之间的雷达位姿变换，Tk包括了雷达在六自由度空间里的刚体运动，Tk＝[tx,ty,tz,θx,θy,θz]T，对于激光点pki，pki∈Pk，令ti为其时间戳，T(k,i)为[tk,ti]时刻之间的雷达位姿变换，则Y(k,i)通过线性插值得到：
为解决雷达的运动估计问题，需要分别建立和与和之间的几何关系，根据式(8)可得
其中V(k,i)为或中得一个激光点i的坐标，为与其相对应的点坐标，T(k,i)(1:3)为T(k,i)的第一项到第三项，即(tx,ty,tz)，R表示由罗德里格斯公式定义的旋转矩阵
其中，θ是旋转角：
θ＝||T(k,i)(4:6)|| (11)
表示旋转方向的单位向量：
表示由向量张成的反对称阵，即对于向量有：
根据式(6)和式(8)-(12)可以得到中的激光点和其对应点之间的几何关系：
同理可得中的激光点和其对应点之间的几何关系：
以式(14)为例，进行列文伯格-马夸尔特L-M(Levenberg-Marquardt)优化，fe中的每一行代表一个边缘特征点，he包含了边缘特征点与对应边缘线之间的距离，计算相对于Tk的fe的雅可比矩阵，记为J，然后，L-M方法利用求解优化问题式(14)，得到Tk的迭代式：
Tk←Tk-(JTJ+λdiag(JTJ))-1JThe (16)
其中λ是L-M方法中的系数因子，由此得到位姿估计Tk＝[tx,ty,tz,θx,θy,θz]T，基于其中的[tz,θx,θy]和式(15)进行第二次L-M优化，得到Tk＝[tx,ty,tz,θx,θy,θz]T选择其中的[tx,ty,θz]和第一次优化得到的[tz,θx,θy]组成最终的位姿估计；
进行两次L-M优化的前提条件是，激光雷达在运动过程中地面变化不大，如果地面变化剧烈，或者无人机在高空飞行，扫描到的地面点少于7200时，平面特征则从非地面点中得到，且在进行位姿估计时，不采用两次L-M优化，而是将式(14)-(15)联合起来得到：
f(Tk)＝h (17)
f中的每一行代表一个平面特征点或边缘特征点，h则包含了其对应的平面特征或边缘特征之间的距离，记f相对于Tk的雅可比矩阵为通过一次L-M优化得到六个自由度的位姿估计，迭代式如下：
Tk←Tk-(JTJ+λdiag(JTJ))-1JTh (18)
接下来进行位姿图优化设计，定义k时刻之前所有扫描到的点云在惯性坐标系下的投影为Qk-1，那么Qk-1就是k时刻时已经建立完成的三维点云地图，后端优化的目的就在于将k时刻获得的特征点匹配到点云地图Qk-1中，由此得到一组位姿估计结果；
每一时刻点云地图的存储方式为存储每次扫描得到的特征点集令 为存储k时刻之前的所有特征点集的集合，Qk-1中的每个集合都与雷达扫描时的位姿相关联，因为经过位姿图优化的传感器位姿估计漂移较低，假设在短时间内没有漂移，令s定义为的大小，以中的每一项为位姿图中的一个节点，为该节点上的观测数据，作为对该节点的约束，将作为新节点加入位姿图中，由此得到位姿图优化的目标函数
其中V表示位姿图中所有位姿节点的位姿估计，∈为位姿图中所有边的集合，eij表示边(i,j)代表的位姿估计的误差，∑ij表示位姿估计的协方差矩阵，至此，优化问题变成了一个最小二乘问题，利用L-M优化求解，得到更为精确的位姿变换估计；
对于新的特征点集使用迭代最近点(Iterative Closest Point,ICP)法将其与之前时刻的特征点集进行匹配，若匹配成功，则表示检测到了回环，由此得到新的位姿估计约束，并将其加入到位姿图中，再使用iSAM2方法进行优化计算，更新位姿估计；如果匹配失败，则表示无人机当前位置并非已经过位置，不会加入新的约束；
地图构建模块根据位姿图优化以2赫兹发布的位姿估计，将每一帧局部点云图投影到惯性坐标系下，由此得到三维点云地图，且点云地图的发布频率也为2赫兹；局部点云地图的原点为当前时刻的传感器位置，惯性坐标系的原点则为算法开始时传感器的位置；变换整合模块将雷达里程计发布的10赫兹位姿估计和位姿图优化得到的2赫兹位姿估计进行插值，发布频率为10赫兹的最终的位姿估计；由此，实现了同时定位与建图；
最后利用CloudCompare软件中集成的qPCV算法对三维点云地图进行处理，得到重构过后的三维点云地图。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111114995.0A CN113985429B (zh) | 2021-09-23 | 2021-09-23 | 基于三维激光雷达的无人机环境扫描与重构方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111114995.0A CN113985429B (zh) | 2021-09-23 | 2021-09-23 | 基于三维激光雷达的无人机环境扫描与重构方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113985429A CN113985429A (zh) | 2022-01-28 |
| CN113985429B true CN113985429B (zh) | 2024-07-26 |
Family
ID=79736368
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111114995.0A Active CN113985429B (zh) | 2021-09-23 | 2021-09-23 | 基于三维激光雷达的无人机环境扫描与重构方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113985429B (zh) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114563795B (zh) * | 2022-02-25 | 2023-01-17 | 湖南大学无锡智能控制研究院 | 基于激光里程计和标签融合算法的定位追踪方法及系统 |
| CN115435784B (zh) * | 2022-08-31 | 2024-06-14 | 中国科学技术大学 | 高空作业平台激光雷达与惯导融合定位建图装置及方法 |
| CN115166686B (zh) * | 2022-09-06 | 2022-11-11 | 天津大学 | 卫星拒止环境下多无人机分布式协同定位与建图方法 |
| CN115328163B (zh) * | 2022-09-16 | 2023-03-28 | 西南交通大学 | 一种巡检机器人雷达里程计的速度与精度优化方法 |
| CN115586511B (zh) * | 2022-11-25 | 2023-03-03 | 唐山百川工业服务有限公司 | 一种基于阵列立柱的激光雷达二维定位方法 |
| CN116954265B (zh) * | 2023-09-20 | 2023-12-05 | 天津云圣智能科技有限责任公司 | 局部运动轨迹的重规划方法、装置和电子设备 |
| CN117863190B (zh) * | 2024-03-08 | 2024-07-19 | 广州小鹏汽车科技有限公司 | 足式机器人的移动控制方法及足式机器人 |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109945856A (zh) * | 2019-02-18 | 2019-06-28 | 天津大学 | 基于惯性/雷达的无人机自主定位与建图方法 |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103268609B (zh) * | 2013-05-17 | 2016-04-20 | 清华大学 | 一种有序提取地面的点云分割方法 |
| CN109297510B (zh) * | 2018-09-27 | 2021-01-01 | 百度在线网络技术（北京）有限公司 | 相对位姿标定方法、装置、设备及介质 |
| CN109961440B (zh) * | 2019-03-11 | 2021-06-18 | 重庆邮电大学 | 一种基于深度图的三维激光雷达点云目标分割方法 |
| CN110223379A (zh) * | 2019-06-10 | 2019-09-10 | 于兴虎 | 基于激光雷达的三维点云重建方法 |
| CN113050993A (zh) * | 2019-12-27 | 2021-06-29 | 中兴通讯股份有限公司 | 基于激光雷达的检测方法、装置及计算机可读存储介质 |
| CN111929699B (zh) * | 2020-07-21 | 2023-05-09 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统 |
| CN112767490B (zh) * | 2021-01-29 | 2022-06-21 | 福州大学 | 一种基于激光雷达的室外三维同步定位与建图方法 |
| CN113066105B (zh) * | 2021-04-02 | 2022-10-21 | 北京理工大学 | 激光雷达和惯性测量单元融合的定位与建图方法及系统 |
-
2021
- 2021-09-23 CN CN202111114995.0A patent/CN113985429B/zh active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109945856A (zh) * | 2019-02-18 | 2019-06-28 | 天津大学 | 基于惯性/雷达的无人机自主定位与建图方法 |
Non-Patent Citations (1)
| Title |
|---|
| LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain;Tixiao Shan et al.;《2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)》;20190106;4758-4765 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113985429A (zh) | 2022-01-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113985429B (zh) | 基于三维激光雷达的无人机环境扫描与重构方法 | |
| CN109709801B (zh) | 一种基于激光雷达的室内无人机定位系统及方法 | |
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN103411609B (zh) | 一种基于在线构图的飞行器返航路线规划方法 | |
| CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN110361027A (zh) | 基于单线激光雷达与双目相机数据融合的机器人路径规划方法 | |
| CN112634451A (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
| CN115407357B (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| WO2021021862A1 (en) | Mapping and localization system for autonomous vehicles | |
| Qian et al. | Robust visual-lidar simultaneous localization and mapping system for UAV | |
| CN112833892B (zh) | 一种基于轨迹对齐的语义建图方法 | |
| CN113447949B (zh) | 一种基于激光雷达和先验地图的实时定位系统及方法 | |
| Niewola et al. | PSD–probabilistic algorithm for mobile robot 6D localization without natural and artificial landmarks based on 2.5 D map and a new type of laser scanner in GPS-denied scenarios | |
| Zhang et al. | Online ground multitarget geolocation based on 3-D map construction using a UAV platform | |
| CN113838129B (zh) | 一种获得位姿信息的方法、装置以及系统 | |
| CN113554705B (zh) | 一种变化场景下的激光雷达鲁棒定位方法 | |
| Majdik et al. | Micro air vehicle localization and position tracking from textured 3d cadastral models | |
| CN114066972A (zh) | 一种基于单目视觉的无人机自主定位方法 | |
| CN113379915A (zh) | 一种基于点云融合的行车场景构建方法 | |
| Alliez et al. | Indoor localization and mapping: Towards tracking resilience through a multi-slam approach | |
| Pan et al. | SLAM-based Forest Plot Mapping by Integrating IMU and Self-calibrated Dual 3D Laser Scanners | |
| CN113403942B (zh) | 一种基于标签辅助的桥梁检测无人机视觉导航方法 | |
| Wang et al. | Raillomer: Rail vehicle localization and mapping with LiDAR-IMU-odometer-GNSS data fusion |
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 |