An obstacle state prediction method, apparatus, device, and storage medium

Abstract

This application relates to an obstacle state prediction method, apparatus, device, and storage medium. The main technical solution includes: acquiring initial obstacle state information, including the obstacle's initial heading angle and initial heading angular velocity; performing Kalman filtering on the initial heading angle based on a first set of preset equations using a Kalman filter to obtain corrected state information; and updating the obstacle state information based on the corrected state information using a second set of preset equations using a constant turning rate and acceleration motion model to obtain predicted state information. This application can provide more stable and accurate obstacle state information for downstream functions.

Description

Technical Field

[0001] This application relates to the field of multi-sensor fusion tracking technology, and in particular to an obstacle state prediction method, apparatus, device, and storage medium. Background Technology

[0002] Autonomous vehicles acquire data from sensors, which is then processed by perception algorithms to generate data results. This enables information exchange between vehicles, roads, and people, allowing the vehicle to automatically analyze whether its driving is in a safe or dangerous state. This allows the vehicle to achieve intelligent driving according to human intentions, ultimately replacing humans in decision-making and achieving autonomous driving goals. A key technology is ensuring that the perceived obstacle position, speed, acceleration, and heading angle are provided to downstream control systems with relatively accurate values ​​to guarantee accurate downstream decision-making.

[0003] Currently, in autonomous vehicles, Kalman filters are used to combine measurements from different sensors and the results of existing tracks to ultimately output a stable and smooth obstacle state, such as position, speed, acceleration, heading angle, and type. However, Kalman filtering depends on the accuracy of the measurements, especially the heading angle of the obstacle vehicle. Cameras, LiDAR, and millimeter-wave radar all have certain limitations in sensing the heading angle, making it difficult to guarantee the provision of stable obstacle position, speed, acceleration, heading angle, and other state information to downstream systems. Summary of the Invention

[0004] Based on this, this application provides an obstacle state prediction method, apparatus, device, and storage medium to solve the technical problem of difficulty in ensuring the provision of stable obstacle position, velocity, acceleration, heading angle, and other state information to downstream applications.

[0005] Firstly, an obstacle state prediction method is provided, the method comprising:

[0006] Obtain the initial state information of the obstacle, including the obstacle's initial heading angle and initial heading angular velocity;

[0007] Based on the initial heading angle and initial heading angular velocity, the initial heading angle is Kalman filtered by the first set of preset equations based on the Kalman filter to obtain the corrected state information.

[0008] Based on the corrected state information, the state information of the obstacle is updated using the second set of preset equations based on the constant turning rate and acceleration motion model, thus obtaining the predicted state information.

[0009] According to one achievable method in an embodiment of this application, the first preset equation set includes a state equation, a first measurement equation, a first prediction equation, a first Kalman gain equation, and a first Kalman filter equation set; based on the initial heading angle and initial heading angular velocity, the initial heading angle is Kalman filtered using the preset equations of the Kalman filter to obtain corrected state information, including:

[0010] Based on the initial heading angle and initial heading angular velocity at the current moment, determine the first state vector for the next moment based on the state equation;

[0011] Based on the initial heading angle and initial heading angular velocity at the current moment, the first measurement vector at the current moment is determined based on the first measurement equation;

[0012] Based on the first state matrix at the current moment, the first state matrix at the next moment is determined based on the first prediction equation. The first state matrix at the current moment is obtained recursively from the first initial error covariance matrix.

[0013] The first Kalman gain is determined based on the first state matrix and the first Kalman gain equation.

[0014] Based on the first state vector at the next time step, the first measurement vector at the current time step, the first state matrix at the next time step, and the first Kalman gain, the optimal estimated first state quantity and the optimal estimated first state matrix are determined based on the first Kalman filter equations. The optimal estimated first state quantity and the optimal estimated first state matrix constitute the corrected state information.

[0015] According to one feasible embodiment of this application, the state equation and the first measurement equation are respectively expressed as the following formulas:

[0016] X k1+1 =F*X k1 +Q1

[0017] z k1 =H1X k1 +R1

[0018] Among them, X k1+1 Let X represent the first state vector at the next moment. k1 =[yaw1,v_yaw1] represents the state vector consisting of the initial heading angle yaw1 and initial heading angular velocity v_yaw1 of the obstacle at the current moment, and the measurement vector Z. k1 =[yaw1] represents the measurement vector composed of the initial heading angles of the obstacle at the current moment, F represents the state transition matrix, H1 represents the first measurement matrix, Q1 represents the first system noise autocovariance matrix, and R1 represents the first measurement noise autocovariance matrix;

[0019] The first prediction equation is expressed as the following formula:

[0020] P k1+1 =F*P k1 *F -1 +Q1

[0021] Among them, P k1+1 Let P represent the first state matrix at the next time step. k1 Let F represent the first state matrix at the current time. -1 Let P be the inverse of the state transition matrix. k1 The first initial error covariance matrix is ​​obtained recursively from the first initial error covariance matrix and is set as follows:

[0022]

[0023] According to one possible implementation method in an embodiment of this application, the first Kalman gain equation is expressed as the following formula:

[0024] S1=H1*P k1+1 *H1 -1 +R1

[0025] G1 = P k1+1 *H1 -1 *S1 -1

[0026] Where G1 is the first Kalman gain, H1 -1 S1 represents the inverse of the first measurement matrix, and S1 is the first intermediate variable matrix for calculating the first Kalman gain. -1 The matrix representing the inverse of the first intermediate variable matrix;

[0027] The first Kalman filter equation set can be expressed as follows:

[0028]

[0029]

[0030] in, This represents the first state variable in the optimal estimate. Let I represent the first state matrix of the optimal estimate, and let I represent the identity matrix.

[0031] According to one achievable method in an embodiment of this application, the corrected state information includes the x-coordinate and y-coordinate of the obstacle's position, the corrected heading angle, the corrected heading angular velocity change rate, and acceleration. The second preset equation set includes the motion equation, the second measurement equation, the second prediction equation, the second Kalman gain equation, and the second Kalman filter equation set. Based on the corrected state information, the obstacle's state information is updated using the second preset equation of the constant turning rate and acceleration motion model to obtain the predicted state information, including: determining the second state vector for the next moment based on the motion equation according to the current obstacle's x-coordinate, y-coordinate, corrected heading angle, corrected heading angular velocity change rate, and acceleration.

[0032] Based on the obstacle's current position's x-coordinate, y-coordinate, corrected heading angle, corrected heading angular velocity change rate, and acceleration, the second measurement vector at the current moment is determined based on the second measurement equation.

[0033] Based on the second state matrix at the current time, the second state matrix at the next time is determined based on the second prediction equation. The second state matrix at the current time is recursively obtained from the second initial error covariance matrix.

[0034] The second Kalman gain is obtained based on the second state matrix and the second Kalman gain equation.

[0035] Based on the second state vector at the next time step, the second measurement vector at the current time step, the second state matrix at the next time step, and the second Kalman gain, the optimal estimated second state quantity and the optimal estimated second state matrix are determined based on the second Kalman filter equations. The optimal estimated second state quantity and the optimal estimated second state matrix constitute the corrected state information.

[0036] According to one feasible embodiment of this application, the motion equation and the second measurement equation are respectively expressed as the following formulas:

[0037]

[0038] Z k2 =H2X k2 +R2

[0039] Among them, X k2+1 The second state vector, X, represents the state vector at the next moment. k2 =[x,y,v,yaw2,w,a] represents the state vector consisting of the current obstacle's x-coordinate, y-coordinate, velocity v, corrected heading angle yaw2, corrected heading angular velocity change rate w, and acceleration a, and the measurement vector Z. k2=[x,y,v,yaw2,w,a] represents a measurement vector consisting of the x-coordinate, y-coordinate, velocity v, corrected heading angle yaw2, corrected heading angular velocity change rate w, and acceleration a at the current obstacle position. H2 represents the second measurement matrix, Q2 represents the second system noise autocovariance matrix, R2 represents the second measurement noise autocovariance matrix, and v(t) k ) represents t k The velocity of the obstacle at time t, cos(θ(t) k )) represents t k The cosine of the obstacle's heading angle at time t, sin(θ(t) k )) represents t k The sine of the obstacle's heading angle at any given moment, where Δt represents the time interval. Time to t k+1 The time interval is a single integration interval;

[0040] The second prediction equation is expressed as the following formula:

[0041] P k2+1 =J*P k2 *J -1 +Q2

[0042] Among them, P k2+1 P represents the second state matrix at the next time step. k2 Let X represent the second state matrix at the current time, J be the Jacobian matrix, and X be the second state matrix at the current time. k2+1 For X k2 The derivative is obtained, where P k2 The second initial error covariance matrix is ​​obtained recursively and is set as follows:

[0043]

[0044] According to one possible implementation method in an embodiment of this application, the second Kalman gain equation is expressed as the following formula:

[0045] S2=H2*P k2+1 *H2 -1 +R2

[0046] G2 = P k2+1 *H2 -1 *S2 -1

[0047] Where G2 is the second Kalman gain, H1 -1 S2 is the inverse matrix of the second measurement matrix, and S2 is the second intermediate variable matrix for calculating the second Kalman gain. -1 This represents the inverse matrix of the second intermediate variable matrix;

[0048] The second Kalman filter equation set can be expressed as follows:

[0049]

[0050]

[0051] in, This represents the second state variable in the optimal estimate. Let I represent the second state matrix of the optimal estimate, and let I represent the identity matrix.

[0052] Secondly, an obstacle state prediction device is provided, the device comprising:

[0053] The acquisition module is used to acquire the initial state information of the obstacle, including the initial heading angle and initial heading angular velocity of the obstacle;

[0054] The filtering module is used to perform Kalman filtering on the initial heading angle based on the first preset equation set of the Kalman filter according to the initial heading angle and initial heading angular velocity, so as to obtain the corrected state information;

[0055] The prediction module is used to update the obstacle's state information based on the second preset equation set of the constant turning rate and acceleration motion model, according to the corrected state information, to obtain the predicted state information.

[0056] Thirdly, a computer device is provided, comprising:

[0057] At least one processor; and

[0058] A memory communicatively connected to the at least one processor; wherein,

[0059] The memory stores computer instructions that can be executed by the at least one processor to enable the at least one processor to perform the method involved in the first aspect above.

[0060] Fourthly, a computer-readable storage medium is provided, having stored thereon computer instructions, wherein the computer instructions are used to cause a computer to perform the methods involved in the first aspect above.

[0061] According to the technical content provided in the embodiments of this application, the initial state information of the obstacle is obtained. The initial state information includes the initial heading angle and initial heading angular velocity of the obstacle. Based on the initial heading angle and initial heading angular velocity, Kalman filtering is performed on the initial heading angle based on the first preset equation set of the Kalman filter to obtain corrected state information. Based on the corrected state information, the state information of the obstacle is updated based on the second preset equation set of the constant turning rate and acceleration motion model to obtain predicted state information. This can provide more stable and accurate obstacle state information for downstream functions. Attached Figure Description

[0062] Figure 1 This is an application environment diagram of the obstacle state prediction method in one embodiment;

[0063] Figure 2 This is a flowchart illustrating an obstacle state prediction method in one embodiment;

[0064] Figure 3 This is a structural block diagram of an obstacle state prediction device in one embodiment;

[0065] Figure 4 This is a schematic structural diagram of a computer device in one embodiment. Detailed Implementation

[0066] The present application will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the present application and are not intended to limit the scope of the present application.

[0067] For ease of understanding, the system to which this application applies will first be described. The obstacle state prediction method provided in this application can be applied to, for example... Figure 1 In the system architecture shown, vehicle 100 includes an on-board terminal 110. The on-board terminal 110 acquires initial state information of the obstacle, including the obstacle's initial heading angle and initial heading angular velocity. Based on the initial heading angle and initial heading angular velocity, Kalman filtering is performed on the initial heading angle using a first set of preset equations based on a Kalman filter to obtain corrected state information. Based on the corrected state information, the obstacle's state information is updated using a second set of preset equations based on a constant turning rate and acceleration motion model to obtain predicted state information. The on-board terminal 110 can be, but is not limited to, various personal computers or laptops connected to the vehicle.

[0068] Figure 2 A flowchart illustrating an obstacle state prediction method provided in this application embodiment, the method can be performed by, for example... Figure 1 The vehicle-mounted terminal 110 in the system shown executes this. For example... Figure 2 As shown, the method may include the following steps:

[0069] S210, Obtain the initial state information of the obstacle.

[0070] Obstacles refer to vehicles, pedestrians, signs, etc., around the autonomous vehicle that affect its normal driving. Initial state information refers to the state information of obstacles collected by the autonomous vehicle through the data acquisition equipment, including initial heading angle, initial heading angular velocity, velocity, acceleration, position information, etc.

[0071] The data acquisition equipment consists of vehicle-mounted sensors, including cameras, lidar, millimeter-wave radar, etc., to perceive and identify the environment around the vehicle, including road signs, traffic signals, obstacles, vehicles, and pedestrians. These sensors can provide a wealth of information, thus enabling a comprehensive understanding of the vehicle's driving environment.

[0072] S220: Based on the initial heading angle and initial heading angular velocity, Kalman filtering is performed on the initial heading angle using the first preset equation set of the Kalman filter to obtain the corrected state information.

[0073] The first set of preset equations includes the state equation, the first measurement equation, the first prediction equation, the first Kalman gain equation, and the first Kalman filter equation.

[0074] When the data acquisition device collects information about the environment around the vehicle, it may be affected by other factors, which may cause errors in the initial heading angle. The initial heading angle can be corrected by Kalman filtering using the first preset equation set of the Kalman filter, thereby obtaining a more reliable heading angle for the obstacle.

[0075] The initial heading angle and initial heading angular velocity are substituted into the first preset equation set of the Kalman filter for iterative calculation to obtain the corrected state information. The corrected state information includes the x-coordinate and y-coordinate of the obstacle position, the corrected heading angle, the rate of change of the corrected heading angular velocity, and the acceleration.

[0076] S230: Based on the corrected state information, the state information of the obstacle is updated using the second preset equation set of the constant turning rate and acceleration motion model to obtain the predicted state information.

[0077] The second set of preset equations includes the equations of motion, the second measurement equation, the second prediction equation, the second Kalman gain equation, and the second Kalman filter equation.

[0078] The corrected heading angle is used as one of the measurement parameters of the constant turning rate and acceleration motion model, and Kalman update is performed on the constant turning rate and acceleration motion model to output more stable and accurate information such as the speed, acceleration, heading angle and position of the obstacle.

[0079] The corrected state information is substituted into the second preset equation set of the constant turning rate and acceleration motion model for iterative calculation to obtain the predicted state information. The corrected state information includes the x-coordinate, y-coordinate, heading angle, rate of change of heading angular velocity, and acceleration of the obstacle position.

[0080] As can be seen, the embodiments of this application obtain the initial state information of the obstacle, including the initial heading angle and initial heading angular velocity of the obstacle. Based on the initial heading angle and initial heading angular velocity, Kalman filtering is performed on the initial heading angle based on the first preset equation set of the Kalman filter to obtain corrected state information. Based on the corrected state information, the state information of the obstacle is updated based on the second preset equation set of the constant turning rate and acceleration motion model to obtain predicted state information. This can provide more stable and accurate obstacle state information for downstream functions.

[0081] As one feasible approach, based on the initial heading angle and initial heading angular velocity, Kalman filtering is applied to the initial heading angle using a predefined equation of the Kalman filter to obtain corrected state information, including:

[0082] Based on the initial heading angle and initial heading angular velocity at the current moment, determine the first state vector for the next moment based on the state equation;

[0083] Based on the initial heading angle and initial heading angular velocity at the current moment, the first measurement vector at the current moment is determined based on the first measurement equation;

[0084] Based on the first state matrix at the current moment, the first state matrix at the next moment is determined based on the first prediction equation. The first state matrix at the current moment is obtained recursively from the first initial error covariance matrix.

[0085] The first Kalman gain is determined based on the first state matrix and the first Kalman gain equation.

[0086] Based on the first state vector at the next time step, the first measurement vector at the current time step, the first state matrix at the next time step, and the first Kalman gain, the optimal estimated first state quantity and the optimal estimated first state matrix are determined based on the first Kalman filter equations. The optimal estimated first state quantity and the optimal estimated first state matrix constitute the corrected state information.

[0087] Specifically, the state equation and the first measurement equation are expressed as the following formulas:

[0088] X k1+1 =F*X k1 +Q1

[0089] z k1 =H1X k1 +R1

[0090] Among them, X k1+1 Let X represent the first state vector at the next moment. k1 =[yaw1,v_yaw1] represents the state vector consisting of the initial heading angle yaw1 and initial heading angular velocity v_yaw1 of the obstacle at the current moment, and the measurement vector Z. k1 =[yaw1] represents the measurement vector composed of the initial heading angles of the obstacle at the current moment, F represents the state transition matrix, H1 represents the first measurement matrix, Q1 represents the first system noise autocovariance matrix, and R1 represents the first measurement noise autocovariance matrix.

[0091] The first prediction equation is expressed as the following formula:

[0092] P k1+1 =F*P k1 *F -1 +Q1

[0093] Among them, P k1+1 Let P represent the first state matrix at the next time step. k1 Let F represent the first state matrix at the current time. -1 Let P be the inverse of the state transition matrix. k1 The first initial error covariance matrix is ​​obtained recursively from the first initial error covariance matrix and is set as follows:

[0094]

[0095] The state transition matrix can be set as follows:

[0096]

[0097] Where dt is the time interval between time k1+1 and time k1.

[0098] The first measurement matrix can be set as follows:

[0099] H1 = (1 0)

[0100] The first Kalman gain equation is expressed as the following formula:

[0101] S1=H1*P k1+1 *H1 -1 +R1

[0102] G1 = P k1+1 *H1 -1 *S1 -1

[0103] Where G1 is the first Kalman gain, H1 -1S1 represents the inverse of the first measurement matrix, and S1 is the first intermediate variable matrix for calculating the first Kalman gain. -1 This represents the inverse matrix of the first intermediate variable matrix.

[0104] The first Kalman filter equation set can be expressed as follows:

[0105]

[0106]

[0107] in, This represents the first state variable in the optimal estimate. Let I represent the first state matrix of the optimal estimate, and let I represent the identity matrix.

[0108] Before applying Kalman filtering to the initial heading angle based on the preset equations of the Kalman filter, the Kalman filter is first initialized. Variables requiring initialization include the interval time, the initial heading angle and initial heading angular velocity of the obstacle, the first system noise autocovariance matrix, the first measurement noise autocovariance matrix, and the first initial error covariance matrix. These variables can be obtained from the obstacle state information acquired in the first two frames. The interval time, the obstacle's heading angle, and the heading angular velocity are initialized using the state information acquired in the first two frames. The system noise autocovariance matrix is ​​continuously updated with the calculation of state information in each frame; therefore, a relatively large initial value is given to the first system noise autocovariance matrix.

[0109] The first system noise autocovariance matrix is ​​initialized as follows:

[0110]

[0111] The first measurement noise autocovariance matrix is ​​initialized as follows:

[0112]

[0113] As an achievable method, based on the corrected state information, the state information of the obstacle is updated according to the second preset equation of the constant turning rate and acceleration motion model to obtain the predicted state information, including: based on the x-coordinate, y-coordinate, corrected heading angle, corrected heading angular velocity change rate and acceleration of the obstacle's position at the current moment, the second state vector at the next moment is determined based on the motion equation;

[0114] Based on the obstacle's current position's x-coordinate, y-coordinate, corrected heading angle, corrected heading angular velocity change rate, and acceleration, the second measurement vector at the current moment is determined based on the second measurement equation.

[0115] Based on the second state matrix at the current time, the second state matrix at the next time is determined based on the second prediction equation. The second state matrix at the current time is recursively obtained from the second initial error covariance matrix.

[0116] The second Kalman gain is obtained based on the second state matrix and the second Kalman gain equation.

[0117] Based on the second state vector at the next time step, the second measurement vector at the current time step, the second state matrix at the next time step, and the second Kalman gain, the optimal estimated second state quantity and the optimal estimated second state matrix are determined based on the second Kalman filter equations. The optimal estimated second state quantity and the optimal estimated second state matrix constitute the corrected state information.

[0118] Specifically, the equations of motion and the second measurement equations are expressed as follows:

[0119]

[0120] Z k2 =H2X k2 +R2

[0121] Among them, X k2+1 The second state vector, X, represents the state vector at the next moment. k2 =[x,y,v,yaw2,w,a] represents the state vector consisting of the current obstacle's x-coordinate, y-coordinate, velocity v, corrected heading angle yaw2, corrected heading angular velocity change rate w, and acceleration a, and the measurement vector Z. k2 =[x,y,v,yaw2,w,a] represents a measurement vector consisting of the x-coordinate, y-coordinate, velocity v, corrected heading angle yaw2, corrected heading angular velocity change rate w, and acceleration a at the current obstacle position. H2 represents the second measurement matrix, Q2 represents the second system noise autocovariance matrix, R2 represents the second measurement noise autocovariance matrix, and v(t) k ) represents t k The velocity of the obstacle at time t, cos(θ(t) k )) represents t k The cosine of the obstacle's heading angle at time t, sin(θ(t) k )) represents t k The sine of the obstacle's heading angle at time Δt k Indicates t k Time and t k+1 The time interval, t k Time to t k+1 Let time be an integration interval, where t k+1 Greater than t k , t k The time corresponds to time k2, tk+1 The time corresponds to the time k2+1.

[0122] The second measurement matrix can be set as follows:

[0123]

[0124] The second prediction equation is expressed as the following formula:

[0125] P k2+1 =J*P k2 *J -1 +Q2

[0126] Among them, P k2+1 P represents the second state matrix at the next time step. k2 Let X represent the second state matrix at the current time, J be the Jacobian matrix, and X be the second state matrix at the current time. k2+1 For X k2 The derivative is obtained, where P k2 The second initial error covariance matrix is ​​obtained recursively and is set as follows:

[0127]

[0128] The second Kalman gain equation is expressed as the following formula:

[0129] S2=H2*P k2+1 *H2 -1 +R2

[0130] G2 = P k2+1 *H2 -1 *S2 -1

[0131] Where G2 is the second Kalman gain, H1 -1 S2 is the inverse matrix of the second measurement matrix, and S2 is the second intermediate variable matrix for calculating the second Kalman gain. -1 This represents the inverse matrix of the second intermediate variable matrix;

[0132] The second Kalman filter equation set can be expressed as follows:

[0133]

[0134]

[0135] in, This represents the second state variable in the optimal estimate. Let I represent the second state matrix of the optimal estimate, and let I represent the identity matrix.

[0136] Before updating the obstacle's state information using the second preset equation based on the constant turning rate and acceleration motion model, the constant turning rate and acceleration motion model is first initialized. Variables requiring initialization include the interval time, obstacle position information, initial velocity, initial heading angle, initial heading angular velocity, second system noise autocovariance matrix, second measurement noise autocovariance matrix, and second initial error covariance matrix. These variables can be obtained from the obstacle state information collected in the first two frames. The interval time, obstacle position information, initial velocity, initial heading angle, and initial heading angular velocity are calculated from the state information collected in the first two frames. The system noise autocovariance matrix is ​​continuously updated with the calculation of state information in each frame; therefore, a relatively large initial value is given to the second system noise autocovariance matrix.

[0137] The second system noise autocovariance matrix is ​​initialized as follows:

[0138]

[0139] The second measurement noise autocovariance matrix is ​​initialized as follows:

[0140]

[0141] It should be understood that, although Figure 2 The steps in the flowchart are shown sequentially as indicated by the arrows, but these steps are not necessarily executed in the order indicated by the arrows. Unless explicitly stated in this application, there is no strict order restriction on the execution of these steps, and they can be executed in other orders. Furthermore, Figure 2 At least some of the steps in the process may include multiple sub-steps or multiple stages. These sub-steps or stages are not necessarily completed at the same time, but can be executed at different times. The execution order of these sub-steps or stages is not necessarily sequential, but can be executed in turn or alternately with other steps or at least some of the sub-steps or stages of other steps.

[0142] Figure 3 This is a schematic diagram of an obstacle state prediction device provided in an embodiment of this application. The device can be installed in... Figure 1 The vehicle-mounted terminal 110 in the system shown is used to perform, for example... Figure 2 The method flow is shown below. Figure 3 As shown, the obstacle state prediction device 300 may include: an acquisition module 310, a filtering module 320, and a prediction module 330. The main functions of each component module are as follows:

[0143] The acquisition module 310 is used to acquire the initial state information of the obstacle, which includes the initial heading angle and initial heading angular velocity of the obstacle.

[0144] The filtering module 320 is used to perform Kalman filtering on the initial heading angle based on the first preset equation set of the Kalman filter according to the initial heading angle and the initial heading angular velocity, so as to obtain the corrected state information;

[0145] The prediction module 330 is used to update the state information of the obstacle based on the second preset equation set of the constant turning rate and acceleration motion model according to the corrected state information, so as to obtain the predicted state information.

[0146] As one possible implementation, the filtering module 320 is specifically used for: determining the first state vector for the next moment based on the initial heading angle and initial heading angular velocity at the current moment, according to the state equation; determining the first measurement vector for the current moment based on the first measurement equation, according to the initial heading angle and initial heading angular velocity at the current moment; determining the first state matrix for the next moment based on the first prediction equation, the first state matrix for the current moment being recursively obtained from the first initial error covariance matrix; determining the first Kalman gain based on the first Kalman gain equation, according to the first state matrix; and determining the optimally estimated first state quantity and the optimally estimated first state matrix based on the first Kalman filter equation set, according to the first state vector for the next moment, the first measurement vector for the current moment, the first state matrix for the next moment, and the first Kalman gain, the optimally estimated first state quantity and the optimally estimated first state matrix forming the corrected state information.

[0147] As one feasible approach, the state equation and the first measurement equation are expressed as the following formulas:

[0148] X k1+1 =F*X k1 +Q1

[0149] z k1 =H1X k1 +R1

[0150] Among them, X k1+1 Let X represent the first state vector at the next moment. k1 =[yaw1,v_yaw1] represents the state vector consisting of the initial heading angle yaw1 and initial heading angular velocity v_yaw1 of the obstacle at the current moment, and the measurement vector Z. k1 =[yaw1] represents the measurement vector composed of the initial heading angles of the obstacle at the current moment, F represents the state transition matrix, H1 represents the first measurement matrix, Q1 represents the first system noise autocovariance matrix, and R1 represents the first measurement noise autocovariance matrix;

[0151] The first prediction equation is expressed as the following formula:

[0152] Pk1+1 =F*P k1 *F -1 +Q1

[0153] Among them, P k1+1 Let P represent the first state matrix at the next time step. k1 Let F represent the first state matrix at the current time. -1 Let P be the inverse of the state transition matrix. k1 The first initial error covariance matrix is ​​obtained recursively from the first initial error covariance matrix and is set as follows:

[0154]

[0155] According to one possible implementation method in an embodiment of this application, the first Kalman gain equation is expressed as the following formula:

[0156] S1=H1*P k1+1 *H1 -1 +R1

[0157] G1 = P k1+1 *H1 -1 *S1 -1

[0158] Where G1 is the first Kalman gain, H1 -1 S1 represents the inverse of the first measurement matrix, and S1 is the first intermediate variable matrix for calculating the first Kalman gain. -1 The matrix representing the inverse of the first intermediate variable matrix;

[0159] The first Kalman filter equation set can be expressed as follows:

[0160]

[0161]

[0162] in, This represents the first state variable in the optimal estimate. Let I represent the first state matrix of the optimal estimate, and let I represent the identity matrix.

[0163] As one feasible approach, the corrected state information includes the x-coordinate and y-coordinate of the obstacle's position, the corrected heading angle, the corrected heading angular velocity change rate, and acceleration. The second preset equation set includes the motion equation, the second measurement equation, the second prediction equation, the second Kalman gain equation, and the second Kalman filter equation set. The prediction module 330 is specifically used to: determine the second measurement vector at the current moment based on the second measurement equation, according to the obstacle's current x-coordinate, y-coordinate, corrected heading angle, corrected heading angular velocity change rate, and acceleration.

[0164] Based on the second state matrix at the current time, the second state matrix at the next time is determined based on the second prediction equation. The second state matrix at the current time is recursively obtained from the second initial error covariance matrix. Based on the second state matrix, the second Kalman gain is obtained based on the second Kalman gain equation. Based on the second state vector at the next time, the second measurement vector at the current time, the second state matrix at the next time, and the second Kalman gain, the optimal estimated second state quantity and the optimal estimated second state matrix are determined based on the second Kalman filter equation set. The optimal estimated second state quantity and the optimal estimated second state matrix constitute the corrected state information.

[0165] As one feasible approach, the equations of motion and the second measurement equations are expressed as follows:

[0166]

[0167] Z k2 =H2X k2 +R2

[0168] Among them, X k2+1 The second state vector, X, represents the state vector at the next moment. k2 =[x,y,v,yaw2,w,a] represents the state vector consisting of the current obstacle's x-coordinate, y-coordinate, velocity v, corrected heading angle yaw2, corrected heading angular velocity change rate w, and acceleration a, and the measurement vector Z. k2 =[x,y,v,yaw2,w,a] represents a measurement vector consisting of the x-coordinate, y-coordinate, velocity v, corrected heading angle yaw2, corrected heading angular velocity change rate w, and acceleration a at the current obstacle position. H2 represents the second measurement matrix, Q2 represents the second system noise autocovariance matrix, R2 represents the second measurement noise autocovariance matrix, and v(t) k ) represents t k The velocity of the obstacle at time t, cos(θ(t) k )) represents t k The cosine of the obstacle's heading angle at time t, sin(θ(t) k )) represents t k The sine of the obstacle's heading angle at time Δt k t represents the time interval. k Time to t k+1 The time interval is a single integration interval;

[0169] The second prediction equation is expressed as the following formula:

[0170] P k2+1 =j*P k2 *J -1 +Q2

[0171] Among them, P k2+1 P represents the second state matrix at the next time step. k2 Let X represent the second state matrix at the current time, J be the Jacobian matrix, and X be the second state matrix at the current time. k2+1 For X k2 The derivative is obtained, where P k2 The second initial error covariance matrix is ​​obtained recursively and is set as follows:

[0172]

[0173] As one possible approach, the second Kalman gain equation can be expressed as the following formula:

[0174] S2=H2*P k2+1 *H2 -1 +R2

[0175] G2 = P k2+1 *H2 -1 *S2 -1

[0176] Where G2 is the second Kalman gain, H1 -1 S2 is the inverse matrix of the second measurement matrix, and S2 is the second intermediate variable matrix for calculating the second Kalman gain. -1 This represents the inverse matrix of the second intermediate variable matrix;

[0177] The second Kalman filter equation set can be expressed as follows:

[0178]

[0179]

[0180] in, This represents the second state variable in the optimal estimate. Let I represent the second state matrix of the optimal estimate, and let I represent the identity matrix.

[0181] The same or similar parts among the above embodiments can be referred to interchangeably. Each embodiment focuses on describing the differences from other embodiments. In particular, the device embodiments are basically similar to the method embodiments, so the description is relatively simple, and the relevant parts can be referred to the description of the method embodiments.

[0182] It should be noted that the embodiments of this application may involve the use of user data. In practical applications, user-specific personal data may be used in the scheme described herein within the scope permitted by applicable laws and regulations, provided that it complies with the applicable laws and regulations of the country (e.g., explicit consent from the user, actual notification to the user, explicit authorization from the user, etc.).

[0183] According to embodiments of this application, this application also provides a computer device and a computer-readable storage medium.

[0184] like Figure 4 The diagram shown is a block diagram of a computer device according to an embodiment of this application. The term "computer device" is intended to represent various forms of digital computers or mobile devices. The digital computer may include a desktop computer, a portable computer, a workbench, a personal digital assistant, a server, a mainframe computer, and other suitable computers. The mobile device may include a tablet computer, a smartphone, a wearable device, etc.

[0185] like Figure 4 As shown, the computer device 400 includes a computing unit 401, a ROM 402, a RAM 403, a bus 404, and an input / output (I / O) interface 405. The computing unit 401, ROM 402, and RAM 403 are interconnected via the bus 404. The input / output (I / O) interface 405 is also connected to the bus 404.

[0186] The computing unit 401 can execute various processes in the method embodiments of this application according to computer instructions stored in the read-only memory (ROM) 402 or computer instructions loaded from the storage unit 408 into the random access memory (RAM) 403. The computing unit 401 can be various general-purpose and / or special-purpose processing components with processing and computing capabilities. The computing unit 401 can include, but is not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. In some embodiments, the methods provided in the embodiments of this application can be implemented as computer software programs, which are tangibly contained in a computer-readable storage medium, such as the storage unit 408.

[0187] RAM 403 can also store various programs and data required for the operation of computer device 400. Part or all of the computer program can be loaded and / or installed on computer device 400 via ROM 402 and / or communication unit 409.

[0188] The input unit 406, output unit 407, storage unit 408, and communication unit 409 in the computer device 400 can be connected to the I / O interface 405. The input unit 406 can be, for example, a keyboard, mouse, touchscreen, or microphone; the output unit 407 can be, for example, a monitor, speaker, or indicator light. The computer device 400 can exchange information and data with other devices through the communication unit 409.

[0189] It should be noted that the device may also include other components necessary for normal operation. It may also include only the components necessary for implementing the solution of this application, without necessarily including all the components shown in the figures.

[0190] Various implementations of the systems and techniques described herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SOCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof.

[0191] The computer instructions used to implement the methods of this application may be written in any combination of one or more programming languages. These computer instructions may be provided to the computing unit 401 such that when executed by the computing unit 401, such as a processor, the computer instructions cause the execution of the steps involved in the embodiments of the methods of this application.

[0192] The computer-readable storage medium provided in this application can be a tangible medium that can contain or store computer instructions for performing the steps involved in the method embodiments of this application. The computer-readable storage medium can be, but is not limited to, electronic, magnetic, optical, electromagnetic, and other forms of storage media.

[0193] The specific embodiments described above do not constitute a limitation on the scope of protection of this application. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this application should be included within the scope of protection of this application.

Claims

1. An obstacle state prediction method characterized by, The method comprises: obtaining initial state information of the obstacle, the initial state information comprising an initial heading angle and an initial heading angle velocity of the obstacle; performing Kalman filtering on the initial heading angle based on a first preset equation set of a Kalman filter according to the initial heading angle and the initial heading angle velocity, to obtain corrected state information; updating state information of the obstacle based on a second preset equation set of a constant turning rate and acceleration motion model according to the corrected state information, to obtain predicted state information.

2. The method of claim 1, wherein, The first preset equation set comprises a state equation, a first measurement equation, a first prediction equation, a first Kalman gain equation and a first Kalman filtering equation set; and the Kalman filtering on the initial heading angle based on the preset equation of the Kalman filter according to the initial heading angle and the initial heading angle velocity to obtain the corrected state information comprises: determining a first state vector at a next time based on the state equation according to the initial heading angle and the initial heading angle velocity at a current time; determining a first measurement vector at the current time based on the first measurement equation according to the initial heading angle and the initial heading angle velocity at the current time; determining a first state matrix at the next time based on the first prediction equation according to a first state matrix at the current time, the first state matrix at the current time being obtained by recursion from a first initial error covariance matrix; determining a first Kalman gain based on the first Kalman gain equation according to the first state matrix; determining an optimally estimated first state quantity and an optimally estimated first state matrix based on the first Kalman filtering equation set according to the first state vector at the next time, the first measurement vector at the current time, the first state matrix at the next time and the first Kalman gain, the optimally estimated first state quantity and the optimally estimated first state matrix constituting the corrected state information.

3. The method of claim 2, wherein, The state equation and the first measurement equation are respectively represented by the following formulas: X k1+1 = F * X k1 + Q1 z k1 = H1X k1 + R1 wherein X k1+1 represents the first state vector at the next time, the state vector X k1 = [yaw1, v_yaw1] represents a state vector composed of an initial yaw angle yaw1 and an initial yaw angular velocity v_yaw1 of the obstacle at the current time, the measurement vector Z k1 = [yaw1] represents a measurement vector composed of an initial yaw angle yaw1 of the obstacle at the current time, F represents a state transition matrix, H1 represents a first measurement matrix, Q1 represents a first system noise autocovariance matrix, and R1 represents a first measurement noise autocovariance matrix; The first prediction equation is represented by the following formula: P k1+1 = F * P k1 * F -1 + Q1 wherein P k1+1 denotes the first state matrix of the next time instant, P k1 denotes the first state matrix of the current time instant, F -1 denotes the inverse matrix of the state transition matrix, wherein P k1 is obtained by recursion from a first initial error covariance matrix, which is set to:

4. The method of claim 2, wherein, The first Kalman gain equation is represented by the following formula: S1 = H1 * P k1+1 *H1 -1 +R1 G1 = P k1+1 *H1 -1 *S1 -1 wherein G1is a first Kalman gain, H1 -1 denotes an inverse matrix of a first measurement matrix, S1is a first intermediate variable matrix for calculating the first Kalman gain, S1 -1 denotes an inverse matrix of the first intermediate variable matrix; The first Kalman filtering equation set is represented by the following formula: wherein denotes the first state quantity of the optimal estimate, denotes the first state matrix of the optimal estimate, I denotes the identity matrix.

5. The method of claim 1, wherein, The corrected state information comprises a horizontal coordinate, a vertical coordinate, a corrected heading angle, a corrected heading angle velocity change rate and an acceleration of the obstacle position, and the second preset equation set comprises a motion equation, a second measurement equation, a second prediction equation, a second Kalman gain equation and a second Kalman filtering equation set; The updating of the state information of the obstacle based on the second preset equation of the constant turning rate and acceleration motion model according to the corrected state information to obtain the predicted state information comprises: determining a second state vector at a next time based on the motion equation according to a horizontal coordinate, a vertical coordinate, a corrected heading angle, a corrected heading angle velocity change rate and an acceleration of the obstacle position at a current time; determining a second measurement vector at the current time based on the second measurement equation according to the horizontal coordinate, the vertical coordinate, the corrected heading angle, the corrected heading angle velocity change rate and the acceleration of the obstacle position at the current time; and determining a second state matrix at the next time based on the second prediction equation according to a second state matrix at the current time, the second state matrix at the current time being obtained by recursion from a second initial error covariance matrix. determining a second state matrix of a next time based on the second prediction equation according to the second state matrix of the current time, the second state matrix of the current time being recursively obtained from a second initial error covariance matrix; obtaining a second Kalman gain based on the second Kalman gain equation according to the second state matrix; determining an optimal estimated second state quantity and an optimal estimated second state matrix based on a second Kalman filtering equation set according to the second state vector of the next time, the second measurement vector of the current time, the second state matrix of the next time and the second Kalman gain, the optimal estimated second state quantity and the optimal estimated second state matrix constituting the modified state information.

6. The method of claim 5, wherein, The motion equation and the second measurement equation are respectively represented by the following formulas: Z k2 = H2X k2 + R2 wherein X k2+1 represents the second state vector at the next time, the state vector X k2 = [x, y, v, yaw2, w, a] represents a state vector composed of the x coordinate, the y coordinate, the velocity v, the corrected yaw angle yaw2, the corrected yaw angle velocity change rate w, and the acceleration a of the obstacle position at the current time, the measurement vector Z k2 = [x, y, v, yaw2, w, a] represents a measurement vector composed of the x coordinate, the y coordinate, the velocity v, the corrected yaw angle yaw2, the corrected yaw angle velocity change rate w, and the acceleration a of the obstacle position at the current time, H2 represents the second measurement matrix, Q2 represents the second system noise autocovariance matrix, R2 represents the second measurement noise autocovariance matrix, v(t k ) represents the velocity of the obstacle at time t k , cos(θ(t k )) represents the cosine value of the yaw angle of the obstacle at time t k , sin(θ(t k )) represents the sine value of the yaw angle of the obstacle at time t k , Δt k represents a time interval, t k is an integral interval from time t k+1 to time t The second prediction equation is represented by the following formula: P k2+1 = J*P k2 *J -1 +Q2 wherein P k2+1 represents the second state matrix of the next time, P k2 represents the second state matrix of the current time, J is the Jacobian matrix, and is derived by taking the derivative of X k2+1 with respect to X k2 , wherein P k2 is derived by recursion from a second initial error covariance matrix, which is set as:

7. The method of claim 5, wherein, The second Kalman gain equation is represented by the following formula: S2 = H2*P k2+1 *H2 -1 +R2 G2 = P k2+1 *H2 -1 *S2 -1 where G2 is a second Kalman gain, H1 -1 denotes an inverse of the second measurement matrix, S2 is a second intermediate variable matrix for calculating the second Kalman gain, S2 -1 denotes an inverse of the second intermediate variable matrix; The second Kalman filtering equation set is represented by the following formula: wherein denotes the second state quantity of the optimal estimate, denotes the second state matrix of the optimal estimate, I denotes the identity matrix.

8. An obstacle state prediction device characterized by comprising: The device comprises: an acquisition module configured to acquire initial state information of an obstacle, the initial state information comprising an initial heading angle and an initial heading angle velocity of the obstacle; a filtering module configured to perform Kalman filtering on the initial heading angle based on a first preset equation set of a Kalman filter according to the initial heading angle and the initial heading angle velocity, and obtain modified state information; a prediction module configured to update state information of the obstacle based on a second preset equation set of a constant turning rate and acceleration motion model according to the modified state information, and obtain predicted state information.

9. A computer device comprising: at least one processor; and a memory connected in communication with the at least one processor; wherein the memory stores computer instructions executable by the at least one processor, and the computer instructions are executed by the at least one processor to enable the at least one processor to perform the method of any one of claims 1-7.

10. A computer readable storage medium having stored thereon computer instructions, wherein, The computer instructions are used to enable the computer to perform the method of any one of claims 1-7. The computer instructions are used to enable the computer to perform the method of any one of claims 1-7.

Images