CN106125087A - Dancing Robot indoor based on laser radar pedestrian tracting method - Google Patents

Abstract

The present invention proposes a kind of Dancing Robot indoor based on laser radar pedestrian tracting method, comprises the steps: (1) laser data pretreatment；(2) laser data based on density cluster；(3) pedestrian based on laser collection point identifies；(4) graphic software platform；(5) pedestrian tracking based on particle filter；(6) pedestrian movement's track is drawn.The pedestrian that the present invention utilizes single laser radar to realize different distance different shape identifies, and can be tracked the pedestrian of the different motion patterns such as static, walking, and following range is wide, and real-time is good, and the accuracy of recognition and tracking is high.

Description

Dancing Robot indoor based on laser radar pedestrian tracting method

Technical field

The invention belongs to robotics, be specifically related to the indoor based on laser radar for Dancing Robot Pedestrian tracting method.

Background technology

Robotics achieves surprising development between decades, and the various occasions of human lives, such as medical treatment, family, meal Shop, hotel etc., with the presence of substituting people or the robot made with indirect labor.Owing to needing to coexist with people in the case of many, because of This robot wants the safety of warrantor while completing self work, and its working level requires higher.Active negotiation robot The hight coordinate work of robot and people can be realized under multisensor, improve the probability coexisted with people.Typical application Such as: dance with people in the case of contacting with people to realize Dancing Robot, robot needs to observe environment around, and And self motor coordination is carried out according to the movement locus of pedestrian, on the premise of ensureing other people safety, complete dance movement.Cause This, the pedestrian in surrounding is identified with follow the tracks of to robot dance during motor coordination have with trajectory planning Significance.

The pedestrian tracting method of robot has a lot, and pedestrian tracking based on video is current most common method, but by In being affected so that utilizing visual information to complete the row of Dancing Robot by camera fields of view scope, light, pedestrian's attitudes vibration People follows the tracks of and becomes abnormal difficult, and especially when people's distance video camera is nearer, video camera can only collect pedestrian topography.Therefore The pedestrian tracking of view-based access control model is used for the long distance pedestrian of outdoor or indoor to be followed the tracks of.Utilize laser radar to realize pedestrian tracking to be Another kind of common methods.Laser radar, is a kind of new type measuring instrument table utilizing laser technology to measure, have precision high, Speed is fast, measure the features such as scope is wide.But pedestrian tracting method typically cost based on laser radar is of a relatively high, and is easily subject to Impact to algorithm makes real-time and accuracy rate the highest.Patent CN102253391A discloses a kind of based on multilasered optical radar Pedestrian target tracking, can detect pedestrian position the movement locus of real-time tracking pedestrian of indoor effectively.But the party The radar of method is in a fixed position, and needs to demarcate multiple radars, and multiple radar cost is high, is not suitable for portable dance Step the pedestrian tracking of robot.

In sum, the subject matter of the indoor pedestrian tracting method existence being applied to Dancing Robot at present is: based on The pedestrian tracting method of video is difficult to meet short-range pedestrian tracking, and pedestrian tracting method based on laser radar need to consider into This, and meet real-time and accuracy requirement, therefore research and develop a low cost, real-time and practicality good, follow the tracks of accuracy rate high Indoor pedestrian tracting method there is the highest using value.

Summary of the invention

It is an object of the invention to overcome the shortcomings and deficiencies of existing indoor pedestrian tracting method, it is provided that a kind of based on swashing The Dancing Robot indoor pedestrian tracting method of optical radar, utilizes single laser radar to realize the pedestrian of different distance different shape Identifying, and can be tracked the pedestrian of the different motion patterns such as static, walking, following range is wide, and real-time is good, identify with The accuracy of track is high.

For reaching above-mentioned purpose, the technical scheme is that and be achieved in that, a kind of dancing machine based on laser radar Device people indoor pedestrian tracting method, comprises the steps:

(1) laser data pretreatment: be connected to the laser radar of robot main frame, collects the thing scanned at certain angle Body is to the range information of radar；Main frame obtains laser initial data, initial data is carried out pretreatment, during preprocess method includes Value filtering and coordinate system conversion；

(2) laser data based on density cluster: the data obtained for step (1) are realized by density clustering Environment is split, and is made a distinction by different objects；

(3) pedestrian based on laser collection point identifies: identifies the every trade people that clicks in the class bunch clustered out, removes non-row People bunch, using pedestrian bunch as the input data of pedestrian tracking；

(4) graphic software platform: the scan data continuously acquired by laser is patterned display, will laser data conversion Become video flowing；Each collection point is done a mapping, and collection point is carried out image amplification, it is simple to more efficient carry out intuitively Follow the tracks of；

(5) pedestrian tracking based on particle filter, including:

(501) initial phase:

After completing pedestrian's identification, using the position of pedestrian bunch as the initial position of pedestrian tracking, first initialize tracking Device, extracts the feature in rectangle frame, and provincial characteristics is described by the distribution of color probability density of target area, and enters particle collection Row initializes；

(502) propagation stage: according to equation of transfer s_t=As_t-1+w_t-1, calculate new particle collection and predict the position of new particle Putting, wherein A is state-transition matrix, w_t-1For Gaussian noise；

(503) decision phase: choose the criterion of the characteristic similarity of particle, the similarity calculated or mark are i.e. For the weight of corresponding particle, calculate the weight of each particle, and be normalized；

(504) in the state estimation stage: according to particle weights, the position of particle is obtained according to weight summation the position of target Output；

(505) in the resampling stage: particle is carried out resampling according to weights of importance, new particle collection is obtained；

(506) step (502) is repeated to step (505), it is achieved the tracking of moving object；

(6) pedestrian movement's track is drawn: each two field picture utilizes particle filter obtain pedestrian position, calculates target's center Position, Bing Jianggai center and former frame target's center do line, obtain pedestrian movement's trajectory diagram.

Further, the method for step (1) described medium filtering is: the collection point of laser radar belongs to two-dimemsional number and it is believed that Number, the data collected are the some distances to laser, enter the range data of the m data point in pending data point neighborhood Row sequence, replaces the distance of current data point by (1+m)/2 distance after sequence.

Further, the method for step (1) described coordinate system conversion is: coordinate system is changed, and initial data is filtered through intermediate value Eliminate part measurement error after ripple, owing to initial data is under polar coordinate system, be converted for the ease of data analysis To rectangular coordinate system, raw data format is:

s_k=d_k, k=0,1 ..., N (1),

Wherein, d_kDistance for kth collection point；

The binary vector group being converted to rectangular coordinate system:

u_k=(x_k,y_k)^T, k=0,1 ..., N_e(2),

Wherein, u_kFor the rectangular coordinate that kth laser collection point is corresponding, x_kWith y_kIt is respectively X-axis and Y-axis coordinate, N_eFor having Effect collection point number；；

If some P is kth collection point, the azimuth that laser radar obtains is θ_k, distance is ρ_k, angular resolution is β, then θ_k =k × β, trying to achieve rectangular coordinate corresponding for P is:

P_x=ρ_k cos(θ_k-45 °) (3),

P_y=ρ_k sin(θ_k-45 °) (4),

Wherein, P_xWith P_yIt is respectively X-axis and Y-axis the coordinate, (θ of some P_k-45 °) for putting the angle of P and X-axis.

Further, in step (2), using DBSCAN algorithm to cluster, key step includes:

(201) unvisited will be labeled as a little；

(202) take the some p of unvisited at random, and be labeled as visited；

(203) judging whether some p are core point, if the number to the some p distance point less than ε is more than MinPts, then p is core Heart point, continues step (4), and otherwise p is noise, returns step (2)；

(204) p is core point, builds a bunch C, and adds p to C, by P_NThe set put in being defined as the ε-neighborhood of a p, continues Continuous step (5)；

(205) for P_NMidpoint q, repeats to walk (6) to P_NFor sky；

(206) if q's is labeled as unvisited, first it is marked as visited, then judges whether q is core Point, if q is core point, adds the point in the ε-neighborhood of q to P_NIf q is not the member of any bunch, then p is added to C；

(207) output bunch C；

(208) repeat to walk (202) to (207) to being labeled as a little visited；

Further, when above-mentioned DBSCAN algorithm carries out core point calculating, for kth collection point P_k, need P_k With remaining all calculating clicking on row distance, time complexity is high, therefore when carrying out core point and calculating, only considers P_kAround Point and P_kDistance, i.e. calculate P_(k-MinPts)To P_(k+MinPts)In the range of point to P_kDistance.

Further, the concrete grammar that step (3) described pedestrian identifies is as follows:

(301) first the point in the class bunch clustered out is scanned, it is thus achieved that the information of each class bunch, adopts including in class bunch Number N of collection point_c, class bunch distance laser radar average distance D_avgAnd the position coordinates at the edge up and down of class bunch；

(302) coarse sizing is carried out according to the size of class bunch；

(303) carrying out fine screening to remaining bunch, first acquisition laser radar, to the distance of this bunch, is denoted as r, makes r =D_avgIf pedestrian's waist width is W_ped, W_pedSpan be W_min<W_ped<W_max；If with r as radius, by W_pedAs arc Long, then there is a following formula:

$W_{ped}=\frac{\mathrm{\&theta;}\mathrm{\&pi;}r}{180}---\left(5\right),$

Calculating the corresponding central angle θ of sector region, formula is as follows:

$\mathrm{\&theta;}=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r}---\left(6\right),$

Angular resolution β according to laser radar utilizes formula (7) to calculate number P of collection point with bunch corresponding central angle θ_c, Computing formula is as follows:

$P_c=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(7\right),$

If the number of the point that pedestrian bunch is corresponding is denoted as P_num, then P_numThe computational methods of scope estimated value as follows:

$\frac{180\&times;W_{min}}{\mathrm{\&pi;}r\mathrm{\&beta;}}<P_{num}<\frac{180\&times;W_{max}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(8\right),$

Bunch judging according to said method after clustering each, removes non-pedestrian bunch, using pedestrian bunch as capable The input data that people follows the tracks of.

Relative to prior art, a kind of Dancing Robot indoor based on laser radar of the present invention pedestrian tracking side Method, mainly has a following advantage:

(1) the inventive method make use of single laser radar to realize indoor pedestrian tracking, compared to pedestrian based on video Tracking expands following range, reduces cost compared to the pedestrian tracting method of multilasered optical radar；

(2) original laser data are clustered by the present invention, in order to promote the speed of cluster, according to laser data collection Characteristic distributions, is optimized original DBSCAN algorithm, is greatly improved cluster speed, improves efficiency of algorithm；

(3) relation that laser radar data collection point quantity is far and near with object is analyzed by the present invention, it is proposed that base In distance and the pedestrian recognition method of pedestrian's health width of class bunch to laser radar, the method recognition speed is fast, and accuracy rate is high；

(4) present invention realizes pedestrian tracking in order to accurate and visual, laser data is carried out image conversion and shows, by dispersion number Strong point is converted to video data and processes, and real-time is good, it is simple to more intuitively realize pedestrian tracking efficiently.

Accompanying drawing explanation

Fig. 1 is present invention Dancing Robot based on laser radar indoor pedestrian tracting method flow charts；

Fig. 2 is coordinate system transition diagram in the inventive method；

Fig. 3 is the pedestrian of diverse location different shape view under laser radar in the inventive method；

Fig. 4 is that the number of collection point in pedestrian bunch in the inventive method estimates schematic diagram；

Fig. 5 (a) is the track path of resting state pedestrian and Actual path comparison diagram in the inventive method；

Fig. 5 (b) is track path and the Actual path comparison diagram of the inventive method linear movement pedestrian；

Fig. 5 (c) is the track path of curvilinear motion pedestrian and Actual path comparison diagram in the inventive method.

Detailed description of the invention

It should be noted that in the case of not conflicting, the embodiment in the present invention and the feature in embodiment can phases Combination mutually.

The present invention is described in detail below in conjunction with embodiment and accompanying drawing.

The present invention installs laser radar at Dancing Robot waist, obtains the range data scanned, and carries out data Pretreatment and cluster analysis, identify by the analysis of class bunch realizes pedestrian, by laser data graphic software platform, and utilize particle Filtering algorithm realizes pedestrian tracking；Fig. 1 is present invention Dancing Robot based on laser radar indoor pedestrian tracting method flow processs Figure, said method comprising the steps of:

The first step, laser data pretreatment:

Laser radar is positioned over the waist location of Dancing Robot, distance ground 1m, selects Hokuyo company of Japan URG laser radar, and it is connected to main frame by USB interface, utilize SCIP2.0 data protocol to communicate, collect is certain The object scanned at individual angle is to the range information of radar；Main frame first passes through transmission protocol instructions and obtains laser original number According to, in order to remove the measurement error of initial data and accelerate later stage arithmetic speed, initial data is carried out pretreatment, pretreatment bag Include medium filtering and coordinate system conversion；

(1) medium filtering:

The collection point of laser radar belongs to 2-D data signal, and the data collected are the some distances to laser, treat place The range data of the m data point in the data point neighborhood of reason is ranked up, and replaces working as by (1+m)/2 distance after sequence The distance of front data point, wherein m takes 5, completes the Data correction of this point；

(2) coordinate system conversion:

Initial data eliminates part measurement error after medium filtering, owing to initial data is under polar coordinate system, Being converted to rectangular coordinate system for the ease of data analysis, raw data format is:

s_k=d_k, k=0,1 ..., N (1),

Wherein, d_kBeing the coordinate system transition diagram in the inventive method for the distance of kth collection point, such as Fig. 2, A is Sweep starting point, C is sweep stopping point, and B is first the effective collection point got；

The binary vector group being converted to rectangular coordinate system:

u_k=(x_k,y_k)^T, k=0,1 ..., N_e(2),

Wherein, u_kFor the rectangular coordinate that kth laser collection point is corresponding, x_kWith y_kIt is respectively X-axis and Y-axis coordinate, N_eFor having Effect collection point number；

If Fig. 2 midpoint P is kth collection point, the azimuth that laser radar obtains is θ_k, distance is ρ_k, angular resolution is β, Then θ_k=k × β, trying to achieve rectangular coordinate corresponding for P is:

P_x=ρ_k cos(θ_k-45 °) (3),

P_y=ρ_k sin(θ_k-45 °) (4),

Wherein, P_xWith P_yIt is respectively X-axis and Y-axis the coordinate, (θ of some P_k-45 °) for putting the angle of P and X-axis；

Second step: laser data based on density clusters:

The data obtained for the first step realize environment segmentation by density clustering, and different objects is carried out district Point；DBSCAN is a kind of density-based algorithms, for finding the dense Region in data set, and be susceptible to noise and The impact of outlier；Density threshold MinPts that input parameter is radius of neighbourhood ε and dense Region of DBSCAN algorithm；If one Including at least MinPts point in the ε-neighborhood of individual point, then the some composition one in this point is referred to as core point, core point and ε-neighborhood Individual bunch, if one bunch is contained within multiple core point, then by bunch merging belonging to these core points, finally clustered knot Really；

DBSCAN algorithm carries out the key step clustered:

(1) unvisited will be labeled as a little；

(2) take the some p of unvisited at random, and be labeled as visited；

(3) judging whether some p are core point, if the number to the some p distance point less than ε is more than MinPts, then p is core Point, continues step (4), and otherwise p is noise, returns step (2)；

(4) p is core point, builds a bunch C, and adds p to C, by P_NThe set put in being defined as the ε-neighborhood of a p, continues Step (5)；

(5) for P_NMidpoint q, repeats to walk (6) to P_NFor sky；

(6) if q's is labeled as unvisited, first it is marked as visited, then judges whether q is core point, If q is core point, the point in the ε-neighborhood of q is added to P_NIf q is not the member of any bunch, then p is added to C；

(7) output bunch C；

(8) repeat to walk (2) to (7) to being labeled as a little visited；

The increase of the distance owing to arriving along with Laser Radar Scanning, the distribution of point increasingly disperses, in order to enable more accurately Clustering, ε is set to 100mm, MinPts and is set to 5 by the present invention；According to original DBSCAN clustering algorithm, clustered Journey time complexity is higher, does not reaches requirement of real-time, therefore enters DBSCAN algorithm according to the data characteristics of laser collection point Row optimizes；

When original DBSCAN algorithm carries out core point calculating, for kth collection point P_k, need P_kWith remaining institute Carrying out a little the calculating of distance, time complexity is high, owing to laser collection point is the point on sector scanning face, and the point that distance is the most remote The probability belonging to same object is the least, therefore when carrying out core point and calculating, only considers P_kSurrounding's point and P_kDistance, i.e. Calculate P_(k-5)To P_(k+5)In the range of point to P_kDistance, while not affecting Clustering Effect, greatly reduce the calculating time, Improve the speed of cluster；

3rd step: pedestrian based on laser collection point identifies:

The laser beam that laser radar is launched is covering of the fan, pedestrian bunch generally arc or near linear, distance laser radar Distance directly affect the quantity of the point scanning object；Direction, angle, size the most identical but apart from two different objects, Many compared with the quantity of collection point near object to laser radar, density is high, the number of collection point on laser radar object farther out Amount is few, and density is low；

In addition to the quantity that the distance of pedestrian to laser radar affects collection point, pedestrian in motor process with laser radar Position relatively can change, and the different shape of pedestrian also can produce impact to the quantity of collection point；As Fig. 3 be diverse location not With the pedestrian of the form state under laser radar, round rectangle represents the waist profile of pedestrian, shows five kinds of differences in figure The pedestrian under distance different shape state under laser radar, scans the collection with the pedestrian of different shape diverse location Point is also different, but can adopt on the pedestrian's health under diverse location different shape according to the width information of the waist location of pedestrian The quantity of collection point is estimated, thus carries out pedestrian's identification in multiple bunches；

The concrete grammar that pedestrian identifies is as follows:

(1) first the point in the class bunch clustered out is scanned, it is thus achieved that the information of each class bunch, gathers including in class bunch Number N of point_c, class bunch distance laser radar average distance D_avgAnd the position coordinates at the edge up and down of class bunch；

(2) carry out coarse sizing according to the size of class bunch, the number at class bunch midpoint bunch is considered as non-pedestrian bunch more than 200；

(3) carry out fine screening to remaining bunch, utilize the health width of pedestrian's distance with laser radar and pedestrian The number of the laser collection point of reflection on pedestrian's health is estimated；Fig. 4 is that the number of collection point estimates signal in pedestrian bunch Figure, first acquisition laser radar, to the distance of this bunch, is denoted as r, makes r=D_avgIf pedestrian's waist width is W_ped, W_pedValue Scope is W_min<W_ped<W_max；If with r as radius, by W_pedAs arc length, then there is a following formula:

$W_{ped}=\frac{\mathrm{\&theta;}\mathrm{\&pi;}r}{180}---\left(5\right),$

Calculating the corresponding central angle θ of sector region, formula is as follows:

$\mathrm{\&theta;}=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r}---\left(6\right),$

Angular resolution β according to laser radar utilizes formula (7) to calculate number P of collection point with bunch corresponding central angle θ_c, Then the number of bunch corresponding point is P_cOr P_c+ 1, P_cComputing formula as follows:

$P_c=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(7\right),$

If the number of the point that pedestrian bunch is corresponding is denoted as P_num, then P_numThe computational methods of scope estimated value as follows:

$\frac{180\&times;W_{min}}{\mathrm{\&pi;}r\mathrm{\&beta;}}<P_{num}<\frac{180\&times;W_{max}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(8\right),$

The present invention takes W_pedIn the range of 200mm < W_ped< 500mm, after each is clustered bunch according to said method Judge, remove non-pedestrian bunch, using pedestrian bunch as the input data of pedestrian tracking；

4th step: graphic software platform:

Laser collection point owing to obtaining is relatively decentralized, and sweep spacing is 100ms, and the time is longer, the most adjacent twice sweep The data variation obtained is relatively big, and on pedestrian's health, position and the deformation of collection point become big the most therewith；In consideration of it, laser is continuous The scan data obtained is patterned display, laser data will be converted into the video flowing that frequency acquisition is 10 frames/s；

Data to scanning every time obtain the rectangular coordinate of each collection point after coordinate system is changed, in order to accelerate tracking Speed, reduces 20 times of displays by the coordinate data in rectangular coordinate system, using image center location as the position of laser radar, i.e. Zero；Each collection point is done a mapping, and collection point has been carried out image amplification, it is simple to more efficient enter intuitively Line trace；

5th step: pedestrian tracking based on particle filter:

(1) initial phase:

After completing pedestrian's identification, using the position of pedestrian bunch as the initial position of pedestrian tracking, first initialize tracking Device, extracts the feature in rectangle frame, and initializes particle collection, and particle number is 100；

Provincial characteristics is described by the distribution of color probability density of target area, and method is as follows:

First region to be tracked is carried out discrete statistics and obtain the color histogram of RGB；For strengthen distribution of color can By property, giving less weights by distance center pixel farther out, weight function is as follows:

Wherein, r is a little to the distance of target's center, if target area centre coordinate is X_c=(x_c,y_c), H_xAnd H_yFor target The width in region and height, then target area size is usedDescribing, the coordinate of target area ith pixel is X_i= (x_i,y_i), i=1 ..., n, then current state X_tColor distribution model be:

$p^{\left(u\right)}\left(X_t\right)=f\underset{i=1}{\overset{n}{\&Sigma;}}k\left(\frac{\left|\right|X_c-X_i\left|\right|}{a}\right)\mathrm{\&delta;}\&lsqb;h\left(X_i\right)-u\&rsqb;,(u=1,...,m)---\left(10\right),$

$f=\frac{1}{\underset{n=1}{\overset{n}{\&Sigma;}}k\left(\frac{\left|\right|X_c-X_i\left|\right|}{a}\right)}---\left(11\right),$

Wherein, n is the number of pixels of current region, and m is histogram feature number, wherein m=256, f be normalization because of Son, u is histogrammic color grade index, δ [h (X_i)-u] represent X in target area_iWhether the color of position belongs to Nogata The u feature in figure, if belonging to, this value is 1, is otherwise 0；

(2) propagation stage: according to equation of transfer s_t=As_t-1+w_t-1, calculate new particle collection and predict the position of new particle Putting, wherein A is state-transition matrix, w_t-1For Gaussian noise；

(3) decision phase: choose the criterion of the characteristic similarity of particle, the similarity or the mark that calculate are The weight of corresponding particle, method is as follows:

Each particle calculates Color histogram distribution according to above-mentioned steps (1), and by itself and the color histogram of target area Figure distribution is compared, and uses Bhattacharyya distance to weigh, and computational methods are as follows:

$d\&lsqb;p\left(X_t\right),p_d\&rsqb;=\underset{u=1}{\overset{m}{\&Sigma;}}\sqrt{p^{\left(u\right)}\left(X_t\right){p_d}^{\left(u\right)}}---\left(12\right),$

Wherein, p (X_t) it is the Color histogram distribution at the t particle position, p_dColor histogram for target area divides Cloth；

The weighted value of each particle is obtained according to Bhattacharyya distance:

$w_t=\frac{1}{\sqrt{2\mathrm{\&pi;}}\mathrm{\&sigma;}}\exp (-\frac{1-d\&lsqb;p\left(X_t\right),p_d\&rsqb;}{2{\mathrm{\&sigma;}}^2})---\left(13\right),$

Wherein, σ is the Gauss difference of two squares, value 0.1, and the weights that particle is corresponding are the biggest, then this particle is with target similarity more Greatly；

Calculate the weight of each particle, and be normalized；

(4) in the state estimation stage: according to particle weights, the position that according to weight summation, the position of particle is obtained target is defeated Go out；

(5) in the resampling stage: particle is carried out resampling according to weights of importance, new particle collection is obtained；

(6) repeated execution of steps (2) to (5), it is achieved the tracking of moving object；

6th step: pedestrian movement's track is drawn:

Each two field picture utilizing particle filter obtain pedestrian position, calculates target's center position, Bing Jianggai center is with front Line does in one frame target's center, obtains pedestrian movement's trajectory diagram；

In order to verify the pedestrian tracking effect of the present invention, different distance, different shape, the pedestrian of Different Exercise Mode are entered Row identifies and follows the tracks of；If Fig. 5 (a) is the track path of resting state pedestrian and Actual path comparison diagram in the inventive method, its Middle tracing positional fluctuates near pedestrian, relatively large deviation does not occurs, and tracking effect is good；As Fig. 5 (b) be in the inventive method straight Track path and the Actual path comparison diagram of line motion pedestrian, attitude in pedestrian movement and the relative position with laser radar Being continually changing, it is accurate that experiment shows to follow the tracks of result, and less error occurs in part path, but does not cause big shadow to following the tracks of Ring；As Fig. 5 (c) is the track path of curvilinear motion pedestrian and Actual path comparison diagram in the inventive method, due to the fortune of pedestrian Dynamic direction changes very fast, and metamorphosis is the most violent, therefore can be seen that in figure that fluctuation is more apparent, but algorithm correct in time with Track route；From three groups of experiments, the pedestrian of motion can be tracked by the track algorithm of the present embodiment rapidly and accurately, and Can correction timely to tracing deviation.

The foregoing is only presently preferred embodiments of the present invention, not in order to limit the present invention, all essences in the present invention Within god and principle, any modification, equivalent substitution and improvement etc. made, should be included within the scope of the present invention.

Claims (6)

1. Dancing Robot indoor based on a laser radar pedestrian tracting method, it is characterised in that comprise the steps:

(1) laser data pretreatment: be connected to the laser radar of robot main frame, collects the object scanned at certain angle and arrives The range information of radar；Main frame obtains laser initial data, and initial data is carried out pretreatment, and preprocess method includes that intermediate value is filtered Ripple and coordinate system conversion；

(2) laser data based on density cluster: the data obtained for step (1) realize environment by density clustering Segmentation, makes a distinction different objects；

(3) pedestrian based on laser collection point identifies: identifies the every trade people that clicks in the class bunch clustered out, removes non-pedestrian Bunch, using pedestrian bunch as the input data of pedestrian tracking；

(4) graphic software platform: the scan data continuously acquired by laser is patterned display, laser data will be converted into and regard Frequency stream；Each collection point is done a mapping, and collection point is carried out image amplification, it is simple to more efficient carry out intuitively with Track；

(5) pedestrian tracking based on particle filter, including:

(501) initial phase:

After completing pedestrian's identification, using the position of pedestrian bunch as the initial position of pedestrian tracking, first initialize tracker, carry Taking the feature in rectangle frame, provincial characteristics is described by the distribution of color probability density of target area, and at the beginning of particle collection is carried out Beginningization；

(502) propagation stage: according to equation of transfer s_t=As_t-1+w_t-1, calculate new particle collection and predict the position of new particle, its Middle A is state-transition matrix, w_t-1For Gaussian noise；

(503) decision phase: choose the criterion of the characteristic similarity of particle, the similarity or the mark that calculate are phase Answer the weight of particle, calculate the weight of each particle, and be normalized；

(504) in the state estimation stage: according to particle weights, the position that according to weight summation, the position of particle is obtained target is defeated Go out；

(505) in the resampling stage: particle is carried out resampling according to weights of importance, new particle collection is obtained；

(506) repeated execution of steps (502) is to step (505), it is achieved the tracking of moving object；

(6) pedestrian movement's track is drawn: each two field picture utilizes particle filter obtain pedestrian position, calculates target's center position Putting, line does in Bing Jianggai center and former frame target's center, obtains pedestrian movement's trajectory diagram.

A kind of Dancing Robot indoor based on laser radar the most according to claim 1 pedestrian tracting method, its feature Being, the method for step (1) described medium filtering is: the collection point of laser radar belongs to 2-D data signal, the number collected According to being the some distance to laser, the range data of the m data point in pending data point neighborhood is ranked up, will sequence After (1+m)/2 distance replace current data point distance.

A kind of Dancing Robot indoor based on laser radar the most according to claim 1 and 2 pedestrian tracting method, it is special Levying and be, the method for step (1) described coordinate system conversion is: coordinate system is changed, and initial data eliminates after medium filtering Part measurement error, owing to initial data is under polar coordinate system, is converted to rectangular coordinate system for the ease of data analysis, Raw data format is:

s_k=d_k, k=0,1 ..., N (1),

Wherein, d_kDistance for kth collection point；

The binary vector group being converted to rectangular coordinate system:

u_k=(x_k,y_k)^T, k=0,1 ..., N_e(2),

Wherein, u_kFor the rectangular coordinate that kth laser collection point is corresponding, x_kWith y_kIt is respectively X-axis and Y-axis coordinate, N_eFor effectively adopting Collection point number；

If some P is kth collection point, the azimuth that laser radar obtains is θ_k, distance is ρ_k, angular resolution is β, then θ_k=k × β, trying to achieve rectangular coordinate corresponding for P is:

P_x=ρ_kcos(θ_k-45 °) (3),

P_y=ρ_ksin(θ_k-45 °) (4),

Wherein, P_xWith P_yIt is respectively X-axis and Y-axis the coordinate, (θ of some P_k-45 °) for putting the angle of P and X-axis.

A kind of Dancing Robot indoor based on laser radar the most according to claim 1 pedestrian tracting method, its feature Being, in step (2), using DBSCAN algorithm to cluster, key step includes:

(201) unvisited will be labeled as a little；

(202) take the some p of unvisited at random, and be labeled as visited；

(203) judging whether some p are core point, if the number to the some p distance point less than ε is more than MinPts, then p is core Point, continues step (204), and otherwise p is noise, returns step (202)；

(204) p is core point, builds a bunch C, and adds p to C, by P_NThe set put in being defined as the ε-neighborhood of a p, continues step (205)；

(205) for P_NMidpoint q, repetition step (206) to P_NFor sky；

(206) if q's is labeled as unvisited, first it is marked as visited, then judges whether q is core point, if q For core point, the point in the ε-neighborhood of q is added to P_NIf q is not the member of any bunch, then p is added to C；

(207) output bunch C；

(208) step (202) to (207) is repeated to being labeled as a little visited.

A kind of Dancing Robot indoor based on laser radar the most according to claim 4 pedestrian tracting method, its feature It is, when above-mentioned DBSCAN algorithm carries out core point calculating, for kth collection point P_k, need P_kAll click on remaining The calculating of row distance, time complexity is high, therefore when carrying out core point and calculating, only considers P_kSurrounding's point and P_kDistance, i.e. Calculate P_(k-MinPts)To P_(k+MinPts)In the range of point to P_kDistance.

A kind of Dancing Robot indoor based on laser radar the most according to claim 1 pedestrian tracting method, its feature Being, the concrete grammar that step (3) described pedestrian identifies is as follows:

(301) first the point in the class bunch clustered out is scanned, it is thus achieved that the information of each class bunch, including collection point in class bunch Number N_c, class bunch distance laser radar average distance D_avgAnd the position coordinates at the edge up and down of class bunch；

(302) coarse sizing is carried out according to the size of class bunch；

(303) carrying out fine screening to remaining bunch, first acquisition laser radar, to the distance of this bunch, is denoted as r, makes r= D_avgIf pedestrian's waist width is W_ped, W_pedSpan be W_min<W_ped<W_max；If with r as radius, by W_pedAs arc length, Then there is a following formula:

$W_{ped}=\frac{\mathrm{\&theta;}\mathrm{\&pi;}r}{180}---\left(5\right),$

Calculating the corresponding central angle θ of sector region, formula is as follows:

$\mathrm{\&theta;}=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r}---\left(6\right),$

Angular resolution β according to laser radar utilizes formula (7) to calculate number P of collection point with bunch corresponding central angle θ_c, calculate public affairs Formula is as follows:

$P_c=\frac{180\&times;W_{ped}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(7\right),$

If the number of the point that pedestrian bunch is corresponding is denoted as P_num, then P_numThe computational methods of scope estimated value as follows:

$\frac{180\&times;W_{min}}{\mathrm{\&pi;}r\mathrm{\&beta;}}<P_{num}<\frac{180\&times;W_{max}}{\mathrm{\&pi;}r\mathrm{\&beta;}}---\left(8\right),$

Bunch judging according to said method after clustering each, removes non-pedestrian bunch, using pedestrian bunch as pedestrian with The input data of track.