动态阈值点云匹配的移动智能体相对位姿的协同获取方法技术

本发明专利技术公布了一种动态阈值点云匹配的移动智能体相对位姿的协同获取方法，利用共享激光点云数据与自主定位结果，采用改进的迭代近邻点(ICP)配准算法，计算移动智能体之间的相对位姿，并评估计算结果的可信性，进一步提升相对位姿的观测可靠性与应用价值。本发明专利技术方法只需在自主定位基础上复用激光雷达数据，无需额外的增强标志即可完成相对位姿的观测计算，对原有系统改动小，不影响系统原有布局，无需依赖其他特征增强硬件，无附加影响，成本较低；具有更好的准确性和更强的鲁棒性，且对于点云配准结果的有效性评估使得本发明专利技术更符合相对位姿的应用场景，增强了对结果利用的可靠性。增强了对结果利用的可靠性。增强了对结果利用的可靠性。

【技术实现步骤摘要】
动态阈值点云匹配的移动智能体相对位姿的协同获取方法


[0001]本专利技术属于多智能体协同定位
，具体涉及一种动态阈值点云匹配的移动智能体相对位姿的协同获取方法，通过共享移动智能体的自主定位结果和雷达点云数据，实现移动智能体间灵活可靠的相对位姿的获取与应用。

技术介绍

[0002]近些年来，移动智能体愈发成为信息科技产业的热门方向，以SLAM(即时定位与建图)为代表的在未知环境内自主定位是移动智能体得以安全、高效运行的基础。同时伴随群体智能、社群智能等概念的推广，多智能体系统迅速发展。尤其是室内环境下，低速运动的任务场景中，相对位姿具有支撑合作建图，定位优化，协同调度等功能，在多智能体系统尤其多机器人协同SLAM中地位重要。
[0003]相对位姿即多智能体系统中某一移动智能体的位姿在另一智能体参考系下的结果，需要通过有效的方式进行获取。直接通过自主定位作差获得相对位姿的方式，会使得自定位误差叠加，精度较差，难以有效使用，故需要基于观测的相对位姿获取方法。
[0004]随着该领域研究的日益蓬勃，迄今诞生了一定数量的的基于观测的相对位姿获取方法，其中的多数基于已知的对象特征实现观测。这样的方式依赖单体SLAM质量或依赖额外的标识物，鲁棒性不强，且装置配备成本高。而通过采集的激光点云数据，基于点云配准的方式独立性较强，适合用于进行相对位姿的间接观测，用以支撑协同SLAM以及多智能体系统的其他功能。同时，在网联条件下，容易通过共享自定位结果获得点云配准所必需的初值，因此点云配准在对应场景下具有相当可观的适用性。
[0005]点云配准方法中，基于传统数学优化理论的典型方法为迭代近邻点(ICP)，原始ICP方法在多机相对观测、点云仅部分重叠的任务场景下，匹配性能并不足够理想，而其他基于ICP的改进方法侧重于提取结构特征和提高匹配速度，没有针对点云部分重叠场景下的匹配效果进行改善。同时，原始ICP的评估方式在室内环境、低速运动的任务场景下不适用，在点云部分重叠时无法有效反映相对位姿的测量准确性，其他基于ICP的改进方法并没有在结果评估方面做出改善与补充。现有的相关技术难以实现准确且可靠地获取室内场景下低速作业的多智能体系统之间的相对位姿。

技术实现思路

[0006]本专利技术提出了一种应用于多智能体系统中的，基于动态阈值点云匹配的移动智能体相对位姿协同获取方法及应用装置，装置包括自主定位模块、数据通信模块、ICP配准模块、位姿关系可信度评估模块，实现以较低的成本快速且可靠地获取多智能体系统中的智能体相对位姿。
[0007]本专利技术中，在室内地面上低速移动作业移动智能体(包括但不限于自动导引运输车(AGV，Automated Guided Vehicle)、具有自主移动能力的机器人等)上配备必要的运动传感器、环境感知设备，以支撑自主定位；并在网联条件下配备有数据通信模块，以支持信
息共享。利用共享激光点云数据与自主定位结果实现改进的迭代近邻点(ICP)配准算法，计算智能体之间的相对位姿，并对该计算结果可信性加以评估，进一步提升相对位姿的观测可靠性与应用价值。该方法可应用在室内准封闭场景下低速作业的多智能体系统上，不影响原有系统布局，无需依赖其他用以增强特征的硬件标志，以较低的成本实现快速且可靠的相对位姿获取与应用。
[0008]为达到上述目的，本专利技术要求在移动智能体上装备有相应的运动传感器(惯性测量单元，轮速计等)与环境感知设备(其中必需激光雷达)，使其能够形成具有分米级精度的自主定位模块。并且装有数据通信模块，使得不同智能体之间可以通过网联共享数据。每个移动智能体通过网联通信得到其他智能体实时的自主定位结果与激光点云数据。具体的相对位姿协同获取与应用步骤如下：
[0009]1)根据每个移动智能体上的传感器采集的数据，通过自主定位模块得到每个智能体分别的自主定位结果。
[0010]2)通过数据通信模块，各个需要进行相对观测的移动智能体之间共享各自的激光点云数据与1)中得到的自主定位结果。
[0011]3)根据2)中共享的激光点云数据与自主定位结果，通过坐标预变换，基于动态阈值的逐步ICP点云配准，坐标反变换，输出两个智能体间的相对位姿观测结果。
[0012]4)基于两个移动智能体采集的两簇激光点云数据，通过KD树有效点的提取，对有效点做数量统计与线性回归分析，输出重叠度指标与线性指标，用以评估3)中计算的相对位姿关系的可信度。
[0013]5)将3)中的相对位姿观测结果与4)中的相对位姿可信度指标打包，提供给自主定位模块以优化，形成更准确可靠的协同定位；进一步提供给中心服务器或其他智能体以支持系统的其他功能。
[0014]与现有技术相比，本专利技术的有益效果是：
[0015]本专利技术提供一种基于动态阈值点云匹配的移动智能体相对位姿协同获取及应用方法，只需在自主定位基础上复用激光雷达数据，无需额外的增强标志即可完成相对位姿的观测计算，对原有系统改动小，无附加影响，成本较低。本专利技术提出的基于动态阈值的改进ICP点云配准方法有更好的准确性和更强的鲁棒性，且位姿关系结果可信度评估模块对于点云配准结果的有效性评估更符合相对位姿的应用场景，增强对该结果利用的可靠性。
[0016]本专利技术提出的基于动态阈值点云匹配的移动智能体相对位姿协同获取方法及应用具有以下优点：
[0017](一)利用原始激光传感器的点云数据，将原始ICP中固定的配准阈值修改为一组动态递减的阈值，在基本不增加时间开销的前提下，点云配准的变换结果误差至少降低20％。
[0018](二)针对部分重叠场景下原始ICP的评分无法准确反映相对位姿测量准确性的缺点，结合基于聚类算法的阈值计算策略，利用KD树提取出有效点及其距离。对有效点计数与线性回归，作为相对位姿观测结果可信度的指标，有利于提升对相对位姿获取结果应用的可靠性。
[0019](三)所述的改进点云配准算法，可应用于多智能体分布式协同定位场景。分布式协同定位与相对位姿观测二者间形成耦合架构，前者为后者提供相对可靠的初值，后者优
化前者结果，形成良性循环。
附图说明
[0020]图1是本专利技术相对位姿获取与应用系统结构框图。
[0021]图2是本专利技术点云ICP配准模块算法的流程框图。
[0022]图3是本专利技术配准结果评估流程框图。
[0023]图4是本专利技术有效阈值计算模块流程框图。
具体实施方式
[0024]为使本专利技术的目的、特征与优势更加清晰易懂，下面结合附图和具体实施方式对本专利技术作进一步详细的说明。
[0025]参考图1，本专利技术需要系统各移动智能体上配备一套传感器，其中环境感知设备必须包含激光雷达，运动传感器至少包含惯性测量单元，轮速计，用于进行室内准封闭场景下的自主定位。如图1所示，依赖各智能体的自主定位结果和激光点云数据，可以实现基于动态阈值点云匹配的相对位姿协同获取，故各智能体之间需要通过数据通信模块在网联条件下共享自己实时的自主定位结果和激光本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种动态阈值点云匹配的移动智能体相对位姿的协同获取方法，利用共享激光点云数据与自主定位结果，采用改进的迭代近邻点(ICP)配准算法，计算移动智能体之间的相对位姿，并评估计算结果的可信性，进一步提升相对位姿的观测可靠性与应用价值，不影响系统原有布局，无需依赖其他特征增强硬件；包括如下步骤：1)根据每个移动智能体上的传感器采集的数据，分别获取每个移动智能体的自主定位结果；2)将上一步得到的每个移动智能体的自主定位结果与移动智能体本身的激光点云数据，进行时间戳同步并重新封装；每个移动智能体通过网联通信进行数据共享，并得到其他智能体实时的自主定位结果与激光点云数据；3)进行动态阈值的点云匹配，获取智能体之间的相对位姿；根据共享的激光点云数据与自主定位结果，待观测智能体点云P
m
通过坐标系变换，得到待配准的目标点云P
′
m
；自身智能体点云P
n
进行基于动态阈值的逐步ICP点云配准、执行刚体变换后得到点云P
′
n
；再输出两个智能体间的相对位姿观测结果；包括如下步骤：S31：获取距离阈值，方法为：取待观测智能体点云P
m
与自身智能体点云P
n
的最大激光测量距离，记为l；二者激光角分辨率较大的值为δ；构造一组递减的等差数列作为距离阈值，记该等差数列为{a
i
}，i＝1
…
k，其中，项数k也是动态匹配次数；{a
i
}的首项为αklδ，末项为αlδ，公差为
‑
αlδ；α为距离阈值伸缩参数；S32：通过两个移动智能体的自主定位结果计算二者之间的坐标系变换关系；将点云P
m
变换为点云P
′
m
；在编号为m的智能体坐标系中，点云P
m
中的每一个点包含x方向和y方向两个维度的数据，点坐标表示为点云P
m
每一个点坐标在编号为n的智能体坐标系中的坐标为表示为：上式即表示点云在两个不同的智能体坐标系之间的变换关系；其中，A为二维旋转矩阵，即是以β为旋转角的二维旋转矩阵；S33：分k步计算得到点云P
n
向点云P
′
m
的配准矩阵；包括：首先，记P
n,0
＝P
n
；对正整数i，i＝1
…
k，第i步设置ICP匹配的阈值为a
i
，执行ICP匹配，其中待配准点云为P
n,i
‑1；匹配所得刚体变换结果为线性刚体变换方阵tran
i
，P
n,i
‑1变换后的点云数据记为P
n,i
；变换后得到点云P
′
n
；P
′
n
＝P
n,k
；配准后得到结果为二维线性刚体变换方阵totaltran，其中∏表示累乘；
S34：totaltran是一个3行3列的线性刚体变换方阵，表示为：定义三个元素Δx,Δy,Δθ：Δx＝totaltran
1，3
；Δy＝totaltran
2，3
；其中Δθ二维刚体变换totaltran的旋转角；Δx,Δy分别表示totaltran在x/y方向上的平移量；对于点云P
n
中的数据点坐标totaltran作用在该数据点上得到的坐标即为点云P
′
n
中与之对应的数据点的坐标；表达式为：上式与S32中的表达式进行联立：得：得：得到编号为n、m的两个智能体之间的相对位姿其中是以编号为n的智能体为参考系的二维笛卡尔坐标系中，编号为m的智能体定位估计的横坐标与纵坐标的估计值；是以编号为n的智能体为参考系的二维笛卡尔坐标系中，编号为m的智能体的朝向角度估计值；4)设计可信度评估指标，...

【专利技术属性】
技术研发人员：程翔，李昊臻，刘恩嵩，李思江，郑心湖，杨柳青，
申请(专利权)人：香港科技大学广州，
类型：发明
国别省市：