CN111595337A - 一种基于视觉建模的惯性定位的自校正方法 - Google Patents

一种基于视觉建模的惯性定位的自校正方法 Download PDF

Info

Publication number
CN111595337A
CN111595337A CN202010284017.XA CN202010284017A CN111595337A CN 111595337 A CN111595337 A CN 111595337A CN 202010284017 A CN202010284017 A CN 202010284017A CN 111595337 A CN111595337 A CN 111595337A
Authority
CN
China
Prior art keywords
error
visual modeling
inertial positioning
point
inertial
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
Application number
CN202010284017.XA
Other languages
English (en)
Other versions
CN111595337B (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.)
Ningbo Shenxun Information Technology Co ltd
Original Assignee
Ningbo Shenxun Information Technology Co ltd
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 Ningbo Shenxun Information Technology Co ltd filed Critical Ningbo Shenxun Information Technology Co ltd
Priority to CN202010284017.XA priority Critical patent/CN111595337B/zh
Publication of CN111595337A publication Critical patent/CN111595337A/zh
Application granted granted Critical
Publication of CN111595337B publication Critical patent/CN111595337B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/18—Stabilised platforms, e.g. by gyroscope

Landscapes

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

Abstract

本发明涉及惯性定位技术领域，尤其涉及一种基于视觉建模的惯性定位的自校正方法。它包括以下步骤：S1、惯性设备与视觉建模设备同时采集数据；S2、得到状态方程；S3、得到观测方程；S4、将状态方程与观测方程代入到计算预测值与实际值之间误差的协方差矩阵中；S5、代入多个特征点进行非线性化迭代，不断逼近离散时间模型曲线，得到最终校正结果。采用这种校正方法，在不需要事先做任何布置的前提下，让自主式惯性定位系统在所有场景下，都能够保持长时间的高精度定位效果。

Description

一种基于视觉建模的惯性定位的自校正方法
技术领域
本发明涉及惯性定位技术领域，尤其涉及一种基于视觉建模的惯性定位的自校正方法。
背景技术
现下主流的室内定位技术中，惯性定位技术凭借着其成本低、不受信号强弱影响、不依赖外部辅助即可进行高精度定位的特点占据了一定的市场。然而，惯性定位由于其误差累积的特性，无法满足人员长时间或长距离定位的需求，因此需要一项技术来对惯性定位的结果进行不定时的校正，以弥补误差带来的精度失准。
常见的校正方式主要分为两种，一种是卫星定位校正，使用北斗/GPS定位对惯性定位结果进行修正，保持其定位精度；第二种是架设如UWB、蓝牙信标这类的基站，通过此类基站所得的人员位置来修正惯性定位的结果。
前者使用卫星定位来校正惯性定位结果，将使得惯性定位不再具有原本的不受信号强弱影响特点，一旦来到室内或者高楼附近这类卫星信号差的地区，卫星定位的修正效果就会失效。
后者虽然适用于室内等卫星信号无法覆盖的地方，但是由于对基站有要求，因此具有成本高、需要事先部署基站灵活性差等缺点。
发明内容
本发明所要解决的技术问题是：提供一种基于视觉建模的惯性定位的自校正方法，采用这种校正方法，在不需要事先做任何布置的前提下，让自主式惯性定位系统在所有场景下，都能够保持长时间的高精度定位效果。
本发明所采用的技术方案是：一种基于视觉建模的惯性定位的自校正方法，它包括以下步骤：
S1、惯性设备与视觉建模设备同时采集数据；
S2、根据步骤S1采集到的数据计算得到惯性定位的误差向量与视觉建模的特征点坐标向量，然后根据误差向量与特征点向量得到动点的状态方程；
S3、根据采集到的数据得到视觉建模中的动点与特征点之间的距离，同时得到惯性定位中的动点与特征点的距离，然后根据两个距离的均方差得到动点的观测方程；
S4、将步骤S2得到的状态方程与步骤S3得到观测方程代入到计算预测值与实际值之间误差的协方差矩阵中；
S5、代入多个特征点进行非线性化迭代，不断逼近离散时间模型曲线，得到最终校正结果。
作为优选，步骤S5之后还需要进行平滑处理。
作为优选，所述平滑处理是指采用容积Rauch-Tung-Striebel平滑器进行处理。
作为优选，步骤S2中的误差向量包括姿态误差、位置误差、速度误差、加速度计误差以及陀螺仪误差。
作为优选，步骤S2中得到特征点的具体方法包括以下步骤：
S21、将采集的图像进行灰度化和模糊化处理；
S22、采用黑塞矩阵对处理后的图像中各像素点进行偏导处理，获得突变点；
S23、将突变点与其周围26个像素点进行比较，保留其中的极值点作为特征点。
作为优选，得到特征点后还需要进行特征点筛选。
采用以上方法与现有技术相比，本发明具有以下优点：采用视觉建模技术给惯导定位技术进行校正，这样并不需要事先布置基站，而能让自主式惯性定位系统在所有场景下，都能够保持长时间的高精度定位效果，并且整套定位系统可以完全自主独立进行定位工作，不容易受到外界信号影响。
附图说明
图1为本发明一种基于视觉建模的惯性定位的自校正方法的原理图。
具体实施方式
以下通过具体实施方式对本发明做进一步描述，但是本发明不仅限于以下具体实施方式。
本方法的主要原理就是使用卡尔曼滤波将视觉定位所获得的定位点和惯性技术所获得的定位点进行误差计算。
当来到一个陌生的环境时，集成在一起的惯性设备与视觉建模设备会同时开始采集数据并进行处理。以运行过程中的任意时间点t为例。在t时刻，惯性设备中的加速度计与陀螺仪都采集到了佩戴人员当前的数值，并用这些数值进行处理，得到了此时该人员对应的位置、速度、姿态。显然，使用带有误差的数值计算出来的结果也都含有误差，这里我们使用
Figure BDA0002447802970000022
来表示t时刻时惯性技术解算出的15维误差向量。
Figure BDA0002447802970000021
其中，Фt表示t时刻的姿态误差向量，
Figure BDA0002447802970000031
表示t时刻的位置误差向量，
Figure BDA0002447802970000032
表示t时刻的速度误差向量，
Figure BDA0002447802970000033
表示t时刻的加速度计误差向量，
Figure BDA0002447802970000034
表示t时刻的陀螺仪误差向量，这些误差向量都包括3个元素(即x,y,z,轴或对应各轴的旋转角)。上标b表示该数值建立在载体坐标系上，上标n表示该数值建立在导航坐标系上。
而在惯性设备获取相对定位数值并处理的同时，视觉建模也在不同的采集现场画面并处理着。在t时刻，视觉建模设备采集到现场的画面图像，在对该图像进行灰度化与模糊化处理之后，使用黑塞矩阵对所得到的新图像中的各像素点进行偏导处理，以此初步获得一些突变点，然后用这些突变点与其周围的26个像素点进行比较，只保留其中的极值点作为最后的特征点。
而这些特征点不一定都能够达到自校正算法中用于标定的特征点的标准。因此，要对这些特征点进行进一步的筛选处理，即：根据所需要的特征点个数选择对应的对比度阈值，将这些特征点进行筛选处理，仅保留满足阈值要求的特征点，否则剔除掉。这样就能保证最后留下来的特征点都是满足要求的高对比度的点。将m个这种特征点的坐标向量记为
Figure BDA0002447802970000035
Figure BDA0002447802970000036
结合所获得这些特征点坐标向量与惯性定位的误差向量，得到的状态方程为：
Figure BDA0002447802970000037
其中，
Figure BDA0002447802970000038
矩阵I表示单位矩阵。ωt为满足均值为零正态分布的系统噪声，其协方差矩阵为Q。
Figure BDA0002447802970000039
为t时刻惯性定位系统的误差状态转移矩阵。其表达式为：
Figure BDA00024478029700000310
矩阵中的
Figure BDA00024478029700000311
Figure BDA00024478029700000312
分别表示导航坐标系下的东向，北向与天向，
Figure BDA00024478029700000313
表示坐标系从载体坐标系b系到导航坐标系n系的转换矩阵，T为系统的采样周期。
而因为卡尔曼滤波在进行数值的最优化选择的时候还需要该点的观测值，所以还需要确定t时刻时，动点对应的观测方程。本方法采用特征点与动点之间的距离作为参考数据，然后对其进行均方误差计算减少其中的噪声误差，来确定最后的观测方程。
首先，记视觉建模中的动点与特征点之间的距离为
Figure BDA0002447802970000041
根据距离计算公式，得：
Figure BDA0002447802970000042
式中的
Figure BDA0002447802970000043
表示定点在视觉定位中的理论值，
Figure BDA0002447802970000044
为第i个特征点的理论位置值，特征点取自前文中提到的保留的高对比度得特征点。
那么在同一时刻，该动点在惯性定位中与特征点的间距的表达式为：
Figure BDA0002447802970000045
那么相同点在两个系统中的间距的均方误差为：
Figure BDA0002447802970000046
由于本发明中要用视觉定位的结果来校正惯性定位的定位结果，因此这里先假定视觉定位所得结果精准，为惯性定位结果减去其误差后所得的最后结果，其表达式为：
Figure BDA0002447802970000047
式中的
Figure BDA0002447802970000048
表示t时刻惯性定位推导的定点位置的误差。
将此关系式代入到均方误差表达式中，可得：
Figure BDA0002447802970000049
代入
Figure BDA00024478029700000410
的表达式，得：
Figure BDA00024478029700000411
将等式左边的特征点坐标移至等式右边，使左边仅保留定点坐标值，即：
Figure BDA00024478029700000412
再代入视觉定位中的噪声ε，获得视觉定位得实际距离表达式：
Figure BDA0002447802970000051
可得最后的均方差表达式变为：
Figure BDA0002447802970000052
令
Figure BDA0002447802970000053
Figure BDA0002447802970000054
Figure BDA0002447802970000055
就可以获得最后化简得观测方程：
Figure BDA0002447802970000056
将得到的动点的状态方程与观测方程代入到计算预测值与实际值之间误差的协方差矩阵
Figure BDA0002447802970000057
中，即可获得最后的校正结果。
假设，两个方程所得结果分别为P1＝(x1,y1)，P2＝(x2,y2)，而它们的高斯白噪声分别为Q1,Q2。那么经过校正后得到的定位点坐标为:
Figure BDA0002447802970000058
或
Figure BDA0002447802970000059
完成以上校正计算之后，还需要进行最后的算法优化。因为简单的卡尔曼滤波所得到的是一个线性方程A＝TB，T为转换矩阵。而作为一个描述定位曲线的方程，该定点的移动轨迹是无规则的，显然是一个非线性的方程，那么就需要代入多个特征点来对该方程进行一个多次的迭代逼近我们所需要的方程曲线。
首先，我们需要将这个曲线方程与时间的关系表示出来。假设非线性系统的离散时间模型为：
Xk＝f(Xk-1)+Bk-1ωk-1
yk＝h(Xk)+vk
然后再将相邻定点之间的递归关系代入到协方差矩阵中，即：
Figure BDA00024478029700000510
Figure BDA00024478029700000511
Figure BDA0002447802970000061
Figure BDA0002447802970000062
其中，
Figure BDA0002447802970000063
表示矩阵Xk在k位置的变化率，上标数字表示迭代次数。
通过对上面的协方差矩阵进行迭代运算，可以得到最后的递归关系式为：
Figure BDA0002447802970000064
Figure BDA0002447802970000065
Figure BDA0002447802970000066
上式
Figure BDA0002447802970000067
为将迭代的关系式代入卡尔曼滤波方程后的表达式，
Figure BDA0002447802970000068
与Pk|k是根据
Figure BDA0002447802970000069
表达式推导出来的姿态与位置矩阵，其中
Figure BDA00024478029700000610
经过以上方法拟合得到的曲线可能存在节点不够平滑的问题，因此需要将取得的非线性系统用容积Rauch-Tung-Striebel平滑器来进行平滑处理，也就是将上面的递归关系式代入到下面这个正向数据处理方程组里：
Figure BDA00024478029700000611
其中
Figure BDA00024478029700000612
这样，我们最后得到的关于定位点的递归关系式就是经过优化后的最后的非线性曲线表达式：
Figure BDA00024478029700000613
Figure BDA00024478029700000614
Figure BDA00024478029700000615
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的技术人员应当理解，其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行同等替换；而这些修改或者替换，并不使相应的技术方案的本质脱离本发明各实施例技术方案的精神与范围。

Claims (6)

1.一种基于视觉建模的惯性定位的自校正方法，其特征在于，它包括以下步骤：
S1、惯性设备与视觉建模设备同时采集数据；
S2、根据步骤S1采集到的数据计算得到惯性定位的误差向量与视觉建模的特征点坐标向量，然后根据误差向量与特征点向量得到动点的状态方程；
S3、根据采集到的数据得到视觉建模中的动点与特征点之间的距离，同时得到惯性定位中的动点与特征点的距离，然后根据两个距离的均方差得到动点的观测方程；
S4、将步骤S2得到的状态方程与步骤S3得到观测方程代入到计算预测值与实际值之间误差的协方差矩阵中；
S5、代入多个特征点进行非线性化迭代，不断逼近离散时间模型曲线，得到最终校正结果。
2.根据权利要求1所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：步骤S5之后还需要进行平滑处理。
3.根据权利要求2所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：所述平滑处理是指采用容积Rauch-Tung-Striebel平滑器进行处理。
4.根据权利要求1所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：步骤S2中的误差向量包括姿态误差、位置误差、速度误差、加速度计误差以及陀螺仪误差。
5.根据权利要求1所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：步骤S2中得到特征点的具体方法包括以下步骤：
S21、将采集的图像进行灰度化和模糊化处理；
S22、采用黑塞矩阵对处理后的图像中各像素点进行偏导处理，获得突变点；
S23、将突变点与其周围26个像素点进行比较，保留其中的极值点作为特征点。
6.根据权利要求5所述的一种基于视觉建模的惯性定位的自校正方法，其特征在于：得到特征点后还需要进行特征点筛选。
CN202010284017.XA 2020-04-13 2020-04-13 一种基于视觉建模的惯性定位的自校正方法 Active CN111595337B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010284017.XA CN111595337B (zh) 2020-04-13 2020-04-13 一种基于视觉建模的惯性定位的自校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010284017.XA CN111595337B (zh) 2020-04-13 2020-04-13 一种基于视觉建模的惯性定位的自校正方法

Publications (2)

Publication Number Publication Date
CN111595337A true CN111595337A (zh) 2020-08-28
CN111595337B CN111595337B (zh) 2023-09-26

Family

ID=72187496

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010284017.XA Active CN111595337B (zh) 2020-04-13 2020-04-13 一种基于视觉建模的惯性定位的自校正方法

Country Status (1)

Country Link
CN (1) CN111595337B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2553473A1 (en) * 2005-07-26 2007-01-26 Wa James Tam Generating a depth map from a tw0-dimensional source image for stereoscopic and multiview imaging
US20110141485A1 (en) * 2009-12-16 2011-06-16 Industrial Technology Research Institute System and Method for Localizing a Carrier, Estimating a Posture of the Carrier and Establishing a Map
CN108731670A (zh) * 2018-05-18 2018-11-02 南京航空航天大学 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109387219A (zh) * 2017-08-02 2019-02-26 珊口（上海）智能科技有限公司 误差标定系统
CN109523529A (zh) * 2018-11-12 2019-03-26 西安交通大学 一种基于surf算法的输电线路缺陷识别方法
CN110766720A (zh) * 2019-09-23 2020-02-07 盐城吉大智能终端产业研究院有限公司 一种基于深度学习的多摄像头车辆跟踪系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2553473A1 (en) * 2005-07-26 2007-01-26 Wa James Tam Generating a depth map from a tw0-dimensional source image for stereoscopic and multiview imaging
US20110141485A1 (en) * 2009-12-16 2011-06-16 Industrial Technology Research Institute System and Method for Localizing a Carrier, Estimating a Posture of the Carrier and Establishing a Map
CN109387219A (zh) * 2017-08-02 2019-02-26 珊口（上海）智能科技有限公司 误差标定系统
CN108731670A (zh) * 2018-05-18 2018-11-02 南京航空航天大学 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109523529A (zh) * 2018-11-12 2019-03-26 西安交通大学 一种基于surf算法的输电线路缺陷识别方法
CN110766720A (zh) * 2019-09-23 2020-02-07 盐城吉大智能终端产业研究院有限公司 一种基于深度学习的多摄像头车辆跟踪系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
王龙;董新民;张宗麟;: "紧耦合INS/视觉相对位姿测量方法" *
顾洪洋;方针;刘宇;张泽欣;付乐乐;: "基于卡尔曼滤波的视觉辅助惯导定位算法研究" *

Also Published As

Publication number Publication date
CN111595337B (zh) 2023-09-26

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN106840148B (zh) 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法
CN108519615B (zh) 基于组合导航和特征点匹配的移动机器人自主导航方法
Olson et al. Rover navigation using stereo ego-motion
Amidi et al. A visual odometer for autonomous helicopter flight
CN110361010B (zh) 一种基于占据栅格地图且结合imu的移动机器人定位方法
CN110595466B (zh) 轻量级的基于深度学习的惯性辅助视觉里程计实现方法
CN108235735A (zh) 一种定位方法、装置及电子设备、计算机程序产品
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN107192375B (zh) 一种基于航拍姿态的无人机多帧图像自适应定位校正方法
CN112229405B (zh) 一种基于图像跟踪与激光测距的无人机目标运动估计方法
GB2436740A (en) Video navigation
CN108917753B (zh) 基于从运动恢复结构的飞行器位置确定方法
CN106767791A (zh) 一种采用基于粒子群优化的ckf的惯性/视觉组合导航方法
CN111829532B (zh) 一种飞行器重定位系统和重定位方法
CN112965063B (zh) 一种机器人建图定位方法
CN110751123B (zh) 一种单目视觉惯性里程计系统及方法
CN107621266A (zh) 基于特征点跟踪的空间非合作目标相对导航方法
CN112346104A (zh) 一种无人机信息融合定位方法
CN113223161A (zh) 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法
CN114419109B (zh) 一种基于视觉和气压信息融合的飞行器定位方法
Liu et al. Tightly coupled modeling and reliable fusion strategy for polarization-based attitude and heading reference system
Liu et al. High altitude monocular visual-inertial state estimation: Initialization and sensor fusion
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN113345032B (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
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 315194 room 209-2, building B, science and technology information Incubation Park, No. 655, bachelor Road, Yinzhou District, Ningbo City, Zhejiang Province

Applicant after: Zhejiang shenxun Technology Co.,Ltd.

Address before: 315194 room 209-2, building B, science and technology information Incubation Park, No. 655, bachelor Road, Yinzhou District, Ningbo City, Zhejiang Province

Applicant before: Ningbo shenxun Information Technology Co.,Ltd.

GR01 Patent grant
GR01 Patent grant