CN114485648B - 一种基于仿生复眼惯性***的导航定位方法 - Google Patents
一种基于仿生复眼惯性***的导航定位方法 Download PDFInfo
- Publication number
- CN114485648B CN114485648B CN202210119005.0A CN202210119005A CN114485648B CN 114485648 B CN114485648 B CN 114485648B CN 202210119005 A CN202210119005 A CN 202210119005A CN 114485648 B CN114485648 B CN 114485648B
- Authority
- CN
- China
- Prior art keywords
- compound eye
- inertial
- measurement unit
- image
- bionic
- 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
- 150000001875 compounds Chemical class 0.000 title claims abstract description 141
- 238000000034 method Methods 0.000 title claims abstract description 53
- 239000011664 nicotinic acid Substances 0.000 title claims abstract description 43
- 230000000007 visual effect Effects 0.000 claims abstract description 31
- 238000010168 coupling process Methods 0.000 claims abstract description 10
- 238000005259 measurement Methods 0.000 claims description 60
- 239000011159 matrix material Substances 0.000 claims description 41
- 238000005457 optimization Methods 0.000 claims description 36
- 239000013598 vector Substances 0.000 claims description 18
- 238000005516 engineering process Methods 0.000 claims description 17
- 230000009466 transformation Effects 0.000 claims description 16
- 230000033001 locomotion Effects 0.000 claims description 13
- 238000012545 processing Methods 0.000 claims description 13
- 230000010354 integration Effects 0.000 claims description 11
- 230000008878 coupling Effects 0.000 claims description 8
- 238000005859 coupling reaction Methods 0.000 claims description 8
- 238000011084 recovery Methods 0.000 claims description 5
- 238000012937 correction Methods 0.000 claims description 3
- 238000007499 fusion processing Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 abstract description 9
- 230000008859 change Effects 0.000 abstract description 5
- 238000013519 translation Methods 0.000 description 15
- 238000010586 diagram Methods 0.000 description 13
- 230000003287 optical effect Effects 0.000 description 12
- 238000003384 imaging method Methods 0.000 description 10
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000006243 chemical reaction Methods 0.000 description 6
- 230000009467 reduction Effects 0.000 description 6
- 238000010276 construction Methods 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 238000013461 design Methods 0.000 description 4
- 238000005295 random walk Methods 0.000 description 4
- 230000004044 response Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 230000007123 defense Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 241000238631 Hexapoda Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 210000001328 optic nerve Anatomy 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 230000001360 synchronised effect Effects 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)
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于仿生复眼惯性***的导航定位方法，利用仿生复眼的惯性***，根据“动态基线+三角测距”的方法对目标物体实现高准确性和高精度的导航定位，使用“复眼+惯性测量单元”耦合的方法对视觉信息和惯性信息进行采集和处理，得到目标物体的高准确度的相对位姿变化，通过“动态基线+三角测距”的方法实现高精度的定姿定位。
Description
技术领域
本发明涉及导航技术领域，尤其涉及一种基于仿生复眼惯性***的导航定位方法。
背景技术
近年来，随着无人机、无人车以及机器人的快速发展，导航技术成为了制约无人平台广泛应用的瓶颈技术之一，视觉/惯性组合导航技术，特别是视觉与惯性传感器的组合，逐渐发展成为当前自主导航与机器人领域研究和应用的热点，在国民经济和国防建设中也有广泛应用。其中优势之一就是视觉/惯性能够，相比较单一***能实现更高精度定位、地图构建。
在国防建设领域，由于视觉/惯性组合导航技术不依赖于外部人造设施，在卫星领域中有重要应用，美国陆军研发的一种新型联合精确空投***采用视觉/惯性组合导航技术解决定位问题，嫦娥三号巡视器也采用了视觉/惯性组合导航技术实现高精度的定姿定位。目前，传统视觉惯性组合导航技术根据视觉类型分为：单目视惯***和双目视惯***。
双目视惯***通过两个摄像机之间的精确空间位置关系得到的视差进行定位，但计算量比较巨大，对应点匹配难度比较大，实时性比较差，不适合快速、高效的进行实时定位，且双目***体积大，高质量的摄像机成本更大；单目视惯***中，相较于双目视惯成本有所降低，但单目相机在纹理少的区域无法正常工作，相机视场相对比较小，无法采集大范围图像信息，且物体快速运动时，相机采集到的图像模糊导致动态场景出现错误匹配。仿生复眼技术发展已经比较完善，能为***提供大视场，其具备的快速动态响应特性可以改善单目视惯中由于快速运动而造成的错误匹配，提高定位精度。针对单目视惯***中定位精度和准确度的问题，本发明设计一种基于仿生复眼惯性定位***，提出“动态基线+三角测距”法实现目标的高精度定位，能有效改善上述问题。
发明内容
为解决现有技术存在的局限和缺陷，本发明提供一种基于仿生复眼惯性***的导航定位方法，包括：
所述仿生复眼惯性***包括传感器采集***和***主板，所述传感器采集***包括复眼和惯性测量单元，所述***主板包括视觉处理模块、惯性测量单元数据处理模块和视觉惯性融合处理模块，所述传感器采集***用于采集位姿信息，所述***主板用于处理采集的位姿信息，以获得预设的位姿信息；
所述复眼和所述惯性测量单元联合标定，建立仿生复眼惯性***标校模型，对所述仿生复眼惯性***的时间偏移与坐标变换矩阵进行最优估计；
使用复眼物象尺度还原技术，还原图像中物体的尺度；
根据逆深度优化技术，利用视觉与惯性测量单元的紧耦合方式，构建整体残差模型，根据基于视觉构造的重投影误差、基于惯性测量单元构造的残差项以及基于先验信息的因子，获得当所述仿生复眼惯性***的***误差最小时的逆深度和惯性测量单元优化速度。
可选的，所述复眼和所述惯性测量单元联合标定，建立仿生复眼惯性***标校模型，对所述仿生复眼惯性***的时间偏移与坐标变换矩阵进行最优估计的步骤包括：
设置图像物理坐标系的原点在图像像素坐标系中的坐标为(u0,v0)，所述图像物理坐标系与所述图像像素坐标系之间关系的表达式如下：
其中，dx和dy为像素在所述图像物理坐标系中的尺寸，u和v为像素在数字图像中的列数和行数；
将表达式(1)转换为齐次坐标和矩阵形式，表达式如下：
获得世界坐标系到复眼相机坐标系的转换关系，表达式如下：
其中，复眼的光心Oc为所述复眼相机坐标系的原点，所述复眼相机坐标系的Zc轴与复眼光轴重合，所述复眼相机坐标系的Zc轴与相机的成像平面垂直，所述复眼相机坐标系的Xc轴与所述图像物理坐标系的x轴平行，所述复眼相机坐标系的Yc轴与所述图像物理坐标系的y轴平行，OOc为所述复眼相机坐标系的原点与所述图像物理坐标系原点之间的距离，焦距f为复眼的透镜焦点到透镜中心的距离；
将上述变换过程转换为矩阵相乘的形式，表达式如下：
其中，所述世界坐标系的点P＝(X,Y,Z)通过刚体变换转换为所述复眼相机坐标系的点，所述复眼相机坐标系的点通过透视投影转换为所述相机的成像平面上的点p＝(x,y)，所述相机的成像平面上的点p＝(x,y)通过缩放和平移转换为所述图像像素坐标系的点p＝(μ,ν)；
获得所述复眼的内参数K，表达式如下：
设置Pc是P在所述复眼相机坐标系的坐标，Pc是P在所述世界坐标系的坐标，使用旋转矩阵R和平移向量t将Pc转换为Pw，表达式如下：
Pc＝RPw+t (6)
设置R是3×3的旋转矩阵，t是3×1的平移向量，获得表达式如下：
将所述旋转矩阵R和所述平移向量t带入表达式(7)，获得所述复眼的外参数，表达式如下：
利用张正友标定法对所述复眼进行标定，得到所述复眼的内参数、外参数以及畸变系数，利用A11an方差法对所述惯性测量单元进行标定，得到所述惯性测量单元的高斯噪声和随机游走；
将标定后的所述复眼和所述惯性测量单元进行固联，对所述仿生复眼惯性***进行联合标校。
可选的，所述使用复眼物象尺度还原技术，还原图像中物体的尺度的步骤包括：
对视觉信息和惯性信息进行联合初始化，获取尺度因子；
根据所述复眼和所述惯性测量单元分别采集数据的频率，对所述惯性测量单元进行预积分处理，获得位置、旋转和速度的变化量；
对于所述复眼的采集数据，根据运动恢复结构的预设操作求解滑动窗口内所有帧的位姿和所有路标点的3D位置；
使用所述惯性测量单元的预积分获得预设的两个关键帧时刻本体坐标系之间的相对旋转约束；
将所述运动恢复结构的结果与所述惯性测量单元的预积分的值进行对齐，实现对偏置的校正；
求解每一帧对应的速度和重力向量方向，恢复所述复眼的尺度因子；
还原复眼图像中物体的尺度，同时得到米制单位下的惯性测量单元速度。
可选的，所述根据逆深度优化技术，利用视觉与惯性测量单元的紧耦合方式，构建整体残差模型，根据基于视觉构造的重投影误差、基于惯性测量单元构造的残差项以及基于先验信息的因子，获得当所述仿生复眼惯性***的***误差最小时的逆深度和惯性测量单元优化速度的步骤包括：
根据所述惯性测量单元的预积分，得到所述惯性测量单元的预积分残差，表达式如下：
其中，δba、δbg表示所述仿生复眼惯性***的误差状态，所述误差状态为所述仿生复眼惯性***的估计值与真实值的差值；
计算所述惯性测量单元的预积分残差的目标函数的最小值，表达式如下：
其中，为误差项rB关于所有优化变量X的雅可比矩阵；
将表达式(10)展开，令关于增量ΔX的导数为0，获得增量ΔX的表达式如下：
其中，所述优化变量分别为两帧的位置、方向、速度、惯性测量单元高斯白噪声和惯性测量单元随机游走，表达式如下：
计算残差对应每个优化变量的偏导，获得雅可比矩阵的表达式如下：
根据表达式(11)、表达式(12)和表达式(13)通过非线性优化算法得到当误差最小时的状态量；
视觉测量残差为特征点的重投影误差，当第1个特征点被第i幅图像第一次观测到时，定义第j幅图像中特征观测的残差的表达式如下：
其中，是第i幅图像第一次观测到第1个特征，/>是在j幅图像中对同样特征的观测；
获得所述视觉测量残差的表达式如下：
其中，为误差项rC关于所有优化变量X的雅可比矩阵；
将表达式(15)展开，令关于增量ΔX的导数为0，获得增量ΔX的表达式如下：
其中，所述优化变量的表达式如下：
根据视觉残差公式计算相对于所述优化变量的雅可比矩阵，表达式如下：
视觉约束的噪声协方差与标定相机内参时的重投影误差，对应到归一化相机平面上的协方差矩阵除以焦距f，信息矩阵等于协方差矩阵的逆，表达式如下：
根据表达式(17)、表达式(18)和表达式(19)获得视觉残差最小化的优化变量；
所述仿生复眼惯性***优化的目标函数的表达式如下：
其中，三个残差项依次是先验信息、惯性测量单元测量残差、视觉观测残差，状态量X＝[x0，x1，...，xn，λ1，...λn]为n+1个所有相机的状态和m+1个3D点的逆深度；
表达式(20)的整体目标函数的整体增量方程的表达式如下：
其中，为所述惯性测量单元的噪声协方差，/>为所述惯性测量单元的信息矩阵；
将表达式(21)简化为：
(Λp+ΛB+ΛC)ΔX＝bp+bB+bC (22)
其中，Λp，ΛB和ΛC为黑塞矩阵；
计算整个目标函数对所有优化变量的偏导数，得到所述仿生复眼惯性***的雅可比矩阵，获得状态增量ΔX，状态量包括优化后的深度与惯性测量单元速度信息；
对所有状态量进行迭代更新，直到目标函数达到最小值，此时所述深度与所述惯性测量单元速度信息达到最优。
可选的，还包括：
O1和O2是复眼的光心，I1和I2为同一位置处物体在两帧图像中的特征点，通过三角测距直线O1I1和O2I2在场景中相交于点P；
将特征点I1和特征点I2进行归一化处理，特征点I1的坐标为X1，特征点I2的坐标为X2，满足如下表达式：
S1X2＝S1RX1+t (23)
其中，S1为特征点I1的深度，S2为特征点I2的深度，R为两帧之间的旋转矩阵，t为平移向量；
对表达式(23)进行求解，获得两帧图像中的特征点的深度；
根据所述仿生复眼惯性***初始化时确定的尺度信息确定目标点的空间坐标。
本发明具有下述有益效果：
本发明提供一种基于仿生复眼惯性***的导航定位方法，利用仿生复眼的惯性***，根据“动态基线+三角测距”的方法对目标物体实现高准确性和高精度的导航定位，使用“复眼+惯性测量单元”耦合的方法对视觉信息和惯性信息进行采集和处理，得到目标物体的高准确度的相对位姿变化，通过“动态基线+三角测距”的方法实现高精度的定姿定位。
附图说明
图1为本发明实施例一提供的基于仿生复眼惯性***的总体框架示意图。
图2为本发明实施例一提供的基于仿生复眼惯性***的算法设计框架示意图。
图3为本发明实施例一提供的坐标轴示意图。
图4为本发明实施例一提供的复眼三坐标轴位置关系示意图。
图5为本发明实施例一提供的小孔成像模型示意图。
图6为本发明实施例一提供的联合标定示意图。
图7为本发明实施例一提供的视觉、惯性测量单元数据频率和预积分示意图。
图8为本发明实施例一提供的尺度还原技术流程图。
图9为本发明实施例一提供的逆深度优化技术流程图。
图10为本发明实施例一提供的动态基线与三角测距的测量图。
具体实施方式
为使本领域的技术人员更好地理解本发明的技术方案，下面结合附图对本发明提供的基于仿生复眼惯性***的导航定位方法进行详细描述。
实施例一
传统视惯***采用相机作为图像采集传感器，由于视场比较小，对周围环境图像的采集范围比较小，局部类似环境可能造成***的误判断，进行闭环操作，影响导航的定位准确性。其次，若在市场中出现运动速度较快的物体，相机的快速响应性较差，采集的图像会出现模糊的情况，影响视惯***的定位精度。提高定位准确度和定位精度是视惯***普遍存在和亟待解决的问题。
仿生复眼成像模仿昆虫视神经运动感知机理，具备快速响应特性、高分辨率、大视场，较普通相机对环境图像采集范围更大，对运动物体姿态更为敏感，同时还具有体积小、重量轻的优点。因此，为实现高准确性和高精度的定位，本实施例提出一种基于仿生复眼惯性***的导航定位方法，利用仿生复眼的惯性***，根据“动态基线+三角测距”法对目标物体实现高准确性和高精度的导航定位，其中，复眼+惯性测量单元(Inertial MeasurementUnit，IMU)紧耦合的方法对视觉信息和惯性信息进行采集和处理，得到目标物体的准确度较高的相对位姿变化，“动态基线+三角测距”法实现高精度的定姿定位。
图1为本发明实施例一提供的基于仿生复眼惯性***的总体框架示意图。如图1所示，仿生复眼惯性***硬件部分由传感器采集***、***主板两部分组成。传感器采集***由复眼与IMU模块组成，通过电源提供的12v与5v电压进行供电。***主板分为三个部分，分别是视觉处理模块、IMU数据处理模块与视觉惯性融合处理模块，采集的数据经过***主板处理得到优化后的位姿信息。
图2为本发明实施例一提供的基于仿生复眼惯性***的算法设计框架示意图。如图2所示，仿生复眼惯性***算法设计分为三部分，第一部分为复眼和IMU联合标定，建立复眼和惯性***标校模型，对复眼和惯性***的时间偏移与坐标变换矩阵进行最优估计；第二部分为复眼物象尺度还原技术，实现图像中物体的尺度还原；第三部分为逆深度优化技术，利用视觉与IMU紧耦合方式，构建整体残差模型，得到使***误差最小化的逆深度。
本实施例提供复眼惯性***联合标定。为了恢复复眼物象尺度信息，需要对多孔径仿生复眼相机进行定位。通过对复眼相机运动前后的两张图像进行特征点匹配，就可以通过这些二维图像点的对应关系，恢复出。在两帧之间摄像机的运动，需要对复眼模型进行标定。复眼的标定主要涉及三个坐标系：图像坐标系、摄像机坐标系和世界坐标系。
图3为本发明实施例一提供的坐标轴示意图。如图3所示，图像坐标系是一个以像素为单位的坐标系，原点在左上方，每个像素点的位置通过像素单位表示，其中u和v表示像素在数字图像中的列、行数，此时没有用物理单位表示像素位置，因此需要建立以物理单位表示的图像坐标系，图像物理坐标系(x,y)，该坐标系以光轴与图像平面交点为原点，以毫米为单位，两个坐标轴分别与图像像素坐标系平行。
若图像物理坐标系的原点在图像像素坐标系中的坐标为(u0,v0)，每个像素在图像物理坐标系中的尺寸为dx和dy，则两个坐标系的关系为：
化为齐次坐标和矩阵形式：
复眼坐标系是以复眼的光心为原点Oc，Zc轴与光轴重合，与成像平面垂直，Xc和Yc轴与图像坐标系的两个坐标轴平行。OOc为复眼的焦距，即相机坐标系的原点与图像物理坐标系原点之间的距离。
世界坐标系是由用户定义的三维坐标系，用于描述三维空间中的物体和相机的位置。三者可用图4表示，图4为本发明实施例一提供的复眼三坐标轴位置关系示意图。如图4所示，Oc点为复眼光心，Xc轴和Yc轴与图像的X轴与Y轴平行，Zc轴为复眼光轴，它与图像平面垂直。光轴与图像平面的交点，即为图像坐标系的原点。由点Oc与Xc、Yc、Zc轴组成的直角坐标系称为相机坐标系，OOc为复眼相机焦距。
本实施例提供的复眼模型中，复眼的镜头是一组透镜，当平行于主光轴的光线穿过透镜时，会聚到一点上，这个点叫做焦点，焦点到透镜中心的距离叫做焦距f。数码相机的镜头相当于一个凸透镜，感光元件就处在这个凸透镜的焦点附近，将焦距近似为凸透镜中心到感光元件的距离时就成为小孔成像模型。小孔成像模型如图5所示。
经过上述坐标系的转换可以得到如下世界坐标系到复眼相机坐标系的转换：
根据复眼模型设计多孔径复眼，利用张正友标定法对其进行标定，得到复眼的内、外参数以及畸变系数。
设P＝(X,Y,Z)为场景中的一点，在针孔相机模型中，其要经过以下几个变换，最终变为二维图像上的像点p＝(μ,v)：将从世界坐标系通过刚体变换变换到相机坐标系，这个变换过程使用的是相机间的相对位姿，也就是相机的外参数；从相机坐标系，通过透视投影变换到相机的成像平面上的像点p＝(x,y)；将像点从成像坐标系，通过缩放和平移变换到像素坐标系上点p＝(μ,v)。
复眼将场景中的三维点变换为图像中的二维点，也就是各个坐标系变换的组合，可将上面的变换过程整理为矩阵相乘的形式：
将矩阵K称为相机的内参数：
引进一个稳定不变坐标系：世界坐标系，该坐标系是绝对不变，同步定位与建图(Simultaneous Localization and Mapping，SLAM)中的视觉里程计就是求解相机在世界坐标系下的运动轨迹。
设Pc是P在复眼坐标系坐标，Pc是其在世界坐标系下的坐标，用旋转矩阵R和一个平移向量t，将Pc变换为Pw，表达式如下：
Pc＝RPv+t (6)
表达式(6)中，R是3×3的旋转矩阵，t是3×1的平移向量，表达式如下：
将旋转矩阵R和平移向量t带入，就推导得到表达式(8)的复眼外参数如下：
图6为本发明实施例一提供的联合标定示意图。如图6所示，得到复眼的内参和外参后，利用Allan方差法对IMU进行标定，得到IM的高斯噪声和随机游走，将标定好的复眼和IMU进行固联，固联成功后进行复眼惯性的***联合标校。标校主要确定复眼相机与IMU之间的空间位置变换以及两传感器的时间偏移。
现实世界中的物体通过仿生复眼在图像传感器上成像，要完成三维空间距离测量，首先要对图像上的物体尺度进行还原。借助视觉/惯性联合初始化，可以获取尺度因子，完成图像中物体的尺度还原，并获得初步优化后的IMU速度等参数。
图7为本发明实施例一提供的视觉、惯性测量单元数据频率和预积分示意图。如图7所示，在尺度还原之前，由于复眼和IMU分别按照一定频率采集数据，IMU的采集频率要远大于复眼频率，因此，在相邻两个关键帧之间会有很多个IMU测量数据。进行IMU预积分将这若干IMU数据进行积分，得到位置、旋转和速度的变化量以便进行优化。
图8为本发明实施例一提供的尺度还原技术流程图。如图8所示，***捕获的复眼数据首先通过运动恢复结构(Structure from Motion，SFM)中8点法、三角化、PnP等一系列操作求解滑动窗口内所有帧的位姿和所有路标点的3D位置，同时使用IMU预积分获得两个关键帧时刻本体坐标系之间的相对旋转约束。但因此时的IMU结果存在陀螺仪偏置，所以需要将SFM的结果与IMU预积分的值进行对齐，实现对偏置的校正，随后再求解每一帧对应的速度，重力向量方向，并恢复仿生复眼的尺度因子。从而还原复眼图像中物体的尺度并得到米制单位下的IMU速度。
图9为本发明实施例一提供的逆深度优化技术流程图。如图9所示，在实际运行过程中，为进一步减小***误差，提高定位精度，对***进行逆深度优化，利用视觉与IMU紧耦合方式，构建整体残差模型，将基于视觉构造的重投影误差、基于IMU构造的残差项和基于先验信息的因子联合优化，得到使***误差最小化的逆深度与IMU优化速度。
根据IMU预积分，得到IMU预积分残差，表达式如下：
其中，δba、δbg表示***的误差状态，误差状态由***估计值减去真实值所得，若要计算目标函数的最小值，需要优化变量有一个增量后，使目标函数值最小，对于IMU残差，可写成：
其中为误差项rB关于所有状态向量(即优化变量)X的Jacobian，将上式展开并令关于ΔX的导数为0，可得增量ΔX的计算公式：
优化变量分别为两帧的位置、方向、速度、IMU高斯白噪声、IMU随机游走，表达式如下：
计算残差对应每个优化量的偏导，得到Jacobian，表达式如下：
联立表达式(11)、表达式(12)、表达式(13)通过非线性优化算法得到使误差最小的状态量。
视觉测量残差即特征点的重投影误差，第1个特征点被第i幅图像第一次观测到时，可定义第j幅图像中特征观测的残差，表达式如下：
指第i幅图像第一次观测到第1个特征，/>是在j幅图像中对同样特征的观测。因为视觉残差的自由度为2，因此将视觉残差投影到正切平面上。
对于视觉残差，可写成：
其中，为误差项rC关于所有状态向量(即优化变量)X的Jacobian，将上式展开并令关于ΔX的导数为0，可得增量ΔX的计算公式：
优化变量为表达式(17)如下：
根据视觉残差公式，计算相对于各优化变量的Jacobian，表达式如下：
视觉约束的噪声协方差与标定相机内参时的重投影误差，与偏离几个像素有关，取的1.5个像素，对应到归一化相机平面上的协方差矩阵需除以焦距f，则信息矩阵等于协方差矩阵的逆，表达式如下：
通过表达式(17)、表达式(18)、表达式(19)则可以寻找出式视觉残差最小化的优化变量。
本实施例获得***优化的目标函数，表达式如下：
其中，三个残差项依次是先验信息、IMU测量残差、视觉的观测残差，状态量X＝[x0，x1，...，xn，λ1，...λn]为n+1个所有相机的状态(包括位置、方向、速度、加速度计和陀螺仪的bias)和m+1个3D点的逆深度。
对于表达式(20)的整体目标函数的整体增量方程可写成：
其中，为IMU预积分噪声项的协方差。当IMU的噪声协方差/>越大时，其信息矩阵/>将越小，意味着该IMU观测越不可信，此时便会更加相信视觉观测。
可将表达式(21)继续简化为：
(Λp+ΛB+ΛC)ΔX＝bp+bB+bC (22)
其中，Λp，ΛB和ΛC为Hessian矩阵，计算整个目标函数对所有优化变量的偏导数，得到***的Jacobian，从而得到状态增量ΔX，状态量包含优化后的深度与IMU速度信息，对所有状态量进行迭代更新，直到目标函数达到最小值，即达到最优化的深度与IMU速度信息。
本实施例使用“动态基线+三角测距”法实现目标定位。三角测量最早应用于天文学和地理学的测量中，在本***中，复眼通过不同位置对同一路标点进行观察，得到观察到的位置出的相对位姿变化，在推断路标点的距离。
图10为本发明实施例一提供的动态基线与三角测距的测量图。如图10所示，首次观测位置为O1处，以此处的图像为参考帧，复眼运动到O2处时，存在一个变换矩阵T，同时O1、O2也是复眼的光心，I1、I2为同一位置处物体在两帧图像中的特征点，通过三角测距直线O1I1、O2I2在场景中会相交于点P。实际测量中由于噪声的影响不能完全相交，利用最小二乘法求解。
将两个特征点进行归一化处理，其坐标为X1和X2两帧之间的旋转矩阵R和平移向量t已知。那么满足如下表达式：
S1X2＝S1RX1+t (23)
其中，S1和S2为两个体征点的深度，对上式进行求解，即可求得两帧下的点的深度，根据复眼惯性***初始化时确定的尺度信息确定目标点的空间坐标。实现复眼惯性***对目标点的定姿定位。
实际测量中，我们可以根据需求控制观察点O1和观察点O2的距离，因此，观察点O1和观察点O2之间的连线为动态基线。参见图10，当复眼惯性***运动到O3处时，依然可以通过此方法进行位置求解，因此，运动的。每一帧都可作为起始帧来进行实时定位，测量得到相应距离后，丢掉之前的数据信息，确保测量的实时性和稳定性。
本实施例利用复眼大视场和快速响应的特性，结合惯性器件，提供一套低功耗的仿生复眼惯性导航定位***，复眼功耗仅为8W。本实施例通过动态基线和三角测距方法，获取了更精确的相对位置数据，有效提高了视惯***中的定位精度和导航效率，与常见“相机+IMU”***或单独视觉***相比有所改善。本实施例通过“复眼+惯性”紧耦合算法，对后端采集的信息进行非线性优化，比常见滤波方法更有效，更适用于实时的大型SLAM场景。本实施例将IMU方向信息和相机坐标系进行融合处理，能够直观获取图像坐标在惯性导航坐标系中的位置，简化了数据处理过程。
本实施例提供的视觉惯性导航定位的方案除了本专利的复眼+惯性方案外，还可以采用相机、GPS、激光雷达等定位方式。在逆深度优化部分，可以采用视觉信息和IMU信息进行松耦合处理，分别对视觉信息和IMU信息进行处理，不进行联合优化。在逆深度优化部分，优化策略有多种，不局限于非线性优化方式，本实施例提供的技术方案实现对数据的非线性闭环优化，在实时导航定位方面，有显著优点。
本实施例介绍的方法主要意义在于利用复眼替代视惯***中的相机，利用动态基线和三角测距改善视觉惯性导航***的定位方式，使得视觉惯性融合算法计算精度更高，得到更为精确的导航定位精度。本实施例将复眼采集的图像信息进采集，采集的图像信息同惯性信息进行融合，建立复眼和惯性***标校模型，对复眼和惯性***的时间偏移与坐标变换矩阵进行最优估计。在复眼和惯性模型中，用旋转矩阵和平移向量来表示两者的坐标变换，利用坐标变换获得复眼的外参数，实现复眼惯性的联合标定。复眼外参数标定方式如下：
设Pc是P在复眼相机坐标系坐标，Pc是其在世界坐标系下的坐标，可以使用一个旋转矩阵R和一个平移向量t，将Pc变换为Pw，表达式如下：
Pc＝RPw+t (6)
其中，R是3×3的旋转矩阵，t是3×1的平移向量，变换过程如下表达式：
将旋转矩阵R和平移向量t带入，就推导得到复眼外参数，表达式如下：
本实施例提出“动态基线+三角测距”法进行高精度的实时导航定位方法，对检测的目标进行跟踪和定位，改善了视觉惯性***在大市场SLAM中的实时导航定位功能。参见图10，首次观测位置为O1处，以此处的图像为参考帧，复眼运动到O2处时，存在一个变换矩阵T，同时O1、O2也是复眼的光心，I1、I2为同一位置处物体在两帧图像中的特征点，通过三角测距直线O1I1、O2I2在场景中会相交于点P。实际测量中由于噪声的影响不能完全相交，利用最小二乘法求解。
将两个特征点进行归一化处理，其坐标为X1和X2两帧之间的旋转矩阵R和平移向量t已知。那么满足如下表达式：
S1X2＝S1RX1+t (23)
其中，S1和S2为两个体征点的深度，对上式进行求解，即可求得两帧下的点的深度，根据复眼惯性***初始化时确定的尺度信息确定目标点的空间坐标。实现复眼惯性***对目标点的定姿定位。通过增加逆深度优化技术，在优化函数中加入先验信息，改善了复眼惯性***在定位中的尺度精度差的问题。
可以理解的是，以上实施方式仅仅是为了说明本发明的原理而采用的示例性实施方式，然而本发明并不局限于此。对于本领域内的普通技术人员而言，在不脱离本发明的精神和实质的情况下，可以做出各种变型和改进，这些变型和改进也视为本发明的保护范围。
Claims (2)
1.一种基于仿生复眼惯性***的导航定位方法，其特征在于，包括：
所述仿生复眼惯性***包括传感器采集***和***主板，所述传感器采集***包括复眼和惯性测量单元，所述***主板包括视觉处理模块、惯性测量单元数据处理模块和视觉惯性融合处理模块，所述传感器采集***用于采集位姿信息，所述***主板用于处理采集的位姿信息，以获得预设的位姿信息；
所述复眼和所述惯性测量单元联合标定，建立仿生复眼惯性***标校模型，对所述仿生复眼惯性***的时间偏移与坐标变换矩阵进行最优估计；
使用复眼物象尺度还原技术，还原图像中物体的尺度；
根据逆深度优化技术，利用视觉与惯性测量单元的紧耦合方式，构建整体残差模型，根据基于视觉构造的重投影误差、基于惯性测量单元构造的残差项以及基于先验信息的因子，获得当所述仿生复眼惯性***的***误差最小时的逆深度和惯性测量单元优化速度。
2.根据权利要求1所述的基于仿生复眼惯性***的导航定位方法，其特征在于，所述使用复眼物象尺度还原技术，还原图像中物体的尺度的步骤包括：
对视觉信息和惯性信息进行联合初始化，获取尺度因子；
根据所述复眼和所述惯性测量单元分别采集数据的频率，对所述惯性测量单元进行预积分处理，获得位置、旋转和速度的变化量；
对于所述复眼的采集数据，根据运动恢复结构的预设操作求解滑动窗口内所有帧的位姿和所有路标点的3D位置；
使用所述惯性测量单元的预积分获得预设的两个关键帧时刻本体坐标系之间的相对旋转约束；
将所述运动恢复结构的结果与所述惯性测量单元的预积分的值进行对齐，实现对偏置的校正；
求解每一帧对应的速度和重力向量方向，恢复所述复眼的尺度因子；
还原复眼图像中物体的尺度，同时得到米制单位下的惯性测量单元速度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210119005.0A CN114485648B (zh) | 2022-02-08 | 2022-02-08 | 一种基于仿生复眼惯性***的导航定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210119005.0A CN114485648B (zh) | 2022-02-08 | 2022-02-08 | 一种基于仿生复眼惯性***的导航定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114485648A CN114485648A (zh) | 2022-05-13 |
| CN114485648B true CN114485648B (zh) | 2024-02-02 |
Family
ID=81479124
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210119005.0A Active CN114485648B (zh) | 2022-02-08 | 2022-02-08 | 一种基于仿生复眼惯性***的导航定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114485648B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116681732B (zh) * | 2023-08-03 | 2023-10-20 | 南昌工程学院 | 一种基于复眼形态视觉的目标运动识别方法及*** |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109079799A (zh) * | 2018-10-23 | 2018-12-25 | 哈尔滨工业大学（深圳） | 一种基于仿生的机器人感知控制***及控制方法 |
| CN110375738A (zh) * | 2019-06-21 | 2019-10-25 | 西安电子科技大学 | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 |
| CN110887473A (zh) * | 2019-12-09 | 2020-03-17 | 北京航空航天大学 | 一种基于偏振度加权的仿生偏振自主组合导航方法 |
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN111307139A (zh) * | 2019-12-09 | 2020-06-19 | 北京航空航天大学 | 一种基于偏振/天文信息融合的航向与姿态确定方法 |
| CN112697138A (zh) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | 一种基于因子图优化的仿生偏振同步定位与构图的方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20190301871A1 (en) * | 2018-03-27 | 2019-10-03 | Artisense Corporation | Direct Sparse Visual-Inertial Odometry Using Dynamic Marginalization |
| CN109085845B (zh) * | 2018-07-31 | 2020-08-11 | 北京航空航天大学 | 一种自主空中加油对接仿生视觉导航控制***及方法 |
-
2022
- 2022-02-08 CN CN202210119005.0A patent/CN114485648B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109079799A (zh) * | 2018-10-23 | 2018-12-25 | 哈尔滨工业大学（深圳） | 一种基于仿生的机器人感知控制***及控制方法 |
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN110375738A (zh) * | 2019-06-21 | 2019-10-25 | 西安电子科技大学 | 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 |
| CN110887473A (zh) * | 2019-12-09 | 2020-03-17 | 北京航空航天大学 | 一种基于偏振度加权的仿生偏振自主组合导航方法 |
| CN111307139A (zh) * | 2019-12-09 | 2020-06-19 | 北京航空航天大学 | 一种基于偏振/天文信息融合的航向与姿态确定方法 |
| CN112697138A (zh) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | 一种基于因子图优化的仿生偏振同步定位与构图的方法 |
Non-Patent Citations (2)
| Title |
|---|
| 仿生导航技术综述;胡小平;毛军;范晨;张礼廉;何晓峰;韩国良;范颖;;导航定位与授时(第04期);全文 * |
| 基于微惯性/偏振视觉的组合定向方法;范晨;何晓峰;范颖;胡小平;张礼廉;于化鹏;;中国惯性技术学报(第02期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114485648A (zh) | 2022-05-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108717712B (zh) | 一种基于地平面假设的视觉惯导slam方法 | |
| Heng et al. | Project autovision: Localization and 3d scene perception for an autonomous vehicle with a multi-camera system | |
| CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
| CN109993113B (zh) | 一种基于rgb-d和imu信息融合的位姿估计方法 | |
| CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
| CN112669354B (zh) | 一种基于车辆非完整约束的多相机运动状态估计方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| WO2024045632A1 (zh) | 基于双目视觉和imu的水下场景三维重建方法及设备 | |
| CN111932674A (zh) | 一种线激光视觉惯性***的优化方法 | |
| JP7502440B2 (ja) | 環境のトポグラフィを測定するための方法 | |
| CN112781582A (zh) | 卫星弱观测条件下的多传感器融合高精度位姿估计算法 | |
| CN115371673A (zh) | 一种未知环境中基于Bundle Adjustment的双目相机目标定位方法 | |
| CN112444245A (zh) | 一种基于偏振光、光流矢量、双目视觉传感器的仿昆虫视觉组合导航方法 | |
| CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
| CN114485648B (zh) | 一种基于仿生复眼惯性***的导航定位方法 | |
| CN113450334B (zh) | 一种水上目标检测方法、电子设备及存储介质 | |
| CN113345032B (zh) | 一种基于广角相机大畸变图的初始化建图方法及*** | |
| CN116182855B (zh) | 一种弱光强环境下仿复眼偏振视觉无人机组合导航方法 | |
| CN117434294A (zh) | 一种用于无人机的多孔径纯视觉光流测速方法 | |
| Aminzadeh et al. | Implementation and performance evaluation of optical flow navigation system under specific conditions for a flying robot | |
| CN116500595A (zh) | 激光雷达、相机和惯性传感器相互约束的外参标定方法 | |
| Donnelly | Semi-dense stereo reconstruction from aerial imagery for improved obstacle detection | |
| CN116597106A (zh) | 一种空对地大场景稀疏重建与目标定位方法 | |
| Autera | Study and development of calibration algorithms of multi-camera systems for precision agriculture robotics | |
| CN116400333A (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 |