本发明涉及一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法，可用于任何包含单目视觉传感器和硅MEMS陀螺的无人机导航系统中。
背景技术：
：MEMS陀螺仪是一类应用MEMS(MicroElectro-MechanicalSystem)技术制成的测量运动物体角速率的惯性测量单元，因其具有体积小、重量轻、成本低以及可靠性高等优点，从而推动捷联惯导系统的迅速发展，在无人机领域获得广泛的研究。但这类MEMS陀螺仪容易受自身材料、制造水平以及工作环境等一系列因素的影响，性能普遍不高。一般来讲，陀螺的误差主要分为确定性误差及随机误差，前者主要指由扰动(敏感物理模型中的参数变化)和环境敏感(敏感环境的干扰)引起的误差，后者主要指由不确定因素引起的随机漂移，其中确定性误差是捷联惯导系统最主要的误差源。因此捷联惯导在使用之前必须通过标定实验确定出MEMS陀螺仪的各项误差系数，以在捷联惯导系统中对其进行补偿。现阶段视觉导航是无人机导航的一种热门研究方向。部分无人机上安装有小型图像传感器，利用获取的图像信息计算无人机的速度信息或辅助旋翼自主悬停与降落。但是现阶段在无已知地标的情况下，关于利用机载图像传感器辅助计算无人机姿态信息的研究仍很少见。技术实现要素：本发明的技术解决问题是：克服现有技术的不足，提供一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法。该方法以低成本硅MEMS陀螺和单目视觉传感器为测量器件，采用Kalman滤波信息融合的思路，实时估计校正硅MEMS陀螺误差，以提高全飞行过程中惯性导航和飞行控制系统的精度。本发明可用于任何包含单目视觉传感器和硅MEMS陀螺的无人机导航系统中。本发明的技术解决方案为：一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法，以单目视觉传感器和低成本硅MEMS陀螺作为测量传感器；在将单目视觉传感器实时获取的当前帧图像与上一帧图像进行特征点匹配，将匹配成功的特征点组成特征点组，利用匹配成功的特征点组计算相邻时刻的姿态转换矩阵，从而获取机体坐标系下相邻时刻无人机姿态转换矩阵；通过硅MEMS陀螺的测量值计算相邻时刻的姿态转换矩阵，将通过MEMS陀螺计算得到的姿态转换矩阵与通过单目视觉传感器实时获取的姿态转换矩阵分别作为观测值进行Kalman滤波，估计硅MEMS陀螺误差；根据Kalman滤波结果校正硅MEMS陀螺误差，更新导航数据和滤波器参数；进行下一个重复计算周期。所述单目视觉传感器实时获取的当前帧图像与上一帧图像进行特征点匹配时，匹配过程为：分别提取两张图像的特征点(μ,ν)t-1,((μ,ν)t-1为上一时刻特征点在图像坐标系下的坐标值，(μ,ν)t为当前时刻特征点在图像坐标系下的坐标值)，利用RANSAC算法剔除误匹配点后，将匹配成功的特征点组成特征点组；如果匹配上的特征点组的个数大于等于3，则认为该次匹配成功，否则认为匹配失败。所述方法并不依赖于利用已知图形作为地标，且在特定地标上空确定无人机姿态，而是利用普通单目视觉传感器获取的当前帧图像，与上一帧图像进行特征匹配，可以在任意时刻、在航路任意位置利用图像信息实时估计并校正硅MEMS陀螺误差，提高全飞行过程中惯性导航和飞行控制系统的精度。通过硅MEMS陀螺测量值时，在每个计算周期内，利用硅MEMS陀螺提供的三维正交角速度测量数据，将上一时刻方向余弦矩阵定义为经过捷联导航算法更新得当前时刻方向余弦矩阵计算出相邻时刻的姿态转换矩阵所述Kalman滤波时，构建九维观测量Kalman滤波器模型，其中Z＝[Z1Z2Z3Z4Z5Z6Z7Z8Z9]T，表示惯导解算得来的阵，表示利用图像特征匹配计算得来的阵，九维观测量由与相减获得。所述方法在飞行过程期间始终迭代重复计算。以低成本、硅MEMS工艺陀螺和低成本单目视觉传感器为测量设备，满足无人机机载电子设备低功耗、小体积、轻质量的限制。本发明的原理是：硅MEMS陀螺在惯性测量单元独立工作，误差随时间快速积累，无法提供可信的航向姿态信息。本发明利用了单目视觉传感器实时解算姿态信息作为观测量，通过信息融合滤波器为惯性测量单元提供了误差抑制的途径。依据Kalman滤波结果校正惯性测量单元误差，更新导航数据和滤波器参数，提供无人机飞行过程中高精度导航信息。本发明与现有技术相比的优点在于：以低成本、硅MEMS工艺陀螺和低成本单目视觉传感器作为测量传感器，满足无人机机载电子设备低功耗、小体积、轻质量的限制。利用单目视觉图像信息实时估计校正陀螺误差，无需外界提供地标或已知图形等模板信息，摆脱了飞行时间与地点的限制。在飞行的过程中利用Kalman滤波器，在每个迭代计算步骤中估计陀螺误差，提高了单目视觉传感器和硅MEMS陀螺信息融合的精度，为无人机提供了可信有效的定姿信息，以提高全飞行过程中惯性导航和飞行控制系统的精度。本发明可用于任何包含单目视觉传感器和硅MEMS陀螺的无人机导航系统中。附图说明图1为本发明的基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法流程图。具体实施方式如图1所示，本发明的具体方法如下：一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法，以普通单目视觉传感器和低成本硅MEMS陀螺作为测量传感器；在每个计算周期，硅MEMS陀螺经过捷联导航算法，参与更新当前无人机姿态和其他导航数据；根据单目视觉传感器实时获取的图像信息实时估计与校正陀螺误差的方法：将单目视觉传感器实时获取的当前帧图像与上一帧图像进行特征点匹配，获取机体坐标系下相邻时刻无人机姿态转换矩阵；通过硅MEMS陀螺的测量值计算相邻时刻的姿态转换矩阵，将通过陀螺与图像获取的姿态转换矩阵作为观测值进行Kalman滤波，估计硅MEMS陀螺误差；根据Kalman滤波结果校正硅MEMS陀螺误差，更新导航数据和滤波器参数；进行下一个重复计算周期。本发明所用到的坐标系有：机体坐标系(Σb)机体坐标系Ob-XbYbZb原点位于无人机质量中心,ObXb轴位于无人机参考平面内平行于机身轴线并指向无人机右方，ObYb轴垂直于无人机参考面并指向无人机前方，ObZb轴在参考面内垂直于XOY平面，指向无人机上方。图像坐标系(u,v)图像坐标系(u,v)原点为摄像机光心(投影中心)，x,y坐标(u,v)以像素为单位。摄像机坐标系(Σc)计算机坐标系Oc-XcYcZc原点为摄像机光心(投影中心)，OcXc轴和OcYc轴与成像平面坐标系的X轴和Y轴平行，OcZc轴为摄像机的光轴，和图像平面垂直。地理坐标系(∑n)地理坐标系On-XnYnZnOnXn轴当地指东，OnYn轴当地指北，OnZn轴指天。具体包括以下步骤：(1)利用单目视觉传感器获取的当前帧图像与上一帧图像进行特征点匹配，获取机体坐标系下无人机在两个时刻间的姿态转换矩阵。(2)利用硅MEMS陀螺获取三维角速度信息，经过捷联导航算法，实时获取机体坐标系下无人机在两个时刻间的姿态转换矩阵。(3)在每个计算周期，首先利用惯性测量单元提供的三维正交角速度和加速度测量数据，经过捷联导航算法，实时更新当前无人机姿态。(4)以图像测量信息与硅MEMS陀螺测量信息为观测量，构建九维观测量Kalman滤波器模型。(5)用Kalman滤波器状态变量估计值校正硅MEMS陀螺误差，提高导航精度。(6)上述步骤迭代重复计算。下面进行详细说明。1、无人机机载单目视觉传感器接收到新一帧图像信息，并与前一帧图像进行特征点匹配：分别提取两张图像的特征点(μ,ν)t-1,((μ,ν)t-1为上一时刻特征点在图像坐标系下的坐标值，(μ,ν)t为当前时刻特征点在图像坐标系下的坐标值)，利用RANSAC算法剔除误匹配点后，将匹配成功的特征点组成特征点组；如果匹配上的特征点组的个数大于等于3，则认为该次匹配成功，否则认为匹配失败。2、利用匹配成功的图像特征点组计算姿态转换矩阵：可以假设两张图片上匹配成功的特征点在地理坐标系下坐标相同，即：XnYnZnt=XnYnZnt-1]]>根据坐标系转换关系：其中，u,v表示特征点在图像坐标系下的坐标，Xc,Yc,Zc表示特征点在摄像机坐标系下坐标，Xn,Yn,Zn表示特征点在地理坐标系下坐标，f表示摄像机焦距。设姿态转换矩阵将特征点在地理坐标系下坐标投影到图像坐标系下，即可得到：uvt=k*R2×2uvt-1+t2×1]]>其中，由于姿态转换矩阵为单位正交矩阵，通过约束条件：|C1|=|C2|=|C3|=1C1*C2=C1*C3=C2*C3=0,(Ct-1t=C1C2C3)]]>即可通过R2×2算出表示利用图像特征匹配计算得来的阵。3、利用硅MEMS陀螺提供的三维正交角速度测量数据，经过标准捷联惯性导航算法，实时更新当前固定翼无人机航向姿态信息，同时更新姿态对应的方向余弦矩阵根据当前时刻的与上一时刻的更新硅MEMS陀螺对应的姿态转换矩阵表示惯导解算得来的阵。4、设定Kalman滤波器状态变量为X＝[φeφnφuεbxεbyεbz]T，其中εx,εy,εz表示机体坐标系三个方向的陀螺零偏。滤波器的状态方程为：X＝FX+GW，W＝W[3×1]为系统噪声，状态转移矩阵F＝F6×6和系统噪声系数阵G＝G[6×3]中除了以下元素外均为零元素：F12＝ωiesin(L)+VEtan(L)/(Rn+h)F13＝-ωiecos(L)-VE/(Rn+h)F21＝-F12F23＝-VN/(Rm+h)F31＝-F13F32＝-F23F14=(Cbn)t[1,1]F15=(Cbn)t[1,2]]]>F16=(Cbn)t[1,3]F24=(Cbn)t[2,1]]]>F25=(Cbn)t[2,2]F26=(Cbn)t[2,3]]]>F34=(Cbn)t[3,1]F35=(Cbn)t[3,2]]]>F36=(Cbn)t[3,3]]]>G[1,1]=(Cbn)t[1,1]G[1,2]=(Cbn)t[1,2]]]>G[1,3]=(Cbn)t[1,3]G[2,1]=(Cbn)t[2,1]]]>G[2,2]=(Cbn)t[2,2]G[2,3]=(Cbn)t[2,3]]]>G[3,1]=(Cbn)t[3,1]G[3,2]=(Cbn)t[3,2]]]>G[3,3]=(Cbn)t[3,3]]]>其中，ωie表示地球的自转角速度，L表示无人机所在点的纬度，VE和VN表示无人机的东向和北向水平速度分量，Rm为无人机所在点处子午圈的曲率半径，Rn为无人机所在点处卯酉圈的曲率半径，h表示无人机据当地地面的高度，表示当前时刻惯导解算获取的阵。5、滤波器观测方程为：Z＝HX+V。其中表示惯导解算得来的阵，表示利用图像特征匹配计算得来的阵，九维观测量由与相减获得。观测矩阵H中除了以下元素外均为零元素：：H[1,1]=(Cbn)t[1,2]*(Cbn)t-1[1,1]-(Cbn)t[1,1]*(Cbn)t-1[2,1]]]>H[1,2]=(Cbn)t[1,3]*(Cbn)t-1[1,1]+(Cbn)t[1,1]*(Cbn)t-1[3,1]]]>H[1,3]=(Cbn)t[1,3]*(Cbn)t-1[2,1]-(Cbn)t[1,2]*(Cbn)t-1[3,1]]]>H[2,1]=(Cbn)t[1,2]*(Cbn)t-1[1,2]-(Cbn)t[1,1]*(Cbn)t-1[2,2]]]>H[2,2]=(Cbn)t[1,3]*(Cbn)t-1[1,2]+(Cbn)t[1,1]*(Cbn)t-1[3,2]]]>H[2,3]=(Cbn)t[1,3]*(Cbn)t-1[2,2]-(Cbn)t[1,2]*(Cbn)t-1[3,2]]]>H[3,1]=(Cbn)t[1,2]*(Cbn)t-1[1,3]-(Cbn)t[1,1]*(Cbn)t-1[2,3]]]>H[3,2]=(Cbn)t[1,3]*(Cbn)t-1[1,3]+(Cbn)t[1,1]*(Cbn)t-1[3,3]]]>H[3,3]=(Cbn)t[1,3]*(Cbn)t-1[2,3]-(Cbn)t[1,2]*(Cbn)t-1[3,3]]]>H[4,1]=(Cbn)t[2,2]*(Cbn)t-1[1,1]-(Cbn)t[2,1]*(Cbn)t-1[2,1]]]>H[4,2]=(Cbn)t[2,3]*(Cbn)t-1[1,1]+(Cbn)t[2,1]*(Cbn)t-1[3,1]]]>H[4,3]=(Cbn)t[2,3]*(Cbn)t-1[2,1]-(Cbn)t[2,2]*(Cbn)t-1[3,1]]]>H[5,1]=(Cbn)t[2,2]*(Cbn)t-1[1,2]-(Cbn)t[2,1]*(Cbn)t-1[2,2]]]>H[5,2]=(Cbn)t[2,3]*(Cbn)t-1[1,2]+(Cbn)t[2,1]*(Cbn)t-1[3,2]]]>H[5,3]=(Cbn)t[2,3]*(Cbn)t-1[2,2]-(Cbn)t[2,2]*(Cbn)t-1[3,2]]]>H[6,1]=(Cbn)t[2,2]*(Cbn)t-1[1,3]-(Cbn)t[2,1]*(Cbn)t-1[2,3]]]>H[6,2]=(Cbn)t[2,3]*(Cbn)t-1[1,3]+(Cbn)t[2,1]*(Cbn)t-1[3,3]]]>H[6,3]=(Cbn)t[2,3]*(Cbn)t-1[2,3]-(Cbn)t[2,2]*(Cbn)t-1[3,3]]]>H[7,1]=(Cbn)t[3,2]*(Cbn)t-1[1,1]-(Cbn)t[3,1]*(Cbn)t-1[2,1]]]>H[7,2]=(Cbn)t[3,3]*(Cbn)t-1[1,1]+(Cbn)t[3,1]*(Cbn)t-1[3,1]]]>H[7,3]=(Cbn)t[3,3]*(Cbn)t-1[2,1]-(Cbn)t[3,2]*(Cbn)t-1[3,1]]]>H[8,1]=(Cbn)t[3,2]*(Cbn)t-1[1,2]-(Cbn)t[3,1]*(Cbn)t-1[2,2]]]>H[8,2]=(Cbn)t[3,3]*(Cbn)t-1[1,2]+(Cbn)t[3,1]*(Cbn)t-1[3,2]]]>H[8,3]=(Cbn)t[3,3]*(Cbn)t-1[2,2]-(Cbn)t[3,2]*(Cbn)t-1[3,2]]]>H[9,1]=(Cbn)t[3,2]*(Cbn)t-1[1,3]-(Cbn)t[3,1]*(Cbn)t-1[2,3]]]>H[9,2]=(Cbn)t[3,3]*(Cbn)t-1[1,3]+(Cbn)t[3,1]*(Cbn)t-1[3,3]]]>H[9,3]=(Cbn)t[3,3]*(Cbn)t-1[2,3]-(Cbn)t[3,2]*(Cbn)t-1[3,3].]]>其中，表示当前时刻惯导解算获取的阵，表示上一时刻惯导解算获取的阵6、基于图像与硅MEMS陀螺的解算结果，滤波器的观测量为：Z＝[Z1Z2Z3Z4Z5Z6Z7Z8Z9]T，其中，表示惯导解算获取的阵，表示利用图像特征匹配计算获取的阵，九维观测量由与相减获得。7、滤波器采用标准卡尔曼滤波器估计状态变量X，获得无人机误差角和陀螺零偏；8、在飞行过程中迭代重复上述过程。本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。当前第1页1 2 3