CN107941212B - 一种视觉与惯性联合定位方法 - Google Patents
一种视觉与惯性联合定位方法 Download PDFInfo
- Publication number
- CN107941212B CN107941212B CN201711122305.XA CN201711122305A CN107941212B CN 107941212 B CN107941212 B CN 107941212B CN 201711122305 A CN201711122305 A CN 201711122305A CN 107941212 B CN107941212 B CN 107941212B
- Authority
- CN
- China
- Prior art keywords
- positioning result
- positioning
- result
- predicted
- visual
- 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
Images
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
Abstract
本发明涉及定位技术领域,具体地说,涉及一种视觉与惯性联合定位方法。本发明通过视觉定位结果结合惯性元件姿态结果推算预测定位结果,在新的视觉定位结果出现时,对所述新的视觉定位结果与所述预测定位结果进行一致性判断。当所述新的视觉定位结果与所述预测定位结果具有一致性时,对新的视觉定位结果以及其后一段时间内推算的新的预测定位结果与预测定位结果进行姿态融合,取融合后的姿态作为定位结果输出。本发明通过一致性判断和姿态融合使得定位的实时性和精度更高、定位结果更加平缓。
Description
技术领域
本发明涉及定位技术领域,具体地说,涉及一种视觉与惯性联合定位方法。
背景技术
利用传感器数据得到目标在环境中三维位置信息的过程称为定位,这对移动设备在运输、服务等领域具有重要作用。通常,惯性和视觉传感器是移动设备普遍采用的两种机载/车载传感器。
视觉定位方法是利用视觉系统在移动设备运动过程中,通过成像设备采集环境图像并提取不同图像的特征点,根据检查图像特征点的变化来估计移动设备的运动情况。惯性定位是从已知初始位置根据连续测得移动设备的加速度和角速度推算下一时刻的位置和姿态,从而实时估计移动设备当前姿态。
视觉定位方法虽然可以获得高精度定位结果,但其定位频率太低而且存在定位失败的可能。
惯性元件短期精度高,具有精度约束,但由于惯性元件输出频率较高,累积计算的姿态会引入相应的累积误差,出现“漂移”问题。
为了解决惯性定位和视觉定位各自存在的问题,现有的定位技术通过将惯性与视觉信息相结合进行联合定位,克服各自的缺点,提高定位的实时性和精度。一般来说,视觉-惯性融合方法以视觉定位结果为主,在视觉定位结果出现时,以当前视觉定位结果作为定位结果输出;此后至新的视觉定位结果出现前,以视觉定位结果和惯性元件所得姿态推算预测定位结果作为定位结果输出;在下一次视觉定位结果出现时,以当前时刻的视觉定位结果作为定位结果输出。
上述方法主要存在两个问题:
1.新的视觉定位结果出现时,直接以当前视觉定位结果作为定位结果输出,而不考虑之前的定位输出结果,这样会导致输出的定位结果出现不同程度的跳跃。
2.视觉-惯性的融合缺乏对视觉定位结果的可靠性分析,一旦视觉定位结果误差过大,将导致此后若干时间内融合定位精度下降。
发明内容
针对移动设备视觉惯性联合定位存在鲁棒性差,定位输出不平滑等问题,本发明提供了一种视觉与惯性联合定位方法,其能够提高定位的精度和实时性,并且使定位结果更加平缓。
本发明的目的通过以下技术方案实现:
本发明提出一种视觉与惯性联合定位方法,包括以下步骤:
步骤一,预测定位
通过成像设备采集视觉图像,获得视觉定位结果;通过惯性元件测量数据,推算惯性元件姿态结果;所述视觉定位结果结合所述惯性元件姿态结果推算预测定位结果,所述预测定位结果作为定位结果输出;
步骤二,一致性判断
新的视觉定位结果出现时,所述新的视觉定位结果与所述预测定位结果进行一致性判断,所述新的视觉定位结果和所述预测定位结果不具有一致性时,所述预测定位结果作为定位结果输出;所述新的视觉定位结果和所述预测定位结果具有一致性时,进行步骤三姿态融合;
步骤三,姿态融合
在所述新的视觉定位结果出现时以及其后一段时间内,对所述新的视觉定位结果以及其后新的预测定位结果与所述预测定位结果进行姿态融合,取融合后的结果作为定位结果输出,所述新的预测定位结果由所述新的视觉定位结果结合惯性元件姿态结果推算所得。
进一步地,所述一致性判断包括:
a)旋转一致性判断,所述视觉定位结果的旋转量和所述预测定位结果的旋转量比较计算角度差值,所述角度差值与设定的阈值比较;
b)位置一致性判断,所述视觉定位结果的位置和所述预测定位结果的位置比较计算空间距离,所述空间距离与设定的阈值比较;
所述角度差值、空间距离同时小于相对应的阈值时,所述新的视觉定位结果和所述预测定位结果具有一致性。
进一步地,所述姿态融合包括所述新的视觉定位结果、所述新的预测定位结果与所述预测定位结果进行加权融合。
进一步地,所述姿态融合时,所述预测定位结果的权重每一时刻均不相等且呈递减,所述权重介于0和1之间。
进一步地,所述新的视觉定位结果出现时以及其后一段时间进行的姿态融合在下次视觉定位结果出现之前。
进一步地,所述新的视觉定位结果出现时以及其后一段时间,包括从新的视觉定位结果出现时刻起若干连续时刻,对每一时刻进行姿态融合。
进一步地,所述姿态融合包括旋转融合和位置融合。
进一步地,所述旋转融合为四元数插值融合。
相比于现有技术,本发明的优点在于:
1、本发明将惯性与视觉信息相结合进行联合定位,克服各自的缺点,提高定位的实时性和精度;采用一致性判断,排除由于视觉定位结果出现较大误差而使定位失败或误差较大的可能,提高定位可靠度和稳定性;通过所述姿态融合使得输出的定位结果过渡平滑。
2、本发明中,视觉惯性联合定位中的一致性判断包括旋转一致性判断和位置一致性判断,当两者同时满足要求时,才进行姿态融合,使得联合定位结果精度更高。
3、本发明视觉惯性联合定位中,所述的加权融合算法简单高效,占据的运算空间小,对计算能力要求不高。
4、本发明视觉惯性联合定位中,预测定位结果权重逐渐减小使输出结果逐渐回归到新的预测定位结果上,定位结果准确,减少由惯性数据误差累积影响精度。
5、本发明视觉惯性联合定位中,姿态融合为有限次的姿态融合,其保证姿态过渡平滑的同时减少惯性定位“累积误差”的影响。
6、本发明视觉惯性联合定位中,所述姿态融合包括旋转融合和位置融合,同时对位置和角度分别进行融合使得定位更精确。
附图说明
图1为本发明具体步骤示意图;
图2为本发明输出定位结果示意图。
具体实施方式
下面结合说明书附图和具体的实施例,对发明作详细描述。
实施例
本联合定位方法主要是借助惯性元件来完成移动设备视觉定位的插值处理与相互校正,从而获得高精度、稳定的定位信息。
惯性元件定位:通过惯性测量元件测量线加速度和旋转角速率,根据连续时刻姿态的变化进行推算得到某时刻(以k时刻为例)惯性元件姿态结果并推导出k时刻的预测定位结果所述预测定位结果由前一时刻即k-1时刻预测定位结果和k时刻惯性元件姿态结果结合姿态变换推导获得。所述每一时刻为惯性元件数据采集的时刻。
视觉惯性联合定位方法,如图1所示包括步骤一至步骤三:
步骤一,预测定位
通过成像设备采集视觉图像,获得视觉定位结果;通过惯性元件测量数据,推算惯性元件姿态结果,所述视觉定位结果结合所述惯性元件姿态结果推算预测定位结果,所述预测定位结果作为定位结果输出;当新的视觉定位结果出现时,结合惯性元件姿态结果推算新的预测定位结果。
步骤二,一致性判断
具体地,一致性判断包括旋转一致性判断和位置一致性判断:
旋转一致性判断
位置一致性判断
步骤三,姿态融合
为了提高定位精度,取从k时刻开始的连续N个时刻,每个时刻均进行姿态融合处理。其中N根据实际需求取值且k+(N-1)时刻在下一次视觉定位结果出现时刻之前。
对每一时刻的两个定位结果进行融合,其中,每一时刻的预测定位定位结果的权重α均不相等且所述权重α逐渐减小。当出现视觉定位结果时,此时刻有视觉定位结果和预测定位结果。
旋转融合方法:
在m时刻之后、下一次视觉定位结果出现之前,每一时刻有两个定位结果,即预测定位结果和新的预测定位结果,所述新的预测定位结果为m时刻新的视觉定位结果结合惯性元件姿态结果推算获得。
以n时刻为例:
旋转融合方法:
一些实施例中,如图2所示,图中线1上的点表示为上一次视觉定位结果(图中未表示)结合惯性元件姿态结果推算的各时刻的预测定位结果;图中所述空心黑点为k时刻出现的新的视觉定位结果;图中线2上的点表示为新的视觉定位结果结合惯性元件姿态结果推算的各时刻的新的预测定位结果;图中线3上的实心黑点表示为姿态融合后的各时刻的输出定位结果;线1和线2中的白点为未作为输出定位结果的预测定位结果,线1和线2中的实心黑点为作为输出定位结果的预测定位结果。
在k时刻,对新的视觉定位结果与预测定位进行一致性判断,新的视觉定位结果与预测定位结果具有一致性时,k时刻以及之后一段时间进行姿态融合。
k时刻时,两个定位结果,一个是新的视觉定位结果图2中空心黑点所示;另一个是预测定位结果[由前一时刻即k-1时刻预测定位结果和此时刻惯性元件姿态结果结合姿态变换关系推导获得],k时刻时线1上的白点所示。将与融合得到与融合得到则取做为k时刻定位结果k时刻线3的黑点表示。
在k时刻之后一段时间,如图2取连续的4个时刻(即N=5),在k+4时刻为下一视觉定位结果出现时刻之前,对预测定位结果(线1上的白点)和新的预测定位结果(线2上的白点)进行姿态融合。
k+1时刻,有两个预测定位结果,一个是线1白点表示的预测定位结果[由上一时刻预测定位结果和此时刻惯性元件姿态结合姿态变换关系推导获得],另一个是线2白点表示的新的预测定位结果[由上一时刻视觉定位结果和此时刻惯性元件姿态结合姿态变换关系推导获得]。
同样,k+2时刻,有两个预测定位结果,一个是线1白点表示的预测定位结果[由上一时刻预测定位结果和此时刻惯性元件姿态结合姿态变换关系推导获得],一个是线2白点表示的新的预测定位结果[由上一时刻新的预测定位结果和此时刻惯性元件姿态结合姿态变换关系推导获得]。
k+3、k+4时刻依次类推。
k+5时刻以及k+5时刻之后、下一次某时刻视觉定位结果出现之前,不需要做姿态融合,其输出定位结果由上一时刻输出定位结果以及惯性元件输出定位结果预测得到,上一时刻预测定位结果和此时刻惯性元件姿态结果预测得到取为此时刻输出定位结果对应线2上的黑点。
之后每次出现新的视觉定位结果时按上述一致性判断和姿态融合类推。
以上示意性地对本发明及其实施方式进行了描述,该描述没有限制性,附图中所示的也只是本发明方法的实施方式之一,实际并不局限于此。所以,如果本领域的普通技术人员受其启示,在不脱离本发明创造宗旨的情况下,不经创造性地设计出与该技术方案相似的方式及实施例,均应属于本发明的保护范围。
Claims (7)
1.一种视觉与惯性联合定位方法,其特征在于,包括以下步骤:
步骤一,预测定位,通过成像设备采集视觉图像,获得视觉定位结果;通过惯性元件测量数据,推算惯性元件姿态结果;所述视觉定位结果结合所述惯性元件姿态结果推算预测定位结果,所述预测定位结果作为定位结果输出;
步骤二,一致性判断,新的视觉定位结果出现时,所述新的视觉定位结果与所述预测定位结果进行一致性判断,所述新的视觉定位结果和所述预测定位结果不具有一致性时,所述预测定位结果作为定位结果输出;所述新的视觉定位结果和所述预测定位结果具有一致性时,进行步骤三姿态融合;
步骤三,姿态融合,在所述新的视觉定位结果出现时以及其后一段时间内,对所述新的视觉定位结果以及其后新的预测定位结果与所述预测定位结果进行姿态融合,取融合后的结果作为定位结果输出,所述新的预测定位结果由所述新的视觉定位结果结合惯性元件姿态结果推算所得;
所述姿态融合包括所述新的视觉定位结果、所述新的预测定位结果与所述预测定位结果进行加权融合,所述姿态融合时,所述预测定位结果的权重逐渐减小。
2.根据权利要求1所述的视觉与惯性联合定位方法,其特征在于,所述一致性判断包括:
a)旋转一致性判断,所述新的视觉定位结果的旋转量和所述预测定位结果的旋转量比较计算角度差值,所述角度差值与设定的阈值比较;
b)位置一致性判断,所述新的视觉定位结果的位置和所述预测定位结果的位置比较计算空间距离,所述空间距离与设定的阈值比较;
所述角度差值、空间距离同时小于相对应的阈值时,所述新的视觉定位结果和所述预测定位结果具有一致性。
3.根据权利要求1所述的视觉与惯性联合定位方法,其特征在于,所述姿态融合时,所述预测定位结果的权重每一时刻均不相等且呈递减,所述权重介于0和1之间。
4.根据权利要求1所述的视觉与惯性联合定位方法,其特征在于,所述新的视觉定位结果出现时以及其后一段时间的姿态融合在下次视觉定位结果出现之前。
5.根据权利要求1所述的视觉与惯性联合定位方法,其特征在于,所述新的视觉定位结果出现时以及其后一段时间,包括从新的视觉定位结果出现时刻起若干连续时刻,对每一时刻进行姿态融合。
6.根据权利要求1所述的视觉与惯性联合定位方法,其特征在于,所述姿态融合包括旋转融合和位置融合。
7.根据权利要求6所述的视觉与惯性联合定位方法,其特征在于,所述旋转融合为四元数插值融合。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711122305.XA CN107941212B (zh) | 2017-11-14 | 2017-11-14 | 一种视觉与惯性联合定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711122305.XA CN107941212B (zh) | 2017-11-14 | 2017-11-14 | 一种视觉与惯性联合定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107941212A CN107941212A (zh) | 2018-04-20 |
CN107941212B true CN107941212B (zh) | 2020-07-28 |
Family
ID=61934051
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711122305.XA Active CN107941212B (zh) | 2017-11-14 | 2017-11-14 | 一种视觉与惯性联合定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107941212B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110146074B (zh) * | 2018-08-28 | 2021-06-15 | 北京初速度科技有限公司 | 一种应用于自动驾驶的实时定位方法及装置 |
CN109175832B (zh) * | 2018-09-20 | 2020-11-10 | 上海理工大学 | 一种基于单目测量的3d焊接定位系统的控制方法 |
CN112033412A (zh) * | 2020-09-07 | 2020-12-04 | 中国南方电网有限责任公司超高压输电公司天生桥局 | 一种提高巡检机器人定位精度的方法及装置 |
CN112923923A (zh) * | 2021-01-28 | 2021-06-08 | 深圳市瑞立视多媒体科技有限公司 | 一种imu与刚体姿态、位置对齐的方法、装置、设备以及可读存储介质 |
CN112945231A (zh) * | 2021-01-28 | 2021-06-11 | 深圳市瑞立视多媒体科技有限公司 | 一种imu与刚体姿态对齐的方法、装置、设备以及可读存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102435188A (zh) * | 2011-09-15 | 2012-05-02 | 南京航空航天大学 | 一种用于室内环境的单目视觉/惯性全自主导航方法 |
CN102768042A (zh) * | 2012-07-11 | 2012-11-07 | 清华大学 | 一种视觉-惯性组合导航方法 |
CN104501814A (zh) * | 2014-12-12 | 2015-04-08 | 浙江大学 | 一种基于视觉和惯性信息的姿态与位置估计方法 |
CN104729506A (zh) * | 2015-03-27 | 2015-06-24 | 北京航空航天大学 | 一种视觉信息辅助的无人机自主导航定位方法 |
CN106056664A (zh) * | 2016-05-23 | 2016-10-26 | 武汉盈力科技有限公司 | 一种基于惯性和深度视觉的实时三维场景重构系统及方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2016073642A1 (en) * | 2014-11-04 | 2016-05-12 | The Regents Of The University Of California | Visual-inertial sensor fusion for navigation, localization, mapping, and 3d reconstruction |
-
2017
- 2017-11-14 CN CN201711122305.XA patent/CN107941212B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102435188A (zh) * | 2011-09-15 | 2012-05-02 | 南京航空航天大学 | 一种用于室内环境的单目视觉/惯性全自主导航方法 |
CN102768042A (zh) * | 2012-07-11 | 2012-11-07 | 清华大学 | 一种视觉-惯性组合导航方法 |
CN104501814A (zh) * | 2014-12-12 | 2015-04-08 | 浙江大学 | 一种基于视觉和惯性信息的姿态与位置估计方法 |
CN104729506A (zh) * | 2015-03-27 | 2015-06-24 | 北京航空航天大学 | 一种视觉信息辅助的无人机自主导航定位方法 |
CN106056664A (zh) * | 2016-05-23 | 2016-10-26 | 武汉盈力科技有限公司 | 一种基于惯性和深度视觉的实时三维场景重构系统及方法 |
Non-Patent Citations (2)
Title |
---|
"基于惯性导航和视觉传感器信息融合的移动机器人定位研究";韩利华,;《中国优秀硕士学位论文全文数据库 信息科技辑》;20130715(第07期);38-54页 * |
"基于非线性优化的单目视觉惯性组合导航算法";程传奇 等,;《中国惯性技术学报》;20171031;第25卷(第5期);643-349页 * |
Also Published As
Publication number | Publication date |
---|---|
CN107941212A (zh) | 2018-04-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107941212B (zh) | 一种视觉与惯性联合定位方法 | |
CN111258313B (zh) | 多传感器融合slam系统及机器人 | |
CN108955718B (zh) | 一种视觉里程计及其定位方法、机器人以及存储介质 | |
JP4809291B2 (ja) | 計測装置及びプログラム | |
CN109059907B (zh) | 轨迹数据处理方法、装置、计算机设备和存储介质 | |
EP2824425B1 (en) | Moving-object position/attitude estimation apparatus and method for estimating position/attitude of moving object | |
CN112197770A (zh) | 一种机器人的定位方法及其定位装置 | |
CN107300382B (zh) | 一种用于水下机器人的单目视觉定位方法 | |
CN112004183B (zh) | 一种基于卷积神经网络融合IMU和WiFi信息的机器人自主定位方法 | |
KR101737950B1 (ko) | 지형참조항법에서 영상 기반 항법해 추정 시스템 및 방법 | |
CN110751123A (zh) | 一种单目视觉惯性里程计系统及方法 | |
CN111623773A (zh) | 一种基于鱼眼视觉和惯性测量的目标定位方法及装置 | |
CN115218906A (zh) | 面向室内slam的视觉惯性融合定位方法及系统 | |
CN113899364B (zh) | 定位方法及装置、设备、存储介质 | |
JP5267100B2 (ja) | 運動推定装置及びプログラム | |
JP2018173882A (ja) | 情報処理装置、方法、及びプログラム | |
CN113701750A (zh) | 一种井下多传感器的融合定位系统 | |
JP2009186244A (ja) | 傾斜角度推定システム、相対角度推定システム及び角速度推定システム | |
CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
Qian et al. | Optical flow based step length estimation for indoor pedestrian navigation on a smartphone | |
CN115540854A (zh) | 一种基于uwb辅助的主动定位方法、设备和介质 | |
CN111862146A (zh) | 一种目标对象的定位方法及装置 | |
CN113847907A (zh) | 定位方法及装置、设备、存储介质 | |
CN113566828A (zh) | 一种基于多传感器决策融合的抗冲击扫描匹配方法及系统 | |
JP5424224B2 (ja) | 相対角度推定システム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
TR01 | Transfer of patent right | ||
TR01 | Transfer of patent right |
Effective date of registration: 20220620 Address after: Room 107-f, building 25, No. 1399, liangmu Road, Cangqian street, Yuhang District, Hangzhou, Zhejiang 311121 Patentee after: Smiles (Hangzhou) Technology Co.,Ltd. Address before: Room b1-605-2, No. 198, Qidi Road, Xiaoshan Economic and Technological Development Zone, Hangzhou, Zhejiang, 311200 Patentee before: HANGZHOU DEZE ROBOT TECHNOLOGY Co.,Ltd. |