CN112987065B - 一种融合多传感器的手持式slam装置及其控制方法 - Google Patents
一种融合多传感器的手持式slam装置及其控制方法 Download PDFInfo
- Publication number
- CN112987065B CN112987065B CN202110166885.2A CN202110166885A CN112987065B CN 112987065 B CN112987065 B CN 112987065B CN 202110166885 A CN202110166885 A CN 202110166885A CN 112987065 B CN112987065 B CN 112987065B
- Authority
- CN
- China
- Prior art keywords
- gnss
- ins
- coordinate system
- laser
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- PJHDLKOEJMDTBE-QEDARVKJSA-N glycerol 1,2-di-(9z,12z-octadecadienoate) 3-octadecanoate Chemical compound CCCCCCCCCCCCCCCCCC(=O)OCC(OC(=O)CCCCCCC\C=C\C\C=C\CCCCC)COC(=O)CCCCCCC\C=C\C\C=C\CCCCC PJHDLKOEJMDTBE-QEDARVKJSA-N 0.000 claims description 55
- 230000001133 acceleration Effects 0.000 claims description 23
- 238000005259 measurement Methods 0.000 claims description 23
- 239000011159 matrix material Substances 0.000 claims description 20
- 230000005484 gravity Effects 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 11
- 238000005457 optimization Methods 0.000 claims description 10
- 230000033001 locomotion Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 7
- 238000012937 correction Methods 0.000 claims description 6
- 208000014733 refractive error Diseases 0.000 claims description 6
- 230000004927 fusion Effects 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 4
- 230000006870 function Effects 0.000 claims description 4
- 238000012800 visualization Methods 0.000 claims description 4
- 238000013459 approach Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 239000005433 ionosphere Substances 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 239000005436 troposphere Substances 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 2
- 235000008331 Pinus X rigitaeda Nutrition 0.000 claims 1
- 235000011613 Pinus brutia Nutrition 0.000 claims 1
- 241000018646 Pinus brutia Species 0.000 claims 1
- 238000013507 mapping Methods 0.000 abstract description 5
- 238000004040 coloring Methods 0.000 abstract description 2
- 230000006872 improvement Effects 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 4
- 238000011835 investigation Methods 0.000 description 4
- 238000011161 development Methods 0.000 description 3
- 230000018109 developmental process Effects 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 239000000047 product Substances 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 208000010415 Low Vision Diseases 0.000 description 1
- 241001596784 Pegasus Species 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000019771 cognition Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 229910021647 smectite Inorganic materials 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Navigation (AREA)
Abstract
一种融合多传感器的手持式SLAM装置及其控制方法，包括GNSS/INS组合定位模块、GNSS柱状天线、16线激光雷达、彩色双目摄像头、处理器、平板显示器；GNSS柱状天线与GNSS/INS组合定位模块相连；GNSS/INS组合定位模块、16线激光雷达、彩色双目摄像头、平板显示器均与处理器相连接，可实现设备的小型化。本发明针对视觉在纹理较少的环境中无法进行特征点匹配，从而无法得到三维点在相机坐标系下的深度，激光虽然能够非常直接地测量到深度，但是无法捕捉空间中的纹理信息的问题，提出了在有限的硬件资源下基于正方形体素的建图方案，并利用彩色图像给进行点云上色的空间平面进行稠密重建。
Description
技术领域
本发明涉及融合多传感器的手持式终端的同步定位与建图方法的领域，特别是涉及一种融合多传感器的手持式SLAM装置及其控制方法。
背景技术
土地资源调查目前多以卫星遥感数据为主，外业实地调查为辅。随着遥感技术的发展，高分遥感数据在其中扮演着越来越重要的作用，并且已被证明是一种最直接有效的方法。该项工作主要以遥感技术为依托，将同一空间不同时相的土地利用数据进行叠加对比分析，以发现地球表面变化，从时间、空间、数量及质量方面分析土地利用动态变化特征和未来发展趋势。但卫星遥感在村镇土地调查中存在以下缺点：1)覆盖面积越大，图像的局部信息越不明显；2)遥感卫星目前的资源是有限的，卫星过顶周期和过顶时间通常是固定的，对地观测的频度相对较低，对于突发性问题不能给出及时的遥感图像；3)获取的数据主要为二维平面数据，已不能满足土地资源调查管理和村镇规划建设的需要。而手持式SLAM(Simultaneous LoCalization and Mapping,同时定位与地图构建技术)装置是卫星遥感测量手段的有力补充，具有机动灵活、高效快速、精细准确、作业成本低、适用范围广、生产周期短等特点，在小区域和环境复杂地区三维数据快速获取方面具有明显优势。
对于手持式SLAM装置而言，重点要先解决移动终端的定位与环境感知问题。目前，定位领域中使用最多的是全球导航卫星***(Global Navigation Satellite System,GNSS)，它是全球定位、导航和授时(Positioning,Navigation and Timing,PNT)服务的基础设施。但是，卫星导航信号微弱，信号易被遮挡、抗干扰能力差、易欺骗以及GNSS接收机数据输出频率低。特别在村镇复杂环境中，受密集建筑物、树木等影响，GNSS经常会因信号遮挡而导致定位输出中断。为了弥补了仅依靠GNSS定位的不足，通常与惯性导航***(Inertial Navigation System，INS)结合形成的GNSS/INS组合定位技术。INS具有与外界不发生任何光、电和磁联系，工作不受气象条件的限制，完全依靠运动载体设备自主完成导航任务，且能够提供比较齐全的导航参数等优点，可以在GNSS信号被遮挡时保持连续的定位输出。但其最大的缺点是具有误差累积效应，定位精度会随定位过程的进行而不断下降。高精度的INS可以在较长时间内保持较高的定位精度，但是其价格成本也较高，难以满足民用低成本的要求。激光雷达(Light DeteCtion and Ranging,LiDAR)具有良好的相对定位性能，相比INS发散更为缓慢，可有效的抑制导航误差的累积，激光雷达的环境需求与GNSS具有很好的互补性。但其获取的环境信息在细节上略显不足，且无法获取环境的色彩信息。相较于其他传感器，视觉传感器安可以获取足够多的环境细节，可以描绘物体的外观和形状、读取标志等，帮助载体进行环境认知，这些功能其他传感器无法做到，并且近些年更是得益于数字图像处理技术的快速发展和计算机硬件性能的提高。但受环境因素以及外部因素影响较大，比如光线不足、以及天气因素导致的视线缩小等。特别是村镇复杂环境下，存在图像计算量大、算法难以实现的问题。国内外的相关厂商已生产了相关集成性产品，如Hexagon旗下的LeiCa测量***的BLK20go手持机集成了激光雷达和全景相机，但该设备主要应用于室内；其背包设备LeiCa Pegasus: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的瞬时坐标为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滤波求解得到载体的位姿。
作为本发明控制方法进一步改进，步骤(三)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滤波求解得到载体的位姿。
作为本发明控制方法进一步改进，所述16线激光雷达采用Velodyne VLP-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线激光雷达采用Velodyne VLP-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的瞬时坐标为(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) (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,δθ,δba,δbg,δg] (14)
其中：分别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) (22)
其中：AGLI(t)为GNSS-激光/INS松组合时的状态方程系数矩阵；WGLI(t)为GNSS-激光/INS松组合时的状态方程***噪声；ZGLI(t)为GNSS-激光/INS松组合时的外部量测值；HGLI(t)为GNSS-激光/INS松组合时的量测方程系数矩阵；wGLI(t)为GNSS-激光/INS松组合时的量测噪声。
最后，利用Kalman滤波求解得到载体的位姿。
(四)基于体素的可视化方案
由于视觉在纹理较少的环境中无法进行特征点匹配，从而无法得到三维点在相机坐标系下的深度。激光虽然能够非常直接地测量到深度，但是无法捕捉空间中的纹理信息。针对视觉和激光各自的优缺点，在有限的硬件资源下基于正方形体素的建图，并结合彩色图像的色彩信息对体素点云进行上色，将将空间中的最小单位的体素代替传统三维重建中的面片。稠密化平面的点主要为通过三次区域增长得到的激光点和图像中与平面对应的像素点。首先，对于激光点需要将其投影到图像中获取灰度值；然后，对于图像的像素点需要与激光坐标系下的平面进行匹配；最后，计算与平面匹配成功的像素坐标深度。
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。
Claims (3)
1.一种融合多传感器的手持式SLAM装置的控制方法，所述融合多传感器的手持式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)，具体步骤如下，其特征在于：
(一)基于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的瞬时坐标为Xs,Ys,Zs，测站r的近似坐标为其改正数为dx,dy,dz并线性化，简记为：
其中，▽Δ表示站际星际双差算子；▽表示星际差分算子；s、r为卫星和测站编号；j为伪距或载波观测值类型j＝1，2，3；p为伪距观测值；/>为卫星与接收机之间的几何距离/>c为真空环境里的光速；δion、δtrop为电离层折射误差、对流层折射误差；λj为观测值类型j对应的载波波长；/>为载波相位观测值、整周模糊度；εP、/>为伪距、载波相位观测噪声；
先利用Lambda算法求解整周模糊度，再求解改正数dx,dy,dz，从而确定载体在GNSS坐标系下的坐标X,Y,Z：
2)基于加速度和角速度信息的位姿求解，首先，设INS坐标系为B，世界坐标系为W，设GNSS/INS组合定位模块采集的加速度和角速度/>
其中：aW、ω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滤波求解得到载体的位姿；
步骤(三)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滤波求解得到载体的位姿；
(四)基于体素的可视化方案；
由于视觉在纹理较少的环境中无法进行特征点匹配，从而无法得到三维点在相机坐标系下的深度，激光虽然能够非常直接地测量到深度，但是无法捕捉空间中的纹理信息，针对视觉和激光各自的优缺点，在有限的硬件资源下基于正方形体素的建图，并结合彩色图像的色彩信息对体素点云进行上色，将将空间中的最小单位的体素代替传统三维重建中的面片，稠密化平面的点主要为通过三次区域增长得到的激光点和图像中与平面对应的像素点，首先，对于激光点需要将其投影到图像中获取灰度值；然后，对于图像的像素点需要与激光坐标系下的平面进行匹配；最后，计算与平面匹配成功的像素坐标深度。
2.根据权利要求1所述的一种融合多传感器的手持式SLAM装置的控制方法，其特征在于：所述16线激光雷达采用Velodyne VLP-16型的激光雷达。
3.根据权利要求1所述的一种融合多传感器的手持式SLAM装置的控制方法，其特征在于：GNSS/INS组合定位模块采集GNSS观测数据、GNSS差分数据、加速度和角速度数据。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110166885.2A CN112987065B (zh) | 2021-02-04 | 2021-02-04 | 一种融合多传感器的手持式slam装置及其控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110166885.2A CN112987065B (zh) | 2021-02-04 | 2021-02-04 | 一种融合多传感器的手持式slam装置及其控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112987065A CN112987065A (zh) | 2021-06-18 |
| CN112987065B true CN112987065B (zh) | 2024-01-12 |
Family
ID=76348702
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110166885.2A Active CN112987065B (zh) | 2021-02-04 | 2021-02-04 | 一种融合多传感器的手持式slam装置及其控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112987065B (zh) |
Families Citing this family (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113534227B (zh) * | 2021-07-26 | 2022-07-01 | 中国电子科技集团公司第五十四研究所 | 一种适用于复杂非合作场景的多传感器融合绝对定位方法 |
| CN113820735B (zh) * | 2021-08-31 | 2023-12-01 | 上海华测导航技术股份有限公司 | 位置信息的确定方法、位置测量设备、终端及存储介质 |
| CN114396943A (zh) * | 2022-01-12 | 2022-04-26 | 国家电网有限公司 | 一种融合定位方法与终端 |
| CN115267725B (zh) * | 2022-09-27 | 2023-01-31 | 上海仙工智能科技有限公司 | 一种基于单线激光雷达的建图方法及装置、存储介质 |
| CN116540286B (zh) * | 2023-07-06 | 2023-08-29 | 中国科学院空天信息创新研究院 | 基于非完整性约束的多源鲁棒融合定位方法 |
| CN117351140B (zh) * | 2023-09-15 | 2024-04-05 | 中国科学院自动化研究所 | 融合全景相机和激光雷达的三维重建方法、装置及设备 |
| CN117168441B (zh) * | 2023-11-02 | 2024-02-20 | 西安因诺航空科技有限公司 | 一种多传感器融合的slam定位和重建方法及*** |
| CN117871943B (zh) * | 2024-03-13 | 2024-05-24 | 保定朗信电子科技有限公司 | 一种分体式高压电能计量方法及*** |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109597095A (zh) * | 2018-11-12 | 2019-04-09 | 北京大学 | 背包式三维激光扫描与立体成像组合***及数据获取方法 |
| CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
| CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及*** |
-
2021
- 2021-02-04 CN CN202110166885.2A patent/CN112987065B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109597095A (zh) * | 2018-11-12 | 2019-04-09 | 北京大学 | 背包式三维激光扫描与立体成像组合***及数据获取方法 |
| CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
| CN111929699A (zh) * | 2020-07-21 | 2020-11-13 | 北京建筑大学 | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及*** |
Non-Patent Citations (5)
| Title |
|---|
| A LiDAR Odometry for Outdoor Mobile Robots Using NDT Based Scan Matching in GPS-denied environments;Bo Zhou等;《2017 IEEE 7th Annual International Conference on CYBER Technology in Automation, Control, and Intelligent Systems》;全文 * |
| Lidar/UWB Fusion Based SLAM With Anti-Degeneration Capability;Haoyu Zhou 等;《 IEEE Transactions on Vehicular Technology》;全文 * |
| Robust localization and localizability estimation with a rotating laser scanner;Weikun Zhen等;《2017 IEEE International Conference on Robotics and Automation》;全文 * |
| 单目-无扫描3D激光雷达融合的非合作目标相对位姿估计;郝刚涛;杜小平;赵继广;宋建军;;宇航学报(10);全文 * |
| 面向室外自然环境的移动机器人视觉仿真***;张浩峰;赵春霞;王晓丽;李白云;成伟明;刘华军;;***仿真学报(03);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112987065A (zh) | 2021-06-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112987065B (zh) | 一种融合多传感器的手持式slam装置及其控制方法 | |
| CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN110412635B (zh) | 一种环境信标支持下的gnss/sins/视觉紧组合方法 | |
| CN101598556B (zh) | 一种未知环境下无人机视觉/惯性组合导航方法 | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| CN111288989B (zh) | 一种小型无人机视觉定位方法 | |
| CN113899375B (zh) | 车辆定位方法和装置、存储介质及电子设备 | |
| CN115407357B (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
| CN110187375A (zh) | 一种基于slam定位结果提高定位精度的方法及装置 | |
| Bryson et al. | Airborne smoothing and mapping using vision and inertial sensors | |
| CN110412596A (zh) | 一种基于图像信息和激光点云的机器人定位方法 | |
| CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
| Yuan et al. | GNSS-IMU-assisted colored ICP for UAV-LiDAR point cloud registration of peach trees | |
| CN115183762A (zh) | 一种机场仓库内外建图方法、***、电子设备及介质 | |
| CN113763549A (zh) | 融合激光雷达和imu的同时定位建图方法、装置和存储介质 | |
| Khoshelham et al. | Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry | |
| Li et al. | Aerial-triangulation aided boresight calibration for a low-cost UAV-LiDAR system | |
| CN109341685B (zh) | 一种基于单应变换的固定翼飞机视觉辅助着陆导航方法 | |
| CN115218889A (zh) | 一种基于点线特征融合的多传感器室内定位方法 | |
| Wu et al. | AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry | |
| CN115930948A (zh) | 一种果园机器人融合定位方法 | |
| CN114459474A (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
| Hu et al. | Efficient Visual-Inertial navigation with point-plane map |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |