无人机上的主动旋转激光雷达系统及其控制方法

技术领域

本发明涉及激光雷达导航技术，具体涉及一种无人机上的主动旋转激光雷达系统及其控制方法。

背景技术

无人机(UAV)是指无人驾驶的由无线电遥控设备控制的或预先编译的程序控制的飞行器。与载人飞机相比，它具有体积小、造价低、使用方便、对作战环境要求低和战场生存能力较强等优点。随着近几年无人机的研究有很大进展，小型自主旋翼无人机能够在受限空间内进行高度敏捷的操作，并且不受地面障碍物的阻碍，在危险区域或人类无法抵达空间开展巡检、探测领域有着广阔的应用需求，无人机的应用领域也在不断扩大中。为了无人机能够适应各种不同场景的任务，无人机对周围环境感知的要求也不断提高。因此无人机的导航技术是无人机领域的重要技术之一。

现阶段大多数无人机的定位方法还是基于惯性传感器和全球导航卫星系统(GNSS)。而在应用场景不再是室外GNSS信号无遮挡的区域，而是室内、城市环境和树林等GNSS信号不稳定或无GNSS信号的环境的时候，无人机在这些场景的应用受到了严重的限制。

同步定位与建图(Simultaneous Localization and Mapping,SLAM)技术是是机器人视觉的一个重要研究内容，采用SLAM技术可以通过无人机搭载的激光雷达传感器对环境进行感知。结合IMU惯性测量元件，无人机可以估计自身的位姿。并且无人机通过对激光雷达采集的数据进行处理后，还可以建立起点云地图，通过对障碍物的检测，可以实现避障功能，从而提高无人机的避障飞行能力。

多数SLAM技术采用的是视觉摄像头导航，但是相机对环境的敏感性较高，对硬件的处理能力要求较高，应用场景限制在了无强光照的室内环境下。而在室外环境下，由于激光雷达具有精度高，对计算量要求小和不受光照干扰的特点，更多的方案则采用激光雷达导航。

在反制低慢小无人机时，激光SLAM可以为反制无人机提供良好的定位能力，但同时由于低慢小无人机的空间位置不确定性，要求反低慢小无人机在导航方面具有更强的环境适应性。无人机搭载固定放置的激光雷达传感器，在遇到狭长环境或只有高楼环境等这类俯仰方向约束极小的情况下，会因为几何特征不明显，容易引起退化现象，导致定位可靠性降低。虽然激光雷达的水平方向的视野可以达到360°，但垂直只有30°左右，俯仰方向的约束少，几何特征不明显的缺陷会被无限放大。而如果采用多摄像头或者多激光雷达的方式会造成负重增大的结果，必须提升无人机的载重，同时也会提高大量的经济成本。因此如何在几何特征少，约束不够的环境下进行定位和导航的问题仍需解决。

由于上述原因，本发明人对现有的激光雷达及其制导方法做了深入研究，以期待设计出一种能够解决上述问题的新的系统及方法。

发明内容

为了克服上述问题，本发明人进行了锐意研究，设计出一种无人机上的主动旋转激光雷达系统及其控制方法，该系统及方法中设置能够旋转的激光雷达，在激光雷达主动旋转的过程中，因为IMU短期的高精准性，可以用IMU的姿态数据暂时代替激光雷达的里程计结果，在激光雷达采集到约束足够的数据时，选择使用激光雷达的里程计结果，具体来说，实时解算并判断激光雷达获得的数据图像是否可靠，在不可靠的情况下，主动旋转该激光雷达，以使得下一帧数据图像的可靠性提高，另外，通过设置特异性的非线性方程来处理激光雷达获得的数据图像，从而得到无人机的位姿，进而据此对无人机进行控制，从而完成本发明。

具体来说，本发明的目的在于提供以一种无人机上的主动旋转激光雷达系统，其特征在于，该系统包括安装在无人机上的转台，在所述转台上安装有能够随着转台旋转的激光雷达，在所述激光雷达上还固定安装有IMU模块。

其中，该系统还包括转台控制模块和位姿解算模块；

通过所述转台控制模块实时判断是否需要旋转转台和转台的旋转角度；

通过所述位姿解算模块实时解算无人机的位姿。

其中，所述转台控制模块实时调取激光雷达实扫描获得的数据图像；

在所述转台控制模块中执行如下步骤：

S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；

S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；

S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。

其中，在步骤S1中，所述每帧图像都包括多条扫描线，将每条扫描线分成多个部分，从每个部分中选取曲率最大的两个点作为边缘点，再从每个部分中选取曲率最小的四个点作为平面点；

优选地，将每帧图像分成6个部分。

本发明还提供一种装载有主动旋转激光雷达的无人机的控制方法，

该方法中，在无人机上安装转台1，在转台1上设置能够随着转台旋转的激光雷达2，在所述激光雷达上还固定安装有IMU模块21；

通过转台控制模块实时解算安装在无人机上的转台的控制指令，

该方法中，通过位姿解算模块实时解算无人机的位姿。

其中，在所述转台控制模块中执行如下步骤：

S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；

S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；

S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。

其中，在步骤S3中，通过下式(四)实时判断数据图像是否可靠：

其中，当f(λ

当f(λ

优选地，旋转角度Δθ通过下式(五)获得：

其中，

u

逆时针方向为正方向，顺时针方向为负方向。

其中，所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值；

在所述位姿解算模块中，通过下式(六)迭代获得无人机的位姿：

f(T

其中，f(T

式(六)的迭代过程为：

将T

将得到的f(T

将再次得到的J代入到-(J

重复上述迭代过程，直至：

且同时满足：

d

其中，所述d

其中，

其中，点j是第k帧图像特征点在世界坐标系中离点i最近的点，

点l是点j所在扫描线中距离j最近的点，

点m是第k帧图像特征点在世界坐标系中离j最近的点。

其中，

其中，R表示旋转矩阵，通过下式(十)实时获得：

R＝I+ωsinθ+ω

I表示单位矩阵，

ω＝T

θ＝||T

本发明所具有的有益效果包括：

(1)根据本发明提供的无人机上的主动旋转激光雷达系统及其控制方法决了无人机在低成本下，一个激光雷达无法完全感知周围的环境，导致数据不足和定位不准的问题，降低了成本，减少了对负重的要求；

(2)根据本发明提供的无人机上的主动旋转激光雷达系统及其控制方法针对激光雷达数据在低空多变的环境下，由于缺少俯仰方向的约束，会产生定位可靠性降低的情况，通过采用可主动旋转激光雷达的方法，有效地提高了定位准确性。

附图说明

图1示出根据本发明一种优选实施方式的无人机上的主动旋转激光雷达系统整体结构逻辑图；

图2示出本发明实施例中飞行轨迹对比图；

图3示出本发明对比例中飞行轨迹对比图；

图4示出本发明实施例中x/y/z方向的里程对比图；

图5示出本发明对比例中x/y/z方向的里程对比图；

图6示出本发明实施例中x/y/z方向的误差对比图；

图7示出本发明对比例中x/y/z方向的误差对比图。

附图标号说明：

1-转台

2-激光雷达

21-IMU模块

3-转台控制模块

4-位姿解算模块

具体实施方式

下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。

在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

根据本发明提供的无人机上的主动旋转激光雷达系统，如图1中所示，该系统包括安装在无人机上的转台1，在所述转台上安装有能够随着转台旋转的激光雷达2，在所述激光雷达上还固定安装有IMU模块21。

其中，所述转台能够带动激光雷达绕着滚转方向旋转。

所述激光雷达扫描的频率是低频的，IMU模块是高频率的，可以达到400HZ，同时具有短期的高精准性，在成功获取激光雷达里程计结果前，使用IMU的姿态变换数据来代替里程计结果，提升激光雷达里程计的输出频率，从而提高可靠性。

在一个优选的实施方式中，该系统还包括转台控制模块3和位姿解算模块4；

通过所述转台控制模块3实时判断是否需要旋转转台1和转台1的旋转角度；

通过所述位姿解算模块4实时解算无人机的位姿，所述位姿包括无人机的位置和速度信息，在获得位姿的情况下，即可集合目标位置信息对无人机进行制导控制。

在一个优选的实施方式中，所述转台控制模块实时调取激光雷达实扫描获得的数据图像；所述激光雷达的工作频率为10HZ，激光雷达每工作一次，会有多条扫描线，由多条扫描线的扫描结果组合成一帧数据图像，也称之为一帧点云数据。

在所述转台控制模块中执行如下步骤：

S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；

S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；

S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。

在一个优选的实施方式中，在步骤S1中，通过下式解算每个点的曲率：

c(

优选地，在步骤S1中，所述每帧图像都包括多条扫描线，将每条扫描线分成多个部分，从每个部分中选取曲率最大的两个点作为边缘点，再从每个部分中选取曲率最小的四个点作为平面点；

为使得特征点分布均匀，优选地，将每帧图像分成6个部分。具体来说，每一次扫描线，扫描的角度是360°，从0°开始，每60°为一个部分，从而将扫描线平均分为6个部分。

在一个优选的实施方式中，在步骤S2中，协方差通过下式(一)获得：

其中，C表示协方差，m表示数据集中点的数量，

优选地，通过下式(三)进行特征值分解：

C＝UΛU

U＝[u

λ

u

在一个优选的实施方式中，在步骤S3中，通过下式(四)实时判断数据图像是否可靠：

其中，当f(λ

当f(λ

优选地，旋转角度Δθ通过下式(五)获得：

其中，

u

其获得方式为：通过对特征点数据进行特征值分解分别得到三个坐标轴对应的特征值和特征向量；

通过比较特征值大小，获得最大的特征值λ

优选地，逆时针方向为正方向，顺时针方向为负方向。

在一个优选的实施方式中，所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值；

在所述位姿解算模块中，通过下式(六)迭代获得无人机的位姿：

f(T

其中，f(T

式(六)的迭代过程为：

将T

将得到的f(T

将再次得到的J代入到-(J

将得到的姿态增量ΔT

重复上述迭代过程，直至：

且同时满足：

d

本申请中，通过迭代得到T

T

其中，t

θ

所述世界坐标系是指程序初始运行时刻激光雷达所在的坐标系，并设激光雷达正前方为x轴，正左方为y轴，通过右手定则确定正上方为z轴。

在一个优选的实施方式中，所述d

其中，

其中，点j是第k帧图像特征点在世界坐标系中离点i最近的点，即将特征点全部转换到世界坐标系下进行比较；

点l是点j所在扫描线中距离j最近的点，

点m是第k帧图像特征点在世界坐标系中与j不同扫描线离j最近的点。

所述点j、l和m都是步骤S1中的数据集中的点；

点j、l和m构成平面，该平面即为d

在一个优选的实施方式中，

其中，R表示旋转矩阵，通过下式(十)实时获得：

R＝I+ωsinθ+ω

I表示单位矩阵，

ω＝T

θ＝||T

在计算过程中T

本申请中，T

本发明还提供一种装载有主动旋转激光雷达的无人机的控制方法，

该方法中，在无人机上安装转台1，在转台1上设置能够随着转台旋转的激光雷达2，在所述激光雷达上还固定安装有IMU模块21；

通过转台控制模块3实时解算安装在无人机上的转台的控制指令，

所述转台控制指令包括是否需要旋转转台和转台的旋转角度；

该方法中，通过位姿解算模块4实时解算无人机的位姿。

具体来说，在所述转台控制模块中执行如下步骤：

S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；

S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；

S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。

在一个优选的实施方式中，在步骤S1中，通过下式解算每个点的曲率：

c

优选地，在步骤S1中，所述每帧图像都包括多条扫描线，将每条扫描线分成多个部分，从每个部分中选取曲率最大的两个点作为边缘点，再从每个部分中选取曲率最小的四个点作为平面点；

优选地，将每帧图像分成6个部分。每一次扫描线，扫描的角度是360°，从0°开始，每60°为一个部分，从而可以将扫描线平均分为6个部分。

在一个优选的实施方式中，在步骤S2中，协方差通过下式(一)获得：

其中，C表示协方差，m表示数据集中点的数量，

优选地，通过下式(三)进行特征值分解：

C＝UΛU

U＝[u

λ

u

在一个优选的实施方式中，在步骤S3中，通过下式(四)实时判断数据图像是否可靠：

其中，当f(λ

当f(λ

优选地，旋转角度Δθ通过下式(五)获得：

其中，

u

通过比较特征值大小，获得最大的特征值λ

优选地，逆时针方向为正方向，顺时针方向为负方向。

在一个优选的实施方式中，所述位姿解算模块实时调取激光雷达实扫描获得的数据图像和IMU模块的测量值；

在所述位姿解算模块中，通过下式(六)迭代获得无人机的位姿：

f(T

其中，f(T

式(六)的迭代过程为：

将T

将得到的f(T

将再次得到的J代入到-(J

将得到的姿态增量ΔT

重复上述迭代过程，直至：

且同时满足：

d

本申请中，通过迭代得到T

T

其中，t

θ

所述世界坐标系是指程序初始运行时刻激光雷达所在的坐标系，并设激光雷达正前方为x轴，正左方为y轴，通过右手定则确定正上方为z轴。

在一个优选的实施方式中，所述d

其中，

其中，点j是第k帧图像特征点在世界坐标系中离点i最近的点，

点l是点j所在扫描线中距离j最近的点，

点m是第k帧图像特征点在世界坐标系中和j不同扫描线离j最近的点。

所述点j、l和m都是步骤S1中的数据集中的点；

点j、l和m构成平面，该平面即为d

在一个优选的实施方式中，

其中，R表示旋转矩阵，通过下式(十)实时获得：

R＝I+ωsinθ+ω

I表示单位矩阵，

ω＝T

θ＝||T

在计算过程中T

本申请中，T

实施例：

选用以大疆M600无人机，在其上安装转台，在转台上安装能够随转台绕滚转轴旋转的线三维激光雷达VLP-16，在激光雷达上安装、MTI-300惯性传感器；该激光雷达每秒能够有30万个点数据输出，±15°的垂直视场，360°水平视场扫描。

将无人机放置在开阔的土地上，周围只具有一些不超过20m的楼房。设置轨迹为一个半矩形，控制无人机按照该矩形轨迹飞行，在无人机上实时通过转台控制模块控制转台旋转，实时通过位姿解算模块解算无人机自身的位姿，并且将解算得到的位姿存储在无人机中，

在无人机按照轨迹飞行的过程中，还实时记录无人机真实的位姿。

无人机在飞行过程中，转台控制模块的工作过程为：

S1，解算数据图像中每个点的曲率，从中挑取出一定数量的边缘点和平面点，共同组成数据集；

S2，对数据集做去中心化处理，解算协方差，并通过特征值分解得到特征值和特征向量；

S3，比较特征值大小，并据此判断此次激光雷达扫描获得的数据图像是否可靠，如数据不可靠，解算出转台旋转角度Δθ，并据此控制转台旋转。

其中，是否需要旋转转台通过述方法判断：

其中，当f(λ

当f(λ

在数据不可靠时，控制转台旋转角度Δθ。

Δθ通过下式获得：

其中，

u

位姿解算模块的工作过程为：

调取激光雷达实扫描获得的数据图像，

通过下述迭代方法来解算无人机的位姿：

f(T

其中，f(T

式(六)的迭代过程为：

将T

将得到的f(T

将再次得到的J代入到-(J

将得到的姿态增量ΔT

重复上述迭代过程，直至：

且同时满足：

通过迭代得到的T

记录得到的飞行轨迹如图2中的红色线条所示，对应真实的飞行轨迹如图2中的蓝色线条所示；

记录得到的飞行器在x/y/z方向的里程如图4中的红色线条所示，对应真实的飞行器在x/y/z方向的里程如图4中的绿色线条所示；

记录得到的飞行器在x/y/z方向的误差如图6中的红/绿/蓝色线条所示。

对比例：

选用以大疆M600无人机，将线三维激光雷达VLP-16固定安装在该无人机上。

将无人机放置在开阔的土地上，周围只具有一些不超过20m的楼房。设置轨迹为一个半矩形，控制无人机按照该矩形轨迹飞行，在无人机上实时通过读取解算无人机自身的位姿，并且将解算得到的位姿存储在无人机中，

在无人机按照轨迹飞行的过程中，还实时记录无人机真实的位姿。

记录得到的飞行轨迹如图3中的红色线条所示，对应真实的飞行轨迹如图3中的蓝蓝色线条所示；

记录得到的飞行器在x/y/z方向的里程如图5中的红色线条所示，对应真实的飞行器在x/y/z方向的里程如图5中的绿色线条所示；

记录得到的飞行器在x/y/z方向的误差如图7中的红/绿/蓝色线条所示；

根据实施例和对比例的结果，比较图2和图3，图4和图5，图6和图7可知，实施例中得到的轨迹与真实值之间的平均误差为0.190m，对比例中得到的轨迹与真实值之间的平均误差为0.501m，本申请提供的无人机上的主动旋转激光雷达系统及其控制方法在相同条件下能够显著提高自身位姿的获取精度。

以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。