[发明专利]基于SIP和EKF融合的多旋翼无人机鲁棒姿态估计

说明书

本发明提出了一种基于SIP和EKF相融合的姿态解算方法，属于无人机状态估计领域，主要用于解决捷联惯性测量单元(IMU)噪声大、精度低的缺点和常规的姿态解算算法精度不高等问题。针对多旋翼无人机的姿态估计问题，还提出了一种利用机载摄像机进行姿态估计的方法。然而，EKF的精度和鲁棒性在一定程度上受到现有的适用于许多刚体的线性恒速过程模型的限制。为此，本发明提出了一种具有多旋翼无人机特点的非线性恒速过程模型，在考虑EKF实现的基础上，进一步设计了一种能够处理任意数量特征点的通用对应方法。所提出的SIP和EKF相融合的方法比现有的滤波方法具有更好的抗噪声和抗遮挡能力，并有效提高了姿态角的解算精度。

技术领域

本发明属于无人机状态估计领域，具体涉及一种基于生存信息势(SIP)和扩展卡尔曼滤波(EKF)融合的姿态解算方法。

背景技术

目前，多旋翼无人机正被广泛应用于许多领域。例如，Amazon已经设计并测试了一个未来的交付系统prime Air，该系统使用多旋翼无人机交付货物。PRENAV使用一个地面机器人和一个多旋翼无人机协同(地空协同)执行工业检查。准确可靠的姿态估计是这些无人机自主运行的基础问题。虽然全球定位系统(GPS)是常用的，但不适用于城市或建筑物内部等没有GPS的情况。相比之下，基于视觉的导航方法不依赖GPS，可以在近距离内提供高精度的姿态。针对小型多旋翼无人机的中央处理器(CPU)容量有限的特点，本发明仅利用单目相机对多旋翼无人机进行视觉伺服(基于视觉的控制)姿态估计。在现有的单目运动捕捉系统中，如Vicon和OptiTrack在单相机模式下，往往会发生由于遮挡或相机视场(FOV)引起的标记缺失，当检测到的标记数小于3时，往往无法获得准确的姿态信息。因此，本发明拟从以下两个方面解决这一问题：(1)使用鱼眼相机，因为它可以提供一个非常大的FOV(约180°)的图像，而不需要外部镜子或旋转设备；(2)提出一种含较少未知数的非线性恒速过程模型。

在计算机视觉中，从n个三维-二维点对应关系估计标定相机的位姿，称为透视-n点(PnP)问题。刚体姿态估计问题很容易转化为PnP问题。因此，现有的刚体(包括多旋翼无人机)姿态估计方法一般可分为三类：线性方法、迭代方法和递归方法。线性方法简单直观，但对噪声敏感。迭代法具有较高的精度和鲁棒性，但计算量比线性方法大，容易陷入局部极小值。递归方法依赖于时间滤波方法，尤其是卡尔曼滤波。这些方法准确、高效，适用于图像序列处理。由于测量模型在系统状态下是非线性的(由摄像机成像模型决定)，因此常采用扩展卡尔曼滤波器(EKF)对刚性物体进行视觉伺服。然而，EKF过程模型是一个线性恒速过程模型，适用于许多刚体，对于多旋翼无人机来说不是一个非常合适的模型(需要检测更多的标记才能观察状态)。另一方面，大多数递归方法都是基于传统摄像机的，它们服从针孔投影模型，具有有限的FOV。但这些方法不能直接应用于鱼眼相机的姿态估计，因为鱼眼相机可以提供一个非常大的FOV(约180°)以及针孔相机模型不再有效。

为了解决过程建模问题，提出了一种具有多旋翼无人机特点的非线性恒速过程模型。在此基础上，提出了一种结合过程模型和单眼视觉信息下的融合SIP和EKF的方法。该过程模型提高了姿态估计对噪声和遮挡的鲁棒性。本发明从可观测性的角度出发，将摄像机观测到的特征点的最小数量由3个减少到2个。此外，由于采用了通用的相机模型，所提出的融合最大熵和EKF的方法适用于传统相机以及鱼眼相机。本发明的研究成果有助于提高单摄像机运动捕捉系统或地空协同系统的鲁棒性。此外，大多数基于视觉的EKF方法的实现需要知道3D-2D点对应关系。对于P4P这种单目姿态估计的对应算法，如果在图像中检测到的特征点少于4个(如遮挡或相机FOV引起的)，该算法就会失败。这意味着EKF不能在这些情况下工作。为了解决对应问题，本发明还提出了一种将EKF与P4P算法相结合的通用对应方法，该方法可以处理任意数量的图像点。

发明内容