CN112731335B - 一种基于全区域激光扫描的多无人机协作定位方法 - Google Patents

Abstract

本发明涉及多旋翼无人机技术领域，尤其涉及一种基于全区域激光扫描的多无人机协作定位方法。一种基于全区域激光扫描的多无人机协作定位方法，包括以下步骤：步骤一、基于三维点云的多目标在线定位观测；步骤二、基于三维点云的多目标在线定位跟踪。本发明的有益效果：本发明基于旋转二维激光获取全区域的稠密点云，减小了视野盲区。使用无人机搭载轻量化旋转二维激光雷达对场景中的多个运动无人机目标进行实时跟踪定位，将多机协作定位问题转化为多目标追踪问题，降低了被观测无人机对自身所搭载传感器的依赖。由于整个定位追踪过程只基于激光雷达的扫描，所以不受光照条件的影响。

Description

一种基于全区域激光扫描的多无人机协作定位方法

技术领域

本发明涉及多旋翼无人机技术领域，尤其涉及一种基于全区域激光扫描的多无人机协作定位方法。

背景技术

近年来，多旋翼无人机广泛应用在农药喷洒、环境探索、搜寻救援等领域，为了提高工作效率，往往需要多架无人机在同一场景内同时进行工作，鲁棒的相对位姿估计能够确保多无人机各自规划的轨迹互不干扰，从而保证飞行任务的安全性。

目前应用在无人机上的定位方法按照传感器划分主要分为基于视觉的定位和基于激光的定位。基于视觉的定位通过相机感光元件来捕获环境中物体的影像。随着相机位置的变化，相机所捕获的场景图像也相对的发生变化，视觉里程计算法从变化的图像中推断出机器人自身的位姿信息。基于激光的定位以激光作为信号源发射脉冲激光，打到周围环境中的树木、桥梁和建筑物上。激光在这些物体表面引起散射，一部分会被反射到雷达的接收器上。雷达根据激光测距原理计算其自身与周围环境之间的相对位姿。

文献(VINS-Mono:A Robust and Versatile Monocular Visual-Inertial StateEstimator)将单目视觉与IMU进行融合，有效的消除了IMU积分漂移并解决了单目尺度不可观测的问题，实现了鲁棒的定位效果，但是基于视觉的定位方法易受到光照条件的影响，所以无法应用在光照强度变化较为剧烈或夜间无光照场景中。

文献(Robust and Efficient Quadrotor Trajectory Generation for FastAutonomous Flight)利用扩展卡尔曼滤波器将三维激光与IMU数据进行融合实现了鲁棒的三维空间定位，并基于此进行了一系列避障飞行实验，但是由于无人机所携带的三维激光视角有限，存在较大观测盲区，并且使用三维激光扫描其他无人机目标获得的点云较为稀疏，不容易进行聚类处理，所以这种方法不适用于多台无人机之间的协同定位。

发明内容

为解决现有技术的不足，本发明提出一种基于全区域激光扫描的多无人机协作定位方法。通过在一台无人机下方固定电机装置来驱动二维激光雷达沿着电机轴以固定速率旋转，如图1(a)所示。无人机斜上方至下方区域均不存在盲区，都可进行观测，如图1(b)所示。当二维激光雷达沿着电机轴经过180度旋转以后，雷达所扫描的二维平面点云累积为三维空间场景的稠密点云，对这一帧稠密点云进行降采样、地面分割、聚类等处理，获得场景内其他多个无人机目标相对于搭载旋转激光雷达无人机的位置。再利用联合概率数据关联滤波器对每连续两帧点云中的多个聚类目标各自进行关联以实现对这些目标的实时追踪定位。本发明基于旋转二维激光获取全区域的稠密点云，减小了视野盲区，并且不受外界可见光影响，使得搭载轻量化旋转二维激光的无人机能够与场景内其他多个无人机目标进行实时的协作定位。

本发明的技术方案：

一种基于全区域激光扫描的多无人机协作定位方法具体包括以下步骤：

步骤一、基于三维点云的多目标在线定位观测

由于旋转激光旋转180度(一帧)所累积的三维点云数据量很大，会影响分割、聚类的效率，本发明基于体素栅格对原始点云进行降采样处理。又因为旋转激光雷达存在测量噪声，会产生噪点影响聚类效果，本发明基于Radius Outlier Removal方法对点云进行降噪处理。首先以点云中的每个点为中心各自确定半径为r的球形邻域，当邻域内点的数量小于预先设定的阈值k时，即认为该中心点为噪声点，滤除该噪声点。

在对点云数据进行降噪以后，基于RANSAC算法对地面进行提取并滤除，首先估计出地面高度的大致范围，并从该范围内的点云中随机选择最小子集(三个点)来确定平面，再计算点云中每个点到平面模型的距离：

将所有点到平面的距离进行累加，重复以上操作N次，最后选出累加和最小的平面模型作为地面拟合结果。

在拟合出地面点云后，将地面点云与非地面点云进行分割，并基于DBSCAN算法对非地面点云场景中的多个无人机目标进行聚类。首先在点云中随机选择一个未访问过的点p，找到在其半径为r邻域内的所有邻近点。将p标记为已访问过。当在r邻域内的点数量大于阈值min_samples，则判定p是一个“核心点”，并创建一个聚类C；否则，将p判定为“噪声点”。遍历点p的r邻域内所有点，并将他们加入到聚类C中，当发现某一个点也是一个“核心点”，将其更新为新的p点，并且重复遍历点p的r邻域内所有点来检测是否为核心点，直到邻域内的点都被访问过，将聚类C从点云中提取出来，重新聚类剩余点云，当全部点云都被访问过以后算法终止。

从非地面点云中获取到多无人机目标的聚类之后，基于主成分分析法(PCA)确定每个聚类目标的三维尺寸和朝向。首先将点云数据去中心化，得到去中心化之后的点云坐标Y＝(y₁,y₂,...,y_n)，再计算协方差矩阵S＝YY^T，S的特征向量是最大方差的方向，用特征值分解法求解协方差矩阵的特征值与特征向量，并对特征值由小到大排序，选择其中最大的3个，这3个特征向量所指的方向即为该目标点云的主方向，也就是聚类目标最小外接矩形长宽高所指的方向。分别统计出聚类中的点到三个特征向量的最大距离，将该距离乘以2即得到目标对象的最小外接矩形的尺寸。

步骤二、基于三维点云的多目标在线定位跟踪

在处理完连续两帧点云数据之后，需要对这两帧数据中的多个无人机目标状态进行关联，为形成目标连续运动轨迹和状态估计提供数据基础，本发明基于联合概率数据关联滤波器对连续两帧的多目标状态进行关联，算法的流程分为预测、测量验证、关联概率计算与状态估计四部分，具体如下：

(1)预测

联合概率数据关联滤波器预测部分与卡尔曼滤波器的预测部分相近，都是要预测出当前时刻目标的先验估计状态和先验误差协方差矩阵。首先根据上一时刻从点云数据中检测出的多个无人机目标的最优状态估计和更新误差协方差矩阵P^t(k-1|k-1),t＝1,2,...n，计算当前时刻无人机目标的先验估计状态和先验误差协方差矩阵：

其中t为目标编号，k为当前时刻。Q^t(k-1)为上一时刻系统过程噪声的协方差矩阵，F^t(k-1)为被观测无人机目标的位置参数矩阵。

根据先验估计状态计算先验观测值：

其中H(k)为无人机目标的速度参数矩阵，这里约定所有无人机都匀速飞行。

下一步计算先验新息向量，即为真实观测值与先验观测值的差值：

(2)测量验证

在测量验证部分建立关联门来筛选有效观测目标。首先以每个目标的先验估计值为中心建立椭球形关联门，只有位于关联门内的观测值才视为有效观测。然后根据每个目标的可关联门限和有效观测，建立确认矩阵：

其中，j表示观测值编号，t表示目标编号，ω_jt表示第j个观测值与第t个目标的关联情况。

(3)关联概率计算

对确认矩阵按照拆分原则(1.每个观测只能来源于一个目标，2.每个目标最多只能产生一个真实观测)进行分解得到互联矩阵，表示在基于确认矩阵的条件下所有可能的关联情况，互联矩阵表示如下：

其中，表示在第i个互联事件中，当第j个测量值属于第t个目标时，取值为1；否则，取值为0。

下一步计算每一个互联事件θ_i的可行联合事件概率：

其中θ_i,i＝1,2,…,L为可行事件，Z^k为所有的有效观测值，λ为泊松分布参数，c为归一化因子，为测量关联指示器，表明在可行事件θ_i中，第j个观测值是否关联到目标，/>为目标检测指示器，表示在可行事件θ_i中第t个目标是否被观测到。/>表示检测到目标t的概率，/>表示第j个观测值属于目标t的新息向量，/>为新息向量/>的协方差矩阵。

根据可行联合事件概率和互联矩阵计算关联概率：

(4)状态更新

由上一时刻(k-1)预测出的状态值误差协方差矩阵/>和当前时刻(k)的观测值Z_j(k)估计出目标当前的最优状态，并完成对误差协方差矩阵的更新。首先计算卡尔曼滤波增益：

K^t(k)＝P^t(k|k-1)H^t(k)[S^t(k)]^-1 (10)

其中，为观测误差的协方差矩阵。根据三维多目标检测部分获取的mk个观测值，计算第t个目标的新息向量：

再根据先验估计状态和新息向量计算后验估计状态：

最后对当前时刻(k)的后验误差协方差矩阵进行更新：

本发明的有益效果：本发明基于旋转二维激光获取全区域的稠密点云，减小了视野盲区。使用无人机搭载轻量化旋转二维激光雷达对场景中的多个运动无人机目标进行实时跟踪定位，将多机协作定位问题转化为多目标追踪问题，降低了被观测无人机对自身所搭载传感器的依赖。由于整个定位追踪过程只基于激光雷达的扫描，所以不受光照条件的影响。

附图说明

图1为旋转二维激光雷达硬件示意图。(a)为旋转二维激光雷达的左视图。(b)为旋转二维激光雷达的主视图，图中箭头区域表示二维激光雷达扫描的垂直平面角度范围为270度。

图2为旋转二维激光雷达旋转180度所累积的三维点云数据。

图3为经过降采样和去噪处理后的三维点云数据。

图4为使用RANSAC方法将地面与非地面进行分割后的点云数据。

图5为利用主成分分析法估计出的两个目标点云的包围盒尺寸及朝向。

具体实施方式

为了使本技术领域的技术人员更好地理解本发明的技术方案，下面结合附图和最佳实施例对本发明作进一步的详细说明。

如图所示，本发明选择使用大疆M100四旋翼无人机、旋转二维激光、机载计算机(ARM或X86)搭建硬件平台。其中，旋转二维激光负责采集场景中其他无人机的三维点云数据，机载计算机负责三维点云数据处理、多目标数据关联计算等。

控制一台搭载旋转二维激光雷达的M100四旋翼无人机悬停在离地面高度为3米的位置，同时控制另外两架四旋翼无人机在第一台无人机下方匀速飞行。旋转二维激光雷达在室内场景中进行连续扫描(约定旋转二维激光外置电机每旋转180度累积一帧三维点云数据)。获取到场景内另两台四旋翼无人机的稠密点云数据，如图2所示。对每帧三维点云数据进行如下操作：

(1)三维多目标检测

首先利用体素栅格对一帧点云数据进行降采样处理，降低点云的数据规模，加快对三维点云的处理速度，并利用Radius Outlier Removal方法对噪点进行去除，以降低噪点对点云后续聚类效果的影响，处理效果如图3所示。再利用RANSAC方法拟合地面，将地面与非地面进行分割，分割效果如图4所示。对于分割好的场景使用DBSCAN算法进行聚类操作(设定好无人机目标点云数量的阈值范围)。最后利用主成分分析法(PCA)估计已聚类目标点云的最小外接矩形尺寸和朝向，如图5所示。

(2)三维多目标跟踪

在目标检测阶段获取到两架无人机目标的位置(包围盒的中心坐标)。由于两架无人机在场景中不断移动，所以还要将此刻激光雷达检测到的两个目标状态与它们上一时刻的状态进行关联。

首先，用卡尔曼滤波器估计运动目标在三维空间内的坐标，定义状态向量和状态转移矩阵如下：

其中，表示目标t在第k时刻的三维坐标，/>表示目标t在第k时刻的速度，△t为旋转二维激光外置电机旋转180度的时间间隔。

接下来建立系统模型。首先由目标检测模块获取两个目标在初始时刻(k＝0)的状态t代表目标编号，再由公式(2)计算下一时刻(k＝1)的先验估计状态然后根据公式(3)计算先验误差协方差矩阵/>根据公式(4)计算出目标的先验观测值/>最后获取由目标检测模块观测到的两个移动目标的状态信息Z^t(1|0)。由于场景中同时存在两个运动目标，并且由于外界干扰还会产生一些杂波信息，卡尔曼滤波器并不能处理此类多目标跟踪问题，所以下一部分利用联合概率数据关联滤波器进行处理。

首先确定关联门范围，将两个目标的初始状态分别作为两个可关联区域的中心点，误差协方差矩阵是一个3*3的对称矩阵，形式如下：

可关联区域的形状是一个椭球，其大小(椭球体积)为：

其中，λ_x、λ_y、λ_z是误差协方差矩阵的特征值。

在k＝0时刻，分别以两个目标的最优估计状态为中心确立椭圆形关联门，X₁、X₂、X₃、X₄为三维目标检测模块在k＝1时刻获得的有效观测值。基于以上信息建立确认矩阵Ω如下：

其中，t＝0为杂波干扰项，t＝1,2为目标编号，j＝1,2,3,4为观测值编号，按照技术方案步骤二(3)中的拆分原则，将确认矩阵拆分成如下11个互联矩阵：

假设4个观测值均服从泊松分布，首先根据公式(8)求出可行事件θ₁的联合事件概率：

同理，计算出θ₂～θ₁₁的联合事件概率P{θ₂|Z^k}～P{θ₁₁|Z^k}。最后，由公式(9)求出所有可行事件θ₁～θ₁₁的关联概率

在状态更新部分，首先由公式(11)求出经过关联后的两个目标各自的新息向量再根据预测部分获得的先验误差协方差矩阵P^t(1|0)和新息向量协方差矩阵S^t(1)求出卡尔曼滤波增益K^t(1)，最后由公式(12)和公式(13)计算当前时刻(k＝1)两个目标的最优估计状态/>和更新后的误差协方差矩阵P^t(1|1)，作为对下一时刻(k＝2)两个目标状态估计的输入。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种基于全区域激光扫描的多无人机协作定位方法，其特征在于包括以下步骤：

步骤一、基于三维点云的多目标在线定位观测：

对点云数据进行降噪处理；然后对地面进行提取并滤除，得到地面拟合结果；再将地面点云与非地面点云进行分割，并对非地面点云场景中的多个无人机目标进行聚类；从非地面点云中获取到多无人机目标的聚类之后，确定每个聚类目标的三维尺寸和朝向，其中，确定每个聚类目标的三维尺寸和朝向的具体步骤为：首先将点云数据去中心化，得到去中心化之后的点云坐标，再计算协方差矩阵/>，S的特征向量是最大方差的方向，用特征值分解法求解协方差矩阵的特征值与特征向量，并对特征值由小到大排序，选择其中最大的3个，这3个特征向量所指的方向即为该目标点云的主方向，也就是聚类目标最小外接矩形长宽高所指的方向，分别统计出聚类中的点到三个特征向量的最大距离，将该距离乘以2即得到目标对象的最小外接矩形的尺寸；

步骤二、基于三维点云的多目标在线定位跟踪：

在处理完连续两帧点云数据之后，需要对这两帧数据中的多个无人机目标状态进行关联，为形成目标连续运动轨迹和状态估计提供数据基础，其中，关联滤波器对连续两帧的多目标状态进行关联，算法的流程分为预测、测量验证、关联概率计算与状态估计四部分；

其中，预测部分包括以下具体步骤：

首先根据上一时刻从点云数据中检测出的多个无人机目标的最优状态估计和更新误差协方差矩阵/>，计算当前时刻无人机目标的先验估计状态和先验误差协方差矩阵：

；

；

其中，t为目标编号，k为当前时刻，为上一时刻系统过程噪声

的协方差矩阵，为被观测无人机目标的位置参数矩阵；

根据先验估计状态计算先验观测值：

；

其中，为无人机目标的速度参数矩阵，这里约定所有无人机都匀速飞行；

下一步计算先验新息向量，即为真实观测值与先验观测值的差值：

。

2.根据权利要求1所述的一种基于全区域激光扫描的多无人机协作定位方法，其特征在于，对点云数据进行降噪处理的具体步骤为：首先以点云中的每个点为中心各自确定半径为r的球形邻域，当邻域内点的数量小于预先设定的阈值k时，即认为该中心点为噪声点，滤除该噪声点。

3.根据权利要求1所述的一种基于全区域激光扫描的多无人机协作定位方法，其特征在于，对地面进行提取并滤除的具体步骤为：首先估计出地面高度的大致范围，并从该范围内的点云中随机选择最小子集来确定平面，再计算点云中每个点到平面模型的距离：

；

将所有点到平面的距离进行累加，重复以上操作N次，最后选出累加和最小的平面模型作为地面拟合结果。

4.根据权利要求1所述的一种基于全区域激光扫描的多无人机协作定位方法，其特征在于，将地面点云与非地面点云进行分割，并对非地面点云场景中的多个无人机目标进行聚类的具体步骤为：首先在点云中随机选择一个未访问过的点p，找到在其半径为r邻域内的所有邻近点，将p标记为已访问过，当在r邻域内的点数量大于阈值min_samples，则判定p是一个“核心点”，并创建一个聚类C；否则，将p判定为“噪声点”；遍历点p的r邻域内所有点，并将他们加入到聚类C中，当发现某一个点也是一个“核心点”，将其更新为新的p点，并且重复遍历点p的r邻域内所有点来检测是否为核心点，直到邻域内的点都被访问过，将聚类C从点云中提取出来，重新聚类剩余点云，当全部点云都被访问过以后算法终止。

5.根据权利要求1所述的一种基于全区域激光扫描的多无人机协作定位方法，其特征在于，测量验证部分包括以下具体步骤：

在测量验证部分建立关联门来筛选有效观测目标，首先以每个目标的先验估计值为中心建立椭球形关联门，只有位于关联门内的观测值才视为有效观测，然后根据每个目标的可关联门限和有效观测，建立确认矩阵：

其中，j表示观测值编号，t表示目标编号，jt表示第j个观测值与第t个目标的关联情况。

6.根据权利要求1所述的一种基于全区域激光扫描的多无人机协作定位方法，其特征在于，关联概率计算部分包括以下具体步骤：

对确认矩阵按照拆分原则进行分解得到互联矩阵，表示在基于确认矩阵的条件下所有可能的关联情况，互联矩阵表示如下：

；

；

其中，表示在第i个互联事件中，当第j个测量值属于第t个目标时，取值为1；否则，取值为0；

下一步计算每一个互联事件的可行联合事件概率：

其中，为可行事件，/>为所有的有效观测值，/>为泊松分布参数，c为归一化因子，/>为测量关联指示器，表明在可行事件/>中，第j个观测值是否关联到目标，/>为目标检测指示器，表示在可行事件/>中第t个目标是否被观测到，/>表示检测到目标t的概率，/>表示第j个观测值属于目标t的新息向量，/>为新息向量/>的协方差矩阵；

根据可行联合事件概率和互联矩阵计算关联概率：

。

7.根据权利要求1所述的一种基于全区域激光扫描的多无人机协作定位方法，其特征在于，状态更新部分包括以下具体步骤：

由上一时刻（k-1）预测出的状态值、误差协方差矩阵/>和当前时刻（k）的观测值/>估计出目标当前的最优状态，并完成对误差协方差矩阵的更新，首先计算卡尔曼滤波增益：

；

其中，为观测误差的协方差矩阵，根据三维多目标检测部分获取的mk个观测值，计算第t个目标的新息向量：

；

最后对当前时刻（k）的后验误差协方差矩阵进行更新：

。