CN116027351A - 一种手持/背包式slam装置及定位方法 - Google Patents

Abstract

一种手持/背包式SLAM装置及定位方法，本发明分为手持部分和背包部分两部分，两部分可以通过螺栓和航空插头进行连接，利用所述的手持/背包SLAM设备进行数据采集、定位方法，步骤如下：S1.通过航空接口将手持部分与背包部分连接，将背包部分放入背包中，将屏幕通过绑带固定在身体上，启动设备。S2.安装ROS系统及上述传感器驱动。S3.开启串口的低延时模式，使所述组合导航的数据能够以200Hz高频率传入主机。S4.发布双目相机图像数据、IMU数据、激光点云数据、GNSS数据。S5.融合相机、激光雷达、GNSS、IMU的定位。本发明提出了与设备配套的高精度半紧耦合的视觉‑IMU‑激光雷达‑INS融合的SLAM定位算法。

Description

一种手持/背包式SLAM装置及定位方法

技术领域

本发明属于SLAM装置领域，特别涉及一种手持/背包式SLAM装置及定位方法。

背景技术

土地资源调查目前多以卫星遥感数据为主，外业实地调查为辅。但卫星遥感在村镇土地调查中存在局部信息精度差、难以获得三维立体数据的缺点。而SLAM(同步定位与地图构建)技术可以弥补这些缺点，实时的构建出精确的三维地图，并得到自身的位置。

目前，多传感器融合已成为了SLAM领域的一个主要发展趋势，单一的传感器已难以满足实时定位建图的需求。GNSS(全球导航卫星系统)可以提供精确的位置信息，但GNSS易受到遮挡，在村镇环境中，易受到树木、建筑的影响，精度出现大幅度下降。同时，GNSS-RTK(实时动态载波相位差分技术)虽然精度高达厘米级，但他的频率较低，难以满足实时性的要求。GNSS与IMU(惯性测量单元)融合形成INS系统，可以输出高频率的位置信息，同时在遮挡环境下也具有了一定的定位能力。激光雷达可以满足快速实时建图的要求，但点云图像中缺乏纹理信息。视觉可以获取外部环境丰富的细节信息，但易受光照的影响，在村镇复杂环境下，存在图像计算量大、算法难以实现的问题。多传感器融合可以有效克服单一传感器的缺点，实现更好的定位建图效果。

在SLAM数据采集设备方面，国内已出现了许多的可穿戴产品，比较有代表性的有数字绿土推的出LiBackpack DGC50背包激光雷达扫描系统，它配备了水平和垂直两个方向的激光雷达传感器，并且支持选配高精度GNSS设备和全景相机，同时结合了SLAM技术，无论扫描环境中GNSS信息强度如何，均可获取到扫描范围内的高精度三维点云数据，因此适用于室内和室外多场景测量。另一个比较有代表性的产品是NavVis VLX可穿戴式室内移动绘图系统，它配备了Velodyne激光雷达传感器，可提供高质量3D测量数据捕获。NavVis VLX将Velodyne图像数据与SLAM技术相结合，可通过移动设备提供测量级点云。其紧凑的通用设计使该系统能够绘制小的、分散的和狭窄的空间，以及有着大量障碍和不平坦地形的环境。此外，它的顶部安装了四个2000万像素的摄像头，构成全景摄像，可以在每个方向上拍摄高分辨率的图像，且不会被操作者阻挡视野。

现有的可穿戴采集设备产品比较有代表性的有数字绿土推的出LiBackpackDGC50背包激光雷达扫描系统和NavVis VLX可穿戴式室内移动绘图系统。LiBackpackDGC50背包激光雷达扫描系统只能提供事后的GNSS-RTK处理，其提供的定位算法中，并没有融合视觉的信息，激光点云和图像数据无法做到实时处理，只能通过配置后处理软件获得高精度的定位信息，没有手持部分，缺乏对手持场景的支持。NavVis VLX没有配备GNSS模块，无法通过卫星定位，搭载的SLAM算法同样没有融合视觉信息，也同样缺乏对手持场景的支持。且虽然目前，市面上已出现相关的集成产品，但大多数价格昂贵，缺乏手持终端，且无法充分利用各个传感器的数据。

发明内容

为了解决以上问题，提出一种手持/背包式SLAM装置及定位方法，，将双目相机，激光雷达，GNSS组合导航模块安装到手持部分中，将处理器，电源模块等安装到背包部分中，既可以手持采集数据，又可以将手持部分安装到背包上，在只背包状态下显示数据。同时，本发明搭载了高精度、高频率的组合导航设备，在遮挡不严重的环境下，可以将RTK定位数据与IMU数据融合，实现高达200Hz的厘米级位姿输出。同时，在遮挡较为严重的场景下，也可以通过视觉-IMU-激光-INS半紧耦合的的定位算法实现高精度的定位输出。此外，由于融合了相机、激光雷达与IMU，在纹理较少或光照不好的环境中亦可以提供高精度定位。

为实现上述目的，本发明采取的技术方案是：

一种手持/背包式SLAM装置，分为手持部分和背包部分两部分，其特征在于：

所述手持设备部分包括激光雷达、GNSS柱状天线、组合导航设备和相机，手持设备部分头部的部分为圆柱体形状，圆柱体上方通过圆柱体边缘的钢柱连接一个倒的卡扣，所述卡扣固定激光雷达，所述手持设备部分尾部为长柄的形状，所述GNSS柱状天线通过磁吸的方式固定于手持部分的顶部，所述手持部分的圆柱体内部通过螺丝固定着组合导航设备、手持设备前部处固定着相机；

所述背包部分包括通讯模块的天线和屏幕，所述背包部分为长方体，手持部分上方有手持部分和背包部分的连接接口，手持设备部分尾部有插口，所述手持部分上方有航空插口与手持部分的插口处通过航空插口及数据线连接，所述通讯模块的天线通过磁吸的方式吸附于背包部分外壳上，所述背包部分的内部搭载了处理器，扩展板卡和电源模块，所述屏幕通过绑带连接于背包部分。

作为本发明进一步改进，所述激光雷达为32线激光雷达。

作为本发明进一步改进，所述相机为ZED相机。

作为本发明进一步改进，所述处理器为JETSON AGX XAVIER处理器。

作为本发明进一步改进，所述组合导航设备为Inertial Labs INS-D组合导航设备。

本发明提供一种手持/背包式SLAM装置的定位方法，其特征在于：

(1)通过组合导航设备输出的标志位判断组合导航的卫星定位是否处于RTK模式下，若是，则只通过组合导航输出200Hz的高频定位信息，若不是，则进行视觉-IMU与激光-INS松耦合的位姿估计；

(2)激光-INS紧融合的位姿估计；

1)对于32线激光雷达扫描得到的每一线数据，采用如下方法计算每个扫描点的曲率：

其中P_k为激光点在激光坐标系下的坐标；

2)对计算出来的点进行归类，曲率大的点归类为边缘点，曲率小的点归类为平面点，同时，进行地面点的剔除；

3)对于相邻第i帧激光雷达点云和第i+1帧激光雷达点云，添加点到线的约束d_ek和点到面的约束d_pk，k、u、v、w代表这些激光雷达点位于不同的线上；

设定一个滑窗，对于滑窗中的激光雷达帧，利用列文伯格-马夸尔特算法优化以下残差：

其中，优化变量为：

[t_x,t_y,t_z,θ_roll,θ_pitch,θ_yaw](5)

得到了激光雷达坐标系下的相对位姿参数；

4)利用组合导航设备采集的GNSS定位数据、加速度、角速度数据以及两帧连续点云提取的特征信息来计算载体的相对变换；

由于GNSS的数据以1Hz的远低于激光雷达10Hz的频率进行输出，对激光雷达的输出的位姿进行插值，得到连续轨迹，采用线性插值的方法，在τ时刻的姿态用以下公式计算出：对于τ_k＜τ＜τ_k+1，插值比率α＝(τ-τ_k)/(τ_k+1-τ_k)，则

R_τ＝R_ke^α[ω]× (6)

t_τ＝(1-α)t_k+αt_k+1 (7)

其中，

R∈SO(3)；

利用组合导航设备输出的加速度，角速度数据，对轨迹添加约束，对于轨迹的位移进行二次求导，利用激光与组合导航设备之间的外参得到轨迹在IMU帧下的加速度，同理，对轨迹的姿态进行一次求导得到角速度，得到以下约束方程：

e_ω＝∑||γ_τ-ω_τ+b_ω||²  (9)

其中，α_τ和γ_τ为组合导航设备输出的加速度和角速度，t_τ为轨迹的位移，ω_τ为对轨迹姿态求导得到的角速度；

每运动的时间或距离大于设定的阀值th时，添加GPS约束，由于GPS高程不准，只使用GPS数据的经度和纬度数据，将其转换到当地坐标系L下；

求解以下最小二乘问题来优化连续激光雷达轨迹：

(3)视觉-IMU紧耦合的位姿估计；

1)使用光流法对采集到的视觉信息进行特征点追踪；

2)建立一个滑动窗口，对滑动窗口中的视觉数据和IMU数据添加视觉约束和IMU约束，进行位姿的优化；

3)优化的变量为：

其中，Ι_k是第k个相机帧时刻IMU的状态，包含该时刻IMU位置、速度、姿态和随机游走噪声，s_l是第l帧特征点的深度；

4)视觉约束为：将相机第i帧中的特征点，投影到第j帧，构建残差如下：

其中，u_ci与v_ci是第i帧图像的特征点在归一化平面上的坐标，

为第j帧图像的特征点在相机坐标系下的坐标；

IMU约束如下：对于相机的第k帧和第k+1帧对应的IMU的第b_k和b_k+1帧，约束如下

其中，

是预积分的测量值；

(4)视觉-IMU与激光-INS松耦合的位姿估计；

在以地球坐标系为导航坐标系时，分别以姿态、速度、位置以及陀螺漂移和加速度计的随机游走误差作为状态变量X_GVLI:

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

Z_GVLI(t)＝H_GVLI(t)X_GVLI(t)+w_GVLI(t)

其中，A_GVLI(t)为松组合时的状态方程系数矩阵，它与视觉-IMU子系统有关，W_GVLI(t)为松组合时的状态方程系统噪声；Z_GVLI(t)为组合时的外部量测值，由INS-激光子系统得到，H_GVLI(t)为松组合时的量测方程系数矩阵；w_GVLI(t)为松组合时的量测噪声；

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

(5)每隔t时间重新判断，若卫星定位重新进入RTK定位模式，则对过去的位置信息进行校正并用组合导航设备输出的位姿重新确定位置。

与现有技术相比，本发明具有以下优点和有益效果：

本发明提出了一种融合多传感器的手持/背包两用的SLAM装置，将双目相机，激光雷达，GNSS组合导航模块安装到手持部分中，将处理器，电源模块等安装到背包部分中，既可以手持采集数据，又可以将手持部分安装到背包上，在只背包状态下显示数据。同时，本发明搭载了高精度、高频率的组合导航设备，在遮挡不严重的环境下，可以将RTK定位数据与IMU数据融合，实现高达200Hz的厘米级位姿输出。同时，在遮挡较为严重的场景下，也可以通过视觉-IMU-激光-INS半紧耦合的的定位算法实现高精度的定位输出。此外，由于融合了相机、激光雷达与IMU，在纹理较少和缺少光照的环境中亦可以提供高精度定位。

附图说明

图1是本发明手持SLAM装置局部设计图一；

图2是本发明手持SLAM装置局部设计图二；

图3是本发明手持SLAM装置局部设计图三；

图4是本发明手持SLAM装置局部设计图四；

图5是本发明手持SLAM装置实施例结构示意图；

图6是本发明为所发明定位算法的设计图；

部件说明：

1.手持设备头部；2.手持设备尾部；3.激光雷达；4.相机；5.GNSS柱状天线；6.插口；7.连接接口；8.航空插口；9.天线；10.背包部分；11.处理器；12.4G通信模块；13.扩展板卡；14.电池模组。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

本发明以一种手持/背包式SLAM装置及定位方法。

(一)一种手持/背包两用的SLAM设备

一种手持/背包两用的SLAM设备，其内部组件包括ZED相机，32线激光雷达3(Velodyne VLP-32C)、Inertial Labs INS-D组合导航设备、4G通讯模块12、GNSS柱状天线5、JETSON AGX XAVIER处理器11、树莓派触摸屏显示器、电池模组14、扩展板卡13等。所述的ZED2相机，Inertial Labs INS-D组合导航设备与扩展板卡相连，扩展板卡与处理器相连，4G通讯模块和GNSS柱状天线与Inertial Labs INS-D组合导航设备相连。扩展板卡、显示器和32线激光雷达与处理器相连。

本发明分为手持部分和背包部分两部分，两部分可以通过螺栓和航空插头进行连接。

结合图1-5所示，所述手持部分，手持设备部分头部1的部分为圆柱体形状，圆柱体上方通过圆柱体边缘的钢柱连接一个倒的卡扣，所述32线激光雷达3固定于其中，手持设备部分尾部2为长柄的形状。GNSS柱状天线5通过磁吸的方式固定于手持部分的顶部。所述手持部分的圆柱体1内部通过螺丝固定着Inertial Labs INS-D组合导航设备，手持设备前部4处固定着ZED2相机。所述背包部分为长方体，手持部分上方有手持部分和背包部分的连接接口7，手持设备部分尾部2有插口6，所述手持部分上方有航空插口8与手持部分的插口6处通过航空插口及数据线连接。4G通讯模块的天线9通过磁吸的方式吸附于外壳上。背包部分10的内部搭载了处理器，扩展板卡，电源模块等。屏幕可以通过绑带连接于背包部分。

(二)一种融合视觉-IMU-激光雷达-INS的半紧耦合的定位方法，本发明定位算法的设计图如图6所示。

(1)通过组合导航设备输出的标志位判断组合导航的卫星定位是否处于RTK模式下，若是，则只通过组合导航输出200Hz的高频定位信息，若不是，则进行视觉-IMU与激光-INS松耦合的位姿估计。

(2)激光-INS紧融合的位姿估计。

1)对于32线激光雷达扫描得到的每一线数据，采用如下方法计算每个扫描点的曲率：

其中P_k为激光点在激光坐标系下的坐标。

2)对计算出来的点进行归类，曲率大的点归类为边缘点，曲率小的点归类为平面点，同时，进行地面点的剔除。

3)对于相邻第i帧激光雷达点云和第i+1帧激光雷达点云，添加点到线的约束

和点到面的约束

k、u、v、w代表这些激光雷达点位于不同的线上。

设定一个滑窗，对于滑窗中的激光雷达帧，利用列文伯格-马夸尔特算法优化以下残差：

其中，优化变量为：

[t_x,t_y,t_z,θ_roll,θ_pitch,θ_yaw](5)

得到了激光雷达坐标系下的相对位姿参数。

4)利用组合导航设备采集的GNSS定位数据、加速度、角速度数据以及两帧连续点云提取的特征信息来计算载体的相对变换。

由于GNSS的数据以1Hz的远低于激光雷达(10Hz)的频率进行输出，为了得到激光雷达输出时刻的位姿，对激光雷达的输出的位姿进行插值，得到连续轨迹，为了保证实时性，采用线性插值的方法，在τ时刻的姿态可以用以下公式计算出：对于τ_k＜τ＜τ_k+1，插值比率α＝(τ-τ_k)/(τ_k+1-τ_k)，则

R_τ＝R_ke^α[ω]×  (6)

t_τ＝(1-α)t_k+αt_k+1  (7)

其中，

R∈SO(3)。

为了提高连续轨迹的准确性，利用组合导航设备输出的加速度，角速度数据，对轨迹添加约束。对于轨迹的位移进行二次求导，利用激光与组合导航设备之间的外参得到轨迹在IMU帧下的加速度，同理，对轨迹的姿态进行一次求导得到角速度，得到以下约束方程：

e_ω＝∑||γ_τ-ω_τ+b_ω||²  (9)

其中，α_τ和γ_τ为组合导航设备输出的加速度和角速度，t_τ为轨迹的位移，ω_τ为对轨迹姿态求导得到的角速度。

每运动的时间或距离大于设定的阀值th时，添加GPS约束，由于GPS高程不准，只使用GPS数据的经度和纬度数据，将其转换到当地坐标系L下。

求解以下最小二乘问题来优化连续激光雷达轨迹：

(3)视觉-IMU紧耦合的位姿估计。

5)使用光流法对采集到的视觉信息进行特征点追踪

6)建立一个滑动窗口，对滑动窗口中的视觉数据和IMU数据添加视觉约束和IMU约束，进行位姿的优化。

7)优化的变量为：

其中，Ι_k是第k个相机帧时刻IMU的状态，包含该时刻IMU位置、速度、姿态和随机游走噪声，s_l是第l帧特征点的深度。

8)视觉约束为：将相机第i帧中的特征点，投影到第j帧，构建残差如下：

其中，u_ci与v_ci是第i帧图像的特征点在归一化平面上的坐标，

为第j帧图像的特征点在相机坐标系下的坐标。

IMU约束如下：对于相机的第k帧和第k+1帧对应的IMU的第b_k和b_k+1帧，约束如下

其中，

是预积分的测量值。

(4)视觉-IMU与激光-INS松耦合的位姿估计。

松组合是视觉-IMU与激光-INS两个子系统的松组合。由于激光-INS子系统建立了连续时间轨迹模型，可以轻松得到对应时刻两个子系统之间的位姿。

在以地球坐标系为导航坐标系时，分别以姿态、速度、位置以及陀螺漂移和加速度计的随机游走误差作为状态变量X_GVLI:

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

Z_GVLI(t)＝H_GVLI(t)X_GVLI(t)+w_GVLI(t)

其中，A_GVLI(t)为松组合时的状态方程系数矩阵，它与视觉-IMU子系统有关。W_GVLI(t)为松组合时的状态方程系统噪声；Z_GVLI(t)为组合时的外部量测值，它由INS-激光子系统得到，H_GVLI(t)为松组合时的量测方程系数矩阵；w_GVLI(t)为松组合时的量测噪声。

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

(5)每隔t时间重新判断，若卫星定位重新进入RTK定位模式，则对过去的位置信息进行校正并用组合导航设备输出的位姿重新确定位置。

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

Claims (6)

1.一种手持/背包式SLAM装置，分为手持部分和背包部分两部分，其特征在于：

所述手持设备部分包括激光雷达、GNSS柱状天线、组合导航设备和相机，手持设备部分头部的部分为圆柱体形状，圆柱体上方通过圆柱体边缘的钢柱连接一个倒的卡扣，所述卡扣固定激光雷达，所述手持设备部分尾部为长柄的形状，所述GNSS柱状天线通过磁吸的方式固定于手持部分的顶部，所述手持部分的圆柱体内部通过螺丝固定着组合导航设备、手持设备前部处固定着相机；

所述背包部分包括通讯模块的天线和屏幕，所述背包部分为长方体，手持部分上方有手持部分和背包部分的连接接口，手持设备部分尾部有插口，所述手持部分上方有航空插口与手持部分的插口处通过航空插口及数据线连接，所述通讯模块的天线通过磁吸的方式吸附于背包部分外壳上，所述背包部分的内部搭载了处理器，扩展板卡和电源模块，所述屏幕通过绑带连接于背包部分。

2.根据权利要求1所述的一种手持/背包式SLAM装置，其特征在于：所述激光雷达为32线激光雷达。

3.根据权利要求1所述的一种手持/背包式SLAM装置，其特征在于：所述相机为ZED相机。

4.根据权利要求1所述的一种手持/背包式SLAM装置，其特征在于：所述处理器为JETSON AGX XAVIER处理器。

5.根据权利要求1所述的一种手持/背包式SLAM装置，其特征在于：所述组合导航设备为Inertial Labs INS-D组合导航设备。

6.根据权利要求1-5任意一项所述手持/背包式SLAM装置的定位方法，其特征在于：

(1)通过组合导航设备输出的标志位判断组合导航的卫星定位是否处于RTK模式下，若是，则只通过组合导航输出200Hz的高频定位信息，若不是，则进行视觉-IMU与激光-INS松耦合的位姿估计；

(2)激光-INS紧融合的位姿估计；

1)对于32线激光雷达扫描得到的每一线数据，采用如下方法计算每个扫描点的曲率：

其中P_k为激光点在激光坐标系下的坐标；

2)对计算出来的点进行归类，曲率大的点归类为边缘点，曲率小的点归类为平面点，同时，进行地面点的剔除；

3)对于相邻第i帧激光雷达点云和第i+1帧激光雷达点云，添加点到线的约束d_ek和点到面的约束d_pk，k、u、v、w代表这些激光雷达点位于不同的线上；

设定一个滑窗，对于滑窗中的激光雷达帧，利用列文伯格-马夸尔特算法优化以下残差：

其中，优化变量为：

[t_x,t_y,t_z,θ_roll,θ_pitch,θ_yaw](5)

得到了激光雷达坐标系下的相对位姿参数；

4)利用组合导航设备采集的GNSS定位数据、加速度、角速度数据以及两帧连续点云提取的特征信息来计算载体的相对变换；

由于GNSS的数据以1Hz的远低于激光雷达10Hz的频率进行输出，对激光雷达的输出的位姿进行插值，得到连续轨迹，采用线性插值的方法，在τ时刻的姿态用以下公式计算出：对于τ_k＜τ＜τ_k+1，插值比率α＝(τ-τ_k)/(τ_k+1-τ_k)，则

R_τ＝R_ke^α[ω]× (6)

t_τ＝(1-α)t_k+αt_k+1 (7)

其中，

R∈SO(3)；

利用组合导航设备输出的加速度，角速度数据，对轨迹添加约束，对于轨迹的位移进行二次求导，利用激光与组合导航设备之间的外参得到轨迹在IMU帧下的加速度，同理，对轨迹的姿态进行一次求导得到角速度，得到以下约束方程：

e_ω＝∑γ_τ-ω_τ+b_ω ² (9)

其中，α_τ和γ_τ为组合导航设备输出的加速度和角速度，t_τ为轨迹的位移，ω_τ为对轨迹姿态求导得到的角速度；

每运动的时间或距离大于设定的阀值th时，添加GPS约束，由于GPS高程不准，只使用GPS数据的经度和纬度数据，将其转换到当地坐标系L下；

求解以下最小二乘问题来优化连续激光雷达轨迹：

(3)视觉-IMU紧耦合的位姿估计；

1)使用光流法对采集到的视觉信息进行特征点追踪；

2)建立一个滑动窗口，对滑动窗口中的视觉数据和IMU数据添加视觉约束和IMU约束，进行位姿的优化；

3)优化的变量为：

其中，Ι_k是第k个相机帧时刻IMU的状态，包含该时刻IMU位置、速度、姿态和随机游走噪声，s_l是第l帧特征点的深度；

4)视觉约束为：将相机第i帧中的特征点，投影到第j帧，构建残差如下：

其中，u_ci与v_ci是第i帧图像的特征点在归一化平面上的坐标，

为第j帧图像的特征点在相机坐标系下的坐标；

IMU约束如下：对于相机的第k帧和第k+1帧对应的IMU的第b_k和b_k+1帧，约束如下

其中，

是预积分的测量值；

(4)视觉-IMU与激光-INS松耦合的位姿估计；

在以地球坐标系为导航坐标系时，分别以姿态、速度、位置以及陀螺漂移和加速度计的随机游走误差作为状态变量X_GVLI:

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

Z_GVLI(t)＝H_GVLI(t)X_GVLI(t)+w_GVLI(t)

其中，A_GVLI(t)为松组合时的状态方程系数矩阵，它与视觉-IMU子系统有关，W_GVLI(t)为松组合时的状态方程系统噪声；Z_GVLI(t)为组合时的外部量测值，由INS-激光子系统得到，H_GVLI(t)为松组合时的量测方程系数矩阵；w_GVLI(t)为松组合时的量测噪声；

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

(5)每隔t时间重新判断，若卫星定位重新进入RTK定位模式，则对过去的位置信息进行校正并用组合导航设备输出的位姿重新确定位置。

Images