CN115100264B - 一种基于三维点云的地形和目标实时检测及定位方法 - Google Patents
一种基于三维点云的地形和目标实时检测及定位方法 Download PDFInfo
- Publication number
- CN115100264B CN115100264B CN202210518497.0A CN202210518497A CN115100264B CN 115100264 B CN115100264 B CN 115100264B CN 202210518497 A CN202210518497 A CN 202210518497A CN 115100264 B CN115100264 B CN 115100264B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- module
- dimensional
- point
- terrain
- 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 31
- 238000011897 real-time detection Methods 0.000 title claims abstract description 26
- 238000003384 imaging method Methods 0.000 claims abstract description 45
- 238000001914 filtration Methods 0.000 claims abstract description 32
- 238000005259 measurement Methods 0.000 claims abstract description 21
- 238000000605 extraction Methods 0.000 claims abstract description 19
- 230000011218 segmentation Effects 0.000 claims abstract description 18
- 238000006243 chemical reaction Methods 0.000 claims abstract description 16
- 230000002159 abnormal effect Effects 0.000 claims abstract description 10
- 238000001514 detection method Methods 0.000 claims abstract description 10
- 238000012876 topography Methods 0.000 claims abstract description 8
- 239000000284 extract Substances 0.000 claims abstract description 5
- 238000009499 grossing Methods 0.000 claims abstract description 4
- 230000006870 function Effects 0.000 claims description 33
- 239000013598 vector Substances 0.000 claims description 6
- 230000009194 climbing Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 abstract 1
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 2
- 238000013135 deep learning Methods 0.000 description 2
- 238000003702 image correction Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000013528 artificial neural network Methods 0.000 description 1
- 238000002372 labelling Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
-
- 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G06T3/00—Geometric image transformation in the plane of the image
- G06T3/40—Scaling the whole image or part thereof
- G06T3/4007—Interpolation-based scaling, e.g. bilinear interpolation
-
- G06T5/70—
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/55—Depth or shape recovery from multiple images
-
- 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/30181—Earth observation
Abstract
本发明公开了一种基于三维点云的地形和目标实时检测及定位方法，三维成像传感器模块采集环境三维点云信息，姿态测量模块测量空间姿态角，平滑填补模块填补点云缺失信息，体素栅格滤波模块降采样点云，统计滤波模块滤除异常点，坐标转换模块转换点云到地理坐标系下，聚类分割模块提取候选地形和目标的子点云，平面提取模块提取平面，参数确定模块确定平面参数，类型判别模块判别地形和目标的类别，定位确定模块确定地形和目标的形状、位置、距离和方向的信息并提供给载体导航使用。本发明无需提前采集、标注和训练大量地形和目标样本，具有检测类型多、信息丰富、实时性强、准确率高、成本低和适应性强的优点，应用前景十分广阔。
Description
技术领域
本发明涉及一种对地形和目标检测及定位方法,特别是一种基于三维点云的地形和目标实时检测及定位方法。
背景技术
盲人行走、地面机器人移动或无人机贴地飞行时，需要感知设备实时探测载体前方场景的地形和目标，获得地形和目标，包括：坡道、楼梯、台阶、墙、栏杆和平地的类型、形状、位置、距离和方向信息，及时调整载体行走、移动或飞行的导航策略，避免碰撞事故。目前基于三维点云的目标检测方法一般都使用深度学习神经网络，因此需要提前采集和标注大量目标样本并训练识别模型，存在采集和标注的工作量极大和成本高的问题。还有基于三维点云匹配的目标检测方法则需要提前获取目标完整的三维点云模型，存在适应性差和准确率低的问题。基于摄像机二维图像的地形和目标检测方法，则存在缺乏三维位置信息而无法实现三维空间定位、检测目标类型单一和光环境适应性差的问题。
发明内容
本发明目的在于提供一种基于三维点云的地形和目标实时检测及定位方法，解决当前方法存在深度学习需提前采集和标注工作量大和成本高，目标模型匹配适应性差和准确率低，摄像机二维图像缺乏三维位置信息、检测目标类型单一和光环境适应性差的问题。
一种基于三维点云的地形和目标实时检测及定位方法的具体步骤为：
第一步构建地形和目标实时检测和定位系统
地形和目标实时检测和定位系统，包括：三维成像传感器模块、姿态测量模块、平滑填补模块、体素栅格滤波模块、统计滤波模块、坐标转换模块、聚类分割模块、平面提取模块、参数确定模块、类型判别模块和定位确定模块。
三维成像传感器模块的功能为：采集环境三维点云信息。
姿态测量模块的功能为：测量三维成像传感器模块的空间姿态角。
平滑填补模块的功能为：填补三维点云缺失信息。
体素栅格滤波模块的功能为：对三维点云降采样。
统计滤波模块的功能为：对三维点云进行统计滤波。
坐标转换模块的功能为：转换三维点云到东北天地理坐标系下。
聚类分割模块的功能为：提取属于同一地形和目标的点数目最多的子点云。
平面提取模块的功能为：从子点云中提取三维空间平面。
参数确定模块的功能为：确定提取平面的相关参数。
类型判别模块的功能为：根据平面参数判别地形和目标的具体类别。
定位确定模块的功能为：确定地形和目标的形状、位置、距离和方向的信息并提供给载体导航使用。
第二步三维成像传感器模块采集三维点云
三维成像传感器模块采集环境的原始的三维点云数据，姿态测量模块同步采集三维成像传感器的三维姿态角。三维成像传感器模块选择双目立体相机、RGBD深度相机或三维激光雷达。
利用双目立体相机，通过双摄像头同时采集左右两幅图像，经图像校正和立体匹配转换成视差图，根据相机内外参数生成传感器坐标系下的三维点云。
或利用RGBD深度相机，同时采集RGB彩色图和深度图，根据相机参数生成传感器坐标系下的三维点云。
或利用三维激光雷达，通过激光测距扫描直接获得传感器坐标系下的三维点云。
第三步姿态测量模块采集姿态角
姿态测量模块采集自身三维姿态角，再根据三维成像传感器模块与姿态测量模块安装关系，确定三维成像传感器模块的三维姿态角，即航向角ψ、俯仰角θ和横滚角φ。
第四步平滑填补模块填补点云空洞
平滑填补模块填补三维点云的空洞并对点云进行滤波去噪。首先利用空洞周围的有效视差或点云数据进行插值填补，再通过中值滤波器进行平滑去噪。
对于三维成像传感器的每个缺失视差/深度的像素点Pi，在视差/深度图的同一行向左右两个方向同时搜索第一个未缺失的正常像素，根据左右两个正常像素与Pi的距离通过加权得到其视差/深度，缺失视差/深度di的插值填补用公式(1)表示：
di＝αdL+(1-α)dR，其中
公式(1)中，|LiPi|和|RiPi|分别表示左右两个正常像素与Pi的像素距离，dL和dR分别表示左右两个正常像素的视差/深度。
第五步体素栅格滤波模块降采样点云
体素栅格滤波模块对点云进行降采样，保留点云特征同时，降低点云密度和点的数量，加快运算速度。首先确定点云各点到成像传感器坐标系原点的距离，剔除距离大于给定阈值DIST的点；再利用体素栅格滤波器对点云进行降采样，将三维空间分割成很多体素栅格，对于点云中落入同一栅格的所有点{qi}，确定质心pc，用pc替代该栅格的所有点。
第六步 统计滤波模块去除点云异常点
统计滤波模块首先统计点云各点与其周围邻居点的距离，确定平均距离，再滤波去除平均距离大于预定阈值的异常点。假定点云各点与其周围最近N个邻居点的平均距离dmean符合高斯分布，确定每个点的dmean，对于dmean大于阈值dmean-th的点，作为统计异常点剔除。点p的与其N个邻居点{qi}的平均距离dmean用公式(2)表示：
公式(2)中，|pqi|表示点p与第i个邻居点qi的三维空间距离。
dmean-th用公式(3)表示：
公式(3)中，和σ分别为全局统计的距离均值和标准差，λ根据滤波效果需求在一定范围内设置。
第七步 坐标转换模块转换点云至地理坐标系
坐标转换模块将点云从三维成像传感器坐标系转换到东北天地理坐标系下，使其具备真实空间方位信息。根据三维姿态角，即航向角ψ、俯仰角θ和横滚角φ，及成像传感器距离地面高度H，将点云转换至东北天地理坐标系下，转换后的点云记为C1。坐标转换用公式(4)表示：
公式(4)中，[XG YG ZG]T是空间点在东北天地理坐标系下的坐标，[XS YS ZS]T是其在传感器坐标系下的坐标。
第八步 聚类分割模块确定目标子点云
聚类分割模块分割属于同一地形和目标的点数量最多的子点云，利用欧式聚类方式分割点云，分割后点数目最多的子点云记为E，E的空间点属于同一地形或目标。创建kd-tree搜索树表示C1，设置一个点云簇空列表U和点队列V。对于C1中的每个点pi，先将其添加到当前队列V；对于V中的每个点，在以pi为球心，球半径为r的球形空间内，搜索得到其全部邻居点集合Pi k；对于Pi k中的每个点，检查其是否已被处理过，当没有时，则将其添加到V中并标记被处理过；当V中的每个点都已经被处理过时，将V添加到点云簇列表U中，然后清空队列V；最后当C1中的所有点均被处理过，聚类分割过程结束，输出点云簇列表U，U中点数目最多的点云即为E。
第九步 平面提取模块确定目标平面
平面提取模块提取三维空间平面，反复选择点云中的一组随机点云子集来提取最优平面。被选取的点云子集被假设为属于同一个三维空间平面的局内点，首先随机假设点云的一小组局内点为初始值，用此局内点拟合一个平面模型，假设的局内点位于此平面内，平面方程Ax+By+Cz+D＝0的A、B、C和D四个平面模型参数从假设的局内点确定。用得到的模型去测试点云的所有点，当某个点适用于估计的模型时，则认为它也是局内点，用所有假设的局内点去重新估计平面模型，最后通过估计局内点与平面模型的错误率来评估模型。通过迭代最后得到最佳拟合平面P的参数A、B、C和D，向量(A,B,C)为平面P的法线向量，E中参与平面P拟合的空间点集合记为点云F。在平面提取过程中，提取合理结果的概率p在[0.95,0.99]范围内设置，最大迭代次数为NUM，根据速度需求在预定范围内设置。
第十步 参数确定模块确定平面参数特征
参数确定模块确定下面8个平面相关参数：
确定平面P与水平地面的夹角θ，用公式(5)表示：
公式(5)中，A、B和C为平面P的部分模型参数。
确定点云F的质心Q：确定F中所有点的坐标均值。
确定质心Q与东北天地理坐标系原点O的距离dQ，用公式(6)表示：
确定判断点M的位置：确定过质心Q(xQ，yQ，zQ)的平面P的法线上与Q距离为H的两个三维点M1和M2，选取M1和M2中坐标z值较大者，作为判断点M(xM，yM，zM),其中H根据三维成像传感器测量精度设置。利用公式(7)确定M1和M2：
确定判断点M与东北天地理坐标系原点O的距离dM，用公式(8)表示：
确定点云F中各点与地面的平均距离L，用公式(9)表示：
确定点云F中各点的z坐标均值用公式(10)表示：
确定点云F中离地最高的点的z坐标zmax，用公式(11)表示：
zmax＝max(zi) (11)
第十一步 类型判别模块确定地形和目标的类型
类型判别模块判定地形和目标的具体类型：
平地类型判别条件：θ<10°，且L<5cm。
上台阶类型判别条件：θ<10°，且L>5cm，且
下台阶类型判别条件：θ<10°，且L>5cm，且
上坡或上楼梯类型判别条件：10°≤θ≤40°，且dM<dQ，且且L>0.2m。
下坡或下楼梯类型判别条件：10°≤θ≤45°，且dM>dQ，且且L>0.2m。
栏杆类型判别条件：75°≤θ≤90°，且且zmax<1.2m。
墙类型判别条件：75°≤θ≤90°，且且zmax≥1.2m。
不满足以上条件的其他情况，则判定为地形和目标检测失败。
第十二步 定位确定模块确定地形和目标的定位信息
定位确定模块确定地形和目标的形状、位置、距离和方向的信息，提供给载体导航使用。
确定形状：确定点云F的最小包围盒，将包围盒的顶点作为地形和目标的形状信息输出给载体。
确定位置：确定F中各点与坐标系G原点O的距离，选择距离最近的点Pnear，将质心Q和最近点Pnear作为地形和目标的位置信息输出给载体。
确定距离：将距离|OQ|和|OPnear|作为地形和目标的距离信息输出给载体。
确定方向：将射线OQ在水平地面的投影与东北天地理坐标系OX正轴的夹角α，作为地形和目标的方向信息输出给载体。
至此，实现基于三维点云的地形和目标实时检测及定位。
本发明相比以往地形和目标检测方法的优势主要在于：无需提前采集和标注大量地形和目标样本，能够为载体的三维空间导航有效提供地形和目标信息，具有成本低、适应性强、准确率高、信息丰富和实时性强的优点，工程应用价值很高。
具体实施方式
一种基于三维点云的地形和目标实时检测及定位方法的具体步骤为：
第一步 构建地形和目标实时检测和定位系统
地形和目标实时检测和定位系统，包括：三维成像传感器模块、姿态测量模块、平滑填补模块、体素栅格滤波模块、统计滤波模块、坐标转换模块、聚类分割模块、平面提取模块、参数确定模块、类型判别模块和定位确定模块。
三维成像传感器模块的功能为：采集环境三维点云信息。
姿态测量模块的功能为：测量三维成像传感器模块的空间姿态角。
平滑填补模块的功能为：填补三维点云缺失信息。
体素栅格滤波模块的功能为：对三维点云降采样。
统计滤波模块的功能为：对三维点云进行统计滤波。
坐标转换模块的功能为：转换三维点云到东北天地理坐标系下。
聚类分割模块的功能为：提取属于同一地形和目标的点数目最多的子点云。
平面提取模块的功能为：从子点云中提取三维空间平面。
参数确定模块的功能为：确定提取平面的相关参数。
类型判别模块的功能为：根据平面参数判别地形和目标的具体类别。
定位确定模块的功能为：确定地形和目标的形状、位置、距离和方向的信息并提供给载体导航使用。
第二步 三维成像传感器模块采集三维点云
三维成像传感器模块采集环境的原始的三维点云数据，姿态测量模块同步采集三维成像传感器的三维姿态角。三维成像传感器模块选择双目立体相机、RGBD深度相机或三维激光雷达。
利用双目立体相机，通过双摄像头同时采集左右两幅图像，经图像校正和立体匹配转换成视差图，根据相机内外参数生成传感器坐标系下的三维点云。
或利用RGBD深度相机，同时采集RGB彩色图和深度图，根据相机参数生成传感器坐标系下的三维点云。
或利用三维激光雷达，通过激光测距扫描直接获得传感器坐标系下的三维点云。
第三步 姿态测量模块采集姿态角
姿态测量模块采集自身三维姿态角，再根据三维成像传感器模块与姿态测量模块安装关系，确定三维成像传感器模块的三维姿态角，即航向角ψ、俯仰角θ和横滚角φ。
第四步 平滑填补模块填补点云空洞
平滑填补模块填补三维点云的空洞并对点云进行滤波去噪。首先利用空洞周围的有效视差或点云数据进行插值填补，再通过中值滤波器进行平滑去噪。
对于三维成像传感器的每个缺失视差/深度的像素点Pi，在视差/深度图的同一行向左右两个方向同时搜索第一个未缺失的正常像素，根据左右两个正常像素与Pi的距离通过加权得到其视差/深度，缺失视差/深度di的插值填补用公式(1)表示：
di＝αdL+(l-α)dR，其中
公式(1)中，|LiPi|和|RiPi|分别表示左右两个正常像素与Pi的像素距离，dL和dR分别表示左右两个正常像素的视差/深度。
第五步 体素栅格滤波模块降采样点云
体素栅格滤波模块对点云进行降采样，保留点云特征同时，降低点云密度和点的数量，加快运算速度。首先确定点云各点到成像传感器坐标系原点的距离，剔除距离大于给定阈值DIST的点；再利用体素栅格滤波器对点云进行降采样，将三维空间分割成很多体素栅格，对于点云中落入同一栅格的所有点{qi}，确定质心pc，用pc替代该栅格的所有点。
第六步 统计滤波模块去除点云异常点
统计滤波模块首先统计点云各点与其周围邻居点的距离，确定平均距离，再滤波去除平均距离大于预定阈值的异常点。假定点云各点与其周围最近N个邻居点的平均距离dmean符合高斯分布，确定每个点的dmean，对于dmean大于阈值dmean-th的点，作为统计异常点剔除。点p的与其N个邻居点{qi}的平均距离dmean用公式(2)表示：
公式(2)中，|pqi|表示点p与第i个邻居点qi的三维空间距离。
dmean-th用公式(3)表示：
公式(3)中，和σ分别为全局统计的距离均值和标准差，λ根据滤波效果需求在一定范围内设置。
第七步 坐标转换模块转换点云至地理坐标系
坐标转换模块将点云从三维成像传感器坐标系转换到东北天地理坐标系下，使其具备真实空间方位信息。根据三维姿态角，即航向角ψ、俯仰角θ和横滚角φ，及成像传感器距离地面高度H，将点云转换至东北天地理坐标系下，转换后的点云记为C1。坐标转换用公式(4)表示：
公式(4)中，[XG YG ZG]T是空间点在东北天地理坐标系下的坐标，[XS YS ZS]T是其在传感器坐标系下的坐标。
第八步 聚类分割模块确定目标子点云
聚类分割模块分割属于同一地形和目标的点数量最多的子点云，利用欧式聚类方式分割点云，分割后点数目最多的子点云记为E，E的空间点属于同一地形或目标。创建kd-tree搜索树表示C1，设置一个点云簇空列表U和点队列V。对于C1中的每个点pi，先将其添加到当前队列V；对于V中的每个点，在以pi为球心，球半径为r的球形空间内，搜索得到其全部邻居点集合Pi k；对于Pi k中的每个点，检查其是否已被处理过，当没有时，则将其添加到V中并标记被处理过；当V中的每个点都已经被处理过时，将V添加到点云簇列表U中，然后清空队列V；最后当C1中的所有点均被处理过，聚类分割过程结束，输出点云簇列表U，U中点数目最多的点云即为E。
第九步 平面提取模块确定目标平面
平面提取模块提取三维空间平面，反复选择点云中的一组随机点云子集来提取最优平面。被选取的点云子集被假设为属于同一个三维空间平面的局内点，首先随机假设点云的一小组局内点为初始值，用此局内点拟合一个平面模型，假设的局内点位于此平面内，平面方程Ax+By+Cz+D＝0的A、B、C和D四个平面模型参数从假设的局内点确定。用得到的模型去测试点云的所有点，当某个点适用于估计的模型时，则认为它也是局内点，用所有假设的局内点去重新估计平面模型，最后通过估计局内点与平面模型的错误率来评估模型。通过迭代最后得到最佳拟合平面P的参数A、B、C和D，向量(A,B,C)为平面P的法线向量，E中参与平面P拟合的空间点集合记为点云F。在平面提取过程中，提取合理结果的概率p在[0.95,0.99]范围内设置，最大迭代次数为NUM，根据速度需求在预定范围内设置。
第十步 参数确定模块确定平面参数特征
参数确定模块确定下面8个平面相关参数：
确定平面P与水平地面的夹角θ，用公式(5)表示：
公式(5)中，A、B和C为平面P的部分模型参数。
确定点云F的质心Q：确定F中所有点的坐标均值。
确定质心Q与东北天地理坐标系原点O的距离dQ，用公式(6)表示：
确定判断点M的位置：确定过质心Q(xQ，yQ，zQ)的平面P的法线上与Q距离为H的两个三维点M1和M2，选取M1和M2中坐标z值较大者，作为判断点M(xM，yM，zM),其中H根据三维成像传感器测量精度设置。利用公式(7)确定M1和M2：
确定判断点M与东北天地理坐标系原点O的距离dM，用公式(8)表示：
确定点云F中各点与地面的平均距离L，用公式(9)表示：
确定点云F中各点的z坐标均值用公式(10)表示：
确定点云F中离地最高的点的z坐标zmax，用公式(11)表示：
zmax＝max(zi) (11)
第十一步类型判别模块确定地形和目标的类型
类型判别模块判定地形和目标的具体类型：
平地类型判别条件：θ<10°，且L<5cm。
上台阶类型判别条件：θ<10°，且L>5cm，且
下台阶类型判别条件：θ<10°，且L>5cm，且
上坡或上楼梯类型判别条件：10°≤θ≤40°，且dM<dQ，且且L>0.2m。
下坡或下楼梯类型判别条件：10°≤θ≤45°，且dM>dQ，且且L>0.2m。
栏杆类型判别条件：75°≤θ≤90°，且且zmax<1.2m。
墙类型判别条件：75°≤θ≤90°，且且zmax≥1.2m。
不满足以上条件的其他情况，则判定为地形和目标检测失败。
第十二步 定位确定模块确定地形和目标的定位信息
定位确定模块确定地形和目标的形状、位置、距离和方向的信息，提供给载体导航使用。
确定形状：确定点云F的最小包围盒，将包围盒的顶点作为地形和目标的形状信息输出给载体。
确定位置：确定F中各点与坐标系G原点O的距离，选择距离最近的点Pnear，将质心Q和最近点Pnear作为地形和目标的位置信息输出给载体。
确定距离：将距离|OQ|和|OPnear|作为地形和目标的距离信息输出给载体。
确定方向：将射线OQ在水平地面的投影与东北天地理坐标系OX正轴的夹角α，作为地形和目标的方向信息输出给载体。
至此，实现基于三维点云的地形和目标实时检测及定位。
Claims (12)
1.一种基于三维点云的地形和目标实时检测及定位方法，其特征在于具体步骤为：
第一步 构建地形和目标实时检测和定位系统
地形和目标实时检测和定位系统，包括：三维成像传感器模块、姿态测量模块、平滑填补模块、体素栅格滤波模块、统计滤波模块、坐标转换模块、聚类分割模块、平面提取模块、参数确定模块、类型判别模块和定位确定模块；
第二步 三维成像传感器模块采集三维点云三维成像传感器模块采集环境的原始的三维点云数据；三维成像传感器模块选择双目立体相机、RGBD深度相机或三维激光雷达；
利用双目立体相机，通过双摄像头同时采集左右两幅图像，经图像校正和立体匹配转换成视差图，根据相机内外参数生成传感器坐标系下的三维点云；
或利用RGBD深度相机，同时采集RGB彩色图和深度图，根据相机参数生成传感器坐标系下的三维点云；
或利用三维激光雷达，通过激光测距扫描直接获得传感器坐标系下的三维点云；
第三步 姿态测量模块采集姿态角
姿态测量模块采集自身三维姿态角，再根据三维成像传感器模块与姿态测量模块安装关系，确定三维成像传感器模块的三维姿态角，即航向角ψ、俯仰角θ和横滚角φ；
第四步 平滑填补模块填补点云空洞
平滑填补模块填补三维点云的空洞并对点云进行滤波去噪；首先利用空洞周围的有效视差或点云数据进行插值填补，再通过中值滤波器进行平滑去噪；
对于三维成像传感器的每个缺失视差/深度的像素点Pi，在视差/深度图的同一行向左右两个方向同时搜索第一个未缺失的正常像素，根据左右两个正常像素与Pi的距离通过加权得到其视差/深度，缺失视差/深度di的插值填补用公式(1)表示：
di＝αdL+(1-α)dR，其中
公式(1)中，|LiPi|和|RiPi|分别表示左右两个正常像素与Pi的像素距离，dL和dR分别表示左右两个正常像素的视差/深度；
第五步 体素栅格滤波模块降采样点云
体素栅格滤波模块对点云进行降采样，保留点云特征同时，降低点云密度和点的数量，加快运算速度；首先确定点云各点到成像传感器坐标系原点的距离，剔除距离大于给定阈值DIST的点；再利用体素栅格滤波器对点云进行降采样，将三维空间分割成很多体素栅格，对于点云中落入同一栅格的所有点{qi}，确定质心pc，用pc替代该栅格的所有点；
第六步 统计滤波模块去除点云异常点
统计滤波模块首先统计点云各点与其周围邻居点的距离，确定平均距离，再滤波去除平均距离大于预定阈值的异常点；假定点云各点与其周围最近N个邻居点的平均距离dmean符合高斯分布，确定每个点的dmean，对于dmean大于阈值dmean-th的点，作为统计异常点剔除；点p的与其N个邻居点{qi}的平均距离dmean用公式(2)表示：
公式(2)中，|pqi|表示点p与第i个邻居点qi的三维空间距离；
dmean-th用公式(3)表示：
公式(3)中，和σ分别为全局统计的距离均值和标准差，λ根据滤波效果需求在一定范围内设置；
第七步 坐标转换模块转换点云至地理坐标系
坐标转换模块将点云从三维成像传感器坐标系转换到东北天地理坐标系下，使其具备真实空间方位信息；根据三维成像传感器的三维空间姿态角和三维成像传感器距离地面高度H，将点云转换至东北天地理坐标系下，转换后的点云记为C1；坐标转换用公式(4)表示：
公式(4)中，[XG YG ZG]T是空间点在东北天地理坐标系下的坐标，[XS YS ZS]T是其在传感器坐标系下的坐标；
第八步 聚类分割模块确定目标子点云
聚类分割模块分割属于同一地形和目标的点数量最多的子点云，利用欧式聚类方式分割点云，分割后点数目最多的子点云记为E，E的空间点属于同一地形或目标；创建kd-tree搜索树表示C1，设置一个点云簇空列表U和点队列V；对于C1中的每个点pi，先将其添加到当前队列V；对于V中的每个点，在以pi为球心，球半径为r的球形空间内，搜索得到其全部邻居点集合Pi k；对于Pi k中的每个点，检查其是否已被处理过，当没有时，则将其添加到V中并标记被处理过；当V中的每个点都已经被处理过时，将V添加到点云簇列表U中，然后清空队列V；最后当C1中的所有点均被处理过，聚类分割过程结束，输出点云簇列表U，U中点数目最多的点云即为E；
第九步 平面提取模块确定目标平面
平面提取模块提取三维空间平面，反复选择点云中的一组随机点云子集来提取最优平面；被选取的点云子集被假设为属于同一个三维空间平面的局内点，首先随机假设点云的一小组局内点为初始值，用此局内点拟合一个平面模型，假设的局内点位于此平面内，平面方程Ax+By+Cz+D＝0的A、B、C和D四个平面模型参数从假设的局内点确定；用得到的模型去测试点云的所有点，当某个点适用于估计的模型时，则认为它也是局内点，用所有假设的局内点去重新估计平面模型，最后通过估计局内点与平面模型的错误率来评估模型；通过迭代最后得到最佳拟合平面P的参数A、B、C和D，向量(A,B,C)为平面P的法线向量，E中参与平面P拟合的空间点集合记为点云F；在平面提取过程中，提取合理结果的概率p在[0.95,0.99]范围内设置，最大迭代次数为NUM，根据速度需求在预定范围内设置；
第十步 参数确定模块确定平面参数特征
参数确定模块确定下面8个平面相关参数：
确定平面P与水平地面的夹角θ，用公式(5)表示：
公式(5)中，A、B和C为平面P的部分模型参数；
确定点云F的质心Q：确定F中所有点的坐标均值；
确定质心Q与东北天地理坐标系原点O的距离dQ，用公式(6)表示：
确定判断点M的位置：确定过质心Q(xQ，yQ，zQ)的平面P的法线上与Q距离为H的两个三维点M1和M2，选取M1和M2中坐标z值较大者，作为判断点M(xM，yM，zM),其中H根据三维成像传感器测量精度设置；利用公式(7)确定M1和M2：
确定判断点M与东北天地理坐标系原点O的距离dM，用公式(8)表示：
确定点云F中各点与地面的平均距离L，用公式(9)表示：
确定点云F中各点的z坐标均值用公式(10)表示：
确定点云F中离地最高的点的z坐标zmax，用公式(11)表示：
zmax＝max(zi) (11)
第十一步类型判别模块确定地形和目标的类型
类型判别模块判定地形和目标的具体类型：
平地类型判别条件：θ<10°，且L<5cm；
上台阶类型判别条件：θ<10°，且L>5cm，且
下台阶类型判别条件：θ<10°，且L>5cm，且
上坡或上楼梯类型判别条件：10°≤θ≤40°，且dM<dQ，且且L>0.2m；
下坡或下楼梯类型判别条件：10°≤θ≤45°，且dM>dQ，且且L>0.2m；
栏杆类型判别条件：75°≤θ≤90°，且且zmax<1.2m；
墙类型判别条件：75°≤θ≤90°，且且zmax≥1.2m；
不满足以上条件的其他情况，则判定为地形和目标检测失败；
第十二步 定位确定模块确定地形和目标的定位信息
定位确定模块确定地形和目标的形状、位置、距离和方向的信息，提供给载体导航使用；
确定形状：确定点云F的最小包围盒，将包围盒的顶点作为地形和目标的形状信息输出给载体；
确定位置：确定F中各点与坐标系G原点O的距离，选择距离最近的点Pnear，将质心Q和最近点Pnear作为地形和目标的位置信息输出给载体；
确定距离：将距离|OQ|和|OPnear|作为地形和目标的距离信息输出给载体；
确定方向：将射线OQ在水平地面的投影与东北天地理坐标系OX正轴的夹角α，作为地形和目标的方向信息输出给载体；
至此，实现基于三维点云的地形和目标实时检测及定位。
2.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述三维成像传感器模块的功能为：采集环境三维点云信息。
3.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述姿态测量模块的功能为：测量三维成像传感器模块的空间姿态角。
4.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述平滑填补模块的功能为：填补三维点云缺失信息。
5.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述体素栅格滤波模块的功能为：对三维点云降采样。
6.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述统计滤波模块的功能为：对三维点云进行统计滤波。
7.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述坐标转换模块的功能为：转换三维点云到东北天地理坐标系下。
8.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述聚类分割模块的功能为：提取属于同一地形和目标的点数目最多的子点云。
9.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述平面提取模块的功能为：从子点云中提取三维空间平面。
10.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述参数确定模块的功能为：确定提取平面的相关参数。
11.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述类型判别模块的功能为：根据平面参数判别地形和目标的具体类别。
12.根据权利要求1所述的一种基于三维点云的地形和目标实时检测及定位方法，其特征在于所述定位确定模块的功能为：确定地形和目标的形状、位置、距离和方向的信息并提供给载体导航使用。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210518497.0A CN115100264B (zh) | 2022-05-12 | 2022-05-12 | 一种基于三维点云的地形和目标实时检测及定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210518497.0A CN115100264B (zh) | 2022-05-12 | 2022-05-12 | 一种基于三维点云的地形和目标实时检测及定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115100264A CN115100264A (zh) | 2022-09-23 |
| CN115100264B true CN115100264B (zh) | 2024-04-02 |
Family
ID=83287951
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210518497.0A Active CN115100264B (zh) | 2022-05-12 | 2022-05-12 | 一种基于三维点云的地形和目标实时检测及定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115100264B (zh) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020135446A1 (zh) * | 2018-12-24 | 2020-07-02 | 深圳市道通智能航空技术有限公司 | 一种目标定位方法和装置、无人机 |
| CN112396650A (zh) * | 2020-03-30 | 2021-02-23 | 青岛慧拓智能机器有限公司 | 一种基于图像和激光雷达融合的目标测距系统及方法 |
| CN113450408A (zh) * | 2021-06-23 | 2021-09-28 | 中国人民解放军63653部队 | 一种基于深度相机的非规则物体位姿估计方法及装置 |
-
2022
- 2022-05-12 CN CN202210518497.0A patent/CN115100264B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020135446A1 (zh) * | 2018-12-24 | 2020-07-02 | 深圳市道通智能航空技术有限公司 | 一种目标定位方法和装置、无人机 |
| CN112396650A (zh) * | 2020-03-30 | 2021-02-23 | 青岛慧拓智能机器有限公司 | 一种基于图像和激光雷达融合的目标测距系统及方法 |
| CN113450408A (zh) * | 2021-06-23 | 2021-09-28 | 中国人民解放军63653部队 | 一种基于深度相机的非规则物体位姿估计方法及装置 |
Non-Patent Citations (1)
| Title |
|---|
| 基于深度投影的三维点云目标分割和碰撞检测;王张飞;刘春阳;隋新;杨芳;马喜强;陈立海;;光学精密工程;20200731(第07期);第1600-1608页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115100264A (zh) | 2022-09-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110781827B (zh) | 一种基于激光雷达与扇状空间分割的路沿检测系统及其方法 | |
| CN106650640B (zh) | 一种基于激光雷达点云局部结构特征的负障碍物检测方法 | |
| CN110221603A (zh) | 一种基于激光雷达多帧点云融合的远距离障碍物检测方法 | |
| CN102435174B (zh) | 基于混合式双目视觉的障碍物检测方法及装置 | |
| CN104091369B (zh) | 一种无人机遥感影像建筑物三维损毁检测方法 | |
| CN101975951B (zh) | 一种融合距离和图像信息的野外环境障碍检测方法 | |
| CN110175576A (zh) | 一种结合激光点云数据的行驶车辆视觉检测方法 | |
| CN107392929B (zh) | 一种基于人眼视觉模型的智能化目标检测及尺寸测量方法 | |
| CN113359782B (zh) | 一种融合lidar点云与图像数据的无人机自主选址降落方法 | |
| CN112800938B (zh) | 无人驾驶车辆检测侧面落石发生的方法及装置 | |
| CN112346463B (zh) | 一种基于速度采样的无人车路径规划方法 | |
| WO2023060632A1 (zh) | 基于点云数据的街景地物多维度提取方法和系统 | |
| Zhang et al. | Filtering photogrammetric point clouds using standard lidar filters towards dtm generation | |
| CN114325634A (zh) | 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法 | |
| CN114170535A (zh) | 目标检测定位方法、装置、控制器及存储介质和无人机 | |
| CN111563957B (zh) | 一种煤田火灾及矸石山火灾三维温度场数字化成像方法 | |
| CN106709432B (zh) | 基于双目立体视觉的人头检测计数方法 | |
| Yao et al. | Automated detection of 3D individual trees along urban road corridors by mobile laser scanning systems | |
| CN115100264B (zh) | 一种基于三维点云的地形和目标实时检测及定位方法 | |
| CN115930946A (zh) | 室内外交变环境下动态障碍物多特征描述的方法 | |
| Abdullah et al. | Automatic segmentation of LiDAR point cloud data at different height levels for 3D building extraction | |
| CN114972358B (zh) | 一种基于人工智能的城市测绘激光点云偏移检测方法 | |
| CN114972541B (zh) | 基于三维激光雷达和双目相机融合的轮胎吊立体防撞方法 | |
| CN111414848B (zh) | 一种全类别3d障碍物检测方法、系统和介质 | |
| CN110110645A (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |