1.一种室内无人车的激光与视觉SLAM系统,其特征在于:包括传感器模块、前端里程计、后端模块及回环检测模块,所述传感器模块负责接入各类外部传感器并向所述前端里程计输入传感数据,所述前端里程计负责完成无人车的位姿估计和运动估计,所述后端模块负责对所述前端里程计估计的位姿进行优化,所述回环检测模块负责对回环进行检测并加以纠正,消除运动估计中的累计误差;
所述传感器模块包括互相连接的激光雷达、摄像头及惯性测量单元,所述激光雷达至少包括单线激光雷达、多线激光雷达及固态激光雷达,所述摄像头至少包括单目摄像头、双目摄像头及RGB-D摄像头,激光雷达和摄像头分别向所述前端里程计输入激光点云数据与图像数据,所述惯性测量单元至少包括三轴陀螺仪、三轴加速度计及三轴磁力计,用于辅助激光传感器进行位姿估计;
所述前端里程计包括视觉里程计模块与激光里程计模块,根据室内无人车所处室内环境的光线、结构和组成的不同,SLAM系统采用适用的里程计模块进行位姿估计;
所述激光里程计模块利用迭代最近点ICP方法进行3D-3D位姿估计,利用奇异值分解SVD方法完成ICP求解过程,对于已完成配准的3D点坐标:P={p1,…,pn},P′={p1′,…,p2′}
构建最小二乘问题,使得第i对点的误差项矩阵:
ei=pi-(Rpi′+t)
最小时对应的旋转矩阵R与平移矩阵t:
求解时,误差函数处理为:
首先,计算两组点的质心位置坐标p,p′,然后计算每个点的去质心坐标:qi=pi-p,qi′=pi′-p′
接着,根据优化问题计算旋转矩阵:
最后计算平移向量:
t*=p-Rp′
最终得到位姿变换R,t;
所述视觉里程计模块根据输入传感器数据的不同,分别采用不同的运动求解方法;
针对单目相机,所述视觉里程计模块基于对极几何约束求解位姿,即通过SVD从估计得到的本质矩阵E中恢复出R,t;针对双目相机,所述视觉里程计模块采用PnP求解3D到2D点对运动,并运用光束法平差BA来求解PnP问题,对于空间点坐标Pi=[Xi,Yi,Zi]T及其投影像素坐标ui=[ui,vi]T,满足:siui=KTPi
其中,K为相机内参矩阵(3*3维度的矩阵),T表示相机位姿R,t的李群值;
对应重投影误差的目标函数为:
采用李代数构建无约束的最小二乘优化模型,并运用高斯牛顿法及列文伯格-马夸尔特法对其进行求解,在求解过程中引入扰动来对李代数进行求导,最终得到观测矩阵;
针对可获取深度数据的RGB-D相机,所述视觉里程计模块采用ICP方法来进行位姿估计,在完成特征点匹配后,通过所述SVD方法对位姿进行求解;
所述后端模块采用位姿图优化方法,以相机位姿为节点,以里程计模块估计的两个位姿节点之间的相对运动为边,在对位姿图进行优化时,采用李代数来表示位姿,对于节点Ti和Tj之间的运动的李群值Tij:其中,Ti和Tj分别表示i和j处位姿R,t对应的李群值;
构建误差矩阵eij:
其中,Δξij表示Ti和Tj间运动的运动矩阵:
引入扰动模型后,对误差求导,总体目标函数为:
ε代表位姿集合,为n*m的矩阵,n为位姿采样数,m为位置坐标维度;
所述回环检测模块基于词袋模型对回环进行检测,根据图像的ORB特征创建字典,通过TF-IDF方式来对图像特征相似度进行计算,根据相似度结果判别是否发生回环,并在时间上设立缓存机制,规定在特定时间ts内持续检测到的回环判定为真正回环,最终所述后端模块根据回环检测的结果对累计误差进行修正。
2.根据权利要求1所述的室内无人车的激光与视觉SLAM系统,其特征在于:当室内亮度L
3.根据权利要求1所述的室内无人车的激光与视觉SLAM系统,其特征在于:针对所述视觉里程计中已获取深度数据的空间上的特征点,采用3D-3D位姿估计,并运用ICP方法进行位姿求解,针对未知深度数据的特征点,计算3D-2D最小重投影误差来进行位姿估计。