CN108871337B - 基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法 - Google Patents

基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法 Download PDF

Info

Publication number
CN108871337B
CN108871337B CN201810642823.2A CN201810642823A CN108871337B CN 108871337 B CN108871337 B CN 108871337B CN 201810642823 A CN201810642823 A CN 201810642823A CN 108871337 B CN108871337 B CN 108871337B
Authority
CN
China
Prior art keywords
coordinate system
local
state
subsystem
fusion
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
CN201810642823.2A
Other languages
English (en)
Other versions
CN108871337A (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.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
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 Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN201810642823.2A priority Critical patent/CN108871337B/zh
Publication of CN108871337A publication Critical patent/CN108871337A/zh
Application granted granted Critical
Publication of CN108871337B publication Critical patent/CN108871337B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Image Analysis (AREA)

Abstract

一种基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法,判断所有视觉传感器局部子系统中是否出现严重遮挡情况,若出现则放弃对该局部子系统进行计算,并等待剩余子系统融合结束,反之,则判断是否出现部分遮挡情况,若没有,利用无迹卡尔曼滤波器求出局部子系统的状态信息,进而筛选出基础状态量;若出现遮挡,利用得到的基础状态量和先验知识估计出现部分遮挡的子系统中缺失的特征点像素坐标,然后再利用无迹卡尔曼滤波器得到相应局部子系统的状态信息;利用分布式融合方法得到最终的融合结果,最后将出现严重遮挡局部子系统初始化。本发明能够在出现遮挡的情况下有效地跟踪运动刚体的位姿,具有很强的鲁棒性和有效性。

Description

基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估 计方法
技术领域
本发明主要针对视觉跟踪领域,尤其是一种风洞试验、飞行器试飞等高噪声、高振动、眩光等环境中目标遮挡情况下的位姿估计方法。
背景技术
基于视觉的目标位姿(位置和姿态,简称位姿)估计是视觉跟踪中重要的技术之一,姿态参数是描述飞行器飞行状态、研究飞行性能、开展事故分析的重要基础信息。针对风洞试验、飞行器试飞等存在高噪声、高振动、眩光等测试环境,获取高精度的位姿参数,能够为复杂环境下分析气动力问题研究提供保障。
目前,大多数位姿估计都是基于相机(视觉传感器)内参已知的前提下,通过相机获取图像并加以处理,得到图像中的2D特征信息,利用2D-3D坐标系间的对应关系来设计位姿估计方法以实现对目标位姿的估计。在现有方法中大多数都是基于点特征的,目前也存在一些相对成熟的点特征位姿估计方法,例如OI正交迭代法、Epnp算法、自适应扩展卡尔曼方法(AEKF)以及迭代自适应扩展卡尔曼方法(IAEKF)。在上述的四种方法中,前二者的估计精度过分依赖观测噪声,当噪声过大时,它们的估计精度便会急剧下降,在这种情况下,AEKF方法被提出,它通过估计每一个采样步长中的过程噪声和观测噪声来克服环境中噪声过大或者环境发生异变时产生的影响,增强了系统的稳定性。相对于AEKF,IAEKF将AEKF的结果进行迭代,使其估计方差最小,从而实现提高精度,但其计算量却远超AEKF。在某些极端状况下,如相机标定误差过大、初始值误差过大,基于单个视觉传感器的估计方法所得到的估计结果误差将会过大甚至发散。而基于多个视觉传感器的集中式融合估计方法,结合多个视觉传感器提供的图像信息,采用AEKF或者IAEKF估计目标的位姿,在一定程度上弥补了单视觉传感器方法的不足,提高了系统的估计精度,但随着观测信息的增加,观测矩阵维数也会上升,集中式融合方法消耗的计算资源成平方增长,导致系统的响应速度大大降低。在刚体运动过程中,刚体可能被背景中的静止物体遮挡,也有可能因为光线问题导致无法识别,还有可能因为人为干预而出现遮挡,在基于多视觉传感器的位姿估计系统中,当某个局部子系统出现遮挡时,集中式卡尔曼滤波方法将会失效。
为此,可利用分布式融合估计方法对目标进行跟踪。在传统的分布式融合方法中,对缺失观测数据的子系统只进行状态预测,再将其预测结果与其他子系统的估计结果进行融合,而一旦出现连续丢失数据的情况,相应子系统将过度依赖于模型,往往导致跟踪失败,而遮挡过程是一个连续丢失数据的过程,因此目前还没有一种能够有效地处理遮挡问题且具有抗干扰性能的视觉位姿估计方法。
发明内容
为了克服刚体位姿估计过程中因遮挡导致的位姿估计失效问题,本发明设计了基于多视觉传感器分布式信息融合的位姿估计方法,实现了特征信息在被遮挡情况下的刚体位姿准确估计。
本发明所采用的主要技术方案为:
一种基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法,所述的位姿估计方法包括以下步骤:
(1)判断所有视觉传感器局部子系统中是否出现严重遮挡情况,若没有出现严重遮挡,执行1.1);若出现严重遮挡,执行1.2);
1.1)判断是否出现部分遮挡情况,若无部分遮挡情况,执行1.1.1);若出现部分遮挡情况,执行1.1.2);
1.1.1)利用无迹卡尔曼滤波器(Unscented Kalman Filter,UKF)求出局部子系统的状态估计值和误差协方差矩阵,进一步计算各个局部估计误差协方差的迹,将最小的协方差迹对应的状态估计值选为基础状态量xs
Figure BDA0001702813760000021
其中,m表示无遮挡的局部子系统个数,Pi和trace(Pi)分别表示第i个局部子系统误差协方差矩阵和对应的迹,s是局部子系统对应的序号,即第s个局部子系统的误差协方差的迹为最小的,基础状态量xs包含了位置向量
Figure BDA0001702813760000022
及姿态向量
Figure BDA0001702813760000023
其中
Figure BDA0001702813760000024
Figure BDA0001702813760000025
分别为第s个局部子系统中物体坐标系相对于世界坐标系的位置和姿态,b表示物体坐标系,w为世界坐标系,x、y、z分别表示x、y、z三个轴向的坐标值,α、β、γ分别表示绕x、y、z三个轴旋转的角度;
1.1.2)针对部分遮挡的情况,执行步骤1.1.2.1至步骤1.1.2.5;
步骤1.1.2.1利用得到的基础状态量xs计算从物体坐标系到世界坐标系的旋转矩阵
Figure BDA0001702813760000031
Figure BDA0001702813760000032
步骤1.1.2.2利用位置向量
Figure BDA0001702813760000033
和求得的旋转矩阵
Figure BDA0001702813760000034
求出特征点在世界坐标系下的坐标
Figure BDA0001702813760000035
为特征点的序号:
Figure BDA0001702813760000036
其中
Figure BDA0001702813760000037
为特征点在物体坐标系下的坐标;
步骤1.1.2.3根据相机与世界坐标系之间的转换关系,利用求得的坐标
Figure BDA0001702813760000038
计算出特征点在相应相机坐标系中的坐标
Figure BDA0001702813760000039
Figure BDA00017028137600000310
其中cjRwcjtw分别为第j个相机坐标系与世界坐标系的旋转矩阵与平移矩阵,c表示相机坐标系;
步骤1.1.2.4将步骤1.1.2.3中求得的第l个特征点在相机坐标系下的坐标,反投影到第j个相机对应的图像存储坐标系中,得到该特征点在相应像素坐标系下的坐标(ul,vl),从而补全因遮挡而缺失的特征点像素坐标:
Figure BDA00017028137600000311
其中,l、ul、ul分别表示第j个出现遮挡的子系统没有观测到的特征点序号以及该特征点在图像存储坐标中对应的水平和竖直方向坐标值,
Figure BDA00017028137600000312
Figure BDA00017028137600000313
分别表示第j个出现遮挡的子系统没有观测到的第l个特征点在相机坐标系下的x、y、z方向的坐标值,fx、fy分别表示图像存储坐标系中的x、y方向的焦距,u0、v0分别表示图像中心在图像存储坐标系中的横纵坐标;
步骤1.1.2.5补全缺失的观测信息后,利用UKF得到出现部分遮挡的局部子系统的状态信息;
1.2)放弃对该局部子系统进行计算,并利用矩阵加权分布式融合算法对剩余子系统的估计结果进行融合,得到k时刻融合后的估计值,将融合结果反馈给该局部子系统对应的局部估计器,从而使该局部子系统的估计结果重新初始化:
xj(k|k)=x(d)(k|k)
Pj(k|k)=P(d)(k|k)
其中,k表示离散时刻,x(d)(k|k)和P(d)(k|k)分别表示k时刻通过分布式融合方法得到的状态的估计值和误差协方差矩阵,xj(k|k)和Pj(k|k)分别表示k时刻该出现严重遮挡的局部子系统对应的状态估计量和状态误差协方差。
进一步,所述步骤(1)与步骤1.1)中,所述严重遮挡与部分遮挡的定义如下:
根据局部子系统观测到的特征点个数ns对遮挡程度进行区分,不同情况下的定义如下:
若满足nl<ns<n,则判断该子系统出现部分遮挡,其中nl为区分严重遮挡和部分遮挡的特征点个数阈值,n为特征点总个数;
若满足ns<nl,则判断该子系统出现严重遮挡。
再进一步,所述步骤1.2)中,所述分布式融合方法包括以下步骤:
1.2.1)对系统进行初始化设置,设定各个局部子系统初始时刻的状态值
Figure BDA0001702813760000041
误差协方差Pi(0|0),同时设定融合估计的状态值
Figure BDA0001702813760000042
及P(d)(0|0);
1.2.2)对于第k个时刻,对于每个局部子系统,将采集的图像进行处理,提取特征点的二维图像存储坐标,然后利用UKF计算出各个未被遮挡和出现部分遮挡的局部子系统的状态估计值
Figure BDA0001702813760000043
及其对应的误差协方差矩阵Pi(k|k);
1.2.3)计算获得第k个时刻矩阵加权融合的融合状态预测值
Figure BDA0001702813760000044
和状态预测协方差P(d)(k|k-1):
Figure BDA0001702813760000045
Figure BDA0001702813760000051
其中
Figure BDA0001702813760000052
表示k时刻第si个sigma点的状态预测值,Aa是状态转移矩阵,
Figure BDA0001702813760000053
表示k-1时刻第si个sigma点的状态估计值,
Figure BDA0001702813760000054
Figure BDA0001702813760000055
分别为第si个sigma点对应的计算均值及其协方差的权值系数,
Figure BDA0001702813760000056
为第si个sigma点对应的融合状态预测误差,Q(d)为过程噪声协方差矩阵,sigma点的总数为2sn+1个;
1.2.4)更新k时刻的融合误差协方差矩阵P(d)(k|k)及融合后的状态估计值
Figure BDA0001702813760000057
Figure BDA0001702813760000058
Figure BDA0001702813760000059
其中i=1,2,...,L,L表示待融合的局部滤波器个数,
Figure BDA00017028137600000510
Figure BDA00017028137600000511
分别表示k时刻第i个局部滤波器状态的预测值和估计值,Pi(k|k-1)和Pi(k|k)表示二者对应的误差协方差。
本发明的有益效果表现在:能够在出现遮挡的情况下有效地跟踪运动刚体的位姿,保证了系统的鲁棒性和精确性。
附图说明
图1是本方案的遮挡处理流程图。
具体实施方式
下面对本发明做进一步说明。
参照图1,一种基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法,所述的位姿估计方法包括以下步骤:
(1)判断所有视觉传感器局部子系统中是否出现严重遮挡情况,若没有出现严重遮挡,执行步骤1.1);若出现严重遮挡,执行步骤1.2);
1.1)判断是否出现部分遮挡情况,若无部分遮挡情况,执行1.1.1);若出现部分遮挡情况,执行1.1.2);
1.1.1)利用无迹卡尔曼滤波器(Unscented Kalman Filter,UKF)求出局部子系统的状态估计值和误差协方差矩阵,进一步计算各个局部估计误差协方差的迹,将最小的协方差迹对应的状态估计值选为基础状态量xs
Figure BDA0001702813760000061
其中,m表示无遮挡的局部子系统个数,Pi和trace(Pi)分别表示第i个局部子系统误差协方差矩阵和对应的迹,s是局部子系统对应的序号,即第s个局部子系统的误差协方差的迹为最小的,基础状态量xs包含了位置向量
Figure BDA0001702813760000062
及姿态向量
Figure BDA0001702813760000063
其中
Figure BDA0001702813760000064
Figure BDA0001702813760000065
分别为第s个局部子系统中物体坐标系相对于世界坐标系的位置和姿态,b表示物体坐标系,w为世界坐标系,x、y、z分别表示x、y、z三个轴向的坐标值,α、β、γ分别表示绕x、y、z三个轴旋转的角度;
1.1.2)针对出现部分遮挡情况,执行步骤1.1.2.1至步骤1.1.2.5;
步骤1.1.2.1利用得到的基础状态量xs计算从物体坐标系到世界坐标系的旋转矩阵
Figure BDA0001702813760000066
Figure BDA0001702813760000067
步骤1.1.2.2利用位置向量
Figure BDA0001702813760000068
和求得的旋转矩阵
Figure BDA0001702813760000069
求出特征点在世界坐标系下的坐标
Figure BDA00017028137600000610
为特征点的序号:
Figure BDA00017028137600000611
其中
Figure BDA00017028137600000612
为特征点在物体坐标系下的坐标;
步骤1.1.2.3根据相机与世界坐标系之间的转换关系,利用求得的坐标
Figure BDA00017028137600000613
计算出特征点在相应相机坐标系中的坐标
Figure BDA00017028137600000614
Figure BDA00017028137600000615
其中cjRwcjtw分别为第j个相机坐标系与世界坐标系的旋转矩阵与平移矩阵,c表示相机坐标系;
步骤1.1.2.4将步骤1.1.2.3中求得的第l个特征点在相机坐标系下的坐标,反投影到第j个相机对应的图像存储坐标系中,得到该特征点在相应像素坐标系下的坐标(ul,vl),从而补全因遮挡而缺失的特征点像素坐标:
Figure BDA0001702813760000071
其中,l、ul、vl分别表示第j个出现遮挡的子系统没有观测到的特征点序号以及该特征点在图像存储坐标中对应的水平和竖直方向坐标值,
Figure BDA0001702813760000072
Figure BDA0001702813760000073
分别表示第j个出现遮挡的子系统没有观测到的第l个特征点在相机坐标系下的x、y、z方向的坐标值,fx、fy分别表示图像存储坐标系中的x、y方向的焦距,u0、v0分别表示图像中心在图像存储坐标系中的横纵坐标;
步骤1.1.2.5补全缺失的观测信息后,利用UKF得到出现部分遮挡的局部子系统的状态信息;
1.2)若出现严重遮挡,则放弃对该局部子系统进行计算,并利用矩阵加权分布式融合算法对剩余子系统的估计结果进行融合,得到k时刻融合后的估计值,将融合结果反馈给该局部子系统对应的局部估计器,从而使该局部子系统的估计结果重新初始化:
xj(k|k)=x(d)(k|k)
Pj(k|k)=P(d)(k|k)
其中,k表示离散时刻,x(d)(k|k)和P(d)(k|k)分别表示k时刻通过分布式融合方法得到的状态的估计值和误差协方差矩阵,xj(k|k)和Pj(k|k)分别表示k时刻该出现严重遮挡的局部子系统对应的状态估计量和状态误差协方差。
进一步,所述步骤(1)与步骤1.1)中,所述严重遮挡与部分遮挡的定义如下:
根据局部子系统观测到的特征点个数ns对遮挡程度进行区分,不同情况下的定义如下:
若满足nl<ns<n,则判断该子系统出现部分遮挡,其中nl为区分严重遮挡和部分遮挡的特征点个数阈值,n为特征点总个数;
若满足ns<nl,则判断该子系统出现严重遮挡。
再进一步,所述步骤1.2)中,所述分布式融合方法包括以下步骤:
1.2.1)对系统进行初始化设置,设定各个局部子系统初始时刻的状态值
Figure BDA0001702813760000081
误差协方差Pi(0|0),同时设定融合估计的状态值
Figure BDA0001702813760000082
及P(d)(0|0);
1.2.2)对于第k个时刻,对于每个局部子系统,将采集的图像进行处理,提取特征点的二维图像存储坐标,然后利用UKF计算出各个未被遮挡和出现部分遮挡的局部子系统的状态估计值
Figure BDA0001702813760000083
及其对应的误差协方差矩阵Pi(k|k);
1.2.3)计算获得第k个时刻矩阵加权融合的融合状态预测值
Figure BDA0001702813760000084
和状态预测协方差P(d)(k|k-1):
Figure BDA0001702813760000085
Figure BDA0001702813760000086
其中
Figure BDA0001702813760000087
表示k时刻第si个sigma点的状态预测值,Aa是状态转移矩阵,
Figure BDA0001702813760000088
表示k-1时刻第si个sigma点的状态估计值,
Figure BDA0001702813760000089
Figure BDA00017028137600000810
分别为第si个sigma点对应的计算均值及其协方差的权值系数,
Figure BDA00017028137600000811
为第si个sigma点对应的融合状态预测误差,Q(d)为过程噪声协方差矩阵,sigma点的总数为2sn+1个;
1.2.4)更新k时刻的融合误差协方差矩阵P(d)(k|k)及融合后的状态估计值
Figure BDA00017028137600000812
Figure BDA00017028137600000813
Figure BDA0001702813760000091
其中i=1,2,...,L,L表示待融合的局部滤波器个数,
Figure BDA0001702813760000092
Figure BDA0001702813760000093
分别表示k时刻第i个局部滤波器状态的预测值和估计值,Pi(k|k-1)和Pi(k|k)表示二者对应的误差协方差。

Claims (1)

1.一种基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法,其特征在于:所述的位姿估计方法包括以下步骤:
(1)判断所有视觉传感器局部子系统中是否出现严重遮挡情况,若没有出现严重遮挡,执行1.1);若出现严重遮挡,执行1.2);
1.1)判断是否出现部分遮挡情况,若无部分遮挡情况,执行1.1.1);若出现部分遮挡情况,执行1.1.2);
1.1.1)利用无迹卡尔曼滤波器(Unscented Kalman Filter,UKF)求出局部子系统的状态估计值和误差协方差矩阵,进一步计算各个局部估计误差协方差的迹,将最小的协方差迹对应的状态估计值选为基础状态量xs
Figure FDA0002638328270000011
其中,m表示无遮挡的局部子系统个数,Pi和trace(Pi)分别表示第i个局部子系统误差协方差矩阵和对应的迹,s是局部子系统对应的序号,即第s个局部子系统的误差协方差的迹为最小的,基础状态量xs包含了位置向量
Figure FDA0002638328270000012
及姿态向量
Figure FDA0002638328270000013
其中
Figure FDA0002638328270000014
Figure FDA0002638328270000015
分别为第s个局部子系统中物体坐标系相对于世界坐标系的位置和姿态,b表示物体坐标系,w为世界坐标系,x、y、z分别表示x、y、z三个轴向的坐标值,α、β、γ分别表示绕x、y、z三个轴旋转的角度;
1.1.2)针对部分遮挡的情况,执行步骤1.1.2.1至步骤1.1.2.5;
步骤1.1.2.1利用得到的基础状态量xs计算从物体坐标系到世界坐标系的旋转矩阵
Figure FDA0002638328270000016
Figure FDA0002638328270000017
步骤1.1.2.2利用位置向量
Figure FDA0002638328270000018
和求得的旋转矩阵
Figure FDA0002638328270000019
求出特征点在世界坐标系下的坐标
Figure FDA00026383282700000110
l为特征点的序号:
Figure FDA00026383282700000111
其中
Figure FDA0002638328270000021
为特征点在物体坐标系下的坐标;
步骤1.1.2.3根据相机与世界坐标系之间的转换关系,利用求得的坐标
Figure FDA0002638328270000022
计算出特征点在相应相机坐标系中的坐标
Figure FDA0002638328270000023
Figure FDA0002638328270000024
其中cjRwcjtw分别为第j个相机坐标系与世界坐标系的旋转矩阵与平移矩阵,c表示相机坐标系;
步骤1.1.2.4将步骤1.1.2.3中求得的第l个特征点在相机坐标系下的坐标,反投影到第j个相机对应的图像存储坐标系中,得到该特征点在相应像素坐标系下的坐标(ul,vl),从而补全因遮挡而缺失的特征点像素坐标:
Figure FDA0002638328270000025
其中,l、ul、vl分别表示第j个出现遮挡的子系统没有观测到的特征点序号以及该特征点在图像存储坐标中对应的水平和竖直方向坐标值,
Figure FDA0002638328270000026
Figure FDA0002638328270000027
分别表示第j个出现遮挡的子系统没有观测到的第l个特征点在相机坐标系下的x、y、z方向的坐标值,fx、fy分别表示图像存储坐标系中的x、y方向的焦距,u0、υ0分别表示图像中心在图像存储坐标系中的横纵坐标;
步骤1.1.2.5补全缺失的观测信息后,利用UKF得到出现部分遮挡的局部子系统的状态信息;
1.2)放弃对该局部子系统进行计算,并利用矩阵加权分布式融合算法对剩余子系统的估计结果进行融合,得到k时刻融合后的估计值,将融合结果反馈给该局部子系统对应的局部估计器,从而使该局部子系统的估计结果重新初始化:
xj(k|k)=x(d)(k|k)
Pj(k|k)=P(d)(k|k)
其中,k表示离散时刻,x(d)(k|k)和P(d)(k|k)分别表示k时刻通过分布式融合方法得到的状态的估计值和误差协方差矩阵,xj(k|k)和Pj(k|k)分别表示k时刻该出现严重遮挡的局部子系统对应的状态估计量和状态误差协方差;
所述步骤(1)与步骤1.1)中,所述严重遮挡与部分遮挡的定义如下:
根据局部子系统观测到的特征点个数ns对遮挡程度进行区分,不同情况下的定义如下:
若满足nl<ns<n,则判断该子系统出现部分遮挡,其中nl为区分严重遮挡和部分遮挡的特征点个数阈值,n为特征点总个数;
若满足ns<nl,则判断该子系统出现严重遮挡;
所述步骤1.2)中,所述分布式融合方法包括以下步骤:
1.2.1)对系统进行初始化设置,设定各个局部子系统初始时刻的状态值
Figure FDA0002638328270000031
误差协方差Pi(0|0),同时设定融合估计的状态值
Figure FDA0002638328270000032
及P(d)(0|0);
1.2.2)对于第k个时刻,对于每个局部子系统,将采集的图像进行处理,提取特征点的二维图像存储坐标,然后利用UKF计算出各个未被遮挡和出现部分遮挡的局部子系统的状态估计值
Figure FDA0002638328270000033
及其对应的误差协方差矩阵Pi(k|k);
1.2.3)计算获得第k个时刻矩阵加权融合的融合状态预测值
Figure FDA0002638328270000034
和状态预测协方差P(d)(k|k-1):
Figure FDA0002638328270000035
Figure FDA0002638328270000036
其中
Figure FDA0002638328270000037
表示k时刻第si个sigma点的状态预测值,Aa是状态转移矩阵,
Figure FDA0002638328270000038
表示k-1时刻第si个sigma点的状态估计值,
Figure FDA0002638328270000039
Figure FDA00026383282700000310
分别为第si个sigma点对应的计算均值及其协方差的权值系数,
Figure FDA00026383282700000311
为第si个sigma点对应的融合状态预测误差,Q(d)为过程噪声协方差矩阵,sigma点的总数为2sn+1个;
1.2.4)更新k时刻的融合误差协方差矩阵P(d)(k|k)及融合后的状态估计值
Figure FDA00026383282700000312
Figure FDA0002638328270000041
Figure FDA0002638328270000042
其中i=1,2,...,L,L表示待融合的局部滤波器个数,
Figure FDA0002638328270000043
Figure FDA0002638328270000044
分别表示k时刻第i个局部滤波器状态的预测值和估计值,Pi(k|k-1)和Pi(k|k)表示二者对应的误差协方差。
CN201810642823.2A 2018-06-21 2018-06-21 基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法 Active CN108871337B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810642823.2A CN108871337B (zh) 2018-06-21 2018-06-21 基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810642823.2A CN108871337B (zh) 2018-06-21 2018-06-21 基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法

Publications (2)

Publication Number Publication Date
CN108871337A CN108871337A (zh) 2018-11-23
CN108871337B true CN108871337B (zh) 2021-01-01

Family

ID=64340084

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810642823.2A Active CN108871337B (zh) 2018-06-21 2018-06-21 基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法

Country Status (1)

Country Link
CN (1) CN108871337B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110327048B (zh) * 2019-03-11 2022-07-15 浙江工业大学 一种基于可穿戴式惯性传感器的人体上肢姿态重建系统
CN110118556A (zh) * 2019-04-12 2019-08-13 浙江工业大学 一种基于协方差交叉融合slam的机器人定位方法及装置
CN110648354B (zh) * 2019-09-29 2022-02-01 电子科技大学 一种动态环境下的slam方法
CN111222437A (zh) * 2019-12-31 2020-06-02 浙江工业大学 一种基于多深度图像特征融合的人体姿态估计方法
CN111681283B (zh) * 2020-05-11 2023-04-07 哈尔滨工业大学 一种基于单目立体视觉的应用于无线充电对位的相对位姿解算方法
CN111667535B (zh) * 2020-06-04 2023-04-18 电子科技大学 一种针对遮挡场景下的六自由度位姿估计方法
CN112472481A (zh) * 2020-12-15 2021-03-12 沈阳工业大学 一种躯干遮挡状态下动态人体位姿识别嵌入式平台
CN117689713A (zh) * 2023-11-23 2024-03-12 温州大学 基于渐进滤波融合的人体姿态估计方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101179707A (zh) * 2007-09-21 2008-05-14 清华大学 无线网络视频图像多视角协作目标跟踪测量方法
CN102509020A (zh) * 2011-11-15 2012-06-20 哈尔滨工程大学 基于传感器网络的复杂环境下多目标信息融合方法
CN102737386A (zh) * 2012-05-15 2012-10-17 北京硅盾安全技术有限公司 一种运动目标抗融合遮挡跟踪算法
CN103559725A (zh) * 2013-08-09 2014-02-05 中国地质大学(武汉) 一种面向视觉跟踪的无线传感器节点优化选择方法
CN105469430A (zh) * 2015-12-10 2016-04-06 中国石油大学(华东) 一种大场景中的小目标抗遮挡跟踪方法
CN107330468A (zh) * 2017-07-04 2017-11-07 电子科技大学 基于切尔诺夫融合准则的多传感器分布式数据融合方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2012138828A2 (en) * 2011-04-08 2012-10-11 The Trustees Of Columbia University In The City Of New York Kalman filter approach to augment object tracking

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101179707A (zh) * 2007-09-21 2008-05-14 清华大学 无线网络视频图像多视角协作目标跟踪测量方法
CN102509020A (zh) * 2011-11-15 2012-06-20 哈尔滨工程大学 基于传感器网络的复杂环境下多目标信息融合方法
CN102737386A (zh) * 2012-05-15 2012-10-17 北京硅盾安全技术有限公司 一种运动目标抗融合遮挡跟踪算法
CN103559725A (zh) * 2013-08-09 2014-02-05 中国地质大学(武汉) 一种面向视觉跟踪的无线传感器节点优化选择方法
CN105469430A (zh) * 2015-12-10 2016-04-06 中国石油大学(华东) 一种大场景中的小目标抗遮挡跟踪方法
CN107330468A (zh) * 2017-07-04 2017-11-07 电子科技大学 基于切尔诺夫融合准则的多传感器分布式数据融合方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
一种基于多目立体视觉的手术器械跟踪定位策略;赵昕扬等;《江苏大学学报(自然科学版)》;20180131;第39卷(第1期);全文 *

Also Published As

Publication number Publication date
CN108871337A (zh) 2018-11-23

Similar Documents

Publication Publication Date Title
CN108871337B (zh) 基于多视觉传感器分布式信息融合的遮挡情况下目标位姿估计方法
CN110223348B (zh) 基于rgb-d相机的机器人场景自适应位姿估计方法
CN111024066A (zh) 一种无人机视觉-惯性融合室内定位方法
CN106679634B (zh) 一种基于立体视觉的空间非合作目标位姿测量方法
CN112233179B (zh) 一种视觉里程计测量方法
CN103994755B (zh) 一种基于模型的空间非合作目标位姿测量方法
CN111521195B (zh) 一种智能机器人
CN111322993B (zh) 一种视觉定位方法和装置
CN110580713A (zh) 基于全卷积孪生网络和轨迹预测的卫星视频目标跟踪方法
CN111288989A (zh) 一种小型无人机视觉定位方法
CN114119732B (zh) 基于目标检测和K-means聚类的联合优化动态SLAM方法
CN116449384A (zh) 基于固态激光雷达的雷达惯性紧耦合定位建图方法
CN110599522A (zh) 一种视频序列中动态目标检测剔除方法
CN114549549B (zh) 一种动态环境下基于实例分割的动态目标建模跟踪方法
JP7173471B2 (ja) 3次元位置推定装置及びプログラム
CN116879870B (zh) 一种适用于低线束3d激光雷达的动态障碍物去除方法
CN114202579B (zh) 一种面向动态场景的实时多体slam系统
Min et al. Coeb-slam: A robust vslam in dynamic environments combined object detection, epipolar geometry constraint, and blur filtering
CN113362377B (zh) 一种基于单目相机的vo加权优化方法
CN112767481B (zh) 一种基于视觉边缘特征的高精度定位及建图方法
CN110211148A (zh) 一种基于目标状态预估的水下图像预分割方法
CN110570473A (zh) 一种基于点线融合的权重自适应位姿估计方法
CN113592947B (zh) 一种半直接法的视觉里程计实现方法
CN107992677B (zh) 基于惯导信息和亮度修正的红外弱小移动目标跟踪方法
CN116147618A (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