CN107765220A - Pedestrian's system for tracking and method based on UWB and laser radar mixed positioning - Google Patents
Pedestrian's system for tracking and method based on UWB and laser radar mixed positioning Download PDFInfo
- Publication number
- CN107765220A CN107765220A CN201710849238.5A CN201710849238A CN107765220A CN 107765220 A CN107765220 A CN 107765220A CN 201710849238 A CN201710849238 A CN 201710849238A CN 107765220 A CN107765220 A CN 107765220A
- Authority
- CN
- China
- Prior art keywords
- mrow
- robot
- uwb
- laser radar
- target
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Abstract
The present invention proposes a kind of pedestrian's system for tracking method based on UWB and laser radar mixed positioning, preliminary positioning and identification are carried out to pedestrian using UWB, then accurate positioning and identification are carried out to pedestrian using laser radar, effective avoidance, meet that the accurate of pedestrian follows and positioned under complex environment；Avoidance is carried out using dynamic window method, precision is higher；Processing is filtered using motion of the Kalman filter to robot, stability is stronger.
Description
Technical field
The present invention relates to robot system for tracking, more particularly to a kind of pedestrian based on UWB and laser radar mixed positioning
System for tracking and method.
Background technology
Robot follow technology refer to robot maintain a certain distance with speed follower pedestrian, and aid in pedestrian complete phase
The social production activity of pass.Robot is followed all to have broad application prospects in service robot, the storage industry etc. that disappears soon.
In sorting system of storing in a warehouse, robot is followed to follow pedestrian to reach shelf picking area order sorting goods, this is not only substantially reduced
The labor intensity of people, but also the greatly low efficiency for improving whole sorting task；In service robot field, technology is followed
It can help to be similar to guest-meeting robot progress personalized service.
Technology is followed mainly to be followed using ultrasonic wave positioning at present, bluetooth positioning follows, ultra-wide band radio-frequency signal (UWB) is fixed
Position follows.There is diffraction phenomena due to ultrasonic wave in ultrasonic wave positioning, cause to position unstable, it is impossible to carry out more accurate positioning
Follow task；Bluetooth positioning due to by Bluetooth signal carry out triangle polyester fibre, this method position error is larger to reach meter level, also without
Method carries out accurate positioning and follows task；And ultra-wide band radio-frequency signal (UWB) positioning can reach 10cm levels, and moderate cost at present
Have become application and study hotspot that robot follows field at present.
Chinese patent CN201610259595.1 provide for a kind of autonomous based on UWB follow robot localization method and
System, realized using UWB wireless location technologies and independently follow robot localization, communicated using narrow-band impulse, it is not necessary to carry
Ripple, transimission power is high, and small power consumption, antijamming capability and penetration capacity are strong, the precision and accuracy of positioning are improved, so as to realize
Specific objective follows.
The content of the invention
In view of this, the present invention proposes a kind of based on UWB and pedestrian's system for tracking of laser radar mixed positioning and side
Method, meet that the accurate of pedestrian follows and positioned under complex environment, carries out preliminary positioning and identification to pedestrian, so using UWB
Carry out accurate positioning and identification to pedestrian using laser radar afterwards, finally allow the robot to without collision accurately with retinue
People, precision is higher, and stability is stronger.
The technical proposal of the invention is realized in this way：It is fixed based on UWB and laser radar mixing the invention provides one kind
Pedestrian's system for tracking of position, it includes at least three UWB base stations, UWB labels, data processing equipment and motion planning and robot control dress
Put, in addition to laser radar, wherein,
UWB labels, which are placed in, to be followed in target, receives the UWB signal sent from UWB base stations；
UWB base stations send UWB signal, and at least three positions are fixed into the UWB base stations of triangle in robot；
Laser radar, measurement follow the distance between target and robot and angle, robot measurement ambient condition information；
Data processing equipment, by UWB base stations and UWB tag computations follow between target and robot UWB distance and
UWB angles, and the distance between target and robot and angle are followed according to lidar measurement, to UWB distances and UWB angles
Degree is corrected, and generates a reference path；According to the robot information of lidar measurement, in reference path
On the basis of generate a collisionless final path；For final coordinates measurement rate control instruction, and to the speed of generation
Control instruction is filtered processing, obtains smooth rate control instruction；
Robot movement control device, the rate control instruction of data processing equipment processing is converted into movement executing mechanism
Execute instruction.
Second aspect, the invention provides a kind of pedestrian's follower method based on UWB and laser radar mixed positioning, including
Following steps,
S1, UWB signal is sent to UWB labels by UWB base stations, calculates the UWB distances followed between target and robot
And UWB angles；
S2, the distance between target and robot and angle are followed by lidar measurement, to UWB distances and UWB angles
Degree is corrected, it is assumed that clear around robot, generates a nearest reference path；
S3, by lidar measurement robot information, local map is created, then according to the machine after correction
Device people generates a collisionless final path relative to the distance and angle for following target on the basis of reference path；
S4, processing is filtered for final coordinates measurement rate control instruction, and to the rate control instruction of generation, is obtained
To smooth rate control instruction；
S5, the execution that smooth rate control instruction is converted into movement executing mechanism by robot movement control device refer to
Order.
On the basis of above technical scheme, it is preferred that in the step S1, according to principle of triangulation robot measurement
Relative to UWB distance S1 and the UWB angle, θ 1 of UWB labels, position (S1, θ 1) of the UWB labels relative to robot is obtained.
It is further preferred that in the step S2, a frame laser radar data is gathered and then according to UWB by laser radar
The determination of angle, θ 1 follows size (θ 1- Δ/2, θ 1+ Δ/2) of the target in the range of lidar measurement, wherein, Δ is to follow mesh
The measurement range of laser radar corresponding to target width；Then, search follows target phase in the range of (θ 1- Δ/2, θ 1+ Δ/2)
For the position of robot, the point of the n laser radar measured is added, is removed first in this n point in scope (S1- Δs
Noise spot in s, S1+ Δ s), wherein, wherein S1 is UWB distances, and Δ s is the Breadth Maximum of 2 times of human legs, obtains n1 point,
Then the average distance for following target to laser radar is soughtAnd willAs robot and after following target correction
Distance, further calculating robot is with following the angle after target correctionWherein Δ θ is point of laser radar
Resolution, then finally follow position of the target under robot coordinate to be
Still more preferably, in the step S2, reference path was that the origin of robot coordinate system points toPoint
Straight line, it controls linear velocity and the angular speed to beWherein K1 is the ratio pass of distance and linear velocity
System, K2 are Schemes of Angular Velocity Estimation for Robots and robot front and the normal angle proportionate relationship for following target measurement width.
On the basis of above technical scheme, it is preferred that in the step S3, robot is carried out using dynamic window method
Avoidance on travel path, so as to obtain final path, dynamic window method comprises the following steps,
S3-1, laser radar search space is constrained to the controllable control instruction space of robot；
S3-2, the order in search space is evaluated using object function, selection makes object function maximumlly refer to
Order is used as optimum instruction.
It is further preferred that in the step S3-1, robot speed's scope is determined using below equation,
θt=θt+ωΔt
Vm={ v ∈ [vmin,vmax],ω∈[ωmin,ωmax]}
Wherein, r is the radius that robot does circular motion；V is robot linear velocity；ω is robot rotary speed；
Robot coordinate is (x, y), θtIt is robot in the course angle of t, t is current time；
VmFor robot starting velocity space, VdIt is robot up to the velocity space, vc,ωcIt is the current speed of robot
Degree,For the maximum deceleration of robot,For the peak acceleration of robot,For the maximum angular acceleration of robot,
For the maximum angular deceleration of robot, VaThe feasible speed space of barrier is not struck against for robot, dist (v, ω) is speed
The nearest distance of (v, ω) corresponding trajectory distance barrier.
On the basis of above technical scheme, it is preferred that in the step S3-2, every track is entered using below equation
Row evaluation,
Wherein, H (v, ω) is used for evaluating robot under the sample rate currently set, when reaching analog track end
Differential seat angle between direction and target；
G (v, ω) is track evaluation function, and deviation is smaller, and evaluation of estimate is higher, and distance is bigger, and evaluation of estimate is higher, and speed is got over
Greatly, evaluation of estimate is higher；
D (v, ω) represents distance of the robot on current track between nearest barrier；
V (v, ω) is used for evaluating the velocity magnitude of current track, and α, beta, gamma is weight coefficient；
N is all tracks of sampling, and i is current track to be evaluated；
H (i) is differential seat angle of the robot between the direction and target of i-th trailing end away from d (i) is that robot exists
The distance between with nearest barrier on i-th track, v (i) is speed of the robot on i-th track.
On the basis of above technical scheme, it is preferred that in the step S4, the speed using Kalman filter to generation
Control instruction is filtered processing, the signal and the state-space model of noise pre-established, in implementation procedure, according to current moment
Observation and previous moment estimate update the estimation to state variable, reach linear optimal effect.
Pedestrian's system for tracking method based on UWB and laser radar mixed positioning of the present invention has relative to prior art
Following beneficial effect：
(1) preliminary positioning and identification are carried out to pedestrian using UWB, then pedestrian carried out using laser radar accurate
Positioning and identification, effective avoidance, meet that the accurate of pedestrian follows and positioned under complex environment；
(2) avoidance is carried out using dynamic window method, precision is higher；
(3) processing is filtered using motion of the Kalman filter to robot, stability is stronger.
Brief description of the drawings
In order to illustrate more clearly about the embodiment of the present invention or technical scheme of the prior art, below will be to embodiment or existing
There is the required accompanying drawing used in technology description to be briefly described, it should be apparent that, drawings in the following description are only this
Some embodiments of invention, for those of ordinary skill in the art, on the premise of not paying creative work, can be with
Other accompanying drawings are obtained according to these accompanying drawings.
Fig. 1 is the block diagram of pedestrian's system for tracking of the invention based on UWB and laser radar mixed positioning.
Embodiment
Below in conjunction with the accompanying drawing in embodiment of the present invention, the technical scheme in embodiment of the present invention is carried out clear
Chu, it is fully described by, it is clear that described embodiment only a part of embodiment of the present invention, rather than whole realities
Apply mode.Based on the embodiment in the present invention, those of ordinary skill in the art institute under the premise of creative work is not made
The every other embodiment obtained, belongs to the scope of protection of the invention.
As shown in figure 1, the UWB of the present invention and pedestrian's system for tracking of laser radar mixed positioning, it includes laser radar
1st, at least three UWB base stations 2, UWB labels 3, data processing equipment 4 and robot movement control device 5.
UWB labels 3, which are placed in, to be followed in target, receives the UWB signal sent from UWB base stations 2；
UWB base stations 2 send UWB signal, and at least three positions are fixed into the UWB base stations 2 of triangle in robot；
Laser radar 1, measurement follow the distance between target and robot and angle, robot measurement surrounding environment letter
Breath；
Data processing equipment 4, the UWB distances followed between target and robot are calculated by UWB base stations 2 and UWB labels 3
And UWB angles, and according to laser radar 1 measure follow the distance between target and robot and angle, to UWB apart from and
UWB angles are corrected, and generate a reference path；The robot information measured according to laser radar 1, is joining
Examine and a collisionless final path is generated on the basis of path；For final coordinates measurement rate control instruction, and to generation
Rate control instruction be filtered processing, obtain smooth rate control instruction；
Robot movement control device 5, the rate control instruction that data processing equipment 4 is handled is converted into Motor execution machine
The execute instruction of structure.
Pedestrian's follower method based on UWB and laser radar mixed positioning of the present invention, comprises the following steps,
S1, UWB signal is sent to UWB labels 3 by UWB base stations 2, calculate follow UWB between target and robot away from
From and UWB angles.
Specifically, according to principle of triangulation robot measurement relative to UWB distance S1 and the UWB angle, θs of UWB labels 3
1, obtain position (S1, θ 1) of the UWB labels 3 relative to robot.
Principle of triangulation：Two-way time-of-flight method, the arteries and veins of Ta1 transmitting request property of the UWB base stations 2 on its timestamp
Signal is rushed, by UWB labels 3 in Ta2 receptions, UWB labels 3 launch the signal of a response property at the Tb1 moment, by UWB
Time stamp T b2 reception of the base station 2 at oneself.Pulse signal can be calculated successively between UWB base stations 2 and UWB labels 3
Flight time, so that it is determined that flying distance S.S=C × [(Ta2-Ta1)-(Tb2-Tb1)] (C is the light velocity), then by
Multiple cans of diverse location arrangement UWB base stations 2 measure UWB labels 3 relative to machine by principle of triangulation in robot
The position (S1, θ 1) of device people.
S2, the distance between target and robot and angle are followed by the measurement of laser radar 1, to UWB distances and UWB angles
Degree is corrected, it is assumed that clear around robot, generates a nearest reference path.
One frame laser radar data is gathered by laser radar 1 and then is determined to follow target in lidar measurement according to θ 1
In the range of size (θ 1- Δ/2, θ 1+ Δ/2), wherein, Δ is the measurement model for following laser radar 1 corresponding to the width of target
Enclose；Then search follows target to add the n measured relative to the position of robot in the range of (θ 1- Δ/2, θ 1+ Δ/2)
The point of individual laser radar 1, removed first in this n point scope (noise spot in S1- Δs s, S1+ Δ s), wherein, wherein
S1 is UWB distances, and Δ s is the Breadth Maximum of 2 times of human legs, obtains n1 point, then asks and follows target to the flat of laser radar 1
Equal distanceAnd willAs robot and the distance after target correction is followed, further calculating robot and follows mesh
Angle after calibration justWherein Δ θ is the resolution ratio of laser radar 1, then finally follows target in robot
Position under coordinate is
Reference path was that the origin of robot coordinate system points toThe straight line of point, it controls linear velocity and angular speed
ForWherein K1 is the proportionate relationship of distance and linear velocity, and K2 is Schemes of Angular Velocity Estimation for Robots and robot
Front and the normal angle proportionate relationship for following target measurement width.
S3, by the robot measurement ambient condition information of laser radar 1, create local map, then according to correction after
Robot generates a collisionless final path relative to the distance and angle for following target on the basis of reference path.
Robot is tracking target, it is possible that the barrier incoherent with target, can influence robotic tracking
Target, the present invention carry out avoidance using dynamic window method (dwa), and dynamic window method is directly to be searched in control instruction space
The automatic obstacle avoiding algorithm for making object function take the Optimal Control of maximum to instruct.This method can be summarized as two steps：First, by laser
The search space of radar 1 is constrained to the controllable control instruction space of robot；Second, using object function to the life in search space
Order is evaluated, and selection makes object function maximumlly instruct as optimum instruction.Concrete principle is as follows：
Assuming that robot is not omnidirectional moving robot, what rotation of advancing can only be carried out, a pair (v, ω) just represent one
Arc track, the radius that it does circular motion be,
Wherein, r is the radius that robot does circular motion；V is robot linear velocity；ω is robot rotary speed.
When rotary speed ω is not equal to 0, robot coordinate is,
θtt=θt+ωΔt
Wherein, robot coordinate is (x, y), θtIt is robot in the course angle of t, t is current time；
Track is extrapolated according to speed can, then evaluates these tracks.
Limitation of the mobile robot by the kinematical constraint of itself, i.e. maximal rate and minimum speed：
Vm={ v ∈ [vmin,vmax],ω∈[ωmin,ωmax]}
Wherein, VmFor robot starting velocity space.
Because the motor torque of robot is limited, the acceleration and deceleration limitation of maximum be present, therefore mobile robot is in a meter
Calculate in the cycle, a dynamic window be present, the speed in the window is the speed that robot can actually reach：
VdIt is robot up to the velocity space, vc,ωcIt is the present speed of robot,For the maximum deceleration of robot
Degree,For the peak acceleration of robot,For the maximum angular acceleration of robot,For the maximum angular deceleration of robot；
Further contemplating robot be able to can stop before barrier is encountered, therefore under the conditions of maximum deceleration, speed has
One scope：
Wherein, VaThe feasible speed space of barrier is not struck against for robot, dist (v, ω) is corresponding for speed (v, ω)
The nearest distance of trajectory distance barrier.
In the velocity group of sampling, it is feasible to have some groups of tracks, therefore uses the mode of evaluation function as every rail
Mark is evaluated.Evaluation function is：
H (v, ω) is for evaluating robot under the sample rate currently set, reaching court during analog track end
To the differential seat angle between target；
G (v, ω) is track evaluation function, and deviation is smaller, and evaluation of estimate is higher, and distance is bigger, and evaluation of estimate is higher, and speed is got over
Greatly, evaluation of estimate is higher；
D (v, ω) represents distance of the robot on current track between nearest barrier, if on this track
There is no barrier, that just sets it to a constant；
V (v, ω) is used for evaluating the velocity magnitude of current track, and α, beta, gamma is weight coefficient；
In order to avoid occur a certain item in evaluation function it is too dominant, place is normalized to each single item of evaluation function
Reason：
Wherein, n is all tracks of sampling, and i is current track to be evaluated；
H (i) is differential seat angle of the robot between the direction and target of i-th trailing end away from d (i) is that robot exists
The distance between with nearest barrier on i-th track, v (i) is speed of the robot on i-th track.
In each calculating cycle, the obstacle information and target information around robot are obtained by laser radar 1, obtained
To feasible track, and by calculating the value of evaluation function corresponding to every track, evaluation function is set to obtain maximum corresponding
The set-point of (v, ω) next period velocity and angular speed corresponding to track.
S4, processing is filtered for final coordinates measurement rate control instruction, and to the rate control instruction of generation, is obtained
To smooth rate control instruction.
When tracking target person motion, jitter phenomenon affected by noise can occur in speed, while also have and measure for robot
Error, robot motion is filtered used here as Kalman filter.Kalman filter is built in advance using recursive algorithm
Vertical signal and the state-space model of noise, in implementation procedure, according to the observation of current moment and previous moment estimate
To update the estimation to state variable, reach linear optimal effect.The algorithm is concise, and has only used previous moment
Data, so Kalman filtering is adapted to and computer real-time operation.Filtering principle is as follows：
The motion state equation of system
X (k)=AX (k-1)+Wk
Wherein, Wk is process noise amount, and such a noise is white Gaussian noise, meets wk~(0, Q (k)).
X (k)=[lk,vl.k,θk,vθ.k]TState variable is respectively to follow the length at target k moment, linear velocity, angle, angle
Speed.
State-transition matrix：For the controlling cycle of system.
The observational equation of system is：
Z (k)=HX (k)+υk
Wherein, υkIt is the noise vector of observation, such a noise is that white Gaussian noise meets υk~(0, R (k)).
Observing matrix:
If the state X (k-1 | k-1) of the system at our known system k-1 moment and its corresponding covariance P (k-1 | k-
1) the state X (k | k-1) and its corresponding covariance P (k | k-1) of the system of subsequent time can, be predicted
Predictive equation is
X (k | k-1)=AX (k-1 | k-1)
P (k | k-1)=AP (k-1 | k-1) AT+Q (1)
Then we can obtain the observation Z (k) at k moment, and by predicted value X (k | k-1) and its corresponding covariance P
(k | k-1) obtains X (k | k) and its corresponding covariance P (k | k).Wherein X (k | k) is the estimate to system actual value.
Make kalman gain
Kg (k)=P (k | k-1) HT(HP(k|k-1)HT+R)-1 (2)
It can so update X (k | k), and P (k | k), renewal equation is as follows：
X (k | k)=c+Kg (k) [Z (k)-HX (k | k-1)]
P (k | k)=[I-Kg (k) H] P (k | k-1) (3)
Each k moment we by using human body measurement position Z (k) and (2) (3) update Karman equation, and
The estimate X (k | k) of position of human body is obtained, predicted value X (k+1 | k) is then obtained according to formula (1), and put using this and be used as prediction
Value.Such iterative cycles reach filter effect.
S5, the execution that smooth rate control instruction is converted into movement executing mechanism by robot movement control device 5 refer to
Order.
The better embodiment of the present invention is the foregoing is only, is not intended to limit the invention, it is all the present invention's
Within spirit and principle, any modification, equivalent substitution and improvements made etc., it should be included in the scope of the protection.
Claims (9)
1. a kind of pedestrian's system for tracking based on UWB and laser radar mixed positioning, it include at least three UWB base stations (2),
UWB labels (3), data processing equipment (4) and robot movement control device (5), it is characterised in that：Also include laser radar
(1), wherein,
UWB labels (3), which are placed in, to be followed in target, receives the UWB signal sent from UWB base stations (2)；
UWB base stations (2) send UWB signal, and at least three positions are fixed into the UWB base stations (2) of triangle in robot；
Laser radar (1), measurement follow the distance between target and robot and angle, robot measurement ambient condition information；
Data processing equipment (4), calculated by UWB base stations (2) and UWB labels (3) follow UWB between target and robot away from
From and UWB angles, and according to laser radar (1) measure follow the distance between target and robot and angle, to UWB distances
And UWB angles are corrected, and generate a reference path；The robot information measured according to laser radar (1),
A collisionless final path is generated on the basis of reference path；For final coordinates measurement rate control instruction, and it is right
The rate control instruction of generation is filtered processing, obtains smooth rate control instruction；
Robot movement control device (5), the rate control instruction of data processing equipment (4) processing is converted into Motor execution machine
The execute instruction of structure.
A kind of 2. pedestrian's follower method based on UWB and laser radar mixed positioning, it is characterised in that：Comprise the following steps,
S1, UWB signal is sent to UWB labels (3) by UWB base stations (2), calculate follow UWB between target and robot away from
From and UWB angles；
S2, the distance between target and robot and angle are followed by laser radar (1) measurement, to UWB distances and UWB angles
It is corrected, it is assumed that clear around robot, generate a nearest reference path；
S3, by laser radar (1) robot measurement ambient condition information, local map is created, then according to the machine after correction
Device people generates a collisionless final path relative to the distance and angle for following target on the basis of reference path；
S4, processing is filtered for final coordinates measurement rate control instruction, and to the rate control instruction of generation, is put down
Sliding rate control instruction；
Smooth rate control instruction is converted into the execute instruction of movement executing mechanism by S5, robot movement control device (5).
3. pedestrian's follower method based on UWB and laser radar mixed positioning as claimed in claim 2, it is characterised in that：Institute
State in step S1, UWB distance S1 and the UWB angle, θ 1 according to principle of triangulation robot measurement relative to UWB labels (3),
Obtain position (S1, θ 1) of the UWB labels (3) relative to robot.
4. pedestrian's follower method based on UWB and laser radar mixed positioning as claimed in claim 3, it is characterised in that：Institute
State in step S2, a frame laser radar data is gathered by laser radar (1) and then is determined to follow target to exist according to UWB angle, θs 1
Size (θ 1- Δ/2, θ 1+ Δ/2) in laser radar (1) measurement range, wherein, Δ is to follow to swash corresponding to the width of target
The measurement range of optical radar (1)；Then, search follows target relative to robot in the range of (θ 1- Δ/2, θ 1+ Δ/2)
Position, the point of the n laser radar (1) measured is added, removed first in this n point in scope (S1- Δs s, S1+ Δ s)
Interior noise spot, wherein, wherein S1 is UWB distances, and Δ s is 2 times of human leg's Breadth Maximums, obtains n1 point, then asks and follow
Average distance of the target to laser radar (1)And willAs robot and the distance after target correction is followed, is entered
One step calculating robot is with following the angle after target correctionWherein Δ θ is the resolution of laser radar (1)
Rate, then finally follow position of the target under robot coordinate to be
5. pedestrian's follower method based on UWB and laser radar mixed positioning as claimed in claim 4, it is characterised in that：Institute
State in step S2, reference path was that the origin of robot coordinate system points toThe straight line of point, it controls linear velocity and angle speed
Spend and beWherein K1 is the proportionate relationship of distance and linear velocity, and K2 is Schemes of Angular Velocity Estimation for Robots and machine
People front and the normal angle proportionate relationship for following target measurement width.
6. pedestrian's follower method based on UWB and laser radar mixed positioning as claimed in claim 2, it is characterised in that：Institute
State in step S3, the avoidance on robot travel path is carried out using dynamic window method, so as to obtain final path, dynamic window
Mouth method comprises the following steps,
S3-1, laser radar (1) search space is constrained to the controllable control instruction space of robot；
S3-2, the order in search space is evaluated using object function, selection makes object function maximumlly instruct work
For optimum instruction.
7. pedestrian's follower method based on UWB and laser radar mixed positioning as claimed in claim 6, it is characterised in that：Institute
State in step S3-1, robot speed's scope determined using below equation,
<mrow>
<mi>r</mi>
<mo>=</mo>
<mfrac>
<mi>v</mi>
<mi>&omega;</mi>
</mfrac>
</mrow>
<mrow>
<mi>x</mi>
<mo>=</mo>
<mi>x</mi>
<mo>-</mo>
<mfrac>
<mi>v</mi>
<mi>&omega;</mi>
</mfrac>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mfrac>
<mi>v</mi>
<mi>&omega;</mi>
</mfrac>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mi>t</mi>
</msub>
<mo>+</mo>
<mi>&omega;</mi>
<mi>&Delta;</mi>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>y</mi>
<mo>=</mo>
<mi>y</mi>
<mo>-</mo>
<mfrac>
<mi>v</mi>
<mi>&omega;</mi>
</mfrac>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mfrac>
<mi>v</mi>
<mi>&omega;</mi>
</mfrac>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&theta;</mi>
<mi>t</mi>
</msub>
<mo>+</mo>
<mi>&omega;</mi>
<mi>&Delta;</mi>
<mi>t</mi>
<mo>)</mo>
</mrow>
</mrow>
θt=θt+ωΔt
Vm={ v ∈ [vmin,vmax],ω∈[ωmin,ωmax]}
Wherein, r is the radius that robot does circular motion；V is robot linear velocity；ω is robot rotary speed；
Robot coordinate is (x, y), θtIt is robot in the course angle of t, t is current time；
VmFor robot starting velocity space, VdIt is robot up to the velocity space, vc,ωcIt is the present speed of robot,For
The maximum deceleration of robot,For the peak acceleration of robot,For the maximum angular acceleration of robot,For machine
The maximum angular deceleration of people, VaThe feasible speed space of barrier is not struck against for robot, dist (v, ω) is speed (v, ω)
The nearest distance of corresponding trajectory distance barrier.
8. pedestrian's follower method based on UWB and laser radar mixed positioning as claimed in claim 7, it is characterised in that：Institute
State in step S3-2, every track evaluated using below equation,
<mrow>
<mi>n</mi>
<mi>o</mi>
<mi>r</mi>
<mi>m</mi>
<mi>a</mi>
<mi>l</mi>
<mo>_</mo>
<mi>h</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<mi>h</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<munderover>
<mi>&Sigma;</mi>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</munderover>
<mi>h</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
</mrow>
<mrow>
<mi>n</mi>
<mi>o</mi>
<mi>r</mi>
<mi>m</mi>
<mi>a</mi>
<mi>l</mi>
<mo>_</mo>
<mi>d</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<mi>d</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</munderover>
<mi>d</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
</mrow>
<mrow>
<mi>n</mi>
<mi>o</mi>
<mi>r</mi>
<mi>m</mi>
<mi>a</mi>
<mi>l</mi>
<mo>_</mo>
<mi>v</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<mi>v</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>n</mi>
</munderover>
<mi>v</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</mrow>
</mfrac>
</mrow>
Wherein, H (v, ω) is used for evaluating robot under the sample rate currently set, reaches direction during analog track end
Differential seat angle between target；
G (v, ω) is track evaluation function, and deviation is smaller, and evaluation of estimate is higher, and distance is bigger, and evaluation of estimate is higher, and speed is bigger, comments
Value is higher；
D (v, ω) represents distance of the robot on current track between nearest barrier；
V (v, ω) is used for evaluating the velocity magnitude of current track, and α, beta, gamma is weight coefficient；
N is all tracks of sampling, and i is current track to be evaluated；
H (i) is differential seat angle of the robot between the direction and target of i-th trailing end away from d (i) is robot at i-th
The distance between with nearest barrier on track, v (i) is speed of the robot on i-th track.
9. pedestrian's follower method based on UWB and laser radar mixed positioning as claimed in claim 7, it is characterised in that：Institute
State in step S4, processing is filtered to the rate control instruction of generation using Kalman filter, the signal pre-established is with making an uproar
The state-space model of sound, in implementation procedure, according to the observation of current moment and previous moment estimate is updated to state
The estimation of variable, reach linear optimal effect.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710849238.5A CN107765220B (en) | 2017-09-20 | 2017-09-20 | Pedestrian following system and method based on UWB and laser radar hybrid positioning |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710849238.5A CN107765220B (en) | 2017-09-20 | 2017-09-20 | Pedestrian following system and method based on UWB and laser radar hybrid positioning |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107765220A true CN107765220A (en) | 2018-03-06 |
| CN107765220B CN107765220B (en) | 2020-10-23 |
Family
ID=61266029
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710849238.5A Active CN107765220B (en) | 2017-09-20 | 2017-09-20 | Pedestrian following system and method based on UWB and laser radar hybrid positioning |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107765220B (en) |
Cited By (29)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107121677A (en) * | 2017-06-02 | 2017-09-01 | 太原理工大学 | Avoidance radar method and device based on ultra wide band cognition CPPM signals |
| CN108436922A (en) * | 2018-06-15 | 2018-08-24 | 成都精位科技有限公司 | Associated movement robot and its control method, device, system |
| CN108614563A (en) * | 2018-06-12 | 2018-10-02 | 北京康力优蓝机器人科技有限公司 | A method of realizing that mobile robot target follows by location navigation |
| CN108930890A (en) * | 2018-07-20 | 2018-12-04 | 辽宁工业大学 | A kind of packaged type external display screen bracket and its control method for driving assistance system experiment porch |
| CN109062215A (en) * | 2018-08-24 | 2018-12-21 | 北京京东尚科信息技术有限公司 | Robot and barrier-avoiding method, system, equipment and medium are followed based on its target |
| CN109129507A (en) * | 2018-09-10 | 2019-01-04 | 北京联合大学 | A kind of medium intelligent introduction robot and explanation method and system |
| CN109307857A (en) * | 2018-11-12 | 2019-02-05 | 湖北工业大学 | It is a kind of that positioning device and method are independently followed based on multisensor |
| CN109490825A (en) * | 2018-11-20 | 2019-03-19 | 武汉万集信息技术有限公司 | Positioning navigation method, device, equipment, system and storage medium |
| CN109828558A (en) * | 2018-12-20 | 2019-05-31 | 钟胤舜 | A kind of smart motion system and method |
| CN109947119A (en) * | 2019-04-23 | 2019-06-28 | 东北大学 | A kind of autonomous system for tracking of mobile robot based on Multi-sensor Fusion and method |
| CN110109129A (en) * | 2019-05-20 | 2019-08-09 | 桂林电子科技大学 | Warehouse logistics navigation positioning system and localization method in a kind of room AGV |
| CN110164439A (en) * | 2019-05-29 | 2019-08-23 | 姚英泽 | Intellect service robot |
| CN110340935A (en) * | 2018-04-03 | 2019-10-18 | 深圳市神州云海智能科技有限公司 | A kind of method and robot of robot fusion positioning |
| CN110865534A (en) * | 2019-11-15 | 2020-03-06 | 合肥工业大学 | Intelligent following system with improved Kalman filtering for navigation positioning |
| CN110927742A (en) * | 2019-11-19 | 2020-03-27 | 杭州飞步科技有限公司 | Obstacle tracking method, device, equipment and storage medium |
| CN110942474A (en) * | 2019-11-27 | 2020-03-31 | 炬星科技（深圳）有限公司 | Robot target tracking method, device and storage medium |
| CN111708042A (en) * | 2020-05-09 | 2020-09-25 | 汕头大学 | Robot method and system for pedestrian trajectory prediction and following |
| CN112130559A (en) * | 2020-08-21 | 2020-12-25 | 同济大学 | Indoor pedestrian following and obstacle avoiding method based on UWB and laser radar |
| CN112394733A (en) * | 2020-11-26 | 2021-02-23 | 吉林大学 | Intelligent vehicle autonomous following obstacle avoidance method based on UWB and ultrasonic wave |
| CN112462782A (en) * | 2020-11-30 | 2021-03-09 | 北京航天光华电子技术有限公司 | Multifunctional intelligent following trolley system |
| CN112540345A (en) * | 2020-11-18 | 2021-03-23 | 广东工业大学 | Dual-model positioning method and system for detecting UWB quality based on Gaussian distribution |
| CN112764076A (en) * | 2020-12-30 | 2021-05-07 | 珠海爱肯智能设备有限公司 | Method for positioning following navigation system based on UWB and GPS |
| CN113069094A (en) * | 2021-03-22 | 2021-07-06 | 浙江华彩光电科技有限公司 | Wearable appliance with explosion-proof abnormality detection and physiological early warning functions |
| CN113126600A (en) * | 2019-12-26 | 2021-07-16 | 沈阳新松机器人自动化股份有限公司 | Follow system and article transfer cart based on UWB |
| CN113296119A (en) * | 2021-05-24 | 2021-08-24 | 福建盛海智能科技有限公司 | Unmanned obstacle avoidance driving method and terminal based on laser radar and UWB array |
| CN113466791A (en) * | 2021-07-14 | 2021-10-01 | 深圳市武测空间信息有限公司 | Laser mapping and positioning equipment and method |
| CN114326732A (en) * | 2021-12-28 | 2022-04-12 | 无锡笠泽智能科技有限公司 | Robot autonomous following system and autonomous following control method |
| CN114360274A (en) * | 2021-12-13 | 2022-04-15 | 珠海格力智能装备有限公司 | Distribution vehicle navigation method, system, computer equipment and storage medium |
| WO2022262594A1 (en) * | 2021-06-15 | 2022-12-22 | 同方威视技术股份有限公司 | Method and apparatus for following target, robot, and computer-readable storage medium |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105100710A (en) * | 2015-07-07 | 2015-11-25 | 小米科技有限责任公司 | Indoor monitoring method and device |
| CN105807775A (en) * | 2016-05-17 | 2016-07-27 | 上海酷哇机器人有限公司 | Movable robot with autonomous following and obstacle-avoidance function |
| CN106155065A (en) * | 2016-09-28 | 2016-11-23 | 上海仙知机器人科技有限公司 | A kind of robot follower method and the equipment followed for robot |
| US20160349362A1 (en) * | 2015-05-08 | 2016-12-01 | 5D Robotics, Inc. | Mobile localization using sparse time-of-flight ranges and dead reckoning |
| CN106885575A (en) * | 2017-02-17 | 2017-06-23 | 浙江工商职业技术学院 | A kind of indoor positioning bootstrap technique and system |
-
2017
- 2017-09-20 CN CN201710849238.5A patent/CN107765220B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20160349362A1 (en) * | 2015-05-08 | 2016-12-01 | 5D Robotics, Inc. | Mobile localization using sparse time-of-flight ranges and dead reckoning |
| CN105100710A (en) * | 2015-07-07 | 2015-11-25 | 小米科技有限责任公司 | Indoor monitoring method and device |
| CN105807775A (en) * | 2016-05-17 | 2016-07-27 | 上海酷哇机器人有限公司 | Movable robot with autonomous following and obstacle-avoidance function |
| CN106155065A (en) * | 2016-09-28 | 2016-11-23 | 上海仙知机器人科技有限公司 | A kind of robot follower method and the equipment followed for robot |
| CN106885575A (en) * | 2017-02-17 | 2017-06-23 | 浙江工商职业技术学院 | A kind of indoor positioning bootstrap technique and system |
Non-Patent Citations (1)
| Title |
|---|
| 许坤: "物联网环境下的移动机器人定位与控制", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (36)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107121677A (en) * | 2017-06-02 | 2017-09-01 | 太原理工大学 | Avoidance radar method and device based on ultra wide band cognition CPPM signals |
| CN110340935A (en) * | 2018-04-03 | 2019-10-18 | 深圳市神州云海智能科技有限公司 | A kind of method and robot of robot fusion positioning |
| CN108614563A (en) * | 2018-06-12 | 2018-10-02 | 北京康力优蓝机器人科技有限公司 | A method of realizing that mobile robot target follows by location navigation |
| CN108436922A (en) * | 2018-06-15 | 2018-08-24 | 成都精位科技有限公司 | Associated movement robot and its control method, device, system |
| CN108930890A (en) * | 2018-07-20 | 2018-12-04 | 辽宁工业大学 | A kind of packaged type external display screen bracket and its control method for driving assistance system experiment porch |
| CN108930890B (en) * | 2018-07-20 | 2023-08-01 | 辽宁工业大学 | Movable external display screen bracket for driving assistance system experiment platform and control method thereof |
| CN109062215A (en) * | 2018-08-24 | 2018-12-21 | 北京京东尚科信息技术有限公司 | Robot and barrier-avoiding method, system, equipment and medium are followed based on its target |
| CN109129507B (en) * | 2018-09-10 | 2022-04-19 | 北京联合大学 | Intelligent explaining robot and explaining method and system |
| CN109129507A (en) * | 2018-09-10 | 2019-01-04 | 北京联合大学 | A kind of medium intelligent introduction robot and explanation method and system |
| CN109307857A (en) * | 2018-11-12 | 2019-02-05 | 湖北工业大学 | It is a kind of that positioning device and method are independently followed based on multisensor |
| CN109490825A (en) * | 2018-11-20 | 2019-03-19 | 武汉万集信息技术有限公司 | Positioning navigation method, device, equipment, system and storage medium |
| CN109828558A (en) * | 2018-12-20 | 2019-05-31 | 钟胤舜 | A kind of smart motion system and method |
| WO2020124623A1 (en) * | 2018-12-20 | 2020-06-25 | 钟胤舜 | Intelligent motion system and method |
| CN109947119A (en) * | 2019-04-23 | 2019-06-28 | 东北大学 | A kind of autonomous system for tracking of mobile robot based on Multi-sensor Fusion and method |
| CN110109129A (en) * | 2019-05-20 | 2019-08-09 | 桂林电子科技大学 | Warehouse logistics navigation positioning system and localization method in a kind of room AGV |
| CN110164439A (en) * | 2019-05-29 | 2019-08-23 | 姚英泽 | Intellect service robot |
| CN110865534A (en) * | 2019-11-15 | 2020-03-06 | 合肥工业大学 | Intelligent following system with improved Kalman filtering for navigation positioning |
| CN110927742A (en) * | 2019-11-19 | 2020-03-27 | 杭州飞步科技有限公司 | Obstacle tracking method, device, equipment and storage medium |
| CN110942474A (en) * | 2019-11-27 | 2020-03-31 | 炬星科技（深圳）有限公司 | Robot target tracking method, device and storage medium |
| CN113126600A (en) * | 2019-12-26 | 2021-07-16 | 沈阳新松机器人自动化股份有限公司 | Follow system and article transfer cart based on UWB |
| CN111708042A (en) * | 2020-05-09 | 2020-09-25 | 汕头大学 | Robot method and system for pedestrian trajectory prediction and following |
| CN112130559A (en) * | 2020-08-21 | 2020-12-25 | 同济大学 | Indoor pedestrian following and obstacle avoiding method based on UWB and laser radar |
| CN112540345B (en) * | 2020-11-18 | 2023-08-01 | 广东工业大学 | Dual-model positioning method and system for detecting UWB quality based on Gaussian distribution |
| CN112540345A (en) * | 2020-11-18 | 2021-03-23 | 广东工业大学 | Dual-model positioning method and system for detecting UWB quality based on Gaussian distribution |
| CN112394733A (en) * | 2020-11-26 | 2021-02-23 | 吉林大学 | Intelligent vehicle autonomous following obstacle avoidance method based on UWB and ultrasonic wave |
| CN112462782A (en) * | 2020-11-30 | 2021-03-09 | 北京航天光华电子技术有限公司 | Multifunctional intelligent following trolley system |
| CN112462782B (en) * | 2020-11-30 | 2022-10-28 | 北京航天光华电子技术有限公司 | Multifunctional intelligent following trolley system |
| CN112764076B (en) * | 2020-12-30 | 2024-04-02 | 珠海爱肯智能设备有限公司 | Method for positioning following navigation system based on UWB and GPS |
| CN112764076A (en) * | 2020-12-30 | 2021-05-07 | 珠海爱肯智能设备有限公司 | Method for positioning following navigation system based on UWB and GPS |
| CN113069094A (en) * | 2021-03-22 | 2021-07-06 | 浙江华彩光电科技有限公司 | Wearable appliance with explosion-proof abnormality detection and physiological early warning functions |
| CN113296119A (en) * | 2021-05-24 | 2021-08-24 | 福建盛海智能科技有限公司 | Unmanned obstacle avoidance driving method and terminal based on laser radar and UWB array |
| CN113296119B (en) * | 2021-05-24 | 2023-11-28 | 江苏盛海智能科技有限公司 | Unmanned obstacle avoidance driving method and terminal based on laser radar and UWB array |
| WO2022262594A1 (en) * | 2021-06-15 | 2022-12-22 | 同方威视技术股份有限公司 | Method and apparatus for following target, robot, and computer-readable storage medium |
| CN113466791A (en) * | 2021-07-14 | 2021-10-01 | 深圳市武测空间信息有限公司 | Laser mapping and positioning equipment and method |
| CN114360274A (en) * | 2021-12-13 | 2022-04-15 | 珠海格力智能装备有限公司 | Distribution vehicle navigation method, system, computer equipment and storage medium |
| CN114326732A (en) * | 2021-12-28 | 2022-04-12 | 无锡笠泽智能科技有限公司 | Robot autonomous following system and autonomous following control method |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107765220B (en) | 2020-10-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107765220A (en) | Pedestrian's system for tracking and method based on UWB and laser radar mixed positioning | |
| CN109947119B (en) | Mobile robot autonomous following method based on multi-sensor fusion | |
| Lau et al. | Kinodynamic motion planning for mobile robots using splines | |
| CN108646215B (en) | Automatic following rapid positioning method based on ultra wide band | |
| CN108762264A (en) | The dynamic obstacle avoidance method of robot based on Artificial Potential Field and rolling window | |
| CN107992035A (en) | A kind of Multi Mobile Robots Formation's control method based on indoor Global localization | |
| CN110341711A (en) | A kind of driving trace generation system and method based on port environment | |
| CN103149937A (en) | Transverse lateral curve flight-path tracking method based on curvature compensation | |
| CN109002046A (en) | A kind of Navigation System for Mobile Robot and air navigation aid | |
| CN107544534A (en) | A kind of plant protection unmanned plane automatic fine operation and barrier-avoiding method based on BDS, INS | |
| CN103995984A (en) | Robot path planning method and device based on elliptic constrains | |
| CN108801269A (en) | A kind of interior cloud Algorithms of Robots Navigation System and method | |
| CN112486183B (en) | Path planning algorithm of indoor mobile robot | |
| Yao et al. | Wireless positioning and path tracking for a mobile platform in greenhouse | |
| CN114217632B (en) | Self-adaptive fault-tolerant unmanned aerial vehicle tracking cruise system and method | |
| CN115857504A (en) | DWA-based robot local path planning method, equipment and storage medium in narrow environment | |
| CN106502244A (en) | A kind of navigation positioning system of automatic guided vehicle | |
| Gokhale et al. | FEEL: Fast, energy-efficient localization for autonomous indoor vehicles | |
| CN102958153A (en) | Wireless sensor network based positioning method | |
| CN106125733A (en) | Mobile machine People's Bank of China based on infrared top mark sensor walking system | |
| CN113341999A (en) | Forklift path planning method and device based on optimized D-x algorithm | |
| CN111045428B (en) | Obstacle avoidance method, mobile robot, and computer-readable storage medium | |
| CN110442014B (en) | Mobile robot RFID servo method based on position | |
| US20230333262A1 (en) | Precision tracking control techniques for smart trolley | |
| Xu et al. | Indoor navigation based on global and local path planning for wheeled mobile robot in airport |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20221230 Address after: Room 715, Block A, Science and Innovation Center, No. 128 Shuanglian Road, Haining Economic Development Zone, Jiaxing, Zhejiang 314000 Patentee after: HISBIDE (Jiaxing) Electromechanical Technology Co.,Ltd. Address before: 430000 joint office of 1201, Huagong science and technology industry building, 243 Luoyu Road, Donghu New Technology Development Zone, Wuhan City, Hubei Province (y) Patentee before: WUHAN MUSHEN ROBOT CO.,LTD. |
|
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20230718 Address after: 315000 No. 28, Yunshan Middle Road, Sanqi Town, Yuyao City, Ningbo City, Zhejiang Province (in Yuyao talent entrepreneurship Park) Patentee after: Ningbo Kesen Intelligent Equipment Co.,Ltd. Address before: Room 715, Block A, Science and Innovation Center, No. 128 Shuanglian Road, Haining Economic Development Zone, Jiaxing, Zhejiang 314000 Patentee before: HISBIDE (Jiaxing) Electromechanical Technology Co.,Ltd. |
|
| TR01 | Transfer of patent right |