CN109885080B

CN109885080B - 自主控制系统及自主控制方法

Info

Publication number: CN109885080B
Application number: CN201910048778.2A
Authority: CN
Inventors: S·沈
Original assignee: University of Pennsylvania Penn
Current assignee: University of Pennsylvania Penn
Priority date: 2013-11-27
Filing date: 2014-11-28
Publication date: 2022-09-20
Anticipated expiration: 2034-11-28
Also published as: WO2015105597A4; US20170212529A1; WO2015105597A3; EP3074832A2; JP2016540211A; EP3470787A1; JP6739463B2; CA2931632C; CN109885080A; JP2018156660A; US20180088597A1; CN106030430A; WO2015105597A2; EP3470787B1; EP3074832A4; CA2931632A1; US10732647B2

Abstract

本发明涉及自主控制系统及自主控制方法。本文中所描述的主题包括用于集成来自多个异类传感器的有噪声的测量结果、并用于实时地为自主飞行提供对位置平滑的和全局上一致的估算值的模块化的以及可扩展的方法，该多个异类传感器以不同的和变化的时间间隔产生绝对观测结果或相对观测结果。我们描述了对用于装备有IMU、激光扫描仪、立体摄像机、气压测高计、磁力计、以及GPS接收器的新的1.9kg MAV平台的算法和软件架构的开发，其中，在Intel NUC第三代i3处理器上机上执行状态估算和控制。我们例示了我们的框架在大规模的、室内‑室外自主空中航行实验中的稳健性，该实验涉及在进入和退出建筑物的同时以1.5m/s的平均速度以及大约10mph的风速横越超过440米。

Description

自主控制系统及自主控制方法

本申请是申请日为2014年11月28日，名称为“用于使用旋翼微型航空载具(MAV)在室内和室外环境中的稳健的自主飞行的多传感器融合”，申请号为201480064737.8的发明专利申请的分案申请。

优先权声明

本申请请求2013年11月27日提交的、美国临时申请序列号61/910,022的权益，通过引用将其公开内容整体并入本文中。

政府兴趣

在美国海军研究办公室授予的基金第N00014-07-1-0829号、第N00014-08-1-0696号、第N00014-09-1-1031号、以及第N00014-09-1-1051号、美国国家科学基金会授予的基金第1138847号、第113830号、以及第1138110号、美国空军科学研究办公室授予的基金第W911NF-08-2-0004号和第W911NF-10-2-0016号、以及美国陆军研究实验室授予的基金第FA9550-10-1-0567号下，通过政府支持作出本发明。政府具有本发明中的某些权益。

技术领域

本文中所描述的主题涉及在微型航空载具中控制自主飞行。更具体来说，本文中所描述的主题涉及用于使用旋翼微型航空载具(MAV)在室内和室外环境中的强健的自主飞行的多传感器融合。

背景技术

微型航空载具(例如旋翼微型航空载具)能够自主飞行。假设存在可用于为自主飞行提供控制输入的足够的传感器数据，那么可以实现精确的自主飞行。例如，在全球定位系统(GPS)是可用的某些室外环境中，可以基于GPS信号来实现自主飞行。然而，在GPS不可用的环境(例如室内环境以及甚至室外城市环境)中，仅基于GPS的自主飞行是不可能的。在某些室内环境中，由于结构所引起的磁场干扰，因此磁力计输出可能是不可用的或不可靠的。因此，依赖于单模态的传感器来控制旋翼飞机MAV的飞行可能是不期望的。

控制旋翼飞机MAV的自主飞行的另一个目标是在当先前不可用的传感器模态变为可用时的状态之间平滑过渡。例如，当旋翼飞机MAV正在GPS不可用的室内飞行并且随后过渡到GPS突然变得可用的室外环境时，旋翼飞机可以确定其远远偏离航线并可能尝试通过立即移动至位于航线上来纠正错误。期望的是，这样的过渡是平滑的，而不是使旋翼飞机在速度和轨迹上立即进行大的改变来返回到航线上。

多种类型的传感器数据可用于在旋翼微型航空载具中控制自主飞行。例如，机上摄像机、激光扫描仪、GPS收发机、以及加速度计可以提供适合作为用于控制飞行的控制输入的多个输入。然而，如上所述，当与传感器相关联的假设不成立时，则依赖于这些传感器中的任何一个传感器也失败。由于每种类型的传感器在其测量结果中产生具有独特水平的不确定性的一种独特的输出，因此存在对针对用于使用旋翼飞机MAV在室内和室外环境中的稳健的自主飞行的多传感器融合的改进的方法、系统、以及计算机可读介质的需求。

发明内容

本文中所描述的主题包括用于集成来自多个异类传感器的有噪声的测量结果、并用于实时地为自主飞行提供对位置平滑的和全局上一致的估算值的模块化的以及可扩展的方法，该多个异类传感器以不同的和变化的时间间隔产生绝对观测结果或相对观测结果。我们描述了对用于装备有惯性测量单元(IMU)、激光扫描仪、立体摄像机、气压测高计、磁力计、以及GPS接收器的新的1.9kg MAV平台的算法和软件架构的开发，其中，在IntelNUC第三代i3处理器上机上执行状态估算和控制。我们例示了我们的框架在大型的、室内-室外自主空中航行实验中的稳健性，这些实验涉及在进入和退出建筑物的同时以1.5m/s的平均速度以及大约10mph的风速横越超过440米。

可以用硬件、软件、固件、或者它们的任意组合来实施本文中所描述的主题。因此，如本文中所使用的术语“功能”、“节点”或“模块”指代硬件，其也可以包括用于执行所描述的特征的软件和/或固件部件。在一个示例性的实施方式中，可以使用具有储存在其上的计算机可执行指令的计算机可读介质来实施本文中所描述的主题，当由计算机的处理器来执行该计算机可执行指令时，其控制计算机执行步骤。适合于实施本文中所描述的主题的示例性的计算机可读介质包括非暂态计算机可读介质，例如磁盘存储设备、芯片存储设备、可编程逻辑设备、以及专用集成电路。此外，实施本文中所描述的主题的计算机可读介质可以位于单个设备或计算平台上，或者可以分布跨越多个设备或计算平台。

根据本发明的第一方面，提供了一种实现旋翼微型航空载具在室内和室外环境中的自主控制的系统，所述系统包括：传感器融合模块，所述传感器融合模块用于对来自不同模态的多个传感器的测量结果进行组合以鉴于来自所述传感器的当前测量结果和先前测量结果以及所述旋翼微型航空载具的先前估算状态来估算所述旋翼微型航空载具的当前状态，其中，所述传感器融合模块被配置为在以下情况下保持在对所述旋翼微型航空载具的姿态估算方面的平滑性：一个或多个传感器提供不准确的信息；全球定位系统(GPS)测量结果在一段时间可用之后变得不可用；或者GPS测量结果在一段时间不可用之后变得可用，其中，所述姿态估算方面的平滑性通过转换先前的全局测量的GPS姿态约束(z_k-1)以使转换后的全局测量的GPS姿态约束

与先前的姿态估算(s_k-1)之间的差异最小化而被保持，其中，所述传感器包括全球定位系统(GPS)接收器、惯性测量单元(IMU)、激光扫描仪、以及摄像机，并且其中，所述传感器融合模块被配置为将相对测量结果转换成取决于增广状态的测量结果，其中，所述增广状态是用由所述传感器测量的所述旋翼微型航空载具的过去状态增广的当前状态；以及轨迹生成器，所述轨迹生成器用于基于所估算的当前状态以及由用户或所述系统的的高等级规划装置所提供的目标或航路点输入，来生成用于控制所述旋翼微型航空载具的轨迹的时间参数化轨迹，其中，所述高等级规划装置被配置为产生一系列期望的3D位置和偏航角且将得到的路径简化为最小数量的航路点，其中，所述轨迹生成器被配置为使用来自视觉SLAM的姿态估算，将这些航路点转换成里程计框架。

根据本发明的第二方面，提供了一种实现旋翼微型航空载具在室内和室外环境中的自主控制的方法，所述方法包括：对来自不同模态的多个传感器的测量结果进行组合，以便鉴于来自所述传感器的当前测量结果以及所述旋翼微型航空载具的先前估算状态来生成对所述旋翼微型航空载具的当前状态的估算；基于所估算的当前状态以及由用户或高等级规划装置提供的目标或航路点输入，来生成用于控制所述旋翼微型航空载具的轨迹的时间参数化轨迹，其中，所述高等级规划装置产生一系列期望的3D位置和偏航角且将得到的路径简化为最小数量的航路点，其中，所述时间参数化轨迹的生成涉及使用来自视觉SLAM的姿态估算，将这些航路点转换成里程计框架；以及在以下情况下保持所述旋翼微型航空载具的姿态估算方面的平滑性：来自所述传感器中的一个或多个传感器的输出不准确；全球定位系统(GPS)测量结果在一段时间不可用之后变得可用；或者GPS测量结果在一段时间可用之后变得不可用，其中，所述姿态估算方面的平滑性通过转换先前的全局测量的GPS姿态约束(z_k-1)以使转换后的全局测量的GPS姿态约束

与先前的姿态估算(s_k-1)之间的差异最小化而被保持，其中，所述传感器包括全球定位系统(GPS)接收器、惯性测量单元(IMU)、激光扫描仪、以及摄像机，以及其中，所述传感器融合模块被配置为将相对测量结果转换成取决于增广状态的测量结果，其中，所述增广状态是用由所述传感器测量的所述旋翼微型航空载具的过去状态增广的当前状态。

根据本发明的第三方面，提供了一种非暂态计算机可读介质，所述非暂态计算机可读介质具有储存在其上的可执行指令，所述可执行指令当被计算机的处理器执行时控制所述计算机执行包括以下各项的步骤：对来自不同模态的多个传感器的测量结果进行组合，以鉴于来自所述传感器的当前测量结果以及旋翼微型航空载具的先前估算状态来生成对所述旋翼微型航空载具的当前状态的估算；基于所估算的当前状态以及由用户或高等级规划装置提供的目标或航路点输入，来生成用于控制所述旋翼微型航空载具的轨迹的时间参数化轨迹，其中，所述高等级规划装置产生一系列期望的3D位置和偏航角且将得到的路径简化为最小数量的航路点，其中，所述时间参数化轨迹的生成涉及使用来自视觉SLAM的姿态估算，将这些航路点转换成里程计框架；以及在以下情况下保持所述旋翼微型航空载具的姿态估算方面的平滑性：来自所述传感器中的一个或多个传感器的输出不准确；全球定位系统(GPS)测量结果在一段时间不可用之后变得可用；或者GPS测量结果在一段时间可用之后变得不可用，其中，所述姿态估算方面的平滑性通过转换先前的全局测量的GPS姿态约束(z_k-1)以使转换后的全局测量的GPS姿态约束

与先前的姿态估算(s_k-1)之间的差异最小化而被保持，其中所述传感器包括全球定位系统(GPS)接收器、惯性测量单元(IMU)、激光扫描仪、以及摄像机，以及其中，所述传感器融合模块被配置为将相对测量结果转换成取决于增广状态的测量结果，其中，所述增广状态是用由所述传感器测量的所述旋翼微型航空载具的过去状态增广的当前状态。

附图说明

现在将参照附图来解释本文中所描述的主题，在附图中：

图1描绘了装备有IMU、激光扫描仪、立体摄像机、气压测高计、磁力计、以及GPS接收器的1.9kg MAV平台。所有的计算都在具有第三代i3处理器的Intel NUC计算机上机上执行。

图2描绘了具有优先队列的延迟的、无序的测量结果。当Z₄在Z₂之前到达时，Z₂首先被应用到滤波器。Z₄暂时被储存在队列中。Z₁被丢弃，因为其比来自当前状态的t_d旧。直到在将最新的测量结果应用到滤波器的时间，才会传播协方差。直到输入最新的IMU，才传播状态。

图3例示了在k＝5重新获得GPS信号，导致了测量结果Z₅与状态S₅之间的大的差异(图3(a))。姿态图SLAM产生了全局上一致的图示(图3(b))；

图4例示了替代的GPS融合，使经转换的GPS测量结果Z₅与未优化的状态S₅之间的差异最小化。对这种非直接的GPS测量结果的融合将得到平滑的状态估算(S₆与S₅之间的虚线)；

图5描绘了MAV以3.5m/s的最大速度攻击性地进行演习(图5(b))。水平位置也以轻微的偏移很好地与地面实况进行比较(图5(a))；

图6描绘了来自机上摄像机(图6(a)-图6(d))和外部摄像机(图6(e)-图6(h))的图像。注意各种各样的环境，包括开放空间、树木、复杂的建筑物结构、以及室内环境。我们用圆圈突出了MAV的位置。在视频附件中以及在http://mrsl.grasp.upenn.edu/shaojie/ ICRA2014.mp4可以获得实验的录像。

图7描绘了与卫星影像对准的航空载具轨迹。不同的颜色指示感测模态的不同组合。G＝GPS，V＝视觉，并且L＝激光；

图8例示了随着时间的传感器可用性。注意所有传感器都发生故障。这示出了多传感器融合对这种室内-室外任务来说是必须的；

图9例示了当航空载具飞过建筑物密集区时(在200s-300s，图7的顶部)，协方差改变。由于建筑物遮蔽而导致GPS进进出出。当GPS失灵，x、y的协方差以及偏航增加，并且当GPS恢复时，x、y的协方差以及偏航减小。注意机架速度是可观测到的，而不管GPS测量结果，因此其方差仍然是小的。速度方差中的毛刺是因为航空载具直接面对太阳。根据协方差子矩阵的弗罗贝尼乌斯(Frobenius)范数来计算X-Y协方差；

图10描绘了叠加在卫星地图上的航空载具轨迹。该航空载具在树木成行的校园环境中运行，其中，在运行期间存在GPS失灵的高风险；

图11描绘了当MAV自主飞过树木成行的校园环境时的机上摄像机(图11(a))和外部摄像机(图11(b))图像。注意非平凡的光亮环境；

图12是根据本文中所描述的主题的实施例的用于执行多传感器融合的旋翼飞机MAV的框图；

图13是根据本文中所描述的主题的实施例的用于控制旋翼飞机MAV的自主性的多传感器融合的示例性过程的流程图；

图14例示了具有有限的机上计算(Intel Atom 1.6GHz处理器)和感测(具有鱼眼镜头和现成的廉价IMU的两台摄像机)的实验平台。平台质量为740g；

图15例示了在标记出的模块之间具有更新的速率和信息流的系统架构；

图16例示了在VIII-A部分中呈现的轨迹的自主追踪期间的机架速度估算的性能；

图17例示了由于快速平移(图17(a)-图17(b))和快速旋转(图17(a)-图17(d))而造成的对特征追踪性能的影响。所追踪的特征的数量在旋转之后显著减小；

图18例示了模拟的四旋翼航空载具追踪根据一系列的航路点所生成的平滑轨迹。在航路点在20s处改变之后发生轨迹的重新产生；

图19例示了用于MAV航行的基于有限状态机的方法，该方法使得操作者能够在实验期间与航空载具交互；

图20例示了当命令机器人跟随根据矩形图案所生成的平滑轨迹时的期望的、估算的和实际的轨迹；

图21是室内环境的快照图像(图21(a))，以及机上摄像机捕获的图像(图21(b))。应当指出，地面是无特征的，这会对依赖于朝下的摄像机的方法造成挑战；

图22例示了在室内航行实验期间的地图和估算的位置。注意在环路闭合之后经由SLAM所获得的姿态估算的非平凡的不连续性(图22(c))；

图23例示了在闭合环路之后室外环境的最终的3D地图和轨迹；以及

图24包含复杂室外环境中的自主航行的图像。示出了来自外部摄像机和机上摄像机两者的图像。在http://mrsl.grasp.upenn.edu/shaojie/IROS2013.mov可获得实验的视频。

具体实施方式

旋翼微型航空载具(MAV)是用于在禁闭的室内和室外环境中进行监视和搜索以及营救的理想平台，这是因为它们的小尺寸、优良的移动性、以及盘旋能力。在这样的任务中，MAV能够自主飞行来使操作者的工作量最小化是重要的。稳健的状态估算对于自主飞行来说是关键的，尤其是因为MAV的内在快速动态。由于成本和有效载重的限制，大部分MAV装备有低成本的本体感受的传感器(例如，MEMS IMU)，该本体感受的传感器不能用于长期状态估算。因此，外感受性的传感器(例如GPS、摄像机、以及激光扫描仪)通常与本体感受的传感器融合来提高估算的准确性。除了良好发展的基于GPS的航行技术[1,2]，存在最近的关于使用激光扫描仪[3,4]、单目摄像机[5,6]、立体摄像机[7,8]以及RGB-D传感器[9]来在拒绝GPS的环境中对自主飞行进行稳健的状态估算的文献。然而，所有这些方法都依赖于只在某些环境条件下是功能性的单个外感受性的感测模态。例如，基于激光的方法需要结构化的环境、基于视觉的方法要求足够的照明和特征，并且GPS只在室外工作。这使得它们易于在涉及室内-室外过渡的大型环境中失灵，在大型环境中，环境可能显著变化。很显然，在这些场景中，来自GPS、摄像机、以及激光器的多个测量结果可以是可用的，并且对所有这些测量结果的融合产生了增加的估算量的准确性和稳健性。然而，事实上，这种额外信息被忽略，或者用于在传感器套件之间切换[10]。

本工作的主要目的是开发一种模块化的和可扩展的方法，该方法用于集成来自多个异类传感器的有噪声的测量结果，并用于实时地为自主飞行提供平滑的和全局上一致的位置估算，该多个异类传感器以不同的和变化的时间间隔产生绝对观测结果或相对观测结果。第一个关键的贡献(对我们的工作是首要的)是建立于[11]之上的、用于通过采用先前状态的副本增广航空载具状态来融合相关测量结果以创建增广的状态向量的原则性方法，使用滤波器框架来针对该增广的状态向量获得并保持一致的估算。第二个显著的贡献是我们的无迹卡尔曼滤波器(UKF)构想，其中，传播和更新步骤避免了由用于增广的状态的协方差矩阵的半定性所造成的困难。最后，我们采用新的实验平台(图1)证明了结果以例示我们的框架在大规模的、室内-室外自主空中航行实验中的稳健性，该实验涉及在进入和退出两个建筑物的同时以1.5m/s的平均速度以及大约10mph的风速横越超过440米。

接下来，我们呈现我们的工作所基于的先前工作。先于在部分IV中呈现基于UKF的传感器融合方案的关键贡献，我们在部分III中概述了建模框架。在部分VI中，在我们对实验性的平台和试验性的结果的描述中，我们将所有的想法结合在一起。

II先前的工作

我们对应用恒定的计算复杂性的基于滤波的方法(例如卡尔曼滤波器的非线性变型)来融合所有可用的传感器信息感兴趣。我们强调尽管基于SLAM的多传感器融合方法[12,13]产生最佳的结果，但它们对于用于自主控制的目的的实时状态反馈在计算上是昂贵的。

尽管将诸如GPS、气压/激光测高计之类的多个绝对测量结果融合在递归的滤波构想中是直观的，但更多地涉及根据激光测程法或视觉测程法获得的多个相对测量结果的融合。累积相对测量结果与先前的状态估算并将它们融合为伪绝对测量结果[5,14]是常见的。然而，这种融合是次佳的，因为与实际的估算误差相比，得到的全局位置和偏航协方差是不一致地小的。这违背了可观测性属性[6]，其建议这些全局的量事实上是无法观测到的。因此，我们基于状态增广技术来开发我们的方法[11]，以便当应用来自多个传感器的多个相对测量结果时适当考虑状态的不确定性。

我们的目的在于开发允许通过最少的编码和数学推导来容易地增加和移除传感器的模块化框架。我们注意到，在普遍的基于EKF的构想中[5,8]，对于复杂的系统(例如MAV)，雅克比行列式的计算可能是有问题的。因此，我们采用松散耦合的、无导数的无迹卡尔曼滤波器(UKF)框架[1]。从EKF切换到UKF造成了若干挑战，将在部分IV-A中详细说明并解决这些挑战。[15]与我们的工作类似。然而，[15]中的基于EKF的估算装置并不支持多个相对测量结果的融合。

III.多传感器系统模型

我们分别将世界坐标系和体坐标系中的向量定义为(·)^w和(·)^b。为了简洁起见，我们假设对所有的机上传感器进行校准并且该所有的机上传感器附接到机架。MAV的主状态被定义为：

其中，p^w＝[x^w,y^w,z^w]^T是在世界坐标系中的3D位置，Φ^w＝[ψ^w,Θ^w,φ^w]^Τ是表示主体在世界坐标系中的3D方向的偏航、俯仰、以及翻转欧拉角，根据该欧拉角，可以获得表示向量从体坐标系到世界坐标系的旋转的矩阵Rwb。

是体坐标系中的3D速度。

和

是加速度计和陀螺仪的偏移量(都在体坐标系中表示)。

为激光器和/或气压测高计在世界坐标系中的偏移量建模。

我们考虑基于IMU的状态传播模型：

u_t＝[a^b,ω^b]^T

v_t＝[v_a,v_ω,vb_a,vb_ω,vb_z]^T (1)

x_t+1＝f(x_t,u_t,v_t)

其中，u是来自IMU的对机架的线性加速度和角速度的测量结果。

是过程噪声。v_a和v_ω表示与陀螺仪和加速度计相关联的加性噪声。v_ba、v_bω、v_bz为陀螺仪、加速度计和测高计偏移量的高斯随机游走建模。函数f(·)是连续时间动态方程的离散化的版本[6]。

外感受性的传感器通常用于校正状态传播中的误差。下面[11]，我们将测量结果考虑为是绝对的或相对的，取决于潜在的传感器的本质。我们允许任意数量的绝对或相对的测量模型。

A.绝对测量

所有的绝对测量可以以以下方式建模：

z_t+m＝h_a(x_t+m n_t+m) (2)

其中，

是测量噪声，该测量噪声可以是加性的或者可以不是。h_a(·)总体上是非线性函数。绝对测量将当前状态与传感器输出连接。在部分V-B中示出了示例。

B.相对测量

相对测量将当前状态和过去状态与传感器输出连接，其可以被写为：

z_t+m＝h_r(x_t+m,x_t,n_t+m) (3)

在测程法测量状态的两个时间点之间的递增量时，该构想准确地为类似于测程法的算法(部分V-C和部分V-D)的本质建模。我们还注意到，为了避免临时漂移，大多数现有的激光测程法/视觉测程法的算法是基于关键帧的。因此，我们允许与相同的过去状态x_t相对应的多个将来的测量

IV基于UKF的多传感器融合

我们希望设计即使对于无经验的用户也是容易地可扩展的模块化的传感器融合滤波器。这表示用于增加/移除传感器的编码和数学推导的量应当是最小的。普遍的基于EKF的过滤框架的一个缺点是需要计算雅克比矩阵，其被证明为对于复杂的MAV系统是困难的和耗时的。因此，我们采用无导数的基于UKF的方法[1]。UKF的关键是经由传播sigma点来通过非线性函数传播高斯随机向量的近似法。假设

并考虑非线性函数：

y＝g(x), (4)

并且假设：

其中，g(·)是非线性函数，λ是UKF参数。

是平方根协方差矩阵的第i列；其通常经由柯列斯基分解来计算。并且

被称为sigma点。随机向量y的平均值、协方差、以及x与y之间的互协方差可以被近似为：

其中，

和

是对于sigma点的权重。该无迹的转换可用于对状态传播和测量结果更新两者中的协方差保持追踪，因此避免了对基于雅克比行列式的方差近似的需求。

A.用于多个相对测量结果的状态增广

由于相对测量结果取决于当前状态和过去状态两者，因此这违背了在卡尔曼滤波器中测量结果应当仅取决于当前状态的基本假设。解决其的一种方式是通过状态增广[11]，其中，将对过去状态的副本保持在滤波器中。这里，我们呈现了对[11]的扩展，以便处理任意数量的相对测量模型，而具有多个测量结果对应于相同的增广状态的可能性。我们的通用滤波框架允许方便地设置、增加和移除绝对测量模型和相对测量模型。

注意，测量结果不会影响状态x中的所有部分。例如，视觉测程法仅影响6-DOF(自由度)姿态，而不会影响速度或偏移项。我们将第i个增广状态定义为

x_i是X的任意子集。我们定义二进制选择矩阵B_i具有大小n_i×n，以使得x_i＝B_ix。考虑时间常数，在滤波器中存在I个增广状态，连同协方差：

可以通过以下等式来完成新的增广状态x_I+1的增加：

类似地，移除增广状态x_j被给出为：

其中，

并且

更新的增广状态协方差被给出为：

关键帧在类似于测程法的测量模型中的改变是简单地移除增广状态x_i，接着是增加具有相同的B_i的另一个增广状态。由于我们允许对应于相同的增广状态的多个相对测量结果，因此与[11]相反，在测量结果更新后并未删除增广状态(部分IV-D)。

该状态增广构想在EKF设置中工作良好，然而，当我们试图将其应用到UKF时造成了问题。因为新的增广状态的增加(8)实质上是对主状态的副本。得到的协方差矩阵

将不是正定的，并且用于状态传播的科列斯基分解(5)将失败(非唯一)。我们现在希望具有与用于EKF的雅克比矩阵类似的某个矩阵，但不需要明确计算雅克比行列式。

B.用于UKF的雅克比行列式

在[16]中，作者将对UKF的新的解释呈现为线性回归卡尔曼滤波器(LRKF)。在LRKF中，给出对分布

的加权的离散(或者sigma点(6))表示，我们力图找出线性函数(4)的最佳的线性近似y＝Ax+b+e。目的是找出使线性化误差e最小化的回归矩阵A和向量b：

如在[16]中示出的，由以下等式给出最佳的线性回归：

(9)中的线性回归矩阵A作为非线性函数(4)的线性近似。这与EKF构想中的雅克比行列式类似。因此，可以以与EKF类似的方式来执行UKF中的传播和更新步骤。

C.状态传播

观测到在状态传播期间只有主状态改变的事实，我们通过将增广状态和方差(7)分割成以下等式开始：

应用于增广状态(7)的非线性状态传播(1)的线性近似为：

从以上等式我们可以看到完全增广的状态的传播事实上是不必要的，因为唯一的非平凡的回归矩阵与主状态相对应。我们可以经由根据

生成的sigma点来仅传播主状态x，并使用UKF雅克比行列式F_t来更新互协方差

由于主状态的协方差矩阵

总是正定的，因此我们避免了科列斯基分解失败的问题。

由于过程噪声不是加性的，因此我们采用过程噪声来增广主状态，并且根据以下等式生成sigma点：

随后通过将(11)代入到(1)、(5)以及(6)中来正向传播状态。给出直到t的测量结果，我们得到

(在时间t+1的x的估算值)，以及

和

接着(9)，我们知道：

根据(10)来更新传播的增广状态及其方差：

D.测量结果更新

假设在两个测量结果之间存在m个状态传播，并且在最新的测量结果到达时，我们保持

和

考虑到取决于第j个增广状态的相对测量结果(3)，测量结果预测及其线性回归近似可以被写为：

再次，由于在每次测量结果更新中仅涉及主状态和一个增广状态，因此我们可以构建另一个增广状态连同可能的非加性的测量噪声：

在状态传播(12)之后，保证

是正定的，因此如在(5)和(6)中执行sigma点传播是安全的。我们得到

并且

我们可以应用与EKF类似的测量结果更新：

其中，z_t+m是实际上的传感器测量结果。在测量结果更新期间将校正主状态和增广状态两者。我们注意到，对应于非活动的增广状态的H_t+m|t中的条目为零。这可以用于使矩阵乘法加速。

可以简单地通过

并应用对应的绝对测量模型(2)来完成绝对测量结果的融合。

如在图9中示出的，多个相对测量结果的融合引起缓慢增长，但引起全局位置和偏航的无界的协方差。这与这些全局的量是不可观测到的[6]中的结果一致。

E.延迟的、无序的测量结果更新

当融合多个测量结果时，测量结果无序地到达滤波器是可能的，即，对应于较早状态的测量结果在对应于较晚状态的测量结果之后到达。这违背了卡尔曼滤波器的马尔科夫假设。此外，由于传感器处理延迟，因此测量结果可能跟在状态传播后面。

我们通过将测量结果储存在优先队列中来解决这两个问题，其中，队列的顶部对应于最旧的测量结果。100ms的预定义的最大可允许传感器延迟t_d被设置用于我们的MAV平台。直接丢弃与比来自当前状态(通过状态传播生成)的t_d旧的状态相对应的新到达的测量结果。在每次状态传播后，我们核查队列并对队列中所有比t_d旧的测量结果进行处理。优先队列主要作为用于不比来自当前状态的t_d旧的所有测量结果的测量结果再排序机制(图2)。在滤波器中，我们总是利用最新的IMU测量结果来将状态正向传播。然而，我们只按照需要来传播方差。如在图2中例示出的，只从最后的测量结果的时间到当前测量结果的时间传播方差。

F.用于处理全局姿态测量结果的替代方式

在航空载具移动通过环境时，来自GPS和磁力计的全局姿态测量结果可以是可用的。将GPS融合为全局姿态测量结果并生成最佳的状态估算是直观的。然而，这对于现实世界应用可能不是最佳的。在拒绝GPS的环境中运行的航空载具可能遭受累积的漂移。如在图3(a)中例示出的，当航空载具获得GPS信号时，在GPS测量结果与估算状态(z₅—s₅)之间可能存在大的差异。将GPS直接应用为全局测量结果将引起估算(大的线性化误差)和控制(突然的姿态改变)两者上的不期望的行为。

这并不是新的问题并且已经在基于局部坐标系的航行的条件下对于地面车辆进行了研究。然而，[17]假设对车辆的合理地准确的局部估算总是可用的(例如，车轮测程法)。对于MAV并不是这种情形，因为仅采用机上IMU的状态估算在几秒内大幅地漂移离开。IMU与车轮测程法之间的主要差异在于IMU在时间上漂移，而车轮车距法仅在空间上漂移。然而，我们具有能够产生时间上无漂移的估算的相对的外感受性传感器。因此，我们只需要处理所有的相对的外感受性传感器发生故障的情形。因而，我们的目标是将全局GPS测量结果适当地转换成局部坐标系，以渡过相对的传感器故障之间的差距。

考虑具有

为2D姿态的只有姿态的图的SLAM构想。给出来自激光/视觉测程法的增量运动约束d_k、空间闭环约束1_k、以及来自GPS的绝对姿态约束z_k，我们尝试找出姿态图的最佳配置：

如在图3(b)中示出的，可以通过可用的求解程序[18]来找出最佳的姿态图配置。如果在两个节点之间不存在相对的外感受性测量结果，则姿态图是分离的。假设两个姿态图在k—1与k之间分离。

姿态图SLAM提供了未优化的s_k-1状态与经SLAM优化的

状态之间的转换。这种转换可用于将全局GPS测量结果转换为与s_k-1对齐：

其中，

和

是如在[19]中定义的姿态复合运算。可以接着[19]来计算Δ_t-1的协方差

以及随后

的协方差

该构想使

与s_k-1之间的差异最小化，并因此在状态估算中保持平滑。经转换的GPS

仍然被应用为对UKF的绝对测量(图4(a))。

然而，尽管我们的现场实验中的大规模(部分VI)，但我们很难找到累积的漂移足够大而造成具有直接GPS融合的问题的情形。将来，我们将寻求更大规模的实验来验证以上基于局部坐标系的方法的必要性。

V.实施细节

A.实验平台

图1中示出的实验平台是基于Ascending Technologies,GmbH(http:// www.asctec.de/)的鹈鹕四旋翼航空载具。该平台初始地被装备有包括IMU和用户可编程的ARM7微控制器的自动驾驶仪板。机上的主计算单元是具有1.8GHz i3核处理器的IntelNUC，该处理器具有8GB的RAM和120GB的SSD。传感器套件包括ublox LEA-6T GPS模块、Hokuyo UTM-30LX LiDAR以及具有在25Hz捕获752x 480图像的鱼眼镜头的两个mvBlueFOX-1VILC200w灰度级HDR摄像机。我们使用用于坐标系同步的硬件驱动。对机上自动曝光控制器进行微调，以实现在快速光照条件改变期间的快速适应。3-D打印的激光壳体使激光束中的某些激光束重定向，以用于高度测量。平台的总质量为1.87kg。使用机器人操作系统(ROS)(http://www.ros.org)作为连接机器人的中间件来用C++开发整个算法。

B.绝对测量结果

某些机上传感器能够产生绝对测量结果(部分111-A)，这里是它们的细节：

1)GPS和磁力计：

2)激光/气压测高计：

3)伪重力向量：如果MAV在附近盘旋或者以大致恒定的速度移动，我们可以声称加速度计的输出提供重力向量的伪测量结果。假设g＝[0,0,g]^T，我们具有：

C.相对测量结果-基于激光的测程法

我们利用基于激光的测程法，在我们的先前工作[4]中开发了该基于激光的测程法。观测到人造的室内环境大部分都包含垂直墙壁，我们可以作出2.5-D的环境设想。通过该设想，我们可以使用机上的翻滚和俯仰估算装置来将激光扫描仪投影到共同的地平面上。因此，2D扫描匹配可用于估算航空载具的增量水平运动。我们保持局部地图来避免盘旋时的漂移。

其中，

并且

是如在[19]中定义的2-D姿态复合运算。

D.相对测量结果-视觉测程法

我们实施了传统的基于关键帧的视觉测程法算法。基于关键帧的方法具有时间上无漂移的优点。我们选择使用轻重量的角点特征但以高速率(25Hz)运行算法。经由KLT追踪装置来跨图像追踪特征。给定具有一组三角特征点的关键帧，我们运行稳健的迭代式2D-3D姿态估算[8]来估算航空载具相对于关键帧的6-DOF运动。取决于所行进的距离和有效3D点的当前数量来插入新的关键帧。

E.反馈控制

为了获得具有可能大的方向改变的跨不同环境的稳定飞行，我们选择使用具有非线性误差度量标准[20]的位置追踪控制器。100Hz滤波器输出(部分IV)被直接用作为用于控制器的反馈。在我们的实施方式中，高度控制器在MAV的自动驾驶仪板上的ARM处理器上以1KHz运行，而位置追踪控制在主计算机上以100Hz运行。我们实现了设定点轨迹追踪和速度控制两者来允许灵活的运行。

VI实验结果

实施了多个实验来证明我们的系统的稳健性。我们以在装备有运动捕获系统的实验室环境中的定量评估开始。我们随后在两个现实世界的自主飞行实验(包括工业综合体和树木成行的校园)中测试我们的系统。

A.估算装置的性能评估

我们想要对我们的机上估算装置施加限制。因此，我们具有专业的飞行员来以3.5m/s最大速度和多达40°的大姿态来侵略性地驾驶四旋翼航空载具。将机上的状态估算值与来自运动捕获系统的地面实况进行比较。由于室内不存在GPS测量结果，因此我们的系统依赖于来自激光和视觉的相对测量结果的融合。我们确实观测到由于大姿态违背了2.5-D设想(部分V-C)而导致的偶然的激光故障。然而，多传感器滤波器仍然自始至终追踪航空载具状态(图5)。我们并不量化绝对姿态误差，因为其是无界的。然而，机架速度(图5(b))很好地与地面实况进行比较，在x、y、以及z上分别具有{0.1021,0.1185,0.0755}^T(m/s)的标准偏差。

B.在大规模的室内和室外环境中的自主飞行

我们在具有挑战性的工业综合体中测试我们的系统。测试位置跨越多种环境，包括室外开放空间、密集的树木、杂乱的建筑物区域、以及室内环境(图6)。使用机上状态估算装置来自主控制MAV。然而，人类操作者总是具有向航空载具发送高等级的航路点或速度命令的选择。总的飞行时间大约为8分钟，并且航空载具以1.5m/s的平均速度行进445米。如在对准图的轨迹(图7)中示出的，在实验期间，发生频繁的传感器故障(图8)，指示了多传感器融合的必要性。图9示出了在航空载具飞过遮蔽GPS的区域时对方差的评估。通过GPS测量来限制全局x、y和偏航误差，在没有GPS测量的情况下，误差将扩展为无界的。这与可观测性的分析结果相匹配。应当注意，机架速度上的误差并不增大，而不管GPS的可用性。图9中的速度协方差中的尖峰是因为摄像机直接面对阳光。

C.在树木成行的校园中的自主飞行

如在图10中示出的，我们还在树木成行的校园环境中实施了实验。在该环境中的自主飞行是具有挑战性的，因为在航空载具在树木阴影中进进出出时，非平凡的光线条件改变。由于航空载具上方的树木而导致GPS发生故障的风险也是非常高的。基于激光的测程法仅当接近建筑物时工作。总的轨迹长度为281米。

VII.结论和将来的工作

在本公开内容中，我们呈现了用于对来自多个异类传感器的有噪声的测量结果进行集成的模块化的和可扩展的方法，该多个异类传感器以不同的和变化的时间间隔产生绝对观测结果和相对观测结果。我们的方法实时生成用于自主飞行的高速率的状态估算。所提出的方法在机上运行我们新的1.9kg的装备有多个异类传感器的MAV平台。我们证明了我们的框架在大规模的、室内和室外自主飞行实验中的稳健性，这些实验涉及横越工业综合体和树木成行的校园。

在不久的将来，我们想要在我们的MAV平台上集成更高等级的规划和情境感知，以实现跨大规模的复杂环境的完全自主运行。

图12是根据本文中所描述的主题的实施例的例示了用于执行对来自传感器的测量结果进行融合的MAV的框图，该传感器产生绝对测量结果和相对测量结果两者。参考图12，MAV 100包括用于使用一个或多个转子104来控制MAV的运动的一个或多个马达102。如上所述，在本文中所描述的实验中，使用了可从Ascending Technologies获得的鹈鹕四旋翼转子。然而，在不脱离本文中所描述的主题的范围的情况下，可以用其它旋翼飞机来代替。其还包括用于基于传感器输入来控制马达102的操作的控制器106。计算单元108包括融合来自多个传感器的测量结果并向控制器106产生输出信号的传感器融合模块110。在例示出的实施例中，传感器融合模块110从IMU 112、气压测高计114、磁力计116、激光扫描仪118、GPS接收器120、摄像机122、以及气压测高计123接收输入。传感器融合模块110将诸如由激光扫描仪118和摄像机112产生的测量结果之类的相对测量结果转换为取决于如上面所描述的增广状态的测量结果。使用上面所描述的无迹卡尔曼滤波器来组合经转换的测量结果并输出到控制器。被提供为对控制器106的输出的信号作为对用于控制MAV 100的位置、速度、以及加速度的控制器106的反馈。控制器106还从轨迹估算装置124接收输入，该轨迹估算装置对到达用户指定的航路点所需要的MAV 100的轨迹进行估算。

图13是根据本文中所描述的主题的实施例的例示了用于使用多传感器融合来控制旋翼飞机MAV的运动的示例性过程的流程图。参考图13，在步骤200中，从多个不同模态的传感器接收输入。例如，计算单元108和传感器融合模块110可以从图12中例示出的传感器中的一个或多个传感器接收输入，从该传感器的输出在给定时间可用。在步骤202中，将取决于先前状态的由传感器中的某些传感器产生的相对输出测量结果转换成取决于增广状态的测量结果。在上面的部分IV(A)中描述了执行这些转换的过程。在步骤204中，对来自不同传感器的测量结果进行组合并滤波。例如，可以使用无迹卡尔曼滤波器来对测量结果进行组合。在步骤206中，将经组合的测量结果连同用户的航路点输入输出到轨迹生成器。在步骤208中，轨迹生成器的输出用于控制旋翼飞机MAV的运动。

以下参考文献中的每篇参考文献的公开内容都通过引用将其整体并入本文中。

[1]S.J.Julier和J.K.Uhlmann,"A new extension of the kalman filter tononlinear systems,"SPIE会刊，卡达尔(I.Kadar)，Ed.，第3068卷，1997年7月，182-193页。

[2]R.V.D.Merwe,E.A.Wan,以及S.I.Julier,"Sigma-point kalman fil

tersfor nonlinear estimation:Applications to integrated navigation,"AIAA指导、导航和控制会议会刊，普罗维登斯(Providence)，罗德岛州(RI)，2004年8月。

[3]A.Bachrach,S.Prentice、R.He,以及N.Roy,"RANGE-robust au-tonomousnavigation in gps-denied environments,"J.Field Robotics，第28卷，第5期，644-666页，2011。

[4]S.Shen,N.Michael,以及V.Kumar，"Autonomous multi-floor indoornavigation with a computationally constrained MAV,"IEEE机器人和自动化国际会议会刊，上海，中国，2011年5月，20-25页。

[5]S.Weiss,M.W.Achtelik,S.Lynen,M.Chli和R.Siegwart,"Real-time onboardvisual-inertial state estimation and self-calibration of mays in unknownenvironments,"IEEE机器人和自动化国际会议会刊，圣保罗(Saint Paul)，明尼苏达州(MN)，2012年5月，957-964页。

[6]D.G.Kottas,J.A.Hesch,S.L.Bowman,以及S.I.Roumeliotis,"On theconsistency of vision-aided inertial navigation,"实验机器人国际符号(Intl.Sym.on Exp.Robot.)会刊，魁北克，加拿大，2012年6月。

[7]F.Fraundorfer,L.Heng,D.Honegger,G.H.Lee,L.Meier,P.Tanskanen,以及M.Pollefeys,"Vision-based autonomous mapping and exploration using aquadrotor MAV,"IEEE/RSJ智能机器人和系统国际会议会刊，维拉摩拉(Vilamoura)，阿尔加维(Algarve)，葡萄牙，2012年10月。

[8]K.Schmid,T.Tornio,E Ruess,H.Hirsclunuller,以及M.Suppa,"Stereovision based indoor/outdoor navigation for flying robots,"IEEE/RSJ智能机器人和系统国际会议会刊，东京，日本，2013年11月。

[9]A.S.Huang,A.Bachrach,P.Henry,M.Krainin,D.Maturana,D.Fox,以及N.Roy,"Visual odometry and mapping for autonomous flight using an RGB-Dcamera,"机器人研究国际标准(Intl.Spit.of Robot.Research)会刊，弗拉格斯塔夫(Flagstaff)，亚利桑那(AZ)，2011年8月。

[10]T.Tomic,K.Schmid,P.Lutz,A.Domel,M.Kassecker,E.Mair,I.L.Grixa,FRuess,M.Suppa,以及D.Burschka,"Autonomous UAV:Research platform for indoor andoutdoor urban search and rescue,"IEEE机器人自动化期刊，第19卷，第3期，46-56页，2012年。

[11]S.I.Roumeliotis和J.W.Burdick,"Stochastic cloning:A generalizedframework for processing relative state measurements,"IEEE机器人和自动化国际会议会刊，华盛顿，哥伦比亚特区(DC)，2002年5月，1788-1795页。

[12]J.Carlson,"Mapping large urban environments with GPS-aided SLAM,"Ph.D.论文，卡内基梅隆大学(CMU)，匹兹堡，巴拿马(PA)，2010年10月。

[13]D.Schleicher,L.M.Bergasa,M.Ocaa,R.Barea,和E.Lopez,"Real-timehierarchical GPS aided visual SLAM on urban environments,"IEEE机器人和自动化国际会议会刊，神户，日本，2009年6月，4381-4386页。

[14]S.Shen,Y.Mulgaonkar,N.Michael,以及V.Kumar,"Vision-based stateestimation and trajectory control towards high-speed flight with aquadrotor,"机器人：科学与系统期刊，柏林，德国，2013。

[15]S.Lynen,M.W.Achtelik,S.Weiss,M.Chli,以及R.Siegwart,"A robust andmodular multi-sensor fusion approach applied to may navigation,"IEEE/RSJ智能机器人和系统国际会议会刊，东京，日本，2013年11月。

[16]T.Lefebvre,H.Bruyninckx,以及J.D.Schuller,"Comment on"a new methodfor the nonlinear transformation of means and covariances in filters andestimators","IEEE自动控制学报，第47卷，第8期，1406-1409页，2002年。

[17]D.C.Moore,A.S.Huang,M.Walter,以及E.Olson,"Simultaneous local andglobal state estimation for robotic navigation,"IEEE机器人和自动化国际会议会刊，神户，日本，2009年5月，3794-3799页。

[18]R.Kuemmerle,G.Grisetti,H.Strasdat,K.Konolige,以及W.Burgard,"g2o:Ageneral framework for graph optimizations,"the IEEE机器人和自动化国际会议会刊，上海，中国，2011年5月，3607-3613页。

[19]R.Smith,M.Self,以及P.Cheeseman,"Estimating uncertain spatialrelationships in robotics,"IEEE机器人和自动化国际会议会刊，第4卷，瑞利(Rayleigh)，新喀里多尼亚(NC)，1987年5月，850页。

[20]T.Lee,M.Leoky,以及N.McClamroch,"Geometric tracking control of aquadrotor uav on SE(3),"决策与控制国际会议会刊，亚特兰大，佐治亚州(GA)，2010年12月，5420-5425页。

如上所述，根据本文中所描述的主题的实施例的自主旋翼飞机MAV可以包括轨迹生成器以及用于基于旋翼飞机MAV的经估算的当前状态和用户的航路点输入来生成轨迹方案的估算装置124，该轨迹方案用于控制旋翼飞机MAV在飞行期间的轨迹。以下描述例示了根据本文中所描述的主题的一个实施例的可以由轨迹生成器或估算装置124来执行的轨迹方案。

使用四旋翼航空载具在复杂环境中的基于视觉的自主航行

本文中所描述的主题包括呈现了使得仅装备有前向摄像机和廉价IMU的轻重量四旋翼航空载具能够自主航行并有效地为复杂环境绘制地图的系统设计。我们关注于高速率的机上的基于视觉的状态估算和控制、低速率的机上的视觉SLAM的稳健集成、以及在线规划和轨迹生成方法。在诸如突然的航路点改变和大规模的闭环之类的具有挑战性的情形下实现了对平滑轨迹的稳定追踪。经由复杂的室内和室外环境中的实验证明了所提出的系统的性能。

I.引言

四旋翼微型航空载具(MAV)是用于在禁闭的室内和室外环境中进行监视和搜索以及营救的理想平台，这是因为它们的小尺寸和优良的移动性。在这样的任务中，四旋翼航空载具是自主的以使操作者的工作量最小化是重要的。在本工作中，我们对追求轻重量的、现成的四旋翼航空载具仅使用具有关键的控制计算在机器人上实时运行的机上传感器来自动航行复杂的未知室内和室外环境感兴趣。

在过去的几年已经广泛研究了自主空中航行的问题。早期工作[1]-[3]主要依赖于激光扫描仪作为主传感器并且对具有沿着垂直方向并不变化很大(2.5D设想)的结构元件的室内环境中的航空载具进行定位。在[4,5]中使用了增加相当大的有效负载质量的机械化的移动激光扫描仪以用于状态估算。基于视觉的方法(例如在[6]-[8]中的那些方法)分别依赖于朝下的摄像机、立体视觉和朝下的光学流量传感器的组合、以及RGB-D传感器，以实现在室内和/或室外环境中的稳定自主飞行。然而，这些方法并不能利用四旋翼航空载具平台的移动性和可操作性，这是因为由环境结构设想、降低的算法更新速率、或者大的航空载具尺寸引起的实际关注。此外，依赖于朝下的视觉传感器[6,7]的方法通常不能在具有无特征的地面或低高度处的环境中稳健地执行。

在光谱的另一端，存在许多不依赖于度量标准的状态估算[9,10]的成功的反应式航行方法。尽管这些方法实现了具有低计算功率的自主飞行，但当在复杂环境中运行时，它们从根本上限制了MAV的飞行能力。

我们追求使得航空载具能够在未知环境或者未结构化的环境中估算其状态、为环境绘制地图、在地图中规划、并沿着根据该方案开发的轨迹来自主控制的自主航行方法。在线障碍物探测和重新规划允许以大于1m/s的平均飞行速度在静态和动态环境中运行。以这样的速度，由于平台的敏捷性而导致使低延迟状态估算、在线的平滑轨迹生成、以及响应的航空载具控制成为必要。追求该目标所产生的挑战是确保所估算的姿态即使在由于同时定位和绘制地图的闭环期间也保持平稳和一致的需要。传统上，闭环校正直接被融合到高速率的机上状态估算装置中。这造成了所估算的状态的不连续，这可能导致四旋翼航空载具的灾难性坠毁(尤其在快速演习期间)。

在本工作中，我们通过提出采用两个前向摄像机作为主传感器的系统架构、以及在重新规划和闭环期间保持估算的平滑性和控制的稳定性的新颖方法来处理这些要求，这反过来实现了在复杂环境中的有效自主航行。

II.系统设计和方法

我们通过提供对系统架构和方法、以及对于我们的设计所需要的硬件和软件部件的概述开始。在系统框图的逻辑流程(图15)之后的后续部分中给出了对主要部件的详细讨论。

A.硬件平台

实验平台(图14)是基于来自Ascending Technologies的蜂鸟四旋翼航空载具(见http:/www.asctec/de)。该现成的平台伴随装备有廉价IMU和用户可编程的ARM7微控制器的自动驾驶仪板。高等级的机上计算机包括Intel Atom 1.6GHz处理器和1GB RAM。机上计算机与地面站之间的通信经由802.11n无线网络。对平台唯一的新的增加是具有硬件HDR的两个灰度级mvBlueFOX-MLC200w摄像机。所有的摄像机都装备有鱼眼镜头。经由硬件触发来确保摄像机与IMU之间的同步。平台的总重量是740g。

B.软件架构和方法

图15中示出了软件架构。该架构允许我们划分机上的低等级和高等级处理器与机下的地面站之间的计算。在机上的高等级计算机上，基于视觉的估算装置提供了在20Hz的6-DOF姿态估算值。我们采用无迹卡尔曼滤波器(UKF)来融合姿态估算值与IMU测量结果，并生成100Hz的状态估算值，它们直接被用作为用于非线性追踪控制器的反馈。在地面站上，基于立体的视觉SLAM模型为高等级的规划装置生成3D体素栅格图。SLAM模块还提供全局姿态校正。然而，我们并不将该姿态校正与基于视觉的状态估算值直接融合，因为如果发生大规模的闭环，其可能导致显著的姿态不连续。相反，我们使用姿态校正来转换航路点，以使得如果航空载具跟随这些经转换的航路点，则其仍然能够到达全局目标。我们还开发了轨迹生成器，该轨迹生成器以100Hz在机上运行高等级计算机，以便将期望的航路点转换成平滑的多项式轨迹。

III.视觉惯性(VINS)状态估算

A.基于视觉的姿态估算

我们使用对我们先前工作[11]的修改来估算航空载具的6-DOF姿态。注意到尽管我们为平台装备了两个摄像机，但我们并不执行传统的基于立体的状态估算。事实上，我们设置了在20Hz捕获图像的一个摄像机作为主摄像机，而另一个摄像机被配置为在1Hz捕获图像。因为我们不执行高速率的视差计算，因此减少了所需要的计算功率。然而，立体几何允许我们对保留局部地图的规模和姿态估算值的度量信息进行估算。

1)基于单目的姿态估算：对于由主鱼眼摄像机捕获的图像，我们检测FAST角点[12]并使用KLT追踪装置来追踪它们[13]。注意到由于主摄像机的高帧频，因此我们能够直接在失真的鱼眼图像上执行特征追踪，避免了对图像不失真的额外的计算开销。我们利用来自陀螺仪测量结果的短期集成的增量旋转估算并执行2点RANSAC来拒绝追踪异常值。我们提出了解耦的方向和位置估算方案，以便使用尚未三角形化的远处特征。通过回顾历史经由极线约束来估算机器人R_j的方向以使得漂移最小化。假设存在完美的3D局部地图(其包括三角形化的3D特征p_i,i∈I)，则可以通过解出以下线性系统来有效地找出机器人r_j的位置：

其中，u_ij是单位长度特征观测向量；

并且d_i＝‖r_j-1-p_i‖。

一旦找出6-DOF姿态，则可以通过解决以下线性系统来找出特征p_i的位置：

A_ij p_i＝b_ij (2)

其中，A_ij和b_ij代表直到第j个帧的第i个特征的所有观测结果。这是无记忆的问题，因此特征的三角形化的复杂性是恒定的，而不管该具体特征的观测结果的数量。

2)基于立体的规模恢复：上面所描述的姿态估算方法由于在基于单目的三角形化中的累积的误差而遭受规模漂移。每个瞬时的立体测量结果用于规模漂移补偿。假设K表示两个摄像机都看到的一组特征。我们可以将平均场景深度的差异计算为：

其中，

是仅经由立体三角形化而获得3D特征的位置。随后，我们可以通过将b_ij修改为(4)并再次解出(2)来补偿规模的漂移。

B.基于UKF的传感器融合

来自视觉系统的20Hz姿态估算单独并不足以控制机器人。具有延迟的测量补偿的UKF用于估算机器人在100Hz的姿态和速度[14]。系统状态被定义为：

其中，r是机器人的3D位置；q是机器人的3D方向的四元数表示；以及a_b是加速度计测量结果在体坐标系中的偏移量。我们使用传统的基于IMU的过程模型来传播系统状态，并使用包括用于状态校正的6-DOF姿态的线性测量模型。

C.视觉惯性状态估算装置的性能

图16示出了在对预定义的轨迹进行自主追踪(部分VIII-A)期间的VINS估算装置的性能相对于来自威康(Vicon)动作捕获系统²的地面实况的比较。机上速度估算值很好地与威康估算值(所有都被转换为体坐标系)进行比较，具有

(m/s)的标准偏差。然而，缺少VINS估算装置的全局光束法平差由于递归算法而导致经估算的姿态的长期漂移。因此，我们引入了测程法坐标系

来表示这种漂移行为。

IV.视觉SLAM

我们实现了视觉SLAM模型来消除VINS系统中的漂移。视觉SLAM是广泛研究的领域。在小的工作空间中，使用递归滤波器[15]或并行追踪或映射技术[16]的方法产生准确的结果。使用基于姿态图的构想来实现通过单目摄像机[17]或立体摄像机[18]的大规模映射。在我们的系统中，由于有限的机上计算资源、有限的无线传输带宽、以及机上估算装置的准确性，高速率的可视SLAM是非必须的也是不可实行的。因此，我们的视觉SLAM模块以1Hz的最大速率在机下运行。基于姿态图的SLAM后端、连同利用用于宽基线闭环检测的SURF特征[19]的前端以这样的低速率产生稳健的性能。我们稀疏地对经估算的机器人的轨迹进行采样，以便为姿态图生成节点。对于每个节点，我们通过检测并匹配立体图像之间的SURF特征来计算稀疏的3D点。还计算了密度差异图像和密度点云。

我们通过检查落入当前节点的不确定性椭圆体内的节点来检测闭环。我们从最早的候选节点开始针对可能的闭环来检查恒定数量的节点。SURF特征用于测试两个场景之间的相似性。我们使用RANSAC PnP[20]来计算当前节点与闭环候选节点之间的相对转换。执行了在([21]的部分3.4)中提出的硬度测试来验证闭环转换的几何一致性。将通过几何验证的候选转换增加到姿态图。最后，我们将iSAM库用于姿态图优化[22]。一旦找出经优化的姿态图，我们就可以通过将密度点云投射到全局坐标系来构建3D体素栅格图。该图用于高等级规划(部分V)并用于使得人类操作者能够监控实验的进度。经优化的姿态表示世界坐标系中的估算值并用

来表示。

来自视觉SLAM的姿态校正

(其作为测程法坐标系与世界坐标系之间的转换)用公式被表示为：

其中，

是在[23]中定义的姿态更新函数。与传统的方法相反，我们并不使用

作为用于校正VINS系统中的漂移的全局姿态测量结果。相反，我们将

馈送到轨迹生成器(部分VI)中并计算轨迹，这些轨迹被保证是平滑的，即使由于闭环而在视觉SLAM姿态估算值中存在大的不连续性(即，

是大的)。这是我们的系统与现有方法的主要偏差，并且，实现复杂环境中的高速自主航行是关键的。在部分VI中提供了另外的细节。

V.高等级规划

我们采用了两级规划方法。在较高的等级，给出了世界坐标系中的用户指定的航路点，并将四旋翼航空载具看作圆柱体，当在开放式运动规划库(OMPL)[25]中实施时，使用RRT*[24]来生成连接当前机器人位置和期望目标(其包括一系列期望的3D位置和偏航角)的高等级路径。得到的路径被简化为最小数量的K个航路点

并被发送到轨迹生成器(部分VI)以用于进一步的细化。以与地图更新相同的频率(1Hz，部分IV)来针对可能的碰撞检查路径。尽管高等级的规划装置只需要适度的计算资源，但我们在机下运行该规划装置，因为对于高等级的规划所需要的所有信息都来自机下视觉SLAM模块。我们还允许用户绕过该规划装置并明确设置一系列航路点。

VI.轨迹生成

我们首先使用来自视觉SLAM的最新的姿态校正(6)来将来自高等级规划装置的所有航路点转换成测程法坐标系：

如果机器人使用测程法坐标系中的状态估算值用于反馈控制来飞过所有经转换的航路点，则其还将飞过世界坐标系中相同的航路点的组。此外，如果存在大规模的闭环(即，

的大的改变)，则机器人正前往的航路点的组将显著改变。然而，如果我们能够以与机器人的当前状态相等的初始条件来再生成平滑轨迹，则轨迹之间的过度将是平滑的，并且在机上的状态估算装置和控制器内不需要特殊处理。

我们希望确保四旋翼航空载具平滑地穿过所有航路点，而同时保持可靠的状态估算。确定基于视觉的估算的质量的关键条件是追踪性能。通过我们的鱼眼摄像机设置，从图17中可以看到，快速平移对追踪性能影响很小，这是因为大的视野。然而，快速旋转可能容易使图像模糊，导致KLT追踪装置的故障。这种观测促使我们设计使翻转和俯仰的角速度最小化的轨迹。

通过区分四旋翼航空载具的运动方程[26]，可以看到，机架的角速度与加加速度(线性加速度的导数)仿射地相关。因此，我们生成使四旋翼航空载具在水平方向上的加加速度最小化的轨迹。

对于垂直方向。我们期望使马达的RPM改变最小化，RPM改变又与加加速度相对应。如果连接一个航路点的两条线段之间的角度超过阈值，则为了避免来自高等级路径的大偏差，在该航路点的不久之前和不久之后增加中间航路点。我们利用多项式轨迹生成算法[27]，该算法以10nm的数量级的运行时间在机器人上机上运行。可以通过解出以下不受约束的二次规划来找出最佳轨迹：

其中，y是每个航路点处的一些期望的导数值，其可以是自由的或固定的。我们将第一航路点处的位置、速度、加速度固定为机器人的当前状态，以便在再规划和闭环期间保持平滑轨迹。对于最后一个航路点，速度和加速度被设置为零。对于所有其它航路点，只有位置是固定的，并且轨迹生成器将提供速度和加速度简况。可以经由线性映射s＝My来找出多项式轨迹的系数s。

以上轨迹生成方法的限制是预定义航路点之间的行程时间的必要性。由于计算约束，我们并不执行任何迭代的时间优化[27,28]来找出最佳的分段时间，而是使用使分段时间接近于线性轨迹的启发法，该线性轨迹总是在一段的开始和结束处以恒定的加速度从零速度加速并减速到零，并在一段的中间保持恒定速度。该简单的启发法可以有助于避免短的段期间的过度加速，并对于长的段是合理的时间近似。

图18示出了对根据一系列航路点生成的平滑轨迹进行追踪的四旋翼航空载具的模拟。航路点的改变和轨迹再生成在20s处发生。再生成的轨迹平滑地连接到初始轨迹并且四旋翼航空载具能够平滑地切换航路点。

VII.控制

A.位置追踪控制器

对于本工作，我们选择使用具有非线性误差度量标准的位置追踪控制器，这是因为其在高动力运动上的优越性能，该性能涉及大的角度改变和显著的加速度。来自VINS系统(部分III)的100Hz状态估算直接被用作为对于控制器的反馈。在我们的实施方式中，姿态控制器在机器人的自动驾驶仪板上的ARM处理器上以1kHz运行，而位置追踪控制在Atom处理器上以100Hz运行。

B.混合系统控制器

尽管我们的目标是开发完全自主的航空载具，但在实验期间的某个点处，人类操作者可能希望对航空载具具有简单的、但直接的控制。因此，我们开发了基于有限状态机的混合系统控制器(图19)，以允许人机交互。在该控制器中存在四种模式，在部分VI中呈现的控制器LA在位置模式下运行。在任何时间，操作者能够经由远程控制发送输入。这些命令被航空载具解释为运动学的速度命令(其中，没有命令引起盘旋状态)。我们实验性地测试在未经训练的操作者能够在没有直接视线的情况下仅使用1Hz的图像和全局3D地图来控制航空载具的意义上，速度控制模式是容易使用的。盘旋模式作为空闲状态，其中，航空载具等待来自操作者的命令。

VIII.实验结果

我们呈现了三个代表性的实验来证明所提出的系统的性能。第一实验证明了所提出的系统保持全局上一致的追踪的能力。我们提供了与地面实况的比较来量化性能。在第二实验中，机器人以大的回路(大约190m)航行于室内环境并在一次电池充电(少于5分钟的飞行时间)内完成回路。最后，我们呈现了室外航行实验，该实验强调了所提出的系统相对于环境改变和强风干扰的稳健性。

A.对系统性能与地面实况的比较的评估

在本实验中，机器人以大约1m/s自主跟随根据矩形图案所生成的平滑轨迹。来自Vicon的地面实况用于量化全局追踪性能。如从图20(a)和图20(b)中看到的，在VINS状态估算中存在缓慢的位置漂移。然而，来自机下的视觉SLAM的全局校正引起了全局上一致的操作。应当注意，尽管全局闭环清楚地被并入到系统中，但使用VINS状态估算来控制机器人。由于来自视觉SLAM的校正，在测程法坐标系中的期望的平滑轨迹重新生成并随着时间改变。从图20(b)中可以看到，机器人的实际位置以{σ_x,σ_y,σ_z}＝{0.1108,0.1186,0.0834}(m)的标准偏差聚集到期望位置，指示全局上一致的追踪。

B.以大回路的室内环境的航行

我们现在考虑机器人以回路自主航行通过大规模环境的情况。由于环路的大小(大约为190m)，以及短的电池寿命周期(少于5分钟)，因此我们必须实现高速航行，以便完成任务。由于无特征的地面(图21)而导致该环境对使用朝下的摄像机[6,7]的方法造成显著的挑战。然而，通过所提出的系统获得了可靠的状态估算，并且机器人以超过1.5m/s的最大速度和1m/s的平均速度成功地完成了实验。在257s检测到了大规模的闭环(图22(c))，在闭环期间，SLAM姿态和3D地图显著地改变(图22(a)-图22(b))。然而，如在图22(c)中看到的，用于机器人的反馈控制的状态估算贯穿实验保持平滑，并且机器人能够通跟随测程法坐标系中的经转换的航路点而返回到全局原点(部分VI)。

C.在复杂的室外环境中的自主航行

本实验证明了所提出的系统在室外环境中的性能。在巴拿马费城(Philadelphia,PA)的一个典型的冬日实施了该实验，其中，风速上升到20km/hr。总的行进距离为大约170m，并具有166s的总的持续时间(图23)。图24中示出了来自摄像机的快照以及由机上摄像机捕获的图像。注意，室外环境是很大程度上未结构化的，包括树木和植物，证明了系统也在未结构化的环境中运行的能力。

IX.结论和将来的工作

如本文中所描述的，我们提出了一种系统设计，该系统设计采用轻重量的、现成的四旋翼航空载具实现了在复杂环境中的全局上一致的自主航行，该四旋翼航空载具仅使用机上摄像机和IMU作为传感器。我们解决了在具有挑战性的情况(例如，突然的航路点改变和闭环)期间保持平滑的轨迹追踪的问题。在室内环境和室外环境中的在线实验结果被呈现为证明所提出的系统的性能。

集成的基于激光的和/或基于GPS的状态估算方法可以被并入到我们的当前系统中来扩展操作环境并增强系统的稳健性。

参考文献

[1].A.Bachrach,S.Prentice,R.He,以及N,Roy,"RANGE-robust au-tonomousnavigation in gps-denied environments,"现场机器人期刊，第28卷，第5期，644-666页，2011年。

[2].S.Grzonka,G.Grisetti,以及W.Burgard,"A fully autonomous indoorquadrotor,"IEEE机器人学报，第PP卷，第99期，1-11页，2011年。

[3].S.Shen,N.Michael,以及V.Kumar,"Autonomous multi-floor indoornavigation with a computationally constrained MAV,"IEEE机器人与自动化国际会议，上海，中国，2011年5月，20-25页。

[4].S.Scherer,J.Rehder,S.Achar,H.Cover,A,Chambers,S.Nuske,以及S.Singh,"River mapping from a flying robot:state estimation,river detection,and obstacle mapping,"自主机器人，第33卷，第1-2期，189-214页，2012年8月。

[5].A.Kushleyev,B.MacAllister,以及M.Likhachev,"Planning for landingsite selection in the aerial supply delivery,"IEEE/RSJ智能机器人与系统国际会议会刊，旧金山，加州(CA)，2011年9月，1146-1153页。

[6].S.Weiss,M.W.Achtelik,S.Lynen,M.Chli,以及R.Siegwart,"Real-timeonboard visual-inertial state estimation and self-calibration of mays inunknown environments,"IEEE机器人与自动化国际会议，圣保罗明尼苏达州(MN)，2012年5月，957-964页。

[7].F.Fraundorfer,L.Heng,D.Honegger,G.H.Lee,L.Meier,P.Tan-skanen,以及M.Pollefeys,"Vision-based autonomous mapping and exploration using aquadrotor MAV,"IEEE/RSJ智能机器人与系统国际会议会刊，维拉摩拉(Vilamoura)，阿尔加维(Algarve)，葡萄牙，2012年10月。

[8].A.S.Huang,A.Bachrach,P.Henry,M.Krainin,D.Maturana,D.Fox,以及N.Roy,"Visual odometry and mapping for autonomous flight using an RGB-Dcamera,"机器人研究国际符号(Intl.Sym,of Robot.Research)会刊，弗拉格斯塔夫(Flagstaff)，亚利桑那州(AZ)，2011年8月。

[9].C.Bills,J.Chen,以及A.Saxena,"Autonomous MAV flight in indoorenvironments using single image perspective cues,"IEEE机器人与自动化国际会议，上海，中国，2011年5月，5776-5783页。

[10].G.de Croon,C.D.Wagterb,B.Remesb,以及R.Ruijsinkb,"Sub-sampling:Real-time vision for micro air vehicles,"机器人与自动化系统，第60卷，第2期，167-181页，2012年2月。

[11].S.Shen,Y.Mulgaonlcar,N.Michael,及V.Kumar,"Vision-based stateestimation for autonomous rotorcraft MAVs in complex envi-ronments,"IEEE机器人与自动化国际会议会刊，卡尔斯鲁厄(Karlsruhe)，德国，2013年5月，即将出版。

[12].E.Rosten和T.Drummond,"Machine learning for high-speed cornerdetection,"计算机视觉欧洲会议会刊，格拉兹，奥地利，2006年5月。

[13].B.D.Lucas和T.Kanade,"An iterative image registration techniquewith an application to stereo vision,"人工智能国际联合会议会刊，温哥华，加拿大，1981年8月，24-28页。

[14].R.V.D.Merwe,E.A.Wan,以及S.I.Julier,"Sigma-point Kalman fil-tersfor nonlinear estimation:Applications to integrated navigation,"AIAA指导、导航和控制会议会刊，普罗维登斯(Providence)，罗得岛州(RI)，2004年8月。

[15].J.Civera,A.J.Davison,以及J.Montiel,"Inverse depth parameteriza-tion for monocular SLAM,"IEEE机器人学报，第24卷，第5期，932-945页，2008年10月。

[16].G.Klein和D.Murray,"Parallel tracking and mapping for small ARworkspaces,"第六次IEEE和ACM混合和增强现实国际研讨会(ISMAR'07)会刊，奈良，日本，2007年11月。

[17].H.Strasdat,J.M.M.Montiel,以及A.J.Davison,"Scale drift-awarelarge scale monocular SLAM,"机器人：科学与系统会刊，萨拉戈萨(Zaragoza)，西班牙，2010年6月。

[18].C.Mei,G.Sibley,M.Cummins,P.Newman,以及I.Reid,"RSLAM:A system forlarge-scale mapping in constant-time using stereo,"计算机视觉国际期刊，1-17页，2010年6月。

[19].H.Bay,T.Tuytelaars,以及L.V.Gool,"SURF:Speeded up robustfeatures,"计算机视觉欧洲会议会刊，格拉兹，奥地利，2006年5月。

[20].F.Moreno-Noguer,V.Lepetit,以及P.Fua,"Accurate non-iterative 0(n)solution to the PnP problem,"IEEE计算机视觉国际会议会刊，里约热内卢，巴西，2007年10月。

[21].E.B.Olson,"Robust and efficient robotic mapping,"Ph.D.论文，麻省理工学院(MIT)，剑桥，马萨诸塞州(MA)，2008年6月。

[22].M.Kaess,A.Ranganathan,以及F.Dellaert,"iSAM:Incremental smoothingand mapping,"IEEE机器人学报，第24卷，第6期，1365-1378页，2008年12月。

[23].R.Smith,M.Self,以及P.Cheeseman,"Estimating uncertain spatialrelationships in robotics,"IEEE机器人与自动化国际会议会刊，瑞利(Rayleigh)，新喀里多尼亚(NC)，1987年3月，850页。

[24].S.Karaman和E.Frazzoli,"Incremental sampling-based algorithms foroptimal motion planning,"机器人：科学和系统会刊，萨拉戈萨(Zaragoza)，西班牙，2010年6月。

[25].I.A.Sucan,M.Moll,以及L.E.Kavraki,"The Open Motion PlanningLibrary,"IEEE机器人自动化期刊，第19卷，第4期，72-82页，2012年12月，http:// ompl.kavrakilab.org。

[26].N.Michael,D.Mellinger,Q.Lindsey,以及V.Kumar,"The GRASP multiplemicro UAV testbed,"IEEE机器人自动化期刊(IEEE Robot,Autoin.Mag.)，第17卷，第3期，56-65页，2010年9月。

[27].C.Richter,A.Bry,以及N.Roy,"Polynomial trajectory planning forquadrotor flight,"IEEE机器人与自动化国际会议会刊，卡尔斯鲁尔(Karlsruhe)，德国，2013年5月，即将出版。

[28].D.Mellinger和V.Kumar,"Minimum snap trajectory generation andcontrol for quadrotors,"IEEE机器人与自动化国际会议会刊，上海，中国，2011年5月，2520-2525页。

[29].T.Lee,M.Leoky,以及N.McClamroch,"Geometric tracking control of aquadrotor uav on SE(3),"决策与控制国际会议会刊，亚特兰大，佐治亚州(GA)，2010年12月，5420-5425页。

本文中所描述的主题包括本文中所描述的元件或技术的任意组合(即使未被明确描述为组合)。例如，在标题为“采用四旋翼航空载具在复杂环境中的基于视觉的自主航行”的部分中所描述的元件或方法可以与先于该部分所描述的方法或元件中的任何方法或元件进行组合。

将理解的是，在不脱离本文中所描述的主题的范围的情况下，可以改变本文中所描述的主题的各个细节。此外，前述描述仅用于例示的目的，而不是用于限制的目的。

Claims

1.一种实现旋翼微型航空载具在室内和室外环境中的自主控制的系统，所述系统包括：

传感器融合模块，所述传感器融合模块用于对来自不同模态的多个传感器的测量结果进行组合以鉴于来自所述传感器的当前测量结果和先前测量结果以及所述旋翼微型航空载具的先前估算状态来估算所述旋翼微型航空载具的当前状态，其中，所述传感器融合模块被配置为在以下情况下保持在对所述旋翼微型航空载具的姿态估算方面的平滑性：一个或多个传感器提供不准确的信息；全球定位系统(GPS)测量结果在一段时间可用之后变得不可用；或者GPS测量结果在一段时间不可用之后变得可用，其中，所述姿态估算方面的平滑性通过转换先前的全局测量的GPS姿态约束(z_k-1)以使转换后的全局测量的GPS姿态约束

与先前的姿态估算(s_k-1)之间的差异最小化而被保持，其中，所述传感器包括全球定位系统(GPS)接收器、惯性测量单元(IMU)、激光扫描仪、以及摄像机，并且其中，所述传感器融合模块被配置为将相对测量结果转换成取决于增广状态的测量结果，其中，所述增广状态是用由所述传感器测量的所述旋翼微型航空载具的过去状态增广的当前状态；以及

轨迹生成器，所述轨迹生成器用于基于所估算的当前状态以及由用户或所述系统的高等级规划装置所提供的目标或航路点输入，来生成用于控制所述旋翼微型航空载具的轨迹的时间参数化轨迹，其中，所述高等级规划装置被配置为产生一系列期望的3D位置和偏航角且将得到的路径简化为最小数量的航路点，其中，所述轨迹生成器被配置为使用来自视觉SLAM的姿态估算，将这些航路点转换成里程计框架。

2.根据权利要求1所述的系统，其中，所述传感器包括以下部件中的至少一个：气压测高计、磁力计和朝下的光学传感器。

3.根据权利要求1所述的系统，其中，所述传感器融合模块被配置为使用无迹卡尔曼滤波器(UKF)来对来自不同模态的所述传感器的测量结果进行组合，其中，所述无迹卡尔曼滤波器包括过滤框架，所述过滤框架被配置为实现所述传感器的测量模型的增加和移除。

4.根据权利要求3所述的系统，其中，所述传感器融合模块被配置为使用所述滤波器中保留的所述过去状态的副本增广的所述相对测量结果来估算所述旋翼微型航空载具的当前状态。

5.根据权利要求3所述的系统，其中，所述传感器融合模块被配置为从所述滤波器中移除过去状态且将新增广的状态增加至所述滤波器。

6.根据权利要求3所述的系统，其中，所述传感器融合模块被配置为对来自所述传感器的测量结果进行融合，所述测量结果是无序地到达所述滤波器的，其中，所述无序地到达的测量结果包括与在对应于所述旋翼微型航空载具的较晚状态的测量结果之后到达所述滤波器的所述旋翼微型航空载具的较早状态对应的测量结果。

7.一种实现旋翼微型航空载具在室内和室外环境中的自主控制的方法，所述方法包括：

对来自不同模态的多个传感器的测量结果进行组合，以便鉴于来自所述传感器的当前测量结果以及所述旋翼微型航空载具的先前估算状态来生成对所述旋翼微型航空载具的当前状态的估算；

基于所估算的当前状态以及由用户或高等级规划装置提供的目标或航路点输入，来生成用于控制所述旋翼微型航空载具的轨迹的时间参数化轨迹，其中，所述高等级规划装置产生一系列期望的3D位置和偏航角且将得到的路径简化为最小数量的航路点，其中，所述时间参数化轨迹的生成涉及使用来自视觉SLAM的姿态估算，将这些航路点转换成里程计框架；以及

在以下情况下保持所述旋翼微型航空载具的姿态估算方面的平滑性：来自所述传感器中的一个或多个传感器的输出不准确；全球定位系统(GPS)测量结果在一段时间不可用之后变得可用；或者GPS测量结果在一段时间可用之后变得不可用，其中，所述姿态估算方面的平滑性通过转换先前的全局测量的GPS姿态约束(z_k-1)以使转换后的全局测量的GPS姿态约束

与先前的姿态估算(s_k-1)之间的差异最小化而被保持，

其中，所述传感器包括全球定位系统(GPS)接收器、惯性测量单元(IMU)、激光扫描仪、以及摄像机，以及

其中，所述传感器融合模块被配置为将相对测量结果转换成取决于增广状态的测量结果，其中，所述增广状态是用由所述传感器测量的所述旋翼微型航空载具的过去状态增广的当前状态。

8.根据权利要求7所述的方法，其中，所述传感器还包括以下部件中的至少一个：气压测高计和磁力计。

9.根据权利要求7所述的方法，其中，组合所述测量结果包括：无迹卡尔曼滤波器(UKF)对来自不同模态的所述传感器的测量结果进行组合，其中，所述无迹卡尔曼滤波器包括过滤框架，所述过滤框架实现所述传感器的测量模型的增加和移除。

10.根据权利要求9所述的方法，其中，估算所述当前状态包括：使用所述滤波器中保留的过去状态的副本增广的相对测量结果。

11.根据权利要求9所述的方法，包括：响应于新的增广状态的增加而从所述滤波器中移除增广状态，其中所述新的增广状态具有与所述滤波器中存储的过去状态的二进制选择矩阵相对应的二进制选择矩阵，其中，所述二进制选择矩阵是用于选择当前状态的状态向量的分量，所述分量将被选择用于增广状态的状态向量。

12.根据权利要求9所述的方法，包括：对来自所述传感器的测量结果进行融合，所述测量结果是无序地到达所述滤波器的，其中，所述无序地到达的测量结果包括与在对应于所述旋翼微型航空载具的较晚状态的测量结果之后到达所述滤波器的所述旋翼微型航空载具的较早状态对应的测量结果。

13.一种非暂态计算机可读介质，所述非暂态计算机可读介质具有储存在其上的可执行指令，所述可执行指令当被计算机的处理器执行时控制所述计算机执行包括以下各项的步骤：

对来自不同模态的多个传感器的测量结果进行组合，以鉴于来自所述传感器的当前测量结果以及旋翼微型航空载具的先前估算状态来生成对所述旋翼微型航空载具的当前状态的估算；