CN112945233A - 一种全局无漂移的自主机器人同时定位与地图构建方法 - Google Patents

一种全局无漂移的自主机器人同时定位与地图构建方法 Download PDF

Info

Publication number
CN112945233A
CN112945233A CN202110057910.3A CN202110057910A CN112945233A CN 112945233 A CN112945233 A CN 112945233A CN 202110057910 A CN202110057910 A CN 202110057910A CN 112945233 A CN112945233 A CN 112945233A
Authority
CN
China
Prior art keywords
global
pose
representing
camera
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110057910.3A
Other languages
English (en)
Other versions
CN112945233B (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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202110057910.3A priority Critical patent/CN112945233B/zh
Publication of CN112945233A publication Critical patent/CN112945233A/zh
Application granted granted Critical
Publication of CN112945233B publication Critical patent/CN112945233B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种全局无漂移的自主机器人同时定位与地图构建方法，在局部状态估计中加入线特征更加直观的表示环境的几何结构信息，同时使用基于线性误差的直线表示方法，并通过一种局部状态和全局传感器信息融合的算法，有效解决了大尺度弱纹理场景下的精准状态估计问题，实现了局部精确和全局无漂移的位姿估计，提高了重复线特征纹理场景中的鲁棒性。

Description

一种全局无漂移的自主机器人同时定位与地图构建方法
技术领域
本发明涉及一种自主机器人同时定位与地图构建方法，尤其涉及一种全局无漂移的自主机器人同时定位与地图构建方法，属于机器人智能控制领域。
背景技术
同时定位与地图创建(Simultaneous Localization and Mapping,SLAM)系统是自主导航关键技术，视觉惯性融合的SLAM系统可通过其惯性测量单元(IMU)敏感到的加速度和角速度信息来改善相机因强烈光照变化等引起的问题，其在小范围内定位精度高。
然而在大尺度弱纹理场景下自主机器人状态估计技术仍然面临巨大挑战。一方面，基于特征点法的视觉惯性同时定位与地图创建(SLAM)方法在弱纹理的场景中，由于较难提取大量有效特征点使得定位精度大幅降低，纹理较差的视频或由于运动模糊等原因甚至造成系统完全失效。现有技术中还具有直接法(EngelJ,SchopsT,CremersD,et al.LSD-SLAM:Large-scale direct monocular SLAM[C].13th Euro-pean Conference onComputer Vision,Zurich,Switzerland,September 6-12,2014.)，其在某种程度上缓解了特征点的依赖问题，但是稠密和半稠密的直接跟踪法计算量较大，无法在一些计算能力有限的平台上运行。
另一方面，大尺度场景用到的多源信息融合方法中，局部定位传感器(如相机、IMU、激光雷达等)未考虑全局定位的误差，全局测量(如GNSS、磁力计、气压计等)通常含有噪声且采集频率较低，在做高精度定位时无法单独使用。
由于上述原因，本发明人对现有的自主机器人状态估计系统做了深入的研究，提出了一种全局无漂移的自主机器人同时定位与地图构建方法，以期解决上述问题。
发明内容
为了克服上述问题，本发明人进行了锐意研究，设计出一种全局无漂移的自主机器人同时定位与地图构建方法，将局部定位和全局感知定位融合，实现精确的全局感知定位，
所述局部定位包括采用相机、惯性测量单元、激光雷达进行状态估计；
所述全局感知定位包括全球导航卫星系统、磁力计、气压计进行状态估计。
进一步地，将视觉惯性与全球导航卫星系统进行多源融合，所述视觉惯性包括惯性测量单元和相机。
根据本发明，该方法包括以下步骤：
S1、前端点线特征的跟踪；
S2、后端视觉惯性SLAM位姿态联合优化；
S3、与GNSS位姿融合获得全局位姿。
在步骤S1中，采用线性误差将线性特征表示为直线端点上的线性约束，从而将线特征自然地整合到基于特征点算法的线性表示中。
具体地，步骤S1包含以下子步骤：
S11、建立点线特征表示观测模型；
S12、建立IMU预积分观测模型。
根据本发明，在步骤S1中，由相机的光心和pi,k及qi,k构成的直线可以确定一个平面，其单位法向量li,k表示为：
Figure BDA0002901280460000021
其中，l1、l2、l3为参数系数，为待求解值，i表示相机不同帧图像，在一帧图像中通常具有多个点特征，观测到的第j个特征点的3D坐标表示为Xw,j∈R3，第k条线段端点为Pw,k、Qw,k∈R3，其在图像平面的投影坐标为
Figure BDA0002901280460000031
Figure BDA0002901280460000032
对应的齐次坐标为
Figure BDA0002901280460000033
R3表示三维坐标、R2表示二维坐标；
相机的投影模型表示为π，齐次坐标表示为πh：
Figure BDA0002901280460000034
其中，Rw以及tw分别表示Tiw的旋转和平移，Tiw表示相机第i帧的位姿；w表示世界坐标系；Pw∈R3表示世界坐标系下的点坐标，O是其对应的相机坐标系下的点坐标，fx、fy和cx、cy分别为相机的焦距和主点；
所述点线特征表示观测模型通过点特征的重投影误差
Figure BDA0002901280460000035
和线特征的重投影误差
Figure BDA0002901280460000036
表示，分别为：
Figure BDA0002901280460000037
其中，上标p表示点，上标l表示线，xi，j，yi，j表示第i帧匹配得到的第j个特征点的像素坐标，dpi,k和dqi,k分别为pi,k和qi,k到投影直线
Figure BDA0002901280460000038
的距离。
在步骤S12中，离散时间为k和k+1之间的IMU预积分观测模型为：
Figure BDA0002901280460000039
其中，bm表示当前帧，bm+1表示下一帧；
Figure BDA0002901280460000041
表示离散形式的第m个IMU时刻到第m+1个IMU时刻积分得到的的位置、速度和旋转的增量；
Figure BDA0002901280460000042
表示上一时刻前后两帧之间经过中值积分得到的IMU增量信息；
Figure BDA0002901280460000043
表示加速度，
Figure BDA0002901280460000044
表示角速度。
根据本发明，在S2中，通过最小化所有测量残差的代价项之和来优化滑动窗口中的所有状态变量，从而获得优化后的位姿，可以表示为：
Figure BDA0002901280460000045
其中，
Figure BDA0002901280460000046
为本体坐标系中i帧到i+1帧时刻IMU测量残差，χb为滑动窗口中所有IMU预积分的集合；
Figure BDA0002901280460000047
与
Figure BDA0002901280460000048
分别为点特征和线特征的重投影误差可根据式3获得，χp和χl分别为i帧观测到的点特征和线特征集合；ρ是用于抑制异常值的Cauchy鲁棒函数；eprior为滑动窗口边缘化一帧之后计算出的先验信息，在相机初始化过程中获得。
根据本发明，在S3中，将步骤S2获得的优化后的位姿与GNSS位姿融合从而获得全局位姿，融合过程可以通过下式表示：
Figure BDA0002901280460000049
其中，χ*表示融合后的全局位姿，S为测量值的集合，包含视觉惯性局部测量和全局测量GNSS，p表示最大似然估计，
Figure BDA00029012804600000410
表示t时刻所有测量值；
所有点的全局位姿χ＝{x0，x1...，xn}，其中
Figure BDA0002901280460000051
pw是全局坐标系下的位置，qw是全局坐标系下的方位。
根据本发明，在步骤S3中，将步骤S2获得的优化后的位姿与GNSS位姿融合从而获得全局位姿，融合过程可表示为：
Figure BDA0002901280460000052
其中，χ*表示融合后的全局位姿，S为测量值的集合，包含视觉惯性局部测量和全局测量GNSS，
Figure BDA0002901280460000053
表示视觉惯性协方差，
Figure BDA0002901280460000054
表示GNSS协方差，
Figure BDA0002901280460000055
表示t时刻视觉惯性的测量值，
Figure BDA0002901280460000056
表示t时刻视觉惯性测量模型的得到的测量值，
Figure BDA0002901280460000057
表示t时刻GNSS的测量值，
Figure BDA0002901280460000058
表示t时刻GNSS测量模型的得到的测量值。
本发明所具有的有益效果包括：
(1)根据本发明提供的全局无漂移的自主机器人同时定位与地图构建方法，能够使自主机器人在大尺度弱纹理场景中实现局部精准和全局无漂移的姿态估计；
(2)根据本发明提供的全局无漂移的自主机器人同时定位与地图构建方法，在局部状态估计中加入线特征，有效提升了关键帧之间相对位姿估计的准确性；
(3)根据本发明提供的全局无漂移的自主机器人同时定位与地图构建方法，将线性特征表示为直线端点上的线性约束，从而将线特征自然地整合到基于特征点算法的线性表示中，有效改善算法在重复线特征场景下的鲁棒性；
(4)根据本发明提供的全局无漂移的自主机器人同时定位与地图构建方法，实现了局部精确和全局无漂移的位姿估计。
附图说明
图1示出根据本发明一种优选实施方式的自主机器人同时定位与地图构建方法流程示意图；
图2示出一种自主机器人同时定位与地图构建方法中线特征的投影误差示意图；
图3示出一种自主机器人同时定位与地图构建方法中线特征的投影误差示意图；
图4示出一种优选实施方式的自主机器人同时定位与地图构建方法预积分过程示意图；
图5示出实验例2中仿真轨迹图；
图6示出实验例2中仿真轨迹图；
图7示出实验例2中仿真轨迹图；
图8示出实验例2中仿真轨迹图；
图9示出实验例2中仿真轨迹图；
图10示出实验例2中仿真轨迹图；
图11示出实验例3中仿真轨迹图；
图12示出实验例4中仿真轨迹图。
具体实施方式
下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。
在这里专用的词“示例性”意为“用作例子、实施例或说明性”。这里作为“示例性”所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
在机器人状态估计中，根据使用的传感器类型不同，可以分为局部定位和全局感知定位。
局部定位是指采用相机、IMU、激光雷达等进行六自由度状态估计，然而，相机在快速移动或者光照发生剧烈变化时特征点容易跟踪丢失、IMU长时间运行会存在累计误差产生漂移、激光雷达对玻璃等物体存在反射，使得局部定位精度难以提高。
全局定位通常包含全球导航卫星系统(Global navigationsatellite system,GNSS)、磁力计、气压计等，全局感知定位的测量相对于地球框架，独立于起始点，可有效避免累计漂移的问题。然而，全局测量通常含有噪声且采集频率较低，若只有全局传感器不足以实现6自由度状态的实时估计，在做高精度定位时往往无法单独使用。
在本发明中，将局部定位和全局感知定位融合，从而实现精确的全局感知定位。
进一步地，在本发明中，将视觉惯性与全球导航卫星系统(GNSS)进行多源融合，从而提高状态估计的精度和鲁棒性。
所述视觉惯性包括惯性测量单元(IMU)和相机。
具体地，根据本发明提供的全局无漂移的自主机器人同时定位与地图构建方法，包括以下步骤将局部定位和全局感知定位融合：
S1、前端点线特征的跟踪；
S2、后端视觉惯性SLAM位姿态联合优化；
S3、与GNSS位姿融合获得全局位姿。
在步骤S1中，采用光流法对相机采集相邻图像帧之间进行前端数据关联，从而进行特征的跟踪。
所述光流法(Optical flow or optic flow)是关于视域中的物体运动检测中的概念，用来描述相对于观察者的运动所造成的观测目标、表面或边缘的运动，是一种常用的视觉识别检测方法，在本发明中不做特别赘述。
在传统的光流法进行前端数据关联中，大都基于点特征进行，例如CamposC,ElviraR,JuanJ,et al.ORB-SLAM3:An accurate open-source library for visual,visual-inertial and multi-map SLAM[J].arXiv:2007.11898(2020)中提到的方法。
然而，点特征表示的物体在较大的视角变化下不稳定，并且光照不变性较差；在弱光照下鲁棒性也较差。
在本发明中，采用线性误差将将线性特征表示为直线端点上的线性约束，从而将线特征自然地整合到基于特征点算法的线性表示中，有效改善算法在重复线特征场景下的鲁棒性。
具体地，步骤S1包含以下子步骤：
S11、建立点线特征表示观测模型；
S12、建立IMU预积分观测模型。
在步骤S11中，线特征的投影误差如图2、图3所示，其中，i表示相机不同帧图像，相机第i帧的位姿Tiw∈SE(3)，SE(3)表示欧式变换；w表示世界坐标系，在一帧图像中通常具有多个点特征，观测到的第j个特征点的3D坐标表示为Xw,j∈R3，第k条线段端点为Pw,k、Qw,k∈R3，其在图像平面的投影坐标为
Figure BDA0002901280460000084
对应的齐次坐标为
Figure BDA0002901280460000081
Pw,k、Qw,k在图像平面对应的检测到的2D点坐标为pi,k、qi,k∈R2，对应的齐次坐标为
Figure BDA0002901280460000082
R3表示三维坐标、R2表示二维坐标。
进一步地，在本发明中，由相机的光心和pi,k及qi,k构成的直线可以确定一个平面，其单位法向量li,k表示为：
Figure BDA0002901280460000083
其中，l1、l2、l3为参数系数，为待求解值，
Figure BDA0002901280460000091
的可从图像中获取。
在本发明中，相机的投影模型表示为π，齐次坐标表示为πh：
Figure BDA0002901280460000092
其中，Rw∈so(3)以及tw分别表示Tiw的旋转和平移；Pw∈R3表示世界坐标系下的点坐标，O是其对应的相机坐标系下的点坐标，fx、fy和cx、cy分别为相机的焦距和主点。
进一步地，在本发明中所述点线特征表示观测模型通过点特征的重投影误差
Figure BDA0002901280460000093
和线特征的重投影误差
Figure BDA0002901280460000094
表示，分别为：
Figure BDA0002901280460000095
其中，上标p表示点，上标l表示线，xi，j，yi，j表示第i帧匹配得到的第j个特征点的像素坐标，dpi,k和dqi,k分别为pi,k和qi,k到投影直线
Figure BDA0002901280460000096
的距离。
进一步地，在式3中，线特征重投影误差对相机位姿扰动的雅可比矩阵为：
Figure BDA0002901280460000097
其中，ξ为Tiw的李代数表示形式，[]∧为反对称矩阵，可以表示为：
Figure BDA0002901280460000101
进一步地，线性特征重投影误差对线段端点Pi，k，Qi，k的雅可比矩阵为：
Figure BDA0002901280460000102
进一步地，在公式6中，
Figure BDA0002901280460000103
是Op点的坐标分量，Op表示点Pw，k对应的相机坐标，
Figure BDA0002901280460000104
是Oq点的坐标分量，Oq表示点Qw，k对应的相机坐标，可以通过下式表示；
Figure BDA0002901280460000105
其中，Riw世界坐标系到相机坐标系的旋转，tiw表示世界坐标系到相机坐标系的平移。
在步骤S12中，利用惯性测量数据优化视觉测量的尺度因子，并对齐两个传感器的位姿估计结果，实现视觉惯性联合位姿校准。
进一步地，IMU的状态传递方程可以表示为：
Figure BDA0002901280460000111
将第i帧和第i+1帧之间的所有IMU进行积分，得到第i+1帧的位置
Figure BDA0002901280460000112
速度
Figure BDA0002901280460000113
旋转
Figure BDA0002901280460000114
Figure BDA0002901280460000115
表示第i帧的速度，
Figure BDA0002901280460000116
表示第i帧本体坐标系到世界坐标系的旋转，
Figure BDA0002901280460000117
表示本体坐标系下IMU测量的加速度，t表示从第i帧到第i+1帧的时间；
gw表示世界坐标系下重力加速度，
Figure BDA0002901280460000118
表示IMU的角速度。
IMU的状态传递依赖于第i帧的旋转、位置和速度，在传统的算法中，每次机器人调整位姿，均需重新迭代更新第i帧的速度和旋转，并需要根据每次迭代后的值重新进行积分，使得该传递策略非常耗时。
在本发明中，将优化变量从第i帧到第i+1帧的IMU预积分项中分离出来：
Figure BDA0002901280460000119
其中，
Figure BDA00029012804600001110
表示世界坐标系到本体坐标系的旋转，
Figure BDA00029012804600001111
表示世界坐标系到本体坐标系下的旋转；
Figure BDA00029012804600001112
为预积分测量值，进一步地，其满足：
Figure BDA0002901280460000121
其中，
Figure BDA0002901280460000122
表示世界坐标系下bt时刻到bi时刻的旋转，
Figure BDA0002901280460000123
表示本体坐标系下bt时刻到bi时刻的旋转。
由于IMU测量值通常含有噪声，且采用离散时间形式采集，考虑噪声并在离散时间条件下，根据式(10)可以获得离散时间为k和k+1之间的IMU预积分观测模型：
Figure BDA0002901280460000124
其中，bm表示当前帧，bm+1表示下一帧；
Figure BDA0002901280460000125
表示离散形式的第m个IMU时刻到第m+1个IMU时刻积分得到的的位置、速度、旋转的增量；
Figure BDA0002901280460000126
表示上一时刻前后两帧之间经过中值积分得到的IMU增量信息；
Figure BDA0002901280460000127
表示加速度，
Figure BDA0002901280460000128
表示角速度。
进一步地，
Figure BDA0002901280460000131
其中，上标表示bm帧和bm+1帧，
Figure BDA0002901280460000132
表示IMU偏置，
Figure BDA0002901280460000133
表示bm帧中的角速度白噪声、
Figure BDA0002901280460000134
表示bm帧中的加速度白噪声。
预积分的过程如图4所示，在本发明中，由于偏置更新较小，可以将预积分近似为：
Figure BDA0002901280460000135
其中，
Figure BDA0002901280460000136
为预积分，
Figure BDA0002901280460000137
Figure BDA0002901280460000138
是第i帧时刻预积分对各偏置的雅克比矩阵,n表示第n帧；
Figure BDA0002901280460000139
表示IMU加速度偏置。
IMU的测量残差为：
Figure BDA0002901280460000141
其中，[]xyz是以四元数形式表示的旋转误差的实部；
Figure BDA0002901280460000142
为bn帧中角速度偏置。
在S2中，通过最小化所有测量残差的代价项之和来优化滑动窗口中的所有状态变量，从而获得优化后的位姿，可以表示为：
Figure BDA0002901280460000143
其中，
Figure BDA0002901280460000144
为本体坐标系中i帧到i+1帧时刻IMU测量残差，可根据式14获得，χb为滑动窗口中所有IMU预积分的集合，可根据式11获得；
Figure BDA0002901280460000145
与
Figure BDA0002901280460000146
分别为点特征和线特征的重投影误差可根据式3获得，χp和χl分别为i帧观测到的点特征和线特征集合；ρ是用于抑制异常值的Cauchy鲁棒函数；eprior为滑动窗口边缘化一帧之后计算出的先验信息，在相机初始化过程中获得，具体获得方式本领域技术人员可根据经验设置，例如文献MascaroR,TeixeiraL,HinamannT,et al.GOMSF:Graph-optimization based multi-sensor fusionfor ro-bust uav pose estimation[C].IEEE International Con-ference on Roboticsand Automation.Brisbane,Aus-tralia,May 21-25,2018.中提到的方法，在本发明中不做特别赘述。
在S3中，将步骤S2获得的优化后的位姿与GNSS位姿融合从而获得全局位姿。
具体地，在融合过程中，待估计的变量是所有点的全局位姿χ＝{x0，x1...，xn}，其中
Figure BDA0002901280460000151
pw是全局坐标系下的位置，qw是全局坐标系下的方位。
进一步度，融合过程可以通过下式表示：
Figure BDA0002901280460000152
其中，χ*表示融合后的全局位姿，S为测量值的集合，包含视觉惯性局部测量和全局测量GNSS，p表示最大似然规律，
Figure BDA0002901280460000153
表示t时刻所有测量值。
进一步地，测量值的不确定度符合高斯分布，式16可表示为：
Figure BDA0002901280460000154
其中，
Figure BDA0002901280460000161
表示t时刻测量模型得到的测量值；
Figure BDA0002901280460000162
表示协方差。
由于视觉惯性局部估计在小范围内是精确的，因此可以利用两帧之间的相对姿态。考虑到两个连续帧t-1和帧t，可获得视觉惯性因子：
Figure BDA0002901280460000163
其中，
Figure BDA0002901280460000164
和
Figure BDA0002901280460000165
分别表示局部估计在连续帧t-1和帧t的位姿，
Figure BDA0002901280460000166
为位置和旋转状态的四元数差值，式18中，等式右侧首行为两姿态的相对位置误差，第二行表示两姿态的相对旋转误差，
Figure BDA0002901280460000167
表示t时刻视觉惯性的测量值、
Figure BDA0002901280460000168
表示t时刻视觉惯性测量模型的得到的测量值、xt-1为t-1时刻的全局位姿、
Figure BDA0002901280460000169
为视觉惯性局部帧中t-1时刻的旋转、
Figure BDA00029012804600001610
视觉惯性局部估计中t时刻位置、
Figure BDA00029012804600001611
为全局帧t-1时刻旋转的逆、
Figure BDA00029012804600001612
全局帧t时刻位置、
Figure BDA00029012804600001613
全局帧t时刻旋转。
GNSS得到的位置原始测量值可以表示为
Figure BDA00029012804600001614
则GNSS因子表示为：
Figure BDA00029012804600001615
其中，
Figure BDA00029012804600001616
表示t时刻GNSS的测量值，
Figure BDA00029012804600001617
表示t时刻GNSS测量模型的得到的测量值，
Figure BDA00029012804600001618
为GNSS原始测量值，Pt w为测量模型得到的测量值。
根据式18～19，可将式17表示为：
Figure BDA00029012804600001619
其中，
Figure BDA0002901280460000171
表示视觉惯性协方差，
Figure BDA0002901280460000172
表示GNSS协方差，从而获得融合后的全局位姿。
实施例
实施例1
进行仿真实验，选用EuRoC数据集作为实验数据。EuRoC数据集是由苏黎世联邦理工采集的MAV(Micro Aerial Vehicle微型飞行器)视觉惯性数据集。数据集包括立体图像(Aptina MT9V034全局快门，单色WVGA，相机频率20FPS)，并同步IMU(ADIS 16448,200Hz)数据以及精准的地面真实状态(动作捕捉系统VICON，激光追踪器LeicaMS50)，数据集中提供了所有外部和内部参数，在仿真实验中，只使用左摄像头的图像。
仿真实验按照如下步骤进行：
S1、前端点线特征的跟踪；
S2、后端视觉惯性SLAM位姿态联合优化；
S3、与GNSS位姿融合获得全局位姿。
在步骤S1中，建立的点线特征表示观测模型为：
Figure BDA0002901280460000173
建立的IMU预积分观测模型为：
Figure BDA0002901280460000174
IMU的测量残差为：
Figure BDA0002901280460000181
在步骤S2中，通过最小化所有测量残差的代价项之和来优化滑动窗口中的所有状态变量，从而获得优化后的位姿，可以表示为：
Figure BDA0002901280460000182
其中，
Figure BDA0002901280460000183
为本体坐标系中i到i+1时刻IMU测量残差，可根据式14获得，χb为滑动窗口中所有IMU预积分的集合，可根据式11获得；
Figure BDA0002901280460000184
与
Figure BDA0002901280460000185
分别为点特征和线特征的重投影误差可根据式3获得，χp和χl分别为i帧观测到的点特征和线特征集合；ρ是用于抑制异常值的Cauchy鲁棒函数，eprior为滑动窗口边缘化一帧之后计算出的先验信息，在相机初始化过程中获得。
在步骤S3中，将步骤S2获得的优化后的位姿与GNSS位姿融合从而获得全局位姿，融合过程可表示为：
Figure BDA0002901280460000191
实施例2
重复实施例1，区别在于，选用KITTI数据集作为实验数据。KITTI数据集由德国卡尔斯鲁厄理工学院和丰田美国技术研究院联合记录，是目前最大的自动驾驶场景下的计算机视觉算法评测数据集，KITTI包含市区、乡村和高速公路等场景采集的真实图像数据，有各种程度的遮挡与截断，其中采用的传感器包含立体图像(相机型号：PointGrey Flea 2，单色1382×512，相机频率为10PFS)和GPS，地面真实状态由惯性导航系统(OXTS RT 3003)提供。
对比例1
采用与实施例1相同的数据集，采用Leutenegger S,Lynen S,Bosse M,etal.Keyframe-based visual-inertial odometryusing nonlinear opti-mization[J].International Journal of Robotics Re-search,2014,34(3):314-334.中的OKVIS方法进行仿真。
对比例2
采用与实施例1相同的数据集，采用QinT,LiP L,ShenS J.VINS-Mono:A robustandversatile monocular visual-inertial state estimator[J].IEEE Transactionson Robotics,2018,34(4):1004-1020.中的VINS-mono方法进行仿真。
对比例3
采用与实施例1相同的数据集，采用HeYJ,ZhaoJ,GuoY,et al.PL-VIO:Tightly-coupled monocular visual-inertial odometry using point and line features[J].Sensors,2018,18(4):1159.中的PL-VIO方法进行仿真。
对比例4
采用与实施例2相同的数据集，采用Qin T,Cao SH Z,Pan J,etal.A generaloptimiza-tion-based framework for local odometry estimation with multiplesensors[J].arXiv:1901.03638(2019).中的VINS-Fusion方法进行仿真。
实验例1
对比实施例1、对比例1～3在EuRoC数据集11个序列估计的轨迹和真实轨迹的均方根误差如表一所示。
表一EuRoC数据集上位移(单位:米)与旋转(单位:度)均方根误差RMSE比较
Figure BDA0002901280460000201
通过表一可明显看出，实施例1中均方根误差明显低于基于点特征的VIO算法OKVIS(对比例1)与VINS-Mono(对比例2)；而与同样是点线特征提取的PL-VIO(对比例3)相对比，PL-VIO在V1_02_medium数据集运行时，出现追踪失败导致严重漂移没有得到最终轨迹，而实施例1中无漂移现象，表明实施例1具有更好的鲁棒性。
另外，实施例1在快速运动、特征稀疏的数据集V2_03上仍能够保持0.2m左右的高精度定位，精度相比对比例3提升了19％左右，较未添加线特征和全局定位的对比例2提升39％左右。且实施例1中获得的定位误差以及旋转误差的波动更小，定位估计具有更好的一致性，实现了好的一致性，实现了弱纹理环境下鲁棒、精准的定位。
实验例2
对比实施例1与对比例3仿真的轨迹值，结果如图5～10所示，真实轨迹中颜色由蓝色到红色，表示误差逐渐增大，即蓝色处误差小，红色处误差大，其中，图5、图7、图9为对比例3在MH_04_difficult、MH_03_medium、V1_03_difficult三个序列上的轨迹；图6、图8、图10是实施例1在MH_04_difficult、MH_03_medium、V1_03_difficult三个序列上的轨迹。
从图上可以看出：当摄像机快速旋转时，实施例1比对比例3产生的误差更小。
实验例3
对比实施例1与对比例1～3在EuRoC数据集MH_04上的测试结果轨迹对比图如图11所示。
从图上可以发现，实施例1的定位轨迹能够更准确地接近真实轨迹，定位误差小于对比例1～3，表明实施例1的定位更准确。
实验例4
实施例2与对比例4上位移(单位:米)均方根误差RMSE结果如表二所示。
表二上位移(单位:米)均方根误差RMSE比较
Figure BDA0002901280460000211
Figure BDA0002901280460000221
从表二可以看出，实施例2获得的误差均小于对比例4，表明大尺度场景下同样融合GNSS的前提下添加线特征可有效提高估计轨迹的精度。
图12示出了数据序列10_03_drive_0027中实施例2与对比例4轨迹对比，可以看出实施例2的轨迹更加贴合真值，尤其在拐角处实施例4的轨迹与真值相差较大，实施例2在大尺度运行场景下更加鲁棒，定位精度更高。
以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。

Claims (10)

1.一种全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
将局部定位和全局感知定位融合，实现精确的全局感知定位，
所述局部定位包括采用相机、惯性测量单元、激光雷达进行状态估计；
所述全局感知定位包括全球导航卫星系统、磁力计、气压计进行状态估计。
2.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
将视觉惯性与全球导航卫星系统进行多源融合，所述视觉惯性包括惯性测量单元和相机。
3.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
该方法包括以下步骤：
S1、前端点线特征的跟踪；
S2、后端视觉惯性SLAM位姿态联合优化；
S3、与GNSS位姿融合获得全局位姿。
4.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
在步骤S1中，采用线性误差将线性特征表示为直线端点上的线性约束，从而将线特征自然地整合到基于特征点算法的线性表示中。
5.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
步骤S1包含以下子步骤：
S11、建立点线特征表示观测模型；
S12、建立IMU预积分观测模型。
6.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
在步骤S1中，由相机的光心和pi,k及qi,k构成的直线可以确定一个平面，其单位法向量li,k表示为：
Figure FDA0002901280450000021
其中，l1、l2、l3为参数系数，为待求解值，i表示相机不同帧图像，在一帧图像中通常具有多个点特征，观测到的第j个特征点的3D坐标表示为Xw,j∈R3，第k条线段端点为Pw,k、Qw,k∈R3，其在图像平面的投影坐标为
Figure FDA0002901280450000022
Figure FDA0002901280450000023
对应的齐次坐标为
Figure FDA0002901280450000024
Figure FDA0002901280450000025
R3表示三维坐标、R2表示二维坐标；
相机的投影模型表示为π，齐次坐标表示为πh：
Figure FDA0002901280450000026
其中，Rw以及tw分别表示Tiw的旋转和平移，Tiw表示相机第i帧的位姿；w表示世界坐标系；Pw∈R3表示世界坐标系下的点坐标，O是其对应的相机坐标系下的点坐标，fx、fy和cx、cy分别为相机的焦距和主点；
所述点线特征表示观测模型通过点特征的重投影误差
Figure FDA0002901280450000027
和线特征的重投影误差
Figure FDA0002901280450000028
表示，分别为：
Figure FDA0002901280450000031
其中，上标p表示点，上标l表示线，xi，j，yi，j表示第i帧匹配得到的第j个特征点的像素坐标，
Figure FDA0002901280450000032
和
Figure FDA0002901280450000033
分别为pi,k和qi,k到投影直线
Figure FDA0002901280450000034
的距离。
7.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
在步骤S12中，离散时间为k和k+1之间的IMU预积分观测模型为：
Figure FDA0002901280450000035
其中，bm表示当前帧，bm+1表示下一帧；
Figure FDA0002901280450000036
表示离散形式的第m个IMU时刻到第m+1个IMU时刻积分得到的的位置、速度和旋转的增量；
Figure FDA0002901280450000037
Figure FDA0002901280450000038
表示上一时刻前后两帧之间经过中值积分得到的IMU增量信息；
Figure FDA0002901280450000039
表示加速度，
Figure FDA00029012804500000310
表示角速度。
8.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
在S2中，通过最小化所有测量残差的代价项之和来优化滑动窗口中的所有状态变量，从而获得优化后的位姿，可以表示为：
Figure FDA0002901280450000041
其中，
Figure FDA0002901280450000042
为本体坐标系中i帧到i+1帧时刻IMU测量残差，χb为滑动窗口中所有IMU预积分的集合；
Figure FDA0002901280450000043
与
Figure FDA0002901280450000044
分别为点特征和线特征的重投影误差可根据式3获得，χp和χl分别为i帧观测到的点特征和线特征集合；ρ是用于抑制异常值的Cauchy鲁棒函数；eprior为滑动窗口边缘化一帧之后计算出的先验信息，在相机初始化过程中获得。
9.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
在S3中，将步骤S2获得的优化后的位姿与GNSS位姿融合从而获得全局位姿，融合过程可以通过下式表示：
Figure FDA0002901280450000045
其中，χ*表示融合后的全局位姿，S为测量值的集合，包含视觉惯性局部测量和全局测量GNSS，p表示最大似然估计，
Figure FDA0002901280450000046
表示t时刻所有测量值；
所有点的全局位姿χ＝{x0，x1...，xn}，其中
Figure FDA0002901280450000047
pw是全局坐标系下的位置，qw是全局坐标系下的方位。
10.根据权利要求1所述的全局无漂移的自主机器人同时定位与地图构建方法，其特征在于，
在步骤S3中，将步骤S2获得的优化后的位姿与GNSS位姿融合从而获得全局位姿，融合过程可表示为：
Figure FDA0002901280450000051
其中，χ*表示融合后的全局位姿，S为测量值的集合，包含视觉惯性局部测量和全局测量GNSS，
Figure FDA0002901280450000052
表示视觉惯性协方差，
Figure FDA0002901280450000053
表示GNSS协方差，
Figure FDA0002901280450000054
表示t时刻视觉惯性的测量值，
Figure FDA0002901280450000055
表示t时刻视觉惯性测量模型的得到的测量值，
Figure FDA0002901280450000056
表示t时刻GNSS的测量值，
Figure FDA0002901280450000057
表示t时刻GNSS测量模型的得到的测量值。
CN202110057910.3A 2021-01-15 2021-01-15 一种全局无漂移的自主机器人同时定位与地图构建方法 Active CN112945233B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110057910.3A CN112945233B (zh) 2021-01-15 2021-01-15 一种全局无漂移的自主机器人同时定位与地图构建方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110057910.3A CN112945233B (zh) 2021-01-15 2021-01-15 一种全局无漂移的自主机器人同时定位与地图构建方法

Publications (2)

Publication Number Publication Date
CN112945233A true CN112945233A (zh) 2021-06-11
CN112945233B CN112945233B (zh) 2024-02-20

Family

ID=76235412

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110057910.3A Active CN112945233B (zh) 2021-01-15 2021-01-15 一种全局无漂移的自主机器人同时定位与地图构建方法

Country Status (1)

Country Link
CN (1) CN112945233B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113532431A (zh) * 2021-07-15 2021-10-22 贵州电网有限责任公司 一种用于电力巡检与作业的视觉惯性slam方法
CN113721260A (zh) * 2021-08-26 2021-11-30 南京邮电大学 一种激光雷达、双目相机和惯导的在线联合标定方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109579840A (zh) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 一种点线特征融合的紧耦合双目视觉惯性slam方法
CN110060277A (zh) * 2019-04-30 2019-07-26 哈尔滨理工大学 一种多特征融合的视觉slam方法
CN110490085A (zh) * 2019-07-24 2019-11-22 西北工业大学 点线特征视觉slam系统的快速位姿估计算法
CN111595334A (zh) * 2020-04-30 2020-08-28 东南大学 基于视觉点线特征与imu紧耦合的室内自主定位方法
US20200349730A1 (en) * 2018-04-26 2020-11-05 Tencent Technology (Shenzhen) Company Limited Simultaneous localization and mapping method, computer device, and storage medium

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200349730A1 (en) * 2018-04-26 2020-11-05 Tencent Technology (Shenzhen) Company Limited Simultaneous localization and mapping method, computer device, and storage medium
CN109579840A (zh) * 2018-10-25 2019-04-05 中国科学院上海微系统与信息技术研究所 一种点线特征融合的紧耦合双目视觉惯性slam方法
CN110060277A (zh) * 2019-04-30 2019-07-26 哈尔滨理工大学 一种多特征融合的视觉slam方法
CN110490085A (zh) * 2019-07-24 2019-11-22 西北工业大学 点线特征视觉slam系统的快速位姿估计算法
CN111595334A (zh) * 2020-04-30 2020-08-28 东南大学 基于视觉点线特征与imu紧耦合的室内自主定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
TONG QIN等: "A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors", 《ARXIV:1901.03642V1》, pages 1 - 7 *
朱叶青 等: "大尺度弱纹理场景下多源信息融合SLAM算法", 《宇航学报》, vol. 42, no. 10, pages 1271 - 1282 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113532431A (zh) * 2021-07-15 2021-10-22 贵州电网有限责任公司 一种用于电力巡检与作业的视觉惯性slam方法
CN113721260A (zh) * 2021-08-26 2021-11-30 南京邮电大学 一种激光雷达、双目相机和惯导的在线联合标定方法
CN113721260B (zh) * 2021-08-26 2023-12-12 南京邮电大学 一种激光雷达、双目相机和惯导的在线联合标定方法

Also Published As

Publication number Publication date
CN112945233B (zh) 2024-02-20

Similar Documents

Publication Publication Date Title
Heng et al. Project autovision: Localization and 3d scene perception for an autonomous vehicle with a multi-camera system
US11461912B2 (en) Gaussian mixture models for temporal depth fusion
CN109211241B (zh) 基于视觉slam的无人机自主定位方法
CN111258313A (zh) 多传感器融合slam系统及机器人
CN111024066A (zh) 一种无人机视觉-惯性融合室内定位方法
Meilland et al. A spherical robot-centered representation for urban navigation
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN111288989B (zh) 一种小型无人机视觉定位方法
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
Sanfourche et al. Perception for UAV: Vision-Based Navigation and Environment Modeling.
Yang et al. Asynchronous multi-view SLAM
Qian et al. Robust visual-lidar simultaneous localization and mapping system for UAV
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN112945233B (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
Liu A robust and efficient lidar-inertial-visual fused simultaneous localization and mapping system with loop closure
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN117115271A (zh) 无人机飞行过程中的双目相机外参数自标定方法及系统
Lu et al. Vision-based localization methods under GPS-denied conditions
Cigla et al. Gaussian mixture models for temporal depth fusion
CN114964276A (zh) 一种融合惯导的动态视觉slam方法
Beauvisage et al. Multimodal visual-inertial odometry for navigation in cold and low contrast environment
CN112432653B (zh) 基于点线特征的单目视觉惯性里程计方法
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
Liu et al. 6-DOF motion estimation using optical flow based on dual cameras
Liu et al. A multi-sensor fusion with automatic vision-LiDAR calibration based on Factor graph joint optimization for SLAM

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