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

权利要求书

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)、用表示机体坐标系和世界坐标系之间的相对姿态，T＝[X Y Z]^T表示机体坐标系和世界坐标系之间的相对位置坐标，表示机体坐标系和世界坐标系之间的相对取向，

其中，θ,φ分别表示俯仰角、偏航角和横滚角；

16)、然后将P_j多旋翼无人机刚性固定的第j个特征点从体坐标系到世界坐标系的转换描述为为世界坐标系中第j个特征点的坐标向量，为体坐标系中第j个特征点的坐标向量，为转换矩阵，将P_j从世界坐标系到相机坐标系的转换描述为为相机坐标系中第j个特征点的坐标向量，然后联立和可得其中，

17)、最后将P_j映射到处，其坐标可根据13)求出；

18)、已知三维到二维特征对应的则定义测量矢量与状态矢量之间的测量模型可以表示为z_k＝g(x_k)+v_k，

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

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

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

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

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

22)、因为存在3个姿态角参数θ,φ，当生存信息势的值达到最小时，有无穷多个解，结合矩量法得到确定的一组解；

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

其中，状态一步转移矩阵观测矩阵

24)、状态一步预测：

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

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

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

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₅θ⁹+...。