CN114185059A - 基于多雷达融合的多人跟踪***、方法、介质、终端 - Google Patents

Abstract

本发明公开了基于多雷达融合的多人跟踪***、方法、介质、终端，涉及雷达测量技术领域。主机接收来自不同雷达的多组数据，将不同雷达的数据进行对齐，同步到同一坐标轴上；对不同雷达数据同步时先将时间进行同步，使同步的数据为同一时间不同雷达采集的数据；对每个激光雷达数据中的动点数据提取出来；使用卡尔曼滤波器跟踪并计算目标的位置；数据处理完毕后即时的将数据存储到文件，并即时的上传到云。本发明是一个即时性的***，很多同类发明使用的都是机器学习算法来进行目标识别和跟踪，但是此类算法占用资源多，计算时间相当长，不满足即时性的需要，而本***可以即时的根据激光雷达反馈结果，而且延迟在1s之内。

Description

基于多雷达融合的多人跟踪***、方法、介质、终端

技术领域

本发明属于雷达测量技术领域，尤其涉及一种基于多雷达融合的多人跟踪***、方法、介质、终端。

背景技术

目前，随着我国进入老龄化社会，老龄人口的安全防护是社会保障非常重要的一个环节。其中独居老人的防护为我国老龄人口工作的重点。当今人们处于工作家庭等各种各样的原因，子女很少回家更难以及时照顾父母已经成为一个普遍现象，加上很多老人的安全意识较差，经济条件也难以负担人工看护，需要一个全新的低成本的持续的看护手段保障老人的独居健康和安全。

现阶段智能家居养老方面，对人的行走的监控与分析是非常重要的一部分。在此，现有多种途径完成此部分，比如计算机视觉，通过计算机分析实际图像可以得到众多信息，包括人的位置、行走状态、姿势、运动轨迹等。现有很多关于图像分析的发明。但是处于成本方面考虑，计算机视觉计算成本非常高，算法非常复杂。

通过激光雷达进行分析有着独特的优势，激光雷达不同于光学传感器，激光雷达获取的是一维线性的点云数据，比计算机视觉需要处理的二维图像数据量大大减少。而且激光雷达获取信息受干扰更小，脏数据更少，更加适用于复杂环境的简单分析。目前激光雷达的运用范围非常广泛，在生产、生活、军事都有着运用。

随着硬件传感器的不断发展，多传感器冗余已经成为提高精度与功能的重要手段，本发明旨在使用多激光雷达组成网络共同分析。单激光雷达存在很多问题，激光雷达探测的是一个平面，所以遮挡对于探测影响非常巨大，单激光雷达并不能对于室内环境探测而形成一个完整的地图，多激光雷达则可以通过不同雷达对于同一物体的观察而得到更精确的数据。

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

(1)目前大多数智能养老的往往采用的摄像头等光学传感器进行分析，其中存在很多问题。光学传感器图像复杂，分析困难，往往难于适用于复杂环境，而且成本较激光雷达大，成本包括三个方面，传感器、分析算法、计算单元，各方面都大于激光雷达。在许多现有技术中，光学传感器所组成的智能看护***对于家中实际情况表现较差，对于大量的不可控因素，如光照和遮挡等适应性不佳。光学传感器还存在一个隐私问题，如何保证图像数据不被泄露也是一个需要关注的重点。激光雷达的工作原理是向需要探测的方向发射扫描激光，激光碰到可以反射的物体表面后反射，雷达接受到激光反射后就可以测量到目标点的距离，形成一个距离和弧度的线性数据。激光雷达对光照等条件有着良好的适应性。现有技术采用成本较低数据量较小的激光雷达还没进行实际应用。

(2)现有技术中，获得的数据精度低。数据处理繁琐，数据传输量大，计算量大，成本较高。

解决以上问题及缺陷的意义为：

本发明面向健康养老，旨在无人看护时能够实时监控老人的健康水平，通过监控行走以及长时间跟踪的方式分析人的身体状况。本发明采用多雷达共同分析的方式，可以得到同一时刻不同雷达的信息，激光雷达对于越远的物体分辨率越低，而过于接近的物体则遮挡严重，比如人在行走的过程中很多时候都会出现一只脚遮挡另一种脚的情况。因此激光雷达探测的最佳距离存在一个距离区间。采用多个雷达形成雷达网络则可获得大量数据冗余提高精度。

本发明采用自主建立的多激光雷达网络，关于多传感器***的建立主要有两种方式，集中式和分布式。集中式指将所有传感器信息汇总到一个计算设备上，统一进行分析，优点是数据处理简单结果精确，缺点是数据传输量大，计算量大，成本较高。分布式指每个传感器独立计算单元，分别进行计算，最后统一得出最优解，优点是计算量较小，缺点是不够精确。本发明拟采用混合式，将两种网络建立方法组合一起，由各个设备进行初步计算后汇总到某一中心完成计算。拟采用ROS主从机***完成多激光雷达网络建立。

本发明旨在获取人在室内的行动轨迹与特征，通过对轨迹和特征时间和空间上分析可以得到老人长期的健康状况，也可获得即时的情况来监控老人的活动状况，如有无跌倒。

发明内容

为克服相关技术中存在的问题，本发明公开实施例提供了一种基于多雷达融合的多人跟踪***、方法、终端、介质、设备。所述技术方案如下：

根据本发明公开实施例的第一方面，提供一种基于多雷达融合的多人跟踪方法，包括：

步骤一，使不同树莓派与多激光雷达通信，并确立处理数据的主机；

步骤二，所述主机接收来自不同雷达的多组数据，将不同雷达的数据进行配准对齐，同步到同一坐标轴上；对不同雷达数据同步时先将时间进行同步，使同步的数据为同一时间不同雷达采集的数据；

步骤三，对每个激光雷达数据中的动点数据提取出来，处理后传输出去；

步骤四，主机对接收的步骤二的数据，进行处理，最终得到行人的脚步并且进行持续跟踪；对接收的步骤三的动点数据，使用聚类方法，使点集形成簇，计算簇几何中心，进行下一步的跟踪；再使用卡尔曼滤波器跟踪并计算目标的位置；

步骤五，数据处理完毕后即时的将数据存储到文件，并即时的上传到云。

在本发明一实施例中，所述步骤一不同树莓派组成处理数据的主机包括：

基于ROS***的程序，对于各个树莓派从机修改配置文件，并标记为从机，存储主机的IP地址，主机存储所有从机的IP地址并标记自己为主机，所有树莓派在连接WIFI时将使用固定IP；或先使用局域网广播的形式进行初步确认，然后将动态IP存入文件再进行下一步连接；

当***运行时，主机运行一个脚本文件，脚本文件中写入通过ssh协议连接从机的部分，然后通过ssh协议传输指令启动相应ROS节点，ROS节点通过接收数据和发送数据，完成各个树莓派的连接。

在本发明一实施例中，所述步骤二主机接收来自不同雷达的多组数据，将不同雷达的数据进行对齐，同步到同一坐标轴上包括：

首先建立一个ROS节点用于运行雷达驱动，每当雷达驱动返回数据时，建立一个发布者ROS线程将源数据广播至ROS***里；

然后建立一个ROS节点用于接收源数据并对数据预处理

再对每一帧接收的数据进行极坐标转直角坐标，与地图文件进行比对；

在动点判断时，将接收数据和地图数据精度全部放大至5cm，计算过程如下公式：

提取接收数据中不在地图数据中的点，并广播在ROS***下。

在本发明一实施例中，所述步骤二对不同雷达数据同步时先将时间进行同步，使同步的数据为同一时间不同雷达采集的数据包括：

在同一ROS节点同时接收来自所有树莓派的动点数据，建立一个队列数组用于存储数据，该队列数组大小为树莓派数量，当接收者接收到数据时，按照数据包含主机名首先按照提前存储的旋转角和平移量将空间坐标变换然后将存储在对应位置的队列中，空间坐标变化如下公式：

p′＝Hp+t；

p′是变化后点坐标向量，p为变化前点坐标向量，t为平移量，H为转换矩阵其值为

θ为旋转角；建立线程用于完成时间同步，将各个队列出队一个数据包，将从机时间属性分别如主机时间属性相减，若主机时间快则从机队列出队相应数量数据包，若从机时间快则主机队列出队相应数量数据包，若差距绝对值小于阈值则将该从机队列对应标志位置为真，表示主机已和该从机对齐；对齐后将两个雷达对应时间存储在文件中；帧数丢失在可接受范围内，自此完成时间同步。

在本发明一实施例中，所述步骤四使用卡尔曼滤波器跟踪并计算目标的位置包括：

卡尔曼滤波器模型的核心方程如下公式：

X_k＝A_kX_k-1+B_ku_k+w_k；

z_k＝H_kX_k+v_k；

式中X_k＝[x_k，y_k，x′_k，y′_k]是第k帧的实际人脚状态向量；

z_k＝[x_k，y_k]是第k帧的激光雷达得到人脚位置；

A_k是状态转移矩阵；

B_k是控制输入矩阵，将运动测量值映射到状态向量上；

u_k是第k帧的***控制向量，在此模型中忽略；

w_k是过程噪声；

H_k是转换矩阵，将状态向量转换成测量向量的格式；

v_k是观测噪声；

X_k＝A_kX_k-1+B_ku_k；

X′_k＝X_k+K(z_k-H_kX_k)；

P′_k＝P_k-KH_kP_k；

式中X′_k是第k帧的***修正后人脚状态向量；

P_k是第k帧估测状态的协方差矩阵；

P′_k是第k帧***修正后人脚状态的协方差矩阵；

K是卡尔曼增量。

根据本发明公开实施例的第二方面，提供一种基于多雷达融合的多人跟踪***包括：

雷达网络模块：采用多雷达建立雷达网络的结构，用于对不同雷达的调度进行分配；

雷达数据处理预处理模块：用于多雷达***存在多组数据，对数据进行降噪筛选；

多雷达数据配准融合模块：用于对于同一场景有不定数量的雷达图像，进行配准融合，还用于将同一场景的两幅或多幅图像进行配准以使图像对在一个公共空间轴上对齐；

目标分析模块：用于对于点云数据进行分析，分离出疑似行人的部分，并进行跟踪；

结果展示模块：用于将行走数据以表格和图像的形式打印输出。

在本发明一实施例中，所述基于多雷达融合的多人跟踪***进一步包括：

激光雷达直接连接树莓派USB接口，树莓派都运行在同一网段下，主机树莓派启动ROSCORE进程，并通过连接模块连接若干从机树莓派，所有装有雷达的树莓派运行读取雷达数据的ROS进程和处理并发送数据的ROS进程，主机树莓派运行接收和分析处理后数据的ROS进程；运行结果上传至云。

根据本发明公开实施例的第三方面，提供一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行本发明的方法。

根据本发明公开实施例的第四方面，提供一种信息数据处理终端，所述信息数据处理终端包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行本发明的方法。

根据本发明公开实施例的第五方面，提供一种多激光雷达网络设备，所述多激光雷达网络设备采用ROS主从机***，将集中式和分布式网络建立组合一起，由各个设备进行初步计算后汇总到某一中心完成计算。

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

本发明在分析到现阶段很多老人并没有经济实力的现实情况下，利用低成本的2D激光雷达和微型计算机树莓派构建了一个多传感器的目标识别跟踪***，该***可以在部署后长时间的利用多激光雷达数据检测室内行人的脚步。该***利用ROS***将多个树莓派和雷达连接成一个网络，利用其他雷达的数据对主雷达进行数据补充，能在不大幅度提高成本的前提下提高精度和解决部分单传感器下的遮挡问题，扩大检测范围。该***核心算法是卡尔曼滤波器和基于区域生长的聚类算法，通过这两个算法完成目标识别和跟踪部分。该***是一个即时性的，可离线运行的，通过局域网进行互联的室内目标识别跟踪***，在保证低成本的前提下，实现对人脚一定准确度的实时跟踪，可以满足现代智能养老的基本需求。

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

本发明主要是完成一个通过激光雷达网络识别追踪室内行人的***。以软件发明文档的方式说明了软件从发明到实现的全过程并给出部分结果展示。通过本发明，使用者可以实时的获取室内行人脚步跟踪的结果，通过这些数据可以得到更多的结论。

本发明使用2D激光雷达作为传感器，激光雷达受环境影响小，使用功率小，无噪音以及其他污染，而且不受光线影响，可24小时运行。而本***所采用的激光雷达比起其他工业级激光雷达成更低，整套***没有使用昂贵的设备，符合现实居家养老的实际需要，老龄化日益严重的社会需要成本低，智能化程度高的保障体系来负担严重缺人力和经济的养老产业。

本发明使用多传感器观测的结构，多传感器可以在不增加很大成本的条件下有效提高结果准确率，比起传统单激光雷达进行的目标识别跟踪，多激光雷达不受单方向遮挡障碍物的的影响，可以持续性的对室内行人进行追踪。多传感器还会提高***的健壮性，当有一个雷达或一个树莓派故障时，只要不是主机树莓派故障，***任然可以继续保持一定精准度继续保持功能。

本发明是一个即时性的***，很多同类发明使用的都是机器学习算法来进行目标识别和跟踪，但是此类算法占用资源多，计算时间相当长，不满足即时性的需要，而本***可以即时的根据激光雷达反馈结果，而且延迟在1s之内。

本发明可以完全离线，只在局域网下运行，虽然本***可以将数据文件上传至云，但是所有运算工作都是在树莓派本地完成，所以本发明没有使用复杂的算法，以简单的算法完成一定准度的目标识别跟踪，如果存在部分功能在云上完成，虽然提高了计算速度，如果碰到网络波动或者不适宜连接互联网的情况将直接失去功能，而本***可以完全离线完成。

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

附图说明

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

图1是本发明实施例提供的基于多雷达融合的多人跟踪***示意图。

图中：1、雷达网络模块；2、雷达数据处理预处理模块；3、多雷达数据配准融合模块；4、目标分析模块；5、结果展示模块。

图2是本发明实施例提供的基于多雷达融合的多人跟踪***原理图。

图3本发明实施例提供的基于多雷达融合的多人跟踪***运行的总体流程图。

图4本发明实施例提供的基于多雷达融合的多人跟踪***架构图.

图5是本发明实施例提供的主雷达被遮挡图。

图6是本发明实施例提供的同步后动点图。

图7是本发明实施例提供的动点文件通过程序展示图。

图8是本发明实施例提供的脚步跟踪结果通过程序展示图。

具体实施方式

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

1.下面结合***及方法对本发明技术方案作详细描述。

如图1所示，本发明实施例提供的基于多雷达融合的多人跟踪***，目的是通过激光雷达网络对室内行人脚步实现跟踪。需要对雷达网络中不同雷达的数据进行处理，通过多个雷达的点云数据，将人从点云数据中分离出来并完成跟踪。包括：

雷达网络模块1：采用多雷达建立雷达网络的结构，用于对不同雷达的调度进行分配。

雷达数据处理预处理模块2：用于多雷达***存在多组数据，对数据进行降噪筛选。

多雷达数据配准融合模块3：图像配准是图像处理的基本任务,即将同一场景的两幅或多幅图像进行配准以保证图像对在一个公共空间轴上对齐。采用自主建网的多雷达***，用于对于同一场景有不定数量的雷达图像，需要进行配准融合。

目标分析模块4：用于对于点云数据进行分析，分离出疑似行人的部分，并对其进行跟踪。

结果展示模块5：用于将行走数据以表格和图像的形式打印输出。

根据本发明公开实施例提供一种基于多雷达融合的多人跟踪方法，包括：

步骤一，使不同树莓派与多激光雷达通信，并确立处理数据的主机；

步骤二，所述主机接收来自不同雷达的多组数据，将不同雷达的数据进行配准对齐，同步到同一坐标轴上；对不同雷达数据同步时先将时间进行同步，使同步的数据为同一时间不同雷达采集的数据；

步骤三，对每个激光雷达数据中的动点数据提取出来，处理后传输出去；

步骤四，主机对接收的步骤二的数据，进行处理，最终得到行人的脚步并且进行持续跟踪；对接收的步骤三的动点数据，使用聚类方法，使点集形成簇，计算簇几何中心，进行下一步的跟踪；再使用卡尔曼滤波器跟踪并计算目标的位置；

步骤五，数据处理完毕后即时的将数据存储到文件，并即时的上传到云。

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

2.1本发明旨在搭建一个可以对室内行人进行识别并跟踪的激光雷达网络，通过分析即时采集的数据，从中识别行人并建立跟踪轨迹，并将结果存储在文件中并上传到云服务器，包括源数据以及分析结果。

2.1.1雷达网络***的总体业务流程包括建立激光雷达网络、同步不同雷达设备间数据与时间、单独分析不同雷达数据、总体分析所有雷达数据、展示结果。***的主要运行步骤为以上几个流程。激光雷达是传感器作为数据来源，已经布置在房间里的各个位置。然后激光雷达连接树莓派，作为数据处理设备和通信设备。不同树莓派之间需要建立稳定连接用于传输数据，对于传输的数据要进行同步，包括时间上的同步和空间上的同步。对于接收的数据进行分析包括单独分析以及总体分析，最后将处理结果存储在文件并上传到云。

2.1.2功能性需求分析

(1)树莓派的连接与激光雷达网络的建立

本***是一个基于多激光雷达的***。首要的任务是使不同树莓派激光雷达之间可以相互连接。给树莓派选择一个***使之满足激光雷达驱动所需的运行环境，然后实现树莓派之间的通信。并确立作为最终处理数据的主机。

(2)激光雷达的同步

主机上会收到来自不同雷达的很多组数据，对于这些数据有同一雷达不同时间的数据，有不同雷达同一时间的数据，不同雷达的数据坐标轴不一样，所有雷达都是以自己设备为中心进行扫描，需要将不同雷达的数据进行对齐，同步到同一坐标轴上。同一雷达的数据有时间属性，再对不同雷达数据同步的时候要先将时间进行同步，使同步的数据为同一时间不同雷达采集的数据。

(3)数据的单独分析

每个激光雷达需要单独分析自己接收的数据，包括地图数据，动点数据等。将雷达数据中的动点数据提取出来，处理后传输给后续模块进行进一步分析。

(4)主机对所有数据进行分析

主机将接收到的数据，进行一系列处理，最终得到行人的脚步并且进行持续跟踪。对于接收的动点数据，要识别其中的行人，考虑使用聚类方法，使点集形成簇，计算其几何中心方便下一步的跟踪。然后考虑使用卡尔曼滤波器跟踪并计算目标的位置与速度，卡尔曼滤波可以降低***误差并得到相对准确的位置与速度。

(5)跟踪结果存储并上传到云

数据处理完毕后即时的将数据存储到文件，并即时的上传到云，用户可以看到多雷达网络的脚步跟踪结果，并以此得到更多信息。

2.1.3非功能性需求分析

(1)运行环境

本***需要若干台2D激光雷达作为基础传感器。本***采用的是深圳市3irobotix有限公司生产的Delta-1A系列激光雷达。这是一种新一代低功耗、低成本二维激光雷达。比起市面上其他传统激光雷达它的特点是成本相当低，但是同样具有精度低、误差大的特点。它有每秒大约4000次的采样能力，每秒约可扫描6圈图像，每帧图像约为690个点，并实现了长时间可靠的稳定运行。 Delta-1A系列激光雷达可以实现在2D平面的0.15m到8m半径范围内进行360°全方位扫描，每一帧产生一幅所在空间的平面点云信息。激光雷达在机器人定位导航、地图测绘、物体环境识别等方向运用广泛。而且，Delta-1A系列激光雷达所发射激光功率很低，符合FDA Class I人眼安全等级。

若干台树莓派Raspberry Pi微型计算机与激光雷达对应，用来连接雷达并进行驱动，驱动程序基于ROS***。树莓派是一种大小类似卡片的微型电脑，其***可自定义，它基于ARM的微型电脑主板，以SD/MicroSD卡为内存硬盘，同时有PC的所有基本功能。使用树莓派驱动雷达，极大的提高了***的实用性。同本发明采用Raspberry Pi 4B型号的树莓派，安装Ubuntu 20.04***。

一台树莓派选择同版本但具有图形化界面的***方便操作，之后默认该树莓派为主机，以此为基础通过ssh协议连接各个从机树莓派并通过脚步文件自动运行各个ROS***的程序。本***通过ROS自带的订阅者发布者进行数据传输，本***的大部分程序基于ROS运行，包括python程序和C++程序，并通过ROS 进行统一组织调度。

一个路由器，需要路由器作为网关，所有树莓派应在统一网段下，并锁定静态IP，才进行后续连接。

(2)即时性需求

本发明旨在即时接收并分析多雷达数据，并即时得出结果，对即时性有很高要求。即时性需求分为通信和处理两部分，第一部分要求在数据传输量必须符合带宽要求，树莓派是通过路由器连接在同一网段下，路由器传输速度有限，信号距离有限，树莓派必须处于强信号位置，并且广播的数据通过路由器能很快的传输到需要的设备，要求路由器具有一定的带宽，支持三到五台设备同时传输数据。第二部分则是主机和从机的数据处理速度必须大于激光雷达驱动的数据反馈速度，使尽量减少帧数损失，而且能够即时的返回最终目标识别跟踪结果。

(3)可移植性需求

本***需要能够在多种版本***环境下运行，由于发明是在ROS***环境下运行，那么本***需要所有能够安装ROS***的Linux***环境下运行，而且如果树莓派***版本不一样应也能在同版本的ROS***环境下完成连接。

(4)可靠性需求

本***将会运行在多个设备上，当有除主机之外的设备失联时应也能继续完成目标识别跟踪工作，并有一定的错误处理能力。

2.2***功能模块

如图2所示，整个基于雷达网络的***原理，包括树莓派连接、源数据广播、地图建立、数据预处理、数据同步、目标识别，目标跟踪，结果展示。其中，源数据广播、地图建立、数据预处理为全体装载雷达的树莓派(公用)运行，树莓派连接、数据同步、目标识别、目标跟踪为主机运行。

2.3***核心业务流程

目标识别与跟踪是本***的核心。***运行的总体流程如图3所示，主要流程有建立环境地图、对动点进行聚类、随机森林预测、发布行人脚步点、脚步配对、卡尔曼滤波器跟踪、获取行走参数这几项。

2.4***架构

***架构图如图4所示。激光雷达直接连接树莓派USB接口，树莓派都运行在同一网段下，主机树莓派启动ROSCORE进程，并通过连接模块连接若干从机树莓派，所有装有雷达的树莓派运行读取雷达数据的ROS进程和处理并发送数据的ROS进程，主机树莓派运行接收和分析处理后数据的ROS进程。运行结果上传至云。

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

3.1***环境搭建

3.1.1树莓派***的选择与连接

雷达驱动适配***为Linux/ros，Windows，所以选择Ubuntu***作为程序的运行环境，由于程序涉及到多设备间通信，ros***可以提供较好的通信接口，所以选择Ubuntu安装ros***作为二级运行环境，针对Ubuntu官方提供的应用于树莓派的***，目前包括Ubuntu 20.04服务器版，Ubuntu 20.04mate2桌面版，综合设备性能和实际开发需求考虑，最终选择Ubuntu 20.04mate2桌面版安装在性能最佳的设备上(树莓派4b+128G内存卡)，其他设备安装Ubuntu 20.04服务器版，而程序拟在两平台兼容运行。桌面版使用realVNC连接，服务器版使用 ssh连接使用控制台操作，使用XFTP进行文件传输。

3.1.2激光雷达驱动安装与连接

该型号激光雷达驱动共有Linux，ROS，Windows几种，综合以上考虑，选择ROS***下的驱动。在树莓派上安装ROS***，将激光雷达通过USB接口连接到树莓派上，安装对应驱动，并按照使用说明获取雷达数据。

3.2树莓派***指令控制与数据传输

树莓派***为Ubuntu***，Ubuntu***是基于Linux***的操作***，需要同时对多个树莓派进行指令控制，通过主机树莓派运行脚步命令，通过建立 ssh连接控制各个从机的进程运行与停止。通过ssh连接控制从机的各个控制台，再通过控制台控制本地程序。ssh协议(安全外壳协议)是一个建立在应用层的协议，其优势是轻量级的连接，建立连接之后可以直接控制目标***的控制台，方便运行权限要求很高的指令。

树莓派间数据传输本***一律通过ROS***的订阅者和发布者传输，ROS 的订阅者与发布者是ROS***自带的一个多机、多进程通信的框架，其优点在于，只要各个终端在ROS***下完成一次连接并由一个主机运行ROSCORE进程，则数据传输的连接过程将会有ROS***自动完成，对于同设备不同进程间通信和不同设备间进程通信会自动识别并完成，并可以具有缓冲和数据完整性验证功能。

3.3源数据广播模块

建立一个ROS节点，并内嵌雷达驱动，功能为连接雷达获取雷达原始数据并运行发布者将其广播。这个进程为***的基础进程，通常情况下是为本设备的数据预处理模块提供原始数据，也可以由其他进程或其他设备订阅完成其他功能。

3.4地图建立模块

建立一个ROS节点，订阅本设备连接雷达对应的发布者节点，获取一段时间雷达数据，并将该时间间隔内所有数据帧相加，得到的结果为一张图，图中的点为以上所有帧点数之和，每个点对应一个权值，权值为该点在以上所有帧中出现的次数。激光雷达本身具有误差，误差原因包括多个方面，其中包括机械部件的抖动，本***使用激光雷达原理为在一个可以全方位旋转的机械部件上固定一个激光发射器和一个接收器，然后机械部件以一秒六圈的速度旋转，而机械部件的旋转则会因为工艺的问题发生抖动，产生误差点。所以将图中权值小于一定阈值的点删去作为降噪，最后将图去除权值存储在文件中。

3.5数据预处理模块

3.5.1雷达原始数据分析

通过雷达驱动获取的雷达原始数据为：角度加距离，角度指该点到雷达起始点的旋转角，距离指目标点到激光雷达的距离，每一帧约为690个点，相邻两点角度差别约为0.6°，一秒约为6帧，相邻两帧间时间间隔约为0.16秒，然后发布者发布数据的时候以帧为单位，驱动每返回一帧数据，则发布一次，并在此时包装进***时间与***主机号。ROS***发布者接收者框架是以“消息”为单位，经查询发现ROS***自带的支持激光雷达的“消息”是“LaserScan”结构如表1所示

表1数据结构表

不符合实际需要，源数据为角度加距离，角度相差并不固定，有一定波动范围，而且一帧实际约为690个点，而该结构固定为360个，不符合需要，所以选择“自定义消息”，包括不定长距离数组，不定长角度数组，时间，主机号。

3.5.2数据预处理

本***是一个即时性的***，需要即时的反馈当前结果，所以从行人移动到结果输出之间延迟有限制。所以预处理是运行在直接与雷达连接的每一个树莓派上，对激光雷达原始数据进行初步处理获取进一步数据以减少主机的计算量从而减少延迟，本***的数据预处理为动点的提取，读取动点方法有帧差法，指存储上一帧数据找到相邻帧间不同的点作为动点输出，优点是在数据精准的情况下，不需要静态地图即可得到动点，但是本***采用的激光雷达误差较大，所以不采用帧差法。第二种方法是先建立静态地图，读取存储在本地的静态地图，对于读取的每一帧数据与地图进行比对，找到地图中不存在的点，将地图中不存在的点认为是动点。这里采用第二种方法。将得到的动点继续在ROS***里广播。

3.6数据同步模块

3.6.1时间同步

主机树莓派需要运行一个ROS节点接收来自所有树莓派的动点信息，收到的信息格式为：1.横坐标值数组2.纵坐标值数组3.时间戳4.主机名。其中横坐标数组长度与纵坐标数组长度相同，主机名为发布者本身运行的树莓派的主机名，时间戳为发布时加上的***时间。由于各个设备发布者运行的时间节点不确定，所以需要对数据进行时间上的粗同步，因为各个雷达驱动返回数据周期不重合，数据不可能做到完全同步，然后时间同步完成前无法进行下一步计算，为了满足即时性的要求，基于人行走速度有限，所以使用最简单最直接的方法进行时间同步，将主机与从机时间做差，如果主机时间快则抛弃从机数据至与主机时间差为阈值范围内，如果从机时间快则抛弃主机数据至与从机时间差为阈值范围内，重复此操作，完成后将该从机标志位置为真，表示该从机已时间对齐。

3.6.2空间同步

所有雷达采集的数据都是以自身为中心向四周发散的，虽然多个雷达在同一环境，但是坐标***不一样，由于雷达布置在同一房间的同一水平线上，则通过平移和旋转可以将多个雷达的数据对齐到同一坐标体系下，而平移量和旋转角的获取则有几种办法，比如使用ICP算法对点云数据进行配准。事先使用测量工具测量几个激光雷达的相对位置和相对角度。这里使用算法对比图像后人工调整图像至配准。比如激光雷达可以看到四周的墙壁，则不同激光雷达看到的墙壁是部分重合的，可以以此为依据将激光雷达图像对齐得到旋转角和平移量，然后将旋转角平移量存在文件中或上传到云中。主机树莓派运行时会根据本地文件将接收数据平移旋转和存储。

3.7目标识别模块

3.7.1人脚图像分析

自此，主机树莓派得到了对齐的来自各个雷达的动点数据，需要在其中找到行人数据，本***认为，在室内长时间移动物体只有行人，现实中也有宠物机器人之类的运动物体，但本***将这些忽略。所以依据此进行下一步分析。对于多雷达的图像融合有几种处理办法，比如直接将图像相加，多幅图像相加得到融合图像。但是这种办法对于本***不适用。而实际情况因为雷达的精度和误差问题，激光雷达图像将会在该图的基础上向四周漂移，在这些情况下，提取人脚的特征将是非常困难的。有些技术会使用机器学习来解决这种问题，比如随机森林算法，马尔科夫链识别算法，但是这些算法共同特征是计算时间长占用资源大，本***是部署在单片机树莓派上可完全在局域网下运行的即时***，如果采用机器学习的办法识别人脚，无论是从延迟上还是计算量上考虑都无法完成任务。所以，本***采用以一个雷达数据为主要计算数据，另一个雷达为补充数据的方式识别人脚。如果只考虑单雷达数据，那么人脚的识别将会是简单的，可以认为，动点集中的位置就是人脚的位置，问题从识别人脚简化为，识别点集。

3.7.3从雷达数据补充主雷达数据

对于主雷达动点数据和副雷达动点数据分别使用基于区域生长的聚类算法，可以得到不同雷达视角下的人脚几何中心，现在需要使用副雷达数据对主雷达数据进行补充。只有在主雷达对某个行人的某只脚有信息缺失的情况下，需要使用副雷达数据补充主雷达数据。如5所示。

假设上方雷达为主雷达，下方雷达为副雷达，黑色直线为遮挡物，则主雷达无法得到右脚的数据，这个时候就可以用副雷达数据填补这个空缺，那么需要在副雷达数据中找到主雷达没有的数据，由于现实情况会有多个人在同一场景中行动，需要一个算法将，主雷达和副雷达的点一一对应，并找出副雷达中没有对应的点。本发明使用匈牙利算法完成此过程。匈牙利算法是一种求二分图中增广路径的算法，可以解决指派问题，同样可以计算两幅图中的对应点。是一个求全局最优解的算法。通过此算法可以得到主雷达图像和副雷达图像一一对应的点。然后得到副雷达图像中主雷达没看到的点集，将其添加到主雷达图像中。

3.8目标跟踪模块

3.8.1目标跟踪流程

目标跟踪是基于卡尔曼滤波器实现的，其中卡尔曼滤波器中有观测值和预测值，预测值为本***的存储值。目标跟踪分为两个部分，一个部分是将观测值与存储值匹配，当一帧图像观测到数个人脚的点时，要将观测点与已经存储的值对应，存储值是通过之前观测值计算得到的点，现实意义是上一帧人脚的位置点，将这两个值匹配可以得到人脚点的轨迹完成跟踪过程。第二个部分为将匹配好的观测值和存储值输入卡尔曼滤波器，通过卡尔曼滤波计算降低噪音影响并完成跟踪。

3.8.2存储值与观测值匹配

观测值现实意义为当前帧激光雷达观测到的人脚点位置，存储值现实意义为上一帧人脚的位置。那么观测值与存储值的区别则是相邻两帧的时间间隔内，人脚移动的出发点和终止点。由于时间间隔约为0.16s，行人在此时间内移动距离非常小，所以可以认为存储值和观测值变化很小，将二者匹配同样可以使用匈牙利算法将两者一一对应，未能成功匹配的点则是有新人脚出现在激光雷达的扫描范围内。

3.8.2卡尔曼滤波器完成目标跟踪

以上的过程***得到了行人轨迹，轨迹含义是，每一帧内同一只脚的位置。然而，激光雷达的观测是存在误差的，卡尔曼滤波器可以降低误差。因为把人脚在极短时间内看作匀速直线运动，那么之前帧人脚的位置和速度，实际上决定了下一帧人脚的位置和速度，卡尔曼滤波器利用了这种关系，利用前一时间间隔物体的状态和变化趋势，加上当前传感器观测到的物体状态，计算得到一个相对准确当前物体状态。卡尔曼滤波器认为，任何时刻存在两个误差，***误差和观测误差，分别对应软件上的误差和硬件上的误差，而认为其误差符合正态分布，卡尔曼滤波器可以在线性状态空间下，输入有误差的观测值和其他输入，得到相对准确的真实状态。该模型广泛运用在激光雷达的目标跟踪上，本文运用卡尔曼滤波器得到一个相对准确跟踪轨迹，这是一种递归的估计模型，当前时间的实际值可以通过上一时间间隔的实际值和当前时间的观测值计算得到。

卡尔曼滤波器模型的核心方程如公式(3-1)和(3-2)：

X_k＝A_kX_k-1+B_ku_k+w_k (3-1)

z_k＝H_kX_k+v_k (3-2)

式中X_k＝[x_k，y_k，x′_k，y′_k]是第k帧的实际人脚状态向量；

z_k＝[x_k，y_k]是第k帧的激光雷达得到人脚位置；

A_k是状态转移矩阵；

B_k是控制输入矩阵，将运动测量值映射到状态向量上；

u_k是第k帧的***控制向量，在此模型中忽略；

w_k是过程噪声；

H_k是转换矩阵，将状态向量转换成测量向量的格式；

v_k是观测噪声。

X_k＝A_kX_k-1+B_ku_k (3-3)

X′_k＝X_k+K(z_k-H_kX_k) (3--5)

P′_k＝P_k-KH_kP_k (3-6)

式中X′_k是第k帧的***修正后人脚状态向量；

P_k是第k帧估测状态的协方差矩阵；

P′_k是第k帧***修正后人脚状态的协方差矩阵；

K是卡尔曼增量；

在一个人脚点新出现在***中时，***需要给其分配一个轨迹，这个过程对应卡尔曼滤波器中的初始化过程，将初始时刻的测量值作为初始时刻的实际值，然后将激光雷达每一帧的观测图像输入进卡尔曼滤波，可以通过该轨迹上一帧的实际值得到当前帧的实际值，完成一次卡尔曼计算。通过此过程可以有效减少观测值的误差从而得到相对准确的人脚轨迹。

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

4.1树莓派连接模块的实现

本发明主要说明树莓派连接模块的代码层面实现，前面详细发明已经说明了该模块使用的技术已经实现思路。

本***是基于ROS***的程序，对于各个从机，需要修改配置文件，并标记为从机，存储主机的IP地址，主机需要存储所有从机的IP地址并标记自己为主机，所有树莓派在连接WIFI时将使用固定IP。也可以先使用局域网广播的形式进行初步确认，然后将动态IP存入文件再进行下一步连接。本***使用固定 IP。

当***需要运行时，主机会运行一个脚本文件，脚本文件中写入了通过ssh 协议连接从机的部分，然后通过ssh协议传输指令启动相应ROS节点，ROS节点中有接收数据和发送数据的模块。自此完成各个树莓派的连接过程。

4.2源数据广播与预处理

本发明主要说明数据广播和预处理两个模块的代码层面实现。首先需要建立一个ROS节点用于运行雷达驱动，每当雷达驱动返回数据时，需要建立一个发布者ROS线程将源数据广播至ROS***里。然后建立一个ROS节点用于接收源数据并对数据预处理，本***中源数据广播和数据预处理是在同一设备中运行，数据预处理过程是动点识别过程，从本地文件中读取地图文件，然后对于每一帧接收的数据进行极坐标转直角坐标，然后与地图文件进行比对。经过实际测试发现，由于激光雷达误差较大，如果按照原始精度进行动点识别动点数量过多，原始精度为1cm，因为地图建立的时间很难把所有误差都扫描出，所以在动点判断时，将接收数据和地图数据精度全部放大至5cm，具体计算过程如公式4-1：

Round表示以四舍五入的方式取整。放大精度后，提取接收数据中不在地图数据中的点，并将其广播在ROS***下。

4.3数据同步

本发明主要说明数据同步模块的代码层面实现。数据同步有时间同步和坐标同步。在同一ROS节点进程下完成，该节点同时接收来自所有树莓派的动点数据，建立一个队列数组用于存储数据，该队列数组大小为树莓派数量，当接收者接收到数据时，会按照数据包含主机名首先按照提前存储的旋转角和平移量将其空间坐标变换然后将其存储在对应位置的队列中，空间坐标变化如公式 4-2：

p′＝Hp+t (4-2)

p′是变化后点坐标向量，p为变化前点坐标向量，t为平移量，H为转换矩阵其值为

θ为旋转角。建立线程用于完成时间同步，将各个队列出队一个数据包，将从机时间属性分别如主机时间属性相减，若主机时间快则从机队列出队相应数量数据包，若从机时间快则主机队列出队相应数量数据包，若差距绝对值小于阈值则将该从机队列对应标志位置为真，表示主机已和该从机对齐。经过实际测试发现，不同激光雷达扫描频率并不绝对相等而且差值较大，对齐后按照一帧对一帧的输出方式。随着帧数增加会出现两帧时间差值越来越大，原因在于其中一激光雷达扫描频率大于另一激光雷达，所以时间对齐将不是一次性完成的工作，当输出两帧时间差值过大时需要重新进入对齐过程。对齐后将两个雷达对应时间存储在文件中。帧数丢失在可接受范围内，自此完成时间同步。

4.4目标识别及跟踪

本发明主要说明目标识别及跟踪模块的代码层面的具体实现。目标识别和跟踪与时间同步位于同一线程，在时间同步确认之后进入识别跟踪。对于主雷达和副雷达数据，分别进行基于密度的聚类方法，聚类之后，大于3个点位于一簇***认为这是人脚并除去小于三个点的簇。如图6示，为多雷达动点同步后的结果，蓝色部分为静态地图，红色点为动点，图中为三个人在雷达扫描范围内，左边墙上红点为雷达误差带来的噪点。主雷达副雷达聚类降噪后得到簇，并计算其几何中心，计算之后使用匈牙利算法进行点的匹配，将副雷达图像中匹配成功的点删除，剩余点加入主雷达图像中。

将补充后的主雷达图像点送入目标跟踪模块，需要将当前帧观测点与上一帧实际点进行匹配，通过匈牙利算法匹配，因为相邻两帧人脚不可能跨度过大，如果匹配结果大于13cm，则将此匹配结果删除。若当前帧存在观测点没有匹配前一帧实际点，则说明扫描图像中存在了新人脚轨迹，为其建立一个新轨迹并初始化卡尔曼滤波。对于匹配成功的观测点，进入卡尔曼滤波并通过上一帧实际点计算当前帧实际点，同时轨迹存续帧数加1。如果存在轨迹当前帧没有观测到，那么要对轨迹点进行卡尔曼滤波无参预测，并且轨迹点跳过帧数加1。当跳过帧数超过5帧时，***认为该轨迹已经离开。对于每一条轨迹，***要即时的存储到文件中并上传到云上，保证即时性。自此，整个***流程完毕。

本发明从***代码层面实现说明整个***的最后实现，首先进行树莓派连接，通过ssh协议完成，然后ssh协议发送控制命令运行ROS节点，ROS节点完成数据广播，数据预处理，数据同步，目标识别跟踪等功能，最后将结果即时的输出，当前结果输出方式是存储文件。

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

5.1总体***测试

本发明对整个***最后运行情况测试，包括功能性需求的测试和非功能性需求的测试。最后测试结果如表2所示。

表2***测试用例表

5.2部分结果展示

本发明将展示***运行的部分结果。测试在此环境完成的。激光雷达最好摆放位置为墙壁中心并离墙20cm的位置，然后不同激光雷达摆放位置应该角度相差大，比如对面的两堵墙中心各放一个。角度相差越大，副雷达主雷达数据补充的效果越好。

如图7所示，总共有三个人在这个环境下测试，一个人进行摄像记录。对于动点、静态环境点，周围的噪点，会在后续数据处理中过滤。多雷达对于提取脚步动点效果中，由于雷达精度问题，效果一般。

如图8所示，可以看到四个点为对脚步跟踪的结果输出，一个点代表跟踪到一只脚，摄像的行人因为距离过远未能识别出人脚。

本发明进行了整个***的测试与验证部分，包括功能性需求测试和非功能性需求测试，所有测试都通过。然后展示了部分结果，包括动点提取的结果，包括目标识别跟踪的结果，并通过程序进行展示图像，结果是以文件的形式存储在树莓派主机上。

本领域技术人员在考虑说明书及实践这里公开的公开后，将容易想到本公开的其它实施方案。本申请旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本公开的真正范围和精神由所附的权利要求指出。

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

Claims (10)

1.一种基于多雷达融合的多人跟踪方法，其特征在于，所述基于多雷达融合的多人跟踪方法包括：

步骤一，使不同树莓派与多激光雷达通信，并确立处理数据的主机；

步骤二，所述主机接收来自不同雷达的多组数据，将不同雷达的数据进行配准对齐，同步到同一坐标轴上；对不同雷达数据同步时先将时间进行同步，使同步的数据为同一时间不同雷达采集的数据；

步骤三，对每个激光雷达数据中的动点数据提取出来，处理后传输出去；

步骤四，主机对接收的步骤二的数据，进行处理，最终得到行人的脚步并且进行持续跟踪；对接收的步骤三的动点数据，使用聚类方法，使点集形成簇，计算簇几何中心，进行下一步的跟踪；再使用卡尔曼滤波器跟踪并计算目标的位置；

步骤五，数据处理完毕后即时的将数据存储到文件，并即时的上传到云。

2.根据权利要求1所述的基于多雷达融合的多人跟踪方法，其特征在于，所述步骤一不同树莓派组成处理数据的主机包括：

基于ROS***的程序，对于各个树莓派从机修改配置文件，并标记为从机，存储主机的IP地址，主机存储所有从机的IP地址并标记自己为主机，所有树莓派在连接WIFI时将使用固定IP；或先使用局域网广播的形式进行初步确认，然后将动态IP存入文件再进行下一步连接。

3.根据权利要求2所述的基于多雷达融合的多人跟踪方法，其特征在于，当***运行时，主机运行一个脚本文件，脚本文件中写入通过ssh协议连接从机的部分，然后通过ssh协议传输指令启动相应ROS节点，ROS节点通过接收数据和发送数据，完成各个树莓派的连接。

4.根据权利要求1所述的基于多雷达融合的多人跟踪方法，其特征在于，所述步骤二主机接收来自不同雷达的多组数据，将不同雷达的数据进行对齐，同步到同一坐标轴上包括：

首先建立一个ROS节点用于运行雷达驱动，每当雷达驱动返回数据时，建立一个发布者ROS线程将源数据广播至ROS***里；

然后建立一个ROS节点用于接收源数据并对数据预处理

再对每一帧接收的数据进行极坐标转直角坐标，与地图文件进行比对；

在动点判断时，将接收数据和地图数据精度全部放大至5cm，计算过程如下公式：

提取接收数据中不在地图数据中的点，并广播在ROS***下。

5.根据权利要求1所述的基于多雷达融合的多人跟踪方法，其特征在于，所述步骤二对不同雷达数据同步时先将时间进行同步，使同步的数据为同一时间不同雷达采集的数据包括：

在同一ROS节点同时接收来自所有树莓派的动点数据，建立一个队列数组用于存储数据，该队列数组大小为树莓派数量，当接收者接收到数据时，按照数据包含主机名首先按照提前存储的旋转角和平移量将空间坐标变换然后将存储在对应位置的队列中，空间坐标变化如下公式：

p′＝Hp+t；

p′是变化后点坐标向量，p为变化前点坐标向量，t为平移量，H为转换矩阵其值为

θ为旋转角；建立线程用于完成时间同步，将各个队列出队一个数据包，将从机时间属性分别如主机时间属性相减，若主机时间快则从机队列出队相应数量数据包，若从机时间快则主机队列出队相应数量数据包，若差距绝对值小于阈值则将该从机队列对应标志位置为真，表示主机已和该从机对齐；对齐后将两个雷达对应时间存储在文件中；帧数丢失在可接受范围内，自此完成时间同步。

6.根据权利要求1所述的基于多雷达融合的多人跟踪方法，其特征在于，所述步骤四使用卡尔曼滤波器跟踪并计算目标的位置包括：

卡尔曼滤波器模型的核心方程如下公式：

X_k＝A_kX_k-1+B_ku_k+w_k；

z_k＝H_kX_k+v_k；

式中X_k＝[x_k，y_k，x′_k，y′_k]是第k帧的实际人脚状态向量；

z_k＝[x_k，y_k]是第k帧的激光雷达得到人脚位置；

A_k是状态转移矩阵；

B_k是控制输入矩阵，将运动测量值映射到状态向量上；

u_k是第k帧的***控制向量，在此模型中忽略；

w_k是过程噪声；

H_k是转换矩阵，将状态向量转换成测量向量的格式；

v_k是观测噪声；

X_k＝A_kX_k-1+B_ku_k；

X′_k＝X_k+K(z_k-H_kX_k)；

P′_k＝P_k-KH_kP_k；

式中X′_k是第k帧的***修正后人脚状态向量；

P_k是第k帧估测状态的协方差矩阵；

P′_k是第k帧***修正后人脚状态的协方差矩阵；

K是卡尔曼增量。

7.一种实施根据权利要求1-6任意一项所述基于多雷达融合的多人跟踪方法的基于多雷达融合的多人跟踪***，其特征在于，所述基于多雷达融合的多人跟踪***包括：

雷达网络模块：采用多雷达建立雷达网络的结构，用于对不同雷达的调度进行分配；

雷达数据处理预处理模块：用于多雷达***存在多组数据，对数据进行降噪筛选；

多雷达数据配准融合模块：用于对于同一场景有不定数量的雷达图像，进行配准融合，还用于将同一场景的两幅或多幅图像进行配准以使图像对在一个公共空间轴上对齐；

目标分析模块：用于对于点云数据进行分析，分离出疑似行人的部分，并进行跟踪；

结果展示模块：用于将行走数据以表格和图像的形式打印输出。

8.根据权利要求7所述的基于多雷达融合的多人跟踪***，其特征在于，所述基于多雷达融合的多人跟踪***进一步包括：

激光雷达直接连接树莓派USB接口，树莓派都运行在同一网段下，主机树莓派启动ROSCORE进程，并通过连接模块连接若干从机树莓派，所有装有雷达的树莓派运行读取雷达数据的ROS进程和处理并发送数据的ROS进程，主机树莓派运行接收和分析处理后数据的ROS进程；运行结果上传至云。

9.一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行权利要求1-6任意一项所述基于多雷达融合的多人跟踪方法。

10.一种信息数据处理终端，其特征在于，所述信息数据处理终端包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行权利要求1-6任意一项所述基于多雷达融合的多人跟踪方法。

Images