CN116337048B - 一种无人平台的偏振视觉惯性同步建图与定位方法和装置 - Google Patents
一种无人平台的偏振视觉惯性同步建图与定位方法和装置 Download PDFInfo
- Publication number
- CN116337048B CN116337048B CN202310623208.8A CN202310623208A CN116337048B CN 116337048 B CN116337048 B CN 116337048B CN 202310623208 A CN202310623208 A CN 202310623208A CN 116337048 B CN116337048 B CN 116337048B
- Authority
- CN
- China
- Prior art keywords
- information
- key frame
- autonomous
- map
- navigation
- 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
- 238000000034 method Methods 0.000 title claims abstract description 68
- 230000000007 visual effect Effects 0.000 title claims abstract description 47
- 238000013507 mapping Methods 0.000 title claims abstract description 12
- 230000010287 polarization Effects 0.000 title claims description 10
- 230000001360 synchronised effect Effects 0.000 title claims description 10
- 238000006243 chemical reaction Methods 0.000 claims abstract description 29
- 238000007476 Maximum Likelihood Methods 0.000 claims description 15
- 238000012937 correction Methods 0.000 claims description 8
- 238000009826 distribution Methods 0.000 claims description 5
- 238000000605 extraction Methods 0.000 claims description 5
- 230000004927 fusion Effects 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 238000005457 optimization Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims 1
- 230000036544 posture Effects 0.000 description 11
- 238000005259 measurement Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000010365 information processing Effects 0.000 description 2
- 238000011835 investigation Methods 0.000 description 2
- 239000011664 nicotinic acid Substances 0.000 description 2
- 238000003860 storage Methods 0.000 description 2
- 238000004590 computer program Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000001960 triggered 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- 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
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
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)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发公开了一种无人平台的偏振视觉惯性同步建图与定位方法和装置，所述方法包括：获取载体的惯性、视觉组合导航***中关键帧的第一导航信息。根据卫星的位置信息和偏振光的航向信息，利用因子图优化关键帧对应的第一位置和第一姿态，生成载体的第二导航信息。根据当前关键帧的第二导航信息以及当前关键帧与上一关键帧的位姿转换关系，构建任务区域地图。脱离卫星信息情况下，通过自主关键帧的特征信息在任务区域地图中进行特征匹配，得到地图匹配帧。解析地图残差、航向信息的观测残差、惯性残差以及视觉残差的组合信息，得到自主导航信息。本发明通过构建高精度任务区域地图，在卫星拒止条件下提高载体自主定位精度。
Description
技术领域
本发明涉及仿生偏振光导航领域，特别是涉及一种无人平台的偏振视觉惯性同步建图与定位方法和装置。
背景技术
随着导航技术的发展，成本较低的仿生偏振光传感器具备高精度、结构简单等优势，广泛应用于无人平台的应急救援、侦查和检测以及工业生产等多个领域，对于无人平台在执行救援、侦查、导航等任务的时候，通过惯性、视觉组合导航***时时获取无人平台的位置信息和姿态信息，现有技术中通过增加偏振光传感器的航向信息和卫星导航位置信息，辅助无人平台避障、路径规划等功能，提高无人平台飞行中的定位精度，但是卫星导航本身容易受到信号干扰、多径效应和复杂环境遮挡等情况，导致卫星导航位置信息存在丢失或者错误的情况，使得无人平台在自主导航时候，随着时间累积，惯性、视觉组合导航***的误差增大，仅靠偏振光的航向信息无法满足导航精度需求。
发明内容
针对目前卫星导航不可靠的情况下，本发明提出一种无人平台的偏振视觉惯性同步建图与定位方法和装置。
一种无人平台的偏振视觉惯性同步建图与定位方法，所述方法包括：
获取载体的惯性、视觉组合导航***中关键帧的第一导航信息。第一导航信息包括：第一位置、第一姿态以及第一特征信息。
根据卫星的位置信息和偏振光的航向信息，利用因子图优化关键帧对应的第一位置和第一姿态，生成载体的第二导航信息。第二导航信息包括：第二位置、第二姿态以及第二特征信息。
根据当前关键帧的第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系、当前关键帧与上一关键帧的姿态旋转关系以及第二特征信息，构建任务区域地图。
在脱离卫星导航信息的自主飞行阶段，通过自主关键帧的特征信息在任务区域地图中进行特征匹配，得到地图匹配帧。
根据地图匹配帧的第二姿态与自主关键帧的姿态的位姿转化关系，得到地图残差。将地图残差、自主关键帧对应的偏振光航向信息的观测残差、惯性残差以及视觉残差的组合信息进行解析，采用极大似然估计法和最小二乘法求解得到自主导航信息。
在其中一个实施例中，还包括：接收卫星信号得到位置信息，以及通过偏振光传感器获取航向信息。将关键帧作为因子图的节点，采集卫星的位置信息和偏振光的航向信息，校正节点的第一位置和第一姿态，并更新节点的特征信息，得到关键帧的第二位置、第二姿态以及第二特征信息，按照载体飞行时长确定关键帧总数，根据关键帧总数生成载体的第二导航信息。
在其中一个实施例中，还包括：第二特征信息作为任务区域地图的索引元素，通过索引元素、第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系以及当前关键帧与上一关键帧的姿态旋转关系组成当前关键帧的特征向量，根据特征向量构建任务区域地图。
在其中一个实施例中，还包括：载体脱离卫星位置信息导航的自主飞行阶段，根据第二特征信息构建任务区域地图中关键帧的索引，通过索引查找任务区域地图中关键帧的特征信息，将自主关键帧的特征信息与任务区域地图中关键帧的特征信息进行匹配，获取特征信息匹配点达阈值对应的任务区域地图中的匹配帧。
在其中一个实施例中，还包括：提取自主关键帧的位置和姿态，将自主关键帧的滑动窗口与地图匹配帧的滑动窗口对齐，通过地图匹配帧的位置转换关系和姿态旋转关系，得到自主关键帧的地图估计值，将地图匹配帧的第二位置和第二姿态作为校正值，构建地图估计值与所述校正值的重投影误差，得到地图匹配的残差。地图估计值包括：地图估计位置和地图估计姿态。
在其中一个实施例中，还包括：根据当前时刻北东地坐标中传感器与北向的夹角，获取自主关键帧对应的偏振光航向信息，并与自主关键帧的第二导航信息进行误差求和，得到自主关键帧对应的偏振光航向信息的观测残差。根据自主关键帧的惯性漂移与相邻图像帧对应的惯性漂移之间的误差进行求和，得到惯性残差。根据自主关键帧的特征信息在视觉传感器采集的图像中的重投影误差进行求和，得到视觉残差。
在其中一个实施例中，还包括：通过极大似然估计法迭代求解自主关键帧对应的北东地坐标系下的位置和姿态，利用最小二乘法解算所述自主关键帧的第二导航信息之间的误差分布，得到自主导航信息。
在其中一个实施例中，还包括：构建因子图优化惯性、视觉组合导航***中各个关键帧的边缘信息，得到每一个关键帧在世界坐标系中的位置信息、速度信息、姿态信息以及惯性传感器的漂移信息，根据关键帧总数和特征总数，提取惯性、视觉组合导航***中关键帧的第一位置、第一姿态以及第一特征信息，并组合成第一导航信息。
一种无人平台的偏振视觉惯性同步建图与定位装置，所述装置包括：
导航信息提取模块，用于获取载体的惯性、视觉组合导航***中关键帧的第一导航信息。第一导航信息包括：第一位置、第一姿态以及第一特征信息。
信息融合模块，用于根据卫星的位置信息和偏振光的航向信息，利用因子图优化关键帧对应的第一位置和第一姿态，生成载体的第二导航信息。第二导航信息包括：第二位置、第二姿态以及第二特征信息。
构建地图模块，用于根据当前关键帧的第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系、当前关键帧与上一关键帧的姿态旋转关系以及第二特征信息，构建任务区域地图。
自主定位模块，用于在脱离卫星导航信息的自主飞行阶段，通过自主关键帧的特征信息在任务区域地图中进行特征匹配，得到地图匹配帧。根据地图匹配帧的第二姿态与自主关键帧的姿态的位姿转化关系，得到地图残差。将地图残差、自主关键帧对应的偏振光航向信息的观测残差、惯性残差以及视觉残差的组合信息进行解析，采用极大似然估计法和最小二乘法求解得到自主导航信息。
相较于现有技术，本发明具备以下技术效果：
利用关键帧信息决定载体的飞行位姿和图像的特征信息，提取惯性、视觉组合导航***中关键帧的信息，保留影响载体飞行位姿的关键帧信息作为第一导航信息，在卫星可用的情况下，通过构建载体导航信息处理的因子图，将偏振光的航向信息和卫星导航的位置信息融合到第一导航信息中，校对每一个关键帧对应的位置信息、姿态信息和特征信息，可以进一步提高关键帧的第一导航信息的精度，得到关键帧的第二导航信息，进而通过关键帧之间的位姿转换关系和姿态旋转关系，制约每个关键帧对应的载***置信息和姿态信息，基于此制约关系和当前关键帧的位姿信息构建任务区域地图，可以获得高精度定位信息。在卫星拒止条件下，载体自主飞行阶段，可以通过偏振光的航向信息与任务区域地图的位置信息对自主关键帧的导航信息进行优化，由惯性残差、视觉残差以及航向观测残差构成自主关键帧的导航信息解算方程，在此方程中增加地图匹配帧的残差的求解，从而提高导航***的定位精度，采取极大似然估计法和最小二乘法进行求解，可以实现载体自主定位的同时，克服了惯性、视觉组合导航***随时间累积的误差，保障了自主飞行过程中导航定位的精度和可靠性。
附图说明
图1为一个实施例中一种无人平台的偏振视觉惯性同步建图与定位方法的流程示意图；
图2为一个实施例中多一种无人平台的偏振视觉惯性同步建图与定位装置的结构框图；
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
本申请提供的一种无人平台的偏振视觉惯性同步建图与定位方法，参阅图1所示，具体包括以下步骤：
步骤102，获取载体的惯性、视觉组合导航***中关键帧的第一导航信息。第一导航信息包括：第一位置、第一姿态以及第一特征信息。
载体可以是空中飞行器、无人机等，载体上安装有惯性传感器、视觉传感器、偏振光传感器以及卫星接收机，将惯性传感器定位***与视觉传感***组合成载体的导航***，由视觉传感器拍摄载体飞行过程中每个时刻的图片，得到每张图片对应的图像特征信息和惯性传感器的状态信息，提取关键帧图片对应的图像特征信息和惯性传感器的状态信息，得到第一导航信息。具体的，惯性传感器状态信息包括载体在世界坐标系中的位置信息、速度信息、姿态信息以及惯性传感器的漂移，另外，图像特征信息包括关键帧的描述符、图像特征点以及图像特征点的深度，通过提取惯性传感器状态信息中的位置信息和姿态信息、以及图像特征信息，从而组成第一导航信息。
步骤104，根据卫星的位置信息和偏振光的航向信息，利用因子图优化关键帧对应的第一位置和第一姿态，生成载体的第二导航信息。第二导航信息包括：第二位置、第二姿态以及第二特征信息。
将惯性、视觉组合导航***中的关键帧作为因子图节点，节点之间由惯性传感器信息和视觉传感器信息组成边缘信息，载体接收卫星信号得到位置信息时，触发偏振光传感器捕获载体的航向信息，使得位置信息和航向信息可以同时被关键帧采集到，对于因子图内同一个节点而言，需要融合多传感器内部信息，与此同时还需要融合多传感器信息以外的卫星的位置信息，以此提高关键帧信息的精度。
具体的，将卫星的位置信息作为第一导航信息的先验信息，用于校对第一位置得到第二位置，将偏振光传感器的航向信息作为第一导航信息中新的观测量，用于校对第一姿态得到第二姿态，根据第二位置和第二姿态更新第一特征信息，进而得到第二特征信息，从载体计算机中提取关键帧的第二位置、第二姿态以及第二特征信息，构成第二导航信息。
步骤106，据当前关键帧的第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系、当前关键帧与上一关键帧的姿态旋转关系以及第二特征信息，构建任务区域地图。
第二导航信息融合了卫星信息和多传感器信息，通过获取第二导航信息对应的关键帧之间的位姿转化关系，将第二特征信息作为当前关键帧的索引元素，保存当前关键帧的位置信息、姿态信息、关键帧之间的位姿转化关系以及索引元素，根据视觉传感器捕捉到的关键帧对应照片的总数，形成任务区域地图。
步骤108，在脱离卫星导航信息的自主飞行阶段，通过自主关键帧的特征信息在任务区域地图中进行特征匹配，得到地图匹配帧。
在载体自主飞行阶段，遇到卫星信号受干扰时，在视觉传感器、惯性传感器以及偏振光传感器获取的信息的基础上，增加任务区域地图的位置信息作为新的先验位置信息，对自主导航过程中关键帧的位置信息和姿态信息进行校对。
具体的，通过视觉传感器拍摄的图片获取自主关键帧的特征信息，当前时刻自主关键帧的特征信息包括位置信息、姿态信息、速度信息、航向信息以及图像特征信息等，根据自主关键帧的特征信息，利用第二特征信息的索引，查找任务区域地图中关键帧的特征信息，使用DBoW2对关键帧进行闭环检测，从而得到图像特征信息匹配点达到阈值对应的任务区域地图的关键帧，并以此作为地图匹配帧，该地图匹配帧包括北东地坐标系下的位置、姿态以及匹配帧中的特征信息，用于辅助载体的自主定位。
步骤110，根据地图匹配帧的第二姿态与自主关键帧的姿态的位姿转化关系，得到地图残差。将地图残差、自主关键帧对应的偏振光航向信息的观测残差、惯性残差以及视觉残差的组合信息进行解析，采用极大似然估计法和最小二乘法求解得到自主导航信息。
将当前时刻自主关键帧的滑动窗口与地图匹配帧的滑动窗口进行对齐，使得自主关键帧与地图匹配帧处在同一时刻条件下，以地图匹配帧的第二位置和第二姿态为基准，将自主关键帧的位置信息和姿态信息，通过地图匹配帧中的位置转换关系和姿态旋转关系，生成自主关键帧的地图估计位置信息和地图估计姿态信息，进而与地图匹配帧的第二位置和第二姿态构成地图残差，将地图残差、自主关键帧对应的偏振光航向信息的观测残差、惯性残差以及视觉残差的组合信息进行解析，采用极大似然估计法进行迭代运算，利用ceres求解最小二乘法，得到载体自主导航阶段时时的定位信息。
基于以上步骤可见，利用关键帧信息决定载体的飞行位姿和图像的特征信息，提取惯性、视觉组合导航***中关键帧的信息，保留影响载体飞行位姿的关键帧信息作为第一导航信息，在卫星可用的情况下，通过构建载体导航信息处理的因子图，将偏振光的航向信息和卫星导航的位置信息融合到第一导航信息中，校对每一个关键帧对应的位置信息、姿态信息和特征信息，可以进一步提高关键帧的第一导航信息的精度，得到关键帧的第二导航信息，进而通过关键帧之间的位姿转换关系和姿态旋转关系，制约每个关键帧对应的载***置信息和姿态信息，基于此制约关系和当前关键帧的位姿信息构建任务区域地图，可以获得高精度定位信息。在卫星拒止条件下，载体自主飞行阶段，可以通过偏振光的航向信息与任务区域地图的位置信息对自主关键帧的导航信息进行优化，由惯性残差、视觉残差以及航向观测残差构成自主关键帧的导航信息解算方程，在此方程中增加地图匹配帧的残差的求解，从而提高导航***的定位精度，采取极大似然估计法和最小二乘法进行求解，可以实现载体自主定位的同时，克服了惯性、视觉组合导航***随时间累积的误差，保障了自主飞行过程中导航定位的精度和可靠性。
在其中一个实施例中，接收卫星信号得到位置信息，以及通过偏振光传感器获取航向信息。将关键帧作为因子图的节点，采集卫星的位置信息和偏振光的航向信息，校正节点的第一位置和第一姿态，并更新节点的特征信息，得到关键帧的第二位置、第二姿态以及第二特征信息，按照载体飞行时长确定关键帧总数，根据关键帧总数生成载体的第二导航信息。
值得说明的是，多传感器信息和卫星信息通过因子图结构，构建载体状态向量求解方程，表示为：
（1）
其中为边缘化的先验信息的残差项，/>是惯性测量单元的测量残差项，/>为视觉的残差项，/>为huber范数，/>是惯性传感器在所有时间的测量结果，/>为当前滑动窗口中的特征集。
具体的，先验信息为位置信息，可以是通过卫星获取的，也可以是其他可以获取载***置信息的途径，将卫星的位置信息和偏振光传感器的航向信息组成观测集合，基于卫星的位置信息和偏振光传感器测量的方差服从高斯分布，即，可以获得t时刻对应关键帧的第二导航信息，其中，t时刻卫星测得的经纬高转为北东地坐标系（/>）下位置的测量结果为/>，因此卫星观测的载***置残差表示为：
（2）
（3）
另外，偏振光传感器测量得到的载体航向信息转化为北东地坐标中传感器与北向的夹角，输出航向信息的测量结果为，因此偏振光传感器测量的载体航向残差表示为：
（4）
将公式（2）和公式（4）并入公式（1）中，用卫星测量的位置残差替换先验信息残差，增加航向残差，通过ceres求解公式（1）得到载体高精度的状态向量，即t时刻下关键帧的第二导航信息。由此可见，融合偏振光传感器提供的航向信息和卫星提供的位置信息，校正惯性、视觉紧组合***输出的导航信息，可以提高多传感器组合导航***的定位精度。
在其中一个实施例中，第二特征信息作为任务区域地图的索引元素，通过索引元素、第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系以及当前关键帧与上一关键帧的姿态旋转关系组成当前关键帧的特征向量，根据特征向量构建任务区域地图。
值得说明的是，第二特征信息包括图像信息、像素坐标以及特征描述子等，保留第二特征信息的像素坐标和特征描述子，通过关键帧对应的特征描述子作为图像帧之间的特征信息匹配字符，利用像素坐标作为闭环检测的索引帧号，结合当前关键帧与上一关键帧之间的位置转换关系和姿态旋转关系，唯一确定当前关键帧的特征向量，按照视觉传感器拍摄的关键帧总数，组合成任务区域地图。
由此可见，融合了多传感器信息、卫星的位置信息、以及关键帧之间的转化关系，生成的任务区域地图中关键帧的特征向量，相较于传统的偏振光/惯性/视觉组合导航信息而言精度更高，在保留第二导航信息的同时提取关键帧的描述符，利用关键帧的描述符的短字符优势，提高整体导航***的运算效率。
在其中一个实施例中，载体脱离卫星位置信息导航的自主飞行阶段，根据第二特征信息构建任务区域地图中关键帧的索引，通过索引查找任务区域地图中关键帧的特征信息，将自主关键帧的特征信息与任务区域地图中关键帧的特征信息进行匹配，获取特征信息匹配点达阈值对应的任务区域地图中的匹配帧。
值得说明的是，地图的匹配帧包括北东地坐标系下的位置信息、姿态信息以及关键帧的描述符，特征匹配通过自主关键帧中特征信息的特征描述子对任务区域地图中关键帧进行查找，找到特征信息匹配点达到阈值的描述子对应的任务区域地图中的关键帧，定义此任务区域地图中的关键帧为地图匹配帧。由此可见，传统基于视觉的算法在由于障碍物遮挡、运动模糊以及过曝等原因导致特征跟踪丢失后，无法继续进行定位解算的问题，本申请通过引入任务区域地图，利用任务区域地图中关键帧的特征信息，可以满足在特征丢失后可以与任务区域地图进行特征匹配，以实现载体继续导航。
在其中一个实施例中，提取自主关键帧的位置和姿态，将自主关键帧的滑动窗口与地图匹配帧的滑动窗口对齐，通过地图匹配帧的位置转换关系和姿态旋转关系，得到自主关键帧的地图估计值，将地图匹配帧的第二位置和第二姿态作为校正值，构建地图估计值与所述校正值的重投影误差，得到地图匹配的残差。地图估计值包括：地图估计位置和地图估计姿态。
值得说明的是，地图匹配的残差可以表示为：
（5）
由此可见，利用地图匹配的残差克服了惯性/视觉组合导航***的误差随时间累计的缺点，在脱离卫星观测的位置信息时，通过偏振光传感器的航向残差与地图匹配残差分别对第二导航信息的姿态和位置进行校对，满足载体自主定位的同时，提高了基于视觉的组合导航***的环境适应性，可以有效的提高导航***的精度。
在其中一个实施例中，根据当前时刻北东地坐标中传感器与北向的夹角，获取自主关键帧对应的偏振光航向信息，并与自主关键帧的第二导航信息进行误差求和，得到自主关键帧对应的偏振光航向信息的观测残差。根据自主关键帧的惯性漂移与相邻图像帧对应的惯性漂移之间的误差进行求和，得到惯性残差。根据自主关键帧的特征信息在视觉传感器采集的图像中的重投影误差进行求和，得到视觉残差。
值得说明的是，考虑到计算量和精度，选择了基于滑动窗口的VIO紧耦合算法，该算法可以获得载波的位置、方向、速度和惯性测量单元的漂移等信息，对应完整的载体状态向量表示为：
；
其中，当视觉传感器获取第张图片时，/>是惯性传感器在当前/>时刻的状态，载体世界坐标系/>中的位置为/>、速度为/>、姿态为/>，/>为加速度计零偏，/>为陀螺仪的零偏，/>为采集第k张图片时的惯性测试单元坐标系，/>是要优化的状态总数，即关键帧总数。/>是滑动窗口中的特征总数，/>是相对于第一次观测到的第/>个特征的逆深度。
在其中一个实施例中，通过极大似然估计法迭代求解自主关键帧对应的北东地坐标系下的位置和姿态，利用最小二乘法解算所述自主关键帧的第二导航信息之间的误差分布，得到自主导航信息。
值得说明的是，公式（1）为非线性最小二乘法的载体状态向量的求解方程，因此采用极大似然估计法获得最大后验估计，状态向量可以表示为：
；
；
其中，和/>分别为第/>次迭代求解时北东地坐标系（/>）下的关键帧的位置和姿态，故对于观测量和状态量的求解公式表示为：
（6）
其中，和/>分别代表观测量和状态量，由此可见，将公式（6）和公式（4）加入公式（1）中，利用ceres联合优化求解最小二乘法问题，得到载体的高精度自主定位信息，因此，在无卫星条件下，仅依靠偏振光/视觉/惯性传感器与建立的任务区域地图进行组合优化，可以得到高精度载体的位置和姿态信息。
在其中一个实施例中，构建因子图优化惯性、视觉组合导航***中各个关键帧的边缘信息，得到每一个关键帧在世界坐标系中的位置信息、速度信息、姿态信息以及惯性传感器的漂移信息，根据关键帧总数和特征总数，提取惯性、视觉组合导航***中关键帧的第一位置、第一姿态以及第一特征信息，并组合成第一导航信息。
应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在其中一个实施例中，如图2所示，提供了一种无人平台的偏振视觉惯性同步建图与定位装置，包括：导航信息提取模块202、信息融合模块204、构建地图模块206以及自主定位模块208，其中：
导航信息提取模块202，用于获取载体的惯性、视觉组合导航***中关键帧的第一导航信息。第一导航信息包括：第一位置、第一姿态以及第一特征信息。
信息融合模块204，用于根据卫星的位置信息和偏振光的航向信息，利用因子图优化关键帧对应的第一位置和第一姿态，生成载体的第二导航信息。第二导航信息包括：第二位置、第二姿态以及第二特征信息。
构建地图模块206，用于根据当前关键帧的第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系、当前关键帧与上一关键帧的姿态旋转关系以及第二特征信息，构建任务区域地图。
自主定位模块208，用于在脱离卫星导航信息的自主飞行阶段，通过自主关键帧的特征信息在任务区域地图中进行特征匹配，得到地图匹配帧。根据地图匹配帧的第二姿态与自主关键帧的姿态的位姿转化关系，得到地图残差。将地图残差、自主关键帧对应的偏振光航向信息的观测残差、惯性残差以及视觉残差的组合信息进行解析，采用极大似然估计法和最小二乘法求解得到自主导航信息。
在一个实施例中，构建地图模块206，还用于第二特征信息作为任务区域地图的索引元素，通过索引元素、第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系以及当前关键帧与上一关键帧的姿态旋转关系组成当前关键帧的特征向量，根据特征向量构建任务区域地图。
在一个实施例中，信息融合模块204，还用于通过接收卫星信号得到位置信息，以及通过偏振光传感器获取航向信息。将关键帧作为因子图的节点，采集卫星的位置信息和偏振光的航向信息，校正节点的第一位置和第一姿态，并更新节点的特征信息，得到关键帧的第二位置、第二姿态以及第二特征信息，按照载体飞行时长确定关键帧总数，根据关键帧总数生成载体的第二导航信息。
在一个实施例中，自主定位模块208，还用于通过载体脱离卫星位置信息导航的自主飞行阶段，根据第二特征信息构建任务区域地图中关键帧的索引，通过索引查找任务区域地图中关键帧的特征信息，将自主关键帧的特征信息与任务区域地图中关键帧的特征信息进行匹配，获取特征信息匹配点达阈值对应的任务区域地图中的匹配帧。
在一个实施例中，自主定位模块208，还用于通过提取自主关键帧的位置和姿态，将自主关键帧的滑动窗口与地图匹配帧的滑动窗口对齐，通过地图匹配帧的位置转换关系和姿态旋转关系，得到自主关键帧的地图估计值，将地图匹配帧的第二位置和第二姿态作为校正值，构建地图估计值与所述校正值的重投影误差，得到地图匹配的残差。地图估计值包括：地图估计位置和地图估计姿态。
在一个实施例中，自主定位模块208，还用于通过根据当前时刻北东地坐标中传感器与北向的夹角，获取自主关键帧对应的偏振光航向信息，并与自主关键帧的第二导航信息进行误差求和，得到自主关键帧对应的偏振光航向信息的观测残差。根据自主关键帧的惯性漂移与相邻图像帧对应的惯性漂移之间的误差进行求和，得到惯性残差。根据自主关键帧的特征信息在视觉传感器采集的图像中的重投影误差进行求和，得到视觉残差。
在一个实施例中，自主定位模块208，还用于通过极大似然估计法迭代求解自主关键帧对应的北东地坐标系下的位置和姿态，利用最小二乘法解算所述自主关键帧的第二导航信息之间的误差分布，得到自主导航信息。
在一个实施例中，导航信息提取模块202，还用于通过构建因子图优化惯性、视觉组合导航***中各个关键帧的边缘信息，得到每一个关键帧在世界坐标系中的位置信息、速度信息、姿态信息以及惯性传感器的漂移信息，根据关键帧总数和特征总数，提取惯性、视觉组合导航***中关键帧的第一位置、第一姿态以及第一特征信息，并组合成第一导航信息。
关于一种无人平台的偏振视觉惯性同步建图与定位装置的具体限定可以参见上文中对于一种无人平台的偏振视觉惯性同步建图与定位方法的限定，在此不再赘述。上述一种无人平台的偏振视觉惯性同步建图与定位装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
本领域技术人员可以理解，图2中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请的保护范围应以所附权利要求为准。
Claims (10)
1.一种无人平台的偏振视觉惯性同步建图与定位方法，其特征在于，所述方法包括：
获取载体的惯性、视觉组合导航***中关键帧的第一导航信息；所述第一导航信息包括：第一位置、第一姿态以及第一特征信息；
根据卫星的位置信息和偏振光的航向信息，利用因子图优化所述关键帧对应的所述第一位置和所述第一姿态，生成载体的第二导航信息；所述第二导航信息包括：第二位置、第二姿态以及第二特征信息；
根据当前关键帧的第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系、当前关键帧与上一关键帧的姿态旋转关系以及所述第二特征信息，构建任务区域地图；
在脱离卫星导航信息的自主飞行阶段，通过自主关键帧的特征信息在所述任务区域地图中进行特征匹配，得到地图匹配帧；
根据所述地图匹配帧的所述第二姿态与自主关键帧的姿态的位姿转化关系，得到地图残差；
将所述地图残差、自主关键帧对应的偏振光航向信息的观测残差、惯性残差以及视觉残差的组合信息进行解析，采用极大似然估计法和最小二乘法求解得到自主导航信息。
2.根据权利要求1所述的方法，其特征在于，根据卫星的位置信息和偏振光的航向信息，利用因子图优化所述关键帧对应的所述第一位置和所述第一姿态，生成载体的第二导航信息，包括：
接收卫星信号得到位置信息，以及通过偏振光传感器获取航向信息；
将所述关键帧作为因子图的节点，采集卫星的位置信息和偏振光的航向信息，校正所述节点的第一位置和第一姿态，并更新所述节点的特征信息，得到所述关键帧的第二位置、第二姿态以及第二特征信息，按照载体飞行时长确定关键帧总数，根据所述关键帧总数生成载体的第二导航信息。
3.根据权利要求2所述的方法，其特征在于，根据当前关键帧的第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系、当前关键帧与上一关键帧的姿态旋转关系以及所述第二特征信息，构建任务区域地图，包括：
所述第二特征信息作为所述任务区域地图的索引元素，通过所述索引元素、第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系以及当前关键帧与上一关键帧的姿态旋转关系组成当前关键帧的特征向量，根据所述特征向量构建任务区域地图。
4.根据权利要求3所述的方法，其特征在于，在脱离卫星导航信息的自主飞行阶段，通过自主关键帧的特征信息在所述任务区域地图中进行特征匹配，得到地图匹配帧，包括：
载体脱离卫星位置信息导航的自主飞行阶段，根据所述第二特征信息构建所述任务区域地图中关键帧的索引，通过所述索引查找所述任务区域地图中关键帧的特征信息，将自主关键帧的特征信息与所述任务区域地图中关键帧的特征信息进行匹配，获取特征信息匹配点达阈值对应的所述任务区域地图中的匹配帧。
5.根据权利要求4所述的方法，其特征在于，根据所述地图匹配帧的所述第二姿态与自主关键帧的姿态的位姿转化关系，得到地图残差，包括：
提取自主关键帧的位置和姿态，将自主关键帧的滑动窗口与所述地图匹配帧的滑动窗口对齐，通过所述地图匹配帧的位置转换关系和姿态旋转关系，得到自主关键帧的地图估计值，将所述地图匹配帧的第二位置和第二姿态作为校正值，构建所述地图估计值与所述校正值的重投影误差，得到地图匹配的残差；
所述地图估计值包括：地图估计位置和地图估计姿态。
6.根据权利要求5所述的方法，其特征在于，在将所述地图残差、自主关键帧对应的偏振光航向信息的观测残差、惯性残差以及视觉残差的组合信息进行解析的步骤之前，包括：
根据当前时刻北东地坐标中传感器与北向的夹角，获取自主关键帧对应的偏振光航向信息，并与所述自主关键帧的第二导航信息进行误差求和，得到所述自主关键帧对应的偏振光航向信息的观测残差；
根据自主关键帧的惯性漂移与相邻图像帧对应的惯性漂移之间的误差进行求和，得到所述惯性残差；根据自主关键帧的特征信息在视觉传感器采集的图像中的重投影误差进行求和，得到所述视觉残差。
7.根据权利要求6所述的方法，其特征在于，采用极大似然估计法和最小二乘法求解得到自主导航信息，包括：
通过极大似然估计法迭代求解自主关键帧对应的北东地坐标系下的位置和姿态，利用最小二乘法解算所述自主关键帧的第二导航信息之间的误差分布，得到自主导航信息。
8.根据权利要求7所述的方法，其特征在于，获取载体的惯性、视觉组合导航***中关键帧的第一导航信息，包括：
构建因子图优化惯性、视觉组合导航***中各个关键帧的边缘信息，得到每一个关键帧在世界坐标系中的位置信息、速度信息、姿态信息以及惯性传感器的漂移信息，根据所述关键帧总数和特征总数，提取所述惯性、视觉组合导航***中关键帧的第一位置、第一姿态以及第一特征信息，并组合成第一导航信息。
9.一种无人平台的偏振视觉惯性同步建图与定位装置，其特征在于，所述装置包括：
导航信息提取模块，用于获取载体的惯性、视觉组合导航***中关键帧的第一导航信息；所述第一导航信息包括：第一位置、第一姿态以及第一特征信息；
信息融合模块，用于根据卫星的位置信息和偏振光的航向信息，利用因子图优化所述关键帧对应的所述第一位置和所述第一姿态，生成载体的第二导航信息；所述第二导航信息包括：第二位置、第二姿态以及第二特征信息；
构建地图模块，用于根据当前关键帧的第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系、当前关键帧与上一关键帧的姿态旋转关系以及所述第二特征信息，构建任务区域地图；
自主定位模块，用于在脱离卫星导航信息的自主飞行阶段，通过自主关键帧的特征信息在所述任务区域地图中进行特征匹配，得到地图匹配帧；根据所述地图匹配帧的所述第二姿态与自主关键帧的姿态的位姿转化关系，得到地图残差；将所述地图残差、自主关键帧对应的偏振光航向信息的观测残差、惯性残差以及视觉残差的组合信息进行解析，采用极大似然估计法和最小二乘法求解得到自主导航信息。
10.根据权利要求9所述的一种无人平台的偏振视觉惯性同步建图与定位装置，其特征在于，所述构建地图模块，还用于所述第二特征信息作为所述任务区域地图的索引元素，通过所述索引元素、第二位置、第二姿态、当前关键帧与上一关键帧的位置转换关系以及当前关键帧与上一关键帧的姿态旋转关系组成当前关键帧的特征向量，根据所述特征向量构建任务区域地图。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310623208.8A CN116337048B (zh) | 2023-05-30 | 2023-05-30 | 一种无人平台的偏振视觉惯性同步建图与定位方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310623208.8A CN116337048B (zh) | 2023-05-30 | 2023-05-30 | 一种无人平台的偏振视觉惯性同步建图与定位方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116337048A CN116337048A (zh) | 2023-06-27 |
| CN116337048B true CN116337048B (zh) | 2023-07-21 |
Family
ID=86876326
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310623208.8A Active CN116337048B (zh) | 2023-05-30 | 2023-05-30 | 一种无人平台的偏振视觉惯性同步建图与定位方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116337048B (zh) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2015126499A2 (en) * | 2013-12-02 | 2015-08-27 | Andrew Irish | Systems and methods for gnss snr probabilistic localization and 3-d mapping |
| CN109307508A (zh) * | 2018-08-29 | 2019-02-05 | 中国科学院合肥物质科学研究院 | 一种基于多关键帧的全景惯导slam方法 |
| CN112697138A (zh) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | 一种基于因子图优化的仿生偏振同步定位与构图的方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11313684B2 (en) * | 2016-03-28 | 2022-04-26 | Sri International | Collaborative navigation and mapping |
| US10295365B2 (en) * | 2016-07-29 | 2019-05-21 | Carnegie Mellon University | State estimation for aerial vehicles using multi-sensor fusion |
-
2023
- 2023-05-30 CN CN202310623208.8A patent/CN116337048B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2015126499A2 (en) * | 2013-12-02 | 2015-08-27 | Andrew Irish | Systems and methods for gnss snr probabilistic localization and 3-d mapping |
| CN109307508A (zh) * | 2018-08-29 | 2019-02-05 | 中国科学院合肥物质科学研究院 | 一种基于多关键帧的全景惯导slam方法 |
| CN112697138A (zh) * | 2020-12-07 | 2021-04-23 | 北方工业大学 | 一种基于因子图优化的仿生偏振同步定位与构图的方法 |
Non-Patent Citations (4)
| Title |
|---|
| An Integrated INS/Lidar Odometry/Polarized Camera Pose Estimation via Factor Graph Optimization for Sparse Environment;Tao Du et.al;IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT;第71卷;8501511 * |
| 偏振光辅助定姿在基于光流自主导航中的应用;黄显林 等;传感器与微***;第30卷(第1期);137-143 * |
| 基于动态关键帧的自然地标景象匹配视觉导航;李耀军 等;光电工程;第37卷(第9期);32-38 * |
| 基于微惯性/偏振视觉的组合定向方法;范晨 等;中国惯性技术学报;第28卷(第2期);231-236 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116337048A (zh) | 2023-06-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109974693B (zh) | 无人机定位方法、装置、计算机设备及存储介质 | |
| JP7326720B2 (ja) | 移動***置推定システムおよび移動***置推定方法 | |
| EP2503510B1 (en) | Wide baseline feature matching using collaborative navigation and digital terrain elevation data constraints | |
| CN110084832B (zh) | 相机位姿的纠正方法、装置、***、设备和存储介质 | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| CN111561923A (zh) | 基于多传感器融合的slam制图方法、*** | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN114623817B (zh) | 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法 | |
| KR101985344B1 (ko) | 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치 | |
| CN112577493B (zh) | 一种基于遥感地图辅助的无人机自主定位方法及*** | |
| CN114693754B (zh) | 一种基于单目视觉惯导融合的无人机自主定位方法与*** | |
| Samadzadegan et al. | Autonomous navigation of Unmanned Aerial Vehicles based on multi-sensor data fusion | |
| CN115127554B (zh) | 一种基于多源视觉辅助的无人机自主导航方法与*** | |
| JP2002532770A (ja) | 映像に関連してカメラポーズを決定する方法及びシステム | |
| WO2020135183A1 (zh) | 点云地图的构建方法、装置、计算机设备和存储介质 | |
| Hong et al. | Visual inertial odometry using coupled nonlinear optimization | |
| CN116337048B (zh) | 一种无人平台的偏振视觉惯性同步建图与定位方法和装置 | |
| Zhu et al. | Camera, LiDAR, and IMU based multi-sensor fusion SLAM: A survey | |
| US11481920B2 (en) | Information processing apparatus, server, movable object device, and information processing method | |
| US11037018B2 (en) | Navigation augmentation system and method | |
| Ren et al. | Adaptive sensor fusion of camera, GNSS and IMU for autonomous driving navigation | |
| Samadzadegan et al. | Vision-based pose estimation for autonomous outdoor navigation of aerial vehicles | |
| CN114842224A (zh) | 一种基于地理底图的单目无人机绝对视觉匹配定位方案 | |
| Saeedi et al. | Vision-aided inertial navigation for pose estimation of aerial vehicles | |
| He et al. | Foundloc: Vision-based onboard aerial localization in the wild |
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 |