CN107807661B - 轨迹控制室内四旋翼无人机编队演示验证平台及方法 - Google Patents

Abstract

本发明涉及无人机领域、嵌入式技术领域、Wi‑Fi无线数据传输领域，为突破传统演示验证平台在应对通信时滞、丢包能力的不足，根据无人机飞行状态实时进行编队轨迹规划，提高四旋翼无人机的编队飞行演示效果。为此，本发明采用的技术方案是，轨迹控制室内四旋翼无人机编队演示验证平台，由无人机端、地面计算机端和室内动作捕捉系统组成，结合分布式编队结构，通过无线网络获取临近无人机编队飞行轨迹和飞行状态，考虑四旋翼无人机在实际飞行环境下的动力学模型，基于微分平坦理论设计四旋翼无人机实时轨迹规划方法，并配合室内动作捕捉系统，实现室内多架四旋翼无人机编队飞行演示验证。本发明主要应用于无人机控制演示场合。

Description

轨迹控制室内四旋翼无人机编队演示验证平台及方法

技术领域

本发明涉及无人机领域、嵌入式技术领域、Wi-Fi无线数据传输领域，尤其涉及一种基于轨迹控制的无人机编队智能控制领域，具体来说，是一种基于轨迹跟踪控制方法、Pixhawk飞控、Wi-Fi无线网络和Optitrack室内动作捕捉系统的多无人机系统仿真与算法验证平台。

背景技术

多无人机编队技术已经被广泛应用于多类实际任务，如搜救、巡检和自动化工业等方面。在这些场景中，多个无人机可能需要形成一定的特定队形来执行任务，例如通过携带不同的设备，执行侦察监视、对地攻击、电子干扰、通信中继、目标定位、攻击损伤有效评估等任务。多无人机编队技术在民用方面也被广泛应用如：气象探测、公路巡视、勘探测绘、水灾监视、电力线路查巡、森林火灾防救等。

在过去一段时间，小型四旋翼无人机编队控制技术已经得到了广泛关注，为了让多无人机保持稳定的队形，需要每个无人机确切知道自己的位置和无人机间的相对距离。因此，需要借助较为精确的定位系统和一些机载感知设备以便提供给无人机相对精确的状态信息。本发明采用OptiTrack三维运动捕捉系统，以每秒数百帧的拍摄速率捕捉多架无人机上固定的特制标记点，能够实时精确地构建出标记点三维空间位置信息，实现无人机位置和姿态信息的捕捉，使无人机可以获取准确的自身运动状态。

Wi-Fi的频段在世界范围内是无需任何电信运营执照的，因此为WLAN无线设备提供了一个世界范围内可以使用的，费用极其低廉而且数据带宽极高的无线空中接口。本发明采用Raspberry Pi zero-w作为机载计算机，并结合Wi-Fi无线通信技术搭建多无人机演示验证平台。该系统不仅可以降低各个无人机之间以及无人机与地面计算机之间的连接复杂度，便于系统开发，同时可以使无人机之间进行双向数据传输，使仿真环境更加接近于实际飞行情况。

小型四旋翼无人机的飞行控制计算机大致可以分成三大类：以单片机、数字信号处理器(DSP)为核心的飞行控制计算机；以专用嵌入式微处理器为核心的飞行控制计算机。DSP在一些通用性的应用方面与CPU有所差异，一般很少用到嵌入式操作系统，也缺乏一些通用的应用软件的支持，因此不利于实现复杂的应用设计，如网络协议、多线程任务处理；ARM处理器体积小，性能强，功耗低，易移植操作系统，但是在无人机应用方面需要选择高端ARM，同时需要采用协控制器分担部分计算量。本发明无人机端采用Pixhawk开源飞控作为无人机飞行控制器，其自身具备MEMS(Micro-Electro-Mechanical-System)传感器可以结合Optitrack动作捕捉系统发送给无人机的位置信息测量并估算无人机自身的位姿数据，其采用的Nuttx嵌入式实时操作系统支持多线程任务的开发，不仅降低了系统的功耗和体积，而且可扩展性强，硬件平台支持后期开发各种应用程序。无人机端的机载计算机使用的Linux嵌入式操作系统支持多线程任务和应用程序的开发，通过Wi-Fi无线网络技术实现地面计算机与无人机以及无人机与无人机间通讯，解决了多机系统布线成本过高以及各机交互通信的问题，还提高了系统的实时性。地面端的地面控制系统采用两台PC机通过Wi-Fi无线网络分别为无人机发送轨迹控制指令和Optitrack动作捕捉系统所捕捉到的位姿数据。

针对多无人机编队控制问题，目前美国宾夕法尼亚大学的GRASP实验室、麻省理工学院、瑞士苏黎世联邦理工大学、新加坡国立大学等国家的科研团队都在进行深入研究；近年来，国内的清华大学，北京航空航天大学，天津大学等高校也开展了相关研究。由此可见多无人机编队技术是国际无人机领域研究的前沿热点问题。因此，设计一套易扩展、稳定性强、能够实现无人机间通信和状态测量的多无人机编队演示验证系统具有极高的实际价值和应用意义。目前，现有的多无人机编队实物演示验证平台大都采用离线规划路径点方式，以发送期望路径点给对应无人机的方法控制无人机形成编队，这种平台结构简单，易于实现。然而，这类平台存在的不足主要体现在以下几个方面：(1)基于发送期望路径点的方法在实现无人机快速飞行的场景时，通常需要较高的通信带宽以维持期望路径点的刷新频率，通信发生时滞、丢包都会严重影响无人机的飞行效果；(2)在离线规划路径点的过程中没有考虑无人机自身动态特性，限制了无人机的极限飞行能力，不能确保无人机获取最优飞行轨迹；(3)基于发送期望路径点的控制方法在求取多维期望运动状态时无法获取精确的解析解，导致给控制器的设计带来困难，难以实现对飞行路径的高精度跟踪；(4)基于路径点的轨迹规划方法多为离线路径规划方法，离线规划方法无法实时根据动态的地图信息，改变无人机轨迹规划策略。

针对上述局限，本发明综合Wi-Fi网络、嵌入式微处理器、高精度动作捕捉系统以及ROS操作系统搭建室内多无人机编队演示验证平台，基于微分平台理论设计实现单机轨迹规划器和编队轨迹规划器，基于传统PID控制方法设计实现轨迹跟踪控制器。根据该类方案设计的多无人机编队演示验证平台，能有效较低无人机与无人机间、无人机与地面控制系统的通信带宽，显著提高四旋翼无人机应对通信时滞、丢包的能力；此外，基于这种方法设计的轨迹规划器能够有效结合四旋翼无人机的动态特性，能确保得到最优的飞行轨迹函数，同时，通过无人机飞行轨迹函数可以根据当前系统时间计算得到精确的多维期望运动状态，大大提高了轨迹跟踪控制器的控制精度；更重要的是基于本发明给出的平台系统，可以对轨迹规划方法和轨迹跟踪方法进行综合验证，快速推进编队控制理论研究的发展进程。

发明内容

为克服现有技术的不足，本发明旨在提出一种将实时轨迹规划算法应用于多架四旋翼无人机编队飞行演示的平台设计方案，突破传统演示验证平台在应对通信时滞、丢包能力的不足，根据无人机飞行状态实时进行编队轨迹规划，提高四旋翼无人机的编队飞行演示效果。为此，本发明采用的技术方案是，轨迹控制室内四旋翼无人机编队演示验证平台，由无人机端、地面计算机端和室内动作捕捉系统组成，结合分布式编队结构，通过无线网络获取临近无人机编队飞行轨迹和飞行状态，考虑四旋翼无人机在实际飞行环境下的动力学模型，基于微分平坦理论设计四旋翼无人机实时轨迹规划方法，并配合室内动作捕捉系统，实现室内多架四旋翼无人机编队飞行演示验证。

无人机端包括多架四旋翼无人机，每架四旋翼无人机包含机械部分、动力部分和电子部分，其中机械部分即无人机的机架，动力部分包括电池、电机、电机驱动、桨叶，电子部分主要包括机载控制器、机载计算机和遥控器接收器模块；无人机底板安置电池、遥控器接收器模块，无人机中间机架板安置4个电机、4个电调和Pixhawk飞行控制器，无人机顶板用于安置机载计算机和定位系统标记点；机载控制器内置三轴陀螺仪、三轴加速度计以及三轴磁阻计，无人机在室内飞行，依靠动作捕捉系统提供位置信息，机载控制器采用串级PID加前馈控制的位置环控制器结合无人机自身运动状态和期望运动状态实现轨迹跟踪；机载计算机基于MAVLink协议通过串口与机载控制器进行数据交互，获取当前无人机的运动状态，并将无人机的运动状态发布在Wi-Fi无线网络中，同时该机载计算机通过linux系统中的ntpdate工具与计算机进行系统时间同步，并接受由地面计算机规划好的飞行轨迹，基于该飞行轨迹和当前系统时间计算无人机的期望运动状态，最后通过串口将该期望运动状态发送至机载控制器中用于轨迹跟踪控制。

地面计算机端由两台PC机组成，一台计算机负责接受动作捕捉系统的数据并通过Wi-Fi无线网络转发给所有无人机；对每架无人机飞行模式和飞行状态进行管控。另一台计算机负责通过无线网络接受所有无人机的运动状态；作为系统时间服务器与每架无人机的系统时间进行同步；基于MATLAB/Simulink软件仿真环境结合每架无人机的运动状态进行实时轨迹规划，再将规划好的飞行轨迹发送至每架无人机。

轨迹控制室内四旋翼无人机编队演示验证方法：首先通过室内定位系统经过标定构建出室内xyz三维坐标系，通过红外摄像头对各个无人机上的标记点进行捕捉，通过交换机将标记点信息传递给动作捕捉计算机；动作捕捉计算机的运动捕捉系统配套软件根据标记点信息实时解算出每个无人机在当前室内三维坐标系下的空间坐标和自身姿态，并通过数据打包通过UDP/IP协议以多播的形式发送给地面计算机；地面计算机端由两台计算机组成，一台计算机运行地面站控制软件，另一台计算机运行轨迹规划软件；地面控制软件在Linux环境下基于ROS操作系统进行开发，整套软件由两个主要进程组成，定位数据处理进程基于NatNetSDK接收来自动作捕捉系统的无人机位姿信息并经过坐标变换得到适用于无人机定位的数据，再通过MAVLink协议对数据进行打包，并通过Wi-Fi无线网络发送至无人机端，无人机控制进程通过终端获取用户指令，再基于MAVLink协议对指令打包通过无线网络发送至无人机；轨迹规划软件基于MATLAB/Simulink仿真软件进行开发，运行轨迹规划算法，获取每架无人机当前的运动状态并计算出合理的飞行，再通过无线网络将轨迹信息发送至无人机端；无人机端机载计算机负责接收地面控制软件的定位信息和控制指令以及来自轨迹规划软件的轨迹信息，并负责与轨迹规划计算机的系统时间进行同步，根据系统时间和轨迹信息计算当前时刻无人机的期望运动状态并发送给飞控，无人机根据自身运动状态和期望运动状态，通过控制器算法驱使无人机按照轨迹飞行，最终实现无人机编队飞行。

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

步骤1：设计带有前馈作用的串级PID控制器，包括：

(1)确定四旋翼无人机动力学模型

设计右手惯性坐标系定义惯性坐标为W＝[x_W,y_W,z_W]，惯性坐标系中无人机机体坐标系设定为B＝[x_B,y_B,z_B],则无人机在空间中的位置坐标表示为r＝[x,y,z]∈R³,无人机在惯性坐标系下的姿态表示为R∈SO(3)，姿态角速率表示为ω＝[ω₁ ω₂ ω₃]^T∈R³,ω₁,ω₂,ω₃分别表示滚转角速率、俯仰角速率和偏航角速率，定义映射关系f如下：

该映射关系的逆映射表示为f^-1，则无人机的运动学方程可以表示为：

其中,m表示无人机的质量，J∈R³表示无人机的转动惯量，g表示重力加速度，控制量τ＝[τ₁ τ₂ τ₃ τ₄]^T与电机转速v＝[v₁ v₂ v₃ v₄]之间的关系如下：

其中，τ₁表示四个旋翼产生的总升力，τ₂,τ₃,τ₄分别表示滚转、俯仰和偏航通道的控制力矩，v_i表示旋翼i的旋转速度，L表示旋翼转动中心到无人机中心的距离，k_F,k_M分别表示旋翼的升力系数和扭矩系数。

(2)机载控制器控制算法设计

①轨迹跟踪控制器设计：采用经典内外环控制结构，外环为位置控制环，通过轨迹规划算法获取当前时刻的期望运动状态r_d＝[x_d,y_d,z_d]^T，依据期望位置和位置状态r的误差e_p，设计比例-前馈控制器得到期望速度控制量v_des；

e_p＝r_d-r

再利用期望速度控制量和速度状态量

的误差e_v，设计带有前馈作用的PID控制器，得到期望加速度控制量acc_des；

②姿态解算：根据期望加速度求取总升力控制器τ₁和期望姿态下体坐标系z轴向量z_B,des：

通过z_B,des和从轨迹规划中得到的期望偏航角ψ_d，结合李集群思想可以获取旋转矩阵形式的期望姿态R_d,表达式如下：

x_B,des＝y_B,des×z_B,des

R_d＝[x_B,des y_B,des z_B,des]

③姿态跟踪控制器设计：根据期望姿态和当前无人机姿态状态R，基于SO(3)理论求取姿态误差e_R，依据该误差设计比例-微分控制器得到期望角速率控制量ω_d；

定义角速率误差e_ω，并基于该误差设计比例-微分-积分控制器，用以计算得到控制量[τ₂ τ₃ τ₄]^T；

e_ω＝ω_d-ω

步骤2：基于微分平坦的leader无人机飞行轨迹规划

所描述的基于微分平坦的轨迹规划方法是：将leader无人机的期望飞行轨迹描述成跟时间t相关的多项式函数表达σ(t)，形式如下：

由此，轨迹的输出

其中

表示无人机当前在空间中第i个路径点的期望位置，

表示无人机第i个路径点的偏航角，轨迹考虑四旋翼无人机的动态特性和轨迹的输入满足微分平坦的判定条件，根据微分平坦理论对问题进行了转化，并将轨迹的约束条件映射到了平坦输出空间中，考虑对r进行4次微分得到控制量u₂,u₃,u₄的线性表达形式,因此一架无人机在室内空间中运动的轨迹规划问题，转化为如下形式：

s.t.σ(t_i)＝σ_i,i＝0,…,m

其中μ_r,μ_ψ分别为二次规划求解器中参数，通过该问题的求解得到最优的leader无人机飞行轨迹；

步骤3：利用一致性方法对follower无人机的飞行轨迹进行规划；

考虑follower无人机j与leader无人机i构成leader-follower的编队关系，无人机之间保持的相对关系通过s_i,j＝σ_i(t)-σ_j(t)表示，则leader无人机与follower无人机之间的误差e(t)可以定义为e(t)＝x_i(t)-x_j(t)-s_i,j(t)，将无人机编队控制问题转换成如下形式：

s.t.x_j(t₀)＝t₀时刻无人机j的状态

时刻无人机j的状态

其中k_k为二次规划求解器中参数，该优化问题为标准二次规划形式，通过对该问题的求解得到最优的follower无人机在t＝[t₀,t_f]时间段的飞行轨迹，整个leader与follower无人机的编队飞行过程，就是在保证每次二次规划问题的求解时间小于[t₀,t_f]的同时，实时不断循环地计算follower无人机的飞行轨迹。

本发明的特点及有益效果是：

社会效益：此项发明对于多无人机协同控制方法的研究和发展具有十分重要的意义。该项发明具有国际先进水平，它可以作为无人机协同编队飞行研究的试验平台，进而有助于推动多机控制理论的发展，仿真结果不仅可以为无人机编队控制系统的早期设计提供数据和实物演示验证支持，同时为多机编队飞行控制理论的发展提供一个良好的实验平台。

经济效益：无人机由于具有尺寸小、机动性高、造价低以及适应性强等特点，受到了广泛关注。无人机编队协同可以提高执行任务的成功率和抗突发事件的能力，在军事和民用方面具有很大的潜在价值。该发明针对小型四旋翼无人机进行设计，不仅可以为多机控制方法提供实物演示验证平台，解决轨迹规划及跟踪控制问题，而且能够为多无人机编队协同控制系统的设计开发提供依据，避免不必要的经费投入。该发明能够显著地降低系统早期的开发成本，具有重大的经济价值。

附图说明：

附图1室内小型四旋翼无人机编队演示验证平台总体结构图。

附图2无人机控制系统结构图。

附图3地面控制软件功能结构图。

附图4编队轨迹规划软件流程图(leader无人机部分)。

附图5编队轨迹规划软件流程图(follower无人机部分)。

附图6轨迹规划效果图。

附图7两架小型四旋翼无人机编队实际编队飞行效果图。

具体实施方式

本发明的目的在于提出了一种将实时轨迹规划算法应用于多架四旋翼无人机编队飞行演示的平台设计方案。具体而言，就是结合分布式编队结构，通过无线网络获取临近无人机编队飞行轨迹和飞行状态，考虑四旋翼无人机在实际飞行环境下的动力学模型，基于微分平坦理论设计四旋翼无人机实时轨迹规划方法，并配合室内动作捕捉系统提出了一套完整的室内多架四旋翼无人机编队飞行演示验证平台设计方案，突破传统演示验证平台在应对通信时滞、丢包能力的不足，根据无人机飞行状态实时进行编队轨迹规划，提高四旋翼无人机的编队飞行演示效果。

本发明在实际编队飞行中可实现获取飞行数据，加以改进编队飞行控制算法，进而促进理论研究的发展。

本发明在仿真验证的基础上可同时作为多无人机编队飞行演示平台，更真实地显示多无人机编队控制算法的控制效果。

(一)平台结构设计方案

室内小型四旋翼无人机编队演示验证平台结构如图1所示，其总体结构主要由室内动作捕捉系统、地面控制系统、多无人机系统组成，通过室内动作捕捉系统同时获取多架无人机的高精度位姿信息；通过地面控制系统实现对无人机飞行模式的切换管理以及编队轨迹的求解；通过多无人机系统接收地面控制系统的控制指令实现室内多无人机编队飞行演示效果。同时，本发明在实际编队飞行中可实现获取飞行数据，加以改进编队飞行控制算法，进而促进理论研究的发展。在仿真验证的基础上，又可同时作为多无人机编队飞行演示平台，更真实地显示多无人机编队控制算法的控制效果。

基于轨迹控制的室内小型四旋翼无人机编队演示验证平台主要由无人机端和地面计算机端两部分组成。

无人机端一般包括多架四旋翼无人机，每架四旋翼无人机包含机械部分、动力部分和电子部分，其中机械部分即无人机的机架，动力部分包括电池、电机、电机驱动、桨叶等，电子部分主要包括机载控制器、机载计算机和RC遥控器接收器模块。考虑到室内空间环境有限，因此需要体积小可以用于室内飞行实验的小型四旋翼无人机，综合考虑后，本发明选用3寸3叶桨，机架大小设定为轴距140mm，边距90mm的小型四旋翼无人机。整个机体由底板、中间机架板、顶板连接而成(通过尼龙螺柱和尼龙螺丝实现连接)；底板安置电池、遥控器接收器模块，中间机架板安置4个电机、4个电调和Pixhawk飞行控制器，顶板用于安置Raspberry Pi zero-w机载计算机和定位系统标记点(同时起到保护飞行控制器以及其走线的作用)；为便于安装，所有开孔、镂空均根据拟选器件匹配设计。无人机机架加工材料采用3D打印技术一次打印成型，相比其他材料具有更强的硬度和更好的韧性，且易于维护。

小型四旋翼无人机的控制系统结构图如图2所示，系统主要由无人机飞行控制器和机载处理器两个部分组成，无人机飞行控制器具有完整的闭环控制回路，通过状态估计器结合高精度的定位信息和自身传感器数据融合得到自身运动状态，根据自身运动状态和期望运动状态利用串级前馈PID控制器实现轨迹跟踪。本发明选用Pixhawk开源飞行控制器作为小型四旋翼无人机的飞行控制器，内置三轴陀螺仪、三轴加速度计以及三轴磁阻计，依靠动作捕捉系统并融合传感器信息计算位置信息，并对针对开源算法进行二次开发实现轨迹跟踪。机载处理器负责接受来自地面控制计算机的定位数据和控制指令；接受来自轨迹规划计算机的最优轨迹函数，并与该计算机进行系统时间同步；根据系统时间、飞行模式和轨迹函数计算得到无人机的期望运动状态，并实时发送给无人机飞行控制器。本发明选用Raspberry Pi zero-w作为小型四旋翼无人机的机载计算机，基于MAVLink协议通过串口与Pixhawk飞行控制器进行数据交互，获取当前无人机的运动状态，并将无人机的运动状态发布在Wi-Fi无线网络中，同时该机载计算机通过linux系统中的ntpdate工具与计算机进行系统时间同步，并接受由地面计算机规划好的飞行轨迹，基于该飞行轨迹和当前系统时间计算无人机的期望运动状态，最后通过串口将该期望运动状态发送至Pixhawk控制器中用于轨迹跟踪控制。

地面计算机端由两台PC机组成，一台计算机为地面控制计算机，负责接受Optitrack动作捕捉系统的数据并通过Wi-Fi无线网络转发给所有无人机，同时对每架无人机飞行模式和飞行状态进行管控。地面控制软件功能结构如图3所示，在该计算机上运行Linux操作系统并运行各个功能模块，同时结合ROS操作系统的通信框架实现各个功能间的数据交互。通过用户终端可以实时观测到无人机的飞行状态，并通过输入指令实现对无人机模式的切换以及简单的控制。

另一台计算机为轨迹规划计算机，负责通过无线网络接受所有无人机的运动状态，并作为系统时间服务器与每架无人机的系统时间进行同步，同时基于MATLAB/Simulink软件仿真环境结合每架无人机的运动状态进行实时轨迹规划，再将规划好的飞行轨迹发送至每架无人机。实时轨迹规划软件主要分为leader无人机轨迹规划和follower无人机轨迹规划两个部分。其中，leader无人机轨迹规划程序流程图如图4所示，包括如下步骤：①通过无线网络获取无人机当前的运动状态作为轨迹的初始状态；②确立轨迹的约束条件，如：必须经过的航迹点、加速度限制和速度限制等；③将无人机的轨迹规划问题转换为标准的二次规划问题的形式，通过对这类问题的求解得到最优轨迹；④通过无线网络将最优轨迹函数传递到无人机。follower无人机轨迹规划程序被设定为间隔周期t_h＝t_f-t₀的定时中断，其中断程序的流程图如图5所示，具体步骤如下：①以为周期获取无人机自身的运动状态和leader无人机到时间段的轨迹函数；②设定轨迹的约束条件；③将follower无人机的轨迹规划问题转换为标准的二次规划问题，并在时间内实现对该问题的求解得到最优轨迹；④通过无线网络将轨迹发送到follower无人机。

室内小型四旋翼无人机编队演示验证平台工作原理为：首先通过室内定位系统经过标定构建出室内xyz三维坐标系，通过红外摄像头对各个无人机上的标记点进行捕捉，通过交换机将标记点信息传递给动作捕捉计算机；动作捕捉计算机的运动捕捉系统配套软件根据标记点信息实时解算出每个无人机在当前室内三维坐标系下的空间坐标和自身姿态，并通过数据打包通过UDP/IP协议以多播的形式发送给地面计算机；地面计算机端由两台计算机组成，一台计算机运行地面站控制软件，另一台计算机运行轨迹规划软件；地面控制软件在Linux环境下基于ROS操作系统进行开发，整套软件由两个主要进程组成，定位数据处理进程基于NatNet SDK接收来自动作捕捉系统的无人机位姿信息并经过坐标变换得到适用于无人机定位的数据，再通过MAVLink协议对数据进行打包，并通过Wi-Fi无线网络发送至无人机端，无人机控制进程通过终端获取用户指令，再基于MAVLink协议对指令打包通过无线网络发送至无人机；轨迹规划软件基于MATLAB/Simulink仿真软件进行开发，运行轨迹规划算法，获取每架无人机当前的运动状态并计算出合理的飞行，再通过无线网络将轨迹信息发送至无人机端；无人机端机载计算机负责接收地面控制软件的定位信息和控制指令以及来自轨迹规划软件的轨迹信息，并负责与轨迹规划计算机的系统时间进行同步，根据系统时间和轨迹信息计算当前时刻无人机的期望运动状态并发送给飞控，无人机根据自身运动状态和期望运动状态，通过控制器算法驱使无人机按照轨迹飞行，最终实现无人机编队飞行。

(二)轨迹控制设计方案

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

步骤1：设计带有前馈作用的串级PID控制器，包括：

(1)确定四旋翼无人机动力学模型

设计右手惯性坐标系定义惯性坐标为W＝[x_W,y_W,z_W]，惯性坐标系中无人机机体坐标系设定为B＝[x_B,y_B,z_B],则无人机在空间中的位置坐标表示为r＝[x,y,z]∈R³,无人机在惯性坐标系下的姿态表示为R∈SO(3)，姿态角速率表示为ω＝[ω₁ ω₂ ω₃]^T∈R³,ω₁,ω₂,ω₃分别表示滚转角速率、俯仰角速率和偏航角速率，定义映射关系f如下：

该映射关系的逆映射表示为f^-1，则无人机的运动学方程可以表示为：

其中，m表示无人机的质量，J∈R³表示无人机的转动惯量，g表示重力加速度，控制量τ＝[τ₁ τ₂ τ₃ τ₄]^T与电机转速v＝[v₁ v₂ v₃ v₄]之间的关系如下：

其中，τ₁表示四个旋翼产生的总升力，τ₂,τ₃,τ₄分别表示滚转、俯仰和偏航通道的控制力矩，v_i表示旋翼i的旋转速度，L表示旋翼转动中心到无人机中心的距离，k_F,k_M分别表示旋翼的升力系数和扭矩系数；

(2)机载控制器控制算法设计

本发明提出的机载控制器控制算法设计思路主要包括三个部分：①轨迹跟踪控制器设计：结合轨迹规划算法得出的期望运动状态，设计比例-前馈位置状态跟踪控制器得到期望速度控制量；再设计带有前馈作用的PID控制器得到期望加速度控制量，轨迹跟踪控制器保证对参考轨迹的有效跟踪，并提供给姿态解算模块可行的虚拟控制指令；②姿态解算：基于四旋翼无人机自身的飞行特性，结合李集群思想构建外环虚拟控制指令与内外期望姿态指令的解析关系，确保得到精准的期望姿态解，并保证后续的姿态跟踪控制器在对期望姿态跟踪的同时，达到间接跟踪参考轨迹的目的；③姿态跟踪控制器设计：基于SO(3)理论设计姿态误差的求解方法，针对姿态误差设计比例-微分控制器得到期望角速率控制量，再结合角速率状态误差设计PID控制器，保证对期望姿态状态的有效跟踪。各个部分的具体实现过程如下：

①轨迹跟踪控制器设计：采用经典内外环控制结构，外环为位置控制环，通过轨迹规划算法获取当前时刻的期望运动状态r_d＝[x_d,y_d,z_d]^T，依据期望位置和位置状态r的误差e_p，设计比例-前馈控制器得到期望速度控制量v_des。

e_p＝r_d-r

再利用期望速度控制量和速度状态量

的误差e_v，设计带有前馈作用的PID控制器，得到期望加速度控制量acc_des。

②姿态解算：根据期望加速度求取总升力控制器τ₁和期望姿态下体坐标系z轴向量z_B,des。

通过z_B,des和从轨迹规划中得到的期望偏航角ψ_d，结合李集群思想可以获取旋转矩阵形式的期望姿态R_d,表达式如下：

x_B,des＝y_B,des×z_B,des

R_d＝[x_B,des y_B,des z_B,des]

③姿态跟踪控制器设计：根据期望姿态和当前无人机姿态状态R，基于SO(3)理论求取姿态误差e_R。依据该误差设计比例-微分控制器得到期望角速率控制量ω_d。

定义角速率误差e_ω，并基于该误差设计比例-微分-积分控制器，用以计算得到控制量[τ₂ τ₃ τ₄]^T。

e_ω＝ω_d-ω

步骤2：基于微分平坦的leader无人机飞行轨迹规划

所描述的基于微分平坦的轨迹规划方法是：将leader无人机的期望飞行轨迹描述成跟时间t相关的多项式函数表达σ(t)，形式如下：

由此，轨迹的输出

其中

表示无人机当前在空间中第i个路径点的期望位置，

表示无人机第i个路径点的偏航角，轨迹考虑四旋翼无人机的动态特性和轨迹的输入满足微分平坦的判定条件，根据微分平坦理论对问题进行了转化，并将轨迹的约束条件映射到了平坦输出空间中，考虑对r进行4次微分得到控制量τ₂,τ₃,τ₄的线性表达形式，因此一架无人机在室内空间中运动的轨迹规划问题，转化为如下标准二次规划问题的形式：

s.t.σ(t_i)＝σ_i,i＝0,…,m

其中μ_r,μ_ψ分别为二次规划求解器中参数，通过该问题的求解得到最优的leader无人机飞行轨迹；

步骤3：利用一致性方法对follower无人机的飞行轨迹进行规划；

考虑follower无人机j与leader无人机i构成leader-follower的编队关系，无人机之间保持的相对关系通过s_i,j＝σ_i(t)-σ_j(t)表示，则leader无人机与follower无人机之间的误差e(t)可以定义为e(t)＝x_i(t)-x_j(t)-s_i,j(t)，将无人机编队控制问题转换成如下形式：

s.t.x_j(t₀)＝t₀时刻无人机j的状态

时刻无人机j的状态

其中k_k为二次规划求解器中参数，该优化问题为标准二次规划形式，通过对该问题的求解得到最优的follower无人机在t＝[t₀,t_f]时间段的飞行轨迹。整个leader与follower无人机的编队飞行过程，就是在保证每次二次规划问题的求解时间小于[t₀,t_f]的同时，实时不断循环地计算follower无人机的飞行轨迹。

(三)具体实例分析

①小型四旋翼无人机：本实例采用轴距140mm，边距90mm，3D打印技术一次打印成型的x字型机架、4000kv无刷电机、3寸3叶螺旋桨组装小型四旋翼无人机。总重量为150克，最大负载50克，满载飞行时间最长6分钟，飞行控制器采用Pixhawk开源飞控，机载处理器采用Raspberry Pi zero-w开发板。

②地面控制系统：本实例地面控制系统由两台台式机，一台笔记本计算机组成。动作捕捉计算机为台式机，配置了因特尔酷睿i7处理器，主频是3.6GHz，内存为8GB，运行运动捕捉系统软件，实时捕捉多台四旋翼无人机的位姿信息；地面控制计算机为台式机，配置了i3处理器，主频是2GHz，内存为4GB，运行地面控制系统软件，为所有无人机发送控制指令和飞行模式指令；轨迹规划计算机为笔记本，配置了i5处理器，主频为2.3GHz，内存为6GB，运行轨迹规划软件，为每架无人机规划并发送编队飞行轨迹。发明相关软件基于C/C++、Python和MATLAB语言编写。

③实验结果分析：本实例在室内进行了两架编队飞行控制实验，室内飞行空间为2m×2m×1.8m的立方体空间，图6，轨迹规划效果图给出了有轨迹规划计算机生成的轨迹变化曲线，其中，虚线表示follower无人机的跟踪轨迹曲线，实线表示leader无人机的跟踪轨迹曲线，

表示leader无人机必须经过的路径点。图7给出了两架无人机编队的实际飞行效果，红色圈中为leader无人机，蓝色圈中为follower无人机，leader无人机按照预定飞行轨迹进行跟踪飞行，follower无人机根据leader无人机的飞行轨迹和自身飞行状态对自身的飞行轨迹进行规划并跟踪，图中记录了从开始编队飞行时刻开始第0-6秒两架无人机的飞行过程，可以看出leader无人机与follower无人机可以在编队过程中保持一定的编队构型完成编队演示。

Claims (3)

1.一种轨迹控制室内四旋翼无人机编队演示验证平台，其特征是，由无人机端、地面计算机端和室内动作捕捉系统组成，结合分布式编队结构，通过无线网络获取临近无人机编队飞行轨迹和飞行状态，考虑四旋翼无人机在实际飞行环境下的动力学模型，基于微分平坦理论设计四旋翼无人机实时轨迹规划方法，并配合室内动作捕捉系统，实现室内多架四旋翼无人机编队飞行演示验证；

其中，无人机端包括多架四旋翼无人机，每架四旋翼无人机包含机械部分、动力部分和电子部分，其中机械部分即无人机的机架，动力部分包括电池、电机、电机驱动、桨叶，电子部分主要包括机载控制器、机载计算机和遥控器接收器模块；无人机底板安置电池、遥控器接收器模块，无人机中间机架板安置4个电机、4个电调和Pixhawk飞行控制器，无人机顶板用于安置机载计算机和定位系统标记点；机载控制器内置三轴陀螺仪、三轴加速度计以及三轴磁阻计，无人机在室内飞行，依靠动作捕捉系统提供位置信息，机载控制器采用串级PID加前馈控制的位置环控制器结合无人机自身运动状态和期望运动状态实现轨迹跟踪；机载计算机基于MAVLink协议通过串口与机载控制器进行数据交互，获取当前无人机的运动状态，并将无人机的运动状态发布在Wi-Fi无线网络中，同时该机载计算机通过linux系统中的ntpdate工具与计算机进行系统时间同步，并接受由地面计算机规划好的飞行轨迹，基于该飞行轨迹和当前系统时间计算无人机的期望运动状态，最后通过串口将该期望运动状态发送至机载控制器中用于轨迹跟踪控制；

带有前馈作用的串级PID控制器中：

(1)确定四旋翼无人机动力学模型

右手惯性坐标系定义惯性坐标为W＝[x_w，y_w，z_w]，惯性坐标系中无人机机体坐标系设定为B＝[x_B，y_B，z_B]，则无人机在空间中的位置坐标表示为r＝[x，y，z]∈R³，无人机在惯性坐标系下的姿态表示为R∈SO(3)，姿态角速率表示为ω＝[ω₁ω₂ω₃]^T∈R³，ω₁，ω₂，ω₃分别表示滚转角速率、俯仰角速率和偏航角速率，定义映射关系f如下：

该映射关系的逆映射表示为f^-1，则无人机的运动学方程可以表示为：

其中，m表示无人机的质量，J∈R³表示无人机的转动惯量，g表示重力加速度，控制量τ＝[τ₁ τ₂ τ₃ τ₄]^T与电机转速v＝[v₁ v₂ v₃ v₄]之间的关系如下：

其中，τ₁表示四个旋翼产生的总升力，τ₂，τ₃，τ₄分别表示滚转、俯仰和偏航通道的控制力矩，v_i表示旋翼i的旋转速度，L表示旋翼转动中心到无人机中心的距离，k_F，k_M分别表示旋翼的升力系数和扭矩系数；

(2)机载控制器控制中完成如下工作：

①轨迹跟踪控制器：采用经典内外环控制结构，外环为位置控制环，通过轨迹规划算法获取当前时刻的期望运动状态r_d＝[x_d，y_d，z_d]^T，依据期望位置和位置坐标r的误差e_p，设计比例-前馈控制器得到期望速度控制量v_des；

e_p＝r_d-r

再利用期望速度控制量和速度状态量

的误差e_v，设计带有前馈作用的PID控制器，得到期望加速度控制量acc_des；

②姿态解算：根据期望加速度求取总升力控制器τ₁和期望姿态下体坐标系z轴向量z_B，des：

τ₁＝aCC_des·z_B，

通过z_B，des和从轨迹规划中得到的期望偏航角ψ_d，结合李集群思想可以获取旋转矩阵形式的期望姿态R_d，表达式如下：

x_B，des＝y_B，des×z_B，des

R_d＝[x_B，des y_B，des z_B，des]

③姿态跟踪控制器：根据期望姿态和当前无人机姿态状态R，基于SO(3)理论求取姿态误差e_R，依据该误差设计比例-微分控制器得到期望角速率控制量ω_d；

定义角速率误差e_ω，并基于该误差设计比例-微分-积分控制器，用以计算得到控制量[τ₂ τ₃ τ₄]^T；

e_ω＝ω_d-ω

所述轨迹规划算法包括基于微分平坦的leader无人机飞行轨迹规划

基于微分平坦的轨迹规划方法是：将leader无人机的期望飞行轨迹描述成跟时间t相关的多项式函数表达σ(t)，形式如下：

由此，轨迹的输出

其中r_id表示无人机当前在空间中第i个路径点的期望位置，

表示无人机第i个路径点的偏航角，轨迹考虑四旋翼无人机的动态特性和轨迹的输入满足微分平坦的判定条件，根据微分平坦理论对问题进行了转化，并将轨迹的约束条件映射到了平坦输出空间中，考虑对r进行4次微分得到控制量u₂，u₃，u₄的线性表达形式，因此一架无人机在室内空间中运动的轨迹规划问题，转化为如下形式：

s.t.σ(t_i)＝σ_i，i＝0，...，m

其中μ_r，μ_ψ分别为二次规划求解器中参数，通过该问题的求解得到最优的leader无人机飞行轨迹；

所述轨迹规划算法还包括利用一致性方法对follower无人机的飞行轨迹进行规划；

考虑follower无人机j与leader无人机i构成leader-follower的编队关系，无人机之间保持的相对关系通过s_i，j＝σ_i(t)-σ_j(t)表示，则leader无人机与follower无人机之间的误差e(t)可以定义为e(t)＝x_i(t)-x_j(t)-s_i，j(t)，将无人机编队控制问题转换成如下形式：

s.t.x_j(t₀)＝t₀时刻无人机j的状态

时刻无人机j的状态

其中k_k为二次规划求解器中参数，优化问题为标准二次规划形式，通过对该优化问题的求解得到最优的follower无人机在t＝[t₀，t_f]时间段的飞行轨迹，整个leader与follower无人机的编队飞行过程，就是在保证每次二次规划问题的求解时间小于[t₀，t_f]的同时，实时不断循环地计算follower无人机的飞行轨迹。

2.如权利要求1所述的轨迹控制室内四旋翼无人机编队演示验证平台，其特征是，地面计算机端由两台计算机组成，一台计算机负责接受动作捕捉系统的数据并通过Wi-Fi无线网络转发给所有无人机；对每架无人机飞行模式和飞行状态进行管控；另一台计算机负责通过无线网络接受所有无人机的运动状态；作为系统时间服务器与每架无人机的系统时间进行同步；基于MATLAB/Simulink软件仿真环境结合每架无人机的运动状态进行实时轨迹规划，再将规划好的飞行轨迹发送至每架无人机。

3.一种轨迹控制室内四旋翼无人机编队演示验证方法，其特征是，首先通过室内定位系统经过标定构建出室内xyz三维坐标系，通过红外摄像头对各个无人机上的标记点进行捕捉，通过交换机将标记点信息传递给动作捕捉计算机；动作捕捉计算机的运动捕捉系统配套软件根据标记点信息实时解算出每个无人机在当前室内三维坐标系下的空间坐标和自身姿态，并通过数据打包通过UDP/IP协议以多播的形式发送给地面计算机；地面计算机端由两台计算机组成，一台计算机运行地面站控制软件，另一台计算机运行轨迹规划软件；地面站控制软件在Linux环境下基于ROS操作系统进行开发，整套软件由两个进程组成，定位数据处理进程基于NatNet SDK接收来自动作捕捉系统的无人机位姿信息并经过坐标变换得到适用于无人机定位的数据，再通过MAVLink协议对数据进行打包，并通过Wi-Fi无线网络发送至无人机端，无人机控制进程通过终端获取用户指令，再基于MAVLink协议对指令打包通过无线网络发送至无人机；轨迹规划软件基于MATLAB/Simulink仿真软件进行开发，运行轨迹规划算法，获取每架无人机当前的运动状态并计算出合理的飞行，再通过无线网络将轨迹信息发送至无人机端；无人机端机载计算机负责接收地面站控制软件的定位信息和控制指令以及来自轨迹规划软件的轨迹信息，并负责与轨迹规划计算机的系统时间进行同步，根据系统时间和轨迹信息计算当前时刻无人机的期望运动状态并发送给飞控，无人机根据自身运动状态和期望运动状态，通过控制器算法驱使无人机按照轨迹飞行，最终实现无人机编队飞行；进一步细化的具体步骤是：

步骤1：设计带有前馈作用的串级PID控制器，包括：

(1)确定四旋翼无人机动力学模型

设计右手惯性坐标系定义惯性坐标为W＝[x_W，y_W，z_W]，惯性坐标系中无人机机体坐标系设定为B＝[x_B，y_B，z_B]，则无人机在空间中的位置坐标表示为r＝[x，y，z]∈R³，无人机在惯性坐标系下的姿态表示为R∈SO(3)，姿态角速率表示为ω＝[ω₁ ω₂ ω₃]^T∈R³，ω₁，ω₂，ω₃分别表示滚转角速率、俯仰角速率和偏航角速率，定义映射关系f如下：

该映射关系的逆映射表示为f^-1，则无人机的运动学方程可以表示为：

其中，m表示无人机的质量，J∈R³表示无人机的转动惯量，g表示重力加速度，控制量τ＝[τ₁ τ₂ τ₃ τ₄]^T与电机转速v＝[v₁ v₂ v₃ v₄]之间的关系如下：

其中，τ₁表示四个旋翼产生的总升力，τ₂，τ₃，τ₄分别表示滚转、俯仰和偏航通道的控制力矩，v_i表示旋翼i的旋转速度，L表示旋翼转动中心到无人机中心的距离，k_F，k_M分别表示旋翼的升力系数和扭矩系数；

(2)机载控制器控制算法设计

①轨迹跟踪控制器设计：采用经典内外环控制结构，外环为位置控制环，通过轨迹规划算法获取当前时刻的期望运动状态r_d＝[x_d，y_d，z_d]^T，依据期望位置和位置坐标r的误差e_p，设计比例-前馈控制器得到期望速度控制量v_des；

e_p＝r_d-r

再利用期望速度控制量和速度状态量

的误差e_v，设计带有前馈作用的PID控制器，得到期望加速度控制量acc_des；

②姿态解算：根据期望加速度求取总升力控制器τ₁和期望姿态下体坐标系z轴向量z_B，des：

τ₁＝acc_des·z_B，

通过z_B，des和从轨迹规划中得到的期望偏航角ψ_d，结合李集群思想可以获取旋转矩阵形式的期望姿态R_d，表达式如下：

x_B，des＝y_B，des×z_B，des

R_d＝[x_B，des y_B，des z_B，des]

③姿态跟踪控制器设计：根据期望姿态和当前无人机姿态状态R，基于SO(3)理论求取姿态误差e_R，依据该误差设计比例-微分控制器得到期望角速率控制量ω_d；

定义角速率误差e_ω，并基于该误差设计比例-微分-积分控制器，用以计算得到控制量[τ₂ τ₃ τ₄]^T；

e_ω＝ω_d-ω

步骤2：基于微分平坦的leader无人机飞行轨迹规划

基于微分平坦的轨迹规划方法是：将leader无人机的期望飞行轨迹描述成跟时间t相关的多项式函数表达σ(t)，形式如下：

由此，轨迹的输出

其中

表示无人机当前在空间中第i个路径点的期望位置，

表示无人机第i个路径点的偏航角，轨迹考虑四旋翼无人机的动态特性和轨迹的输入满足微分平坦的判定条件，根据微分平坦理论对问题进行了转化，并将轨迹的约束条件映射到了平坦输出空间中，考虑对r进行4次微分得到控制量u₂，u₃，u₄的线性表达形式，因此一架无人机在室内空间中运动的轨迹规划问题，转化为如下形式：

s.t.σ(t_i)＝σ_i，i＝0，...，m

其中μ_r，μ_ψ分别为二次规划求解器中参数，通过该问题的求解得到最优的leader无人机飞行轨迹；

步骤3：利用一致性方法对follower无人机的飞行轨迹进行规划；

考虑follower无人机j与leader无人机i构成leader-follower的编队关系，无人机之间保持的相对关系通过s_i，j＝σ_i(t)-σ_j(t)表示，则leader无人机与follower无人机之间的误差e(t)可以定义为e(t)＝x_i(t)-x_j(t)-s_i，j(t)，将无人机编队控制问题转换成如下形式：

s.t.x_j(t₀)＝t₀时刻无人机j的状态

时刻无人机j的状态

其中k_k为二次规划求解器中参数，优化问题为标准二次规划形式，通过对该优化问题的求解得到最优的follower无人机在t＝[t₀，t_f]时间段的飞行轨迹，整个leader与follower无人机的编队飞行过程，就是在保证每次二次规划问题的求解时间小于[t₀，t_f]的同时，实时不断循环地计算follower无人机的飞行轨迹。

Images