一种对无人机的噪声协方差进行估算的方法与流程

本发明涉及无人机
技术领域：
，更具体地说，涉及一种对无人机的噪声协方差进行估算的方法。
背景技术：
：无人驾驶飞机简称“无人机”，是利用无线电遥控设备和自备的程序控制装置操纵的不载人飞机，其用途广泛，被广泛应用在军用和民用领域，例如空中预警，农业、地质、气象、电力、抢险救灾、视频拍摄等行业。作为利用自备程序控制装置操纵的无人机，其具有强耦合、非线性、控制难度高等特性，动力学建模较为复杂。无人机的控制和操纵面临的干扰包括：重力等外部干扰力矩，以及无人机动力学模型中的系统造声以及量测方程中的观测噪声等来自无人机自身内部的干扰，以上多源干扰需要通过高精度模型和算法进行估算，从而达到高精度控制。然而，现有技术中，处理无人机过程噪声和量测噪声的滤波算法，例如卡尔曼滤波，在噪声特性未知时，一般使用经验值或通过调试的方法得到噪声的协方差矩阵值，即，现有技术中，对无人机动力学模型中的过程噪声的协方差以及量测方程中的观测噪声的协方差均需要通过调试方能达到最佳效果。因此，现有技术中存下如下缺陷：噪声的协方差矩阵需要通过经验值或者调试的方法方可获得，需要较有经验的操作人员，对操作人员的要求较高，且若调试所给的噪声协方差偏差过大，会直接导致滤波值在递推的某一过程中，甚至整个过程中越来越偏离实际值，导致滤波发散，造成无人机的飞行偏离目标结果。odelson于2003年提出的自协方差最小二乘理论方法可以有效地估计出噪声的协方差矩阵，节省调试的时间。自协方差最小二乘理论算法目前常用于化工行业的噪声估计，尚未用于无人机领域。rajamani等人于2011年提出了针对非线性系统的线性时变自协方差最小二乘方法。本发明将以上方法结合无人机动力学模型对无人机的过程噪声和观测噪声进行了估算。技术实现要素：本发明的技术解决问题是：克服了现有的处理无人机过程噪声和量测噪声的滤波算法中，在噪声特性未知时，需要使用经验值或通过调试的方法得到噪声的协方差矩阵值，对操作人员要求较高，且易造成偏差的问题，设计一种能够自动对过程噪声和观测造成的协方差矩阵值进行估值的估算方法。本发明解决其技术问题所采用的技术方案是：提供一种对无人机的噪声协方差进行估算的方法，其特征在于，利用自协方差最小二乘理论方法对无人机动力学模型中的过程噪声以及量测方程中的观测噪声的特性进行估算，包括以下步骤：s1、建立无人机动力学模型；s2、利用扩展卡尔曼滤波器结合控制量和观测值将无人机动力学模型线性化得到线性化后的状态估计方程；并根据所述线性化后的状态估计方程得到的系数矩阵、卡尔曼增益以及新息；s3、将所述系数矩阵、所述卡尔曼增益以及所述新息代入到自协方差最小二乘算法中得到用新息的期望值构成的协方差矩阵和所述用新息期望值构成的自协方差矩阵的估计值；s4、利用所述新息的期望值构成的协方差矩阵和所述用新息期望值构成的自协方差矩阵的估计值，经过半定规划算法，得到过程噪声的协方差和量测噪声的协方差。进一步地，所述步骤s1进一步包括如下步骤：s11、所述无人机动力学模型在地面坐标系中建立的线运动方程和在机体坐标系中建立的转动方程为：其中，m为无人机的质量，x为无人机的位置矢量，为无人机的加速度矢量，t为旋翼产生的推力，m为旋翼产生的转矩，j为无人机的转动惯量，ω为无人机转动的角速度，为无人机转动的角加速度，g为无人机受到的重力，f为无人机受到的空气阻力，其中，l为旋翼中心到x轴或y轴的距离，ω1、ω2、ω3、ω4分别为四个桨叶的转速，b、d分别为桨叶的拉力系数和扭矩系数；s12、将机体轴x与水平面之间的夹角定义为俯仰角θ，抬头为正；将机体轴x在水平面上的投影与xe轴之间的夹角定义为偏航角机头右偏为正；将机体轴z与通过机体x轴的铅垂面间的夹角定义为滚转角φ，无人机右滚转为正，这三个角即为欧拉角；令则从机体坐标系到地面坐标系的坐标转换矩阵为则无人机的动力学模型写为其中，分别为无人机在地面坐标系xe,ye,ze轴方向上的加速度；分别为无人机在机体坐标系x,y,z轴方向上的角加速度；ixx、iyy、izz分别为无人机绕机体坐标系x轴、y轴、z轴的转动惯量，ffx、ffy、ffz分别为f在机体坐标系三轴上的分量，[ωx,ωy,ωz]为ω在机体坐标系三轴上的分量，[ωx,ωy,ωz]与欧拉角的关系如下进一步地，所述步骤s2进一步包括如下步骤：s21、将系统的状态方程和量测方程写成如下的一般形式：其中，xk为状态量，yk为所述观测量，所述观测量从传感器中获取，uk为所述控制量，所述控制量由人为设定，wk为所述过程噪声，vk为所述量测噪声；wk～n(0,q)与vk～n(0,r)不相关，通过方程式(5)对其状态进行估计，得到其中，lk为所述卡尔曼滤波增益，所述新息为s22、将所述方程式(5)在处线性化得其中，ak，bk，gk，ck均为所述系数矩阵，s23、所述线性化后的状态估计方程为：进一步地，所述步骤s3进一步包括如下步骤：s31、所述用新息的期望值构成自协方差矩阵，s32、以[r(n)]s和所述用新息期望值构成的自协方差矩阵的估计值之差的二范数的平方最小为优化目标,其中，进一步地，当对线运动的噪声协方差进行估算时，将所述欧拉角和所述四个桨叶的转速作为所述控制量。进一步地，当对角运动的噪声协方差进行估算时，将所述四个桨叶的转速作为所述控制量。本发明提供的一个或多个技术方案，至少具有如下技术效果或优点：通过自协方差最小二乘理论方法能够对无人机的过程噪声和过程造型进行估算，降低了现有技术中，调试过程噪声和观测噪声的协方差矩阵值了对经验值和操作人员的要求，能够给操作人员提供较为准确的估值区间，节省调试的时间，同时确保了系统造成和观测噪声的精确度。附图说明为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。图1是本发明中无人机的噪声协方差进行估算的方法原理图；图2是本发明中地面坐标系和机体坐标系的示意图；图3是本发明地面坐标系中xe轴方向的位置曲线；图4是本发明地面坐标系中xe轴方向的速度曲线；图5是本发明地面坐标系中ye轴方向的位置曲线；图6是本发明地面坐标系中ye轴方向的速度曲线；图7是本发明地面坐标系中ze轴方向的位置曲线；图8是本发明地面坐标系中ze轴方向的速度曲线；图9是本发明滚转角φ的变化曲线；图10是本发明滚转角变化率的变化曲线；图11是本发明俯仰角θ的变化曲线；图12是本发明俯仰角变化率的变化曲线；图13是本发明偏航角的变化曲线；图14是本发明偏航角变化率的变化曲线；具体实施方式如图1所示，为本发明的无人机的噪声协方差进行估算的方法原理图，首先，本发明包括：步骤s1，建立无人机动力学模型，步骤s2，利用扩展卡尔曼滤波器结合控制量u和通过传感器获得的观测值h(x)将无人机动力学模型线性化；步骤s3，将线性化得到的系数矩阵a,b,c,g,卡尔曼增益l以及新息y代入自线性时变-自协方差最小二乘算法中，获得新息的期望值构成自协方差矩阵[r(n)]s和用新息期望值构成的自协方差矩阵的估计值步骤s4，将获得新息的期望值构成自协方差矩阵[r(n)]s和所述用新息期望值构成的自协方差矩阵的估计值代入半正定规划算法中，求得过程噪声协方差q和量测噪声协方差r。步骤s1包括，如图2所示，针对“x”形的四旋翼无人机，建立地面坐标系和机体坐标系如图2所示。机体坐标系b-xyz：原点取在无人机的质心，坐标系与机体固连。x轴与机身设计的纵轴平行，且处于无人机的对称平面内，指向前方；y轴垂直于无人机对称平面指向右方；z轴在无人机对称平面内，且垂直于x轴指向下方。整个坐标系符合欧拉坐标系右手定则。地面坐标系e-xeyeze：采用北东地坐标系，xe轴指向北面，ye轴指向东面，而ze轴垂直向下。在只考虑无人机重力g、旋翼产生的拉力t和无人机受到的空气阻力f的情况下，s1进一步包括步骤s11，建立无人机的动力学模型如下。分别在地面坐标系中建立线运动方程和在机体坐标系中建立转动方程如下：其中，m为无人机的质量，x为无人机的位置矢量，为无人机的加速度矢量，m为旋翼产生的转矩，j为无人机的转动惯量，ω为无人机转动的角速度，为无人机转动的角加速度，其中，l为旋翼中心到x轴或y轴的距离，ω1、ω2、ω3、ω4分别为四个桨叶的转速，b、d分别为桨叶的拉力系数和扭矩系数。s1进一步包括步骤s12，将机体轴x与水平面之间的夹角定义为俯仰角θ，抬头为正；将机体轴x在水平面上的投影与xe轴之间的夹角定义为偏航角机头右偏为正；将机体轴z与通过机体x轴的铅垂面间的夹角定义为滚转角φ，无人机右滚转为正。这三个角即为欧拉角。令则从机体坐标系到地面坐标系的坐标转换矩阵为则无人机的动力学模型写为其中，分别为无人机在地面坐标系xe,ye,ze轴方向上的加速度；分别为无人机在机体坐标系x,y,z轴方向上的角加速度；ixx、iyy、izz分别为无人机绕机体坐标系x轴、y轴、z轴的转动惯量，ffx、ffy、ffz分别为f在机体坐标系三轴上的分量，[ωx,ωy,ωz]为ω在机体坐标系三轴上的分量，[ωx,ωy,ωz]与欧拉角的关系如下步骤s2进一步包括s21将系统的状态方程和量测方程写成如下的一般形式：对其状态进行估计，其中，xk为状态量，yk为所述观测量，所述观测量从传感器中获取，uk为所述控制量，所述控制量由人为设定，wk为所述过程噪声，vk为所述量测噪声；wk～n(0,q)与vk～n(0,r)不相关，通过方程式(1)对其状态进行估计，得到其中，lk为卡尔曼滤波增益，新息步骤s2进一步包括s22，将所述方程式(5)在处线性化得其中，ak，bk，gk，ck均为所述系数矩阵，步骤s2进一步包括s23，线性化后的状态估计方程为状态估计误差表示为且则步骤s3进一步包括s31，用新息的期望值构成自协方差矩阵步骤s3进一步包括s31，以和所述用新息期望值构成的自协方差矩阵的估计值之差的二范数的平方最小为优化目标其中，实施例一由于无人机动力学模型为一个12维的方程组，使用协方差最小二乘理论方法算法估计模型中的噪声协方差时，为缩短程序运行时间，分别对线运动与角运动中的噪声协方差进行估计。仿真时使用的模型中各参数值如表1所示。表1参数值参数名参数值参数名参数值无人机质量m1.5kg重力加速度g9.79m/s2x轴转动惯量ixx0.024kg·m2y轴转动惯量iyy0.024kg·m2z轴转动惯量izz0.112kg·m2l0.232m桨叶拉力系数b1.0643×10-5桨叶扭矩系数d2.3528×10-7空气阻力系数k-0.2334①线运动线运动中状态方程即为无人机动力学模型的线运动方程，即其中，状态变量为x＝[x,y,z,u,v,w]t。将欧拉角和四个电机的转速作为控制量，[φ,θ,φ]＝[0,0,0]，[ω1,ω2,ω3,ω4]＝[588.9737,588.9737,588.9737,588.9737]。状态量初值为并设定量测方程如下在线运动模型中加入协方差为q＝2.5×10-5的零均值高斯白噪声，在观测方程中加入协方差为r＝10-3i6×6的高斯白噪声，利用ode45函数以0.001秒的定步长生成10秒的观测数据。首先，利用扩展卡尔曼滤波算法结合状态方程、控制量和观测数据求出系数矩阵和卡尔曼增益。然后，应用自协方差最小二乘算法的理论给出新息期望值构成的自协方差矩阵以及新息期望值构成的自协方差矩阵的估计值。最后，使用半正定规划方法求解式(14)中的最优问题，得到所求的过程噪声协方差q和量测噪声协方差r。②角运动角运动中状态方程即为无人机动力学模型的角运动方程，即其中，状态变量为角运动中控制量为四个电机的转速，[ω1,ω2,ω3,ω4]＝[481.5,481.5,481.5,481.5]。状态量初值为设定量测方程如下在线运动模型中加入协方差为q＝2.5×10-5的零均值高斯白噪声，在观测方程中加入协方差为r＝10-3i6×6的高斯白噪声，利用ode45函数以0.001秒的定步长生成10秒的观测数据。协方差估计的过程与线运动中相同。实施例一的仿真结果如下：①线运动利用协方差最小二乘理论方法算法时采样时间设为0.1秒，q的初始估计值设为8×10-3，r的初始估计值设为3×10-3i6×6，协方差最小二乘理论方法程序辨识得到的结果为②角运动利用协方差最小二乘理论方法算法仿真时采样时间设为0.1秒，q的初始估计值均设为3×10-3，r的初始估计值均设为3×10-3i6×6，辨识得到的结果为将线运动和角运动模型中估计到的噪声特性代入扩展卡尔曼滤波中进行滤波，得到滤波后的各变量曲线与各变量真实值、观测值的对比图如图3-14所示，图3-14中画出了各状态变量的观测值、真实值和使用扩展卡尔曼滤波滤波后的值。图中灰色分散的点为各变量的观测值，黑色的实线为真实值。可以看出，由于存在过程噪声和观测噪声，观测值存在误差。将协方差最小二乘理论方法算法辨识得到的过程噪声协方差q和量测噪声协方差r代入扩展卡尔曼滤波算法中进行滤波，得到了图中黑色虚线表示的滤波后的值。虽然滤波后的值仍存在误差，但已十分接近真实值，说明了协方差最小二乘理论方法算法估计得到的协方差较准确。综上所述，通过本发明中的对无人机的噪声协方差进行估算的方法，对过程噪声协方差q和量测噪声协方差r的估算精确度高。同时，该估算方法为操作人员提供了数值参考，故此一般操作人员在调试过程噪声协方差q和量测噪声协方差r时较为方便快捷。当前第1页12