CN113063412A - 一种基于可靠性分析的多机器人协同定位与建图方法 - Google Patents

一种基于可靠性分析的多机器人协同定位与建图方法 Download PDF

Info

Publication number
CN113063412A
CN113063412A CN202110318362.5A CN202110318362A CN113063412A CN 113063412 A CN113063412 A CN 113063412A CN 202110318362 A CN202110318362 A CN 202110318362A CN 113063412 A CN113063412 A CN 113063412A
Authority
CN
China
Prior art keywords
robot
frame
laser point
laser
distance
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.)
Granted
Application number
CN202110318362.5A
Other languages
English (en)
Other versions
CN113063412B (zh
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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202110318362.5A priority Critical patent/CN113063412B/zh
Publication of CN113063412A publication Critical patent/CN113063412A/zh
Application granted granted Critical
Publication of CN113063412B publication Critical patent/CN113063412B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/08—Systems determining position data of a target for measuring distance only
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/29—Graphical models, e.g. Bayesian networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/207—Analysis of motion for motion estimation over a hierarchy of resolutions
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Electromagnetism (AREA)
  • Data Mining & Analysis (AREA)
  • Geometry (AREA)
  • Multimedia (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Software Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Computer Graphics (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了一种基于可靠性分析的多机器人协同定位与建图方法，属于同步定位与建图领域。该方法包括：基于距离加权与动态加权的SLAM模型，实现单机器人高精度建图与定位；基于以视点特征直方图为输入、多机器人之间相对位姿及其可靠性为输出的贝叶斯神经网络，通过对贝叶斯神经网络输出的判定，利用地图整合技术合并机器人局部建图结果。本方法通过可靠性计算与分析，实现了更鲁棒的多机器人协同定位与建图，解决了现有技术中存在的特征点利用不充分、特征点可靠性判断缺失、相对位姿辨识方法不鲁棒、相对位姿辨识结果的可靠性建模不完整等问题，提高了在特征稀疏的未知大场景下探索的效率和精度。

Description

一种基于可靠性分析的多机器人协同定位与建图方法
技术领域
本发明公开了一种基于可靠性分析的多机器人协同定位与建图方法，属于同步定位与建图领域。
背景技术
随着人工智能技术的发展，自动化设备越来越多地代替了人类完成复杂、危险的探索任务，起到了保护人类生命、提高探索效率和准确率的作用。在机器人探索未知环境时，通常采用同步定位与建图(Simultaneous Localization and Mapping,SLAM)技术进行自身位置确定并建立环境地图，特别是基于特征的激光雷达SLAM技术因不受环境光照影响相对于视觉SLAM技术更为鲁棒，在保证较低计算量的同时直接生成特征点地图，更有利于直观展示探索区域实时情况。在类似灾害区域抢险救援、危险区域探索等任务中，大尺度场景的探索任务对基于特征的激光雷达SLAM技术的效率和准确率都提出了新的要求。然而大场景往往伴随着稀疏的可用特征，使得基于特征的单机器人激光雷达SLAM技术不仅效率低下，且常因可用特征点少、所选特征点不可靠而失效。因此如何提高特征稀疏的大场景下，基于特征的激光雷达SLAM技术的效率和精度，便成为该领域的重要研究内容之一。
多机器人同步定位与建图(Multi-Robot Simultaneous Localization andMapping,MR-SLAM)，是指机器人集群中的每个机器人各自运行单机器人SLAM，然后在多机器人汇合期间将单机器人SLAM的结果进行合并，从而获得具有一致性的全局地图的技术。MR-SLAM技术利用多机器人并行运作，极大提高了大场景下的探索效率。然而现有的基于特征的激光雷达MR-SLAM大多是单机器人SLAM在多机器人上的简单扩展，在面对特征稀疏的大场景时仍然较为脆弱，且不能合理利用包含其它成员机器人的激光点云，同时对于环境中特征点的“好坏”(可靠性)缺乏判断；此外，在多机器人汇合后进行相对位姿辨识计算时，现有技术常采用识别机器人外加标志物推算相对位姿，一定程度影响了方法的鲁棒性和机器人的通过性；而且，现有技术对相对位姿计算结果的可靠性建模不完整，严重影响了地图合并的成功率。
发明内容
鉴于现有多机器人协同定位与建图方法存在的特征点利用不充分、特征点可靠性判断缺失、相对位姿辨识方法不鲁棒、相对位姿计算结果的可靠性建模不完整等问题，本发明旨在提供一种基于可靠性分析的多机器人协同定位与建图方法。发明的实现方式为：使用面向特征点的距离加权及动态加权，加强可靠特征点对定位建图优化过程的贡献程度，削弱不可靠特征点对定位建图结果精度的影响，同时充分利用被现有技术滤除的特征点，扩展定位系统输入，提高定位精度；使用视点特征直方图与贝叶斯神经网络结合的方式进行机器人间相对位姿辨识，直接对点云输入进行处理，节约了现有技术外加标志物的硬件成本、解除了硬件条件约束，从而提升鲁棒性；贝叶斯神经网络相对于现有技术更能够完整建模机器人位姿辨识的可靠性及不确定性，提高地图合并的成功率及合并的精度。本发明适用于多机器人集群针对大尺度、特征稀疏场景的探索，提高了探索的效率和准确率。
本发明公开的一种基于可靠性分析的多机器人协同定位与建图方法，具体包含以下步骤：
步骤S1：通过搭载于机器人上的三维激光雷达对周围环境及机器人集群中其它成员机器人进行距离信息感知。
机器人集群中，各机器人需具有相同的外观以及相同的激光雷达传感器安装位置，且不可搭载因大幅度运动而显著影响外观的装置。三维激光雷达线束应不少于16线。
步骤S2：机器人各自使用自身连续帧激光点云信息，进行考虑距离加权的高频帧间匹配，快速获取自身连续位姿。
距离加权基于对距离的分析，加强了近程特征点在优化问题中的贡献，削弱了远程特征点在优化问题中的贡献；考虑实时性的需求，此处不加入动态加权。
帧间匹配过程采用Levenberg-Marquardt算法迭代求解帧间相对位姿，Levenberg-Marquardt算法中，对于本帧每一个激光点设置的残差需要乘以距离权重
Figure BDA0002991735150000021
Figure BDA0002991735150000022
距离权重是基于距离分析对特征点可靠性的量化手段。其中，dk为当前待加入残差的激光点与激光雷达坐标系原点之间的距离；dmax为本帧激光点云中激光点与激光雷达坐标系原点之间的距离最大值；dmin为本帧激光点云中激光点与激光雷达坐标系原点之间的距离最小值；Wmax为设定的最大权重值；θ为当前待加入残差的激光点所在激光束与激光点所对应的拟合平面或拟合直线的延展面法线间的夹角。
步骤S3：根据步骤S2获取的自身连续位姿，更新机器人各自的局部地图，进行考虑距离加权和动态加权的低频帧图匹配，获取高精度位姿。
相比于步骤S2的帧间匹配，帧图匹配的实时性要求较低，因此加入了动态加权。动态加权加强了静态特征点在优化问题中的贡献，削弱了动态特征点在优化问题中的贡献，同时充分利用了包含其它机器人的激光点云所具有的特征点，扩展了帧图匹配的输入。
考虑距离加权和动态加权的低频帧图匹配，首先需要根据步骤S2计算出的本帧激光点的距离权重以及当前机器人相对前帧的相对位姿，然后利用机器人相对前帧的相对位姿以及前帧时机器人在自身局部地图坐标系中的位姿，将本帧激光点转换至机器人自身的局部地图坐标系，进而寻找本帧各激光点在机器人自身的局部地图坐标系中所对应的拟合平面或拟合直线，存储各对应关系；接下来提取拟合平面或拟合直线中各个组成点的动态权重属性，计算平均动态权重
Figure BDA0002991735150000031
其中，fk代表本帧第k个激光点对应的拟合平面或拟合直线；然后采用Levenberg-Marquardt算法迭代求解帧图相对位姿，Levenberg-Marquardt算法中，需要将距离权重和平均动态权重的乘积与本帧每一个激光点设置的残差相乘；接下来利用迭代优化后的帧图相对位姿将本帧各激光点转换至机器人自身局部地图坐标系，根据此前存储的对应关系，计算此时本帧各激光点与其相应拟合平面或拟合直线间的距离D：
Figure BDA0002991735150000032
其中，
Figure BDA0002991735150000033
为本帧激光点在机器人坐标系中的坐标；
Figure BDA0002991735150000034
为优化后的帧图相对位姿，即机器人坐标系在机器人自身局部地图坐标系中的位姿；fk为拟合平面或拟合直线，dist(*)函数用以求解激光点与拟合平面或拟合直线间的距离；接下来基于t-分布计算本帧各激光点的动态权重：
Figure BDA0002991735150000035
σD＝1.4826Median{|D-μD|}
其中，v0与μD为t-分布的常数；σD与本帧激光点云{|D-μD|}的中位数相关；进一步地，拟合平面或拟合直线组成点的平均动态权重小于激光点的
Figure BDA0002991735150000036
的程度越大，则此激光点越有可能依附于动态物体，因此对此激光点计算出的动态权重进行缩小：
Figure BDA0002991735150000037
其中∈为判断激光点是否依附于动态物体的阈值。当激光点的动态权重与平均动态权重之差大于阈值，则使用∈将其进行一次缩小，以靠近平均动态权重。出于鲁棒性的考虑，这里仅仅将激光点动态权重直接减去阈值，而不是用平均动态权重代替激光点动态权重。最终将本帧各激光点赋予动态权重属性，并加入至局部地图。
步骤S4：当机器人间距低于阈值时，机器人使用视点特征直方图与贝叶斯神经网络结合的低频相对位姿辨识模型，计算机器人间相对位姿及其可靠性，并对可靠性进行判定，选取满足要求的相对位姿计算结果，并使用Fractional ICP进一步优化，从而对步骤S3中的局部地图进行合并；进行局部地图合并的机器人将各自的局部地图更新为合并后的地图，并进行下采样，减小所需存储空间。
当机器人间距小于阈值，则机器人实时应用离线训练的贝叶斯神经网络作为相对位姿辨识的模型。以其它成员机器人的本体点云计算得到的视点特征直方图作为网络输入，以其它成员机器人与本机器人的相对位姿及位姿各自由度的方差作为输出，然后对各位姿自由度的输出方差进行加权求和，若求和的结果大于设定阈值则认为不确定性过大、可靠性不足，即相对位姿辨识失效，放弃本次局部地图合并；否则认为相对位姿辨识有效，则双方机器人以贝叶斯神经网络输出的相对位姿作为初值、以输出的各自由度方差推导得到非线性优化的信息矩阵，使用Fractional ICP技术输出精确相对位姿，从而进行两局部地图的合并。贝叶斯神经网络能够准确建模各自由度位姿的方差，由此推导出的信息矩阵用来为非线性优化问题中的计算残差过程进行加权，有利于提高ICP的计算准确度。此外在大多数情况下，地图合并时双方地图重叠区域明显小于双方各自的局部地图整体，而Fractional ICP避免了重叠区域较小导致的误匹配问题。最后，进行局部地图合并的机器人将各自的局部地图更新为合并后的地图，并进行下采样，减小所需存储空间。
步骤S5：不断重复执行步骤S1、S2、S3和S4，得到不断更新的多机器人位姿和存储的局部地图。
有益效果
1、本发明公开的一种基于可靠性分析的多机器人协同定位与建图方法，使用面向特征点的距离加权及动态加权，加强可靠特征点对定位建图优化过程的贡献程度，削弱不可靠特征点对定位建图结果精度的影响，同时充分利用被现有技术滤除的特征点，扩展定位系统输入，提高定位精度；
2、本发明公开的一种基于可靠性分析的多机器人协同定位与建图方法，使用视点特征直方图与贝叶斯神经网络结合的方式进行机器人间相对位姿辨识，直接对点云输入进行处理，节约了现有技术外加标志物的硬件成本、解除了硬件条件约束，从而提升鲁棒性；
3、本发明公开的一种基于可靠性分析的多机器人协同定位与建图方法，采用贝叶斯神经网络相对于现有技术更能够完整建模机器人位姿辨识的可靠性及不确定性，提高地图合并的成功率及合并的精度。
附图说明
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
图1为基于可靠性分析的多机器人协同定位与建图的总体流程图；
图2为距离权重计算相关的几何关系图；
图3为基于距离加权和动态加权的帧图匹配流程图；
图4为附着于不同速度动态物体的特征点的动态加权效果图；
其中，(a)为不使用动态加权、其它成员机器人静止的情况；
(b)为使用动态加权、其它成员机器人静止的情况；
(c)为不使用动态加权、其它成员机器人速度较低的情况；
(d)为使用动态加权、其它成员机器人速度较低的情况；
(e)为不使用动态加权、其它成员机器人速度较高的情况；
(f)为使用动态加权、其它成员机器人速度较高的情况；
图5为建立用于低频相对位姿辨识模型的流程图；
图6为相对位姿可靠性判定的效果曲线图；
其中，(a)为均方根误差(m)随阈值变化的趋势；
(b)为判定合格的相对位姿辨识次数随阈值变化的趋势；
具体实施方式
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
步骤S1：采用多个外观相同的轮式机器人在特征稀疏的大场景中运动，通过64线激光雷达实时采集环境点云信息及其它成员机器人点云数据。
所用机器人搭载惯性导航装置，通过积分推算位姿变化；轮式机器人上不搭载大幅度运动的机械装置，防止机器人外观发生显著变化；多机器人之间通过WIFI进行通信，且均处于同一ROS体系之下，统一的时间标准由ROS提供。
为了便于后续处理，在本实施例中，对于采集的原始激光点云数据，算法可去除无效点和感兴趣区域之外的点、矫正运动畸变、准确分割其它成员机器人本体点云及周围场景点云。通过将强度值为负数的点云数据移除实现去除无效点；根据激光雷达固有属性以及任务的特定需求去除感兴趣区域之外的点；将一帧点云中各点坐标根据激光扫描的角度、频率以及惯导预积分结果，转换至本帧结束时刻对应的激光雷达坐标系；基于区域生长的聚类算法在感兴趣区域内实现对其它成员机器人本体点云及周围场景点云的聚类分割。
步骤S2：机器人各自使用自身连续帧激光点云信息，进行结合特征点可靠性分析的、考虑距离加权的高频帧间匹配，快速获取自身连续位姿；本实施例中，帧间匹配频率设置为8HZ；相对于现有技术而言，距离加权基于对距离的分析加强了近程特征点在优化问题中的贡献，削弱了远程特征点在优化问题中的贡献；考虑实时性的需求，此处不加入动态加权。
其中，机器人间距阈值通过激光雷达线束选定，线束越多，阈值越大；机器人间距通过计算其它成员机器人本体点云坐标的质心得到。基于特征的高频帧间匹配具体包括以下步骤：
步骤S21：提取本帧激光点云中的特征角点和特征平面点。
步骤S22：基于kd树算法，为步骤S21中提取的本帧特征点寻找距离最近的前帧的同类特征点，为特征角点A寻找2个邻近的特征角点B和C，计算点A到直线BC的距离；为特征平面点a寻找3个邻近的特征平面点b、c和d，计算点a到拟合平面bcd的距离。
步骤S23：采用Levenberg-Marquardt算法迭代优化上述距离之和，求解帧间最优相对位姿，Levenberg-Marquardt算法对于每一个激光点设置的残差需要乘以距离权重：
Figure BDA0002991735150000061
其中，dk为当前待加入残差的激光点与激光雷达坐标系原点之间的距离；dmax为本帧激光点云中激光点与激光雷达坐标系原点之间的距离最大值；dmin为本帧激光点云中激光点与激光雷达坐标系原点之间的距离最小值；Wmax为设定的最大权重值，本实施例中设置为2；θ为当前待加入残差的激光点所在激光束与激光点所对应的拟合平面或拟合直线的延展面的法线间的夹角，其中，拟合直线的延展面即为面ABC；计算距离权重时涉及的几何关系如图2所示。
需要强调的是，在激光雷达SLAM中，当特征点处于与激光束近似平行的直线或平面上时，则认为其不可靠，此时通过上述公式的余弦项cosθ实现对残差不同程度的缩小；当特征点距离机器人越远，认为其越不可靠，此时通过上述公式的剩余项实现对残差不同程度的缩小；上述距离权重使得大场景中的激光SLAM能够有效偏向于利用较近的可靠特征点，提高大场景激光SLAM的精度。
步骤S3：根据步骤S2的结果，更新机器人各自的局部地图，进行结合特征点可靠性分析的、考虑距离加权和动态加权的低频帧图匹配，获取高精度位姿；本实施例中，帧图匹配频率设置为0.9HZ。相比于步骤S2的帧间匹配，帧图匹配的实时性要求较低，因此加入了动态加权。动态加权相对于现有技术而言加强了静态特征点在优化问题中的贡献，削弱了动态特征点在化问题中的贡献，同时充分利用了包含其它机器人的激光点云所具有的特征点，扩展了帧图匹配的输入。具体地，基于特征的低频帧图匹配的步骤如图3所示，包括：
步骤S31：根据步骤S2获取当前帧激光点的距离权重以及当前机器人相对前帧的相对位姿。
步骤S32：根据机器人相对前帧的相对位姿以及前帧时机器人在自身局部地图坐标系中的位姿，将当前帧激光点转换至机器人自身的局部地图坐标系。
步骤S33：基于kd树算法，寻找当前帧各激光点在机器人自身的局部地图坐标系中所对应的拟合平面或拟合直线，存储各对应关系；不同于帧间匹配，帧图匹配中为特征角点寻找3个邻近的特征角点进行直线拟合，计算本特征角点到拟合直线的距离；为特征平面点寻找5个邻近的特征平面点，计算本特征平面点到拟合平面的距离。
步骤S34：提取拟合平面或拟合直线中各个组成点的动态权重属性，计算平均动态权重
Figure BDA0002991735150000071
其中，fk代表本帧第k个特征点对应的拟合平面或拟合直线。
步骤S35：采用Levenberg-Marquardt算法迭代求解帧图相对位姿，Levenberg-Marquardt算法中需要将距离权重和平均动态权重的乘积与本帧每一个激光点设置的残差相乘。
步骤S36：根据步骤S35的结果，利用迭代优化后的帧图相对位姿将本帧各激光点转换至机器人自身局部地图坐标系，根据步骤S33的存储结果，计算此时本帧各激光点与其相应拟合平面或拟合直线间的距离：
Figure BDA0002991735150000072
其中，
Figure BDA0002991735150000073
为本帧激光点在机器人坐标系中的坐标；
Figure BDA0002991735150000074
为优化后的帧图相对位姿，即机器人坐标系在机器人自身局部地图坐标系中的位姿；fk为拟合平面或拟合直线，dist(*)函数用以求解激光点与拟合平面或拟合直线间的距离。
步骤S37：基于t-分布计算本帧各激光点的动态权重：
Figure BDA0002991735150000075
σD＝1.4826Median{|D-μD|}
其中，v0与μD为t-分布的常数，本实施例中分别设置为5和0。σD与本帧激光点云{|D-μD|}的中位数相关；若拟合平面或拟合直线组成点的平均动态权重较小，而此激光点的
Figure BDA0002991735150000076
较大，则此激光点更有可能依附于动态物体，因此对此激光点计算出的动态权重进行缩小：
Figure BDA0002991735150000077
其中∈为判断激光点是否依附于动态物体的阈值，本实施例中将其设置为0.6。当特征点的动态权重与平均动态权重之差大于阈值，则使用∈将其进行缩小，以靠近但不小于平均动态权重。出于鲁棒性的考虑，这里仅仅将激光点动态权重缩小至可接受范围之内，即直接减去∈而不是用平均动态权重代替激光点动态权重。
步骤S38：将本帧各激光点赋予动态权重属性，并加入至局部地图。
需要说明的是，对于低频的帧图匹配，除了考虑距离的影响外，还依据上述公式为每一个加入机器人局部地图的激光点计算动态权重，提高定位与建图精度，即使用优化后的帧图相对位姿将当前帧激光点转换至局部地图坐标系下，并计算其与优化前对应的特征线或特征面的距离，距离越远证明此激光点更有可能处于一个动态物体上，动态物体上的激光点对于定位和建图来说是不可靠的，因此动态权重被用来实现对迭代算法中的进行残差不同程度的缩小。动态权重随机器人运动速度的变化趋势如图4所示，图中点的灰度代表权重大小，可见动态权重较好地反映了物体地动态特性，保证了对动态特征的削弱和对其一定程度的利用。
动态权重与距离权重相乘，得到帧图匹配中用来缩小残差的最终权重。相对于帧图匹配，帧间匹配的频率更高，且仅进行本帧激光与前帧激光间的迭代优化，因此受动态物体影响小；且帧间匹配要求较高的实时性，应保证较小计算量；结合上述特点，相对于帧图匹配，帧间匹配并不使用动态权重加权。
为验证动态权重与距离权重的优化效果，为现有同步定位与建图技术LOAM的非线性优化部分加入距离加权和动态加权，并在KITTI数据集多个序列上进行测试，优化效果如下表所示。可见当使用了距离加权或动态加权时，LOAM的均方根误差和误差平均值均有所下降。
Figure BDA0002991735150000081
步骤S4：当机器人间距低于阈值3m时，机器人之间进行基于视点特征直方图和贝叶斯神经网络模型的低频相对位姿辨识与相对位姿不确定性计算，选取可靠的相对位姿计算结果并进一步优化。进行局部地图合并的机器人将各自的局部地图更新为合并后的地图，并以0.4m的分辨率进行下采样，减小所需存储空间。
建立用于低频相对位姿辨识模型包括以下步骤(如图5所示)：
步骤S41：采集大量具有不同位姿的机器人的本体点云，并计算视点特征直方图，构建位姿与视点特征直方图的映射关系，作为训练和测试贝叶斯神经网络的数据集。具体地，包括以下步骤：
步骤S411：将完全相同的搭载有激光雷达的机器人A与机器人B放置于具有丰富特征、相对狭小的空间中，且此空间的栅格地图已提前建立。
步骤S412：使用帧图匹配获取机器人A、B间的初始相对位姿；帧图匹配方法首先使用分支定界确定机器人在地图中的粗略位姿，接着利用非线性优化方法将激光点云所在栅格的概率之和最大化，获取机器人在地图中的精确位姿。
步骤S413：操纵机器人B在距离机器人A 3m的范围内持续运动，以生成大量丰富的位姿数据，运动期间将机器人B捕获的激光点云与此空间的精确栅格地图进行匹配，获取精确位姿，传输至机器人A；同时机器人A捕获此位姿对应时刻的由机器人B反射的激光点云，并计算相应的视点特征直方图；由机器人A记录同一时刻的机器人B的位姿和由机器人B反射的激光点云；为方便获得包含机器人B的激光点云，场景地面应保持尽量平坦，且机器人A与机器人B的一定范围内均无障碍物。
步骤S414：根据步骤S412获取的初始相对位姿与机器人B帧图匹配的结果，获得机器人A与B的相对位姿。
步骤S415：根据步骤S413和步骤S414的结果，存储视点特征直方图与机器人A、B之间相对位姿的映射关系，作为训练和测试贝叶斯神经网络的数据集。本实施例中，贝叶斯神经网络的结构如下表所示。
Figure BDA0002991735150000091
需要说明的是，采用贝叶斯神经网络能够对场景中的所有不确定性进行建模，包括偶然不确定性和认知不确定性。因此采集数据集的步骤并不要求完全的精确，基于概率的行为将弥补这一缺陷。但是此特性也决定了本方法在实际使用中，机器人相对位姿辨识时两机器人所处地形的较大差异对辨识结果影响较大，即输出的不确定性较大。因此相对位姿辨识仅开启与相对平坦的探索路段。此外，上述贝叶斯神经网络可由蒙特卡洛dropout方法进行加速，即在测试阶段开启dropout并结合多次前向传播近似传统的贝叶斯神经网络：
Figure BDA0002991735150000092
其中，D代表步骤S41中采集的数据集；VFH代表本次相对位姿辨识的输入视点特征直方图；δT代表基于MC-dropout的贝叶斯神经网络的输出，即机器人之间的六自由度相对位姿；Wi代表神经网络的权重；
Figure BDA0002991735150000101
代表一次前向传播的输出；T代表前向传播的次数；基于计算T次前向传播结果的统计量(各自由度方差)，进行可靠性分析；T越大则计算实时性越差，但结果可靠性的捕获更精确，因此需要权衡；本实施例中T设置为50。
步骤S42：利用数据集训练加速的贝叶斯神经网络，并进行测试，若测试效果不佳则返回步骤S41增加数据集量。
步骤S43：当机器人间距小于阈值，则实时应用贝叶斯神经网络作为相对位姿辨识的模型，得到其它成员机器人与本机器人的相对位姿及位姿各自由度的可靠性。
步骤S44：对各位姿自由度的输出方差进行加权求和，若加权方差大于设定阈值则认为可靠性不足，即相对位姿辨识失效；否则认为有效。
考虑到轮式机器人运动的特殊性，对各位姿自由度的输出方差的权重设置，满足机器人运行平面的二平移自由度、一转动自由度占据较大权重的需求，本实施例中分别设置为0.25、0.25、0.2，另外三自由度权重均为0.1。依据对位姿各自由度可靠性的判定进行过滤，可提高相对位姿辨识的精度。过滤优化效果如图6所示，图中展示了均方根误差和判定合格的相对位姿辨识次数随阈值变化的趋势，可以发现随着阈值的增加，判定条件逐渐放松，则均方根误差增加，可接受的相对位姿辨识次数也会增加。
步骤S45：以贝叶斯神经网络输出的相对位姿
Figure BDA0002991735150000102
作为初值，以贝叶斯神经网络输出的各自由度方差推导出协方差矩阵的逆作为优化问题的信息矩阵，相对位姿辨识的双方机器人的两局部地图进行Fractional ICP，输出精确相对位姿。其中，以贝叶斯神经网络输出的各自由度方差推导出协方差矩阵的逆作为优化问题的信息矩阵，具体推导如下：
将机器人A捕获的激光点pA[x，y，z]通过相对位姿辨识的结果(旋转矩阵
Figure BDA0002991735150000103
和平移向量
Figure BDA0002991735150000104
)投影到机器人B坐标系，得到p′A[x′，y′，z′]。
Figure BDA0002991735150000105
Figure BDA0002991735150000106
Figure BDA0002991735150000107
Figure BDA0002991735150000108
是ψ，θ，φ的函数，
Figure BDA0002991735150000109
是tx，ty，tz的函数，因此可以进行方差传播。首先根据泰勒一阶展开求取雅可比矩阵K：
Figure BDA00029917351500001010
Kxψ＝ysinθcosφcosψ+ysinψsinφ-zsinθcosψcosφ+zsinφcosψ
Kxθ＝-xsinθcosφ+ysinψcosφcosθ+zcosψcosφcosθ
Kxφ＝--xcosθsinφ-ysinψsinθsinφ-ycosφcosψ-zcosψsinθsinφ+zsinψsinφ
Kyψ＝ysinθsinφcosψ-ycosφsinψ-zsinψsinθsinφ-zcosψcosφ
Kyθ＝-xsinθsinφ+ysinψsinφcosθ+zcosψcosφcosθ
Kyφ＝xcosθcosφ+ysinψsinθcosφ-ycosψsinφ+zcosψsinθcosφ+zsinψsinφ
Kzψ＝ycosθcosψ-zcosθsinψ
Kzθ＝-xcosθ-ysinψsinθ-zcosψsinθ
Kzφ＝0
由此计算传播后的协方差矩阵为：
∑＝KDKT
其中D为贝叶斯神经网络的输出：
Figure BDA0002991735150000111
对求解出的协方差矩阵∑取逆即可得到用于Fractional ICP中非线性优化的信息矩阵∑-1。当机器人A坐标系中的点被映射到机器人B的坐标系中后，得到点p′A，接着利用kd树在机器人B的局部地图中寻找距离点p′A最近的点pB[xB，yB，zB]。Fractional ICP求解残差的公式为：
resudual＝eT∑-1e
e＝[xB-x′，yB-y′，zB-z′]T
通过信息矩阵对计算残差的过程进行加权可以抑制可靠性较差的自由度对非线性优化的影响。下表的计算结果展示了相同的机器人相对位姿下，由于噪声计算得到不同的点特征直方图(VFH)，进而得到不同的局部地图合并效果，可见使用信息矩阵的Fractional ICP(FICP)均产生了更小的均方根误差，提高了地图合并的精度。
Figure BDA0002991735150000121
需要说明的是，仅选取机器人相遇处范围内的部分局部地图进行迭代最近点，以减小计算量和降低时延，提高Fractional ICP的精度。为保证计算量的均衡，相对位姿辨识的两机器人，其中一方担任计算相对位姿的任务，另一方担任Fractional ICP输出精确位姿的任务。此外，由于贝叶斯神经网络的前向传播带来一定的延迟，因此需要准确记录位姿相互辨识时刻下，位姿辨识双方机器人在自身局部地图坐标系(默认为机器人开始运动的初始位姿对应的坐标系)下的位姿，保证局部地图合并的时间同步性。例如，机器人A利用激光点云计算与机器人B之间的相对位姿，则机器人A需记录捕获机器人B本体点云的时刻，此时刻即为相对位姿辨识的时刻；贝叶斯神经网络的前向传播输出结果后，机器人A寻找所记录的时刻下相对于自身局部地图坐标系的位姿，并将此位姿发送至ROS话题，并发送ROS消息至机器人B，消息中包含此次相对位姿辨识的时刻，机器人B同样寻找ROS消息所指定的时刻下相对于自身局部地图坐标系的位姿，也将此位姿发送至ROS话题。只有当机器人的相对位姿、机器人A在局部地图坐标系下的位姿、机器人B在局部地图坐标系下的位姿均已确定，双方机器人使用ROS话题上的位姿和局部地图信息，各自完成地图合并。需要强调的是，由于相对位姿辨识算法需占据较多计算资源，且地图合并涉及激光点云消息在机器人之间的传输，占据较大通信带宽，因此相对位姿辨识应以较低频率运行，本实施例中将频率设置为0.5HZ。
步骤S5：不断重复执行步骤S1、S2、S3和S4，得到不断更新的多机器人位姿和存储的局部地图。
至此，经过对单机器人SLAM中帧图匹配和帧间匹配的优化迭代过程的残差项进行加权、使用加速的贝叶斯神经网络进行多机器人间相对位姿推算、使用结合信息矩阵的Fractional ICP，可以实时对多机器人SLAM场景中各个环节的可靠性进行分析与建模，增强了SLAM各环节可靠因素的主导作用，削弱了不可靠因素对SLAM的影响，解决现有技术中存在的特征点利用不充分、特征点可靠性判断缺失、相对位姿推算方法不鲁棒、相对位姿推算结果的可靠性建模不完整等问题，实现了特征稀疏的大尺度场景下多机器人的高效、高精度、强鲁棒性的协同定位与建图。
本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (5)

1.一种基于可靠性分析的多机器人协同定位与建图方法，其特征在于，包括如下步骤：
步骤S1、通过搭载于机器人上的三维激光雷达对周围环境及机器人集群中其它成员机器人进行距离信息感知；
步骤S2、机器人各自使用自身连续帧激光点云信息，进行考虑距离加权的高频帧间匹配，快速获取自身连续位姿；
步骤S3、根据步骤S2获取的自身连续位姿，更新机器人各自的局部地图，进行考虑距离加权和动态加权的低频帧图匹配，获取高精度位姿；
步骤S4、当机器人间距低于阈值时，机器人使用视点特征直方图与贝叶斯神经网络结合的低频相对位姿辨识模型，计算机器人间相对位姿及其可靠性，并对可靠性进行判定，选取满足要求的相对位姿计算结果，并使用Fractional ICP进一步优化，从而对步骤S3中的局部地图进行合并；进行局部地图合并的机器人将各自的局部地图更新为合并后的地图，并进行下采样，减小所需存储空间；
步骤S5、不断重复执行步骤S1、S2、S3和S4，得到不断更新的多机器人位姿和存储的局部地图。
2.根据权利要求1所述的基于可靠性分析的多机器人协同定位与建图方法，其特征在于：所述步骤S1中的机器人集群内部各机器人，需具有相同的外观以及相同的激光雷达传感器安装位置，且不可搭载因大幅度运动而显著影响外观的装置；机器人上搭载的三维激光雷达线束应不少于16线。
3.根据权利要求1所述的基于可靠性分析的多机器人协同定位与建图方法，其特征在于：所述步骤S2的具体步骤为：
帧间匹配过程采用Levenberg-Marquardt算法迭代求解帧间相对位姿，Levenberg-Marquardt算法中，对于本帧每一个激光点设置的残差需要乘以距离权重
Figure FDA0002991735140000011
Figure FDA0002991735140000012
距离权重是基于距离分析对特征点可靠性的量化手段；其中，dk为当前待加入残差的激光点与激光雷达坐标系原点之间的距离；dmax为本帧激光点云中激光点与激光雷达坐标系原点之间的距离最大值；dmin为本帧激光点云中激光点与激光雷达坐标系原点之间的距离最小值；Wmax为设定的最大权重值；θ为当前待加入残差的激光点所在激光束与激光点所对应的拟合平面或拟合直线的延展面的法线间的夹角。
4.根据权利要求1所述的基于可靠性分析的多机器人协同定位与建图方法，其特征在于：所述步骤S3的具体步骤为：
考虑距离加权和动态加权的低频帧图匹配，首先需要根据步骤S2计算出的本帧激光点的距离权重以及当前机器人相对前帧的相对位姿，然后利用机器人相对前帧的相对位姿以及前帧时机器人在自身局部地图坐标系中的位姿，将本帧激光点转换至机器人自身的局部地图坐标系，进而寻找本帧各激光点在机器人自身的局部地图坐标系中所对应的拟合平面或拟合直线，存储各对应关系；接下来提取拟合平面或拟合直线中各个组成点的动态权重属性，计算平均动态权重
Figure FDA0002991735140000021
其中，fk代表本帧第k个激光点对应的拟合平面或拟合直线；然后采用Levenberg-Marquardt算法迭代求解帧图相对位姿，Levenberg-Marquardt算法中，需要将距离权重和平均动态权重的乘积与本帧每一个激光点设置的残差相乘；接下来利用迭代优化后的帧图相对位姿将本帧各激光点转换至机器人自身局部地图坐标系，根据此前存储的对应关系，计算此时本帧各激光点与其相应拟合平面或拟合直线间的距离D：
Figure FDA0002991735140000022
其中，
Figure FDA0002991735140000023
为本帧激光点在机器人坐标系中的坐标；
Figure FDA0002991735140000024
为优化后的帧图相对位姿，即机器人坐标系在机器人自身局部地图坐标系中的位姿；fk为拟合平面或拟合直线，dist(*)函数用以求解激光点与拟合平面或拟合直线间的距离；接下来基于t-分布计算本帧各激光点的动态权重：
Figure FDA0002991735140000025
σD＝1.4826Median{|D-μD|}
其中，ν0与μD为t-分布的常数；σD与本帧激光点云{|D-μD|}的中位数相关；进一步地，拟合平面或拟合直线组成点的平均动态权重小于激光点的
Figure FDA0002991735140000026
的程度越大，则此激光点越有可能依附于动态物体，因此对此激光点计算出的动态权重进行缩小：
Figure FDA0002991735140000027
其中∈为判断激光点是否依附于动态物体的阈值，当激光点的动态权重与平均动态权重之差大于阈值，则使用∈将其进行一次缩小，以靠近平均动态权重；出于鲁棒性的考虑，这里仅仅将激光点动态权重直接减去阈值，而不是用平均动态权重代替激光点动态权重；最终将本帧各激光点赋予动态权重属性，并加入至局部地图。
5.根据权利要求1所述的基于可靠性分析的多机器人协同定位与建图方法，其特征在于：所述步骤S4的具体步骤为：
当机器人间距小于阈值，则实时应用上述离线训练的贝叶斯神经网络作为相对位姿辨识的模型，以其它成员机器人的本体点云计算得到的视点特征直方图作为网络输入，以其它成员机器人与本机器人的相对位姿及位姿各自由度的方差作为输出，然后对各位姿自由度的输出方差进行加权求和，若求和的结果大于设定阈值则认为不确定性过大、可靠性不足，即相对位姿辨识失效，放弃本次局部地图合并；否则认为相对位姿辨识有效，则双方机器人以贝叶斯神经网络输出的相对位姿作为初值、以输出的各自由度方差推导得到非线性优化的信息矩阵，使用FractionalICP技术输出精确相对位姿，从而进行两局部地图的合并；贝叶斯神经网络能够准确建模各自由度位姿的方差，由此推导出的信息矩阵用来为非线性优化问题中的计算残差过程进行加权，有利于提高ICP的计算准确度；此外在大多数情况下，地图合并时双方地图重叠区域明显小于双方各自的局部地图整体，而FractionalICP避免了重叠区域较小导致的误匹配问题；最后，进行局部地图合并的机器人将各自的局部地图更新为合并后的地图，并进行下采样，减小所需存储空间。
CN202110318362.5A 2021-03-25 2021-03-25 一种基于可靠性分析的多机器人协同定位与建图方法 Active CN113063412B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110318362.5A CN113063412B (zh) 2021-03-25 2021-03-25 一种基于可靠性分析的多机器人协同定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110318362.5A CN113063412B (zh) 2021-03-25 2021-03-25 一种基于可靠性分析的多机器人协同定位与建图方法

Publications (2)

Publication Number Publication Date
CN113063412A true CN113063412A (zh) 2021-07-02
CN113063412B CN113063412B (zh) 2022-12-02

Family

ID=76561918

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110318362.5A Active CN113063412B (zh) 2021-03-25 2021-03-25 一种基于可靠性分析的多机器人协同定位与建图方法

Country Status (1)

Country Link
CN (1) CN113063412B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113878578A (zh) * 2021-09-30 2022-01-04 上海景吾智能科技有限公司 适用于复合型机器人的动态自适应定位方法和系统
CN115371719A (zh) * 2022-10-10 2022-11-22 福思(杭州)智能科技有限公司 探测设备的参数标定方法和装置、存储介质及电子装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20100081588A (ko) * 2009-01-06 2010-07-15 삼성전자주식회사 로봇 및 그 제어방법
CN109308809A (zh) * 2017-07-29 2019-02-05 周晓辉 一种基于动态图像特征处理的隧道车辆监控装置
CN109387204A (zh) * 2018-09-26 2019-02-26 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN111310833A (zh) * 2020-02-19 2020-06-19 清华大学 一种基于贝叶斯神经网络的出行方式识别方法
CN112363158A (zh) * 2020-10-23 2021-02-12 浙江华睿科技有限公司 机器人的位姿估计方法、机器人和计算机存储介质
CN112419501A (zh) * 2020-12-10 2021-02-26 中山大学 一种地空异构协同地图构建方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20100081588A (ko) * 2009-01-06 2010-07-15 삼성전자주식회사 로봇 및 그 제어방법
CN109308809A (zh) * 2017-07-29 2019-02-05 周晓辉 一种基于动态图像特征处理的隧道车辆监控装置
CN109387204A (zh) * 2018-09-26 2019-02-26 东北大学 面向室内动态环境的移动机器人同步定位与构图方法
CN111310833A (zh) * 2020-02-19 2020-06-19 清华大学 一种基于贝叶斯神经网络的出行方式识别方法
CN112363158A (zh) * 2020-10-23 2021-02-12 浙江华睿科技有限公司 机器人的位姿估计方法、机器人和计算机存储介质
CN112419501A (zh) * 2020-12-10 2021-02-26 中山大学 一种地空异构协同地图构建方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113878578A (zh) * 2021-09-30 2022-01-04 上海景吾智能科技有限公司 适用于复合型机器人的动态自适应定位方法和系统
CN113878578B (zh) * 2021-09-30 2024-01-16 上海景吾智能科技有限公司 适用于复合型机器人的动态自适应定位方法和系统
CN115371719A (zh) * 2022-10-10 2022-11-22 福思(杭州)智能科技有限公司 探测设备的参数标定方法和装置、存储介质及电子装置
CN115371719B (zh) * 2022-10-10 2023-01-24 福思(杭州)智能科技有限公司 探测设备的参数标定方法和装置、存储介质及电子装置

Also Published As

Publication number Publication date
CN113063412B (zh) 2022-12-02

Similar Documents

Publication Publication Date Title
CN112902953B (zh) 一种基于slam技术的自主位姿测量方法
US10553026B2 (en) Dense visual SLAM with probabilistic surfel map
CN111563442A (zh) 基于激光雷达的点云和相机图像数据融合的slam方法及系统
CN108615246B (zh) 提高视觉里程计系统鲁棒性和降低算法计算消耗的方法
CN112304307A (zh) 一种基于多传感器融合的定位方法、装置和存储介质
CN111429574A (zh) 基于三维点云和视觉融合的移动机器人定位方法和系统
CN112784873B (zh) 一种语义地图的构建方法及设备
CN113063412B (zh) 一种基于可靠性分析的多机器人协同定位与建图方法
CN112505065A (zh) 一种实现室内无人机对大部件表面缺陷进行检测的方法
JP2006260527A (ja) 画像マッチング方法およびこれを用いた画像補間方法
JP2007310707A (ja) 姿勢推定装置及びその方法
CN110223351B (zh) 一种基于卷积神经网络的深度相机定位方法
CN112651944B (zh) 基于cad模型的3c部件高精度六维位姿估计方法及系统
CN112991534B (zh) 一种基于多粒度物体模型的室内语义地图构建方法及系统
CN111739066B (zh) 一种基于高斯过程的视觉定位方法、系统及存储介质
CN116449384A (zh) 基于固态激光雷达的雷达惯性紧耦合定位建图方法
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合系统
CN112017188B (zh) 一种空间非合作目标语义识别与重构方法
CN113984068A (zh) 定位方法、定位装置和计算机可读存储介质
CN112465858A (zh) 基于概率网格滤波的语义视觉slam方法
CN111812978B (zh) 一种多无人机协作slam方法与系统
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
CN115218906A (zh) 面向室内slam的视觉惯性融合定位方法及系统
CN114066773B (zh) 一种基于点云特征与蒙特卡洛扩展法的动态物体去除
CN117367427A (zh) 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法

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