[发明专利]一种手持/背包式SLAM装置及定位方法

权利要求书

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定位模式，则对过去的位置信息进行校正并用组合导航设备输出的位姿重新确定位置。