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

文档序号:25539596发布日期:2021-06-18 20:34阅读:来源:国知局

技术特征:

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

5.根据权利要求3所述的一种融合多传感器的手持式slam装置的控制方法,其特征在于:步骤(二)基于gnss/ins组合定位模块的位姿估计,具体步骤如下:

1)gnss双差相对定位gnss-rtk,利用gnss观测数据和gnss差分数据,建立伪距和载波双差观测方程,令卫星s的瞬时坐标为xs,ys,zs,测站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组合定位模块采集的加速度和角速度

其中:ab、ωb分别为加速度和角速度的真值;ba、bg分别为加速度和角速度的漂移值;ηa、ηg为测量噪声;gw为重力值;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滤波求解得到载体的位姿。

6.根据权利要求3所述的一种融合多传感器的手持式slam装置的控制方法,其特征在于:步骤(三)gnss-激光/ins半紧融合位姿估计,具体步骤如下;

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

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

δx:=[δp,δv,δθ,δba,δbg,δg]

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

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

其中:ηw为高斯噪声,ft为误差状态转移矩阵,gt为噪声雅可比;

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

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

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

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

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

最后,得到全局坐标系下的坐标:

其中:为从帧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滤波求解得到载体的位姿。

7.根据权利要求3所述的一种融合多传感器的手持式slam装置的控制方法,其特征在于:所述16线激光雷达采用velodynevlp-16型的激光雷达。

8.根据权利要求3所述的一种融合多传感器的手持式slam装置的控制方法,其特征在于:gnss/ins组合定位模块采集gnss观测数据、gnss差分数据、加速度和角速度数据。


技术总结
一种融合多传感器的手持式SLAM装置及其控制方法,包括GNSS/INS组合定位模块、GNSS柱状天线、16线激光雷达、彩色双目摄像头、处理器、平板显示器;GNSS柱状天线与GNSS/INS组合定位模块相连;GNSS/INS组合定位模块、16线激光雷达、彩色双目摄像头、平板显示器均与处理器相连接,可实现设备的小型化。本发明针对视觉在纹理较少的环境中无法进行特征点匹配,从而无法得到三维点在相机坐标系下的深度,激光虽然能够非常直接地测量到深度,但是无法捕捉空间中的纹理信息的问题,提出了在有限的硬件资源下基于正方形体素的建图方案,并利用彩色图像给进行点云上色的空间平面进行稠密重建。

技术研发人员:严超;王庆;刘玉;张昊;李明;许九婧;张波
受保护的技术使用者:东南大学;北京农业信息技术研究中心
技术研发日:2021.02.04
技术公布日:2021.06.18
当前第2页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1