CN111739063B - 一种基于多传感器融合的电力巡检机器人定位方法 - Google Patents

一种基于多传感器融合的电力巡检机器人定位方法 Download PDF

Info

Publication number
CN111739063B
CN111739063B CN202010581200.6A CN202010581200A CN111739063B CN 111739063 B CN111739063 B CN 111739063B CN 202010581200 A CN202010581200 A CN 202010581200A CN 111739063 B CN111739063 B CN 111739063B
Authority
CN
China
Prior art keywords
frame
coordinate system
imu
odometer
key
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
Application number
CN202010581200.6A
Other languages
English (en)
Other versions
CN111739063A (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.)
Zhengzhou University
Original Assignee
Zhengzhou University
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 Zhengzhou University filed Critical Zhengzhou University
Priority to CN202010581200.6A priority Critical patent/CN111739063B/zh
Publication of CN111739063A publication Critical patent/CN111739063A/zh
Application granted granted Critical
Publication of CN111739063B publication Critical patent/CN111739063B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • 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
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
    • G06Q50/00—Information and communication technology [ICT] specially adapted for implementation of business processes of specific business sectors, e.g. utilities or tourism
    • G06Q50/06—Energy or water supply
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/269—Analysis of motion using gradient-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G07—CHECKING-DEVICES
    • G07C—TIME OR ATTENDANCE REGISTERS; REGISTERING OR INDICATING THE WORKING OF MACHINES; GENERATING RANDOM NUMBERS; VOTING OR LOTTERY APPARATUS; ARRANGEMENTS, SYSTEMS OR APPARATUS FOR CHECKING NOT PROVIDED FOR ELSEWHERE
    • G07C1/00—Registering, indicating or recording the time of events or elapsed time, e.g. time-recorders for work people
    • G07C1/20—Checking timed patrols, e.g. of watchman
    • 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
    • Y04—INFORMATION OR COMMUNICATION TECHNOLOGIES HAVING AN IMPACT ON OTHER TECHNOLOGY AREAS
    • Y04S—SYSTEMS INTEGRATING TECHNOLOGIES RELATED TO POWER NETWORK OPERATION, COMMUNICATION OR INFORMATION TECHNOLOGIES FOR IMPROVING THE ELECTRICAL POWER GENERATION, TRANSMISSION, DISTRIBUTION, MANAGEMENT OR USAGE, i.e. SMART GRIDS
    • Y04S10/00—Systems supporting electrical power generation, transmission or distribution
    • Y04S10/50—Systems or methods supporting the power network operation or management, involving a certain degree of interaction with the load-side end user applications

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Business, Economics & Management (AREA)
  • Health & Medical Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Economics (AREA)
  • Multimedia (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Public Health (AREA)
  • Water Supply & Treatment (AREA)
  • General Health & Medical Sciences (AREA)
  • Human Resources & Organizations (AREA)
  • Marketing (AREA)
  • Primary Health Care (AREA)
  • Strategic Management (AREA)
  • Tourism & Hospitality (AREA)
  • General Business, Economics & Management (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)

Abstract

本发明提出了一种基于多传感器融合的电力巡检机器人定位方法，其步骤为：首先对相机、IMU和里程计采集的数据进行预处理以及系统的标定，完成机器人系统的初始化；其次，提取关键帧，利用关键帧的实时视觉位姿对机器人的位置、速度、角度和IMU中的陀螺仪的偏置进行后端优化，得到机器人的实时位姿；然后，构建关键帧数据库，并计算当前帧图像与关键帧数据库中的所有关键帧的相似度；最后，对形成闭环的关键帧数据库中的关键帧进行闭环优化，输出闭环优化后的位姿，完成对机器人的定位。本发明提出的后端优化方法提高了定位的精度；且在视觉定位过程中增加了闭环优化，有效的消除了定位过程中存在的累计误差，保证了长时间运行情况下的准确性。

Description

一种基于多传感器融合的电力巡检机器人定位方法
技术领域
本发明涉及智能机器人技术领域，特别是指一种基于多传感器融合的电力巡检机器人定位方法。
背景技术
电力企业是进行电力生产和安全输送的企业，其设备的安全性是维持日常供电的重要保障。电气设备是否安全稳定的运行决定着整个电力系统是否安全稳定运行和电力能否可靠供应，因此电力企业制定了巡视检查制度。但传统的人力巡检方式主要由工作人员通过感官观察记录设备读数，并根据经验知识判断设备运行状态。这样的方式不但需要大量的人工配备到变电站，而且耗时耗力，容易出现误判，巡视效率较低，在暴雨下雪等恶劣天气条件下人工无法巡检，造成设备缺陷难以及时发现，对整个电网系统正常运行造成影响。近年来，随着社会经济和电气化技术的快速发展，国内对电力供应的需求不断增加，投入建造和使用的电气设备数量也随之急剧增加，且新投入的设备种类较多，系统愈发复杂，这使得传统的人力巡检方式的人员配置越发不能适应日益增加的巡视检修的工作量和工作难度。
为了解决传统电力巡检方式存在的不足之处，电网企业和研究机构将计算机技术和机器人技术结合实际巡检方式的特点，提出了电力巡检机器人的实际解决方案。电力巡检机器人可以周期性自主对变电站或发电厂内部进行巡检，通过红外、视觉等传感器采集站内设备的数据，判断设备运行状况，有效提升了电力巡检工作的科技含量，极大减少了工作人员的巡检工作量，保证巡检工作的实时性和准确性，在保障发电和传输设备乃至整个电网系统的安全稳定运行中发挥了重要作用。
但是电力巡检机器人在应用过程中仍然存在一些不足之处，如巡检机器人定位导航系统，近年来，主要使用预铺设磁轨的循迹方式，机器人按照预定的路线运行，这样就必须提前铺设磁轨道，且不容易改变轨道，不具备灵活性。其他定位方法包括视觉定位、GPS定位、惯导定位和里程计定位等一些新的定位方法。然而，基于单一传感器的定位方法应用场景较为单一，在变电站复杂环境中实际过程会出现较大偏差。视觉传感器拥有体积小、信息量大、成本低等优点，吸引了众多研究者基于视觉信息实现定位算法并使其成为如今的热门课题，但单一视觉定位容易受到光照强度、动态物体以及纹理等因素的影响，在光照较弱、纹理缺失以及动态物体的占比较大的情况下无法提取足够的特征，从而导致定位系统无法正常工作或定位结果误差较大；GPS传感器广泛应用在户外场景中，但GPS定位系统的误差无法满足机器人的应用要求，且过于依赖于磁场，在变电站或发电厂的复杂电磁环境中定位精度不高，无法正常使用。惯导定位应用的内部传感器为惯性测量单元(IMU)，其主要优点是不依赖外部条件就可实现移动机器人定位，短时精度比较好，但是其定位结果随着时间的增长会有飘移，即使比较小的常数误差被积分后都会无限变大，因此不能满足巡检机器人长时间、长距离精确定位要求。轮式里程计通过光电编码器在采样周期内脉冲的变化量计算出车轮相对于地面移动的距离和方向角的变化量，结合机器人运动学模型推算位姿相对变化，其优点是稳定性较好，与除地面环境以外的环境变化无关，但里程计仅能提供二维的运动信息，且该方法也存在累积误差，不适合机器人长距离内的定位。
近几年随着科学技术的发展，国内外的电力部门和相关企业都在开展电力巡检机器人方面的研究，尤其是对其定位系统的研究。基于不同的传感器的定位算法虽各有缺陷，但同时也具备互补性，例如部分视觉定位算法无法正常工作的场景，IMU与里程计往往是可以正常工作的；视觉定位算法能长距离长时间运行的特点很大程度上补偿了IMU与里程计的缺陷。所以，面向电力巡检机器人定位系统的低成本、高精度和高稳定性的应用需求，研究如何通过多传感器融合的方式以获得稳定、准确的定位结果，以实现巡检机器人高效稳定的作业。
发明内容
针对上述背景技术中存在的不足，本发明提出了一种基于多传感器融合的电力巡检机器人定位方法，解决了现有电力巡检机器人定位系统存在成本高、精度低和稳定性差的技术问题。
本发明的技术方案是这样实现的：
一种基于多传感器融合的电力巡检机器人定位方法，其步骤如下：
步骤一：分别对机器人系统中的相机、IMU和里程计采集的数据进行预处理，得到图像特征点、IMU预积分和里程计预积分；
步骤二：根据步骤一中的图像特征点提取关键帧图像，并利用ENFT-SFM算法对关键帧图像进行求解，得到视觉位姿，其中，视觉位姿包括位姿的旋转矩阵和平移向量；
步骤三：利用权重计算法分别对相机与IMU的外参、相机与里程计的外参进行标定；
步骤四：利用动态加权法将视觉位姿与IMU预积分和里程计预积分进行对齐处理，完成了机器人系统的初始化；
步骤五：对初始化后的机器人系统进行数据的采集，并对采集的数据进行预处理，得到实时图像特征点、实时IMU预积分和实时里程计预积分；
步骤六：根据步骤五中的实时图像特征点提取关键帧，并利用非线性优化法对关键帧进行优化，得到实时视觉位姿；
步骤七：根据步骤六中的实时视觉位姿对机器人的位置、速度、角度和IMU中的陀螺仪的偏置进行后端优化，得到机器人的实时位姿；
步骤八：将步骤六中的关键帧添加到关键帧数据库，循环步骤五至步骤八，直到关键帧数据库中的关键帧的数量大于H，完成关键帧数据库的构建；
步骤九：根据步骤五和步骤六获得当前帧图像，并计算当前帧图像与关键帧数据库中的所有关键帧的相似度，若存在相似度为1的关键帧，则将与当前帧图像相似度为1的关键帧作为闭环帧，执行步骤十，否则，将当前帧图像插入关键帧数据库，输出机器人的实时位姿；
步骤十：对闭环帧进行重定位后，再对关键帧数据库中的关键帧进行闭环优化，输出闭环优化后的位姿，并将当前帧图像插入关键帧数据库，完成对机器人的定位。
所述步骤一中分别对机器人系统中的相机、IMU和里程计采集的数据进行预处理，得到图像特征点、IMU预积分和里程计预积分的方法为：
S1.1、图像特征点提取与跟踪：
S1.1.1、对M帧图像进行自适应直方图均衡化处理，得到M帧直方图图像；
S1.1.2、利用光流法对步骤S1.1.1中的M帧直方图图像进行处理得到每一帧图像的特征点；
S1.1.3、初始化m＝1，对第m帧图像的m'个特征点与第m+1帧图像的m'个特征点进行相对应，剔除无法被对应上的特征点，得到跟踪特征点b'个；
S1.1.4、根据b'个跟踪特征点的像素位置求出基础矩阵，由基础矩阵去除异常值，再采用随机采样一致性算法去除误匹配的特征点，得到剩余跟踪特征点c'个；
S1.1.5、根据c'个跟踪特征点的被跟踪到的次数按照从大到小排序，得到跟踪特点序列，然后遍历跟踪特点序列，将遍历到的每个特征点的图像掩码的周围半径区域内的掩码设置为0，其中，半径大小为m”个像素；
S1.1.6、在图像掩码不为0的区域检测新的特征点，使用shi-tomasi算法对第m+1帧图像提取m'-c'个角点，获得第m+1帧图像的图像特征点；
S1.1.7、m＝m+1，循环步骤S1.1.3至S1.1.6，直至遍历完M帧直方图图像；
S1.2、IMU预积分：
S1.2.1、利用IMU中的速度计和陀螺仪测量IMU坐标系下机器人的加速度和角速度/>
其中，at为IMU坐标系下机器人的加速度理论值，mat为速度计的偏置，为世界坐标系到IMU坐标系的旋转矩阵，gw为世界坐标系下重力向量，na为速度计的噪声，ωt为IMU坐标系下机器人的角速度理论值，/>为陀螺仪的偏置，nω为陀螺仪的噪声；
S1.2.2、对于连续两个关键帧图像，时间间隔为△t＝[j/f,j/f+1]，根据步骤S1.2.1中的加速度和角速度/>计算下一时刻由IMU得到的世界坐标系下机器人的位置、速度和角度：
其中，世界坐标系是指相机第一帧图像的坐标系，为由IMU得到的世界坐标系下j+1帧时刻的机器人的位置，/>为由IMU得到的世界坐标系下j帧时刻的机器人的位置，为由IMU得到的世界坐标系下j帧时刻的机器人的速度，/>为由IMU得到的世界坐标系下t帧时刻的机器人的旋转矩阵，/>为IMU的加速度计得到的线加速度，gw为世界坐标系下的重力向量，/>为由IMU得到的世界坐标系下j+1帧时刻的机器人的速度，/>为由IMU得到的世界坐标系下j+1帧时刻的机器人的角度，/>为由IMU得到的世界坐标系下j帧时刻的机器人的角度，/>为由IMU得到的世界坐标系下t帧时刻的机器人的角度，f表示相机的频率，/>为IMU的陀螺仪得到的角加速度理论值，/>为IMU角速度四元数的右乘运算，/>为角速度四元数的实部向量的反对称矩阵，ωt,x为角速度四元数在t/f时刻x轴方向上的分量，ωt,y为角速度四元数在t/f时刻y轴方向上的分量，ωt,z为角速度四元数在t/f时刻z轴方向上的分量，/>表示四元数乘法；
S1.2.3、将步骤S1.2.2中世界坐标系下机器人的位置、速度和角度转换到IMU坐标系下，得到IMU的预积分项：
其中，为第j+1帧时刻到第j帧时刻IMU坐标系下的位置增量，/>为第j+1帧时刻到第j帧时刻IMU坐标系的下速度增量，/>为第j+1帧时刻到第j帧时刻IMU坐标系下的旋转增量，/>为t时刻到第j帧时刻IMU坐标系下旋转增量，/>为IMU坐标系下t/f时刻到第j帧时刻的旋转矩阵；
S1.2.4、分别对步骤S1.2.3中的IMU的预积分项进行一阶近似泰特展开，得到IMU的预积分为：
其中，为IMU预积分位置增量的近似值，/>为IMU预积分速度增量的近似值，为IMU预积分旋转增量的近似值，/>为第j帧时刻陀螺仪偏置变化量， 均是零偏值在j帧时刻的IMU的预积分测量值的雅可比矩阵；
S1.3、里程计预积分：
S1.3.1、利用里程计测量机器人的线速度和角速度/>对线速度/>和角速度/>进行积分，得到下一时刻由里程计得到的世界坐标系下机器人的位置和角度：
其中，为由里程计得到的世界坐标系下j+1帧时刻的机器人的位置，/>为由里程计得到的世界坐标系下j帧时刻的机器人的位置，/>为t时刻里程计坐标系相对于世界坐标系的旋转矩阵，/>为里程计速度测量值，nv为速度测量值的高斯噪声，/>为由里程计得到的世界坐标系下j+1帧时刻的机器人的角度，/>为由里程计得到的世界坐标系下j+1帧时刻的机器人的位置，/>为角速度测量值的高斯噪声，/>为里程计角速度四元数的右乘运算，ωd为里程计角速度理论值，/>为里程计角速度四元数的实部向量的反对称矩阵，ωd,x为里程计角速度四元数在x轴方向上的分量，ωd,y为里程计角速度四元数在y轴方向上的分量，ωd,z为里程计角速度四元数在z轴方向上的分量；
S1.3.2、将步骤S1.3.1中的世界坐标系下机器人的位置和角度转换到里程计坐标系下，得到的里程计的预积分项：
其中，为第j+1帧时刻到第j帧时刻里程计坐标系下的位置增量，/>为里程计速度理论值，/>为第j+1帧时刻到第j帧时刻里程计坐标系下的旋转增量，/>为t帧时刻到第j帧时刻里程计坐标系下旋转增量，/>为t帧时刻到第j帧时刻里程计坐标系下旋转矩阵，/>为里程计测得的角速度理论值；
S1.3.3、将里程计的预积分项中的噪声项分离后，对里程计的预积分项进行一阶近似泰特展开，可得里程计的预积分：
其中，为里程计预积分位置增量的近似值，/>为里程计预积分旋转增量的近似值，/>为第j帧时刻里程计速度测量值的高斯噪声，/>均是噪声在j时刻里程计的预积分测量值的雅可比矩阵。
所述提取关键帧图像的方法为：
S2.1、将第一帧图像设置为关键帧，将第一帧图像添加到滑窗中；
S2.2、计算当前帧与前一个关键帧的平均视差c：
其中，是指当前帧第i个特征点的像素坐标，/>是指前一个关键帧与当前帧第i个特征点相匹配的特征点的像素坐标，c'为剩余跟踪特征点；
S2.3、判断平均视差c是否大于视差阈值cd，若是，则当前帧为关键帧，并将当前帧添加到关键帧数据库中，否则，执行步骤S2.4；
S2.4、判断匹配特征点数c'是否大于匹配阈值c”，若是，则当前帧为关键帧，并将当前帧添加到滑窗中，否则，执行步骤S2.5；
S2.5、计算当前帧与前一个关键帧的时间间隔△t'；
S2.6、判断时间间隔△t'是否大于间隔阈值td，若是，则当前帧为关键帧，并将当前帧添加到滑窗中，否则，转至下一帧图像，循环执行步骤S2.2至步骤S2.6，直至遍历完M帧图像。
所述利用权重计算法分别对相机与IMU的外参、相机与里程计的外参进行标定的方法为：
S3.1、相机与IMU的外参标定：
S3.1.1、利用四元数表示位姿的旋转矩阵，根据手眼标定法建立位姿的旋转矩阵/>和IMU的旋转增量/>的约束方程：
其中，四元数为相机与IMU之间的旋转矩阵；
S3.1.2、根据四元数乘法的相关性质，将步骤S3.1.1中的约束方程转化为：
其中，Q1(·)表示四元数的左乘矩阵，Q2(·)表示四元数的右乘矩阵，为两相邻时刻之间IMU旋转增量左乘矩阵与相机旋转增量的右乘矩阵的差值；
S3.1.3、利用关键帧的索引值从0到j帧的测量数据和约束方程(10)相结合构建超定方程：
其中，为权重值，/>为两部分计算结果的接近程度，threshold1为设定的阈值，Qj为超定方程系数集合；
S3.1.4、对步骤S3.1.3中的超定方程做SVD奇异值分解，判断最小奇异值是否大于设置的阈值，如果大于阈值，则停止外参在线估计，相机与IMU的外参的旋转矩阵标定成功；
S3.2、相机与里程计的外参标定：
S3.2.1、根据手眼标定法建立位姿的旋转矩阵和里程计的旋转增量/>的约束方程：
其中，四元数为相机与里程计之间的旋转矩阵；
S3.2.2、根据四元数乘法的相关性质，将步骤S3.2.1中的约束方程转化为：
其中，表示四元数的左乘，/>表示四元数的右乘，/>为两相邻时刻之间里程计旋转增量左乘矩阵与相机旋转增量的右乘矩阵的差值；
S3.2.3、利用关键帧的索引值从0到j帧的测量数据和约束方程(13)相结合构建超定方程：
其中，为权重值，/>为两部分计算结果的接近程度，threshold2为设定的阈值，/>为超定方程系数集合；
S3.2.4、对步骤S3.2.3中的超定方程进行SVD奇异值分解，判断最小奇异值是否大于设置的阈值，如果大于阈值，则停止外参在线估计，相机与里程计的外参的旋转矩阵标定成功。
所述利用动态加权法将视觉位姿与IMU预积分和里程计预积分进行对齐处理的方法为：
S4.1、视觉位姿与IMU预积分对齐：
S4.1.1、构建相机坐标系下的待优化的向量χI：
其中，为IMU坐标系下j帧的机器人的速度，j＝1,2,…,n'，n'为滑窗中关键帧图像的数量，/>是相机坐标系中的重力向量，s1是由视觉位姿估计与IMU预积分对齐过程获得的视觉的尺度参数；
S4.1.2、将相机坐标系下的所有帧位姿转换到IMU坐标系下：
其中，为第j帧时刻IMU坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻IMU坐标系相对于第一帧相机坐标系的旋转矩阵，/>为第j帧时刻相机坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻相机坐标系相对于第一帧相机坐标系的旋转，/>为第j帧时刻IMU坐标系相对于第一帧相机坐标系的旋转，/>为相机坐标系到IMU坐标系的位置参数，/>为相机坐标系到IMU坐标系的旋转参数；
S4.1.3、将公式(16)与IMU的预积分项相结合，得到两个关键帧之间的预积分的预测值，计算两个关键帧之间的IMU预积分增量与预测值之间的残差：
其中，为第j+1帧时刻到j帧时刻之间的IMU预积分增量与预测值之间的残差，/>为两相邻关键帧时刻之间由IMU观测计算得到的位置增量与旋转增量标称值，/>为第1帧相机坐标系相对于第j帧时刻IMU坐标系的旋转矩阵，/>为第j+1帧时刻IMU坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻图像在IMU坐标系下的速度，△tj为第j+1帧时刻到第j帧时刻的时间间隔，/>为第j+1帧时刻IMU坐标系相对于第一帧相机坐标系的旋转矩阵，/>为第j+1帧时刻图像在IMU坐标系下的速度，/>为真实尺度的估计量；
S4.1.4、将残差公式(17)写为Hx＝b的形式，得到：
S4.1.5、将公式(18)转化为待优化函数：
其中，为待优化函数的海森矩阵；
S4.1.6、利用线性最小二乘法求解步骤S4.1.5中的待优化函数，得到相机坐标系下的速度、重力向量和尺度参数s1；
S4.2、视觉位姿与里程计预积分对齐：
S4.2.1、构建里程计坐标系下的待优化的向量：
其中，是里程计坐标系下j帧的机器人的速度，s2是由视觉与里程计预积分对齐得到的单目视觉的尺度参数；
S4.2.2、将相机坐标系下的所有帧位姿转换到里程计坐标系下：
其中，为第j帧时刻里程计坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的旋转矩阵，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的旋转，/>为相机坐标系到里程计坐标系的位置参数，/>为相机坐标系到里程计坐标系的旋转参数；
S4.2.3、将公式(21)与里程计的预积分项相结合，得到两个关键帧之间的预积分的预测值，计算两个关键帧之间的里程计预积分增量与预测值之间的残差：
其中，为第j+1帧时刻到j帧时刻之间的里程计预积分增量与预测值之间的残差，/>为/>的集合，/>为第一帧时刻相机坐标系相对于第j帧时刻里程计坐标系的旋转矩阵，/>为第j+1帧时刻里程计坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻图像在里程计坐标系下的速度，△tj为第j+1帧时刻到第j帧时刻的时间间隔，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的位置；
S4.2.4、将残差公式(22)写为Hx＝b的形式，得到：
S4.2.5、将公式(23)转化为待优化函数：
其中，为待优化函数的海森矩阵；
S4.2.6、利用线性最小二乘法求解步骤S4.2.5中的待优化函数，得到里程计坐标系下的速度和尺度参数s2；
S4.3、尺度参数优化：
S4.3.1、设置一个大小为5帧的滑动窗口，对大小为M帧的滑窗进行再操作，也即从滑窗中选择关键帧其中，B1包括滑窗内1-5帧，B2包括滑窗内2-6帧，并以此类推得到BM-5；
S4.3.2、分别计算滑动窗口为时的尺度参数s1,i和s2,i，同时计算最小均方误差/>和/>
S4.3.3、初始化权值W1＝0.5、W2＝0.5，根据最小均方误差和/>降低大误差对应的权值，同时提高小误差对应的权值，保证W1+W2＝1；
S4.3.4、计算尺度参数s＝W1s1+W2s2，当s的计算结果大于0时，则初始化成功。
所述利用非线性优化法对关键帧进行优化，得到实时视觉位姿的方法为：
S6.1、按照步骤S2.1至步骤S2.6的方法将M帧关键帧添加至滑窗中；
S6.2、计算第M+1帧关键帧与滑窗中所有关键帧的匹配特征点数和平均视差；
S6.3、将平均视差大于视差阈值cd，且匹配特征点数最多的一帧关键帧作为参考帧；
S6.4、利用八点法计算第M+1帧关键帧与参考帧之间的本质矩阵E：
E＝t∧R (25)，
其中，R为旋转矩阵，t为平移向量；
S6.5、利用ENFT-SFM算法计算滑窗中所有关键帧相对于参考帧的相对位姿；
S6.6、利用EPnP方法计算的每一关键帧的位姿作为非线性优化的初值：
其中，Pi'＝[Xi',Yi',Zi']T为第i'个三维空间点坐标，i'＝1,2,…,k，k为匹配的特征点的数量，投影的像素坐标为pi'＝[ui',vi']，K为相机内参矩阵，zi'表示物体在相机坐标系中的z轴坐标；
S6.7、对k个匹配点的重投影误差求和，根据重投影误差和的二范数构建目标函数：
S6.8、利用最小二乘法对目标函数进行最小化得到实时视觉位姿。
所述根据步骤六中的实时视觉位姿对机器人的位置、速度、角度和IMU中的陀螺仪的偏置进行后端优化，得到机器人的实时位姿的方法为：
S7.1、将机器人的位置、速度、角度和IMU中的陀螺仪的偏置定义为待优化变量：
其中，xj为世界坐标系下j帧的机器人的状态，χ为待优化变量，λk”代表第k”个路标点首次被观测到的逆深度，k”表示3D路标点总数；为由IMU得到的世界坐标系下j帧的机器人的位置，/>为由IMU得到的世界坐标系下j帧的机器人的速度，/>为由IMU得到的世界坐标系下j帧的机器人的角度，bω为陀螺仪的偏置；
S7.2、定义机器人系统中的单目视觉重投影误差、IMU预积分误差、里程计预积分误差和先验误差，通过最小化各测量误差马氏距离平方和可得系统状态向量的最优估计：
其中，为IMU在状态xj与xj+1之间的测量残差，/>为里程计在状态xj与xj+1之间的测量残差，/>为视觉重投影误差，B为IMU的预积分测量值，D为里程计的预积分测量值，C为在滑窗中观察到两次以上的一组特征；rp为窗口中先验信息，Jp为雅可比矩；ρ为Huber核函数，P是协方差矩阵。
所述视觉重投影误差为：
其中，表示第j帧关键帧在归一化相机平面上的投影，/>为投影的横坐标，/>为投影的纵坐标，且为IMU坐标系到相机坐标系的旋转矩阵，/>为世界坐标系相对于第j+1帧时刻IMU坐标系的旋转矩阵，/>为第j+1帧时刻IMU坐标系相对于世界坐标系的旋转矩阵，/>为第j帧图像像素横坐标，/>为第j帧图像像素纵坐标，/>为相机坐标系相对于IMU坐标系的位置变换，/>为第j帧时刻IMU坐标系相对于世界坐标系的位置变换，/>为第j+1帧时刻IMU坐标系相对于世界坐标系的位置变换；
所述IMU在状态xj与xj+1之间的测量残差为：
其中，是四元数的三维误差状态表示，[q]xyz表示提取四元数q的虚部；
所述里程计在状态xj与xj+1之间的测量残差为：
所述将步骤六中的关键帧添加到关键帧数据库的方法为：首先将关键帧添加到滑窗，直至滑窗内的关键帧的数量为M，当第M+1帧关键帧出现时，将滑窗中的第一帧关键帧移入关键帧数据库中，将第M+1帧关键帧添加至滑窗，保证滑窗内的关键帧的数量为M。
所述计算当前帧图像与关键帧数据库中的所有关键帧的相似度的方法为：
其中，s(·)为相似度函数，m1为当前帧图像的向量表示形式，n'a”为关键帧数据库中的第a'个关键帧的向量表示形式，W表示字典。
所述对闭环帧进行重定位后，再对关键帧数据库中的关键帧进行闭环优化的方法为：
S10.1、将当前帧图像和闭环帧的位姿和特征点作为视觉约束项，构建目标函数，得到当前帧与闭环帧之间的相对位姿：
其中，为当前帧图像与闭环帧之间的重投影误差，Γ为闭环帧中检测到的特征集合，(l,v)为闭环帧v中第l个特征观测，ρ为Huber核函数，j1＝1,2,…,H1为关键帧数据库中第j1帧关键帧，H1为当前帧图像至闭环帧之间的关键帧的数量；rp为边缘化的先验信息，Jp为先验信息对待优化变量的雅可比矩阵，χ为目标函数的待优化变量，/>为视觉的重投影残差，/>为由相邻两帧图像的观测计算的重投影像素坐标标称值，/>为由第v帧闭环帧图像对路标点l的观测计算出的像素坐标标称值，/>为闭环边L的相对旋转，/>为闭环边L的相对位置，/>为里程计观测值计算出的位置增量与旋转增量标称值，/>为里程计的测量残差，/>为相邻两关键帧视觉观测噪声的协方差矩阵，为相邻两关键帧时刻里程计观测噪声的协方差矩阵，/>为当前帧与闭环帧视觉观测噪声的协方差矩阵，/>为IMU的测量残差，/>为IMU观测值计算出的位置增量与旋转增量标称值，/>为相邻两关键帧时刻IMU观测噪声的协方差矩阵，C为第j帧时刻的关键帧对第各路标点观测的集合，B为用于IMU预积分的第j帧时刻关键帧的集合，D为用于里程计预积分的第j帧时刻关键帧的集合；
S10.2、计算当前帧图像与闭环帧之间的所有关键帧的相邻两帧之间的残差：
其中，为第j1帧关键帧与第j1+1帧关键帧之间的残差，/>为第j1帧关键帧的翻滚角，/>为第j1帧关键帧的俯仰角，/>为第j1+1帧关键帧的位置，ψi为第j1帧关键帧的偏航角，/>为第j1帧关键帧的位置，/>为第j1帧关键帧的偏航角，/>第j1+1帧关键帧的偏航角，/>为第j1帧关键帧与第j1+1帧关键帧之间的相对位移，/>为第j1帧关键帧与第j1+1帧关键帧之间的相对偏航角；
S10.3、根据步骤S10.2的残差构建整体目标函数：
其中，ξ＝{R，t}为序列边，R为旋转矩阵，t为平移向量，为闭环边。
本技术方案能产生的有益效果。
(1)本发明利用改进的光流法提取的图像特征点的数量更多、分布更加均匀，未出现特征点的聚集现象；
(2)本发明利用权重计算法对系统进行标定，使得标定的结果更接近基准值，降低了相机的重投影误差；
(3)本发明的初始化成功率达到90％以上，相比使用IMU和相机初始化的VINS_mono算法高出24％，且本发明的初始化状态估计算法在尺度估计精度和稳定性上都有明显提高；
(4)本发明提出的后端优化方法在定位精度上优于单目惯导融合算法，提高了定位的精度；
(5)本发明在视觉定位过程中增加了闭环优化，有效的消除了定位过程中存在的累计误差，保证了本发明方法在长时间运行情况下的准确性，得到了全局一致性的定位轨迹。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明的流程图；
图2为光流法示意图；
图3为ORB特征点法匹配结果；
图4为LK光流法匹配结果；
图5为本发明采用的改进后光流法匹配结果；
图6为本发明的IMU预积分流程图；
图7为本发明的关键帧判断流程图；
图8为Kalibr工具测得外参时重投影误差结果；
图9为本发明的相机-IMU外参标定与Kalibr工具标定结果对比；
图10为本发明的动态加权流程图；
图11为本发明的位姿求解流程图；
图12为本发明的重投影误差示意图；
图13为本发明方法与VINS_mono算法得到的轨迹对比结果；
图14为本发明方法与VINS_mono算法结果与真实轨迹在x轴与y轴方向上数值对比；
图15为本发明方法与VINS_mono算法的APE对比；
图16为本发明方法与VINS_mono算法结果与真实轨迹在z轴方向上数值对比；
图17为本发明的闭环检测和优化框图；
图18为本发明的闭环检测流程图；
图19为无闭环检测的定位轨迹；
图20为本发明的闭环检测效果；
图21为本发明增加闭环检测后的定位结果；
图22为采用本发明方法在室外大范围场景产生的定位轨迹；
图23为本发明方法在室外环境下轨迹与Google地图对齐结果图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
一种基于多传感器融合的电力巡检机器人定位方法，其步骤如下：
步骤一：分别对机器人系统中的相机、IMU和里程计采集的数据进行预处理，得到图像特征点、IMU预积分和里程计预积分，具体方法为；
S1.1、图像特征点提取与跟踪：
光流法是利用图像序列中像素随时间的变化，根据前一帧与当前帧之间的相关性和对应关系，计算相邻帧之间物体的运动的方法。如图2所示，在t1时刻，像素点的灰度值为I(x1,y1,t1)，在t2和t3时刻像素点移动到了不同位置，且灰度值分别为I(x2,y2,t2)和I(x3,y3,t3)，根据光流法灰度值不变假设可以得到：
I(x1,y1,t1)＝I(x2,y2,t2)＝I(x3,y3,t3)
针对光流法在实际应用中出现的图像提取特征点角点容易发生聚集的现象，进一步改进了算法流程，对跟踪角点进行排序，并使用非极大抑制去掉密集点，使特征点分布更加均匀，最后再额外提取角点补全需要跟踪的特征点。具体步骤如下：
S1.1.1、对预处理后的M＝10帧图像进行自适应直方图均衡化处理，避免由于图片明暗分布的问题导致明部或者暗部的细节丢失，得到M帧直方图图像；
S1.1.2、利用光流法对步骤S1.1.1中的M帧直方图图像进行处理得到每一帧图像的特征点；
S1.1.3、初始化m＝1，对第m帧图像的m'个特征点与第m+1帧图像的m'个特征点进行相对应，剔除无法被对应上的特征点，得到跟踪特征点b'个；
S1.1.4、根据b'个跟踪特征点的像素位置求出基础矩阵，由基础矩阵去除异常值(异常值是指特征点超出图像坐标范围)，再采用随机采样一致性算法(RANSAC)去除误匹配的特征点，得到剩余跟踪特征点c'个；
S1.1.5、根据c'个跟踪特征点的被跟踪到的次数按照从大到小排序，得到跟踪特点序列，然后遍历跟踪特点序列，将遍历到的每个特征点的图像掩码的周围半径区域内的掩码设置为0，其中，半径大小为m”＝10个像素；
S1.1.6、由于跟踪过程中，一些特征点无法被跟踪，需要补充新的特征点保证跟踪过程能够持续进行。因此，在图像掩码不为0的区域检测新的特征点，使用shi-tomasi算法对第m+1帧图像提取m'-c'个角点，获得第m+1帧图像的图像特征点，以保证确保每帧都有足够的特征点；
S1.1.7、m＝m+1，循环步骤S1.1.3至S1.1.6，直至遍历完M帧直方图图像。
通过实验对比ORB特征点法、光流法和改进的光流法的图像跟踪效果，在实验室环境下分别对比三种方法的跟踪特征点数量以及耗时情况。图3为采用ORB特征点法进行特征匹配的效果图；图4为LK光流法得到的特征匹配情况，图中的点表示在另一幅图像中也出现与其相匹配的点；图5为改进后的光流法特征跟踪效果；对比可以看出，相同场景下改进光流法比ORB特征点法跟踪的特征点数量多，比LK光流法特征点的分布更均匀，特征点未出现聚集现象，对明显的误匹配进行了剔除。
S1.2、IMU预积分：
IMU预积分技术在多传感器融合的过程中能够有效耦合低频的相机关键帧数据与IMU的高频数据，因此可以满足基于后端优化框架的视觉定位算法的实时性要求，大幅降低优化器的规模。IMU积分过程如图6所示。
S1.2.1、假设加速度计和陀螺仪中附加的噪声为高斯噪声，加速度计和陀螺仪的偏置假设为随机游走模型，其导数服从高斯正态分布，/>利用IMU中的速度计和陀螺仪测量IMU坐标系下机器人的加速度/>和角速度/>
其中，at为IMU坐标系下机器人的加速度理论值，为速度计的偏置，/>为世界坐标系到IMU坐标系的旋转矩阵，gw为世界坐标系下重力向量，na为速度计的噪声，ωt为IMU坐标系下机器人的角速度理论值，/>为陀螺仪的偏置，nω为陀螺仪的噪声；
S1.2.2、对于连续两个关键帧图像，时间间隔为△t＝[j/f,j/f+1]，根据步骤S1.2.1中的加速度和角速度/>计算下一时刻由IMU得到的世界坐标系下机器人的位置、速度和角度：
其中，世界坐标系是指相机第一帧图像的坐标系，为由IMU得到的世界坐标系下j+1帧时刻的机器人的位置，/>为由IMU得到的世界坐标系下j帧时刻的机器人的位置，为由IMU得到的世界坐标系下j帧时刻的机器人的速度，/>为由IMU得到的世界坐标系下t帧时刻的机器人的旋转矩阵，/>为IMU的加速度计得到的线加速度，gw为世界坐标系下的重力向量，/>为由IMU得到的世界坐标系下j+1帧时刻的机器人的速度，/>为由IMU得到的世界坐标系下j+1帧时刻的机器人的角度，/>为由IMU得到的世界坐标系下j帧时刻的机器人的角度，/>为由IMU得到的世界坐标系下t帧时刻的机器人的角度，f表示相机的频率，/>为IMU的陀螺仪得到的角加速度理论值，/>为IMU角速度四元数的右乘运算，/>为角速度四元数实部向量的反对称矩阵，ωt,x为角速度四元数在t/f时刻x轴方向上的分量，ωt,y为角速度四元数在t/f时刻y轴方向上的分量，ωt,z为角速度四元数在t/f时刻z轴方向上的分量，/>表示四元数乘法；
S1.2.3、将世界坐标系转换到IMU坐标系下，得到IMU的预积分项：
/>
其中，世界坐标系是指相机第一帧图像的坐标系，为第j+1帧时刻到第j帧时刻IMU坐标系下的位置增量，/>为第j+1帧时刻到第j帧时刻IMU坐标系的下速度增量，/>为第j+1帧时刻到第j帧时刻IMU坐标系下的旋转增量，/>为t时刻到第j帧时刻IMU坐标系下旋转增量，/>为IMU坐标系下t/f时刻到第j帧时刻的旋转矩阵；
S1.2.4、IMU的预积分项忽略了零偏和噪声，而当实际上对惯导数据进行离散点采样时，零偏值会发生缓慢微小的变化，因此分别对步骤S1.2.3中的IMU的预积分项进行一阶近似泰特展开，得到IMU的预积分为：
其中，为IMU预积分位置增量的近似值，/>为IMU预积分速度增量的近似值，为IMU预积分旋转增量的近似值，/>为第j帧时刻陀螺仪偏置变化量， 均是零偏值在j时刻的IMU的预积分测量值的雅可比矩阵。
S1.3、里程计预积分：
S1.3.1、利用里程计测量机器人的线速度和角速度/>对线速度/>和角速度/>进行积分，得到下一时刻里程计坐标系下机器人的位置：
其中，为由里程计得到的世界坐标系下j+1帧的机器人的位置，/>为由里程计得到的世界坐标系下j帧的机器人的位置，/>为t时刻里程计坐标系相对于世界坐标系的旋转矩阵，/>为里程计速度测量值，nv为速度测量值的高斯噪声，/>为由里程计得到的世界坐标系下j+1帧的机器人的角度，/>为由里程计得到的世界坐标系下j+1帧的机器人的位置，/>为角速度测量值的高斯噪声，/>为里程计角速度四元数的右乘运算，ωd为里程计角速度理论值，/>为里程计角速度四元数实部向量的反对称矩阵，ωd,x为里程计角速度四元数在x轴方向上的分量，ωd,y为里程计角速度四元数在y轴方向上的分量，ωd,z为里程计角速度四元数在z轴方向上的分量；
S1.3.2、将世界坐标系转换到里程计坐标系下，得到的里程计的预积分项：
其中，为第j+1帧时刻到第j帧时刻里程计坐标系下的位置增量，/>为里程计速度理论值，/>为第j+1帧时刻到第j帧时刻里程计坐标系下的旋转增量，/>为t时刻到第j帧时刻里程计坐标系下旋转增量，/>为t时刻到第j帧时刻里程计坐标系下旋转矩阵，/>为里程计测得的角速度理论值；
S1.3.3、里程计的预积分项中会受到噪声的影响，因此将里程计的预积分项中的噪声项分离后，对里程计的预积分项进行一阶近似泰特展开，可得里程计的预积分：
其中，为里程计预积分位置增量的近似值，/>为里程计预积分旋转增量的近似值，/>为第j帧时刻里程计速度测量值的高斯噪声，/>均是噪声在j时刻里程计的预积分测量值的雅可比矩阵。
步骤二：根据步骤一中的图像特征点提取关键帧图像，并利用ENFT-SFM算法对关键帧图像进行求解，得到视觉位姿，其中，视觉位姿包括位姿的旋转矩阵和平移向量；
系统运行时对每一帧图像进行特征点匹配与回环检测，会造成大量计算资源的浪费，生成大量冗余信息，从而导致系统效率逐渐下降，直至不能满足实时性要求。因此只需要保留其中的有效信息，选择对系统跟踪有较大影响的图像帧作为系统关键帧。具体步骤如下：
S2.1、将第一帧图像设置为关键帧，将第一帧图像添加到滑窗中；
S2.2、计算当前帧与前一个关键帧的平均视差c：
其中，是指当前帧第i个特征点的像素坐标，/>是指前一个关键帧与当前帧第i个特征点相匹配的特征点的像素坐标，c'为剩余跟踪特征点；
S2.3、判断平均视差c是否大于视差阈值cd＝30，若是，则当前帧为关键帧，并将当前帧添加到关键帧数据库中，否则，执行步骤S2.4；
S2.4、判断匹配特征点数c'是否大于匹配阈值c”，若是，则当前帧为关键帧，并将当前帧添加到滑窗中，否则，执行步骤S2.5；
S2.5、计算当前帧与前一个关键帧的时间间隔△t'；
S2.6、判断时间间隔△t'是否大于间隔阈值td，若是，则当前帧为关键帧，并将当前帧添加到滑窗中，否则，转至下一帧图像，循环执行步骤S2.2至步骤S2.6，直至遍历完M帧图像。
关键帧选择的策略，即去掉了冗余信息，又能保证提取过程不会间断，相对于VINS_mono中的策略使系统更具鲁棒性和高效性。具体的判断流程如图7所示。
步骤三：利用权重计算法分别对相机与IMU的外参、相机与里程计的外参进行标定；
S3.1、相机与IMU的外参标定：
S3.1.1、利用四元数表示位姿的旋转矩阵，根据手眼标定法建立位姿的旋转矩阵/>和IMU的旋转增量/>的约束方程：
其中，四元数为相机与IMU之间的旋转矩阵；
S3.1.2、根据四元数乘法的相关性质，将步骤S3.1.1中的约束方程转化为：
其中，Q1(·)表示四元数的左乘，Q2(·)表示四元数的右乘，为两相邻时刻之间IMU旋转增量左乘矩阵与相机旋转增量的右乘矩阵的差值；
S3.1.3、利用关键帧的索引值从0到j时刻的测量数据和约束方程(10)相结合构建超定方程：
其中，为权重值，/>为两部分计算结果的接近程度，threshold1为设定的阈值，Qj为超定方程系数集合；公式(11)中每一项都包含了对应的权重值wj。公式中的/>是与时间间隔内的旋转估计有关，如果某时间段内由于噪声或者帧间特征点误匹配造成IMU预积分和纯视觉旋转之间的误差增大，则该段时间内的/>可信度就会降低，相应的权重就应该调小。IMU预积分和纯视觉旋转越接近参量rj越小。
S3.1.4、对步骤S3.1.3中的超定方程进行SVD奇异值分解，判断最小奇异值是否大于设置的阈值，如果大于阈值，则停止外参在线估计，相机与IMU的外参的旋转矩阵标定成功。
相机-IMU外参外参可以通过借助Kalibr工具箱辅助测量获得较为准确的参考值，所以本实验将通过对比上述提出算法结果与Kalibr测得参考结果对比，证明本发明标定算法的有效性和准确性。具体步骤如下。
步骤一，固定标定板，同时在软件平台中订阅小觅相机的左目相机图像和IMU的消息，并将先前已经标定好的相机内参写入配置文件；
步骤二，充分旋转相机和IMU，使用rosbag工具记录上述信息；
步骤三，使用Kalibr工具计算由IMU坐标系到左目相机坐标系的外参变换矩阵，并重复步骤三5次，将第一次检测结果添加到表1中；
步骤四，将步骤二中记录的信息进行重放，并使用本发明提出的标定方法计算外参矩阵，将结果添加到表1中；
Kalibr工具测得的IMU与相机外参衡量标准是相机的重投影误差的结果，关于相机的重投影误差的显示结果如图8所示。其误差基本保持在2个像素距离之内，证明标定结果良好。
表1 IMU坐标系与相机间的外参变换
根据两坐标系对应关系可知，相机与IMU之间的方向偏移在偏航角，俯仰角和翻滚角方面分别为[0°,0°,180°]，现将其作为基准值对比本发明提出的标定算法与Kalibr标定工具结果，如图9所示。从图中可以看出发明提出的标定算法的相机-IMU外参标定结果，在偏航角、俯仰角及翻滚角均比Kalibr标定结果更接近基准值。
S3.2、相机与里程计的外参标定：
S3.2.1、根据手眼标定法建立位姿的旋转矩阵和里程计的旋转增量/>的约束方程：
其中，四元数为相机与里程计之间的旋转矩阵；
S3.2.2、根据四元数乘法的相关性质，将步骤S3.2.1中的约束方程转化为：
/>
其中，表示四元数的左乘，/>表示四元数的右乘，/>为两相邻时刻之间里程计旋转增量左乘矩阵与相机旋转增量的右乘矩阵的差值；
S3.2.3、利用关键帧的索引值从0到j时刻的测量数据和约束方程(13)相结合构建超定方程：
其中，为权重值，/>为两部分计算结果的接近程度，threshold2为设定的阈值，/>为超定方程系数集合；
S3.2.4、对步骤S3.2.3中的超定方程进行SVD奇异值分解，判断最小奇异值是否大于设置的阈值，如果大于阈值，则停止外参在线估计，相机与里程计的外参的旋转矩阵标定成功。
步骤四：利用动态加权法将视觉位姿与IMU预积分和里程计预积分进行对齐处理，完成了机器人系统的初始化；
S4.1、视觉位姿与IMU预积分对齐：视觉与IMU预计分的对齐过程，也即两者联合初始化，恢复机器人的速度、在相机坐标系下的重力向量和尺度参数。
S4.1.1、构建相机坐标系下的待优化的向量：
其中，将第一个相机坐标系设置为单目SFM的参考坐标系，/>是j时刻IMU坐标系的速度，j＝1,2,…,n'，/>是相机坐标系中的重力向量，s1是视觉位姿估计与IMU预积分对齐过程获得的视觉的尺度参数；
S4.1.2、将相机坐标系下的所有帧位姿转换到IMU坐标系下，由于两者之间缺少一个尺度参数，所以把尺度因子加入转换过程中：
其中，为第j帧时刻IMU坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻IMU坐标系相对于第一帧相机坐标系的旋转矩阵，/>为第j帧时刻IMU坐标系相对于第一帧相机坐标系的旋转，/>为相机坐标系到IMU坐标系的位置参数，/>为相机坐标系到IMU坐标系的旋转参数；
S4.1.3、将公式(16)与IMU的预积分项相结合，得到两个关键帧之间的预积分的预测值，计算两个关键帧之间的IMU预积分增量与预测值之间的残差：
其中，为第j+1在t/f时刻到j在t/f时刻之间的IMU预积分增量与预测值之间的残差，/>为两相邻关键帧时刻之间由IMU观测计算得到的位置增量与旋转增量标称值，/>为第一帧时刻相机坐标系相对于第j帧时刻IMU坐标系的旋转矩阵，/>为第j+1帧时刻IMU坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻图像在IMU坐标系下的速度，△tj为第j+1帧时刻到第j帧时刻的时间间隔，/>为第j+1帧时刻IMU坐标系相对于第一帧相机坐标系的旋转矩阵，/>为第j+1帧时刻图像在IMU坐标系下的速度，/>为真实尺度的估计量；
S4.1.4、将残差公式(17)写为Hx＝b的形式，得到：
S4.1.5、将公式(18)转化为待优化函数：
其中，为待优化函数的海森矩阵；
S4.1.6、利用线性最小二乘法求解待优化函数，得到相机坐标系下的速度、重力向量和尺度参数s1。
S4.2、视觉位姿与里程计预积分对齐：视觉与里程计预计分的对齐过程，通过二者联合初始化，恢复里程计坐标系下机器人的速度和尺度参数。
S4.2.1、构建里程计坐标系下的待优化的向量：
其中，是j时刻里程计坐标系的速度，s2是由视觉与里程计预积分对齐得到的单目视觉的尺度参数；
S4.2.2、将相机坐标系下的所有帧位姿转换到里程计坐标系下，转换过程加入尺度参数：
/>
其中，为第j帧时刻里程计坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的旋转矩阵，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的旋转，/>为相机坐标系到里程计坐标系的位置参数，/>为相机坐标系到里程计坐标系的旋转参数；
S4.2.3、将公式(21)与里程计的预积分项相结合，得到两个关键帧之间的预积分的预测值，计算两个关键帧之间的里程计预积分增量与预测值之间的残差：
其中，为第j+1帧时刻到j帧时刻之间的里程计预积分增量与预测值之间的残差，/>为两相邻关键帧时刻之间由里程计观测计算得到的位置增量与旋转增量标称值，/>为第一帧时刻相机坐标系相对于第j帧图像时刻里程计坐标系的旋转矩阵，/>为第j+1帧时刻里程计坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻图像在里程计坐标系下的速度，△tj为第j+1帧时刻到第j帧时刻的时间间隔，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的位置；
S4.2.4、将残差公式(22)写为Hx＝b的形式，得到：
S4.2.5、将公式(23)转化为待优化函数：
其中，为待优化函数的海森矩阵；
S4.2.6、利用线性最小二乘法求解待优化函数，得到里程计坐标系下的速度和尺度参数s2。
S4.3、尺度参数优化：
为了提高系统初始化过程的鲁棒性和精度，本发明基于动态加权的思想对滑动窗口内的对齐过程进行改进，从而对尺度参数s1，s2进行综合评价，获得精度更高的尺度参数。如图10所示，具体过程如下：
S4.3.1、设置一个大小为5帧的滑动窗口，对大小为M帧的滑窗进行再操作，也即从滑窗中选择关键帧其中，B1包括滑窗内1-5帧，B2包括滑窗内2-6帧，并以此类推得到BM-5；
S4.3.2、分别计算滑动窗口为时各自的尺度参数/>和/>同时计算各阶段的最小均方误差/>和/>
S4.3.3、初始化权值W1＝0.5、W2＝0.5，根据最小均方误差和/>降低大误差对应的权值，每次降低0.1，同时提高小误差对应的权值，保证W1+W2＝1；
S4.3.4、计算尺度参数s＝W1s1+W2s2，当s的计算结果大于0时，则初始化成功。
初始化状态估计实验
本实验主要验证初始化状态估计算法的效果，并通过选择与仅采用IMU进行初始化的VINS_mono算法进行对比验证算法有效性。本发明初始化算法与VINS_mono分别从初始化成功率、尺度值估计和收敛时间进行比较，并采用均方根误差(Root Mean SquareError，RMSE)定量表示算法精度。成功率实验是在机器人同一室内环境，初始位置，速度运行两种算法各50次，并记录各自初始化成功次数，如表2所示。尺度估计实验是从成功率实验中不同算法各随机抽取10次成功初始化的结果，并对比尺度值与收敛时间，如表3所示。
表1初始化成功率
表2尺度值与收敛时间
从表2结果可以看出，本发明的初始化成功率相比使用IMU和相机初始化的VINS_mono算法高出24％，达到基本满足要求的90％。从表3结果可以看出，本发明初始化状态估计算法比VINS_mono进行初始化，尺度值的RMSE减少72.2％，收敛时间RMSE减少62.9％。因此本发明提出的初始化状态估计算法在尺度估计精度和稳定性都有明显的提高。
步骤五：对初始化后的机器人系统进行数据的采集，并对采集的数据进行预处理，得到实时图像特征点、实时IMU预积分和实时里程计预积分；
步骤六：根据步骤五中的实时图像特征点提取关键帧，并利用非线性优化法对关键帧进行优化，得到实时视觉位姿；由于本发明研究的对象为单目相机，所以仅能得到2D的像素坐标，需要解决的问题关键就是使用2D点估计相机运动。单目相机采集到的图像数据量会越来越大，需要保存的数据也会越来越多，如果当前图像的通过整个采集过程的数据来计算当前位姿，计算量也会随着数据量增加而变得十分庞大，使得整个系统无法满足实时性。因此，为了减小单目视觉定位系统的计算复杂度，保证系统稳定实时运行，本发明基于滑动窗口的非线性优化思想，设置固定大小的滑窗保存图像帧，并只对窗口中的状态量进行局部优化，随着最新图像帧的产生边缘化最老关键帧作为先验信息加入到非线性优化过程中。如图11所示，本发明将滑动窗口大小设置为十帧，在窗口接收十帧图像后，检测每帧到滑动窗口最后一帧的特征点对应关系，如果它与窗口中所有的图像帧都具有充足的视差(帧间距离超过30个像素)，并在滑窗内找到与当前帧的匹配特征点数最多的一帧为参考帧，根据第M+1帧得到的2D像素坐标之间的几何关系，利用对极几何求解当前帧与参考帧的相对位移和姿态。具体步骤为：
S6.1、按照步骤S2.1至步骤S2.6的方法将M帧关键帧添加至滑窗中；
S6.2、计算第M+1帧关键帧与滑窗中所有关键帧的匹配特征点数和平均视差；
S6.3、将平均视差大于视差阈值cd，且匹配特征点数最多的一帧关键帧作为参考帧；
S6.4、利用八点法计算第M+1帧关键帧与参考帧之间的本质矩阵E：
E＝t∧R (25)，
其中，R为旋转矩阵，t为平移向量；因此在检测图像角点后可根据特征点对来计算本质矩阵。其中，当图像特征点都落在一个平面上时，可以通过同时求解并比较单应矩阵和本质矩阵的重投影误差，选择误差小的用于求解两帧之间的相机运动。
S6.5、利用ENFT-SFM算法计算滑窗中所有关键帧相对于参考帧的相对位姿；SFM(Structure-from-Motion)是一种能够从图像序列中自动地恢复出相机的参数以及场景三维结构的技术，通常从特征提取和匹配开始，然后进行几何检查。而单目视觉里程计也可被认为是一种实时的SFM，因此本发明借鉴SFM的思想对相机位姿进行估计。但是SFM的精度受场景中运动物体和图像噪声的影响，会造成特征跟踪不准确、寿命短和三维空间点漂移的现象，所以本发明采用浙江大学CAD&CG国家重点实验室的章国锋团队提出的高效的非连续特征跟踪方法(Efficient Non-Consecutive Feature Tracking,ENFT)求出所有图像帧相对于参考帧的姿态四元数Q、平移向量T和特征点三维空间坐标，提高了算法鲁棒性，并能快速消除误差累积和空间点漂移问题。
S6.6、利用EPnP方法计算的每一关键帧的位姿作为非线性优化的初值：
其中，Pi'＝[Xi',Yi',Zi']T为第i'个三维空间点坐标，i'＝1,2,…,k，k为匹配的特征点的数量，投影的像素坐标为pi'＝[ui',vi']，K为相机内参矩阵，zi'表示物体在相机坐标系中的z轴坐标；EPnP方法的优点是计算效率高，噪声影响小。
S6.7、重投影误差是指利用估计的旋转矩阵R和平移矩阵t将前一帧图像观测点在相机坐标系中的三维坐标通过相机内参投影到下一帧图像中。由于观测噪声、光流跟踪误差和特征点误匹配等，预测位置与投影位置之间存在误差就是重投影误差，如图12所示。因此需要采用光束平差法最小化重投影误差，对相机运动和三维点放在一起进一步优化，减小单目视觉里程计中的局部定位误差得到较为准确的位姿结果。本文相机模型采用针孔模型，使用特征点的重投影误差来建立优化模型。对k个匹配点的重投影误差求和，根据重投影误差和的二范数构建目标函数：
S6.8、利用最小二乘法对目标函数进行最小化得到实时视觉位姿。
步骤七：根据步骤六中的实时视觉位姿对机器人的位置、速度、姿态和IMU中的陀螺仪的偏置进行后端优化，得到机器人的实时位姿；在后端非线性优化的过程中，由于关键帧的数量较多，采用滑动窗口的方式对常量的关键帧进行局部优化，通过边缘化的方式将其他信息作为先验信息与约束条件，保证滑动窗口计算复杂度的稳定性，并且充分利用历史信息进行优化，保证局部信息传递的一致性。具体优化方法为：
S7.1、将机器人的位置、速度、角度和IMU中的陀螺仪的偏置定义为待优化变量：
其中，xj为j时刻机器人在世界坐标系下的位置、速度和角度，χ为待优化变量，λk”代表第k”个路标点的首次被观测到时的逆深度，k”表示3D路标点总数；为由IMU得到的世界坐标系下j帧的机器人的位置，/>为由IMU得到的世界坐标系下j帧的机器人的速度，为由IMU得到的世界坐标系下j帧的机器人的角度，bω为陀螺仪的偏置；/>bω、λ的初始值均设置为0，位置/>的初始值设置为0，速度/>的初始值选择初始化阶段IMU坐标系下求得的结果；
S7.2、定义机器人系统中的单目视觉重投影误差、IMU预积分误差、里程计预积分误差和先验误差，通过最小化各测量误差马氏距离平方和可得系统状态向量的最优估计：
其中，为IMU在状态xj与xj+1之间的测量残差，/>为里程计在状态xj与xj+1之间的测量残差，/>为视觉重投影误差，B为IMU的预积分测量值，D为里程计的预积分测量值，C为在滑窗中观察到两次以上的一组特征；rp为窗口中先验信息，Jp为雅可比矩；ρ为Huber核函数，其作用是使优化更加稳健，P是协方差矩阵，决定着相机、IMU、里程计三个传感器的置信度。
假设相机关键帧第j个图像观测到第k”个路标点，并随着相机运动，路标点k”再次被第j+1个图像观测到，由视觉观测可以表示其在归一化相机平面上的投影为那么所述视觉重投影误差为：
其中，表示第j帧关键帧在归一化相机平面上的投影，/>为投影的横坐标，/>为投影的纵坐标，且为相机坐标系相对于IMU坐标系的旋转矩阵，/>为世界坐标系相对于第j+1帧时刻IMU坐标系的旋转矩阵，/>为第j+1帧时刻IMU坐标系相对于世界坐标系的旋转矩阵，/>为第j帧时刻图像像素横坐标，/>为第j帧时刻图像像素纵坐标，/>为相机坐标系相对于IMU坐标系的位置变换，/>为第j帧时刻IMU坐标系相对于世界坐标系的位置变换，/>为第j+1帧时刻IMU坐标系相对于世界坐标系的位置变换；为了使得公式(30)的重投影误差最小化，需要优化的状态量包括两个关键帧j，j+1的状态量/> 以及逆深度λk”。
所述IMU在状态xj与xj+1之间的测量残差为：
其中，是四元数的三维误差状态表示，[q]xyz表示提取四元数q的虚部；在后端优化中，需要优化的状态量为两连续关键帧时刻的状态xj和xj+1，同时计算残差对应两个时刻的状态量求偏导即可得到相应的雅可比矩阵。
假设轮轴位于机器人中轴线上，由里程计测量的位移量可得到机器人中心的位移量。所述里程计在状态xj与xj+1之间的测量残差为：
后端优化中需要优化连续两帧下的状态xj和xj+1，同时采用扰动方式计算里程计残差对应两个时刻的状态量的雅可比矩阵。
室内环境定位精度试验
实验场景为检测平台的室内环境，机器人在3m×3m的区域内移动且整个过程机器人的瓶颈速度为0.2m/s。图13和图14中使用mono_imu的轨迹代表VINS_mono算法在机器人平台上运行的结果，本发明的实验结果与VINS_mono算法进行对比。
本发明方法与VINS_mono算法的轨迹与真值对比如图13、图14所示。其中虚线为检测系统得到的真实轨迹，实线为本发明方法与VINS_mono算法得到的轨迹。
为了定量分析算法的精度，通过将算法估计位姿结果与每个时间步长的真实运动位姿对比，通过绝对位姿误差(Absolute Pose Error,APE)来衡量精度。本发明方法与VINS_mono算法的位姿APE如图15所示。具体的APE统计数据如表4所示。
表3三种方法的APE统计数据
通过对比两种方法不难看出，本发明提出的算法在定位精度上优于单目惯导融合算法，说明里程计信息的加入对定位结果有一定程度的提升，并且利用里程计更好的平面约束使得本发明算法在zw方向上的数值基本稳定，而VINS_mono算法在zw方向上有一定程度的发散，如图16所示。由于在Vicon系统中内对机器人建立的刚体重心高度为0.03m，所以真值在zw方向上保持在0.03m。
步骤八：将步骤六中的关键帧添加到关键帧数据库，循环步骤五至步骤八，直到关键帧数据库中的关键帧的数量大于H，完成关键帧数据库的构建；首先将关键帧添加到滑窗，直至滑窗内的关键帧的数量为M，当第M+1帧关键帧出现时，将滑窗中的第一帧关键帧移入关键帧数据库中，将第M+1帧关键帧添加至滑窗，保证滑窗内的关键帧的数量为M。关键帧数据库保存的关键帧是当滑窗内的关键帧被移出滑窗之后才添加到数据库当中，并且这些关键帧已经经过前面的多次滑窗优化，因此可以保证添加进来的关键帧位姿基本准确。数据库中的帧对于闭环优化会提供两种边：
序列边：是指通过后端优化计算的在滑动窗口中关键帧之间的相对位姿。
闭环边：是指检测到形成闭环的两帧，通过重定位得到闭环边中相对位置和相对旋转。
步骤九：根据步骤五和步骤六获得当前帧图像，并计算当前帧图像与关键帧数据库中的所有关键帧的相似度，若存在相似度为1的关键帧，则将与当前帧图像相似度为1的关键帧作为闭环帧，执行步骤十，否则，将当前帧图像插入关键帧数据库，输出机器人的实时位姿；
所述计算当前帧图像与关键帧数据库中的所有关键帧的相似度的方法为：
其中，s(·)为相似度函数，m1为当前帧图像的向量表示形式，n”a’为关键帧数据库中的第a'个关键帧的向量表示形式，W表示字典。当s(m1,n”a’)结果为1时表示两图像最相似，结果为0时说明两图像完全不相关。字典W使用的是DBoW3库，这是一个开源的C++库，用于给图像特征排序，并将图像转化成视觉词袋表示，它采用层级树状结构将相近的图像特征在物理存储上聚集在一起，创建一个视觉词典。
步骤十：对闭环帧进行重定位后，再对关键帧数据库中的关键帧进行闭环优化，输出闭环优化后的位姿，并将当前帧图像插入关键帧数据库，完成对机器人的定位。
单目视觉定位系统中采用滑动窗口和边缘化的策略维持系统稳定的计算复杂度，保证了系统的实时运行，但是随着系统的长时间运行系统的累积漂移也会随之产生。为了消除系统的累积漂移，本发明系统设计了基于BOW2词袋模型的闭环检测模块。闭环检测是指相机在运动过程中记录自身经过的场景信息，当检测识别到记录的历史场景后，移动机器人就根据当前帧与闭环帧之间的闭环约束来校正当前位置。
因此，回环检测模块对定位系统的长期稳定运行、定位结果的全局一致性和提高机器人的定位精度有着至关重要的作用。闭环检测模块首先对从滑窗内滑出的关键帧进行闭环检测，之后确定闭环帧后对滑窗内的所有帧进行调整，完成重定位，接着重定位得到的回环边和序列边，最后对关键帧数据库中的所有帧进行闭环优化，得到全局最优位姿估计，从而提高了定位结果的精度和轨迹的全局一致性。系统闭环检测和闭环优化框架如图17所示。
闭环检测主要是采用基于外观的方法来计算并对比系统中图像间的相似性的过程。现阶段大部分研究中的闭环检测方法均是按照基于外观的词袋模型(Bag of Words)进行处理的。因此，采用BoW2词袋进行闭环检测，由于视觉跟踪的改进光流法使用Harris角点进行特征提取，最多仅能提取的特征点数量为150个，不能满足闭环检测要求，因此当关键帧进入关键帧数据库时将对图像额外提取500个角点，并对新老角点计算BRIEF描述子。接着计算当前帧与词袋的相似度分数，并与关键帧数据库中的所有帧进行对比，进行闭环一致性对比，获得闭环候选帧。然后，将当前关键帧与闭环候选帧进行描述子匹配，由于描述子的直接匹配会产生一定量的误匹配，因此需要剔除匹配失败的点。如果经过3D-2D之间的RANSAC的PnP检测得到的匹配点数大于阈值，则认为经过检测后该候选帧为正确的闭环帧。同时求解两帧相对位姿保存起来用于后续的闭环优化环节。闭环检测流程如图18所示。
S10.1、当检测到滑窗内的某一帧与数据库中的第v帧发生闭环时，重定位模块会将当前帧图像和闭环帧的位姿和特征点作为视觉约束项，加入到后端优化的整体目标函数中，构建目标函数，用滑窗优化更精确计算第v帧的位姿，得到当前帧与闭环帧之间的相对位姿：
其中，为当前帧图像与闭环帧之间的重投影误差，Γ为闭环帧中检测到的特征集合，(l,v)为闭环帧v中第l个特征观测，ρ为Huber核函数，j1＝1,2,…,H1为关键帧数据库中第j1帧关键帧，H1为当前帧图像至闭环帧之间的关键帧的数量；rp为边缘化的先验信息，Jp为先验信息对待优化变量的雅可比矩阵，χ为目标函数的待优化变量，为视觉的重投影残差，/>为由相邻两帧图像的观测计算的重投影像素坐标标称值，/>为由第v帧闭环帧图像对路标点l的观测计算出的像素坐标标称值，/>为闭环边L的相对旋转，/>为闭环边L的相对位置，/>为里程计观测值计算出的位置增量与旋转增量标称值，/>为里程计的测量残差，/>为相邻两关键帧视觉观测噪声的协方差矩阵，为相邻两关键帧时刻里程计观测噪声的协方差矩阵，/>为当前帧与闭环帧视觉观测噪声的协方差矩阵，/>为IMU的测量残差，/>为IMU观测值计算出的位置增量与旋转增量标称值，/>为相邻两关键帧时刻IMU观测噪声的协方差矩阵，C为第j帧时刻的关键帧对第各路标点观测的集合，B为用于IMU预积分的第j帧时刻关键帧的集合，D为用于里程计预积分的第j帧时刻关键帧的集合。
重定位虽然可以消除发生闭环时滑动窗口中的累计误差，但系统长期运行所造成的误差累积依然存在，并会造成偏离真实轨迹和难以形成闭合回环。为了保证轨迹的全局一致性和平滑性，融合定位系统额外增加了全局位姿图优化过程。由于图像的路标点信息会随着时间增长，如果对其优化会使得计算量过于庞大，因此当从滑窗内滑出的帧与数据库中的帧为闭环帧时，全局优化不在对路标点进行优化，而只对数据库中所有关键帧的位姿进行优化。
S10.2、计算当前帧图像与闭环帧之间的所有关键帧的相邻两帧之间的残差：
其中，为第j1帧关键帧与第j1+1帧关键帧之间的残差，/>为第j1帧关键帧的翻滚角，/>为第j1帧关键帧的俯仰角，/>为第j1+1帧关键帧的位置，ψi为第j1帧关键帧的偏航角，/>为第j1帧关键帧的位置，/>为第j1帧关键帧的偏航角，/>第j1+1帧关键帧的偏航角，/>为第j1帧关键帧与第j1+1帧关键帧之间的相对位移，/>为第j1帧关键帧与第j1+1帧关键帧之间的相对偏航角；
S10.3、根据步骤S10.2的残差构建整体目标函数：
其中，ξ＝{R，t}为序列边，为闭环边。
闭环检测实验
在机器人上运行单目视觉算法，首先关闭闭环检测模块，通过ROS(RobotOperating System)可视化工具RVIZ观察相机移动轨迹，并观察机器人经过同一位置时产生的偏差，如图19所示。从图19中可以看出，累积误差随着时间增长，当相机回到起始位置时，实验中相机位置偏离原有位置较远。
当添加闭环检测模块后，再次重复实验，当系统检测到闭环帧时，对整个轨迹进行优化，其闭环检测效果如图20所示。
通过可视化工具观测定位结果如图21所示。从图21中可以看出，视觉定位算法增加了闭环检测模块后，有效的消除了定位过程中存在的累积误差，保证了算法在长时间运行情况下的准确性，并且得到了具有全局一致性的定位轨迹。
室外大范围场景下定位实验
本实验选定Karlsruher Institut für Technologie学院在室外场景中录制的Kitti数据集为实验数据，运行本发明方法得到定位结果，整个路径超过1000米，轨迹如图22所示，同时为了验证本发明方法有效性，将轨迹与Google地图对齐，如图23所示。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于多传感器融合的电力巡检机器人定位方法，其特征在于，其步骤如下：
步骤一：分别对机器人系统中的相机、IMU和里程计采集的数据进行预处理，得到图像特征点、IMU预积分和里程计预积分；
S1.1、图像特征点提取与跟踪：
S1.1.1、对M帧图像进行自适应直方图均衡化处理，得到M帧直方图图像；
S1.1.2、利用光流法对步骤S1.1.1中的M帧直方图图像进行处理得到每一帧图像的特征点；
S1.1.3、初始化m＝1，对第m帧图像的m'个特征点与第m+1帧图像的m'个特征点进行相对应，剔除无法被对应上的特征点，得到跟踪特征点b'个；
S1.1.4、根据b'个跟踪特征点的像素位置求出基础矩阵，由基础矩阵去除异常值，再采用随机采样一致性算法去除误匹配的特征点，得到剩余跟踪特征点c'个；
S1.1.5、根据c'个跟踪特征点的被跟踪到的次数按照从大到小排序，得到跟踪特点序列，然后遍历跟踪特点序列，将遍历到的每个特征点的图像掩码的周围半径区域内的掩码设置为0，其中，半径大小为m”个像素；
S1.1.6、在图像掩码不为0的区域检测新的特征点，使用shi-tomasi算法对第m+1帧图像提取m'-c'个角点，获得第m+1帧图像的图像特征点；
S1.1.7、m＝m+1，循环步骤S1.1.3至S1.1.6，直至遍历完M帧直方图图像；
S1.2、IMU预积分：
S1.2.1、利用IMU中的速度计和陀螺仪测量IMU坐标系下机器人的加速度和角速度/>
其中，at为IMU坐标系下机器人的加速度理论值，为速度计的偏置，/>为世界坐标系到IMU坐标系的旋转矩阵，gw为世界坐标系下重力向量，na为速度计的噪声，ωt为IMU坐标系下机器人的角速度理论值，/>为陀螺仪的偏置，nω为陀螺仪的噪声；
S1.2.2、对于连续两个关键帧图像，时间间隔为△t＝[j/f,j/f+1]，根据步骤S1.2.1中的加速度和角速度/>计算下一时刻由IMU得到的世界坐标系下机器人的位置、速度和角度：
其中，世界坐标系是指相机第一帧图像的坐标系，为由IMU得到的世界坐标系下j+1帧时刻的机器人的位置，/>为由IMU得到的世界坐标系下j帧时刻的机器人的位置，/>为由IMU得到的世界坐标系下j帧时刻的机器人的速度，/>为由IMU得到的世界坐标系下t帧时刻的机器人的旋转矩阵，/>为IMU的加速度计得到的线加速度，gw为世界坐标系下的重力向量，/>为由IMU得到的世界坐标系下j+1帧时刻的机器人的速度，/>为由IMU得到的世界坐标系下j+1帧时刻的机器人的角度，/>为由IMU得到的世界坐标系下j帧时刻的机器人的角度，/>为由IMU得到的世界坐标系下t帧时刻的机器人的角度，f表示相机的频率，/>为IMU的陀螺仪得到的角加速度理论值，/>为IMU角速度四元数的右乘运算，/>为角速度四元数的实部向量的反对称矩阵，ωt,x为角速度四元数在t/f时刻x轴方向上的分量，ωt,y为角速度四元数在t/f时刻y轴方向上的分量，ωt,z为角速度四元数在t/f时刻z轴方向上的分量，/>表示四元数乘法；
S1.2.3、将步骤S1.2.2中世界坐标系下机器人的位置、速度和角度转换到IMU坐标系下，得到IMU的预积分项：
其中，为第j+1帧时刻到第j帧时刻IMU坐标系下的位置增量，/>为第j+1帧时刻到第j帧时刻IMU坐标系的下速度增量，/>为第j+1帧时刻到第j帧时刻IMU坐标系下的旋转增量，/>为t时刻到第j帧时刻IMU坐标系下旋转增量，/>为IMU坐标系下t/f时刻到第j帧时刻的旋转矩阵；
S1.2.4、分别对步骤S1.2.3中的IMU的预积分项进行一阶近似泰特展开，得到IMU的预积分为：
其中，为IMU预积分位置增量的近似值，/>为IMU预积分速度增量的近似值，/>为IMU预积分旋转增量的近似值，/>为第j帧时刻陀螺仪偏置变化量， 均是零偏值在j帧时刻的IMU的预积分测量值的雅可比矩阵；
S1.3、里程计预积分：
S1.3.1、利用里程计测量机器人的线速度和角速度/>对线速度/>和角速度/>进行积分，得到下一时刻由里程计得到的世界坐标系下机器人的位置和角度：
其中，为由里程计得到的世界坐标系下j+1帧时刻的机器人的位置，/>为由里程计得到的世界坐标系下j帧时刻的机器人的位置，/>为t时刻里程计坐标系相对于世界坐标系的旋转矩阵，/>为里程计速度测量值，nv为速度测量值的高斯噪声，/>为由里程计得到的世界坐标系下j+1帧时刻的机器人的角度，/>为由里程计得到的世界坐标系下j+1帧时刻的机器人的位置，nωd为角速度测量值的高斯噪声，/>为里程计角速度四元数的右乘运算，ωd为里程计角速度理论值，/>为里程计角速度四元数的实部向量的反对称矩阵，ωd,x为里程计角速度四元数在x轴方向上的分量，ωd,y为里程计角速度四元数在y轴方向上的分量，ωd,z为里程计角速度四元数在z轴方向上的分量；
S1.3.2、将步骤S1.3.1中的世界坐标系下机器人的位置和角度转换到里程计坐标系下，得到的里程计的预积分项：
其中，为第j+1帧时刻到第j帧时刻里程计坐标系下的位置增量，/>为里程计速度理论值，/>为第j+1帧时刻到第j帧时刻里程计坐标系下的旋转增量，/>为t帧时刻到第j帧时刻里程计坐标系下旋转增量，/>为t帧时刻到第j帧时刻里程计坐标系下旋转矩阵，/>为里程计测得的角速度理论值；
S1.3.3、将里程计的预积分项中的噪声项分离后，对里程计的预积分项进行一阶近似泰特展开，可得里程计的预积分：
其中，为里程计预积分位置增量的近似值，/>为里程计预积分旋转增量的近似值，/>为第j帧时刻里程计速度测量值的高斯噪声，/>均是噪声在j时刻里程计的预积分测量值的雅可比矩阵；
步骤二：根据步骤一中的图像特征点提取关键帧图像，并利用ENFT-SFM算法对关键帧图像进行求解，得到视觉位姿，其中，视觉位姿包括位姿的旋转矩阵和平移向量；
S2.1、将第一帧图像设置为关键帧，将第一帧图像添加到滑窗中；
S2.2、计算当前帧与前一个关键帧的平均视差c：
其中，是指当前帧第i个特征点的像素坐标，/>是指前一个关键帧与当前帧第i个特征点相匹配的特征点的像素坐标，c'为剩余跟踪特征点；
S2.3、判断平均视差c是否大于视差阈值cd，若是，则当前帧为关键帧，并将当前帧添加到关键帧数据库中，否则，执行步骤S2.4；
S2.4、判断匹配特征点数c'是否大于匹配阈值c”，若是，则当前帧为关键帧，并将当前帧添加到滑窗中，否则，执行步骤S2.5；
S2.5、计算当前帧与前一个关键帧的时间间隔△t'；
S2.6、判断时间间隔△t'是否大于间隔阈值td，若是，则当前帧为关键帧，并将当前帧添加到滑窗中，否则，转至下一帧图像，循环执行步骤S2.2至步骤S2.6，直至遍历完M帧图像；
步骤三：利用权重计算法分别对相机与IMU的外参、相机与里程计的外参进行标定；
步骤四：利用动态加权法将视觉位姿与IMU预积分和里程计预积分进行对齐处理，完成了机器人系统的初始化；
步骤五：对初始化后的机器人系统进行数据的采集，并对采集的数据进行预处理，得到实时图像特征点、实时IMU预积分和实时里程计预积分；
步骤六：根据步骤五中的实时图像特征点提取关键帧，并利用非线性优化法对关键帧进行优化，得到实时视觉位姿；
步骤七：根据步骤六中的实时视觉位姿对机器人的位置、速度、角度和IMU中的陀螺仪的偏置进行后端优化，得到机器人的实时位姿；
步骤八：将步骤六中的关键帧添加到关键帧数据库，循环步骤五至步骤八，直到关键帧数据库中的关键帧的数量大于H，完成关键帧数据库的构建；
步骤九：根据步骤五和步骤六获得当前帧图像，并计算当前帧图像与关键帧数据库中的所有关键帧的相似度，若存在相似度为1的关键帧，则将与当前帧图像相似度为1的关键帧作为闭环帧，执行步骤十，否则，将当前帧图像插入关键帧数据库，输出机器人的实时位姿；
其中，s(·)为相似度函数，m1为当前帧图像的向量表示形式，n″a′为关键帧数据库中的第a'个关键帧的向量表示形式，W表示字典；
步骤十：对闭环帧进行重定位后，再对关键帧数据库中的关键帧进行闭环优化，输出闭环优化后的位姿，并将当前帧图像插入关键帧数据库，完成对机器人的定位。
2.根据权利要求1所述的基于多传感器融合的电力巡检机器人定位方法，其特征在于，所述利用权重计算法分别对相机与IMU的外参、相机与里程计的外参进行标定的方法为：
S3.1、相机与IMU的外参标定：
S3.1.1、利用四元数表示位姿的旋转矩阵，根据手眼标定法建立位姿的旋转矩阵和IMU的旋转增量/>的约束方程：
其中，四元数为相机与IMU之间的旋转矩阵；
S3.1.2、根据四元数乘法的相关性质，将步骤S3.1.1中的约束方程转化为：
其中，Q1(·)表示四元数的左乘矩阵，Q2(·)表示四元数的右乘矩阵，为两相邻时刻之间IMU旋转增量左乘矩阵与相机旋转增量的右乘矩阵的差值；
S3.1.3、利用关键帧的索引值从0到j帧的测量数据和约束方程(10)相结合构建超定方程：
其中，为权重值，/>为两部分计算结果的接近程度，threshold1为设定的阈值，Qj为超定方程系数集合；
S3.1.4、对步骤S3.1.3中的超定方程做SVD奇异值分解，判断最小奇异值是否大于设置的阈值，如果大于阈值，则停止外参在线估计，相机与IMU的外参的旋转矩阵标定成功；
S3.2、相机与里程计的外参标定：
S3.2.1、根据手眼标定法建立位姿的旋转矩阵和里程计的旋转增量/>的约束方程：
其中，四元数为相机与里程计之间的旋转矩阵；
S3.2.2、根据四元数乘法的相关性质，将步骤S3.2.1中的约束方程转化为：
其中，表示四元数的左乘，/>表示四元数的右乘，/>为两相邻时刻之间里程计旋转增量左乘矩阵与相机旋转增量的右乘矩阵的差值；
S3.2.3、利用关键帧的索引值从0到j帧的测量数据和约束方程(13)相结合构建超定方程：
其中，为权重值，/>为两部分计算结果的接近程度，threshold2为设定的阈值，/>为超定方程系数集合；
S3.2.4、对步骤S3.2.3中的超定方程进行SVD奇异值分解，判断最小奇异值是否大于设置的阈值，如果大于阈值，则停止外参在线估计，相机与里程计的外参的旋转矩阵标定成功。
3.根据权利要求1所述的基于多传感器融合的电力巡检机器人定位方法，其特征在于，所述利用动态加权法将视觉位姿与IMU预积分和里程计预积分进行对齐处理的方法为：
S4.1、视觉位姿与IMU预积分对齐：
S4.1.1、构建相机坐标系下的待优化的向量χI：
其中，为IMU坐标系下j帧的机器人的速度，j＝1,2,…,n'，n'为滑窗中关键帧图像的数量，/>是相机坐标系中的重力向量，s1是由视觉位姿估计与IMU预积分对齐过程获得的视觉的尺度参数；
S4.1.2、将相机坐标系下的所有帧位姿转换到IMU坐标系下：
其中，为第j帧时刻IMU坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻IMU坐标系相对于第一帧相机坐标系的旋转矩阵，/>为第j帧时刻相机坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻相机坐标系相对于第一帧相机坐标系的旋转，/>为第j帧时刻IMU坐标系相对于第一帧相机坐标系的旋转，/>为相机坐标系到IMU坐标系的位置参数，/>为相机坐标系到IMU坐标系的旋转参数；
S4.1.3、将公式(16)与IMU的预积分项相结合，得到两个关键帧之间的预积分的预测值，计算两个关键帧之间的IMU预积分增量与预测值之间的残差：
其中，为第j+1帧时刻到j帧时刻之间的IMU预积分增量与预测值之间的残差，为两相邻关键帧时刻之间由IMU观测计算得到的位置增量与旋转增量标称值，/>为第1帧相机坐标系相对于第j帧时刻IMU坐标系的旋转矩阵，/>为第j+1帧时刻IMU坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻图像在IMU坐标系下的速度，△tj为第j+1帧时刻到第j帧时刻的时间间隔，/>为第j+1帧时刻IMU坐标系相对于第一帧相机坐标系的旋转矩阵，/>为第j+1帧时刻图像在IMU坐标系下的速度，/>为真实尺度的估计量；
S4.1.4、将残差公式(17)写为Hx＝b的形式，得到：
S4.1.5、将公式(18)转化为待优化函数：
其中，为待优化函数的海森矩阵；
S4.1.6、利用线性最小二乘法求解步骤S4.1.5中的待优化函数，得到相机坐标系下的速度、重力向量和尺度参数s1；
S4.2、视觉位姿与里程计预积分对齐：
S4.2.1、构建里程计坐标系下的待优化的向量：
其中，是里程计坐标系下j帧的机器人的速度，s2是由视觉与里程计预积分对齐得到的单目视觉的尺度参数；
S4.2.2、将相机坐标系下的所有帧位姿转换到里程计坐标系下：
其中，为第j帧时刻里程计坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的旋转矩阵，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的旋转，/>为相机坐标系到里程计坐标系的位置参数，/>为相机坐标系到里程计坐标系的旋转参数；
S4.2.3、将公式(21)与里程计的预积分项相结合，得到两个关键帧之间的预积分的预测值，计算两个关键帧之间的里程计预积分增量与预测值之间的残差：
其中，为第j+1帧时刻到j帧时刻之间的里程计预积分增量与预测值之间的残差，/>为/>的集合，/>为第一帧时刻相机坐标系相对于第j帧时刻里程计坐标系的旋转矩阵，/>为第j+1帧时刻里程计坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的位置，/>为第j帧时刻图像在里程计坐标系下的速度，△tj为第j+1帧时刻到第j帧时刻的时间间隔，/>为第j帧时刻里程计坐标系相对于第一帧相机坐标系的位置；
S4.2.4、将残差公式(22)写为Hx＝b的形式，得到：
S4.2.5、将公式(23)转化为待优化函数：
其中，为待优化函数的海森矩阵；
S4.2.6、利用线性最小二乘法求解步骤S4.2.5中的待优化函数，得到里程计坐标系下的速度和尺度参数s2；
S4.3、尺度参数优化：
S4.3.1、设置一个大小为5帧的滑动窗口，对大小为M帧的滑窗进行再操作，也即从滑窗中选择关键帧Bi1∈{1,2,…,M-5}，其中，B1包括滑窗内1-5帧，B2包括滑窗内2-6帧，并以此类推得到BM-5；
S4.3.2、分别计算滑动窗口为Bi1∈{1,2,…,M-5}时的尺度参数s1,i和s2,i，同时计算最小均方误差和/>
S4.3.3、初始化权值W1＝0.5、W2＝0.5，根据最小均方误差和/>降低大误差对应的权值，同时提高小误差对应的权值，保证W1+W2＝1；
S4.3.4、计算尺度参数s＝W1s1+W2s2，当s的计算结果大于0时，则初始化成功。
4.根据权利要求1所述的基于多传感器融合的电力巡检机器人定位方法，其特征在于，所述利用非线性优化法对关键帧进行优化，得到实时视觉位姿的方法为：
S6.1、按照步骤S2.1至步骤S2.6的方法将M帧关键帧添加至滑窗中；
S6.2、计算第M+1帧关键帧与滑窗中所有关键帧的匹配特征点数和平均视差；
S6.3、将平均视差大于视差阈值cd，且匹配特征点数最多的一帧关键帧作为参考帧；
S6.4、利用八点法计算第M+1帧关键帧与参考帧之间的本质矩阵E：
E＝t∧R (25)，
其中，R为旋转矩阵，t为平移向量；
S6.5、利用ENFT-SFM算法计算滑窗中所有关键帧相对于参考帧的相对位姿；
S6.6、利用EPnP方法计算的每一关键帧的位姿作为非线性优化的初值：
其中，Pi'＝[Xi',Yi',Zi']T为第i'个三维空间点坐标，i'＝1,2,…,k，k为匹配的特征点的数量，投影的像素坐标为pi'＝[ui',vi']，K为相机内参矩阵，zi'表示物体在相机坐标系中的z轴坐标；
S6.7、对k个匹配点的重投影误差求和，根据重投影误差和的二范数构建目标函数：
S6.8、利用最小二乘法对目标函数进行最小化得到实时视觉位姿。
5.根据权利要求4所述的基于多传感器融合的电力巡检机器人定位方法，其特征在于，所述根据步骤六中的实时视觉位姿对机器人的位置、速度、角度和IMU中的陀螺仪的偏置进行后端优化，得到机器人的实时位姿的方法为：
S7.1、将机器人的位置、速度、角度和IMU中的陀螺仪的偏置定义为待优化变量：
其中，xj为世界坐标系下j帧的机器人的状态，χ为待优化变量，λk”代表第k”个路标点首次被观测到的逆深度，k”表示3D路标点总数；为由IMU得到的世界坐标系下j帧的机器人的位置，/>为由IMU得到的世界坐标系下j帧的机器人的速度，/>为由IMU得到的世界坐标系下j帧的机器人的角度，bω为陀螺仪的偏置；
S7.2、定义机器人系统中的单目视觉重投影误差、IMU预积分误差、里程计预积分误差和先验误差，通过最小化各测量误差马氏距离平方和可得系统状态向量的最优估计：
其中，为IMU在状态xj与xj+1之间的测量残差，/>为里程计在状态xj与xj+1之间的测量残差，/>为视觉重投影误差，B为IMU的预积分测量值，D为里程计的预积分测量值，C为在滑窗中观察到两次以上的一组特征；rp为窗口中先验信息，Jp为雅可比矩；ρ为Huber核函数，P是协方差矩阵。
6.根据权利要求5所述的基于多传感器融合的电力巡检机器人定位方法，其特征在于，所述视觉重投影误差为：
其中，表示第j帧关键帧在归一化相机平面上的投影，/>为投影的横坐标，/>为投影的纵坐标，且/> 为IMU坐标系到相机坐标系的旋转矩阵，/>为世界坐标系相对于第j+1帧时刻IMU坐标系的旋转矩阵，/>为第j+1帧时刻IMU坐标系相对于世界坐标系的旋转矩阵，/>为第j帧图像像素横坐标，/>为第j帧图像像素纵坐标，/>为相机坐标系相对于IMU坐标系的位置变换，/>为第j帧时刻IMU坐标系相对于世界坐标系的位置变换，/>为第j+1帧时刻IMU坐标系相对于世界坐标系的位置变换；
所述IMU在状态xj与xj+1之间的测量残差为：
其中，是四元数的三维误差状态表示，[q]xyz表示提取四元数q的虚部；
所述里程计在状态xj与xj+1之间的测量残差为：
7.根据权利要求4所述的基于多传感器融合的电力巡检机器人定位方法，其特征在于，所述将步骤六中的关键帧添加到关键帧数据库的方法为：首先将关键帧添加到滑窗，直至滑窗内的关键帧的数量为M，当第M+1帧关键帧出现时，将滑窗中的第一帧关键帧移入关键帧数据库中，将第M+1帧关键帧添加至滑窗，保证滑窗内的关键帧的数量为M。
8.根据权利要求1所述的基于多传感器融合的电力巡检机器人定位方法，其特征在于，所述对闭环帧进行重定位后，再对关键帧数据库中的关键帧进行闭环优化的方法为：
S10.1、将当前帧图像和闭环帧的位姿和特征点作为视觉约束项，构建目标函数，得到当前帧与闭环帧之间的相对位姿：
其中，为当前帧图像与闭环帧之间的重投影误差，Γ为闭环帧中检测到的特征集合，(l,v)为闭环帧v中第l个特征观测，ρ为Huber核函数，j1＝1,2,…,H1为关键帧数据库中第j1帧关键帧，H1为当前帧图像至闭环帧之间的关键帧的数量；rp为边缘化的先验信息，Jp为先验信息对待优化变量的雅可比矩阵，χ为目标函数的待优化变量，/>为视觉的重投影残差，/>为由相邻两帧图像的观测计算的重投影像素坐标标称值，/>为由第v帧闭环帧图像对路标点l的观测计算出的像素坐标标称值，/>为闭环边L的相对旋转，为闭环边L的相对位置，/>为里程计观测值计算出的位置增量与旋转增量标称值，为里程计的测量残差，/>为相邻两关键帧视觉观测噪声的协方差矩阵，/>为相邻两关键帧时刻里程计观测噪声的协方差矩阵，/>为当前帧与闭环帧视觉观测噪声的协方差矩阵，/>为IMU的测量残差，/>为IMU观测值计算出的位置增量与旋转增量标称值，/>为相邻两关键帧时刻IMU观测噪声的协方差矩阵，C为第j帧时刻的关键帧对第各路标点观测的集合，B为用于IMU预积分的第j帧时刻关键帧的集合，D为用于里程计预积分的第j帧时刻关键帧的集合；
S10.2、计算当前帧图像与闭环帧之间的所有关键帧的相邻两帧之间的残差：
其中，为第j1帧关键帧与第j1+1帧关键帧之间的残差，/>为第j1帧关键帧的翻滚角，/>为第j1帧关键帧的俯仰角，/>为第j1+1帧关键帧的位置，ψi为第j1帧关键帧的偏航角，/>为第j1帧关键帧的位置，/>为第j1帧关键帧的偏航角，/>第j1+1帧关键帧的偏航角，/>为第j1帧关键帧与第j1+1帧关键帧之间的相对位移，/>为第j1帧关键帧与第j1+1帧关键帧之间的相对偏航角；
S10.3、根据步骤S10.2的残差构建整体目标函数：
其中，ξ＝{R，t}为序列边，R为旋转矩阵，t为平移向量，为闭环边。/>
CN202010581200.6A 2020-06-23 2020-06-23 一种基于多传感器融合的电力巡检机器人定位方法 Active CN111739063B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010581200.6A CN111739063B (zh) 2020-06-23 2020-06-23 一种基于多传感器融合的电力巡检机器人定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010581200.6A CN111739063B (zh) 2020-06-23 2020-06-23 一种基于多传感器融合的电力巡检机器人定位方法

Publications (2)

Publication Number Publication Date
CN111739063A CN111739063A (zh) 2020-10-02
CN111739063B true CN111739063B (zh) 2023-08-18

Family

ID=72650606

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010581200.6A Active CN111739063B (zh) 2020-06-23 2020-06-23 一种基于多传感器融合的电力巡检机器人定位方法

Country Status (1)

Country Link
CN (1) CN111739063B (zh)

Families Citing this family (36)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112308917A (zh) * 2020-10-15 2021-02-02 江苏大学 一种基于视觉的移动机器人定位方法
CN112434559A (zh) * 2020-10-26 2021-03-02 广东凯宝机器人科技有限公司 一种机器人识别定位方法
CN112652001B (zh) * 2020-11-13 2023-03-31 山东交通学院 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统
CN114518108B (zh) * 2020-11-18 2023-09-08 宇通客车股份有限公司 一种定位地图构建方法及装置
CN112506190A (zh) * 2020-11-19 2021-03-16 深圳市优必选科技股份有限公司 一种机器人定位方法、机器人定位装置及机器人
CN112450820B (zh) * 2020-11-23 2022-01-21 深圳市银星智能科技股份有限公司 位姿优化方法、移动机器人及存储介质
CN112197770B (zh) * 2020-12-02 2021-03-12 北京欣奕华数字科技有限公司 一种机器人的定位方法及其定位装置
CN112697128B (zh) * 2020-12-04 2024-03-01 盐城中科高通量计算研究院有限公司 一种巡逻车用路线循迹方法
CN112197764B (zh) * 2020-12-07 2021-04-06 广州极飞科技有限公司 实时位姿确定方法、装置及电子设备
CN112712018B (zh) * 2020-12-29 2024-06-28 东软睿驰汽车技术（沈阳）有限公司 标识地图的建立方法、视觉定位方法及装置
CN112729344B (zh) * 2020-12-30 2022-09-13 珠海市岭南大数据研究院 无需参照物的传感器外参标定方法
CN112344923B (zh) * 2021-01-11 2021-04-16 浙江欣奕华智能科技有限公司 一种机器人的定位方法及其定位装置
CN112880687B (zh) * 2021-01-21 2024-05-17 深圳市普渡科技有限公司 一种室内定位方法、装置、设备和计算机可读存储介质
CN112967341B (zh) * 2021-02-23 2023-04-25 湖北枫丹白露智慧标识科技有限公司 基于实景图像的室内视觉定位方法、系统、设备及存储介质
CN112837374B (zh) * 2021-03-09 2023-11-03 中国矿业大学 一种空间定位方法及系统
CN113029127B (zh) * 2021-03-10 2023-05-09 中国人民解放军海军航空大学 基于分布式循环结构的飞行器自主姿态估计方法
CN113091733A (zh) * 2021-03-15 2021-07-09 武汉大学 一种基于毫米波雷达与imu融合的实时定位装置及方法
CN113155121B (zh) * 2021-03-22 2024-04-02 珠海深圳清华大学研究院创新中心 一种车辆定位方法、装置及电子设备
CN113052241A (zh) * 2021-03-28 2021-06-29 重庆长安汽车股份有限公司 一种多传感器数据融合方法、装置及汽车
CN113066127B (zh) * 2021-04-02 2024-04-19 视辰信息科技(上海)有限公司 一种在线标定设备参数的视觉惯性里程计方法和系统
CN113192222B (zh) * 2021-04-14 2022-11-15 山东理工大学 一种电力线路可视化告警的设备级告警策略方法
CN113237484A (zh) * 2021-04-21 2021-08-10 四川轻化工大学 基于slam的相机和imu的外部旋转参数求解方法
CN113345006A (zh) * 2021-04-28 2021-09-03 视辰信息科技(上海)有限公司 一种闭环检测方法、装置、电子设备和存储介质
CN113311452B (zh) * 2021-05-26 2022-12-30 上海新纪元机器人有限公司 一种基于多传感器的定位方法及其系统
CN113467464A (zh) * 2021-07-17 2021-10-01 普达迪泰（天津）智能装备科技有限公司 一种用于无人车轨迹模拟测试的卫星定位运动轨迹方法
CN113701760B (zh) * 2021-09-01 2024-02-27 火种源码(中山)科技有限公司 基于滑动窗口位姿图优化的机器人抗干扰定位方法及装置
CN113470121B (zh) * 2021-09-06 2021-12-28 深圳市普渡科技有限公司 自主移动平台、外参优化方法、装置及存储介质
CN113654555A (zh) * 2021-09-14 2021-11-16 上海智驾汽车科技有限公司 一种基于多传感器数据融合的自动驾驶车辆高精定位方法
CN114167866B (zh) * 2021-12-02 2024-04-12 桂林电子科技大学 一种智能物流机器人及控制方法
CN114234967B (zh) * 2021-12-16 2023-10-20 浙江大学 一种基于多传感器融合的六足机器人定位方法
CN114623817B (zh) * 2022-02-21 2024-04-26 武汉大学 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法
CN116222556B (zh) * 2023-01-13 2024-03-26 浙江大学 一种基于多源传感器融合的室内定位方法及系统
CN115861322B (zh) * 2023-02-28 2023-05-16 江西省智能产业技术创新研究院 口腔区域定位方法、系统、可读存储介质及电子设备
CN116408808B (zh) * 2023-06-09 2023-08-01 未来机器人（深圳）有限公司 机器人取货检测方法及装置、机器人
CN117034191A (zh) * 2023-08-04 2023-11-10 广东省机场管理集团有限公司工程建设指挥部 基于5g云平台的车辆多源信息融合方法、装置及介质
CN117584989A (zh) * 2023-11-23 2024-02-23 昆明理工大学 一种激光雷达/imu/车辆运动学约束紧耦合slam系统及算法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN111121767A (zh) * 2019-12-18 2020-05-08 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN111121767A (zh) * 2019-12-18 2020-05-08 南京理工大学 一种融合gps的机器人视觉惯导组合定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于ORB关键帧匹配算法的机器人SLAM实现;艾青林;余杰;胡克用;陈琦;;机电工程(05);全文 *

Also Published As

Publication number Publication date
CN111739063A (zh) 2020-10-02

Similar Documents

Publication Publication Date Title
CN111739063B (zh) 一种基于多传感器融合的电力巡检机器人定位方法
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
Barnes et al. Driven to distraction: Self-supervised distractor learning for robust monocular visual odometry in urban environments
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN102538781B (zh) 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN108682027A (zh) 基于点、线特征融合的vSLAM实现方法及系统
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
JP2018522345A (ja) リアルタイムのマッピングと位置確認のための方法及び装置
Xiao et al. Monocular vehicle self-localization method based on compact semantic map
CN112115874B (zh) 一种融合云端的视觉slam系统及方法
CN111707272A (zh) 一种地下车库自动驾驶激光定位系统
CN116878501A (zh) 一种基于多传感器融合的高精度定位与建图系统及方法
Zhang LILO: A novel LiDAR–IMU SLAM system with loop optimization
Chen et al. I2D-Loc: Camera localization via image to lidar depth flow
Zhou et al. Visual localization and mapping leveraging the constraints of local ground manifolds
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
Rozsypálek et al. Multidimensional particle filter for long-term visual teach and repeat in changing environments
Zeng et al. Monocular visual odometry using template matching and IMU
Chi et al. Low-latency Visual-based High-Quality 3D Reconstruction using Point Cloud Optimization
Liu et al. A multi-sensor fusion with automatic vision-LiDAR calibration based on Factor graph joint optimization for SLAM
Zhang et al. A robust lidar slam system based on multi-sensor fusion
CN115797490A (zh) 基于激光视觉融合的建图方法及系统
CN113720323A (zh) 基于点线特征融合的单目视觉贯导slam方法及装置
Hu et al. A novel lidar inertial odometry with moving object detection for dynamic scenes

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