A vehicle pose prediction method, device and electronic equipment

Abstract

The application relates to the technical field of intelligent driving, in particular to a vehicle pose prediction method and device and electronic equipment. In the method, first posterior information of a vehicle system at a first time is acquired, that is, a first posterior estimation value of system state information and a second posterior estimation value of a covariance matrix of error system state information; prior prediction information of the vehicle system at a second time is predicted based on acceleration and angular velocity in inertial measurement information; in response to receiving satellite system information or wheel speed meter information, second posterior information of the vehicle system at the second time is predicted based on corresponding position information, heading angle information or corresponding speed and prior prediction information; the second posterior information comprises an updated first posterior estimation value and an updated second posterior estimation value; and the updated first posterior estimation value is taken as a target pose of the vehicle system at the second time, so that the robustness of vehicle pose prediction is improved.

Description

Technical Field

[0001] This application relates to the field of intelligent driving technology, and in particular to a method, device and electronic device for predicting vehicle pose. Background Technology

[0002] In the process of autonomous driving of intelligent vehicles, it is necessary to fuse various on-board sensors to calculate the vehicle's pose, and then give a relatively accurate prediction of the vehicle's pose without relying on map matching.

[0003] In related technologies, a common approach is to use Error State Kalman Filter (ESKF) to process the position information collected by the Global Navigation Satellite System (GNSS) and the acceleration and angular velocity collected by the Inertial Measurement Unit (INS) to predict the vehicle's pose at high frequencies. Then, the absolute position information collected by the GNSS is used to update the filtering process.

[0004] It can be seen that although the above-mentioned ESKF-based solution can provide a relatively accurate prediction of vehicle pose without map matching, it has the following technical defects in practical applications.

[0005] I. Although the above scheme uses the absolute position information of the Global Navigation Satellite System as an observation to update the vehicle's pose, in some scenarios where the signal quality of the Global Navigation Satellite System is poor, this scheme has the problem of predicting the vehicle's pose leading to abnormal trajectory.

[0006] Second, although the above scheme is based on the acceleration collected by the inertial prediction unit for prediction, in the case of low vehicle speed, due to the characteristics of the inertial measurement unit, the accuracy of the acceleration collected is low, which leads to abnormalities in the prediction of vehicle pose. This will affect the subsequent trajectory inference and thus introduce a large error; for example, the filtered trajectory has local abnormal protrusions. Summary of the Invention

[0007] This application provides a method, apparatus, and electronic device for predicting vehicle pose, in order to solve the above-mentioned problems and improve the robustness of vehicle pose prediction.

[0008] In a first aspect, this application provides a method for predicting vehicle pose, the method comprising:

[0009] Obtain the first posterior information of the vehicle system at the first moment, the first posterior information including: the first posterior estimate of the system state information and the second posterior estimate of the covariance matrix of the error system state information;

[0010] Based on the acceleration and angular velocity in the inertial measurement information, and based on the first posterior information, the prior prediction information of the vehicle system at the second time moment is predicted;

[0011] In response to receiving satellite system information, based on the corresponding position information and heading angle information, and based on the prior prediction information, the vehicle system predicts the second posterior information at the second time moment, the second posterior information including: the updated first posterior estimate and the updated second posterior estimate;

[0012] In response to receiving wheel speed meter information, based on the corresponding speed and, based on the prior prediction information, predict the second posterior information of the vehicle system at the second moment;

[0013] The updated first posterior estimate is used as the target pose of the vehicle at the second time step.

[0014] Secondly, this application provides a vehicle pose prediction device, the device comprising:

[0015] The acquisition module acquires the first posterior information of the vehicle system at the first moment. The first posterior information includes: the first posterior estimate of the system state information and the second posterior estimate of the covariance matrix of the error system state information.

[0016] The first prediction module predicts the prior prediction information of the vehicle system at the second moment based on the acceleration and angular velocity in the inertial measurement information, and based on the first posterior information.

[0017] The second prediction module, in response to receiving satellite system information, predicts second posterior information of the vehicle system at the second time moment based on the corresponding position information and heading angle information, and based on the prior prediction information. The second posterior information includes: an updated first posterior estimate and an updated second posterior estimate. In response to receiving wheel speedometer information, predicts second posterior information of the vehicle system at the second time moment based on the corresponding speed and based on the prior prediction information. The updated first posterior estimate is used as the target pose of the vehicle at the second time moment.

[0018] Optionally, the acquisition module is specifically used for:

[0019] When the first moment is the initial moment, based on the satellite system information, the initial system state information and the initial covariance matrix of the initial error system state information of the vehicle system at the initial moment are obtained;

[0020] Calculate the first posterior estimate of the initial system state information, and calculate the second posterior estimate of the initial covariance matrix;

[0021] The first posterior estimate and the second posterior estimate are used as the first posterior information of the vehicle system at the first time.

[0022] Optionally, the acquisition module is specifically used for:

[0023] When the first moment is not the initial moment, based on historical data, the second posterior information generated by updating the system state information of the vehicle system at the first moment is obtained and used as the first posterior information of the vehicle system at the first moment.

[0024] Optionally, the second prediction module is configured to, before using the updated first posterior estimate as the target pose of the vehicle at the second time moment, specifically:

[0025] In response to receiving the satellite system information and the wheel speedometer information at the same time, then:

[0026] Based on the position information and heading angle information in the satellite system information, and based on the prior prediction information, predict the second posterior information of the vehicle system at the second moment;

[0027] Based on the speed in the wheel speed meter information, the second posterior information is measured and updated to obtain the updated second posterior information, which covers the second posterior information.

[0028] or,

[0029] Based on the speed in the wheel speedometer information, and based on the prior prediction information, predict the second posterior information of the vehicle system at the second moment;

[0030] Based on the position information and heading angle information in the satellite system information, and based on the prior prediction information, the second posterior information is measured and updated to obtain the updated second posterior information, which covers the second posterior information.

[0031] Optionally, the second prediction module is configured to update the second posterior information based on the corresponding speed in response to receiving wheel speed meter information, specifically for:

[0032] When wheel speed meter information is received, the speed observation is obtained based on the speed measurement noise in the wheel speed meter information and based on the system state information.

[0033] Using the error system state variables, the velocity observation is differentiated to obtain the target Jacobian matrix;

[0034] The second posterior information is updated based on the target Jacobian matrix.

[0035] Optionally, the second prediction module is used to take the updated first posterior estimate as the target pose of the vehicle at the second time moment, specifically for:

[0036] Obtain the first timestamp of the speed from the wheel speed meter information;

[0037] Based on the inertial measurement information, the relative pose between two adjacent wheel speed frames is obtained for the first timestamp.

[0038] Using the relative pose as an inter-frame constraint, the updated first posterior estimate is subjected to sliding window pose optimization processing to obtain the target pose of the vehicle system at the second time moment.

[0039] Optionally, the second prediction module is used to obtain the relative pose between two adjacent wheel speed frames based on the inertial measurement information for the first timestamp, specifically for:

[0040] Based on the inertial measurement information, the first angular velocity and the zero bias of the first gyroscope corresponding to the first timestamp are obtained;

[0041] Based on the velocity, the first angular velocity, and the zero bias of the first gyroscope, pre-integration is performed on two adjacent wheel speed frames to obtain the relative pose between the two wheel speed frames.

[0042] Thirdly, this application provides an electronic device, the electronic device comprising:

[0043] Memory, used to store computer programs;

[0044] When the processor executes the computer program stored in the memory, it implements the steps of the vehicle pose prediction method described above.

[0045] Fourthly, this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the steps of the above-described vehicle pose prediction method.

[0046] The beneficial effects of this application are as follows:

[0047] In this application embodiment, a method for predicting vehicle pose is provided. This method combines satellite system information collected by a global navigation satellite system and inertial measurement information collected by an inertial measurement unit to predict the vehicle pose.

[0048] Specifically, the electronic device first acquires the first posterior information of the vehicle system at a first moment, namely, the first posterior estimate of the system state information and the second posterior estimate of the covariance matrix of the error system state information; then, based on the acceleration and angular velocity in the inertial measurement information, it predicts the prior prediction information of the vehicle system at a second moment. Here, the introduction of angular velocity for prediction can compensate for the problem of low prediction accuracy at low vehicle speeds caused by prediction based solely on acceleration; and, in response to receiving satellite system information, based on the position information and heading angle information in the satellite system information, it predicts the second posterior information of the vehicle system at a second moment, namely, the updated first posterior estimate and the updated second posterior estimate. The value here introduces heading angle information for prediction, which can solve the problem of trajectory abnormalities caused by prediction based solely on position information in scenarios with poor signal quality of global navigation satellite systems, i.e., inaccurate prediction results; or, in response to the received wheel speedometer information, the second posterior information of the vehicle system at the second moment is predicted based on the speed in the wheel speedometer information, that is, the updated first posterior estimate and the updated second posterior estimate. The introduction of wheel speedometer information for prediction can make up for the limitations of inertial measurement information or satellite system information; in addition, the updated first posterior estimate in the aforementioned second posterior information can be used as the target pose of the vehicle at the second moment, which can effectively improve the robustness of vehicle pose prediction.

[0049] Other features and advantages of this application will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the application. The objectives and other advantages of this application may be realized and obtained by means of the structures particularly pointed out in the written description, claims, and drawings. Attached Figure Description

[0050] Figure 1 A schematic diagram illustrating one possible application scenario provided by this application;

[0051] Figure 2 A flowchart illustrating a method for predicting vehicle pose provided in this application;

[0052] Figure 3 A schematic diagram of a pose diagram provided in this application;

[0053] Figure 4 A schematic diagram illustrating the specific process of predicting vehicle pose provided in this application;

[0054] Figure 5 A schematic diagram of a vehicle pose prediction device provided in this application;

[0055] Figure 6 A schematic diagram of the structure of an electronic device provided in this application. Detailed Implementation

[0056] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings.

[0057] It should be noted that in the description of this application, "multiple" is understood as "at least two". "And / or" describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. A connected to B can represent: A and B directly connected, or A and B connected through C. Furthermore, in the description of this application, terms such as "first" and "second" are used only for descriptive purposes and should not be construed as indicating or implying relative importance or order.

[0058] In this embodiment of the application, the collection, storage, use, processing, transmission, provision, and disclosure of user personal information comply with relevant laws and regulations and do not violate public order and good morals.

[0059] This application relates to a method for predicting vehicle pose, wherein information collected by an inertial measurement unit and a global navigation satellite system is fused, and further, information from wheel speed odometers is fused to achieve a relatively accurate prediction of vehicle pose; to facilitate understanding by those skilled in the art, some terms used in this application embodiment are explained below.

[0060] An inertial measurement unit (IMU) is used to measure the three-axis attitude angles (or angular velocities) and acceleration of an object. For example, in the embodiments of this application, the inertial measurement unit is used to collect inertial measurement information, which can be acquired by an electronic device. The inertial measurement information may include at least acceleration, angular velocity, zero bias of acceleration, and zero bias of gyroscope.

[0061] Zero bias, or zero-point offset, primarily refers to sensors used in engineering machinery and the solar energy industry. Zero bias depends mainly on the inherent characteristics of the core sensing device. It means that when there is no angular input (such as on an absolutely horizontal plane), the sensor's measured output is not zero; this actual output angle value is the zero-point bias. This indicator has nothing to do with whether the sensor can be zeroed. For example, in the embodiments of this application, zero bias of acceleration and zero bias of gyroscopes may be involved.

[0062] A Global Navigation Satellite System (GNSS), also known as a global satellite navigation system, is a space-based radio navigation and positioning system that can provide users with all-weather three-dimensional coordinates, velocity, and time information at any location on the Earth's surface or in near-Earth space. For example, in this embodiment, the GNSS is used to collect satellite system information, which can be acquired by electronic devices. The satellite system information may include at least position information and heading angle information.

[0063] Wheel odometer (WO) is one of the simplest forms of vehicle self-localization. The WO method relies on wheel encoders mounted on the vehicle to track the number of revolutions of each wheel. These revolutions are integrated into a vehicle motion model to determine the vehicle's current position relative to a starting point. For example, in this embodiment, the WO is used to acquire wheel speed information, which can be obtained by an electronic device. The wheel speed information may include at least speed and its measurement noise.

[0064] This application provides a method for predicting vehicle pose. Specifically, it predicts the target pose of a vehicle at a specified time by fusing inertial measurement information (such as acceleration and angular velocity) collected by an inertial measurement unit and satellite system information (such as position information and heading angle information) collected by a global navigation satellite system.

[0065] Furthermore, based on the above method, wheel speed information collected by wheel speed odometers (such as speed and its measurement noise) is also integrated to compensate for the limitations of inertial measurement information and satellite system information, thereby enhancing robustness.

[0066] In addition, for the method of fusing wheel speed measurement information, a sliding window pose optimization processing method can be adopted to optimize the trajectory of the filtered output, making the generated trajectory smoother. This solves the problem of unsmooth trajectories in related schemes (for example, the pose of the current frame of the filtering frame is only related to the pose of the previous frame, but not to the pose of the historical frames, so the current information cannot be used to correct the historical results, resulting in the retention of unsmooth parts in the historical trajectory).

[0067] refer to Figure 1 As shown, this is an application scenario applicable to the embodiments of this application, which can be an intelligent driving vehicle 10.

[0068] For example, the intelligent driving vehicle 10 is any drivable vehicle with an intelligent driving mode, such as... Figure 1 As shown, the intelligent driving vehicle may be equipped with a vehicle system 101 for predicting the vehicle's pose. When the vehicle system 101 is started, it can instruct the intelligent driving vehicle 10 to predict the vehicle's pose.

[0069] More specifically, in this embodiment, the intelligent driving vehicle 10 may further include: a global navigation satellite system 102, an inertial measurement unit 103, and a wheel speed odometer 104. The sampling frequency relationship among these three components may be: inertial measurement unit 103 > wheel speed odometer 104 > global navigation satellite system 102; however, this is only one possible implementation and is not specifically limited here.

[0070] For the above application scenarios, after the vehicle system 101 is started, it performs pose initialization based on satellite system information (such as vehicle position information, speed information, and rotational torque) collected by the global navigation satellite system 102, and / or, in response to receiving inertial measurement information (such as acceleration and angular velocity) collected by the inertial measurement unit 103, it performs ESKF prediction, and then performs ESKF update based on satellite system information (such as position information and heading angle information) collected by the global navigation satellite system to obtain the filtered pose as the target pose.

[0071] Furthermore, based on the wheel speed meter information collected by the wheel speed odometer 104, the above filtered pose can be processed to obtain the optimized INS (Information Network System) pose, which can be used as the target pose.

[0072] In detail, the acquisition frequency of the wheel speed odometer 104 is lower than that of the inertial measurement unit 103. If the time interval between two adjacent acquisitions by the wheel speed odometer 104 is considered as a time period, multiple filtered poses can be obtained within one time period. When a time period ends (i.e., after obtaining the filtered pose based on one acquisition of wheel speed information by the wheel speed odometer 104), a pose graph can be constructed based on the filtered poses obtained within this time period. Then, the pose graph is optimized based on the wheel speed information acquired within this time period. Specifically, pre-integration is performed based on the inertial measurement information (e.g., angular velocity) acquired by the inertial measurement unit 103 and the wheel speed information (e.g., velocity) acquired by the wheel speed odometer 104 to obtain the inter-frame relative pose. The pose graph is then optimized to obtain the optimized INS pose. That is, after optimizing the pose graph based on the wheel speed information, the optimized INS pose is obtained as the target pose.

[0073] It should be noted that the above-mentioned time period is one possible design, and there can also be multiple time periods, which are not specifically limited here; in addition, for the construction of the above pose graph, there is no specific limit on the number of filtered poses used to construct it, which can be set according to the actual situation.

[0074] The method for predicting vehicle pose provided in this application embodiment will be described in detail below. Here, we mainly take the single case of predicting vehicle pose as an example. Other cases can be obtained in the same way and will not be repeated. It should be noted that this application embodiment does not specifically limit the execution order of steps 203 and 204.

[0075] refer to Figure 2 The method includes steps 201-205, as detailed below.

[0076] Step 201: Obtain the first posterior information of the vehicle system at the first moment. The first posterior information includes: the first posterior estimate of the system state information and the second posterior estimate of the covariance matrix of the error system state information.

[0077] In this embodiment, the aforementioned system state information refers to the information required for each prediction of the vehicle pose in this embodiment; for example, it is represented as: x = {P, V, R, b} a b g}; where P is the vehicle's position at a specified time; V is the vehicle's speed at a specified time; R is the vehicle's rotation matrix at a specified time; b a b. Zero bias of the accelerometer in the inertial measurement unit; g This is the zero bias of the gyroscope in the inertial measurement unit.

[0078] It should be noted that the vehicle's rotation matrix R at a specified time and the vehicle's position P at a specified time can be used together to construct the vehicle's pose at a specified time.

[0079] The aforementioned error system state information refers to the state quantity information that needs to be maintained for each prediction of vehicle pose in this embodiment of the application. This error system state information can be used to characterize the error of the aforementioned system state information; specifically, it is expressed as: Wherein, δp T Let δv be the error value of P (the vehicle's position at a specified time); T Let δθ be the error in V (the vehicle's speed at a specified time). T The error is R (the rotation matrix of the vehicle at a specified time). For b a The error amount of (zero bias of the accelerometer in the inertial measurement unit); For b g The error amount of the gyroscope in the inertial measurement unit (IGMU).

[0080] In one optional implementation, the first posterior information of the vehicle system at a first moment is obtained. The first moment can be the previous moment or a specified moment. Simply put, taking the previous moment as an example: in some scenarios, see Case 1 below, since the vehicle system starts at the current moment, the second posterior information predicted at the previous moment cannot be obtained. In this case, the first moment is regarded as the initial moment, and the first posterior information of the initial moment is obtained. In some scenarios, see Case 2 below, the second posterior information predicted at the previous moment can be used as the first posterior information obtained at the first moment.

[0081] Scenario 1: The first moment is the initial moment.

[0082] When the first moment is the initial moment, based on the satellite system information, the initial system state information and the initial covariance matrix of the initial error system state information of the vehicle system are obtained at the initial moment. Then, the first posterior estimate of the initial system state information and the second posterior estimate of the initial covariance matrix are calculated. The first posterior estimate and the second posterior estimate are then used as the first posterior information of the vehicle system at the first moment.

[0083] For example, regarding the above initialization process, based on the pose (i.e., the vehicle's rotation matrix R and position P at the initial moment) and velocity (i.e., the vehicle's velocity V at the initial moment) recorded by the satellite system information at the initial moment, the system state information x = {P, V, R, b} of the vehicle system is determined. a b g Initialization is performed to obtain the initial system state information x0; in addition, based on the sensor data (e.g., Global Navigation Satellite System, Inertial Measurement Unit, Wheel Speed ​​Odometer, etc.), the error of the system state information is calculated. Initialization is performed to obtain the initial covariance matrix P0 of the initial error system state information.

[0084] It should be noted that in real-world scenarios, the first posterior information of the vehicle system at the first moment can also be obtained from sensors. No specific limitations are made here on the method of obtaining the first posterior information.

[0085] In one optional implementation, the first posterior information of the vehicle system at a first moment is obtained. The first moment can be the previous moment before the current moment or a specified moment. Taking the previous moment before the current moment as an example, in some scenarios, the vehicle system starts before the current moment, so the second posterior information of the previous moment (i.e., the first moment) can be directly obtained as the first posterior information of the first moment obtained here. See Case 2 below for details.

[0086] Scenario 2: The first moment is not the initial moment.

[0087] When the first moment is not the initial moment, the second verification information generated by predicting the system state information of the vehicle system at the first moment is obtained from historical data, and is used as the first post-verification information for obtaining the first moment.

[0088] In summary, step 201 obtains the first posterior information of the vehicle system at the first moment.

[0089] Step 202: Based on the acceleration and angular velocity in the inertial measurement information, and based on the first posterior information, predict the prior prediction information of the vehicle system at the second time moment.

[0090] In this embodiment of the application, the aforementioned prior information includes a first prior estimate and a second prior estimate. The first prior estimate corresponds to the system state information, and the second prior estimate corresponds to the covariance matrix of the error system state information.

[0091] Specifically, based on the first posterior information of time k-1 obtained in step 201 Second posterior information Furthermore, based on the acceleration 'a' and angular velocity 'w' from the inertial measurement information, ESKF prediction is performed, and according to the kinematic equations, the first prior estimate of the vehicle system at the second time point k is predicted. Second prior estimate

[0092] Step 203: In response to receiving satellite system information, based on the corresponding position information and heading angle information, and based on prior prediction information, predict the second posterior information of the vehicle system at the second time point. The second posterior information includes: the updated first posterior estimate and the updated second posterior estimate.

[0093] Specifically, when satellite system information is received, the first prior estimate of time k obtained in step 202 is used. Second prior estimate Using the satellite system's position information P and heading angle information yaw as measured values, ESKF measurement updates are performed to obtain the first posterior estimate of the system state information at the second time k. And, to obtain the second posterior estimate of the covariance matrix of the error system state information at the second time k.

[0094] Step 204: In response to receiving wheel speed meter information, based on the corresponding speed and based on prior prediction information, predict the second posterior information of the vehicle system at the second time moment.

[0095] In this embodiment of the application, when wheel speed meter information is received, at least the following two situations may be included: Situation 1, wheel speed meter information is received without receiving satellite system information; Situation 2, wheel speed meter information is received at the same time as receiving satellite system information; Situation 3, wheel speed meter information is received when satellite system information is received but inertial measurement information is not received again.

[0096] Scenario 1: When wheel speed meter information is received, the first prior estimate of time k obtained in step 202 is used. Second prior estimate Using the speed b from the wheel speedometer information as the measured value, ESKF measurement updates are performed to obtain the first posterior estimate of the system state information at the second time k. And, to obtain the second posterior estimate of the covariance matrix of the error system state information at the second time k.

[0097] Scenario 2: When both satellite system information and wheel speedometer information are received simultaneously, taking the update based on satellite system information as an example (the same logic applies to updating based on wheel speedometer information), according to step 203, the second posterior information obtained after updating based on satellite system information (i.e., the first posterior estimate) is... Second posterior estimate Then, based on the speed b in the wheel speedometer information, the second posterior information (i.e., the first posterior estimate) is used to estimate the second posterior information. Second posterior estimate The measurement is updated to obtain the updated second posterior information (i.e., the updated first posterior estimate). and the updated second posterior estimate This is to cover the original second posterior information.

[0098] Scenario 3: After receiving satellite system information, if no further inertial measurement information is received, but wheel speedometer information is received, the second posterior information (i.e., the first posterior estimate) obtained after updating based on the satellite system information in step 203 is used. Second posterior estimate Then, based on the speed b in the wheel speedometer information, the second posterior information (i.e., the first posterior estimate) is used to estimate the second posterior information. Second posterior estimate The measurement is updated to obtain the updated second posterior information (i.e., the updated first posterior estimate). and the updated second posterior estimate This is to cover the original second posterior information.

[0099] In one implementation, for the above measurement update, when wheel speed meter information is received, the speed observation V in the wheel speed meter information is acquired. v Then, using the error system state variable δx, the velocity observation V is... v By performing differentiation, the target Jacobian matrix H is obtained. v and H δx Then based on the target Jacobian matrix H v and H δx Update the first posterior estimate in the second posterior information respectively. Second posterior estimate

[0100] Regarding the above velocity observation V v The value can be obtained by calculating it based on the following formula.

[0101] V v =R T V+v i =h(δx)

[0102] Among them, V v For velocity observation, R T Let V be the rotation matrix, and V be the vehicle speed. i h(δx) represents the measurement noise of velocity v in the wheel speed gauge information, and is a functional equation.

[0103] For the aforementioned target Jacobian matrix H v and H δx The method for obtaining can be derived based on the following formula.

[0104] The first step is to construct the Jacobian matrix, and the calculation formula is as follows.

[0105]

[0106] Among them, H x The derivatives of the observed quantities with respect to the system state variables, and the derivatives of the system state information with respect to the error state information, are as follows:

[0107] Hx = [0 H v HR2 0 0] 3×15 .

[0108]

[0109]

[0110]

[0111] Furthermore, the aforementioned target Jacobian matrix H v and H δx(Hereinafter referred to as H) Substituting this into the ESKF update formula below, we can obtain the first posterior estimate in the updated second posterior information. Second posterior estimate

[0112]

[0113]

[0114]

[0115]

[0116] Where V is the covariance matrix of the measurement noise. This is the second prior estimate. Here, is the second posterior estimate. and This can be collectively referred to as P, which is the covariance matrix of the error system state information.

[0117] In the embodiments of this application, the first posterior estimate is obtained. Second posterior estimate It can be used as the first posterior information obtained at the third time point (e.g., the time point after the second time point).

[0118] Step 205: Use the updated first posterior estimate as the target pose of the vehicle at the second time step.

[0119] In this embodiment of the application, the first posterior estimate after ESKF update is... The target pose is the pose after ESKF filtering.

[0120] Furthermore, when preset triggering conditions are met (e.g., every time wheel speed meter information is received, every N times wheel speed meter information is received, after a preset time period), a pose graph will be constructed based on at least two filtered poses, and then a sliding window-style pose optimization will be performed to address any potential local non-smoothness.

[0121] Specifically, the sampling frequency of the wheel speed odometer is lower than that of the inertial measurement unit. If the time interval between two adjacent samplings of the wheel speed odometer is regarded as a time period, multiple filtered poses can be obtained within a time period. When a time period ends (i.e., after obtaining the filtered pose based on the wheel speed odometer information collected once), a pose graph can be constructed based on the filtered poses obtained within this time period. Then, based on the wheel speed odometer information collected within this time period, the pose graph can be optimized using a sliding window method.

[0122] Furthermore, for the pose map, the first timestamp of the speed in the wheel speed meter information is obtained. Then, based on the first timestamp and inertial measurement information, the relative pose between two adjacent wheel speed frames is obtained. After using the relative pose as an inter-frame constraint and performing sliding window-style pose optimization processing on the updated first posterior estimate, the target pose of the vehicle system at the second time moment is obtained.

[0123] In one implementation, to obtain the relative pose, firstly, based on inertial measurement information, the first angular velocity corresponding to the first timestamp and the zero bias of the first gyroscope can be obtained. Then, based on the velocity, the first angular velocity and the zero bias of the first gyroscope in the inertial measurement information, pre-integration is performed on two adjacent wheel speed frames to obtain the relative pose between the two wheel speed frames.

[0124] As an example, when wheel speed meter information arrives, taking the second moment as the current moment, in addition to performing ESKF updates according to steps 203-204, the wheel speed meter readings at the current moment are also updated. k timestamp t k Linear interpolation of t k The corresponding angular velocity w of the inertial measurement information k , find according to t k The most recent ESKF state variable is used to extract the corresponding gyroscope zero bias from the state variable. United v k w k ,and Perform pre-integration between two adjacent wheel speed frames to obtain the inter-frame relative pose ΔT between two adjacent wheel speed frames. ij Then use the inter-frame relative pose ΔT ij As an inter-frame constraint, for v k Pose after ESKF update and filtering For further sliding window-style pose graph optimization, see [link / reference]. Figure 3 As shown, the optimized INS pose As the target pose.

[0125] See Figure 4As shown, in a specific implementation scenario, combining GNSS, IMU, and WO, based on the above description, the system achieves target pose prediction and pose graph optimization. Specifically, on the one hand, the WO's v and GNSS's yaw are used as observations for ESKF updates to compensate for the limitations of IMU and GNSS. In other words, the information collected by IMU, GNSS, and WO is fully utilized, with IMU's a and w used for prediction, GNSS's p and yaw used for updates, and WO's v used for updates, increasing the system's robustness. On the other hand, based on WO's v, addressing the limitation that the current frame pose of the filtering framework is only related to the previous frame, a sliding window-style pose graph optimization is performed on the filtered output pose to make the trajectory smoother. In other words, for the non-smoothness of the filtered trajectory, pose graph optimization is used to further optimize the pose nodes, thereby obtaining a smoother trajectory and higher accuracy.

[0126] Based on the same inventive concept, this application also provides a vehicle pose prediction device, for use in, see [link to relevant documentation] Figure 5 The device includes:

[0127] The acquisition module 501 acquires the first posterior information of the vehicle system at the first moment. The first posterior information includes: the first posterior estimate of the system state information and the second posterior estimate of the covariance matrix of the error system state information.

[0128] The first prediction module 502 predicts the prior prediction information of the vehicle system at the second moment based on the acceleration and angular velocity in the inertial measurement information, and based on the first posterior information.

[0129] The second prediction module 503, in response to receiving satellite system information, predicts second posterior information of the vehicle system at the second time moment based on the corresponding position information and heading angle information, and based on the prior prediction information. The second posterior information includes: an updated first posterior estimate and an updated second posterior estimate. In response to receiving wheel speedometer information, predicts second posterior information of the vehicle system at the second time moment based on the corresponding speed and based on the prior prediction information. The updated first posterior estimate is used as the target pose of the vehicle at the second time moment.

[0130] Optionally, the acquisition module 501 is specifically used for:

[0131] When the first moment is the initial moment, based on the satellite system information, the initial system state information and the initial covariance matrix of the initial error system state information of the vehicle system at the initial moment are obtained;

[0132] Calculate the first posterior estimate of the initial system state information, and calculate the second posterior estimate of the initial covariance matrix;

[0133] The first posterior estimate and the second posterior estimate are used as the first posterior information of the vehicle system at the first time.

[0134] Optionally, the acquisition module 501 is specifically used for:

[0135] When the first moment is not the initial moment, based on historical data, the second posterior information generated by predicting the system state information of the vehicle system at the first moment is obtained and used as the first posterior information of the vehicle system at the first moment.

[0136] Optionally, the second prediction module 503 is further configured to, before using the updated first posterior estimate as the target pose of the vehicle at the second time moment, specifically:

[0137] In response to receiving the satellite system information and the wheel speedometer information at the same time, then:

[0138] Based on the position information and heading angle information in the satellite system information, and based on the prior prediction information, predict the second posterior information of the vehicle system at the second moment;

[0139] Based on the speed in the wheel speed meter information, the second posterior information is measured and updated to obtain the updated second posterior information, which covers the second posterior information.

[0140] or,

[0141] Based on the speed in the wheel speedometer information, and based on the prior prediction information, predict the second posterior information of the vehicle system at the second moment;

[0142] Based on the position information and heading angle information in the satellite system information, and based on the prior prediction information, the second posterior information is measured and updated to obtain the updated second posterior information, which covers the second posterior information.

[0143] Optionally, the second prediction module 503 is configured to update the second posterior information based on the corresponding speed in response to receiving wheel speed meter information, specifically for:

[0144] When wheel speed meter information is received, the speed observation is obtained based on the speed measurement noise in the wheel speed meter information and based on the system state information.

[0145] Using the error system state variables, the velocity observation is differentiated to obtain the target Jacobian matrix;

[0146] The second posterior information is updated based on the target Jacobian matrix.

[0147] Optionally, the second prediction module 503 is used to take the updated first posterior estimate as the target pose of the vehicle at the second time moment, specifically for:

[0148] Obtain the first timestamp of the speed from the wheel speed meter information;

[0149] Based on the inertial measurement information, the relative pose between two adjacent wheel speed frames is obtained for the first timestamp.

[0150] Using the relative pose as an inter-frame constraint, the updated first posterior estimate is subjected to sliding window pose optimization processing to obtain the target pose of the vehicle system at the second time moment.

[0151] Optionally, the second prediction module 503 is used to obtain the relative pose between two adjacent wheel speed frames based on the inertial measurement information for the first timestamp, specifically for:

[0152] Based on the inertial measurement information, the first angular velocity and the zero bias of the first gyroscope corresponding to the first timestamp are obtained;

[0153] Based on the velocity, the first angular velocity, and the zero bias of the first gyroscope, pre-integration is performed on two adjacent wheel speed frames to obtain the relative pose between the two wheel speed frames.

[0154] Based on the same inventive concept, this application also provides an electronic device that can realize the function of the aforementioned vehicle pose prediction device, referring to... Figure 6 The electronic device includes:

[0155] At least one processor 61 and a memory 62 connected to at least one processor 61. In this embodiment, the specific connection medium between the processor 61 and the memory 62 is not limited. Figure 6 The example shown is the connection between processor 61 and memory 62 via bus 60. Bus 60 is... Figure 6 The connections between other components are indicated by thick lines and are for illustrative purposes only, not as limiting information. Bus 60 can be divided into address bus, data bus, control bus, etc., for ease of representation. Figure 6 The term 61 is represented by a single thick line, but this does not imply that there is only one bus or one type of bus. Alternatively, the processor 61 can also be called a controller; there is no restriction on the name.

[0156] In this embodiment, memory 62 stores instructions executable by at least one processor 61. By executing the instructions stored in memory 62, at least one processor 61 can perform the vehicle pose prediction method discussed above. Processor 61 can implement... Figure 5 The functions of each module in the device / system shown.

[0157] The processor 61 is the control center of the device / system. It can connect to various parts of the control device through various interfaces and lines. By running or executing instructions stored in memory 62 and calling data stored in memory 62, it can monitor the various functions and data processing of the device / system as a whole.

[0158] In one possible design, processor 61 may include one or more processing units. Processor 61 may integrate an application processor and a modem processor, wherein the application processor mainly handles the operating system, user interface, and applications, and the modem processor mainly handles wireless communication. It is understood that the modem processor may also not be integrated into processor 61. In some embodiments, processor 61 and memory 62 may be implemented on the same chip; in some embodiments, they may also be implemented on separate chips.

[0159] Processor 61 can be a general-purpose processor, such as a central processing unit (CPU), digital signal processor, application-specific integrated circuit, field-programmable gate array or other programmable logic device, discrete gate or transistor logic device, or discrete hardware component, capable of implementing or executing the methods, steps, and logic block diagrams disclosed in the embodiments of this application. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the vehicle pose prediction method disclosed in the embodiments of this application can be directly manifested as being executed by a hardware processor, or executed by a combination of hardware and software modules within the processor.

[0160] Memory 62, as a non-volatile computer-readable storage medium, can be used to store non-volatile software programs, non-volatile computer-executable programs, and modules. Memory 62 may include at least one type of storage medium, such as flash memory, hard disk, multimedia card, card-type memory, random access memory (RAM), static random access memory (SRAM), programmable read-only memory (PROM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), magnetic storage, magnetic disk, optical disk, etc. Memory 62 can be any other medium capable of carrying or storing desired program code in the form of instructions or data structures that can be accessed by a computer, but is not limited thereto. In the embodiments of this application, memory 62 can also be a circuit or any other device / system capable of implementing storage functions for storing program instructions and / or data.

[0161] By designing and programming the processor 61, the code corresponding to the vehicle pose prediction method described in the foregoing embodiments can be embedded into the chip, enabling the chip to execute the code during runtime. Figure 2 The steps of the vehicle pose prediction method in the illustrated embodiment are as follows. How to design and program the processor 61 is a technique well-known to those skilled in the art and will not be described further here.

[0162] Based on the same inventive concept, embodiments of this application also provide a storage medium storing computer instructions that, when executed on a computer, cause the computer to perform the vehicle pose prediction method described above.

[0163] In some possible implementations, various aspects of the vehicle pose prediction method provided in this application may also be implemented in the form of a program product, which includes program code that, when the program product is run on a device, causes the control device to perform the steps in the vehicle pose prediction method according to the various exemplary embodiments of this application described above.

[0164] Those skilled in the art will understand that embodiments of this application can be provided as methods, apparatus / systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0165] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of 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, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0166] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0167] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0168] Obviously, those skilled in the art can make various modifications and variations to this application without departing from the spirit and scope of this application. Therefore, if such modifications and variations fall within the scope of the claims of this application and their equivalents, this application also intends to include such modifications and variations.

Claims

1. A method for predicting vehicle pose, characterized in that, The method includes: Obtain the first posterior information of the vehicle system at the first moment, the first posterior information including: the first posterior estimate of the system state information and the second posterior estimate of the covariance matrix of the error system state information; Based on the acceleration and angular velocity in the inertial measurement information, and based on the first posterior information, the prior prediction information of the vehicle system at the second time moment is predicted; In response to receiving satellite system information, based on the corresponding position information and heading angle information, and based on the prior prediction information, the vehicle system predicts the second posterior information at the second time moment, the second posterior information including: the updated first posterior estimate and the updated second posterior estimate; In response to receiving wheel speed meter information, acquire the speed observation from the wheel speed meter information; The target Jacobian matrix is ​​obtained by differentiating the velocity observation with respect to the error system state variables. Based on the target Jacobian matrix, the corresponding velocity, and the prior prediction information, the second posterior information of the vehicle system at the second time moment is predicted. In response to simultaneously receiving the satellite system information and the wheel speed meter information, the second posterior information is updated based on the satellite system information, and the second posterior information is measured and updated based on the wheel speed meter information to obtain the updated second posterior information; In response to receiving the satellite system information, if the inertial measurement information is not received again, and the wheel velocity meter information is received, the second posterior information is obtained after updating based on the satellite system information, and the second posterior information is measured and updated based on the wheel velocity meter information to obtain the updated second posterior information; The updated first posterior estimate is used as the target pose of the vehicle at the second time step.

2. The method as described in claim 1, characterized in that, The acquisition of the first retrospective information of the vehicle system at the first moment includes: When the first moment is the initial moment, based on the satellite system information, the initial system state information and the initial covariance matrix of the initial error system state information of the vehicle system at the initial moment are obtained; Calculate the first posterior estimate of the initial system state information, and calculate the second posterior estimate of the initial covariance matrix; The first posterior estimate and the second posterior estimate are used as the first posterior information of the vehicle system at the first time.

3. The method as described in claim 1, characterized in that, The acquisition of the first retrospective information of the vehicle system at the first moment includes: When the first moment is not the initial moment, based on historical data, the second posterior information generated by updating the system state information of the vehicle system at the first moment is obtained and used as the first posterior information of the vehicle system at the first moment.

4. The method according to any one of claims 1-3, characterized in that, The step of using the updated first posterior estimate as the target pose of the vehicle at the second time moment includes: Obtain the first timestamp of the speed from the wheel speed meter information; Based on the inertial measurement information, the relative pose between two adjacent wheel speed frames is obtained for the first timestamp. Using the relative pose as an inter-frame constraint, the updated first posterior estimate is subjected to sliding window-style pose optimization processing to obtain the target pose of the vehicle system at the second time moment.

5. The method as described in claim 4, characterized in that, The step of obtaining the relative pose between two adjacent wheel speed frames based on the inertial measurement information for the first timestamp includes: Based on the inertial measurement information, the first angular velocity and the zero bias of the first gyroscope corresponding to the first timestamp are obtained; Based on the velocity, the first angular velocity, and the zero bias of the first gyroscope, pre-integration is performed on two adjacent wheel speed frames to obtain the relative pose between the two wheel speed frames.

6. A vehicle pose prediction device, characterized in that, The device includes: The acquisition module acquires the first posterior information of the vehicle system at the first moment. The first posterior information includes: the first posterior estimate of the system state information and the second posterior estimate of the covariance matrix of the error system state information. The first prediction module predicts the prior prediction information of the vehicle system at the second moment based on the acceleration and angular velocity in the inertial measurement information, and based on the first posterior information. The second prediction module, in response to receiving satellite system information, predicts the second posterior information of the vehicle system at the second time moment based on the corresponding position information and heading angle information, and based on the prior prediction information. The second posterior information includes: an updated first posterior estimate and an updated second posterior estimate. In response to receiving wheel speedometer information, it acquires the speed observation from the wheel speedometer information; it obtains the target Jacobian matrix by differentiating the speed observation using the error system state variables, and predicts the second posterior information of the vehicle system at the second time moment based on the target Jacobian matrix, the corresponding speed, and the prior prediction information. In response to simultaneously receiving satellite system information... The information and the wheel speedometer information are used to update the prior prediction information based on the satellite system information to obtain second posterior information, and the second posterior information is updated based on the wheel speedometer information to obtain updated second posterior information; in response to receiving the satellite system information, if the inertial measurement information is not received again, and the wheel speedometer information is received, the second posterior information is updated based on the satellite system information, and the second posterior information is updated based on the wheel speedometer information to obtain updated second posterior information; the updated first posterior estimate is used as the target pose of the vehicle at the second time moment.

7. An electronic device, characterized in that, include: Memory, used to store computer programs; A processor, when executing a computer program stored in the memory, implements the method steps of any one of claims 1-5.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the steps of the method described in any one of claims 1-5.

Images