一种融合多传感器的手持式SLAM装置及其控制方法

本发明涉及融合多传感器的手持式终端的同步定位与建图方法的领域，特别是涉及一种融合多传感器的手持式slam装置及其控制方法。

背景技术：

土地资源调查目前多以卫星遥感数据为主，外业实地调查为辅。随着遥感技术的发展，高分遥感数据在其中扮演着越来越重要的作用，并且已被证明是一种最直接有效的方法。该项工作主要以遥感技术为依托，将同一空间不同时相的土地利用数据进行叠加对比分析，以发现地球表面变化，从时间、空间、数量及质量方面分析土地利用动态变化特征和未来发展趋势。但卫星遥感在村镇土地调查中存在以下缺点：1)覆盖面积越大，图像的局部信息越不明显；2)遥感卫星目前的资源是有限的，卫星过顶周期和过顶时间通常是固定的，对地观测的频度相对较低，对于突发性问题不能给出及时的遥感图像；3)获取的数据主要为二维平面数据，已不能满足土地资源调查管理和村镇规划建设的需要。而手持式slam(simultaneouslocalizationandmapping,同时定位与地图构建技术)装置是卫星遥感测量手段的有力补充，具有机动灵活、高效快速、精细准确、作业成本低、适用范围广、生产周期短等特点，在小区域和环境复杂地区三维数据快速获取方面具有明显优势。

对于手持式slam装置而言，重点要先解决移动终端的定位与环境感知问题。目前，定位领域中使用最多的是全球导航卫星系统(globalnavigationsatellitesystem,gnss)，它是全球定位、导航和授时(positioning,navigationandtiming,pnt)服务的基础设施。但是，卫星导航信号微弱，信号易被遮挡、抗干扰能力差、易欺骗以及gnss接收机数据输出频率低。特别在村镇复杂环境中，受密集建筑物、树木等影响，gnss经常会因信号遮挡而导致定位输出中断。为了弥补了仅依靠gnss定位的不足，通常与惯性导航系统(inertialnavigationsystem，ins)结合形成的gnss/ins组合定位技术。ins具有与外界不发生任何光、电和磁联系，工作不受气象条件的限制，完全依靠运动载体设备自主完成导航任务，且能够提供比较齐全的导航参数等优点，可以在gnss信号被遮挡时保持连续的定位输出。但其最大的缺点是具有误差累积效应，定位精度会随定位过程的进行而不断下降。高精度的ins可以在较长时间内保持较高的定位精度，但是其价格成本也较高，难以满足民用低成本的要求。激光雷达(lightdetectionandranging,lidar)具有良好的相对定位性能，相比ins发散更为缓慢，可有效的抑制导航误差的累积，激光雷达的环境需求与gnss具有很好的互补性。但其获取的环境信息在细节上略显不足，且无法获取环境的色彩信息。相较于其他传感器，视觉传感器安可以获取足够多的环境细节，可以描绘物体的外观和形状、读取标志等，帮助载体进行环境认知，这些功能其他传感器无法做到，并且近些年更是得益于数字图像处理技术的快速发展和计算机硬件性能的提高。但受环境因素以及外部因素影响较大，比如光线不足、以及天气因素导致的视线缩小等。特别是村镇复杂环境下，存在图像计算量大、算法难以实现的问题。国内外的相关厂商已生产了相关集成性产品，如hexagon旗下的leica测量系统的blk20go手持机集成了激光雷达和全景相机，但该设备主要应用于室内；其背包设备leicapegasus:backpack集成了gnss、imu、两个16线的velodyne激光雷达和4个全景相机，国内数字绿土公司也有同类型的产品libackpackdgc50，但上述的设备价格非常贵，普及率不高，设备体积大，不利于携带，且各传感器只是物理集成，并未实现数据层的融合。

技术实现要素：

为解决上述问题，本发明提出了一种融合多传感器的手持式slam装置及其控制方法，基于误差状态卡尔曼滤波(eskf)的slam方法,考虑单一传感器的优点并针对其局限性,提出融合多传感器的slam方法,通过融合多传感器的数据,计算得到手持终端的位姿变换,减小gnss无法定位以及特征点稀少所造成的误差,使获得的手持终端运动轨迹更为平滑准确,同时也提高建图的准确度。

本发明提供一种融合多传感器的手持式slam装置，包括装置主体、gnss/ins组合定位模块、gnss柱状天线、16线激光雷达、彩色双目摄像头、处理器和平板显示器，所述的gnss柱状天线与gnss/ins组合定位模块相连；所述的gnss/ins组合定位模块、16线激光雷达、彩色双目摄像头、平板显示器均与处理器相连接，所述装置主体前端有平板显示器，所述装置主体内有gnss/ins组合定位模块和处理器，所述装置主体上有gnss柱状天线和16线激光雷达，所述装置主体上部后侧有彩色双目摄像头。

作为本发明结构进一步改进，所述装置主体底部有手柄。

本发明提供一种融合多传感器的手持式slam装置的控制方法，具体步骤如下：

(一)基于16线激光雷达的位姿估计；

1)点云分割；

2)特征提取；

3)激光里程计；

(二)基于gnss/ins组合定位模块的位姿估计；

1)gnss双差相对定位gnss-rtk；

2)基于加速度和角速度信息的位姿求解；

3)gnss/ins松组合位姿求解；

(三)gnss-激光/ins半紧融合位姿估计；

1)激光/ins紧融合位姿估计；

2)gnss-激光/inss松组合位姿求解；

(四)基于体素的可视化方案；

由于视觉在纹理较少的环境中无法进行特征点匹配，从而无法得到三维点在相机坐标系下的深度，激光虽然能够非常直接地测量到深度，但是无法捕捉空间中的纹理信息，针对视觉和激光各自的优缺点，在有限的硬件资源下基于正方形体素的建图，并结合彩色图像的色彩信息对体素点云进行上色，将将空间中的最小单位的体素代替传统三维重建中的面片，稠密化平面的点主要为通过三次区域增长得到的激光点和图像中与平面对应的像素点，首先，对于激光点需要将其投影到图像中获取灰度值；然后，对于图像的像素点需要与激光坐标系下的平面进行匹配；最后，计算与平面匹配成功的像素坐标深度。

作为本发明控制方法进一步改进，步骤(一)基于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，选择特征点和以及t-1时刻的和构建的点到线的对应关系和的点到面的对应关系，利用lm优化计算的对应点约束，得到[tz,θroll,θpitch]；再利用lm优化计算的对应点约束，并结合[tz,θroll,θpitch]，从而得到[tx,ty,θyaw]，于是，就得到了载体局部坐标系下的位姿参数[tx,ty,tz,θroll,θpitch,θyaw]。

作为本发明控制方法进一步改进，步骤(二)基于gnss/ins组合定位模块的位姿估计，具体步骤如下：

1)gnss双差相对定位gnss-rtk，利用gnss观测数据和gnss差分数据，建立伪距和载波双差观测方程，令卫星s的瞬时坐标为x^s,y^s,z^s，测站r的近似坐标为其改正数为dx,dy,dz并线性化，简记为：

其中，表示站际星际双差算子；表示星际差分算子；s、r为卫星和测站编号；j为伪距或载波观测值类型j＝1，2，3；p为伪距观测值；为卫星与接收机之间的几何距离c为真空环境里的光速；δion、δtrop为电离层折射误差、对流层折射误差；λi载波波长；为载波相位观测值、整周模糊度；εp、为伪距、载波相位观测噪声；

先利用lambda算法求解整周模糊度，再求解改正数dx,dy,dz，从而确定载体在gnss坐标系下的坐标x,y,z：

2)基于加速度和角速度信息的位姿求解，首先，设ins坐标系为b，世界坐标系为w，设gnss/ins组合定位模块采集的加速度和角速度

其中：a^b、ω^b分别为加速度和角速度的真值；b^a、b^g分别为加速度和角速度的漂移值；η^a、η^g为测量噪声；g^w为重力值；qwb表示从ins坐标系b旋转到世界坐标系w的旋转四元数；

对加速度和角速度数据进行积分，计算相应的位姿：

其中：分别表示世界坐标系下bk时刻的位置、速度和旋转四元数；δt为时刻bk+1与bk之间的时间差；表示时刻bt相对于第bk时刻的姿态；

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滤波求解得到载体的位姿。

作为本发明控制方法进一步改进，步骤(三)gnss-激光/ins半紧融合位姿估计，具体步骤如下；

1)激光/ins紧融合位姿估计，利用gnss/ins组合定位模块采集的加速度、角速度数据以及两帧连续点云提取的特征信息来计算载体的相对变换，设bk为激光雷达时间k下的ins数据，其中，局部坐标系为bk-1；

令连续两帧之间的位姿关系及误差状态δx为：

δx:＝[δp,δv,δθ,δb^a,δb^g,δg]

其中：分别bk+1时刻相对与bk时刻的位置、速度和旋转四元数；为bk时刻局部坐标系下重力值；δp、δv、δθ分别为位置、速度、旋转角对应横滚角、俯仰角及偏航角的误差值；δb^a、δb^g、δg为加速度计零偏、陀螺漂移以及重力误差；

当得到当前一帧ins测量数据时，其误差状态为：

其中：ηw为高斯噪声，ft为误差状态转移矩阵，gt为噪声雅可比；

状态更新可以看成是一个优化问题，即对位姿状态先验的偏差，以及基于观测模型引入的残差函数的优化问题，首先，基于位姿变换和观测方程计算残差：

其中：∑为误差状态δx的协方差；j为雅可比矩阵；m为测量噪声的协方差矩阵,上标lk为时间k下的点云数据，lpi为激光点云；

接下来，将计算出观测方程的雅可比jk；按照eskf的更新过程求得误差状态的后验；用误差状态的后验去更新位姿变换；最后，判断误差状态的后验是否趋近于0，若是，则停止迭代；否则，再次计算基于位姿变换和观测方程的残差，进行迭代计算；

将解出的误差状加入先验中，可得到最终的帧间转换

其中：分别bk+1时刻相对与bk时刻的先验位置、速度和旋转四元数；为bk时刻局部坐标系下先验重力值；^-b^a、^-b^g为先验加速度计零偏、陀螺漂移；

最后，得到全局坐标系下的坐标：

其中：和为从帧bk到帧bk+1的姿态变换；指全局坐标原点在帧bk+1坐标系下的坐标信息；

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滤波求解得到载体的位姿。

作为本发明控制方法进一步改进，所述16线激光雷达采用velodynevlp-16型的激光雷达。

作为本发明控制方法进一步改进，gnss/ins组合定位模块采集gnss观测数据、gnss差分数据、加速度和角速度数据。

与现有技术相比，有益的效果是：

本发明在未知的环境中，载体使用手持式slam装置获取周围环境的信息，进行同步定位与建图，我们所考虑的手持式slam装置包括gnss/ins组合定位模块、gnss柱状天线、16线激光雷达、彩色双目摄像头、处理器、平板显示器，可实现设备的小型化。针对现有ekf模型激光雷达观测约束易出现性能低下甚至发散的情况，因此，我们考虑需要eskf替换ekf，融合gnss/ins组合定位模块中的加速度与角速度数据，优化位姿估计，再利用gnss/ins组合定位模块中gnss定位结果与其进行松耦合，以得到gnss坐标系下的全局位姿。针对视觉在纹理较少的环境中无法进行特征点匹配，从而无法得到三维点在相机坐标系下的深度，激光虽然能够非常直接地测量到深度，但是无法捕捉空间中的纹理信息的问题。提出在有限的硬件资源下基于正方形体素的建图方案，并利用彩色图像给进行点云上色的空间平面进行稠密重建。

附图说明

图1是本发明手持式slam装置实时例的结构示意图；

图2是本发明手持式slam装置的设计图；

图3是融合多传感器的slam方法流程图。

标记说明：

1、gnss柱状天线；2、彩色双目摄像头；3、处理器；4、手柄；5、16线激光雷达；6、平板显示器；7、gnss/ins组合定位模块。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述：

本发明要解决的技术问题是提供一种融合多传感器的手持式slam装置及其控制方法，基于误差状态卡尔曼滤波(eskf)的slam方法,考虑单一传感器的优点并针对其局限性,提出融合多传感器的slam方法,通过融合多传感器的数据,计算得到手持终端的位姿变换,减小gnss无法定位以及特征点稀少所造成的误差,使获得的手持终端运动轨迹更为平滑准确,同时也提高建图的准确度。

如图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，所述装置主体底部有手柄4。图2为手持式slam装置的设计图。

本发明中，在未知环境下，载体在运动的过程中通过16线激光雷达和彩色双目摄像头获取周围环境的激光点云和彩色图像，利用激光点云配准解算出自身的运动变换，同时还通过gnss柱状天线和gnss/ins组合定位模块获取载体的运动数据，对这些数据进行融合处理，以获得载体所处的位置。再利用激光点云和彩色图像构建周围环境的三维地图，同时利用平板显示器进行可视，实现载体的同步定位与建图。

(一)基于16线激光雷达的位姿估计

16线激光雷达采用velodynevlp-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。选择特征点和以及t-1时刻的和构建的点到线的对应关系和的点到面的对应关系。利用lm优化计算的对应点约束，得到[tz,θroll,θpitch]；再利用lm优化计算的对应点约束，并结合[tz,θroll,θpitch]，从而得到[tx,ty,θyaw]。于是，就得到了载体局部坐标系下的位姿参数[tx,ty,tz,θroll,θpitch,θyaw]。

(二)基于gnss/ins组合定位模块的位姿估计

gnss/ins组合定位模块采集gnss观测数据、gnss差分数据、加速度和角速度数据。

1.gnss双差相对定位(gnss-rtk)。利用gnss观测数据和gnss差分数据，建立伪距和载波双差观测方程。令卫星s的瞬时坐标为(x^s,y^s,z^s)，测站r的近似坐标为其改正数为(dx,dy,dz)并线性化，简记为：

其中，表示站际星际双差算子；表示星际差分算子；s、r为卫星和测站编号；j为伪距或载波观测值类型(j＝1，2，3)；p为伪距观测值；为卫星与接收机之间的几何距离c为真空环境里的光速；δion、δtrop为电离层折射误差、对流层折射误差；λi载波波长；为载波相位观测值、整周模糊度；εp、为伪距、载波相位观测噪声。

先利用lambda算法求解整周模糊度，再求解改正数(dx,dy,dz)，从而确定载体在gnss坐标系下的坐标(x,y,z)：

2.基于加速度和角速度信息的位姿求解。首先，设ins坐标系为b，世界坐标系为w。设gnss/ins组合定位模块采集的加速度和角速度

其中：a^b、ω^b分别为加速度和角速度的真值；b^a、b^g分别为加速度和角速度的漂移值；η^a、η^g为测量噪声；g^w为重力值；qwb表示从ins坐标系b旋转到世界坐标系w的旋转四元数。

对加速度和角速度数据进行积分，可以计算相应的位姿：

其中：分别表示世界坐标系下bk时刻的位置、速度和旋转四元数；δt为时刻bk+1与bk之间的时间差；表示时刻bt相对于第bk时刻的姿态。

3.gnss/ins松组合位姿求解。松组合是以位置、速度为基础的gnss/ins组合。在以地球坐标系为导航坐标系时，组合导航系统有15个状态参数，分别以姿态、速度、位置以及陀螺漂移和加速度计的零偏作为状态变量xgi:

其中：p、v为位置和速度矩阵；θ为横滚角、俯仰角及偏航角；分别表示沿载体运动方向的加速度计零偏和陀螺漂移。

建立系统状态和量测方程如下：

zgi(t)＝hgi(t)xgi(t)+wgi(t)(12)

其中：agi(t)为gnss/ins松组合时的状态方程系数矩阵；wgi(t)为gnss/ins松组合时的状态方程系统噪声；zgi(t)为外部量测值；hgi(t)为gnss/ins松组合时的量测方程系数矩阵；wgi(t)为gnss/ins松组合时的量测噪声。

最后，利用kalman滤波求解得到载体的位姿。

(三)gnss-激光/ins半紧融合位姿估计

1.激光/ins紧融合位姿估计。利用gnss/ins组合定位模块采集的加速度、角速度数据以及两帧连续点云提取的特征信息来计算载体的相对变换。设bk为激光雷达时间k下的ins数据，其中，局部坐标系为bk-1。

令连续两帧之间的位姿关系及误差状态δx为：

δx:＝[δp,δv,δθ,δb^a,δb^g,δg](14)

其中：分别bk+1时刻相对与bk时刻的位置、速度和旋转四元数；为bk时刻局部坐标系下重力值；δp、δv、δθ分别为位置、速度、旋转角(横滚角、俯仰角及偏航角)的误差值；δb^a、δb^g、δg为加速度计零偏、陀螺漂移以及重力误差。

当得到当前一帧ins测量数据时，其误差状态为：

其中：ηw为高斯噪声，ft为误差状态转移矩阵，gt为噪声雅可比。

状态更新可以看成是一个优化问题，即对位姿状态先验的偏差，以及基于观测模型引入的残差函数的优化问题。首先，基于位姿变换和观测方程计算残差：

其中：∑为误差状态δx的协方差；j为雅可比矩阵；m为测量噪声的协方差矩阵,上标lk为时间k下的点云数据，lpi为激光点云。

接下来，将计算出观测方程的雅可比jk；按照eskf的更新过程求得误差状态的后验；用误差状态的后验去更新位姿变换；最后，判断误差状态的后验是否趋近于0，若是，则停止迭代；否则，再次计算基于位姿变换和观测方程的残差，进行迭代计算。

将解出的误差状加入先验中，可得到最终的帧间转换

其中：分别bk+1时刻相对与bk时刻的先验位置、速度和旋转四元数；为bk时刻局部坐标系下先验重力值；^-b^a、^-b^g为先验加速度计零偏、陀螺漂移。

最后，得到全局坐标系下的坐标：

其中：和为从帧bk到帧bk+1的姿态变换；指全局坐标原点在帧bk+1坐标系下的坐标信息。

2.gnss-激光/inss松组合位姿求解。松组合是以位置、速度为基础的gnss-激光/inss组合。在以地球坐标系为导航坐标系时，分别以姿态、速度、位置以及陀螺漂移和加速度计的零偏作为状态变量xgli:

建立系统状态和量测方程如下：

zgli(t)＝hgli(t)xgli(t)+wgli(t)(22)

其中：agli(t)为gnss-激光/ins松组合时的状态方程系数矩阵；wgli(t)为gnss-激光/ins松组合时的状态方程系统噪声；zgli(t)为gnss-激光/ins松组合时的外部量测值；hgli(t)为gnss-激光/ins松组合时的量测方程系数矩阵；wgli(t)为gnss-激光/ins松组合时的量测噪声。

最后，利用kalman滤波求解得到载体的位姿。

(四)基于体素的可视化方案

由于视觉在纹理较少的环境中无法进行特征点匹配，从而无法得到三维点在相机坐标系下的深度。激光虽然能够非常直接地测量到深度，但是无法捕捉空间中的纹理信息。针对视觉和激光各自的优缺点，在有限的硬件资源下基于正方形体素的建图，并结合彩色图像的色彩信息对体素点云进行上色，将将空间中的最小单位的体素代替传统三维重建中的面片。稠密化平面的点主要为通过三次区域增长得到的激光点和图像中与平面对应的像素点。首先，对于激光点需要将其投影到图像中获取灰度值；然后，对于图像的像素点需要与激光坐标系下的平面进行匹配；最后，计算与平面匹配成功的像素坐标深度。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。