CN113447020B - 一种基于多传感器融合的姿态解算方法 - Google Patents

一种基于多传感器融合的姿态解算方法 Download PDF

Info

Publication number
CN113447020B
CN113447020B CN202110714107.2A CN202110714107A CN113447020B CN 113447020 B CN113447020 B CN 113447020B CN 202110714107 A CN202110714107 A CN 202110714107A CN 113447020 B CN113447020 B CN 113447020B
Authority
CN
China
Prior art keywords
angle
pitch angle
target plate
azimuth angle
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
CN202110714107.2A
Other languages
English (en)
Other versions
CN113447020A (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.)
CETC 58 Research Institute
Original Assignee
CETC 58 Research Institute
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 CETC 58 Research Institute filed Critical CETC 58 Research Institute
Priority to CN202110714107.2A priority Critical patent/CN113447020B/zh
Publication of CN113447020A publication Critical patent/CN113447020A/zh
Application granted granted Critical
Publication of CN113447020B publication Critical patent/CN113447020B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开一种基于多传感器融合的姿态解算方法，属于图像处理模式识别领域。在靶板上安装陀螺仪和LED灯，将靶板固定在头盔上，左右两部相机安放在与靶板等高位置；DSP分别计算两部相机采集的图像中LED灯的坐标；根据坐标系的转换关系解算得到靶板与左右两部相机相对的姿态角信息；解算出初始位置的方位角和俯仰角、实时位置的方位角和俯仰角、当前位置相对初始位置方位角与俯仰角的变换量；根据靶板与左右两部相机的相对位置，融合左右两路图像解算出方位角与俯仰角；实时获取陀螺仪的方位角速度和俯仰角速度；根据姿态角输出时刻计算陀螺仪方位角和俯仰角的累积量；根据图像解算出来的方位角和俯仰角与陀螺仪累积的方位角和俯仰角进行融合。

Description

一种基于多传感器融合的姿态解算方法
技术领域
本发明涉及图像处理模式识别技术领域，特别涉及伺服系统跟随驾驶员头部运动的系统。
背景技术
近年来，战场上作战方式的多样化、智能化，对作战辅助系统也提出了很高的要求。驾驶员在装甲车辆内部(全密闭环境下)，观察外部环境时，需借助光电系统获取正常甚至更广阔的驾驶视野；为了更快捷的获取不同角度的视野，亟需一种基于通过解算人体头部姿态控制光电系统随动的系统。
国内外现有的技术中，通常有三种方法：一是通过采集并分析人脸的特征，基于人脸的特征点检测，根据特征点的旋转变化解算出运动的姿态。基于该原理的系统受环境影响因素大，解算出的姿态角误差大，在密闭黑暗的空间内甚至无法正常工作；二是通过单一的图像传感器采集图像数据，分析解算头部运动的姿态，但受到处理器性能的限制，不能满足系统的实时性要求，解算出的姿态角存在滞后现象，同时存在人体头部转动角速度大时(大于100°/s)，采集的图像会有拖尾现象，导致通过图像无法正常解算姿态角；三是使用单个相机采集图像数据，受到相机的视场角限制，人体头部运动的角度有限。
发明内容
本发明的目的在于提供一种基于多传感器融合的姿态解算方法，以解决头部转动角速度大时无法正常工作和实时性问题。
为解决上述技术问题，本发明提供了一种基于多传感器融合的姿态解算方法，包括：
在靶板上安装陀螺仪和若干个LED灯，将靶板固定在头盔上，左右两部相机安放在与靶板等高位置；
其中，两部相机采集的图像数据和陀螺仪输出的数据均传至外接DSP；
DSP分别计算两部相机采集的图像中LED灯的坐标；
根据坐标系的转换关系解算得到靶板与左右两部相机相对的姿态角信息；
解算出初始位置的方位角和俯仰角、实时位置的方位角和俯仰角、当前位置相对初始位置方位角与俯仰角的变换量；
根据靶板与左右两部相机的相对位置，融合左右两路图像解算出方位角与俯仰角；
实时获取陀螺仪的方位角速度和俯仰角速度；
根据左右两部相机相对的姿态角输出时刻计算陀螺仪方位角和俯仰角的累积量；
根据图像解算出来的方位角和俯仰角与陀螺仪累积的方位角和俯仰角进行融合。
可选的，所述两部相机安放的位置需确保在头部转动时，至少有一部相机能够观察到靶板。
可选的，所述陀螺仪安装在靶板的中心，LED灯的数量为8个，围绕所述陀螺仪安装在靶板的四个边。
可选的，所述两部相机安装在靶板法线两侧，两部相机与所述靶板之间等距且等高。
在本发明提供的基于多传感器融合的姿态解算方法中，在靶板上安装陀螺仪和若干个LED灯，将靶板固定在头盔上，左右两部相机安放在与靶板等高位置；DSP分别计算两部相机采集的图像中LED灯的坐标；根据坐标系的转换关系解算得到靶板与左右两部相机相对的姿态角信息；解算出初始位置的方位角和俯仰角、实时位置的方位角和俯仰角、当前位置相对初始位置方位角与俯仰角的变换量；根据靶板与左右两部相机的相对位置，融合左右两路图像解算出方位角与俯仰角；实时获取陀螺仪的方位角速度和俯仰角速度；根据姿态角输出时刻计算陀螺仪方位角和俯仰角的累积量；根据图像解算出来的方位角和俯仰角与陀螺仪累积的方位角和俯仰角进行融合。本发明通过高精度陀螺仪和双高清高速相机同时计算并进行数据融合，实现了实时获取头部转动的方位角和俯仰角，有效解决了现有方法存在的实时性问题，避免了在头部快速转动时，解算误差大的问题；同时，近红外LED灯组成主动发光灯点阵列，在相机镜头与相机之间加装近红外波段成像滤光片，解决了现有方法受环境光干扰解算误差大，在密闭黑暗的环境下无法正常工作的问题。
附图说明
图1是本发明提供的靶板组成结构示意图；
图2是本发明提供的靶板与相机相对位置示意图；
图3是本发明提供的零位标定阶段步骤流程示意图；
图4是本发明提供的正常工作阶段步骤流程示意图。
具体实施方式
以下结合附图和具体实施例对本发明提出的一种基于多传感器融合的姿态解算方法作进一步详细说明。根据下面说明和权利要求书，本发明的优点和特征将更清楚。需说明的是，附图均采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施例的目的。
实施例一
本发明提供了一种基于多传感器融合的姿态解算方法，包括如下步骤：
如图1所示，在靶板的中心安装高精度陀螺仪，围绕该陀螺仪在靶板的四个边均匀安装近红外LED灯，每个边的安装的数量为3个。LED灯的列间距L1＝20mm，行间距L2＝11mm。
将靶板固定在头盔上。左右两部高清高速相机(即左相机和右相机)安放在与靶板等高(高度为H)的位置，两部相机与靶板之间等距，距离为D1＝250mm，两部相机之间相距D2＝100mm。
FPGA同步采集两部高清高速相机的图像数据，通过高速接口传输至外接DSP，所述陀螺仪输出的数据也通过DSP采集。
DSP对收集到的数据进行零位标定阶段和正常工作阶段：
首先零位标定阶段的流程如图3所示，包括：
S1、两部相机同步曝光成像；
S2、DSP同步采集两部相机的图像，得到含有LED灯点阵列信息的图像；
S3、DSP计算出两路相机采集到图像上LED灯点的坐标；
S4、DSP根据世界坐标系、相机坐标系、图像中心坐标系和像素坐标系的转换关系，由图像上LED灯点的坐标计算出每路图像LED灯点阵列的欧拉角；
S5、DSP根据LED灯点阵列欧拉角与姿态角(姿态角包含方位角和俯仰角)的关系，计算得到当前头部的姿态角，并作为左右两路图像计算姿态的初始方位、俯仰角保存。
然后正常工作阶段的流程如图4所示，包括：
S6、DSP周期性(T1＝4ms)获取陀螺仪的方位角速度和俯仰角速度/>
S7、DSP周期性(T2＝20ms)采集两部相机的图像，并进行姿态解算，即重复步骤S1、S2、S3、S4，得到当前帧(编号：n)图像解算的方位、俯仰角
T1<T2，且T2＝m*T1，m为正整数。
S8、对于在图像采集周期边界的姿态解算结果为：
左相机方位：是左相机在第n帧时刻解算出的方位角，/>是左相机初始时刻解算出的方位角，/>是左相机在第n帧时刻解算出的相对方位角。
左相机俯仰：是左相机第n帧时刻解算出的俯仰角角，/>是左相机初始时刻解算出的俯仰角，PitL是左相机在第n帧时刻解算出的相对俯仰角。
右相机方位：是右相机在第n帧时刻解算出的方位角，/>是右相机在初始时刻解算出的方位角，AziR是左相机在第n帧时刻解算出的相对方位角。
右相机俯仰：是右相机第n帧时刻解算出的俯仰角角，/>是右相机初始时刻解算出的俯仰角，PitR是右相机在第n帧时刻解算出的相对俯仰角。
S9、对于不在图像采集周期边界的姿态解算，如：
时刻T＝n*T2+k*T1，其中k的范围为{1，m-1}，
给出如下计算方法：
左相机方位：为k时刻时陀螺仪的方位角速度。
左相机俯仰：为k时刻时陀螺仪的俯仰角速度。
右相机方位：
右相机俯仰：
S10、计算两部相机结果融合后的姿态角，
方位：Azi＝a*AziL+(1-a)*AziR
俯仰：Pit＝a*PitL+(1-a)*PitR
其中a为0～1之间的小数。
基于以上步骤，可以毫秒级的获取头部转动的方位角和俯仰角，解决了现有方法实时性的问题，解决了现有方法因头部快速转动无法获取头部转动的方位角和俯仰角问题。同时，使用了近红外LED灯作为目标靶板，解决了常有方法在黑暗环境中无法获取头部转动的方位角和俯仰角问题。
上述描述仅是对本发明较佳实施例的描述，并非对本发明范围的任何限定，本发明领域的普通技术人员根据上述揭示内容做的任何变更、修饰，均属于权利要求书的保护范围。

Claims (4)

1.一种基于多传感器融合的姿态解算方法，其特征在于，包括：
在靶板上安装陀螺仪和若干个LED灯，将靶板固定在头盔上，左右两部相机安放在与靶板等高位置；
其中，两部相机采集的图像数据和陀螺仪输出的数据均传至外接DSP；
DSP分别计算两部相机采集的图像中LED灯的坐标；
根据坐标系的转换关系解算得到靶板与左右两部相机相对的姿态角信息；
解算出初始位置的方位角和俯仰角、实时位置的方位角和俯仰角、当前位置相对初始位置方位角与俯仰角的变换量；
根据靶板与左右两部相机的相对位置，融合左右两路图像解算出方位角与俯仰角；
实时获取陀螺仪的方位角速度和俯仰角速度；
根据左右两部相机相对的姿态角输出时刻计算陀螺仪方位角和俯仰角的累积量；
根据图像解算出来的方位角和俯仰角与陀螺仪累积的方位角和俯仰角进行融合。
2.如权利要求1所述的基于多传感器融合的姿态解算方法，其特征在于，所述两部相机安放的位置需确保在头部转动时，至少有一部相机能够观察到靶板。
3.如权利要求1所述的基于多传感器融合的姿态解算方法，其特征在于，所述陀螺仪安装在靶板的中心，LED灯的数量为8个，围绕所述陀螺仪安装在靶板的四个边。
4.如权利要求1所述的基于多传感器融合的姿态解算方法，其特征在于，所述两部相机安装在靶板法线两侧，两部相机与所述靶板之间等距且等高。
CN202110714107.2A 2021-06-25 2021-06-25 一种基于多传感器融合的姿态解算方法 Active CN113447020B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110714107.2A CN113447020B (zh) 2021-06-25 2021-06-25 一种基于多传感器融合的姿态解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110714107.2A CN113447020B (zh) 2021-06-25 2021-06-25 一种基于多传感器融合的姿态解算方法

Publications (2)

Publication Number Publication Date
CN113447020A CN113447020A (zh) 2021-09-28
CN113447020B true CN113447020B (zh) 2024-02-13

Family

ID=77812967

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110714107.2A Active CN113447020B (zh) 2021-06-25 2021-06-25 一种基于多传感器融合的姿态解算方法

Country Status (1)

Country Link
CN (1) CN113447020B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107063228A (zh) * 2016-12-21 2017-08-18 上海交通大学 基于双目视觉的目标姿态解算方法
CN110633015A (zh) * 2019-10-31 2019-12-31 江苏北方湖光光电有限公司 一种驾驶员头部姿态检测装置
CN110674888A (zh) * 2019-10-11 2020-01-10 中国人民解放军海军航空大学青岛校区 一种基于数据融合的头部姿态识别方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6684559B2 (ja) * 2015-09-16 2020-04-22 株式会社バンダイナムコエンターテインメント プログラムおよび画像生成装置

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107063228A (zh) * 2016-12-21 2017-08-18 上海交通大学 基于双目视觉的目标姿态解算方法
CN110674888A (zh) * 2019-10-11 2020-01-10 中国人民解放军海军航空大学青岛校区 一种基于数据融合的头部姿态识别方法
CN110633015A (zh) * 2019-10-31 2019-12-31 江苏北方湖光光电有限公司 一种驾驶员头部姿态检测装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于惯性与光学技术的头部测量方法设计;王勇军等;《压电与声光》;第36卷(第6期);第1002-1005页 *

Also Published As

Publication number Publication date
CN113447020A (zh) 2021-09-28

Similar Documents

Publication Publication Date Title
WO2022142759A1 (zh) 一种激光雷达与相机联合标定方法
CN110103217B (zh) 工业机器人手眼标定方法
CN110243283B (zh) 一种可变视轴视觉测量系统及方法
CN103065323B (zh) 一种基于单应性变换矩阵的分段空间对准方法
CN108413987B (zh) 一种定日镜的校准方法、装置及系统
CN109975759B (zh) 一种基于三色激光的水下无人机定位方法及装置
CN113379839B (zh) 一种基于事件相机系统的地面视角单目视觉里程计方法
CN106403900A (zh) 飞行物追踪定位系统及方法
CN106679676A (zh) 一种单视场多功能光学敏感器及实现方法
CN112819711B (zh) 一种基于单目视觉的利用道路车道线的车辆反向定位方法
CN112484722A (zh) 结合惯性导航系统的视觉传感器全局定位方法
CN110030988B (zh) 一种用于高动态位姿测量的多信标高速同步识别方法
CN109765931B (zh) 一种适用于防波堤巡检无人机的近红外视频自动导航方法
CN115717867A (zh) 一种基于机载双相机和目标追踪的桥梁变形测量方法
CN115239822A (zh) 分体式飞行车辆多模块间实时视觉识别定位方法及系统
CN113447020B (zh) 一种基于多传感器融合的姿态解算方法
CN114659523A (zh) 一种大范围高精度姿态测量方法及装置
CN117058209B (zh) 一种基于三维地图的飞行汽车视觉图像深度信息计算方法
CN110889353A (zh) 基于主焦点大视场光电望远镜的空间目标识别方法
CN107990825B (zh) 基于先验数据校正的高精度位置测量装置与方法
CN114973037B (zh) 一种无人机智能检测与同步定位多目标的方法
CN109993768B (zh) 一种提高伺服跟踪实时性和精准性的空中目标测谱方法
CN102798391B (zh) 一种激光影像定位导航装置
CN115855041A (zh) 一种农业机器人定位方法、系统及装置
CN113959412A (zh) 一种基于红外探测的无人机对接过程主动视觉导航方法

Legal Events

Date Code Title Description
PB01 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