A method for constructing a high-precision navigation system based on a five-sensor conical configuration

Abstract

This invention relates to the field of navigation system technology, specifically disclosing a method for constructing a high-precision navigation system based on a five-sensor conical configuration. The method includes the following steps: S1, defining the carrier coordinate system and sensor position matrix; S2, determining the optimal spatial configuration of the five sensors; and S3, fault detection and isolation of the five sensors. The beneficial effects of this invention are: by employing a non-orthogonal conical configuration of the five sensors, the data from the five sensors are fused together, effectively reducing the overall navigation system error; by using a multi-threshold moving average residual method to detect sensor faults, the probability of false positives is reduced while effectively detecting sensor faults; and by constructing a position matrix for the sensors and performing variance analysis, the optimal installation angle of the sensors is found, improving installation efficiency and the accuracy of the navigation system.

Description

Technical Field

[0001] This invention relates to the field of navigation system technology, specifically a method for constructing a high-precision navigation system based on a five-sensor conical configuration, which can be widely applied to scenarios with high navigation accuracy requirements, such as drones and robots. Background Technology

[0002] Inertial sensors are important sensors used to measure the motion and attitude of objects. They mainly include accelerometers and gyroscopes. The former calculates acceleration by measuring the deformation or displacement of the sensitive element based on Newton's second law, while the latter measures angular velocity using the principle of conservation of angular momentum or the Coriolis force. They are characterized by strong autonomy, good real-time performance, high accuracy, and strong anti-interference capabilities. They are widely used in aerospace, automotive, robotics, and many other fields.

[0003] A three-sensor orthogonal navigation system arranges multiple sensors perpendicularly to each other, forming a structure similar to an orthogonal coordinate system. Utilizing the independent and interference-free characteristics of each sensor axis, it can accurately measure physical quantities in different directions, such as independently acquiring information about an object in three-dimensional space along the x, y, and z axes. It is widely used in fields requiring precise perception of object motion. A four-sensor non-orthogonal navigation system introduces a redundant sensor. The sensor's input axis is perpendicular to the four faces of a regular tetrahedron, which can improve navigation accuracy and fault tolerance to a certain extent.

[0004] However, existing three-sensor orthogonal navigation systems lack fault tolerance. When one sensor fails, the remaining two sensors can only measure acceleration and angular velocity in one plane, failing to measure acceleration and angular velocity across the entire three-dimensional space. While four-sensor non-orthogonal systems offer some fault tolerance, the system error increases significantly when a sensor fails. Four-sensor non-orthogonal systems introduce only one redundant sensor, offering limited improvement to navigation accuracy. Furthermore, existing navigation systems typically employ a single-threshold detection mechanism (such as the chi-square test), directly determining system anomalies based on a preset confidence threshold. Once the chi-square statistic exceeds a threshold, a system fault is identified. This conventional fault detection method uses a single-level threshold trigger, which cannot distinguish fault levels. It is highly sensitive to transient noise, easily leading to misjudgments, and has low fault location efficiency. This conventional fault detection method lacks preprocessing of the residual vector and does not eliminate the influence of transient interference. At the same time, the conventional method of determining the specific installation angle of sensors in a navigation system generally relies on personal experience, determining a better installation angle through multiple experiments, or determining the sensor installation angle based on the mechanical structure of the navigation equipment. This method lacks theoretical analysis, cannot accurately find the optimal installation angle, and requires a lot of time and effort in the experimental process, resulting in low efficiency.

[0005] Therefore, further solutions to the above problems are needed. The applicant proposes a method for constructing a high-precision navigation system based on a five-sensor conical configuration. Summary of the Invention

[0006] To address the shortcomings of existing technologies, this invention provides a method for constructing a high-precision navigation system based on a five-sensor conical configuration, thus solving the problems mentioned in the background section.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] A method for constructing a high-precision navigation system based on a five-sensor conical configuration, the specific steps of which include:

[0009] S1. Define the carrier coordinate system and sensor position matrix;

[0010] S2. Determine the optimal sensor space configuration for the five sensors;

[0011] S3, Fault detection and isolation of five sensors;

[0012] The specific steps of step S1 are as follows:

[0013] S11. Establish an orthogonal coordinate system (x, y, y) with the center of mass of the carrier as the origin of the coordinate system. b ,y b ,z b ), where x b The axis is along the horizontal forward direction of the carrier, y b The axis is along the horizontal rightward direction of the carrier, z b The axis is perpendicular to the plane of the carrier and points vertically upward.

[0014] S12. The orientation of each sensor is defined by its relative position in an orthogonal coordinate system, expressed by a unit vector v. i This represents the direction pointed to by the sensing axis of the i-th sensor, where i = 1, 2, 3, 4, 5, and vector v. i The definition is as follows:

[0015] v i =cos(yaw) i cos(pitch) i )·i+cos(yaw i sin(pitchi) ) ·j+sin(yaw i )·k

[0016] Where i represents the direction in the orthogonal coordinate system relative to the coordinate axis x. b The same unit vector, j is the direction in the orthogonal coordinate system and the coordinate axis y. bThe same unit vector, k is the direction in the orthogonal coordinate system and the coordinate axis z. b The same unit vector, yaw i Indicates that the i-th sensor is around z b Azimuth of the axis, pitch i Indicates that the i-th sensor is orbiting y b The pitch angle of the axis;

[0017] S13. Construct the sensor position matrix H, project the sensors onto the orthogonal coordinate system x, y, z axes, and represent the projection relationship of the five sensors using a matrix. This projection relationship is represented by the position matrix H, as shown in the following expression:

[0018]

[0019] As a further aspect of the present invention, step S2 specifically includes the following steps:

[0020] S21. Five sensors are symmetrically distributed on the base, with azimuth angles evenly spaced at 72° intervals. The first sensor is aligned with the orthogonal coordinate system. b With axes aligned, assuming each sensor has an elevation angle of α, the sensor position matrix H can be represented as follows:

[0021]

[0022] As a further aspect of the present invention, step S2 further includes the following steps:

[0023] S22. Determine the sensor elevation angle that minimizes the overall system error, and construct the measurement matrix m for the five sensors:

[0024] m = [m1, m2, m2, m4, m5] T =H·[w x ,w y ,w z ] T +[v1,v2,v3,v4,v5] T

[0025] Among them, w x ,w y ,w z The vector represents the components of the actual physical input quantities, acceleration and angular velocity, along the three coordinate axes in an orthogonal coordinate system. The product of H and w represents a linear combination of projections of the physical quantities onto the sensor's sensing axes, reflecting the theoretical measurement contribution of the physical quantities to each sensor. i This represents the measurement error vector of the i-th sensor, reflecting the unavoidable random interference factors during the measurement process. i This represents the measurement value of the i-th sensor.

[0026] As a further aspect of the present invention, step S2 further includes the following steps:

[0027] S23. The least weighted squares method can be used to fuse measurement data from multiple sensors. The objective of the weighted least squares method is to minimize the sum of squared weighted residuals. The objective function is as follows:

[0028] J(w)=(m-Hw) T W(m-Hw)

[0029] Where W is the weight matrix, These represent the noise variances of the five sensors;

[0030] Differentiating the above equation, we can find the estimated value when the residual is minimized. The expression is as follows:

[0031]

[0032] Calculating the covariance of the estimated values ​​yields:

[0033]

[0034] Assume that the noise of each sensor is independent, and the standard deviation of the noise measured by each sensor is σ. v Then the covariance of the estimated value can be simplified to:

[0035]

[0036] The system noise error is minimized when the covariance is minimized, that is, when the elevation angle α makes det(H) the minimum. T When H is at its maximum, the navigation system error is minimized.

[0037] The optimal elevation angle for the sensor, calculated to be:

[0038]

[0039] det(H T H)=31.25sin 4 αcos 2 α

[0040]

[0041] As a further aspect of the present invention, step S3 specifically includes the following steps:

[0042] S31. Establish residual historical records for the most recent 5 moments for each sensor to form a time series buffer. This step disperses transient interference across multiple time points for analysis. The buffer sequence expression is as follows:

[0043] Q i =[r i (t-5+1),....r i (t-1),r i (t)]

[0044]

[0045] Among them, Q i r represents the residual buffer queue of the i-th sensor. i Let represent the residual of the i-th sensor. Next, the filtered residual vector is constructed using a moving average calculation, as shown in the following formula:

[0046]

[0047] in, Q is the processed residual vector. i (tk) represents the k-th historical residual value. The chi-square statistic is constructed as follows:

[0048]

[0049] As a further aspect of the present invention, step S3 further includes the following steps:

[0050] S32. Based on the confidence level and the degrees of freedom of the statistic, select an appropriate threshold using a lookup table. The subscript α represents the significance level. α = 0.05 corresponds to a 95% confidence level, and α = 0.01 corresponds to a 99% confidence level. n represents the degrees of freedom of the statistic. In this system, five sensors are used to estimate three parameters, so the degrees of freedom n = 2. When the system is in normal condition, no operation is triggered.

[0051] when When the system enters an early warning state, it initiates secondary residual analysis, calculates the local residuals of each sensor, and constructs a sliding window with a window length of N=50. The sliding window length N=50 indicates that the system selects the latest 50 consecutive data points from each sensor as the analysis unit, and counts the residuals of each sensor exceeding 3σ within the window. i The number of abnormal occurrences of a sensor within three consecutive windows is considered as a fault if the sensor is found to have malfunctioned.

[0052] when If a system malfunction occurs, it is immediately identified. The faulty sensors are then eliminated by iterating through the matrix H, removing the i-th sensor sequentially from the position matrix H to construct a reduced-dimensional position matrix H. i (H i ∈R 4×3Construct the chi-square statistic after removing the i-th sensor.

[0053]

[0054] Where, m i , W i Let be the measurement matrix, estimated value, and weight matrix after the i-th sensor, respectively. If removing the i-th sensor satisfies the new chi-square statistic... Then it is determined that the i-th sensor is faulty.

[0055] Compared with the prior art, the beneficial effects of the present invention are:

[0056] 1. The system constructed by this method introduces two redundant sensors and uses the weighted least squares method to fuse the data of five sensors, thereby reducing the influence of external noise on the entire system, effectively solving the navigation inaccuracy problem, and reducing the overall navigation system error.

[0057] 2. The system constructed by this method introduces redundant sensors and proposes a multi-threshold moving average residual analysis method to build a fault detection system. When a sensor in the navigation system fails, the faulty sensor can be detected and eliminated in time, which improves the fault tolerance of the system and reduces the probability of misjudgment.

[0058] 3. The system constructed by this method finds the optimal installation angle of the sensor by building a position matrix of the sensor and performing variance analysis, thereby improving installation efficiency and the accuracy of the navigation system. Attached Figure Description

[0059] Figure 1 This is a diagram showing the sensor configuration of the navigation system of the present invention;

[0060] Figure 2 This is a flowchart of the sensor fault detection process of the present invention;

[0061] Figure 3 Comparison chart of navigation paths generated for different system configurations of the present invention;

[0062] Figure 4 This is a navigation speed error diagram for the five-sensor system of the present invention;

[0063] Figure 5 This is a navigation speed error diagram for the three-sensor system of the present invention. Detailed Implementation

[0064] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0065] Example

[0066] Please see Figure 1-2 This invention provides a method for constructing a high-precision navigation system based on a five-sensor conical configuration. The specific steps of this method include:

[0067] Step 1: Define the carrier coordinate system and sensor position matrix

[0068] 1. Establish an orthogonal coordinate system (x, y, z) with the center of mass of the carrier as the origin of the coordinate system. b ,y b ,z b ), where x b The axis is along the horizontal forward direction of the carrier, y b The axis is along the horizontal rightward direction of the carrier, z b The axis is perpendicular to the plane of the carrier and points vertically upward.

[0069] 2. The orientation of each sensor is defined by its relative position in an orthogonal coordinate system, expressed by the unit vector v. i This represents the direction pointed to by the sensing axis of the i-th sensor, where i = 1, 2, 3, 4, 5, and vector v. i The definition is as follows:

[0070] v i =cos(yaw) i cos(pitch) i )·i+cos(yaw i sin(pitch) i )·j+sin(yaw i )·k

[0071] Where i represents the direction in the orthogonal coordinate system relative to the coordinate axis x. b The same unit vector, j is the direction in the orthogonal coordinate system and the coordinate axis y. b The same unit vector, k is the direction in the orthogonal coordinate system and the coordinate axis z. b The same unit vector, yaw i Indicates that the i-th sensor is around z b Azimuth of the axis, pitch i Indicates that the i-th sensor is orbiting y b The pitch angle of the axis;

[0072] 3. Construct the sensor position matrix H, project the sensors onto the orthogonal coordinate system x, y, z axes, and represent the projection relationship of the five sensors using a matrix. This projection relationship is represented by the position matrix H, as shown in the following expression:

[0073]

[0074] Step 2: Determine the optimal sensor space configuration for the five sensors.

[0075] 1. Five sensors are symmetrically distributed on the base, with azimuth angles evenly spaced at 72° intervals. The first sensor is aligned with the orthogonal coordinate system. b With axes aligned, assuming each sensor has an elevation angle of α, the sensor position matrix H can be represented as follows:

[0076]

[0077] 2. Determine the sensor elevation angle that minimizes the overall system error, and construct the measurement matrix m for the five sensors:

[0078] m = [m1, m2, m2, m4, m5] T =H·[w x ,w y ,w z ] T +[v1,v2,v3,v4,v5] T

[0079] Among them, w x ,w y ,w z The vector represents the components of the actual physical input quantities, acceleration and angular velocity, along the three coordinate axes in an orthogonal coordinate system. The product of H and w represents a linear combination of projections of the physical quantities onto the sensor's sensing axes, reflecting the theoretical measurement contribution of the physical quantities to each sensor. i This represents the measurement error vector of the i-th sensor, reflecting the unavoidable random interference factors during the measurement process. i This represents the measurement value of the i-th sensor;

[0080] 3. The least weighted squares method can be used to fuse measurement data from multiple sensors. The objective of the weighted least squares method is to minimize the sum of squared weighted residuals. The objective function is as follows:

[0081] J(w)=(m-Hw) T W(m-Hw)

[0082] Where W is the weight matrix, These represent the noise variances of the five sensors;

[0083] Differentiating the above equation, we can find the estimated value when the residual is minimized. The expression is as follows:

[0084]

[0085] Calculating the covariance of the estimated values ​​yields:

[0086]

[0087] Assume that the noise of each sensor is independent, and the standard deviation of the noise measured by each sensor is σ. v Then the covariance of the estimated value can be simplified to:

[0088]

[0089] The system noise error is minimized when the covariance is minimized, that is, when the elevation angle α makes det(H) the minimum. T When H is at its maximum, the navigation system error is minimized.

[0090] The optimal elevation angle for the sensor, calculated to be:

[0091]

[0092] det(H T H)=31.25sin 4 αcos 2 α

[0093]

[0094] Step 3: Fault Detection and Isolation of Five Sensors

[0095] 1. Establish residual historical records for the most recent 5 time points for each sensor to form a time series buffer. This step disperses transient interference across multiple time points for analysis. The buffer sequence expression is as follows:

[0096] Q i =[r i (t-5+1),....r i (t-1),r i (t)]

[0097]

[0098] Among them, Q i r represents the residual buffer queue of the i-th sensor. i Let represent the residual of the i-th sensor. Next, the filtered residual vector is constructed using a moving average calculation, as shown in the following formula:

[0099]

[0100] in, Q is the processed residual vector. i (tk) represents the k-th historical residual value. The chi-square statistic is constructed as follows:

[0101]

[0102] 2. Based on the confidence level and the degrees of freedom of the statistic, select an appropriate threshold using a lookup table. The subscript α represents the significance level. α = 0.05 corresponds to a 95% confidence level, and α = 0.01 corresponds to a 99% confidence level. n represents the degrees of freedom of the statistic. In this system, five sensors are used to estimate three parameters, so the degrees of freedom n = 2. When the system is in normal condition, no operation is triggered.

[0103] when When the system enters an early warning state, it initiates secondary residual analysis, calculates the local residuals of each sensor, and constructs a sliding window with a window length of N=50. The sliding window length N=50 indicates that the system selects the latest 50 consecutive data points from each sensor as the analysis unit, and counts the residuals of each sensor exceeding 3σ within the window. i The number of abnormal occurrences of a sensor within three consecutive windows is considered as a fault if the sensor is found to have malfunctioned.

[0104] when If a system malfunction occurs, it is immediately identified. The faulty sensors are then eliminated by iterating through the matrix H, removing the i-th sensor sequentially from the position matrix H to construct a reduced-dimensional position matrix H. i (H i ∈R 4×3 Construct the chi-square statistic after removing the i-th sensor.

[0105]

[0106] Where, m i , W i Let be the measurement matrix, estimated value, and weight matrix after the i-th sensor, respectively. If removing the i-th sensor satisfies the new chi-square statistic... Then it is determined that the i-th sensor is faulty.

[0107] For details, please refer to Figure 3As shown, navigation was performed on a path with a total length of 150m using both a five-sensor and a three-sensor navigation system. The generated trajectories were compared with the ideal trajectory. In the figure, the red trajectory is the ideal trajectory, i.e., the accurate trajectory recorded by humans. The blue trajectory is the trajectory generated by the five-sensor navigation system, and the green trajectory is the trajectory generated by the three-sensor navigation system. Ultimately, the blue trajectory has an eastward position error of 0.8m and a northward error of 0.5m; the green trajectory has an eastward position error of -0.6m and a northward error of -7.2m. Therefore, the blue trajectory is closer to the ideal trajectory, and the five-sensor navigation system has a smaller error.

[0108] For details, please refer to Figure 4-5 As shown, Figure 4 This is a navigation speed error diagram for a five-sensor system. Figure 5 The diagram shows the navigation speed error of a three-sensor system. By comparison, it can be seen that the speed error of the five-sensor system is significantly lower than that of the three-sensor system.

[0109] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A high-precision navigation system construction method based on a five-sensor conical configuration, characterized in that: The method comprises the following specific steps: S1, defining a carrier coordinate system and a sensor position matrix; S2, determining an optimal sensor spatial configuration of the five sensors; S3, fault detection and isolation of the five sensors; The specific steps of step S1 are as follows: S11. Establishing an orthogonal coordinate system with the centroid of the carrier as the origin of the coordinate system wherein the x-axis is in the horizontal forward direction of the carrier, the y-axis is in the horizontal rightward direction of the carrier, the z-axis is perpendicular to the plane of the carrier vertically upward; S12, the direction of each sensor is defined by its relative position in the orthogonal coordinate system with a unit vector denotes the direction of the sensitive axis of the th sensor, the vector is defined as follows: wherein is the unit vector in the direction of the coordinate axis in the orthogonal coordinate system, is the unit vector in the direction of the coordinate axis in the orthogonal coordinate system, is the unit vector in the direction of the coordinate axis in the orthogonal coordinate system, denotes the azimuth angle of the sensor around the axis, denotes the pitch angle of the sensor around the axis; S13, Constructing the sensor position matrix Projecting the sensors on the orthogonal coordinate system , , axis, and express the projection relationship of the five sensors by a matrix, which is expressed by the position matrix , and the expression is as follows: ； The specific steps of step S3 comprise: S31, establishing a residual history record of the last 5 time points for each sensor to form a time sequence buffer, wherein this step disperses transient interference to multiple time points for analysis, and a buffer sequence expression is as follows: wherein, represent the residual buffer queue of the th sensor, represent the residual of the th sensor, and then a filtered residual vector is constructed by a moving average calculation, as follows: in, The processed residual vector, For the first Based on the historical residuals, the chi-square statistic is constructed as follows: ； S32, according to the confidence and the degree of freedom of the statistical quantity, select the appropriate threshold value by table method , the subscript of is the significance level, when corresponds to 95% confidence, corresponds to 99% confidence, represents the degree of freedom of the statistical quantity, in this system, five sensors are used to estimate three parameters, then the degree of freedom when , it is determined that the system is normal, and no operation is triggered; When , the system enters the early warning state, starts the secondary residual error analysis, calculates the local residual error of each sensor, constructs a sliding window, the window length , the sliding window length N=50 represents that the system intercepts the latest 50 consecutive data points of the sensor as an analysis unit each time, and counts the number of times that the residual error of each sensor in the window exceeds , if the number of times of abnormality of a certain sensor in the last 3 windows is , it is determined that the sensor has failed; When , immediately determine that the system is malfunctioning, at this time, the malfunctioning sensor is excluded by traversal, the first sensor is removed from the position matrix in turn, the reduced dimension position matrix ( ) is constructed, and the chi-square statistic after removing the first sensor is constructed : wherein, , , are the measurement matrix, the estimate and the weight matrix after removing the th sensor, respectively, and the th sensor is determined to be faulty if the new chi-square statistic is satisfied after removing the th sensor.

2. The method according to claim 1, wherein the high-precision navigation system is based on a five-sensor conical configuration. The specific steps of step S2 comprise: S21, five sensors are symmetrically distributed on the base, and the azimuth angles are uniformly spaced The first sensor is aligned with the axis of the orthogonal coordinate system, and it is assumed that the elevation angle of each sensor is The sensor position matrix can be represented as follows: 。 3. The method according to claim 2, wherein the high-precision navigation system is based on a five-sensor conical configuration. The specific steps of step S2 further comprise: S22, determine the sensor elevation angle that makes the system overall error minimum, construct the measurement matrix of the five sensors : in, The vector represents the components of the actual physical input quantities, acceleration and angular velocity, in the three coordinate axes in an orthogonal coordinate system. and Multiplying the two represents a linear combination of projections of the physical quantities onto the sensor's sensitive axis, reflecting the theoretical measurement contribution of each physical quantity to the various sensors. Indicates the first The measurement error vector of each sensor reflects the unavoidable random interference factors during the measurement process. Indicates the first The measurement values ​​of each sensor.

4. The method according to claim 3, wherein the high-precision navigation system is based on a five-sensor conical configuration. The specific steps of step S2 further comprise: S23, the measurement data of multiple sensors can be fused by applying a least weighted square method, and the target of the weighted least square method is to minimize the weighted residual square sum, and a target function is as follows: wherein is a weight matrix, , are noise variances of the five sensors, respectively; Taking the derivative of the above equation, the estimate is solved for the minimum residual , The expression is as follows: The covariance of the estimated value is as follows: Assuming that each sensor noise is independent of each other and the standard deviation of each sensor measurement noise is , the covariance of the estimates can be simplified as: The system noise error is minimum when the covariance is minimum, i.e. when the elevation is made such that det( The navigation system error is minimum when the covariance is maximum, Through calculation, the optimal elevation angle of the sensor is as follows: 。

Images