CN114383612B - 一种视觉辅助惯性差分位姿测量系统 - Google Patents
一种视觉辅助惯性差分位姿测量系统 Download PDFInfo
- Publication number
- CN114383612B CN114383612B CN202210035732.9A CN202210035732A CN114383612B CN 114383612 B CN114383612 B CN 114383612B CN 202210035732 A CN202210035732 A CN 202210035732A CN 114383612 B CN114383612 B CN 114383612B
- Authority
- CN
- China
- Prior art keywords
- carrier
- measured object
- imu
- vision
- relative
- 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
- 238000005259 measurement Methods 0.000 title claims abstract description 68
- 230000033001 locomotion Effects 0.000 claims abstract description 85
- 230000000007 visual effect Effects 0.000 claims abstract description 60
- 238000001914 filtration Methods 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000000034 method Methods 0.000 claims description 32
- 230000001133 acceleration Effects 0.000 claims description 28
- 239000013598 vector Substances 0.000 claims description 18
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000001360 synchronised effect Effects 0.000 claims description 3
- 238000005070 sampling Methods 0.000 abstract description 6
- 238000001514 detection method Methods 0.000 abstract description 5
- 230000008569 process Effects 0.000 description 7
- 230000009466 transformation Effects 0.000 description 7
- 230000008901 benefit Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 239000003550 marker Substances 0.000 description 3
- 230000007704 transition Effects 0.000 description 3
- 238000012937 correction Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 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
- 238000010586 diagram Methods 0.000 description 1
- 230000009977 dual effect Effects 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
- 238000012545 processing Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method 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
- 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
-
- 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/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- 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/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
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差分获得的载体坐标系下被测物体相对载体的惯性位姿数据;接收视觉位姿数据;并以视觉位姿数据为外部观测量对包括惯性位姿数据在内的状态数据进行卡尔曼滤波,输出滤波后的被测物体与运动载具的相对位姿数据。本发明充分利用惯性测姿采样频率高、短时精度高,视觉测姿无累积误差的特性,实现高精度的位姿追踪。
Description
技术领域
本发明涉及位姿测量和卡尔曼滤波技术领域,尤其涉及一种视觉辅助惯性差分位姿测量系统。
背景技术
随着现代装备的智能化升级,包括被测物体在运动载具中位姿信息获取提出了新的需求。例如头戴显示功能对运动载具中乘员姿态位姿信息。这类装备需要获取运动载具中操控人员与运动载具实时准确的相对姿态变化,将乘员头部动作与装备操控结合,实现人机一体的自然交互方式,从而实现装备快速协同、信息互联等功能,提升战场效率。相对位姿测量相比于惯性系下绝对姿态测量存在的主要差别为:其一,在载具相对惯性系存在未知运动的条件下,被测目标的单一惯性信息也不可作为直接参考,需要同载具的惯性信息差分处理,通过转换到载具坐标系下进行相对姿态解算;其二,卫星导航与惯性导航这种常用的组合导航手段不再可用,需要使用卫星导航、视觉导航、雷达导航等多种导航方式分别对运动载具上与被测物体上两个IMU的零偏分别进行修正。
在目前常用的相对位姿测量手段中,惯性与视觉组合是一种可靠的相对姿态测量方式。惯性测姿的短时精度较高,长时积分会产生累积误差,而视觉测姿虽然易受外部近似特征干扰及光线变化影响,但特征准确时精度稳定,在测姿精度上与惯性测量形成互补。从测量频率上,惯性信息可实现高达1kHz采样的实时测量,图像信息通常为60Hz或120Hz,且存在稳定解算延迟,惯性与视觉的融合可实现与纯惯性相同的高频率输出。惯性与视觉的组合可充分利用惯性测量采样周期短、短时精度高的特性,并发挥视觉测量可直接获得无累积误差相对姿态的特点。
现有的视觉观测与惯性组合的形式,缺少足够的观测信息,且无法同时抑制两个惯性器件漂移,导致导航系统积分发散。
发明内容
鉴于上述的分析,本发明旨在提供一种视觉辅助惯性差分位姿测量系统,提升相对位姿测量结果的准确性。
本发明提供的技术方案是:
本发明公开了一种视觉辅助惯性差分位姿测量系统,包括:测量单元,以及与测量单元连接的运动载具IMU、被测物体IMU和视觉单元;
运动载具承载被测物体,所述运动载具IMU和被测物体IMU分别布置于运动载具和被测物体上,用于获取运动载具和被测物体的运动信息;
所述视觉单元布置于运动载具内,用于获取被测物体相对于运动载具的视觉位姿数据;
所述测量单元,接收所述运动载具和被测物体的运动信息,进行双IMU差分获得的载体坐标系下被测物体相对运动载体的惯性位姿数据;接收所述视觉位姿数据;并以视觉位姿数据为外部观测量对包括惯性位姿数据在内的状态数据进行卡尔曼滤波,输出滤波后的被测物体与运动载具的相对位姿数据。
进一步地,所述视觉单元包括第一视觉模块和/或第二视觉模块;
所述第一视觉模块的第一相机模组安装于运动载具上,与第一相机配合的第一标志点布置于被测物体内;第一相机模组测量被测物体上布置的第一标志点,获得被测物体相对运动载具的第一视觉位姿数据;
所述第二视觉模块的第二相机模组安装于被测物体上,与第二相机模组配合的第二标志点布置于运动载具上;第二相机模组测量运动载具上布置的第二标志点,通过坐标系转换获得被测物体相对运动载具的第二视觉位姿数据。
进一步地,所述运动载具IMU与第一相机模组固连,所述被测物体IMU与第二相机模组固连。
进一步地,所述第一标志点在被测物体上进行布局,保证在被测物体在其运动范围内,使所述第一标志点处于第一相机模组的视觉范围内;所述第一标志点的数量大于等于4;
所述第二标志点在运动载具内的布局,保证在固定角度内,使所述第二标志点处于第二相机模组的视觉范围内;所述第二标志点的数量大于等于4。
进一步地,所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式;所述第一相机模组和第二相机模组采用单目、双目或结构光的方式。
进一步地,所述第一标志点和第二标志点采用同频闪烁的方式与对应的相机模组同步。
进一步地,所述卡尔曼滤波中的状态向量:
X=[ph-p,vh-p,qh-p,bha,bpa,bhω,bpω]T;
其中,ph-p、vh-p、qh-p定义为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数;bhω、bha为被测物体IMU测量角速度与加速度的零偏;bpω、bpa为运动载具IMU测量角速度与加速度的零偏。
进一步地,所述运动载具IMU测量角速度与加速度的零偏为经过修正过的零偏数据。
进一步地,所述旋转四元数qh-p采用角速度差分法进行求解;
在角速度差分法中,通过对当前时刻被测物体与运动载具的相对角速度进行积分得到被测物体与运动载具的相对位姿。
进一步地,所述角速度差分法,包括:
获取被测物体坐标系到运动载体坐标系的相对旋转矩阵;
根据被测物体与运动载体的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量;
使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载体的位姿数据。
本发明的可以至少实现以下有益效果之一:
本发明提出的视觉辅助惯性差分位姿测量系统,通过视觉辅助获取被测物体与运动载具的相对位姿关系,对进行惯性积分差分的位姿测量结果进行修正,得到准确的位姿测量结果。这种方法充分利用惯性测姿采样频率高、短时精度高,视觉测姿无累积误差的特性,实现高精度的位姿追踪。该方法相较于传统相对位姿测量方案,更为有效调用各种导航方式特性,且布置简便,适用于乘用车辆、战斗机、武装直升机中头戴显示系统等工程应用。
附图说明
附图仅用于示出具体实施例的目的,而并不认为是对本发明的限制,在整个附图中,相同的参考符号表示相同的部件。
图1为本发明实施例中的觉辅助惯性差分位姿测量系统组成框图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例,其中,附图构成本申请一部分,并与本发明的实施例一起用于阐释本发明的原理。
本实施例所公开的一种视觉辅助惯性差分位姿测量系统,如图1所示,包括:测量单元,与测量单元连接的运动载具IMU、被测物体IMU和视觉单元;
所述运动载具IMU和被测物体IMU分别布置于运动载具和运动载具内的被测物体上,用于获取运动载具和被测物体的运动信息;
所述视觉单元布置于运动载具内,用于获取被测物体相对于运动载具的视觉位姿数据;
所述测量单元,接收运动载具和被测物体的运动信息,进行双IMU差分获得的载体坐标系下被测物体相对载体的惯性位姿数据;接收所述视觉位姿数据;并以视觉位姿数据为外部观测量对包括惯性位姿数据在内的状态数据进行卡尔曼滤波,输出滤波后的被测物体与运动载具的相对位姿数据。
具体的,所述视觉单元包括第一视觉模块和/或第二视觉模块;
所述第一视觉模块的第一相机模组安装于运动载具上,与第一相机配合的第一标志点布置于被测物体内;第一相机模组测量被测物体上布置的第一标志点,获得被测物体相对运动载具的第一视觉位姿数据;
所述第一标志点在被测物体上的布局,保证在被测物体在较大范围运动的情况下,仍使所述第一标志点处于第一相机模组的视觉范围内,所述第一标志点的数量大于等于4;
所述第一视觉模块的优势在于,被测物体带动标志点靶标运动时,靶标始终不会超出相机视野,视觉测量系统能够全时段工作。
所述第二视觉模块的第二相机模组安装于被测物体上,与第二相机模组配合的第二标志点布置于运动载具上;第二相机模组测量运动载具上布置的第二标志点,通过坐标系转换获得被测物体相对运动载具的第二视觉位姿数据。
所述第二标志点在运动载具内的布局,保证在固定角度内,使所述第二标志点处于第二相机模组的视觉范围内,所述第二标志点的数量大于等于4。
所述第二视觉模块的优势在于,标志点靶标可以在相机视野内占有更大面积,因此视觉相对测量精度也更高,同时这种布置方式还带来了另外一种优势,即多套测量系统的多个相机可共享同一套标志点靶标,保证各套系统工作互不干扰。但缺点在于,在被测物体运动过程中,一旦标志点靶标超出相机视野,视觉测量将无法工作。
优选的,所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式;
所述第一相机模组和第二相机模组采用单目、双目或结构光方式。只要能准确捕获红外标志点的图像信息即可。
更优选的,所述第一标志点和第二标志点采用同频闪烁的方式与对应的相机模组同步。通过图像处理提取红外点位置,避免外部自然光红外波段的光线干扰。
本实施例中的优选方案中采用同时包括第一视觉模块和第二视觉模块的视觉单元,综合了以上两者,既使用了布置于运动载具上的相机模组也使用了布置于被测物体上的相机模组,保证了测量精度的同时保证了较大的测量范围。
由于第一视觉模块和第二视觉模块测量的视觉位姿数据是用来对双IMU差分获得的载体坐标系下被测物体相对载体的惯性位姿数据进行修正的,因此为了减少标定工作量,在进行布置时,所述运动载具IMU与第一相机模组固连,所述被测物体IMU与第二相机模组固连。
具体的,所述测量单元中的卡尔曼滤波的状态向量
X=[ph-p,vh-p,qh-p,bha,bpa,bhω,bpω]T;
其中,ph-p、vh-p、qh-p定义为双IMU差分获得的载体坐标系下被测物体相对运动载体的位置、速度、旋转四元数;bhω、bha为被测物体IMU测量角速度与加速度的零偏;bpω、bpa为运动载具IMU测量角速度与加速度的零偏。
其中,载体坐标系下被测物体相对载体的位置ph-p、速度vh-p可采用现有的技术方法获得,在此就不赘述。
优选的,所述旋转四元数qh-p采用角速度差分法进行求解;
在角速度差分法中,通过对当前时刻被测物体与运动载具的相对角速度进行积分得到被测物体与运动载具的相对位姿。
进一步地,所述角速度差分法,包括:
1)获取被测物体坐标系到运动载具坐标系的相对旋转矩阵Rrel;
被测物体坐标系到运动载具坐标系的相对旋转矩阵Rrel可通过对被测物体和运动载具的标定获得。
2)根据被测物体与运动载具的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量;
根据被测物体与运动载具的IMU获取角速度信息ωh(t)、ωp(t)与tk-1时刻相对旋转矩阵计算当前时刻t的相对角增量为:
3)使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载具的位姿数据。
用当前时刻相对角增量计算相对旋转四元数/>为
相对旋转四元数即为旋转四元数qh-p。
更优选的方案中,为了提高测量的精度,状态向量中的运动载具IMU的角速度与加速度的零偏作为bpω、bpa可以采用经过修正过的零偏数据。
具体的修正可采用卫星导航系统或里程计以及高度计等测量系统。
优选的,通过卫星导航对运动载具IMU的角速度与加速度的零偏进行修正的方案中,
系统还包括卫星导航单元;该卫星导航单元为卫星导航载波相位差分系统;
其天线布置于运动载具外接收卫星信号;处理器布置于运动载具内,进行地理坐标系中的运动载具运动数据测量。
更优选的,根据所述运动载具的运动数据对运动载具IMU测量角速度与加速度的零偏进行修正过程包括两种方式:
方式一、构建运动载具在地理坐标系运动的状态方程,建立卡尔曼滤波器,利用卫星导航单元测量的数据生成运动载具在地理坐标系中的观测量,对载具在地理坐标系运动状态方程的零偏进行更新,得到运动载具IMU的零偏;
方式二、构建运动载具和卫星导航单元深组合状态方程,建立卡尔曼滤波器,将运动载具IMU介入到卫星导航的卫星捕获环路,通过滤波器得到运动载具IMU的零偏。可采用现有的深组合状态方程进行滤波处理,在本实施例中并不需要做出具体的限定。
在测量单元中,忽略杆臂效应,卡尔曼滤波的状态方程可以用下式表示:
式中,为相对旋转矩阵;ah、bha和bha为被测物体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也不计入均方根偏差基准。
将视觉单元的视觉位姿数据按照上述更新过程接入卡尔曼滤波,修正惯性测姿误差,提高测量精度。
综上所述,本实施例的视觉辅助惯性差分位姿测量系统,通过视觉辅助获取被测物体与运动载具的相对位姿关系,对进行惯性积分差分的位姿测量结果进行修正,得到准确的位姿测量结果。这种方法充分利用惯性测姿采样频率高、短时精度高,视觉测姿无累积误差的特性,实现高精度的位姿追踪。该方法相较于传统相对位姿测量方案,更为有效调用各种导航方式特性,且布置简便。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。
Claims (7)
1.一种视觉辅助惯性差分位姿测量系统,其特征在于,包括:测量单元,以及与测量单元连接的运动载具IMU、被测物体IMU和视觉单元;
运动载具承载被测物体,所述运动载具IMU和被测物体IMU分别布置于运动载具和被测物体上,用于获取运动载具和被测物体的运动信息;
所述视觉单元布置于运动载具内,用于获取被测物体相对于运动载具的视觉位姿数据;
所述测量单元,接收所述运动载具和被测物体的运动信息,进行双IMU差分获得的载体坐标系下被测物体相对运动载体的惯性位姿数据;接收所述视觉位姿数据;并以视觉位姿数据为外部观测量对包括惯性位姿数据在内的状态数据进行卡尔曼滤波,输出滤波后的被测物体与运动载具的相对位姿数据;
所述卡尔曼滤波中的状态向量:
X=[ph-p,vh-p,qh-p,bha,bpa,bhω,bpω]T;
其中,ph-p、vh-p、qh-p定义为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数;bhω、bha为被测物体IMU测量角速度与加速度的零偏;bpω、bpa为运动载具IMU测量角速度与加速度的零偏;
所述旋转四元数qh-p采用角速度差分法进行求解;
在角速度差分法中,通过对当前时刻被测物体与运动载具的相对角速度进行积分得到被测物体与运动载具的相对位姿;
所述角速度差分法,包括:
获取被测物体坐标系到运动载体坐标系的相对旋转矩阵;
根据被测物体与运动载体的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量;
使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载体的位姿数据。
2.根据权利要求1所述的视觉辅助惯性差分位姿测量系统,其特征在于,所述视觉单元包括第一视觉模块和/或第二视觉模块;
所述第一视觉模块的第一相机模组安装于运动载具上,与第一相机配合的第一标志点布置于被测物体内;第一相机模组测量被测物体上布置的第一标志点,获得被测物体相对运动载具的第一视觉位姿数据;
所述第二视觉模块的第二相机模组安装于被测物体上,与第二相机模组配合的第二标志点布置于运动载具上;第二相机模组测量运动载具上布置的第二标志点,通过坐标系转换获得被测物体相对运动载具的第二视觉位姿数据。
3.根据权利要求2所述的视觉辅助惯性差分位姿测量系统,其特征在于,
所述运动载具IMU与第一相机模组固连,所述被测物体IMU与第二相机模组固连。
4.根据权利要求2所述的视觉辅助惯性差分位姿测量系统,其特征在于,
所述第一标志点在被测物体上进行布局,保证在被测物体在其运动范围内,使所述第一标志点处于第一相机模组的视觉范围内;所述第一标志点的数量大于等于4;
所述第二标志点在运动载具内的布局,保证在固定角度内,使所述第二标志点处于第二相机模组的视觉范围内;所述第二标志点的数量大于等于4。
5.根据权利要求4所述的视觉辅助惯性差分位姿测量系统,其特征在于,
所述第一标志点和第二标志点使用红外发光点的方式或红外发光二维码的方式;所述第一相机模组和第二相机模组采用单目、双目或结构光的方式。
6.根据权利要求5所述的视觉辅助惯性差分位姿测量系统,其特征在于,
所述第一标志点和第二标志点采用同频闪烁的方式与对应的相机模组同步。
7.根据权利要求1所述的视觉辅助惯性差分位姿测量系统,其特征在于,所述运动载具IMU测量角速度与加速度的零偏为经过修正过的零偏数据。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210035732.9A CN114383612B (zh) | 2022-01-05 | 2022-01-05 | 一种视觉辅助惯性差分位姿测量系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210035732.9A CN114383612B (zh) | 2022-01-05 | 2022-01-05 | 一种视觉辅助惯性差分位姿测量系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114383612A CN114383612A (zh) | 2022-04-22 |
CN114383612B true CN114383612B (zh) | 2024-04-12 |
Family
ID=81202654
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210035732.9A Active CN114383612B (zh) | 2022-01-05 | 2022-01-05 | 一种视觉辅助惯性差分位姿测量系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114383612B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115981465B (zh) * | 2022-12-22 | 2024-01-19 | 广州阿路比电子科技有限公司 | 移动环境下ar头盔运动捕捉方法和系统 |
CN118274822A (zh) * | 2022-12-30 | 2024-07-02 | 优奈柯恩(北京)科技有限公司 | 用于修正用户设备与目标设备的相对姿态的方法及装置 |
CN118279385A (zh) * | 2022-12-30 | 2024-07-02 | 优奈柯恩(北京)科技有限公司 | 用于确定相对位姿的方法、装置、系统、设备和介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102506857A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法 |
CN105698765A (zh) * | 2016-02-22 | 2016-06-22 | 天津大学 | 双imu单目视觉组合测量非惯性系下目标物位姿方法 |
CN106595640A (zh) * | 2016-12-27 | 2017-04-26 | 天津大学 | 基于双imu和视觉融合的动基座上物体相对姿态测量方法及系统 |
CN107782309A (zh) * | 2017-09-21 | 2018-03-09 | 天津大学 | 非惯性系视觉和双陀螺仪多速率ckf融合姿态测量方法 |
CN111649737A (zh) * | 2020-05-08 | 2020-09-11 | 中国航空工业集团公司西安航空计算技术研究所 | 一种面向飞机精密进近着陆的视觉-惯性组合导航方法 |
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 CN202210035732.9A patent/CN114383612B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102506857A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法 |
CN105698765A (zh) * | 2016-02-22 | 2016-06-22 | 天津大学 | 双imu单目视觉组合测量非惯性系下目标物位姿方法 |
CN106595640A (zh) * | 2016-12-27 | 2017-04-26 | 天津大学 | 基于双imu和视觉融合的动基座上物体相对姿态测量方法及系统 |
CN107782309A (zh) * | 2017-09-21 | 2018-03-09 | 天津大学 | 非惯性系视觉和双陀螺仪多速率ckf融合姿态测量方法 |
CN111649737A (zh) * | 2020-05-08 | 2020-09-11 | 中国航空工业集团公司西安航空计算技术研究所 | 一种面向飞机精密进近着陆的视觉-惯性组合导航方法 |
Non-Patent Citations (4)
Title |
---|
Head tracking relative to a moving vehicle or simulator platform using differential inertial sensors;Eric Foxlin;《HELMET- AND HEAD-MOUNTED DISPLAYS V》;第4021卷;133-144 * |
Vision and dual IMU integrated attitude measurement system;Guo, Xiaoting et al.;《》;第10621卷;1-10 * |
双IMU与视觉组合姿态测量信息融合算法研究;黄璐;《中国优秀硕士学位论文全文数据库 (工程科技Ⅱ辑)》;C031-170 * |
运动平台双IMU与视觉组合姿态测量算法;孙长库;黄璐;王鹏;郭肖亭;;传感技术学报(第09期);69-76 * |
Also Published As
Publication number | Publication date |
---|---|
CN114383612A (zh) | 2022-04-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114383612B (zh) | 一种视觉辅助惯性差分位姿测量系统 | |
CN109376785B (zh) | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 | |
CN109540126B (zh) | 一种基于光流法的惯性视觉组合导航方法 | |
CN114199239B (zh) | 结合北斗导航的双视觉辅助惯性差分座舱内头部测姿系统 | |
CN106017463B (zh) | 一种基于定位传感装置的飞行器定位方法 | |
EP2434256B1 (en) | Camera and inertial measurement unit integration with navigation data feedback for feature tracking | |
CN108917746B (zh) | 头盔姿态测量方法、测量装置及测量系统 | |
CN107289910B (zh) | 一种基于tof的光流定位系统 | |
CN106814753B (zh) | 一种目标位置矫正方法、装置及系统 | |
KR20130127991A (ko) | 이동체의 경로를 추정하는 방법 및 시스템 | |
CN109520476B (zh) | 基于惯性测量单元的后方交会动态位姿测量系统及方法 | |
CN105352509A (zh) | 地理信息时空约束下的无人机运动目标跟踪与定位方法 | |
CN112050806B (zh) | 一种移动车辆的定位方法及装置 | |
CN110044377B (zh) | 一种基于Vicon的IMU离线标定方法 | |
CN112591148A (zh) | 利用在轨恒星感光评估姿态稳定度的方法及系统 | |
CA2720437C (en) | Systems and methods for determining heading | |
CN112862818B (zh) | 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法 | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN110986997A (zh) | 一种提高室内惯性导航精度的方法及系统 | |
CN108444468B (zh) | 一种融合下视视觉与惯导信息的定向罗盘 | |
CN114419109B (zh) | 一种基于视觉和气压信息融合的飞行器定位方法 | |
CN114690229A (zh) | 一种融合gps的移动机器人视觉惯性导航方法 | |
CN112154480B (zh) | 可移动平台的定位方法、装置、可移动平台及存储介质 | |
CN116518959A (zh) | 基于uwb与3d视觉结合的空间摄像机的定位方法和装置 | |
CN114323011B (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 |