CN117705107A - 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 - Google Patents
基于两阶段稀疏舒尔补的面向视觉惯性定位方法 Download PDFInfo
- Publication number
- CN117705107A CN117705107A CN202410170461.7A CN202410170461A CN117705107A CN 117705107 A CN117705107 A CN 117705107A CN 202410170461 A CN202410170461 A CN 202410170461A CN 117705107 A CN117705107 A CN 117705107A
- Authority
- CN
- China
- Prior art keywords
- matrix
- inertial
- variable
- block
- pose
- 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.)
- Granted
Links
- 230000000007 visual effect Effects 0.000 title claims abstract description 59
- 238000000034 method Methods 0.000 title claims abstract description 47
- 239000011159 matrix material Substances 0.000 claims abstract description 185
- 230000001360 synchronised effect Effects 0.000 claims abstract description 19
- 230000008569 process Effects 0.000 claims abstract description 11
- 239000013598 vector Substances 0.000 claims description 83
- 230000009466 transformation Effects 0.000 claims description 12
- 238000000638 solvent extraction Methods 0.000 claims description 7
- 230000000295 complement effect Effects 0.000 claims description 6
- 230000000903 blocking effect Effects 0.000 claims description 5
- APVPOHHVBBYQAV-UHFFFAOYSA-N n-(4-aminophenyl)sulfonyloctadecanamide Chemical compound CCCCCCCCCCCCCCCCCC(=O)NS(=O)(=O)C1=CC=C(N)C=C1 APVPOHHVBBYQAV-UHFFFAOYSA-N 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 13
- 230000003190 augmentative effect Effects 0.000 abstract description 5
- 230000009467 reduction Effects 0.000 abstract description 4
- 238000011946 reduction process Methods 0.000 abstract description 3
- 230000001133 acceleration Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 238000007781 pre-processing Methods 0.000 description 5
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011423 initialization method Methods 0.000 description 1
- 238000006467 substitution reaction 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/20—Instruments 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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于两阶段稀疏舒尔补的面向视觉惯性定位方法,属于视觉定位技术领域。本发明使用两阶段稀疏舒尔补将对海森矩阵进行降维处理,不仅显著降低了面向视觉惯性定位时的方程组求解的计算量,从而提升面向视觉惯性定位的定位效率,同时还利用海森矩阵的特殊稀疏性来进一步减少降维过程中产生的计算量。此外,本发明方法还通过惯性变量和路标点变量的并行求解模式,以进一步减少冗余计算,提升面向视觉惯性定位的定位效率。本发明实施例方法在保证计算精度的同时提升了同步定位的运算速度,使其更好地应用到自动驾驶,移动机器人,增强现实等应用。
Description
技术领域
本发明属于视觉定位技术领域,具体涉及一种基于两阶段稀疏舒尔补的面向视觉惯性定位方法。
背景技术
基于视觉惯性传感器的同步定位是自动驾驶,移动机器人,增强现实等应用中的核心模块,由于其利用了视觉传感器和惯性传感器的互补特性,使得定位结果相比纯视觉方法更加鲁棒,即使应对一些对于纯视觉传感器相对棘手的场景,也能表现出不错的效果。但是与之相对的,由于引入了惯性传感器,基于视觉惯性传感器的同步定位的理论更加复杂,算法复杂度更高,对于自动驾驶,移动机器人,增强现实这些实时性需求比较强的应用很不友好。在基于视觉惯性传感器的同步定位中,方程组求解占据了大量的时间,导致了视觉惯性同步定位处理的计算复杂度高,计算资源浪费。
发明内容
本发明提供了一种基于两阶段稀疏舒尔补的面向视觉惯性定位方法,可用于降低视觉惯性同步定位处理的计算复杂度。
本发明采用的技术方案为:
基于两阶段稀疏舒尔补的面向视觉惯性定位方法,该方法包括下列步骤:
步骤S1,输入待定位目标的初始位姿;
步骤S2,基于置于待定位目标上或待定位目标携带的视觉传感器和惯性传感器,分别获取视觉数据和惯性数据(通常指待定位目标的加速度和角速度);
步骤S3,确定待定位目标的当前位姿:
若当前为首次定位处理,则以输入的初始位姿作为待定位目标的当前位姿;
否则,以上一时刻更新后的位姿得到待定位目标的当前位姿;
步骤S4,基于当前位姿、当前获取的视觉数据和惯性数据构建海森矩阵及其标准向量;
步骤S5,对海森矩阵和标准向量进行两阶段稀疏舒尔补处理,得到最终的面向视觉惯性同步定位的待求解方程组;
步骤S6,对步骤S5得到的待求解方程组进行求解,得到视觉变量;再并行求解惯性变量和路标点变量;
步骤S7,基于当前求解得到的视觉变量、惯性变量和路标点变量更新待定位目标在当前时刻的位姿;
步骤S8,检测是否收到同步定位结束指令,若否,则返回步骤;若是,则输出所有时刻的位姿和路标点变量,基于所有时刻的位姿构成待定位目标的轨迹图,基于所有时刻的路标点变量构成待定位目标的稀疏的点云地图。
进一步的,步骤S4具体包括下列步骤:
步骤S401,基于视觉数据获取位姿残差变量rv,位姿雅可比矩阵Jv,路标点残差变量rm,路标点雅可比矩阵Jm,基于惯性数据获取惯性残差变量ri,惯性雅可比矩阵Ji;
步骤S402,按照惯性变量、位姿变量和路标点变量的顺序确定海森矩阵结构和标准向量结构,其中,海森矩阵为方阵,其行和列方向的变量依次为:惯性变量、位姿变量和路标点变量,海森矩阵的标准向量为N维的列向量,行方向的变量依次为:惯性变量、位姿变量和路标点变量,其中,N表示海森矩阵行或列的维度。
定义惯性变量的维度为Ni,位姿变量的维度为Nv,路标点变量的维度为Nm,一般Ni与Nv与实际优化中包含的帧数有关,比如实际中每次优化帧数为Nk,每帧表示最近某一时刻的所有观测数据(包括视觉数据和惯性数据),假设每一帧的惯性变量维度为9,视觉变量维度为6,那么Ni等于9Nk,Nv等于6Nk。Nm跟实际中需要优化的路标点个数有关,假设实际中需要优化的路标点个数为M,则Nm等于M,从而最终得到海森矩阵的维度N等于9Nk +6Nk +M。
海森矩阵的矩阵块包括:惯性-惯性矩阵块Hii、惯性-视觉矩阵块Hiv、视觉-惯性矩阵块Hvi、视觉-视觉矩阵块Hvv、视觉-路标点矩阵块Hvm、路标点-视觉矩阵块Hmv和路标点-路标点矩阵块Hmm;
标准向量的向量块包括:惯性向量块bi,位姿向量块bv和路标点向量块bm;
步骤S403,按照既定的数据填充规则填充海森矩阵的各矩阵块,以及标准向量的各向量块;再将矩阵块和向量块按照位置关系填充到海森矩阵和标准向量。
进一步的,步骤S5具体包括:
步骤S501,对海森矩阵和标准向量进行分块:
其中,
矩阵块包括:惯性-惯性矩阵块Hii、惯性-视觉矩阵块Hiv、视觉-惯性矩阵块Hvi和视觉-视觉矩阵块Hvv;
矩阵块包括视觉-路标点矩阵块Hvm;
矩阵块包括路标点-视觉矩阵块Hmv;
向量块包括惯性向量块bi和位姿向量块bv;
步骤S502,求矩阵块Hmm的逆矩阵;
步骤S503,对步骤S501的矩阵分块形式进行变换处理:
其中,I表示单位矩阵,表示惯性变量,/>表示路标点变量;
步骤S504,得到第二阶段的标准方程需要的新的海森矩阵和标准向量:
更新矩阵块为:/>;
并更新标准向量为:/>;
步骤S505,进行第二阶段稀疏舒尔补,得到最终的求解方程组:
基于和/>进行第二阶段的矩阵分块:
其中,表示位姿变量,/>表示惯性变量;
求矩阵的逆矩阵;
对第二阶段的矩阵分块进行变换处理:
得到最终的待求解方程组:
。
本发明提供的技术方案至少带来如下有益效果:
本发明采用两阶段稀疏舒尔补减少了基于视觉惯性传感器的同步定位中方程组求解的计算复杂度,大大提升了方程组求解的速度。同时减少了冗余计算,并且实现了惯性变量和路标点变量的并行求解,更有利于并行硬件结构的实现。另一方面,本发明在减少计算复杂度的同时保证了求解的正确性,使得基于视觉惯性传感器的同步定位可以更好地应用于自动驾驶,移动机器人,增强现实等应用。
附图说明
为了更清楚地说明本发明实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其它的附图。
图1是第一种变量排列下的海森矩阵和标准向量结构示意图;
图2是第二种变量排列下的海森矩阵和标准向量结构示意图;
图3是海森矩阵和标准向量结构示意图;
图4是海森矩阵和标准向量第一阶段分块示意图;
图5是新海森矩阵和标准向量第二阶段分块示意图;
图6是新惯性-惯性矩阵块结构示意图;
图7是基于两阶段稀疏舒尔补的面向视觉惯性定位方法的流程示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚,下面将结合附图对本发明实施方式作进一步地详细描述。
本发明实施例提出了一种基于两阶段稀疏舒尔补的面向视觉惯性定位方法,其使用两阶段稀疏舒尔补将对海森矩阵进行降维处理,不仅显著降低了面向视觉惯性定位时的方程组求解的计算量,从而提升面向视觉惯性定位的定位效率,同时还利用海森矩阵的特殊稀疏性来进一步减少降维过程中产生的计算量。此外,本发明实施例方法还通过惯性变量和路标点变量的并行求解模式,进一步减少冗余计算,提升面向视觉惯性定位的定位效率。本发明实施例方法在保证计算精度的同时提升了同步定位的运算速度,使其更好地应用到自动驾驶,移动机器人,增强现实等应用。
基于视觉惯性传感器(通过加速度计和陀螺仪测量设备的加速度和角速度)的同步定位的处理流程中方程组求解往往占据了大量的时间,通常求解的标准方程如下所示:
(1)
其中,H是海森矩阵,b是标准向量,x是待求解变量。通常先计算出海森矩阵H和标准向量b后,然后采用cholesky方法(又称平方根法)求解,得到x。通常标准方程的维度比较大,比如海森矩阵维度是465×465,标准向量是465×1,大维度的方程组求解的计算量会大量增加,不利于快速计算。
本发明实施例中,采用两阶段稀疏舒尔补对海森矩阵进行降维,以减少方程组求解的计算量,同时还利用海森矩阵的特殊稀疏性来进一步减少降维过程中产生的计算量,此外,通过惯性变量和路标点变量的并行求解,以进一步减少冗余计算。
对海森矩阵和标准向量的结构进行分析可知,以465×465的海森矩阵,465×1的标准向量为例,其中465表示了所有变量的维度,海森矩阵跟标准向量是统一的。整个海森矩阵是对称矩阵,总共由三种变量构成,分别是惯性变量,位姿变量和路标点变量。每一个惯性变量有9个自由度,每一个位姿变量有6个自由度,表示对应时刻的设备位置和旋转,每一个路标点变量有1个自由度。根据基于视觉惯性传感器的同步定位构建的问题,假设有11个关键帧,每一帧有一个惯性变量和一个位姿变量,则所有关键帧一共关联海森矩阵中的165维,关联标准向量的165维。假设有300个路标点,则关联海森矩阵中的300维,关联标准向量中的300维。所以海森矩阵维度是465×465,标准向量的维度是465×1。
作为一种可能的实现方式,本发明实施例中,首先基于两阶段稀疏舒尔补对视觉惯性同步定位中的海森矩阵和标准向量进行求解,其具体实现过程包括:
第一步:通过传感器获取输入数据,即通过视觉传感器获取图片,通过惯性传感器获取6维数据,其中加速度有3维,角加速度有3维。
第二步:根据获取的当前位姿,分别对视觉图片和6维惯性数据进行不同的预处理,得到位姿残差变量rv,位姿雅可比矩阵Jv,路标点残差变量rm,路标点雅可比矩阵Jm;惯性残差变量ri,惯性雅可比Ji。
记整体的雅可比矩阵为J,整体的残差变量为r,则可以根据下面的公式计算出海森矩阵H中对应的矩阵块以及标准向量b中对应的向量块:
(2)
由于惯性变量与路标点变量的雅可比之间没有相关性,H矩阵中右上角和左下角部分,由惯性变量和路标点变量雅可比构成的部分都是0示。
第三步:确定海森矩阵的维度和结构。
海森矩阵和标准向量的结构由三种变量的排列关系决定,比如图1和图2所示的两种不同的变量排列,虽然海森矩阵和标准向量的维度一样,但是内部结构却全然不同。可以发现有规律的变量排列,更容易进行优化。所以本发明实施例方法使用了如下的特定排列:
按照惯性变量,位姿变量和路标点变量排列,对应的海森矩阵和标准向量结构如图3所示,进一步可以把海森矩阵进行分块,如图4所示,分成四部分,左上角的Hpp,右上角的Hpm,左下角的Hmp和右下角的Hmm。把标准向量分块,上面是bp,下面是bm。
其中,图3和和图4中的参数符号注释如下:
Hpp:指惯性变量和位姿变量相关的数据,包括惯性-惯性矩阵块Hii,惯性-视觉矩阵块Hiv和视觉-惯性矩阵块Hvi,视觉-视觉矩阵块Hvv;
Hmm:路标点变量相关的数据,是路标点-路标点矩阵块,并且Hmm是对角线矩阵;
Hpm:位姿变量与路标点变量相关的数据,包括视觉-路标点矩阵块Hvm;
Hpm:路标点变量与位姿变量相关的数据,包括路标点-视觉矩阵块Hmv;
bp:惯性变量和位姿变量相关的数据,包括惯性向量块bi,位姿向量块bv。
bm:指路标点变量相关的数据,是路标点向量块。
第四步:填充海森矩阵。
通过预处理得到的位姿残差变量rv,位姿雅可比矩阵Jv,路标点残差变量rm,路标点雅可比矩阵Jm;惯性残差变量ri,惯性雅可比Ji。
第五步:两阶段稀疏舒尔补求解。
501:第一阶段稀疏舒尔补:消除路标点变量带来的维度。
基于公式(1)所示的待求解的标准方程进行矩阵和向量分块,具体分块情况如图4所示,将路标点变量从所有变量x中分块出来,位姿变量和惯性变量放在一起记为/>,得到如下:
(3)
通过矩阵乘法实现矩阵的变换,方程左右同时左乘一个变换矩阵,得到如下:
(4)
(5)
此时公式(3)等效的转化为公式(5),此时可以根据公式(5)化简得到公式(6)和公式(7)并且可以根据公式(5)先求出位姿变量和惯性变量,再根据公式(7)求出路标点变量/>:
(6)
(7)
由于Hmm是对角块矩阵,所以在计算Hmm的逆的时候只需要把对角元逐个取反即可,不需要进行复杂的方程组取逆操作,以减少了方程降维时的计算量。
第一阶段稀疏舒尔补的用途主要体现在:
(1)在方程组求解中消除了路标点变量的维度,将原来465x465维度的方程组求解,转成了165x165的方程组求解和矩阵乘法,大大减少了方程组求解的计算量。
(2)因为Hmm是对角线矩阵,所以利用其特殊稀疏性,Hmm的逆矩阵很容易求出。对于路标点的变量xm使用矩阵乘法求出,具有更高的并行度,便于硬件加速。
502:第二阶段稀疏舒尔补。
在第一阶段稀疏舒尔补之后,因为Hpm中与惯性变量相关的位置都是0,所以对于路标点的变量xm的计算中存在大量0元素乘法,这些计算在矩阵乘法中都可以省去。另一方面,由于路标点变量与惯性变量不相关,所以是可以并行处理的。综上,一方面为了省去路标点的变量xm计算中的冗余计算,另一方面为了提高处理的并行度。
由第一阶段稀疏舒尔补之后,得到公式(5),将其重新记为新的海森矩阵:
(8)
新的标准向量为:
(9)
进而得到公式(10):
(10)
将新的海森矩阵和标准向量进行分块,具体分块情况如图5所示,并且将位姿变量和惯性变量分块,其中位姿变量为/>,惯性变量为/>,得到公式(11),其中得到新惯性-惯性矩阵块/>,新惯性-位姿矩阵块/>,新位姿-惯性矩阵块/>,新位姿-位姿矩阵块。分别计算出对应的向量块,新惯性向量块/>,新视觉向量块/>:
(11)
通过矩阵乘法实现矩阵的变换,方程左右同时左乘一个变换矩阵,得到如下:
(12)
(13)
此时公式(10)等效的转化为公式(13),然后化简得到公式(14)和公式(15)并且可以根据公式(14)先求出位姿变量,再根据公式(15)求出惯性变量/>:
(14)
(15)
由于新惯性-惯性矩阵块为对角块矩阵,具体结构如图6所示,本发明实施例中,采用针对对角块矩阵的求逆方式(利用/>的特殊稀疏性),相比于完整的矩阵求逆,节省了零元素位置的计算量,大大减少了求逆的复杂度,进一步减少了矩阵降维时的计算量。
通过第二阶段稀疏舒尔补可以发现,实际中可以先进行公式(14)的求解,再得到位姿变量之后,可以并行求解路标点变量/>和惯性变量/>。根据公式(7)的求解路标点变量/>时,因为不需要惯性变量/>,所以把相应的那部分0元素乘法计算都省去,直接使用位姿变量/>求解即可,见公式(16):
(16)
与此同时,可以根据公式(15)求解惯性变量。这两个求解路线没有关联,完全可以并行执行。
第二阶段稀疏舒尔补的用途主要体现在:
(1)进一步减少方程组求解的维度,从第一阶段舒尔补165x165降低到66x66,进一步减少了方程组求解的复杂度,提升计算速度。
(2)实现了并行求解路标点变量和惯性变量,方便硬件实现加速。
(3)省去了计算路标点变量时的0元素乘法计算,减少了冗余计算。
(4)因为是对角块矩阵,相比普通矩阵更容易求逆,没有引入更大的计算量。
作为一种可能的实现方式,如图7所示,本发明实施例提供的基于两阶段稀疏舒尔补的面向视觉惯性定位方法具体包括下列步骤:
步骤1:通过初始化方法得到(待定位)设备的初始位姿;
步骤2:通过视觉传感器获取视觉传感器数据(图像数据),通过惯性传感器获取惯性数据;
步骤3:获取设备当前的位姿。
如果是第一次,则取出初始化得到的初始位姿;否则,则取出步骤11更新后的位姿。
然后带着图像数据和惯性数据,到步骤4进行预处理。
步骤4:对视觉数据和惯性数据分别进行预处理:
步骤4.1:单独对视觉数据进行预处理,得到位姿残差变量rv,位姿雅可比矩阵Jv,路标点残差变量rm,路标点雅可比矩阵Jm;
步骤4.2:单独对惯性数据进行预处理,得到惯性残差变量ri,惯性雅可比Ji;
步骤5:构建并填充海森矩阵和标准向量:
步骤5.1:按照惯性变量,位姿变量,路标点变量的顺序,确定海森矩阵结构和标准向量结构。
步骤5.2:分别计算出对应的矩阵块,惯性-惯性矩阵块Hii,惯性-视觉矩阵块Hiv和Hvi,视觉-视觉矩阵块Hvv,视觉-路标点矩阵块Hvm和Hmv,路标点-路标点矩阵块Hmm。分别计算出对应的向量块,惯性向量块bi,视觉向量块bv,路标点向量块bm。
步骤5.3:将计算出来的矩阵块和向量块按照位置关系填充到海森矩阵和标准向量。
步骤6:进行第一阶段稀疏舒尔补,得到第二阶段的标准方程中新的海森矩阵和标准向量。
步骤6.1:根据公式(3)将矩阵和向量分块;
步骤6.2:求矩阵Hmm的逆矩阵;
步骤6.3:方程左右乘以变换矩阵,得到公式(4)、(5)所示的变换结果:
步骤6.4:得到第二阶段的标准方程需要的新的海森矩阵和标准向量: ;
步骤7:进行第二阶段稀疏舒尔补,得到最终的求解方程组;
步骤7.1:根据公式(11)将矩阵分块;
步骤7.2:求矩阵的逆矩阵;
步骤7.3:方程左右乘以变换矩阵,得到如公式(12)、(13)所示的变换结果;
步骤7.4:从而基于步骤7.3得到最终的待求解方程组,如公式(14)所示;
步骤8:利用cholesky进行方程组求解,得到视觉变量xv;
步骤9:并行求解惯性变量xi和路标点变量xm,即步骤9.1和步骤9.2是可以并行执行的。
步骤9.1:利用矩阵乘法求解惯性变量xi,得到如公式(15)所示的求解结果;
步骤9.2:利用矩阵乘法求解路标点变量xm,得到如公式(16)所示的求解结果。注意这里与惯性变量相关的地方都直接去掉,减少0的冗余计算;
步骤10:输出所有的求解变量,即视觉变量xv,惯性变量xi和路标点变量xm;
步骤11:根据步骤10输出的结果,更新当前位姿,完成同步定位。
步骤12:判断是否结束,如果是,则进行步骤13;否则进行步骤2。
步骤13:输出所有的时刻的位姿和路标点变量,所有的位姿构成轨迹图,所有的路标点变量构成稀疏的点云地图。
最后应说明的是:以上实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
以上所述的仅是本发明的一些实施方式。对于本领域的普通技术人员来说,在不脱离本发明创造构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。
Claims (5)
1.基于两阶段稀疏舒尔补的面向视觉惯性定位方法,其特征在于,包括下列步骤:
步骤S1,输入待定位目标的初始位姿;
步骤S2,基于置于待定位目标上或待定位目标携带的视觉传感器和惯性传感器,分别获取视觉数据和惯性数据;
步骤S3,确定待定位目标的当前位姿:
若当前为首次定位处理,则以输入的初始位姿作为待定位目标的当前位姿;否则,以上一时刻更新后的位姿得到待定位目标的当前位姿;
步骤S4,基于当前位姿、当前获取的视觉数据和惯性数据构建海森矩阵及其标准向量;
步骤S5,对海森矩阵和标准向量进行两阶段稀疏舒尔补处理,得到最终的面向视觉惯性同步定位的待求解方程组;
步骤S6,对步骤S5得到的待求解方程组进行求解,得到视觉变量;再并行求解惯性变量和路标点变量;
步骤S7,基于当前求解得到的视觉变量、惯性变量和路标点变量更新待定位目标在当前时刻的位姿;
步骤S8,检测是否收到同步定位结束指令,若否,则返回步骤;若是,则输出所有时刻的位姿和路标点变量,基于所有时刻的位姿构成待定位目标的轨迹图,基于所有时刻的路标点变量构成待定位目标的稀疏的点云地图。
2.如权利要求1所述的基于两阶段稀疏舒尔补的面向视觉惯性定位方法,其特征在于,步骤S4中,按照惯性变量、位姿变量和路标点变量的顺序确定海森矩阵结构和标准向量结构,其中,海森矩阵为方阵,其行和列方向的变量依次为:惯性变量、位姿变量和路标点变量,海森矩阵的标准向量为N维的列向量,行方向的变量依次为:惯性变量、位姿变量和路标点变量,其中,N表示海森矩阵行或列的维度;
海森矩阵的矩阵块包括:惯性-惯性矩阵块Hii、惯性-视觉矩阵块Hiv、视觉-惯性矩阵块Hvi、视觉-视觉矩阵块Hvv、视觉-路标点矩阵块Hvm、路标点-视觉矩阵块Hmv和路标点-路标点矩阵块Hmm;
标准向量的向量块包括:惯性向量块bi,位姿向量块bv和路标点向量块bm。
3.如权利要求2所述的基于两阶段稀疏舒尔补的面向视觉惯性定位方法,其特征在于,步骤S4具体包括下列步骤:
步骤S401,基于视觉数据获取位姿残差变量rv,位姿雅可比矩阵Jv,路标点残差变量rm,路标点雅可比矩阵Jm,基于惯性数据获取惯性残差变量ri,惯性雅可比矩阵Ji;
步骤S402,记整体的雅可比矩阵为J,整体的残差变量为r,根据公式计算出海森矩阵H中对应的矩阵块以及标准向量b中对应的向量块:
;
步骤S403,按照既定的数据填充规则填充海森矩阵的各矩阵块,以及标准向量的各向量块;再将矩阵块和向量块按照位置关系填充到海森矩阵和标准向量。
4.如权利要求2所述的基于两阶段稀疏舒尔补的面向视觉惯性定位方法,其特征在于,海森矩阵H的维度N具体设置为:n1×Nk + n2×Nk +M;
其中,Nk为预置的同步定位处理的目标帧数,用于表征定义惯性变量和位姿变量的维度,n1表示每一帧的惯性变量维度,n2表示每一帧的视觉变量维度,M表示同步定位处理的路标点的目标点数,用于表征位姿变量的维度。
5.如权利要求2至4任一项所述的基于两阶段稀疏舒尔补的面向视觉惯性定位方法,其特征在于,步骤S5具体包括:
步骤S501,对海森矩阵和标准向量进行分块:
;
其中,
矩阵块包括:惯性-惯性矩阵块Hii、惯性-视觉矩阵块Hiv、视觉-惯性矩阵块Hvi和视觉-视觉矩阵块Hvv;
矩阵块包括视觉-路标点矩阵块Hvm;
矩阵块包括路标点-视觉矩阵块Hmv;
向量块包括惯性向量块bi和位姿向量块bv;
步骤S502,求矩阵块Hmm的逆矩阵;
步骤S503,对步骤S501的矩阵分块形式进行变换处理:
;
;
其中,I表示单位矩阵,表示惯性变量,/>表示路标点变量;
步骤S504,得到第二阶段的标准方程需要的新的海森矩阵和标准向量:
更新矩阵块为:/>;
并更新标准向量为:/>;
步骤S505,进行第二阶段稀疏舒尔补,得到最终的求解方程组:
基于和/>进行第二阶段的矩阵分块:
;
其中,表示位姿变量,/>表示惯性变量;
求矩阵的逆矩阵;
对第二阶段的矩阵分块进行变换处理:
;
;
得到最终的待求解方程组:。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410170461.7A CN117705107B (zh) | 2024-02-06 | 2024-02-06 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202410170461.7A CN117705107B (zh) | 2024-02-06 | 2024-02-06 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN117705107A true CN117705107A (zh) | 2024-03-15 |
CN117705107B CN117705107B (zh) | 2024-04-16 |
Family
ID=90148434
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202410170461.7A Active CN117705107B (zh) | 2024-02-06 | 2024-02-06 | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117705107B (zh) |
Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140333741A1 (en) * | 2013-05-08 | 2014-11-13 | Regents Of The University Of Minnesota | Constrained key frame localization and mapping for vision-aided inertial navigation |
US20150043784A1 (en) * | 2013-08-12 | 2015-02-12 | Flyby Media, Inc. | Visual-Based Inertial Navigation |
US20160327395A1 (en) * | 2014-07-11 | 2016-11-10 | Regents Of The University Of Minnesota | Inverse sliding-window filters for vision-aided inertial navigation systems |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
CN110160522A (zh) * | 2019-04-16 | 2019-08-23 | 浙江大学 | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 |
CN111797906A (zh) * | 2020-06-15 | 2020-10-20 | 北京三快在线科技有限公司 | 基于视觉和惯性里程定位的方法及装置 |
GB2599948A (en) * | 2020-10-16 | 2022-04-20 | Slamcore Ltd | Initialising keyframes for visual-inertial localisation and/or mapping |
CN114440877A (zh) * | 2022-01-26 | 2022-05-06 | 北京航空航天大学 | 一种异步多相机视觉惯性里程计定位方法 |
CN114812601A (zh) * | 2021-01-29 | 2022-07-29 | 阿里巴巴集团控股有限公司 | 视觉惯性里程计的状态估计方法、装置、电子设备 |
CN114860857A (zh) * | 2021-02-04 | 2022-08-05 | 三星电子株式会社 | 用于执行同步定位与建图的方法以及使用该方法的设备 |
CN115168283A (zh) * | 2022-07-05 | 2022-10-11 | 北京航空航天大学 | 一种数据融合模块加速器及方法 |
CN115265529A (zh) * | 2022-07-05 | 2022-11-01 | 北京鉴智科技有限公司 | 目标对象的定位方法、***、装置、电子设备及存储介质 |
US20220358262A1 (en) * | 2021-04-26 | 2022-11-10 | Samsung Electronics Co., Ltd. | Method and apparatus for accelerating simultaneous localization and mapping |
CN115371665A (zh) * | 2022-09-13 | 2022-11-22 | 哈尔滨工业大学 | 一种基于深度相机和惯性融合的移动机器人定位方法 |
US20230145875A1 (en) * | 2021-11-02 | 2023-05-11 | Samsung Electronics Co., Ltd. | Apparatus for accelerating simultaneous localization and mapping and electronic device including the same |
WO2023155258A1 (zh) * | 2022-02-21 | 2023-08-24 | 武汉大学 | 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法 |
-
2024
- 2024-02-06 CN CN202410170461.7A patent/CN117705107B/zh active Active
Patent Citations (16)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20140333741A1 (en) * | 2013-05-08 | 2014-11-13 | Regents Of The University Of Minnesota | Constrained key frame localization and mapping for vision-aided inertial navigation |
US20150043784A1 (en) * | 2013-08-12 | 2015-02-12 | Flyby Media, Inc. | Visual-Based Inertial Navigation |
US20160327395A1 (en) * | 2014-07-11 | 2016-11-10 | Regents Of The University Of Minnesota | Inverse sliding-window filters for vision-aided inertial navigation systems |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
CN110160522A (zh) * | 2019-04-16 | 2019-08-23 | 浙江大学 | 一种基于稀疏特征法的视觉惯导里程计的位姿估计方法 |
CN111797906A (zh) * | 2020-06-15 | 2020-10-20 | 北京三快在线科技有限公司 | 基于视觉和惯性里程定位的方法及装置 |
GB2599948A (en) * | 2020-10-16 | 2022-04-20 | Slamcore Ltd | Initialising keyframes for visual-inertial localisation and/or mapping |
CN114812601A (zh) * | 2021-01-29 | 2022-07-29 | 阿里巴巴集团控股有限公司 | 视觉惯性里程计的状态估计方法、装置、电子设备 |
CN114860857A (zh) * | 2021-02-04 | 2022-08-05 | 三星电子株式会社 | 用于执行同步定位与建图的方法以及使用该方法的设备 |
US20220358262A1 (en) * | 2021-04-26 | 2022-11-10 | Samsung Electronics Co., Ltd. | Method and apparatus for accelerating simultaneous localization and mapping |
US20230145875A1 (en) * | 2021-11-02 | 2023-05-11 | Samsung Electronics Co., Ltd. | Apparatus for accelerating simultaneous localization and mapping and electronic device including the same |
CN114440877A (zh) * | 2022-01-26 | 2022-05-06 | 北京航空航天大学 | 一种异步多相机视觉惯性里程计定位方法 |
WO2023155258A1 (zh) * | 2022-02-21 | 2023-08-24 | 武汉大学 | 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法 |
CN115168283A (zh) * | 2022-07-05 | 2022-10-11 | 北京航空航天大学 | 一种数据融合模块加速器及方法 |
CN115265529A (zh) * | 2022-07-05 | 2022-11-01 | 北京鉴智科技有限公司 | 目标对象的定位方法、***、装置、电子设备及存储介质 |
CN115371665A (zh) * | 2022-09-13 | 2022-11-22 | 哈尔滨工业大学 | 一种基于深度相机和惯性融合的移动机器人定位方法 |
Non-Patent Citations (4)
Title |
---|
CEN ET AL.: "A low-cost visual inertial odometry for mobile vehicle based on double stage Kalman filter", SIGNAL PROCESSING, vol. 197, 14 July 2022 (2022-07-14), pages 1 - 10 * |
XIN ET AL.: "Information sparsification for visual-inertial odometry by manipulating Bayes tree", AEROSPACE SCIENCE AND TECHNOLOGY, vol. 118, 26 August 2021 (2021-08-26), pages 1 - 12, XP086780839, DOI: 10.1016/j.ast.2021.107065 * |
刘颖, 王树勋, 史公正: "移动终端定位参数联合估计方法的研究", 长春邮电学院学报, vol. 19, no. 02, 30 June 2001 (2001-06-30), pages 1 - 9 * |
张月圆: "惯性信息辅助的单目视觉SLAM技术研究", 中国优秀硕士学位论文全文数据库 信息科技辑, no. 2021, 15 July 2021 (2021-07-15), pages 138 - 543 * |
Also Published As
Publication number | Publication date |
---|---|
CN117705107B (zh) | 2024-04-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110009681B (zh) | 一种基于imu辅助的单目视觉里程计位姿处理方法 | |
CN108648215B (zh) | 基于imu的slam运动模糊位姿跟踪算法 | |
CN106679648B (zh) | 一种基于遗传算法的视觉惯性组合的slam方法 | |
CN107341814B (zh) | 基于稀疏直接法的四旋翼无人机单目视觉测程方法 | |
CN110517324B (zh) | 基于变分贝叶斯自适应算法的双目vio实现方法 | |
Li et al. | Real-time 3D motion tracking and reconstruction system using camera and IMU sensors | |
CN111754579B (zh) | 多目相机外参确定方法及装置 | |
CN110264528B (zh) | 一种鱼眼镜头双目像机快速自标定方法 | |
CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
CN111507222B (zh) | 一种基于多源数据知识迁移的三维物体检测框架 | |
Geneva et al. | An efficient schmidt-ekf for 3D visual-inertial SLAM | |
WO2020221307A1 (zh) | 一种运动物体的追踪方法和装置 | |
CN110533724B (zh) | 基于深度学习和注意力机制的单目视觉里程计的计算方法 | |
CN108731700B (zh) | 一种视觉惯性里程计中的加权欧拉预积分方法 | |
CN116342661A (zh) | 利用路标点偏置位姿修正的双目视觉惯性里程计方法 | |
CN110617813A (zh) | 单目视觉信息和imu信息相融合的尺度估计***及方法 | |
CN112183506A (zh) | 一种人体姿态生成方法及其*** | |
CN112985450B (zh) | 一种具有同步时间误差估计的双目视觉惯性里程计方法 | |
CN116205947A (zh) | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 | |
WO2023005457A1 (zh) | 位姿计算方法和装置、电子设备、可读存储介质 | |
CN117705107B (zh) | 基于两阶段稀疏舒尔补的面向视觉惯性定位方法 | |
CN113570716A (zh) | 云端三维地图构建方法、***及设备 | |
WO2023207230A1 (zh) | 一种面向视觉惯性定位的基于地图的一致高效滤波算法 | |
CN114993338B (zh) | 基于多段独立地图序列的一致性高效视觉惯性里程计算法 | |
Martinelli | Visual-inertial structure from motion: observability vs minimum number of sensors |
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 |