CN115436968A - 一种基于激光雷达的位图化重定位方法 - Google Patents

一种基于激光雷达的位图化重定位方法 Download PDF

Info

Publication number
CN115436968A
CN115436968A CN202211167300.XA CN202211167300A CN115436968A CN 115436968 A CN115436968 A CN 115436968A CN 202211167300 A CN202211167300 A CN 202211167300A CN 115436968 A CN115436968 A CN 115436968A
Authority
CN
China
Prior art keywords
bitmap
robot
pose
laser
relocation
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.)
Pending
Application number
CN202211167300.XA
Other languages
English (en)
Inventor
黄小春
李瑞峰
霍光磊
蒋嘉骏
温宽昌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Fujian Quanzhou HIT Research Institute of Engineering and Technology
Original Assignee
Fujian Quanzhou HIT Research Institute of Engineering and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Fujian Quanzhou HIT Research Institute of Engineering and Technology filed Critical Fujian Quanzhou HIT Research Institute of Engineering and Technology
Priority to CN202211167300.XA priority Critical patent/CN115436968A/zh
Publication of CN115436968A publication Critical patent/CN115436968A/zh
Pending legal-status Critical Current

Links

Images

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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Numerical Control (AREA)

Abstract

本发明涉及激光雷达的定位方法技术领域，具体是公开一种基于激光雷达的位图化重定位方法，其特征在于，机器人导航系统已经完成地图Map的构建，并且可以通过实时激光点云与Map的匹配完成定位；机器人进行重定位的方法包括有重定位信息构建、位图化、重定位和定位丢失判定处理。该方法无需依赖额外的传感器，对光照具有较好的抗干扰能力，响应速度快，有着较好的重定位性能。

Description

一种基于激光雷达的位图化重定位方法
技术领域
本发明涉及激光雷达的定位方法技术领域。
背景技术
近年来，随着人工智能技术的蓬勃兴起，移动机器人自主导航也迎来了空前发展，作为导航系统的关键技术，定位虽然多种多样，但也各有其局限性，主要表现在初始位姿需要人工给定，运行过程偶发的定位丢失等问题。为了实现完全的自主导航，需要机器人在运行场景中，能够在上电时自主定位，并对外部干扰导致的定位丢失有一定的位姿修正能力，因此机器人的重定位问题是一个绕不开的问题，也是备受机器人领域关注的核心问题之一。
现有实现方案主要有：1.基于GNSS的重定位，该方案通过标定GNSS传感器，实现GNSS坐标系与Map坐标系绑定，即可将GNSS Pose转换到Map坐标系下，并将其作为机器人的当前位姿，实现重定位功能。2.基于深度学习的重定位方法，该方法通过采集运行场景中各处的环境信息作为数据集，并完成卷积神经网络的模型训练。实际运行过程中，通过采集当前环境信息，生成局部地图，并输入给上述神经网络，即可得出机器人最有可能的位姿，实现重定位功能。3.视觉辅助方法，该方法通过视觉与激光的同步定位与建图，同时记录相机与机器人的当前位姿，生成视觉特征图与机器人导航地图Map，建立两者的映射表。实际运行过程中，通过当前视觉特征在视觉特征图中估计当前相机的位姿，即可通过kd-tree检索出最佳的机器人当前位姿，最终通过自适应蒙特卡洛算法完成重定位。
上述现有的技术方案存在的问题：1.GNSS重定位，该方法需要增加GNSS传感器，为提高定位精度，通常采用双天线的RTK方案，这就对机器人的最小尺寸有要求，且只用应用于室外相对空旷的场景。2.基于深度学习的重定位方法，该方法对算力要求高，通常需要GPU的支持，同时需要大量的数据集进行神经网络模型的训练，在实际产品落地时，周期相对较长。3.视觉辅助方法，该方法需要增加视觉传感器，且受光照影响严重，无法在夜晚等光线不足或光线太强导致曝光过度的场景中使用。
发明内容
本发明的目的在于提供一种基于激光雷达的位图化重定位方法，该方法无需依赖额外的传感器，对光照具有较好的抗干扰能力，响应速度快，有着较好的重定位性能。
为实现上述目的，本发明的技术方案是：一种基于激光雷达的位图化重定位方法，其特征在于，机器人导航系统已经完成地图Map的构建，并且可以通过实时激光点云与Map的匹配完成定位；机器人进行重定位的方法包括有重定位信息构建、位图化、重定位和定位丢失判定处理。
所述重定位信息构建为机器人投入运行前预进行所述位图化的重定位信息构建完成，投入运行中，在机器人上电时刻或者在所述定位丢失判定处理判定为定位丢失时，通过所述位图化处理当前激光扫描数据进行所述重定位处理。
机器人采用单线激光雷达，所述位图化过程包括：
步骤2.1、初始化一个二维double类型数组，其行记为Nr对应激光雷达的检测半径范围内径向划分的检测区段数，列记为Nc对应激光雷达数据被划分的扇区数，并将元素设置为0，激光雷达的检测半径用r表示，激光点在雷达坐标系中的方位用θ表示，则，
扇区分辨率rc计算公式为：
Figure BDA0003861939930000021
径向检测区段分辨率rr计算公式为：
Figure BDA0003861939930000031
步骤2.2、对于任意激光点其所在BitMap索引，任意激光点记为ranges[i](ros的sensor_msgs/LaserScan数据结构)，索引表达为
Figure BDA0003861939930000032
其中i激光雷达扫描时得到的数据数组的索引，用来访问数组的元素，angle_increment激光雷达的角度增量，即激光雷达的水平分辨率，angle_min表示激光雷达的起始扫描角度，
将索引到的元素设置为1；
步骤2.3、在遍历完所有激光点后，将各列中行号大于置1所在行的元素也全部置1，即完成一帧激光数据的BitMap构建；
或者，机器人采用多线激光雷达，所述位图化过程包括：
步骤2.1、初始化一个二维double类型数组，其行记为Nr对应激光雷达的检测半径范围内径向划分的检测区段数，列记为Nc对应激光雷达数据被划分的扇区数，并将元素设置为0，激光雷达的检测半径用r表示，激光点在雷达坐标系中的方位用θ表示，则，
扇区分辨率rc计算公式为：
Figure BDA0003861939930000033
径向检测区段分辨率rr计算公式为：
Figure BDA0003861939930000034
步骤2.2、对于任意激光点其所在BitMap索引，设任意激光点i的坐标为(xi，yi，zi)，则其极坐标表示为(ri，θi)，即
Figure BDA0003861939930000035
其中θ可通过std：：atan2函数来求得，该函数的取值范围(-π，π)，能反映360°范围内的任一方向，因此其索引表达为
Figure BDA0003861939930000041
然后将zi填入该索引所指向的BitMap元素中，若有多个激光点的索引相同，则将这些激光点中最大值的z值对该元素进行更新；
步骤2.3、在遍历完所有激光点后，即完成一帧激光数据的BitMap构建。
所述定位丢失判定的判定方法是，设t-2时刻接收到的机器人位姿为Pt-2，t-1时刻接收到的机器人位姿为Pt-1，则当前时刻的预测位姿
Figure BDA0003861939930000042
此处的位姿由齐次变换矩阵
Figure BDA0003861939930000043
表示，其中R3×3表示姿态，t3×1表示位置，设当前时刻的定位模块给出的位姿为Pt，
则两者的马氏距离
Figure BDA0003861939930000044
其中∑为协方差矩阵，协方差矩阵通过设有近n(n＞6)次采集的位姿差构成的矩阵
Figure BDA0003861939930000045
令
Figure BDA0003861939930000046
其中H为用来求协方差矩阵的一个中间变量，则，
Figure BDA0003861939930000047
此处的位姿差由向量表示为Δx Δy Δz Δrx ΔryΔrz]T，当dM超过预设定阈值，则可以认为定位丢失。
所述重定位信息构建的处理构建流程步骤包括，
步骤1.1、由人工设置机器人的初始位置，实时激光数据能与Map正确匹配，输出机器人的当前位姿，
步骤1.2、将当前激光数据进行位图化处理，生成位图BitMap，并缓存到队列Q1中，同时接收来自定位模块输出的当前机器人位姿，缓存至另一队列Q2中，
步骤1.3、从Q1和Q2中查找时间间隔小于预设定阈值的BitMap-Pose对进行绑定，并缓存至队列Q3，
步骤1.4、待机器人沿整个作业路网采集完数据后，将Q3队列中的元素依次输出保存至磁盘，生成重定位信息文件F。
所述重定位处理流程步骤包括，
步骤3.1、将机器人转移至运行场景中，
步骤3.2、重定位模块载入所述重定位信息文件F，遍历其中所有的BitMap-Pose对，保存至数组BPS中，然后依次对每个BitMap按行求和，生成一个Nr维的向量，并利用这些向量构建一个向量数组Vs，之后再利用Vs构造kd-tree。
步骤3.3、获取当前实时激光数据进行位图化处理，生成位图A，对A按行求和得到一个Nr维向量Vr，将Vr输入至kd-tree进行检索，可得到k个与Vr最接近的向量的索引，即一个k个元素的数组，元素值为BPS的索引，用Is表示；
步骤3.4、将步骤3.3所生成位图A按列求和得到一个Nc维向量Vc，设j＝Is[i]，j表示任一Is的元素，其中i表示Is的索引，
具体处理过程是：首先对BPS[j].BitMap按列求和，得到一个Nc维向量
Figure BDA0003861939930000051
则模长为
Figure BDA0003861939930000052
，在一定程度上表达了两个向量间的距离，之后对向量
Figure BDA0003861939930000053
的元素依次进行循环后移，同时也是位图的按列循环右移，通过计算对比循环后移的模长，可找出在给定j时，当l取得最小值的循环位移量Δc；
步骤3.5、遍历Is数组的所有元素，并依次按照步骤3.4计算比较所有元素所对应的BitMap的模长l，记录在l取得最小值时索引j和Δc；
步骤3.6、将步骤3.3所生成位图A按列求和得到一个Nc维向量Vc，对步骤3.4中查找到的模长最小的元素索引j，平移量Δc，再次对BPS[j].BitMap按列求和并进行平移，得到一个Nc维向量
Figure BDA0003861939930000063
，此时两个向量的夹角
Figure BDA0003861939930000061
αΔc越小则表示了两个位图越相似；
步骤3.7、在步骤3.6的基础上，为了防止步骤3.4中的向量直接相减的比较方式误差较大，有必要对Δc附近的列偏移依次进行上述处理，如比较αΔc-2、αΔc-1、αΔc、αΔc+1、αΔc+2五个偏移量的角度，设当列偏移为αΔc+1时，取得最小的向量夹角；
步骤3.8、至此可知最佳匹配的BPS索引值为j，列偏移为Δc+1，则列偏移角度值为Δrz＝Δc-1*rc，而绑定的位姿BPS[j].Pose只需修正rz，即当前机器人的位姿为
Figure BDA0003861939930000062
通过采用上述技术方案，本发明的有益效果是：本发明通过将激光传感器当扫描得到的环境信息进行位图化处理，利用kd-tree及环境特征旋转不变性、向量相似性等方法在前期预构建的位图库中搜索最相似的位图，同时结合绑定于查找到的位图上的机器人位姿，即可计算出当前机器人的位姿，完成机器人的重定位。
该方法仅需使用用于定位的激光传感器构建BitMap，并通过与其绑定的机器人位姿和计算出的航向偏差来实现重定位，可无需额外增加新的传感器等设备(如视觉传感器、GNSS传感器或计算资源如GPU)，对机器人的结构尺寸无特殊要求；由于仅需使用激光雷达，受光照影响程度小，对光照具有较好的抗干扰能力，可应用于夜间场景，可用于夜晚等场景；而基于kd-tree的BitMap检索计算量小，算力要求低，无需GPU(图形处理器)支持。
该方法响应速度快，有着较好的重定位性能能够增强重定位的环境适应性，且降低了机器人的建造成本。应用于移动机器人可主要针对机器人因被人为移动、轮子打滑、激光传感器的短暂遮挡、环境干扰等异常情况导致的激光传感器扫描得到的环境信息与地图不匹配，使得机器人在全局坐标系中位姿发生改变的定位问题。
附图说明
图1是本发明涉及的一种基于激光雷达的位图化重定位方法的位图化示意图。
图2是本发明涉及的一种基于激光雷达的位图化重定位方法的重定位信息构建流程图。
图3是本发明涉及的一种基于激光雷达的位图化重定位方法的重定位流程图。
具体实施方式
为了进一步解释本发明的技术方案，下面通过具体实施例来对本发明进行详细阐述。
一种基于激光雷达的位图化重定位方法，
机器人初始状态，导航系统已经完成地图Map的构建，并且可以通过实时激光点云与Map的匹配完成定位；这里由于本发明技术方案着重的是重定位方法，因此着重阐述重定位方法相关内容，故这里的初始状态是假设导航系统已经完成Map的构建，并且可以通过实时激光点云与Map的匹配完成定位。
机器人进行重定位包括有重定位信息构建、位图化、重定位、定位丢失判定等的处理，本实施例中所述重定位信息构建为机器人投入运行前预进行所述位图化的重定位信息构建完成，投入运行中，在机器人上电时刻或者在所述定位丢失判定处理判定为定位丢失时，通过所述位图化处理当前激光扫描数据，进行所述重定位处理。
所述重定位信息构建，构建流程如图2所示，流程步骤包括，
步骤1.1、此过程首先需要人工设置机器人的初始位置，且实时激光数据能与Map正确匹配，输出机器人的当前位姿，
步骤1.2、将当前激光数据进行位图化处理，生成位图BitMap，并缓存到队列Q1中，同时接收来自定位模块输出的当前机器人位姿，缓存至另一队列Q2中，
步骤1.3、从Q1和Q2中查找时间间隔小于预设定阈值的BitMap-Pose对进行绑定，并缓存至队列Q3，这里BitMap-Pose对根据一个BitMap能找到一个Pose，
步骤1.4、待机器人沿整个作业路网采集完数据后，将Q3队列中的元素依次输出保存至磁盘，生成重定位信息文件F。
所述位图化，是指将以极坐标表示的激光检测数据转换至平面直角坐标系表示，即BitMap，同时将检测到的激光点至检测范围之间的区域填充成占据的过程；位图化过程包括：
步骤2.1、初始化一个二维double类型数组，其行记为Nr对应激光雷达的检测半径范围内径向划分的检测区段数，列记为Nc对应激光雷达数据被划分的扇区数，并将元素设置为0，激光雷达的检测半径用r表示，激光点在雷达坐标系中的方位用θ表示，如图1所示，激光雷达数据被划分为20个扇区，12个径向区段(为示意方便，图中的Nc和Nr较小，误差大，实际应用应根据精度要求设置)，则，
扇区分辨率rc计算公式为：
Figure BDA0003861939930000091
径向检测区段分辨率rr计算公式为：
Figure BDA0003861939930000092
步骤2.2、对于任意激光点其所在BitMap索引，任意激光点记为ranges[i](ros的sensor_msgs/LaserScan数据结构)，索引表达为
Figure BDA0003861939930000093
其中i激光雷达扫描时得到的数据数组的索引，用来访问数组的元素，angle_increment激光雷达的角度增量，即激光雷达的水平分辨率，angle_min表示激光雷达的起始扫描角度(如激光雷达不一定是360°的扫描范围，可能是只能扫-135°～135度的范围，该值指的就是-135在ros中是用弧度表示)，
将索引到的元素设置为1；
步骤2.3、在遍历完所有激光点后，将各列中行号大于置1所在行的元素也全部置1，即完成一帧激光数据的BitMap构建；
若机器人采用的激光雷达为多线激光雷达的点云数据可进行类似处理，值得注意的是，多线激光雷达的点云数据具有高程信息，可将雷达坐标系中的z轴高程数据作为BitMap的元素值直接填入，对于某一位图元素有多个激光点的情况，则取这些激光点中高程的最大值，这样在转化过程可以保留更多的环境特征，具体的所述位图化过程包括：
步骤2.1、初始化一个二维double类型数组，其行记为Nr对应激光雷达的检测半径范围内径向划分的检测区段数，列记为Nc对应激光雷达数据被划分的扇区数，并将元素设置为0，激光雷达的检测半径用r表示，激光点在雷达坐标系中的方位用θ表示，则，
扇区分辨率rc计算公式为：
Figure BDA0003861939930000101
径向检测区段分辨率rr计算公式为：
Figure BDA0003861939930000102
步骤2.2、对于任意激光点其所在BitMap索引，设任意激光点i的坐标为(xi，yi，zi)，则其极坐标表示为(ri，θi)，即
Figure BDA0003861939930000103
其中θ可通过std：：atan2函数来求得，该函数的取值范围(-π，π)，能反映360°范围内的任一方向，因此其索引表达为
Figure BDA0003861939930000104
然后将zi填入该索引所指向的BitMap元素中，若有多个激光点的索引相同，则将这些激光点中最大值的z值对该元素进行更新；
步骤2.3、在遍历完所有激光点后，即完成一帧激光数据的BitMap构建。
所述重定位，重定位流程如图3所示，流程步骤包括，
步骤3.1、首先将机器人转移至运行场景中，
步骤3.2、重定位模块载入所述重定位信息文件F，遍历其中所有的BitMap-Pose对，保存至数组BPS中，然后依次对每个BitMap按行求和，生成一个Nr维的向量，并利用这些向量构建一个向量数组Vs，之后再利用Vs构造kd-tree。
步骤3.3、获取当前实时激光数据进行位图化处理，生成位图A，对A按行求和得到一个Nr维向量Vr，将Vr输入至kd-tree进行检索，可得到k个与Vr最接近的向量的索引，即一个k个元素的数组，元素值为BPS的索引，用Is表示；
步骤3.4、将步骤3.3所生成位图A按列求和得到一个Nc维向量Vc，设j＝Is[i]，表示任一Is的元素，其中i表示Is的索引，j表示Is中的某一元素，
具体处理过程是：首先对BPS[j].BitMap按列求和，得到一个Nc维向量
Figure BDA0003861939930000113
，则模长为
Figure BDA0003861939930000114
，在一定程度上表达了两个向量间的距离，之后对向量
Figure BDA0003861939930000115
的元素依次进行循环后移(即最后一个元素移到第一个索引位置，原第一个索引位置的元素移到第二个索引位置，以此类推)，同时也是位图的按列循环右移，通过计算对比循环后移的模长，可找出在给定j时，当l取得最小值的循环位移量Δc；
步骤3.5、遍历Is数组的所有元素，并依次按照步骤3.4计算比较所有元素所对应的BitMap的模长l，记录在l取得最小值时索引j和Δc；
步骤3.6、将步骤3.3所生成位图A按列求和得到一个Nc维向量Vc，对步骤3.4中查找到的模长最小的元素索引j，平移量Δc，再次对BPS[j].BitMap按列求和并进行平移，得到一个Nc维向量
Figure BDA0003861939930000116
，此时两个向量的夹角
Figure BDA0003861939930000111
αΔc越小则表示了两个位图越相似；
步骤3.7、在步骤3.6的基础上，为了防止步骤3.4中的向量直接相减的比较方式误差较大，有必要对Δc附近的列偏移依次进行上述处理，如比较αΔc-2、αΔc-1、αΔc、αΔc+1、αΔc+2五个偏移量的角度，设当列偏移为αΔc+1时，取得最小的向量夹角；
步骤3.8、至此可知最佳匹配的BPS索引值为j，列偏移为Δc+1，则列偏移角度值为Δrz＝Δc+1*rc，而绑定的位姿BPS[j].Pose只需修正rz，即当前机器人的位姿为
Figure BDA0003861939930000112
当机器人上电初始时刻以及定位丢失时，即可通过该位姿来修正。
所述定位丢失判定，其判定方法是基于马氏距离的定位丢失判定方法，由于机器人的运动应该是一个连续的过程，不应该出现明显的跳变，因此，设t-2时刻接收到的机器人位姿为Pt-2，t-1时刻接收到的机器人位姿为Pt-1，则当前时刻的预测位姿
Figure BDA0003861939930000121
此处的位姿由齐次变换矩阵
Figure BDA0003861939930000122
表示，其中R3×3表示姿态，t3×1表示位置，设当前时刻的定位模块给出的位姿为Pt，
则两者的马氏距离
Figure BDA0003861939930000123
其中∑为协方差矩阵，协方差矩阵通过设有近n(n＞6)次采集的位姿差构成的矩阵
Figure BDA0003861939930000124
令
Figure BDA0003861939930000125
其中H为用来求协方差矩阵的一个中间变量，则，
Figure BDA0003861939930000126
此处的位姿差由向量表示为[Δx Δy Δz ΔrxΔry Δrz]T，当dM超过预设定阈值，则可以认为定位丢失，可以通过上述的重定位方法进行重定位。
上述实施例和图式并非限定本发明的产品形态和式样，任何所属技术领域的普通技术人员对其所做的适当变化或修饰，皆应视为不脱离本发明的专利范畴。

Claims (9)

1.一种基于激光雷达的位图化重定位方法，其特征在于，机器人导航系统已经完成地图Map的构建，并且可以通过实时激光点云与Map的匹配完成定位；机器人进行重定位的方法包括有重定位信息构建、位图化、重定位和定位丢失判定处理。
2.如权利要求1所述的一种基于激光雷达的位图化重定位方法，其特征在于，机器人采用单线激光雷达，所述位图化过程包括：
步骤2.1、初始化一个二维double类型数组，其行记为Nr对应激光雷达的检测半径范围内径向划分的检测区段数，列记为Nc对应激光雷达数据被划分的扇区数，并将元素设置为0，激光雷达的检测半径用r表示，激光点在雷达坐标系中的方位用0表示，则，
扇区分辨率rc计算公式为：
Figure FDA0003861939920000011
径向检测区段分辨率rr计算公式为：
Figure FDA0003861939920000012
步骤2.2、对于任意激光点其所在BitMap索引，任意激光点记为ranges[i](ros的sensor_msgs/LaserScan数据结构)，索引表达为
Figure FDA0003861939920000013
其中i激光雷达扫描时得到的数据数组的索引，用来访问数组的元素，angle_increment激光雷达的角度增量，angle_min表示激光雷达的起始扫描角度，
将索引到的元素设置为1；
步骤2.3、在遍历完所有激光点后，将各列中行号大于置1所在行的元素也全部置1，即完成一帧激光数据的BitMap构建；
或者，机器人采用多线激光雷达，所述位图化过程包括：
步骤2.1、初始化一个二维double类型数组，其行记为Nr对应激光雷达的检测半径范围内径向划分的检测区段数，列记为Nc对应激光雷达数据被划分的扇区数，并将元素设置为0，激光雷达的检测半径用r表示，激光点在雷达坐标系中的方位用θ表示，则，
扇区分辨率rc计算公式为：
Figure FDA0003861939920000021
径向检测区段分辨率rr计算公式为：
Figure FDA0003861939920000022
步骤2.2、对于任意激光点其所在BitMap索引，设任意激光点i的坐标为(xi，yi，zi)，则其极坐标表示为(ri，θi)，即
Figure FDA0003861939920000023
其中θ可通过std：：atan2函数来求得，该函数的取值范围(-π，π)，能反映360°范围内的任一方向，因此其索引表达为
Figure FDA0003861939920000024
然后将zi填入该索引所指向的BitMap元素中，若有多个激光点的索引相同，则将这些激光点中最大值的z值对该元素进行更新；
步骤2.3、在遍历完所有激光点后，即完成一帧激光数据的BitMap构建。
3.如权利要求1所述的一种基于激光雷达的位图化重定位方法，其特征在于，所述定位丢失判定的判定方法是，设t-2时刻接收到的机器人位姿为Pt-2，t-1时刻接收到的机器人位姿为Pt-1，则当前时刻的预测位姿
Figure FDA0003861939920000025
此处的位姿由齐次变换矩阵
Figure FDA0003861939920000026
表示，其中R3×3表示姿态，t3×1表示位置，设当前时刻的定位模块给出的位姿为Pt，
则两者的马氏距离
Figure FDA0003861939920000031
其中∑为协方差矩阵，协方差矩阵通过设有近n(n＞6)次采集的位姿差构成的矩阵
Figure FDA0003861939920000032
令
Figure FDA0003861939920000033
其中H为用来求协方差矩阵的一个中间变量，则，
Figure FDA0003861939920000034
此处的位姿差由向量表示为[Δx Δy Δz Δrx ΔryΔrz ]T，当dM超过预设定阈值，则可以认为定位丢失。
4.如权利要求1、2或3所述的一种基于激光雷达的位图化重定位方法，其特征在于，所述重定位信息构建的处理构建流程步骤包括，
步骤1.1、由人工设置机器人的初始位置，实时激光数据能与Map正确匹配，输出机器人的当前位姿，
步骤1.2、将当前激光数据进行位图化处理，生成位图BitMap，并缓存到队列Q1中，同时接收来自定位模块输出的当前机器人位姿，缓存至另一队列Q2中，
步骤1.3、从Q1和Q2中查找时间间隔小于预设定阈值的BitMap-Pose对进行绑定，并缓存至队列Q3，
步骤1.4、待机器人沿整个作业路网采集完数据后，将Q3队列中的元素依次输出保存至磁盘，生成重定位信息文件F。
5.如权利要求4所述的一种基于激光雷达的位图化重定位方法，其特征在于，所述重定位处理流程步骤包括，
步骤3.1、将机器人转移至运行场景中，
步骤3.2、重定位模块载入所述重定位信息文件F，遍历其中所有的BitMap-Pose对，保存至数组BPS中，然后依次对每个BitMap按行求和，生成一个Nr维的向量，并利用这些向量构建一个向量数组Vs，之后再利用Vs构造kd-tree。
步骤3.3、获取当前实时激光数据进行位图化处理，生成位图A，对A按行求和得到一个Nr维向量Vr，将Vr输入至kd-tree进行检索，可得到k个与Vr最接近的向量的索引，即一个k个元素的数组，元素值为BPS的索引，用Is表示；
步骤3.4、将步骤3.3所生成位图A按列求和得到一个Nc维向量Vc，设j＝Is[i]，j表示任一Is的元素，其中i表示Is的索引，
具体处理过程是：首先对BPS[j].BitMap按列求和，得到一个Nc维向量V′c (j)，则模长为l＝|Vc-V′c (j)|，在一定程度上表达了两个向量间的距离，之后对向量V′c (j)的元素依次进行循环后移，同时也是位图的按列循环右移，通过计算对比循环后移的模长，可找出在给定j时，当l取得最小值的循环位移量Δc；
步骤3.5、遍历Is数组的所有元素，并依次按照步骤3.4计算比较所有元素所对应的BitMap的模长l，记录在l取得最小值时索引j和Δc；
步骤3.6、将步骤3.3所生成位图A按列求和得到一个Nc维向量Vc，对步骤3.4中查找到的模长最小的元素索引j，平移量Δc，再次对BPS[j].BitMap按列求和并进行平移，得到一个Nc维向量V′c (Δc)，此时两个向量的夹角
Figure FDA0003861939920000045
Figure FDA0003861939920000046
越小则表示了两个位图越相似；
步骤3.7、在步骤3.6的基础上，为了防止步骤3.4中的向量直接相减的比较方式误差较大，有必要对Δc附近的列偏移依次进行上述处理，如比较
Figure FDA0003861939920000047
Figure FDA0003861939920000048
五个偏移量的角度，设当列偏移为
Figure FDA0003861939920000049
时，取得最小的向量夹角；
步骤3.8、至此可知最佳匹配的BPS索引值为j，列偏移为Δc+1，则列偏移角度值为Δr2＝Δc+1*rc，而绑定的位姿BPS[j].Pose只需修正rz，即当前机器人的位姿为
Figure FDA0003861939920000051
6.如权利要求2所述的一种基于激光雷达的位图化重定位方法，其特征在于，所述定位丢失判定的判定方法是，设t-2时刻接收到的机器人位姿为Pt-2，t-1时刻接收到的机器人位姿为Pt-1，则当前时刻的预测位姿
Figure FDA0003861939920000052
此处的位姿由齐次变换矩阵
Figure FDA0003861939920000053
表示，其中R3×3表示姿态，t3×1表示位置，设当前时刻的定位模块给出的位姿为Pt，
则两者的马氏距离
Figure FDA0003861939920000054
Figure FDA0003861939920000055
[ΔxΔy Δz Δrx Δry Δrz]T，当dM超过预设定阈值，则可以认为定位丢失。
7.如权利要求1、2、3或6所述的一种基于激光雷达的位图化重定位方法，其特征在于，所述重定位信息构建为机器人投入运行前预进行所述位图化的重定位信息构建完成，投入运行中，在机器人上电时刻或者在所述定位丢失判定处理判定为定位丢失时，通过所述位图化处理当前激光扫描数据进行所述重定位处理。
8.如权利要求4所述的一种基于激光雷达的位图化重定位方法，其特征在于，所述重定位信息构建为机器人投入运行前预进行所述位图化的重定位信息构建完成，投入运行中，在机器人上电时刻或者在所述定位丢失判定处理判定为定位丢失时，通过所述位图化处理当前激光扫描数据进行所述重定位处理。
9.如权利要求5所述的一种基于激光雷达的位图化重定位方法，其特征在于，所述重定位信息构建为机器人投入运行前预进行所述位图化的重定位信息构建完成，投入运行中，在机器人上电时刻或者在所述定位丢失判定处理判定为定位丢失时，通过所述位图化处理当前激光扫描数据进行所述重定位处理。
CN202211167300.XA 2022-09-23 2022-09-23 一种基于激光雷达的位图化重定位方法 Pending CN115436968A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211167300.XA CN115436968A (zh) 2022-09-23 2022-09-23 一种基于激光雷达的位图化重定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211167300.XA CN115436968A (zh) 2022-09-23 2022-09-23 一种基于激光雷达的位图化重定位方法

Publications (1)

Publication Number Publication Date
CN115436968A true CN115436968A (zh) 2022-12-06

Family

ID=84248801

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211167300.XA Pending CN115436968A (zh) 2022-09-23 2022-09-23 一种基于激光雷达的位图化重定位方法

Country Status (1)

Country Link
CN (1) CN115436968A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117589154A (zh) * 2024-01-19 2024-02-23 深圳竹芒科技有限公司 自移动设备的重定位方法、自移动设备和可读存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117589154A (zh) * 2024-01-19 2024-02-23 深圳竹芒科技有限公司 自移动设备的重定位方法、自移动设备和可读存储介质
CN117589154B (zh) * 2024-01-19 2024-05-24 深圳竹芒科技有限公司 自移动设备的重定位方法、自移动设备和可读存储介质

Similar Documents

Publication Publication Date Title
CN110645974B (zh) 一种融合多传感器的移动机器人室内地图构建方法
Huang Review on LiDAR-based SLAM techniques
CN109186606B (zh) 一种基于slam和图像信息的机器人构图及导航方法
CN105865449B (zh) 基于激光和视觉的移动机器人的混合定位方法
Pirker et al. CD SLAM-continuous localization and mapping in a dynamic world
CN111505662B (zh) 一种无人驾驶车辆定位方法及系统
CN112784873B (zh) 一种语义地图的构建方法及设备
CN110110763B (zh) 一种基于最大公共子图的栅格地图融合方法
CN104520732A (zh) 定位传感器的方法以及相关设备
CN114137560A (zh) 基于改进激光描述符的车辆重定位方法、装置及电子设备
CN114119659A (zh) 一种多传感器融合的目标跟踪方法
US20240094343A1 (en) Method, device, system, and storage medium for tracking moving target
CN111812978B (zh) 一种多无人机协作slam方法与系统
CN115436968A (zh) 一种基于激光雷达的位图化重定位方法
CN113947636B (zh) 一种基于深度学习的激光slam定位系统及方法
CN114217303B (zh) 目标定位跟踪方法及装置、水下机器人和存储介质
CN114627253A (zh) 地图构建方法、装置与设备
CN114049362A (zh) 一种基于transformer的点云实例分割方法
CN112581610B (zh) 从多波束声呐数据中建立地图的鲁棒优化方法和系统
US20220164595A1 (en) Method, electronic device and storage medium for vehicle localization
CN112733971A (zh) 扫描设备的位姿确定方法、装置、设备及存储介质
CN117053779A (zh) 一种基于冗余关键帧去除的紧耦合激光slam方法及装置
CN109633531B (zh) 一种复合噪声条件下的无线传感器网络节点定位系统
Wu et al. 2d lidar slam based on Gauss-Newton
CN116823929A (zh) 基于视觉图像与点云地图的跨模态匹配定位方法及系统

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