Embodiment
Below in conjunction with accompanying drawing, specific implementation process of the present invention is elaborated.
The embodiment of the present invention provides a kind of positioning robot method, comprising:
(1) 3 Kinect sensors are discharged with the rectangular coordinate system form, the sound of certain time is sent in sound source a certain position in this coordinate system, preserves the sound source direction angle that three Kinect sensors obtain.
(2) because each Kinect sensor receives voice signal, certain deviation is arranged, mean misalignment angle with α, the sound source direction deviation range that the Kinect sensor detects is [α, α], be called the deviation covering of the fan, mean two sound source zones that the Kinect sensor is definite, this method is called deviation covering of the fan method.Then utilize method of geometry to try to achieve the center of gravity of the formed intersection region, deviation covering of the fan zone of every two Kinect sensors, be called the 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 definite sound source region of every two Kinect sensors, 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 the mobile robot, robot obtains the positional information of oneself, adjusts its direction of motion, moves to objective by sound localization, thereby 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 the sound detecting source direction is realized the robot indoor positioning.
Compared with prior art, the embodiment of the present invention has the following advantages:
The 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 fast the position of sound source.
The sound localization precision is high.Take full advantage of the microphone array of Kinect sensor and the background of Kinect software driver and suppress and echo cancellation process, eliminate the impact of virtual sound source and ground unrest, survey true Sounnd source direction, obtain the optimum position of sound source.
Localization method is general.Owing to being the source direction that utilizes the Kinect sound detecting, so, even barrier is arranged between Kinect and sound source, Kinect still can access the source direction of sound; On the other hand, this localization method not only can, for indoor sound localization, can be applicable to outdoor equally.
The accurate location and the Navigational Movements that can be applicable in robot chamber are controlled.
The embodiment of the present invention also provides a kind of robot location and air navigation aid, comprising:
1, Kinect sound zone centre of gravity definition method
With reference to Fig. 1, for the detection that makes sound source, all in the optimal detection angular range of Kinect sensor, the layout of three Kinect sensors as shown in the figure.The Kinect sensor obtains sound source direction angle and take its center line as reference, i.e. figure acceptance of the bid has the dotted line of sound source angle reference line, and in the face of the Kinect sensor, the angle in dotted line left side is negative value, the angle on dotted line right side be on the occasion of.Sound source sends with the frequency of 16KHz the sound that continues 50ms in a certain position.Deviation covering of the fan method and regional barycenter method are as follows in conjunction with obtaining the definite sound source intersection region center of gravity process of three Kinect sensors:
Mean the sound source direction angle that No. 1 Kinect sensor detects, l with β
k1mean the sound source place straight line that No. 1 Kinect sensor detects, therefore true sound source is at straight line l
k1in the sector region of deflection α degree, No. 2 and No. 3 Kinect sensor class are seemingly.
mean the sound source direction angle that No. 2 Kinect sensors detect, l
k2mean the sound source place straight line that No. 2 Kinect sensors detect.γ means the sound source direction angle that No. 3 Kinect sensors detect, l
k3mean the sound source place straight line that No. 3 Kinect sensors detect.From the above β and
for negative value, γ on the occasion of.
(x
k1, y
k1) mean Kinect sensor position coordinate, (x No. 1
k2, y
k2) mean Kinect sensor position coordinate, (x No. 2
k3, y
k3) mean Kinect sensor position coordinate No. 3, in reality, be easy to measure, so be known parameters.
Fig. 1 meaned sound source direction that three Kinect sensors detect and in twos the deviation covering of the fan mutually intersect determined sound source zone, the zone that the deviation covering of the fan of No. 1 and No. 3 Kinect sensor of take intersects to form mutually is example, and its intersection region center of gravity asks method as follows:
As Fig. 2,
mean that No. 1 Kinect sensor detects the counterclockwise deflection α of sound source place straight line degree,
mean that No. 1 Kinect sensor detects the clockwise deflection α of sound source place straight line degree;
mean that No. 3 Kinect sensors detect the counterclockwise deflection α of sound source place straight line degree,
mean that No. 3 Kinect sensors detect the clockwise deflection α of sound source place straight line degree.A(x
a, y
a) mean
with
intersection point, B (x
b, y
b) mean
with
intersection point, C (x
c, y
c) mean
with
intersection point, D (x
d, y
d) mean
with
intersection point.Quadrilateral ABCD is by No. 1 and No. 3 common definite sound source regions of Kinect sensor.Ask for convenience of description the center of gravity of quadrilateral ABCD, by its amplification.
With reference to Fig. 3, N (x
n, y
n) mean the center of gravity of triangle DAB, O (x
o, y
o) mean the center of gravity of triangle ABC, P (x
p, y
p) mean the center of gravity of triangle BCD, Q (x
q, y
q) mean the center of gravity of triangle CDA, R (x
r, y
r) mean the center of gravity of quadrilateral ABCD.
For quadrilateral ABCD, connect one bar diagonal line AC, so just quadrilateral ABCD is divided into to the assembly of triangle ABC and triangle CDA, 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 to the assembly of triangle DAB and triangle BCD, the center of gravity of quadrilateral ABCD is equally on line segment NP, therefore the center of gravity of quadrilateral ABCD is arranged on the intersection point of line segment OQ and NP,
the point.
With reference to Fig. 4 and Fig. 5, quadrilateral EFGH is by No. 1 and No. 2 common definite sound source regions of Kinect sensor, quadrilateral IJKL is by No. 2 and No. 3 common definite sound source regions of Kinect sensor, tries to achieve quadrilateral EFGH center of gravity according to above-mentioned same method and is
quadrilateral IJKL center of gravity is
Finally, ask the center of gravity of quadrilateral ABCD, EFGH, IJKL
the average of three coordinates is the optimal location S (x of sound source
s, y
s).
According to the method described above, in like manner can try to achieve No. 1 and the center of gravity in the zone that the center of gravity in the zone that No. 2 mutual intersections of Kinect sensor form and No. 2 and No. 3 mutual intersections of Kinect sensor form.
2, optimum sound source position algorithm
The method of determining optimum sound source position comprises:
Step 1: a plurality of parameters of initialization
The position coordinates of three Kinect sensors all can actually record, i.e. (x
k1, y
k1), (x
k2, y
k2) and (x
k3, y
k3) being known parameters, the error angle [alpha] is set as 5 ° according to technical indicator and the actual experiment of Kinect, and sound source and three Kinect sensors are started working.
Step 2: ask the straight-line intersection coordinate
Sound source stops sounding after sending the sound that continues 50ms, preserves the sound source direction angle β of No. 1 Kinect sensor acquisition, the sound source direction angle of No. 2 Kinect sensor acquisitions
, the sound source direction angle γ of No. 3 Kinect sensor acquisitions.
Can list following straight-line equation according to point slope form:
Straight line
y=tan (45 °+β+α) x (1)
Straight line
y=tan (45 °+β-α) x (2)
Straight line
y=y
k3+ tan (135 °+γ+α) (x-x
k3) (3)
Straight line
y=y
k3+ tan (135 °+γ-α) (x-x
k3) (4)
Straight line
and straight line
intersection point be A (x
a, y
a), the system of equations of separating straight-line equation (1) and (4) composition 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 of separating straight-line equation (2) and (4) composition 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 of separating straight-line equation (2) and (3) composition 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 of separating straight-line equation (1) and (3) composition obtains intersection point D (x
d, y
d) coordinate.
Step 3: ask the irregular quadrilateral center of gravity
By the 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),
Can list following straight-line equation 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 of separating straight-line equation (5) and (6) composition obtains quadrilateral ABCD center of gravity and is
in like manner, try to achieve the center of gravity of quadrilateral EFGH, IJKL according to the method for
step 2 and
step 3
Step 4: ask the sound source optimal location
Sound source optimal location S (x
s, y
s) coordinate
Represent the position of real sound source with this.
3, the Kinect of robot sound localization navigation algorithm
With reference to Fig. 6, sound source is housed in robot, send sound in space, a certain moment, robot was positioned at W (x
w, y
w), the robot target position is V (x
v, y
v).Magnetic compass is housed in robot, can measures the coordinate axis y of robot self
1angle with north orientation.Without loss of generality, establish the angle that δ is coordinate axis y and north orientation.
φ means the angle of robot deviation theory walking path, if the whole control system of robot and wheel structure are all error free, gives robot one craspedodrome signal, makes its walking certain distance arrive target V (x
v, y
v), the theoretic track route of robot is as shown in figure WV solid line.And the actual robot one craspedodrome signal of giving, due to the wheel structure error, robot always departs from its original orientation, and actual robot is walked along the WV dotted line.
ε means 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 factorss in reality, the size of robot itself for example, the position of the final arrival of robot not necessarily with impact point V (x
v, y
v) overlap fully, so according to realistic accuracy requirements definition one circle of uncertainty.As long as robot arrives with impact point V (x
v, y
v) be the center of circle, in the circle of uncertainty that ε is radius, think that robot has arrived impact point.Kinect sound localization robot navigation algorithm steps is as follows:
Step 1:
According to the current location W (x of robot
w, y
w) and target location V (x
v, y
v) can calculate the angle theta of straight line WV and horizontal x axle forward and line segment WV apart from d,
for towards target V (x
v, y
v) motion, robot adjusts its y
1direction of principal axis and north orientation angle are 90 °-θ-δ.Robot is from starting point W (x
w, y
w) start to walk.
Step 2:
Robot sends the sound that continues 50ms every 1s.If a certain moment, robot was positioned at U (x
u, y
u), the Sounnd source direction information that computing machine is processed three Kinect sensors acquisitions obtains the current location U (x of robot
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) line WU and the deviation angle φ of WV, robot adjusts its y automatically
1the direction motion that axle reduces towards deviation angle φ.
Step 4:
Repeating step 2 is to step 3, final, processes when computing machine Sounnd source direction information that three Kinect sensors obtain and obtains the robot current location with impact point V (x
v, y
v) be the center of circle, in the circle of uncertainty that ε is radius, think that robot has arrived target location, completed 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 repeating step 1 to 4.
According to above-mentioned steps, constantly repeat, the sound localization based on Kinect just can realize the continuous navigation task of robot, has proved that the sound localization algorithm of above-mentioned Kinect is correct and effective.
Fig. 7 is robot whole sound localization Navigation Control theory diagram, the PC(PC) connect 3 Kinect sensors and a wireless data transmission module on machine, wireless data transmission module is for the communication between PC and robot.Comprise robot core processor, motor driving, magnetic compass, wireless data transmission module and sound source module in robot.The robot core processor is for the motion control of robot, the communicating by letter of robot and PC, pick-up transducers information etc.Motor drives the amplification for robot motor's power.Magnetic compass is for the angle in the relatively geographical geographical north of robot measurement self.Sound source module is for the robot signal of sounding, in order to realize the robot sound localization with 3 Kinect sensors.
Foregoing is only the preferred embodiments of the present invention, and on this basis, those skilled in the art can make some distortion, in the situation that do not depart from thought of the present invention, these distortion also should be within protection scope of the present invention.