Vehicle state determination method, apparatus, device, medium, and product

Abstract

The application discloses a vehicle state determination method, device, equipment, medium and product. The method comprises the following steps: acquiring driving data of a target vehicle, wherein the driving data comprises a longitudinal speed, a front wheel steering angle and inertial navigation system (IMU) data; determining a lateral speed of the target vehicle based on the driving data through a first Kalman filter; and determining a three-dimensional speed of the target vehicle based on the lateral speed, the longitudinal speed and the IMU data through a second Kalman filter. In this way, the longitudinal speed and the lateral speed of the target vehicle can be accurately obtained, so that the positioning accuracy is improved.

Description

Technical Field

[0001] This application belongs to the field of autonomous driving technology, and in particular relates to a method, device, equipment, medium and product for determining vehicle state. Background Technology

[0002] With the rapid development of autonomous driving technology, the functional modules within autonomous driving systems are becoming increasingly diverse, such as control modules, perception modules, and positioning modules. Fully utilizing the information from the sensors in each module can improve the system's reliability and versatility. The minimum system of the positioning module consists of an inertial measurement unit (IMU) and a wheel speed-based trajectory calculation module, which ensures stable vehicle speed output.

[0003] Currently, in traditional IMU and wheel speed fusion positioning technology, because the three-dimensional velocity of the vehicle cannot be determined, it can only assume that the lateral and vertical velocities of the vehicle are zero, and positioning is based solely on the vehicle's wheel speed, i.e., longitudinal velocity. However, in reality, the lateral velocity of a vehicle is not necessarily zero; for example, when a vehicle is turning, the lateral velocity is not zero. Therefore, traditional IMU and wheel speed fusion positioning technology suffers from inconsistencies between assumptions and reality, leading to inaccurate positioning. Summary of the Invention

[0004] This application provides a vehicle state determination method, apparatus, device, medium, and product that can accurately obtain the longitudinal and lateral speeds of a target vehicle, thereby improving positioning accuracy.

[0005] In a first aspect, embodiments of this application provide a method for determining the state of a vehicle, the method comprising:

[0006] Acquire the target vehicle's driving data, including longitudinal speed, front wheel steering angle, and inertial navigation system (IMU) data;

[0007] The lateral velocity of the target vehicle is determined based on the driving data using the first Kalman filter.

[0008] The three-dimensional velocity of the target vehicle is determined based on lateral velocity, longitudinal velocity, and IMU data using a second Kalman filter.

[0009] Secondly, embodiments of this application provide a vehicle state determination device, the device comprising:

[0010] The first acquisition module is used to acquire the driving data of the target vehicle, including longitudinal speed, front wheel steering angle and inertial navigation system (IMU) data;

[0011] The first determining module is used to determine the lateral speed of the target vehicle based on driving data through a first Kalman filter;

[0012] The second determination module is used to determine the three-dimensional velocity of the target vehicle based on the lateral velocity, longitudinal velocity, and IMU data using a second Kalman filter.

[0013] Thirdly, embodiments of this application provide an electronic device, the device including: a processor and a memory storing computer program instructions;

[0014] When the processor executes computer program instructions, it implements the vehicle state determination method as shown in any embodiment of the first aspect.

[0015] Fourthly, embodiments of this application provide a computer storage medium storing computer program instructions, which, when executed by a processor, implement the vehicle state determination method shown in any embodiment of the first aspect.

[0016] Fifthly, embodiments of this application provide a computer program product in which instructions, when executed by a processor of an electronic device, cause the electronic device to perform the vehicle state determination method shown in any embodiment of the first aspect.

[0017] The vehicle state determination method, apparatus, device, medium, and product of this application embodiment can acquire the driving data of a target vehicle, which includes longitudinal speed, front wheel steering angle, and IMU data. Then, through a first Kalman filter, the lateral speed of the target vehicle is determined based on the driving data. Then, through a second Kalman filter, the three-dimensional speed of the target vehicle can be determined based on the lateral speed, longitudinal speed, and IMU data. In this way, the longitudinal speed and lateral speed included in the three-dimensional speed of the target vehicle can be accurately obtained without assuming the lateral speed to be 0. Positioning based on accurate longitudinal speed and lateral speed can improve the accuracy of positioning. Attached Figure Description

[0018] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments of this application will be briefly introduced below. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0019] Figure 1 This is a flowchart of a vehicle state determination method provided in one embodiment of this application;

[0020] Figure 2 This is a schematic diagram of a single-vehicle model provided in one embodiment of this application;

[0021] Figure 3This is a schematic diagram of a global coordinate system and a carrier coordinate system provided in one embodiment of this application;

[0022] Figure 4 This is a schematic diagram of the structure of a vehicle status determination device provided in one embodiment of this application;

[0023] Figure 5 This is a schematic diagram of the structure of an electronic device provided in one embodiment of this application. Detailed Implementation

[0024] The features and exemplary embodiments of various aspects of this application will be described in detail below. To make the objectives, technical solutions, and advantages of this application clearer, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only intended to explain this application and not to limit it. For those skilled in the art, this application can be implemented without some of these specific details. The following description of the embodiments is merely to provide a better understanding of this application by illustrating examples.

[0025] It should be noted that, in this document, relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising..." does not exclude the presence of additional identical elements in the process, method, article, or apparatus that includes said element.

[0026] As the background technology suggests, with the rapid development of the autonomous driving industry, the functional modules within autonomous driving systems are becoming increasingly diverse, such as control, perception, and positioning modules. Fully utilizing the information from the sensors in each module can improve the system's reliability and versatility. Among these, the minimum system of the positioning module is a trajectory estimation module combining an IMU (Integrated Measurement Unit) and wheel speed, ensuring stable vehicle speed output. Currently, in traditional IMU-wheel speed fusion positioning, wheel speed only provides one dimension of longitudinal velocity information, while the other two dimensions are considered zero. This assumption holds true when the vehicle is traveling in a straight line, but when the vehicle is turning, the lateral velocity is not zero, thus causing a discrepancy between the assumption and reality, leading to inaccurate positioning.

[0027] This application provides a vehicle state determination method, apparatus, device, medium, and product that can acquire the driving data of a target vehicle, including longitudinal speed, front wheel steering angle, and IMU data. Then, a first Kalman filter is used to determine the lateral speed of the target vehicle based on the driving data. A second Kalman filter is used to determine the three-dimensional speed of the target vehicle based on the lateral speed, longitudinal speed, and IMU data. In this way, the longitudinal speed and lateral speed included in the three-dimensional speed of the target vehicle can be accurately obtained without assuming the lateral speed to be 0. Positioning based on accurate longitudinal speed and lateral speed can improve the accuracy of positioning.

[0028] Figure 1 The diagram illustrates a flowchart of a vehicle state determination method according to an embodiment of this application. It should be noted that the executing entity of this vehicle state determination method can be a vehicle state determination device, such as... Figure 1 As shown, the vehicle status determination method may include the following steps:

[0029] S110, acquire the driving data of the target vehicle;

[0030] S120, using the first Kalman filter, determines the lateral speed of the target vehicle based on driving data;

[0031] S130 determines the three-dimensional velocity of the target vehicle based on lateral velocity, longitudinal velocity, and IMU data using a second Kalman filter.

[0032] Therefore, the driving data of the target vehicle can be obtained, including longitudinal speed, front wheel steering angle, and IMU data. Then, the lateral speed of the target vehicle is determined based on the driving data by a first Kalman filter. Then, the three-dimensional speed of the target vehicle can be determined based on the lateral speed, longitudinal speed, and IMU data by a second Kalman filter. In this way, the longitudinal speed and lateral speed included in the three-dimensional speed of the target vehicle can be accurately obtained without assuming the lateral speed to be 0. The positioning accuracy can be improved by using accurate longitudinal speed and lateral speed for positioning.

[0033] Regarding S110, driving data may include longitudinal velocity, front wheel steering angle, and IMU data. IMU data may include triaxial acceleration and triaxial angular velocity.

[0034] Regarding S120, the first Kalman filter can be a Kalman filter based on dynamic constraints. By inputting longitudinal velocity, front wheel steering angle, and IMU data into the first Kalman filter, the lateral velocity of the target vehicle can be estimated.

[0035] For example, after the vehicle state determination device is initialized, the longitudinal speed, front wheel steering angle and IMU data output in real time by the autonomous driving system of the target vehicle can be transmitted to the single-vehicle model observer of the target vehicle. After the single-vehicle model observer completes the parameter initialization, it performs real-time lateral speed estimation based on the first Kalman filter to obtain the lateral speed of the target vehicle.

[0036] In some implementations, to more accurately estimate the lateral velocity of the target vehicle, the method may further include, prior to S120:

[0037] Using the sideslip angle, lateral velocity, and steering angular velocity at the center of mass of the target vehicle as the system state of the first Kalman filter, the differential equations for the sideslip angle, lateral velocity, and steering angular velocity at the center of mass of the target vehicle are determined respectively.

[0038] Based on the differential equations of the sideslip angle, lateral velocity, and steering angular velocity at the center of mass of the target vehicle, the system state equation of the first Kalman filter is constructed. With the product of the longitudinal velocity and the sideslip angle at the center of mass and the Z-axis angular velocity of the IMU as the constraint observations, the observation equation of the first Kalman filter is constructed based on the system state of the first Kalman filter, thus obtaining the first Kalman filter.

[0039] Here, it is possible to... Figure 2 As shown, the parameters of the first Kalman filter can be based on a single-vehicle model. The sideslip angle β at the center of mass and the lateral velocity v are used as parameters. y The steering angular velocity r is the system state x(t) of the first Kalman filter. The sideslip angle β and lateral velocity v at the centroid are determined respectively. y The differential equation for the steering angular velocity r is as follows:

[0040]

[0041]

[0042]

[0043] Where β can be the sideslip angle at the centroid. It can be the differential of the sideslip angle at the center of mass, v y It can be lateral velocity. It can be the derivative of the lateral velocity, and r can be the steering angular velocity. It can be the derivative of the steering angular velocity, C f This can be the front wheel lateral stiffness, C r It can be the rear wheel lateral stiffness, I f It can be the distance from the vehicle's center of gravity to the front axle, I r This can be the distance from the vehicle's center of gravity to the rear axle, where m can be the vehicle's mass, and I... zThis can be the vehicle's moment of inertia about the Z-axis, v. x It can be the longitudinal velocity, δ f It can be the front wheel steering angle, and w can be the quantization noise of the front wheel steering angle.

[0044] It should be noted that C f C r I f I r m and I z These parameters can be provided by the manufacturer, and they remain fixed for the same vehicle.

[0045] also, Figure 2 The distance between the front and rear wheels, L (L = L), is also shown. f +L r Front wheel lateral force F yf Rear wheel lateral force F yr Lateral force can be the cause of lateral velocity in a vehicle, F yr =C r *The pressure of the tire on the ground and the lateral force can be intermediate quantities in the derivation of the state equation of the first Kalman filter system.

[0046] Then, based on the differential equations of the sideslip angle, lateral velocity, and steering angular velocity at the centroid mentioned above, the system state equation of the first Kalman filter can be constructed.

[0047] In some implementations, the system state equation of the first Kalman filter can be:

[0048]

[0049] Where x(t) can be the system state of the first Kalman filter. Let u(t) be the differential of the system state of the first Kalman filter, u(t) be the system input of the first Kalman filter, and w(t) be the system noise of the first Kalman filter. This can be the system state matrix of the first Kalman filter. This can be the system control matrix of the first Kalman filter. The system noise matrix of the first Kalman filter can be C. f This can be the front wheel lateral stiffness, C r It can be the rear wheel lateral stiffness, I f It can be the distance from the vehicle's center of gravity to the front axle, I r This can be the distance from the vehicle's center of gravity to the rear axle, where m can be the vehicle's mass, and I... z This can be the vehicle's moment of inertia about the Z-axis, v. x It can be the longitudinal velocity.

[0050] It should be noted that u(t) can be equal to the front wheel steering angle δ. f w(t) can be equal to the quantization noise w of the front wheel steering angle.

[0051] Furthermore, during vehicle operation, the steering angular velocity r and the Z-axis angular velocity ω of the IMU... z To maintain consistency, with a smaller sideslip angle β at the center of mass, the longitudinal velocity v x The product of v and the sideslip angle β at the center of mass x β and lateral velocity v y To maintain consistency, the constrained observation z(t) = [v] can be constructed based on this relationship. x βω z ] T And with z(t) = [v x βω z ] T To constrain the observations, the observation equations of the first Kalman filter are constructed by combining the system state x(t) of the first Kalman filter.

[0052] In some implementations, the observation equation of the first Kalman filter can be:

[0053] z(t)=H1(t)x(t)+v1(t)

[0054] Where z(t) can be the constraint observation of the first Kalman filter, H1(t) = [0 v x [1] can be the observation matrix of the first Kalman filter, and v1(t) can be the observation noise of the first Kalman filter.

[0055] It should be noted that v1(t) can be determined statistically in advance. The specific method of determination is a conventional technique in this field and will not be elaborated here.

[0056] Thus, through the above process, a Kalman filter based on dynamic constraints, namely the first Kalman filter, can be designed, which can then be used to more accurately estimate the lateral velocity of the target vehicle.

[0057] Regarding S130, the second Kalman filter can be a fully constrained Kalman filter assisted by speed information. This speed information can include longitudinal speed, lateral speed, vertical speed, and IMU data. The longitudinal speed (i.e., wheel speed) and IMU data can be acquired from the autonomous driving system, the lateral speed can be estimated using the first Kalman filter, and the vertical speed can be assumed to be 0. Inputting the lateral speed, longitudinal speed, and IMU data into the second Kalman filter allows for further estimation of the target vehicle's three-dimensional speed, namely, the longitudinal speed, the first lateral speed, and the vertical speed. The first lateral speed can be more accurate than the lateral speed estimated using the first Kalman filter.

[0058] For example, after obtaining the lateral velocity through the single-vehicle model observer, the longitudinal velocity, lateral velocity, and IMU data can be passed to the error kinematics model observer. After the error kinematics model observer completes parameter initialization, it can perform real-time three-dimensional velocity estimation based on the second Kalman filter to obtain the three-dimensional velocity of the target vehicle.

[0059] In some implementations, to more accurately estimate the three-dimensional velocity of the target vehicle, the method may further include, prior to S130:

[0060] Using the multivariate errors of the target vehicle as the system state and the system state of the second Kalman filter, the differential equations of the multivariate errors are determined respectively.

[0061] Based on the differential equations of multiple errors, the system state equation of the second Kalman filter is constructed. With the three-dimensional velocity error of the target vehicle as the constraint observation, the observation equation of the second Kalman filter is constructed based on the system state of the second Kalman filter, thus obtaining the second Kalman filter.

[0062] Here, multiple errors can include position error, velocity error, attitude error, accelerometer drift, and gyroscope drift. Let position error δP be an example. n Speed ​​error δV n Attitude error δθ n accelerometer drift δa b gyroscope drift δω b Given the system state X(t) of the second Kalman filter, determine the position error δP. n Speed ​​error δV n Attitude error δθ n accelerometer drift δa b and gyroscope drift δω b The differential equation is as follows:

[0063]

[0064]

[0065]

[0066]

[0067]

[0068] Wherein, δP n For position error, δV n For velocity error, δθ n For attitude error, δa b This can be used to measure accelerometer drift, δω b This can be used to drift the gyroscope. It can be the derivative of the position error. It can be the derivative of the speed error. It can be the derivative of the attitude error. It can be the derivative of the accelerometer drift. R can be the differential of the gyroscope drift, and R can be the attitude matrix between the carrier coordinate system and the global coordinate system. m It can be a measurement of acceleration, a b It can be zero bias of acceleration, a n The noise for acceleration measurement, ω n The noise in the measurement of angular velocity can be denoted as a. w It can be the random walk noise of acceleration, ω w It can be random walk noise of angular velocity.

[0069] For example, the carrier coordinate system (b-system) and the global coordinate system (n-system) can be as follows: Figure 3 As shown in the diagram. The carrier coordinate system (b-system) can be defined with the center of the IMU fixed to the target vehicle as its origin. The Y-axis points forward along the vehicle's longitudinal direction, the X-axis is perpendicular to the Y-axis and points to the right side of the vehicle, and the Z-axis is perpendicular to the plane formed by the X and Y axes and points upward (the Z-axis is not shown in the diagram due to angle). If the carrier coordinate system at the time of vehicle startup is fixed in the world coordinate system, and this coordinate system does not change with the movement of the carrier and is stationary relative to the Earth, then this fixed coordinate system can be defined as the global coordinate system (n-system).

[0070] Then, based on the differential equations of the position error, velocity error, attitude error, accelerometer drift, and gyroscope drift mentioned above, the system state equation of the second Kalman filter can be constructed.

[0071] In some implementations, the system state equation of the second Kalman filter can be:

[0072]

[0073] Where X(t) can be the system state of the second Kalman filter. It can be the derivative of the system state of the second Kalman filter. This can be the system state matrix of the second Kalman filter. The system noise matrix can be the second Kalman filter, W(t) = [a n ω n a w ω w ] T I can be the identity matrix, R can be the attitude matrix between the carrier coordinate system and the global coordinate system, and a m It can be a measurement of acceleration, a b It can be zero bias of acceleration, a n The noise for acceleration measurement, ω n The noise in the measurement of angular velocity can be denoted as a. w It can be the random walk noise of acceleration, ω w It can be random walk noise of angular velocity.

[0074] Furthermore, three-dimensional velocity errors can include longitudinal velocity error, lateral velocity error, and vertical velocity error. It can be assumed that the target vehicle's vertical velocity is 0, and based on the longitudinal velocity error... Lateral velocity error and vertical velocity error Constructing complete constrained observations and with To complete the constrained observations, the observation equations of the second Kalman filter are constructed by combining the system state X(t) of the second Kalman filter.

[0075] Specifically, the true three-dimensional velocity in the global coordinate system that is affected by noise can be... Projecting onto the carrier coordinate system, the formula is as follows:

[0076]

[0077] in, It can be the projection of the noisy, real 3D velocity in the global coordinate system onto the carrier coordinate system, v b It can be the noise-free three-dimensional nominal velocity in the carrier coordinate system, δv b This can be the three-dimensional velocity error in the carrier coordinate system, δv n This can be the three-dimensional velocity error in the global coordinate system, v n It can be the projection of a noise-free 3D velocity onto the global coordinate system, δθ. n This can be considered as attitude error.

[0078] Then, with δv b As a complete constrained observation and with To complete the constrained observations, the observation equations of the second Kalman filter are constructed by combining the system state X(t) of the second Kalman filter.

[0079] In some implementations, the observation equation of the second Kalman filter can be:

[0080] Z(t)=H2(t)X(t)+v2(t)

[0081] Where Z(t) can be the constraint observation of the second Kalman filter. v2(t) can be the observation matrix of the second Kalman filter, and v2(t) can be the observation noise of the second Kalman filter. n It can be the projection of noise-free 3D velocity onto the global coordinate system.

[0082] Thus, through the above process, a complete constrained Kalman filter based on velocity information, namely the second Kalman filter, can be designed, thereby allowing for a more accurate estimation of the three-dimensional velocity of the target vehicle.

[0083] In some implementations, the second Kalman filter simultaneously corrects for position, velocity, attitude, gyroscope, and accelerometer bias. However, vehicle motion is primarily two-dimensional, and the observability of the heading angle is relatively weak. Therefore, correcting the heading angle error leads to cumulative heading angle drift. Since the heading angle is linearly related to the roll and pitch angles, to maintain filter consistency, obtain accurate roll and pitch angles, and reduce heading angle drift, the method may further include the following after S120:

[0084] Obtain the first attitude information of the target vehicle;

[0085] The second roll angle and second pitch angle of the target vehicle are determined based on the lateral velocity, longitudinal velocity and first attitude information using the second Kalman filter.

[0086] Output the target vehicle's attitude information, which includes the first heading angle, the second roll angle, and the second pitch angle.

[0087] Here, the first attitude information can be the target vehicle's current attitude information obtained through IMU integration (i.e., trajectory extrapolation). The first attitude information may include a first heading angle, a first roll angle, and a first pitch angle. The target attitude information may include a first heading angle, a second roll angle, and a second pitch angle.

[0088] Specifically, during the IMU integration process, the output state of the isolated gyroscope's Z-axis zero bias can be calculated simultaneously. Here, attitude information is represented by a quaternion, denoted as Q for the regularly updated attitude quaternion, and Q is the final output attitude quaternion. out Q and Q out The update methods are as follows:

[0089]

[0090]

[0091] Among them, Q i The attitude information that can be regularly updated at time i, Q i+1 The pose information that can be regularly updated at time i+1, (Q out ) i This can be the final output attitude information at time i, (Q out ) i+1 ω can be the final output attitude information at time i+1. m It can be a measurement of the triaxial angular velocity, ω b It can be zero bias of the triaxial angular velocity, ω bz It can be the Z-axis component of the gyroscope with zero bias, and Δt can be the preset period.

[0092] It should be noted that ω b It can be estimated using a second Kalman filter.

[0093] Q out The update method can isolate the correction of the gyroscope's Z-axis zero bias by the second Kalman filter.

[0094] After filtering and correction by the second Kalman filter, the roll, pitch, and sail angles of Q are all corrected. The corrections for roll and pitch are relatively effective, while the heading angle and the gyroscope Z-axis zero-bias estimation are inaccurate, and both are fed back into Q, causing Q to gradually deviate from its true heading. Because Q... out During the update, inaccurate gyroscope Z-axis zero bias was isolated, and its yaw angle remained basically consistent with the true yaw angle. Therefore, Q can be used. out The heading angle of Q is covered to prevent heading drift caused by the second Kalman filter correcting the gyroscope's Z-axis zero bias. The specific correction method is as follows:

[0095] [roll pitchyaw] = f(Q)

[0096] [roll pitchyaw] out =f(Q) out )

[0097] Qout =f -1 ([roll pitchyaw out ])

[0098] Here, f can represent the mapping from attitude quaternions to Euler angles, f -1 Let represent the inverse mapping that converts attitude quaternions into Euler angles.

[0099] In this way, the correction effect of the second Kalman filter on the roll and pitch angles is preserved, and the drift of the yaw angle is prevented, thus obtaining the optimal attitude information.

[0100] The vehicle state determination method provided in this application applies the lateral velocity estimation of the dynamic model to the error kinematics model, making full use of the rich data of the autonomous driving system. It provides accurate lateral velocity constraints for the error kinematics model without the addition of levers, reducing the dimensionality of the error kinematics model and improving the accuracy and stability of vehicle three-dimensional velocity estimation. Furthermore, a state-optimized attitude-maintaining technique is proposed, isolating the gyroscope Z-axis zero bias and heading angle correction during state output, while ensuring filter consistency. This reduces the cumulative drift of IMU and wheel speed fusion positioning, improving the accuracy and stability of attitude estimation and laying an important foundation for the operation of the control and planning modules.

[0101] Furthermore, some schemes estimate the vehicle's lateral velocity and use it for kinematic model observation, ultimately outputting vehicle speed and attitude information. However, these schemes use nonlinear kinematic models and do not consider factors such as IMU drift, resulting in low accuracy and robustness. The vehicle state determination method provided in this application, however, uses the estimated lateral velocity for linearized error kinematic model observation, solving the problem of insufficient information from longitudinal velocity observation alone. The linearized error kinematic model and lateral velocity observation improve positioning accuracy and stability, enabling full utilization of autonomous driving system data. Then, combined with attitude stabilization technology, it reduces the cumulative drift error of IMU and wheel speed fusion, thus facilitating better development of fusion positioning and perception.

[0102] Based on the same inventive concept, embodiments of this application also provide a vehicle state determination device. The following, in conjunction with… Figure 4 The vehicle status determination device provided in the embodiments of this application will be described in detail.

[0103] Figure 4 A schematic diagram of a vehicle status determination device according to an embodiment of this application is shown.

[0104] like Figure 4 As shown, the vehicle status determination device may include:

[0105] The first acquisition module 401 is used to acquire the driving data of the target vehicle, including longitudinal speed, front wheel steering angle and inertial navigation system (IMU) data.

[0106] The first determining module 402 is used to determine the lateral speed of the target vehicle based on driving data through a first Kalman filter;

[0107] The second determination module 403 is used to determine the three-dimensional velocity of the target vehicle based on the lateral velocity, longitudinal velocity and IMU data through a second Kalman filter.

[0108] Therefore, the driving data of the target vehicle can be obtained, including longitudinal speed, front wheel steering angle, and IMU data. Then, the lateral speed of the target vehicle is determined based on the driving data by a first Kalman filter. Then, the three-dimensional speed of the target vehicle can be determined based on the lateral speed, longitudinal speed, and IMU data by a second Kalman filter. In this way, the longitudinal speed and lateral speed included in the three-dimensional speed of the target vehicle can be accurately obtained without assuming the lateral speed to be 0. The positioning accuracy can be improved by using accurate longitudinal speed and lateral speed for positioning.

[0109] In some implementations, the second Kalman filter simultaneously corrects for position, velocity, attitude, gyroscope, and accelerometer bias. However, vehicle motion is primarily two-dimensional, and the observability of the heading angle is relatively weak. Therefore, correcting the heading angle error leads to cumulative heading angle drift. Since the heading angle is linearly related to the roll and pitch angles, to maintain filter consistency, obtain accurate roll and pitch angles, and reduce heading angle drift, the device may further include:

[0110] The second acquisition module is used to acquire the first attitude information of the target vehicle after determining the lateral speed of the target vehicle based on the driving data through the first Kalman filter. The first attitude information includes the first heading angle, the first roll angle and the first pitch angle.

[0111] The third determining module is used to determine the second roll angle and the second pitch angle of the target vehicle based on the lateral velocity, longitudinal velocity and the first attitude information through the second Kalman filter;

[0112] The output module is used to output the target attitude information of the target vehicle, which includes the first heading angle, the second roll angle, and the second pitch angle.

[0113] In some embodiments, to more accurately estimate the lateral velocity of the target vehicle, the device may further include:

[0114] The fourth determination module is used to determine the differential equations of the target vehicle's sideslip angle, lateral velocity, and steering angular velocity at the center of gravity of the target vehicle, respectively, before determining the lateral velocity of the target vehicle based on the driving data through the first Kalman filter.

[0115] The first construction module is used to construct the system state equation of the first Kalman filter based on the differential equations of the sideslip angle, lateral velocity, and steering angular velocity at the center of gravity of the target vehicle. It also constructs the observation equation of the first Kalman filter based on the system state of the first Kalman filter, using the product of the longitudinal velocity and the sideslip angle at the center of gravity and the Z-axis angular velocity of the IMU as constraint observations, thus obtaining the first Kalman filter.

[0116] In some implementations, the system state equation of the first Kalman filter is:

[0117]

[0118] Where x(t) represents the system state of the first Kalman filter. Let be the differential of the system state of the first Kalman filter, u(t) be the system input of the first Kalman filter, and w(t) be the system noise of the first Kalman filter. Here is the system state matrix of the first Kalman filter. The system control matrix for the first Kalman filter is... Let C be the system noise matrix of the first Kalman filter. f For the front wheel lateral stiffness, C r For the rear wheel lateral stiffness, I f I is the distance from the vehicle's center of gravity to the front axle. r The distance from the vehicle's center of gravity to the rear axle is given by m, where m is the vehicle's mass and I is the distance from the center of gravity to the rear axle. z Let v be the moment of inertia of the vehicle about the Z-axis. x For longitudinal velocity,

[0119] The observation equation for the first Kalman filter is:

[0120] z(t)=H1(t)x(t)+v1(t)

[0121] Where z(t) is the constraint observation of the first Kalman filter, and H1(t) = [0v x [1] represents the observation matrix of the first Kalman filter, and v1(t) represents the observation noise of the first Kalman filter.

[0122] In some embodiments, to more accurately estimate the three-dimensional velocity of the target vehicle, the device may further include:

[0123] The fifth determination module is used to determine the differential equations of the multiple errors of the target vehicle before determining the three-dimensional velocity of the target vehicle based on the lateral velocity, longitudinal velocity and IMU data through the second Kalman filter.

[0124] The second construction module is used to construct the system state equation of the second Kalman filter based on the differential equation of multiple errors, and to construct the observation equation of the second Kalman filter based on the system state of the second Kalman filter, using the three-dimensional velocity error of the target vehicle as the constraint observation, thus obtaining the second Kalman filter.

[0125] In some implementations, the system state equation of the second Kalman filter is:

[0126]

[0127] Where X(t) represents the system state of the second Kalman filter. The derivative of the system state of the second Kalman filter. Here is the system state matrix of the second Kalman filter. Let W(t) be the system noise matrix of the second Kalman filter. n ω n a w ω w ] T I is the identity matrix, R is the attitude matrix between the carrier coordinate system and the global coordinate system, and a m For the measured value of acceleration, a b For zero bias of acceleration, a n For the measurement noise of acceleration, ω n For the noise in the measurement of angular velocity, a w For the random walk noise of acceleration, ω w The random walk noise is the angular velocity.

[0128] The observation equation for the second Kalman filter is:

[0129] Z(t)=H2(t)X(t)+v2(t)

[0130] Where Z(t) represents the constraint observations of the second Kalman filter. Let v2(t) be the observation matrix of the second Kalman filter, and v2(t) be the observation noise of the second Kalman filter. n The projection of noise-free 3D velocity onto the global coordinate system.

[0131] Figure 5A schematic diagram of the structure of an electronic device provided in one embodiment of this application is shown.

[0132] like Figure 5 As shown, the electronic device 5 is capable of implementing the vehicle state determination method and vehicle state determination device according to the embodiments of this application, and is an exemplary hardware architecture of the electronic device.

[0133] The electronic device may refer to the electronic device in the embodiments of this application.

[0134] The electronic device 5 may include a processor 501 and a memory 502 storing computer program instructions.

[0135] Specifically, the processor 501 may include a central processing unit (CPU), an application-specific integrated circuit (ASIC), or one or more integrated circuits that can be configured to implement the embodiments of this application.

[0136] Memory 502 may include mass storage for data or instructions. For example, and not limitingly, memory 502 may include a hard disk drive (HDD), floppy disk drive, flash memory, optical disk, magneto-optical disk, magnetic tape, or Universal Serial Bus (USB) drive, or a combination of two or more of these. Where appropriate, memory 502 may include removable or non-removable (or fixed) media. Where appropriate, memory 502 may be internal or external to the integrated gateway disaster recovery device. In a particular embodiment, memory 502 is non-volatile solid-state memory. In a particular embodiment, memory 502 may include read-only memory (ROM), random access memory (RAM), disk storage media device, optical storage media device, flash memory device, electrical, optical, or other physical / tangible memory storage device. Thus, generally, memory 502 includes one or more tangible (non-transitory) computer-readable storage media (e.g., memory devices) encoded with software including computer-executable instructions, and when the software is executed (e.g., by one or more processors), it is operable to perform the operations described with reference to the method according to one aspect of this application.

[0137] The processor 501 reads and executes computer program instructions stored in the memory 502 to implement any of the vehicle state determination methods in the above embodiments.

[0138] In one example, the electronic device may also include a communication interface 503 and a bus 504. Wherein, as... Figure 5As shown, the processor 501, memory 502, and communication interface 503 are connected through bus 504 and complete communication with each other.

[0139] The communication interface 503 is mainly used to realize communication between various modules, devices, units and / or equipment in the embodiments of this application.

[0140] Bus 504 includes hardware, software, or both, that couples components of an electronic device together. For example, and not limitingly, the bus may include an Accelerated Graphics Port (AGP) or other graphics bus, an Enhanced Industry Standard Architecture (EISA) bus, a Front Side Bus (FSB), HyperTransport (HT) interconnect, an Industry Standard Architecture (ISA) bus, an Infinite Bandwidth Interconnect, a Low Pin Count (LPC) bus, a memory bus, a Microchannel Architecture (MCA) bus, a Peripheral Component Interconnect (PCI) bus, a PCI-Express (PCI-X) bus, a Serial Advanced Technology Attachment (SATA) bus, a Video Electronics Standards Association Local (VLB) bus, or other suitable buses, or combinations of two or more of these. Where appropriate, bus 504 may include one or more buses. Although specific buses are described and illustrated in embodiments of this application, this application contemplates any suitable bus or interconnect.

[0141] The electronic device can execute the vehicle state determination method in the embodiments of this application, thereby achieving the combination Figures 1 to 4 The described method and apparatus for determining the vehicle's state.

[0142] Furthermore, in conjunction with the vehicle state determination methods in the above embodiments, this application embodiment can provide a computer storage medium for implementation. The computer storage medium stores computer program instructions; when these computer program instructions are executed by a processor, they implement any of the vehicle state determination methods in the above embodiments.

[0143] It should be clarified that this application is not limited to the specific configurations and processes described above and shown in the figures. For the sake of brevity, detailed descriptions of known methods are omitted here. In the above embodiments, several specific steps are described and shown as examples. However, the method process of this application is not limited to the specific steps described and shown. Those skilled in the art can make various changes, modifications, and additions, or change the order of steps, after understanding the spirit of this application.

[0144] The functional blocks shown in the above-described structural diagram can be implemented as hardware, software, firmware, or a combination thereof. When implemented in hardware, they can be, for example, electronic circuits, application-specific integrated circuits (ASICs), appropriate firmware, plug-ins, function cards, etc. When implemented in software, the elements of this application are programs or code segments used to perform the required tasks. Programs or code segments can be stored on a machine-readable medium or transmitted over a transmission medium or communication link via data signals carried on a carrier wave. "Machine-readable medium" can include any medium capable of storing or transmitting information. Examples of machine-readable media include electronic circuits, semiconductor memory devices, ROM, flash memory, erasable ROM (EROM), floppy disks, CD-ROMs, optical disks, hard disks, fiber optic media, radio frequency (RF) links, etc. Code segments can be downloaded via computer networks such as the Internet, intranets, etc.

[0145] It should also be noted that the exemplary embodiments mentioned in this application describe methods or systems based on a series of steps or apparatus. However, this application is not limited to the order of the above steps; that is, the steps can be performed in the order mentioned in the embodiments, or in a different order, or several steps can be performed simultaneously.

[0146] The aspects of this application have been described above with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It should be understood that each block in the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing apparatus to produce a machine such that these instructions, executable via the processor of the computer or other programmable data processing apparatus, enable the implementation of the functions / actions specified in one or more blocks of the flowchart illustrations and / or block diagrams. Such a processor can be, but is not limited to, a general-purpose processor, a special-purpose processor, a special application processor, or a field-programmable logic circuit. It is also understood that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can also be implemented by dedicated hardware performing the specified functions or actions, or can be implemented by a combination of dedicated hardware and computer instructions.

[0147] The above description is merely a specific implementation of this application. Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, modules, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here. It should be understood that the protection scope of this application is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in this application, and these modifications or substitutions should all be covered within the protection scope of this application.

Claims

1. A method for determining the state of a vehicle, characterized in that, include: Acquire the driving data of the target vehicle, including longitudinal speed, front wheel steering angle, and inertial navigation system (IMU) data; The lateral speed of the target vehicle is determined based on the driving data using a first Kalman filter. Using the multinomial errors of the target vehicle as the system state of the second Kalman filter, the differential equations of the multinomial errors are determined respectively; Based on the differential equations of the multiple errors, the system state equation of the second Kalman filter is constructed, and the observation equation of the second Kalman filter is constructed based on the system state of the second Kalman filter, with the three-dimensional velocity error of the target vehicle as the constraint observation, to obtain the second Kalman filter; The three-dimensional velocity of the target vehicle is determined based on the lateral velocity, the longitudinal velocity, and the IMU data using the second Kalman filter.

2. The method according to claim 1, characterized in that, After determining the lateral velocity of the target vehicle based on the driving data using the first Kalman filter, the method further includes: Acquire the first attitude information of the target vehicle, the first attitude information including a first heading angle, a first roll angle and a first pitch angle; The second roll angle and the second pitch angle of the target vehicle are determined using a second Kalman filter based on the lateral velocity, the longitudinal velocity, and the first attitude information. Output the target attitude information of the target vehicle, which includes the first heading angle, the second roll angle, and the second pitch angle.

3. The method according to claim 1, characterized in that, Before determining the lateral velocity of the target vehicle based on the driving data using the first Kalman filter, the method further includes: Using the sideslip angle, lateral velocity, and steering angular velocity at the center of gravity of the target vehicle as the system state of the first Kalman filter, the differential equations for the sideslip angle, lateral velocity, and steering angular velocity at the center of gravity of the target vehicle are determined respectively. Based on the differential equations of the sideslip angle, lateral velocity, and steering angular velocity at the center of gravity of the target vehicle, the system state equation of the first Kalman filter is constructed. Using the product of the longitudinal velocity and the sideslip angle at the center of gravity and the Z-axis angular velocity of the IMU as constrained observations, the observation equation of the first Kalman filter is constructed based on the system state of the first Kalman filter, thus obtaining the first Kalman filter.

4. The method according to claim 3, characterized in that, The system state equation of the first Kalman filter is: in, This represents the system state of the first Kalman filter. The derivative of the system state of the first Kalman filter. This is the system input of the first Kalman filter. The system noise of the first Kalman filter, Let be the system state matrix of the first Kalman filter. The system control matrix of the first Kalman filter is... The system noise matrix of the first Kalman filter is... For the front wheel lateral stiffness, For rear wheel lateral stiffness, This is the distance from the vehicle's center of gravity to the front axle. This is the distance from the vehicle's center of gravity to the rear axle. For the overall vehicle quality, Let Z be the moment of inertia of the vehicle about the Z-axis. For longitudinal velocity, The observation equation of the first Kalman filter is: in, The constraint observations of the first Kalman filter, Let be the observation matrix of the first Kalman filter. This represents the observation noise of the first Kalman filter.

5. The method according to claim 1, characterized in that, The system state equation of the second Kalman filter is: in, This represents the system state of the second Kalman filter. The derivative of the system state of the second Kalman filter. Here is the system state matrix of the second Kalman filter. Here is the system noise matrix of the second Kalman filter. , It is the identity matrix. This represents the attitude matrix between the carrier coordinate system and the global coordinate system. The measured value of acceleration. For zero bias of acceleration, For the noise of acceleration measurement, For angular velocity measurement noise, For random walk noise with acceleration, The random walk noise is the angular velocity. The observation equation for the second Kalman filter is: in, The constraint observations of the second Kalman filter are... This is the observation matrix of the second Kalman filter. The observation noise of the second Kalman filter, The projection of noise-free 3D velocity onto the global coordinate system.

6. A vehicle status determination device, characterized in that, The device includes: The first acquisition module is used to acquire the driving data of the target vehicle, including longitudinal speed, front wheel steering angle and inertial navigation system (IMU) data. The first determining module is used to determine the lateral speed of the target vehicle based on the driving data using a first Kalman filter; The fifth determining module is used to determine the differential equations of the multiple errors, using the multiple errors of the target vehicle as the system state of the second Kalman filter. The second construction module is used to construct the system state equation of the second Kalman filter based on the differential equation of the multiple errors, and to construct the observation equation of the second Kalman filter based on the system state of the second Kalman filter, using the three-dimensional velocity error of the target vehicle as the constraint observation, to obtain the second Kalman filter. The second determining module is used to determine the three-dimensional velocity of the target vehicle based on the lateral velocity, the longitudinal velocity, and the IMU data using the second Kalman filter.

7. An electronic device, characterized in that, The device includes: a processor and a memory storing computer program instructions; When the processor executes the computer program instructions, it implements the vehicle state determination method as described in any one of claims 1-5.

8. A computer storage medium, characterized in that, The computer storage medium stores computer program instructions, which, when executed by a processor, implement the vehicle state determination method as described in any one of claims 1-5.

9. A computer program product, characterized in that, When the instructions in the computer program product are executed by the processor of the electronic device, the electronic device performs the vehicle state determination method as described in any one of claims 1-5.

Images