Positioning fault detection method and device, equipment, storage medium and program product
Through the inertial measurement unit and wheel speed sensor combined with Kalman filtering technology, the lateral error covariance and error amount of the positioning target's posture information is detected, which solves the accuracy of positioning sensor fault detection and realizes effective detection of positioning sensor faults. The error covariance jump caused by vehicle attitude changes is avoided and safe operation is ensured.
Patent Information
- Application Number
- CN202411437375.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-10-15
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-10-15
AI Technical Summary
The prior art is difficult to effectively detect and report sensor failures for positioning, especially in mobile robots or autonomous vehicles, resulting in a threat to safe operation.
Through the inertial measurement unit and wheel speed sensor measurement information, Kalman filtering is used to determine the lateral error covariance and error quantity of the position information of the moving target, calculate the ratio of the lateral error quantity and the lateral error covariance, perform positioning sensor fault detection, and eliminate Error covariance jump due to vehicle attitude changes.
Accurate detection of positioning sensor faults is achieved, avoiding the interference of error covariance jumps when turning, ensuring the accuracy and stability of fault detection, and reducing cost and calculation complexity.
Smart Images

Figure CN118960787A_ABST
Abstract
Description
Technical Field
[0001] Embodiments of this disclosure relate to the fields of robotics and autonomous driving technologies, and in particular to a positioning fault detection method, a positioning fault detection device, an electronic device, a computer-readable storage medium, and a computer program product. Background Technology
[0002] With the development of robotics and autonomous driving technologies, localization is a crucial condition for the normal operation of both mobile robots and autonomous vehicles. Therefore, accurately detecting malfunctions in localization sensors, such as sudden jumps, and correctly reporting these malfunctions to the relevant systems in the mobile robot or autonomous vehicle is essential for ensuring their safe operation. Summary of the Invention
[0003] The embodiments of this disclosure provide a location fault detection method, location fault detection device, electronic device, computer-readable storage medium, and computer program product, which can solve or partially solve the above-mentioned deficiencies or other deficiencies in the prior art.
[0004] A positioning fault detection method according to a first aspect of this disclosure includes: determining the lateral error covariance of the pose information of the moving target over a preset time period based on information measured by an inertial measurement unit and a wheel speed sensor mounted on the moving target; determining the lateral error between the pose information of the moving target based on first pose information and second pose information of the moving target at a target time, wherein the target time is the end time of the preset time period, the first pose information is measured by a positioning sensor mounted on the moving target, and the second pose information is determined by the inertial measurement unit and the wheel speed sensor; and detecting a fault in the positioning sensor based on the ratio of the lateral error to the lateral error covariance.
[0005] In one embodiment of this disclosure, determining the lateral error covariance of the pose information of the moving target over a preset time period includes: determining the accumulated error covariance of the pose information of the moving target over the preset time period using Kalman filtering based on information measured by the inertial measurement unit and the wheel speed sensor; and determining the lateral error covariance based on the error covariance and the pose information of the moving target at the target time determined by the inertial measurement unit.
[0006] In one embodiment of this disclosure, determining the accumulated error covariance of the pose information of the moving target within a preset time period using Kalman filtering based on the information measured by the inertial measurement unit and the wheel speed sensor includes: determining an error differential equation based on the noise and measured information of the inertial measurement unit and the wheel speed sensor, wherein the error includes position error, heading angle error, wheel speed coefficient error, and gyroscope bias error; and discretizing the error differential equation and performing Kalman filtering on the discretized error differential equation to obtain the accumulated error covariance of the pose information of the moving target within the preset time period.
[0007] In one embodiment of this disclosure, determining the error differential equation based on the noise and measured information of the inertial measurement unit and the wheel speed sensor includes: determining gyroscope angular velocity noise and gyroscope angular velocity bias noise based on the parameters of the inertial measurement unit; determining wheel speed noise and wheel speed coefficient noise based on the parameters of the wheel speed sensor; and determining the error differential equation based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the gyroscope angular velocity noise, the gyroscope angular velocity bias noise, the wheel speed noise, and the wheel speed coefficient noise.
[0008] In one embodiment of this disclosure, determining the error differential equation based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the gyroscope angular velocity noise, the gyroscope angular velocity bias noise, the wheel speed noise, and the wheel speed coefficient noise includes: determining the wheel speed coefficient error differential equation based on the wheel speed coefficient noise; determining the gyroscope bias error differential equation based on the gyroscope angular velocity bias noise; determining the heading angle error differential equation based on the gyroscope angular velocity noise and the gyroscope bias error; and determining the position error differential equation based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the gyroscope angular velocity noise, the wheel speed noise, the wheel speed coefficient error, and the heading angle error.
[0009] In one embodiment of this disclosure, determining the lateral error covariance based on the attitude information of the moving target at the target time determined by the inertial measurement unit, using the error covariance, includes: performing singular value decomposition on the position error covariance matrix in the error covariance to obtain two eigenvalues and corresponding eigenvectors; determining the heading vector based on the heading angle of the moving target at the target time determined by the inertial measurement unit; and determining the lateral error covariance based on the two eigenvalues, the corresponding eigenvectors, and the heading vector.
[0010] In one embodiment of this disclosure, determining the lateral error covariance based on the two eigenvalues and the corresponding eigenvectors and the heading vector includes: constructing an ellipse equation based on the two eigenvalues and the corresponding eigenvectors; determining the equation of a straight line for a lateral vector perpendicular to the heading vector based on the heading vector and the eigenvectors; and determining the lateral error covariance based on the ellipse equation and the straight line equation.
[0011] In one embodiment of this disclosure, determining the lateral error between the pose information of the moving target based on the first pose information and the second pose information of the moving target at the target time includes: acquiring the first pose information of the moving target at the target time determined by the positioning sensor; determining the second pose information of the moving target at the target time based on the information measured by the inertial measurement unit and the wheel speed sensor; and determining the lateral error based on the first pose information and the second pose information.
[0012] In one embodiment of this disclosure, determining the lateral error based on the first pose information and the second pose information includes: determining the error between the first pose information and the second pose information; and determining the lateral error based on the error and the pose information of the moving target at the target time determined by the inertial measurement unit.
[0013] In one embodiment of this disclosure, the positioning fault detection method further includes: acquiring the initial pose information of the moving target at the start time of the preset time period determined by the positioning sensor; determining attitude compensation information based on the pose information of the moving target at a first moment in the preset time period determined by the positioning sensor, and the pose information of the moving target at the first moment determined by the inertial measurement unit and the wheel speed sensor; and compensating the attitude information in the initial pose information based on the attitude compensation information to obtain the initial pose information of the inertial measurement unit and the wheel speed sensor.
[0014] In one embodiment of this disclosure, determining attitude compensation information based on the pose information of the moving target at a first moment within a preset time period determined by the positioning sensor, and the pose information of the moving target at the first moment determined by the inertial measurement unit and the wheel speed sensor, includes: determining a first vector based on the pose information of the moving target at the first moment within the preset time period determined by the positioning sensor and the initial pose information; determining a second vector based on the pose information of the moving target at the first moment determined by the inertial measurement unit and the wheel speed sensor and the initial pose information; and determining the angle between the first vector and the second vector; and compensating the attitude information in the initial pose information based on the attitude compensation information to obtain the initial pose information of the inertial measurement unit and the wheel speed sensor includes: compensating the heading angle in the initial pose information through the angle to obtain the initial pose information of the inertial measurement unit and the wheel speed sensor.
[0015] In one embodiment of this disclosure, the fault detection of the positioning sensor based on the ratio of the lateral error quantity to the lateral error covariance includes: determining whether the ratio of the lateral error quantity to the lateral error covariance is greater than a preset threshold; and determining that the positioning sensor has experienced a lateral jump fault in response to the ratio of the lateral error quantity to the lateral error covariance being greater than the preset threshold.
[0016] A positioning fault detection device according to a second aspect of this disclosure includes: a lateral error covariance determination module configured to determine the lateral error covariance of the pose information of the moving target over a preset time period based on information measured by an inertial measurement unit and a wheel speed sensor disposed on the moving target; a lateral error quantity determination module configured to determine the lateral error quantity between the pose information of the moving target based on first pose information and second pose information of the moving target at a target time, wherein the target time is the end time of the preset time period, the first pose information is measured by a positioning sensor disposed on the moving target, and the second pose information is determined by the inertial measurement unit and the wheel speed sensor; and a fault detection module configured to perform fault detection on the positioning sensor based on the ratio of the lateral error quantity to the lateral error covariance.
[0017] An electronic device according to a third aspect of this disclosure may include: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to enable the at least one processor to perform the fault location detection method according to the first aspect of this disclosure.
[0018] The computer-readable storage medium provided according to the fourth aspect of this disclosure includes a computer program that, when executed by a processor, implements the fault location detection method described in the first aspect of this disclosure.
[0019] The computer program product provided according to the fifth aspect of this disclosure stores a computer program, which, when executed by a processor, implements the fault location detection method described in the first aspect of this disclosure.
[0020] According to the embodiments of this disclosure, the positioning fault detection method, positioning fault detection device, electronic device, computer-readable storage medium, and computer program product use two commonly used and highly stable sensors, an inertial measurement unit and a wheel speed sensor, to detect faults in the positioning sensor. This allows for accurate detection of positioning sensor faults, ensuring that fault detection is unaffected by the turning of the moving target.
[0021] It should be understood that the description in this section is not intended to identify key or important features of the embodiments of this disclosure, nor is it intended to limit the scope of this disclosure. Other features of this disclosure will become readily apparent from the following description. Attached Figure Description
[0022] Other features, objects, and advantages of this disclosure will become more apparent from the following detailed description of non-limiting embodiments with reference to the accompanying drawings. The drawings are provided for a better understanding of the invention and are not intended to limit the scope of this disclosure. Wherein: Figure 1 This is a flowchart of a fault location detection method according to an embodiment of the present disclosure; Figure 2 This is a flowchart for determining the lateral error covariance according to some embodiments of this disclosure; Figure 3 This is a flowchart for determining the error covariance according to some embodiments of this disclosure; Figure 4 This is a flowchart illustrating the determination of the transverse error covariance based on some embodiments of this disclosure; Figure 5 This is a schematic diagram of the trajectory of a vehicle in a global coordinate system according to an embodiment of the present disclosure; Figure 6 This is a schematic diagram illustrating the determination of lateral error covariance according to an embodiment of the present disclosure; Figure 7 This is a flowchart for determining the lateral error amount according to some embodiments of this disclosure; Figure 8 This is a flowchart of initial pose alignment according to some embodiments of this disclosure; Figure 9This is a flowchart of a location fault detection method according to an embodiment of the present disclosure; Figures 10 to 13 This is a schematic diagram showing the experimental comparison results of a fault location detection method according to an embodiment of the present disclosure; Figure 14 This is a block diagram of a location fault detection device according to an embodiment of the present disclosure; Figure 15 This is a block diagram of an electronic device that can be used to implement embodiments of the present disclosure. Detailed Implementation
[0023] The exemplary embodiments of this disclosure are described below with reference to the accompanying drawings, including various details of these embodiments to aid understanding; however, these should be considered merely exemplary. Therefore, those skilled in the art will recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope and spirit of this disclosure. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.
[0024] It should be noted that, where there is no conflict, the embodiments and features described in this disclosure can be combined with each other. This disclosure will now be described in detail with reference to the accompanying drawings and embodiments.
[0025] The embodiments of this disclosure provide a fault location detection method 1000.
[0026] Figure 1 A flowchart of a fault location detection method 1000 according to an embodiment of the present disclosure is shown. Figure 1 As shown, the fault location detection method 1000 may include the following steps: S100: Based on the information measured by the inertial measurement unit and wheel speed sensor set on the moving target, determine the lateral error covariance of the pose information of the moving target within a preset time period.
[0027] S200. Based on the first pose information and the second pose information of the moving target at the target time, determine the lateral error between the pose information of the moving target. The target time is the end time of a preset time period. The first pose information is determined by the positioning sensor set on the moving target, and the second pose information is determined by the inertial measurement unit and the wheel speed sensor.
[0028] S300: Fault detection of positioning sensors is performed based on the ratio of lateral error to lateral error covariance.
[0029] The positioning fault detection method 1000 of this disclosure can be executed by a positioning fault detection device. The positioning fault detection device can be hardware, software, or both; this disclosure does not limit this. The positioning fault detection device is mounted on a moving target; this disclosure does not limit the type of moving target, for example, the moving target can be an autonomous vehicle or a mobile robot. The inertial measurement unit (IMU), wheel speed sensor, and positioning sensor of this disclosure are mounted on the moving target and can be existing sensors on the moving target. The IMU measures the acceleration and angular velocity of the moving target, and the wheel speed sensor measures the velocity of the moving target. The pose information of the moving target can be determined based on the acceleration, angular velocity, and velocity. The positioning sensor can be, for example, a lidar and / or a camera; this disclosure does not limit this. The lidar is used to collect point cloud information of the environment surrounding the moving target; the pose information of the moving target can be determined based on the point cloud information of the environment surrounding the moving target. The camera is used to collect image information of the environment surrounding the moving target; the pose information of the moving target can be determined based on the image information of the environment surrounding the moving target.
[0030] In this disclosure, the direction of the moving target's movement is defined as the longitudinal direction, and the direction perpendicular to the moving target's movement in the plane is defined as the lateral direction. As the moving target moves, the error covariance of the pose information of the moving target determined by the positioning sensor increases laterally as the target moves, making the lateral error covariance greater than the longitudinal error covariance. For example, when the vehicle moves along the y-axis of the global coordinate system (where the y-axis is longitudinal and the x-axis is lateral), the error covariance of the vehicle's pose information determined by the positioning sensor increases along the x-axis of the global coordinate system as the vehicle moves. When the vehicle turns and moves along the x-axis of the global coordinate system (where the x-axis is longitudinal and the y-axis is lateral), the error covariance of the vehicle's pose determined by the positioning sensor increases along the y-axis of the global coordinate system as the vehicle moves.
[0031] Because a vehicle turns, its lateral and longitudinal directions change. As in the example above, the y-axis changes from longitudinal to lateral, and the x-axis changes from lateral to longitudinal. Consequently, the lateral and longitudinal error covariances also change. For instance, the x-axis error covariance changes from lateral to longitudinal, and the y-axis error covariance changes from longitudinal to lateral. In the example above, since the lateral error covariance in the x-axis direction is greater than the longitudinal error covariance in the y-axis direction before the turn, the longitudinal error covariance in the x-axis direction is greater than the lateral error covariance in the y-axis direction after the turn. In other words, before the turn, the lateral error covariance was greater than the longitudinal error covariance; after the turn, the longitudinal error covariance was greater than the lateral error covariance. The turn caused the lateral and longitudinal error covariances to interchange, changing the longitudinal error covariance from being less than the lateral error covariance to being greater than the lateral error covariance. This jump in the longitudinal error covariance is due to the change in vehicle attitude, not a malfunction of the positioning sensor. When fault detection of the positioning sensor, this jump in the error covariance caused by the change in vehicle attitude must be excluded from the scope of positioning sensor failure.
[0032] The positioning fault detection method 1000 of this disclosure determines the lateral error covariance of the pose information of a moving target over a preset time period based on information measured by an IMU and wheel speed sensors. It then determines the lateral error between the pose information of the moving target based on the first pose information of the moving target at a target time measured by the positioning sensor and the second pose information of the moving target at the target time determined by the IMU and wheel speed sensors. Finally, it detects a fault in the positioning sensor based on the ratio of the lateral error to the lateral error covariance. By utilizing the fact that the ratio of the lateral error between the pose information of the moving target measured by the positioning sensor and the pose information of the moving target determined by the IMU and wheel speed sensors to the lateral error covariance of the pose information of the moving target determined by the IMU and wheel speed sensors remains within a certain range as the vehicle's attitude changes, the method can detect faults in the positioning sensor. This can exclude jumps in the error covariance caused by changes in vehicle attitude from positioning sensor faults and can detect jumps in the error covariance caused by positioning sensor faults, such as lateral jump faults.
[0033] As can be seen from the above, the embodiments of this disclosure use two commonly used and highly stable sensors, namely IMU and wheel speed sensor, to detect faults in the positioning sensor. This can correctly detect faults in the positioning sensor, making fault detection unaffected by the turning of the moving target. Furthermore, the calculation is simple, the cost is low, and it is not easily affected by factors such as weather and environment.
[0034] It should be understood that the steps shown in method 1000 are not exclusive, and other steps may be performed before, after, or between any of the steps shown. Furthermore, some of the steps shown may be performed simultaneously, or may be performed in a manner different from [the steps shown in the method described ... Figure 1 The execution is performed in the order shown.
[0035] The following is a detailed description of each step in the fault location detection method 1000 according to the embodiments of this disclosure.
[0036] Figure 2 A flowchart illustrating the determination of lateral error covariance according to some embodiments of this disclosure is shown. For example... Figure 2 As shown, the lateral error covariance of the pose information of the moving target determined in step S100 over a preset time period may include the following steps: S110. Based on the information measured by the inertial measurement unit and wheel speed sensor, the error covariance accumulated over a preset time period is determined by Kalman filtering of the pose information of the moving target.
[0037] S120. Based on the error covariance, the lateral error covariance is determined by using the attitude information of the moving target at the target time determined by the inertial measurement unit.
[0038] In some optional embodiments of this disclosure, the executing entity can first determine the accumulated error covariance of the pose information of the moving target determined by the inertial measurement unit and wheel speed sensor over a preset time period using Kalman filtering, based on the information measured by the inertial measurement unit and wheel speed sensor. Then, based on the error covariance determined by Kalman filtering, and using the pose information of the moving target at the target time determined by the inertial measurement unit, the lateral error covariance is obtained. Kalman filtering performs state estimation by fusing the dynamic model of the system and measurement data. It adopts a recursive approach, continuously updating the estimated value while considering past estimates and new measurements. Optionally, position error, heading angle error, wheel speed coefficient error, and gyroscope bias error can be used as state variables. A state differential equation is determined based on the noise and measurement information from the IMU and wheel speed sensor. By discretizing the state differential equation and calculating the discretized state differential equation using Kalman filtering, the accumulated error covariance of the pose information of the moving target determined by the IMU and wheel speed sensor over a preset time period is obtained.
[0039] Therefore, as Figure 3As shown, step S110, based on the information measured by the inertial measurement unit and wheel speed sensor, determines the cumulative error covariance of the pose information of the moving target over a preset time period through Kalman filtering. It may also include: S111, determining the error differential equation based on the noise and measured information of the inertial measurement unit and wheel speed sensor, wherein the error includes position error, heading angle error, wheel speed coefficient error and gyroscope bias error; S112, discretizing the error differential equation and performing Kalman filtering on the discretized error differential equation to obtain the cumulative error covariance of the pose information of the moving target over a preset time period.
[0040] In some optional examples, the noise of the IMU and wheel speed sensor may include: gyroscope angular velocity noise, gyroscope angular velocity bias noise, wheel speed noise, and wheel speed coefficient noise. These inherent noises of the IMU and wheel speed sensor can be determined based on the parameters of the IMU and wheel speed sensor, for example, by consulting the noise parameters in the IMU and wheel speed sensor datasheets; this disclosure does not limit this. Therefore, S111, determining the error differential equation based on the noise and measured information of the inertial measurement unit and wheel speed sensor, may include: determining the gyroscope angular velocity noise and gyroscope angular velocity bias noise based on the parameters of the inertial measurement unit; determining the wheel speed noise and wheel speed coefficient noise based on the parameters of the wheel speed sensor; and determining the error differential equation based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the gyroscope angular velocity noise, the gyroscope angular velocity bias noise, the wheel speed noise, and the wheel speed coefficient noise.
[0041] Specifically, determining the error differential equation based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the gyroscope angular velocity noise, the gyroscope angular velocity bias noise, the wheel speed noise, and the wheel speed coefficient noise can include: determining the wheel speed coefficient error differential equation based on the wheel speed coefficient noise; determining the gyroscope bias error differential equation based on the gyroscope angular velocity bias noise; determining the heading angle error differential equation based on the gyroscope angular velocity noise and the gyroscope bias error; and determining the position error differential equation based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the gyroscope angular velocity noise, the wheel speed noise, the wheel speed coefficient error, and the heading angle error.
[0042] For example, the vehicle's pose information determined by the IMU and wheel speed sensors is... The error covariance at time is ,exist arrive The cumulative error covariance over the time period is , The calculation process is as follows: State quantity: Position error Heading angle error Wheel speed coefficient error Gyroscope bias error .
[0043] Noise: Wheel speed noise Wheel speed coefficient noise gyroscope angular velocity noise Gyroscope angular velocity bias noise .
[0044] The speed measured by the rear axle center wheel speed sensor of the vehicle is The wheel speed proportionality coefficient is k If the angular velocity measured by the IMU is w, then the velocity in the IMU coordinate system is... .in, The wheel speed sensor is the external parameter of the horizontal lever arm of the IMU. The wheel speed sensor is set at the center of the rear axle of the vehicle and outputs the longitudinal speed of the rear axle center of the vehicle.
[0045] The differential equation is as follows: (Formula 1) (Formula 2) (Formula 3) (Formula 4) Where b is the IMU's coordinate system, and n is the global coordinate system. For heading angle, The rotation matrix is used to convert the IMU's coordinate system to the global coordinate system. , x, y, and z represent the horizontal rightward, horizontal forward, and vertical upward directions, respectively. In this disclosure, the coordinate system is represented by superscript.
[0046] By discretizing the above differential equation and applying a Kalman filter, we can calculate the result from... arrive The cumulative error covariance over a period of time .
[0047] Figure 4 A flowchart illustrating the determination of transverse error covariance using error covariance according to some embodiments of this disclosure is shown. For example... Figure 4 As shown, step S120, based on the error covariance and using the attitude information of the moving target at the target time determined by the inertial measurement unit, determines the lateral error covariance, and may include the following steps: S121. Perform singular value decomposition on the position error covariance matrix in the error covariance to obtain two eigenvalues and the corresponding eigenvectors.
[0048] S122. Determine the heading vector based on the heading angle of the moving target at the target time determined by the inertial measurement unit.
[0049] S123. Determine the lateral error covariance based on two eigenvalues and their corresponding eigenvectors and heading vectors.
[0050] Figure 5 A schematic diagram of the trajectory of a vehicle in a global coordinate system according to an embodiment of the present disclosure is shown, as follows: Figure 5 As shown, the orange solid line 501 represents the vehicle location determined by the LiDAR (Light Detection and Ranging) from... Time's up The real-time global trajectory, with the blue solid line 502 representing the vehicle's movement as determined by the IMU and wheel speed sensors. Time's up The global trajectory at all times, with the blue dashed line 503 representing the position. The direction of travel of the vehicle at any given time; the purple dashed line 504 indicates the direction of travel. The lateral direction of the vehicle's travel direction at time tk is perpendicular to the vehicle's travel direction. The yellow solid line 505 represents the lateral error between the global pose of the vehicle determined by the LiDAR and the global pose determined by the IMU and wheel speed sensors. The green ellipse 506 represents the error covariance of the vehicle's position determined by the IMU and wheel speed sensors at time tk in the global coordinate system. Figure 5 It can be seen that the lateral error covariance that needs to be determined in step S120 is: Figure 5 The length of the line segment between the intersection of the purple dashed line 504 and the green ellipse 506 and the center of the green ellipse 504.
[0051] Figure 6 A schematic diagram illustrating the determination of the lateral error covariance according to an embodiment of the present disclosure is shown, as follows: Figure 6 As shown, the green ellipse 601 corresponds to Figure 5 The green ellipse 506 represents the positional error covariance in the global coordinate system, and the blue dashed line 602 corresponds to... Figure 5 The blue dashed line 503 indicates the vehicle's direction of travel, and the purple dashed line 603 corresponds to... Figure 5 The purple dashed line 504 in the diagram represents the lateral direction of the vehicle. The length of the line segment between the intersection point A of the purple dashed line 603 and the green ellipse 601 and the center O of the green ellipse 601 is the lateral error covariance that needs to be determined in step S120. This can be determined first based on the vehicle's pose determined by the IMU and wheel speed sensors. The error covariance at each moment is used to obtain the equation corresponding to the green ellipse 601. Then, based on the vehicle's position determined by the inertial measurement unit... Heading angle at any moment We obtain the angle between the x-axis of the blue dashed line 602 and the green ellipse 601. Then, based on the perpendicular relationship between the purple dashed line 603 and the blue dashed line 602, we obtain the equation corresponding to the purple dashed line 603. We can obtain the lateral error covariance by solving the equations corresponding to the green ellipse 601 and the purple dashed line 603 simultaneously.
[0052] Therefore, the position error covariance matrix in the error covariance can be decomposed using singular value decomposition to obtain two eigenvalues and their corresponding eigenvectors. Then, based on the heading angle of the moving target at the target time determined by the IMU, the heading vector is determined. Finally, the lateral error covariance is determined based on the two eigenvalues, their corresponding eigenvectors, and the heading vector. Specifically, determining the lateral error covariance based on the two eigenvalues, their corresponding eigenvectors, and the heading vector can include: constructing an ellipse equation based on the two eigenvalues and their corresponding eigenvectors; determining the equation of a straight line perpendicular to the heading vector based on the heading vector and the eigenvectors; and determining the lateral error covariance based on the ellipse equation and the straight line equation.
[0053] For example, such as Figure 6 As shown, the lateral error covariance The calculation process is as follows: The vehicle's pose information determined by the IMU and wheel speed sensors is in The cumulative error covariance at time step is Given a 5x5 matrix, we can take the error covariance representing the position (x, y). , Given a 2x2 matrix, we can perform Singular Value Decomposition (SVD) on this matrix to obtain two eigenvalues and their corresponding eigenvectors, where the larger eigenvalue is a and the smaller eigenvalue is b. It can be represented as an ellipse with a as the major axis and b as the minor axis. The equation of the ellipse can be: (Formula 5) The heading angle of the vehicle at that time, determined by the IMU. According to the heading angle The corresponding heading vector v1(sin() can be obtained. ),cos( The eigenvector corresponding to the eigenvalue 'a' is v2. Based on v1 and v2, the angle between v1 and v2 can be calculated as α. Therefore, the slope of the heading vector v1 in the coordinate system corresponding to the ellipse equation is... Then the slope of the lateral vector perpendicular to the heading vector in the coordinate system corresponding to the ellipse equation is... Then, by solving the following two equations, we can obtain... :
[0054] Solving the above equations simultaneously yields the coordinates of point A in the coordinate system corresponding to the ellipse equation. Set the coordinates of point A Substituting into the following formula, we can obtain the transverse error covariance. .
[0055] (Formula 8)
[0056] The embodiments of this disclosure obtain the error covariance by establishing a Kalman filter equation fused from the IMU and wheel speed sensor, determine the lateral error covariance using the error covariance updated by the measurement-free system, and determine whether the positioning sensor has malfunctioned by the ratio between the lateral error covariance and the lateral error quantity. It takes into account that the magnitude of the lateral error covariance changes with the x and y directions in the global coordinate system when the moving target turns, so that the fault detection of the positioning sensor can be unaffected by the turning of the moving target.
[0057] Figure 7 A flowchart illustrating the determination of the lateral error amount according to some embodiments of this disclosure is shown. For example... Figure 7 As shown, step S200, based on the first pose information and the second pose information of the moving target at the target time, determines the lateral error between the pose information of the moving target, which may include the following steps: S210. Obtain the first pose information of the moving target at the target time as determined by the positioning sensor.
[0058] S220: Based on the information measured by the inertial measurement unit and wheel speed sensor, determine the second pose information of the moving target at the target time.
[0059] S230. Determine the lateral error based on the first pose information and the second pose information.
[0060] In some optional embodiments of this disclosure, the executing entity can first acquire the first pose information of the moving target at the target time as determined by the positioning sensor, and then determine the second pose information of the moving target at the target time based on the information measured by the inertial measurement unit and the wheel speed sensor. Afterwards, the lateral error is determined based on the first pose information and the second pose information. For example, Figure 5 As shown, the solid green line 507 represents the vehicle location determined by the LiDAR (Light Detection and Ranging) in [location missing]. Global pose at time step The vehicle determined by the IMU and wheel speed sensors Global pose at time step The amount of error between The error in the vehicle's global pose can be determined based on the LiDAR, IMU, and wheel speed sensors. The vehicle identified by the IMU The attitude information at any given moment is used to obtain the vehicle's position. Lateral error at time ,Right now Figure 5 The solid yellow line in the middle is 505.
[0061] Therefore, step S230, which determines the lateral error based on the first pose information and the second pose information, may include: determining the error between the first pose information and the second pose information; and determining the lateral error based on the error and the pose information of the moving target at the target time determined by the inertial measurement unit. Figure 5 As shown, the direction of the solid yellow line 505 is consistent with the direction of the dashed purple line 504, and represents the error of the solid green line 507. The projection of the error between the two along the purple dashed line 504 is the lateral component of the error in the IMU coordinate system, which can be calculated using the following formula: (Formula 9) in, To retrieve here The x-value of the inner vector.
[0062] In some optional embodiments of this disclosure, determining the pose information of the moving target at the target time based on the IMU and wheel speed sensor requires determining the pose information of the moving target at the beginning of a preset time period. This can be achieved by acquiring the initial pose information of the moving target at the beginning of the preset time period determined by the positioning sensor, which can be used as the pose information of the IMU and wheel speed sensor at the beginning of the preset time period. To ensure the accuracy of the pose information of the moving target at the target time determined by the IMU and wheel speed sensor, attitude compensation needs to be performed on the acquired initial pose information of the moving target at the beginning of the preset time period determined by the positioning sensor. Therefore, as... Figure 8 As shown, the fault location detection method 1000 may further include the following steps: S410: Obtain the initial pose information of the moving target determined by the positioning sensor at the beginning of a preset time period.
[0063] S420. Based on the pose information of the moving target at the first moment in a preset time period determined by the positioning sensor, and the pose information of the moving target at the first moment determined by the inertial measurement unit and the wheel speed sensor, determine the attitude compensation information.
[0064] S420. Based on the attitude compensation information, the attitude information in the initial pose information is compensated to obtain the initial pose information of the inertial measurement unit and the wheel speed sensor.
[0065] The first moment can be a moment close to the start time within a preset time period. The distance between the start time and the first moment must be a straight line; for example, the distance between the start time and the first moment can be a distance of 3-5 meters in length. For example, at the start time of the preset time period... It can obtain the vehicle's horizontal position in the global coordinate system determined by the LiDAR. and heading angle The horizontal position and heading angle are then compared with the extrinsic parameters of the lidar via the IMU. Switch to the IMU's coordinate system, as the IMU and wheel speed sensor at the beginning moment. Starting pose .in It can include rotation and translation matrices.
[0066] To obtain the IMU and wheel speed sensor at the initial moment The actual starting pose within the preset time period The global pose of the LiDAR can be obtained at any time. The vehicle speed is calculated using IMU and wheel speed sensors. The global pose at time t is Based on the acquired LiDAR data at the start time Determine the initial pose and the lidar in The global pose determined at time t and the values calculated by the IMU and wheel speed sensors at time t are given. Global pose can be obtained as a vector representing the global position transformation of the LiDAR over a given time interval. , and the vector of global position transformation of IMU and wheel speed sensor According to vector and The angle between two vectors can be obtained. It can be used To compensate for the acquired LiDAR data at the initial moment The heading angle in the initial pose The IMU and wheel speed sensor data were obtained at the beginning. True starting posture This allows us to obtain the true initial pose of the IMU and wheel speed sensor at the beginning. Then, the pose information of the IMU and wheel speed sensors is calculated starting from their actual initial poses.
[0067] Therefore, step S420, based on the pose information of the moving target at the first moment within a preset time period determined by the positioning sensor, and the pose information of the moving target at the first moment determined by the inertial measurement unit and wheel speed sensor, determines attitude compensation information. This may include: determining a first vector based on the pose information of the moving target at the first moment within the preset time period determined by the positioning sensor and the initial pose information; determining a second vector based on the pose information of the moving target at the first moment and the initial pose information determined by the inertial measurement unit and wheel speed sensor; and determining the angle between the first vector and the second vector. Step S430, based on the attitude compensation information, compensates for the attitude information in the initial pose information to obtain the initial pose information of the inertial measurement unit and wheel speed sensor. This may include: compensating for the heading angle in the initial pose information using the determined angle to obtain the initial pose information of the inertial measurement unit and wheel speed sensor.
[0068] In some optional embodiments of this disclosure, step S300, which detects a fault in the positioning sensor based on the ratio of the lateral error to the lateral error covariance, may include: determining whether the ratio of the lateral error to the lateral error covariance is greater than a preset threshold; and, in response to the ratio of the lateral error to the lateral error covariance being greater than the preset threshold, determining that the positioning sensor has experienced a lateral jump fault. For example, This is the lateral error. lateral error Covariance, the ratio of the lateral error to the lateral error covariance. .
[0069] Figure 9 A flowchart illustrating a location fault detection method according to an embodiment of this disclosure is shown. Figure 9 As shown, a LiDAR (LiDAR system) is used as a positioning sensor on the vehicle to provide global position. For example, the LiDAR could be a 32-line 3D mechanical LiDAR mounted on the roof, with a pose information update frequency of 10 Hz. An IMU (Insulated Unit) and wheel speed sensors are used for fault detection of the LiDAR; for example, the IMU and wheel speed sensors could update pose information at a frequency of 100 Hz. The specific process is as follows: Step 701, Determine the initial pose: Obtain the initial pose of the vehicle at time t0 as determined by the LiDAR.
[0070] Step 702, align the position vectors to obtain the true initial pose: based on The initial pose of the vehicle at the moment determined by the LiDAR and the LiDAR. The pose at a given moment is used to determine the position of the LiDAR. Time's up The position transformation vector at any given time. The initial pose of the vehicle at a given moment, determined by the IMU and wheel speed sensors. The pose at a given moment is used to determine the position of the IMU and wheel speed sensor. Time's up The position transformation vector at time t. The compensation angle is obtained by determining the angle between the two position transformation vectors, and the heading angle in the initial pose is compensated to obtain the true initial pose of the IMU and wheel speed sensor.
[0071] Step 703, Fault Detection and Prediction: Based on the noise and measurement information from the IMU and wheel speed sensors, the error differential equation is determined, discretized, and then Kalman filtered to obtain the vehicle's pose. arrive The cumulative error covariance over a time period. This error includes position error, heading angle error, wheel speed coefficient error, and gyroscope bias error. Noise includes gyroscope angular velocity noise, gyroscope angular velocity bias noise, wheel speed noise, and wheel speed coefficient noise.
[0072] Step 704, calculate the lateral error: based on the vehicle's position determined by the LiDAR, ... The vehicle's pose at any given moment, determined by the IMU and wheel speed sensors. The pose at each moment is used to determine the error between the two. Based on the error, the vehicle's position is determined by the IMU. The lateral error is obtained by determining the attitude at any given time.
[0073] Step 705, Calculate the lateral error covariance: Based on the error covariance, the vehicle's position determined by the IMU is... The attitude at any given time determines the lateral error covariance.
[0074] Step 706, Fault Detection and Judgment: Based on the ratio of lateral error quantity to lateral error covariance. Perform fault detection on the LiDAR. If... If the threshold is exceeded, it is determined that the LiDAR has malfunctioned, and the fault can be troubleshooted through step 707.
[0075] like If the preset threshold is not exceeded, step 708 can be used to determine whether the preset mileage threshold has been exceeded. If the preset mileage threshold is not exceeded, the process returns to step 703 to begin fault detection and prediction. If the preset mileage threshold is exceeded, the process returns to step 701 to determine the starting pose and begin execution.
[0076] To avoid affecting the accuracy of LiDAR fault detection as IMU errors accumulate, the method disclosed herein typically requires resetting the entire detection process after a certain period or distance, for example, by setting a preset mileage threshold of 100m. The method disclosed herein can accurately detect whether the LiDAR has experienced a lateral jump fault. If a lateral jump fault occurs, If the threshold is exceeded, the LiDAR is considered to have malfunctioned.
[0077] Figures 10 to 13 A schematic diagram illustrating experimental comparison results of a fault location detection method according to an embodiment of this disclosure is shown. For example... Figure 10 As shown, when the LiDAR positioning trajectory is normal throughout and without any jumps, as... Figure 11 As shown, The data variation is within a range of less than 6. For example... Figure 12 As shown, when with Figure 10 When the same trajectory shows a 0.5m jump, such as Figure 13 As shown, for the transition part of LiDAR positioning, The data will surge, reaching over 20.
[0078] Embodiments of this disclosure also provide a fault location detection device 2000. Figure 14 A block diagram of a positioning fault detection apparatus 2000 according to an embodiment of the present disclosure is shown. The positioning fault detection apparatus 2000 according to the embodiment of the present disclosure can perform the positioning fault detection method 1000 described above. Figure 14 As shown, the fault location detection device 2000 may include: The lateral error covariance determination module 810 is configured to determine the lateral error covariance of the pose information of the moving target determined by the inertial measurement unit and the wheel speed sensor over a preset time period based on the information measured by the inertial measurement unit and the wheel speed sensor.
[0079] The lateral error determination module 820 is configured to determine the lateral error between the pose information of the moving target based on the first pose information and the second pose information of the moving target at the target time, wherein the target time is the end time of the preset time period, the first pose information is determined by the positioning sensor installed on the moving target, and the second pose information is determined by the inertial measurement unit and the wheel speed sensor.
[0080] The fault detection module 830 is configured to detect faults in the positioning sensor based on the ratio of the lateral error to the lateral error covariance.
[0081] In some alternative implementations, the lateral error covariance determination module 810 includes: The error covariance determination unit is configured to determine the accumulated error covariance of the pose information of the moving target during the preset time period based on the information measured by the inertial measurement unit and the wheel speed sensor through Kalman filtering.
[0082] The lateral error covariance determination unit is configured to determine the lateral error covariance based on the error covariance and the attitude information of the moving target at the target time determined by the inertial measurement unit.
[0083] In some alternative implementations, the error covariance determination unit is further configured to: The error differential equation is determined based on the noise and measurement information from the inertial measurement unit and the wheel speed sensor, wherein the errors include position error, heading angle error, wheel speed coefficient error, and gyroscope bias error; and The error differential equation is discretized, and Kalman filtering is applied to the discretized error differential equation to obtain the cumulative error covariance of the pose information of the moving target during the preset time period.
[0084] In some alternative implementations, the error covariance determination unit is further configured to: Based on the parameters of the inertial measurement unit, the gyroscope angular velocity noise and gyroscope angular velocity bias noise are determined; Based on the parameters of the wheel speed sensor, wheel speed noise and wheel speed coefficient noise are determined; and The error differential equation is determined based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the gyroscope angular velocity noise, the gyroscope angular velocity bias noise, the wheel speed noise, and the wheel speed coefficient noise.
[0085] In some alternative implementations, the error covariance determination unit is further configured to: The differential equation for wheel speed coefficient error is determined based on the wheel speed coefficient noise. The differential equation for the gyroscope bias error is determined based on the gyroscope angular velocity bias noise. The differential equation for the heading angle error is determined based on the gyroscope angular velocity noise and the gyroscope bias error; and Based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the angular velocity noise of the gyroscope, the wheel speed noise, the wheel speed coefficient error, and the heading angle error, the differential equation of position error is determined.
[0086] In some alternative implementations, the lateral error covariance determination unit is further configured to: Singular value decomposition is performed on the position error covariance matrix in the error covariance to obtain two eigenvalues and the corresponding eigenvectors. Based on the heading angle of the moving target at the target time determined by the inertial measurement unit, the heading vector is determined; and The lateral error covariance is determined based on the two eigenvalues and their corresponding eigenvectors and the heading vector.
[0087] In some alternative implementations, the lateral error covariance determination unit is further configured to: Construct an ellipse equation based on the two eigenvalues and their corresponding eigenvectors; Based on the heading vector and the feature vector, determine the equation of the straight line for the lateral vector perpendicular to the heading vector; and The lateral error covariance is determined based on the equations of the ellipse and the line.
[0088] In some alternative implementations, the lateral error determination module 820 is further configured to: The first pose information of the moving target at the target time is obtained as determined by the positioning sensor; Based on the information measured by the inertial measurement unit and the wheel speed sensor, the second pose information of the moving target at the target time is determined; and The lateral error is determined based on the first pose information and the second pose information.
[0089] In some alternative implementations, the lateral error determination module 820 is further configured to: Determine the error between the first pose information and the second pose information; and Based on the error amount, the lateral error amount is determined by the attitude information of the moving target at the target time determined by the inertial measurement unit.
[0090] In some optional embodiments, the positioning fault detection device 2000 further includes: a starting pose determination module, configured to: The starting pose information of the moving target at the beginning of the preset time period, as determined by the positioning sensor, is obtained. Based on the pose information of the moving target at the first moment in the preset time period determined by the positioning sensor, and the pose information of the moving target at the first moment determined by the inertial measurement unit and the wheel speed sensor, attitude compensation information is determined. The attitude information in the initial pose information is compensated based on the attitude compensation information to obtain the initial pose information of the inertial measurement unit and the wheel speed sensor.
[0091] In some alternative implementations, the initial pose determination module is further configured to: Based on the pose information of the moving target at the first moment in the preset time period determined by the positioning sensor and the initial pose information, a first vector is determined; Based on the pose information of the moving target at the first moment and the initial pose information determined by the inertial measurement unit and the wheel speed sensor, a second vector is determined; and Determine the angle between the first vector and the second vector; The heading angle in the initial pose information is compensated by the included angle to obtain the initial pose information of the inertial measurement unit and the wheel speed sensor.
[0092] In some alternative implementations, the fault detection module 830 is further configured to: Determine whether the ratio of the lateral error to the lateral error covariance is greater than a preset threshold; If the ratio of the lateral error to the lateral error covariance is greater than the preset threshold, it is determined that the positioning sensor has experienced a lateral jump fault.
[0093] In addition, this disclosure also provides an electronic device, which includes at least one processor and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor to enable the at least one processor to perform the above-described fault location detection method 1000.
[0094] This disclosure also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the above-described fault location detection method 1000.
[0095] This disclosure also provides a computer program product, including a computer program that, when executed by a processor, implements the above-described fault location detection method 1000.
[0096] Figure 15A block diagram of an electronic device 3000, illustrating examples of embodiments that can be used to implement the present disclosure, is shown. The electronic device 3000 is intended to represent various forms of digital computers. The electronic device 3000 may also represent various forms of mobile devices capable of running computing programs. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present disclosure described and / or claimed herein.
[0097] like Figure 15 As shown, the electronic device 3000 includes a computing unit 910, which can perform various appropriate actions and processes based on a computer program stored in a read-only memory (ROM) 920 or a computer program loaded from a storage unit 980 into a random access memory (RAM) 930. The RAM 930 may also store various programs and data required for the operation of the autonomous vehicle or mobile robot 3000. The computing unit 910, ROM 920, and RAM 930 are interconnected via a bus 940. An input / output (I / O) interface 950 is also connected to the bus 940.
[0098] Multiple components in electronic device 3000 are connected to I / O interface 950, including: input unit 960, such as a touch screen; output unit 970, such as various types of displays, speakers, etc.; storage unit 980, such as a hard disk; and communication unit 990, such as a network card, modem, wireless transceiver, etc. Communication unit 990 allows electronic device 3000 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.
[0099] The computing unit 910 can be a variety of general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of the computing unit 910 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. The computing unit 910 performs the various methods and processes described above, such as the location fault detection method 1000. For example, in some embodiments, the location fault detection method 1000 may be implemented as a computer software program tangibly contained in a machine-readable medium, such as storage unit 980. In some embodiments, part or all of the computer program may be loaded and / or installed on the electronic device 3000 via ROM 920 and / or communication unit 990. When the computer program is loaded into RAM 930 and executed by the computing unit 910, one or more steps of the location fault detection method 1000 described above may be performed. Alternatively, in other embodiments, the computing unit 910 may be configured to perform the location fault detection method 1000 by any other suitable means (e.g., by means of firmware).
[0100] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.
[0101] The program code used to implement the methods of this disclosure may be written in any combination of one or more programming languages. This program code may be provided to a processor or controller of a general-purpose computer, special-purpose computer, or other programmable data processing apparatus, such that when executed by the processor or controller, the program code causes the functions / operations specified in the flowcharts and / or block diagrams to be implemented. The program code may be executed entirely on a machine, partially on a machine, as a standalone software package partially on a machine and partially on a remote machine, or entirely on a remote machine or server.
[0102] In the context of this disclosure, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can be, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.
[0103] It should be understood that the various forms of processes shown above can be used to rearrange, add, or delete steps. For example, the steps described in this disclosure can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this disclosure can be achieved, and this is not limited herein.
[0104] The specific embodiments described above do not constitute a limitation on the scope of protection of this disclosure. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this disclosure should be included within the scope of protection of this disclosure.
Claims
1. A method for locating fault detection, characterized in that: include: Determine the lateral error covariance of the position information of the moving target in a preset time period based on information measured by an inertial measurement unit and a wheel speed sensor disposed on the moving target; Determine a lateral error amount between the posture information of the moving target based on first posture information and second posture information of the moving target at a target time, wherein the target time is an end time of the preset time period, the first posture information is determined by a positioning sensor disposed on the moving target, and the second posture information is determined by the inertial measurement unit and the wheel speed sensor; as well as Fault detection is performed on the positioning sensor based on a ratio of the lateral error amount to the lateral error covariance.
2. The method according to claim 1, characterized in that: Determining the lateral error covariance of the position and posture information of the moving target in a preset time period includes: Based on the information measured by the inertial measurement unit and the wheel speed sensor, determining the error covariance accumulated in the preset time period of the posture information of the moving target by Kalman filtering; and Based on the error covariance, the lateral error covariance is determined by the posture information of the moving target at the target time determined by the inertial measurement unit.
3. The method according to claim 2, characterized in that The method of determining the error covariance accumulated in the preset time period of the posture information of the moving target by Kalman filtering based on the information measured by the inertial measurement unit and the wheel speed sensor includes: determining an error differential equation based on noise and measured information of the inertial measurement unit and the wheel speed sensor, wherein the error includes position error, heading angle error, wheel speed coefficient error, and gyroscope bias error; and The error differential equation is discretized, and Kalman filtering is performed on the discretized error differential equation to obtain the error covariance accumulated in the preset time period of the position and posture information of the moving target.
4. The method according to claim 3, characterized in that The determining of the error differential equation based on the noise and measured information of the inertial measurement unit and the wheel speed sensor comprises: Determining gyroscope angular velocity noise and gyroscope angular velocity bias noise based on the parameters of the inertial measurement unit; determining wheel speed noise and wheel speed coefficient noise based on parameters of the wheel speed sensor; and The error differential equation is determined based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the gyroscope angular velocity noise, the gyroscope angular velocity bias noise, the wheel speed noise, and the wheel speed coefficient noise.
5. The method according to claim 4, characterized in that The determining the error differential equation based on the angular velocity measured by the inertial measurement unit, the speed measured by the wheel speed sensor, the gyroscope angular velocity noise, the gyroscope angular velocity bias noise, the wheel speed noise and the wheel speed coefficient noise comprises: determining a wheel speed coefficient error differential equation based on the wheel speed coefficient noise; determining a gyroscope bias error differential equation based on the gyroscope angular velocity bias noise; determining a heading angle error differential equation based on the gyroscope angular velocity noise and the gyroscope bias error; and A position error differential equation is determined based on the angular velocity measured by the inertial measurement unit, the velocity measured by the wheel speed sensor, the gyroscope angular velocity noise, the wheel speed noise, the wheel speed coefficient error, and the heading angle error.
6. The method according to claim 2, characterized in that The determining the lateral error covariance based on the error covariance and the posture information of the moving target at the target time determined by the inertial measurement unit includes: Performing singular value decomposition on the position error covariance matrix in the error covariance to obtain two eigenvalues and corresponding eigenvectors; Determining a heading vector based on the heading angle of the moving target at the target time determined by the inertial measurement unit; and The lateral error covariance is determined based on the two eigenvalues and the corresponding eigenvectors and the heading vector.
7. The method according to claim 6, characterized in that The determining the lateral error covariance based on the two eigenvalues and the corresponding eigenvectors and the heading vector comprises: constructing an elliptic equation based on the two eigenvalues and the corresponding eigenvectors; Determine a straight line equation of a lateral vector perpendicular to the heading vector based on the heading vector and the eigenvector; and The lateral error covariance is determined based on the ellipse equation and the line equation.
8. The method according to claim 1, characterized in that Determining a lateral error amount between the posture information of the moving target based on the first posture information and the second posture information of the moving target at the target time includes: Acquire the first position information of the moving target at the target time determined by the positioning sensor; Determining second position information of the moving target at the target time based on information measured by the inertial measurement unit and the wheel speed sensor; and The lateral error amount is determined based on the first posture information and the second posture information.
9. The method according to claim 8, characterized in that The determining the lateral error amount based on the first posture information and the second posture information includes: Determining an error amount between the first posture information and the second posture information; and Based on the error amount, the lateral error amount is determined by the posture information of the moving target at the target time determined by the inertial measurement unit.
10. The method according to claim 1, characterized in that Also includes: Acquire the starting position information of the moving target at the beginning of the preset time period determined by the positioning sensor; Determine attitude compensation information based on the attitude information of the moving target at a first moment in the preset time period determined by the positioning sensor and the attitude information of the moving target at the first moment determined by the inertial measurement unit and the wheel speed sensor; The posture information in the initial posture information is compensated based on the posture compensation information to obtain the initial posture information of the inertial measurement unit and the wheel speed sensor.
11. The method according to claim 10, characterized in that The determining of attitude compensation information based on the attitude information of the moving target at the first moment in the preset time period determined by the positioning sensor and the attitude information of the moving target at the first moment determined by the inertial measurement unit and the wheel speed sensor includes: Determine a first vector based on the position information of the moving target at a first moment in the preset time period determined by the positioning sensor and the starting position information; Determine a second vector based on the position information of the moving target at the first moment determined by the inertial measurement unit and the wheel speed sensor and the initial position information; and determining an angle between the first vector and the second vector; The step of compensating the posture information in the initial posture information based on the posture compensation information to obtain the initial posture information of the inertial measurement unit and the wheel speed sensor includes: The heading angle in the initial posture information is compensated by the included angle to obtain the initial posture information of the inertial measurement unit and the wheel speed sensor.
12. The method according to claim 1, characterized in that The performing fault detection on the positioning sensor based on the ratio of the lateral error amount to the lateral error covariance includes: Determining whether a ratio of the lateral error amount to the lateral error covariance is greater than a preset threshold; In response to a ratio of the lateral error amount to the lateral error covariance being greater than the preset threshold, it is determined that a lateral jump fault occurs in the positioning sensor.
13. A positioning fault detection device, characterized in that: include: A lateral error covariance determination module is configured to determine the lateral error covariance of the position and posture information of the moving target in a preset time period based on information measured by an inertial measurement unit and a wheel speed sensor provided on the moving target; a lateral error amount determination module, configured to determine a lateral error amount between the posture information of the moving target based on first posture information and second posture information of the moving target at a target time, wherein the target time is an end time of the preset time period, the first posture information is determined by a positioning sensor provided on the moving target, and the second posture information is determined by the inertial measurement unit and the wheel speed sensor; as well as The fault detection module is configured to perform fault detection on the positioning sensor based on a ratio of the lateral error amount to the lateral error covariance.
14. An electronic device, characterized in that: include: at least one processor; as well as, A memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, the instructions being executed by the at least one processor so that the at least one processor can execute the positioning fault detection method described in any one of claims 1 to 12.
15. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the positioning fault detection method as claimed in any one of claims 1 to 12 is implemented.
16. A computer program product comprising a computer program, characterized in that When the computer program is executed by a processor, the positioning fault detection method as claimed in any one of claims 1 to 12 is implemented.
Citation Information
Patent Citations
Positioning method, apparatus, robot, and computer readable storage medium
CN109959381A
Positioning method and device based on multi-source fusion positioning system
CN111578928A
Wheel speed sensor fault detection method
CN111965390A
Driving equipment positioning method and device, equipment and storage medium
CN112880674A
Positioning method and device and positioning system
CN113324541A
Cited By
Radar-aided inertial measurement unit anomaly detection method and application thereof
CN119245695A
