CN103472434A - Robot sound positioning method - Google Patents

Abstract

The invention discloses a robot sound positioning method, which relates to sound positioning and robot navigation. At least two Kinect sensors are used to obtain the sound source direction detected by each Kinect, and to determine the deviation fan area where the sound source determined by each two Kinect sensors is located. , a total of three areas, the center of gravity of each area is obtained by the center of gravity method, and the average of the three centers of gravity is the optimal position of the sound source. Through this method, its positioning accuracy is improved, it has strong practicability and flexibility, and can be used in multiple fields such as sound positioning, robot navigation motion control, and the like.

Description

A method of robot sound localization

technical field

The invention belongs to the field of robots, and relates to a robot sound positioning method, which can be used in the fields of robot motion control, robot indoor positioning and navigation, and the like.

Background technique

Kinect is a three-dimensional (3D) somatosensory camera, and it introduces functions such as real-time motion capture, image recognition, microphone input, voice recognition, and community interaction. When Kinect was first released as an Xbox360 peripheral, skeletal tracking and speech recognition were the most popular features of the Kinect SDK for developers, but compared to skeletal tracking, the power of the microphone array in speech recognition was overlooked. This is partly due to the exciting skeletal tracking system in the Kinect, and partly due to the fact that Xbox game consoles and Kinect motion games don't take full advantage of the Kinect's audio processing.

The Kinect's microphone array is on the underside of the Kinect device. This array consists of 4 independent microphones distributed horizontally below the Kinect. Although each microphone captures the same audio signal, forming an array allows the direction of sound to be detected. Enables the identification of sounds coming from a particular direction. The audio data stream captured by the microphone array is processed by sophisticated audio enhancement algorithms to remove irrelevant background noise. All these complex operations are processed between the Kinect hardware and the Kinect SDK, which makes it possible to recognize voice commands and determine the source direction of the sound even if the person is a certain distance away from the Kinect in a large space.

Robot indoor positioning technology is a hot spot in the field of robot research, and it is also a difficult point. Researchers have proposed various methods. The typical one is RFID technology. Firstly, a smart space or sensor network space is built indoors, that is, an RFID tag is laid on the indoor ground at a certain distance in advance, and each RFID tag stores the information of its location. Absolute coordinates. Secondly, an RFID tag information reading device is installed on the mobile robot. When the robot moves to the RFID tag, the current position of the robot can be known by reading the coordinate data in the RFID tag. However, this positioning method has certain requirements on the environment, and the laying interval of RFID tags is different, and the positioning accuracy of the robot is also different. The accuracy of other positioning technologies is also affected by many factors. For example, the reckoning navigation method has a great dependence on the accuracy of the sensor and the motion system of the robot itself; WIFI, Bluetooth and other technologies also have certain requirements for the environment; sound positioning technology It is also used in robots, but it is limited to its complex processing and is easily affected by environmental noise, so the positioning accuracy is not high and it is difficult to promote; although the positioning accuracy of indoor map construction is high, the map construction process is complicated and the amount of calculation is large. Real-time Sex is difficult to satisfy.

Invention content:

The invention provides a robot sound positioning method, which is used to solve the problem of low positioning accuracy existing in the prior art.

On the one hand, a kind of robot sound localization method is provided, comprising: through at least two Kinects as sound sensors, obtain the source direction angle of the sound from robot; According to the position of described source direction angle and described at least two Kinects, determine Every two Kinect determined sound source place deviation fan areas; Determine the geometric center of gravity of the intersection area of every two deviation fan areas; According to the geometric center of gravity determined, the optimal position of the sound source is calculated by the geometric center of gravity method, the sound source The source optimal position is the position of the robot obtained by positioning.

Preferably, after the optimal position of the sound source is calculated by the geometric center of gravity method, the route of the robot's action is determined according to the target position and the optimal position of the sound source; the robot is controlled to move to the target position along the route .

Through the above solution, the position of the robot can be accurately and conveniently positioned.

Description of drawings

Fig. 1 is the overall schematic diagram of Kinect sound positioning;

Fig. 2 is a schematic diagram of the area where the sound source is jointly determined by No. 1 and No. 3 Kinect sensors;

Fig. 3 is a schematic diagram of the method for finding the center of gravity of a trapezoid;

Fig. 4 is a schematic diagram of the area where the sound source is jointly determined by No. 1 and No. 2 Kinect sensors;

Fig. 5 is a schematic diagram of the area where the sound source is jointly determined by No. 2 and No. 3 Kinect sensors;

Fig. 6 is the schematic diagram of realizing robot navigation through the sound positioning of Kinect;

Fig. 7 is a schematic block diagram of robot sound positioning and navigation control.

Detailed ways

The specific implementation process of the present invention will be described in detail below in conjunction with the accompanying drawings.

An embodiment of the present invention provides a method for positioning a robot, including:

(1) Arrange the three Kinect sensors in the form of a rectangular coordinate system. The sound source emits a sound for a certain period of time at a certain position in the coordinate system, and save the sound source direction angle obtained by the three Kinect sensors.

(2) Since each Kinect sensor has a certain deviation in receiving the sound signal, α is used to represent the deviation angle. The deviation range of the sound source direction detected by the Kinect sensor is [-α, α], which is called the deviation sector, which means that the two Kinect sensors determine The sound source area, this method is called the deviation fan method. Then use the geometric method to obtain the center of gravity of the intersection area formed by the deviation fan areas of every two Kinect sensors, which is called the geometric center of gravity method.

(3) Use the deviation sector method and the geometric center of gravity method to obtain the center of gravity of the area where the sound source is located for every two Kinect sensors.

(4) Send the optimal sound source position to the mobile robot, the robot obtains its own position information, adjusts its movement direction, and moves to the target location through sound positioning, thereby realizing the sound positioning and navigation of the mobile robot.

In the embodiment of the present invention, the audio processing capability of the Kinect sensor is used to detect the direction of the sound source to realize the indoor positioning of the robot.

Compared with the existing technology, the embodiment of the present invention has the following advantages:

Sound localization method is simple. The real-time requirements of the sound positioning system are very high. If a more complex algorithm is used, it will not be able to meet the real-time requirements of the sound positioning system. The present invention can quickly determine the position of the sound source by adopting the relatively general method of the center of gravity method.

Sound positioning accuracy is high. Make full use of the microphone array of the Kinect sensor and the background suppression and echo cancellation processing of the Kinect software driver to eliminate the influence of the virtual sound source and background noise, detect the direction of the real sound source, and obtain the best position of the sound source.

The positioning method is general. Since the Kinect is used to detect the source direction of the sound, even if there are obstacles between the Kinect and the sound source, the Kinect can still obtain the source direction of the sound; on the other hand, this positioning method can not only be used for indoor sound positioning, but also can be applied outdoors.

It can be applied to precise positioning and navigation motion control in the robot room.

The embodiment of the present invention also provides a robot positioning and navigation method, including:

1. Kinect sound area center of gravity positioning method

Referring to Figure 1, in order to make the detection of the sound source within the optimum detection angle range of the Kinect sensor, the arrangement of the three Kinect sensors is shown in the figure. The direction angle of the sound source obtained by the Kinect sensor is based on the midline, that is, the dotted line marked with the sound source angle reference line in the figure. Facing the Kinect sensor, the angle on the left side of the dotted line is a negative value, and the angle on the right side of the dotted line is a positive value. The sound source emits a sound at a frequency of 16KHz for 50ms at a certain position. The process of combining the deviation sector method and the regional center of gravity method to obtain the center of gravity of the sound source intersection area determined by the three Kinect sensors is as follows:

表示2号Kinect传感器探测到的声音来源方向角，l_k2表示2号Kinect传感器探测到的声源所在直线。γ表示3号Kinect传感器探测到的声音来源方向角，l_k3表示3号Kinect传感器探测到的声源所在直线。由上述可知β和

为负值，γ正值。Use β to represent the direction angle of the sound source detected by No. 1 Kinect sensor, l _k1 represents the straight line where the sound source detected by No. 1 Kinect sensor is located, so the real sound source is in the fan-shaped area where the straight line l _k1 is deflected by α degrees, and No. 2 and Kinect sensor number 3 is similar.

Indicates the direction angle of the sound source detected by the No. 2 Kinect sensor, l _k2 represents the straight line where the sound source detected by the No. 2 Kinect sensor. γ represents the direction angle of the sound source detected by No. 3 Kinect sensor, l _k3 represents the straight line where the sound source detected by No. 3 Kinect sensor. From the above we know that β and

is a negative value, and γ is a positive value.

(x _k1 , y _k1 ) represents the position coordinates of No. 1 Kinect sensor, (x _k2 , y _k2 ) represents the position coordinates of No. 2 Kinect sensor, (x _k3 , y _k3 ) represents the position coordinates of No. 3 Kinect sensor, in practice It is easy to measure, so they are all known parameters.

Figure 1 shows the direction of the sound source detected by the three Kinect sensors and the sound source area determined by the intersecting of the deviation sectors. Taking the area formed by the intersecting deviation sectors of the No. 1 and No. 3 Kinect sensors as an example, the intersecting area The method of finding the center of gravity is as follows:

表示1号Kinect传感器探测到声源所在直线逆时针偏转α度，

表示1号Kinect传感器探测到声源所在直线顺时针偏转α度；

表示3号Kinect传感器探测到声源所在直线逆时针偏转α度，

表示3号Kinect传感器探测到声源所在直线顺时针偏转α度。A(x_A,y_A)表示

和

的交点，B(x_B,y_B)表示

和

的交点，C(x_C,y_C)表示和

的交点，D(x_D,y_D)表示

和

的交点。四边形ABCD即是由1号和3号Kinect传感器共同确定的声源所在区域。为了方便说明求四边形ABCD的重心，将其放大。As shown in Figure 2,

Indicates that No. 1 Kinect sensor detects that the line where the sound source is located is deflected by α degrees counterclockwise,

Indicates that the line where the No. 1 Kinect sensor detects the sound source is deflected by α degrees clockwise;

Indicates that No. 3 Kinect sensor detects that the line where the sound source is located is deflected by α degrees counterclockwise,

Indicates that the line where the No. 3 Kinect sensor detects the sound source is deflected by α degrees clockwise. A(x _A ,y _A ) means

and

The intersection point, B(x _B ,y _B ) means

and

The intersection point, C(x _C ,y _C ) means and

The intersection point, D(x _D ,y _D ) means

and

intersection point. The quadrilateral ABCD is the area where the sound source is determined jointly by No. 1 and No. 3 Kinect sensors. To facilitate the description, find the center of gravity of the quadrilateral ABCD and enlarge it.

Referring to Fig. 3, N(x _N ,y _N ) represents the center of gravity of triangle DAB, O(x _O ,y _O ) represents the center of gravity of triangle ABC, P(x _P ,y _P ) represents the center of gravity of triangle BCD, Q(x _Q ,y _Q ) represents the center of gravity of triangle CDA, and R(x _R ,y _R ) represents the center of gravity of quadrilateral ABCD.

点。For the quadrilateral ABCD, connect its diagonal line AC, so that the quadrilateral ABCD is divided into the combination of the triangle ABC and the triangle CDA, then the center of gravity of the quadrilateral ABCD is on the connecting line OQ between the center of gravity O of the triangle ABC and the center of gravity of the triangle CDA; In this way, the quadrilateral ABCD is divided into a combination of triangle DAB and triangle BCD. The center of gravity of the quadrilateral ABCD is also on the line segment NP, so the center of gravity of the quadrilateral ABCD is on the line segment OQ and NP. at the point of intersection, that is

point.

四边形IJKL重心为

Referring to Fig. 4 and Fig. 5, the quadrilateral EFGH is the region where the sound source is determined jointly by the No. 1 and No. 2 Kinect sensors, and the quadrilateral IJKL is the region where the sound source is determined jointly by the No. 2 and No. 3 Kinect sensors. The method to obtain the center of gravity of the quadrilateral EFGH is

The center of gravity of the quadrilateral IJKL is

三个坐标的均值即为声源的最优位置S(x_S,y_S)。Finally, find the center of gravity of the quadrilateral ABCD, EFGH, IJKL

The mean value of the three coordinates is the optimal position S(x _S , y _S ) of the sound source.

According to the above method, similarly, the center of gravity of the area formed by the intersecting lines of No. 1 and No. 2 Kinect sensors and the center of gravity of the area formed by the intersecting lines of No. 2 and No. 3 Kinect sensors can be obtained.

2. Optimal sound source location algorithm

Methods for determining the optimal sound source location include:

Step 1: Initialize multiple parameters

The position coordinates of the three Kinect sensors can be actually measured, that is, (x _k1 , y _k1 ), (x _k2 , y _k2 ) and (x _k3 , y _k3 ) are known parameters, and the error angle α is based on the Kinect technology The index and the actual experiment are set to 5°, and the sound source and three Kinect sensors start to work.

Step 2: Find the coordinates of the line intersection point

，3号Kinect传感器获得的声音来源方向角γ。The sound source stops after the sound lasts for 50ms, and saves the sound source direction angle β obtained by the No. 1 Kinect sensor, and the sound source direction angle obtained by the No. 2 Kinect sensor

, the direction angle γ of the sound source obtained by No. 3 Kinect sensor.

According to the point-slope formula, the equation of a straight line can be listed as follows:

y=tan(45°+β+α)x    （1）straight line

y=tan(45°+β+α)x (1)

y=tan(45°+β-α)x    （2）straight line

y=tan(45°+β-α)x (2)

y=y_k3+tan(135°+γ+α)(x-x_k3)    （3）straight line

y=y _k3 +tan(135°+γ+α)(xx _k3 ) (3)

straight line y=y _k3 +tan(135°+γ-α)(xx _k3 ) (4)

和直线

的交点为A(x_A,y_A)，解直线方程（1）和（4）组成的方程组得交点A(x_A,y_A)的坐标。直线

和直线

的交点为B(x_B,y_B)，解直线方程（2）和（4）组成的方程组得交点B(x_B,y_B)的坐标。直线和直线

的交点为C(x_C,y_C)，解直线方程（2）和（3）组成的方程组得交点C(x_C,y_C)的坐标。直线和直线的交点为D(x_D,y_D)，解直线方程（1）和（3）组成的方程组得交点D(x_D,y_D)的坐标。straight line

and straight line

The intersection point of is A(x _A ,y _A ), and the coordinates of the intersection point A(x _A ,y _A ) can be obtained by solving the equation system composed of line equations (1) and (4). straight line

and straight line

The intersection point of is B(x _B ,y _B ), and the coordinates of the intersection point B(x _B ,y _B ) can be obtained by solving the system of equations composed of linear equations (2) and (4). straight line and straight line

The intersection point is C(x _C ,y _C ), and the coordinates of the intersection point C(x _C ,y _C ) can be obtained by solving the equation system composed of line equations (2) and (3). straight line and straight line The intersection point of is D(x _D ,y _D ), and the coordinates of the intersection point D(x _D ,y _D ) can be obtained by solving the equation system composed of line equations (1) and (3).

Step 3: Find the center of gravity of the trapezoid

From the coordinate formula of the center of gravity of a triangle,

The center of gravity N(x _N ,y _N ) of the triangle DAB, $x_N=\frac{x_{\mathrm{D.}}+x_A+{\mathrm{<figure-callout\; id="3"\; label="x\; B"\; filenames="HDA0000390088340000061.png"\; state="\{\{state\}\}">x</figure-callout>}}_B}{3},{\mathrm{the\; y}}_N=\frac{{\mathrm{the\; y}}_{\mathrm{D.}}+{\mathrm{the\; y}}_A+{\mathrm{the\; <figure-callout\; id="3"\; label="y\; B"\; filenames="HDA0000390088340000061.png"\; state="\{\{state\}\}">y</figure-callout>}}_B}{3},$

The center of gravity O(x _O ,y _O ) of the triangle ABC, $x_o=\frac{x_A+x_B+{\mathrm{<figure-callout\; id="3"\; label="x\; C"\; filenames="HDA0000390088340000061.png"\; state="\{\{state\}\}">x</figure-callout>}}_C}{3},{\mathrm{the\; y}}_o=\frac{{\mathrm{the\; y}}_A+{\mathrm{the\; y}}_B+{\mathrm{the\; <figure-callout\; id="3"\; label="y\; C"\; filenames="HDA0000390088340000061.png"\; state="\{\{state\}\}">y</figure-callout>}}_C}{3},$

Center of gravity P(x _P ,y _P ) of triangle BCD, $x_P=\frac{x_B+x_C+x_{\mathrm{D.}}}{3},{\mathrm{the\; y}}_P=\frac{{\mathrm{the\; y}}_B+{\mathrm{the\; y}}_C+{\mathrm{the\; y}}_{\mathrm{D.}}}{3},$

The center of gravity Q(x _Q ,y _Q ) of the triangle CDA, $x_Q=\frac{x_C+x_{\mathrm{D.}}+{\mathrm{<figure-callout\; id="3"\; label="x\; A"\; filenames="HDA0000390088340000061.png"\; state="\{\{state\}\}">x</figure-callout>}}_A}{3},{\mathrm{the\; y}}_Q=\frac{{\mathrm{the\; y}}_C+{\mathrm{the\; y}}_{\mathrm{D.}}+{\mathrm{the\; <figure-callout\; id="3"\; label="y\; A"\; filenames="HDA0000390088340000061.png"\; state="\{\{state\}\}">y</figure-callout>}}_A}{3}.$

According to the two-point formula, the equation of a straight line can be listed as follows:

The equation of the straight line where the line segment OQ is located: $\mathrm{the\; y}={\mathrm{the\; y}}_o+\frac{{\mathrm{the\; y}}_Q-{\mathrm{the\; y}}_o}{x_Q-x_o}(x-x_o)---\left(5\right)$

The equation of the straight line where the line segment NP is located: $\mathrm{the\; y}={\mathrm{the\; y}}_N+\frac{{\mathrm{the\; y}}_P-{\mathrm{the\; y}}_N}{x_P-x_N}(x-x_N)---\left(6\right)$

解直线方程（5）和（6）组成的方程组得四边形ABCD重心为

同理，按照步骤2和步骤3的方法求得求四边形EFGH、IJKL的重心

The intersection point of line segment OQ and NP is

Solving the system of equations composed of the straight line equations (5) and (6), the center of gravity of the quadrilateral ABCD is

In the same way, find the center of gravity of the quadrilateral EFGH and IJKL according to the method of step 2 and step 3

Step 4: Find the optimal position of the sound source

Optimal position S(x _S ,y _S ) coordinates of sound source $x_S=\frac{x_{R_1}+x_{{\mathrm{<figure-callout\; id="2"\; label="R"\; filenames="HDA0000390088340000011.png,HDA0000390088340000021.png"\; state="\{\{state\}\}">R</figure-callout>}}_2}+x_{{\mathrm{<figure-callout\; id="3"\; label="R"\; filenames="HDA0000390088340000061.png"\; state="\{\{state\}\}">R</figure-callout>}}_3}}{3}\text{,}{\mathrm{the\; y}}_S=\frac{{\mathrm{the\; y}}_{R_1}+{\mathrm{the\; <figure-callout\; id="2"\; label="y\; R"\; filenames="HDA0000390088340000011.png,HDA0000390088340000021.png"\; state="\{\{state\}\}">y</figure-callout>}}_{R_{}}+{\mathrm{the\; <figure-callout\; id="3"\; label="y\; R"\; filenames="HDA0000390088340000061.png"\; state="\{\{state\}\}">y</figure-callout>}}_{R_{}}}{3},$ This represents the position of the real sound source.

3. Robot Kinect voice positioning and navigation algorithm

Referring to Figure 6, the robot is equipped with a sound source, which sends sound into the space. At a certain moment, the robot is located at W(x _W ,y _W ), and the target position of the robot is V(x _V ,y _V ). The robot is equipped with a magnetic compass, which can measure the angle between the robot's own coordinate axis y ₁ and the north direction. Without loss of generality, let δ be the angle between the coordinate axis y and the north direction.

φ represents the angle at which the robot deviates from the theoretical walking trajectory. If there is no error in the entire control system and wheel structure of the robot, a straight signal is given to the robot to make it walk a certain distance to reach the target V(x _V , y _V ), that is, the robot The theoretical walking route is shown by the solid line WV in the figure. However, when the robot is actually given a straight-going signal, due to the structural error of the wheels, the robot will always deviate from the original direction, that is, the actual robot walks along the WV dotted line.

ε indicates that the robot finally reaches the radius of the error circle with the target point V(x _V , y _V ) as the center, because there are various other factors in practice, such as the size of the robot itself, the final position of the robot is not necessarily the same as the target point V(x _V , y _V ) coincide completely, so an error circle is defined according to the actual precision requirement. As long as the robot reaches the target point V(x _V , y _V ) as the center of the error circle and ε as the radius, the robot is considered to have reached the target point. Kinect voice positioning robot navigation algorithm steps are as follows:

step 1:

则为了朝目标V(x_V,y_V)运动，机器人调整其y₁轴方向和北向夹角为90°-θ-δ。机器人从起点W(x_W,y_W)开始行走。According to the current position of the robot W(x _W ,y _W ) and the target position V(x _V ,y _V ), the angle θ between the straight line WV and the positive direction of the horizontal x-axis and the distance d of the line segment WV can be calculated.

Then, in order to move towards the target V(x _V , y _V ), the robot adjusts the angle between the direction of its y ₁ axis and the north direction to be 90°-θ-δ. The robot starts walking from the starting point W(x _W ,y _W ).

Step 2:

The robot makes a sound lasting 50ms every 1s. Assuming that the robot is located at U(x _U ,y _U ) at a certain moment, the computer processes the sound source direction information obtained by the three Kinect sensors to obtain the current position U(x _U ,y _U ) of the robot, and sends the position to the robot wirelessly.

Step 3:

The robot calculates the deviation angle φ between the current position U(x _U ,y _U ) and the starting point W(x _W ,y _W ) between WU and WV, and the robot automatically adjusts its y ₁ axis to move in the direction where the deviation angle φ decreases .

Step 4:

Repeat steps 2 to 3. Finally, when the computer processes the sound source direction information obtained by the three Kinect sensors, the current position of the robot is within the error circle with the target point V(x _V , y _V ) as the center and ε as the radius, then It is considered that the robot has reached the target position, and the navigation task of the robot from the starting point W(x _W ,y _W ) to the target point V(x _V ,y _V ) is completed.

Step 5:

Steps 1 to 4 are repeated for the navigation task from point V(x _V ,y _V ) to target point Z(x _Z ,y _Z ).

By repeating the above steps continuously, the Kinect-based sound localization can realize the continuous navigation task of the robot, which proves that the above-mentioned Kinect sound localization algorithm is correct and effective.

Figure 7 is a block diagram of the robot's entire sound positioning and navigation control principle. Three Kinect sensors and a wireless data transmission module are connected to the PC (personal computer). The wireless data transmission module is used for communication between the PC and the robot. The robot includes a robot core processor, motor drive, magnetic compass, wireless data transmission module and sound source module. The core processor of the robot is used for the motion control of the robot, the communication between the robot and the PC, and the collection of sensor information. The motor drive is used to amplify the power of the robot motor. The magnetic compass is used to measure the angle between the robot itself and the geographic true north. The sound source module is used for the robot to send out sound signals, so as to realize the sound positioning of the robot with three Kinect sensors.

The above content is only a preferred embodiment of the present invention. On this basis, those skilled in the art can make some modifications, and these modifications should also be within the protection scope of the present invention without departing from the idea of the present invention.

Claims (2)

1. A robot sound localization method is characterized in that, comprising: Obtain the source direction angle of the sound from the robot by using at least two Kinects as sound sensors; According to the source direction angle and the positions of the at least two Kinects, determine the deviation fan area where the sound sources determined by every two Kinects are located; Determine the geometric center of gravity of the intersection region of every two deviation fan regions; According to the determined geometric center of gravity, the optimal position of the sound source is calculated by the geometric center of gravity method, and the optimal position of the sound source is the position of the robot obtained by positioning. 2. robot sound localization method according to claim 1, is characterized in that, after calculating sound source optimal position by geometric center of gravity method, described method also comprises: Determine the route of action of the robot according to the target position and the optimal position of the sound source; The robot is controlled to move to the target position along the route.

Images