CN102253391A - Multi-laser-radar-based pedestrian target tracking method - Google Patents

Abstract

Translated fromChinese

本发明公开了一种基于多激光雷达的行人目标跟踪方法。由多个单线激光雷达组成的传感器网络对整个场景进行监控。每个采样时刻雷达输出行人的脚步点距离数据，不同雷达之间的数据通过系统标定统一到一个坐标系下。建立行人的脚步运动模型，并通过卡尔曼滤波对行人运动状态进行跟踪，最终获得稳定的目标轨迹和相关统计信息。本发明以激光雷达为传感器，克服了图像传感器的跟踪方法中存在的易受光照变化影响的缺点，可靠性得到很大提高；不但跟踪范围大，算法速度快，跟踪数量多，而且能兼容行人的各种运动模式，包括站立、步行、慢跑，能十分有效地检测出行人的位置并实时跟踪行人的运动轨迹。本发明适用于场景中的主要运动目标为行人的场合。

The invention discloses a pedestrian target tracking method based on multiple laser radars. A sensor network consisting of multiple single-line lidars monitors the entire scene. At each sampling moment, the radar outputs the pedestrian's footstep distance data, and the data between different radars are unified into one coordinate system through system calibration. The pedestrian's footstep motion model is established, and the pedestrian's motion state is tracked through Kalman filtering, and finally a stable target trajectory and related statistical information are obtained. The invention uses laser radar as the sensor, which overcomes the shortcomings of the image sensor tracking method that is easily affected by light changes, and greatly improves the reliability; not only the tracking range is large, the algorithm speed is fast, the number of tracking is large, and it is compatible with pedestrians Various motion modes, including standing, walking, and jogging, can effectively detect the position of pedestrians and track the trajectory of pedestrians in real time. The present invention is applicable to occasions where the main moving object in the scene is pedestrians.

Description

Translated fromChinese

一种基于多激光雷达的行人目标跟踪方法A Pedestrian Target Tracking Method Based on Multi-LiDAR

技术领域technical field

本发明涉及一种行人目标跟踪方法，尤其是涉及一种基于多激光雷达的行人目标跟踪方法。The invention relates to a pedestrian target tracking method, in particular to a pedestrian target tracking method based on multiple laser radars.

本发明涉及一种基于多激光雷达的行人目标跟踪方法，适用于在各种公共场合中实现对多个行人在站立，步行，慢跑等运动模式下的跟踪。The invention relates to a pedestrian target tracking method based on multiple laser radars, which is suitable for tracking a plurality of pedestrians in standing, walking, jogging and other motion modes in various public places.

背景技术Background technique

在地铁，车站，百货大楼等人流密集的公共场合中，行人流量、运动方向等统计信息能够帮助工程设计方进行更合理的出入口布局，设计合理的安全紧急预案等，具有巨大的商业价值。想要自动获得这些实时统计信息，就需要对场景中所有行人目标进行准确的跟踪。目前的目标跟踪技术主要基于摄像机等视觉传感器，通过一定的图像处理算法对图像中的运动目标进行跟踪。然而基于图像处理算法的跟踪技术都对环境光照的变化比较敏感，无法适应全天候跟踪的要求。同时一个摄像机覆盖的视野有限，虽然可以靠多个摄像机实现大范围覆盖，但跨相机的目标跟踪实现困难，尚处于实验阶段。相比之下，激光雷达能直接获取目标的距离信息，而且不受光照变化的影响。随着应用的逐渐广泛，价格也逐步降低。In crowded public places such as subways, stations, and department stores, statistical information such as pedestrian flow and movement direction can help engineering designers make more reasonable entrance and exit layouts, design reasonable safety emergency plans, etc., and have huge commercial value. Automatically obtaining these real-time statistics requires accurate tracking of all pedestrian objects in the scene. The current target tracking technology is mainly based on visual sensors such as cameras, and tracks moving targets in images through certain image processing algorithms. However, tracking technologies based on image processing algorithms are sensitive to changes in ambient light and cannot meet the requirements of all-weather tracking. At the same time, the field of view covered by one camera is limited. Although multiple cameras can be used to achieve large-scale coverage, it is difficult to achieve cross-camera target tracking and is still in the experimental stage. In contrast, lidar can directly obtain the distance information of the target and is not affected by changes in illumination. With the gradual widespread application, the price is gradually reduced.

发明内容Contents of the invention

本发明的目的在于提供一种基于多激光雷达的行人目标跟踪方法，不但跟踪范围大，准确度高，算法速度快，而且能兼容行人的各种运动模式，包括站立，步行和慢跑等，能十分有效地检测出行人的位置并实时跟踪行人的运动轨迹。The purpose of the present invention is to provide a pedestrian target tracking method based on multiple laser radars, which not only has a large tracking range, high accuracy, and fast algorithm speed, but also can be compatible with various motion modes of pedestrians, including standing, walking and jogging, etc. It is very effective in detecting the position of pedestrians and tracking the trajectory of pedestrians in real time.

本发明采用的技术方案是包括以下步骤：The technical solution adopted in the present invention comprises the following steps:

(1)雷达位置的设置和系统标定；(1) Radar position setting and system calibration;

(2)数据采集和融合；(2) Data collection and fusion;

(3)运动点的聚类和脚步点的配对；(3) Clustering of movement points and pairing of footstep points;

(4)基于人体模型和卡尔曼滤波的目标跟踪。(4) Target tracking based on human body model and Kalman filter.

所述步骤(1)雷达位置的设置和系统标定：即将多个雷达设置在场景的不同位置，设置位置的选择原则是：(a)使目标在场景中的任何位置都不容易被遮挡；(b)在场景边缘均匀布置激光雷达，以提高目标被检测到的可能性，雷达扫描平面距离地面高度23～25cm，此高度保证雷达在大多数情况下都能检测并获得行人两只脚的距离数据；对多个雷达的标定是通过如下方法实现的：在场景中设置一些棒状物体，设从N个雷达中得到的第i个棒状物体的坐标为：The setting and system calibration of described step (1) radar position: promptly a plurality of radars are arranged in the different positions of scene, the selection principle of setting position is: (a) make the target not easily be blocked in any position in the scene; ( b) Evenly arrange the lidar at the edge of the scene to increase the possibility of the target being detected. The height of the radar scanning plane from the ground is 23-25cm. This height ensures that the radar can detect and obtain the distance between the two feet of pedestrians in most cases Data; the calibration of multiple radars is achieved by the following method: set some rod-shaped objects in the scene, and set the coordinates of the i-th rod-shaped object obtained from N radars as:

$<span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>1</span>1</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>1</span>1</span>}}<span><span>)</span>)</span><span><span>,</span>,</span><span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>2</span>2</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>2</span>2</span>}}<span><span>)</span>)</span><span><span>,</span>,</span><span><span>.</span>.</span><span><span>.</span>.</span><span><span>.</span>.</span><span><span>,</span>,</span><span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>N</span>N</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>N</span>N</span>}}<span><span>)</span>)</span><span><span>;</span>;</span>$

并假设以第1个雷达的坐标为参照坐标，在二维平面上设第n个雷达坐标到第1个雷达坐标的转移矩阵R_n和平移量T_n为：And assuming that the coordinates of the first radar are used as the reference coordinates, the transfer matrix R_n and the translation amount T_n from the coordinates of the nth radar to the coordinates of the first radar are set on the two-dimensional plane as:

${\mathrm{<span><span>R</span>R</span>}}_{\mathrm{<span><span>n</span>no</span>}}<span><span>=</span>=</span>\left[\begin{array}{cc}{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>11</span>11</span>}}^{\mathrm{<span><span>n</span>no</span>}}& {\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>12</span>12</span>}}^{\mathrm{<span><span>n</span>no</span>}}\\ {\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>12</span>12</span>}}^{\mathrm{<span><span>n</span>no</span>}}& {\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>22</span>twenty\; two</span>}}^{\mathrm{<span><span>n</span>no</span>}}\end{array}\right]$${\mathrm{<span><span>T</span>T</span>}}_{\mathrm{<span><span>n</span>no</span>}}<span><span>=</span>=</span>\left[\begin{array}{cc}{\mathrm{<span><span>t</span>t</span>}}_{\mathrm{<span><span>1</span>1</span>}}^{\mathrm{<span><span>n</span>no</span>}}& {\mathrm{<span><span>t</span>t</span>}}_{\mathrm{<span><span>2</span>2</span>}}^{\mathrm{<span><span>n</span>no</span>}}\end{array}\right]$

则得到第i个点从第n个雷达坐标到第1个雷达坐标的转换方程：Then the transformation equation of the i-th point from the n-th radar coordinate to the first radar coordinate is obtained:

$<span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>1</span>1</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>1</span>1</span>}}<span><span>)</span>)</span><span><span>=</span>=</span>{\mathrm{<span><span>R</span>R</span>}}_{\mathrm{<span><span>n</span>no</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>n</span>no</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>n</span>no</span>}}<span><span>)</span>)</span><span><span>+</span>+</span>{\mathrm{<span><span>T</span>T</span>}}_{\mathrm{<span><span>n</span>no</span>}}$

当i＞3时转换矩阵和平移量唯一确定，通过标定将不同雷达坐标系下得到的数据转换到同一个雷达坐标下，式中：x表示点的横坐标值，y表示点的纵坐标值。When i>3, the conversion matrix and the translation amount are uniquely determined, and the data obtained in different radar coordinate systems are converted to the same radar coordinate through calibration. In the formula: x represents the abscissa value of the point, and y represents the ordinate value of the point .

所述步骤(2)数据采集和融合：即通过网络集线器将多个雷达采集的数据传输到一个计算机主机上，同时记录采集的时间；多个雷达之间的数据采集由统一的外同步触发，通过采集50帧以上帧数的数据先自动提取出每个雷达的背景数据，背景数据的提取方法采用了直方图法，即统计所采集帧中每个方向上数据点的距离信息，并做直方图统计，出现频率最高的即为该方向上的背景点数据，保存该背景数据，然后将同一时间采集到的各个方向上的距离数据与背景数据比较，若距离差值小于所设定的阈值量则认为是背景点而去除，剩下的即是场景中的前景点即运动点；将同一时刻各个雷达获得的前景点通过标定参数进行坐标转换，即得到在统一全局坐标系下该时刻采集到的融合后的运动数据点。Described step (2) data collection and fusion: namely by the network hub, the data collected by a plurality of radars is transmitted to a host computer, and the time of collection is recorded simultaneously; The data collection between a plurality of radars is triggered by unified external synchronization, By collecting more than 50 frames of data, the background data of each radar is automatically extracted first. The background data extraction method adopts the histogram method, that is, the distance information of the data points in each direction in the collected frames is counted, and the histogram is made. Figure statistics, the background point data in the direction with the highest frequency of occurrence, save the background data, and then compare the distance data in each direction collected at the same time with the background data, if the distance difference is less than the set threshold The amount is considered as the background point and removed, and the rest is the foreground point in the scene, that is, the moving point; the coordinate conversion of the foreground points obtained by each radar at the same time is carried out through the calibration parameters, that is, the acquisition at that time in the unified global coordinate system is obtained. The fused motion data points obtained.

所述步骤(3)运动点的聚类和脚步点的配对：即对融合得到的运动数据点，对相互距离小于第一距离阈值的点进行聚类，并用类内点的中心作为候选脚步的位置，然后对脚步候选点进行时域和空域上的匹配，以确定某两个候选脚步点是否属于同一个行人，有两种情况：(a)在卡尔曼滤波获得的已有目标的两个脚步点预测位置附近搜索匹配，若本帧中的两个候选脚步点分别和两个预测位置小于第二距离阈值，则判断两个候选脚步点为已有目标的观测点；(b)若当前候选脚步点未能在已有目标中找到匹配，则属于新目标或者噪声干扰点；然后在当前帧中用第三距离阈值搜索未匹配的脚步候选点，若找到则将找到的匹配点联合一起成为属于同一个新目标的两个脚步点。The pairing of the clustering of described step (3) motion point and footstep point: promptly to the motion data point that fusion obtains, the point that mutual distance is less than the first distance threshold is clustered, and uses the center of the point in the class as the candidate footstep position, and then match the footstep candidate points in time domain and air domain to determine whether two candidate footstep points belong to the same pedestrian. There are two situations: (a) two existing targets obtained by Kalman filtering Search and match near the predicted position of the footstep point. If the two candidate footstep points in this frame and the two predicted positions are less than the second distance threshold, it is judged that the two candidate footstep points are the observation points of the existing target; (b) if the current If the candidate footstep points cannot find a match in the existing target, it belongs to a new target or a noise interference point; then use the third distance threshold to search for unmatched footstep candidate points in the current frame, and if found, combine the found matching points together Become two footsteps belonging to the same new goal.

所述步骤(4)基于人体运动模型和卡尔曼滤波的目标跟踪：即对行人建立了运动模型，并根据这个模型采用卡尔曼滤波进行目标跟踪：Described step (4) is based on the target tracking of human body motion model and Kalman filtering: promptly set up motion model to pedestrian, and adopt Kalman filtering to carry out target tracking according to this model:

1)行人模型的建立1) Establishment of pedestrian model

研究表明，行人的状态分为：站立、步行、慢跑，行人在步行和慢跑的运动过程中总是有一个支撑脚，一个摆动脚，并且两者周期性互换，所不同的是步行时支撑脚总是不离地，并且处于静止状态；而慢跑时支撑脚会离地并且会在周期内有运动距离，基于以上提到的站立、步行、慢跑三种行人状态的建模分别如下：Studies have shown that the state of pedestrians is divided into: standing, walking, and jogging. During walking and jogging, pedestrians always have a supporting foot and a swinging foot, and the two are periodically exchanged. The feet are always on the ground and are in a static state; while jogging, the supporting feet will be off the ground and have a movement distance in the cycle. Based on the above-mentioned three pedestrian states of standing, walking, and jogging, the modeling is as follows:

站立模式：没有生理作用力的驱动，左右脚的速度为零，加速度也为零；force＝0，v_L，k＝0，v_R，k＝0，a_L，k＝0，a_R，k＝0，(1)Standing mode: driven without physiological force, the speed of the left and right feet is zero, and the acceleration is also zero; force=0, v_{L, k} = 0, v_{R, k} = 0, a_{L, k} = 0, a_{R, k} = 0, (1)

步行和慢跑运动模式：支撑脚具有一个恒定不变的速度，摆动脚具有一个恒定不变的生理作用力，分别在前半周期和后半周期产生一个正向和负向的大小恒定不变的生理作用力，从而对运动脚形成加速度；Walking and jogging motion patterns: the supporting foot has a constant velocity and the swinging foot has a constant physiological force, producing a positive and negative physiological force of constant magnitude in the first and second half cycles, respectively. force, thereby forming an acceleration on the moving foot;

以T为两帧之间的时间间隔，右脚为支撑脚的前半周期内：Take T as the time interval between two frames, and the right foot as the first half cycle of the supporting foot:

force＝F，v_L，k＝v_L，k-1+A*t，v_R，k＝V，a_L，k＝A，a_R，k＝0，(2)force=F, v_{L, k} = v_{L, k-1} + A*t, v_{R, k} = V, a_{L, k} = A, a_{R, k} = 0, (2)

右脚为支撑脚的后半周期内：During the second half of the cycle with the right foot as the supporting foot:

force＝-F，v_L，k＝v_L，k-1+A*t，v_R，k＝V，a_L，k＝-A，a_R，k＝0，(3)force=-F, v_{L, k} = v_{L, k-1} + A*t, v_{R, k} = V, a_{L, k} = -A, a_{R, k} = 0, (3)

左脚为支撑脚的前半周期内：During the first half of the cycle with the left foot as the supporting foot:

force＝F，v_L，k＝V，v_R，k＝v_R，k-1+A*t a_L，k＝0，a_R，k＝A，(4)force=F, v_{L, k} = V, v_{R, k} = v_{R, k-1} +A*t a_{L, k} = 0, a_{R, k} = A, (4)

左脚为支撑脚的后半周期内：During the second half of the cycle with the left foot as the supporting foot:

force＝-F，v_L，k＝V，v_R，k＝v_R，k-1+A*t a_L，k＝0，a_R，k＝-A，(5)force=-F, v_{L, k} = V, v_{R, k} = v_{R, k-1} +A*t a_{L, k} = 0, a_{R, k} =-A, (5)

式中force表示生理作用力，F表示行人实际产生的生理作用力，v表示运动速度，V表示行人F作用力下产生的速度，a表示加速度，A表示行人在F作用力下产生的加速度，L表示左脚，R表示右脚，k表示第k个时刻；In the formula, force represents the physiological force, F represents the actual physiological force of the pedestrian, v represents the speed of movement, V represents the speed of the pedestrian under the force of F, a represents the acceleration, A represents the acceleration of the pedestrian under the force of F, L represents the left foot, R represents the right foot, and k represents the kth moment;

当V＝0时，行人即处在步行状态下，当V＞0时，行人即处在慢跑状态下；When V=0, the pedestrian is in the walking state; when V>0, the pedestrian is in the jogging state;

2)卡尔曼滤波跟踪2) Kalman filter tracking

把行人的位置P和速度V当作状态变量，加速度A当作系统的输入量，则得到系统的状态方程为：Taking the pedestrian's position P and velocity V as the state variables, and the acceleration A as the input of the system, the state equation of the system is obtained as:

${\mathrm{<span><span>S</span>S</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>\mathrm{<span><span>\&Phi;</span>\&Phi;</span>}{\mathrm{<span><span>S</span>S</span>}}_{\mathrm{<span><span>k</span>k</span>}<span><span>-</span>-</span>\mathrm{<span><span>1</span>1</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&Psi;</span>\&Psi;</span>}{\mathrm{<span><span>A</span>A</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&Omega;</span>\&Omega;</span>}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>6</span>6</span>}<span><span>)</span>)</span>$

其中S表示状态变量，i表示行人标记，Ω表示状态模型误差；where S represents the state variable, i represents the pedestrian marker, and Ω represents the state model error;

${\mathrm{<span><span>S</span>S</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>\left(\begin{array}{c}{\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>v</span>v</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>v</span>v</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>v</span>v</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>v</span>v</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\end{array}\right)$${\mathrm{<span><span>A</span>A</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>\begin{array}{c}\left(\begin{array}{c}{\mathrm{<span><span>a</span>a</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>a</span>a</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>a</span>a</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>a</span>a</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\end{array}\right)\end{array}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>7</span>7</span>}<span><span>)</span>)</span>$

用Φ表示状态转移矩阵，Ψ表示加速度向速度和位移量转换的转换矩阵，当雷达扫描频率为37.5Hz时，T为0.026秒；则有：Use Φ to represent the state transition matrix, and Ψ to represent the conversion matrix from acceleration to velocity and displacement. When the radar scanning frequency is 37.5Hz, T is 0.026 seconds; then:

$\mathrm{<span><span>\&Phi;</span>\&Phi;</span>}<span><span>=</span>=</span>\left(\begin{array}{cccccccc}\mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}\end{array}\right)<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>8</span>8</span>}<span><span>)</span>)</span>$

$\mathrm{<span><span>\&Psi;</span>\&Psi;</span>}<span><span>=</span>=</span>\left(\begin{array}{cccc}\mathrm{<span><span>0.5</span>0.5</span>}{\mathrm{<span><span>T</span>T</span>}}^{\mathrm{<span><span>2</span>2</span>}}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0.5</span>0.5</span>}{\mathrm{<span><span>T</span>T</span>}}^{\mathrm{<span><span>2</span>2</span>}}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0.5</span>0.5</span>}{\mathrm{<span><span>T</span>T</span>}}^{\mathrm{<span><span>2</span>2</span>}}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0.5</span>0.5</span>}{\mathrm{<span><span>T</span>T</span>}}^{\mathrm{<span><span>2</span>2</span>}}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}\end{array}\right)<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>9</span>9</span>}<span><span>)</span>)</span>$

系统观测方程如下：The system observation equation is as follows:

${\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>HS</span>HS</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&zeta;</span>\&zeta;</span>}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>10</span>10</span>}<span><span>)</span>)</span>$

其中M为观测向量，H为S到M的转换矩阵，ζ为观测误差；Where M is the observation vector, H is the conversion matrix from S to M, and ζ is the observation error;

$\mathrm{<span><span>H</span>h</span>}<span><span>=</span>=</span>\left(\begin{array}{cccccccc}\mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\end{array}\right)<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>11</span>11</span>}<span><span>)</span>)</span>$

${\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>\begin{array}{c}\left(\begin{array}{c}{\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\end{array}\right)\end{array}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>12</span>12</span>}<span><span>)</span>)</span>$

方程(10)和方程(11)为卡尔曼滤波的状态方程和观测方程；采用标准卡尔曼滤波技术来对状态向量进行连续实时最优估计，获得每个采样时刻下各个行人目标的估计位置和方差；Equation (10) and equation (11) are the state equation and observation equation of the Kalman filter; the standard Kalman filter technique is used to perform continuous real-time optimal estimation of the state vector, and the estimated position and variance;

3)速度和加速度的更新3) Update of speed and acceleration

定义摆动脚从开始摆动到转换为支撑脚为一个周期；在一个周期内，摆动脚的速度和支撑脚的速度由卡尔曼滤波自动更新，摆动脚的加速度则每过一个周期更新一次，以上个周期内的平均加速度值更新，作为下个周期的加速度输入量，方向始终由摆动脚指向支撑脚；假设周期内摆动脚运动的距离为s，时间为t，则加速度更新公式如下：Define a cycle from the start of the swinging foot to the transition to the supporting foot; within a cycle, the speed of the swinging foot and the speed of the supporting foot are automatically updated by the Kalman filter, and the acceleration of the swinging foot is updated every cycle. The average acceleration value in a cycle is updated as the acceleration input in the next cycle, and the direction is always from the swinging foot to the supporting foot; assuming that the swinging foot moves within a cycle with a distance of s and a time of t, the acceleration update formula is as follows:

$\mathrm{<span><span>a</span>a</span>}<span><span>=</span>=</span>\frac{\mathrm{<span><span>4</span>4</span>}\mathrm{<span><span>s</span>the\; s</span>}<span><span>-</span>-</span>\mathrm{<span><span>4</span>4</span>}\mathrm{<span><span>vt</span>vt</span>}}{{\mathrm{<span><span>t</span>t</span>}}^{\mathrm{<span><span>2</span>2</span>}}}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>13</span>13</span>}<span><span>)</span>)</span>$

本发明的具有的有益效果在于：The beneficial effect that the present invention has is:

本发明以激光雷达为传感器，克服了图像传感器的跟踪方法中存在的易受光照变化影响的缺点，可靠性得到很大提高；不但跟踪范围大，算法速度快，跟踪数量多，而且能兼容行人的各种运动模式，包括站立、步行、慢跑，能十分有效地检测出行人的位置并实时跟踪行人的运动轨迹。本发明适用于场景中的主要运动目标为行人的场合。The invention uses laser radar as the sensor, which overcomes the shortcomings of the image sensor tracking method that is easily affected by light changes, and greatly improves the reliability; not only the tracking range is large, the algorithm speed is fast, the number of tracking is large, and it is compatible with pedestrians Various motion modes, including standing, walking, and jogging, can effectively detect the position of pedestrians and track the trajectory of pedestrians in real time. The present invention is applicable to occasions where the main moving object in the scene is pedestrians.

附图说明Description of drawings

图1是雷达系统布置示意图。Figure 1 is a schematic diagram of the radar system layout.

图2是行人运动模型示意图。Figure 2 is a schematic diagram of a pedestrian motion model.

图3是行人目标跟踪算法流程图。Figure 3 is a flow chart of the pedestrian target tracking algorithm.

具体实施方式Detailed ways

以下结合附图和实施实例对本发明作进一步的说明。The present invention will be further described below in conjunction with the accompanying drawings and implementation examples.

本发明的实施方案主要包括以下步骤：Embodiments of the present invention mainly include the following steps:

(1)雷达位置的设置和系统标定；(1) Radar position setting and system calibration;

(2)数据采集和融合；(2) Data collection and fusion;

(3)运动点的聚类和脚步点的配对；(3) Clustering of movement points and pairing of footstep points;

(4)基于人体模型和卡尔曼滤波的目标跟踪。(4) Target tracking based on human body model and Kalman filter.

所述步骤(1)雷达位置的设置和系统标定：即将多个雷达设置在场景的不同位置，如图1所示，通过网络集线器将不同雷达上采集的数据传输到同一台计算机上，进行数据处理。设置位置的选择原则是：(a)使目标在场景中的任何位置都不容易被遮挡；(b)在场景边缘均匀布置激光雷达，以提高目标被检测到的可能性，雷达扫描平面距离地面高度23～25cm，此高度保证雷达在大多数情况下都能检测并获得行人两只脚的距离数据，该高度过高则激光容易被行人所携带的物体遮挡，过低则无法稳定获得跑步状态下的脚步距离数据。对多个雷达的标定是通过如下方法实现的：在场景中设置一些棒状物体，设从N个雷达中得到的第i个棒状物体的坐标为：The setting of described step (1) radar position and system calibration: be about to a plurality of radars are set in the different positions of scene, as shown in Figure 1, the data collected on different radars is transmitted to the same computer by network hub, carry out data deal with. The selection principle for setting the position is: (a) make the target not easily occluded at any position in the scene; (b) uniformly arrange the lidar at the edge of the scene to improve the possibility of the target being detected, and the radar scanning plane is farther from the ground The height is 23-25cm. This height ensures that the radar can detect and obtain the distance data between the two feet of pedestrians in most cases. If the height is too high, the laser will be easily blocked by objects carried by pedestrians. If the height is too low, the running state cannot be obtained stably. Step distance data under. The calibration of multiple radars is achieved by the following method: set some rod-shaped objects in the scene, and set the coordinates of the i-th rod-shaped object obtained from N radars as:

$<span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>1</span>1</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>1</span>1</span>}}<span><span>)</span>)</span><span><span>,</span>,</span><span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>2</span>2</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>2</span>2</span>}}<span><span>)</span>)</span><span><span>,</span>,</span><span><span>.</span>.</span><span><span>.</span>.</span><span><span>.</span>.</span><span><span>,</span>,</span><span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>N</span>N</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>N</span>N</span>}}<span><span>)</span>)</span><span><span>;</span>;</span>$

并假设以第1个雷达的坐标为参照坐标，在二维平面上设第n个雷达坐标到第1个雷达坐标的转移矩阵R_n和平移量T_n为：And assuming that the coordinates of the first radar are used as the reference coordinates, the transfer matrix R_n and the translation amount T_n from the coordinates of the nth radar to the coordinates of the first radar are set on the two-dimensional plane as:

${\mathrm{<span><span>R</span>R</span>}}_{\mathrm{<span><span>n</span>no</span>}}<span><span>=</span>=</span>\left[\begin{array}{cc}{\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>11</span>11</span>}}^{\mathrm{<span><span>n</span>no</span>}}& {\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>12</span>12</span>}}^{\mathrm{<span><span>n</span>no</span>}}\\ {\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>12</span>12</span>}}^{\mathrm{<span><span>n</span>no</span>}}& {\mathrm{<span><span>r</span>r</span>}}_{\mathrm{<span><span>22</span>twenty\; two</span>}}^{\mathrm{<span><span>n</span>no</span>}}\end{array}\right]$${\mathrm{<span><span>T</span>T</span>}}_{\mathrm{<span><span>n</span>no</span>}}<span><span>=</span>=</span>\left[\begin{array}{cc}{\mathrm{<span><span>t</span>t</span>}}_{\mathrm{<span><span>1</span>1</span>}}^{\mathrm{<span><span>n</span>no</span>}}& {\mathrm{<span><span>t</span>t</span>}}_{\mathrm{<span><span>2</span>2</span>}}^{\mathrm{<span><span>n</span>no</span>}}\end{array}\right]$

则得到第i个点从第n个雷达坐标到第1个雷达坐标的转换方程：Then the transformation equation of the i-th point from the n-th radar coordinate to the first radar coordinate is obtained:

$<span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>1</span>1</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>1</span>1</span>}}<span><span>)</span>)</span><span><span>=</span>=</span>{\mathrm{<span><span>R</span>R</span>}}_{\mathrm{<span><span>n</span>no</span>}}<span><span>(</span>(</span>{\mathrm{<span><span>x</span>x</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>n</span>no</span>}}<span><span>,</span>,</span>{\mathrm{<span><span>y</span>the\; y</span>}}_{\mathrm{<span><span>i</span>i</span>}}^{\mathrm{<span><span>n</span>no</span>}}<span><span>)</span>)</span><span><span>+</span>+</span>{\mathrm{<span><span>T</span>T</span>}}_{\mathrm{<span><span>n</span>no</span>}}$

当i＞3时转换矩阵和平移量唯一确定，通过标定将不同雷达坐标系下得到的数据转换到同一个雷达坐标下，式中：x表示点的横坐标值，y表示点的纵坐标值。When i>3, the conversion matrix and the translation amount are uniquely determined, and the data obtained in different radar coordinate systems are converted to the same radar coordinate through calibration. In the formula: x represents the abscissa value of the point, and y represents the ordinate value of the point .

所述步骤(2)数据采集和融合：即通过网络集线器将多个雷达采集的数据传输到一个计算机主机上，同时记录采集的时间，如图1所示；多个雷达之间的数据采集由统一的外同步触发。实验开始时，通过采集50帧以上帧数的数据先自动提取出每个雷达的背景数据，背景数据的提取方法采用了直方图法，即统计所采集帧中每个方向上数据点的距离信息，并做直方图统计，出现频率最高的即为该方向上的背景点数据，保存该背景数据，然后继续实验，将同一时间采集到的各个方向上的距离数据与背景数据比较，若距离差值小于所设定的阈值量则认为是背景点而去除，剩下的即是场景中的前景点即运动点；将同一时刻各个雷达获得的前景点通过标定参数R和T进行坐标转换，即得到在统一全局坐标系下该时刻采集到的融合后的运动数据点。Described step (2) data collection and fusion: promptly by network hub, the data that a plurality of radars collects is transmitted on a computer host computer, record the time of collection simultaneously, as shown in Figure 1; Data collection between a plurality of radars is by Unified external synchronous triggering. At the beginning of the experiment, the background data of each radar was automatically extracted by collecting more than 50 frames of data. The background data extraction method used the histogram method, that is, the distance information of the data points in each direction in the collected frames was counted. , and do histogram statistics, the background point data with the highest frequency is the background point data in this direction, save the background data, and then continue the experiment, compare the distance data collected at the same time in each direction with the background data, if the distance is poor If the value is less than the set threshold value, it is considered as the background point and removed, and the rest is the foreground point in the scene, that is, the moving point; the coordinate conversion of the foreground points obtained by each radar at the same time is carried out through the calibration parameters R and T, namely The fused motion data points collected at this moment in the unified global coordinate system are obtained.

所述步骤(3)运动点的聚类和脚步点的配对：即对融合得到的运动数据点，将相互距离小于第一距离阈值的点进行聚类，并用类内点的中心作为候选脚步的位置，如图1所示，雷达在一个脚上扫描得到的点往往不止一个，这些点通过聚类的方法找出中心点以代表所扫描的脚步。然后对脚步候选点进行时域和空域上的匹配，时域匹配是指以当前帧中的候选点为搜索中心搜索前帧中的候选点；空域匹配即在当前帧中搜索可以匹配的候选点，以确定某两个候选脚步点是否属于同一个行人，有两种情况：(a)在卡尔曼滤波获得的已有目标的两个脚步点预测位置附近搜索匹配，若本帧中的两个候选脚步点分别和两个预测位置距离小于第二距离阈值，则判断两个候选脚步点为已有目标的观测点；(b)若当前候选脚步点未能在已有目标中找到匹配，则属于新目标或者噪声干扰点；然后在当前帧中用第三距离阈值搜索未匹配的脚步候选点，若找到则将找到的匹配点联合一起成为属于同一个新目标的两个脚步点。The pairing of the clustering of described step (3) motion point and footstep point: promptly to the motion data point that fusion obtains, the point that mutual distance is less than the first distance threshold value is clustered, and uses the center of the point in the class as the candidate footstep Position, as shown in Figure 1, the radar often scans more than one point on a foot, and these points are clustered to find the center point to represent the scanned footsteps. Then match the footstep candidate points in time domain and space domain. Time domain matching refers to searching for candidate points in the previous frame with the candidate points in the current frame as the search center; spatial domain matching means searching for matching candidate points in the current frame. , to determine whether a certain two candidate footsteps belong to the same pedestrian, there are two situations: (a) search for matches near the predicted positions of two footsteps of existing targets obtained by Kalman filtering, if two If the distance between the candidate footstep points and the two predicted positions is less than the second distance threshold, then it is judged that the two candidate footstep points are the observation points of the existing target; (b) if the current candidate footstep point fails to find a match in the existing target, then Belong to a new target or noise interference point; then use the third distance threshold to search for unmatched footstep candidate points in the current frame, and if found, combine the found matching points into two footstep points belonging to the same new target.

所述步骤(4)基于人体运动模型和卡尔曼滤波的目标跟踪：即对行人建立运动模型，并根据这个模型采用卡尔曼滤波进行目标跟踪：Described step (4) is based on the target tracking of human body motion model and Kalman filtering: promptly set up motion model to pedestrian, and adopt Kalman filtering to carry out target tracking according to this model:

1)行人模型的建立1) Establishment of pedestrian model

研究表明，行人的状态分为：站立、步行、慢跑、快跑，慢跑是指跑步速度小于3m/秒的跑步状态，由于快跑(跑步速度大于3m/秒的跑步)时情况极为复杂，较难建模，这里暂不考虑；行人在步行和慢跑的运动过程中总是有一个支撑脚，一个摆动脚，并且两者周期性互换，所不同的是步行时支撑脚总是不离地，并且处于静止状态；而慢跑时支撑脚会离地并且会在周期内有运动距离，基于以上提到的站立、步行、慢跑三种行人状态的建模分别如下：Studies have shown that the states of pedestrians are divided into: standing, walking, jogging, and fast running. Jogging refers to the running state with a running speed of less than 3m/s. Since the situation of fast running (running with a running speed greater than 3m/s) is extremely complicated, it is relatively difficult to Difficult to model, not considered here; pedestrians always have a supporting foot and a swinging foot during walking and jogging, and the two are periodically exchanged, the difference is that the supporting foot always stays on the ground when walking, And it is in a static state; while jogging, the supporting foot will be off the ground and will have a movement distance in the cycle. Based on the above-mentioned three pedestrian states of standing, walking, and jogging, the modeling is as follows:

站立模式：没有生理作用力的驱动，左右脚的速度为零，加速度也为零；force＝0，v_L，k＝0，v_R，k＝0，a_L，k＝0，a_R，k＝0，(1)Standing mode: driven without physiological force, the speed of the left and right feet is zero, and the acceleration is also zero; force=0, v_{L, k} = 0, v_{R, k} = 0, a_{L, k} = 0, a_{R, k} = 0, (1)

步行和慢跑运动模式：支撑脚具有一个恒定不变的速度，摆动脚具有一个恒定不变的生理作用力，分别在前半周期(摆动脚靠近支撑脚的过程)和后半周期(摆动脚远离支撑脚的过程)产生一个正向和负向的大小恒定不变的生理作用力，从而对运动脚形成加速度；Walking and jogging motion patterns: the supporting foot has a constant speed, and the swinging foot has a constant physiological force, respectively in the first half cycle (the process of swinging the foot close to the supporting foot) and the second half cycle (swinging the foot away from the support The process of the foot) produces a positive and negative physiological force with a constant magnitude, thereby forming an acceleration on the moving foot;

以T为两帧之间的时间间隔，右脚为支撑脚的前半周期内：Take T as the time interval between two frames, and the right foot as the first half cycle of the supporting foot:

force＝F，v_L，k＝v_L，k-1+A*t，v_R，k＝V，a_L，k＝A，a_R，k＝0，(2)force=F, v_{L, k} = v_{L, k-1} + A*t, v_{R, k} = V, a_{L, k} = A, a_{R, k} = 0, (2)

右脚为支撑脚的后半周期内：During the second half of the cycle with the right foot as the supporting foot:

force＝-F，v_L，k＝v_L，k-1+A*t，v_R，k＝V，a_L，k＝-A，a_R，k＝0，(3)force=-F, v_{L, k} = v_{L, k-1} + A*t, v_{R, k} = V, a_{L, k} = -A, a_{R, k} = 0, (3)

左脚为支撑脚的前半周期内：During the first half of the cycle with the left foot as the supporting foot:

force＝F，v_L，k＝V，v_R，k＝v_R，k-1+A*t a_L，k＝0，a_R，k＝A，(4)force=F, v_{L, k} = V, v_{R, k} = v_{R, k-1} +A*t a_{L, k} = 0, a_{R, k} = A, (4)

左脚为支撑脚的后半周期内：In the second half of the cycle with the left foot as the supporting foot:

force＝-F，v_L，k＝V，v_R，k＝v_R，k-1+A*t a_L，k＝0，a_R，k＝-A，(5)force=-F, v_{L, k} = V, v_{R, k} = v_{R, k-1} +A*t a_{L, k} = 0, a_{R, k} =-A, (5)

式中force表示生理作用力，F表示行人实际产生的生理作用力，v表示运动速度，V表示行人F作用力下产生的速度，a表示加速度，A表示行人在F作用力下产生的加速度，L表示左脚，R表示右脚，k表示第k个时刻；In the formula, force represents the physiological force, F represents the actual physiological force of the pedestrian, v represents the speed of movement, V represents the speed of the pedestrian under the force of F, a represents the acceleration, A represents the acceleration of the pedestrian under the force of F, L represents the left foot, R represents the right foot, and k represents the kth moment;

当V＝0时，行人即处在步行状态下，当V＞0时，行人即处在慢跑状态下；如图2所示，运动过程中加速度标量值有明显的周期恒定地变化，加速度的方向则始终由摆动脚指向支撑脚，而支撑脚的速度则是匀速递增或匀速递减。When V=0, the pedestrian is in the walking state; when V>0, the pedestrian is in the jogging state; The direction of the swinging foot is always directed from the swinging foot to the supporting foot, and the speed of the supporting foot is increasing or decreasing at a constant speed.

2)卡尔曼滤波跟踪2) Kalman filter tracking

把行人的位置P和速度V当作状态变量，加速度A当作系统的输入量，则得到系统的状态方程为：Taking the pedestrian's position P and velocity V as the state variables, and the acceleration A as the input of the system, the state equation of the system is obtained as:

${\mathrm{<span><span>S</span>S</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>\&Phi;S</span>\&Phi;S</span>}}_{\mathrm{<span><span>k</span>k</span>}<span><span>-</span>-</span>\mathrm{<span><span>1</span>1</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>+</span>+</span>{\mathrm{<span><span>\&Psi;A</span>\&Psi;A</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&Omega;</span>\&Omega;</span>}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>6</span>6</span>}<span><span>)</span>)</span>$

其中S表示状态变量，i表示行人标记，Ω表示状态模型误差；where S represents the state variable, i represents the pedestrian mark, and Ω represents the state model error;

${\mathrm{<span><span>S</span>S</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>\left(\begin{array}{c}{\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>v</span>v</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>v</span>v</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>v</span>v</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>v</span>v</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\end{array}\right)$${\mathrm{<span><span>A</span>A</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>\left(\begin{array}{c}{\mathrm{<span><span>a</span>a</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>a</span>a</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>a</span>a</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>a</span>a</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\end{array}\right)<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>7</span>7</span>}<span><span>)</span>)</span>$

用Φ表示状态转移矩阵，Ψ表示加速度向速度和位移量转换的转换矩阵，当雷达扫描频率为37.5Hz时，T为0.026秒；则有：Use Φ to represent the state transition matrix, and Ψ to represent the conversion matrix from acceleration to velocity and displacement. When the radar scanning frequency is 37.5Hz, T is 0.026 seconds; then:

$\mathrm{<span><span>\&Phi;</span>\&Phi;</span>}<span><span>=</span>=</span>\left(\begin{array}{cccccccc}\mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}\end{array}\right)<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>8</span>8</span>}<span><span>)</span>)</span>$

$\mathrm{<span><span>\&Psi;</span>\&Psi;</span>}<span><span>=</span>=</span>\left(\begin{array}{cccc}\mathrm{<span><span>0.5</span>0.5</span>}{\mathrm{<span><span>T</span>T</span>}}^{\mathrm{<span><span>2</span>2</span>}}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0.5</span>0.5</span>}{\mathrm{<span><span>T</span>T</span>}}^{\mathrm{<span><span>2</span>2</span>}}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}<span><span>.</span>.</span>\mathrm{<span><span>5</span>5</span>}{\mathrm{<span><span>T</span>T</span>}}^{\mathrm{<span><span>2</span>2</span>}}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0.5</span>0.5</span>}{\mathrm{<span><span>T</span>T</span>}}^{\mathrm{<span><span>2</span>2</span>}}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>T</span>T</span>}\end{array}\right)<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>9</span>9</span>}<span><span>)</span>)</span>$

系统观测方程如下：The system observation equation is as follows:

${\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>{\mathrm{<span><span>HS</span>HS</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>+</span>+</span>\mathrm{<span><span>\&zeta;</span>\&zeta;</span>}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>10</span>10</span>}<span><span>)</span>)</span>$

其中M为观测向量，H为S到M的转换矩阵，ζ为观测误差；Where M is the observation vector, H is the conversion matrix from S to M, and ζ is the observation error;

$\mathrm{<span><span>H</span>h</span>}<span><span>=</span>=</span>\begin{array}{}\end{array}\left(\begin{array}{cccccccc}\mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\\ \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>1</span>1</span>}& \mathrm{<span><span>0</span>0</span>}& \mathrm{<span><span>0</span>0</span>}\end{array}\right)<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>11</span>11</span>}<span><span>)</span>)</span>$

${\mathrm{<span><span>M</span>m</span>}}_{\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}<span><span>=</span>=</span>\left(\begin{array}{c}{\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>R</span>R</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>x</span>x</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\\ {\mathrm{<span><span>p</span>p</span>}}_{\mathrm{<span><span>L</span>L</span>}<span><span>\_</span>\_</span>\mathrm{<span><span>y</span>the\; y</span>}<span><span>,</span>,</span>\mathrm{<span><span>k</span>k</span>}}^{\mathrm{<span><span>i</span>i</span>}}\end{array}\right)<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>12</span>12</span>}<span><span>)</span>)</span>$

方程(10)和方程(11)为卡尔曼滤波的状态方程和观测方程；采用标准卡尔曼滤波技术来对状态向量进行连续实时最优估计，获得每个采样时刻下各个行人目标的估计位置和方差；Equation (10) and equation (11) are the state equation and observation equation of the Kalman filter; the standard Kalman filter technology is used to perform continuous real-time optimal estimation of the state vector, and the estimated position and variance;

3)速度和加速度的更新3) Update of speed and acceleration

定义摆动脚从开始摆动到转换为支撑脚为一个周期；在一个周期内，摆动脚的速度和支撑脚的速度由卡尔曼滤波自动更新，摆动脚的加速度则每过一个周期更新一次，以上个周期内的平均加速度值更新，作为下个周期的加速度输入量，方向始终由摆动脚指向支撑脚；假设周期内摆动脚运动的距离为s，时间为t，则加速度更新公式如下：Define a cycle from the start of the swinging foot to the transition to the supporting foot; within a cycle, the speed of the swinging foot and the speed of the supporting foot are automatically updated by the Kalman filter, and the acceleration of the swinging foot is updated every cycle. The average acceleration value in a cycle is updated as the acceleration input in the next cycle, and the direction is always from the swinging foot to the supporting foot; assuming that the swinging foot moves within a cycle with a distance of s and a time of t, the acceleration update formula is as follows:

$\mathrm{<span><span>a</span>a</span>}<span><span>=</span>=</span>\frac{\mathrm{<span><span>4</span>4</span>}\mathrm{<span><span>s</span>the\; s</span>}<span><span>-</span>-</span>\mathrm{<span><span>4</span>4</span>}\mathrm{<span><span>vt</span>vt</span>}}{{\mathrm{<span><span>t</span>t</span>}}^{\mathrm{<span><span>2</span>2</span>}}}<span><span>-</span>-</span><span><span>-</span>-</span><span><span>-</span>-</span><span><span>(</span>(</span>\mathrm{<span><span>13</span>13</span>}<span><span>)</span>)</span>$

如图3所示，当每一帧数据经过融合和聚类以后，便开始对已有跟踪目标(记录在变量中)进行模型预测，首先以已有跟踪目标的预测位置为搜索中心在当前帧以一定阈值范围搜索本帧中的脚步候选点作为观测点，如果搜索成功即找到了跟踪目标在本帧中的观测点，则进行滤波，得到滤波结果作为跟踪目标的当前位置，同时更新速度、加速度(如果一个周期刚好结束)；如果跟踪失败即没有找到脚步候选点作为观测点，则将模型预测结果作为跟踪目标的当前位置，并将此类跟踪目标标记为失跟踪，进行计数，当失跟踪帧数达到20帧时，我们认为此跟踪目标跟踪失败或者已经离开目标场所，停止跟踪。对于没有被识别为已有跟踪目标的观测点的脚步候选点，我们认为其可能是新的行人或者是噪声，在不能立刻确定其真实性的情况下我们采用简单的跟踪手段，对这些点进行一定帧数内的跟踪。由于雷达扫描频率高，相邻点之间的运动距离较小，所以只要以候选点为中心设定一个合适的阈值搜索每一时刻的同类点就可以找到新的候选点，如果没找到，则很可能这个候选点只是噪声，如果找到了则继续跟踪，确定其为真实的行人后进行卡尔曼滤波跟踪，如此重复以上的步骤，直到所有的跟踪目标都离开目标场所，本实验结束。As shown in Figure 3, after each frame of data is fused and clustered, the model prediction of the existing tracking target (recorded in the variable) is started. First, the predicted position of the existing tracking target is used as the search center in the current frame Use a certain threshold range to search for the footstep candidate points in this frame as observation points. If the search is successful, you will find the observation point of the tracking target in this frame, then perform filtering, and obtain the filtering result as the current position of the tracking target. At the same time, update the speed, Acceleration (if a period is just over); if the tracking fails, that is, no footstep candidate points are found as observation points, the model prediction result is used as the current position of the tracking target, and such tracking targets are marked as out of track, and counted. When the number of tracking frames reaches 20 frames, we consider that the tracking target has failed to track or has left the target site, and stop tracking. For the footstep candidate points that have not been identified as the observation points of the existing tracking targets, we think that they may be new pedestrians or noises. If the authenticity cannot be determined immediately, we use simple tracking methods to track these points. Tracking within a certain number of frames. Due to the high scanning frequency of the radar, the movement distance between adjacent points is small, so as long as a suitable threshold is set centering on the candidate point to search for similar points at each moment, a new candidate point can be found. If not found, then It is very likely that this candidate point is just noise. If it is found, continue to track. After confirming that it is a real pedestrian, perform Kalman filter tracking. Repeat the above steps until all tracking targets leave the target place, and the experiment ends.

Claims (5)

1. the pedestrian's method for tracking target based on multilasered optical radar is characterized in that, may further comprise the steps:

(1) setting of radar site and system calibrating;

(2) data acquisition and fusion;

(3) pairing of the cluster in motor point and step point;

(4) based on the target following of manikin and Kalman filtering.

2. a kind of pedestrian's method for tracking target according to claim 1 based on multilasered optical radar, it is characterized in that, the setting and the system calibrating of described step (1) radar site: be about to the diverse location that a plurality of radars are arranged on scene, the selection principle that the position is set is: (a) make any position of target in scene all be not easy to be blocked; (b) evenly arrange laser radar at the scene edge, with the possibility that the raising target is detected, radar scanning plan range floor level 23～25cm, this guarantees that highly radar in most of the cases can both detect and obtain the range data of two pin of pedestrian; Demarcation to a plurality of radars realizes by the following method: some bar-shaped objects are set in scene, and the coordinate of establishing i the bar-shaped object that obtains from N radar is:

$(x_i^1,y_i^1),(x_i^2,y_i^2),...,(x_i^N,y_i^N);$

And suppose that the coordinate with the 1st radar is a reference coordinates, on two dimensional surface, establish the transition matrix R of 1 radar fix of a n radar fix to the_nWith translational movement T_nFor:

$R_n=\left[\begin{array}{cc}{\mathrm{\&gamma;}}_{11}^n& {\mathrm{\&gamma;}}_{12}^n\\ {\mathrm{\&gamma;}}_{12}^n& {\mathrm{\&gamma;}}_{22}^n\end{array}\right]$$T_n=\left[t_1^n{t}_2^n\right]$

Then obtain i o'clock transfer equation from 1 radar fix of a n radar fix to the:

$(x_i^1,y_i^1)=R_n(x_i^n,y_i^n)+T_n$

Transition matrix and translational movement are unique when i＞3 determines, by demarcating data-switching that different radar fixs system is obtained down under same radar fix, in the formula: x represents abscissa value a little, and y represents ordinate value a little.

3. a kind of pedestrian's method for tracking target according to claim 1 based on multilasered optical radar, it is characterized in that, (2) data acquisition of described step and fusion: promptly by on data transmission to a main frame of hub with a plurality of radars collections, the time of while record acquisition; Data acquisition between a plurality of radars is by unified outer synchronous triggering, automatically extract earlier the background data of each radar by the data of gathering the above frame number of 50 frames, the extracting method of background data has adopted histogram method, promptly add up in institute's acquisition frame the range information of data point on each direction, and do statistics with histogram, the frequency of occurrences is the highest is background dot data on this direction, preserve this background data, range data on all directions that the same time is collected and background data are relatively then, if then think background dot less than the threshold quantity that sets and remove apart from difference, the foreground point that remaining promptly is in the scene is the motor point; Coordinate conversion is carried out by calibrating parameters in the foreground point that each radar of synchronization obtains, promptly obtain the exercise data point after this fusion that collects constantly under the unified global coordinate system.

4. a kind of pedestrian's method for tracking target according to claim 1 based on multilasered optical radar, it is characterized in that, the cluster in described step (3) motor point and the pairing of step point: i.e. the exercise data point that fusion is obtained, the phase mutual edge distance is carried out cluster less than the point of first distance threshold, and with position, the center of point in the class as candidate's step, then the step candidate point is carried out coupling on time domain and the spatial domain, to determine whether certain two candidate's step point belongs to same pedestrian, two kinds of situations are arranged: near (a) search matched two step point prediction positions of the existing target that Kalman filtering obtains, if two candidate's step points in this frame respectively with two predicted positions less than the second distance threshold value, judge that then two candidate's step points are the observation station that has target; (b) if current candidate's step point fails to find coupling in existing target, then belong to fresh target or noise point; The step candidate point that does not mate with the search of the 3rd distance threshold in present frame becomes two step points that belong to same fresh target together if find then the match point that finds to be united then.

5. a kind of pedestrian's method for tracking target according to claim 1 based on multilasered optical radar, it is characterized in that, described step (4) is based on the target following of modelling of human body motion and Kalman filtering: promptly the pedestrian has been set up motion model, and adopted Kalman filtering to carry out target following according to this model:

5.1) foundation of pedestrian dummy

Studies show that pedestrian's state is divided into: stand, walking, jog, the pedestrian always has a feet in the walking and the motion process of jogging, a swing pin, and both periodically exchange, and feet was always not liftoff when different was walking, and remained static; And feet can be liftoff and move distance can be arranged in the cycle when jogging, based on above-mentionedly stand, the modeling of walking, the three kinds of pedestrian's states of jogging is as follows respectively:

The pattern of standing: do not have the driving of physiological action power, the speed of left and right sides pin is zero, and acceleration also is zero; Force=0, v_{L, k}=0, v_{R, k}=0, a_{L, k}=0, a_{R, k}=0, (1)

Walking and jogging pattern: feet has an invariable speed, the swing pin has an invariable physiological action power, produce the constant physiological action power of constant magnitude of a forward and negative sense in preceding semiperiod and later half cycle respectively, thereby the motion pin is formed acceleration;

With T is time interval between two frames, and right crus of diaphragm is in the preceding semiperiod of feet:

force＝F，v_L，k＝v_L，k-1+A*t，v_R，k＝V，a_L，k＝A，a_R，k＝0，(2)

Right crus of diaphragm is in the later half cycle of feet:

force＝-F，v_L，k＝v_L，k-1+A*t，v_R，k＝V，a_L，k＝-A，a_R，k＝0，(3)

Left foot is in the preceding semiperiod of feet:

force＝F，v_L，k＝V，v_R，k＝v_R，k-1+A*t?a_L，k＝0，a_R，k＝A，(4)

Left foot is in the later half cycle of feet:

force＝-F，v_L，k＝V，v_R，k＝v_R，k-1+A*t?a_L，k＝0，a_R，k＝-A，(5)

Force represents physiological action power in the formula, and F represents the physiological action power of the actual generation of pedestrian, and v represents movement velocity, V represents the speed of generation under the pedestrian F acting force, and a represents acceleration, and A represents the acceleration that the pedestrian produces under the F acting force, L represents left foot, and R represents right crus of diaphragm, and k represents k constantly;

When V=0, the pedestrian promptly is under the walking state, and when V＞0, the pedestrian promptly is under the state of jogging;

5.2) the Kalman filtering tracking

Pedestrian's position P and speed V are used as state variable, and acceleration A is used as the input quantity of system, and the state equation that then obtains system is:

$S_k^i=\mathrm{\&Phi;}{S}_{k-1}^i+\mathrm{\&Psi;}{A}_k^i+\mathrm{\&Omega;}---\left(6\right)$

Wherein S represents state variable, and i represents pedestrian's mark, and Ω represents the state model error;

$S_k^i=\left(\begin{array}{c}{p}_{R\_x,k}^i\\ p_{R\_y,k}^i\\ v_{R\_x,k}^i\\ v_{R\_y,k}^i\\ p_{L\_x,k}^i\\ p_{L\_y,k}^i\\ v_{L\_x,k}^i\\ v_{L\_y,k}^i\end{array}\right)$$A_k^i=\left(\begin{array}{c}{a}_{R\_x,k}^i\\ a_{R\_y,k}^i\\ a_{L\_x,k}^i\\ a_{L\_y,k}^i\end{array}\right)---\left(7\right)$

Represent state-transition matrix with Φ, Ψ represents the transition matrix of acceleration to speed and displacement conversion, and when the radar scanning frequency was 37.5Hz, T was 0.026 second; Then have:

$\mathrm{\&Phi;}=\left(\begin{array}{cccccccc}1& 0& T& 0& 0& 0& 0& 0\\ 0& 1& 0& T& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& T& 0\\ 0& 0& 0& 0& 0& 1& 0& T\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right)\text{---}\left(8\right)$

$\mathrm{\&Psi;}=\left(\begin{array}{cccc}0.5T^2& 0& 0& 0\\ 0& 0.5T^2& 0& 0\\ T& 0& 0& 0\\ 0& T& 0& 0\\ 0& 0& 0.5T^2& 0\\ 0& 0& 0& 0.5T^2\\ 0& 0& T& 0\\ 0& 0& 0& T\end{array}\right)---\left(9\right)$

The systematic observation equation is as follows:

$M_k^i={\mathrm{HS}}_k^i+\mathrm{\&zeta;}---\left(10\right)$

Wherein M is an observation vector, and H is the transition matrix of S to M, and ζ is an observational error;

$H=\left(\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\end{array}\right)---\left(11\right)$

$M_k^i=\left(\begin{array}{c}{p}_{R\_x,k}^i\\ p_{R\_y,k}^i\\ p_{L\_x,k}^i\\ p_{L\_y,k}^i\end{array}\right)---\left(12\right)$

Equation (10) and equation (11) are the state equation and the observation equation of Kalman filtering; Employing standard Kalman Filter Technology comes state vector is carried out optimal estimation in real time continuously, obtains the estimated position and the variance of each pedestrian's target under each sampling instant;

5.3) renewal of speed and acceleration

To be converted to feet be one-period to definition swing pin from beginning to swing to; In one-period, the speed of swing pin and the speed of feet are upgraded automatically by Kalman filtering, the then every one-period of crossing of the acceleration of swing pin upgrades once, upgrade with the average acceleration value in the last cycle, as the acceleration input quantity in next cycle, direction is all the time by swinging toe to feet; The distance of swing pin motion is s in the assumption period, and the time is t, and then more new formula is as follows for acceleration:

$\frac{4s-4\mathrm{vt}}{t^2}---\left(13\right)$

Images