CN110598370B

CN110598370B - 基于sip和ekf融合的多旋翼无人机鲁棒姿态估计

Info

Publication number: CN110598370B
Application number: CN201910993971.3A
Authority: CN
Inventors: 李志鹏; 程兰; 任密蜂; 阎高伟; 续欣莹
Original assignee: Taiyuan University of Technology
Current assignee: Taiyuan University of Technology
Priority date: 2019-10-18
Filing date: 2019-10-18
Publication date: 2023-04-14
Anticipated expiration: 2039-10-18
Also published as: CN110598370A

Abstract

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

Description

基于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算法相结合的通用对应方法，该方法可以处理任意数量的图像点。

发明内容

由于现有的应用在无人机姿态角上的数据融合算法各有其不足之处，比如互补滤波原理简单，能够滤去噪声和抑制漂移，但不容易确定合适的高通和低通截止频率，因此精度不高；共轭梯度法滤波和互补滤波相结合在姿态解算中的应用，存在迭代计算量和精度的平衡问题；扩展卡尔曼滤波(EKF)和无迹卡尔曼滤波(UKF)是卡尔曼滤波对非线性系统的滤波方法，EKF存在线性化误差和截断误差，对一般的非线性系统影响不大；UKF在收敛速度和估计精度中要高于EKF，但其计算量要比EKF大，并且它们的系统噪声和观测噪声不容易确定，解算的姿态角容易受到噪声干扰。故本发明用于解决该问题，为了进一步减小估计误差，提出了一种基于SIP和EKF相融合的姿态解算方法。该方法主要包括两个部分，第一部分为SIP和EKF的基本介绍及算法流程；第二部分为二者的融合阶段。

本发明是采用如下技术方案实现的：基于SIP和EKF融合的多旋翼无人机鲁棒姿态估计，具体包括下述内容：

(1)、构建模型，具体步骤如下：

11)、建立相机坐标系(O^c-X^cY^cZ^c)和图像坐标系(o-xy)；

12)、设P＝[X,Y,Z]^T，P点方向上的单位向量可表示为

或

13)、利用相机通用模型得到p或p’的图像坐标

然后求出p或p’的像素坐标

其中，m_u,m_v分别表示单位距离在水平方向上的像素个数和单位距离在垂直方向上的像素个数；

14)、建立机体坐标系(O^b-X^bY^bZ^b)和世界坐标系(O^w-X^wY^wZ^w)；

15)、用

表示机体坐标系和世界坐标系之间的相对姿态，T＝[X Y Z]^T表示机体坐标系和世界坐标系之间的相对位置坐标，

表示机体坐标系和世界坐标系之间的相对取向，

其中，

分别表示俯仰角、偏航角和横滚角；

16)、然后将P_j从体坐标系到世界坐标系的转换描述为

为世界坐标系中第j个特征点的坐标向量，

为体坐标系中第j个特征点的坐标向量，

为转换矩阵，将P_j从世界坐标系到相机坐标系的转换描述为

为相机坐标系中第j个特征点的坐标向量，然后联立

和

可得

其中，

17)、最后将P_j映射到

处，其坐标可根据13)求出；

18)、已知三维到二维特征对应的

则定义测量矢量与状

态矢量之间的测量模型可以表示为z_k＝g(x_k)+v_k，其中，

是nF个特征点的测量矢量，v_k是测量噪声矢量；

19)、将无人机非线性恒速过程模型描述为

其中，

将定义的测量模型与其相结合，相比于测量模型与无人机线性恒速过程模型x_k＝Ax_k-1+γ_k相结合，估计误差会明显减小；

(2)基于SIP和EKF融合的姿态解算方法，主要步骤如下：

21)、利用经验SIP计算SIP；

22)、因为存在3个姿态角参数

当SIP的值达到最小时，有无穷多个解，结合矩量法得到确定的一组解；

23)、建立系统的状态方程和观测方程：

其中，

24)、状态一步预测：

其中，

是状态变量X(k)的估计，F_k-1是状态一步转移矩阵；

25)、状态一步预测协方差矩阵：

其中，P(k)是状态协方差矩阵P(k-1)的估计，Q为系统噪声协方差矩阵；

26)、卡尔曼增益更新：

其中，R为观测噪声协方差矩阵；

27)、状态更新：

28)、状态协方差矩阵更新：P(k)＝(I-K(k)H_k)P(k)；

29)、通过SIP解算得到的姿态角，再结合EKF的状态更新方程和观测更新方程得到最后的姿态角

进一步的讲，13)中的相机通用模型为r(θ)＝k₁θ+k₂θ³+k₃θ⁵+k₄θ⁷+k₅θ⁹+…。

进一步的讲，16)、中的P_j为多旋翼无人机刚性固定的第j个特征点。

进一步的讲，16)、中的转换矩阵为

进一步的讲，21)中利用经验SIP计算SIP的详细过程为

其中，重力因素

F为生存函数，I为信息势，之后为演算过程。

进一步的讲，29)、中的状态更新方程和观测更新方程由状态方程和观测方程更新得到。

本发明的有益效果如下：

1、本发明提出了一种具有多旋翼无人机特点的非线性恒速过程模型，此模型与测量模型相结合可以有效地减小估计误差；

2、本发明提出了一种将EKF与P4P算法相结合的通用对应方法，该方法可以处理任意数量的特征点。

3、在新工艺模型和通用相机模型的基础上，提出了一种适用于通用镜头相机的EKF方法；

4、与现有滤波方法相比，本发明提出的融合SIP和EKF的方法对噪声和遮挡具有更强的鲁棒性。

附图说明

图1为一般相机模型。

图2为不同坐标系的说明。

图3EKF流程图。

图4SIP和EKF结合的原理图。

具体实施方式

第一步，建立相机坐标系和图像坐标系，利用相机模型，导出成像点的图像坐标以及像素坐标；

第二步，再建立机体坐标系和世界坐标系，用

表示机体坐标系和世界坐标系之间的相对姿态，T＝[X Y Z]^T表示相对位置坐标，

表示相对取向(俯仰角θ、偏航角

和横滚角φ)，我们一开始知道的是体坐标系下特征点的坐标，然后利用转换矩阵

将其转化为世界坐标系下特征点的坐标，进而转化为相机坐标系下特征点的坐标，最后求出图像坐标系下特征点的二维坐标，再转化为二维像素坐标；

第三步，构建测量矢量与状态矢量之间的测量模型z_k＝g(x_k)+v_k，g(x_k)可通过第一步和第二步求出，再结合非线性恒速过程模型

从而达到更好的减小估计误差的目的。

基于SIP和EKF融合的姿态解算方法，主要步骤如下：

步骤S1、生存信息势(SIP)的基本思想是将传统表达式中的密度函数替换为生存函数，这个新的定义似乎更自然、更合理，因为生存函数(分布函数)比概率密度函数(PDF)更规则、更一般。从某种意义上说，这与信息势(IP)和Renyi熵的关系是平行的。作为一种自适应准则，SIP相对于IP具有以下优点：(1)在连续域和离散域具有一致的定义；(2)它不是移位不变的，其值随分布位置而变化；(3)从样本数据可以很容易地计算(不需要核计算和核宽度的选择)，估计渐近收敛于真实值；(4)由于分布函数比密度函数更有规则，因此它是一个更有力的度量。

步骤S11)、对于随机变量X，α阶SIP可表示为：

其中，

是随机向量|X|的多元生存函数，并且它与姿态角参数

有关；

步骤S12)、通常用以下经验SIP来计算SIP：

其中，N为样本数量，当x为标量时S＝1；

步骤S13)、因为存在3个姿态角参数

当SIP的值达到最小时，可能有无穷多个解，我们结合矩量法(MM)可以得到确定的一组解。

步骤S2、EKF算法流程，如图3所示，具体步骤如下：

步骤S21)、建立系统的状态方程和观测方程：

步骤S22)、状态一步预测：

是状态变量X(k)的估计，F_k-1状态一步转移矩阵；

步骤S23)、状态一步预测协方差矩阵：

P(k)是状态协方差矩阵P(k-1)的估计，Q为系统噪声协方差矩阵；

步骤S24)、卡尔曼增益更新：

R为观测噪声协方差矩阵；

步骤S25)、状态更新：

步骤S26)、状态协方差矩阵更新：P(k)＝(I-K(k)H_k)P(k)。步骤S3、SIP和EKF的融合阶段，如图4所示，具体步骤如下：

步骤S31)、通过SIP将参数估计问题转化为熵优化问题，并结合MM解算得到一组姿态估计量

步骤S32)、将SIP解算得到的姿态角与EKF的状态更新方程和观测更新方程相结合，得到最后的姿态角

下面结合附图对本发明的具体实施进行详细说明。

步骤S1、建立相机坐标系和图像坐标系，利用相机模型，导出成像点的图像坐标以及像素坐标。

步骤S11)、建立相机坐标系(O^c-X^cY^cZ^c)和图像坐标系(o-xy)，如图1所示；

步骤S12)、设P＝[X,Y,Z]^T，那么P点方向上的单位向量可表示为

或

步骤S13)、利用相机通用模型r(θ)＝k₁θ+k₂θ³+k₃θ⁵+k₄θ⁷+k₅θ⁹+…，可以得到p(鱼眼相机成像点)或p’(针孔相机成像点)的图像坐标

然后求出p或p’的像素坐标

m_u,m_v分别表示单位距离在水平方向和垂直方向上的像素个数。

步骤S2、再建立机体坐标系和世界坐标系，用

表示相对取向，我们一开始知道的是体坐标系下特征点的坐标，然后利用转换矩阵

将其转化为世界坐标系下特征点的坐标，进而转化为相机坐标系下特征点的坐标，最后求出图像坐标系下特征点的二维坐标，再转化为二维像素坐标。

步骤S21)、建立机体坐标系(O^b-X^bY^bZ^b)和世界坐标系(O^w-X^wY^wZ^w)，如图2所示；

步骤S22)、用

表示机体坐标系和世界坐标系之间的相对姿态，然后将P_j(多旋翼无人机刚性固定的第j个特征点)从体坐标系到世界坐标系的转换描述为

为世界坐标系中第j个特征点的坐标向量，

为体坐标系中第j个特征点的坐标向量，

为转换矩阵。然后将P_j从世界坐标系到相机坐标系的转换描述为

为相机坐标系中第j个特征点的坐标向量。然后联立

和

可得

其中，

注：转换矩阵

步骤S23)、最后将P_j映射到

处，其坐标可根据步骤S1求出。

步骤S3、构建测量矢量与状态矢量之间的测量模型z_k＝g(x_k)+v_k，再结合非线性恒速过程模型

从而达到减小估计误差的目的。

步骤S31)、如果已知三维到二维特征对应的

则定义测量矢量与状态矢量之间的测量模型可以表示为z_k＝g(x_k)+v_k，

是nF个特征点的测量矢量，v_k是测量噪声矢量；

步骤S32)、将无人机非线性恒速过程模型描述为

其中，

将定义的测量模型与其相结合，相比于测量模型与无人机线性恒速过程模型x_k＝Ax_k-1+γ_k相结合，估计误差会明显减小。

以上所述仅为本发明的具体实施例，但本发明所保护范围的结构特征并不限于此，任何本领域的技术人员在本发明的领域内，所作的变化或修饰皆涵盖在本发明的专利范围内。

Claims

1.基于生存信息势和EKF融合的多旋翼无人机鲁棒姿态估计，其特征在于：具体包括下述内容：

(1)、构建模型，具体步骤如下：

11)、建立相机坐标系(O^c-X^cY^cZ^c)和图像坐标系(o-xy)；

12)、设P＝[X,Y,Z]^T，P点方向上的单位向量可表示为

或

13)、利用相机通用模型r(θ)＝k₁θ+k₂θ³+k₃θ⁵+k₄θ⁷+k₅θ⁹+…，可以得到p鱼眼相机成像点或p’针孔相机成像点的图像坐标

然后求出p或p’的像素坐标

其中，m_u,m_v分别表示单位距离在水平方向和垂直方向上的像素个数，u₀,v₀表示像素坐标的初始值；

14)、建立机体坐标系(O^b-X^bY^bZ^b)和世界坐标系(O^w-X^wY^wZ^w)；

15)、用

表示机体坐标系和世界坐标系之间的相对取向，

其中，θ,

φ分别表示俯仰角、偏航角和横滚角；

16)、然后将P_j多旋翼无人机刚性固定的第j个特征点从体坐标系到世界坐标系的转换描述为

为世界坐标系中第j个特征点的坐标向量，

为体坐标系中第j个特征点的坐标向量，

为转换矩阵，将P_j从世界坐标系到相机坐标系的转换描述为

为相机坐标系中第j个特征点的坐标向量，然后联立

和

可得

其中，

17)、最后将P_j映射到

处，其坐标可根据13)求出；

18)、已知三维到二维特征对应的

其中，

是nF个特征点的测量矢量，v_k是测量噪声矢量；

19)、将无人机非线性恒速过程模型描述为

其中，x_k＝

(2)基于生存信息势和EKF融合的姿态解算方法，主要步骤如下：

21)、利用经验生存信息势计算生存信息势：

其中，N为样本数量，当x为标量时S＝1；

22)、因为存在3个姿态角参数θ,

φ，当生存信息势的值达到最小时，有无穷多个解，结合矩量法得到确定的一组解；

23)、建立系统的状态方程和观测方程：

其中，状态一步转移矩阵

观测矩阵

24)、状态一步预测：

其中，

是状态变量X(k)的估计；

25)、状态一步预测协方差矩阵：

26)、卡尔曼增益更新：

其中，R为观测噪声协方差矩阵；

27)、状态更新：

28)、状态协方差矩阵更新：P(k)＝(I-K(k)H_k)P(k)；

29)、通过生存信息势解算得到的姿态角，再结合EKF的状态更新方程和观测更新方程得到最后的姿态角

2.根据权利要求1所述的基于生存信息势和EKF融合的多旋翼无人机鲁棒姿态估计，其特征在于：13)中的相机通用模型为r(θ)＝k₁θ+k₂θ³+k₃θ⁵+k₄θ⁷+k₅θ⁹+...。

3.根据权利要求1所述的基于生存信息势和EKF融合的多旋翼无人机鲁棒姿态估计，其特征在于：16)中的P_j为多旋翼无人机刚性固定的第j个特征点。

4.根据权利要求1所述的基于生存信息势和EKF融合的多旋翼无人机鲁棒姿态估计，其特征在于：16)中的转换矩阵为

5.根据权利要求1所述的基于生存信息势和EKF融合的多旋翼无人机鲁棒姿态估计，其特征在于：21)中利用经验生存信息势计算生存信息势的详细过程为

其中，重力因素

F为生存函数，I为信息势，之后为演算过程。

6.根据权利要求1所述的基于生存信息势和EKF融合的多旋翼无人机鲁棒姿态估计，其特征在于：29)中的状态更新方程和观测更新方程由状态方程和观测方程更新得到。