CN103925926B - 一种基于camera/mimu室内组合导航系统的四元数测量方法 - Google Patents
一种基于camera/mimu室内组合导航系统的四元数测量方法 Download PDFInfo
- Publication number
- CN103925926B CN103925926B CN201410172298.4A CN201410172298A CN103925926B CN 103925926 B CN103925926 B CN 103925926B CN 201410172298 A CN201410172298 A CN 201410172298A CN 103925926 B CN103925926 B CN 103925926B
- Authority
- CN
- China
- Prior art keywords
- camera
- mimu
- quaternary number
- matrix
- navigation system
- 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
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Gyroscopes (AREA)
- Navigation (AREA)
Abstract
本发明属于室内组合导航导航系统技术领域,尤其涉及解决现有的MIMU误差漂移大导致的导航精度低,可靠性差问题的一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法。本发明包括:采集陀螺输出的角速度和CAMERA拍摄的图像;对CAMERA拍摄的图像进行平滑滤波;得到原始亮度数据的梯度值,定位物体的边缘;确定中心灭点、水平灭点和垂直灭点位置矩阵;测量出标定矩阵;得到的标定矩阵,得到CAMERA数学模型;测量矩阵:得到MIMU的离散域四元数更新模型;确定CAMERA/MIMU室内组合导航系统的状态;估计出四元数。本发明给出了一种组合系统四元数测量方法,有效的提高了导航精度。
Description
技术领域
本发明属于室内组合导航导航系统技术领域,尤其涉及解决现有的MIMU误差漂移大导致的导航精度低,可靠性差问题的一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法。
背景技术
微机械系统(MEMS)惯性器件存在着测量精度低、误差随时间漂移大及温度稳定性差等缺点,MEMS构成的微惯性测量单元(MIMU)的导航参数估计存在严重误差漂移,误差发散导致系统无法正确进行正确导航,最终失去导航的意义。如何抑制误差漂移,提高导航精度一直是室内导航的难题,多传感器融合是解决此难题的方法,其中,CAMERA辅助的MIMU组合系统是抑制误差漂移的一种有效方法,采用卡尔曼滤波算法对系统四元数进行估计,能够克服MIMU误差漂移的缺点,提高四元数精度,进而提高导航精度。
发明内容
本发明实施例的目的在于提供一种提高导航精度和可靠性的基于CAMERA/MIMU室内组合导航系统的四元数测量方法。
本发明的目的是这样实现的:
步骤一:连接CAMERA/MIMU室内组合导航系统,系统上电后进行预热;
步骤二:采集陀螺输出的角速度和CAMERA拍摄的图像;
步骤三:采用空间滤波方法生成3×3模板的高斯卷积滤波器对步骤二中CAMERA拍摄的图像进行平滑滤波;
步骤四:采用边缘检测器追踪步骤三中经平滑滤波后图像中亮度快速变化的点,得到原始亮度数据的梯度值,并利用局部梯度值的最大值定位物体的边缘;
步骤五:采用随机采样一致性方法找到步骤三中经平滑滤波后图像中的灭点位置,确定中心灭点、水平灭点和垂直灭点位置矩阵B;
步骤六:根据CAMERA焦距、像主点和透镜畸变参数测量出标定矩阵K;
步骤七:根据步骤五中得到的灭点位置矩阵B和步骤六中得到的标定矩阵K,得到CAMERA数学模型:
B=KA;
式中,A为CAMERA旋转矩阵:
其中,为四元数,q0,q1,q2,q3分别为四个实数,为载体坐标系三个正交坐标轴上的单位矢量;
步骤八:利用步骤七中获得的CAMERA数学模型确定CAMERA/MIMU室内组合导航系统的量测方程:
Bk=KAk+vk;
式中,Bk为tk时刻灭点位置矩阵、Ak为tk时刻与四元数相关的CAMERA旋转矩阵,vk为量测噪声序列;
步骤九:根据步骤二中陀螺输出的角速度测量矩阵Wk-1:
式中,I为单位阵,[Φa]为由旋转矢量构造的反对称矩阵,Δt=tk-tk-1为更新周期;
步骤十:根据步骤八中得到的矩阵Wk-1得到MIMU的离散域四元数Qk更新模型:
式中,φx,φy,φz为旋转矢量Φ在载体坐标系三个正交坐标轴上的分量;
步骤十一:选取四元数Qk=[q0,k q1,k q2,k q3,k]T作为CAMERA/MIMU室内组合导航系统的状态向量,利用步骤十中获得的四元数更新模型确定CAMERA/MIMU室内组合导航系统的状态方程为:
xk=Fk-1xk-1+wk-1;
式中,qi,k(i=0,1,2,3)为tk时刻四元数的分量值,wk-1为系统激励噪声序列,系统状态向量xk=Qk,一步转移矩阵
步骤十二:根据步骤八和步骤十一建立的CAMERA/MIMU室内组合导航系统的状态方程和量测方程进行卡尔曼滤波,估计出四元数Qk。
本发明的有益效果在于:
本发明通过对CAMERA和MIMU建立模型,CAMERA模型为组合系统量测方程,MIMU四元数更新模型为状态方程,采用卡尔曼滤波算法实现对组合系统的四元数估计,克服了单独MIMU误差漂移大的缺点,提高了导航精度;本发明方法简单,稳定性和可靠性高,给出了一种组合系统四元数测量方法,有效的提高了导航精度。
附图说明
图1是基于CAMERA/MIMU室内组合导航系统的四元数测量方法的流程图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明涉及室内组合导航导航系统技术领域,为解决单兵在室内长时间导航时,微惯性测量单元(MIMU,miniature inertial measurement unit)导航误差发散问题,本发明提出了一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法,通过建立摄像机(CAMERA)的数学模型和MIMU四元数更新模型,以摄像机数学模型为系统量测方程,以MIMU四元数更新模型为系统状态更新方程,采用卡尔曼滤波器对四元数进行估计,提高四元数的测量精度,进而提高导航精度。
本发明实施例是这样实现的,一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法,该基于CAMERA/MIMU室内组合导航系统的四元数测量方法包括以下步骤:
步骤一:连接CAMERA/MIMU室内组合导航系统,系统上电后进行预热;
步骤二:采集陀螺输出的角速度和CAMERA拍摄的图像;
步骤三:采用空间滤波方法生成3×3模板的高斯卷积滤波器对步骤二中CAMERA拍摄的图像进行平滑滤波;
步骤四:采用边缘检测器追踪步骤三中经平滑滤波后图像中亮度快速变化的点,得到原始亮度数据的梯度值,并利用局部梯度值的最大值定位物体的边缘;
步骤五:采用随机采样一致性方法找到步骤三中经平滑滤波后图像中的灭点位置,确定中心灭点、水平灭点和垂直灭点位置矩阵B;
步骤六:根据CAMERA焦距、像主点和透镜畸变参数等固有信息测量出标定矩阵K;
步骤七:根据步骤五中得到的灭点位置矩阵B和步骤六中得到的标定矩阵K,得到CAMERA数学模型如下:
B=KA(1)
式中,A为CAMERA旋转矩阵,其测量公式为:
其中,为四元数,q0,q1,q2,q3分别为四个实数,为载体坐标系三个正交坐标轴上的单位矢量;
步骤八:利用步骤七中获得的CAMERA数学模型确定CAMERA/MIMU室内组合导航系统的量测方程:
Bk=KAk+vk(3)
式中,Bk为tk时刻灭点位置矩阵、Ak为tk时刻与四元数相关的CAMERA旋转矩阵,vk为量测噪声序列。
步骤九:根据步骤二中陀螺输出的角速度测量矩阵Wk-1,其测量公式为:
式中,I为单位阵,[Φa]为由旋转矢量构造的反对称矩阵,Δt=tk-tk-1为更新周期。
步骤十:根据步骤八中得到的矩阵Wk-1得到MIMU的离散域四元数Qk更新模型:
式中,φx,φy,φz为旋转矢量Φ在载体坐标系三个正交坐标轴上的分量。
步骤十一:选取四元数Qk=[q0,k q1,k q2,k q3,k]T作为CAMERA/MIMU室内组合导航系统的状态向量,利用步骤十中获得的四元数更新模型确定CAMERA/MIMU室内组合导航系统的状态方程为:
xk=Fk-1xk-1+wk-1(6)
式中,qi,k(i=0,1,2,3)为tk时刻四元数的分量值,wk-1为系统激励噪声序列,系统状态向量xk=Qk,一步转移矩阵
步骤十二:根据步骤八和步骤十一建立的CAMERA/MIMU室内组合导航系统的状态方程和量测方程进行卡尔曼滤波,估计出四元数Qk。
本发明提供的基于CAMERA/MIMU室内组合导航系统的四元数测量方法,通过对CAMERA和MIMU建立模型,CAMERA模型为组合系统量测方程,MIMU四元数更新模型为状态方程,采用卡尔曼滤波算法实现对组合系统的四元数估计,克服了单独MIMU误差漂移大的缺点,提高了导航精度;本发明方法简单,稳定性和可靠性高,给出了一种组合系统四元数测量方法,有效的提高了导航精度。
Claims (1)
1.一种基于CAMERA/MIMU室内组合导航系统的四元数测量方法,其特征在于:
步骤一:连接CAMERA/MIMU室内组合导航系统,系统上电后进行预热;
步骤二:采集陀螺输出的角速度和CAMERA拍摄的图像;
步骤三:采用空间滤波方法生成3×3模板的高斯卷积滤波器对步骤二中CAMERA拍摄的图像进行平滑滤波;
步骤四:采用边缘检测器追踪步骤三中经平滑滤波后图像中亮度快速变化的点,得到原始亮度数据的梯度值,并利用局部梯度值的最大值定位物体的边缘;
步骤五:采用随机采样一致性方法找到步骤三中经平滑滤波后图像中的灭点位置,确定中心灭点、水平灭点和垂直灭点位置矩阵B;
步骤六:根据CAMERA焦距、像主点和透镜畸变参数测量出标定矩阵K;
步骤七:根据步骤五中得到的灭点位置矩阵B和步骤六中得到的标定矩阵K,得到CAMERA数学模型:
B=KA;
式中,A为CAMERA旋转矩阵:
其中,为四元数,q0,q1,q2,q3分别为四个实数,为载体坐标系三个正交坐标轴上的单位矢量;
步骤八:利用步骤七中获得的CAMERA数学模型确定CAMERA/MIMU室内组合导航系统的量测方程:
Bk=KAk+vk;
式中,Bk为tk时刻灭点位置矩阵、Ak为tk时刻与四元数相关的CAMERA旋转矩阵,vk为量测噪声序列;
步骤九:根据步骤二中陀螺输出的角速度测量矩阵Wk-1:
式中,I为单位阵,[Φa]为由旋转矢量构造的反对称矩阵,Δt=tk-tk-1为更新周期;
步骤十:根据步骤九中得到的矩阵Wk-1得到MIMU的离散域四元数Qk更新模型:
步骤十一:选取四元数Qk=[q0,k q1,k q2,k q3,k]T作为CAMERA/MIMU室内组合导航系统的状态向量,利用步骤十中获得的四元数更新模型确定CAMERA/MIMU室内组合导航系统的状态方程为:
xk=Fk-1xk-1+wk-1;
式中,qi,k(i=0,1,2,3)为tk时刻四元数的分量值,wk-1为系统激励噪声序列,系统状态向量xk=Qk,一步转移矩阵
步骤十二:根据步骤八和步骤十一建立的CAMERA/MIMU室内组合导航系统的状态方程和量测方程进行卡尔曼滤波,估计出四元数Qk。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410172298.4A CN103925926B (zh) | 2014-04-25 | 2014-04-25 | 一种基于camera/mimu室内组合导航系统的四元数测量方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410172298.4A CN103925926B (zh) | 2014-04-25 | 2014-04-25 | 一种基于camera/mimu室内组合导航系统的四元数测量方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103925926A CN103925926A (zh) | 2014-07-16 |
CN103925926B true CN103925926B (zh) | 2016-08-24 |
Family
ID=51144227
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410172298.4A Active CN103925926B (zh) | 2014-04-25 | 2014-04-25 | 一种基于camera/mimu室内组合导航系统的四元数测量方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103925926B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104216202A (zh) * | 2014-08-25 | 2014-12-17 | 太仓中科信息技术研究院 | 一种结合惯性陀螺仪的摄像机视觉实时定位系统及定位方法 |
CN104833354A (zh) * | 2015-05-25 | 2015-08-12 | 梁步阁 | 一种多基多模组网融合室内人员导航定位系统及其实施方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102109348A (zh) * | 2009-12-25 | 2011-06-29 | 财团法人工业技术研究院 | 定位载体、估测载体姿态与建地图的系统与方法 |
EP2434256A2 (en) * | 2010-09-24 | 2012-03-28 | Honeywell International Inc. | Camera and inertial measurement unit integration with navigation data feedback for feature tracking |
CN102445200A (zh) * | 2011-09-30 | 2012-05-09 | 南京理工大学 | 微小型个人组合导航系统及其导航定位方法 |
CN103424114A (zh) * | 2012-05-22 | 2013-12-04 | 同济大学 | 一种视觉导航/惯性导航的全组合方法 |
-
2014
- 2014-04-25 CN CN201410172298.4A patent/CN103925926B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102109348A (zh) * | 2009-12-25 | 2011-06-29 | 财团法人工业技术研究院 | 定位载体、估测载体姿态与建地图的系统与方法 |
EP2434256A2 (en) * | 2010-09-24 | 2012-03-28 | Honeywell International Inc. | Camera and inertial measurement unit integration with navigation data feedback for feature tracking |
CN102445200A (zh) * | 2011-09-30 | 2012-05-09 | 南京理工大学 | 微小型个人组合导航系统及其导航定位方法 |
CN103424114A (zh) * | 2012-05-22 | 2013-12-04 | 同济大学 | 一种视觉导航/惯性导航的全组合方法 |
Non-Patent Citations (2)
Title |
---|
Enhancing Indoor Inertial pedestrian Navigation Using a Shoe-Worn Marker;Mitija Placer et al.;《Sensors 2013》;20130802;第9836-9859页 * |
室内微型组合导航计算机设计;程庆等;《电子测量技术》;20140228;第37卷(第2期);第9-12、43页 * |
Also Published As
Publication number | Publication date |
---|---|
CN103925926A (zh) | 2014-07-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104406607B (zh) | 一种多视场复合光学敏感器的标定装置及方法 | |
CN106803271B (zh) | 一种视觉导航无人机的摄像机标定方法及装置 | |
CN106767752B (zh) | 一种基于偏振信息的组合导航方法 | |
CN106482746B (zh) | 一种用于混合式惯导系统的加速度计内杆臂标定与补偿方法 | |
CN108592945A (zh) | 一种惯性/天文组合系统误差的在线标定方法 | |
CN109520476B (zh) | 基于惯性测量单元的后方交会动态位姿测量系统及方法 | |
WO2016198009A1 (zh) | 一种检测航向的方法和装置 | |
CN110361010A (zh) | 一种基于占据栅格地图且结合imu的移动机器人定位方法 | |
CN104155765A (zh) | 在拼接式集成成像显示器中校正三维图像的方法和设备 | |
CN103278177A (zh) | 基于摄像组网测量的惯性测量组合标定方法 | |
CN105953795B (zh) | 一种用于航天器表面巡视的导航装置及方法 | |
CN109931955A (zh) | 基于状态相关李群滤波的捷联惯性导航系统初始对准方法 | |
CN102944191A (zh) | 一种基于平面圆靶标的三维视觉测量数据拼接方法及装置 | |
CN104075713A (zh) | 一种惯性/天文组合导航方法 | |
CN102519485A (zh) | 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法 | |
CN104848861A (zh) | 一种基于图像消失点识别技术的移动设备姿态测量方法 | |
CN103398725A (zh) | 一种基于星敏感器的捷联惯导系统初始对准的方法 | |
CN103925926B (zh) | 一种基于camera/mimu室内组合导航系统的四元数测量方法 | |
CN106908085B (zh) | 一种基于地球自转补偿的星光测量指向控制方法 | |
CN104913775A (zh) | 无人机对地高度的测量方法、无人机定位方法及装置 | |
CN104864875B (zh) | 一种基于非线性h∞滤波的航天器自主定位方法 | |
CN103954288B (zh) | 一种卫星姿态确定系统精度响应关系确定方法 | |
CN105138756A (zh) | 卫星敏捷成像仿真及定位精度评估方法 | |
CN103364167A (zh) | 一种观察窗折射偏移校正方法 | |
CN109764876A (zh) | 无人平台的多模态融合定位方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
TR01 | Transfer of patent right |
Effective date of registration: 20190530 Address after: 215100 Room 313, No. 99 Jinji Avenue, Suzhou Industrial Zone, Suzhou City, Jiangsu Province Patentee after: SUZHOU GST INFOMATION TECHNOLOGY CO., LTD. Address before: 150001 Intellectual Property Office, Harbin Engineering University science and technology office, 145 Nantong Avenue, Nangang District, Harbin, Heilongjiang Patentee before: Harbin Engineering Univ. |
|
TR01 | Transfer of patent right |