A method for determining state quantity error and vehicle-mounted terminal

Abstract

The embodiment of the present invention discloses a method for determining a state quantity error and a vehicle-mounted terminal. The method includes: determining a first-class speed of a preset point in the vehicle at the first moment according to the wheel speed collected by the wheel speed meter and the device angular velocity collected by the motion detection device; obtaining a second-class speed of the preset point at the first moment determined according to the data collected by the positioning device; using the first-class speed and the second-class speed as state quantity measurement and state prediction quantity, and determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error; the constraint relationship is obtained based on a rigid body model with the same true speed of the same particle calculated in different ways at the same moment, and the relationship between the measured value, the true value and the disturbance error. Applying the scheme provided by the embodiment of the present invention, the state quantity error of the preset point in the vehicle can be determined and the state quantity data can be corrected.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving technology, and in particular to a state quantity error determination method and a vehicle-mounted terminal. Background Art

[0002] In the vehicle positioning system, smart vehicles are usually equipped with sensors such as inertial measurement units (IMUs). The vehicle positioning system can measure the position and state of the vehicle based on the data output by the IMU and combined with the data from other sensors to obtain the vehicle's speed, position, attitude, acceleration, angular velocity and other state quantities. The characteristics of IMU are that the frequency of data collection is above 200Hz and it is more sensitive to changes in speed and attitude. However, if there is no correction of external auxiliary information for a long time, the speed and attitude obtained from the data collected by the IMU will tend to diverge rapidly.

[0003] Therefore, how to correct the vehicle's state data in real time is a problem that needs to be solved urgently. Summary of the invention

[0004] The present invention provides a state quantity error determination method and a vehicle-mounted terminal to determine the state quantity error of a preset point in a vehicle and correct the state quantity data. The specific technical solution is as follows.

[0005] In a first aspect, an embodiment of the present invention discloses a method for determining a state quantity error, comprising:

[0006] Obtaining the wheel speed and device angular velocity respectively collected by the wheel speed meter and the motion detection device at the first moment;

[0007] Determining a first type of speed of a preset point in the vehicle at the first moment according to the wheel speed and the device angular velocity;

[0008] Acquire a second type of speed of the preset point at the first moment determined according to data collected by the positioning device;

[0009] Taking the first type of speed and the second type of speed as state quantity measurement and state prediction quantity, and determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error;

[0010] Among them, the state quantity deviation is the deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship is: based on a rigid body model with the same true velocity of the same particle calculated in different ways at the same time and the relationship between the measured value, the true value and the disturbance error.

[0011] Optionally, after determining the state quantity error of the preset point at the first moment, the method further includes:

[0012] According to the state quantity error of the preset point at the first moment, the state quantity of the preset point at the first moment is corrected to obtain the corrected state quantity of the preset point at the first moment; wherein the state quantity of the preset point at the first moment is: the estimated state quantity obtained after Kalman filtering.

[0013] Optionally, the step of determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, comprises:

[0014] Obtaining the confidence of the state quantity error of the preset point at a second moment; wherein the second moment is a moment before the first moment;

[0015] According to the determined state transfer matrix, the confidence at the second moment is updated in time to obtain the prediction confidence at the first moment;

[0016] Substituting the state quantity of the preset point at the first moment into the coefficient matrix of the state quantity error in the pre-established constraint relationship to obtain the measurement matrix at the first moment; wherein the constraint relationship is: the constraint relationship between the state quantity deviation and the state quantity error;

[0017] Determining a Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment;

[0018] According to the Kalman gain at the first moment, the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship, the Kalman filter is measured and updated to obtain the state quantity error of the preset point at the first moment.

[0019] Optionally, the step of performing time updating on the confidence at the second moment according to the determined state transfer matrix to obtain the prediction confidence at the first moment includes:

[0020] according to The confidence level for the second moment Σ t-1 Perform time update to obtain the prediction confidence at the first moment Among them, the A t is a determined state transfer matrix, R is a state transfer matrix confidence, t and t-1 are the first moment and the second moment respectively, and T is a transposed symbol;

[0021] The step of determining the Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment comprises:

[0022] according to And the measurement matrix H at the first moment t and the prediction confidence at the first moment Determine the Kalman gain K at the first moment t ; Wherein, Q is the measurement noise;

[0023] The step of performing measurement update of Kalman filtering according to the Kalman gain at the first moment, the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship to obtain the state quantity error of the preset point at the first moment includes:

[0024] according to And the state quantity deviation between the state quantity measurement and the state prediction quantity is taken as Perform Kalman filter measurement update to obtain the state error μ of the preset point at the first moment t ;in, The Z t预 is the state prediction quantity, the Z t量 is the state quantity measurement, The K t is the Kalman gain at the first moment.

[0025] Optionally, the constraint relationship between the state quantity deviation and the state quantity error is determined in the following manner:

[0026] The vehicle is regarded as a rigid body, the preset point is regarded as a mass point, and an equal relationship between the first type velocity true value and the second type velocity true value of the preset point is established;

[0027] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured values, the equal relationship is disturbed and decomposed to obtain the constraint relationship between the state quantity deviation and the state quantity error.

[0028] Optionally, the preset point is the center point of the motion detection device; the state quantity error includes at least one of the following: position error, speed error, attitude error, gyro bias error, and accelerometer bias error.

[0029] Optionally, the step of determining a first type of speed of a preset point in the vehicle according to the wheel speed and the device angular velocity comprises:

[0030] According to v1=V+ω v×l, determine the first type velocity of the center point of the motion detection device in the vehicle;

[0031] Wherein, v1 is the first type velocity of the center point in the vehicle body coordinate system, V is the wheel speed in the vehicle body coordinate system, and ω v is the decomposition of the device angular velocity in the vehicle body coordinate system, and l is the position difference vector between the wheel and the center point corresponding to the wheel speed;

[0032] The step of performing disturbance decomposition on the equality relationship based on the relationship that the measured value is equal to the sum of the true value and the disturbance error and taking the first type of speed and the second type of speed as the measured values ​​to obtain the constraint relationship between the state quantity deviation and the state quantity error comprises:

[0033] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured values, the equal relationship is subjected to disturbance decomposition, and the following constraint relationship between the state quantity deviation and the state quantity error is obtained:

[0034]

[0035] Among them, the V n is the second type of velocity of the center point in the world coordinate system, is the rotation matrix between the device coordinate system where the motion detection device is located and the vehicle body coordinate system, is the rotation matrix between the device coordinate system and the world coordinate system, is the rotation matrix between the vehicle coordinate system and the world coordinate system, [V n ×] is V n Transformed into an antisymmetric matrix, the δφ is the attitude error, the δV n is the velocity error, and δε is the gyro bias error.

[0036] Optionally, there are multiple wheel speeds, each wheel speed is a wheel speed of a different wheel in the vehicle; the step of determining the first type of speed of a preset point in the vehicle at the first moment according to the wheel speeds and the device angular velocity comprises:

[0037] Determining first-category speeds of preset points in the vehicle at the first moment, respectively, according to the wheel speeds of the wheels and the angular velocity of the device;

[0038] The step of using the first type of speed and the second type of speed as state quantity measurement and state prediction quantity, and determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, comprises:

[0039] Using the matrix formed by each first-type speed as a state quantity measurement, using the second-type speed as a state prediction quantity, and determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error;

[0040] or,

[0041] A target speed is selected from each first-category speed in order as a state quantity measurement, and a corrected state quantity error corresponding to the target speed is determined in the following manner. When the target speed is the last speed among each first-category speed, the corrected state quantity error corresponding to the target speed is used as the state quantity error of the preset point at the first moment:

[0042] The second type of speed is used as a state prediction quantity, and the state quantity error to be corrected is corrected according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship between the state quantity deviation and the state quantity error established in advance, so as to obtain a corrected state quantity error;

[0043] Among them, when the target speed is not the first first-category speed, the state quantity error to be corrected is: the corrected state quantity error corresponding to the previous first-category speed of the target speed; when the target speed is the first first-category speed, the state quantity error to be corrected is zero.

[0044] In a second aspect, an embodiment of the present invention discloses a vehicle-mounted terminal, including: a processor, a wheel speed meter, a motion detection device, and a positioning device; the processor includes:

[0045] A first acquisition module is used to acquire the wheel speed and device angular velocity respectively collected by the wheel speed meter and the motion detection device at the first moment;

[0046] A first determination module, configured to determine a first type of speed of a preset point in the vehicle at the first moment according to the wheel speed and the device angular velocity;

[0047] A second acquisition module, used to acquire a second type of velocity of the preset point at the first moment determined according to data collected by the positioning device;

[0048] a second determination module, configured to use the first type of speed and the second type of speed as state quantity measurement and state prediction quantity, and determine the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and a pre-established constraint relationship between the state quantity deviation and the state quantity error;

[0049] Among them, the state quantity deviation is the deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship is: based on a rigid body model with the same true velocity of the same particle calculated in different ways at the same time and the relationship between the measured value, the true value and the disturbance error.

[0050] Optionally, the processor further includes:

[0051] A correction module is used to correct the state quantity of the preset point at the first moment after determining the state quantity error of the preset point at the first moment, according to the state quantity error of the preset point at the first moment, to obtain the corrected state quantity of the preset point at the first moment; wherein the state quantity of the preset point at the first moment is: the estimated state quantity obtained after Kalman filtering.

[0052] Optionally, the second determining module is specifically configured to:

[0053] Obtaining the confidence of the state quantity error of the preset point at a second moment; wherein the second moment is a moment before the first moment;

[0054] According to the determined state transfer matrix, the confidence at the second moment is updated in time to obtain the prediction confidence at the first moment;

[0055] Substituting the state quantity of the preset point at the first moment into the coefficient matrix of the state quantity error in the pre-established constraint relationship to obtain the measurement matrix at the first moment; wherein the constraint relationship is: the constraint relationship between the state quantity deviation and the state quantity error;

[0056] Determining a Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment;

[0057] According to the Kalman gain at the first moment, the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship, the Kalman filter is measured and updated to obtain the state quantity error of the preset point at the first moment.

[0058] Optionally, the second determination module performs time updating on the confidence at the second moment according to the determined state transfer matrix to obtain the prediction confidence at the first moment, including:

[0059] according to The confidence level for the second moment Σ t-1 Perform time update to obtain the prediction confidence at the first moment Among them, the A t is a determined state transfer matrix, R is a state transfer matrix confidence, t and t-1 are the first moment and the second moment respectively, and T is a transposed symbol;

[0060] The second determination module, when determining the Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment, includes:

[0061] according to And the measurement matrix H at the first moment t and the prediction confidence at the first moment Determine the Kalman gain K at the first moment t ; Wherein, Q is the measurement noise;

[0062] The second determination module performs measurement update of the Kalman filter according to the Kalman gain at the first moment, the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship, and obtains the state quantity error of the preset point at the first moment, including:

[0063] according to And the state quantity deviation between the state quantity measurement and the state prediction quantity is taken as Perform Kalman filter measurement update to obtain the state error μ of the preset point at the first moment t ;in, The Z t预 is the state prediction quantity, the Z t量 is the state quantity measurement, The K t is the Kalman gain at the first moment.

[0064] Optionally, the processor further includes a third determining module, configured to determine the constraint relationship between the state quantity deviation and the state quantity error by performing the following operations:

[0065] The vehicle is regarded as a rigid body, the preset point is regarded as a mass point, and an equal relationship between the first-type velocity true value and the second-type velocity true value of the preset point is established;

[0066] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured values, the equal relationship is disturbed and decomposed to obtain the constraint relationship between the state quantity deviation and the state quantity error.

[0067] Optionally, the preset point is the center point of the motion detection device; the state quantity error includes at least one of the following: position error, speed error, attitude error, gyro bias error, and accelerometer bias error.

[0068] Optionally, the first determination module is specifically configured to determine a first type of speed of a preset point in the vehicle according to the wheel speed and the device angular velocity, comprising:

[0069] According to v1=V+ω v ×l, determining the first type velocity of the center point of the motion detection device in the vehicle;

[0070] Wherein, v1 is the first type velocity of the center point in the vehicle body coordinate system, V is the wheel speed in the vehicle body coordinate system, and ω v is the decomposition of the device angular velocity in the vehicle body coordinate system, and l is the position difference vector between the wheel and the center point corresponding to the wheel speed;

[0071] The third determination module, according to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured values, performs disturbance decomposition on the equality relationship to obtain the constraint relationship between the state quantity deviation and the state quantity error, includes:

[0072] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured values, the equal relationship is subjected to disturbance decomposition, and the following constraint relationship between the state quantity deviation and the state quantity error is obtained:

[0073]

[0074] Among them, the V n is the second type of velocity of the center point in the world coordinate system, is the rotation matrix between the device coordinate system where the motion detection device is located and the vehicle body coordinate system, is the rotation matrix between the device coordinate system and the world coordinate system, is the rotation matrix between the vehicle coordinate system and the world coordinate system, [V n ×] is V n Transformed into an antisymmetric matrix, the δφ is the attitude error, the δV n is the velocity error, and δε is the gyro bias error.

[0075] Optionally, there are multiple wheel speeds, each wheel speed is a wheel speed of a different wheel in the vehicle; the first determination module determines the first type of speed of a preset point in the vehicle at the first moment according to the wheel speeds and the device angular velocity, comprising:

[0076] Determining first-category speeds of preset points in the vehicle at the first moment, respectively, according to the wheel speeds of the wheels and the angular velocity of the device;

[0077] The second determining module is specifically used to:

[0078] Using the matrix formed by each first-type speed as a state quantity measurement, using the second-type speed as a state prediction quantity, and determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error;

[0079] or,

[0080] A target speed is selected from each first-category speed in order as a state quantity measurement, and a corrected state quantity error corresponding to the target speed is determined in the following manner. When the target speed is the last speed among each first-category speed, the corrected state quantity error corresponding to the target speed is used as the state quantity error of the preset point at the first moment:

[0081] The second type of speed is used as a state prediction quantity, and the state quantity error to be corrected is corrected according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship between the state quantity deviation and the state quantity error established in advance, so as to obtain a corrected state quantity error;

[0082] Among them, when the target speed is not the first first-category speed, the state quantity error to be corrected is: the corrected state quantity error corresponding to the previous first-category speed of the target speed; when the target speed is the first first-category speed, the state quantity error to be corrected is zero.

[0083] It can be seen from the above content that the state quantity error determination method and vehicle-mounted terminal provided in the embodiment of the present invention can determine the first type of speed of the preset point according to the wheel speed and the angular velocity of the device, and determine the second type of speed of the preset point according to the data collected by the positioning device. The true values ​​of these two types of speeds should be equal. Therefore, according to the state quantity deviation between the first type of speed and the second type of speed, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, the state quantity error of the preset point can be determined. After determining the state quantity error, the state quantity error can be used to correct the state quantity to obtain a more accurate state quantity. Therefore, the embodiment of the present invention can determine the state quantity error of the preset point in the vehicle and correct the state quantity data. Of course, it is not necessary to achieve all the advantages described above at the same time for any product or method implementing the present invention.

[0084] The innovative features of the embodiments of the present invention include:

[0085] 1. Two types of speeds at the same preset point in the vehicle are determined in two different ways. Each speed can be regarded as the sum of the true value and the disturbance error. According to the same rigid body model of the true speed of the same particle calculated in different ways and the relationship between the measured value, the true value and the disturbance error, a constraint relationship between the two types of speeds can be established, and the state error of the preset point can be determined based on the constraint relationship and the state deviation.

[0086] 2. The modified Kalman filter algorithm is used to determine the state error of the preset point at the first moment, which can determine the state error more accurately.

[0087] 3. When there are multiple wheel speeds, at least two methods can be used to determine the state error, so as to achieve data fusion between multiple wheel speeds and motion detection devices, thereby improving the accuracy of the state error. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the prior art descriptions. Obviously, the drawings described below are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can also be obtained based on these drawings without creative work.

[0089] Figure 1 A schematic diagram of a flow chart of a method for determining a state quantity error provided by an embodiment of the present invention;

[0090] Figure 2 A schematic diagram of a vehicle motion model provided by an embodiment of the present invention;

[0091] Figure 3 A schematic diagram of the structure of a vehicle-mounted terminal provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0092] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0093] It should be noted that the terms "including" and "having" and any variations thereof in the embodiments of the present invention and the accompanying drawings are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device comprising a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products or devices.

[0094] The embodiment of the present invention discloses a state quantity error determination method and a vehicle-mounted terminal, which can determine the state quantity error of a preset point in a vehicle and correct the state quantity data. The embodiment of the present invention is described in detail below.

[0095] Figure 1 A flow chart of a state quantity error determination method provided in an embodiment of the present invention. The method is applied to electronic equipment. The electronic equipment may be a common computer, a server, or an intelligent terminal device, etc., or may be a vehicle-mounted computer or a vehicle-mounted industrial control computer (Industrial Personal Computer, IPC) and other vehicle-mounted terminals. The vehicle in this embodiment refers to an intelligent vehicle. A vehicle-mounted terminal may be installed in the vehicle. The vehicle-mounted terminal may include a processor and a plurality of sensors. The sensor may include a wheel speed meter, a motion detection device, and a positioning device, etc. The motion detection device may include but is not limited to an inertial measurement unit (Inertial Measurement Unit, IMU). The positioning device may include but is not limited to a global navigation satellite system (Global Navigation Satellite System, GNSS). The method specifically includes the following steps.

[0096] S110: Obtain the wheel speed and device angular velocity respectively collected by the wheel speed meter and the motion detection device at the first moment.

[0097] Both the wheel speed meter and the motion detection device can collect data periodically at the corresponding specified frequency. The wheel speed meter can output the wheel speed of the four wheels of the vehicle, and the frequency of collecting data can vary from 10 to 100 Hz. The motion detection device can collect data such as acceleration and angular velocity, and the frequency of collecting data is above 200 Hz. The sensors involved in this embodiment are all set in the same vehicle.

[0098] This embodiment may be executed periodically at a preset frequency. When obtaining the wheel speed and device angular velocity at the first moment, the wheel speed and device angular velocity at the first moment may be determined from the wheel speeds and device angular velocities at various moments that are stored.

[0099] S120: Determine a first type of speed of a preset point in the vehicle at a first moment according to the wheel speed and the device angular velocity.

[0100] Among them, the preset point can be the center point of the motion detection device. The state quantity error can include at least one of the following: position error, speed error, attitude error, gyro bias error, accelerometer bias error. For example, the preset point can be the center point of the IMU, and the state quantity error can include the position error of the IMU, the speed error of the IMU, the attitude error of the IMU, the gyro bias error of the IMU, and the accelerometer bias error of the IMU. Position, speed, attitude, gyro bias, accelerometer bias, etc. can all be called state quantities. Gyro bias is an important indicator of gyroscope accuracy and is used to correct angular velocity. Accelerometer bias is an important indicator of accelerometer accuracy and is used to correct acceleration.

[0101] The preset point in the vehicle may also be other points in the vehicle, which is not limited in this embodiment. The position of the preset point may be determined in advance. The relative position between the preset point and each wheel, the origin of the vehicle body coordinate system or the sensor may also be determined in advance.

[0102] When determining the first type of speed of a preset point in the vehicle at the first moment, the rotation speed of the preset point caused by the rotation of the vehicle can be determined based on the relative distance between the motion detection device and the preset point and the angular velocity of the device, and the translation speed of the preset point caused by the translation of the vehicle can be determined based on the relative distance between the wheel and the preset point and the wheel speed. The composite speed of the rotation speed and the translation speed is used as the first type of speed of the preset point.

[0103] The first type of speed may be understood as the speed of the preset point determined by the first determination method.

[0104] S130: Obtaining a second type of speed of a preset point at a first moment determined according to data collected by the positioning device.

[0105] The positioning device can collect data periodically at a specified frequency. The data collected by the positioning device is data in the world coordinate system. The data collected by the positioning data can specifically include data such as the position and steering angle in the world coordinate system. The speed of the preset point can be determined based on the data collected by the positioning device at the first moment and the moment before the first moment, and the pre-determined distance between the positioning device and the preset point. The speed determined in this way is called the second type of speed of the preset point.

[0106] In this embodiment, the first type of speed and the second type of speed are both speeds of a preset point, and the two speeds may be speeds in the same coordinate system.

[0107] S140: Taking the first type of speed and the second type of speed as state quantity measurement and state prediction quantity, determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error.

[0108] Among them, the state quantity deviation is the deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship is: based on the rigid body model with the same true velocity of the same particle calculated in different ways at the same time, and the relationship between the measured value, the true value and the disturbance error.

[0109] Specifically, the measured value can be equal to the true value plus the disturbance error, and the first-class speed and the second-class speed are both regarded as measured values, and the true values ​​contained in the first-class speed and the second-class speed should be equal. According to the above relationship, the constraint relationship between the state quantity deviation and the state quantity error can be obtained.

[0110] It can be seen from the above content that the present embodiment can determine the first type of speed of the preset point according to the wheel speed and the angular velocity of the device, and determine the second type of speed of the preset point according to the data collected by the positioning device. The true values ​​of these two types of speeds should be equal. Therefore, according to the state quantity deviation between the first type of speed and the second type of speed, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, the state quantity error of the preset point can be determined. After determining the state quantity error, the state quantity error can be used to correct the state quantity to obtain a more accurate state quantity. Therefore, the present embodiment can determine the state quantity error of the preset point in the vehicle and correct the state quantity data.

[0111] Although the wheel speed meter can directly output the vehicle's speed information, due to the differences in wheel speed output frequency, dead zone and resolution of different types of vehicles, the speed information directly output by the wheel speed meter is often insufficient to meet the needs of unmanned driving.

[0112] This embodiment fuses the data of the wheel speed meter, the motion detection device and the positioning device to determine a more accurate state quantity error. When the state quantity of the preset point is corrected using the state quantity error, more accurate state quantity information can be obtained.

[0113] In another embodiment of the present invention, based on Figure 1 The embodiment shown in the figure, after determining the state quantity error of the preset point at the first moment, further includes:

[0114] According to the state quantity error of the preset point at the first moment, the state quantity of the preset point at the first moment is corrected to obtain the corrected state quantity of the preset point at the first moment.

[0115] The state quantity of the preset point at the first moment is: the estimated state quantity obtained after Kalman filtering.

[0116] Although directly performing Kalman filtering on the state quantity can improve the accuracy of the state quantity, since the state quantity contains values ​​of very large magnitude, such as position data in the world coordinate system, and also includes values ​​of very small data volume, in this case, when using Kalman filtering to perform inverse operations on the state quantity, there will be jitter errors.

[0117] In this embodiment, the estimated state quantity obtained after Kalman filtering is corrected using the state quantity error to obtain a more accurate state quantity.

[0118] In another embodiment of the present invention, based on Figure 1 In the illustrated embodiment, step S140, the step of determining the state quantity error of the preset point at the first moment based on the state quantity deviation between the state quantity measurement and the state prediction, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, may include the following steps 1a to 5a.

[0119] Step 1a: Obtain the confidence of the state error of the preset point at the second moment.

[0120] The second moment is the moment before the first moment. The operation of determining the state quantity error may be performed periodically according to a preset frequency, and the previous moment may be understood as the moment when the state quantity error was determined last time.

[0121] Since the state quantity error determined last time has been corrected to the state quantity at the previous moment, it is considered that there is no error in the corrected state quantity, that is, the state quantity error at the second moment is considered to be zero. The result obtained after time updating the state quantity error at the second moment is still zero.

[0122] Step 2a: According to the determined state transfer matrix, the confidence at the second moment is updated in time to obtain the prediction confidence at the first moment.

[0123] In this step, when determining the state transfer matrix, the preset state transfer matrix can be updated according to the state quantity of the preset point at the first moment to obtain the state transfer matrix at the first moment. When updating the preset state transfer matrix, the state quantity at the first moment can be substituted into the preset state transfer matrix.

[0124] Step 3a: Substitute the state quantity of the preset point at the first moment into the coefficient matrix of the state quantity error in the pre-established constraint relationship to obtain the measurement matrix at the first moment.

[0125] Among them, the constraint relationship is: the constraint relationship between the state quantity deviation and the state quantity error.

[0126] Step 4a: Determine the Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment.

[0127] Step 5a: According to the Kalman gain at the first moment, the state quantity deviation between the above-mentioned state quantity measurement and the above-mentioned state prediction quantity, and the above-mentioned constraint relationship, the Kalman filter measurement update is performed to obtain the state quantity error of the preset point at the first moment.

[0128] In this embodiment, the confidence level of the state quantity error at the first moment may also be determined according to the following steps:

[0129] The Kalman filter is measured and updated according to the Kalman gain at the first moment, the measurement matrix at the first moment and the prediction confidence at the first moment to obtain the confidence of the state quantity error of the preset point at the first moment.

[0130] In summary, the present embodiment provides a specific implementation method for determining the state quantity error based on the state quantity deviation and the constraint relationship. This implementation method is a process of determining the state quantity error of a preset point using a modified Kalman filter algorithm. The modified Kalman filter can use a linear state transfer matrix to optimally estimate the state quantity of the system through the input observation data. Since the observation data includes the influence of noise and interference in the system, the optimal estimation can also be regarded as a filtering process. In this embodiment, the system refers to a system composed of a vehicle and various sensors. Based on the modified Kalman filter algorithm and the state quantity deviation and constraint relationship, the state quantity error can be determined more accurately.

[0131] In another embodiment of the present invention, in the above embodiment, step 2a, i.e., the step of updating the confidence at the second moment according to the determined state transfer matrix to obtain the prediction confidence at the first moment, may include:

[0132] according to Confidence in the second moment Σ t-1Perform time update to obtain the prediction confidence at the first moment

[0133] Among them, A t is the determined state transfer matrix, R is the state transfer matrix confidence, t and t-1 are the first moment and the second moment respectively, and T is the transposed symbol. The state transfer matrix confidence can be a preset value, or it can be calculated based on the state quantity of the preset point at the first moment and the corresponding preset formula.

[0134] In this embodiment, the confidence level may also be referred to as covariance, which is used to indicate the credibility of a parameter.

[0135] Step 4a, i.e., the step of determining the Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment, may include:

[0136] according to And the measurement matrix H at the first moment t and the first moment prediction confidence Determine the Kalman gain K at the first moment t .

[0137] Wherein, Q is the measurement noise. The measurement noise can be calculated by the state quantity of the preset point at the first moment and the corresponding preset formula.

[0138] Step 5a, i.e., performing measurement update of the Kalman filter according to the Kalman gain at the first moment, the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship, to obtain the state quantity error of the preset point at the first moment, comprises:

[0139] according to And the state quantity deviation between the state quantity measurement and the state prediction quantity is taken as Perform Kalman filter measurement update to obtain the state error μ of the preset point at the first moment t .

[0140] in, Z t预 is the state prediction quantity at the first moment, Z t量 is the state quantity measurement at the first moment, K t is the Kalman gain at the first moment.

[0141] In this embodiment, the step of performing measurement update of the Kalman filter according to the Kalman gain at the first moment, the measurement matrix at the first moment, and the prediction confidence at the first moment to obtain the confidence of the state quantity error of the preset point at the first moment includes:

[0142] according to Perform Kalman filter measurement update to obtain the confidence Σ of the state error of the preset point at the first moment t .

[0143] Among them, K t is the Kalman gain at the first moment, H t is the measurement matrix at the first moment, is the prediction confidence at the first moment, and I is the unit matrix.

[0144] In summary, this embodiment provides a specific formula for determining the state error of a preset point at the first moment using the modified Kalman filter algorithm formula.

[0145] In another embodiment of the present invention, based on the above embodiment, the following steps 1b and 2b may be used to determine the constraint relationship between the state quantity deviation and the state quantity error.

[0146] Step 1b: Consider the vehicle as a rigid body and the preset point as a mass point, and establish an equal relationship between the true value of the first type of velocity and the true value of the second type of velocity at the preset point.

[0147] In this embodiment, the first type of speed may be equal to the sum of the first type of speed true value and the corresponding disturbance error, and the second type of speed may be equal to the sum of the second type of speed true value and the corresponding disturbance error. Both the first type of speed and the second type of speed may be regarded as measured values, which are values ​​with errors.

[0148] Step 2b: Based on the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured values, the equal relationship is disturbed and decomposed to obtain the constraint relationship between the state quantity deviation and the state quantity error.

[0149] The relationship between the measured value and the sum of the true value and the disturbance error can be expressed as follows:

[0150]

[0151] is the measured value, x 真 is the true value, and Δx is the disturbance error.

[0152] When performing disturbance decomposition on the equality relationship, the state quantity term is moved to one side of the equation and the error quantity term is moved to the other side of the equation to obtain the constraint relationship between the state quantity deviation and the state quantity error.

[0153] The steps in the above embodiment are further described in detail below with reference to specific examples.

[0154] In this embodiment, the preset point is the center point of the motion detection device, and the state quantity error includes at least one of the following: position error, speed error, attitude error, gyro bias error, and accelerometer bias error.

[0155] Step S120, i.e., the step of determining the first type of speed of a preset point in the vehicle according to the wheel speed and the device angular velocity, may include:

[0156] According to v1=V+ω v ×l, determine the first type velocity of the center point of the motion detection device in the vehicle.

[0157] Among them, v1 is the first type of velocity of the center point of the motion detection device in the vehicle coordinate system, V is the wheel speed in the vehicle coordinate system, V is represented by a vector, ω v is the decomposition of the device angular velocity in the vehicle coordinate system, and l is the position difference vector between the wheel and the center point corresponding to the wheel speed.

[0158] Step 1b, based on the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured value, performing disturbance decomposition on the equality relationship to obtain the constraint relationship between the state quantity deviation and the state quantity error, comprises:

[0159] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first and second types of speed as the measured values, the equal relationship is disturbed and decomposed, and the following constraint relationship between the state quantity deviation and the state quantity error is obtained:

[0160]

[0161] Among them, V n is the second type velocity of the center point in the world coordinate system, is the rotation matrix between the device coordinate system where the motion detection device is located and the vehicle coordinate system. is the rotation matrix between the device coordinate system and the world coordinate system, is the rotation matrix between the vehicle coordinate system and the world coordinate system, [V n ×] is V n Transformed into an antisymmetric matrix, δφ is the attitude error, δV n is the velocity error, and δε is the gyro bias error.

[0162] Combine the following Figure 2 The schematic diagram of the vehicle motion model shown in the figure illustrates the process of establishing the above constraint relationship. In this example, the vehicle turns left around point r, where r is the rotation center. The wheel speeds of the vehicle are represented by V0, V1, V2 and V3 respectively, and the directions are as follows: Figure 2As shown. r0, r1, r2 and r3 are the distances from the center of the four wheels to the center of rotation, also known as the turning radius. l0, l1, l2 and l3 are the lever arms representing the distances from the center of the four wheels to the center of the IMU. xoy is the vehicle coordinate system, whose origin is the center of the rear wheels. The front and rear wheel tracks are H, and the left and right wheel tracks are L.

[0163] From the rigid body motion model, we can get: The left side of the equation represents the speed of the preset point in the vehicle coordinate system determined by the data collected by the positioning device, and the right side of the equation represents the speed of the preset point in the vehicle coordinate system determined by the wheel speed and the angular velocity of the device. The steering wheel and the driving wheel have a correction effect on the attitude, speed, and gyro bias, so V can be any one of V0, V1, V2 and V3.

[0164] The measured value of each parameter is expressed as the sum of the true value and the disturbance error, that is, V n =f(V n 真 , δφ, δV n ),ω v =f(ω v 真 , δε), and combined After sorting, we get

[0165]

[0166] The left side of the above equation represents the deviation between the two types of speeds, that is, the state quantity deviation between the state prediction quantity and the state quantity measurement, and the right side of the equation represents the state quantity error. The right side of the equation can be expressed as H t μ, μ is the matrix corresponding to the state error; H t is the measurement matrix, which is also the coefficient matrix of the state quantity error. t is with V n ,l, And other parameters.

[0167] When the wheel speeds of each wheel collected by the wheel speed meter are the speeds V0′, V1′, V2′ and V3′, the speed of the driving wheel can be determined according to the following formula:

[0168]

[0169] When the vehicle turns, considering the rotational angular velocity of the vehicle body, the speed of the steering wheel during the turn is calculated as follows:

[0170]

[0171] in, Indicates the direction of the vehicle. In the vehicle coordinate system, when driving forward is positive when in reverse and negative when in reverse.

[0172] In the above embodiment, the wheel speed can be any one of the four wheel speeds. When the vehicle is traveling straight, the wheel speeds of the four wheels can all be expressed in the form of V2.

[0173] In another embodiment, based on Figure 1 In the illustrated embodiment, the number of wheel speeds is multiple, and each wheel speed is the speed of a different wheel in the vehicle. For example, the number of wheel speeds may be four. Step S120, determining a first type of speed of a preset point in the vehicle at a first moment according to the wheel speed and the device angular velocity, comprises:

[0174] According to the wheel speeds of the wheels and the angular velocity of the device, first-category speeds of preset points in the vehicle at the first moment are determined respectively.

[0175] In this step, for each wheel speed, a first-class speed of a preset point in the vehicle at the first moment can be determined according to the wheel speed and the device angular velocity, and a total of multiple first-class speeds can be obtained. For example, when there are four wheel speeds, four first-class speeds of the preset point at the first moment can be calculated.

[0176] Step S140, taking the first type of speed and the second type of speed as state quantity measurement and state prediction quantity, and determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, includes at least the following two implementation methods.

[0177] Method 1: Use the matrix composed of each first-class speed as the state quantity measurement, and use the second-class speed as the state prediction quantity. According to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, determine the state quantity error of the preset point at the first moment.

[0178] Method 2: Select a target speed from each first-category speed in order as the state quantity measurement, and determine the corrected state quantity error corresponding to the target speed in the following manner. When the target speed is the last speed in each first-category speed, the corrected state quantity error corresponding to the target speed is used as the state quantity error of the preset point at the first moment:

[0179] The second type of speed is used as the state prediction quantity. According to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship between the state quantity deviation and the state quantity error established in advance, the state quantity error to be corrected is corrected to obtain the corrected state quantity error.

[0180] Among them, when the target speed is not the first first-category speed, the state quantity error to be corrected is: the corrected state quantity error corresponding to the previous first-category speed of the target speed; when the target speed is the first first-category speed, the state quantity error to be corrected is zero.

[0181] According to the state quantity deviation between the state quantity measurement and the state quantity prediction, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, the state quantity error to be corrected is corrected. When the corrected state quantity error is obtained, the corrected state quantity error can be obtained by using Formula, the state quantity deviation between the state quantity measurement and the state prediction quantity is taken as by is the state quantity error to be corrected, and the state quantity error after correction μ is determined t .

[0182] In this embodiment, an iterative method is adopted, and the state error corresponding to the previous wheel speed is used as a basis, and a new state error is obtained by measuring and updating the Kalman filter until the last wheel speed is calculated.

[0183] In summary, in this embodiment, when there are multiple wheel speeds, at least two methods can be used to determine the state error, thereby realizing data fusion between multiple wheel speeds and motion detection devices, and improving the accuracy of the state error.

[0184] Figure 3 A schematic diagram of the structure of a vehicle-mounted terminal provided in an embodiment of the present invention. Figure 1 The vehicle terminal includes: a processor 310, a wheel speed meter 320, a motion detection device 330 and a positioning device 340. The processor 310 includes: a first acquisition module, a first determination module, a second acquisition module and a second determination module. (not shown in the figure)

[0185] A first acquisition module, used to acquire the wheel speed and device angular velocity respectively collected by the wheel speed meter 320 and the motion detection device 330 at a first moment;

[0186] A first determination module, used to determine a first type of speed of a preset point in the vehicle at a first moment according to the wheel speed and the device angular velocity;

[0187] A second acquisition module, used to acquire a second type of velocity of a preset point at a first moment determined according to data collected by the positioning device 340;

[0188] A second determination module is used to use the first type of speed and the second type of speed as state quantity measurement and state prediction quantity, and determine the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship between the state quantity deviation and the state quantity error established in advance;

[0189] Among them, the state quantity deviation is the deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship is: based on the rigid body model with the same true velocity of the same particle calculated in different ways at the same time, and the relationship between the measured value, the true value and the disturbance error.

[0190] The wheel speed meter 320 is used to collect wheel speed. The motion detection device 330 is used to collect data including at least the angular velocity of the device. The positioning device 340 is used to collect positioning data.

[0191] In another embodiment of the present invention, based on Figure 3 In the illustrated embodiment, the processor 310 further includes:

[0192] A correction module (not shown in the figure) is used to correct the state quantity of the preset point at the first moment after determining the state quantity error of the preset point at the first moment, according to the state quantity error of the preset point at the first moment, so as to obtain the corrected state quantity of the preset point at the first moment; wherein the state quantity of the preset point at the first moment is: the estimated state quantity obtained after Kalman filtering.

[0193] In another embodiment of the present invention, based on Figure 3 In the illustrated embodiment, the second determination module is specifically configured to:

[0194] Obtaining the confidence of the state quantity error of the preset point at the second moment; wherein the second moment is the moment before the first moment;

[0195] According to the determined state transfer matrix, the confidence at the second moment is updated in time to obtain the prediction confidence at the first moment;

[0196] Substitute the state quantity of the preset point at the first moment into the coefficient matrix of the state quantity error in the pre-established constraint relationship to obtain the measurement matrix at the first moment; wherein the constraint relationship is: the constraint relationship between the state quantity deviation and the state quantity error;

[0197] Determine the Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment;

[0198] According to the Kalman gain at the first moment, the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship, the Kalman filter is measured and updated to obtain the state quantity error of the preset point at the first moment.

[0199] In another embodiment of the present invention, based on Figure 3 In the illustrated embodiment, the second determination module performs a time update on the confidence at the second moment according to the determined state transfer matrix to obtain the prediction confidence at the first moment, including:

[0200] according to Confidence in the second moment Σ t-1 Perform time update to obtain the prediction confidence at the first moment Among them, A t is the determined state transfer matrix, R is the confidence of the state transfer matrix, t and t-1 are the first moment and the second moment respectively, and T is the transposed symbol;

[0201] The second determination module determines the Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment, including:

[0202] according to And the measurement matrix H at the first moment t and the first moment prediction confidence Determine the Kalman gain K at the first moment t ; Where Q is the measurement noise;

[0203] The second determination module performs a measurement update of the Kalman filter according to the Kalman gain at the first moment, the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship, and obtains the state quantity error of the preset point at the first moment, including:

[0204] according to And the state quantity deviation between the state quantity measurement and the state prediction quantity is taken as Perform Kalman filter measurement update to obtain the state error μ of the preset point at the first moment t ;in, Z t预 is the state prediction quantity, Z t量 is the state quantity measurement, K t is the Kalman gain at the first moment.

[0205] In another embodiment of the present invention, based on Figure 3 In the illustrated embodiment, the processor 310 further includes a third determination module (not shown in the figure), which is configured to determine the constraint relationship between the state quantity deviation and the state quantity error by using the following operations:

[0206] The vehicle is regarded as a rigid body and the preset point is regarded as a mass point, and an equal relationship between the true value of the first type of velocity and the true value of the second type of velocity of the preset point is established;

[0207] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and the first type of speed and the second type of speed are used as the measured values, the equal relationship is disturbed and decomposed to obtain the constraint relationship between the state quantity deviation and the state quantity error.

[0208] In another embodiment of the present invention, based on Figure 3 In the illustrated embodiment, the preset point is the center point of the motion detection device; the state quantity error includes at least one of the following: position error, velocity error, attitude error, gyro bias error, and accelerometer bias error.

[0209] In another embodiment of the present invention, based on Figure 3 In the illustrated embodiment, the first determination module is specifically configured to determine the first type of speed of a preset point in the vehicle according to the wheel speed and the device angular velocity, including:

[0210] According to v1=V+ω v ×l, determine the first type velocity of the center point of the motion detection device in the vehicle;

[0211] Among them, v1 is the first type velocity of the center point in the vehicle body coordinate system, V is the wheel speed in the vehicle body coordinate system, ω v is the decomposition of the device angular velocity in the vehicle coordinate system, l is the position difference vector between the wheel and the center point corresponding to the wheel speed;

[0212] The third determination module, based on the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured value, performs disturbance decomposition on the equality relationship to obtain the constraint relationship between the state quantity deviation and the state quantity error, includes:

[0213] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first and second types of speed as the measured values, the equal relationship is disturbed and decomposed, and the following constraint relationship between the state quantity deviation and the state quantity error is obtained:

[0214]

[0215] Among them, V n is the second type velocity of the center point in the world coordinate system, is the rotation matrix between the device coordinate system where the motion detection device is located and the vehicle coordinate system. is the rotation matrix between the device coordinate system and the world coordinate system, is the rotation matrix between the vehicle coordinate system and the world coordinate system, [V n ×] is V n Transformed into an antisymmetric matrix, δφ is the attitude error, δV n is the velocity error, and δε is the gyro bias error.

[0216] In another embodiment of the present invention, based on Figure 3 In the illustrated embodiment, there are multiple wheel speeds, each wheel speed is a wheel speed of a different wheel in the vehicle; the first determination module determines the first type of speed of a preset point in the vehicle at a first moment according to the wheel speed and the device angular velocity, including:

[0217] Determine first-category velocities of preset points in the vehicle at a first moment, respectively, according to the wheel speeds of the wheels and the angular velocity of the device;

[0218] The second determination module is specifically used for:

[0219] The matrix formed by each first-class speed is used as the state quantity measurement, and the second-class speed is used as the state prediction quantity. According to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship between the state quantity deviation and the state quantity error established in advance, the state quantity error of the preset point at the first moment is determined;

[0220] or,

[0221] Select a target speed from each first-category speed in order as the state quantity measurement, and determine the corrected state quantity error corresponding to the target speed in the following manner. When the target speed is the last speed in each first-category speed, the corrected state quantity error corresponding to the target speed is used as the state quantity error of the preset point at the first moment:

[0222] The second type of speed is used as the state prediction quantity, and the state quantity error to be corrected is corrected according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship between the state quantity deviation and the state quantity error established in advance, so as to obtain the corrected state quantity error;

[0223] Among them, when the target speed is not the first first-category speed, the state quantity error to be corrected is: the corrected state quantity error corresponding to the previous first-category speed of the target speed; when the target speed is the first first-category speed, the state quantity error to be corrected is zero.

[0224] The terminal embodiment and Figure 1 The method embodiments shown are embodiments based on the same inventive concept, and relevant parts can be referenced to each other. The above terminal embodiments correspond to the method embodiments, and have the same technical effects as the method embodiments. For specific descriptions, refer to the method embodiments.

[0225] Those skilled in the art can understand that the accompanying drawings are only schematic diagrams of an embodiment, and the modules or processes in the accompanying drawings are not necessarily required to implement the present invention.

[0226] Those skilled in the art can understand that the modules in the device in the embodiment can be distributed in the device in the embodiment according to the description of the embodiment, or can be changed accordingly and located in one or more devices different from the embodiment. The modules in the above embodiment can be combined into one module, or can be further divided into multiple sub-modules.

[0227] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for determining a state quantity error, characterized in that: include: Obtaining the wheel speed and device angular velocity respectively collected by the wheel speed meter and the motion detection device at the first moment; Determine the first type of speed of the preset point in the vehicle at the first moment according to the wheel speed and the device angular velocity; wherein, according to the relative distance between the motion detection device and the preset point and the device angular velocity, determine the rotation speed of the preset point caused by the rotation of the vehicle, and according to the relative distance between the wheel and the preset point and the wheel speed, determine the translation speed of the preset point caused by the translation of the vehicle, and the composite speed of the rotation speed and the translation speed is used as the first type of speed of the preset point; Acquire a second type of speed of the preset point at the first moment determined according to data collected by the positioning device; Taking the first type of speed and the second type of speed as state quantity measurement and state prediction quantity, and determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error; Among them, the state quantity deviation is the deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship is: based on a rigid body model with the same true velocity of the same particle calculated in different ways at the same time and the relationship between the measured value, the true value and the disturbance error.

2. The method according to claim 1, characterized in that After determining the state error of the preset point at the first moment, the method further includes: According to the state quantity error of the preset point at the first moment, the state quantity of the preset point at the first moment is corrected to obtain the corrected state quantity of the preset point at the first moment; wherein the state quantity of the preset point at the first moment is: the estimated state quantity obtained after Kalman filtering.

3. The method according to claim 1 or 2, characterized in that The step of determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, comprises: Obtaining the confidence of the state quantity error of the preset point at a second moment; wherein the second moment is a moment before the first moment; According to the determined state transfer matrix, the confidence at the second moment is updated in time to obtain the prediction confidence at the first moment; Substituting the state quantity of the preset point at the first moment into the coefficient matrix of the state quantity error in the pre-established constraint relationship to obtain the measurement matrix at the first moment; wherein the constraint relationship is: the constraint relationship between the state quantity deviation and the state quantity error; Determining a Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment; According to the Kalman gain at the first moment, the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship, the Kalman filter is measured and updated to obtain the state quantity error of the preset point at the first moment.

4. The method according to claim 3, characterized in that The step of updating the confidence at the second moment in time according to the determined state transfer matrix to obtain the prediction confidence at the first moment includes: according to The confidence level for the second moment Σ t-1 Perform time update to obtain the prediction confidence at the first moment Among them, the A t is a determined state transfer matrix, R is a state transfer matrix confidence, t and t-1 are the first moment and the second moment respectively, and T is a transposed symbol; The step of determining the Kalman gain at the first moment according to the measurement matrix at the first moment and the prediction confidence at the first moment comprises: according to And the measurement matrix H at the first moment t and the prediction confidence at the first moment Determine the Kalman gain K at the first moment t ; Wherein, Q is the measurement noise; The step of performing measurement update of Kalman filtering according to the Kalman gain at the first moment, the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship to obtain the state quantity error of the preset point at the first moment includes: according to And the state quantity deviation between the state quantity measurement and the state prediction quantity is taken as Perform Kalman filter measurement update to obtain the state error μ of the preset point at the first moment t ;in, The Z t预 is the state prediction quantity, the Z t量 is the state quantity measurement, The K t is the Kalman gain at the first moment, is the state error to be corrected.

5. The method according to claim 4, characterized in that The constraint relationship between the state quantity deviation and the state quantity error is determined in the following manner: The vehicle is regarded as a rigid body, the preset point is regarded as a mass point, and an equal relationship between the first-type velocity true value and the second-type velocity true value of the preset point is established; According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured values, the equal relationship is disturbed and decomposed to obtain the constraint relationship between the state quantity deviation and the state quantity error.

6. The method according to claim 5, characterized in that The preset point is the center point of the motion detection device; the state quantity error includes at least one of the following: position error, speed error, attitude error, gyro bias error, and accelerometer bias error.

7. The method according to claim 6, characterized in that The step of determining the first type of speed of a preset point in the vehicle according to the wheel speed and the device angular velocity comprises: According to v1=V+ω v ×l, determining the first type velocity of the center point of the motion detection device in the vehicle; Wherein, v1 is the first type velocity of the center point in the vehicle body coordinate system, V is the wheel speed in the vehicle body coordinate system, and ω v is the decomposition of the device angular velocity in the vehicle body coordinate system, and l is the position difference vector between the wheel and the center point corresponding to the wheel speed; The step of performing disturbance decomposition on the equality relationship based on the relationship that the measured value is equal to the sum of the true value and the disturbance error and taking the first type of speed and the second type of speed as the measured values ​​to obtain the constraint relationship between the state quantity deviation and the state quantity error comprises: According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking the first type of speed and the second type of speed as the measured values, the equal relationship is subjected to disturbance decomposition, and the following constraint relationship between the state quantity deviation and the state quantity error is obtained: Among them, the V n is the second type of velocity of the center point in the world coordinate system, is the rotation matrix between the device coordinate system where the motion detection device is located and the vehicle body coordinate system, is the rotation matrix between the device coordinate system and the world coordinate system, is the rotation matrix between the vehicle coordinate system and the world coordinate system, [V n ×] is V n Transformed into an antisymmetric matrix, the δφ is the attitude error, the δV n is the velocity error, and δε is the gyro bias error.

8. The method according to claim 1, characterized in that The number of the wheel speeds is multiple, and each wheel speed is the speed of a different wheel in the vehicle; the step of determining the first type of speed of a preset point in the vehicle at the first moment according to the wheel speeds and the device angular velocity comprises: Determining first-category speeds of preset points in the vehicle at the first moment, respectively, according to the wheel speeds of the wheels and the angular velocity of the device; The step of using the first type of speed and the second type of speed as state quantity measurement and state prediction quantity, and determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error, comprises: Using the matrix formed by each first-type speed as a state quantity measurement, using the second-type speed as a state prediction quantity, and determining the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the pre-established constraint relationship between the state quantity deviation and the state quantity error; or, A target speed is selected from each first-category speed in order as a state quantity measurement, and a corrected state quantity error corresponding to the target speed is determined in the following manner. When the target speed is the last speed among each first-category speed, the corrected state quantity error corresponding to the target speed is used as the state quantity error of the preset point at the first moment: The second type of speed is used as a state prediction quantity, and the state quantity error to be corrected is corrected according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship between the state quantity deviation and the state quantity error established in advance, so as to obtain a corrected state quantity error; Among them, when the target speed is not the first first-category speed, the state quantity error to be corrected is: the corrected state quantity error corresponding to the previous first-category speed of the target speed; when the target speed is the first first-category speed, the state quantity error to be corrected is zero.

9. A vehicle-mounted terminal, characterized in that: include: processors, wheel speed meters, motion detection devices, and positioning devices; The processor comprises: A first acquisition module is used to acquire the wheel speed and device angular velocity respectively collected by the wheel speed meter and the motion detection device at the first moment; A first determination module, configured to determine a first type of speed of a preset point in the vehicle at the first moment according to the wheel speed and the device angular velocity; A second acquisition module, used to acquire a second type of velocity of the preset point at the first moment determined according to data collected by the positioning device; a second determination module, configured to use the first type of speed and the second type of speed as state quantity measurement and state prediction quantity, and determine the state quantity error of the preset point at the first moment according to the state quantity deviation between the state quantity measurement and the state prediction quantity, and a pre-established constraint relationship between the state quantity deviation and the state quantity error; Among them, the state quantity deviation is the deviation between the state quantity measurement and the state prediction quantity, and the constraint relationship is: based on a rigid body model with the same true velocity of the same particle calculated in different ways at the same time and the relationship between the measured value, the true value and the disturbance error.

10. The vehicle-mounted terminal according to claim 9, characterized in that: The processor further comprises: A correction module is used to correct the state quantity of the preset point at the first moment after determining the state quantity error of the preset point at the first moment, according to the state quantity error of the preset point at the first moment, to obtain the corrected state quantity of the preset point at the first moment; wherein the state quantity of the preset point at the first moment is: the estimated state quantity obtained after Kalman filtering.

Images