A state quantity error determination method based on wheel speed fusion and vehicle-mounted terminal

Abstract

The embodiment of the present invention discloses a state quantity error determination method based on wheel speed fusion and a vehicle-mounted terminal. The method includes: obtaining multiple wheel speeds collected by a wheel speed meter and a device angular velocity collected by a motion detection device; determining multiple preset point speeds of preset points in the vehicle according to the combination of multiple wheel speeds and device angular velocity; selecting two speeds from each preset point speed as state quantity measurement and state prediction quantity respectively; determining the state quantity error of the preset point 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; the constraint relationship is obtained based on a rigid body model with the same true speed of the same particle calculated by the wheel speeds of different wheels at the same time and the relationship between the measured value, the true value and the disturbance error. By 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 based on wheel speed fusion 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 based on wheel speed fusion and a vehicle-mounted terminal to determine the state quantity error of a preset point in the 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 based on wheel speed fusion, comprising:

[0006] Obtaining wheel speeds of multiple wheels collected by a wheel speed meter at a first moment, and a device angular velocity collected by a motion detection device at the first moment;

[0007] For each wheel speed, a preset point speed of a preset point in the vehicle is determined according to the wheel speed and the device angular velocity, and a plurality of preset point speeds are obtained for a plurality of wheel speeds;

[0008] Select two speeds from the preset point speeds as state quantity measurement and state prediction quantity respectively;

[0009] 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 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 the rigid body model with the same true speed of the same particle calculated based on the wheel speed of different wheels 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 state transfer matrix at the first moment 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 the state transfer matrix at the first moment, R is the confidence of the state equation, t and t-1 are the first moment and the second moment respectively, and T is the 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 real values ​​of the speeds of two different preset points 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 two different preset points 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.

[0028] Optionally, the preset point is the center point of the motion detection device; the state quantity error includes: gyro bias error.

[0029] Optionally, the step of determining a preset point speed of a preset point in the vehicle according to the wheel speed and the device angular velocity for each wheel speed, and obtaining multiple preset point speeds for multiple wheel speeds includes:

[0030] For each wheel speed, according to v1 = V + ω v ×l, determining a preset point speed of the center point of the motion detection device in the vehicle;

[0031] Wherein, v1 is the preset point speed of the center point in the vehicle body coordinate system, V is the wheel speed of a wheel 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 two different preset points 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 two different preset points are used 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, V0 and V1 are two different wheel speeds in the vehicle body coordinate system. 01 is the position difference vector between the two wheels, the δ ε is the gyro bias error.

[0036] Optionally, the step of selecting two speeds from the preset point speeds as the state quantity measurement and the state prediction quantity respectively includes:

[0037] Select two speeds from the speeds of each preset point in turn as state quantity measurement and state prediction quantity, respectively, to obtain multiple groups of state quantity measurement and state prediction quantity;

[0038] 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:

[0039] A target group of state quantity measurements and state prediction quantities is selected from a plurality of groups of state quantity measurements and state prediction quantities in order, and a corrected state quantity error corresponding to the target group is determined in the following manner; when the target group is the last group in each group of state quantity measurements and state prediction quantities, the corrected state quantity error corresponding to the target group is used as the state quantity error of the preset point at the first moment:

[0040] 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 state quantity error to be corrected is corrected to obtain a corrected state quantity error;

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

[0042] In a second aspect, an embodiment of the present invention discloses a vehicle-mounted terminal, comprising: a processor, a wheel speed meter and a motion detection device; wherein the processor comprises:

[0043] An acquisition module, used to acquire the wheel speeds of multiple wheels collected by the wheel speed meter at a first moment, and the device angular velocity collected by the motion detection device at the first moment;

[0044] A first determination module is used to determine a preset point speed of a preset point in the vehicle according to the wheel speed and the device angular velocity for each wheel speed, and obtain multiple preset point speeds for multiple wheel speeds;

[0045] A selection module, used for selecting two speeds from various preset point speeds as state quantity measurement and state prediction quantity respectively;

[0046] A second determination module is used to 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 pre-established constraint relationship between the state quantity deviation and the state quantity error;

[0047] 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 speed of the same particle calculated based on the wheel speed of different wheels at the same time, and the relationship between the measured value, the true value and the disturbance error.

[0048] Optionally, the processor further includes:

[0049] 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.

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

[0051] 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;

[0052] 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;

[0053] 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;

[0054] 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;

[0055] 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.

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

[0057] 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 the state transfer matrix at the first moment, R is the confidence of the state equation, t and t-1 are the first moment and the second moment respectively, and T is the transposed symbol;

[0058] 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:

[0059] 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;

[0060] 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:

[0061] 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.

[0062] 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:

[0063] The vehicle is regarded as a rigid body, the preset point is regarded as a mass point, and an equal relationship between the real values ​​of the speeds of two different preset points of the preset point is established;

[0064] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and two different preset points 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.

[0065] Optionally, the preset point is the center point of the motion detection device; the state quantity error includes: gyro bias error.

[0066] Optionally, the first determining module is specifically configured to:

[0067] For each wheel speed, according to v1 = V + ω v ×l, determining a preset point speed of the center point of the motion detection device in the vehicle;

[0068] Wherein, v1 is the preset point speed of the center point in the vehicle body coordinate system, V is the wheel speed of a wheel in the vehicle body coordinate system, and ω vis 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;

[0069] 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 two different preset points 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:

[0070] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and two different preset points are used 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:

[0071]

[0072] Among them, V0 and V1 are two different wheel speeds in the vehicle body coordinate system. 01 is the position difference vector between the two wheels, the δ ε is the gyro bias error.

[0073] Optionally, the selection module is specifically used to:

[0074] Select two speeds from the speeds of each preset point in turn as state quantity measurement and state prediction quantity, respectively, to obtain multiple groups of state quantity measurement and state prediction quantity;

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

[0076] Select a target group of state quantity measurements and state prediction quantities from multiple groups of state quantity measurements and state prediction quantities in order, determine a corrected state quantity error corresponding to the target group according to the following operations, and when the target group is the last group in each group of state quantity measurements and state prediction quantities, use the corrected state quantity error corresponding to the target group as the state quantity error of the preset point at the first moment:

[0077] 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 state quantity error to be corrected is corrected to obtain a corrected state quantity error;

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

[0079] It can be seen from the above content that the state quantity error determination method based on wheel speed fusion and the vehicle-mounted terminal provided in the embodiments of the present invention can determine multiple preset point speeds of preset points in the vehicle according to the combination of multiple wheel speeds and the angular velocity of the device, select two speeds from each preset point speed as the state quantity measurement and state prediction quantity, and determine the state quantity error of the preset point according to the state quantity deviation and the pre-established constraint relationship. Therefore, the embodiments 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.

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

[0081] 1. The true speed of the same particle calculated by the wheel speeds of different wheels should be the same. The speed of each preset point 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 at different wheel speeds and the relationship between the measured value, the true value and the disturbance error, a constraint relationship between the speeds of the two preset points can be established, and the state error of the preset point can be determined based on the constraint relationship and the state deviation.

[0082] 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.

[0083] 3. Select two speeds from multiple preset point speeds in turn as state quantity measurements and state prediction quantities to obtain multiple groups of state quantity measurements and state prediction quantities. Based on the state quantity error determined according to the previous group of data, different groups are used in turn to calculate the state quantity error to achieve data fusion between multiple wheel speeds and motion detection devices, which can improve the accuracy of the state quantity error. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] 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.

[0085] Figure 1 A schematic flow chart of a method for determining a state quantity error based on wheel speed fusion provided by an embodiment of the present invention;

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

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

[0088] 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.

[0089] 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.

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

[0091] Figure 1 A flow chart of a state quantity error determination method based on wheel speed fusion 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, or may be an on-board computer or an on-board industrial control computer (IPC) or other on-board terminal. The vehicle in this embodiment refers to an intelligent vehicle. An on-board terminal may be installed in the vehicle. The on-board 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 (IMU). The method specifically includes the following steps.

[0092] S110: Obtain wheel speeds of multiple wheels collected by a wheel speed meter at a first moment, and a device angular velocity collected by a motion detection device at the first moment.

[0093] In this embodiment, the wheel speeds of more than one or less than four wheels may be obtained. Each wheel corresponds to a wheel speed.

[0094] 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.

[0095] This embodiment can be executed periodically according to a preset frequency. When obtaining the wheel speeds of multiple wheels at the first moment, it can be obtained from the wheel speeds of multiple wheels saved at various moments. When obtaining the device angular velocity at the first moment, it can be obtained from the device angular velocity saved at various moments.

[0096] S120: For each wheel speed, determine a preset point speed of a preset point in the vehicle according to the wheel speed and the device angular velocity, and obtain multiple preset point speeds for multiple wheel speeds.

[0097] The preset point may be the center point of the motion detection device. The state quantity error may include at least one of the following: gyro bias error. For example, the preset point may be the center point of the IMU, and the state quantity error may include the gyro bias error of the IMU, where the gyro bias is a state quantity. The gyro bias is an important indicator of gyroscope accuracy and is used to correct angular velocity.

[0098] 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.

[0099] For each wheel speed, when determining the speed of a preset point in the vehicle at a preset point, 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 preset point speed.

[0100] The wheel speed of each wheel is respectively combined with the angular velocity of the device to determine multiple preset point speeds of the preset point.

[0101] S130: Select two speeds from the preset point speeds as state quantity measurement and state prediction quantity respectively.

[0102] When selecting the speed, you can select two wheel speeds arbitrarily, or you can select two adjacent wheel speeds according to the adjacent rule.

[0103] S140: 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, and the pre-established constraint relationship between the state quantity deviation and the state quantity error.

[0104] 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 speed of the same particle calculated based on the wheel speed of different wheels at the same time, and the relationship between the measured value, the true value and the disturbance error.

[0105] Specifically, the measured value can be equal to the true value plus the disturbance error, and the two preset point speeds are regarded as measured values, and the true values ​​contained in the two preset point speeds should be equal. According to the above relationship, the constraint relationship between the state quantity deviation and the state quantity error can be obtained.

[0106] As can be seen from the above content, this embodiment can determine multiple preset point speeds of preset points in the vehicle based on the combination of multiple wheel speeds and the angular velocity of the device, select two speeds from each preset point speed as state quantity measurement and state prediction, and determine the state quantity error of the preset point based on the state quantity deviation and the pre-established constraint relationship. Therefore, this embodiment can determine the state quantity error of the preset point in the vehicle and correct the state quantity data.

[0107] 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.

[0108] This embodiment fuses the data of the wheel speed meter, motion detection equipment and other sensors 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.

[0109] In another embodiment of the present invention, based on Figure 1 The illustrated embodiment, after determining the state error of the preset point at the first moment, further includes:

[0110] 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.

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

[0112] 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.

[0113] 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.

[0114] 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.

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

[0116] 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.

[0117] 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.

[0118] 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.

[0119] 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.

[0120] 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.

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

[0122] 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.

[0123] Step 5a: 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 measurement update is performed to obtain the state quantity error of the preset point at the first moment.

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

[0125] 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.

[0126] 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.

[0127] In another embodiment of the present invention, based on the above embodiment, step 2a, the step of updating the confidence at the second moment according to the state transfer matrix at the first moment to obtain the prediction confidence at the first moment may include:

[0128] according to The confidence level ∑ for the second moment t-1 Perform time update to obtain the prediction confidence at the first moment

[0129] Among them, A t is the state transfer matrix at the first moment, R is the confidence of the state equation, t and t-1 are the first moment and the second moment respectively, and T is the transposed symbol. The confidence of the state transfer matrix 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.

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

[0131] Step 4a, 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:

[0132] 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 .

[0133] 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.

[0134] Step 5a, based on 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, including:

[0135] 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 .

[0136] 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.

[0137] 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 may include:

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

[0139] 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.

[0140] 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.

[0141] 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.

[0142] Step 1b: Consider the vehicle as a rigid body and the preset point as a mass point, and establish an equal relationship between the actual values ​​of the velocities of two different preset points.

[0143] In this embodiment, the two different preset point speeds may be equal to the sum of the true value and the corresponding disturbance error, and the two different preset point speeds may be regarded as measured values, which are values ​​with errors.

[0144] 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 two different preset points as the measured values, the above equality relationship is subjected to disturbance decomposition to obtain the constraint relationship between the state quantity deviation and the state quantity error.

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

[0146]

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

[0148] 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.

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

[0150] In this embodiment, the preset point is the center point of the motion detection device, and the state quantity error includes a gyro bias error.

[0151] Step S120, i.e., for each wheel speed, determining a preset point speed of a preset point in the vehicle according to the wheel speed and the device angular velocity, and the step of obtaining multiple preset point speeds for multiple wheel speeds may include:

[0152] For each wheel speed, according to v1 = V + ω v ×l, determine a preset point speed of the center point of the motion detection device in the vehicle.

[0153] Wherein, v1 is the preset point speed of the center point of the motion detection device 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, and l is the position difference vector between the wheel and the center point corresponding to the wheel speed.

[0154] Step 1b, i.e., based on the relationship that the measured value is equal to the sum of the true value and the disturbance error, and taking two different preset points as the measured values, performing disturbance decomposition on the equality relationship to obtain the constraint relationship between the state quantity deviation and the state quantity error, comprises:

[0155] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and two different preset points are used as the measured values, the above equality relationship is subjected to disturbance decomposition, and the following constraint relationship between the state quantity deviation and the state quantity error is obtained:

[0156]

[0157] Among them, V0 and V1 are two different wheel speeds in the vehicle body coordinate system, l 01 is the position difference vector between the two wheels, δ ε is the gyro bias error. This embodiment can be regarded as a wheel speed differential model.

[0158] Taking the driving wheel as an example, when the vehicle is in a straight line, the speed of the driving wheel is consistent, the difference between the wheel speeds of the two wheels is 0, and the azimuth information of the vehicle is kept constant. When the vehicle turns, the speed difference between the two wheels changes, and accordingly, the azimuth of the vehicle also changes. Therefore, it can be obtained that there is a direct correlation between the wheel speed difference of the wheel speed meter and the azimuth change information of the vehicle. The wheel speed differential model of this embodiment is only related to the gyro bias error, and the correction of the gyro bias is more direct.

[0159] Combine the following Figure 2 The schematic diagram of the vehicle motion model shown in FIG. 1 illustrates the process of establishing the above constraint relationship. For example, the vehicle turns left around point r, where r is the rotation center. The wheel speeds of each wheel of the vehicle are respectively V 00 、V 01 、V 02 and V 03 Indicates that the direction is Figure 2 As 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.

[0160] The speed of each preset point is expressed as the sum of the true value and the disturbance error, that is, ω v =f(ω v 真 , δ ε ), after sorting, we get

[0161]

[0162] The left side of the above equation represents the deviation of the speed of the two preset points, 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 l 01 , And other parameters.

[0163] 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:

[0164]

[0165] 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:

[0166]

[0167] 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.

[0168] When the vehicle is moving straight, the wheel speeds of the four wheels can all adopt the above V 02 expressed in the form of .

[0169] In another embodiment of the present invention, based on the above embodiment, step S130, the step of selecting two speeds from the preset point speeds as the state quantity measurement and the state prediction quantity respectively, may include:

[0170] Two speeds are selected in turn from the speeds of each preset point as state quantity measurement and state prediction quantity respectively, to obtain multiple groups of state quantity measurement and state prediction quantity.

[0171] For example, for the four preset point speeds v0, v1, v2, and v3 corresponding to the four wheels 0, 1, 2, and 3, the following two speeds are used as a set of state quantity measurements and state prediction quantities:

[0172] v0 and v1, v1 and v2, v2 and v3, v3 and v0.

[0173] Step S140, 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, may include:

[0174] A target group of state quantity measurements and state prediction quantities is selected from multiple groups of state quantity measurements and state prediction quantities in order, and a corrected state quantity error corresponding to the target group is determined in the following manner. When the target group is the last group in each group of state quantity measurements and state prediction quantities, the corrected state quantity error corresponding to the target group is used as the state quantity error of the preset point at the first moment:

[0175] 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 state quantity error to be corrected is corrected to obtain the corrected state quantity error.

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

[0177] 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 .

[0178] In this embodiment, two speeds are selected in turn from multiple preset point speeds as state quantity measurements and state prediction quantities to obtain multiple groups of state quantity measurements and state prediction quantities. An iterative method is adopted to calculate the state quantity error in turn based on the state quantity error determined according to the previous group of data, thereby realizing data fusion between multiple wheel speeds and motion detection devices, which can improve the accuracy of the state quantity error.

[0179] 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 and a motion detection device 330; wherein the processor 310 includes: an acquisition module, a first determination module, a selection module and a second determination module (not shown in the figure).

[0180] An acquisition module, used to acquire the wheel speeds of the multiple wheels acquired by the wheel speed meter 320 at the first moment, and the device angular velocity acquired by the motion detection device 330 at the first moment;

[0181] A first determination module is used to determine a preset point speed of a preset point in the vehicle according to the wheel speed and the device angular velocity for each wheel speed, and obtain multiple preset point speeds for multiple wheel speeds;

[0182] A selection module, used for selecting two speeds from various preset point speeds as state quantity measurement and state prediction quantity respectively;

[0183] A second determination module is used to 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 pre-established constraint relationship between the state quantity deviation and the state quantity error;

[0184] 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 speed of the same particle calculated based on the wheel speed of different wheels at the same time, and the relationship between the measured value, the true value and the disturbance error.

[0185] The wheel speed meter 320 is used to collect the wheel speed of each wheel, and the motion detection device 330 is used to collect data including at least the angular velocity of the device.

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

[0187] 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.

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

[0189] 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;

[0190] 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;

[0191] 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;

[0192] 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;

[0193] 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.

[0194] 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 state transfer matrix at the first moment to obtain the prediction confidence at the first moment, including:

[0195] 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 state transfer matrix at the first moment, R is the confidence of the state equation, t and t-1 are the first moment and the second moment respectively, and T is the transposition symbol;

[0196] 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:

[0197] 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;

[0198] 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:

[0199] 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.

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

[0201] The vehicle is regarded as a rigid body, the preset point is regarded as a mass point, and the equal relationship between the real values ​​of the speed of two different preset points is established;

[0202] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and two different preset points 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.

[0203] 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: gyro zero bias error.

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

[0205] For each wheel speed, according to v1 = V + ω v ×l, determine a preset point speed of the center point of the motion detection device in the vehicle;

[0206] Among them, v1 is the preset point speed of the center point in the vehicle body coordinate system, V is the wheel speed of a wheel 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;

[0207] 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 two different preset points 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:

[0208] According to the relationship that the measured value is equal to the sum of the true value and the disturbance error, and two different preset points are used 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:

[0209]

[0210] Among them, V0 and V1 are two different wheel speeds in the vehicle body coordinate system, l 01 is the position difference vector between the two wheels, δ ε is the gyro bias error.

[0211] In another embodiment of the present invention, based on Figure 3In the embodiment shown, the selection module is specifically used for:

[0212] Select two speeds from the speeds of each preset point in turn as state quantity measurement and state prediction quantity, respectively, to obtain multiple groups of state quantity measurement and state prediction quantity;

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

[0214] A target group of state quantity measurements and state prediction quantities is selected from multiple groups of state quantity measurements and state prediction quantities in order, and a corrected state quantity error corresponding to the target group is determined in the following manner. When the target group is the last group in each group of state quantity measurements and state prediction quantities, the corrected state quantity error corresponding to the target group is used as the state quantity error of the preset point at the first moment:

[0215] 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;

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

[0217] 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.

[0218] 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.

[0219] 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.

[0220] 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 state error based on wheel speed fusion, characterized in that: include: Obtaining wheel speeds of multiple wheels collected by a wheel speed meter at a first moment, and a device angular velocity collected by a motion detection device at the first moment; For each wheel speed, a preset point speed of a preset point in the vehicle is determined according to the wheel speed and the device angular velocity, and multiple preset point speeds are obtained for multiple wheel speeds; each wheel speed is respectively combined with the device angular velocity to determine multiple preset point speeds of the preset point; Select two speeds from the preset point speeds as state quantity measurement and state prediction quantity respectively; 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 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 the rigid body model with the same true speed of the same particle calculated based on the wheel speed of different wheels 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 the state transfer matrix at the first moment, R is the confidence of the state equation, t and t-1 are the first moment and the second moment respectively, and T is the 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 real values ​​of the speeds of two different preset points 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 two different preset points 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.

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: gyro bias error.

7. The method according to claim 6, characterized in that The step of determining a preset point speed of a preset point in the vehicle according to the wheel speed and the device angular velocity for each wheel speed, and obtaining a plurality of preset point speeds for a plurality of wheel speeds comprises: For each wheel speed, according to v1=v+ω v ×l, determining a preset point speed of the center point of the motion detection device in the vehicle; Wherein, v1 is the preset point speed of the center point in the vehicle body coordinate system, V is the wheel speed of a wheel 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 two different preset points 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 two different preset points are used 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, V0 and V1 are two different wheel speeds in the vehicle body coordinate system. 01 is the position difference vector between the two wheels, and δε is the gyro bias error.

8. The method according to claim 1, characterized in that The step of selecting two speeds from the preset point speeds as the state quantity measurement and the state prediction quantity respectively comprises: Select two speeds from the speeds of each preset point in turn as state quantity measurement and state prediction quantity, respectively, to obtain multiple groups of state quantity measurement and state prediction quantity; 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: A target group of state quantity measurements and state prediction quantities is selected from a plurality of groups of state quantity measurements and state prediction quantities in order, and a corrected state quantity error corresponding to the target group is determined in the following manner; when the target group is the last group in each group of state quantity measurements and state prediction quantities, the corrected state quantity error corresponding to the target group is used as 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 state quantity error to be corrected is corrected to obtain a corrected state quantity error; Among them, when the target group is not the first group, the state quantity error to be corrected is: the corrected state quantity error corresponding to the previous group of the target group; when the target group is the first group, the state quantity error to be corrected is zero.

9. A vehicle-mounted terminal, characterized in that: include: A processor, a wheel speed meter and a motion detection device; wherein the processor includes: An acquisition module, used to acquire the wheel speeds of multiple wheels collected by the wheel speed meter at a first moment, and the device angular velocity collected by the motion detection device at the first moment; A first determination module is used to determine a preset point speed of a preset point in the vehicle according to the wheel speed and the device angular velocity for each wheel speed, and obtain multiple preset point speeds for multiple wheel speeds; each wheel speed is respectively combined with the device angular velocity to determine the multiple preset point speeds of the preset point; A selection module, used for selecting two speeds from various preset point speeds as state quantity measurement and state prediction quantity respectively; A second determination module is used to 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 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 the rigid body model with the same true speed of the same particle calculated based on the wheel speed of different wheels 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