CN114199239B - 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统 - Google Patents
结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统 Download PDFInfo
- Publication number
- CN114199239B CN114199239B CN202210010936.7A CN202210010936A CN114199239B CN 114199239 B CN114199239 B CN 114199239B CN 202210010936 A CN202210010936 A CN 202210010936A CN 114199239 B CN114199239 B CN 114199239B
- Authority
- CN
- China
- Prior art keywords
- cabin
- imu
- helmet
- carrier
- pose data
- 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
- 238000001514 detection method Methods 0.000 title claims abstract description 9
- 238000005259 measurement Methods 0.000 claims abstract description 71
- 230000000007 visual effect Effects 0.000 claims abstract description 54
- 230000033001 locomotion Effects 0.000 claims abstract description 44
- 238000001914 filtration Methods 0.000 claims abstract description 30
- 208000003164 Diplopia Diseases 0.000 claims abstract description 19
- 208000029444 double vision Diseases 0.000 claims abstract description 19
- 239000011159 matrix material Substances 0.000 claims description 33
- 230000001133 acceleration Effects 0.000 claims description 31
- 238000000034 method Methods 0.000 claims description 24
- 239000013598 vector Substances 0.000 claims description 23
- 238000012937 correction Methods 0.000 claims description 18
- 230000009977 dual effect Effects 0.000 claims description 14
- 238000006243 chemical reaction Methods 0.000 claims description 3
- DMBHHRLKUKUOEG-UHFFFAOYSA-N diphenylamine Chemical compound C=1C=CC=CC=1NC1=CC=CC=C1 DMBHHRLKUKUOEG-UHFFFAOYSA-N 0.000 claims 1
- 238000005070 sampling Methods 0.000 abstract description 6
- 230000009466 transformation Effects 0.000 description 7
- 230000008901 benefit Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 230000008859 change Effects 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 238000012216 screening Methods 0.000 description 3
- 230000007704 transition Effects 0.000 description 3
- 238000012546 transfer Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000001308 synthesis method Methods 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
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/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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/53—Determining attitude
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，包括：北斗导航单元、座舱IMU、头盔IMU、双视觉辅助单元和测量单元；座舱IMU和头盔IMU分别用于获取运动载具和头部的运动信息；双视觉辅助单元用于通过双视觉方式获取头盔相对于座舱的视觉位姿数据；北斗导航单元利用北斗数据获得运动载具的导航结果；测量单元用于根据导航结果对座舱IMU零偏进行修正；通过座舱和头盔的双IMU差分获得的载体坐标系中头盔相对座舱的惯性位姿数据；以视觉位姿数据为外部观测量对惯性位姿数据进行卡尔曼滤波，输出滤波后相对位姿数据。本发明充分利用惯性测姿采样频率高、短时精度高，视觉测姿无累积误差的特性，实现高精度的位姿追踪。
Description
技术领域
本发明涉及相对位姿测量和卡尔曼滤波技术领域，尤其涉及一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统。
背景技术
随着现代装备的智能化升级，在运动的车辆或飞行器中头戴显示功能需获取头盔佩戴者的姿态位姿信息。这类装备需要获取运动载具中操控人员与运动载具实时准确的相对姿态变化，将乘员头部动作与装备操控结合，实现人机一体的自然交互方式，从而实现装备快速协同、信息互联等功能，提升战场效率。相对位姿测量相比于惯性系下绝对姿态测量存在的主要差别为：其一，在载具相对惯性系存在未知运动的条件下，被测目标的单一惯性信息也不可作为直接参考，需要同载具的惯性信息差分处理，通过转换到载具坐标系下进行相对姿态解算；其二，卫星导航与惯性导航这种常用的组合导航手段不再可用，需要使用卫星导航、视觉导航、雷达导航等多种导航方式分别对运动载具上与头盔上两个IMU的零偏分别进行修正。
在目前常用的相对位姿测量手段中，惯性与视觉组合是一种可靠的相对姿态测量方式。惯性测姿的短时精度较高，长时积分会产生累积误差，而视觉测姿虽然易受外部近似特征干扰及光线变化影响，但特征准确时精度稳定，在测姿精度上与惯性测量形成互补。从测量频率上，惯性信息可实现高达1kHz采样的实时测量，图像信息通常为60Hz或120Hz，且存在稳定解算延迟，惯性与视觉的融合可实现与纯惯性相同的高频率输出。惯性与视觉的组合可充分利用惯性测量采样周期短、短时精度高的特性，并发挥视觉测量可直接获得无累积误差相对姿态的特点。
现有的视觉观测与惯性组合的形式，缺少足够的观测信息，且无法同时抑制两个惯性器件漂移，导致导航系统积分发散。
发明内容
鉴于上述的分析，本发明旨在提供一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，利用北斗导航和视觉进行辅助，提升头部测姿结果的准确性。
本发明提供的技术方案是：
本发明公开了一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，包括：北斗导航单元、座舱IMU、头盔IMU、双视觉辅助单元和测量单元；
所述座舱IMU与运动载具固连，头盔IMU与载具内人员头盔固连，分别用于获取运动载具和人员头部的运动信息；
所述双视觉辅助单元布置于运动载具内，用于通过双视觉方式获取头盔相对于座舱的视觉位姿数据；
所述北斗导航单元布置于运动载具上，利用北斗数据获得运动载具的导航结果；
所述测量单元，用于根据导航结果对座舱IMU零偏进行修正；通过座舱和头盔的双IMU差分获得的载体坐标系中头盔相对座舱的惯性位姿数据；以视觉位姿数据为外部观测量对惯性位姿数据进行卡尔曼滤波，输出滤波后相对位姿数据。
进一步地，所述测量单元包括座舱IMU零偏校正模块、双IMU差分模块和位姿滤波模块；
所述座舱IMU零偏校正模块与所述北斗导航单元和座舱IMU连接，用于根据导航结果对座舱IMU零偏进行修正；
所述双IMU差分模块与座舱IMU和头盔IMU连接；接收座舱IMU和头盔IMU的惯性测量数据进行双IMU差分运算得到的包括载体坐标系下头盔相对座舱的位置、速度和旋转四元数在内的惯性位姿数据；
所述位姿滤波模块与座舱IMU零偏校正模块、双IMU差分模块和双视觉辅助单元分别连接，以双视觉辅助单元输出的视觉位姿数据为外部观测量对包括惯性位姿数据和修正后座舱IMU零偏在内的状态向量进行卡尔曼滤波，输出滤波后相对位姿数据。
进一步地，所述北斗导航单元为载波相位差分的北斗导航系统；其北斗卫星接收天线布置于运动载具外，接收卫星信号；处理器布置于座舱内，用于测量出运动载具在地理坐标系中的导航结果输出到所述座舱IMU零偏校正模块。
进一步地，所述座舱IMU零偏校正模块内的零偏进行修正的方式包括：
方式一、构建运动载具在地理坐标系中的运动状态方程，建立座舱IMU的卡尔曼滤波器，利用北斗导航单元测量的数据生成运动载具在地理坐标系中的观测量，对载具在地理坐标系中的运动状态方程中包括的角速度与加速度的零偏进行更新，得到修正后的座舱IMU的角速度与加速度的零偏；
方式二、构建运动载具和北斗导航单元深组合状态方程，建立组合后的卡尔曼滤波器，将座舱IMU介入到北斗导航的卫星捕获环路中，滤波后获得修正后的座舱IMU的角速度与加速度的零偏。
进一步地，双视觉辅助单元包括第一视觉模块和第二视觉模块；
所述第一视觉模块包括第一相机模组和第一标志点；
所述第一相机模组安装于座舱内正对头盔佩戴人员供其使用的操纵台上，并与座舱IMU固连；与第一相机模组配合的第一标志点布置于头盔上；第一相机模组通过测量第一标志点获得头盔相对座舱的第一视觉位姿数据；
所述第二视觉模块包括第二相机模组和第二标志点；
所述第二相机模组安装于头盔上，并与头盔IMU固连；与第二相机模组配合的第二标志点布置于正对头盔佩戴人员供其使用的操纵台上；第二相机模组通过测量第二标志点后，进行坐标系转换获得头盔相对座舱的第二视觉位姿数据。
进一步地，所述第二视觉位姿数据有输出时，采用第二视觉位姿数据为卡尔曼滤波的外部观测量；当所述第二视觉位姿数据无输出时，采用第一视觉位姿数据为卡尔曼滤波的外部观测量。
进一步地，所述第一标志点在头盔上的布局，满足在头盔设定的运动范围内，使所述第一标志点处于第一相机模组的视觉范围内；并且，所述第一标志点的数量大于等于4；
所述第二标志点在操纵台上的布局，保证在固定角度内，使所述第二标志点处于第二相机模组的视觉范围内；所述第二标志点的数量大于等于4。
进一步地，所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式；所述第一相机模组和第二相机模组采用单目、双目或结构光方式。
进一步地，所述双IMU差分模块中计算的惯性位姿数据中的旋转四元数采用角速度差分法进行求解；
在角速度差分法中，通过对当前时刻头盔与运动载具的相对角速度进行积分得到头盔与运动载具的旋转四元数。
进一步地，所述角速度差分法，包括：
获取头盔坐标系到运动载具坐标系的相对旋转矩阵；
根据头盔与运动载具的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；
使用当前时刻相对角增量计算相对旋转四元数得到头盔相对于运动载具的位姿数据。
本发明的可以至少实现以下有益效果之一：
本发明提出的一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，通过北斗导航提升了座舱IMU的零偏准确性，通过双视觉辅助头盔与座舱的相对位姿关系，对进行头盔与座舱的双IMU惯性积分差分的位姿测量结果进行修正，得到准确的位姿测量结果。这种方法充分利用惯性测姿采样频率高、短时精度高，卫星导航和视觉测姿无累积误差的特性，实现高精度的位姿追踪。该方法相较于传统相对位姿测量方案，更为有效调用各种导航方式特性，且布置简便，适用于乘用车辆、战斗机、武装直升机中头戴显示系统等工程应用。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为本发明实施例中的觉辅助惯性差分位姿测量系统组成框图；
图2为本发明实施例中的运动载具中姿态测量的坐标系示意图；
图3为本发明实施例中的测量单元的组成和连接示意框图；
图4为本发明实施例中的车辆内的典型座舱布置方式图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。
本实施例以车辆作为运动载具，公开的一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，如图1所示，包括北斗导航单元、座舱IMU、头盔IMU、双视觉辅助单元和测量单元；
所述座舱IMU与运动载具固连，头盔IMU与载具内人员头盔固连，分别用于获取运动载具和人员头部的运动信息；
所述双视觉辅助单元布置于运动载具内，用于通过双视觉方式获取头盔相对于座舱的视觉位姿数据；
所述北斗导航单元布置于运动载具上，利用北斗数据获得运动载具的导航结果；
所述测量单元，用于根据导航结果对座舱IMU零偏进行修正；通过座舱IMU和头盔IMU的双IMU差分获得的载体坐标系中头盔相对座舱的惯性位姿数据；以视觉位姿数据为外部观测量对惯性位姿数据进行卡尔曼滤波，输出滤波后相对位姿数据。
运动车辆中姿态测量的坐标系示意图如图2所示，
其中，如图3所示，所述测量单元包括座舱IMU零偏校正模块、双IMU差分模块和位姿滤波模块；
所述座舱IMU零偏校正模块与所述北斗导航单元和座舱IMU连接，用于根据导航结果对座舱IMU零偏进行修正；
所述双IMU差分模块与座舱IMU和头盔IMU连接；接收座舱IMU和头盔IMU的惯性测量数据进行双IMU差分运算得到的载体坐标系下头盔相对座舱的位置、速度、旋转四元数；
所述位姿滤波模块与座舱IMU零偏校正模块、双IMU差分模块和双视觉辅助单元分别连接，以双视觉辅助单元输出的视觉位姿数据为外部观测量对惯性位姿数据进行卡尔曼滤波，输出滤波后相对位姿数据。
具体的，所述北斗导航单元为载波相位差分的北斗导航系统；即为RTK系统，基于载波相位观测值的实时动态定位，实时地提供运动载具在地理坐标系中的三维定位结果，并达到厘米级精度。
北斗卫星接收天线布置于运动载具外，接收卫星信号；处理器布置于座舱内，用于测量出运动载具在地理坐标系中的导航结果输出到所述座舱IMU零偏校正模块。
具体的，所述座舱IMU零偏校正模块内的零偏进行修正的方式包括：
方式一、构建运动载具在地理坐标系中的运动状态方程，建立座舱IMU的卡尔曼滤波器，利用北斗导航单元测量的数据生成运动载具在地理坐标系中的观测量，对载具在地理坐标系中的运动状态方程中包括的角速度与加速度的零偏进行更新，得到修正后的座舱IMU的角速度与加速度的零偏；
所述座舱IMU的卡尔曼滤波器的状态向量可包括座舱IMU测量的运动载具的三维位置、速度、姿态和IMU的角速度与加速度的零偏；观测向量可为位置偏差及姿态偏差。以此进行卡尔曼滤波，实现IMU的角速度与加速度的零偏的修正。
方式二、构建运动载具和北斗导航单元深组合状态方程，建立组合后的卡尔曼滤波器，将座舱IMU介入到北斗导航的卫星捕获环路中，滤波后获得修正后的座舱IMU的角速度与加速度的零偏。
所述组合后的卡尔曼滤波器中状态向量可包括由北斗导航单元输出和座舱IMU输出混合的三维位置、速度、姿态数据以及IMU的角速度与加速度的零偏；其中，混合的三维位置、速度、姿态数据可以根据卫星导航和IMU测量的不同优势来选取对应的数据组成。观测向量可为位置偏差及姿态偏差。以此进行卡尔曼滤波，实现座舱位置、速度、姿态数据的精确输出和座舱IMU的角速度与加速度的零偏的修正。
具体的，双视觉辅助单元包括第一视觉模块和第二视觉模块；
所述第一视觉模块包括第一相机模组和第一标志点；
所述第一相机模组安装于座舱内正对头盔佩戴人员供其使用的操纵台上，并与座舱IMU固连；与第一相机模组配合的第一标志点布置于头盔上；第一相机模组通过测量第一标志点获得头盔相对座舱的第一视觉位姿数据。
所述第一标志点在头盔上的布局，满足在头盔设定的运动范围内，使所述第一标志点处于第一相机模组的视觉范围内；并且，所述第一标志点的数量大于等于4；
所述第一视觉模块的优势在于，头盔带动第一标志点运动时，第一标志点始终不会超出相机视野，视觉测量系统能够全时段工作。
所述第二视觉模块包括第二相机模组和第二标志点；
所述第二相机模组安装于头盔上，并与头盔IMU固连；与第二相机模组配合的第二标志点布置于正对头盔佩戴人员供其使用的操纵台上；第二相机模组通过测量第二标志点后，进行坐标系转换获得头盔相对座舱的第二视觉位姿数据。
所述第二标志点在操纵台上的布局，保证在固定角度内，使所述第二标志点处于第二相机模组的视觉范围内，例如，安装在佩戴头盔的座舱乘员面前的显示设备的周边边框上，使乘员在观察显示信息时所述第二标志点处于第二相机模组的视觉范围内。所述第二标志点的数量大于等于4。
所述第二视觉模块的优势在于，所述第二标志点可以在相机视野内占有更大面积，因此视觉相对测量精度也更高，同时这种布置方式还带来了另外一种优势，即多套测量系统的多个相机可共享同一套标志点靶标，保证各套系统工作互不干扰。但缺点在于，在头盔运动过程中，一旦标志点靶标超出相机视野，视觉测量将无法工作。
优选的，所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式；
所述第一相机模组和第二相机模组采用单目、双目或结构光方式。只要能准确捕获红外标志点的图像信息即可。
更优选的，所述第一标志点和第二标志点采用同频闪烁的方式与对应的相机模组同步。通过图像处理提取红外点位置，避免外部自然光红外波段的光线干扰。
本实施例中的双视觉辅助单元中的一种具体的视觉位姿数据综合的方式包括：在所述第二视觉位姿数据有输出时，采用第二视觉位姿数据为卡尔曼滤波的外部观测量；当所述第二视觉位姿数据无输出时，采用第一视觉位姿数据为卡尔曼滤波的外部观测量。以此方式，在保证了测量精度的同时保证了较大的测量范围。
并且，在进行布置时，使所述座舱IMU与第一相机模组固连，所述头盔IMU与第二相机模组固连，通过上述固连可以减少标定工作量。
具体的，车辆内的典型座舱布置方式如图4所示。
具体的，双IMU差分模块中，进行双IMU差分运算得到的载体坐标系下头盔相对座舱的位置、速度和旋转四元数；其中，载体坐标系下头盔相对座舱的位置和速度可采用现有的技术方法获得，在此就不赘述。
所述旋转四元数采用角速度差分法进行求解；
在角速度差分法中，通过对当前时刻头盔与运动载具的相对角速度进行积分得到头盔与运动载具的相对位姿。
进一步地，所述角速度差分法，包括：
1)获取头盔坐标系到运动载具坐标系的相对旋转矩阵Rrel；
头盔坐标系到运动载具坐标系的相对旋转矩阵Rrel可通过对头盔和运动载具的标定获得。
2)根据头盔与运动载具的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；
根据头盔与运动载具的IMU获取角速度信息ωh(t)、ωp(t)与tk-1时刻相对旋转矩阵计算当前时刻t的相对角增量为：
3)使用当前时刻相对角增量计算相对旋转四元数得到头盔相对于运动载具的位姿数据。
用当前时刻相对角增量计算相对旋转四元数/>为
相对旋转四元数即为旋转四元数qh-p。
具体的，位姿滤波模块中，卡尔曼滤波中的状态向量
X＝[ph-p,vh-p,qh-p,bha,bpa,bhω,bpω]T；
X中ph-p、vh-p、qh-p定义为双IMU差分获得的载体坐标系下头盔相对运动载具的位置、速度、旋转四元数；bhω、bha为头盔IMU测量角速度与加速度的零偏；bpω、bpa为修正后的座舱IMU的角速度与加速度的零偏。
在忽略杆臂效应时，真实情况下状态方程可以用下式表示：
式中，为相对旋转矩阵；ah、bha和nha为头盔IMU加速度、加速度零偏和加速度噪声；ap、bpa、npa为座舱IMU加速度、加速度零偏和加速度噪声；ωh、bhω、nhω为头盔IMU角速度、角速度零偏和角速度噪声，ωp、bpω、npω为座舱IMU角速度、角速度零偏和角速度噪声；为头盔IMU加速度零偏噪声和角速度零偏噪声；/> 为座舱IMU加速度零偏噪声和角速度零偏噪声。
为不含误差的状态向量；
定义为不含误差的载体坐标系下头盔相对载体的位置、速度、旋转四元数；/>为不含误差的头盔IMU测量角速度与加速度的零偏；bpω、bpa为不含误差的座舱IMU测量角速度与加速度的零偏；
则用于表示真实状态X与不含噪声状态的状态误差向量ΔX为：
其中各项的具体展开如下：
根据建立当前时刻误差状态方程Δx与下一时刻状态误差的递推关系
Δt为当前时刻到下一时刻的时间差。
得到，系统状态误差递推关系即状态误差方程为：
FX为状态转移矩阵；FN为噪声转移矩阵；
U＝[ah,ωh,ap,ωp]T；
ah、ωh、ap、和ωp分别为头盔IMU加速度与角速度输出、座舱IMU的加速度与角速度输出；
N为状态噪声向量，
其中，和/>分别为头盔IMU加速度噪声、座舱IMU加速度噪声、头盔IMU角速度噪声、座舱IMU角速度噪声，头盔IMU加速度零偏噪声、座舱IMU加速度零偏噪声、头盔IMU角速度零偏噪声和座舱IMU角速度零偏噪声的方差。
进一步地，
所述状态转移矩阵
其中，为不含误差的头盔到运动载具相对旋转矩阵；
I为单位矩阵；
噪声转移矩阵
则，系统的状态协方差矩阵为
基于上述过程，所述更新卡尔曼滤波的误差状态协方差矩阵的过程包括：
1)获取头盔、座舱的IMU数据；
2)根据头盔与座舱运动模型更新状态向量
3)更新状态转移矩阵FX、更新协方差矩阵FNNFN T；
4)更新误差状态协方差矩阵
适用于本方案的位姿测量卡尔曼滤波的滤波器的观测方程为：
其中，误差向量Hp为位置量测矩阵；误差向量/>Hq为旋转量测矩阵；zp、zq为卡尔曼滤波的外部观测量的位置向量和角度向量；/>为卡尔曼滤波估计的位置向量和角度向量。
在观测过程中，
1)列写更新部分位置测量模型zp；
其中，位置测量模型
式中，pc-t代表相机观测的位移，由视觉测量经过内参变化后得到；ph-p、为头盔相对座舱的平移向量与旋转矩阵；pc-h为相机相对头盔的位置观测量，可通过标定得到；np为量测噪声。
则误差向量展开有：
展开后忽略二阶项，得到：
根据观测方程Δzp＝HpΔx，位置量测矩阵Hp写作如下：
式中，pc-h为位置观测量，[pc-h]×为对应叉乘运算矩阵。
2)列写更新部分角度测量模型zq；
其中，角度测量模型
则误差向量展开有
根据观测方程Δzq＝HqΔx，旋转量测矩阵Hq写作如下
本实施例的更新状态协方差矩阵与状态向量的过程包括：
1)计算观测残差
2)计算更新矩阵S＝HPHT+R；
3)计算卡尔曼增益K＝PHTS-1；
4)计算状态修正量
5)计算状态协方差矩阵的递推结果P←(Id-KH)P(Id-KH)T+KRKT。
6)将状态更新量与原本的状态向量/>叠加后得到更新后的状态向量。
在本实施例的一个优选方案中，还包括了对双视觉辅助单元的第一或第二视觉位姿数据进行筛选的方法，以第二视觉位姿数据的筛选为例，具体的筛选过程如下：
双IMU差分通过积分过程以较高采样频率获取变换矩阵Timu，同时第二视觉位姿数据以较低速率获取变换矩阵Tcam。记连续几帧第二视觉位姿数据更新得到的变换矩阵与/>(依次递推)，选取差分后IMU历史测姿信息中与第二视觉位姿数据获取时刻最接近的几帧惯性测姿变换矩阵/>与/>(依次递推)。则计算得到第二视觉位姿数据得到的相邻两帧相对位姿变换矩阵/>相邻两帧最近时刻的惯性测量相对姿态变换矩阵/>
为求取相邻帧时间内视觉测量与惯性测量的相对偏差，定义在李代数/>上的对数映射的二范数Dk
计算k时刻前一定时间内，n组有效相对变换Dk均方根偏差基准RMSDk。
判定，若新一帧视觉测量与惯性测量偏差Dk∈[-2RMSDk,2RMSDk]，则认为第二视觉位姿数据可用于对滤波器状态进行更新，同时该Dk计入均方根偏差基准的更新。否则认为是第二视觉位姿数据无效，丢弃不用，该Dk也不计入均方根偏差基准。通过对视觉位姿数据，可以提高测量精度。
综上所述，本实施例的结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，通过惯性差分获取头盔相对于座舱中的相对位姿，通过北斗导航接收机对座舱IMU进行零偏修正，再通过分别布置于头盔、座舱的两组相机模组，获取头盔与运动载具的相对位姿关系，对进行惯性积分差分的位姿测量结果进行修正，得到准确的位姿测量结果。这种方法充分利用惯性测姿采样频率高、短时精度高，视觉测姿无累积误差的特性，通过引入卫导观测抑制了座舱IMU漂移，通过运动载具上设置的一组相机实现对头盔的大范围位姿追踪，通过头盔上设置的一组相机实现高精度的位姿追踪。该方法相较于传统相对位姿测量方案，更为有效调用各种导航方式特性，且布置简便，适用于乘用车辆、战斗机、武装直升机中头戴显示系统等工程应用。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
Claims (8)
1.一种结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统，其特征在于，包括：北斗导航单元、座舱IMU、头盔IMU、双视觉辅助单元和测量单元；
所述座舱IMU与运动载具固连，头盔IMU与载具内人员头盔固连，分别用于获取运动载具和人员头部的运动信息；
所述双视觉辅助单元布置于运动载具内，用于通过双视觉方式获取头盔相对于座舱的视觉位姿数据；
所述北斗导航单元布置于运动载具上，利用北斗数据获得运动载具的导航结果；
所述测量单元，用于根据导航结果对座舱IMU零偏进行修正；通过座舱和头盔的双IMU差分获得的载体坐标系中头盔相对座舱的惯性位姿数据；以视觉位姿数据为外部观测量对惯性位姿数据进行卡尔曼滤波，输出滤波后相对位姿数据；
所述测量单元包括座舱IMU零偏校正模块、双IMU差分模块和位姿滤波模块；
所述座舱IMU零偏校正模块与所述北斗导航单元和座舱IMU连接，用于根据导航结果对座舱IMU零偏进行修正；
所述双IMU差分模块与座舱IMU和头盔IMU连接；接收座舱IMU和头盔IMU的惯性测量数据进行双IMU差分运算得到的包括载体坐标系下头盔相对座舱的位置、速度和旋转四元数在内的惯性位姿数据；
所述位姿滤波模块与座舱IMU零偏校正模块、双IMU差分模块和双视觉辅助单元分别连接，以双视觉辅助单元输出的视觉位姿数据为外部观测量对包括惯性位姿数据和修正后座舱IMU零偏在内的状态向量进行卡尔曼滤波，输出滤波后相对位姿数据；
双视觉辅助单元包括第一视觉模块和第二视觉模块；
所述第一视觉模块包括第一相机模组和第一标志点；
所述第一相机模组安装于座舱内正对头盔佩戴人员供其使用的操纵台上，并与座舱IMU固连；与第一相机模组配合的第一标志点布置于头盔上；第一相机模组通过测量第一标志点获得头盔相对座舱的第一视觉位姿数据；
所述第二视觉模块包括第二相机模组和第二标志点；
所述第二相机模组安装于头盔上，并与头盔IMU固连；与第二相机模组配合的第二标志点布置于正对头盔佩戴人员供其使用的操纵台上；第二相机模组通过测量第二标志点后，进行坐标系转换获得头盔相对座舱的第二视觉位姿数据。
2.根据权利要求1所述的双视觉辅助惯性差分座舱内头部测姿系统，其特征在于，所述北斗导航单元为载波相位差分的北斗导航系统；其北斗卫星接收天线布置于运动载具外，接收卫星信号；处理器布置于座舱内，用于测量出运动载具在地理坐标系中的导航结果输出到所述座舱IMU零偏校正模块。
3.根据权利要求2所述的双视觉辅助惯性差分座舱内头部测姿系统，其特征在于，
所述座舱IMU零偏校正模块内的零偏进行修正的方式包括：
方式一、构建运动载具在地理坐标系中的运动状态方程，建立座舱IMU的卡尔曼滤波器，利用北斗导航单元测量的数据生成运动载具在地理坐标系中的观测量，对载具在地理坐标系中的运动状态方程中包括的角速度与加速度的零偏进行更新，得到修正后的座舱IMU的角速度与加速度的零偏；
方式二、构建运动载具和北斗导航单元深组合状态方程，建立组合后的卡尔曼滤波器，将座舱IMU介入到北斗导航的卫星捕获环路中，滤波后获得修正后的座舱IMU的角速度与加速度的零偏。
4.根据权利要求1所述的双视觉辅助惯性差分座舱内头部测姿系统，其特征在于，
所述第二视觉位姿数据有输出时，采用第二视觉位姿数据为卡尔曼滤波的外部观测量；当所述第二视觉位姿数据无输出时，采用第一视觉位姿数据为卡尔曼滤波的外部观测量。
5.根据权利要求1所述的双视觉辅助惯性差分座舱内头部测姿系统，其特征在于，
所述第一标志点在头盔上的布局，满足在头盔设定的运动范围内，使所述第一标志点处于第一相机模组的视觉范围内；并且，所述第一标志点的数量大于等于4；
所述第二标志点在操纵台上的布局，保证在固定角度内，使所述第二标志点处于第二相机模组的视觉范围内；所述第二标志点的数量大于等于4。
6.根据权利要求5所述的双视觉辅助惯性差分座舱内头部测姿系统，其特征在于，
所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式；所述第一相机模组和第二相机模组采用单目、双目或结构光方式。
7.根据权利要求1所述的双视觉辅助惯性差分座舱内头部测姿系统，其特征在于，所述双IMU差分模块中计算的惯性位姿数据中的旋转四元数采用角速度差分法进行求解；
在角速度差分法中，通过对当前时刻头盔与运动载具的相对角速度进行积分得到头盔与运动载具的旋转四元数。
8.根据权利要求7所述的双视觉辅助惯性差分座舱内头部测姿系统，其特征在于，
所述角速度差分法，包括：
获取头盔坐标系到运动载具坐标系的相对旋转矩阵；
根据头盔与运动载具的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；
使用当前时刻相对角增量计算相对旋转四元数得到头盔相对于运动载具的位姿数据。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210010936.7A CN114199239B (zh) | 2022-01-05 | 2022-01-05 | 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210010936.7A CN114199239B (zh) | 2022-01-05 | 2022-01-05 | 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114199239A CN114199239A (zh) | 2022-03-18 |
| CN114199239B true CN114199239B (zh) | 2024-04-16 |
Family
ID=80658159
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210010936.7A Active CN114199239B (zh) | 2022-01-05 | 2022-01-05 | 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114199239B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115981465B (zh) * | 2022-12-22 | 2024-01-19 | 广州阿路比电子科技有限公司 | 移动环境下ar头盔运动捕捉方法和系统 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5072218A (en) * | 1988-02-24 | 1991-12-10 | Spero Robert E | Contact-analog headup display method and apparatus |
| JP2007232443A (ja) * | 2006-02-28 | 2007-09-13 | Yokogawa Electric Corp | 慣性航法装置およびその誤差補正方法 |
| CN104280022A (zh) * | 2013-07-13 | 2015-01-14 | 哈尔滨点石仿真科技有限公司 | 视觉辅助惯性测量单元的数字头盔显示设备跟踪系统 |
| CN105698765A (zh) * | 2016-02-22 | 2016-06-22 | 天津大学 | 双imu单目视觉组合测量非惯性系下目标物位姿方法 |
| CN106595640A (zh) * | 2016-12-27 | 2017-04-26 | 天津大学 | 基于双imu和视觉融合的动基座上物体相对姿态测量方法及系统 |
| CN106950586A (zh) * | 2017-01-22 | 2017-07-14 | 无锡卡尔曼导航技术有限公司 | 用于农机作业的gnss/ins/车辆组合导航方法 |
| CN207095572U (zh) * | 2017-08-17 | 2018-03-13 | 苏州中德睿博智能科技有限公司 | 一种用于飞行系统中头盔姿态测量的硬件平台 |
| WO2018077176A1 (zh) * | 2016-10-26 | 2018-05-03 | 北京小鸟看看科技有限公司 | 可穿戴设备及在可穿戴设备中确定用户位移的方法 |
| CN108917746A (zh) * | 2018-07-26 | 2018-11-30 | 中国人民解放军国防科技大学 | 头盔姿态测量方法、测量装置及测量系统 |
| CN110736457A (zh) * | 2019-11-12 | 2020-01-31 | 苏州工业职业技术学院 | 一种基于北斗、gps和sins的组合导航方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10216265B1 (en) * | 2017-08-07 | 2019-02-26 | Rockwell Collins, Inc. | System and method for hybrid optical/inertial headtracking via numerically stable Kalman filter |
-
2022
- 2022-01-05 CN CN202210010936.7A patent/CN114199239B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5072218A (en) * | 1988-02-24 | 1991-12-10 | Spero Robert E | Contact-analog headup display method and apparatus |
| JP2007232443A (ja) * | 2006-02-28 | 2007-09-13 | Yokogawa Electric Corp | 慣性航法装置およびその誤差補正方法 |
| CN104280022A (zh) * | 2013-07-13 | 2015-01-14 | 哈尔滨点石仿真科技有限公司 | 视觉辅助惯性测量单元的数字头盔显示设备跟踪系统 |
| CN105698765A (zh) * | 2016-02-22 | 2016-06-22 | 天津大学 | 双imu单目视觉组合测量非惯性系下目标物位姿方法 |
| WO2018077176A1 (zh) * | 2016-10-26 | 2018-05-03 | 北京小鸟看看科技有限公司 | 可穿戴设备及在可穿戴设备中确定用户位移的方法 |
| CN106595640A (zh) * | 2016-12-27 | 2017-04-26 | 天津大学 | 基于双imu和视觉融合的动基座上物体相对姿态测量方法及系统 |
| CN106950586A (zh) * | 2017-01-22 | 2017-07-14 | 无锡卡尔曼导航技术有限公司 | 用于农机作业的gnss/ins/车辆组合导航方法 |
| CN207095572U (zh) * | 2017-08-17 | 2018-03-13 | 苏州中德睿博智能科技有限公司 | 一种用于飞行系统中头盔姿态测量的硬件平台 |
| CN108917746A (zh) * | 2018-07-26 | 2018-11-30 | 中国人民解放军国防科技大学 | 头盔姿态测量方法、测量装置及测量系统 |
| CN110736457A (zh) * | 2019-11-12 | 2020-01-31 | 苏州工业职业技术学院 | 一种基于北斗、gps和sins的组合导航方法 |
Non-Patent Citations (4)
| Title |
|---|
| 双IMU与视觉组合姿态测量信息融合算法研究;黄璐;《中国优秀硕士学位论文全文数据库 (工程科技Ⅱ辑)》;C031-170 * |
| 基于微惯性技术的人体头部运动姿态测量;张天等;《导航与控制》;第20卷(第4期);96-100 * |
| 基于数据融合的座舱头部姿态跟踪方法研究;刘延新;刘世良;丁全心;张劲锋;;电光与控制(第08期);69-73 * |
| 运动平台双IMU与视觉组合姿态测量算法;孙长库;黄璐;王鹏;郭肖亭;;传感技术学报(第09期);69-76 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114199239A (zh) | 2022-03-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US7791529B2 (en) | System for estimating the speed of an aircraft, and an application thereof to detecting obstacles | |
| CA2923988C (en) | Navigation system with rapid gnss and inertial initialization | |
| US8666661B2 (en) | Video navigation | |
| US10322819B2 (en) | Autonomous system for taking moving images from a drone, with target tracking and improved target location | |
| CN108917746B (zh) | 头盔姿态测量方法、测量装置及测量系统 | |
| CN111426320A (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| CN109520476A (zh) | 基于惯性测量单元的后方交会动态位姿测量系统及方法 | |
| CN114199239B (zh) | 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统 | |
| CN112484722B (zh) | 结合惯性导航系统的视觉传感器全局定位方法 | |
| CN102636081A (zh) | 一种基于视觉运动建模的传递对准方法及装置 | |
| CN109143303B (zh) | 飞行定位方法、装置及固定翼无人机 | |
| CN114419109B (zh) | 一种基于视觉和气压信息融合的飞行器定位方法 | |
| CN105424034B (zh) | 一种船载全天时星光惯导组合导航系统 | |
| KR20030056284A (ko) | 차량을 이용한 도로시설물 정보 추출 시스템 및 그 방법 | |
| CN114383612B (zh) | 一种视觉辅助惯性差分位姿测量系统 | |
| CA3064640A1 (en) | Navigation augmentation system and method | |
| CN114812554A (zh) | 基于滤波的多源融合机器人室内绝对定位方法 | |
| CN112213753B (zh) | 一种北斗导航定位功能与增强现实技术相结合规划跳伞训练路径的方法 | |
| CN113821052A (zh) | 集群无人机协同目标定位方法、系统、协同目标定位终端 | |
| US20190094538A1 (en) | Display System, Related Display Method and Computer Program | |
| CN115981465B (zh) | 移动环境下ar头盔运动捕捉方法和系统 | |
| CN117146810B (zh) | 基于卫星导航与mems惯导的组合定位系统 | |
| CN102818570A (zh) | 利用sins/图像匹配组合导航进行火星捕获的方法 | |
| CN117128957B (zh) | 一种多源信息融合的无人机火源定位方法及系统 | |
| Lee et al. | Vertical position estimation of a constrained link using IMU signals and kinematic constraint |
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 |