CN106352897B - 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法 - Google Patents

一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法 Download PDF

Info

Publication number
CN106352897B
CN106352897B CN201610740714.5A CN201610740714A CN106352897B CN 106352897 B CN106352897 B CN 106352897B CN 201610740714 A CN201610740714 A CN 201610740714A CN 106352897 B CN106352897 B CN 106352897B
Authority
CN
China
Prior art keywords
vision sensor
silicon mems
monocular vision
error
mems gyroscope
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201610740714.5A
Other languages
English (en)
Other versions
CN106352897A (zh
Inventor
杨百川
盛蔚
杨睿
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Individual
Original Assignee
Individual
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Individual filed Critical Individual
Priority to CN201610740714.5A priority Critical patent/CN106352897B/zh
Publication of CN106352897A publication Critical patent/CN106352897A/zh
Application granted granted Critical
Publication of CN106352897B publication Critical patent/CN106352897B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Gyroscopes (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法,一种无人机在航路任意位置、无需已知图形、利用单目视觉传感器图像信息实时估计与校正硅MEMS陀螺误差的方法;在每个计算周期,单目视觉传感器获取的图像信息通过与上一帧图像信息的特征匹配,获取机体坐标系下无人机的姿态变化率;再采用硅MEMS陀螺三轴测量数据与通过图像信息获得的姿态变化率作为观测值进行Kalman滤波;依据Kalman滤波结果校正硅MEMS陀螺误差,更新导航数据和滤波器参数;进入下一计算周期。本发明可用于任何包含单目视觉传感器和硅MEMS陀螺的无人机导航系统中。

Description

一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法
技术领域
本发明涉及一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法,可用于任何包含单目视觉传感器和硅MEMS陀螺的无人机导航系统中。
背景技术
MEMS陀螺仪是一类应用MEMS(Micro Electro-Mechanical System)技术制成的测量运动物体角速率的惯性测量单元,因其具有体积小、重量轻、成本低以及可靠性高等优点,从而推动捷联惯导系统的迅速发展,在无人机领域获得广泛的研究。但这类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=[Z1 Z2 Z3 Z4 Z5Z6 Z7 Z8 Z9]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-XnYnZn OnXn轴当地指东,OnYn轴当地指北,OnZn轴指天。
具体包括以下步骤:
(1)利用单目视觉传感器获取的当前帧图像与上一帧图像进行特征点匹配,获取机体坐标系下无人机在两个时刻间的姿态转换矩阵。
(2)利用硅MEMS陀螺获取三维角速度信息,经过捷联导航算法,实时获取机体坐标系下无人机在两个时刻间的姿态转换矩阵。
(3)在每个计算周期,首先利用惯性测量单元提供的三维正交角速度和加速度测量数据,经过捷联导航算法,实时更新当前无人机姿态。
(4)以图像测量信息与硅MEMS陀螺测量信息为观测量,构建九维观测量Kalman滤波器模型。
(5)用Kalman滤波器状态变量估计值校正硅MEMS陀螺误差,提高导航精度。
(6)上述步骤迭代重复计算。
下面进行详细说明。
1、无人机机载单目视觉传感器接收到新一帧图像信息,并与前一帧图像进行特征点匹配:
分别提取两张图像的特征点(μ,ν)t-1,((μ,ν)t-1为上一时刻特征点在图像坐标系下的坐标值,(μ,ν)t为当前时刻特征点在图像坐标系下的坐标值),利用RANSAC算法剔除误匹配点后,将匹配成功的特征点组成特征点组;如果匹配上的特征点组的个数大于等于3,则认为该次匹配成功,否则认为匹配失败。
2、利用匹配成功的图像特征点组计算姿态转换矩阵:
可以假设两张图片上匹配成功的特征点在地理坐标系下坐标相同,即:
根据坐标系转换关系:
其中,u,v表示特征点在图像坐标系下的坐标,Xc,Yc,Zc表示特征点在摄像机坐标系下坐标,Xn,Yn,Zn表示特征点在地理坐标系下坐标,f表示摄像机焦距。
设姿态转换矩阵将特征点在地理坐标系下坐标投影到图像坐标系下,即可得到:
其中,
由于姿态转换矩阵为单位正交矩阵,通过约束条件:
即可通过R2×2算出 表示利用图像特征匹配计算得来的阵。
3、利用硅MEMS陀螺提供的三维正交角速度测量数据,经过标准捷联惯性导航算法,实时更新当前固定翼无人机航向姿态信息,同时更新姿态对应的方向余弦矩阵根据当前时刻的与上一时刻的更新硅MEMS陀螺对应的姿态转换矩阵 表示惯导解算得来的阵。
4、设定Kalman滤波器状态变量为X=[φe φn φu εbx εby εbz]T,其中εxyz表示机体坐标系三个方向的陀螺零偏。滤波器的状态方程为: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=-F12 F23=-VN/(Rm+h)
F31=-F13 F32=-F23
其中,ωie表示地球的自转角速度,L表示无人机所在点的纬度,VE和VN表示无人机的东向和北向水平速度分量,Rm为无人机所在点处子午圈的曲率半径,Rn为无人机所在点处卯酉圈的曲率半径,h表示无人机据当地地面的高度,表示当前时刻惯导解算获取的阵。
5、滤波器观测方程为:Z=HX+V。其中表示惯导解算得来的阵,表示利用图像特征匹配计算得来的阵,九维观测量由相减获得。
观测矩阵H中除了以下元素外均为零元素::
其中,表示当前时刻惯导解算获取的阵,表示上一时刻惯导解算获取的阵
6、基于图像与硅MEMS陀螺的解算结果,滤波器的观测量为:Z=[Z1 Z2 Z3 Z4 Z5 Z6Z7 Z8 Z9]T,其中, 表示惯导解算获取的阵,表示利用图像特征匹配计算获取的阵,九维观测量由相减获得。
7、滤波器采用标准卡尔曼滤波器估计状态变量X,获得无人机误差角和陀螺零偏;
8、在飞行过程中迭代重复上述过程。
本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。
提供以上实施例仅仅是为了描述本发明的目的,而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改,均应涵盖在本发明的范围之内。

Claims (7)

1.一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法,其特征在于:以单目视觉传感器和硅MEMS陀螺作为测量传感器;在将单目视觉传感器实时获取的当前帧图像与上一帧图像进行特征点匹配,将匹配成功的特征点组成特征点组,利用匹配成功的特征点组计算相邻时刻的姿态转换矩阵,从而获取机体坐标系下相邻时刻无人机姿态转换矩阵;通过硅MEMS陀螺的测量值计算相邻时刻的姿态转换矩阵,将通过MEMS陀螺计算得到的姿态转换矩阵与通过单目视觉传感器实时获取的姿态转换矩阵分别作为观测值进行Kalman滤波,估计硅MEMS陀螺误差;根据Kalman滤波结果校正硅MEMS陀螺误差,更新导航数据和滤波器参数;进行下一个重复计算周期。
2.根据权利要求1所述一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法,其特征在于:所述单目视觉传感器实时获取的当前帧图像与上一帧图像进行特征点匹配时,匹配过程为:
分别提取两张图像的特征点(μ,ν)t-1,(μ,ν)t(μ,ν)t-1为上一时刻特征点在图像坐标系下的坐标值,(μ,ν)t为当前时刻特征点在图像坐标系下的坐标值,利用RANSAC算法剔除误匹配点后,将匹配成功的特征点组成特征点组;如果匹配上的特征点组的个数大于等于3,则认为该次匹配成功,否则认为匹配失败。
3.根据权利要求1所述一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法,其特征在于:所述方法并不依赖于利用已知图形作为地标,且在特定地标上空确定无人机姿态,而是利用普通单目视觉传感器获取的当前帧图像,与上一帧图像进行特征匹配,可以在任意时刻、在航路任意位置利用图像信息实时估计并校正硅MEMS陀螺误差,提高全飞行过程中惯性导航和飞行控制系统的精度。
4.根据权利要求1所述一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法,其特征在于:通过硅MEMS陀螺测量值时,在每个计算周期内,利用硅MEMS陀螺提供的三维正交角速度测量数据,将上一时刻方向余弦矩阵定义为经过捷联导航算法更新得当前时刻方向余弦矩阵计算出相邻时刻的姿态转换矩阵
5.根据权利要求1所述一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法,其特征在于:所述Kalman滤波时,构建九维观测量Kalman滤波器模型,其中Z=[Z1 Z2 Z3 Z4Z5 Z6 Z7 Z8 Z9]T 表示惯导解算得来的阵,表示利用图像特征匹配计算得来的阵,九维观测量由相减获得。
6.根据权利要求1所述一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法,其特征在于:所述方法在飞行过程期间始终迭代重复计算。
7.根据权利要求1所述一种基于单目视觉传感器的硅MEMS陀螺误差估计与校正方法,其特征在于:以硅MEMS工艺陀螺和单目视觉传感器为测量设备,满足无人机机载电子设备的限制。
CN201610740714.5A 2016-08-26 2016-08-26 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法 Active CN106352897B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610740714.5A CN106352897B (zh) 2016-08-26 2016-08-26 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610740714.5A CN106352897B (zh) 2016-08-26 2016-08-26 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法

Publications (2)

Publication Number Publication Date
CN106352897A CN106352897A (zh) 2017-01-25
CN106352897B true CN106352897B (zh) 2018-06-15

Family

ID=57855728

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610740714.5A Active CN106352897B (zh) 2016-08-26 2016-08-26 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法

Country Status (1)

Country Link
CN (1) CN106352897B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108253936B (zh) * 2018-01-04 2019-10-11 南京航空航天大学 一种减小光轴指向随机误差的无人机目标定位方法
CN108444468B (zh) * 2018-02-06 2020-08-04 浙江大学 一种融合下视视觉与惯导信息的定向罗盘
CN109059895B (zh) * 2018-03-28 2021-05-25 南京航空航天大学 一种基于手机摄像头和传感器的多模态室内测距及定位方法
CN109164825A (zh) * 2018-08-13 2019-01-08 上海机电工程研究所 一种用于多旋翼无人机的自主导航避障方法及装置
KR20210060779A (ko) * 2019-11-19 2021-05-27 현대자동차주식회사 차량센서의 이상 진단 장치 및 그 방법
CN115618749B (zh) * 2022-12-05 2023-04-07 四川腾盾科技有限公司 一种大型无人机实时定位的误差补偿方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8915116B2 (en) * 2013-01-23 2014-12-23 Freescale Semiconductor, Inc. Systems and method for gyroscope calibration
CN104750969B (zh) * 2013-12-29 2018-01-26 刘进 智能机全方位增强现实信息叠加方法
CN104748746B (zh) * 2013-12-29 2017-11-03 刘进 智能机姿态测定及虚拟现实漫游方法
CN103808331B (zh) * 2014-03-05 2016-10-26 北京理工大学 一种mems三轴陀螺仪误差标定方法

Also Published As

Publication number Publication date
CN106352897A (zh) 2017-01-25

Similar Documents

Publication Publication Date Title
CN106352897B (zh) 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法
CN106708066B (zh) 基于视觉/惯导的无人机自主着陆方法
EP3158293B1 (en) Sensor fusion using inertial and image sensors
EP3158412B1 (en) Sensor fusion using inertial and image sensors
EP3158417B1 (en) Sensor fusion using inertial and image sensors
CN112629538B (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
EP3158411B1 (en) Sensor fusion using inertial and image sensors
CN112630813B (zh) 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN107728182B (zh) 基于相机辅助的柔性多基线测量方法和装置
CN107014371A (zh) 基于扩展自适应区间卡尔曼的无人机组合导航方法与装置
CN106767767A (zh) 一种微纳多模星敏感器系统及其数据融合方法
CN108375383B (zh) 多相机辅助的机载分布式pos柔性基线测量方法和装置
CN108759814B (zh) 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法
CN108444468B (zh) 一种融合下视视觉与惯导信息的定向罗盘
CN109375647A (zh) 微型多源感知计算系统
Rhudy et al. Wide-field optical flow aided inertial navigation for unmanned aerial vehicles
JP6800918B2 (ja) エラー回復を実行するための方法、システム、及びプログラム
Mostafa et al. Optical flow based approach for vision aided inertial navigation using regression trees
Fusini et al. Experimental validation of a uniformly semi-globally exponentially stable non-linear observer for gnss-and camera-aided inertial navigation for fixed-wing uavs
KR100648882B1 (ko) 무인 항공기 자동 항법 시스템의 관성 값 계산 장치 및 그방법
Wu et al. Integrated navigation algorithm based on vision-inertial extended Kalman filter for low-cost unmanned aerial vehicle
Arai et al. Fast vision-based localization for a mars airplane
Romero et al. Fusion of optical flow and inertial sensors for four-rotor rotorcraft stabilization
CN114184194A (zh) 一种拒止环境下的无人机自主导航定位方法

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant