A system and method for air-ground collaborative sound source localization based on RANSAC error compensation

Abstract

The present invention discloses an air-ground collaborative sound source positioning system and method based on RANSAC error compensation, the system comprises an aerial unmanned platform, a ground unmanned platform and a communication device, wherein the aerial unmanned platform is equipped with airborne sensors and communication devices for obtaining inertial navigation and GNSS positioning data, and sending absolute position information to the ground unmanned platform; the ground unmanned platform is equipped with inertial sensors, sound source pickup devices and communication devices for detecting the sound emitted by the target sound source, determining the relative position information of the aerial unmanned platform and the target and sending it to the aerial unmanned platform, and correcting the inertial navigation solution error, constraining the error divergence, and realizing the collaborative navigation of the aerial unmanned platform and the ground unmanned platform; the communication device is used to transmit the collaborative navigation data. The present invention realizes air-ground collaborative positioning and improves the detection capability and positioning accuracy of the ground unmanned platform in dense and complex environments.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned system collaborative navigation, and in particular to an air-to-ground collaborative sound source localization system and method based on RANSAC error compensation. Background Art

[0002] The development of drones and unmanned vehicles is an important part of modern science and technology. With the continuous development and progress of science and technology, drones and unmanned vehicles are also constantly improving and upgrading. Drones and unmanned vehicles have a wide range of applications. Drones and ground unmanned platforms can be used for aerial photography, logistics distribution, environmental monitoring, rescue and search, etc., which can save a lot of manpower and material resources. At present, drones and ground unmanned platforms mainly use optical photography to find people to be rescued. In complex environments such as forests, Gobi, and mountains, the target is easily blocked by environmental objects, making it difficult to find the rescue object.

[0003] Traditional unmanned platforms usually use GNSS / INS or INS / SLAM combined navigation methods for positioning and navigation. When there is a GNSS signal, the positioning accuracy is high. However, when the unmanned platform is in an environment with weak or no GNSS signal, the inertial device error cannot be corrected, and the positioning error will increase over time. In scenes such as dense forests, it is difficult for visual cameras to capture reliable feature points, and SLAM cannot calculate the effective posture information of the unmanned platform, resulting in the inability to constrain the error divergence of the inertial device. In these scenarios, the positioning accuracy of the unmanned platform will be poor.

[0004] Compared with a single unmanned platform, multiple unmanned platforms can achieve collaborative navigation through sound and other means to improve positioning accuracy. At present, there are many studies on collaborative navigation of aerial unmanned platforms, but there are still the following problems in the collaborative navigation of aerial and ground unmanned platforms: ① In a GNSS-denied environment, the inertial navigation errors of multiple unmanned platforms diverge rapidly, making it difficult to maintain formation; ② The traditional geometric constraint method requires unified height calculation, but the height and speed of aerial and ground unmanned platforms do not match, resulting in large navigation errors; ③ Relying on the GNSS system, it is difficult to achieve collaborative navigation in complex environments such as hills, forests, and cities. Summary of the invention

[0005] The purpose of the present invention is to provide an air-to-ground collaborative sound source positioning and detection system and method based on RANSAC error compensation, so as to achieve high-precision collaborative navigation positioning and target detection tasks under the conditions of weak GNSS signals and limited photoelectric detection.

[0006] The technical solution to achieve the purpose of the present invention is: an air-ground collaborative sound source localization system based on RANSAC error compensation, including an air unmanned platform, a ground unmanned platform and communication equipment;

[0007] The aerial unmanned platform is equipped with airborne sensors and communication equipment, wherein the airborne sensors include inertial navigation and GNSS, which are used to obtain inertial navigation and GNSS positioning data; the communication equipment is used to obtain the sound source positioning information sent by the ground unmanned platform, and calculate the absolute position information of the target and the ground unmanned platform through its own positioning, and send the absolute position information to the ground unmanned platform while flying towards the target;

[0008] The ground unmanned platform is equipped with an inertial sensor, a sound source pickup device and a communication device, wherein the sound source pickup device is used to detect the sound emitted by the target sound source, and determine the relative position of the target by a sound source positioning method based on RANSAC error compensation; when the aerial unmanned platform enters the pickup range of the sound source pickup device, the ground unmanned platform relatively positions the aerial unmanned platform by a sound source positioning method based on RANSAC error compensation, and simultaneously sends the relative position of the aerial unmanned platform and the target to the aerial unmanned platform; after receiving its own absolute position information, the ground unmanned platform corrects the inertial navigation solution error, constrains the error divergence, improves its own navigation and positioning accuracy, realizes the collaborative navigation of the aerial unmanned platform and the ground unmanned platform, and moves towards the detection target at the same time;

[0009] The communication device is composed of a data transmission module, which is used to transmit collaborative navigation data, including the absolute position information of the ground unmanned platform measured by the aerial unmanned platform, and the relative position information of the target measured by the ground platform and the aerial unmanned platform.

[0010] Furthermore, the sound source pickup device is composed of 4 microphone sound sensors and a signal processing board, wherein the 4 microphones form a 4-element cross microphone array.

[0011] Furthermore, the ground unmanned platform measures the relative position information of the aerial unmanned platform by means of sound source positioning and sends it to the aerial unmanned platform. The aerial unmanned platform calculates the absolute position information of the ground unmanned platform based on its own absolute position information and the relative position information sent by the ground unmanned platform and sends it to the ground unmanned platform. The ground unmanned platform integrates the absolute position information with the positioning information solved by its own inertial navigation to achieve collaborative positioning of the air-ground unmanned platforms.

[0012] A method for air-ground collaborative sound source localization based on RANSAC error compensation comprises the following steps:

[0013] Step 1: In a field environment, use a sound source pickup device carried by a ground unmanned platform to measure the positioning result error of the UAV at different distances;

[0014] Step 2: Use the RANSAC algorithm to fit the data obtained in step 1 to obtain a relationship model between distance and positioning result error;

[0015] Step 3: The ground unmanned platform itself is used as the coordinate origin to establish a three-dimensional rectangular coordinate system and use inertial sensors to obtain its own position information;

[0016] Step 4: The ground unmanned platform receives the sound source information through the sound source pickup device carried by it, and detects the relative position information of the target and the aerial unmanned platform using the sound source positioning method based on random consistent error compensation according to the relationship model between the distance and the positioning result error;

[0017] Step 5: The ground unmanned platform sends the relative position information of the aerial unmanned platform and the relative position information of the target to the aerial unmanned platform;

[0018] Step 6: The aerial unmanned platform uses airborne sensors to collect inertial navigation data information and GNSS positioning data information, and uses the processor on the aerial unmanned platform to perform position calculation to obtain the current absolute position information of the aerial unmanned platform;

[0019] Step 7: The aerial unmanned platform determines whether there is collaborative navigation data input. If there is no collaborative navigation data, the aerial unmanned platform updates its own posture according to the data of the inertial sensor; if there is collaborative navigation data input, it proceeds to step 8;

[0020] Step 8: The aerial unmanned platform calculates the absolute positions of the target and the ground unmanned platform through its own absolute position and the relative position relationship among itself, the target and the ground unmanned platform, and then sends the absolute positions of the ground unmanned platform and the target to the ground unmanned platform, and flies to the detection target position;

[0021] Step 9: The ground unmanned platform uses the collaborative navigation data and adopts Kalman filtering to update the posture, realizes air-ground collaborative navigation based on sound source positioning with random consistent error compensation, and moves to the detection target position.

[0022] Compared with the prior art, the present invention has the following significant advantages: (1) a ground unmanned platform is used to measure the sound of the flight of an aerial unmanned platform and the target, and relative positioning of the target and the aerial unmanned platform is performed to obtain relative position information. The absolute position information of the target and the ground unmanned platform is inferred through the absolute position information of the aerial unmanned platform itself, thereby guiding the aerial unmanned platform to fly to the detection target while constraining the positioning error of the ground unmanned platform, thereby realizing collaborative navigation and target detection of aerial and ground unmanned platforms in a GNSS-denied environment; (2) a sound source positioning method based on error compensation of random sampling compensation improves the accuracy of sound source positioning in a specific environment, and improves the accuracy of air-ground collaborative navigation; (3) the accuracy of target positioning and detection in environments such as forests and mountains where photoelectric detection means are limited is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1It is a structural schematic diagram of an air-to-ground collaborative sound source localization system based on RANSAC error compensation of the present invention.

[0024] Figure 2 It is a schematic diagram of the principle of the four-element cross microphone array in the present invention.

[0025] Figure 3 The present invention is a flowchart of a random sampling consistent fitting algorithm in air-to-ground collaborative sound source localization based on RANSAC error compensation.

[0026] Figure 4 It is a curve diagram of the effect of sound source localization error compensation in an embodiment of the present invention.

[0027] Figure 5 It is an analysis curve diagram of the positioning error of the ground unmanned platform in an embodiment of the present invention. DETAILED DESCRIPTION

[0028] The present invention provides an air-ground collaborative sound source localization system and method based on RANSAC error compensation. RANSAC is a Random Sample Consensus algorithm, which is an algorithm that calculates mathematical model parameters of data based on a set of sample data sets containing abnormal data to obtain valid sample data, and is often used in computer vision. When fitting the model, RANSAC can ignore data points that do not conform to the model, has good robustness for data with noise and outliers, and the algorithm is relatively simple, easy to understand and implement.

[0029] The present invention aims at the problems that the ground GNSS signal is weak, the photoelectric detection means are limited, the inertial navigation error of the ground unmanned platform diverges quickly, and the target cannot be accurately positioned and detected. The improved sound source positioning is used to realize air-ground collaborative positioning and detection. Specifically, the ground unmanned platform is used to locate the aerial unmanned platform and the detection target, and the data transmission module is used for communication. The aerial unmanned platform uses the received relative positioning information of itself and the target combined with the absolute position given by its own GNSS system to calculate the absolute position of the target and the ground unmanned platform, and sends the calculated position to the ground unmanned platform; the ground unmanned platform fuses the received position information with the information solved by its own inertial navigation, constrains the divergence of inertial navigation errors, improves positioning accuracy, and realizes collaborative navigation in a GNSS-restricted environment; for the error compensation of sound source positioning, the RANSAC algorithm is used, and the azimuth error and distance of the sound source positioning are fitted for a specific environment to obtain an error compensation model. After the relative azimuth, pitch angle and distance obtained by sound source positioning in actual use, the azimuth is compensated according to the error model to improve the relative positioning accuracy. The air-to-ground collaborative sound source localization system and method based on RANSAC error compensation proposed by the present invention can perform collaborative navigation in complex and dense environments such as hills, forests, mountains, and swamps, suppress the error divergence of the navigation system, have strong anti-interference ability, and can detect and locate the target in time. The technical solution of the present invention is described in detail below.

[0030] It is easy to understand that according to the technical solution of the present invention, without changing the essential spirit of the present invention, a person skilled in the art can imagine various embodiments of the present invention. Therefore, the following specific embodiments and drawings are only exemplary descriptions of the technical solution of the present invention, and should not be regarded as the whole of the present invention or as a limitation or limitation of the technical solution of the present invention. It should be noted that, unless otherwise specifically stated, the relative arrangement of the components and steps, numerical expressions and numerical values ​​described in these embodiments do not limit the scope of the present invention.

[0031] The following description of at least one exemplary embodiment is merely illustrative in nature and is in no way intended to limit the invention, its application, or uses.

[0032] Technologies, methods, and equipment known to ordinary technicians in the relevant art may not be discussed in detail, but where appropriate, the technologies, methods, and equipment should be considered part of the specification.

[0033] In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not limiting. Therefore, other examples of the exemplary embodiments may have different values.

[0034] Combination Figure 1, the present invention provides an air-ground collaborative sound source localization system based on RANSAC error compensation, including an air unmanned platform, a ground unmanned platform and communication equipment;

[0035] The aerial unmanned platform is equipped with airborne sensors and communication equipment, wherein the airborne sensors include inertial navigation and GNSS, which are used to obtain inertial navigation and GNSS positioning data; the communication equipment is used to obtain the sound source positioning information sent by the ground unmanned platform, and calculate the absolute position information of the target and the ground unmanned platform through its own positioning, and send the absolute position information to the ground unmanned platform while flying towards the target;

[0036] The ground unmanned platform is equipped with an inertial sensor, a sound source pickup device and a communication device, wherein the sound source pickup device is used to detect the sound emitted by the target sound source, and determine the relative position of the target by a sound source positioning method based on RANSAC error compensation; when the aerial unmanned platform enters the pickup range of the sound source pickup device, the ground unmanned platform relatively positions the aerial unmanned platform by a sound source positioning method based on RANSAC error compensation, and simultaneously sends the relative position of the aerial unmanned platform and the target to the aerial unmanned platform; after receiving its own absolute position information, the ground unmanned platform corrects the inertial navigation solution error, constrains the error divergence, improves its own navigation and positioning accuracy, realizes the collaborative navigation of the aerial unmanned platform and the ground unmanned platform, and moves towards the detection target at the same time;

[0037] The communication device is composed of a data transmission module, which is used to transmit collaborative navigation data, including the absolute position information of the ground unmanned platform measured by the aerial unmanned platform, and the relative position information of the target measured by the ground platform and the aerial unmanned platform.

[0038] As a specific example, the sound source pickup device is composed of 4 microphone sound sensors and a signal processing board, wherein the 4 microphones form a 4-element cross microphone array.

[0039] As a specific example, the aerial unmanned platform is a quad-rotor drone, which carries a GNSS positioning system and can communicate with a ground unmanned platform through a data transmission module. After obtaining the collaborative navigation data and detection target information given by the improved sound source positioning, the absolute position information given by the GNSS positioning system is combined to calculate the absolute position of the target and the ground unmanned platform, and the absolute position information is sent to the ground unmanned platform while flying toward the target;

[0040] As a specific example, the ground unmanned platform is a ground mobile robot that can operate flexibly in complex environments and has high stability; the ground unmanned platform detects the relative position of the target point through sound source positioning based on RANSAC error compensation. After the aerial unmanned platform approaches the ground unmanned platform, the ground unmanned platform detects the relative position of the aerial unmanned platform through improved sound source positioning, and sends the positions of the target and the aerial unmanned platform to the aerial unmanned platform; after receiving the collaborative navigation information sent by the aerial unmanned platform, the ground unmanned platform improves its own navigation positioning accuracy and moves to the target position through information fusion.

[0041] As a specific example, the communication device is composed of a data transmission module for data transmission, has high anti-interference ability, and can communicate at a higher frequency without affecting the normal position update of the system.

[0042] As a specific example, the collaborative navigation of the aerial unmanned platform and the ground unmanned platform is specifically as follows:

[0043] When a drone appears within the sound pickup range of the sound source pickup device carried by the ground unmanned platform, the ground unmanned platform calculates the relative position of the drone through the sound source positioning method based on RANSAC error compensation, and sends the position information to the aerial unmanned platform through the communication device. The aerial unmanned platform combines the position information with the absolute position obtained by its own GNSS positioning system to calculate the absolute positions of the ground unmanned platform and the target, while flying towards the target position; it combines the system observation quantity with the result calculated by the inertial sensor carried by itself, suppresses the error divergence of the aerial platform navigation system, and improves the positioning accuracy.

[0044] A method for air-ground collaborative sound source localization based on RANSAC error compensation comprises the following steps:

[0045] Step 1: In a field environment, use a sound source pickup device carried by a ground unmanned platform to measure the positioning result error of the UAV at different distances;

[0046] Step 2: Use the RANSAC algorithm to fit the data obtained in step 1 to obtain a relationship model between distance and positioning result error;

[0047] Step 3: The ground unmanned platform itself is used as the coordinate origin to establish a three-dimensional rectangular coordinate system and use inertial sensors to obtain its own position information;

[0048] Step 4: The ground unmanned platform receives the sound source information through the sound source pickup device carried by it, and detects the relative position information of the target and the aerial unmanned platform using the sound source positioning method based on random consistent error compensation according to the relationship model between the distance and the positioning result error;

[0049] Step 5: The ground unmanned platform sends the relative position information of the aerial unmanned platform and the relative position information of the target to the aerial unmanned platform;

[0050] Step 6: The aerial unmanned platform uses airborne sensors to collect inertial navigation data information and GNSS positioning data information, and uses the processor on the aerial unmanned platform to perform position calculation to obtain the current absolute position information of the aerial unmanned platform;

[0051] Step 7: The aerial unmanned platform determines whether there is collaborative navigation data input. If there is no collaborative navigation data, the aerial unmanned platform updates its own posture according to the data of the inertial sensor; if there is collaborative navigation data input, it proceeds to step 8;

[0052] Step 8: The aerial unmanned platform calculates the absolute positions of the target and the ground unmanned platform through its own absolute position and the relative position relationship among itself, the target and the ground unmanned platform, and then sends the absolute positions of the ground unmanned platform and the target to the ground unmanned platform, and flies to the detection target position;

[0053] Step 9: The ground unmanned platform uses the collaborative navigation data and adopts Kalman filtering to update the position and attitude, realizes the air-ground collaborative navigation of sound source positioning based on random consistent error compensation, and moves to the detection target position.

[0054] As a specific example, the positioning result error described in step 1 can be obtained by RTK (Real-time kinematic) real-time differential positioning to obtain the true positioning value, and then the error is calculated with the result given by the sound source pickup device, as follows:

[0055] Step 1.1, given a sound source signal s(t), the time function of the signal reaching two adjacent microphones of the microphone array is x1(t) and x2(t), n1(t) and n2(t) are independent white noises in the environment, α is the attenuation factor of the signal propagating in the air, τ1 and τ2 represent the propagation delay from the sound source to the two microphones m1 and m2, respectively. Then the signals received by microphones m1 and m2 are expressed as x1(t) and x2(t), respectively:

[0056] x1(t)=as(t-τ1)+n1(t) (1)

[0057] x2(t)=as(t-τ2)+n2(t) (2)

[0058] The cross-correlation function R of signals x1(t) and x2(t) 12 (τ) is:

[0059] R 12(τ)=E[x1(t)x2(t-τ)] (3)

[0060] Combining equations (1), (2) and (3), we can obtain:

[0061] R 12 (τ) = α 2 E(s(t-τ1)·s(t-τ2-τ))+αE(s(t-τ1)·n2(t-τ))+αE(s(t-τ2-τ)·n2(t))+E(n1(t)n2(t-τ)) (4)

[0062] Assuming that there is no correlation between the signal and the noise, and that different noises are uncorrelated, further simplification yields:

[0063] R 12 (τ) = α 2 E(s(t-τ1)·s(t-τ2-τ))+E(n1(t)n2(t-τ))=α 2 R ss (τ-τ 12 ) (5)

[0064] where τ 12 =τ1-τ2, when τ1-τ2=0, the cross-correlation function reaches its maximum value, and the time delay between the sound source signal reaching two adjacent microphones can be estimated by solving the corresponding τ value;

[0065] According to the relevant positioning:

[0066]

[0067] Where G 12 (ω) is the cross power spectral density function of the two signals received by different microphones in the microphone array, where X1(ω) = FFT[x1(t)], At this time, the time delay τ between the two microphones 12 is equal to max(R 12 (τ));

[0068] Step 1.2: Figure 2 As shown, the four microphones of the sound source pickup device are named m1, m2, m3 and m4 respectively, where m1 and m3 form the plane X axis, m2 and m4 form the plane Y axis, and their intersection is used as the origin to construct a three-dimensional rectangular coordinate system. The distance from the microphone to the origin is d, and τ is selected 12 , τ 13 , τ 14The three groups of delays represent the delays between microphones m2, m3, m4 and microphone m1. The coordinates of the sound source signal S are set to s(x, y, z), the distance from the sound source coordinate position to the origin of the microphone array is r, and the distance to each microphone is r i (i=1,2,3,4), the horizontal plane of the microphone array is taken as plane xoy, the elevation angle between the sound source signal S and the normal vector of xoy is θ, and the azimuth angle between the projection on xoy and the X-axis is c is the propagation speed of sound. According to the geometric relationship of the four-element cross array, the following relationship can be obtained:

[0069]

[0070] Taking m1 as the reference microphone, the distance difference between the two microphones and the sound source coordinates can be obtained:

[0071]

[0072] Convert the sound source position coordinate s into polar coordinates, expressed as:

[0073]

[0074] Eliminate x, y, and z together to obtain the localization result of the sound source signal S:

[0075]

[0076] As a specific example, the RANSAC algorithm described in step 2 is used to fit the data obtained in step 1 to obtain a relationship model between the distance and the positioning result error, which is as follows:

[0077] The error compensation method is implemented using the random sampling consistent RANSAC algorithm. The true value of the azimuth error and the distance given by the sound source localization are taken as a two-dimensional data sample set. A qualified fitting model is found from this sample set. This qualified fitting model is used to compensate the result of the sound source localization to improve the positioning accuracy.

[0078] The RANSAC algorithm is an algorithm that calculates the mathematical model parameters of the data based on a set of sample data sets containing abnormal data to obtain valid sample data, such as Figure 3 As shown, the specific steps are as follows:

[0079] Step 2.1: Set the evaluation criteria of the model, the distance threshold d and the number of internal points T, where d i Used to distinguish between inliers and outliers. Inliers represent data that can be represented by the model, while outliers represent abnormal data. i is equal to the distance from each point to the linear model, where i = 1, 2, 3······, n; if di If ≤d, the i-th point is an interior point, otherwise it is an exterior point;

[0080] Step 2.2, taking the distance and positioning error as a sample set, randomly selecting n points and fitting them with a 2D linear model;

[0081] Step 2.3: Find a qualified model. After generating a fitting model, count the number of inliers that can be represented by this model and record it as N. i , if N i <T, the model is unqualified and returns to step 2.2; if N i ≥T, the model is qualified and proceed to step 2.4;

[0082] Step 2.4: In the actual detection process, first obtain a rough position estimate (x, y, z) of the sound source, calculate the position error (Δx, Δy, Δz) based on the error model obtained in step 2.3, and finally obtain the compensated position (x+Δx, y+Δy, z+Δz).

[0083] As a specific example, the ground unmanned platform described in step 9 uses the collaborative navigation data and adopts Kalman filtering to update the posture to achieve air-ground collaborative navigation based on sound source positioning with random consistent error compensation, as follows:

[0084] Step 9.1, the ground unmanned platform selects attitude angle error, velocity error, position error, gyroscope bias, and accelerometer bias as state variables, and establishes a 15-dimensional sequential Kalman filter state equation;

[0085] The state variables are:

[0086] X=[φ x φ y φ z δV x δV y δV z δL δλ δh ε x ε y ε z ▽ x ▽ y ▽ z ] T

[0087] In the formula, φ x ,φ y ,φ z It represents the three-axis attitude misalignment angle of MINS inertial navigation in the navigation coordinate system; δV x , δV y , δV zrepresents the three-axis velocity error of the MINS inertial navigation in the navigation coordinate system; δL, δλ, δh represent the latitude, longitude and altitude errors in the navigation coordinate system; ε x , ε y , ε z Indicates the zero drift of the gyroscope in the three-axis direction in the carrier coordinate system; ▽ x ,▽ y ,▽ z Indicates the gyro drift and accelerometer zero bias in the three coordinate axis directions of the accelerometer in the carrier coordinate system;

[0088] Then the state equation of the system is:

[0089]

[0090] In the formula, is the prior estimate of the continuous system, F(t) is the system state transfer matrix at time t, X(t) is the system state matrix at time t, G(t) is the system noise input matrix at time t, and W(t) is the system process noise at time t;

[0091] Discretize the system state equation as:

[0092] X(k)=Φ(k)X(k-1)+Γ(k)R(k) (14)

[0093] Where X(k) represents the state matrix of the system at time k, Φ(k) is the system state transfer matrix from time k-1 to time k, X(k-1) is the state matrix of the system at time k-1, Γ(k) is the system noise input matrix at time k, and R(k) is the system process noise at time k, that is, the accelerometer noise;

[0094] Step 9.2: Set the observation is the coordinate of the ground unmanned platform measured by the sound source positioning of the ground unmanned platform in the system, (x I ,y I ,z I ) is the position information calculated by the ground unmanned platform through inertial navigation, then the measurement equation of the system is:

[0095] Z(t)=H(t)X(t)+V(t) (15)

[0096] Where Z(t) is the actual observation value of the system at time t, H(t) is the observation matrix of the system at time t, X(t) is the state matrix of the system at time t, and V(t) is the observation noise of the system at time t.

[0097] Discretize the system measurement equation into:

[0098] Z(k)=H(k)X(k-1)+V(k) (16)

[0099] Where Z(k) is the actual observation value of the system at time k; H(k) is the observation matrix of the system at time k, which converts the state variables into predicted observation values; X(k-1) is the state matrix of the system at time k, and V(k) is the measurement noise of the system at time k;

[0100] Step 9.3, according to the extended Kalman filter:

[0101]

[0102] In the formula, is the prior estimate of the system at time k, Φ(k) is the system state transfer matrix from time k-1 to time k, is the posterior estimate of the system at time k-1;

[0103] Therefore, the prior error variance matrix at time k is estimated as:

[0104]

[0105] In the formula, is the prior prediction error variance matrix of the system at time k, Φ(k) is the system state transition matrix from time k-1 to time k, P(k-1) is the posterior prediction error variance matrix of the system at time k-1, Φ(k) T is the transposed matrix of Φ(k), Q(k) is the system process noise covariance matrix;

[0106] Step 9.4, update the Kalman filter gain:

[0107]

[0108] Where K(k) is the Kalman filter gain of the system at time k, and is the weighting coefficient of observation and prediction; is the prior prediction error variance matrix of the system at time k, H(k) is the measurement matrix of the system at time k, and H T (k) is the transpose of H(k), R(k) is the system measurement noise covariance matrix at time k;

[0109] Therefore, the estimated value of the state vector at time k is:

[0110]

[0111] In the formula, is the posterior state estimate of the system at time k, is the prior state estimate at time k, K(k) is the Kalman filter gain of the system at time k, Z(k) is the system observation value, and H(k) is the measurement matrix of the system at time k;

[0112] Step 9.5, update the state error covariance matrix to:

[0113]

[0114] Where P(k) is the posterior prediction error variance matrix of the system at time k, K(k) is the Kalman filter gain of the system at time k, and H(k) is the measurement matrix of the system at time k. is the prior prediction error variance matrix of the system at time k, and R(k) is the measurement noise covariance matrix of the system at time k;

[0115] Step 9.6: After obtaining the updated result of the Kalman filter, the posterior estimate The error is corrected by compensating the position result calculated by the data of the ground unmanned platform's own inertial sensor, thereby realizing air-ground collaborative navigation.

[0116] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0117] Example

[0118] Combination Figure 1 ,This embodiment provides an air-ground collaborative sound source localization system based on RANSAC error compensation, the system includes an aerial unmanned platform, a ground unmanned platform and a communication device;

[0119] The aerial unmanned aerial platform is equipped with communication modules and various airborne sensors, which can be used to transmit navigation and positioning data acquired by the sensors;

[0120] The ground mobile robot is equipped with a sound source pickup device, a high-precision sensor and an air-to-ground communication module, and can communicate with the aerial unmanned platform. When the aerial unmanned platform enters the sound source pickup range of the ground unmanned platform, the ground unmanned platform can be used to locate the aerial unmanned platform, correct the positioning error of the aerial unmanned platform, and improve the navigation and positioning accuracy of the aerial unmanned platform;

[0121] The communication equipment is connected to multiple unmanned platforms and consists of a data transmission module that transmits data information such as the attitude, speed, and position of each unmanned platform.

[0122] The sound source pickup device consists of 4 microphone sound sensors and a signal processing board, wherein the 4 microphones form a 4-element cross microphone array.

[0123] In this example, the air-ground collaborative sound source localization method based on RANSAC error compensation simulates the results of the geometric sound source localization algorithm based on TDOA. The simulation conditions are: a single aerial unmanned platform and a single ground unmanned platform perform collaborative navigation, the gyro zero drift of the inertial navigation device used is 10deg / h, and the gyro noise is 0.01deg s -1 Hz -1 / 2 , the accelerometer zero bias is 15μg, and the accelerometer noise is The UAV maintains a uniform flight speed of 2m / s, and the ground robot maintains a uniform movement of 2m / s after obtaining the collaborative navigation data. The simulated movement time is 1090s. The target sound source level is 70-85dB. Taking into account the short-term loss of sound source lock, 10% of the flying point jitter results generated when the sound source pickup device does not receive the target sound source are randomly added to compensate for the azimuth in the sound source positioning, and then the relative position is obtained.

[0124] The error fitting environment of sound source positioning is the positioning result of the sound emitted by the ground unmanned platform in the dense forest when the aerial unmanned platform is flying within 0 to 140 meters. By fitting the error results, the relationship model between the azimuth error and the distance is obtained, and then the positioning results within 0 to 140 meters are compensated, such as Figure 4 As shown, it can be seen that the positioning error is further reduced after compensation, and the mean square error after compensation can be reduced by 29.56% compared with that without compensation.

[0125] like Figure 5 As shown in the figure, after the ground unmanned platform measures the target position, the ground unmanned platform only uses the positioning error calculated by its own inertial sensor, the positioning error of the sound source positioning result, and the positioning error of the air-ground collaborative sound source based on RANSAC error compensation during the movement towards the detection target. It can be seen that the positioning error of the sound source positioning result diverges slowly over time, and the positioning accuracy can be greatly improved compared with the inertial navigation solution. After fusing the positioning results of the two, the flying point jitter generated by the sound source positioning when the target sound source is lost can also be suppressed, and the positioning result is more stable. The average error of the navigation result after information fusion is 0.372m. Therefore, in the case of GNSS denial and limited optical field of view, this method can greatly improve the navigation and positioning accuracy of the ground unmanned platform. The implementation process is as follows:

[0126] ① The ground unmanned platform powers on, takes itself as the coordinate origin of the system, and uses inertial sensors to obtain its own position information;

[0127] ② The ground unmanned platform receives the sound source information through the sound source pickup device carried by it, and uses the sound source positioning method based on error compensation to measure the relative position information of the target to be rescued and the aerial unmanned platform;

[0128] ③ After obtaining the location information of the aerial unmanned platform, the ground unmanned platform sends the location information of the aerial unmanned platform and the location information of the target to be rescued to the aerial unmanned platform;

[0129] ④ The aerial unmanned platform uses inertial sensors to collect data information and transmit it to the processor on board the aerial unmanned platform;

[0130] ⑤ Use the processor on the aerial unmanned platform to perform attitude calculation and obtain the current position information of the aerial unmanned platform;

[0131] ⑥ The aerial unmanned platform calculates the absolute positions of the target and the ground unmanned platform through its own absolute position and the relative position relationship among itself, the target and the ground unmanned platform, and then sends the absolute position of the ground unmanned platform to the ground unmanned platform and flies to the detection target position, and then enters step ⑦;

[0132] ⑦ The aerial unmanned platform calculates the absolute positions of the target and the ground unmanned platform through its own absolute position and the relative position relationship among itself, the target and the ground unmanned platform, and then sends the absolute position of the ground unmanned platform to the ground unmanned platform and flies to the detection target position, and then enters step ⑧;

[0133] ⑧The ground unmanned platform uses collaborative navigation data and adopts Kalman filtering to update its posture, realize air-ground collaborative navigation based on sound source positioning, and move towards the detection target position.

[0134] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by any technician familiar with the technical field within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

[0135] It should be understood that in order to simplify the present invention and help those skilled in the art understand the various aspects of the present invention, in the above description of the exemplary embodiments of the present invention, the various features of the present invention are sometimes described in a single embodiment or described with reference to a single figure. However, the present invention should not be interpreted as the features included in the exemplary embodiments are all necessary technical features of the patent claims.

Claims

1. An air-ground collaborative sound source localization system based on RANSAC error compensation, characterized in that: Including unmanned aerial platforms, unmanned ground platforms and communication equipment; The aerial unmanned platform is equipped with airborne sensors and communication equipment, wherein the airborne sensors include inertial navigation and GNSS, which are used to obtain inertial navigation and GNSS positioning data; the communication equipment is used to obtain the sound source positioning information sent by the ground unmanned platform, and calculate the absolute position information of the target and the ground unmanned platform through its own positioning, and send the absolute position information to the ground unmanned platform while flying towards the target; The ground unmanned platform is equipped with an inertial sensor, a sound source pickup device and a communication device, wherein the sound source pickup device is used to detect the sound emitted by the target sound source, and determine the relative position of the target by a sound source positioning method based on RANSAC error compensation; when the aerial unmanned platform enters the pickup range of the sound source pickup device, the ground unmanned platform performs relative positioning of the aerial unmanned platform by a sound source positioning method based on RANSAC error compensation, and simultaneously sends the relative position of the aerial unmanned platform and the target to the aerial unmanned platform; After receiving its own absolute position information, the ground unmanned platform corrects the inertial navigation solution error, constrains the error divergence, improves its own navigation and positioning accuracy, realizes the collaborative navigation of the aerial unmanned platform and the ground unmanned platform, and moves towards the detection target at the same time; the sound source positioning method based on RANSAC error compensation is to compensate for the error of sound source positioning. The RANSAC algorithm is used to fit the azimuth error and distance of the sound source positioning to obtain the error compensation model. In actual use, after the relative azimuth, pitch angle and distance are obtained by sound source positioning, the azimuth is compensated according to the error model to improve the relative positioning accuracy; The communication device is composed of a data transmission module, which is used to transmit collaborative navigation data, including the absolute position information of the ground unmanned platform measured by the aerial unmanned platform, and the relative position information of the target measured by the ground platform and the aerial unmanned platform.

2. The air-ground collaborative sound source localization system based on RANSAC error compensation according to claim 1 is characterized in that: The sound source pickup device is composed of 4 microphone sound sensors and a signal processing board, wherein the 4 microphones form a 4-element cross microphone array.

3. The air-ground collaborative sound source localization system based on RANSAC error compensation according to claim 1 is characterized in that: The ground unmanned platform measures the relative position information of the aerial unmanned platform by means of sound source positioning and sends the information to the aerial unmanned platform. The aerial unmanned platform calculates the absolute position information of the ground unmanned platform based on its own absolute position information and the relative position information sent by the ground unmanned platform and sends the information to the ground unmanned platform. The ground unmanned platform integrates the absolute position information with the positioning information calculated by its own inertial navigation to achieve collaborative positioning of the air-ground unmanned platforms.

4. A method for air-ground collaborative sound source localization based on RANSAC error compensation, characterized in that: The following steps are involved: Step 1: In a field environment, use a sound source pickup device carried by a ground unmanned platform to measure the azimuth error of the drone at different distances; Step 2: Use the RANSAC algorithm to fit the data obtained in step 1 to obtain a relationship model between distance and azimuth error; Step 3: The ground unmanned platform itself is used as the coordinate origin to establish a three-dimensional rectangular coordinate system and use inertial sensors to obtain its own position information; Step 4: The ground unmanned platform receives the sound source information through the sound source pickup device carried by it, and uses the sound source localization method based on RANSAC error compensation to detect the relative position information of the target and the aerial unmanned platform according to the relationship model between the distance and the azimuth error; The sound source localization method based on RANSAC error compensation is to compensate for the error of sound source localization. The RANSAC algorithm is used to fit the azimuth error and distance of sound source localization to obtain an error compensation model. In actual use, after the relative azimuth, pitch angle and distance are obtained by sound source localization, the azimuth is compensated according to the error model to improve the relative positioning accuracy. Step 5: The ground unmanned platform sends the relative position information of the aerial unmanned platform and the relative position information of the target to the aerial unmanned platform; Step 6: The aerial unmanned platform uses airborne sensors to collect inertial navigation data information and GNSS positioning data information, and uses the processor on the aerial unmanned platform to perform position calculation to obtain the current absolute position information of the aerial unmanned platform; Step 7: The aerial unmanned platform determines whether there is collaborative navigation data input. If there is no collaborative navigation data, the aerial unmanned platform updates its own posture according to the data of the inertial sensor; if there is collaborative navigation data input, it proceeds to step 8; Step 8: The aerial unmanned platform calculates the absolute positions of the target and the ground unmanned platform through its own absolute position and the relative position relationship among itself, the target and the ground unmanned platform, and then sends the absolute positions of the ground unmanned platform and the target to the ground unmanned platform, and flies to the detection target position; Step 9: The ground unmanned platform uses the collaborative navigation data and adopts Kalman filtering to update the posture, realizes air-ground collaborative navigation based on sound source positioning with RANSAC error compensation, and moves to the detection target position.

5. The air-ground collaborative sound source localization method based on RANSAC error compensation according to claim 4 is characterized in that: In step 1, in the field environment, the sound source pickup device carried by the ground unmanned platform is used to measure the azimuth error of the drone at different distances, as follows: In the field, a sound source pickup device carried by a ground unmanned platform is used to measure the sound emitted by sound sources at different distances to obtain the measured value of the positioning result. Then, the true value of the positioning result is obtained by actual measurement, and finally the error between the measured value and the true value is calculated.

6. The air-ground collaborative sound source localization method based on RANSAC error compensation according to claim 5 is characterized in that: The sound source pickup device carried by the ground unmanned platform is used to measure the sound emitted by the sound source at different distances to obtain the measurement value of the positioning result, which is as follows: Step 1.1, given a sound source signal s(t), the time function of the signal reaching two adjacent microphones of the microphone array is x1(t) and x2(t), n1(t) and n2(t) are independent white noises in the environment, α is the attenuation factor of the signal propagating in the air, τ1 and τ2 represent the propagation delay from the sound source to the two microphones m1 and m2, respectively. Then the signals received by microphones m1 and m2 are expressed as x1(t) and x2(t), respectively: x1(t)=as(t-τ1)+n1(t) (1) x2(t)=as(t-τ2)+n2(t) (2) The cross-correlation function R of signals x1(t) and x2(t) 12 (τ) is: R 12 (τ)=E[x1(t)x2(t-τ)] (3) Combining equations (1), (2) and (3), we get: Assuming that there is no correlation between the signal and the noise, and that different noises are uncorrelated, further simplification yields: R 12 (t)=a 2 E(s(t-τ1)·s(t-τ2-τ))+E(n1(t)n2(t-τ))=α 2 R ss (t-t 12 ) (5) where τ 12 =τ1-τ2, when τ1-τ2=0, the cross-correlation function reaches its maximum value, and the time delay between the sound source signal reaching two adjacent microphones is estimated by solving the corresponding τ value; According to the relevant positioning: Where G 12 (ω) is the cross power spectral density function of the two signals received by different microphones in the microphone array, where X1(ω) = FFT[x1(t)], At this time, the time delay τ between the two microphones 12 is equal to max(R 12 (τ)); Step 1.2, name the four microphones of the sound source pickup device m1, m2, m3 and m4 respectively, where m1 and m3 form the plane X axis, m2 and m4 form the plane Y axis, and use their intersection as the origin to construct a three-dimensional rectangular coordinate system. The distance from the microphone to the origin is d, and τ is selected. 12 , τ 13 , τ 14 The three groups of delays represent the delays between microphones m2, m3, m4 and microphone m1. The coordinates of the sound source signal S are set to s(x, y, z), the distance from the sound source coordinate position to the origin of the microphone array is r, and the distance to each microphone is r i , i = 1, 2, 3, 4, the horizontal plane of the microphone array is taken as plane xoy, the elevation angle between the sound source signal S and the normal vector of xoy is θ, and the azimuth angle between the projection on xoy and the X-axis is c is the propagation speed of sound. According to the geometric relationship of the quaternary cross array, the following relationship is obtained: Taking m1 as the reference microphone, the distance difference between the two microphones and the sound source coordinates is: Convert the sound source position coordinate s into polar coordinates, expressed as: Eliminate x, y, and z together to obtain the localization result of the sound source signal S:

7. The air-ground collaborative sound source localization method based on RANSAC error compensation according to claim 4 is characterized in that: The RANSAC algorithm described in step 2 is used to fit the data obtained in step 1 to obtain a relationship model between distance and azimuth error, which is as follows: Step 2.1: Set the evaluation criteria of the model, the distance threshold d and the number of internal points T, where d i Used to distinguish between inliers and outliers. Inliers represent data sets that can be represented by the model, while outliers represent abnormal data. i is equal to the distance from each point to the linear model, where i = 1, 2, 3······, n; if d i If ≤d, the i-th point is an interior point, otherwise it is an exterior point; Step 2.2, taking the distance and azimuth errors as a sample set, randomly selecting n points and fitting them with a 2D linear model; Step 2.3: Find a qualified model. After generating a fitting model, count the number of inliers that can be represented by this model and record it as N. i , if N i <T, the model is unqualified and returns to step 2.2; if N i ≥T, the model is qualified and proceed to step 2.4; Step 2.4: In the actual detection process, first obtain a rough position estimate (x, y, z) of the sound source, calculate the position error (Δx, Δy, Δz) based on the error model obtained in step 2.3, and finally obtain the compensated position (x+Δx, y+Δy, z+Δz).

8. The air-ground collaborative sound source localization method based on RANSAC error compensation according to claim 4 is characterized in that: The ground unmanned platform described in step 9 uses the collaborative navigation data and adopts Kalman filtering to update the posture to achieve air-ground collaborative navigation based on sound source positioning with RANSAC error compensation, as follows: Step 9.1, the ground unmanned platform selects attitude angle error, velocity error, position error, gyroscope bias, and accelerometer bias as state variables, and establishes a 15-dimensional sequential Kalman filter state equation; The state variables are: In the formula, φ x ,φ y ,φ z It represents the three-axis attitude misalignment angle of MINS inertial navigation in the navigation coordinate system; δV x , δV y , δV z represents the three-axis velocity error of the MINS inertial navigation in the navigation coordinate system; δL, δλ, δh represent the latitude, longitude and altitude errors in the navigation coordinate system; ε x , ε y , ε z Indicates the zero drift of the gyroscope in the three-axis direction in the carrier coordinate system; Indicates the gyro drift and accelerometer zero bias in the three coordinate axis directions of the accelerometer in the carrier coordinate system; Then the state equation of the system is: In the formula, is the prior estimate of the continuous system, F(t) is the system state transfer matrix at time t, X(t) is the system state matrix at time t, G(t) is the system noise input matrix at time t, and W(t) is the system process noise at time t; Discretize the system state equation as: X(k)=Φ(k)X(k-1)+Γ(k)R(k) (14) Where X(k) represents the state matrix of the system at time k, Φ(k) is the system state transfer matrix from time k-1 to time k, X(k-1) is the state matrix of the system at time k-1, Γ(k) is the system noise input matrix at time k, and R(k) is the system process noise at time k, that is, the accelerometer noise; Step 9.2: Set the observation is the coordinate of the aerial unmanned platform measured by the ground unmanned platform in the system through sound source positioning, (x I ,y I ,z I ) is the position information calculated by the ground unmanned platform through inertial navigation, then the measurement equation of the system is: Z(t)=H(t)X(t)+V(t) (15) Where Z(t) is the actual observation value of the system at time t, H(t) is the observation matrix of the system at time t, X(t) is the state matrix of the system at time t, and V(t) is the observation noise of the system at time t. Discretize the system measurement equation into: Z(k)=H(k)X(k-1)+V(k) (16) Where Z(k) is the actual observation value of the system at time k; H(k) is the observation matrix of the system at time k, which converts the state variables into predicted observation values; V(k) is the observation noise of the system at time k; Step 9.3, according to the extended Kalman filter: In the formula, is the prior estimate of the system at time k, Φ(k) is the system state transfer matrix from time k-1 to time k, is the posterior estimate of the system at time k-1; Therefore, the prior error variance matrix at time k is estimated as: Where P(k) is the prior prediction error variance matrix of the system at time k, Φ(k) is the system state transition matrix from time k-1 to time k, P(k-1) is the posterior prediction error variance matrix of the system at time k-1, and Φ(k) is T is the transposed matrix of Φ(k), Q(k) is the system process noise covariance matrix; Step 9.4, update the Kalman filter gain: Where K(k) is the Kalman filter gain of the system at time k, and is the weighting coefficient of observation and prediction; is the prior prediction error variance matrix of the system at time k, H(k) is the measurement matrix of the system at time k, and H T (k) is the transpose of H(k); Therefore, the estimated value of the state vector at time k is: In the formula, is the posterior state estimate of the system at time k, is the prior state estimate at time k, K(k) is the Kalman filter gain of the system at time k, Z(k) is the system observation value, and H(k) is the measurement matrix of the system at time k; Step 9.5, update the state error covariance matrix to: Where P(k) is the posterior prediction error variance matrix of the system at time k, K(k) is the Kalman filter gain of the system at time k, and H(k) is the measurement matrix of the system at time k. is the prior prediction error variance matrix of the system at time k; Step 9.6: After obtaining the updated result of the Kalman filter, the posterior estimate The error is corrected by compensating the position result calculated by the data of the ground unmanned platform's own inertial sensor, thereby realizing air-ground collaborative navigation.

Images