CN111580554A - 基于逐帧识别生成原始点云的室内无人机编队飞行方法 - Google Patents

Abstract

本专利提供了基于逐帧识别生成原始点云的室内无人机编队飞行方法，首先使用室内定位系统获取无人机三维点云图像，通过局域网传送至地面控制系统；地面站接收到信息后，通过迭代最近点的三维点云匹配算法，利用空间变换将多张点云图像信息融合，以此并行获取多台无人机在空间中的位姿信息；最后，地面站PC通过Crazyradio广播特定的频率，回传飞行信息至无人机完成实验。地面站的工作利用到了基于人工势场和非线性串级控制的方法进行无人机的轨迹规划和控制。本发明可以实现室内小区域面积内多台无人机的集群飞行和避障实验。

Description

基于逐帧识别生成原始点云的室内无人机编队飞行方法

技术领域

本发明涉及一种基于逐帧识别生成原始点云的室内无人机编队飞行,属于无人机编队飞行技术领域。

背景技术

几十年来，来自动物行为学、物理学、生物物理学、社会科学和计算机科学等不同学科的科学家们被成群结队的现象出现所吸引。这些现象包括鸟类、鱼类、企鹅、蚂蚁、蜜蜂和人群。随着无人机技术的日渐成熟，应用场景也越来越多，包括高层物流配送、电力线路检测、侦察监视等等。但是这类应用大多基于单机的行为，只能完成相对简单且不需要协作的任务。而在面对更加复杂的任务时且需要多机协作的任务时，必须考虑无人机群体编队。

近几年，类似于集群行为的多智能体编队飞行吸引了国内外很多学者的研究。对多无人机编队飞行的控制系统越来越多，它们可大致分为室内编队飞行系统和室外编队飞行系统。无论是室内还是室外，编队飞行的首要条件就是获取无人机在三维空间中的位姿信息和无人机间的相对位置信息。室外编队分行主要依靠GPS信号加上地面控制站来获取无人机信息，这一方法相对较成熟。但对于室内相对来说比较封闭的环境，GPS信号很难起作用，这对室内多无人机机协同编队飞行提出了新的挑战。

针对室内无人机编队飞行，传统的捕捉无人机位姿信息的方法是：通过将反光球标记点固定在无人机上，由动态捕捉系统根据无人机上的标记点相互连接而成的刚体的形状或大小来区别不用的无人机，这导致的问题是，由于室内无人机体积本身较小，因此在无人机上固定的标记点构成的刚体形状种类很有限，面对多台无人机，动态捕捉系统将难以区分。再者，标记点由于相对较小，飞行过程中，无人机自身或者周围的无人机可能会将反光球完全遮挡，此时可能会出现在一定时间内将无法获取到无人机的位姿信息的情况，从而导致编队局部发散。因此，面对室内多台无人机编队飞行，一个可靠的同时识别多台无人机在空间中位姿的系统和方法显得尤其重要。

发明内容

为克服现有技术的不足，本发明旨在提出一种基于逐帧识别生成原始点云的室内无人机编队飞行系统方案以解决现有的室内多无人机编队飞行系统中，由于无人机体积较小，利用标记点的方法难以区分多台无人机的技术问题。为此，本发明采用的技术方案是结合分布式编队结构，使用室内动态捕捉系统进行逐帧识别生成原始点云来获取小型无人机编队的飞行轨迹和飞行状态，并配合带有前馈作用的串联PID控制器和人工势场法的轨迹规划方法提出了一套完整的室内多架小型四旋翼无人机协同编队飞行演示验证平台设计方案

本系统采用Optitrack室内动态捕捉系统跟踪被反光球标记的无人机。选择室内动态捕捉系统的原因是它在定位方面的高性能：位置误差小于1毫米。与之相比，最先进的分散定位系统——超带宽无线电三角测量方法的位置误差大于10cm，对于本系统的小型无人机来说，误差过大。

本系统使用的小型无人机自身主要由以下几个部分组成：主控制器为STM32F405，负责无人机状态估计计算、产生PWM波送至电机驱动；六轴陀螺仪MPU-9250,用于检测无人机自身局部的姿态变化；通信模块为nRF51,可发送或接收2.4GHz的无线电波，负责和上位机的通信。在无人机中部装有可拆卸的小型锂电池，电池可利用板载USB2.0接口直接充电，或者取下单独充电。底部和上部都有拓展口，可拓展led环形灯等等各种模块。电机的类型是小型的空心杯，共由四个空心杯和直径大约为7cm的4个旋翼组成，其中A型和B型旋翼各一对。改造无人机内部软件的方法是利用USB2.0通过对主控芯片STM32F4烧写固件以及控制程序。该机型体积小，适用于密集地层的室内飞行。由于其低惯性，它能在高速碰撞中幸存下来，对人类构成的风险也很小。

室内动态捕捉系统有一台PC和多台高速红外摄像头组成，PC上安装Optitrack软件，控制并接收室内动态捕捉系统所获取的无人机编队的数据，并通过以太网将数据传送给地面控制系统；地面控制系统为另一个PC，即无人机编队控制计算机，在该PC上运行Linux操作系统并运行各个功能模块，同时结合ROS操作系统的通信架构实现各个功能间的数据交互。

室内小型四旋翼无人机编队演示验证平台工作原理为首先通过室内定位系统经过标定构建出室内三维坐标系,通过动态捕捉系统使用逐帧识别生成原始点云方法识别各个无人机,控制动态捕捉系统的PC通过以太网将无人机的数据发送给另一台PC，另一台PC上的各个模块根据数据计算出每架无人机当前的运动状态并计算出合理的飞行轨迹，再通过无线电广播设备将轨迹信息发送至无人机，无人机接收到定位信息和控制指令，根据系统时间和轨迹信息计算当前无人机的期望运动状态，无人机根据自身运动状态和期望运动状态，通过控制器控制算法驱使无人机按照轨迹飞行，最终实现无人机编队飞行。

进一步细化的具体步骤是：

步骤1：小型无人机识别

本发明根据多台高速红外摄像机对无人机进行逐帧识别，并将每一帧图像生成原始的点云图。由于每台摄像机拍摄视角不同，对同一架无人机所得到的点云会有较大的差异，通过迭代最近点的点云匹配算法，通过空间变换将多张点云图像信息融合，以此来并行获取多台无人机在空间中的位姿信息。

3D点云图像的匹配详细步骤如下：

(4)假设待配准的点云集合为

前一时刻已知的点云集合为

从待匹配的点云集合P中提取出与X的每个点最近的距离点。此距离代价根据：欧几里得距离

确定，从而确定新的集合p′。

(5)对于两个点集之间的相似度度量，用如下目标函数表示：

其中，R为旋转矩阵，T为平移矩阵，即姿态与位置的矩阵表示，

表示已知点集的某个坐标。系统需要获取无人机的三维坐标和滚动角、俯仰角和偏航角，一共六个维度的信息，由迭代最近点算法，系统可以用四元数，即用q₀到q₃表示旋转矩阵，q₄至q₆表示平移向量。

旋转矩阵R和平移矩阵T为：

T＝[q₄ q₅ q₆]^T

(6)设点云的集合p′和X的中心为

和

则有：

得到平均值后，分别求出两个点云集合的协方差矩阵为：

其中利用反对称矩阵：

构建得到列向量：

Δ＝[A₂₃ A₃₁ A₁₂]^T

最终构建出以下目矩阵：

其中tr(∑px)表示矩阵∑px的迹，即主对角线元素的总和，也就是特征值之和。

(4)代入数据后，对Q矩阵作特征值分解，求得最大的特征值以及对应的特征向量。此特征向量即为对应误差平方和最小时的四元数。将求得的四元数代入矩阵R中，即可求出旋转矩阵，平移矩阵T可通过两个点云集合的中心关系，平移得到。

(5)重复迭代以上4个步骤，直至求出的

小于一个规定的阈值。至此，无人机的位姿信息可得到。

步骤2：设计带有前馈作用的串联PID控制器

本系统的控制器是非线性位置控制器，增加了位置和偏航误差的积分项。控制输入是当前状态(P，V，q，ω_m)以及位置偏微分方程P_des、速度偏微分方程V_des、加速度偏微分方程

偏航偏微分方程ψ_des和角速度的设定点ω_des。该控制器采用级联设计方法，外回路用于定位，内回路用于姿态控制。两个回路都实现了反馈项；外部位置回路从轨迹计划中添加前馈项。将位置和速度误差定义为：

e_p＝p_des-p，e_v＝v_des-v

分别计算所需的力向量，如下所示：

其中K_p，K_i，K_v是正对角增益矩阵

期望的物体旋转矩阵R_des是一个关于F_des、ψ_des的函数。方向误差计算如下

其中R是是姿态四元数q的矩阵形式，(·)^V是V算子映射S03→R³，

然后用另一个PID控制器计算所需的矩：

正对角线增益矩阵K₀，K_m，K_ω.一个简单的线性映射可以计算出M_des和F_des到z轴的投影所需要的平方向速度.

F_des中的位置误差积分补偿了由于不对称(x；y部分)导致的电池电压降(z部分)和质心不平衡。从M_des中的姿态误差积分出发，只使用z分量，它可以补偿由于螺旋桨和发动机磨损不均造成的偏航误差。手工修整和零件更换可以补偿单一飞行器的这类问题，但在大型飞行器编队中是不可行的。

步骤3：基于人工势场法的无人机飞行轨迹规划

人工势场法(APF)的基本思想是假想目标点对无人机存在引力作用，而障碍物对无人机存在斥力作用，无人机在两种力的共同作用下，靠近目标点并避开障碍物。

无人机与目标在m维空间中对应的两点之间的真实距离会对引力势场函数造成影响，而斥力势场函数则与该无人机与障碍物之间的欧几里得距离有关。引力势场函数及斥力势场函数如下所示：

式中，p——无人机的具体位置；

p_des——工作空间目标点的位置；

p_ob——工作空间障碍物的位置；

k_a——引力势场比例系数；

k_r——斥力势场比例系数；

ρ(p，p_ob)——无人机与障碍物在多维空间中的绝对距离；

ρ₀——单个障碍物影响的最大范围。

考虑叠加性原理，对两个势场函数进行叠加运算可得到无人机的运动方向和位姿的合成势场函数，无人机所受矢量合力由引力和斥力合成。

U(p)＝U_a(p)+U_r(p)

F(p)＝F_a(p)+F_r(p)

与现有技术相比，本发明具有以下优点：

(1)解决了现有的室内多无人机编队飞行系统中，由于无人机体积较小，利用标记点的方法难以区分多台无人机的技术问题，提高了四旋翼无人机的协同编队飞行效果；

(2)改善了传统演示验证平台通信时滞、丢包等不足，提高了无人机编队飞行的演示效果；

(3)该机型体积小，适用于室内飞行。由于其低惯性，它能在高速碰撞中幸存下来，对人类构成的风险也很小。

附图说明

图1是本系统总体架构示意图

图2是本系统所使用的无人机控制系统结构图

图3是3D点云图像匹配流程图

图4是人工势场法流程图

图5是人工势场法仿真结果图

图6是最近迭代法点云匹配前仿真图

图7是最近迭代法点云匹配后仿真图

具体实施方式

本发明的目的在于提出了一种将基于逐帧识别生成原始点云的方法应用于多架小型四旋翼无人机协同编队飞行的平台设计方案。具体而言，就是结合分布式编队结构，使用室内动态捕捉系统进行逐帧识别生成原始点云来获取小型无人机编队的飞行轨迹和飞行状态，并配合带有前馈作用的串联PID控制器和人工势场法的轨迹规划方法提出了一套完整的室内多架小型四旋翼无人机协同编队飞行演示验证平台设计方案，解决了现有的室内多无人机编队飞行系统中，由于无人机体积较小，利用标记点的方法难以区分多台无人机的技术问题，提高了四旋翼无人机的协同编队飞行效果。

本发明在实际编队飞行中可通过逐帧识别生成原始点云图像的方法获取无人机数据，配合带有前馈作用的串联PID控制器和人工势场法进行编队飞行，进而促进理论研究的发展。

(一)总体架构设计方案

本发明总体架构如图1所示，其总体结构主要由室内动态捕捉系统、地面控制系统、多无人机系统组成,通过室内动态捕捉系统同时获取多架无人机的点云图像信息，通过地面控制系统实现对无人机的位姿信息和编队轨迹的求解，通过多无人机系统接收地面控制系统的控制指令实现室内多无人机编队飞行演示效果。同时,本发明在实际编队飞行中可通过逐帧识别生成原始点云图像的方法获取无人机数据，配合带有前馈作用的串联PID控制器和人工势场法进行编队飞行，进而促进理论研究的发展。

本系统采用Optitrack室内动态捕捉系统跟踪被反光球标记的无人机。选择室内动态捕捉系统的原因是它在定位方面的高性能：位置误差小于1毫米。与之相比，最先进的分散定位系统——超带宽无线电三角测量方法的位置误差大于10cm，对于本系统的小型无人机来说，误差过大。

如图2所示，本系统使用的小型无人机自身主要由以下几个部分组成：主控制器为STM32F405，负责无人机状态估计计算、产生PWM波送至电机驱动；六轴陀螺仪MPU-9250,用于检测无人机自身局部的姿态变化；通信模块为nRF51,可发送或接收2.4GHz的无线电波，负责和上位机的通信。在无人机中部装有可拆卸的小型锂电池，电池可利用板载USB2.0接口直接充电，或者取下单独充电。底部和上部都有拓展口，可拓展led环形灯等等各种模块。电机的类型是小型的空心杯，共由四个空心杯和直径大约为7cm的4个旋翼组成，其中A型和B型旋翼各一对。改造无人机内部软件的方法是利用USB2.0通过对主控芯片STM32F4烧写固件以及控制程序。该机型体积小，适用于密集地层的室内飞行。由于其低惯性，它能在高速碰撞中幸存下来，对人类构成的风险也很小。

室内动态捕捉系统有一台PC和多台高速红外摄像头组成，PC上安装Optitrack软件，控制并接收室内动态捕捉系统所获取的无人机编队的数据，并通过以太网将数据传送给地面控制系统；地面控制系统为另一个PC，即无人机编队控制计算机，在该PC上运行Linux操作系统并运行各个功能模块，同时结合ROS操作系统的通信架构实现各个功能间的数据交互。

室内小型四旋翼无人机编队演示验证平台工作原理为首先通过室内定位系统经过标定构建出室内三维坐标系,通过动态捕捉系统使用逐帧识别生成原始点云方法识别各个无人机,控制动态捕捉系统的PC通过以太网将无人机的数据发送给另一台PC，另一台PC上的各个模块根据数据计算出每架无人机当前的运动状态并计算出合理的飞行轨迹，再通过无线电广播设备将轨迹信息发送至无人机，无人机接收到定位信息和控制指令，根据系统时间和轨迹信息计算当前无人机的期望运动状态，无人机根据自身运动状态和期望运动状态，通过控制器控制算法驱使无人机按照轨迹飞行，最终实现无人机编队飞行。

(二)小型无人机识别设计方案

本发明根据多台高速红外摄像机对无人机进行逐帧识别，并将每一帧图像生成原始的点云图。由于每台摄像机拍摄视角不同，对同一架无人机所得到的点云会有较大的差异，通过迭代最近点的点云匹配算法，通过空间变换将多张点云图像信息融合，以此来并行获取多台无人机在空间中的位姿信息。

如图3所示，3D点云图像的匹配详细步骤如下：

(1)假设待配准的点云集合为

前一时刻已知的点云集合为

从待匹配的点云集合P中提取出与X的每个点最近的距离点。此距离代价根据：欧几里得距离

确定，从而确定新的集合p′。

(2)对于两个点集之间的相似度度量，用如下目标函数表示：

其中，R为旋转矩阵，T为平移矩阵，即姿态与位置的矩阵表示，

表示已知点集的某个坐标。系统需要获取无人机的三维坐标和滚动角、俯仰角和偏航角，一共六个维度的信息，由迭代最近点算法，系统可以用四元数，即用q₀到q₃表示旋转矩阵，q₄至q₆表示平移向量。

旋转矩阵R和平移矩阵T为：

T＝[q₄ q₅ q₆]^T

(3)设点云的集合p′和X的中心为

和

则有：

得到平均值后，分别求出两个点云集合的协方差矩阵为：

其中利用反对称矩阵：

构建得到列向量：

Δ＝[A₂₃ A₃₁ A₁₂]^T

最终构建出以下目矩阵：

其中tr(∑px)表示矩阵∑px的迹，即主对角线元素的总和，也就是特征值之和。

(4)代入数据后，对Q矩阵作特征值分解，求得最大的特征值以及对应的特征向量。此特征向量即为对应误差平方和最小时的四元数。将求得的四元数代入矩阵R中，即可求出旋转矩阵，平移矩阵T可通过两个点云集合的中心关系，平移得到。

(5)重复迭代以上4个步骤，直至求出的

小于一个规定的阈值。至此，无人机的位姿信息可得到。

(三)轨迹控制设计方案本发明所采用的轨迹控制方案主要包括如下几个步骤：

步骤1：设计带有前馈作用的串联PID控制器

本系统的控制器是非线性位置控制器，增加了位置和偏航误差的积分项。控制输入是当前状态(P，V，q，ω_m)以及位置偏微分方程P_des、速度偏微分方程V_des、加速度偏微分方程

偏航偏微分方程ψ_des和角速度的设定点ω_des。该控制器采用级联设计方法，外回路用于定位，内回路用于姿态控制。两个回路都实现了反馈项；外部位置回路从轨迹计划中添加前馈项。将位置和速度误差定义为：

e_p＝p_des-p，e_v＝v_des-v

分别计算所需的力向量，如下所示：

其中K_p，K_i，K_v是正对角增益矩阵

期望的物体旋转矩阵R_des是一个关于F_des、ψ_des的函数。方向误差计算如下

其中R是是姿态四元数q的矩阵形式，(·)^V是V算子映射S03→R³，

然后用另一个PID控制器计算所需的矩：

正对角线增益矩阵K₀，K_m，K_ω.一个简单的线性映射可以计算出M_des和F_des到z轴的投影所需要的平方向速度.

F_des中的位置误差积分补偿了由于不对称(x；y部分)导致的电池电压降(z部分)和质心不平衡。从M_des中的姿态误差积分出发，只使用z分量，它可以补偿由于螺旋桨和发动机磨损不均造成的偏航误差。手工修整和零件更换可以补偿单一飞行器的这类问题，但在大型飞行器编队中是不可行的。

步骤2：基于人工势场法的无人机飞行轨迹规划

人工势场法(APF)的基本思想是假想目标点对无人机存在引力作用，而障碍物对无人机存在斥力作用，无人机在两种力的共同作用下，靠近目标点并避开障碍物。

如图4所示，无人机与目标在m维空间中对应的两点之间的真实距离会对引力势场函数造成影响，而斥力势场函数则与该无人机与障碍物之间的欧几里得距离有关。引力势场函数及斥力势场函数如下所示：

式中，p——无人机的具体位置；

p_des——工作空间目标点的位置；

p_0b——工作空间障碍物的位置；

k_a——引力势场比例系数；

k_r——斥力势场比例系数；

ρ(p，p_ob)——无人机与障碍物在多维空间中的绝对距离；

ρ₀——单个障碍物影响的最大范围。

考虑叠加性原理，对两个势场函数进行叠加运算可得到无人机的运动方向和位姿的合成势场函数，无人机所受矢量合力由引力和斥力合成。

U(p)＝U_a(p)+U_r(p)

F(p)＝F_a(p)+F_r(p)

(四)具体实例分析

①小型四旋翼无人机：本实例使用Crazyflie2.0，斜对的电机轴之间长92毫米，带电池重27克。它包含一个32位，168兆赫的ARM微控制器与浮点单元，能够显着板载计算。软件和硬件都是开源的。Crazyflie通过Crazyradio PA与PC进行通信，Crazyradio PA是一个2.4GHz的USB无线电广播设备，可以以高达2兆比特/秒的速度传输32字节大小的数据包。

②地面控制系统：本系统由一台台式机组成，处理器为因特尔酷睿i7-8700，运行内存为16G DDR4 2666MHz，主频为3.0GHz,显卡为GTX1060，主要作用是运行基于python的地面系统软件，进行点云匹配和轨迹规划的计算，并将期望位姿实时发送给无人机。

③室内动态捕捉系统：本系统由一台台式机组成，配置了i7-8700处理器，运行内存为8G DDR4 2666MHz，主频为3.0GHz，显卡为GTX1080ti,运行了动态捕捉系统上位机motive。主要作用是获取无人机编队的三维点云信息，并通过局域网发送至地面控制系统。

④实验结果分析：室内飞行空间为4m×5m×3m的立方体。前期，本实例在室内进行了两架编队飞行控制实验。随着无人机数量的增加，由于地面反光，无人机编队出现局部发散的情况。为了消除反射光对原始点云的获取的影响，最终在实验室地面铺设了一层黑地毯，最终成功进行了五架无人机协同编队飞行，五架无人机按照规划好的轨迹，进行了平移和逆时针旋转后降落。

Claims (6)

1.基于逐帧识别生成原始点云的室内无人机编队飞行方法，其特征在于，具体步骤如下：用多台摄像机对无人机进行逐帧识别，并将每一帧图像生成原始的点云图；通过迭代最近点的点云匹配算法，以及空间变换将多张点云图像信息融合，并行获取多台无人机在空间中的位姿信息；摄像机获得各个无人机的位姿信息后，经过PC机计算出每台无人机下一个时刻的目标位姿，并通过无线电进行广播，无人机接收到信息后经板载控制器处理，实现编队飞行。

2.根据权利要求1所述的基于逐帧识别生成原始点云的室内无人机编队飞行方法，其特征在于，所述的无人机由以下几个部分组成：主控制器，用于负责无人机状态估计计算、产生PWM波送至电机驱动；六轴陀螺仪,用于检测无人机自身局部的姿态变化；通信模块,用于发送或接收2.4GHz的无线电波，负责和上位机的通信；无人机中部装有可拆卸的锂电池，底部和上部都有拓展口。

3.根据权利要求1所述的基于逐帧识别生成原始点云的室内无人机编队飞行方法，其特征在于，通过多台高速红外摄像机，在同一时间，不同的角度对所有的无人机进行拍摄，生成多张点云图像，对点云图做旋转或者平移的空间变换，使点云图像中对应于空间同一位置的点一一对应，当得到点集之间的距离小于一个阈值时，则认为是最佳比配，从而获得每架无人机的坐标信息。

4.根据权利要求2所述的基于逐帧识别生成原始点云的室内无人机编队飞行方法，其特征在于，无人机与上位机之间的通信是通过无线电发射器将所有无人机的下一个期待目标的位姿信息发布到整个工作环境中，每台无人机是并行的，都接收到相同的信息，并将接收到的数据用单片机进行处理，根据每台无人机的ID，提取出对应自身的目标位姿信息，并通过控制器控制算法驱使无人机按照各自的期望轨迹飞行，最终实现无人机编队飞行。

5.根据权利要求3所述的基于逐帧识别生成原始点云的室内无人机编队飞行方法，其特征在于，点云图像的匹配步骤具体为：

(1)假设待配准的点云集合为

前一时刻已知的点云集合为

从待匹配的点云集合P中提取出与X的每个点最近的距离点，此距离代价根据：欧几里得距离

确定，从而确定新的集合p′；

(2)对于两个点集之间的相似度度量，用如下目标函数表示：

其中，R为旋转矩阵，T为平移矩阵，即姿态与位置的矩阵表示，

表示已知点集的某个坐标；获取无人机的三维坐标和滚动角、俯仰角和偏航角，一共六个维度的信息，

由迭代最近点算法，系统可以用四元数，即用q₀到q₃表示旋转矩阵，q₄至q₆表示平移向量；

旋转矩阵R和平移矩阵T为：

T＝[q₄ q₅ q₆]^T

(3)设点云的集合p′和X的中心为

和

则有：

得到平均值后，分别求出两个点云集合的协方差矩阵为：

其中利用反对称矩阵：

构建得到列向量：

Δ＝[A₂₃ A₃₁ A₁₂]^T

最终构建出以下目标矩阵：

其中tr(Σpx)表示矩阵Σpx的迹，即主对角线元素的总和，也就是特征值之和；

(4)代入数据后，对Q矩阵作特征值分解，求得最大的特征值以及对应的特征向量，此特征向量即为对应误差平方和最小时的四元数；将求得的四元数代入矩阵R中，即可求出旋转矩阵，平移矩阵T通过两个点云集合的中心关系，平移得到；

(5)重复迭代以上4个步骤，直至求出的

小于一个规定的阈值，至此，无人机的位姿信息可得到。

6.根据权利要求4所述的基于逐帧识别生成原始点云的室内无人机编队飞行方法，其特征在于，无人机目标的位姿信息得到后，采用带前馈的串级PID控制和人工势场法进行轨迹规划。

Images