CN103278159B - 机载2d激光测距机获取3d点云的方法 - Google Patents

Abstract

本发明提出一种机载2D激光测距机获取3D点云的方法，包括：通过EKF算法对惯性测量设备所测量的角速度进行处理，以得到横滚角和俯仰角；通过机载激光测距机获取飞行器的高度信息，并根据横滚角和俯仰角以及高度信息得到飞行器的高度坐标；根据横滚角和俯仰角对激光测距机所采集的扫描数据进行处理以得到第一扫描数据和第二扫描数据；根据第一扫描数据的数据进行数据间的匹配以得到飞行器的水平位置和偏航角；以及根据第二扫描数据、高度坐标、横滚角和俯仰角、水平位置和偏航角得到飞行器的世界坐标集，并根据世界坐标集构成飞行器所在环境的3D点云。根据本发明实施例的方法，可大幅提升无人机的自主性能。

Description

机载2D激光测距机获取3D点云的方法

技术领域

本发明涉及飞行器自主飞行技术领域，特别涉及一种机载2D激光测距机获取3D点云的方法。

背景技术

随着微电子技术和机器人技术的发展，使飞行器具备自主飞行的能力成为可能，同时在民用和军用领域对飞行器的自主性能也提出了更高的需求，特别是在勘察，救援，监控等方面的需求也日益增加。其中，对环境的感知能力是飞行器自主飞行的重要基础，如果能获取飞行器所在环境的信息并实时估计自身位置，将会大幅度提升无人机的自主性能。不同于普通的地面机器人，飞行器在飞行过程中具有6自由度，因此需要获取所在环境的3D信息，但是现有技术中大部分的传感器都难以获取有效的3D环境信息，而成为自主飞行器发展的瓶颈。

发明内容

本发明的目的旨在至少解决上述的技术缺陷之一。

为此，本发明的目的在于提出一种机载2D激光测距机获取3D点云的方法。

为达到上述目的，本发明的实施例提出一种机载2D激光测距机获取3D点云的方法，包括以下步骤：通过EKF算法对惯性测量设备所测量的角速度进行处理，以得到横滚角和俯仰角；通过机载激光测距机获取飞行器的高度信息，并根据所述横滚角和俯仰角以及所述高度信息得到飞行器的高度坐标；根据所述横滚角和俯仰角对激光测距机所采集的扫描数据进行处理以得到第一扫描数据和第二扫描数据；根据所述第一扫描数据的数据进行数据间的匹配以得到所述飞行器的水平位置和偏航角；以及根据所述第二扫描数据、所述高度坐标、所述横滚角和俯仰角、所述水平位置和偏航角得到所述飞行器的世界坐标集，并根据所述世界坐标集构成所述飞行器所在环境的3D点云。

本发明的一个实施例中，通过如下方式获得所述飞行器的世界坐标：将所述第二扫描数据转化为测量点机载坐标系下的坐标；根据所述机载坐标系下的坐标、所述高度坐标、所述横滚角和俯仰角、所述水平位置和偏航角得到所机载坐标系与世界坐标系的转换关系；以及根据所述机载坐标系与世界坐标系的转换关系将所述机载坐标系下的坐标转化为所述世界坐标。

本发明的一个实施例中，所述激光测距机的光束与所述激光测距机反射装置的平面镜成45°角。

本发明的一个实施例中，所述第一扫描数据是通过对所述第二扫描数据进行投影处理得到的。

本发明的一个实施例中，所述激光测距机为2D激光测距机。

本发明的一个实施例中，所述机载坐标系转换为世界坐标系的公式如下，所述公式为：其中，x_i ^W为第i个观测点的x轴世界坐标，y_i ^W为第i个观测点的y轴世界坐标，z_i ^W为第i个观测点的z轴世界坐标，θ为当前激光测距机的偏航角，为当前激光测距机的横滚，ψ为当前激光测距机的俯仰角，x_i ^L为第i个观测点的x轴局部坐标，y_i ^L为第i个观测点的y轴局部坐标，x为激光测距机所在世界坐标系x坐标，y为激光测距机所在世界坐标系y坐标，z为激光测距机所在世界坐标系z坐标，(x_i ^W,y_i ^W,z_i ^W)为第i个观测点的世界坐标，(x_i ^L,y_i ^L,0)为第i个观测点的局部坐标，(x,y,z)为激光测距机中心的世界坐标。

本发明的一个实施例中，所述第一扫描数据通过如下公式表示，所述公式为：

其中，为投影后的测量距离，为投影后的测量角度，r_i为投影前的测量距离，α_i为投影前的测量角度，ψ为飞行器俯仰角，为飞行器横滚角。

根据本发明实施例的方法具有如下优点：

1、可获取飞行器所在环境的3D点云数据，以弥补传统激光测距机只能获取2D数据的不足。

2、仅利用飞行器的惯导传感器和2D激光测距机，而不需要其余设备，因此有效地减轻了飞行器的载重负担。

3、通过激光测距机获取水平位置和偏航角，因此与使用传统的惯导传感器相比更具有鲁棒性。

4、通过激光测距的倾斜校正和噪声处理使得激光测距数据的利用更加有效。

5、可以根据需求获取感兴趣区域的3D点云信息，并将激光测距机扫描的数据累积到环境点云中，因此避免了不必要的测量以及由此带来的资源消耗。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本发明一个实施例的机载2D激光测距机获取3D点云的方法的流程图；

图2为根据本发明一个实施例的机载2D激光测距机获取3D点云的方法的数据处理示意图；

图3为根据本发明一个实施例的2D激光测距机测量范围的示意图；以及

图4为根据本发明一个实施例的飞行器位姿示意图。

具体实施方式

下面详细描述本发明的实施例，实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

图1为根据本发明一个实施例的机载2D激光测距机获取3D点云的方法的流程图。图2为根据本发明一个实施例的机载2D激光测距机获取3D点云的方法的数据处理示意图。如图1所示，根据本发明实施例的机载2D激光测距机获取3D点云的方法，包括以下步骤：

步骤S101，通过EKF算法对惯性测量设备所测量的角速度进行处理，以得到横滚角和俯仰角。

步骤S102，通过机载激光测距机获取飞行器的高度信息，并根据横滚角和俯仰角以及高度信息得到飞行器的高度坐标。激光测距机为2D激光测距机。

图3为根据本发明一个实施例的2D激光测距机测量范围的示意图。图4为根据本发明一个实施例的飞行器位姿示意图。使用平面镜将机载坐标系下和O_L-X_L,Y_L平面平行的激光光束中的一小部分反射成为和机载坐标系Z轴平行的方向，通过这一小部分激光光束所测量的距离得到机载坐标下飞行器的高度h_L。其中，经过反射的激光光束测量得到的飞行器与障碍物的距离为l，并且测量得到的反射光线和O_L-X_L,Y_L平面平行段的长度为l₀，因此测量到的飞行器的高度h_L可以由下式计算得到，h_L=l-l₀。另外，由于飞行器存在水平方向的倾斜角度横滚角俯仰角ψ，因此当前得到的h_L与飞行器在世界坐标系下的坐标z之间存在如下转换关系，

步骤S103，根据横滚角和俯仰角对激光测距机所采集的扫描数据进行处理以得到第一扫描数据和第二扫描数据。第一扫描数据是通过对第二扫描数据进行投影处理得到的。

具体地，通过去噪处理可以去除扫描数据中的杂散点，保留扫描数据中的有效信息，扫描数据S₀表示为{(r₁,α₁),(r₂,α₂),...,(r_n,α_n)}，其中，(r_i,α_i)为第i个扫描线的测量数据，r_i表示角度为α_i方向到扫描点的距离，扫描线的扫描方向是已确定的方向，扫描方向α_i可以表示为α_i=α_i-1+Δ，其中，Δ为固定的扫描间隔，设扫描的起始角度为α₀。去噪处理的过程为：计算当前扫描(r_i,α_i)的相邻的前m次扫描的距离的均值以及相邻的后m次扫描的距离的均值若|r_i-r^l|>r_thr，且|r_i-r^r|>r_thr，则视当前扫描数据(r_i,α_i)为噪点，将其r_i置为0，从而去除噪点。

由于飞行器飞行过程中会有倾斜角，即横滚角仰角ψ，影响水平位置(x,y)和偏航角θ的估计，因此需要使用投影处理去除横滚角俯仰角ψ，将倾斜的测量数据投影为与O_W-X_W,Y_W平行的数据，在横滚角俯仰角ψ，(r_i,α_i)投影后的新的扫描数据为，

其中，为投影后的测量距离，为投影后的测量角度，r_i为投影前的测量距离，α_i为投影前的测量角度，ψ为飞行器俯仰角，为飞行器横滚角。

在本发明的一个实施例中，激光测距机的光束与激光测距机反射装置的平面镜成45°角。

步骤S104，根据第一扫描数据的数据进行数据间的匹配以得到飞行器的水平位置和偏航角。

具体地，ICP算法对连续两次扫描的数据，即第一扫描数据S₁S₂的数据进行数据间的匹配，从而计算得到相邻两次扫描间飞行器水平位置和偏航角的变化(Δx,Δy)和Δθ。使用从初始时刻到的(Δx,Δy)和Δθ进行累加即可得到当前的水平位置(x,y)和偏航角θ。

步骤S105，根据第二扫描数据、高度坐标、横滚角和俯仰角、水平位置和偏航角得到飞行器的世界坐标集，并根据世界坐标集构成飞行器所在环境的3D点云。

具体而言，将激光测距机的第二扫描数据S₂转化为测量点机载坐标系下的坐标，并根据高度坐标z、横滚角和俯仰角ψ、水平位置(x,y)和偏航角θ得到所机载坐标系与世界坐标系的转换关系，并将测量点在机载坐标系下的坐标转化为世界坐标。将扫描数据S₁中的测量点(r_i,α_i)转化为机载坐标系下的坐标的换算公式为，x_i ^L=r_icosα_i，y_i ^L=r_isinα_i，其中，x_i ^L为第i个测量点的x轴局部坐标，y_i ^L为第i个测量点的y轴局部坐标，r_i为第i个测量点的测量距离，α_i为第i个测量点的测量角度。

由机载坐标系转换为世界坐标系的公式为，

其中，x_i ^W为第i个观测点的x轴世界坐标，y_i ^W为第i个观测点的y轴世界坐标，z_i ^W为第i个观测点的z轴世界坐标，θ为当前激光测距机的偏航角，为当前激光测距机的横滚，ψ为当前激光测距机的俯仰角，x_i ^L为第i个观测点的x轴局部坐标，y_i ^L为第i个观测点的y轴局部坐标，x为激光测距机所在世界坐标系x坐标，y为激光测距机所在世界坐标系y坐标，z为激光测距机所在世界坐标系z坐标，(x_i ^W,y_i ^W,z_i ^W)为第i个观测点的世界坐标，(x_i ^L,y_i ^L,0)为第i个观测点的局部坐标，(x,y,z)为激光测距机中心的世界坐标。

根据多个将多次扫描数据所生成的世界坐标集即可得到飞行器周围环境的3D点云数据。

根据本发明实施例的方法具有如下优点：

1、可获取飞行器所在环境的3D点云数据，以弥补传统激光测距机只能获取2D数据的不足。

2、仅利用飞行器的惯导传感器和2D激光测距机，而不需要其余设备，因此有效地减轻了飞行器的载重负担。

3、通过激光测距机获取水平位置和偏航角，因此与使用传统的惯导传感器相比更具有鲁棒性。

4、通过激光测距的倾斜校正和噪声处理使得激光测距数据的利用更加有效。

5、可以根据需求获取感兴趣区域的3D点云信息，并将激光测距机扫描的数据累积到环境点云中，因此避免了不必要的测量以及由此带来的资源消耗。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (7)

1.一种机载2D激光测距机获取3D点云的方法，其特征在于，包括以下步骤：

通过EKF算法对惯性测量设备所测量的角速度进行处理，以得到横滚角和俯仰角；

通过机载激光测距机获取飞行器的高度信息，并根据所述横滚角和俯仰角以及所述高度信息得到飞行器的高度坐标；

根据所述横滚角和俯仰角对激光测距机所采集的扫描数据进行处理以得到第一扫描数据和第二扫描数据；

根据所述第一扫描数据的数据进行数据间的匹配以得到所述飞行器的水平位置和偏航角；以及

根据所述第二扫描数据、所述高度坐标、所述横滚角和俯仰角、所述水平位置和偏航角得到所述飞行器的世界坐标集，并根据所述世界坐标集构成所述飞行器所在环境的3D点云。

2.如权利要求1所述的机载2D激光测距机获取3D点云的方法，其特征在于，通过如下方式获得所述飞行器的世界坐标：

将所述第二扫描数据转化为测量点机载坐标系下的坐标；

根据所述机载坐标系下的坐标、所述高度坐标、所述横滚角和俯仰角、所述水平位置和偏航角得到所述机载坐标系与世界坐标系的转换关系；以及

根据所述机载坐标系与世界坐标系的转换关系将所述机载坐标系下的坐标转化为所述世界坐标。

3.如权利要求1所述的机载2D激光测距机获取3D点云的方法，其特征在于，所述激光测距机的光束与所述激光测距机反射装置的平面镜成45°角。

4.如权利要求1所述的机载2D激光测距机获取3D点云的方法，其特征在于，所述第一扫描数据是通过对所述第二扫描数据进行投影处理得到的。

5.如权利要求1所述的机载2D激光测距机获取3D点云的方法，其特征在于，所述激光测距机为2D激光测距机。

6.如权利要求2所述的机载2D激光测距机获取3D点云的方法，其特征在于，所述机载坐标系转换为世界坐标系的公式如下，所述公式为：

其中，x_i ^W为第i个观测点的x轴世界坐标，y_i ^W为第i个观测点的y轴世界坐标，z_i ^W为第i个观测点的z轴世界坐标，θ为当前激光测距机的偏航角，为当前激光测距机的横滚角，ψ为当前激光测距机的俯仰角，x_i ^L为第i个观测点的x轴局部坐标，y_i ^L为第i个观测点的y轴局部坐标，x为激光测距机所在世界坐标系x坐标，y为激光测距机所在世界坐标系y坐标，z为激光测距机所在世界坐标系z坐标，(x_i ^W,y_i ^W,z_i ^W)为第i个观测点的世界坐标，(x_i ^L,y_i ^L,0)为第i个观测点的局部坐标，(x,y,z)为激光测距机中心的世界坐标。

7.如权利要求4所述的机载2D激光测距机获取3D点云的方法，其特征在于，所述第一扫描数据通过如下公式表示，所述公式为：

其中，为投影后的测量距离，为投影后的测量角度，r_i为投影前的测量距离，α_i为投影前的测量角度，ψ为飞行器俯仰角，为当前激光测距机的横滚角。