CN104718508A - 四旋翼机组的三维操纵 - Google Patents

Abstract

描述了一种用于朝向目标位置控制至少两个飞行器的飞行轨道的系统和方法。该系统包括：至少两个飞行器，具有机载惯性测量单元，用于确定和更新至少两个飞行器的方位、角速度、位置和线速度；运动捕捉系统，用于检测至少两个飞行器中的每个的当前位置和速度；以及基础控制器，所述基础控制器与运动捕捉系统通信，并且与多个飞行器通信。基础控制器对于每个飞行器使用分段多项式平滑函数、应用加权因数以及实施重叠约束以预定时间间隔计算最佳轨道路径。

Description

四旋翼机组的三维操纵

相关申请的交叉引用

本申请要求于2012年4月30日提交的美国临时专利No.61/640,249的权益。该申请的内容通过引用结合于此。

技术领域

本发明在多旋翼飞行器领域中。更特别地，本发明涉及通过使用混合整数二次规划(MIQP)并且使用整数约束生成最佳轨道，以实施碰撞避免的具有复杂障碍物的四旋翼机组的三维操纵。

背景技术

微型空中机器人、尺寸小于1米并且质量是1kg或更小的自动飞行器在过去十年经历了快速进步。有翼航行器可以从固定翼飞行器到扑翼飞行器，后者主要通过昆虫飞行获得灵感。包括直升飞机、同轴旋翼飞机、涵道风扇、四旋翼和六旋翼的旋翼飞机已被证实更加成熟，四旋翼在机器人研究实验室中最广泛地用于载人平台。在该类设备中，由Ascending Technologies GmbH出售的Hummingbird(蜂鸟)四旋翼是非常有能力并且健壮的平台，其具有55cm的端到端翼展、8cm的高度、包括锂聚合物电池的约500克的质量，并且消耗约75瓦特。

多旋翼飞行器已经变为日益流行的机器人平台，这是因为它们的机械简单性、动力学能力、以及对室内和室外环境的适用性。特别是，在设计、控制和计划方面存在用于四旋翼(具有四个旋翼的旋翼飞机)的很多最新发展。如以下将解释的，本发明涉及用于在具有障碍物的环境中生成用于像图1中所示的那些的异质四旋翼机组的最佳轨道的方法。

微型空中机器人具有在很多实际应用中很难克服的有效载荷限制。然而，较大有效载荷可以被操纵，并且通过使用机械爪或电缆的多个UAV传送。诸如要求大区域覆盖或者从多个传感器成像的监视或搜索和救援的应用可以通过协调多个UAV来解决，每个都具有不同传感器。

四旋翼可以快速地和准确地遵循的轨道应该继续达到位置的三阶导数(或C3)。这是因为，对于四旋翼来说，横向加速度的不连续要求翻转和螺旋角的瞬时改变，并且横向摆动的不连续要求角速度的瞬间改变。找到C3轨道要求高维度搜索空间的计划，其对于使用可达性算法的方法、增量搜索技术或基于LQR树的搜索是不切实际的。当计划用于多个飞行器时，问题恶化，这是因为这进一步扩展搜索空间的尺寸。

本发明解决缩减四旋翼平台的问题，以开发真正小的微型UAV。尺寸缩减的最重要和明显益处是四旋翼以紧密编队在紧密约束环境下操作的能力。虽然四旋翼的有效载荷能力明显下降，但是可以部署协作以克服该限制的多个四旋翼。再次，小尺寸是有益的，这是因为较小飞行器可以比大飞行器以更接近近程操作。缩减的另一个感兴趣益处是敏捷。较小四旋翼呈现允许对干扰的更快适应的更高加速度和更高稳定性。

发明人的在先工作显示，用于四旋翼的动力学模型是微分平整的。发明人使用该事实，以得出轨道生成算法，其允许将约束自然地嵌入期望位置、速度、加速度、摆动和输入，同时满足对轨道的平滑度的要求。发明人延伸根据本发明的该方法，以包括多个四旋翼和障碍物。该方法允许不同四旋翼之间的不同尺寸、能力、以及变化动力学效果。发明人使用整数约束实施碰撞避免，整数约束将它们的二次规划(QP)变换为混合整数二次规划(MIQP)。

本发明的在先工作还从广泛文献提取混合整数线性规划(MILP)以及到轨道计划的应用，Schouwenaars等人的“DecentralizedCooperative Trajectory Planning of Multiple Aircraft with Hard SafetyGuarantees”，Proceedings of the AIAA Gauidance，Navigation，以及Control Conference and Exhibit，Providence，RI，2004年8月。在此描述的方法依赖这样的工作。

发明内容

在此描述的方法和系统表示用于固定翼飞行器和旋翼飞机的类似轨道计划问题的整数约束的权力和灵活性。该方法中的关键差异在于，使用分段平滑多项式函数合成平坦输出空间中的轨道。使用分段平滑多项式函数允许实施航路点之间的连续性达到位置的任何期望导数。在此描述的轨道生成的另一个差异是使用二次成本函数，得到与MILP相反的MIOP。

在示例性实施例中，描述一种用于朝向目标位置控制至少两个飞行器的飞行轨道的系统。该系统包括：至少两个飞行器，所述至少两个飞行器具有机载惯性测量单元，用于确定并且更新至少两个飞行器的方位、角速度、位置和线速度；运动捕捉系统，所述运动捕捉系统用于检测至少两个飞行器中的每个的当前位置和速度；基础控制器，所述基础控制器与运动捕捉系统通信并且与多个飞行器通信，基础控制器对于每个飞行器，使用分段平滑多项式函数，应用加权因数，并且实施重叠约束，以预定时间间隔计算最佳轨道路径。基于所计算的最佳轨道路径，基础控制器还将命令发送至每个飞行器，以单独控制它们的状态，使得至少两个飞行器遵循所计算的最佳轨道路径，同时避免碰撞。

本发明还包括一种用于朝向目标位置和方位控制至少两个飞行器的状态的轨道生成方法。在示例性实施例中，该方法包括：确定飞行器的方位和角速度；通过控制飞行器的至少一个电机，控制飞行器的方位和角速度；确定每个飞行器的当前位置和速度；通过指定期望方位和角速度以及至少一个电机所要求的净推力，控制每个飞行器的位置和速度。对于每个飞行器，通过使用分段平滑多项式函数，以及应用加权因数，并且实施重叠约束，以预定时间间隔计算最佳轨道路径。然后，基于所计算的最佳轨道路径，将命令发送至每个飞行器，以单独控制它们的状态，使得这样的飞行器遵循所计算的最佳轨道路径，同时避免碰撞。提供每个飞行器的更新后的当前位置和速度，并且以多个预定时间间隔迭代地执行该过程。在示例性实施例中，飞行器的每个状态都包括其方位和角速度、以及位置和线速度。可以在每个飞行器上估计方位误差和控制方位。

在最佳轨道路径的计算期间，应用至至少两个飞行器中的每个的加权因数对于不同飞行器可以不同。而且，整数约束可以在最佳轨道路径的计算期间被使用，以实施与障碍物和其他飞行器的碰撞约束，并且最优地分配用于至少两个飞行器的目标位置。计算用于每个飞行器的最佳轨道路径还可以包括：生成在指定时间t_w处平滑经过n_w个期望航路点的轨道，同时根据以下等式最小化用于n_q个四旋翼的位置的k_r阶导数的平方的积分：

其中，rTq＝[xTq,yTq,zTq]表示用于四旋翼q的轨道，并且r_wq表示用于四旋翼q的期望航路点。还可以通过将飞行器建模为具有边长I_x、I_y和I_z的通过通用坐标系定向的矩形棱柱体，提供至少两个飞行器之间的碰撞避免，其中，边长I_x、I_y和I_z足够大，使得飞机可以翻转、倾斜和侧滑到任何角度并且待在棱柱体内。然后，棱柱体可以航行通过具有no个凸起障碍物的环境，其中，每个凸起障碍物o由具有nf(o)个面的构形空间中的凸起区域表示，并且对于每个面f，在时间tk处的飞行器的期望位置rTq(tk)在障碍物o外侧的条件被表示为：

n_of·rT_q(tk)≤s_of

其中，n_of是到构形空间中的障碍物的面f的法向矢量，并且s_of是标量，由此，如果用于在时间tk处的飞行器的位置的等式被满足用于至少一个面，则矩形棱柱体并且因此飞机不与障碍物碰撞。飞机q在时间tk处不与障碍物o碰撞的条件可以通过二进制变量b_qofk进一步被实施为：

其中，M是大正数。

在示例性实施例中，后者等式被代入前者等式，用于在航路点之间的n_k个中间时间步幅处的所有障碍物的所有n_q个飞机。当通过在航路点之间的n_k个中间时间步幅处实施约束，在飞行器的轨道路径上的航路点之间转换时，飞行器可以相互之间保持安全距离，其可以由以下集合约束在数学上被表示用于飞行器1和2：

${\∀t}_k:x_{T1}\left(t_k\right)-x_{T2}\left(t_k\right)\≤d_{x12}$

或x_T2(t_k)-x_T1(t_k)≤d_x21

或y_T1(t_k)-y_T2(t_k)≤d_y12

或y_T2(t_k)-y_T1(t_k)≤d_y21

其中，d项表示飞行器1和2之间的安全距离。当飞行器轴向对称时，d_x12＝d_x21＝d_y12＝d_y21。

在进一步示例性实施例中，使用整数约束，以通过应用以下整数约束用于每个四旋翼q和目标g，找到用于飞行器的最佳目标分配：

$x_{\mathrm{Tq}}\left(t_{n_w}\right)\≤x_g+{\mathrm{M\β}}_{\mathrm{qg}}$

$x_{\mathrm{Tq}}\left(t_{n_w}\right)\≥x_g-{\mathrm{M\β}}_{\mathrm{qg}}$

$y_{\mathrm{Tq}}\left(t_{n_w}\right)\≥y_g+{\mathrm{M\β}}_{\mathrm{qg}}$

$y_{\mathrm{Tq}}\left(t_{n_w}\right)\≥y_g-{\mathrm{M\β}}_{\mathrm{qg}}$

$z_{\mathrm{Tq}}\left(t_{n_w}\right)\≤z_g+{\mathrm{M\β}}_{\mathrm{qg}}$

$z_{\mathrm{Tq}}\left(t_{n_w}\right)\≥z_g-{\mathrm{M\β}}_{\mathrm{qg}}$

其中，β_qg是用于实施最佳目标分配的二进制变量。可以进一步应用以下约束以保证至少n_g四旋翼达到期望目标位置：

$\underset{q=1}{\overset{n_q}{\mathrm{\Σ}}}\underset{g=1}{\overset{n_g}{\mathrm{\Σ}}}{\mathrm{\β}}_{\mathrm{qg}}\≤n_g{n}_q-n_g.$

在本发明的方法的其他示例性实施例中，该方法包括以下步骤：将飞行器组织成多个组，其中，多个组中的每个组被单独协调；以及生成用于多个组中的每个组的轨道，以对目标位置分组。在这样的实施例中，用于飞行器的环境被划分为nr个凸起子区域，其中，每个子区域包含相同数量的飞行器起始和目标位置，并且单独轨道被生成用于每个子区域内的飞行器，由此使用对飞行器的位置的线性约束，要求飞行器待在它们自己的子区域内。

附图说明

本发明的以上和其他特征和优点将从附图的以下详细说明变得明显，在附图中：

图1图示20个微型四旋翼在飞行时的编队。

图2图示微型四旋翼的示例性实施例。

图3图示高度控制器性能数据，其中，(a)示出螺旋角步幅输入响应，并且(b)示出用于空翻演习的数据。

图4(a)图示(a)盘旋时的x、y、z误差，并且(b)用于x(顶部)和z(底部)中的位置控制器的步幅输入响应。

图5图示参考坐标系和螺旋桨编号惯例。

图6图示被组织为m个组的四旋翼的机组。

图7示出用于多组四旋翼的软件架构。

图8图示被列出用于4个四旋翼轨道的编队，其中，(a)图示用于四个飞行器中的每个的期望轨道和实际轨道，并且在(b)中示出用于每个四旋翼的编队误差。

图9在(a)图示用于为栅格编队的20个四旋翼的x、y、和z的平均标准偏差，并且(b)图示遵循图形八图案的16个四旋翼。

图10图示飞过窗口的四组四个四旋翼。

图11图示从面栅格转换到三维螺旋(顶部)和金字塔(底部)的十六个飞行器的机组。

图12图示kQuad65(顶部)、Asctec Hummingbird(中间)、以及kQuad1000(底部)四旋翼的飞行。

图13图示参考坐标系和螺旋桨编号惯例。

图14示出用于航行于具有四个障碍物的环境的单个四旋翼的轨道，其中，障碍物是实心框，轨道被示出为实线，在实施碰撞检验的nk个中间时间步幅处的四旋翼的位置通过随着时间的经过变得更黑的较亮阴影框示出，并且其中，(a)示出当不实施时间步幅重叠约束时的轨道，并且(b)示出当实施时间步幅重叠约束时的轨道。

图15在(a)图示以多种速度用于30次试验的RMSE，并且(b)-(d)图示用于单次运行的数据((a)中的框中数据)，其中，框表示与图16中的快照相对应的在实验期间在指定时间处的四旋翼位置，并且实线表示用于该次运行的实际四旋翼轨道，而虚线表示期望轨道。

图16图示环表示间隙的三个四旋翼实验的快照。

图17在(a)图示以多个速度用于11次试验的RMSE，并且(b)-(d)图示用于单次运行的数据(在(a)中的框中数据)，其中，框表示与图18中的快照相对应的在实验期间在指定时间处的四旋翼位置，并且实线表示用于该次运行的实际四旋翼轨道，而虚线表示期望轨道。

图18图示利用kQuad1000四旋翼和AscTec Hummingbird四旋翼的实验的快照，其中，环表示水平间隙。

图19图示用于同质(a)和异质(b)四旋翼机组的编队重新配置的轨道，其中，框表示在轨道期间的中间时间处的四旋翼位置，并且实线表示实际四旋翼轨道，而虚线表示期望轨道。

图20图示在开始(顶部)、中间时间(中间)、以及最后时间(底部)处的线编队内的四个四旋翼转换的快照。

具体实施方式

关于图1至图20在以下说明书中阐述特定特殊详情，以提供本发明的多种实施例的彻底理解。然而，特定众所周知的详情不在以下公开中阐述，以避免不必要地模糊本发明的多种实施例。本领域普通技术人员将理解，他们可以在没有以下描述的一个或多个详情的情况下，实现本发明的其他实施例。而且，虽然参考以下公开中的步骤和顺序描述了多种方法，但是说明书旨在提供本发明的实施例的清楚实现，并且步骤和步骤的顺序不应该被认为要求实现本发明。

微型四旋翼

开发简单物理模型以分析四旋翼从盘旋状态产生线性和角加速度的能力是有用的。如果特征长度是L，则旋翼半径R通过L线性地缩放(scale)。质量缩放为L3，并且惯性力矩缩放为L5。另一方面，来自旋翼的升高或推力F和拖拽D与横截面面积和叶片尖速度v的平方成比例。如果叶片的角速度由ω＝v/L，F～ω²L⁴和D～ω²L⁴限定，则线性加速度a缩放为a～ω²L⁴/L³＝ω²L。来自旋翼的推力通过力矩臂L产生力矩。从而，角加速度a～ω²L⁵/L⁵＝ω²。

旋翼速度ω还与长度成比例，这是因为较小电机产生较小转矩，由于还以与升高相同的方式缩放的拖拽阻抗，其限制它们的峰值速度。存在两种普通接受方法，以研究飞行器中的缩放。马赫(Mach)缩放被用于可压缩流，并且实质上假设尖端速度是恒定的，导致ω～1/R。弗劳德(Froude)缩放被用于不可压缩流，并且假设对于类似航行器结构，弗劳德数v²/L_g恒定。在此，g是由于重力导致的加速度。这产生ω～1/(R)^1/2。然而，弗劳德或马赫数相似不考虑电机特征也不考虑电池特性。虽然电机转矩随着长度增加，但是通过使电机的转矩速度特征与螺旋桨的拖拽比速度特征匹配，确定用于旋翼的操作速度。而且，电机转矩取决于电池发起所要求的电流的能力。所有这些变量都被紧紧耦合用于较小设计，这是因为存在在较小长度缩放时可用的较少选择。最后，螺旋桨叶片可以是刚性的假设可能是错误的，并且叶片的性能可以在较小缩放时非常不同，升高与速度的二次缩放可能不准确。然而，这两种情况对于飞行器的可操作性是有意义的。

弗劳德缩放建议加速度独立于长度，同时角加速度α～L^-1。另一方面，马赫缩放导致α～L，同时α～L^-2的结论。由于四旋翼必须旋转(呈现角加速度)以便转化，所以较小四旋翼敏捷得多。存在说明四旋翼结构的两个设计点。来自Ascending Technologies的Pelican四旋翼装配有传感器(约2kg毛重，0.75m直径，以及盘旋时5400rpm标称旋翼速度)，并且消耗约400W的功率。来自Ascending Technologies的不具有附加传感器的Hummingbird四旋翼(550克毛重，约0.5m直径，以及在盘旋时5000rpm标称旋翼速度)消耗约75W。发明人在此概述了一种四旋翼的设计，其尺寸约为Hummingbird的40％、质量为其15％、以及消耗约20％的功率用于盘旋。

图2中示出根据本发明的四旋翼的示例性实施例。其悬臂由夹在底部上的定制电机控制器板和顶部上的主控制器板之间的碳纤维棒制成。为了产生升高，飞行器使用直径为8cm的四个固定螺距螺旋桨。飞行器螺旋桨尖到螺旋桨尖距离是21cm，并且在没有电池的情况下其重量为50克。盘旋时间约为11分钟，具有重量为23克的2-单元400mAh Li-Po电池。

虽然其尺寸小，但是图2的飞行器包含整套机载传感器。以72MHz运行的ARM Cortex-M3处理器用作主处理器。飞行器包含3-轴磁力计、3-轴加速计、用于翻转和俯仰轴的2-轴2000deg/sec速率陀螺仪、以及用于偏航轴的单轴500deg/sec速率陀螺仪。飞行器还包含可以用于感测高度的改变的压力计。为了通信，飞行器包含可以在900MHz或者2.4GHz操作的两个紫蜂(Zigbee)收发器。

Vicon动作捕捉系统被用于以100Hz感测每个飞行器的位置。该数据跨过吉比特以太网网络流到台式基站。在基站上的MATLAB中进行高级控制和计划，基站以100Hz将命令发送至每个四旋翼。以下参考图7描述用于控制四旋翼的大机组的软件。低级估计和控制循环以600Hz的速率在机载微型处理器上运行。

每个四旋翼都具有在900MHz和2.4GHz操作的两个独立无线电收发器。基站经由定制无线电模块将包含方位、推力、角速率和姿态控制器增益的期望命令发送至各个四旋翼。机载速率陀螺仪和加速计被用于估计航行器的方位和角速度。主微型处理器运行以下姿态控制器，并且将期望螺旋桨速度以全速率(600Hz)发送至四个电机控制器中的每个。

图3中图示用于机载姿态控制器的一些性能数据。飞行器的小惯性力矩使得飞行器能够创建大角加速度。如图3(a)中所示，姿态控制被设计成随着小于0.2秒的沉降时间，近似地紧急缓冲。注意，这比用于AscTec Hummingbird的姿态控制的沉降时间快两倍。在图3(b)中呈现用于空翻演习的数据。在此，飞行器完成在约0.4秒内关于其y轴的完全空翻，并且达到1850deg/sec的最大角速度。

以下描述的位置控制器使用滚转角和螺旋角控制飞行器的x和y位置。为此，刚性姿态控制器被要求用于刚性位置控制。在图4(b)中示出对横向和垂直方向上的步幅输入的响应。对于图4(a)中所示的盘旋性能数据，用于x和y的误差的标准偏差约为0.75cm，并且对于z，约为0.2cm。

动力学和控制

用于微型四旋翼的动力学模型和控制基于发明人的先前工作中的方法。如图5中所示，发明人认为体固定坐标系B与四旋翼的主轴(单位矢量bi)对准，并且惯性坐标系A与单位矢量ai对准。B在A中由到质心C的位置矢量r和旋转矩阵R描述。为了避免与参数化相关联的异常，发明人使用全旋转矩阵来描述方位。体坐标系中的四旋翼的角速度ω由给出，其中，^表示矢量的斜对称矩阵形式。

如图5中所示，四个旋翼被编号为1-4，奇数旋翼具有与偶数旋翼对反的斜度。旋翼的角速度是ω_i。所得到的升高F_i和反酌矩M_i由以下给出：

F_i＝k_Fω_i ²，M_i＝k_Mω_i ²

其中，常数k_F和k_M根据经验确定。对于微型四旋翼，电机动力学具有小于10msec的时间常数，并且比刚性体动力学和空气动力学的时间比例快很多。从而，发明人忽略动力学，并且假设F_i和M_i可以瞬间改变。因此，输入到系统的控制u由b3方向上的净推力构成，并且B中的力矩[u₂；u₃；u₄]^T由以下给出：

$u=\left[\begin{array}{cccc}{k}_F& k_F& k_F& k_F\\ 0& k_{F}L& 0& {-k}_{F}L\\ -k_{F}L& 0& k_{F}L& 0\\ k_M& {-k}_M& k_M& -k_M\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_1^2\\ {\mathrm{\ω}}_2^2\\ {\mathrm{\ω}}_3^2\\ {\mathrm{\ω}}_4^2\end{array}\right],---\left(1\right)$

其中，L是从螺旋桨的旋转轴到四旋翼的中心的距离。运动的牛顿-欧拉等式由以下给出：

$m\stackrel{\·\·}{r}={-\mathrm{mga}}_3+u_1{b}_3---\left(2\right)$

$\stackrel{\·}{\mathrm{\ω}}=L^{-1}\left[-\mathrm{\ω}\×\mathrm{L\ω}+\left[\begin{array}{c}{u}_2\\ u_3\\ u_4\end{array}\right]\right]---\left(3\right)$

其中，L是沿着b_i的惯性矩阵的力矩。

发明人使用时间参数化位置矢量和偏航角，指定期望轨道。给出轨道σ(t):控制器基于位置和速度误差推导输入u₁：

u₁＝(-K_pe_p-K_ve_v+mga₃)·b₃  (4)

其中，ep＝r-rT和其他三个输入通过计算期望旋转矩阵确定。发明人想要使推力矢量u₁b₃与等式(4)中的(-K_pe_p-K_ve_v+mga3)一致。发明人还想要偏航角遵循指定偏航T(t)。从这两段信息看，发明人可以根据以下计算R_des和旋转的误差：

其中，∨表示将so(3)的元素带到R³的vee map。期望角速度通过对用于R的表达式微分来计算，并且期望力矩可以被表示为方位误差e_R和角速度误差e_ω的函数：

[u₂，u₃，u₄]^T＝-K_Re_R-K_ωe_ω，  (5)

其中，K_R和K_ω是对角增益矩阵。最后，发明人计算期望旋翼速度，以通过转换等式(1)实现理想u。

用于组的控制和计划

本发明的主要推力是四旋翼的大机组的协调。为了管理从状态空间维度的生长得到的复杂性并且限制从被标记飞行器之间的交互得到的组合激增，发明人考虑机组架构，其中，机组被组织为被标记组，每个组都具有被标记飞行器。在形式上，发明人可以将一组代理限定为同时工作以完成单个任务的代理的集合。两个或更多组在机组中行动，以完成要求完成多个并行子任务的任务。发明人假设组内的飞行器可以通过低等待时间以高数据速率通信，同时用于跨过组的协调的通信要求不太迫切。最重要地，组中的飞行器被标记。小组尺寸允许发明人设计提供对各个敏捷机器人的形状、通信拓扑和相对位置的综合保证的控制器和计划器。

在此的方法与必须充满成百上千个代理的真正分散方法形成对比。虽然无领袖聚合和充满空中机器人的模型在机器人群落中被论述，但是在此，列举在机器人之间的被标记交互的挑战通过控制作为统计分布的这样的编队的聚合描述符被避免。这些方法不能提供对形状或拓扑的保证。相互碰撞避免算法具有使机器人航行至目标目的地，而不保证可用于瞬变性能，并且没有收敛性证明可用的可能。

另一方面，虽然几个实验结果可用于飞机器人，但是设计用于三维刚性结构的轨道跟踪的分散控制器的问题现在被更好地理解。在此的架构允许组中的这样的刚性结构的维护。

编队飞行减少生成用于大机组飞行器的轨道的复杂性，以生成用于单个实体的轨道。如果控制器被很好地设计，则不存在飞行器之间的显式合作碰撞避免。在时间t用于四旋翼q的位置误差可以被编写为：

e_pq(t)＝ef(t)+el_q(t)  (6)

其中，ef(t)是编队误差，其描述组的位置与规定轨道的误差，并且el_q(t)是组的编队内的四旋翼q的局部误差。发明人以下将示出，即使编队误差可能非常大，局部误差通常对于聚合轨道非常小。

编队飞行的主要缺点在于，固定编队(rigid formation)可能仅适合大间隙。这可以通过改变机组的编队的形状或者将机组划分为更小的组来解决，允许每个组独立协商间隙。

减小轨道生成问题的复杂性的另一种方式是，要求所有飞行器遵循相同机组轨道，但是分离某个时间增量。在此，发明人使用于四旋翼q的轨道被限定为：

rT_q(t)＝rT T(t+Δt_q) (7)

其中，rTT是机组轨道，并且Δt_q是来自某个公共时钟的用于四旋翼q的时间偏移。如果机组轨道不交叉或者不落入其本身的不安全距离内，则飞行器简单地需要以安全时间分离相互跟随。如果时间分离t_q被选择为使得没有两个飞行器在相同时间在任何交叉点处，则大量飞行器可以遵循使它们相互交叉的机组轨道。以下描述用于交叉机组轨道的实验。

发明人现在将描述用于通过满足关于用于多个飞行器的中间航路点的规范的已知3-D环境生成平滑安全轨道的方法。使用整数约束，实施与障碍物和其他飞行器的碰撞约束，并且还最优地分配目标位置。该方法从广泛文献提取混合整数线性规划(MILP)以及来自Schouwenaars等人的到轨道计划的应用。最优化可以被用于生成在指定时间t_w平滑地经过n_w个期望航路点的轨道。以下示出解决该问题同时最小化用于n_q个四旋翼的位置的k_r阶导数的平方的积分的最优化规划。

其中，rT_q＝[xT_q,yT_q,zT_q]表示用于四旋翼q的轨道，并且r_wq表示用于四旋翼q的期望航路点。发明人实施在t1、…、处的rT_q的第一个k_r阶导数的连续性。将轨道编写为分段多项式函数允许轨道被编写为二次规划(或QP)，其中，决策变量是多项式的系数。

对于四旋翼，由于输入u₂和u₃表现为位置的四阶导数的函数，所以发明人生成最小化标称四阶导数(加速度的二阶导数，k_r＝4)的平方的积分的轨道。大阶多项式被用于满足不由中间航路点显式指定的这样的附加轨道约束，作为障碍物避免。

为了碰撞避免，发明人将四旋翼建模为具有边长I_x、I_y和I_z的通过通用坐标系定向的矩形棱柱体。这些长度足够大，使得四旋翼可以翻转、倾斜和侧滑到任何角度并且待在棱柱体内。发明人考虑使该棱柱体航行通过具有no个凸起障碍物的环境。每个凸起障碍物o都可以由具有nf(o)个面的构形空间中的凸起区域表示。对于每个面f，在时间tk处的四旋翼的期望位置rT_q(tk)在障碍物o外侧的条件可以被编写为：

n_of·rT_q(tk)≤s_of，  (9)

其中，n_of是到构形空间中的障碍物o的面f的法向矢量，并且s_of是确定飞机的位置的标量。如果对于至少一个面，满足等式(9)，则矩形棱柱体并且因此四旋翼不与障碍物碰撞。四旋翼q在时间tk不与障碍物q碰撞的条件可以通过二进制变量b_qofk实施为：

其中，M是大正数。注意，如果b_qofk是1，则用于面f的不等式一直被满足。等式(10)中的最后不等式要求，对于障碍物的至少一个面，满足不碰撞约束，这暗示棱柱体不与障碍物碰撞。然后，发明人可以将等式(10)代入等式(8)中，用于在航路点之间的nk个中间时间步幅处的所有非障碍物的所有nq个四旋翼。整数变量到二次规划的添加导致该最优化问题变为混合整数二次规划(MIQP)。

当在航路点之间转换时，四旋翼必须相互保持安全距离。发明人实施在航路点之间的nk个中间时间步幅处的该约束，其可以用数学方法由以下约束集合表示用于四旋翼1和2：

${\∀t}_k:x_{T1}\left(t_k\right)-x_{T2}\left(t_k\right)\≤d_{x12}---\left(11\right)$

或x_T2(t_k)-x_T1(t_k)≤d_x21

或y_T1(t_k)-y_T2(t_k)≤d_y12

或y_T2(t_k)-y_T1(t_k)≤d_y21

在此，d项表示安全距离。对于轴向对称飞行器，d_x12＝d_x21＝d_y12＝d_y21。用实验方法，发明人已经发现，四旋翼必须避免在相互的下降气流中飞行，这是因为在最差情况下的跟踪性能并且甚至稳定性降低。因此，发明人不允许飞行器在此在相互下面飞行。最后，发明人以与等式(10)到等式(8)中相同的方式，在所有n_q个四旋翼之间结合等式(11)的约束。

在很多情况下，可能不太注意，特定四旋翼到特定目标，而不是任何飞行器都这样做。在此，发明人描述了用于使用整数约束找到用于飞行器的最优目标分配的方法。与固定目标分配相比，这导致较低总成本，并且通常导致更快计划时间，这是因为在最优化问题方面存在更多自由度。对于每个四旋翼q和目标g，发明人引入整数约束：

$x_{\mathrm{Tq}}\left(t_{n_w}\right)\≤x_g+{\mathrm{M\β}}_{\mathrm{qg}}---\left(12\right)$

$x_{\mathrm{Tq}}\left(t_{n_w}\right)\≥x_g-{\mathrm{M\β}}_{\mathrm{qg}}$

$y_{\mathrm{Tq}}\left(t_{n_w}\right)\≥y_g+{\mathrm{M\β}}_{\mathrm{qg}}$

$y_{\mathrm{Tq}}\left(t_{n_w}\right)\≥y_g-{\mathrm{M\β}}_{\mathrm{qg}}$

$z_{\mathrm{Tq}}\left(t_{n_w}\right)\≤z_g+{\mathrm{M\β}}_{\mathrm{qg}}$

$z_{\mathrm{Tq}}\left(t_{n_w}\right)\≥z_g-{\mathrm{M\β}}_{\mathrm{qg}}$

在此，β_qg是用于实施最优目标分配的二进制变量。如果β_qg是0，则四旋翼q必须在t_nw时在目标g处。如果β_qg是1，则这些约束被满足用于四旋翼q的任何最终位置。为了保证至少n_g个四旋翼达到期望目标，发明人引入以下约束。

$\underset{q=1}{\overset{n_q}{\mathrm{\Σ}}}\underset{g=1}{\overset{n_g}{\mathrm{\Σ}}}{\mathrm{\β}}_{\mathrm{qg}}\≤n_g{n}_q-n_g---\left(13\right)$

如果存在比目标更多的四旋翼或者反之亦然，该方法可以容易地适应。

MIQP的求解时间随着代入到MIQP中的二进制变量的数量成指数增长。因此，该方法的直接使用不很好地缩放用于大机组。在此，发明人提供使得该方法能够用于大机组飞行器的两种放宽。

如图6中所示，大机组飞行器可以被划分为较小组。然后，发明人使用MIQP方法生成轨道，以使多组飞行器转换到组目标位置。这降低了MIQP的复杂性，这是因为代替用于所有nq个飞行器的计划轨道，发明人简单地计划用于组的轨道。当然，发明人通过不允许四旋翼具有独立移动的灵活性，在此作出了牺牲。

在很多情况下，环境可以被划分为nr个凸起子区域，其中，每个子区域都包含相同数量的四旋翼起始和目标位置。在划分环境之后，MIQP轨道生成方法可以被用于每个区域内的飞行器。在此，发明人使用对飞行器的位置的线性约束，要求四旋翼待在它们自己的区域内。该方法保证无碰撞轨道，并且允许四旋翼具有独立移动的灵活性。发明人在损害最优化的情况下获得易处理，这是因为真正最优方案可能实际上要求四旋翼与区域边界交叉，但是该放宽版本不要求。而且，在子区域中可能不存在可行轨道，而且不存在与区域边界交叉的可行轨道。虽然如此，该方法在很多情况下都很好地工作，并且发明人示出其到用于16个飞行器的机组的编队转换的应用。

用于四旋翼动力学的模型

图13中示出包括通用坐标系W、以及体坐标系B、以及用于四旋翼的螺旋桨编号惯例的坐标系。发明人还使用Z-X-Y。欧拉角将翻转、倾斜和偏航角(θ、和ψ)限定为局部坐标系。从到的旋转矩阵由^WR_B＝^WR_C ^CR_B给出，其中，^WR_C表示到中间坐标系C的偏航角旋转，并且^CR_B表示翻转和倾斜的效果。机器人的角速度由表示，表示坐标系中的坐标系的角速度，以及体坐标系中的分量p、q和r。这些值可以直接与翻转、倾斜和偏航角的导数相关。

每个旋翼都具有角速度ω_i，并且产生力F_i和力矩Mi，根据以下：

F_i＝k_Fω²，M_i＝k_Mω².

实际上，电机动力学与刚性体动力学和空气动力学相比相对快。从而，对于控制器开发，发明人假设它们可以被瞬时实现。因此，输入到系统的控制可以被编写为u，其中，u1是净体力，u2、u3、u4是可以根据旋翼速度表示如下的体力矩：

$u=\left[\begin{array}{cccc}{k}_F& k_F& k_F& k_F\\ 0& k_{F}L& 0& {-k}_{F}L\\ -k_{F}L& 0& k_{F}L& 0\\ k_M& {-k}_M& k_M& -k_M\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_1^2\\ {\mathrm{\ω}}_2^2\\ {\mathrm{\ω}}_3^2\\ {\mathrm{\ω}}_4^2\end{array}\right],---\left(1\right)---\left(14\right)$

其中，L是从螺旋桨的旋转轴到四旋翼的中心的距离。通用坐标系中的质心的位置矢量由r表示。在方向上，坐标系上的力是重力，并且在方向上，是来自每个旋翼的力的总和u1。掌控质心的加速度的运动的牛顿等式是：

$m\stackrel{\·\·}{r}={-\mathrm{mgz}}_W+u_1{z}_B.---\left(15\right)$

由欧拉等式确定的角加速度是：

其中，是沿着x_B-y_B-z_B轴参考质心的惯性力矩矩阵。坐标系的状态由质心的位置和速度以及方位(通过欧拉角被局部参数化)和角速度给出：

或者在不被参数化的情况下，通过质心的位置和速度以及旋转矩阵WR_B和角速度给出

控制

用于以下轨道的控制器可以分别被定义为作为时间的函数的位置和偏航角rT(t)和ψT(t)。位置和速度的误差可以被定义为：

$e_p=\stackrel{\·}{r}-{\stackrel{\·}{r}}_T,e_v=\stackrel{\·}{r}-{\stackrel{\·}{r}}_T.$

接下来，发明人计算用于控制器的期望力矢量和期望体坐标系z轴：

$F_{\mathrm{des}}=-K_p{e}_p-K_v{e}_v+{\mathrm{mgz}}_W+m{\stackrel{\·\·}{r}}_T,$

其中，K_p和K_v是正有限增益矩阵。注意，发明人在此假设‖F_des‖≠0。接下来，发明人将期望力矢量投影到实际体坐标系z轴，以便计算用于四旋翼的期望力和第一控制输入：

u1＝F_des·z_B.

为了确定其他三个输入，必须考虑旋转误差。首先，观察到，期望z_B方向沿着期望推力矢量：

$z_{B,\mathrm{des}}=\frac{F_{\mathrm{des}}}{\left|\right|F_{\mathrm{des}}\left|\right|}.$

从期望加速度和所选偏航角，可以找到总期望方位。方位误差是期望旋转矩阵R_des和实际旋转矩阵^WR_B的函数：

其中，∨表示将so(3)的元素代入R³的vee map。注意，欧拉角的差可以被用作到该矩阵的近似。角速度误差简单地是体坐标系坐标中的实际和期望角速度之间的差：

现在，如下计算期望力矩和三个剩余输入：

[u₂，u₃，u₄]^T＝-K_Re_R-K_ωe_ω，(5)  (17)

其中，K_R和K_ω是对角增益矩阵。这允许唯一增益将被用于翻转、倾斜和偏航角跟踪。最后，发明人计算期望旋翼速度，以通过转化等式(14)实现期望u。

单个四旋翼轨道生成

在本部分中，发明人首先描述使用将障碍物结合到公式化中的勒让德(Legendre)多项式函数的基本四旋翼轨道生成方法。特别是，发明人解决了通过满足关于中间航路点的规范的已知3-D环境生成平滑安全轨道的问题。

考虑使飞行器在指定时间航行经过nw个航路点的问题。满足这些约束的普通轨道是使用直线在航路点之间插入的轨道。然而，该轨道无效率，这是因为其在航路点处具有无限曲率，其要求四旋翼停在每个航路点。在此描述的方法生成在给定时间平滑经过航路点的最优轨道。以下示出在最小化位置的k_r阶导数的平方的积分的同时解决该问题的最优规划。

在此，rT＝[xT,yT,zT]^T和ri＝[xi,yi,zi]^T。发明人实施在t1,...,处的第一个k_r阶导数的连续性。接下来，发明人使用基于多项式的函数P_pw(t)，将轨道编写为跨过nw个时间间隔的阶数np的分段多项式函数：

$r_T\left(t\right)=\left\{\begin{array}{cc}{\mathrm{\&Sigma;}}_{p=0}^{n_p}{r}_{\mathrm{Tp}1}{P}_{p1}\left(t\right)& t_0\&le;t<t_1\\ {\mathrm{\&Sigma;}}_{p=0}^{n_p}{r}_{\mathrm{Tp}2}{P}_{p2}\left(t\right)& t_1\&le;t<t_2\\ .& \\ .& \\ .& \\ {\mathrm{\&Sigma;}}_{p=0}^{n_p}{r}_{{\mathrm{Tpn}}_w}{P}_{{\mathrm{pn}}_w}\left(t\right)& t_{n_w-1}\&le;t\&le;t_{n_w}\end{array}\right.---\left(7\right)---\left(19\right)$

这允许发明人通过将常数rT_pw＝[xT_pw,yT_pw,zT_pw]^T编写为3n_wn_p×1决策变量矢量c，将问题公式化为二次规划(或QP)：

min c^THc+f^Tc  (8)

s.t.Ac≤b

A_eqc＝b_eq  (20)

在在此描述的坐标系中，由于输入u₂和u₃表示为位置的四阶导数的函数，所以发明人生成最小化标称四阶导数的平方的积分(加速度的二阶导数，k_r＝4)。等式(19)中的基允许发明人转到较高阶多项式，其允许发明人满足不由中间航路点显式指定的这样的附加轨道约束，作为障碍物避免。

虽然该问题公式化对于任何集合生成多项式基函数P_pw(t)有效，但是选择确实影响解算器的数值稳定性。基函数的差选择可能导致等式(20)中的矩阵H对于大阶多项式情况变坏。为了对角化H并且确保其是情况良好的矩阵，发明人在此使用勒让德多项式作为用于位置的k_r阶导数的基函数。勒让德多项式是从-1到1的间隔的正交多项式的生成集：

${\&Integral;}_{-1}^1{\mathrm{\&lambda;}}_m\left(T\right){\mathrm{\&lambda;}}_n\left(T\right)d_T=\frac{2}{2n+1}{\mathrm{\&delta;}}_{\mathrm{nm}}$

其中，δ_nm是克罗内克符号(Kronecker delta)并且是无因次时间。然后，发明人将这些勒让德多项式转换为在发明人调用λ_pw(t)的从t_w-1到t_w的隔内正交。发明人使用这些转换后的勒让德多项式表示用于位置函数P_pw(t)的前n_p-k_r个基函数的k_r阶导数。这些前n_p-k_r个多项式必须满足：

$\frac{d^{k_r}{P}_{\mathrm{pw}}\left(t\right)}{{\mathrm{dt}}^{k_r}}={\mathrm{\&lambda;}}_{\mathrm{pw}}\left(t\right),p=1,...,(n_p-k_r)$

发明人将最后k_r个多项式基函数限定为P_pw(t)＝(t-t_w-1)p＝(n_p-k_r+1),...,n。注意，这最后k_r个多项式基函数不影响成本函数，这是因为它们的k_r阶导数是0。在该工作中，发明人获得k_r＝4，并且n _p 通常在9和15之间。

为了碰撞避免，发明人将四旋翼建模为具有边长I_x、I_y和I_z的通过通用坐标系定向的矩形棱柱体。这些长度足够大，使得四旋翼可以翻转、倾斜和侧滑到任何角并且待在棱柱体内。发明人考虑使该棱柱体航行通过具有no个凸起障碍物的环境。每个凸起障碍物o可以由具有n_f(o)个面的构形空间中的凸起区域表示。对于每个面f，在时间t_k处的四旋翼的期望位置r_T(t_k)在障碍物o外侧的条件可以被编写为

m_of·r_T(t_k)≤s_of，  (21)

其中，n_of是到构形空间中的障碍物o的面f的法向矢量，并且sof是确定飞机的位置的标量。如果对于至少一个面，满足等式(21)，则矩形棱柱体并且从而四旋翼不与障碍物碰撞。棱柱体在时间tk不与障碍物碰撞的条件可以通过二进制变量b_ofk实施为：

其中，M是大正数。注意，如果b_ofk是1，则用于面f的不等式一直被满足。等式(22)中的最后不等式要求不碰撞约束被满足用于障碍物的至少一个面，这暗示棱柱体不与障碍物碰撞。然后，发明人可以将等式(22)代入(20)，用于在航路点之间的n_k个中间时间步幅处的所有障碍物。整数变量到二次规划的添加导致该最优化问题变为混合整数二次规划(MIQP)。

该公式化对于任何凸起障碍物有效，但是为了简单起见，发明人仅考虑矩形障碍物。通过在等式(22)中用n_of(t_k)简单地替换n_of，并且用s_of(t_k)替换s_of，该公式化容易扩展至移动障碍物。非凸起障碍物还可以在该框架中被有效地建模。

等式(20)表示连续时间最优化。发明人离散化时间，并且将碰撞约束写入等式(22)用于n_k个时间点。然而，在n_k个离散时间处的碰撞约束不保证轨道在时间步幅之间是不碰撞的。对于薄障碍物，最佳轨道可能导致四旋翼快速地航行通过障碍物，使得碰撞约束仅在通过障碍物之前和仅在其之后被满足，如图14(a)中所示。可以通过要求在时间步幅k处实施碰撞检验的矩形棱柱体具有与用于时间步幅k+1的相应棱柱体的有限交互，确定该问题。

$|x_T\left(t_k\right)-x_T(t_k+1)|\&le;l_x,\&ForAll;k=0,...,n_k$

$|y_T\left(t_k\right)-y_T(t_k+1)|\&le;l_y,\&ForAll;k=0,...,n_k$

$|z_T\left(t_k\right)-z_T(t_k+1)|\&le;l_z,\&ForAll;k=0,...,n_k---\left(23\right)$

这些附加时间步幅重叠约束防止轨道经过障碍物，如图14(b)中所示。实施时间步幅重叠等效于实施时间步幅之间的平均速度约束。当然，必须使用足够时间步幅，使得解决方案可行。由于时间离散化，导致轨道还可以抄近路。发明人通过适当地膨胀障碍物和实施碰撞检验的棱柱体的尺寸，解决该问题。在找到轨道之后，发明人执行碰撞检验，以确保实际四旋翼形状在整个轨道上不与任何障碍物交叉。

我们可以利用时间缩放在安全性和攻击性之间折衷。如果发明人通过因数(例如，α＝2允许轨道在同样多时间内被执行两次)改变时间，以使通过航路点，则对时间缩放问题的原始解决方案的版本是原始解决方案的时间缩放版本。因此，发明人不需要解算MIQP。当α增加时，计划花费更长时间执行，并且变得更加安全。当α达到无穷大时，位置和偏航角以及角速度的所有导数都达到零，其在极限内导致

u(t)→[mg，0，0，0]^T，

在等式(16)和等式(17)中。通过使得足够大，假设小倾斜和翻转，发明人可以满足被生成用于四旋翼的任何运动计划。相反地，当尺寸减小时，轨道花费更少时间来执行，位置的导数增加，并且轨道变得更加有侵略性，导致从零倾斜和零翻转构形的大偏移。

多四旋翼轨道生成

在本部分中，发明人将方法扩展至包括在相同环境(通常为近进程)中航行到指定目标位置的n_q个异质四旋翼，每个都具有指定航路点。这通过求解等式(20)的较大版本作出，其中，决策变量是所有n_q个四旋翼的轨道系数。对于碰撞避免约束，每个四旋翼都可以是由l_x,l_y,l_z的唯一值指定的不同尺寸。发明人还考虑具有相对成本加权和四旋翼键碰撞避免的异质方面。

独立航行的四旋翼机组必须解算导致碰撞并且“共享”三维空间的碰撞。从而，它们必须修改它们单独的轨道，以航行通过环境并且相互避免碰撞。如果所有四旋翼都是相同类型的，则使得它们相等地共享碰撞解决方案的负担是有意义的。然而，对于异质飞行器的机组，可能期望允许一些四旋翼比其他四旋翼遵循相对容易的轨道，或者基于用户偏好将四旋翼区分优先次序。因此，这可以通过给它们的成本加权来实现。如果四旋翼q具有相对成本μ_q，则等式(20)的多四旋翼版本中的二次成本矩阵H_m可以被编写为：

$H_m=\mathrm{diag}({\mathrm{\&mu;}}_1{H}_1,{\mathrm{\&mu;}}_2{H}_2,...,{\mathrm{\&mu;}}_{n_q}{H}_{n_q})---\left(12\right)---\left(24\right)$

将较大加权因数应用至四旋翼使得其获得在其起始和目标之间的更直接路径。应用较小加权因数实施四旋翼修改其轨道，以让步具有较大加权因数的其他四旋翼。当用于慢且大的四旋翼的轨道可以比用于较小且更敏捷的四旋翼的相同轨道分配更高成本时，该能力对于敏捷和慢的四旋翼的机组特别有价值。大四旋翼比小四旋翼要求更好的跟踪准确度，以飞过相同窄间隙，所以在那些情况下分配较高成本用于较大四旋翼也很有用。

四旋翼必须相互分离安全距离。发明人在航路点之间的n _k 个中间时间步幅处实施该约束，其可以由以下集合约束，用数学方法表示用于四旋翼1和2：

${\&ForAll;t}_k:x_{1T}\left(t_k\right)-x_{2T}\left(t_k\right)\&le;d_{x12}$

或x_2T(t_k)-x_1T(t_k)≤d_x21

或y_1T(t_k)-y_2T(t_k)≤d_y12

或y_2T(t_k)-y_1T(t_k)≤d_y21

或z_1T(t_k)-z_2T(t_k)≤d_z12

或z_2T(t_k)-z_1T(t_k)≤d_z21

                    (25)

在此，d项表示安全距离。对于轴向对称飞行器，d_x12＝d_x21＝d_y12＝d_y21。根据经验，发明人发现，四旋翼必须避免在类似尺寸或者较大四旋翼的下降气流中飞行，这是因为在最差情况下的跟踪性能以及甚至稳定性的降低。然而，较大四旋翼可以在较小四旋翼下面飞行。发明人表明，较大四旋翼甚至可以在小四旋翼下面充分稳定地飞行，以用作空中降落平台。因此，如果四旋翼1和2为相同类型，则d_z12＝d_z21。然而，如果四旋翼1比四旋翼2大很多，则四旋翼2必须以一些大距离d_z12，在较大四旋翼下面很好地飞行，同时四旋翼1可以如由较小距离d_z21表示的在四旋翼2下面非常接近地飞行。这些安全距离的准确值可以通过测量用于四旋翼类型之间的不同分离距离的跟踪性能，根据经验找到。最终，发明人以将等式(22)中的约束代入等式(20)的多四旋翼版本中相同的方式，在所有n_q个四旋翼之间结合等式(25)的约束。

在此，发明人分析了通过用于由等式(20)、(22)和(25)形成的三维航行问题的该公式化生成的MIQP的复杂性。在该问题中，连续变量的数量n_c最多为：

n_c＝3n_wn_pn_q.  (26)

可以通过去除等效约束，从MIQP消除一些连续变量。确定计算时间的强因数是被代入的二进制变量的数量n_b。用于三维航行问题的二进制变量的数量是：

$n_b=n_w{n}_k{n}_q\underset{o=1}{\overset{n_o}{\mathrm{\&Pi;}}}{n}_f\left(o\right)+n_w{n}_k\frac{n_q(n_q-1)}{2}6---\left(27\right)$

等式(27)中的第一项说明障碍物避免约束，并且第二项表示四旋翼间安全距离实施。发明人使用分支限界求解器来求解MIQP。在最差情况下，存在检测的两级树叶。从而，这不是很好地缩放用于大量机器人的方法，但是其可以生成用于小机组(在此多达4个四旋翼)和几个障碍物的最佳轨道。该技术的一个优点在于，如果可用高计算预算非常低，则可以非常快地找到(比较下表中的T₁和T_opt)保证安全性和碰撞解决方案的次优的灵活解决方案。

实验结果

为了非常实际原因，示例性四旋翼的架构很重要。对于四旋翼的大机组，不可能运行单个循环，其可以接收所有Vicon数据，计算命令，并且以充分快的速率与每个四旋翼通信。如图7中所示，每组由台式基站20上的在独立线程中运行的专用Vicon软件节点10控制。控制节点30经由共享存储器40从Vicon节点10接收飞行器姿态数据。Vicon节点10连接至Vicon跟踪系统，接收用于每个对象的标记位置，执行与标记数据吻合的6D姿态、以及用于速度估计的附加处理。最后，处理后的姿态估计使用Boost C++库被公开至共享存储器40。共享存储器40是进程间通信的最快方法，其确保时间关键数据的最低等待时间。

在Matlab中实现的控制节点30从共享存储器40直接读取姿态数据，并且基于上述控制器计算用于几个四旋翼的被命令方位和净推力。为了非时间关键数据共享，发明人使用进程间通信(IPC)50。例如，诸如期望飞行器位置的高级用户命令60被发送至计划器70，其计算用于将经由IPC 50被发送至Matlab控制节点30的飞行器的轨道。IPC 50提供灵活消息传送，并且使用TCP/IP套接字在进程之间发送数据。

每个Matlab控制节点都与包含900MHz和2.4GHz Zigbe收发器的无线电模块相关，其被用于与该组中的所有飞行器通信。无线电模块80将控制命令发送至几个飞行器，在示例性实施例中，多达五个。每个飞行器都在单独信道上操作，并且无线电模块80在频率之间跳变用于每个四旋翼，以100Hz将命令输出到每个飞行器。无线电模块80还可以利用两个独立收发器，同时从飞行器接收高带宽反馈。

在图8中，发明人提供用于遵循轨道作为编队的四个四旋翼的机组的数据。组编队误差明显大于局部误差。局部x和y误差一直小于3cm，同时编队x误差为11cm那么大。该数据表示遵循数据的所有编队轨道，因为所有飞行器在名义上都是增益。因此，即使与期望轨道的偏差可能很大，在组内的相对位置误差也很小。

图9(a)示出用于以图1中所示的栅格编队飞行的20个飞行器的平均误差数据。对于该实验，控制飞行器在1.3米的高度处以多个四旋翼中心到四旋翼中心栅格间隔距离，盘旋至少30秒。从所有20个飞行器的下降气流产生的空气干扰很明显，并且导致跟踪性能对于该编队的任何飞行器比对于仍然在空中的单独飞行器更差。然而，如图9(a)中所示，分离距离对于盘旋性能不具有任何影响。注意，间隔螺旋桨尖之间的标称距离的35cm栅格约为14cm。

图9(b)图示了遵循循环图形八图案的16个飞行器的机组。完成整个循环的时间是t_c，并且飞行器以t_c/16的时间增量沿着轨道相等地隔开。为了在交叉点处保证无碰撞轨道，飞行器在轨道的一个循环中花费15/32t_c，并且在其他循环中花费17/32t_c。使用用于单个飞行器的基于最优化的方法生成满足这些定时约束并且在交叉点处具有一些指定速度(具有零加速度和加加速度)的轨道。

而且，发明人使用分支限界求解器来求解MIQP轨道生成问题。用于MIQP的求解时间是二进制约束的数量以及环境的几何复杂度的指数函数。第一种求解通常在几秒内产生，但是找到真正最优求解和证书最优性可能在用于在此呈现的实例的3.4Ghz Corei7Quad-Core台式机上花费20分钟那么长。

1)用于机组内的组的计划：

图10图示了来自用于从间隙的一侧转换到另一侧的四组四个四旋翼的实验的快照。注意，在本实例中，在组等级执行最佳目标分配。

2)用于子区域的计划：

图11图示了来自通过从平坦栅格转换到三维螺旋和金字塔的16个飞行器的实验的快照。直接使用MIQP方法生成用于16个飞行器的轨道是不实际的。因此，在两个实验中，空间被划分为两个区域，每个都具有8个飞行器的单独MIQP被用于在编队的左侧和右侧上生成用于飞行器的轨道。注意，通常，编队不必须是对称的，但是在此，发明人利用对称，并且仅求解用于8个飞行器的单个MIQP，用于这些实例。使用最佳目标分配，使得飞行器共同选择它们的目标，以最小化总成本。

在此提供的实验通过Ascending Technologies Hummingbird四旋翼以及kQuad65和内部开发的kQuad1000四旋翼进行，其重量为457、65和962克，并且分别具有55、21和67cm的叶片尖到叶片尖长度。这样的四旋翼在图12中示出。发明人使用Vicon运动捕捉系统来估计四旋翼和机载IMU的位置和速度，以估计方位和角速度。软件架构在“The grasp multiple micro uavtestbed,”N Michael,D Mellinger,QLindsey和V.Kumar，2010年9月中描述。

在发明人的先前工作中，使用由运动捕捉系统测量的方位，离板计算方位误差项。该离板计算将可变时间延迟代入到控制循环中，当使用多个四旋翼时，这很显著。时间延迟限制姿态控制器的性能。发明人选择使用刚性板上线性化姿态控制器，而不使用更柔软的离板非线性姿态控制器。发明人通过CPLEX软件包中的MIQP求解器求解所有问题。

本实验表明了用于具有障碍物的面情形下的三个飞行器的计划。三个同质Hummingbird四旋翼开始于窄间隙的一侧上，并且必须经过到相对侧上的目标位置。使用在此描述的使用十阶多项式并且在两个航路点(n_p＝10,n_k＝11,n_w＝1)之间的11个中间时间步幅处实施碰撞约束的方法找到轨道。然后，命令四旋翼在将环放在环境中以表示间隙的30次试验中，以多种速度遵循这些轨道。用于该试验的数据和图像在图15和图16中示出。图15(a)示出用于这些试验中的每个的均方根误差(RMSE)。虽然具有较大加速度、加加速度和四阶导数的轨道导致较大误差(如所预期的)，但是性能非常适度地降低。在图15(b-d)中呈现了用于单次运行的数据。

实验表明kQuad1000(四旋翼1)和Hummingbird(四旋翼2)从间隙下面的位置到间隙之上的空间的相对侧上的位置的航行。该问题被公式化为使用十三阶多项式并且在两个航路点(n_p＝13,n_k＝9,n_w＝1)之间的9个中间时间步幅处实施碰撞约束的3-D轨道生成问题。为了问题公式化，使用四个三维矩形棱柱体形状障碍物创建单个3-D间隙，四旋翼必须经过其到它们的目标。图17和图18中示出用于这些实验的数据和图像。因为较大四旋翼具有经过间隙的更严格容限，所以发明人选择比Hummingbird多加权10次其成本函数。这可以从由图17中的四旋翼2获得的更迂回路线观测到。而且，这可以通过用于四旋翼2的较大误差观测，这是因为其遵循要求较大速度和加速度的更难轨道。最后，应该注意，较大四旋翼遵循穿越间隙的较小误差，这是因为允许其在较小四旋翼下面飞行，但是反之不一样。

本实验表明用于四个四旋翼的机组的重新配置。该问题被公式化为使用九阶多项式并且在两个航路点(n_p＝9,n_k＝9,n_w＝1)之间的9个中间时间步幅处实施碰撞约束的3-D轨道生成问题。生成使四旋翼在给定三维编队中的任意位置之间转换或者完全并且快速地转换到完全不同编队的轨道。发明人提供图19和图20中的多种重新配置和在线编队内的单次转换。发明人通过四个Hummingbirds和由两个Hummingbirds，一个kQuad65和一个kQuad1000，构成的异质机组进行实验。对于异质组，发明人给kQuad65的成本加权比其他四旋翼大10倍，这是因为其不太敏捷，并且可能目前仅遵循适度聚合轨道。注意，在图19(b)中示出kQuad65如何获得最直接轨道。对于图19(a)中所示的同质实验，四旋翼待在相同面中，这是因为不允许他们在相互下面飞行，如在此描述的，但是在图19(b)中所示的异质实验中，最佳求解包含z分量，这是因为允许较大四旋翼在较小四旋翼下面飞行。

以下表中阐述用于在此求解的MIQP中的每个的一些问题详情和它们的计算时间：

图	nq	nP	Nk	nb	T1(s)	Topt(s)
							14(b)	1	15	16	208	0.42	35
17	2	13	9	270	0.62	1230
							15	3	10	11	300	0.21	553
19(a)	4	9	9	324	0.11	39
							19(b)	4	9	9	324	0.45	540

所有计算时间都被列出用于具有使用CPLEX MIQP求解器的2.66GHz Intel Core 2Duo处理器的MacBook Pro膝上型电脑。注意，虽然特定问题花费长时间找到最佳求解，并且证明最优性，但是发现第一求解一直不及第二求解。在找到第一可行答案并且返回次最佳求解之后，求解器可以在任何时间停止。

本领域技术人员还将容易地想到，在实质上不脱离本发明的新教导和优点的情况下，很多附加修改和情形都可以在示例性实施例中。从而，任何这样的修改都旨在包括在由示例性权利要求限定的本发明的范围内。

Claims (35)

1.一种用于朝向目标位置和方位控制至少两个飞行器的状态的轨道生成方法，所述方法包括以下步骤：

确定所述飞行器的方位和角速度；

通过控制所述飞行器的至少一个电机，控制所述飞行器的所述方位和角速度；

确定每个所述飞行器的当前位置和速度；

通过指定期望的方位和角速度以及所述至少一个电机所要求的净推力，控制每个所述飞行器的所述位置和速度；

对于每个所述飞行器，通过使用分段平滑多项式函数，应用加权因数，并且实施重叠约束，以预定时间间隔计算最佳轨道路径；

基于所计算的最佳轨道路径，将命令发送至每个所述飞行器，以单独控制它们的状态，使得这样的飞行器遵循所计算的最佳轨道路径，同时避免碰撞；以及

更新每个所述飞行器的当前位置和速度。

2.根据权利要求1所述的方法，其中，飞行器的每个状态包括其方位和角速度、以及位置和线速度。

3.根据权利要求1所述的方法，其中，在每个所述飞行器上估计方位误差和控制所述方位。

4.根据权利要求1所述的方法，其中，应用至所述至少两个飞行器中的每个的所述加权因数是不同的。

5.根据权利要求1所述的方法，其中，所述方法以多个所述预定时间间隔迭代地执行。

6.根据权利要求1所述的方法，进一步包括：使用整数约束来实施与障碍物和其他飞行器的碰撞约束，并且最佳地分配用于所述至少两个飞行器的目标位置。

7.根据权利要求1所述的方法，其中，计算用于每个飞行器的最佳轨道路径包括：生成在指定时间t_w平滑地经过n_w个期望航路点的轨道，同时根据以下等式最小化用于n_q个四旋翼的位置的第k_r阶导数的平方的积分：

s.t.r_Tq(t_w)＝r_wq，w＝0，…，n_w；

或空闲，w＝0，n_w；j＝1，…，k_r；

或空闲，w＝0，n_w；j＝1，…，k_r；

或空闲，w＝0，n_w；j＝1，…，k_r；

其中，rTq＝[xTq,yTq,zTq]表示用于四旋翼q的轨道，并且r_wq表示用于四旋翼q的期望航路点。

8.根据权利要求1所述的方法，进一步包括：通过将飞行器建模为具有边长I_x、I_y和I_z的通过通用坐标系定向的矩形棱柱体，提供所述至少两个飞行器之间的碰撞避免，其中，所述边长I_x、I_y和I_z足够大，使得飞机能够翻转、倾斜和侧滑到任何角度并且待在所述棱柱体内。

9.根据权利要求8所述的方法，进一步包括：使所述棱柱体航行通过具有no凸起障碍物的环境，其中，每个凸起障碍物o都由具有nf(o)个面的构形空间中的凸起区域表示，并且对于每个面f，在时间tk处的所述飞行器的期望位置rTq(tk)在障碍物o外侧的条件被表示为：

n_of·rTq(tk)≤s_of

其中，n_of是到构形空间中的障碍物o的面f的法向矢量，并且s_of是标量，由此，如果用于在时间tk处的所述飞行器的位置的等式被满足用于至少一个所述面，则所述矩形棱柱体并且因此所述飞机不与所述障碍物碰撞。

10.根据权利要求9所述的方法，其中，飞机q在时间t_k处不与障碍物碰撞的条件通过二进制变量b_qofk实施为：

n_of·r_Tq(t_k)≤s_of+Mb_qofk

b_qofk＝0或1 $\&ForAll;f=1,...,n_f\left(o\right)$

$\underset{f=1}{\overset{n_f\left(o\right)}{\mathrm{\&Sigma;}}}{b}_{\mathrm{qofk}}\&le;n_f\left(o\right)-1$

其中，M是大正数。

11.根据权利要求10所述的方法，其中，权利要求10的等式被代入到权利要求7的等式中，用于航路点之间的n_k个中间时间步幅处的所有障碍物的所有n_q个飞机。

12.根据权利要求7所述的方法，其中，当通过实施在航路点之间的n_k个中间时间步幅处的约束，在飞行器的轨道路径上的航路点之间转换时，所述飞行器相互保持安全距离，其中，所述约束能够由以下约束集合在数学上被表示用于飞行器1和2：

${\&ForAll;t}_k:x_{T1}\left(t_k\right)-x_{T2}\left(t_k\right)\&le;d_{x12}$

或x_T2(t_k)-x_T1(t_k)≤d_x21

或y_T1(t_k)-y_T2(t_k)≤d_y12

或y_T2(t_k)-y_了1(t_k)≤d_y21

其中，d项表示飞行器1和2之间的安全距离。

13.根据权利要求12所述的方法，其中，所述飞行器轴向对称，并且d_x12＝d_x21＝d_y12＝d_y21。

14.根据权利要求6所述的方法，其中，所述整数约束被用于通过应用以下整数约束用于每个四旋翼q和目标g，找到用于所述飞行器的最佳目标分配：

$x_{\mathrm{Tq}}\left(t_{n_w}\right)\&le;x_g+{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$x_{\mathrm{Tq}}\left(t_{n_w}\right)\&GreaterEqual;x_g-{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$y_{\mathrm{Tq}}\left(t_{n_w}\right)\&le;y_g+{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$y_{\mathrm{Tq}}\left(t_{n_w}\right)\&GreaterEqual;y_g-{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$z_{\mathrm{Tq}}\left(t_{n_w}\right)\&le;z_g+{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$z_{\mathrm{Tq}}\left(t_{n_w}\right)\&GreaterEqual;z_g-{\mathrm{M\&beta;}}_{\mathrm{qg}}$

其中，β_qg是用于实施最佳目标分配的二进制变量。

15.根据权利要求14所述的方法，进一步包括：应用以下约束以保证至少n_g个四旋翼达到期望的目标位置：

$\underset{q=1}{\overset{n_q}{\mathrm{\&Sigma;}}}\underset{g=1}{\overset{n_g}{\mathrm{\&Sigma;}}}{\mathrm{\&beta;}}_{\mathrm{qg}}\&le;n_g{n}_q-n_g.$

16.根据权利要求1所述的方法，所述方法进一步包括：

将所述飞行器组织成多个组，其中，所述多个组中的每个被单独协调；以及

生成用于所述多个组中的每个的轨道，以对目标位置分组。

17.根据权利要求16所述的方法，其中，用于所述飞行器的环境被划分为nr个凸起子区域，其中，每个子区域包含相同数量的飞行器起始和目标位置，并且对于每个子区域内的所述飞行器生成单独轨道，由此使用对所述飞行器的所述位置的线性约束，要求所述飞行器待在它们自己的子区域内。

18.一种用于朝向目标位置控制至少两个飞行器的飞行轨道的系统，所述系统包括：

至少两个飞行器，所述至少两个飞行器具有机载惯性测量单元，用于确定并且更新所述至少两个飞行器的方位、角速度、位置和线速度；

运动捕捉系统，所述运动捕捉系统用于检测所述至少两个飞行器中的每个的当前位置和速度；

基础控制器，所述基础控制器与所述运动捕捉系统通信并且与所述多个飞行器通信，所述基础控制器对于每个所述飞行器使用分段平滑多项式函数，应用加权因数，并且实施重叠约束，来以预定时间间隔计算最佳轨道路径，并且基于所计算的最佳轨道路径，将命令发送至每个所述飞行器，以单独控制它们的状态，使得所述至少两个飞行器遵循所计算的最佳轨道路径，同时避免碰撞。

19.根据权利要求18所述的系统，其中，所述至少两个飞行器是同质飞行器。

20.根据权利要求18所述的系统，其中，所述至少两个飞行器是异质飞行器。

21.根据权利要求18所述的系统，其中，飞行器的每个状态包括其方位和角速度、以及位置和线速度。

22.根据权利要求18所述的系统，进一步包括：在每个飞行器上的微型控制器，所述微型控制器估计方位误差并且控制关联飞行器的所述方位。

23.根据权利要求20所述的系统，其中，应用至所述至少两个飞行器中的每个的所述加权因数是不同的。

24.根据权利要求18所述的系统，其中，所述基础控制器使用整数约束实施与障碍物和其他飞行器的碰撞约束，并且最佳地分配用于所述至少两个飞行器的目标位置。

25.根据权利要求18所述的系统，其中，所述基础控制器通过生成在指定时间t_w平滑经过n_w个期望航路点的轨道，同时根据以下等式最小化用于n_q个四旋翼的位置的k_r阶导数的平方的积分，计算用于每个飞行器的最佳轨道路径：

s.t.r_Tq(t_w)＝r_wq，w＝0，…，n_w；

或空闲，w＝0，n_w；j＝1，…，k_r；

或空闲，w＝0，n_w；j＝1，…，k_r；

或空闲，w＝0，n_w；j＝1，…，k_r；

其中，rTq＝[xTq,yTq,zTq]表示用于四旋翼q的轨道，并且r_wq表示用于四旋翼q的期望航路点。

26.根据权利要求18所述的系统，其中，所述基础控制器通过将所述飞行器建模为具有边长I_x、I_y和I_z的通过通用坐标系定向的矩形棱柱体，进一步提供所述至少两个飞行器之间的碰撞避免，其中，所述边长I_x、I_y和I_z足够大，使得所述飞机能够翻转、倾斜和侧滑到任何角度并且待在所述棱柱体内。

27.根据权利要求26所述的系统，其中，所述基础控制器使所述棱柱体航行通过具有no个凸起障碍物的环境，其中，每个凸起障碍物o都由具有nf(o)个面的构形空间中的凸起区域表示，并且对于每个面f，在时间tk处的所述飞行器的期望位置rTq(tk)在障碍物o外侧的条件被表示为：

n_of·rTq(tk)≤s_of

其中，n_of是到构形空间中的障碍物o的面f的法向矢量，并且s_of是标量，由此，如果对于至少一个所述面，满足用于在时间tk处的所述飞行器的位置的等式，则所述矩形棱柱体并且因此所述飞机不与所述障碍物碰撞。

28.根据权利要求27所述的系统，其中，所述基础控制器将飞机q在时间tk处不与障碍物o碰撞的条件通过二进制变量b_qofk实施为：

b_qofk＝0或1 $\&ForAll;f=1,...,n_f\left(o\right)$

$\underset{f=1}{\overset{n_f\left(o\right)}{\mathrm{\&Sigma;}}}{b}_{\mathrm{qofk}}\&le;n_f\left(o\right)-1$

基中，M是大正数。

29.根据权利要求28所述的系统，其中，所述基础控制器将权利要求28的等式带入权利要求25的等式，用于在航路点之间的n_k个中间时间步幅处的所有障碍物的所有n_q个飞机。

30.根据权利要求25所述的系统，其中，当通过实施在航路点之间的n_k个中间时间步幅处的约束，在飞行器的轨道路径上的航路点之间转换时，基站使所述飞行器相互保持安全距离，其中，所述约束能够由以下约束集合在数学上被表示用于飞行器1和2：

${\&ForAll;t}_k:x_{T1}\left(t_k\right)-x_{T2}\left(t_k\right)\&le;d_{x12}$

或x_T2(t_k)-x_T1(t_k)≤d_x21

或y_T1(t_k)-y_T2(t_k)≤d_y12

或y_T2(t_k)-y_T1(t_k)≤d_y21

其中，d项表示飞行器1和2之间的安全距离。

31.根据权利要求30所述的系统，其中，所述飞行器轴向对称，并且d_x12＝d_x21＝d_y12＝d_y21。

32.根据权利要求24所述的系统，其中，基站使用所述整数约束，以通过应用以下整数约束用于每个四旋翼q和目标g，找到用于所述飞行器的最佳目标分配：

$x_{\mathrm{Tq}}\left(t_{n_w}\right)\&le;x_g+{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$x_{\mathrm{Tq}}\left(t_{n_w}\right)\&GreaterEqual;x_g-{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$y_{\mathrm{Tq}}\left(t_{n_w}\right)\&le;y_g+{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$y_{\mathrm{Tq}}\left(t_{n_w}\right)\&GreaterEqual;y_g-{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$z_{\mathrm{Tq}}\left(t_{n_w}\right)\&le;z_g+{\mathrm{M\&beta;}}_{\mathrm{qg}}$

$z_{\mathrm{Tq}}\left(t_{n_w}\right)\&GreaterEqual;z_g-{\mathrm{M\&beta;}}_{\mathrm{qg}}$

其中，β_qg是用于实施最佳目标分配的二进制变量。

33.根据权利要求32所述的系统，所述基础控制器进一步应用以下约束以保证至少n_g个四旋翼达到期望的目标位置：

$\underset{q=1}{\overset{n_q}{\mathrm{\&Sigma;}}}\underset{g=1}{\overset{n_g}{\mathrm{\&Sigma;}}}{\mathrm{\&beta;}}_{\mathrm{qg}}\&le;n_g{n}_q-n_g.$

34.根据权利要求18所述的系统，其中，所述基础控制器进一步被编程为：

将所述飞行器组织成多个组，其中，所述多个组中的每个被单独协调；以及

生成用于所述多个组中的每个组的轨道，以对目标位置分组。

35.根据权利要求34所述的系统，其中，所述基础控制器将用于所述飞行器的环境划分为nr个凸起子区域，其中，每个子区域包含相同数量的飞行器起始和目标位置，并且对于每个子区域内的所述飞行器生成单独轨道，由此使用对所述飞行器的所述位置的线性约束，要求所述飞行器待在它们自己的子区域内。