CN114049385A

CN114049385A - 基于自适应配准的多激光雷达自主建网系统、方法、终端

Info

Publication number: CN114049385A
Application number: CN202111202942.4A
Authority: CN
Inventors: 胡鑫; 涂志莹; 刘钦泽; 初佃辉
Original assignee: Harbin Institute of Technology Weihai
Current assignee: Harbin Institute of Technology Weihai
Priority date: 2021-10-15
Filing date: 2021-10-15
Publication date: 2022-02-15

Abstract

本发明公开一种基于自适应配准的多激光雷达自主建网系统、方法、终端，涉及网络构建与网络信息处理技术领域。采用多个激光雷达利用点云配准算法对不同雷达的点云图进行配准和融合，点云配准采用4PCS进行粗配准，Trimmed ICP进行精配准，使同一室内环境的激光雷达自动组建雷达网络，并获得融合后的室内环境点云。数据同时保存本地和云端，使用者通过云端更改相应配准参数或者获取原始点云数据。本发明针对居家养老项目设计的基于自适应配准的多激光雷达自主建网系统。使用者可以远程查看老人居住室内的激光雷达网络情况以及室内环境点云图像。通过雷达网络和环境点云数据，使用者可以用在居家养老项目的进一步设计和使用中。

Description

基于自适应配准的多激光雷达自主建网系统、方法、终端

技术领域

本发明属于网络构建与网络信息处理技术领域，尤其涉及一种基于自适应配准的多激光雷达自主建网系统、方法、接收用户输入程序存储介质、信息数据处理终端。

背景技术

目前，激光雷达从上世纪90年代起得到了迅速的发展。激光雷达自身通过发射激光来获取环境的坐标等信息，由于激光本身的特性，不易受外界环境的影响，在夜间也能正常运行，可靠性较高。因此各个领域中得到了广泛应用。1960年左右，2D点云配准技术最早被使用，后来随着科技和设备的发展和发明的深入，3D点云配准也逐渐应用。

目前，2D点云配准主要分为点云粗配准和精配准两个阶段。点云粗配准主要目的是为位置初始位置的两个点云进行大致配准，为点云精配准提供较好的初始位置。目前点云粗配准最普遍的方法是基于RANSAC(Random Sample Consensus,随机采样一致性)思想的配准。1981年由Fischler等人首次提出。而在21世纪初，Aiger等人提出了基于RANSAC思想的4PCS算法，该算法每次选取共面四点，根据刚性变化来寻找近似全等的四点集，并将其作为对应点集求解。虽然4PCS配准效果不错，但是计算复杂度较高，效率低下，为了解决这个问题Rusu等人于2009年提出SAC-IA算法，该算法基于RANSAC和FPFH描述子，显著降低的计算复杂度，提高了配准效率。精配准方法中最典型的当属ICP，初代ICP提出后经过多年的发明和发展，Liu等人提出了LA-ICP,CHETVERIKOV提出了基于重叠率的一种配准方法Trimmed ICP。

近年来，国内对于激光雷达的发明也越来越丰富。点云配准在许多领域有着不可替代的作用，如：仿真模型自动重建，岩石钻孔激光重建，大尺寸工业测量，建筑物扫描等。虽然已经有不少软件提供人工标定的方法来准确标定点云，但是在不少地方还是需要自动化配准的使用。有不少学者对于激光雷达点云配准算法做出了改进和发明。2007年，戴静兰等人开始了ICP在配准方面的发明。2009年，杨现辉在发明ICP在3D点云中的配准应用中，对ICP算法做出了改进，使ICP不再直接全局配准而是先计算曲率获得相应点云特征点。2015年韩宝昌等人提出一种基于区域生长的点云自动配准方法来解决点云全局配准中不能有效处理重叠率较低的点云数据的问题。2018年陈旭等提出了一种点云粗配准方法，计算点云主成分向量，通过配合自身中心点形成主成分坐标系。

通过上述分析，现有技术存在的问题及缺陷为：

(1)多激光雷达的配准和融合一直是一个热门的发明话题，但大多数发明多为预先设定关键点用作标定参数，或者设置激光雷达的相对位置，加入人工操作来进行配准和融合。人工干预配准融合可以达到一个很好的配准精度，但是在某些情况下需要多激光雷达自动配准融合。而现有技术自动配准融合效果差。

(2)现有技术对远程查看老人居住室内的激光雷达网络情况以及室内环境点云图像获得的信息准确性差。

(3)现有技术对用户的隐私信息保活性差。

(4)现有技术据的完整性和可靠性差。

解决以上问题及缺陷难度和意义为：

对于环境信息的获取，激光雷达有着自己的独特优势。激光雷达是一种主动式的传感器，受外界干扰小，探测范围大，测距精度高，能够比较比较迅速获取当前环境的坐标信息。相比计算机视觉的手段，激光雷达不受外界环境、光线的干扰，数据量更小，计算起来更加快捷，消耗资源更少，使用起来更方便。

但是由于单激光雷达的局限性，对于较复杂环境会有遮挡和精度限制，导致识别精度较低。本发明通过多激光雷达自主建网形成雷达网络组建小型物联网可以减少遮挡，提高识别精度和范围。

多激光雷达点云图像的标定和融合已经有许多方法，但现行的大多数标定多为人为设置关键点或者雷达的相对位置。在某些情况下，人工标定无法进行。而本发明的多激光雷达自适应配准可以在未知环境下完成精度较好的配准和点云融合。

本发明面向居家养老，目的在于发明出一套简单可行的方案去获得室内的点云环境。点云的环境对于进一步医疗、养老方面的发明有着重要意义。一方面，多激光雷达目标识别之前需要进行标定和配准，而本设计可以为目标识别提供有效参数；另一方面，本设计使用自适应点云配准，可以实现养老自动化检测的前置条件。

目前点云配准算法还存在很多待改进的方面，仍然面临着许多困难。比如未知环境下点云配准的参数应该如何设定；低重叠度下，不同激光雷达的图像标定和配准精度较低；点云的初始姿态对于点云配准的精度和效果有很大的影响。本设计可以在居家老人的室内环境下，实现一定程度的自适应点云配准，获得较好的效果。

同时，该发明又具有很大的社会意义，在目前严峻的人口老龄化形势的挑战下，利用计算机技术帮助老年人减少健康风险，减轻社会负担是一个很实用并具有前景的发明题目。本发明基于激光雷达，成本小，操作简单，轻便小巧，对日常生活影响较小；监测老人的居住环境，既可以便于了解老人的生活状态，又可以获得老人的居住环境数据，可以进一步对老人的健康状态进行进一步的综合分析。综合看来，本发明具有易操作性、实用性以及可扩展性，对老年人的行走分析健康领域具有更加深远的意义。

发明内容

为克服相关技术中存在的问题，本发明公开实施例提供了一种基于自适应配准的多激光雷达自主建网系统、方法、接收用户输入程序存储介质、信息数据处理终端。

本发明目的在于如何通过点云配准实现多激光雷达自主建网。要实现这个目标，需要选用合适的方案，综合考虑到室内监测环境，隐私保护，实验简易可行性等因素。本发明将基于激光雷达以及其他必要的硬件设备，对室内环境进行扫描，获取环境的点云数据，并对不同雷达的点云进行配准，将同一室内环境的激光雷达加入雷达网络，用组建的雷达网络获取当前环境的融合点云图。

本发明目的还在于进行：激光雷达数据的采集与数据处理。激光雷达扫描的数据内容较多，包括一些无用信息需要舍弃。另外，由于激光雷达的扫描精度限制等因素，需要对扫描的点云进行数据处理。

点云配准。主要包括两个方面，自适应计算配准参数，根据配准参数进行粗配准和精配准，将目标点云通过变换矩阵移动到与源点云相同的坐标系中，实现点云配准和图像融合。

自主建网。在点云配准的基础上，将能够配准成功的激光雷达加入雷达网络，组建雷达网络。在点云配准前，各机器会自动更新局域网内各机器IP和主机名以及ROS系统配准。

所述技术方案如下：

根据本发明公开实施例的第一方面，提供一种基于自适应配准的多激光雷达自主建网方法，包括：

步骤一，树莓派主机根据数据处理后的点云计算室内环境的大致面积来修改配准参数delta，overlap，dist；首先找到2D点云中x,y均为最大的点A(x1,y1)，x最大，y最小的点B(x2,y2),x最小，y最大的点C(x3,y3)以及x最小y最小的点D(x4,y4)；根据这四点来求点云的大致面积(单位为m²)：

根据Area的大小来修改delta，overlap和dist；

步骤二，采用多个激光雷达利用点云配准算法对不同雷达的点云图进行配准和融合，所述对点云图进行配准点云配准采粗配准和精配准，使同一室内环境的激光雷达自动组建雷达网络，并获得融合后的室内环境点云；

步骤三，对获得的融合后的室内环境点云数据同时保存本地和云端，用户通过云端更改相应配准参数或者获取原始点云数据。

根据本发明公开实施例的第二方面，提供一种基于自适应配准的多激光雷达自主建网系统包括：

激光雷达模块，包含对激光雷达的操作，用于激光雷达驱动以及获取的数据的上传；

数据处理模块，用于对点云数据的处理分析，以及对处理结果的展示；

云端数据模块，用于更新局域网内IP和主机名，上传原始数据，获取配准参数；

自适应配准模块，用于根据环境调整配准参数，粗配准，精配准；

点云可视化展示模块，用于展示点云融合展示，可视化展示；

自主建网模块，用于自动化建网；

数据获取模块，用于获取激光雷达点云数据，采用智能网关下载平台进行下载获取。

根据本发明公开实施例的第三方面，提供一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行所述基于自适应配准的多激光雷达自主建网方法。

根据本发明公开实施例的第四方面，提供一种信息数据处理终端，其特征在于，所述信息数据处理终端包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行所述基于自适应配准的多激光雷达自主建网方法。

本发明公开的实施例提供的技术方案可以包括以下有益效果：

本发明目标为设计一套简单的系统来自动组建雷达网络，减少发明人员的前期准备工作。由于居家养老系统的受众为老人群体，所以系统安装越简单越好。本系统只需要将激光雷达和树莓派放置在相应位置开机即可，大大降低了使用门槛。本发明采用多个激光雷达利用点云配准算法对不同雷达的点云图进行配准和融合，点云配准采用4PCS进行粗配准，Trimmed ICP进行精配准，使同一室内环境的激光雷达自动组建雷达网络，并获得融合后的室内环境点云。数据同时保存本地和云端，使用者可以通过云端更改相应配准参数或者获取原始点云数据。

本发明根据环境自动计算点云配准所需要的配准参数，可以在不同室内环境下完成点云配准。本发明对实验室环境和实际老人居家环境进行了测试，可以较好完成点云配准和自主建网。

相比于现有技术，本发明的优点进一步包括：

本发明主要是针对居家养老项目设计的基于自适应配准的多激光雷达自主建网系统。通过本系统，使用者可以远程查看老人居住室内的激光雷达网络情况以及室内环境点云图像。通过雷达网络和环境点云数据，使用者可以用在居家养老项目的进一步设计和使用中。

本发明的创新性主要有以下方面：

(1)本发明是针对老年人的居家养老综合监测平台的一部分。本发明为居家养老的进一步发明提供了自动化帮助和数据支持。

(2)激光雷达可靠性强，不受光线环境影响，夜间也能使用。且雷达较小，放置比较方便。数据量较小，上传云和计算处理速度都比较快，且不会暴露任何隐私信息。

(3)本发明实现了激光雷达的自主建网，大大方便了分析人员的使用。云边协同保证了数据的完整性和可靠性。

本发明为解决由于激光雷达的硬件限制，配准精度并不是很高。更换每秒扫描点数更多，扫描精度更高的激光雷达，可以做到更好的配准效果。

多激光雷达自主建网对于雷达摆放的位置要求较高，如果放置位置不好，可能会导致判断失误，建网失败。本发明进一步采用相机加激光雷达的形式，会达到更好效果。

当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本发明公开。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施例，并与说明书一起用于解释本公开的原理。

图1是本发明实施例提供的基于自适应配准的多激光雷达自主建网方法流程图。

图2是本发明实施例提供的基于自适应配准的多激光雷达自主建网系统示意图。

图2中:1、激光雷达模块；2、数据获取模块；3、数据处理模块；4、云端数据模块；5、自适应配准模块；6、点云可视化展示模块；7、自主建网模块。

图3是本发明实施例提供的自适应配准程序的流程图。

图4是本发明实施例提供的基于自适应配准的多激光雷达自主建网系统架构图。

图5是本发明实施例提供的自主建网流程图。

图6是本发明实施例提供的1号雷达点云与室内环境标准点云配对效果图。

图7是本发明实施例提供的2号雷达点云与1号雷达点云配对效果图。

图8是本发明实施例提供的4号雷达点云与1号雷达点云配对效果图。

图9是本发明实施例提供的1,2,4号雷达点云融合效果图。

图10是本发明实施例提供的激光雷达扫描的点云效果图。

图11是本发明实施例提供的展示了1,2号机点云配准的结果效果图。

具体实施方式

这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本公开相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本公开的一些方面相一致的装置和方法的例子。

1.本发明公开实施例所提供的基于自适应配准的多激光雷达自主建网方法，包括：

步骤1，树莓派主机根据数据处理后的点云计算室内环境的大致面积来修改配准参数delta，overlap，dist；首先找到2D点云中x,y均为最大的点A(x1,y1)，x最大，y最小的点B(x2,y2),x最小，y最大的点C(x3,y3)以及x最小y最小的点D(x4,y4)；根据这四点来求点云的大致面积(单位为m²)：

根据Area的大小来修改delta，overlap和dist；

步骤2，采用多个激光雷达利用点云配准算法对不同雷达的点云图进行配准和融合，所述对点云图进行配准点云配准采粗配准和精配准，使同一室内环境的激光雷达自动组建雷达网络，并获得融合后的室内环境点云；

步骤3，对获得的融合后的室内环境点云数据同时保存本地和云端，用户通过云端更改相应配准参数或者获取原始点云数据。

2.下面结合系统需求分析对本发明技术方案作进一步描述。

2.1系统整体需求分析

本发明需要设计一个可以使同一室内环境内的激光雷达自动组建雷达网络，并获得当前室内环境融合点云图。使用者只需要将激光雷达放置在适当位置开机即可组建雷达网络，之后可以从minio云或者主机获取室内环境融合点云图。

2.1.1总体业务流程

如图1所示，本系统的业务流程包括激光雷达的数据采集、数据预处理、点云配准、图像融合、结果展示。这几个流程代表着整个系统的主要工作运行步骤。本系统的数据来源为激光雷达，开始前，将多个激光雷达放置在合适位置，雷达扫描的数据保存在主机中，同时会将数据上传到云以供云端和本地操作。点云配准和图像融合均在树莓派主机中运行，结果通过VNC连接树莓派展示。

2.1.2功能性需求分析

(1)激光雷达数据采集

本系统是通过多个激光雷达同时采集数据，并传输到主机以及云。为了同时传输和接收多个从机发送的数据，利用ROS实现多机通讯。ROS多机通讯通过ROS的topic，node和message来完成。Topic相当于一个中转区，发送者节点发送消息到topic，接受者节点订阅该topic来获取发送的message。主机通过订阅话题，接收从机发送的message。接收数据后，同时将数据上传到minio云。

(2)激光雷达数据处理

本系统获得的激光雷达原始数据由于激光雷达本身震动和精度限制等，不能直接用于下一步的点云配准，需要进行数据处理。数据处理首先需要去除数据中的动点，提取静点并去除静点中所有重复点。然后去除点云数据中的离群点。

(3)点云配准和图像融合

在经过数据预处理之后，将主机点云与各从机点云依次进行点云配准，将能够配准(同一室内环境)的激光雷达加入雷达网络。最后将雷达网络中各激光雷达扫描点云进行融合，形成一个相对完整的室内环境点云图。

(4)可视化展示

本系统采用PCL的PCLVisualizer以及cloudcompare来实现可视化。为了展示配准效果，分别为源点云，目标点云，配准后点云赋予不同的颜色在同一窗口内展示。最后展示雷达网络融合的室内环境点云图。

2.1.3非功能性需求分析

(1)运行环境

本系统需要多台(大于三台)2D激光雷达，用来采集数据。本发明采用的是深圳市杉川机器人有限公司生产的Delta-1A系列激光雷达如图2-1。该型号的激光雷达扫描频率为6hz，即1s扫描6帧数据，每秒扫描点数量为4000，每帧数据大约为700，且受光线影响小，即使夜间也可以正常运行。

多台(与激光雷达数量一致)树莓派Raspberry Pi 4B微型计算机，用来连接雷达并进行相应计算和存储工作。树莓派是一种微型电脑，其系统基于Linux，但能够完成家用PC的基本功能。使用树莓派驱动雷达，对激光雷达的环境布置更加友好、便捷。同时，它可以保证持续工作的需求。本发明采用Raspberry Pi 4B+型号的树莓派，从机安装ubuntumate20.04服务器版和ROS noetic，主机安装ubuntu mate 20.04桌面版，ROS noetic，SSH，realVNC和PCL 1.10。SSH用来连接其他从机，realVNC用于共享桌面到Windows系统下PC中便于操作和使用，PCL用于进行点云计算。一台装有realVNC，SSH服务,XFTP的PC，用于个人调试数据和配准树莓派4B。所有程序均在树莓派4B中编译运行。

一个云存储服务器MiniO,用来存储Delta-1A激光雷达扫描的原始点云数据，以及点云配准的预设初始参数。大小4GB以上。

(2)性能需求

本发明需要Delta-1A激光雷达以及树莓派Raspberry Pi 4B+可以同时持续工作6小时以上，保证激光雷达可以在一天内持续不断的扫描周围环境，树莓派计算机同步保持数据文件上传。由于本系统只需要最新的室内环境点云数据，所以只需要保存最新的点云数据，数据大小不超过500kb，所以8G内存树莓派完全能够完成存储和计算任务。

(3)可移植性需求

本发明的激光雷达模块需搭建在要被监测的室内环境中，使用树莓派RaspberryPi计算机进行驱动，树莓派需要安装Ubuntu MATE 20.04 LTS系统，ROS noetic，PCL 1.10以及其他依赖包。

(4)可靠性需求

由于数据量小，每个激光雷达扫描500帧的数据大小经过处理后小于100kb，数据接收出问题概率较低。另外如果数据接收出错，系统会根据错误自动重传数据。

2.2系统功能模块

本系统的各个功能模块的概要设计方案。如图2所示，整个基于自适应配准系统的总体模块如下，整个系统分为激光雷达模块1、数据获取模块2、数据处理模块3。

激光雷达模块1包含对激光雷达的操作，主要是激光雷达驱动以及获取的数据的上传。

数据获取模块2用于获取激光雷达点云数据，采用智能网关下载平台进行下载获取。

数据处理模块3是本系统的核心所在，主要是对点云数据的处理分析，以及对处理结果的展示。

云端数据模块4，用于更新局域网内IP和主机名，上传原始数据，获取配准参数。

自适应配准模块5，用于根据环境调整配准参数，粗配准，精配准。

点云可视化展示模块6，用于展示点云融合展示，可视化展示。

自主建网模块7，用于自动化建网。

2.3系统核心业务流程

本系统的核心为自适应配准。自适应配准程序的流程图如图3所示，该图描述了系统的核心功能的运行流程，主要流程有从云端读取配准参数，计算房间大致面积，根据环境调整参数，粗配准，精配准，判断是否配准。

2.4系统架构

如图4所示。激光雷达作为数据源，树莓派从机从激光雷达获取数据，通过ROS发送消息。树莓派主机和MiniO云订阅从机发送的消息。树莓派主机接收消息后，对消息进行数据处理得到处理后的点云数据。同时主机从MiniO云获得初始配准参数，此时计算室内环境的大致面积，通过面积调整相应配准参数。使用调整后的配准参数对点云进行粗配准，如果配准成功，则此时点云的初始位置已经很好，再使用精配准提高配准的精度。精配准后进行图像融合以及可视化。随后根据配准的结果进行自主建网。

3.下面结合系统详细设计对本发明技术方案作进一步描述。

3.1激光雷达模块1设计

本发明利用激光雷达采集所需要的数据的一系列操作。包括激光雷达的设置，激光雷达的启动，数据传输格式，ROS多机通讯等。

3.1.1激光雷达的摆放设置

激光雷达虽然精度高，但是由于其是直线发射激光，受环境中遮挡物的影响比较严重。所以激光雷达的位置需要选择在受遮挡程度较小，能够扫描到房间大部分物体的地方。另外多激光雷达的自适应点云配准需要点云重叠度最低在40％以上，所以多个激光雷达最好能够扫描到彼此，这样点云的重叠度会比较高，点云配准的效果较好。最后选择将激光雷达放置在墙边和沙发底这样受遮挡程度小，但是能够扫描到大部分环境的位置。最后为了避免扫地或者其他能够影响雷达位置的因素，将激光雷达放置在一个用细柱子组成小盒子中。金属外壳保证了激光雷达不会被踩踏等因素而导致移动，里面的卡扣让激光雷达和树莓派能够良好固定，既不会遮挡激光雷达，又减小了放置面积。

3.1.2激光雷达驱动

将Delta-1A激光雷达与树莓派4B+连接。该型号的激光雷达共有三种驱动，包括win，Linux，ROS，这里使用ROS版本的驱动。通过XFTP将驱动上传到树莓派，安装相应驱动，打开驱动端口权限。然后编译运行，build通过后激光雷达成功扫描数据并在ROS中显示

3.1.3激光雷达数据采集

本系统中激光雷达的数据采集使用ROSbridge获取激光雷达驱动扫描的点云数据，并使用自定义消息格式化数据。本系统使用的Delta-1a型号的激光雷达扫描频率为6hz，每秒获取的点数大约为4000个，为了获取比较好的配准效果以及减小误差，拟采集2-3分钟，大约500帧的数据。

3.1.4树莓派多机通讯

为了实现树莓派主机和从机之间的多机同时通讯，采用ROS来完成主从机之间的消息传输和通讯。ROS的通讯方式是ROS系统最为重要的概念，本系统采用topic的方法，是一种点对点的单向通信方式。传递过程基本如下：发送者节点和接收者节点都要到节点管理器进行注册，然后发送者会发布topic，接收者订阅相应话题从而实现ROS通讯。由于多台机器同时传输数据，可能会导致数据丢失，为了保证数据的完整性，主机通过SSH免密登录从机，控制从机信息的发送顺序和时间，从而避免数据传输失败。

3.2数据处理模块3的设计

功能：获取到本系统获取到激光雷达的点云数据信息之后，如何对原始数据处理和降噪以获得点云配准良好的初始数据。

3.2.1激光雷达数据解析

本激光雷达扫描得到的数据类型信息存储在sensor_msgs/num.msg中，具体数据结构内容介绍如表1、表2所示。在本系统中，对于本型号的激光雷达，每一扫描帧共得到约690-700个点数据。而在该型号的激光雷达驱动中，却只保留了360个点数据，其他数据全部丢弃。这个方法虽然一定程度减小了数据量同时减小计算时间，但却大大影响了后期点云配准的精度。所以本系统选择保留所有的数据点，使用不定长ranges和angles数组来保存激光雷达扫描的距离和角度，使用时间戳来保存每一帧的时间，并通过ROS节点名来命名每个雷达扫描的数据文件为hostname。由于range中有部分值为inf和0的点，而这些点是无效的，所以在发送者节点中用了循环判断来去除这部分无效点。

由于极坐标不好计算和表示，所以需要将激光雷达默认的极坐标系转换为直接坐标系。当发送者节点发送message前，直接使用上式(3-1)将扫描的距离和角度转换为直角坐标系下的x,y坐标，再通过ROS传输转换好的点云数据信息供点云配准使用。

表1行走参数表

表2 Header表

3.2.2静点提取

首先本发明将原始的点云数据进行初步处理，筛选出大多数保持静止的点。对点云数据的初筛可以基本上提取出对本系统分析有作用的点，排除掉动点对实验目标的影响，从而降低后续工作的难度，并保持更高的准确度。

对于提取动点，主要方法为寻找一段时间位置不变的点。因为每次激光雷达扫描的点云为一段时间内扫描的数据和，所以具体方法如下：扫描500帧数据；保留500帧数据中出现次数大于等于40次的点并去除其中重复的点。500帧大概有30000-40000个点，经过提取后静点数数量足够完成点云配准。

3.2.3离群点去除

对于激光雷达来说，数据采集不可避免会出现误差。由于实际环境以及激光雷达摆放位置的影响，雷达扫描的点云密度一般并不均匀，另外激光雷达本身硬件因素也会导致测量误差。这些误差产生的离群点会影响到点云配准所以需要提前去除掉。离群点与激光雷达扫描产生的噪点不同，由于噪点一般是均匀的，且出现概率较低，在3.2.2节中提取静点部分已经删除掉。离群点一般是指异常数据，孤立的点。

因为离群点本身的特性，如它的名字一样，离群点是低密度的，所以可以采用统计的方法，遍历激光雷达扫描点云将距离其他点较远的点删除掉。这里采用统计滤波(Statistical Outlier Removal)的方法，将全局离群点去除。具体方法是：遍历点云数据中所有点，计算每个点到它周围点的平均距离。如果该平均距离的值大于给定的标准域，则认为该点为离群点并将该点从点云中删除。其中标准域由平均值，标准差以及给定标准差倍数确定。设均值为μ，标准差为σ，点云数据中第n个点坐标为Pn(Xn,Yn,Zn),该点到任意一点Pm(Xm,Ym,Zm)的距离为：

点云中每个点到任一点距离的平均值公式为：

点云中每个点到任一点距离的标准差公式为：

设标准差倍数为std，算法实现需要k和std两个阈值，当某个点四周的k个点的平均距离在正常选定范围(μ-σ·std,μ+σ·std)内则保留该点，若不在该范围内，则认为其是离群点删除掉。在本系统中，将500帧点云数据一起进行统计滤波，最后得到滤波之后的点云用于配准。

3.3云端数据模块4设计

功能：如何将Delta-1A激光雷达扫描的原始数据上传到云端以及树莓派主机从云端下载初始配准参数。

3.3.1数据上传

当各个激光雷达扫描结束后，连接该激光雷达的树莓派会将数据保存为文本格式，通过txt转pcd将文本格式的点云数据转为专门用于处理点云的pcd数据格式。然后通过MiniO云与python3的接口将文件上传到云端，如果云端文件已经存在，则删除已有文件然后上传最新的原始数据。

3.3.2云端配准参数获取

在进行配准前，树莓派主机首先从MiniO云获取预设的配准参数。该配准参数可以由使用者根据具体环境和前几次配准效果调整后上传到云。预设的配准参数如表3，统计滤波参数表如表4.

表3配准参数表

表4统计滤波参数表

3.4自适应配准模块5设计

功能：如何为点云配准选择一个较好的配准参数，粗配准，精配准，判断是否配准，图像融合以及展示。

3.4.1配准参数自适应调整

在进行配准前，树莓派主机根据数据处理后的点云计算室内环境的大致面积来修改配准参数delta，overlap，dist。首先找到2D点云中x,y均为最大的点A(x1,y1)，x最大，y最小的点B(x2,y2),x最小，y最大的点C(x3,y3)以及x最小y最小的点D(x4,y4)。根据这四点来求点云的大致面积(单位为m²)：

根据Area的大小来修改delta，overlap和dist。

下面为具体参数设置：

若Area小于10m²，则认为其为小房间，delta设置为0.02m，overlap设置为0.65，dist设置为0.15。若Area小于20m²，则认为其为中等房间，delta设置为0.03m，overlap设置为0.6，dist设置为0.25。若Area小于20m²，则认为其为大房间，delta设置为0.05m，overlap设置为0.5，dist设置为0.4。如果大于30m²，则会超出激光雷达的限制，对于配准会有很大影响，但实际情况老人房间基本可能大于30m²，所以本系统不考虑30m²以上的情况。如果根据实际情况发现某组配准参数更好，可以修改参数并上传云。配准前会从云端下载最新的配准参数。

3.4.2基于4PCS的点云粗配准

点云配准是将两个激光雷达扫描点云通过平移，旋转最终变化到相同的坐标系下，实现最大的重叠。配准分为粗配准与精配准两步。精配准一般采用ICP算法，虽然ICP算法的点云配准效果不错，但对点云的初始位置对于配准的效果影响很大，如果初始位置不佳，精配准很容易陷入局部最优而导致配准失败，所以在精配准之前先进行粗配准将两个点云初步配准。粗配准是指在点云的位置无法确定且无人工标定的情况下，将两个点云初步配准，为精配准提供比较好的初始位置。粗配准主要有以下几种方法：PFH,3Dsc,NDT,RANSC以及4PCS等。

本系统采用4PCS进行粗配准。4PCS(4-Points Congruent Sets)是一种全局配准算法，能够实现任意初始位置的点云的快速配准，非常适合小型点云的粗配准。其大致原理如下：假设目标点云P，源点云Q,通过找出目标点云P与源点云Q中对应的n(n>3)组点进行旋转平移求解出T，然后在候选的n组T中选取具有最大重合的一组T。4PCS通过对应的同一平面四点组仿射不变性(实际上本系统使用2D点云不需要仿射不变性)去寻找对应点对。具体描述如下：在目标点云P中寻找满足要求的长基线(3.4节中选定的配准参数预估最小重叠率与基线选择直接相关，重叠率越大，基线越长)的共面四点基。比如选择一个共面四点基B＝{a,b,c,d},提取B的拓扑信息。根据点云旋转和平移变化中r1,r2的仿射不变性和式(3.6)计算共面四点基B间的两个比例因子r1,r2,再根据式(3.7)计算q1,q2∈Q(源点云)的的交点位置集合，来计算Q中所有长基线点对交点坐标，比较交点坐标确定匹配集合，ei≈ej表示寻找到对应的一致全等四点，i,j分别表示第i个和第j个Q中长基线点对。B＝{a,b,c,d}对应的一致全等四点表示为C＝{q1,q3,q4,q5}。将点云所有计算出P的共面四点集合用E＝{B1,B2,...,Bm}表示，其中用m表示P集合的数量，重复上述步骤得到全等四点集合D＝{C1,C2,...,Cn}，n为全等四点集合总数。最后在D＝{C1,C2,...,Cn}中寻找最优全等四点对，4PCS使用LCP策略寻找最优全等四点匹配，即计算全等四点旋转和平移变化参数，将四点转化应用到全局点云转化，记录全局配准中包含最大的一致区域匹配记为最优匹配，至此完成4PCS算法局部粗配准工作。

其中e为共面不共线四点a,b,c,d的交点。r₁和r₂表示比例，q₁和q₂表示目标点云Q中的任一点对，e₁和e₂表示对应点对可能的交点。若e₁和e₂的差在允许范围内，则认为其可能是a,b,c,d的对应共面点。

3.4.3基于TRICP的点云精配准

进行粗配准之后，此时目标点云与源点云已经处于一个比较好的初始位置上。精配准算法一般使用ICP算法，本系统使用ICP的改进算法Trimmed ICP。由于该算法基于ICP，所以先介绍ICP算法。

ICP算法(Iterative Closest Point迭代最近点))。即计算一个变化矩阵将目标点云变化到源点云A上，所有变化都是在同一笛卡尔坐标系下

ICP分为以下6个步骤：

1)点云归一化

目的：为点云数据选择一个标准且有效的笛卡尔坐标系，在配准前预先消除坐标变换带来的影响。

下式(3.8)取点云的中心：

其中A_i表示A点云第i点的坐标。接下来将每个点都减去这个平均值A_center，即将点云平移，使该点云的形心移动到原点上。

2)寻找最近点(对应点)

目的：寻找A,B两个点云集的对应点。即B点云在经过理论完全正确的平移旋转后应该和A点云是重合的，各个重合的点对就是对应点。此时理论上对应点间距离应该为0。而实际中不一定会有重合的点，此时计算距离最近的点即为对应点。下式(3.9)计算A,B点云空间距离最近的点。

其中

表示点云A中第i点的x坐标。

3)计算变化矩阵(旋转角度，平移距离)

第2)步中保存了A,B的所有对应点对，下面是计算使这些点对的距离和最小的变化矩阵(即旋转某个角度和平移某个距离后，点对和距离最小)。设R为变化矩阵，对应点对距离函数为D(R,t)，所有对应点距离和如下式(3.10)

上式(3.10)打开后为下式(3.11)

上式(3.11)简化后即下式(3.12)

其中

表示点云A中第i点的x坐标。旋转弧度计算如下式(3-13)

其中

表示点云A中第i点的x坐标。平移向量t即A的中心点坐标与B的中心点坐标之差。上面的B是经过旋转后的，因为旋转后B点云距离A距离应该更小。

4)通过变化矩阵变化点云

将原来的点云A,B(均一化之前)通过变化矩阵R变换即下式(3.14)

B′＝R·B+t (0-14)

R为变化矩阵，t为平移向量。实际上就是将B点云中所有点旋转θ，在平移向量t。此时，一次ICP操作已经完成。

5)计算损失

将变化后的点云B'与B进行比较，计算两个点云对应点对(最近点)的距离之和的差，Loss计算如下式(3.15)。

A_i-B′_i表示两个点云对应点之间的距离。

6)是否继续迭代

一般来说ICP的迭代条件有两种：①损失小于预设的损失阈值②迭代次数大于预设的迭代次数。经过实地测试，损失阈值并不好测试，也无法比较好的设置一个损失函数，所以本发明只采用迭代次数作为判定是否继续迭代的条件。经过实地测试，简单室内环境下，30次左右点对距离之和已经不在变化，所以将迭代次数设为50次左右。

Trimmed ICP原理如下：对于点数量不同的源点云A，目标点云B，A中的一些点可能在目标点云B中没有对应关系。假设可以匹配的对应点数占所有点数的最小比例为σ，所有对应点集合为N_P，可以匹配的对应点对为N_po＝σN_P，当σ＝1的时候，Trimmed ICP等于ICP。基本步骤如ICP，只在第3)步计算对应点对的距离和D(R,t)时，将各个点对距离根据式(3.9)计算的{a_i,b_j}升序排序，选取前N_po个点计算D(R,t)。σ的选取与3.3.3节中粗配准的重叠率相同。

3.4.4配准效果判断

在进行点云配准后，源点云需要根据3.3.2节的配准参数中dist判断是否与目标点云配准。由于错误的配准会导致点云距离较大，此时限制最近点距离阈值来选取对应点会导致错误的点对被剔除，从而得到较小的点云平均距离。这会导致dist不好设置，dist过小，即使正确的配准由于部分没有对应关系的点的影响而导致两个点云平均距离大于dist而判断配准失败；dist过大，则会导致原本配准失败的点云也会判断配准正确。所以本系统将最近点距离阈值设置一个较大的值，并且使用Trimmed ICP的思想，选取N_po＝σN_P的目标点云与源点云计算平均距离，即采用配准后点云对应的最近点距离和dist来判断是否配准和配准得分。评定指标dist计算如下式(3.16)：

p_i和q_i代表两点云对应点之间的距离。

3.5点云可视化模块6设计

功能：如何根据点云配准的结果来将多个点云进行融合，并且可视化展示点云图。

3.5.1点云融合

由于激光雷达扫描距离和遮挡等因素，单个激光雷达扫描的图像是不够完整的，需要将多个激光雷达的数据融合在一起，形成一个比较完整的室内环境点云图。根据点云配准得到的变换矩阵R，通过式(3-14)将源点云转换到与目标点云相同的坐标系上，此时由于源点云与目标点云有一部分数据是重合的，导致点云图的密度不均匀，不符合实际情况，所以采用均匀采样的方法，对点云进行处理，得到最后密度比较均匀符合实际情况的室内环境融合点云。点云融合后，使用融合点云与之后其他激光雷达扫描的点云进行配准，这样可以提高点云配准的效果和精度。

3.5.2可视化展示

本模块使用自动化脚本启动ROS多机通讯，主机收到数据后自动计算配准，根据配准结果将成功配准的激光雷达加入雷达网络，最后显示雷达网络的融合点云图(将源点云通过配准得到的矩阵R平移旋转到与目标点云相同的坐标系)。具体业务逻辑如下：主要分为两部分展示，一是单次点云配准后的展示，分为源点云，目标点云以及配准后点云。二是所有点云配准完毕后，显示雷达网络的融合点云。第一部分将三个点云放在一个viewer中展示，分别为三个点云赋予不同颜色。第二部分则分为三个viewer展示，一个viewer展示所有激光雷达的原始点云图，用不同颜色表示；第二个viewer展示点云融合的图像，为不同点云给与不同颜色，用来表示配准的效果；第三个viewer展示室内环境标准点云，统一用红色点云，用来表示室内的大概环境。

3.6自主建网模块7设计

功能：如何根据点云配准的结果来自动组建雷达网络。

首先需要将各个树莓派的主机名与DHCP分发的动态IP地址动态绑定以及更新ROS主从机设置。步骤如下：

(1)各个机器开机自动连接WIFI，路由器或者网关树莓派会自动分配IP地址。然后该机器自动以ROOT用户身份运行脚本从云端下载hosts.txt文件，如果文件包括该机器的主机名和IP地址，则将云端中本机的IP地址更新为本地的最新IP地址。否则在hosts.txt末尾加入本机的主机名和IP地址。并将文件上传到云。云端始终维持最新的hosts.txt文件。

(2)将该网段下所有树莓派上传的对应的IP地址和主机名写入各主机系统的/etc/hosts文件。之后再修改ROS的主从机设置。将获取的主机IP写入/etc/profile的末尾，如果已经存在ROS_MASTER_URI则更新，否则写入获取的主机IP。

(3)将本机的IP地址写入/etc/profile的末尾，如果存在ROS_HOSTNAME则更新，否则写入本机IP。然后注册系统文件，应用设置。

然后主机使用自动化脚本启动ROS多机通讯，主机收到数据后自动计算配准，根据配准结果将成功配准的激光雷达加入雷达网络，最后显示雷达网络的融合点云图(将源点云通过配准得到的矩阵R平移旋转到与目标点云相同的坐标系)。具体业务逻辑如下：

1)主机从云端下载hosts.txt，获取当前网段内能够正常连接的树莓派的IP地址和主机名。

2)雷达网络内各雷达扫描当前环境获得点云数据。

3)判断是否第一次开机，若是则进行4)，否则进行5)。

4)依次使用点云配准算法将各从机点云数据与主机点云数据进行比对，如果两点云平均距离大于dist则移除雷达网络。此时，主机所在室内环境的雷达网络建立成功。同时建立第一张雷达网络点云标准图。

5)使用点云配准算法将主机点云与雷达网络中标准图配准，如果配准，进行6)；否则主机处于新的室内环境，进行7)。

6)依次使用点云配准算法将各从机点云数据与雷达网络中点云标准图进行比对，两点云平均距离大于dist则移除雷达网络。配准后获得更新之后的雷达网络点云标准图，进行8)。

7)依次使用点云配准算法将各从机点云数据与主机点云数据进行比对，如果两点云平均距离大于dist则移除雷达网络。此时，主机所在室内环境的雷达网络建立成功。同时建立新的雷达网络点云标准图。

8)每间隔2小时，重复上述步骤更新雷达网络点云标准图。

自主建网流程图，如图5所示。

本发明激光雷达的数据处理过程，获取到雷达数据之后，分别经过数据处理，自适应选择配准参数，粗配准，精配准以及自主建网。

4.下面结合系统实现对本发明的技术方案作进一步描述。

4.1激光雷达模块的实现

如何实现激光雷达模块。包括：

首先安装在树莓派4B上安装ubuntu mate 20.04LTS版本，并安装对应的ROSnoetic版本。安装Delta-1A型激光雷达的驱动，赋予ttyUSB0所有操作权限。配置ROS系统，建立catkin工作空间catkin_ws,配置相应的packuage.xml,编写rostopic,topic名为delta_lidar,编写发送者节点delta_lidar_node，接收者节点build_map.py，然后在CmakeList.txt文件加入上述自定义消息和节点。最后进行程序注册和编译，此时运行节点可以成功传输消息。激光雷达扫描的数据比较繁杂，对于本系统有用的数据只有角度，距离和时间戳。所以在发送者节点中只发送上述有用数据。

为了保证主机能够同时接收从机的点云数据，本系统采用自动化脚本使用ssh连接各从机控制ros节点的启动和结束时间。

4.2点云数据处理的实现

对点云数据初步处理的具体实现方法。主要包括对静点的提取，离群点的去除。

4.2.1静点提取实现

激光雷达每次扫描地图约2分钟，500帧数据，大概30000-40000个点。提取静点的方法如下：遍历500帧数据，若某点出现次数大于n，则认为该点为静点，否则认为其是动点删除。遍历结束后删除点云中重复的点。经过实际测试，发现n的值为40的时候能够比较好的提取静点。经过提取后，保留的无重复的静点个数大约为2000-4000个点，能够完成点云配准。

4.2.2离群点去除的实现

本系统离群点的去除采用统计滤波(Statistical Outlier Removal)的方法。安装PCL1.10和其他依赖包。PCL(Point Cloud Library)中包括了本系统使用的统计滤波。使用pcl::StatisticalOutlierRemoval<pcl::PointXYZ>统计滤波对象，初步设置meanK为2，std为1。当判断点的k近邻平均距离(mean distance)大于全局的1倍标准差+平均距离(global distances mean and standard)，则为离群点。由于源点云A与目标点云B的点数量不一定相同，且经过统计滤波之后的点数量也不能确定，所以为了确保点云配准时两个点云具有相同数量的点，同时对两个点云进行统计滤波后再随机采样1000个点。经过测试500帧数据经过统计滤波后点数量大于1000，所以可行。统计滤波的参数还需要根据实际环境来调节。

4.3自适应配准模块的实现

自适应配准模块由/root/pcltest1和pcltest2实现。首先从云端获取配准参数由/root/cloud11.py实现。该文件会根据传入的参数自动上传点云数据，同时会自动下载配准参数文件config.txt到/root文件夹中。

pcltest1首先会根据shell脚本传入的参数通过main函数的argv指针给全局变量两个点云名filename和filename2赋值，然后读取云端下载的配准参数文件。随后用函数calarea()根据点云数据计算室内环境大致面积，calarea会计算出点云边缘四点的坐标，然后将四边形分割为两个三角形，通过函数calculateTwoPointArea计算三角形的面积，返回两个三角形面积的和，即大致面积。根据面积的大小，模块会调整配对点对之间最小距离和判断是否配准的平均距离的大小，如果面积较大则会等比例增大两个参数，反之，则等比例减小。调整完参数后，会调用离群点去除的方法，去除掉全局离群点，获得两个具有相同数量点的2D点云。之后开始进行4PCS粗配准，从预设的最小重叠率开始，进行配准，同时通过mydist()计算两点云之间平均距离，然后将重叠率增加0.1，继续计算重复三次。选取之间点云平均距离最小的一次配准，保留matrix_tras变化矩阵。使用pcl::transformPointCloud()将源点云通过变化矩阵平移旋转到目标点云上，将配准成功的点云进行Trimmed ICP精配准，以获得更好的融合效果。然后显示点云图像。将精配准后的点云间平均距离与之前设置的配准参数dist进行比较，如果小于dist，则认为配准成功，加入雷达网络；否则配准失败。

4.4自适应配准模块的难点

本系统最后是在真实环境中测试，受环境和硬件设备的影响，遇到的困难也相当多。主要有以下几点：

一是激光雷达的精度，扫描距离，每秒扫描点数不够，直接影响了配准的效果。实际上，不同的雷达放在几乎相同的位置上，看到的点云图像有20-30cm的差距，会出现部分配准后，另一部分没有配准。原因是因为激光雷达的扫描误差和个体差异。这个问题无法解决，只能选取测量比较好的数据来配准。

二是环境因素非常复杂，激光雷达由于其特性，受到遮挡的影响也非常大，如果激光雷达放置位置不好，则图像会与其他雷达点云图像相差过大。而点云配准算法在处理重叠率过低的点云时都会出现配准失败的情况。所以在放置激光雷达的时候需要放置在遮挡较少，且能够扫描到室内大部分环境的地方。

4.5自主建网的实现

首先各个树莓派开机后自动以ROOT用户启动脚本，从云端下载hosts.txt,更新各自的hosts文件和/etc/profile文件,然后上传最新的hosts.txt至云端。更新完毕后，为了展示运行过程和结果，主机额外需要运行一个终端并在终端中运行脚本。且由于文件在root文件夹下，需要输入密码，建立密码文件_PWD_TEMP_，用bash编译，然后使用shell实现自动输入密码。这里使用管道的方法echo password|sudo-S apt-get update。将运行脚本任务加入ubuntu启动项。运行脚本mytest.sh，脚本会根据是否存在lidar.pcd(室内环境融合点云)判断是否首次开机，若为首次开机，主机会从云端下载最新的hosts.txt获取当前网段下能够正常通讯的主机名和IP，将能够连接的机器加入雷达网络，然后使用pcltest1对主机和从机点云进行点云配准，将不能配准的雷达从雷达网络中删除，将能够配准的点云融合，使用融合点云继续从机点云配准。当全部配准完毕后，主机运行pcltest2显示雷达网络各主机的hostname以及室内环境融合点云。然后每隔1小时，主机会启动脚本自动更新雷达网络和融合点云。

5.下面结合系统测试与验证对本发明的技术方案作进一步描述。

5.1总体系统测试

对整个系统进行的测试，验证系统的部署是否能达到预期的结果。系统测试结果如表5所示。

表5系统测试表

5.2自适应配准模块测试

如图6所示，为1号雷达点云与室内环境标准点云配对效果图。

如图7所示，为2号雷达点云与1号雷达点云配对效果图。

如图8所示，为4号雷达点云与1号雷达点云配对效果图。

如图9所示，为1,2,4号雷达点云融合效果图。

5.3自主建网测试

首先需要测试是否根据DHCP服务器分配的动态IP更新了hosts和/etc/profile文件，云端是否为最新的hosts文件。通过本发明云端hosts文件自动更新为最新的主机名和对应的IP地址。本地自动更新了对应的主机名和IP。本地自动更新了ROS主从机设置。

配置信息自动更新测试完毕后主要分为四种情况来测试。第一种：1号机先开机，其余2,4号机后开机，放在同一室内环境下；第二种：首次建立雷达网络后，将1号机和2号机移动位置，但仍在同一个室内；第三种：第一次雷达网络更新后，将4号机从该室内环境移动到另一个环境中。前三种都为实验室环境下测试，实验室环境比较空旷，且面积较大，与老人实际居住的室内环境还是有一定差距。第四种情况为1,4号机器在老人家里自主建网测试，建立雷达网络。

5.3.1实验室环境测试

分为三种情况测试，一是三个雷达在同一室内；一是移动其中两个雷达；最后是将一个雷达放置到室外。

(1)三个雷达放在同一室内

首先将1号机开机，然后依次将2,4号开机。打开VNC连接1号机，可以看到1号机开机后自动打开终端然后启动脚本开始建立雷达网络。然后开始1号机与2号机开始点云配准，结果为：在重叠率0.7下点云平均距离最小，成功配准。

配准后融合点云用不同颜色代表不同点云，比较好展示配准效果。室内环境融合点云，展示室内环境全部配准完毕后，主机会显示雷达网络的主机名hostname。

(2)移动两个雷达位置

将1,2号激光雷达移动位置，但仍在一个室内。等待一段时间，主机自动更新雷达网络。看到点云融合的图像基本没有变化，说明室内环境没有变动，且激光雷达都在一个室内。所以雷达网络的各个雷达也不会变动。

(3)将一个雷达放在室外

这一次将4号机移动到另一室内环境中，等待主机自动更新雷达网络。1号机与4号机配准失败。

由于4号机配准失败所以雷达网络内的雷达只有1,2，融合点云也只会展示1,2的点云。

现在的雷达网络只有shu4b和shu4b2,4号机已经从雷达网络中删除。

5.3.4实际老人居家环境测试

这一次在一位独居老人的房间内进行测试。实际情况为两个激光雷达放置在老人家的客厅中，一个激光雷达放在墙角的桌子旁，另一个激光雷达放在沙发底下。

激光雷达扫描的点云如图10，白色为1号机雷达扫描点云，灰色为2号机雷达扫描点云，其中1号机扫描点云多出来那一部分为老人家的客厅走廊。图11展示了1,2号机点云配准的结果，1,2成功配准，说明两个激光雷达在同一室内。

实验表明：

本发明主要是针对居家养老项目设计的基于自适应配准的多激光雷达自主建网系统。本文从设计实现方面全方位的介绍了本发明的实施方案。通过本系统，使用者可以远程查看老人居住室内的激光雷达网络情况以及室内环境点云图像。通过雷达网络和环境点云数据，使用者可以用在居家养老项目的进一步设计和使用中。

本发明的创新性主要有以下方面：

由于激光雷达的硬件限制，配准精度并不是很高。如果更换每秒扫描点数更多，扫描精度更高的激光雷达，可以做到更好的配准效果。

多激光雷达自主建网对于雷达摆放的位置要求较高，如果放置位置不好，可能会导致判断失误，建网失败。如果采用相机加激光雷达的形式，可能会达到更好效果。

应当理解的是，本公开并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。本公开的范围应由所附的权利要求来限制。

Claims

1.一种基于自适应配准的多激光雷达自主建网方法，其特征在于，所述基于自适应配准的多激光雷达自主建网方法包括：

采用多个激光雷达利用点云配准算法对不同雷达的点云图进行配准和融合，所述对点云图进行配准点云配准采粗配准和精配准，使同一室内环境的激光雷达自动组建雷达网络，并获得融合后的室内环境点云；

对获得的融合后的室内环境点云数据同时保存本地和云端，用户通过云端更改相应配准参数或者获取原始点云数据。

2.根据权利要求1所述的基于自适应配准的多激光雷达自主建网方法，其特征在于，所述对不同雷达的点云图进行配准前，树莓派主机根据数据处理后的点云计算室内环境的大致面积来修改配准参数delta，overlap，dist；首先找到2D点云中x,y均为最大的点A(x1,y1)，x最大，y最小的点B(x2,y2),x最小，y最大的点C(x3,y3)以及x最小y最小的点D(x4,y4)；根据这四点来求点云的大致面积(单位为m²)：

根据Area的大小来修改delta，overlap和dist；

具体参数设置：

若Area小于10m²，则认为为小房间，delta设置为0.02m，overlap设置为0.65，dist设置为0.15；若Area小于20m²，则认为为中等房间，delta设置为0.03m，overlap设置为0.6，dist设置为0.25；若Area小于20m²，则认为为大房间，delta设置为0.05m，overlap设置为0.5，dist设置为0.4；如果大于30m²，则超出激光雷达的限制；如果根据实际情况发现某组配准参数更好，修改参数并上传云；配准前从云端下载最新的配准参数。

3.根据权利要求1所述的基于自适应配准的多激光雷达自主建网方法，其特征在于，所述多个激光雷达利用点云配准算法包括：将两个激光雷达扫描点云通过平移，旋转最终变化到相同的坐标系下，实现最大的重叠；所述点云配准包括：基于4PCS的点云粗配准、基于TRICP的点云精配准；

所述粗配准为：在点云的位置无法确定且无人工标定的情况下，将两个点云初步配准，为精配准提供好的初始位置；

进行粗配准后，目标点云与源点云已经处于一个好的初始位置上；使用ICP的改进算法Trimmed ICP进行精配准；

在进行点云配准后，源点云需要根据配准参数中dist判断是否与目标点云配准。

4.根据权利要求3所述的基于自适应配准的多激光雷达自主建网方法，其特征在于，所述基于4PCS的点云粗配准包括：

在目标点云P中寻找满足要求的长基线的共面四点基；选择一个共面四点基B＝{a,b,c,d},提取B的拓扑信息；根据点云旋转和平移变化中r1,r2的仿射不变性和公式

计算共面四点基B间的两个比例因子r1,r2,再根据式

计算q1,q2∈Q(源点云)的的交点位置集合，来计算Q中所有长基线点对交点坐标，比较交点坐标确定匹配集合，ei≈ej表示寻找到对应的一致全等四点，i,j分别表示第i个和第j个Q中长基线点对；B＝{a,b,c,d}对应的一致全等四点表示为C＝{q1,q3,q4,q5}；将点云所有计算出P的共面四点集合用E＝{B1,B2,...,Bm}表示，其中用m表示P集合的数量，重复，得到全等四点集合D＝{C1,C2,...,Cn}，n为全等四点集合总数；最后在D＝{C1,C2,...,Cn}中寻找最优全等四点对，4PCS使用LCP策略寻找最优全等四点匹配，即计算全等四点旋转和平移变化参数，将四点转化应用到全局点云转化，记录全局配准中包含最大的一致区域匹配记为最优匹配，至此完成4PCS算法局部粗配准；

其中e为共面不共线四点a,b,c,d的交点；r₁和r₂表示比例，q₁和q₂表示目标点云Q中的任一点对，e₁和e₂表示对应点对可能的交点；若e₁和e₂的差在允许范围内，则认为是a,b,c,d的对应共面点。

5.根据权利要求3所述的基于自适应配准的多激光雷达自主建网方法，其特征在于，所述ICP的改进算法Trimmed ICP包括：

(1)点云归一化：为点云数据选择一个标准且有效的笛卡尔坐标系，在配准前预先消除坐标变换带来的影响；

根据式

取点云的中心；

其中A_i表示A点云第i点的坐标；接下来将每个点都减去这个平均值A_center，即将点云平移，使该点云的形心移动到原点上；

(2)寻找最近点：寻找A,B两个点云集的对应点；即B点云在经过理论完全正确的平移旋转后应该和A点云是重合的，各个重合的点对就是对应点；计算距离最近的点即为对应点；通过式

计算A,B点云空间距离最近的点；其中

表示点云A中第i点的x坐标；

(3)计算变化矩阵：设R为变化矩阵，对应点对距离函数为D(R,t)，所有对应点距离和如式

打开后

简化后即

其中

表示点云A中第i点的x坐标；旋转弧度计算如式

其中

表示点云A中第i点的x坐标；平移向量t即A的中心点坐标与B的中心点坐标之差；上面的B是经过旋转后的，因为旋转后B点云距离A距离应该更小；

(4)通过变化矩阵变化点云：将原来的点云A,B通过变化矩阵R变换即下式B′＝R·B+t；R为变化矩阵，t为平移向量；实际上就是将B点云中所有点旋转θ，在平移向量t；此时，一次ICP操作已经完成；

(5)计算损失

将变化后的点云B'与B进行比较，计算两个点云对应点对的距离之和的差，Loss计算如式A

A_i-B′_i表示两个点云对应点之间的距离；

(6)是否继续迭代：采用迭代次数作为判定是否继续迭代的条件；具体包括：对于点数量不同的源点云A，目标点云B，A中的一些点可能在目标点云B中没有对应关系；假设可以匹配的对应点数占所有点数的最小比例为σ，所有对应点集合为N_P，匹配的对应点对为N_po＝σN_P，当σ＝1时，Trimmed ICP等于ICP。

6.根据权利要求3所述的基于自适应配准的多激光雷达自主建网方法，其特征在于，采用配准后点云对应的最近点距离和dist来判断是否配准和配准得分；评定指标dist计算如下式：

p_i和q_i代表两点云对应点之间的距离。

7.根据权利要求1所述的基于自适应配准的多激光雷达自主建网方法，其特征在于，所述对不同雷达的点云图进行配准和融合前包括：

激光雷达数据的采集与数据处理；

所述对不同雷达的点云图进行配准和融合后包括：

自主建网，在点云配准的基础上，将能够配准成功的激光雷达加入雷达网络，组建雷达网络；在点云配准前，各机器自动更新局域网内各机器IP和主机名以及ROS系统配准。

8.一种实施权利要求1～7任意一项所述基于自适应配准的多激光雷达自主建网方法的基于自适应配准的多激光雷达自主建网系统，其特征在于，所述基于自适应配准的多激光雷达自主建网系统包括：

自主建网模块，用于自动化建网；

9.一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行权利要求1～7任意一项所述基于自适应配准的多激光雷达自主建网方法。

10.一种信息数据处理终端，其特征在于，所述信息数据处理终端包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行权利要求1～7任意一项所述基于自适应配准的多激光雷达自主建网方法。