A method for mobile robot posture estimation based on acoustic signal arrival angle and relative displacement

Abstract

The present invention discloses a method for estimating the position and posture of the arrival angle and relative displacement of an acoustic signal. The method comprises the following steps: arranging a plurality of acoustic signal transmitting base stations in a working environment of a mobile robot, using a microphone array to receive in real time a custom signal transmitted by the acoustic signal transmitting base station; then using a multiple signal classification algorithm to extract the arrival angle for the custom signal received in real time, optimizing the mobile robot according to the arrival angle and the coordinates of the acoustic signal transmitting base station; finally, selecting a key frame signal according to the custom signal received in real time, calculating the distance between each two adjacent key frame signals, combining the distance between each two adjacent key frame signals and the preliminary posture with a graph optimization algorithm to obtain an optimized posture, which is the final posture of the mobile robot. The present invention can work in an indoor environment with complex lighting conditions or smoke at a relatively low cost, and realize high-precision and stable robot posture estimation.

Description

Technical Field

[0001] The invention belongs to the field of mobile robot positioning, and in particular relates to a method for estimating the position and posture of a mobile robot based on an acoustic signal arrival angle and relative displacement. Background Art

[0002] With the widespread application and popularity of mobile robots, the accuracy requirements for robot position-pose estimation are also increasing. Accurate position and pose estimation is the basis for robot autonomous navigation, path planning and task execution. The existing positioning methods have their own advantages and disadvantages:

[0003] LiDAR positioning: High-precision LiDAR is a relatively mature positioning solution. However, common transparent obstacles and reflective objects in indoor environments can seriously affect the measurement accuracy of LiDAR, resulting in increased positioning errors.

[0004] Visual positioning: Visual sensors have become a popular estimation method due to their moderate cost and compact size. However, environmental factors such as light and occlusion can significantly interfere with the visual positioning effect, and the use of cameras may cause privacy issues;

[0005] Ultra-wideband positioning: Ultra-wideband can achieve centimeter-level positioning accuracy under ideal conditions. However, the cost of ultra-wideband base stations is relatively high, the accuracy will drop significantly when the signal is blocked, and there is also the problem of being unable to obtain the posture. Summary of the invention

[0006] In order to solve the problems and shortcomings existing in the background technology, the main purpose of the present invention is to provide a method for estimating the posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal. By combining the acoustic signal with a microphone array and through the high-precision arrival angle estimation of the acoustic signal and the data fusion of multi-source sensors, high-precision measurement of the robot's posture is achieved. This is a low-cost and reliable method for estimating the posture of a mobile robot.

[0007] The technical solution adopted by the present invention is:

[0008] The method for estimating the position and posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal of the present invention comprises:

[0009] S1. Arrange several acoustic signal transmitting base stations in the working environment of the mobile robot. Each acoustic signal transmitting base station transmits a customized signal and uses a microphone array to receive the customized signal in real time.

[0010] S2. A multiple signal classification algorithm is used to extract the arrival angle of the customized signal received in real time, and the initial position and posture of the mobile robot is optimized based on the obtained arrival angle and the coordinates of the acoustic signal transmitting base station.

[0011] S3. Perform signal separation on the customized signal received in real time in step S1 to obtain a direct signal and a reverberation signal. Select the qualified direct signal as the key frame signal according to the direct signal and the reverberation signal, calculate the distance between each two adjacent key frame signals, and use a graph optimization algorithm to obtain an optimized posture in combination with the distance between each two adjacent key frame signals and the preliminary posture as the final posture of the mobile robot.

[0012] In step S1, the customized signal transmitted by the acoustic signal transmitting base station is set according to the following formula:

[0013] X(t)=exp(j×2×π( f 0 t+kt 2 / 2))

[0014] Where X(t) is the function of the customized signal transmitted by the acoustic signal transmitting base station over time, j is an imaginary unit, f 0 is the starting frequency, k is the modulation frequency, t is the time, and exp() is the natural exponential function.

[0015] The step S2 is specifically as follows:

[0016] S21. Preprocess the received customized signal using a fractional-order Fourier transform method.

[0017] S22. Calculate the arrival angle of the preprocessed customized signal using a multiple signal classification algorithm to obtain the arrival angles of several customized signals.

[0018] S23, substituting the coordinates of several acoustic signal transmitting base stations and the arrival angle of the customized signal into the optimization objective function, and calculating the initial position and posture of the mobile robot.

[0019] The step S22 is specifically as follows:

[0020] 1) Processing the customized signal after preprocessing in step S21 to obtain the covariance matrix of the customized signal.

[0021] The covariance matrix of the custom signal is calculated according to the following formula:

[0022] R=(1 / N)*∑ t=1 N (X(t)X H (t))

[0023] Where R is the covariance matrix of the custom signal, N is the number of sampling points, t is the time, X(t) is the custom signal, and X H (t) is the conjugate transpose of the custom signal X(t).

[0024] 2) Perform eigenvalue decomposition on the obtained covariance matrix of the customized signal to obtain the signal subspace quantity and noise subspace quantity of the customized signal.

[0025] The covariance matrix of the custom signal is subjected to eigenvalue decomposition according to the following formula:

[0026] R=E s Λ s E s H +E n Λ n E n H

[0027] Where R is the covariance matrix of the custom signal, E s H is the signal subspace quantity of the customized signal, E n H is the noise subspace quantity of the customized signal, E s and Λ s are the eigenvector and eigenvalue matrix of the signal subspace quantity, E n and Λ n are the eigenvector and eigenvalue matrix of the noise subspace quantity respectively.

[0028] 3) Constructing a spatial spectrum function according to the noise subspace quantity of the obtained customized signal.

[0029] The spatial spectrum function is set according to the following formula:

[0030] P(θ)=1 / (a H (θ)E n E n H a(θ)

[0031] Where θ is the arrival angle of the customized signal, P(θ) is the spatial spectrum function, and E n is the eigenvector of the noise subspace quantity, a(θ) is the array manifold vector, E n H is the noise subspace amount of the customized signal, a H (θ) is the conjugate transpose of the array manifold vector.

[0032] 4) All peak positions are searched according to the spatial spectrum function, and the arrival angles corresponding to all the searched peak positions are the arrival angles of all customized signals.

[0033] In step S23, the optimization objective function is set according to the following formula:

[0034] F=∑ i=1 N|arccos((tr(R P T R Z,i T R D,i )-1) / 2)| 2

[0035] Where F is the optimization objective function, N is the number of acoustic signal transmitting base stations, i is the index of the acoustic signal transmitting base station, and R P T is the rotation matrix of the mobile robot’s position, R Z,i T is the rotation matrix corresponding to the angle between the position of the mobile robot and the vector formed by the coordinates of all acoustic signal transmitting base stations, R D,i The rotation matrix of the arrival angle of the customized signal, tr() represents the matrix trace operation.

[0036] The step S3 is specifically as follows:

[0037] S31 , performing signal separation on the customized signal received by the microphone array in step S1 to obtain a direct signal and a reverberation signal respectively.

[0038] S32, respectively calculating the energy of the direct signal and the reverberation signal, and selecting the direct signal corresponding to the energy ratio of the direct signal to the reverberation signal being greater than an energy ratio threshold as the key frame signal.

[0039] S33. Calculate the distance between every two adjacent key frame signals by using a frequency modulated continuous wave method, that is, obtain the moving distance of the mobile robot between every two adjacent key frame signals.

[0040] S34. Substitute the moving distance obtained in step S33 and the preliminary posture obtained in step S2 into the graph optimization algorithm to optimize the posture of the mobile robot to obtain the optimized posture, which is the final posture of the mobile robot.

[0041] The frequency modulated continuous wave method in step S33 is set according to the following formula:

[0042] f p =(BR) / (cT)+(f min v+Bv) / c

[0043] In the formula, f p is the frequency component, R is the moving distance of the mobile robot between two adjacent key frame signals, B is the bandwidth of the customized signal, c is the speed of light, T is the frequency modulation period of the customized signal, v is the moving speed of the mobile robot, and f min is the minimum frequency.

[0044] In step S34, the graph optimization algorithm is specifically:

[0045] The preliminary pose obtained in step S2 is used as the graph node, and the moving distance obtained in step S33 is used as the edge, and the Gauss-Newton method is used for optimization.

[0046] The beneficial effects of the present invention are:

[0047] 1. The advancement of the present invention lies in using customized signals in the range of 16-20kHz as excitation signals and capturing the arrival angles of these signals through a microphone array to obtain spatial structure information of the environment.

[0048] 2. The customized signal provided by the present invention can, on the one hand, reduce interference to humans as much as possible, and on the other hand, compared with ultrasonic signals, which have weaker attenuation in the air, it can have better positioning accuracy while maintaining a larger working range.

[0049] 3. The present invention uses fractional Fourier transform and multiple signal classification algorithm to preprocess the signal and estimate the arrival angle, which can achieve high-precision posture estimation in complex indoor environments.

[0050] 4. The present invention combines the particle filter-based optimization algorithm and the graph optimization algorithm to further improve the real-time reliability of posture solution.

[0051] 5. The method for estimating the angle of arrival and relative displacement of acoustic signals proposed in the present invention has the advantages of simple deployment and low cost, and is particularly suitable for indoor environments with complex light blocking conditions or smoke obstruction. Therefore, the present invention has broad application prospects in multiple fields such as smart home, security inspection, and indoor navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 It is a flow chart of the method for estimating the position and posture of the acoustic signal arrival angle and relative displacement of the present invention;

[0053] Figure 2 is a time-frequency domain diagram of a customized signal used in the present invention;

[0054] Figure 3 It is a schematic diagram of optimizing posture using a graph optimization algorithm in the present invention. DETAILED DESCRIPTION

[0055] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments.

[0056] like Figure 1 As shown, the embodiments of the present invention and their implementation process are as follows:

[0057] S1. Arrange several acoustic signal transmitting base stations in the indoor working environment of the mobile robot. Each acoustic signal transmitting base station transmits a customized signal and uses a circular microphone array to receive the customized signal in real time.

[0058] The working environment of the mobile robot is set to obstacles, and the default customized signal is direct.

[0059] In step S1, the customized signal transmitted by the acoustic signal transmitting base station is set according to the following formula:

[0060] X(t)=exp(j×2×π( f 0 t+kt 2 / 2))

[0061] Where X(t) is the function of the customized signal transmitted by the acoustic signal transmitting base station over time, j is an imaginary unit, f 0 is the starting frequency, k is the modulation frequency, t is the time, and exp() is the natural exponential function.

[0062] like Figure 2 As shown, the customized signal set according to such a formula has a relatively wide frequency range and a strong anti-multipath capability. At the same time, its time-varying signal property also provides a theoretical basis for the inter-frame frequency modulation continuous wave algorithm.

[0063] In step S1, the starting frequency f of the customized signal transmitted by each acoustic signal transmitting base station is 0 Not less than 16kHz, starting frequency interval T int is 500-1000Hz, the signal bandwidth B is 1kHz, and the sampling rate f s The sampling rate of the signal is 48kHz, the duration T is 0.05-0.1s, the interval between two adjacent frames of customized signals is 0s, the number of elements in the circular microphone array is 6-8, the radius of the element is 0.01-0.03m, and the sampling rate of the element signal is 48kHz.

[0064] S2. A multiple signal classification algorithm is used to extract the arrival angle for the customized signal received in real time, and the initial position and posture of the mobile robot are optimized based on all the continuous arrival angles obtained and the coordinates of all the acoustic signal transmitting base stations.

[0065] Step S2 is specifically as follows:

[0066] S21. Preprocess the received customized signal using a fractional-order Fourier transform method.

[0067] S22. Calculate the arrival angle of the preprocessed customized signal using a multiple signal classification algorithm to obtain the arrival angles of several customized signals.

[0068] Step S22 is specifically as follows:

[0069] 1) Processing the customized signal after preprocessing in step S21 to obtain the covariance matrix of the customized signal.

[0070] The covariance matrix of the custom signal is calculated according to the following formula:

[0071] R=(1 / N)*∑ t=1 N (X(t)X H (t))

[0072] Where R is the covariance matrix of the custom signal, N is the number of sampling points, t is the time, X(t) is the custom signal, and X H (t) is the conjugate transpose of the custom signal X(t).

[0073] 2) Perform eigenvalue decomposition on the obtained covariance matrix of the customized signal to obtain the signal subspace quantity and noise subspace quantity of the customized signal.

[0074] The covariance matrix of the custom signal is eigenvalue decomposed according to the following formula:

[0075] R=E s Λ s E s H +E n Λ n E n H

[0076] Where R is the covariance matrix of the custom signal, E s H is the signal subspace quantity of the customized signal, E n H is the noise subspace quantity of the customized signal, E s and Λ s are the eigenvector and eigenvalue matrix of the signal subspace quantity, E n and Λ n are the eigenvector and eigenvalue matrix of the noise subspace quantity respectively.

[0077] 3) Constructing a spatial spectrum function according to the noise subspace quantity of the obtained customized signal.

[0078] The spatial spectrum function is set according to the following formula:

[0079] P(θ)=1 / (a H (θ)E n E n H a(θ)

[0080] Where θ is the arrival angle of the customized signal, P(θ) is the spatial spectrum function, and E n is the eigenvector of the noise subspace quantity, a(θ) is the array manifold vector, E n H is the noise subspace amount of the customized signal, a H (θ) is the conjugate transpose of the array manifold vector.

[0081] 4) All peak positions are searched according to the spatial spectrum function, and the arrival angles corresponding to all the searched peak positions are the arrival angles of all customized signals.

[0082] S23, substituting the coordinates of several acoustic signal transmitting base stations and the arrival angle of the customized signal into the optimization objective function, and calculating the initial position and posture of the mobile robot.

[0083] Methods for solving the preliminary pose include but are not limited to the particle filter method.

[0084] In step S23, the optimization objective function is set according to the following formula:

[0085] F=∑ i=1 N |arccos((tr(R P T R Z,i T R D,i )-1) / 2)| 2

[0086] Where F is the optimization objective function, N is the number of acoustic signal transmitting base stations, i is the index of the acoustic signal transmitting base station, and R P T is the rotation matrix of the mobile robot’s position, R Z,i T is the rotation matrix corresponding to the angle between the position of the mobile robot and the vector formed by the coordinates of all acoustic signal transmitting base stations, R D,i The rotation matrix of the arrival angle of the customized signal, tr() represents the matrix trace operation.

[0087] S3. Perform signal separation on the customized signal received in real time in step S1 to obtain a direct signal and a reverberation signal. Select the qualified direct signal as the key frame signal according to the direct signal and the reverberation signal, calculate the distance between each two adjacent key frame signals, and use a graph optimization algorithm to obtain an optimized posture in combination with the distance between each two adjacent key frame signals and the preliminary posture as the final posture of the mobile robot.

[0088] Step S3 is specifically as follows:

[0089] S31 , performing signal separation on the customized signal received by the microphone array in step S1 to obtain a direct signal and a reverberation signal respectively.

[0090] S32, respectively calculating the energy of the direct signal and the reverberation signal, and selecting the direct signal corresponding to the energy ratio of the direct signal to the reverberation signal being greater than an energy ratio threshold as the key frame signal.

[0091] The energy ratio threshold is set empirically.

[0092] S33. Calculate the distance between every two adjacent key frame signals by using a frequency modulated continuous wave method, that is, obtain the moving distance of the mobile robot between every two adjacent key frame signals.

[0093] The FMCW method is as follows:

[0094] Mix each two adjacent key frame signals to obtain a function of the difference frequency signal, draw a function spectrum of the difference frequency signal of each two adjacent key frame signals, select the first peak value from the drawn spectrum as the value of the frequency component, and solve the moving distance of the mobile robot between each two adjacent key frame signals.

[0095] The frequency modulated continuous wave method in step S33 is set according to the following formula:

[0096] f p =(BR) / (cT)+(f min v+Bv) / c

[0097] In the formula, f p is the frequency component, R is the moving distance of the mobile robot between two adjacent key frame signals, B is the bandwidth of the customized signal, c is the speed of light, T is the frequency modulation period of the customized signal, v is the moving speed of the mobile robot, and f min is the minimum frequency.

[0098] S34. Substitute the moving distance obtained in step S33 and the preliminary posture obtained in step S2 into the graph optimization algorithm to optimize the posture of the mobile robot to obtain the optimized posture, which is the final posture of the mobile robot.

[0099] Step S34, the graph optimization algorithm is specifically as follows:

[0100] like Figure 3 As shown, the initial pose P obtained in step S2 is i As a graph node, the moving distance R obtained in step S33 is used as an edge and optimized using the Gauss-Newton method.

[0101] The present invention uses fractional Fourier transform and multiple signal classification algorithm to preprocess the signal and estimate the arrival angle, which can achieve high-precision posture estimation in complex indoor environments. Combined with the particle filter-based optimization algorithm and graph optimization algorithm, the real-time reliability of posture solution is further improved.

[0102] The method for estimating the angle of arrival and relative displacement of acoustic signals proposed in the present invention has the advantages of simple deployment and low cost, and is particularly suitable for indoor environments with complex light blocking conditions or smoke obstruction. Therefore, the present invention has broad application prospects in multiple fields such as smart home, security inspection, and indoor navigation.

Claims

1. A method for estimating the position and posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal, characterized in that: Methods include: S1. Arrange a number of acoustic signal transmitting base stations in the working environment of the mobile robot, each of which transmits a customized signal and uses a microphone array to receive the customized signal in real time; S2. Use a multiple signal classification algorithm to extract the arrival angle for the customized signal received in real time, and optimize the initial position and posture of the mobile robot based on the obtained arrival angle and the coordinates of the acoustic signal transmitting base station; S3. Perform signal separation on the customized signal received in real time in step S1 to obtain a direct signal and a reverberation signal. Select the qualified direct signal as the key frame signal according to the direct signal and the reverberation signal, calculate the distance between each two adjacent key frame signals, and use a graph optimization algorithm to obtain an optimized posture in combination with the distance between each two adjacent key frame signals and the preliminary posture as the final posture of the mobile robot.

2. The method for estimating the position and posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal according to claim 1, characterized in that: In step S1, the customized signal transmitted by the acoustic signal transmitting base station is set according to the following formula: X(t)=exp(j×2×π( f 0t+kt 2 / 2)) Where X(t) is the function of the customized signal transmitted by the acoustic signal transmitting base station over time, j is an imaginary unit, f 0 is the starting frequency, k is the modulation frequency, t is the time, and exp() is the natural exponential function.

3. The method for estimating the position and posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal according to claim 1, characterized in that: The step S2 is specifically as follows: S21, preprocessing the received customized signal using a fractional-order Fourier transform method; S22, calculating the arrival angle of the preprocessed customized signal using a multiple signal classification algorithm to obtain the arrival angles of several customized signals; S23, substituting the coordinates of several acoustic signal transmitting base stations and the arrival angle of the customized signal into the optimization objective function, and calculating the initial position and posture of the mobile robot.

4. The method for estimating the position and posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal according to claim 3, characterized in that: The step S22 is specifically as follows: 1) Processing the customized signal preprocessed in step S21 to obtain a covariance matrix of the customized signal; The covariance matrix of the custom signal is calculated according to the following formula: R=(1 / N)*∑ t=1 N (X(t)X H (t)) Where R is the covariance matrix of the custom signal, N is the number of sampling points, t is the time, X(t) is the custom signal, and X H (t) is the conjugate transpose of the custom signal X(t); 2) performing eigenvalue decomposition on the obtained covariance matrix of the customized signal to obtain the signal subspace quantity and the noise subspace quantity of the customized signal; The covariance matrix of the custom signal is subjected to eigenvalue decomposition according to the following formula: R=E s Λ s AND s H +E n Λ n AND n H Where R is the covariance matrix of the custom signal, E s H is the signal subspace quantity of the customized signal, E n H is the noise subspace quantity of the customized signal, E s and Λ s are the eigenvector and eigenvalue matrix of the signal subspace quantity, E n and Λ n are the eigenvector and eigenvalue matrix of the noise subspace quantity respectively; 3) constructing a spatial spectrum function according to the noise subspace quantity of the obtained customized signal; The spatial spectrum function is set according to the following formula: P(θ)=1 / (a H (i)E n E n H a(i)) Where θ is the arrival angle of the customized signal, P(θ) is the spatial spectrum function, and E n is the eigenvector of the noise subspace quantity, a(θ) is the array manifold vector, E n H is the noise subspace amount of the customized signal, a H (θ) is the conjugate transpose of the array manifold vector; 4) All peak positions are searched according to the spatial spectrum function, and the arrival angles corresponding to all the searched peak positions are the arrival angles of all customized signals.

5. The method for estimating the position and posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal according to claim 3, characterized in that: In step S23, the optimization objective function is set according to the following formula: F=∑ i=1 N |arccos((tr(R P T R Z,i T R D,i )-1) / 2)| 2 Where F is the optimization objective function, N is the number of acoustic signal transmitting base stations, i is the index of the acoustic signal transmitting base station, and R P T is the rotation matrix of the mobile robot’s position, R Z,i T is the rotation matrix corresponding to the angle between the position of the mobile robot and the vector formed by the coordinates of all acoustic signal transmitting base stations, R D,i The rotation matrix of the arrival angle of the customized signal, tr() represents the matrix trace operation.

6. The method for estimating the position and posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal according to claim 1, characterized in that: The step S3 is specifically as follows: S31, performing signal separation on the customized signal received by the microphone array in step S1 to obtain a direct signal and a reverberation signal respectively; S32, respectively calculating the energy of the direct signal and the reverberation signal, and selecting the direct signal corresponding to the energy ratio of the direct signal to the reverberation signal being greater than the energy ratio threshold as the key frame signal; S33, using a frequency modulated continuous wave method to calculate the distance between each two adjacent key frame signals, that is, to obtain the moving distance of the mobile robot between each two adjacent key frame signals; S34. Substitute the moving distance obtained in step S33 and the preliminary posture obtained in step S2 into the graph optimization algorithm to optimize the posture of the mobile robot to obtain the optimized posture, which is the final posture of the mobile robot.

7. The method for estimating the position and posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal according to claim 6, characterized in that: The frequency modulated continuous wave method in step S33 is set according to the following formula: f p =(BR) / (cT)+(f min v+Bv) / c In the formula, f p is the frequency component, R is the moving distance of the mobile robot between two adjacent key frame signals, B is the bandwidth of the customized signal, c is the speed of light, T is the frequency modulation period of the customized signal, v is the moving speed of the mobile robot, and f min is the minimum frequency.

8. The method for estimating the position and posture of a mobile robot based on the arrival angle and relative displacement of an acoustic signal according to claim 6, characterized in that: In step S34, the graph optimization algorithm is specifically: The preliminary pose obtained in step S2 is used as the graph node, and the moving distance obtained in step S33 is used as the edge, and the Gauss-Newton method is used for optimization.

Images