1.一种融合多传感器的手持式slam装置,包括装置主体、gnss/ins组合定位模块(7)、gnss柱状天线(1)、16线激光雷达(5)、彩色双目摄像头(2)、处理器(3)和平板显示器(6),其特征在于:所述的gnss柱状天线(1)与gnss/ins组合定位模块(7)相连;所述的gnss/ins组合定位模块(7)、16线激光雷达(5)、彩色双目摄像头(2)、平板显示器(6)均与处理器(3)相连接,所述装置主体前端有平板显示器(6),所述装置主体内有gnss/ins组合定位模块(7)和处理器(3),所述装置主体上有gnss柱状天线(1)和16线激光雷达(5),所述装置主体上部后侧有彩色双目摄像头(2)。
2.根据权利要求1所述的一种融合多传感器的手持式slam装置,其特征在于:所述装置主体底部有手柄(4)。
3.一种融合多传感器的手持式slam装置的控制方法,具体步骤如下,其特征在于:
(一)基于16线激光雷达的位姿估计;
1)点云分割;
2)特征提取;
3)激光里程计;
(二)基于gnss/ins组合定位模块的位姿估计;
1)gnss双差相对定位gnss-rtk;
2)基于加速度和角速度信息的位姿求解;
3)gnss/ins松组合位姿求解;
(三)gnss-激光/ins半紧融合位姿估计;
1)激光/ins紧融合位姿估计;
2)gnss-激光/inss松组合位姿求解;
(四)基于体素的可视化方案;
由于视觉在纹理较少的环境中无法进行特征点匹配,从而无法得到三维点在相机坐标系下的深度,激光虽然能够非常直接地测量到深度,但是无法捕捉空间中的纹理信息,针对视觉和激光各自的优缺点,在有限的硬件资源下基于正方形体素的建图,并结合彩色图像的色彩信息对体素点云进行上色,将将空间中的最小单位的体素代替传统三维重建中的面片,稠密化平面的点主要为通过三次区域增长得到的激光点和图像中与平面对应的像素点,首先,对于激光点需要将其投影到图像中获取灰度值;然后,对于图像的像素点需要与激光坐标系下的平面进行匹配;最后,计算与平面匹配成功的像素坐标深度。
4.根据权利要求3所述的一种融合多传感器的手持式slam装置的控制方法,其特征在于:步骤(一)基于16线激光雷达的位姿估计,具体步骤如下:
1)点云分割,将激光点云投影到距离图像上,利用竖直角度和阈值的判定来判断是否室地面点,从而分割出地面点和非地面点;对于非地面点使用bfs进行聚类处理,并设置相应的阈值,去除动态噪点;
2)特征提取,将距离图像水平均分为若干个子图像,令t时刻的点云集中的一个点,在其同一竖直方向上,左右各找5个点,构建集合s,则每个点的曲率c为:
其中,rk为距离;
对于算出来的曲率值排序,设定阈值为cth,若大于阈值,则为边缘点;反之,则为平面点,从每一行中选取不属于地面点,且具有最大c值的nfme个边缘点,组成集合fme;从每一行中选取最小c值的nfmp个平面点,属于地面点或分割点都行,组成集合fmp,再进行一次筛选,从集合fme中选取不属于地面点,且具有最大c值的nfe个边缘点,组成集合fe;从集合fmp中选取属于地面点,且具有最小c值的nfp个平面点,组成集合fp;
3)激光里程计,设定当前时刻为t,则上一时刻为t-1,选择特征点
5.根据权利要求3所述的一种融合多传感器的手持式slam装置的控制方法,其特征在于:步骤(二)基于gnss/ins组合定位模块的位姿估计,具体步骤如下:
1)gnss双差相对定位gnss-rtk,利用gnss观测数据和gnss差分数据,建立伪距和载波双差观测方程,令卫星s的瞬时坐标为xs,ys,zs,测站r的近似坐标为
其中,
先利用lambda算法求解整周模糊度,再求解改正数dx,dy,dz,从而确定载体在gnss坐标系下的坐标x,y,z:
2)基于加速度和角速度信息的位姿求解,首先,设ins坐标系为b,世界坐标系为w,设gnss/ins组合定位模块采集的加速度
其中:ab、ωb分别为加速度和角速度的真值;ba、bg分别为加速度和角速度的漂移值;ηa、ηg为测量噪声;gw为重力值;qwb表示从ins坐标系b旋转到世界坐标系w的旋转四元数;
对加速度和角速度数据进行积分,计算相应的位姿:
其中:
3)gnss/ins松组合位姿求解,松组合是以位置、速度为基础的gnss/ins组合,在以地球坐标系为导航坐标系时,组合导航系统有15个状态参数,分别以姿态、速度、位置以及陀螺漂移和加速度计的零偏作为状态变量xgi:
其中:p、v为位置和速度矩阵;θ为横滚角、俯仰角及偏航角;
建立系统状态和量测方程如下:
zgi(t)=hgi(t)xgi(t)+wgi(t)
其中:agi(t)为gnss/ins松组合时的状态方程系数矩阵;wgi(t)为gnss/ins松组合时的状态方程系统噪声;zgi(t)为外部量测值;hgi(t)为gnss/ins松组合时的量测方程系数矩阵;wgi(t)为gnss/ins松组合时的量测噪声;
最后,利用kalman滤波求解得到载体的位姿。
6.根据权利要求3所述的一种融合多传感器的手持式slam装置的控制方法,其特征在于:步骤(三)gnss-激光/ins半紧融合位姿估计,具体步骤如下;
1)激光/ins紧融合位姿估计,利用gnss/ins组合定位模块采集的加速度、角速度数据以及两帧连续点云提取的特征信息来计算载体的相对变换,设bk为激光雷达时间k下的ins数据,其中,局部坐标系为bk-1;
令连续两帧之间的位姿关系
δx:=[δp,δv,δθ,δba,δbg,δg]
其中:
当得到当前一帧ins测量数据时,其误差状态为:
其中:ηw为高斯噪声,ft为误差状态转移矩阵,gt为噪声雅可比;
状态更新可以看成是一个优化问题,即对位姿状态先验
其中:∑为误差状态δx的协方差;j为雅可比矩阵;m为测量噪声的协方差矩阵,上标lk为时间k下的点云数据,lpi为激光点云;
接下来,将计算出观测方程
将解出的误差状加入先验
其中:
最后,得到全局坐标系下的坐标:
其中:
2)gnss-激光/inss松组合位姿求解,松组合是以位置、速度为基础的gnss-激光/inss组合,在以地球坐标系为导航坐标系时,分别以姿态、速度、位置以及陀螺漂移和加速度计的零偏作为状态变量xgli:
建立系统状态和量测方程如下:
zgli(t)=hgli(t)xgli(t)+wgli(t)
其中:agli(t)为gnss-激光/ins松组合时的状态方程系数矩阵;wgli(t)为gnss-激光/ins松组合时的状态方程系统噪声;zgli(t)为gnss-激光/ins松组合时的外部量测值;hgli(t)为gnss-激光/ins松组合时的量测方程系数矩阵;wgli(t)为gnss-激光/ins松组合时的量测噪声;
最后,利用kalman滤波求解得到载体的位姿。
7.根据权利要求3所述的一种融合多传感器的手持式slam装置的控制方法,其特征在于:所述16线激光雷达采用velodynevlp-16型的激光雷达。
8.根据权利要求3所述的一种融合多传感器的手持式slam装置的控制方法,其特征在于:gnss/ins组合定位模块采集gnss观测数据、gnss差分数据、加速度和角速度数据。