Embodiment
Below in conjunction with accompanying drawing, specific implementation process of the present invention is described in detail.
Embodiments provide a kind of method of positioning robot, comprising:
(1) 3 Kinect sensor discharged with rectangular coordinate system form, sound source a certain position in this coordinate system sends the sound of certain time, preserves the sound source deflection that three Kinect sensor obtain.
(2) certain deviation is had because each Kinect sensor receives voice signal, misalignment angle is represented with α, the sound source deviation of directivity scope that Kinect sensor detects is [-α, α], be called deviation covering of the fan, represent the sound source region that two Kinect sensor are determined, this method is called deviation covering of the fan method.The center of gravity of the intersection region that the deviation covering of the fan region then utilizing method of geometry to try to achieve every two Kinect sensor is formed, is called geometric center of gravity method.
(3) utilize deviation covering of the fan method and geometric center of gravity method to try to achieve the center of gravity of the sound source region that every two Kinect sensor are determined, totally three, calculate the mean value of these three barycentric coordinates, be the optimal location of sound source.
(4) optimum sound source position is sent to mobile robot, the positional information of robot acquisition oneself, is adjusted its direction of motion, is moved by sound localization to objective, thus realizes the navigation of mobile robot's sound localization.
The embodiment of the present invention is by the audio frequency processing power of Kinect sensor, and sound detecting source direction realizes robot chamber inner position.
Compared with prior art, the embodiment of the present invention has the following advantages:
Sound localization method is simple.The requirement of real-time of sound positioning system is very high, if adopt the algorithm of more complicated, can not meet the requirement of real-time of sound positioning system.The present invention adopts this more common method of gravity model appoach can determine the position of sound source fast.
Sound localization precision is high.Take full advantage of the microphone array of Kinect sensor and the background suppress of Kinect software driver and echo cancellation process, eliminate the impact of virtual sound source and ground unrest, detection real sources direction, obtains the optimum position of sound source.
Localization method is general.Owing to being the source direction utilizing Kinect sound detecting, even if so have barrier between Kinect and sound source, Kinect still can obtain the source direction of sound; On the other hand, this localization method can not only be used for indoor sound localization, can be applicable to outdoor equally.
Can be applicable to the accurate location in robot chamber and Navigational Movements control.
The embodiment of the present invention additionally provides a kind of robot localization and air navigation aid, comprising:
1, Kinect sound field center of gravity localization method
With reference to Fig. 1, in order to make the detection of sound source all in the optimal detection angular range of Kinect sensor, the layout of three Kinect sensor as shown in the figure.Kinect sensor obtains sound source deflection with its center line for reference, and namely figure acceptance of the bid has the dotted line of sound source angle reference line, and in the face of Kinect sensor, the angle on the left of dotted line is negative value, the angle on the right side of dotted line be on the occasion of.Sound source sends the sound of lasting 50ms in a certain position with the frequency of 16KHz.It is as follows that deviation covering of the fan method and regional barycenter methods combining obtain the sound source intersection region center of gravity process that three Kinect sensor determine:
The sound source deflection that No. 1 Kinect sensor detects, l is represented with β
k1represent the sound source place straight line that No. 1 Kinect sensor detects, therefore real sources is at straight line l
k1in the sector region of deflection α degree, No. 2 and No. 3 Kinect sensor similar.
represent the sound source deflection that No. 2 Kinect sensor detect, l
k2represent the sound source place straight line that No. 2 Kinect sensor detect.The sound source deflection that No. 3 Kinect sensor that represent γ detect, l
k3represent the sound source place straight line that No. 3 Kinect sensor detect.From the above β and
for negative value, γ on the occasion of.
(x
k1, y
k1) represent No. 1 Kinect sensor position coordinate, (x
k2, y
k2) represent No. 2 Kinect sensor position coordinates, (x
k3, y
k3) represent No. 3 Kinect sensor position coordinates, be easy in reality measure, so be known parameters.
The sound source direction that three Kinect sensor that illustrate Fig. 1 detect and between two deviation covering of the fan intersect determined sound source region mutually, for the region that the deviation covering of the fan of No. 1 and No. 3 Kinect sensor intersects to form mutually, its intersection region center of gravity asks method as follows:
As Fig. 2,
represent that No. 1 Kinect sensor detects sound source place straight line and deflects α degree counterclockwise,
represent that No. 1 Kinect sensor detects sound source place straight line and deflects α degree clockwise;
represent that No. 3 Kinect sensor detect sound source place straight line and deflect α degree counterclockwise,
represent that No. 3 Kinect sensor detect sound source place straight line and deflect α degree clockwise.A (x
a, y
a) represent
with
intersection point, B (x
b, y
b) represent
with
intersection point, C (x
c, y
c) represent
with
intersection point, D (x
d, y
d) represent
with
intersection point.Namely quadrilateral ABCD is the sound source region jointly determined by No. 1 and No. 3 Kinect sensor.Ask the center of gravity of quadrilateral ABCD for convenience of description, amplified.
With reference to Fig. 3, N (x
n, y
n) represent the center of gravity of triangle DAB, O (x
o, y
o) represent the center of gravity of triangle ABC, P (x
p, y
p) represent the center of gravity of triangle BCD, Q (x
q, y
q) represent the center of gravity of triangle CDA, R (x
r, y
r) represent the center of gravity of quadrilateral ABCD.
For quadrilateral ABCD, connect one bar diagonal line AC, so just quadrilateral ABCD is divided into the assembly of triangle ABC and triangle CDA, then the center of gravity of quadrilateral ABCD is on the line OQ of triangle ABC center of gravity O and triangle CDA center of gravity Q; In like manner, connect another diagonal line of quadrilateral ABCD BD, so just quadrilateral ABCD is divided into the assembly of triangle DAB and triangle BCD, the center of gravity of quadrilateral ABCD is equally on line segment NP, therefore have the center of gravity of quadrilateral ABCD on the intersection point of line segment OQ and NP, namely
point.
With reference to Fig. 4 and Fig. 5, namely quadrilateral EFGH is the sound source region jointly determined by No. 1 and No. 2 Kinect sensor, namely quadrilateral IJKL is the sound source region jointly determined by No. 2 and No. 3 Kinect sensor, tries to achieve quadrilateral EFGH center of gravity be according to above-mentioned same method
quadrilateral IJKL center of gravity is
Finally, the center of gravity of quadrilateral ABCD, EFGH, IJKL is asked
the average of three coordinates is the optimal location S (x of sound source
s, y
s).
According to the method described above, the center of gravity in the region that the center of gravity in the region that No. 1 and No. 2 mutual intersections of Kinect sensor are formed and No. 2 and No. 3 mutual intersections of Kinect sensor are formed can in like manner be tried to achieve.
2, optimum sound source position algorithm
Determine that the method for optimum sound source position comprises:
Step 1: the multiple parameter of initialization
The position coordinates of three Kinect sensor all can actually record, i.e. (x
k1, y
k1), (x
k2, y
k2) and (x
k3, y
k3) being known parameters, error angle α is set as 5 ° according to the technical indicator of Kinect and actual experiment, and sound source and three Kinect sensor are started working.
Step 2: ask straight-line intersection coordinate
Sound source stops sounding after sending the sound of lasting 50ms, preserves the sound source deflection β that No. 1 Kinect sensor obtains, the sound source deflection that No. 2 Kinect sensor obtain
, the sound source deflection γ that No. 3 Kinect sensor obtain.
Following straight-line equation can be listed according to point slope form:
Straight line
y=tan (45 ° of+β+α) x (1)
Straight line
y=tan (45 ° of+β-α) x (2)
Straight line
y=y
k3+ tan (135 ° of+γ+α) (x-x
k3) (3)
Straight line
y=y
k3+ tan (135 ° of+γ-α) (x-x
k3) (4)
Straight line
and straight line
intersection point be A (x
a, y
a), the system of equations that solution straight-line equation (1) and (4) forms obtains intersection point A (x
a, y
a) coordinate.Straight line
and straight line
intersection point be B (x
b, y
b), the system of equations that solution straight-line equation (2) and (4) forms obtains intersection points B (x
b, y
b) coordinate.Straight line
and straight line
intersection point be C (x
c, y
c), the system of equations that solution straight-line equation (2) and (3) forms obtains intersection point C (x
c, y
c) coordinate.Straight line
and straight line
intersection point be D (x
d, y
d), the system of equations that solution straight-line equation (1) and (3) forms obtains intersection point D (x
d, y
d) coordinate.
Step 3: ask irregular quadrilateral center of gravity
By triangle core coordinate formula,
Center of gravity N (the x of triangle DAB
n, y
n),
Center of gravity O (the x of triangle ABC
o, y
o),
Center of gravity P (the x of triangle BCD
p, y
p),
Center of gravity Q (the x of triangle CDA
q, y
q),
Following straight-line equation can be listed according to two point form:
Line segment OQ place straight-line equation:
Line segment NP place straight-line equation:
The intersection point of line segment OQ and NP is
the system of equations that solution straight-line equation (5) and (6) forms obtains quadrilateral ABCD center of gravity and is
in like manner, the center of gravity of quadrilateral EFGH, IJKL is tried to achieve according to the method for step 2 and step 3
Step 4: ask sound source optimal location
Sound source optimal location S (x
s, y
s) coordinate
The position of real sound source is represented with this.
3, robot Kinect sound localization navigation algorithm
With reference to Fig. 6, robot is equipped with sound source, sends sound in space, a certain moment, robot was positioned at W (x
w, y
w), robot target position is V (x
v, y
v).Robot is equipped with magnetic compass, robot self coordinate axis y can be measured
1with the angle of north orientation.Without loss of generality, if δ is the angle of coordinate axis y and north orientation.
φ represents the angle of robot deviation theory walking path, if the whole control system of robot and wheel structure all error free, then to keep straight on signal to robot one, make its certain distance of walking namely arrive target V (x
v, y
v), namely the theoretic track route of robot is as shown in figure WV solid line.And the actual robot one that gives keeps straight on signal, due to wheel structure error, robot always departs from its original orientation, and namely actual robot is walked along WV dotted line.
ε represents that robot finally arrives with impact point V (x
v, y
v) be the error-circular radius in the center of circle because there is the impact of other various factors in reality, the such as size of robot itself, the position that robot finally arrives not necessarily with impact point V (x
v, y
v) overlap completely, so require definition one circle of uncertainty according to realistic accuracy.As long as robot arrives with impact point V (x
v, y
v) be the center of circle, ε is in the circle of uncertainty of radius, then think that robot arrives impact point.Kinect sound localization robot navigation algorithm steps is as follows:
Step 1:
According to robot current location W (x
w, y
w) and target location V (x
v, y
v) angle theta of straight line WV and horizontal x-axis forward and the distance d of line segment WV can be calculated,
then in order to towards target V (x
v, y
v) motion, robot adjusts its y
1direction of principal axis and north orientation angle are 90 ° of-θ-δ.Robot is from starting point W (x
w, y
w) start walking.
Step 2:
Robot sends the sound of lasting 50ms every 1s.If a certain moment, robot was positioned at U (x
u, y
u), the Sounnd source direction information that computer disposal three Kinect sensor obtain obtains robot current location U (x
u, y
u), by this position by being wirelessly sent to robot.
Step 3:
Robot calculates current location U (x
u, y
u) and starting point W (x
w, y
w) the deviation angle φ of line WU and WV, robot adjusts its y automatically
1axle is towards the direction motion that deviation angle φ reduces.
Step 4:
Repeat step 2 to step 3, finally, the Sounnd source direction information obtained when computer disposal three Kinect sensor obtains robot current location with impact point V (x
v, y
v) be the center of circle, ε is in the circle of uncertainty of radius, then think that robot arrives target location, completes robot from starting point W (x
w, y
w) to impact point V (x
v, y
v) navigation task.
Step 5:
From a V (x
v, y
v) to impact point Z (x
z, y
z) navigation task repeat step 1 to 4.
Constantly repeat according to above-mentioned steps, the sound localization based on Kinect just can realize the continuous navigation task of robot, and the sound localization algorithm demonstrating above-mentioned Kinect is correct and effective.
Tu7Shi robot whole sound localization Navigation Control theory diagram, PC(PC) machine connects 3 Kinect sensor and a wireless data transmission module, wireless data transmission module is used for the communication between PC and robot.Robot comprises robot core processor, motor driving, magnetic compass, wireless data transmission module and sound source module.Robot core processor is used for the motion control of robot, and robot communicates with PC, pick-up transducers information etc.Motor drives the amplification being used for robot motor's power.Magnetic compass is used for the angle in the relatively geographical geographical north of robot measurement self.Sound source module is used for robot and sounds signal, locates to realize Robotics Sound by 3 Kinect sensor.
Foregoing is only the preferred embodiments of the present invention, and on this basis, those skilled in the art can make some distortion, and when not departing from thought of the present invention, these distortion also should within protection scope of the present invention.