Positioning diagnosis method, device, equipment and storage medium

Abstract

The present invention relates to the field of data processing and discloses a positioning diagnosis method, device, equipment and storage medium. The method includes: obtaining collected data from multiple sensors and fusing the collected data from all sensors to obtain a positioning state quantity; performing jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result; calculating the error between the collected data and the positioning state quantity; performing ambiguity diagnosis on the same state targeted by the collected data to obtain an ambiguity diagnosis result; and judging abnormal positioning in the positioning system based on the jump detection result, the error and the ambiguity diagnosis result. In this method, by processing the collected data and the positioning state quantity, sensors that do not conform to the kinematic principle, sensors and / or positioning systems with errors, and two sensors with ambiguity for the same state are determined, and the sensor or positioning system with problems is located, thereby achieving positioning diagnosis of the sensor and positioning system.

Description

Technical Field

[0001] The present invention relates to the field of data processing, and in particular to a positioning diagnosis method, device, equipment and storage medium. Background Art

[0002] Multi-sensor fusion positioning methods are widely used in autonomous driving, robotics, drones, and other fields. The state of each sensor during the fusion process determines the reliability and accuracy of the final positioning result. Existing positioning system status assessment methods primarily calculate the difference between the estimated and actual positions as a metric for evaluating sensor accuracy. However, in most cases, it is difficult to determine the accuracy of sensor data collected. This is especially true in large-scale motion scenarios such as vehicle driving, where it is even more difficult to determine the accuracy and reliability of the collected data and to further identify which sensor among the multiple sensors is experiencing a problem. Summary of the Invention

[0003] The main purpose of the present invention is to solve the technical problem that in the existing positioning diagnosis process, the sensor with the problem cannot be located.

[0004] A first aspect of the present invention provides a positioning diagnosis method, which includes: acquiring collected data from several sensors and fusing the collected data from all sensors to obtain a positioning state quantity; performing jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result; calculating the error between the collected data and the positioning state quantity; performing ambiguous diagnosis on the same state targeted by the collected data to obtain an ambiguous diagnosis result; and judging abnormal positioning in the positioning system based on the jump detection result, the error and the ambiguous diagnosis result.

[0005] Optionally, in a first implementation method of the first aspect of the present invention, the acquisition data of several sensors is obtained and the acquisition data of all sensors are fused to obtain the positioning state quantity, including: obtaining the acquisition data of several sensors at the current moment and the positioning state quantity at the previous moment; inputting the positioning state quantity at the previous moment into a conversion function to obtain a first positioning state quantity at the current moment; and correcting the first positioning state quantity according to the acquisition data at the current moment to obtain the positioning state quantity at the current moment.

[0006] Optionally, in the second implementation method of the first aspect of the present invention, before inputting the positioning state quantity at the previous moment into the conversion function to obtain the first positioning state quantity at the current moment, it also includes: calculating the predicted deviation value and the Kalman gain matrix based on the positioning state quantity at the previous moment and the collected data; and updating the conversion function based on the predicted deviation value and the Kalman gain matrix.

[0007] Optionally, in a third implementation method of the first aspect of the present invention, the jump fault detection is performed on the positioning state quantity based on the collected data to obtain a jump detection result, including: calling the collected data at the previous moment and the current moment and the positioning state quantity at the previous moment; calculating the second positioning state quantity based on the collected data at the previous moment and the positioning state quantity; obtaining the jump quantity at the current moment based on the collected data at the current moment and the second positioning state quantity; calculating the second-order determinant of the jump quantity to obtain the jump judgment value at the current moment; determining whether the sensor corresponding to the jump judgment value is regarded as an abnormal positioning based on the size of the jump judgment value; and recording the sensor determined as the abnormal positioning in the jump detection result.

[0008] Optionally, in a fourth implementation manner of the first aspect of the present invention, the calculated error includes a Mahalanobis distance and an absolute error, and the calculation of the error between the collected data and the positioning state quantity includes: calculating the Mahalanobis distance and the absolute error between the collected data at the current moment and the positioning state quantity at the current moment; determining whether the sensor or positioning system corresponding to the Mahalanobis distance and the absolute error is an abnormal positioning based on the size of the Mahalanobis distance and the absolute error; and recording the abnormal positioning to the error.

[0009] Optionally, in a fifth implementation of the first aspect of the present invention, the ambiguous diagnosis is performed on the same state targeted by the collected data to obtain an ambiguous diagnosis result, including: calling the collected data targeted at the same state and collected at the current moment, wherein the collected data includes at least first collected data and second collected data; calculating an absolute deviation and a directional deviation based on the first collected data and the second collected data; based on the magnitude of the absolute deviation and the directional deviation, judging whether the difference between the sensors corresponding to the first collected data and the second collected data is within an allowable range; if not, locating the sensors corresponding to the first collected data and the second collected data as abnormalities and recording them in the ambiguous diagnosis result.

[0010] Optionally, in a sixth implementation manner of the first aspect of the present invention, the judging of the abnormal positioning existing in the positioning system based on the jump detection result, the error and the ambiguous diagnosis result includes: respectively obtaining the abnormal positioning in the judgment result, wherein the judgment result includes the abnormal positioning based on the jump detection result, the error and the ambiguous diagnosis result; judging whether there are consistent items in the abnormal positioning in the jump detection result, the error and the ambiguous diagnosis result; if so, excluding the abnormal positioning that only exists in the error and the ambiguous diagnosis result, and retaining the abnormal positioning that appears in the jump detection result; if not, removing and recording the abnormal positioning with duplicate items in the jump detection result, the error and the ambiguous diagnosis result.

[0011] The second aspect of the present invention provides a positioning diagnosis device, including: a data collection fusion module, which is used to obtain the collected data of several sensors and fuse the collected data of all sensors to obtain a positioning state quantity; a jump detection module, which is used to perform jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result; an error calculation module, which is used to calculate the error between the collected data and the positioning state quantity; an ambiguous diagnosis module, which is used to perform ambiguous diagnosis on the same state targeted by the collected data to obtain an ambiguous diagnosis result; and an abnormal positioning judgment module, which is used to judge the abnormal positioning in the positioning system based on the jump detection result, the error and the ambiguous diagnosis result.

[0012] Optionally, in a first implementation method of the second aspect of the present invention, the collected data fusion module is specifically used for: a data acquisition unit, which obtains the collected data of several sensors at the current moment and the positioning state quantity at the previous moment; a conversion unit, which inputs the positioning state quantity at the previous moment into a conversion function to obtain a first positioning state quantity at the current moment; and a correction unit, which corrects the first positioning state quantity according to the collected data at the current moment to obtain the positioning state quantity at the current moment.

[0013] Optionally, in a second implementation of the second aspect of the present invention, the collected data fusion module also includes a conversion function update unit, which is specifically used to: calculate the predicted deviation value and the Kalman gain matrix based on the positioning state quantity and the collected data at the previous moment; and update the conversion function based on the predicted deviation value and the Kalman gain matrix.

[0014] Optionally, in a third implementation method of the second aspect of the present invention, the jump detection module is specifically used to: call the collected data at the previous moment and the current moment and the positioning state quantity at the previous moment; calculate the second positioning state quantity based on the collected data at the previous moment and the positioning state quantity; calculate the jump quantity at the current moment based on the collected data at the current moment and the second positioning state quantity; calculate the second-order determinant of the jump quantity to obtain the jump judgment value at the current moment; determine whether the sensor corresponding to the jump judgment value is regarded as an abnormal positioning based on the size of the jump judgment value; and record the sensor determined as the abnormal positioning in the jump detection result.

[0015] Optionally, in a fourth implementation of the second aspect of the present invention, the error calculation module is specifically used to: calculate the Mahalanobis distance and absolute error of the collected data at the current moment and the positioning state quantity at the current moment; determine whether the sensor or positioning system corresponding to the Mahalanobis distance and the absolute error is an abnormal positioning based on the size of the Mahalanobis distance and the absolute error; and record the abnormal positioning to the error.

[0016] Optionally, in a fifth implementation of the second aspect of the present invention, the ambiguous diagnosis module is specifically used to: call the collected data for the same state and collected at the current moment, wherein the collected data includes at least first collected data and second collected data; calculate the absolute deviation and the directional deviation based on the first collected data and the second collected data; based on the size of the absolute deviation and the directional deviation, determine whether the difference between the sensors corresponding to the first collected data and the second collected data is within an allowable range; if not, locate the sensors corresponding to the first collected data and the second collected data as abnormalities and record them in the ambiguous diagnosis results.

[0017] Optionally, in a sixth implementation of the second aspect of the present invention, the abnormal location judgment module is specifically used to: obtain the abnormal location in the judgment result respectively, wherein the judgment result includes the abnormal location based on the jump detection result, the error and the ambiguous diagnosis result; judge whether there are consistent items in the abnormal location in the jump detection result, the error and the ambiguous diagnosis result; if so, exclude the abnormal location that only exists in the error and the ambiguous diagnosis result, and retain the abnormal location that appears in the jump detection result; if not, remove and record the abnormal location with duplicate items recorded in the jump detection result, the error and the ambiguous diagnosis result.

[0018] A third aspect of the present invention provides a positioning diagnostic device, comprising: a memory and at least one processor, wherein a request is stored in the memory, and the memory and the at least one processor are interconnected via a line; the at least one processor calls the request in the memory so that the positioning diagnostic device executes the steps of the above-mentioned positioning diagnostic method.

[0019] A fourth aspect of the present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a request, which, when executed on a computer, causes the computer to execute the steps of the above-mentioned positioning diagnosis method.

[0020] In the technical solution of the present invention, the collected data of several sensors are obtained and the collected data of all sensors are fused to obtain a positioning state quantity; based on the collected data, a jump fault detection is performed on the positioning state quantity to obtain a jump detection result; the error between the collected data and the positioning state quantity is calculated; an ambiguity diagnosis is performed on the same state targeted by the collected data to obtain an ambiguity diagnosis result; and the abnormal positioning in the positioning system is judged based on the jump detection result, the error, and the ambiguity diagnosis result. In this method, the positioning state quantity required for positioning at the current moment is estimated by sensor fusion based on the extended Kalman filter on the collected data obtained by the sensors. Subsequently, by determining the sensors that do not conform to the kinematic principle, the sensors and / or positioning systems that have errors, and the two sensors for the same state that are ambiguous, the abnormal positioning between the three results is determined, and the consistent items are found, the sensor or positioning system with the problem is located, and positioning diagnosis of the sensor and positioning system is achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 This is a schematic diagram of a first embodiment of a positioning diagnosis method according to an embodiment of the present invention;

[0022] Figure 2 Schematic diagram of a second embodiment of the positioning diagnosis method according to an embodiment of the present invention;

[0023] Figure 3 Schematic diagram of a third embodiment of the positioning diagnosis method according to an embodiment of the present invention;

[0024] Figure 4 A schematic diagram of an embodiment of a positioning diagnosis device according to an embodiment of the present invention;

[0025] Figure 5 A schematic diagram of another embodiment of a positioning diagnosis device according to an embodiment of the present invention;

[0026] Figure 6 FIG. 1 is a schematic diagram of an embodiment of a positioning diagnosis device in an embodiment of the present invention. DETAILED DESCRIPTION

[0027] In the technical solution of the present invention, the collected data of several sensors are obtained and the collected data of all sensors are fused to obtain a positioning state quantity; based on the collected data, a jump fault detection is performed on the positioning state quantity to obtain a jump detection result; the error between the collected data and the positioning state quantity is calculated; an ambiguity diagnosis is performed on the same state targeted by the collected data to obtain an ambiguity diagnosis result; and the abnormal positioning in the positioning system is judged based on the jump detection result, the error, and the ambiguity diagnosis result. In this method, the positioning state quantity required for positioning at the current moment is estimated by sensor fusion based on the extended Kalman filter on the collected data obtained by the sensors. Subsequently, by determining the sensors that do not conform to the kinematic principle, the sensors and / or positioning systems that have errors, and the two sensors for the same state that are ambiguous, the abnormal positioning between the three results is determined, and the consistent items are found, the sensor or positioning system with the problem is located, and positioning diagnosis of the sensor and positioning system is achieved.

[0028] The terms "first," "second," "third," "fourth," and the like (if any) in the description and claims of the present invention and in the accompanying drawings are used to distinguish similar components and are not necessarily used to describe a particular order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate so that the embodiments described herein can be implemented in an order other than that illustrated or described herein. In addition, the terms "including" or "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product, or apparatus that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units that are not explicitly listed or that are inherent to these processes, methods, products, or apparatus.

[0029] For ease of understanding, the specific process of the embodiment of the present invention is described below. Figure 1 The first embodiment of the positioning diagnosis method in the embodiment of the present invention includes:

[0030] 101. Acquire data collected by several sensors and fuse the data collected by all sensors to obtain positioning state quantity;

[0031] In this embodiment, the plurality of sensors includes at least an IMU (Inertial Measurement Unit), a LiDAR (LiDAR), a Global Navigation Satellite System (GNSS), and a wheel odometer. The sensor-collected data corresponds to an estimated positioning state quantity. That is, for any collected data, there is a corresponding positioning state quantity. The positioning state quantity is a predicted value for the sensor-collected data.

[0032] Specifically, the positioning state quantity or collected data at least includes the subject's position P, velocity V, posture R, IMU acceleration bias α, and IMU angular velocity bias β. The covariance matrix of the state quantity is By using the extended Kalman filter to fuse the above collected data, the positioning state quantity corresponding to the collected data is obtained.

[0033] 102. Perform jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result;

[0034] In this embodiment, by obtaining the collected data of the current frame, a jump fault detection is performed on the positioning state quantity corresponding to the collected data of the previous frame to determine whether there is a jump fault in the positioning state quantity, and the abnormal positioning with a jump fault is recorded in the jump detection result.

[0035] Specifically, the IMU can measure the acceleration and angular velocity of the subject in a short period of time. The motion speed is one of the positioning state quantities of the positioning system. Based on the speed estimated at the previous moment and the current moment and the position estimation result at the previous moment, the current position is predicted, and then the deviation is estimated from the actual position estimation result to determine whether a jump has occurred; based on the angular velocity measured by the IMU at the previous moment and the current moment and the attitude estimation result at the previous moment, the current attitude is predicted, and then the deviation is estimated from the actual attitude estimation result to determine whether a jump has occurred; based on the acceleration measured by the IMU at the previous moment and the current moment and the speed estimation result at the previous moment, the current positioning speed is predicted, and then the deviation is estimated from the actual speed estimation result to determine whether a jump has occurred.

[0036] 103. Calculate the error between the collected data and the positioning state quantity;

[0037] In this embodiment, the error between the collected data and the positioning state quantity includes the Mahalanobis distance and the absolute error.

[0038] Specifically, during the extended Kalman filter-based fusion positioning process, the Mahalanobis distance and absolute error between the positioning state variables (position, attitude, and state) and the sensor data are calculated. When the Mahalanobis distance or absolute error exceeds a set threshold, it indicates a significant deviation between the positioning state variables and the sensor data, indicating a fault in the fusion positioning system or one of the sensors. A fault diagnosis indicating excessive Mahalanobis distance or absolute error is issued, or the sensor or positioning system exceeding the threshold is recorded in the error column.

[0039] 104. Performing ambiguity diagnosis on the same state targeted by the collected data to obtain an ambiguity diagnosis result;

[0040] In this embodiment, when different sensors have large deviations in the data collected for the same state, it is considered that there is ambiguity between the sensors. In this case, a sensor ambiguity fault diagnosis should be sent, or the ambiguous sensor should be recorded in the ambiguity diagnosis result.

[0041] 105. Determine abnormal positioning in the positioning system based on jump detection results, error and ambiguity diagnosis results.

[0042] In this embodiment, the abnormal positioning that may be abnormal is compared and judged by comparing the jump detection results, errors and ambiguous diagnosis results to determine the abnormal positioning of the specific fault in the positioning system, wherein the abnormal positioning is the sensor or positioning system that is judged to have a fault.

[0043] In this embodiment, the collected data of several sensors are acquired and the collected data of all sensors are fused to obtain a positioning state quantity; based on the collected data, a jump fault detection is performed on the positioning state quantity to obtain a jump detection result; the error between the collected data and the positioning state quantity is calculated; an ambiguity diagnosis is performed on the same state targeted by the collected data to obtain an ambiguity diagnosis result; and the abnormal positioning in the positioning system is judged based on the jump detection result, the error, and the ambiguity diagnosis result. In this method, the positioning state quantity required for positioning at the current moment is estimated by sensor fusion based on the extended Kalman filter on the collected data acquired by the sensors. Subsequently, by determining the sensors that do not conform to the kinematic principle, the sensors and / or positioning systems that have errors, and the two sensors for the same state that are ambiguous, the abnormal positioning between the three results is determined, and the consistent items are found, the sensor or positioning system with the problem is located, and positioning diagnosis of the sensor and positioning system is achieved.

[0044] See also Figure 2 The second embodiment of the positioning diagnosis method in the embodiment of the present invention includes:

[0045] 201. Obtaining the current collected data and the last positioning state of several sensors;

[0046] In this embodiment, the acquired sensor data and the positioning state quantity at the previous moment are divided into position, speed and state, which are used to correct the current moment positioning state quantity respectively for position, speed and state in subsequent steps.

[0047] 202. Calculate the prediction deviation value and the Kalman gain matrix based on the positioning state quantity and the collected data at the previous moment;

[0048] In this embodiment, according to the acceleration and angular velocity measured by the IMU sensor, the positioning state quantity of the previous moment is Converted into the current positioning state The conversion function is f(X). The subscript p represents the predicted result of the current positioning state, the subscript c represents the corrected result of the positioning state, which is the final output of the system at each moment, n is the measurement noise of the IMU, and k is the current moment. The update formula for the positioning state and covariance matrix is:

[0049]

[0050]

[0051] In this embodiment, the data collected by the i-th sensor is The current positioning state is The observation function is Where n is the measurement noise of the sensor. The predicted deviation between the solved positioning state and the actual sensor data is calculated as:

[0052]

[0053] In this embodiment, The matrix is ​​the observation function h of the i-th sensor i (X) The Jacobian matrix of the state quantity is Taking the correction process of wheel odometer as an example, is the current state The velocity estimation in the coordinate system is the IMU coordinate system. It is the data collected by the wheel odometer at the current moment, and the coordinate system is the wheel odometer coordinate system. There is a rotation conversion relationship between the IMU and the wheel odometer The specific form of the above formula is:

[0054]

[0055] Measurement noise n c The covariance matrix of The observation Kalman gain matrix can be obtained for:

[0056]

[0057] 203. Update the conversion function based on the prediction deviation value and the Kalman gain matrix;

[0058] 204. Input the positioning state quantity at the previous moment into the conversion function to obtain the first positioning state quantity at the current moment;

[0059] In this embodiment, the positioning state quantity and covariance are corrected and updated:

[0060]

[0061]

[0062] From this, the state estimation result of the extended Kalman filter can be obtained and the corresponding covariance matrix in, This is the positioning state result required by the system. It is used for the next prediction step.

[0063] 205. Correct the first positioning state quantity according to the collected data at the current moment to obtain the positioning state quantity at the current moment;

[0064] 206. Calling the collected data at the previous moment, the current moment, and the positioning state quantity at the previous moment;

[0065] 207. Calculate a second positioning state quantity based on the collected data and the positioning state quantity at the previous moment;

[0066] 208. Calculate the jump variable at the current moment based on the collected data at the current moment and the second positioning state quantity;

[0067] 209. Calculate the second-order determinant of the jump variable to obtain the jump judgment value at the current moment;

[0068] 210. Determine whether the sensor corresponding to the jump judgment value is located as abnormal based on the size of the jump judgment value;

[0069] In this embodiment, a reasonable threshold is preset for the jump judgment value, and whether the sensor is identified as abnormal positioning is determined by judging whether the jump judgment value exceeds the preset threshold.

[0070] 211. Record the sensor determined to be abnormally positioned into the jump detection result;

[0071] Specifically, the position at the last moment is The speed is V k-1 (v x,-1 ,v y, ,v z,k-1 ), the current speed is V k (v x,k ,v y,k ,v z, ), the time interval is dt. Therefore, the current position can be predicted to obtain the second positioning state quantity about the position:

[0072]

[0073] The location where the collected data will be obtained and the corresponding time position of the positioning state Solving for the deviation:

[0074]

[0075] P jump That is the jump variable of the position. When ||P jump ||2, that is, when the jump judgment value is greater than the threshold, it means that the position estimation result has changed between the two moments in time, which is inconsistent with the kinematic principle. Therefore, the system will send a fault judgment of the corresponding position sensor and determine it as abnormal positioning.

[0076] The posture at the previous moment is The angular velocity measured by the IMU is W k-1 (v p,1 ,v r, ,v y, ), the angular velocity at the current moment is W k (v p, ,v r, ,v y,k ), the time interval is dt. Therefore, the posture at the current moment can be predicted to obtain the second positioning state quantity about the posture:

[0077]

[0078] The posture of the collected data and the corresponding time position of the positioning state Solving for the deviation:

[0079]

[0080] R jump That is the jump variable of the position. When ||R jump ||2, that is, when the jump judgment value is greater than the threshold, it means that the estimated result of the posture has changed between these two moments and does not conform to the kinematic principles. Therefore, at this time, the system will send a fault judgment of the corresponding posture sensor and determine it as abnormal positioning.

[0081] The velocity at the previous moment is V k-1 (v x,1 ,v y,k-1 ,v z,k-1 ), the acceleration measured by IMU is a k-1 (a x,1 ,a y, ,a z, ), the acceleration at the current moment is a k (a x, ,a y, ,az, ), the time interval is dt. Therefore, the speed at the current moment can be predicted to obtain the second positioning state quantity about the speed:

[0082]

[0083] The speed at which data is acquired and the positioning state velocity corresponding to the time Solving for the deviation:

[0084]

[0085] V jump That is the jump amount of speed. When ||V jump ||2, that is, when the jump judgment value is greater than the threshold, it means that the estimated result of the posture has changed between the two moments and does not conform to the kinematic principles. Therefore, at this time, the system will send a fault judgment of the speed corresponding sensor and determine it as abnormal positioning.

[0086] In this embodiment, after the jump detection, the sensor confirmed to be abnormally positioned is recorded in the jump detection result, which is used together with the error and ambiguity diagnosis results to determine the abnormal positioning with problems.

[0087] 212. Calculate the error between the collected data and the positioning state quantity;

[0088] 213. Performing ambiguity diagnosis on the same state targeted by the collected data to obtain an ambiguity diagnosis result;

[0089] 214. Determine abnormal positioning in the positioning system based on jump detection results, error and ambiguity diagnosis results.

[0090] Based on the previous embodiment, this embodiment describes in detail the process of calling the collected data at the previous moment and the current moment and the positioning state quantity at the previous moment; calculating the second positioning state quantity based on the collected data at the previous moment and the positioning state quantity; calculating the jump quantity at the current moment based on the collected data at the current moment and the second positioning state quantity; calculating the second-order determinant of the jump quantity to obtain the jump judgment value at the current moment; determining whether the sensor corresponding to the jump judgment value is considered as an abnormal positioning based on the size of the jump judgment value; and recording the sensor determined to be abnormally positioned into the jump detection result. Compared with the traditional method, this embodiment clarifies the specific method of jump detection. By adopting the jump detection method, the sensor that has undergone changes that do not conform to the kinematic principle is located, which facilitates the user to perform subsequent maintenance on the problem sensor.

[0091] See also Figure 3 , a third embodiment of the positioning diagnosis method in the embodiment of the present invention includes:

[0092] 301. Acquire data collected by several sensors and fuse the data collected by all sensors to obtain a positioning state value;

[0093] 302. Perform transition fault detection on the positioning state quantity based on the collected data to obtain a transition detection result;

[0094] 303. Calculate the Mahalanobis distance and absolute error between the collected data at the current moment and the positioning state quantity at the current moment;

[0095] In this embodiment, the collected data and positioning state of the laser radar sensor are taken as examples to illustrate the calculation process of the Mahalanobis distance and the absolute error.

[0096] Specifically, the collected data of the laser radar at the kth moment is: The positioning state of the positioning system is: The Jacobian matrix of the observation function to the state quantity is The result is The deviation between the calculated positioning state and the collected data is:

[0097]

[0098] The measurement noise covariance of the lidar is The covariance matrix of the measurement error is:

[0099]

[0100] The Mahalanobis distance is:

[0101]

[0102] The absolute error is:

[0103]

[0104] 304. Determine whether the sensor or positioning system corresponding to the Mahalanobis distance and the absolute error is considered as an abnormal positioning based on the Mahalanobis distance and the absolute error;

[0105] In this embodiment, the absolute error reflects the most intuitive error between the sensor's collected data and the positioning system's estimate. If the absolute error is too large, it indicates a significant deviation between the sensor's collected data and the positioning state, indicating a malfunction in either of them. The Mahalanobis distance incorporates the covariance matrix of measurement errors and reflects the impact of the sensor's collected data on the positioning result after integration. When the sensor's Mahalanobis distance is too large, it indicates a significant deviation between the sensor's measurement and the positioning system's estimate. This measurement can significantly impact the positioning system's estimate and create ambiguity with measurements from other sensors.

[0106] Specifically, the sensors that need to calculate the Mahalanobis distance and absolute error include at least attitude measurement of a lidar, speed measurement of a wheel odometer, position measurement of a GNSS, speed measurement of a GNSS, and attitude measurement of a GNSS.

[0107] 305. Record the abnormal location to the error;

[0108] In this embodiment, the sensor and positioning system whose Mahalanobis distance and / or absolute error exceeds a preset threshold value are determined as abnormal positioning and recorded in the error.

[0109] Specifically, in this step, the abnormal positioning recorded in the error should include the sensor and the positioning system. By judging the abnormal positioning in the subsequent step 313, the specific sensor with the problem is determined.

[0110] 306. Calling the collected data for the same state and collected at the current moment;

[0111] 307. Calculate an absolute deviation and a directional deviation based on the first collected data and the second collected data;

[0112] Specifically, the speed measurement sensors in this positioning system include two redundant methods: wheel odometer and GNSS. The speed measurement result of the wheel odometer is The GNSS velocity measurement result is The absolute deviation and directional deviation of the two are solved.

[0113] Absolute Deviation:

[0114]

[0115] The cross product is:

[0116]

[0117] Using the second norm of the two to adjust the scale of the cross product, we can get the directional deviation:

[0118]

[0119] 308. Based on the magnitudes of the absolute deviation and the directional deviation, determine whether the difference between the sensor corresponding to the first collected data and the second collected data is within an allowable range;

[0120] In this embodiment, when the absolute deviation and directional deviation If both the absolute deviation and the directional deviation are less than the corresponding preset thresholds, the two sensors are considered to have consistent measurements, indicating a normal state. If either the absolute deviation or the directional deviation is greater than the set threshold, the two sensors are considered to be ambiguous, indicating a problem with at least one of them.

[0121] 309. If not, the sensors corresponding to the first collected data and the second collected data are located as abnormal and recorded in the ambiguous diagnosis result;

[0122] 310. Obtain the abnormality locations in the judgment results respectively;

[0123] In this embodiment, the judgment result includes the abnormal positioning recorded in the jump detection result, error and ambiguity diagnosis result. By comparing the abnormal positioning at the same or similar time, the specific sensor or positioning system with the problem is determined.

[0124] 311. Determine whether there is a consistent item among the abnormality locations in the jump detection results, error and ambiguity diagnosis results;

[0125] In this embodiment, the jump detection result is used to determine the sensor whose changes do not conform to the kinematic principles, the error is used to determine the sensor and / or positioning system with errors, and the ambiguity diagnosis result is used to determine two ambiguous sensors. By determining the abnormal positioning among the three results, the consistent items are found and the sensor or positioning system with the problem is located.

[0126] 312. If yes, exclude the abnormal positioning that only exists in the error and ambiguity diagnosis results, and retain the abnormal positioning that appears in the jump detection results;

[0127] In this embodiment, if there are consistent items, the sensor corresponding to the consistent items is the problematic sensor, facilitating the user's quick location of the problematic sensor. If there are no consistent items, duplicate items between the transition detection results, error and ambiguous diagnosis results are removed, and each abnormal location is recorded and reported. This indicates that the judgment result cannot accurately locate the specific problematic sensor or that there are too many problematic sensors, making it impossible to accurately distinguish between problematic and normal sensors using the location diagnosis method. Therefore, only the abnormal location with duplicate items is removed, retaining the uniqueness of the abnormal location and recording it, narrowing the scope of the judgment result and facilitating further judgment by the user.

[0128] 313. If not, remove the abnormal location with duplicate entries in the jump detection results, error and ambiguity diagnosis results and record them.

[0129] Based on the previous embodiment, this embodiment describes in detail the process of calling the collected data for the same state and collected at the current moment, wherein the collected data includes at least the first collected data and the second collected data; calculating the absolute deviation and the directional deviation based on the first collected data and the second collected data; judging whether the difference between the sensors corresponding to the first collected data and the second collected data is within the allowable range based on the magnitude of the absolute deviation and the directional deviation; if not, locating the sensors corresponding to the first collected data and the second collected data as abnormalities and recording them in the ambiguous diagnosis result. Compared with the traditional method, this embodiment refines the specific ambiguous diagnosis method. When the absolute deviation and the directional deviation are both less than the corresponding thresholds, it can be considered that the measurement results of the two sensors are consistent, and this is a normal state. If one of the absolute deviation or the directional deviation is greater than the set threshold, it is considered that the two sensors are ambiguous, and at least one of the sensors has a problem.

[0130] The positioning diagnosis method in the embodiment of the present invention is described above. The positioning diagnosis device in the embodiment of the present invention is described below. Figure 4 In one embodiment of the present invention, a positioning diagnosis device includes:

[0131] The data collection fusion module 401 is used to obtain the data collected by several sensors and fuse the data collected by all sensors to obtain the positioning state;

[0132] A jump detection module 402 is configured to perform jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result;

[0133] An error calculation module 403 is used to calculate the error between the collected data and the positioning state quantity;

[0134] An ambiguity diagnosis module 404 is configured to perform ambiguity diagnosis on the same state targeted by the collected data to obtain an ambiguity diagnosis result;

[0135] The abnormal positioning judgment module 405 is used to judge the abnormal positioning in the positioning system based on the jump detection result, the error and the ambiguous diagnosis result.

[0136] In an embodiment of the present invention, a positioning diagnosis device runs the above-mentioned positioning diagnosis method, including obtaining collected data from a number of sensors and fusing the collected data from all sensors to obtain a positioning state quantity; performing jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result; calculating the error between the collected data and the positioning state quantity; performing ambiguity diagnosis on the same state targeted by the collected data to obtain an ambiguity diagnosis result; and judging abnormal positioning in the positioning system based on the jump detection result, the error, and the ambiguity diagnosis result. In this method, the positioning state quantity required for positioning at the current moment is estimated by performing sensor fusion based on the extended Kalman filter on the collected data obtained by the sensors. Subsequently, by determining the sensors that do not conform to the kinematic principle, the sensors and / or positioning systems that have errors, and the two sensors for the same state that are ambiguous, the abnormal positioning between the three results is determined, and the consistent items are found, the sensor or positioning system with the problem is located, and positioning diagnosis of the sensor and positioning system is achieved.

[0137] See also Figure 5 The second embodiment of the positioning diagnosis device in the embodiment of the present invention includes:

[0138] The data collection fusion module 401 is used to obtain the data collected by several sensors and fuse the data collected by all sensors to obtain the positioning state;

[0139] A jump detection module 402 is configured to perform jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result;

[0140] An error calculation module 403 is used to calculate the error between the collected data and the positioning state quantity;

[0141] An ambiguity diagnosis module 404 is configured to perform ambiguity diagnosis on the same state targeted by the collected data to obtain an ambiguity diagnosis result;

[0142] The abnormal positioning judgment module 405 is used to judge the abnormal positioning in the positioning system based on the jump detection result, the error and the ambiguous diagnosis result.

[0143] In this embodiment, the collected data fusion module 401 is specifically used to:

[0144] The data acquisition unit 4011 obtains the current collected data of several sensors and the positioning state quantity at the previous moment; the conversion unit 4012 inputs the positioning state quantity at the previous moment into the conversion function to obtain the first positioning state quantity at the current moment; the correction unit 4013 corrects the first positioning state quantity according to the collected data at the current moment to obtain the positioning state quantity at the current moment.

[0145] In this embodiment, the collected data fusion module 401 further includes a conversion function updating unit 4014, which is specifically configured to:

[0146] A prediction deviation value and a Kalman gain matrix are calculated based on the positioning state quantity and the collected data at the previous moment; and the conversion function is updated based on the prediction deviation value and the Kalman gain matrix.

[0147] In this embodiment, the transition detection module 402 is specifically configured to:

[0148] Call the collected data at the previous moment and the current moment and the positioning state quantity at the previous moment; calculate the second positioning state quantity based on the collected data at the previous moment and the positioning state quantity; calculate the jump quantity at the current moment based on the collected data at the current moment and the second positioning state quantity; calculate the second-order determinant of the jump quantity to obtain the jump judgment value at the current moment; determine whether the sensor corresponding to the jump judgment value is regarded as an abnormal positioning based on the size of the jump judgment value; record the sensor determined as the abnormal positioning in the jump detection result.

[0149] In this embodiment, the error calculation module 403 is specifically used to:

[0150] Calculate the Mahalanobis distance and absolute error of the collected data at the current moment and the positioning state quantity at the current moment; determine whether the sensor or positioning system corresponding to the Mahalanobis distance and the absolute error is an abnormal positioning based on the magnitude of the Mahalanobis distance and the absolute error; and record the abnormal positioning to the error.

[0151] In this embodiment, the ambiguity diagnosis module 404 is specifically configured to:

[0152] Call the collected data for the same state and collected at the current moment, wherein the collected data includes at least first collected data and second collected data; calculate an absolute deviation and a directional deviation based on the first collected data and the second collected data; based on the magnitudes of the absolute deviation and the directional deviation, determine whether a difference between sensors corresponding to the first collected data and the second collected data is within an allowable range; if not, locate the sensors corresponding to the first collected data and the second collected data as abnormalities and record them in the ambiguous diagnosis result.

[0153] In this embodiment, the abnormality location judgment module 405 is specifically used to:

[0154] Obtain the abnormal location in the judgment result respectively, wherein the judgment result includes the abnormal location based on the jump detection result, the error and the ambiguous diagnosis result; judge whether there is a consistent item in the abnormal location in the jump detection result, the error and the ambiguous diagnosis result; if so, exclude the abnormal location that only exists in the error and the ambiguous diagnosis result, and retain the abnormal location that appears in the jump detection result; if not, remove the abnormal location with duplicate items recorded in the jump detection result, the error and the ambiguous diagnosis result and record them.

[0155] Based on the previous embodiment, this embodiment describes in detail the specific functions of each module and the unit structure of some modules. Through the above modules and the specific functions of the original modules, the operation of the positioning diagnosis device is improved, the reliability during operation is improved, the actual logic between each step is clarified, and the practicality of the device is improved.

[0156] above Figure 4 and Figure 5 The positioning diagnosis apparatus in the embodiment of the present invention is described in detail from the perspective of modular functional entities. The positioning diagnosis device in the embodiment of the present invention is described in detail from the perspective of hardware processing.

[0157] Figure 6: is a structural diagram of a positioning diagnostic device provided by an embodiment of the present invention. The positioning diagnostic device 600 may have relatively large differences due to different configurations or performances, and may include one or more processors (central processing units, CPU) 610 (for example, one or more processors) and a memory 620, and one or more storage media 630 (for example, one or more mass storage devices) storing application programs 633 or data 632. Among them, the memory 620 and the storage medium 630 can be temporary storage or permanent storage. The program stored in the storage medium 630 may include one or more modules (not shown in the figure), and each module may include a series of request operations in the positioning diagnostic device 600. Furthermore, the processor 610 can be configured to communicate with the storage medium 630 and execute a series of request operations in the storage medium 630 on the positioning diagnostic device 600 to implement the steps of the above-mentioned positioning diagnostic method.

[0158] The positioning diagnostic device 600 may further include one or more power supplies 640, one or more wired or wireless network interfaces 650, one or more input and output interfaces 660, and / or one or more operating systems 631, such as Windows Server, Mac OS X, Unix, Linux, FreeBSD, etc. It will be understood by those skilled in the art that Figure 6 The illustrated structure of the positioning diagnosis device does not constitute a limitation on the positioning diagnosis device provided in this application, and may include more or fewer components than shown in the figure, or a combination of certain components, or a different arrangement of components.

[0159] The present invention also provides a computer-readable storage medium, which may be a non-volatile computer-readable storage medium or a volatile computer-readable storage medium. The computer-readable storage medium stores a request, and when the request is run on a computer, the computer executes the steps of the positioning diagnosis method.

[0160] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described systems, devices, and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0161] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several requests for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM), random access memory (RAM), magnetic disk or optical disk, etc. Various media that can store program codes.

[0162] As described above, the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that the technical solutions described in the above embodiments can still be modified, or some of the technical features thereof can be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A positioning diagnosis method, applied to a positioning system, characterized in that: The positioning diagnosis method comprises: Acquire the data collected by several sensors and fuse the data collected by all sensors to obtain the positioning state; Performing jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result; Calculating the error between the collected data and the positioning state quantity; Performing an ambiguous diagnosis on the same state targeted by the collected data to obtain an ambiguous diagnosis result, wherein the collected data is data collected by different sensors on the same state; Determining abnormal positioning in the positioning system based on the jump detection result, the error, and the ambiguity diagnosis result; The performing jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result includes: Calling the collected data at the previous moment and the current moment and the positioning state quantity at the previous moment; Calculating a second positioning state quantity based on the collected data at the previous moment and the positioning state quantity; Calculating the jump value at the current moment based on the collected data at the current moment and the second positioning state quantity; Calculating the second-order determinant of the jump value to obtain the jump judgment value at the current moment; determining, based on the magnitude of the jump judgment value, whether the sensor corresponding to the jump judgment value is positioned as abnormal; The sensor determined to be abnormally positioned is recorded in the jump detection result.

2. The positioning diagnosis method according to claim 1, characterized in that: The acquisition of data collected by several sensors and fusing the data collected by all sensors to obtain the positioning state quantity includes: Obtain the current collected data of several sensors and the positioning state quantity of the previous moment; Input the positioning state quantity of the previous moment into the conversion function to obtain the first positioning state quantity of the current moment; The first positioning state quantity is corrected according to the collected data at the current moment to obtain the positioning state quantity at the current moment.

3. The positioning diagnosis method according to claim 2, characterized in that: Before inputting the positioning state quantity at the previous moment into the conversion function to obtain the first positioning state quantity at the current moment, the method further includes: Calculating a prediction deviation value and a Kalman gain matrix based on the positioning state quantity and the collected data at the previous moment; The conversion function is updated based on the prediction deviation value and the Kalman gain matrix.

4. The positioning diagnosis method according to claim 2, characterized in that: The calculating the error between the collected data and the positioning state quantity includes: Calculating the Mahalanobis distance and absolute error between the collected data at the current moment and the positioning state quantity at the current moment; determining whether the sensor or positioning system corresponding to the Mahalanobis distance and the absolute error is positioned as abnormal based on the Mahalanobis distance and the absolute error; The anomaly location is recorded as an error.

5. The positioning diagnosis method according to claim 2, characterized in that: The performing of ambiguity diagnosis on the same state targeted by the collected data to obtain an ambiguity diagnosis result includes: Calling the collected data for the same state and collected at the current moment, wherein the collected data at least includes first collected data and second collected data; Calculating an absolute deviation and a directional deviation based on the first collected data and the second collected data; Based on the magnitudes of the absolute deviation and the directional deviation, determining whether a difference between sensors corresponding to the first collected data and the second collected data is within an allowable range; If not, the sensors corresponding to the first collected data and the second collected data are located as abnormalities and recorded in the ambiguous diagnosis result.

6. The positioning diagnosis method according to any one of claims 3 to 5, characterized in that: The determining of abnormal positioning in the positioning system based on the jump detection result, the error and the ambiguity diagnosis result includes: Respectively obtaining abnormality locations in the judgment results, wherein the judgment results include the jump detection results, the error and ambiguity diagnosis results; Determining whether there is a consistent item among the jump detection result, the error, and the abnormality location in the ambiguous diagnosis result; If so, exclude the abnormal positioning that only exists in the error and the ambiguous diagnosis result, and retain the abnormal positioning that appears in the jump detection result; If not, remove and record the abnormal positioning with duplicate items in the jump detection result, the error and ambiguity diagnosis result.

7. A positioning diagnostic device, characterized in that: The positioning diagnosis device is applied to a positioning system, the positioning system includes a plurality of sensors, and the positioning diagnosis device includes: The data fusion module is used to obtain the data collected by several sensors and fuse the data collected by all sensors to obtain the positioning state; A jump detection module is used to perform jump fault detection on the positioning state quantity based on the collected data to obtain a jump detection result; An error calculation module, used to calculate the error between the collected data and the positioning state quantity; an ambiguity diagnosis module, configured to perform ambiguity diagnosis on the same state targeted by the collected data to obtain an ambiguity diagnosis result, wherein the collected data is data collected by different sensors on the same state; an abnormal positioning judgment module, configured to judge abnormal positioning existing in the positioning system based on the jump detection result, the error and the ambiguous diagnosis result; The jump detection module is specifically used to: call the collected data at the previous moment and the current moment and the positioning state quantity at the previous moment; calculate the second positioning state quantity based on the collected data at the previous moment and the positioning state quantity; calculate the jump quantity at the current moment based on the collected data at the current moment and the second positioning state quantity; calculate the second-order determinant of the jump quantity to obtain the jump judgment value at the current moment; determine whether the sensor corresponding to the jump judgment value is regarded as an abnormal positioning based on the size of the jump judgment value; and record the sensor determined as the abnormal positioning in the jump detection result.

8. A positioning diagnostic device, characterized in that: The positioning diagnosis device includes: a memory and at least one processor, wherein the memory stores a request, and the memory and the at least one processor are interconnected via a line; The at least one processor calls the request in the memory to enable the positioning diagnosis device to execute each step of the positioning diagnosis method according to any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, each step of the positioning diagnosis method according to any one of claims 1 to 6 is implemented.

Images