Detailed Description
Exemplary embodiments of the present disclosure are described below in conjunction with the accompanying drawings, which include various details of the embodiments of the present disclosure to facilitate understanding, and should be considered as merely exemplary. Accordingly, one of ordinary skill in the art will recognize that various changes and modifications of the embodiments described herein can be made without departing from the scope and spirit of the present disclosure. Also, descriptions of well-known functions and constructions are omitted in the following description for clarity and conciseness.
In addition, embodiments of the present disclosure and features of the embodiments may be combined with each other without conflict. The present disclosure will be described in detail below with reference to the accompanying drawings in conjunction with embodiments.
Embodiments of the present disclosure provide a localization fault detection method 1000.
Fig. 1 shows a flow chart of a method 1000 of locating fault detection according to an embodiment of the present disclosure. As shown in fig. 1, the positioning failure detection method 1000 may include the steps of:
s100, determining the transverse error covariance of pose information of the moving object in a preset time period based on information measured by an inertial measurement unit and a wheel speed sensor arranged on the moving object.
S200, determining the transverse error amount between the pose information of the moving object based on the first pose information and the second pose information of the moving object at the target moment, wherein the target moment is the ending moment of the preset time period, the first pose information is measured by a positioning sensor arranged on the moving object, and the second pose information is determined by an inertia measuring unit and a wheel speed sensor.
S300, fault detection is carried out on the positioning sensor based on the ratio of the transverse error amount to the transverse error covariance.
The positioning fault detection method 1000 of the embodiment of the present disclosure may be performed by a positioning fault detection device, and the positioning fault detection device may be hardware, software, or both hardware and software, which is not limited in this embodiment, and the positioning fault detection device is disposed on a moving object, which is not limited in type, and for example, the moving object may be an autonomous vehicle or a mobile robot. The inertial measurement unit (Inertial Measurement Unit, abbreviated IMU) of an embodiment of the present disclosure, the wheel speed sensor, and the positioning sensor are disposed on a moving object, which may be existing sensors on the moving object. The IMU is used for measuring the acceleration and the angular velocity of the moving object, the wheel speed sensor is used for measuring the velocity of the moving object, and the pose information of the moving object can be determined according to the acceleration, the angular velocity and the velocity of the moving object. The positioning sensor may be, for example, a lidar and/or a camera, etc., and embodiments of the present disclosure are not limited in this regard. The laser radar is used for collecting point cloud information of the surrounding environment of the moving object, pose information of the moving object can be measured according to the point cloud information of the surrounding environment of the moving object, the camera is used for collecting image information of the surrounding environment of the moving object, and the pose information of the moving object can be measured according to the image information of the surrounding environment of the moving object.
In the present disclosure, a direction in which the moving object advances is taken as a longitudinal direction, and a direction perpendicular to the direction in which the moving object advances in a plane is taken as a lateral direction. In the advancing process of the moving object, the error covariance of the pose information of the moving object, which is determined by the positioning sensor, can be expanded along the transverse direction along with the advancing of the moving object, so that the transverse error covariance is larger than the longitudinal error covariance. For example, when the vehicle advances along the y-axis direction of the global coordinate system, the y-axis direction is longitudinal, the x-axis direction is transverse, and the error covariance of the pose information of the vehicle determined by the positioning sensor expands along the x-axis direction of the global coordinate system as the vehicle advances. When the vehicle turns and advances along the x-axis direction of the global coordinate system, the x-axis direction is longitudinal, the y-axis direction is transverse, and the error covariance of the pose of the vehicle determined by the positioning sensor can be expanded along the y-axis direction of the global coordinate system along with the advance of the vehicle.
As the vehicle turns to change the lateral and longitudinal directions, as in the example above, the y-axis direction changes from longitudinal to lateral and the x-axis direction changes from lateral to longitudinal, as well as the lateral and longitudinal error covariances, as in the example above, the error covariances in the x-axis direction change from lateral to longitudinal and the error covariances in the y-axis direction change from longitudinal to lateral. In the above example, since the lateral error covariance in the x-axis direction before turning is larger than the longitudinal error covariance in the y-axis direction, the longitudinal error covariance in the x-axis direction after turning is larger than the lateral error covariance in the y-axis direction. That is, before turning, the lateral error covariance is larger than the longitudinal error covariance, after turning, the longitudinal error covariance is larger than the lateral error covariance, turning causes the lateral error covariance and the longitudinal error covariance to be interchanged, the longitudinal error covariance is changed from smaller than the lateral error covariance to larger than the lateral error covariance, the longitudinal error covariance jumps along with turning of the vehicle, and the jump of the error covariance is generated due to the change of the posture of the vehicle, but not due to the fault of the positioning sensor. In the case of fault detection of the positioning sensor, it is necessary to exclude such a jump in the error covariance due to the change in the vehicle posture from the fault of the positioning sensor.
According to the positioning fault detection method 1000 of the embodiment of the disclosure, the transverse error covariance of pose information of a moving object in a preset time period is determined according to information measured by an IMU and a wheel speed sensor, the transverse error amount between the pose information of the moving object is determined according to first pose information of the moving object at a target moment measured by the positioning sensor and second pose information of the moving object at the target moment measured by the IMU and the wheel speed sensor, and fault detection is performed on the positioning sensor according to the ratio of the transverse error amount to the transverse error covariance. The fault detection is carried out on the positioning sensor by utilizing the transverse error quantity between the pose information of the moving object measured by the positioning sensor and the pose information of the moving object determined by the IMU and the wheel speed sensor and the transverse error covariance of the pose information of the moving object determined by the IMU and the wheel speed sensor, wherein the ratio of the transverse error covariance to the transverse error covariance is kept within a range along with the change of the vehicle pose, the jump of the error covariance caused by the change of the vehicle pose can be excluded from the fault of the positioning sensor, and the jump of the error covariance caused by the fault of the positioning sensor, such as transverse jump fault, can be detected.
From the above, according to the embodiment of the disclosure, the fault detection is performed on the positioning sensor through the two relatively common sensors with higher stability, such as the IMU and the wheel speed sensor, so that the fault of the positioning sensor can be correctly detected, the fault detection is not affected by turning of the moving object, and the method is simple in calculation, low in cost and not easy to be interfered by factors such as weather and environment.
It should be understood that the steps shown in method 1000 are not exclusive and that other steps may be performed before, after, or between any of the steps shown. Further, some of the illustrated steps may be performed simultaneously, or may be performed in a different order than shown in fig. 1.
Each step in the positioning failure detection method 1000 of the embodiment of the present disclosure is specifically described below.
Fig. 2 illustrates a flow chart for determining lateral error covariance according to some embodiments of the present disclosure. As shown in fig. 2, the lateral error covariance of the pose information of the moving object determined in step S100 in the preset time period may include the following steps:
S110, determining the accumulated error covariance of the pose information of the moving target in a preset time period through Kalman filtering based on the information measured by the inertia measuring unit and the wheel speed sensor.
S120, based on the error covariance, determining the transverse error covariance through the attitude information of the moving target at the target moment, which is determined by the inertia measurement unit.
In some optional embodiments of the present disclosure, the executing body may determine, by using a kalman filter, an error covariance accumulated in a preset time period by pose information of the moving target determined by the inertia measurement unit and the wheel speed sensor according to information measured by the inertia measurement unit and the wheel speed sensor, and then obtain a lateral error covariance by using pose information of the moving target determined by the inertia measurement unit at a target time according to the error covariance determined by the kalman filter. The Kalman filtering is to perform state estimation by fusing dynamic model and measurement data of the system, and the state estimation method adopts a recursion mode to continuously update the estimation value, and considers the past estimation and new measurement. Alternatively, the position error, the course angle error, the wheel speed coefficient error and the gyroscope bias error can be used as state quantities, a state differential equation is determined according to the noise and the measured information of the IMU and the wheel speed sensor, the discretized state differential equation is discretized, and the calculated state differential equation is calculated through Kalman filtering, so that the accumulated error covariance of the pose information of the moving target determined by the IMU and the wheel speed sensor in a preset time period is obtained.
Therefore, as shown in FIG. 3, the step S110 of determining the error covariance accumulated by the pose information of the moving object in the preset time period through Kalman filtering based on the information measured by the inertia measuring unit and the wheel speed sensor may further include the steps of S111 of determining an error differential equation based on the noise of the inertia measuring unit and the wheel speed sensor and the measured information, wherein the error includes a position error, a course angle error, a wheel speed coefficient error and a gyroscope bias error, and S112 of discretizing the error differential equation and performing Kalman filtering on the discretized error differential equation to obtain the error covariance accumulated by the pose information of the moving object in the preset time period.
In some alternative 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. The intrinsic noise of these IMUs and wheel speed sensors may be determined from the parameters of the IMUs and wheel speed sensors, for example, by querying the noise parameters in the data manuals of the IMUs and wheel speed sensors, which embodiments of the present disclosure are not limited. Accordingly, S111 determining an error differential equation based on the noise of the inertial measurement unit and the wheel speed sensor and the measured information may include determining a gyroscope angular velocity noise and a gyroscope angular velocity bias noise based on the parameters of the inertial measurement unit, determining a wheel speed noise and a wheel speed coefficient noise based on the parameters of the wheel speed sensor, and determining an 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.
Wherein 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 may include 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 determining a 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 coefficient error, and the heading angle error.
For example, the pose information of the vehicle determined by the IMU and the wheel speed sensor is determined byThe error covariance of the time isIn the followingTo the point ofThe time period accumulated error covariance of (2) is,The calculation process of (2) is as follows:
state quantity position errorHeading angle errorWheel speed coefficient errorBias error of gyroscope。
Noise wheel speed noiseWheel speed coefficient noiseAngular velocity noise of gyroscopeAngular velocity bias noise of gyroscopes。
The speed measured by the central wheel speed sensor of the rear axle of the vehicle isThe ratio coefficient of the wheel speed is k, the angular velocity measured by the IMU is w, and the velocity under the IMU coordinate system is. Wherein,The wheel speed sensor is arranged in the center of a rear axle of the vehicle and outputs the longitudinal speed of the center of the rear axle of the vehicle.
The differential equation is as follows:
(equation 1)
(Equation 2)
(Equation 3)
(Equation 4)
Wherein b is the coordinate system of the IMU, n is the global coordinate system,In order to be the heading angle,To convert the IMU's coordinate system to a rotation matrix of the global coordinate system,,X, y, z denote horizontal right, horizontal forward, and vertical upward directions, respectively, and the coordinate system is denoted by a superscript in this disclosure.
The above differential equation is discretized and can be calculated by Kalman filteringTo the point ofError covariance accumulated over a period of time of (a)。
Fig. 4 illustrates a flow chart for determining a lateral error covariance from an error covariance according to some embodiments of the disclosure. As shown in fig. 4, step S120 of determining a lateral error covariance based on the error covariance through the pose information of the moving object at the target time determined by the inertial measurement unit may include the steps of:
S121, performing singular value decomposition on a position error covariance matrix in the error covariance to obtain two eigenvalues and corresponding eigenvectors.
S122, a course vector is determined based on the course angle of the moving object at the target moment, which is determined by the inertia measurement unit.
S123, determining a transverse error covariance based on the two eigenvalues and the corresponding eigenvector and heading vector.
FIG. 5 shows a schematic diagram of a trajectory of a vehicle in a global coordinate system according to an embodiment of the present disclosure, as shown in FIG. 5, an orange solid line 501 is a laser radar Lidar determined vehicle slaveFrom moment to momentGlobal trajectory of moment, blue solid line 502 determines vehicle slave for IMU and wheel speed sensorFrom moment to momentThe global trace of time of day, blue dashed line 503 is atThe traveling direction of the vehicle at the moment, the purple dotted line 504 isThe transverse direction of the running direction of the vehicle at the moment is perpendicular to the running direction of the vehicle, a yellow solid line 505 is the transverse error amount of the global pose of the vehicle determined by the laser radar Lidar and the global pose of the vehicle determined by the IMU and the wheel speed sensor, and a green ellipse 506 is the error covariance of the positions of the vehicle determined by the IMU and the wheel speed sensor at the tk moment under the global coordinate system. As can be seen from fig. 5, the lateral error covariance to be determined in step S120 is the length of the line segment between the intersection point of the purple dotted line 504 and the green ellipse 506 and the center of the green ellipse 504 in fig. 5.
Fig. 6 is a schematic diagram illustrating determination of a lateral error covariance according to an embodiment of the present disclosure, as shown in fig. 6, a green ellipse 601 corresponds to the green ellipse 506 in fig. 5, and is an error covariance of a position under a global coordinate system, a blue dotted line 602 corresponds to the blue dotted line 503 in fig. 5, and is a driving direction of a vehicle, a purple dotted line 603 corresponds to the purple dotted line 504 in fig. 5, and is a lateral direction of the vehicle, and a length of a line segment between an intersection point a of the purple dotted line 603 and the green ellipse 601 and a center O of the green ellipse 601 is the lateral error covariance required to be determined in step S120. Wherein the pose of the vehicle can be determined according to the IMU and the wheel speed sensorThe error covariance of the moment obtains an equation corresponding to the green ellipse 601, and the equation is determined according to the inertia measurement unitCourse angle of momentAn included angle between the blue dotted line 602 and the x-axis of the green ellipse 601 is obtained, then an equation corresponding to the purple dotted line 603 is obtained according to the perpendicular relation between the purple dotted line 603 and the blue dotted line 602, and a transverse error covariance can be obtained by solving the equation corresponding to the green ellipse 601 and the equation corresponding to the purple dotted line 603 simultaneously.
The method comprises the steps of obtaining a position error covariance matrix in error covariance by singular value decomposition, obtaining two eigenvalues and corresponding eigenvectors, determining a course vector according to a course angle of a moving target at a target moment determined by an IMU, and determining transverse error covariance according to the two eigenvalues and the corresponding eigenvector and the course vector. The determination of the lateral error covariance from the two eigenvalues and the corresponding eigenvectors and heading vectors may include constructing an elliptical equation based on the two eigenvalues and the corresponding eigenvectors, determining a linear equation for a lateral vector perpendicular to the heading vector based on the heading vectors and the eigenvectors, and determining the lateral error covariance based on the elliptical equation and the linear equation.
For example, as shown in FIG. 6, the lateral error covarianceThe calculation process of (2) is as follows:
the pose information of the vehicle determined by the IMU and the wheel speed sensor is determined in the following wayThe time-of-day accumulated error covariance isAs a 5*5 matrix, the error covariance of the position (x, y) can be taken,A matrix of 2 x 2 may be subjected to singular value decomposition (Singular Value Decomposition, abbreviated as SVD) to obtain two eigenvalues and corresponding eigenvectors, where the larger eigenvalue is a and the smaller eigenvalue is b.Can be represented as an ellipse with a as the major axis and b as the minor axis. The elliptic equation may be:
(equation 5)
IMU-determined heading angle of vehicle at momentAccording to course angleCan obtain corresponding heading vector v1 (sin #)),cos() The characteristic vector corresponding to the characteristic value a is v2, the included angle between v1 and v2 can be calculated to be alpha according to v1 and v2, and the slope of the heading vector v1 in a coordinate system corresponding to an elliptic equation isThe slope of the transverse vector perpendicular to the heading vector in the coordinate system corresponding to the elliptic equation isThen the following two equations can be solved to obtain:
Wherein, solving the equations simultaneously can obtain the coordinates of the point A in the coordinate system corresponding to the elliptic equationCoordinates of point ASubstituting the above formula to obtain the covariance of the transverse error。
(Equation 8)
According to the embodiment of the disclosure, the error covariance is obtained by establishing a Kalman filtering equation fused by the IMU and the wheel speed sensor, the transverse error covariance is determined by using the error covariance updated by no measurement, whether the positioning sensor fails or not is judged by the ratio between the transverse error covariance and the transverse error amount, and the fact that the size of the transverse error covariance is converted along the x direction and the y direction under a global coordinate system when a moving target turns is considered, so that the fault detection of the positioning sensor can not be influenced by turning of the moving target.
Fig. 7 illustrates a flow chart for determining a lateral error amount according to some embodiments of the present disclosure. As shown in fig. 7, step S200 of determining the lateral error amount between the pose information of the moving object based on the first pose information and the second pose information of the moving object at the target time may include the steps of:
S210, acquiring first pose information of a moving target at a target moment, which is determined by a positioning sensor.
S220, determining second pose information of the moving target at the target moment based on information measured by the inertia measuring unit and the wheel speed sensor.
S230, determining the transverse error amount based on the first pose information and the second pose information.
In some optional embodiments of the present disclosure, the executing body may first acquire first pose information of the moving object at the target time determined by the positioning sensor, then may determine second pose information of the moving object at the target time according to information measured by the inertia measurement unit and the wheel speed sensor, and then determine the lateral error amount according to the first pose information and the second pose information. As shown in fig. 5, a green solid line 507 is a vehicle determined by LidarGlobal pose at time of dayVehicle presence with IMU and wheel speed sensor determinationGlobal pose at time of dayError amount between. Error amount of global pose of vehicle determined by laser radar Lidar, IMU and wheel speed sensorAnd IMU-determined vehicle presenceThe attitude information of the moment is obtained to obtain the vehicle at the momentLateral error amount of timeI.e., yellow solid line 505 in fig. 5.
Accordingly, step S230 of determining the lateral error amount based on the first pose information and the second pose information may include determining the error amount of the first pose information and the second pose information and determining the lateral error amount based on the pose information of the moving object at the target time determined by the inertial measurement unit based on the error amount. As shown in fig. 5, the direction of the yellow solid line 505 matches the direction of the purple dotted line 504, and the error amount of the green solid line 507The projection in the direction of the purple dotted line 504, which is the lateral component of the error between the two in the IMU coordinate system, can be calculated according to the following formula:
(equation 9)
Wherein,Here, takeThe x value of the inner vector.
In some optional embodiments of the present disclosure, according to pose information of a moving object at a target time determined by an IMU and a wheel speed sensor, pose information of the moving object at a start time of a preset time period needs to be determined, starting pose information of the moving object at the start time of the preset time period determined by a positioning sensor may be obtained, and the starting pose information of the moving object at the start time of the preset time period determined by the positioning sensor is used as pose information of the IMU and the wheel speed sensor at the start time of the preset time period. Thus, as shown in fig. 8, the localization fault detection method 1000 may further include the steps of:
s410, acquiring initial pose information of a moving target determined by a positioning sensor at the starting moment of a preset time period.
S420, based on pose information of the moving object determined by the positioning sensor at a first moment in a preset time period and pose information of the moving object determined by the inertia measurement unit and the wheel speed sensor at the first moment, pose compensation information is determined.
S420, compensating the posture information in the initial posture information based on the posture compensation information to obtain initial posture information of the inertial measurement unit and the wheel speed sensor.
The first time may be a time close to the start time in the preset time period, and the distance between the start time and the first time needs to be a straight line distance, for example, the distance between the start time and the first time may be a distance with a length of 3-5 m. For example, at the beginning of a preset time periodThe horizontal position of the vehicle under the global coordinate system determined by the Lidar can be obtainedAnd heading angleThe horizontal position and the course angle are passed through the IMU and the external parameters of the LidarTurning to the coordinate system of the IMU, and taking the IMU and the wheel speed sensor as the IMU and the wheel speed sensor at the starting momentInitial pose. Wherein the method comprises the steps ofMay include a rotation and translation matrix.
To obtain IMU and wheel speed sensor at start timeTrue initial pose, in a preset period of timeGlobal pose of Lidar can be obtained at any timeVehicle presence calculated by IMU and wheel speed sensorThe global pose of the moment isAccording to the acquired Lidar at the starting timeDetermining the initial pose and Lidar positionThe global pose determined at the moment and the IMU and the wheel speed sensor calculated at the momentGlobal pose, the vector of global position transformation of the time interval Lidar can be obtainedVector of global position transformation of IMU and wheel speed sensorAccording to vectorsAnd (3) withThe included angle between the two vectors can be obtainedCan be usedTo compensate for the start time of the acquired LidarCourse angle in the initial pose of (2)Obtaining the starting time of the IMU and the wheel speed sensorTrue initial poseThereby obtaining the real initial pose of the IMU and the wheel speed sensor at the initial time. And then the pose information of the IMU and the wheel speed sensor is calculated from the actual initial pose of the IMU and the wheel speed sensor.
Accordingly, step S420 may include determining pose compensation information based on pose information of the moving object determined by the positioning sensor at a first time in a preset time period and pose information of the moving object determined by the inertia measurement unit and the wheel speed sensor at the first time, determining a first vector based on pose information and start pose information of the moving object determined by the positioning sensor at the first time in the preset time period, determining a second vector based on pose information and start pose information of the moving object determined by the inertia measurement unit and the wheel speed sensor at the first time, and determining an angle between the first vector and the second vector. Step S430 compensates the attitude information in the initial attitude information based on the attitude compensation information to obtain initial attitude information of the inertial measurement unit and the wheel speed sensor, and may include compensating a heading angle in the initial attitude information by the determined included angle to obtain initial attitude information of the inertial measurement unit and the wheel speed sensor.
In some optional embodiments of the present disclosure, step S300 of detecting a failure of the positioning sensor based on a ratio of the lateral error amount to the lateral error covariance may include determining whether the ratio of the lateral error amount to the lateral error covariance is greater than a preset threshold, and determining that the positioning sensor is experiencing a lateral jump failure in response to the ratio of the lateral error amount to the lateral error covariance being greater than the preset threshold. For example, the number of the cells to be processed,As the amount of lateral error that is present,Is a transverse error
Covariance, the ratio of the lateral error amount to the lateral error covariance is。
Fig. 9 shows a flowchart of a method of locating fault detection of an embodiment of the present disclosure. As shown in fig. 9, the laser radar Lidar is used as a positioning sensor for providing a global position on the vehicle, for example, the laser radar Lidar may be a 32-line 3D mechanical laser radar mounted on the roof, and the frequency of updating the pose information may be 10hz. The fault detection is performed on the Lidar by using the IMU and the wheel speed sensor, for example, the frequency of updating pose information of the IMU and the wheel speed sensor may be 100hz. The specific flow is as follows:
and 701, determining a starting pose, namely acquiring the starting pose of the vehicle at the moment determined by the Lidar.
Step 702, aligning the position vectors to obtain a true initial pose based onTime-of-day initial pose and Lidar determined vehicle positionDetermining the pose of the laser radar Lidar at the momentFrom moment to momentThe position of the moment changes the vector. Based onTime-of-day starting pose and IMU and wheel speed sensor determined vehicle positionThe position and the posture of moment, and the IMU and the wheel speed sensor are determinedFrom moment to momentThe position of the moment changes the vector. And compensating the course angle in the initial pose by determining the included angle between the two position transformation vectors to obtain the actual initial pose of the IMU and the wheel speed sensor.
Step 703, fault detection prediction, namely determining an error differential equation based on the noise of the IMU and the wheel speed sensor and the measured information, discretizing the error differential equation, and performing Kalman filtering on the discretized error differential equation to obtain the pose of the vehicleTo the point ofTime period accumulated error covariance. Wherein the errors include position errors, heading angle errors, wheel speed coefficient errors, and gyroscope bias errors. The noise includes gyroscope angular velocity noise, gyroscope angular velocity bias noise wheel speed noise, and wheel speed coefficient noise.
Step 704, calculating a lateral error amount, determined based on LidarThe position and the posture of the moment are the same as the position and the posture of the vehicle determined by the IMU and the wheel speed sensorThe pose of the moment and the error amount of the two are determined. Based on the error amount, the vehicle is determined by the IMUThe attitude at the moment gives the lateral error amount.
Step 705, calculating a lateral error covariance based on the error covariance, the determination by the IMU that the vehicle is inAnd determining the transverse error covariance according to the moment posture.
Step 706, failure detection determination based on the ratio of the lateral error amount to the lateral error covarianceAnd performing fault detection on the laser radar Lidar. If it isIf the preset threshold is exceeded, it is determined that the Lidar fails, and the failure can be cleared through step 707.
If it isIf the preset threshold is not exceeded, it may be determined whether the preset mileage threshold is exceeded by step 708. If the preset mileage threshold is not exceeded, the process returns to step 703 where the fault detection prediction starts to be performed. If the preset mileage threshold is exceeded, the process returns to step 701 to determine that the initial pose starts to be executed.
To avoid affecting the accuracy of Lidar fault detection with an increase in the accumulated error of the IMU, the method of the present disclosure typically requires resetting the detection process after a period of time or distance, for example, a preset mileage threshold of 100 m. The method can accurately detect whether the laser radar Lidar has the transverse jump fault, if the transverse jump fault occurs,If the preset threshold is exceeded, the Lidar is determined to be malfunctioning.
Fig. 10 to 13 are schematic diagrams showing experimental comparison results of a localization fault detection method according to an embodiment of the present disclosure. As shown in fig. 10, when the track of Lidar positioning is normal throughout the course, without jump, as shown in fig. 11,The variation of the data is in a range of less than 6. As shown in fig. 12, when a transition of 0.5m occurs in the same track as in fig. 10, as shown in fig. 13, for the transition portion of Lidar positioning,The data will proliferate, reaching more than 20.
Embodiments of the present disclosure also provide a positioning failure detection apparatus 2000, and fig. 14 shows a block diagram of the positioning failure detection apparatus 2000 according to an embodiment of the present disclosure. The localization fault detection apparatus 2000 of the embodiment of the present disclosure may perform the localization fault detection method 1000 described above. As shown in fig. 14, the positioning failure detection apparatus 2000 may include:
A lateral error covariance determination module 810 configured to determine a lateral error covariance of pose information of a moving object determined by an inertia measurement unit and a wheel speed sensor over a preset period of time based on information measured by the inertia measurement unit and the wheel speed sensor.
A lateral error amount determination module 820 configured to determine a lateral error amount between pose information of the moving object based on first pose information and second pose information of the moving object at a target time, wherein the target time is an end time of the preset time period, the first pose information is determined by a positioning sensor provided on the moving object, and the second pose information is determined by the inertia measurement unit and the wheel speed sensor.
A fault detection module 830 is configured to fault detect the positioning sensor based on a ratio of the lateral error amount to the lateral error covariance.
In some alternative embodiments, the lateral error covariance determination module 810 comprises:
An error covariance determination unit configured to determine an error covariance accumulated in pose information of the moving object over the preset time period by kalman filtering based on information measured by the inertia measurement unit and the wheel speed sensor.
And a lateral error covariance determination unit configured to determine the lateral error covariance based on the error covariance by the attitude information of the moving object at the target time determined by the inertia measurement unit.
In some optional embodiments, the error covariance determination unit is further configured to:
Determining an error differential equation based on the noise of the inertial measurement unit and the wheel speed sensor and the measured information, wherein the error includes a position error, a heading angle error, a wheel speed coefficient error, and a 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 in the preset time period.
In some optional embodiments, the error covariance determination unit is further configured to:
determining gyroscope angular velocity noise and gyroscope angular velocity bias noise based on 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
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.
In some optional embodiments, the error covariance determination unit is further configured to:
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 coefficient error, and the heading angle error.
In some optional embodiments, the lateral error covariance determination unit is further configured to:
Singular value decomposition is carried out on a 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 object at the target time determined by the inertial measurement unit, and
And determining the transverse error covariance based on the two eigenvalues and the corresponding eigenvectors and the heading vector.
In some optional embodiments, the lateral error covariance determination unit is further configured to:
constructing an elliptic equation based on the two eigenvalues and the corresponding eigenvectors;
determining a linear equation for a transverse vector perpendicular to the heading vector based on the heading vector and the feature vector, and
The lateral error covariance is determined based on the ellipse equation and the straight line equation.
In some alternative embodiments, the lateral error amount determination module 820 is further configured to:
Acquiring first pose information of the moving target at the target moment, which is determined by the positioning sensor;
Determining second pose 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 pose information and the second pose information.
In some alternative embodiments, the lateral error amount determination module 820 is further configured to:
determining an error amount of the first pose information and the second pose information, and
And determining the transverse error amount based on the error amount through the attitude information of the moving target at the target moment, which is determined by the inertia measurement unit.
In some alternative embodiments, the positioning fault detection device 2000 further includes a starting pose determination module configured to:
acquiring initial pose information of the moving target at the starting moment of the preset time period, which is determined by the positioning sensor;
Determining attitude compensation information based on the attitude information of the moving object at the first moment in the preset time period, which is determined by the positioning sensor, and the attitude information of the moving object at the first moment, which is determined by the inertia measurement unit and the wheel speed sensor;
and compensating the posture information in the initial posture information based on the posture compensation information to obtain initial posture information of the inertial measurement unit and the wheel speed sensor.
In some alternative embodiments, the starting pose determination module is further configured to:
determining a first vector based on pose information of the moving target at a first moment in the preset time period and the initial pose information, which are determined by the positioning sensor;
Determining a second vector based on pose information of the moving object at the first moment and the start pose information determined by the inertial measurement unit and the wheel speed sensor, and
Determining an angle between the first vector and the second vector;
and compensating the course angle in the initial pose information through the included angle to obtain initial pose information of the inertial measurement unit and the wheel speed sensor.
In some alternative embodiments, the fault detection module 830 is further configured to:
Judging whether the ratio of the transverse error amount to the transverse error covariance is larger than a preset threshold value or not;
And determining that the positioning sensor has a transverse jump fault in response to the ratio of the transverse error amount to the transverse error covariance being greater than the preset threshold.
In addition, the embodiment of the disclosure further provides an electronic device, which comprises at least one processor and a memory communicatively connected with the at least one processor, wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor so that the at least one processor can execute the positioning fault detection method 1000.
The disclosed embodiments also provide a computer-readable storage medium storing a computer program that when executed by a processor implements the above-described localization fault detection method 1000.
The disclosed embodiments also provide a computer program product comprising a computer program which, when executed by a processor, implements the above-described localization fault detection method 1000.
Fig. 15 illustrates a block diagram of an example electronic device 3000 that may be used to implement embodiments of the present disclosure. The electronic device 3000 is intended to represent various forms of digital computers. The electronic device 3000 may also represent various forms of mobile apparatuses capable of running a computing program. The components shown herein, their connections and relationships, and their functions, are meant to be exemplary only, and are not meant to limit implementations of the disclosure described and/or claimed herein.
As shown in fig. 15, the electronic device 3000 includes a computing unit 910 that can perform various appropriate actions and processes according to 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. In the RAM 930, various programs and data required for the operation of the autonomous vehicle or the mobile robot 3000 can also be stored. The computing unit 910, ROM 920, and RAM 930 are connected to each other by a bus 940. An input/output (I/O) interface 950 is also connected to bus 940.
Various components in the electronic device 3000 are connected to the I/O interface 950, including an input unit 960, such as a touch screen, etc., an output unit 970, such as various types of displays, speakers, etc., a storage unit 980, such as a disk, etc., and a communication unit 990, such as a network card, modem, wireless communication transceiver, etc. The communication unit 990 allows the electronic device 3000 to exchange information/data with other devices through a computer network, such as the internet, and/or various telecommunication networks.
The computing unit 910 may be a variety of general purpose and/or special purpose processing components having processing and computing capabilities. Some examples of computing unit 910 include, but are not limited to, a Central Processing Unit (CPU), a Graphics Processing Unit (GPU), various specialized 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 localization fault detection method 1000. For example, in some embodiments, the localization fault detection method 1000 may be implemented as a computer software program tangibly embodied on a machine-readable medium, such as the storage unit 980. In some implementations, some or all of the computer programs may be loaded and/or installed onto the electronic device 3000 via the ROM 920 and/or the communication unit 990. When the computer program is loaded into RAM 930 and executed by computing unit 910, one or more steps of the localization fault detection method 1000 described above may be performed. Alternatively, in other embodiments, the computing unit 910 may be configured to perform the localization fault detection method 1000 by any other suitable means (e.g., by means of firmware).
Various implementations of the systems and techniques described here above may be implemented in digital electronic circuitry, integrated circuit systems, field Programmable Gate Arrays (FPGAs), application Specific Integrated Circuits (ASICs), application Specific Standard Products (ASSPs), systems On Chip (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and/or combinations thereof. These various embodiments may include being implemented in one or more computer programs that are executable and/or interpretable on a programmable system including at least one programmable processor, which may be a special or general purpose programmable processor, operable to receive data and instructions from, and to transmit data and instructions to, a storage system, at least one input device, and at least one output device.
Program code for carrying out methods of the present disclosure may be written in any combination of one or more programming languages. These 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 the program code, when executed by the processor or controller, causes the functions/operations specified in the flowchart and/or block diagram to be implemented. The program code may execute entirely on the machine, partly on the machine, as a stand-alone software package, partly on the machine and partly on a remote machine or entirely on the remote machine or server.
In the context of this disclosure, a machine-readable medium may be a tangible medium that can contain, or store a program for use by or in connection with an instruction execution system, apparatus, or device. The machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. The machine-readable medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any suitable combination of the foregoing. More specific examples of a machine-readable storage medium would include an electrical connection based on one or more wires, a portable computer diskette, a hard disk, a Random Access Memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disc read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.
It should be appreciated that various forms of the flows shown above may be used to reorder, add, or delete steps. For example, the steps recited in the present disclosure may be performed in parallel or sequentially or in a different order, provided that the desired results of the technical solutions of the present disclosure are achieved, and are not limited herein.
The above detailed description should not be taken as limiting the scope of the present disclosure. It will be apparent to those skilled in the art that various modifications, combinations, sub-combinations and alternatives are possible, depending on design requirements and other factors. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present disclosure are intended to be included within the scope of the present disclosure.