Method for determining yaw angle of vehicle and related device thereof

By fusing image and point cloud data, using nonlinear state transfer function and Kalman filtering algorithm to predict and update the vehicle yaw angle, the problem of inaccurate prediction of yaw angle in intelligent driving system is solved, and the prediction of vehicle heading angle is improved Accuracy and system robustness.

CN118942072APending Publication Date: 2024-11-12SINO TRUK JINAN POWER CO LTD
View PDF 5 Cites 0 Cited by

Patent Information

Application Number
CN202411222201.6
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-09-02
Publication Date
2024-11-12

AI Technical Summary

Technical Problem

In intelligent driving and assisted driving systems, the prediction of vehicle yaw angle is easily affected by the complexity of the traffic environment and emergencies, resulting in a decrease in the accuracy of position judgment and increasing safety risks.

Method used

By fusing image data and point cloud data, the vehicle's position, speed and yaw angle information are extracted, and the nonlinear state transfer function and Kalman filtering algorithm are used to predict and update the current yaw angle of the target vehicle, combined with the data of multiple sensors Fusion to improve prediction accuracy of yaw angle.

Benefits of technology

Through the fusion of multi-sensor data, more comprehensive environmental information is obtained, the prediction accuracy of vehicle heading angle is improved, the impact of single sensor error is reduced, and the robustness and safety of the system are enhanced.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118942072A_ABST
    Figure CN118942072A_ABST
Patent Text Reader

Abstract

The invention relates to a vehicle yaw angle determination method and a related device thereof, and the method comprises the steps: obtaining image data and point cloud data of a main vehicle, extracting the vehicle information of at least one first vehicle, and extracting the vehicle information of at least one second vehicle; establishing a connecting edge between the two disjoint vertex sets, and taking the vehicle with the minimum distance between the same vehicle corresponding to the connecting edge and the host vehicle as a target vehicle; predicting a yaw angle of the target vehicle at the current moment; predicting a covariance matrix at the current moment, determining a measurement residual error of an image sensor, and determining a measurement residual error of a radar sensor; determining the Kalman gain of the image sensor, and determining the Kalman gain of the radar sensor; and updating the yaw angle of the target vehicle at the current moment to obtain a target yaw angle of the target vehicle. According to the scheme provided by the invention, more comprehensive environment information can be obtained by fusing the sensing data of the multiple sensors, and the prediction accuracy of the vehicle course angle is improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present application relates to the field of data processing technology, and in particular to a method for determining a vehicle yaw angle and a related device thereof. Background Art

[0002] In intelligent driving and assisted driving systems, accurate estimation and prediction of vehicle yaw angle is one of the key technologies to achieve vehicle autonomous navigation, lane keeping, obstacle avoidance and other functions. Various types of intelligent transportation equipment can sense the external environment through installed sensors for sensing environmental data, and then provide data support for intelligent transportation equipment based on the relevant data of the perceived external environment. These intelligent transportation equipment can be, for example, self-driving cars.

[0003] However, due to the complex and changeable traffic environment, vehicles need to deal with various emergencies and uncertain factors during driving, such as other vehicles overtaking and changing lanes, pedestrians crossing the road, etc., which will lead to errors in the prediction of the vehicle's yaw angle, affect the accuracy of position judgment, and lead to danger. Summary of the invention

[0004] In order to solve or partially solve the problems existing in the related technology, the present application provides a vehicle yaw angle determination method and related devices, which can obtain more comprehensive environmental information by fusing the sensor data of multiple sensors and improve the prediction accuracy of the vehicle heading angle.

[0005] The first aspect of the present application provides a method for determining a vehicle yaw angle, comprising acquiring image data and point cloud data collected from the front of a main vehicle during driving, so as to extract vehicle information of at least one first vehicle from the image data, and extract vehicle information of at least one second vehicle from the point cloud data, wherein the vehicle information comprises: position, speed and yaw angle; taking the at least one first vehicle and the at least one second vehicle as two non-intersecting vertex sets respectively, and establishing a connecting edge between the two non-intersecting vertex sets according to the position and the speed, so as to take the vehicle with the smallest distance between the same vehicle corresponding to the connecting edge and the main vehicle as the target vehicle; acquiring the yaw angle of the target vehicle at a previous moment, control information at a previous moment and the covariance matrix of the target vehicle at a previous moment, wherein the control information is information used to control the vehicle, so as to input the yaw angle of the target vehicle at a previous moment and the control information at a previous moment into a preset nonlinear state transfer function, and predict the yaw angle of the target vehicle at a current moment; based on the According to the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at the previous moment, and the noise covariance matrix at the previous moment, the covariance matrix at the current moment is predicted, and the measurement residual of the image sensor is determined according to the yaw angle extracted from the image data and the nonlinear measurement function of the image sensor, and the measurement residual of the radar sensor is determined according to the yaw angle extracted from the point cloud data and the nonlinear measurement function of the radar sensor; the Kalman gain of the image sensor is determined according to the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor, and the Kalman gain of the radar sensor is determined according to the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor; the yaw angle of the target vehicle at the current moment is updated according to the two Kalman gains and the two measurement residuals to obtain the target yaw angle of the target vehicle.

[0006] In combination with the first aspect, in a possible implementation manner of the first aspect, determining the Kalman gain of the image sensor according to the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor, and determining the Kalman gain of the radar sensor according to the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor, including: calculating the Kalman gain by the following formula:

[0007]

[0008] Where i is 1 or 2, and are the Kalman gain of the image sensor and the Kalman gain of the radar sensor, P k is the covariance matrix at the current moment, and are the Jacobian matrix of the nonlinear measurement function of the image sensor and the Jacobian matrix of the nonlinear measurement function of the radar sensor, respectively, They are The transposed matrix and The transposed matrix of is the measurement noise covariance matrix of the image sensor.

[0009] In combination with the first aspect, in a possible implementation of the first aspect, updating the current yaw angle of the target vehicle according to the two Kalman gains and the two measurement residuals to obtain the target yaw angle of the target vehicle includes: updating the target yaw angle of the target vehicle by the following steps:

[0010]

[0011] Among them, x k is the target yaw angle of the target vehicle, is the current yaw angle of the target vehicle, is the Kalman gain of the image sensor, is the measurement residual of the image sensor, is the Kalman gain of the radar sensor, is the measurement residual of the radar sensor.

[0012] In combination with the first aspect, in a possible implementation of the first aspect, predicting the covariance matrix at the current moment according to the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at the previous moment, and the noise covariance matrix at the previous moment includes: predicting the covariance matrix at the current moment by the following formula:

[0013]

[0014] Among them, P k is the covariance matrix at the current moment, F k-1 is the Jacobian matrix of the nonlinear state transfer function, is the transposed matrix of the Jacobian matrix of the nonlinear state transfer function, Q k-1 is the noise covariance matrix at the previous moment.

[0015] In combination with the first aspect, in a possible implementation manner of the first aspect, the method further includes: updating the covariance matrix at the current moment by using the two Kalman gains and the Jacobian matrix of the measurement functions of the two sensors, and the updating can be implemented by the following formula:

[0016]

[0017] Among them, the P k is the covariance matrix at the current moment before updating, I is the identity matrix, is the Kalman gain of the image sensor, the is the Jacobian matrix of the measurement function of the image sensor, is the Kalman gain of the radar sensor, is the Jacobian matrix of the radar sensor’s measurement function, is the updated covariance matrix at the current moment.

[0018] In combination with the first aspect, in a possible implementation of the first aspect, determining the vehicle information of at least one first vehicle based on the image data includes: extracting target object features from the image data; matching the target object features with preset features in a preset database to obtain target object features that successfully match the preset features as target vehicle features; acquiring two-dimensional pose information of the target vehicle features in the image data; determining three-dimensional pose information of the target vehicle in three-dimensional space based on the two-dimensional pose information of the target vehicle features in the image data; and converting the three-dimensional pose information into a rotation matrix or a quaternion to extract the yaw angle of the first vehicle from the rotation matrix or the quaternion.

[0019] In combination with the first aspect, in a possible implementation method of the first aspect, determining the vehicle information of at least one second vehicle based on the point cloud data includes: clustering the point cloud data to obtain at least one point cloud cluster, each point cloud cluster corresponding to a second vehicle; for each point cloud cluster, constructing a covariance matrix based on each point cloud data in the point cloud cluster, and eigendecomposing the covariance matrix to obtain a signal subspace and a noise subspace; and determining the yaw angle of the second vehicle corresponding to the point cloud cluster based on the orthogonality of the signal subspace and the noise subspace.

[0020] A second aspect of the present application provides a vehicle yaw angle determination device, comprising an acquisition module, for acquiring image data and point cloud data collected from the front of a host vehicle during driving, so as to extract vehicle information of at least one first vehicle from the image data, and to extract vehicle information of at least one second vehicle from the point cloud data, wherein the vehicle information includes: position, speed and yaw angle;

[0021] a processing module, configured to respectively regard the at least one first vehicle and the at least one second vehicle as two non-intersecting vertex sets, and establish a connecting edge between the two non-intersecting vertex sets according to the position and the speed, so as to regard a vehicle having a minimum distance between the same vehicle corresponding to the connecting edge and the host vehicle as a target vehicle;

[0022] A prediction module, used to obtain the yaw angle of the target vehicle at the last moment, the control information at the last moment and the covariance matrix of the target vehicle at the last moment, wherein the control information is information used to control the vehicle, so as to input the yaw angle of the target vehicle at the last moment and the control information at the last moment into a preset nonlinear state transfer function, and predict the yaw angle of the target vehicle at the current moment;

[0023] A first determination module is used to predict the covariance matrix at a current moment according to the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at a previous moment, and the noise covariance matrix at a previous moment, and to determine the measurement residual of the image sensor according to the yaw angle extracted from the image data and the nonlinear measurement function of the image sensor, and to determine the measurement residual of the radar sensor according to the yaw angle extracted from the point cloud data and the nonlinear measurement function of the radar sensor;

[0024] a second determination module, configured to determine the Kalman gain of the image sensor according to the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor, and to determine the Kalman gain of the radar sensor according to the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor;

[0025] An updating module is used to update the current yaw angle of the target vehicle according to the two Kalman gains and the two measurement residuals to obtain a target yaw angle of the target vehicle.

[0026] In combination with the second aspect, in a possible implementation manner of the second aspect, the second determination module is further configured to calculate the Kalman gain by using the following formula:

[0027]

[0028] Where i is 1 or 2, and are the Kalman gain of the image sensor and the Kalman gain of the radar sensor, P k is the covariance matrix at the current moment, and are the Jacobian matrix of the nonlinear measurement function of the image sensor and the Jacobian matrix of the nonlinear measurement function of the radar sensor, respectively, They are The transposed matrix and The transposed matrix of is the measurement noise covariance matrix of the image sensor.

[0029] In conjunction with the second aspect, in a possible implementation manner of the second aspect, the updating module is further configured to update and obtain a target yaw angle of the target vehicle through the following steps:

[0030]

[0031] Among them, x k is the target yaw angle of the target vehicle, is the current yaw angle of the target vehicle, is the Kalman gain of the image sensor, is the measurement residual of the image sensor, is the Kalman gain of the radar sensor, is the measurement residual of the radar sensor.

[0032] In combination with the second aspect, in a possible implementation manner of the second aspect, the first determination module is further used to predict the covariance matrix at the current moment by using the following formula:

[0033]

[0034] Among them, P k is the covariance matrix at the current moment, F k-1 is the Jacobian matrix of the nonlinear state transfer function, is the transposed matrix of the Jacobian matrix of the nonlinear state transfer function, Q k-1 is the noise covariance matrix at the previous moment.

[0035] In combination with the second aspect, in a possible implementation manner of the second aspect, the updating module is further used to update the covariance matrix at the current moment through the two Kalman gains and the Jacobian matrix of the measurement function of the two sensors, and the updating can be implemented by the following formula:

[0036]

[0037] Among them, the P k is the covariance matrix at the current moment before updating, I is the identity matrix, is the Kalman gain of the image sensor, the is the Jacobian matrix of the measurement function of the image sensor, is the Kalman gain of the radar sensor, is the Jacobian matrix of the radar sensor’s measurement function, is the updated covariance matrix at the current moment.

[0038] In combination with the second aspect, in a possible implementation of the second aspect, the acquisition module is also used to extract target object features from the image data; match the target object features with preset features in a preset database to obtain target object features that successfully match the preset features as target vehicle features; obtain two-dimensional pose information of the target vehicle features in the image data; determine the three-dimensional pose information of the target vehicle in three-dimensional space based on the two-dimensional pose information of the target vehicle features in the image data; and convert the three-dimensional pose information into a rotation matrix or a quaternion to extract the yaw angle of the first vehicle from the rotation matrix or the quaternion.

[0039] In combination with the second aspect, in a possible implementation of the second aspect, the acquisition module is also used to cluster the point cloud data to obtain at least one point cloud cluster, each point cloud cluster corresponding to a second vehicle; for each point cloud cluster, a covariance matrix is ​​constructed according to each point cloud data in the point cloud cluster, and the covariance matrix is ​​eigendecomposed to obtain a signal subspace and a noise subspace; based on the orthogonality of the signal subspace and the noise subspace, the yaw angle of the second vehicle corresponding to the point cloud cluster is determined.

[0040] A third aspect of the present application provides an electronic device, including:

[0041] Processor; and

[0042] The memory stores executable codes thereon, and when the executable codes are executed by the processor, the processor is caused to execute the method as described above.

[0043] A fourth aspect of the present application provides a computer-readable storage medium having executable code stored thereon. When the executable code is executed by a processor of an electronic device, the processor is caused to execute the method as described above.

[0044] The technical solution provided by this application may have the following beneficial effects:

[0045] The vehicle yaw angle determination method of the present application includes: acquiring image data and point cloud data collected from the front of the main vehicle during driving, so as to extract vehicle information of at least one first vehicle from the image data, and extracting vehicle information of at least one second vehicle from the point cloud data, wherein the vehicle information includes: position, speed and yaw angle; taking at least one first vehicle and at least one second vehicle as two non-intersecting vertex sets respectively, and establishing a connecting edge between the two non-intersecting vertex sets according to the position and speed, so as to take the vehicle with the smallest distance between the same vehicle corresponding to the connecting edge and the main vehicle as the target vehicle; acquiring the yaw angle of the target vehicle at the last moment, the control information at the last moment and the covariance matrix of the target vehicle at the last moment, wherein the control information is information used to control the vehicle, so as to input the yaw angle of the target vehicle at the last moment and the control information at the last moment into a preset nonlinear state transfer function, and predict the yaw angle of the target vehicle at the current moment; according to the Jacobian matrix of the nonlinear state transfer function, the control information at the last moment and the covariance matrix of the target vehicle at the last moment The covariance matrix at the current moment and the noise covariance matrix at the previous moment are used to predict the covariance matrix at the current moment, and the measurement residual of the image sensor is determined according to the yaw angle extracted from the image data and the nonlinear measurement function of the image sensor, and the measurement residual of the radar sensor is determined according to the yaw angle extracted from the point cloud data and the nonlinear measurement function of the radar sensor; the Kalman gain of the image sensor is determined according to the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor, and the Kalman gain of the radar sensor is determined according to the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor; the yaw angle of the target vehicle at the current moment is updated according to the two Kalman gains and the two measurement residuals to obtain the target yaw angle of the target vehicle, which can obtain more comprehensive environmental information by fusing the sensing data of multiple sensors and improve the prediction accuracy of the vehicle heading angle.

[0046] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The above and other objects, features and advantages of the present application will become more apparent by describing in more detail exemplary embodiments of the present application in conjunction with the accompanying drawings, wherein the same reference numerals generally represent the same components in the exemplary embodiments of the present application.

[0048] Figure 1 is a flow chart of a method for determining a vehicle yaw angle shown in an embodiment of the present application;

[0049] Figure 2 is a schematic structural diagram of a vehicle yaw angle determination device shown in an embodiment of the present application;

[0050] Figure 3 It is a schematic diagram of the structure of an electronic device shown in an embodiment of the present application. DETAILED DESCRIPTION

[0051] The embodiments of the present application will be described in more detail below with reference to the accompanying drawings. Although the embodiments of the present application are shown in the accompanying drawings, it should be understood that the present application can be implemented in various forms and should not be limited by the embodiments described herein. On the contrary, these embodiments are provided to make the present application more thorough and complete, and to fully convey the scope of the present application to those skilled in the art.

[0052] The terms used in this application are only for the purpose of describing specific embodiments and are not intended to limit this application. The singular forms of "a", "a", "an" and "the" used in this application and the appended claims are also intended to include plural forms unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used in this article refers to and includes any or all possible combinations of one or more associated listed items.

[0053] It should be understood that although the terms "first", "second", "third", etc. may be used in this application to describe various information, this information should not be limited to these terms. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of this application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of this application, the meaning of "multiple" is two or more, unless otherwise clearly and specifically defined.

[0054] Kalman filtering is an effective recursive filter that uses a linear dynamic system model to estimate the state of a dynamic system through a series of measurements, including statistical noise and other uncertainties. However, the algorithm is based on the assumption of a linear dynamic system, and its performance may be limited for highly nonlinear systems. Particle filtering is a Monte Carlo method based on Bayesian estimation. It represents the probability distribution in the state space through a set of weighted particles. Each particle represents a possible system state and updates its weight and position through an iterative process to approximate the posterior distribution of the true state. However, as the number of iterations increases, the weights of most particles may become very small, resulting in a decrease in the number of effective particles and affecting the estimation accuracy. With the development of deep learning technology, more and more studies are applying deep learning to the field of sensor fusion. Deep learning models can automatically extract features by learning a large amount of data and fuse information from different sensors. However, the model requires a large amount of labeled data to train, and the quality and diversity of the data have a great impact on the model performance. The model usually has a complex structure and its decision-making process is difficult to explain.

[0055] The extended Kalman filter (EKF) is an extension of the Kalman filter. It adapts to nonlinear systems by linearizing nonlinear functions (usually through Taylor series expansion and first-order approximation). The EKF can well fuse data from different sensors. In applications such as autonomous driving, it is usually necessary to combine data from multiple sensors such as radar, LiDAR, and cameras to obtain more comprehensive environmental information. The EKF obtains more accurate state estimates by assigning different observation models and noise models to each sensor and comprehensively considering this information during the state update process. Multi-sensor fusion can also improve the robustness of the system. When a sensor fails or the data is abnormal, the EKF can correct the state estimate through the data of other sensors to avoid system crashes.

[0056] In intelligent driving and assisted driving systems, accurately estimating and predicting the target yaw angle is one of the key technologies to achieve vehicle autonomous navigation, lane keeping, obstacle avoidance and other functions. When a vehicle is driving in a complex and changing traffic environment, it needs to deal with various emergencies and uncertain factors, such as other vehicles overtaking and changing lanes, pedestrians crossing, etc. These factors may affect the estimation of the yaw angle in front of the vehicle.

[0057] In response to the above problems, the embodiments of the present application provide a vehicle yaw angle determination method and related devices, which can obtain more comprehensive environmental information by fusing the sensor data of multiple sensors (such as visual sensors, millimeter-wave radar sensors, etc.), and utilize the EKF's processing capabilities for nonlinear systems to reduce the impact of a single sensor error on the yaw angle estimation, thereby improving the overall yaw angle estimation accuracy and improving the prediction accuracy of the vehicle heading angle.

[0058] The technical solution of the embodiments of the present application is described in detail below with reference to the accompanying drawings.

[0059] Figure 1 It is a flow chart of a method for determining a vehicle yaw angle shown in an embodiment of the present application.

[0060] See also Figure 1 , a method for determining a vehicle yaw angle, comprising:

[0061] S110: Acquire image data and point cloud data collected from the front of the main vehicle during driving, so as to extract vehicle information of at least one first vehicle from the image data, and extract vehicle information of at least one second vehicle from the point cloud data, wherein the vehicle information includes: position, speed and yaw angle.

[0062] Specifically, the image data in front can be collected by the image sensor of the main vehicle, such as a camera, a driving recorder, a visual sensor, etc., and the point cloud data in front of the vehicle can be collected by a radar sensor, such as a microwave radar, a millimeter wave radar, etc., and the vehicle information of at least one first vehicle can be extracted from the collected image data. For example, the vehicle information of the first vehicle can be extracted from the image data through the SIFT algorithm, and the vehicle information of at least one second vehicle can be extracted from the point cloud data. There can be many vehicles in front of the main vehicle. It can be understood that the first vehicle is the same vehicle as the second vehicle, so that more vehicle information can be obtained. The vehicle information may include the positions of the first vehicle and the second vehicle, the speeds of the first vehicle and the second vehicle, and the yaw angles of the first vehicle and the second vehicle. At this time, the yaw angles of the first vehicle and the second vehicle can be calculated from the image data or the point cloud data.

[0063] In one possible implementation, target object features are extracted from image data; the target object features are matched with preset features in a preset database to obtain target object features that successfully match the preset features as target vehicle features; two-dimensional pose information of the target vehicle features in the image data is obtained; three-dimensional pose information of the target vehicle in three-dimensional space is determined based on the two-dimensional pose information of the target vehicle features in the image data; and the three-dimensional pose information is converted into a rotation matrix or a quaternion to extract the yaw angle of the first vehicle from the rotation matrix or the quaternion.

[0064] Specifically, the image data may contain features of multiple vehicles, and the features of each vehicle can be extracted from the image data, and the features of each vehicle can be matched with the preset features in the preset database. If the features of the vehicle are consistent with the preset features in the preset data, the successfully matched vehicle will be used as the target vehicle, and the features of the target vehicle will be obtained; the two-dimensional pose information of the target vehicle in the image data can be obtained, for example, the two-dimensional pose information of the target vehicle can be obtained through the PnP algorithm, the relationship between the image sensor coordinate system and the world coordinate system can be established through calibration, and the three-dimensional pose information of the target vehicle in the three-dimensional space can be obtained according to the two-dimensional pose information of the target vehicle. The two-dimensional pose information can include the horizontal position and vertical position of the vehicle on the ground plane and the heading angle. The three-dimensional pose information may include the horizontal position, vertical position, vertical height, pitch angle, roll angle, heading angle and other information of the vehicle in three-dimensional space. The three-dimensional pose information of the vehicle provides the complete position and attitude of the vehicle in three-dimensional space. After obtaining the three-dimensional pose information of the target vehicle, the three-dimensional pose information can be converted into a rotation matrix or quaternion. For example, the pitch angle, roll angle and heading angle in the three-dimensional pose information of the target vehicle can be converted into a rotation matrix or quaternion to extract the yaw angle of the target vehicle from the rotation matrix or quaternion, and the yaw angle of the target vehicle can be used as the yaw angle of the first vehicle. The rotation matrix is ​​a 3x3 matrix that can be used to describe the rotation of one object relative to another object in three-dimensional space. The quaternion is used to express rotation information in three-dimensional space.

[0065] In one possible implementation, the point cloud data is clustered to obtain at least one point cloud cluster, each point cloud cluster corresponding to a second vehicle; for each point cloud cluster, a covariance matrix is ​​constructed based on each point cloud data in the point cloud cluster, and the covariance matrix is ​​eigendecomposed to obtain a signal subspace and a noise subspace; based on the orthogonality of the signal subspace and the noise subspace, the yaw angle of the second vehicle corresponding to the point cloud cluster is determined.

[0066] Specifically, the point cloud data in front of the vehicle can be collected by the vehicle-mounted radar, and the point cloud data can be clustered to obtain at least one point cloud cluster, each of which corresponds to a second vehicle. It can be understood that the point cloud cluster refers to a set of points with similar features in three-dimensional space formed by a point cloud clustering algorithm. These features can be spatial position, geometric attributes, density, etc., that is, the second vehicle has similar features to the main vehicle, such as spatial position.

[0067] Specifically, the covariance matrix of each point cloud data in the point cloud cluster can be constructed through the multiple signal classification algorithm, and eigendecomposition can be performed to separate the signal subspace and the noise subspace from the point cloud data. Furthermore, the multiple signal classification algorithm can estimate the parameters of the signal by constructing a spatial spectrum function through the orthogonality of the signal subspace and the noise subspace, that is, the inner product between the signal subspace and the noise subspace is zero, and the spatial scanning spectrum is constructed by using the orthogonality of the signal direction vector of the signal subspace and the noise subspace to perform a full-domain search for the spectrum peak to determine the yaw angle of the second vehicle corresponding to the point cloud cluster; it can be understood that the signal subspace is composed of the eigenvectors of the covariance matrix, which correspond to larger eigenvalues, and the signal subspace contains the main information of the signal source, such as the vehicle information of the second vehicle; the noise subspace is also composed of the eigenvectors of the covariance matrix, which correspond to smaller eigenvalues. These eigenvalues ​​represent the energy of the noise, so the noise subspace contains the noise components in the signal and other information that is not the signal source, that is, it contains information other than the second vehicle.

[0068] S120: at least one first vehicle and at least one second vehicle are respectively regarded as two non-intersecting vertex sets, and a connecting edge is established between the two non-intersecting vertex sets according to the position and the speed, so as to regard the vehicle with the smallest distance between the same vehicle corresponding to the connecting edge and the main vehicle as the target vehicle.

[0069] Specifically, the first vehicle and the second vehicle at the same timestamp can be respectively used as two non-intersecting vertex sets of the bipartite graph, for example, the first vehicle can be used as X, and the second vehicle can be used as Y. According to the similarity between the first vehicle and the second vehicle, a connecting edge or a connecting edge set between the two non-intersecting vertex sets is established. The connecting edge and the connecting edge set can be used as E to form a bipartite graph G = (X, Y, E), and by repeatedly searching for augmenting paths until no new augmenting paths are found, the matching obtained at this time is the maximum matching of the current bipartite graph. In the maximum matching, each pair of connecting edges corresponds to the same vehicle. If the distance between the same vehicle corresponding to the connecting edge and the main vehicle is the smallest at this time, the vehicle corresponding to the connecting edge is the target vehicle, and the distance between the target vehicle and the main vehicle is the smallest. Furthermore, in calculating the similarity between the first vehicle and the second vehicle, the similarity between the positions of the first vehicle and the second vehicle and the similarity between the speeds of the first vehicle and the second vehicle can be calculated. By adding the similarity between the positions and the similarity between the speeds, the similarity between the first vehicle and the second vehicle can be obtained. When the similarity between the first vehicle and the second vehicle is greater than a set threshold, a connecting edge between the first vehicle and the second vehicle is established. When the similarity between the first vehicle and the second vehicle is less than the threshold, the connecting edge between the first vehicle and the second vehicle is not established.

[0070] S130: Obtain the yaw angle of the target vehicle at the previous moment, the control information at the previous moment and the covariance matrix of the target vehicle at the previous moment, where the control information is information used to control the vehicle, so that the yaw angle of the target vehicle at the previous moment and the control information at the previous moment are input into a preset nonlinear state transfer function to predict the yaw angle of the target vehicle at the current moment.

[0071] Specifically, the yaw angle of the target vehicle at the last moment is the yaw angle predicted last time by the target vehicle. When the yaw angle of the target vehicle is predicted for the first time at this time, the yaw angle of the target vehicle at the last moment is initialized to a default value. The control information of the target vehicle at the last moment may be information such as the gear position and torque of the target vehicle. The yaw angle of the target vehicle at the last moment and the control information of the target vehicle at the last moment may be input into a preset nonlinear state transfer function to predict the yaw angle of the target vehicle at the current moment. The nonlinear state transfer function may be:

[0072]

[0073] in, is the current yaw angle of the target vehicle, is the preset nonlinear state transfer function, is the yaw angle of the target vehicle at the last moment, u k-1 It is the control information of the target vehicle at the last moment.

[0074] S140: Predict the covariance matrix at the current moment based on the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at the previous moment, and the noise covariance matrix at the previous moment, and determine the measurement residual of the image sensor based on the yaw angle extracted from the image data and the nonlinear measurement function of the image sensor, and determine the measurement residual of the radar sensor based on the yaw angle extracted from the point cloud data and the nonlinear measurement function of the radar sensor.

[0075] Specifically, the nonlinear state transfer function can be linearized by Taylor series expansion and other methods to obtain the linearized state transfer function, and the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at the previous moment, and the noise covariance matrix at the previous moment can be used to predict the covariance matrix at the current moment.

[0076] In a possible implementation, the covariance matrix at the current moment is predicted by the following formula:

[0077]

[0078] Among them, P k is the covariance matrix at the current moment, F k-1 is the Jacobian matrix of the nonlinear state transfer function, is the transposed matrix of the Jacobian matrix of the nonlinear state transfer function, Q k-1 is the noise covariance matrix at the previous moment.

[0079] In a possible implementation, the measurement residuals of the image sensor and the radar sensor may be determined by the following formula:

[0080]

[0081] in, is the yaw angle extracted from the image data or the yaw angle extracted from the radar sensor, h i is the nonlinear measurement function of the image sensor or radar sensor, is the measurement residual of the image sensor or radar sensor.

[0082] S150: Determine the Kalman gain of the image sensor based on the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor; and determine the Kalman gain of the radar sensor based on the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor.

[0083] Specifically, the nonlinear measurement function of the image sensor and the nonlinear measurement function of the radar sensor can be linearized by Taylor series expansion or other methods to obtain the linear measurement function of the image sensor and the linear measurement function of the radar sensor, and the Kalman gain of the image sensor or the Kalman gain of the radar sensor can be determined based on the covariance matrix at the current moment, the measurement noise covariance matrix of the image sensor or the measurement noise covariance matrix of the radar sensor.

[0084] In a possible implementation, the Kalman gain is calculated using the following formula:

[0085]

[0086] Where i is 1 or 2, and are the Kalman gain of the image sensor and the Kalman gain of the radar sensor, P k is the covariance matrix at the current moment, and are the Jacobian matrices of the nonlinear measurement function of the image sensor and the Jacobian matrix of the nonlinear measurement function of the radar sensor, respectively. They are The transposed matrix and The transposed matrix of is the measurement noise covariance matrix of the image sensor.

[0087] S160: updating the current yaw angle of the target vehicle according to the two Kalman gains and the two measurement residuals to obtain a target yaw angle of the target vehicle.

[0088] Specifically, after obtaining the Kalman gain and measurement residual of the image sensor and the Kalman gain and measurement residual of the radar sensor, the new yaw angle of the target vehicle can be calculated and used as the target yaw angle of the target vehicle. The target yaw angle is more accurate than the yaw angle at the current moment and is more consistent with the actual yaw angle of the target vehicle.

[0089] In a possible implementation manner, the target yaw angle of the target vehicle is updated by the following steps:

[0090]

[0091] Among them, x k is the target yaw angle of the target vehicle, is the current yaw angle of the target vehicle, is the Kalman gain of the image sensor, is the measurement residual of the image sensor, is the Kalman gain of the radar sensor, is the measurement residual of the radar sensor.

[0092] In a possible implementation, the covariance matrix at the current moment is updated by two Kalman gains and the Jacobian matrix of the measurement functions of the two sensors. The update can be implemented by the following formula:

[0093]

[0094] Among them, P k is the covariance matrix at the current moment before updating, I is the identity matrix, is the Kalman gain of the image sensor, is the Jacobian matrix of the image sensor’s measurement function, is the Kalman gain of the radar sensor, is the Jacobian matrix of the radar sensor’s measurement function, is the updated covariance matrix at the current moment, and the updated covariance matrix at the current moment can be used to calculate the yaw angle of the vehicle at the next moment.

[0095] The vehicle yaw angle determination method of the present application includes: acquiring image data and point cloud data collected from the front of the main vehicle during driving, so as to extract vehicle information of at least one first vehicle from the image data, and extracting vehicle information of at least one second vehicle from the point cloud data, wherein the vehicle information includes: position, speed and yaw angle; taking at least one first vehicle and at least one second vehicle as two non-intersecting vertex sets respectively, and establishing a connecting edge between the two non-intersecting vertex sets according to the position and speed, so as to take the vehicle with the smallest distance between the same vehicle corresponding to the connecting edge and the main vehicle as the target vehicle; acquiring the yaw angle of the target vehicle at the last moment, the control information at the last moment and the covariance matrix of the target vehicle at the last moment, wherein the control information is information used to control the vehicle, so as to input the yaw angle of the target vehicle at the last moment and the control information at the last moment into a preset nonlinear state transfer function, and predict the yaw angle of the target vehicle at the current moment; according to the Jacobian matrix of the nonlinear state transfer function, the control information at the last moment and the covariance matrix of the target vehicle at the last moment The covariance matrix at the current moment and the noise covariance matrix at the previous moment are used to predict the covariance matrix at the current moment, and the measurement residual of the image sensor is determined according to the yaw angle extracted from the image data and the nonlinear measurement function of the image sensor, and the measurement residual of the radar sensor is determined according to the yaw angle extracted from the point cloud data and the nonlinear measurement function of the radar sensor; the Kalman gain of the image sensor is determined according to the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor, and the Kalman gain of the radar sensor is determined according to the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor; the yaw angle of the target vehicle at the current moment is updated according to the two Kalman gains and the two measurement residuals to obtain the target yaw angle of the target vehicle, which can obtain more comprehensive environmental information by fusing the sensing data of multiple sensors and improve the prediction accuracy of the vehicle heading angle.

[0096] Corresponding to the aforementioned application function implementation method embodiment, the present application also provides a vehicle yaw angle determination device, an electronic device and corresponding embodiments.

[0097] Figure 2 It is a schematic diagram of the structure of a vehicle yaw angle determination device shown in an embodiment of the present application.

[0098] See also Figure 2 A vehicle yaw angle determination device includes an acquisition module 210, a processing module 220, a prediction module 230, a first determination module 240, a second determination module 250, and an update module 260.

[0099] in,

[0100] The acquisition module 210 is used to acquire image data and point cloud data collected from the front of the main vehicle during driving, so as to extract vehicle information of at least one first vehicle from the image data, and to extract vehicle information of at least one second vehicle from the point cloud data, wherein the vehicle information includes: position, speed and yaw angle.

[0101] Specifically, the image data in front can be collected by the image sensor of the main vehicle, such as a camera, a driving recorder, a visual sensor, etc., and the point cloud data in front of the vehicle can be collected by a radar sensor, such as a microwave radar, a millimeter wave radar, etc., and the vehicle information of at least one first vehicle can be extracted from the collected image data. For example, the vehicle information of the first vehicle can be extracted from the image data through the SIFT algorithm, and the vehicle information of at least one second vehicle can be extracted from the point cloud data. There can be many vehicles in front of the main vehicle. It can be understood that the first vehicle is the same vehicle as the second vehicle, so that more vehicle information can be obtained. The vehicle information may include the positions of the first vehicle and the second vehicle, the speeds of the first vehicle and the second vehicle, and the yaw angles of the first vehicle and the second vehicle. At this time, the yaw angles of the first vehicle and the second vehicle can be calculated from the image data or the point cloud data.

[0102] In a possible implementation, the acquisition module 210 is also used to extract target object features from the image data; match the target object features with preset features in a preset database to obtain target object features that successfully match the preset features as target vehicle features; obtain two-dimensional pose information of the target vehicle features in the image data; determine three-dimensional pose information of the target vehicle in three-dimensional space based on the two-dimensional pose information of the target vehicle features in the image data; and convert the three-dimensional pose information into a rotation matrix or quaternion to extract the yaw angle of the first vehicle from the rotation matrix or quaternion.

[0103] In a possible implementation, the acquisition module 210 is also used to cluster the point cloud data to obtain at least one point cloud cluster, each point cloud cluster corresponding to a second vehicle; for each point cloud cluster, a covariance matrix is ​​constructed based on the individual point cloud data in the point cloud cluster, and the covariance matrix is ​​eigendecomposed to obtain a signal subspace and a noise subspace; based on the orthogonality of the signal subspace and the noise subspace, the yaw angle of the second vehicle corresponding to the point cloud cluster is determined.

[0104] The processing module 220 is used to treat at least one first vehicle and at least one second vehicle as two non-intersecting vertex sets respectively, and establish a connecting edge between the two non-intersecting vertex sets according to the position and speed, so as to use the vehicle with the smallest distance between the same vehicle corresponding to the connecting edge and the main vehicle as the target vehicle.

[0105] Specifically, the first vehicle and the second vehicle at the same timestamp can be respectively used as two non-intersecting vertex sets of the bipartite graph, for example, the first vehicle can be used as X, and the second vehicle can be used as Y. According to the similarity between the first vehicle and the second vehicle, a connecting edge or a connecting edge set between the two non-intersecting vertex sets is established. The connecting edge and the connecting edge set can be used as E to form a bipartite graph G = (X, Y, E), and by repeatedly searching for augmenting paths until no new augmenting paths are found, the matching obtained at this time is the maximum matching of the current bipartite graph. In the maximum matching, each pair of connecting edges corresponds to the same vehicle. If the distance between the same vehicle corresponding to the connecting edge and the main vehicle is the smallest at this time, the vehicle corresponding to the connecting edge is the target vehicle, and the distance between the target vehicle and the main vehicle is the smallest. Furthermore, in calculating the similarity between the first vehicle and the second vehicle, the similarity between the positions of the first vehicle and the second vehicle and the similarity between the speeds of the first vehicle and the second vehicle can be calculated. By adding the similarity between the positions and the similarity between the speeds, the similarity between the first vehicle and the second vehicle can be obtained. When the similarity between the first vehicle and the second vehicle is greater than a set threshold, a connecting edge between the first vehicle and the second vehicle is established. When the similarity between the first vehicle and the second vehicle is less than the threshold, the connecting edge between the first vehicle and the second vehicle is not established.

[0106] The prediction module 230 is used to obtain the yaw angle of the target vehicle at the previous moment, the control information at the previous moment and the covariance matrix of the target vehicle at the previous moment. The control information is information used to control the vehicle, so that the yaw angle of the target vehicle at the previous moment and the control information at the previous moment are input into a preset nonlinear state transfer function to predict the yaw angle of the target vehicle at the current moment.

[0107] Specifically, the yaw angle of the target vehicle at the last moment is the yaw angle predicted last time by the target vehicle. When the yaw angle of the target vehicle is predicted for the first time at this time, the yaw angle of the target vehicle at the last moment is initialized to a default value. The control information of the target vehicle at the last moment may be information such as the gear position and torque of the target vehicle. The yaw angle of the target vehicle at the last moment and the control information of the target vehicle at the last moment may be input into a preset nonlinear state transfer function to predict the yaw angle of the target vehicle at the current moment. The nonlinear state transfer function may be:

[0108]

[0109] in, is the current yaw angle of the target vehicle, is the preset nonlinear state transfer function, is the yaw angle of the target vehicle at the last moment, u k-1 It is the control information of the target vehicle at the last moment.

[0110] The first determination module 240 is used to predict the covariance matrix at the current moment based on the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at the previous moment, and the noise covariance matrix at the previous moment, and to determine the measurement residual of the image sensor based on the yaw angle extracted from the image data and the nonlinear measurement function of the image sensor, and to determine the measurement residual of the radar sensor based on the yaw angle extracted from the point cloud data and the nonlinear measurement function of the radar sensor.

[0111] Specifically, the nonlinear state transfer function can be linearized by Taylor series expansion and other methods to obtain the linearized state transfer function, and the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at the previous moment, and the noise covariance matrix at the previous moment can be used to predict the covariance matrix at the current moment.

[0112] In a possible implementation, the first determination module 240 is further configured to predict the covariance matrix at the current moment by using the following formula:

[0113]

[0114] Among them, P k is the covariance matrix at the current moment, F k-1 is the Jacobian matrix of the nonlinear state transfer function, is the transposed matrix of the Jacobian matrix of the nonlinear state transfer function, Q k-1 is the noise covariance matrix at the previous moment.

[0115] The second determination module 250 is used to determine the Kalman gain of the image sensor based on the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor, and to determine the Kalman gain of the radar sensor based on the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor.

[0116] Specifically, the nonlinear measurement function of the image sensor and the nonlinear measurement function of the radar sensor can be linearized by Taylor series expansion or other methods to obtain the linear measurement function of the image sensor and the linear measurement function of the radar sensor, and the Kalman gain of the image sensor or the Kalman gain of the radar sensor can be determined based on the covariance matrix at the current moment, the measurement noise covariance matrix of the image sensor or the measurement noise covariance matrix of the radar sensor.

[0117] The updating module 260 is used to update the current yaw angle of the target vehicle according to the two Kalman gains and the two measurement residuals to obtain the target yaw angle of the target vehicle.

[0118] Specifically, after obtaining the Kalman gain and measurement residual of the image sensor and the Kalman gain and measurement residual of the radar sensor, the new yaw angle of the target vehicle can be calculated and used as the target yaw angle of the target vehicle. The target yaw angle is more accurate than the yaw angle at the current moment and is more consistent with the actual yaw angle of the target vehicle.

[0119] In a possible implementation manner, the updating module 260 is further configured to update and obtain a target yaw angle of the target vehicle through the following steps:

[0120]

[0121] Among them, x k is the target yaw angle of the target vehicle, is the current yaw angle of the target vehicle, is the Kalman gain of the image sensor, is the measurement residual of the image sensor, is the Kalman gain of the radar sensor, is the measurement residual of the radar sensor.

[0122] In a possible implementation, the updating module 260 is further configured to update the covariance matrix at the current moment by using two Kalman gains and the Jacobian matrix of the measurement functions of the two sensors. The updating may be implemented by the following formula:

[0123]

[0124] Among them, P k is the covariance matrix at the current moment before updating, I is the identity matrix, is the Kalman gain of the image sensor, is the Jacobian matrix of the image sensor’s measurement function, is the Kalman gain of the radar sensor, is the Jacobian matrix of the radar sensor’s measurement function, is the updated covariance matrix at the current moment.

[0125] Regarding the device in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated again here.

[0126] Figure 3 It is a schematic diagram of the structure of an electronic device shown in an embodiment of the present application.

[0127] See also Figure 3 , the electronic device 300 includes a memory 310 and a processor 320.

[0128] The processor 320 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may be any conventional processor, etc.

[0129] The memory 310 may include various types of storage units, such as system memory, read-only memory (ROM), and permanent storage devices. Among them, ROM can store static data or instructions required by the processor 320 or other modules of the computer. The permanent storage device may be a readable and writable storage device. The permanent storage device may be a non-volatile storage device that does not lose the stored instructions and data even after the computer is powered off. In some embodiments, the permanent storage device uses a large-capacity storage device (such as a magnetic or optical disk, flash memory) as a permanent storage device. In some other embodiments, the permanent storage device may be a removable storage device (such as a floppy disk, optical drive). The system memory may be a readable and writable storage device or a volatile readable and writable storage device, such as a dynamic random access memory. The system memory may store some or all instructions and data required by the processor at run time. In addition, the memory 310 may include any combination of computer-readable storage media, including various types of semiconductor storage chips (such as DRAM, SRAM, SDRAM, flash memory, programmable read-only memory), and disks and / or optical disks may also be used. In some embodiments, the memory 310 may include a readable and / or writable removable storage device, such as a laser disc (CD), a read-only digital versatile disc (such as a DVD-ROM, a double-layer DVD-ROM), a read-only Blu-ray disc, an ultra-density optical disc, a flash memory card (such as an SD card, a mini SD card, a Micro-SD card, etc.), a magnetic floppy disk, etc. Computer-readable storage media do not include carrier waves and transient electronic signals transmitted wirelessly or wired.

[0130] The memory 310 stores executable codes, and when the executable codes are processed by the processor 320 , the processor 320 can execute part or all of the methods described above.

[0131] In addition, the method according to the present application may also be implemented as a computer program or a computer program product, which includes computer program code instructions for executing some or all of the steps in the above method of the present application.

[0132] Alternatively, the present application can also be implemented as a computer-readable storage medium (or non-transitory machine-readable storage medium or machine-readable storage medium) on which executable code (or computer program or computer instruction code) is stored. When the executable code (or computer program or computer instruction code) is executed by a processor of an electronic device (or server, etc.), the processor executes part or all of the steps of the above-mentioned method according to the present application.

[0133] The embodiments of the present application have been described above, and the above description is exemplary, not exhaustive, and is not limited to the disclosed embodiments. Many modifications and changes will be apparent to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments. The selection of terms used herein is intended to best explain the principles of the embodiments, practical applications, or improvements to the technology in the market, or to enable other persons of ordinary skill in the art to understand the embodiments disclosed herein.

Claims

1. A method for determining a vehicle yaw angle, characterized in that: include: Acquire image data and point cloud data collected from the front of the host vehicle during driving, so as to extract vehicle information of at least one first vehicle from the image data, and extract vehicle information of at least one second vehicle from the point cloud data, wherein the vehicle information includes: position, speed and yaw angle; The at least one first vehicle and the at least one second vehicle are respectively regarded as two non-intersecting vertex sets, and a connecting edge is established between the two non-intersecting vertex sets according to the position and the speed, so that a vehicle having the smallest distance between the same vehicle corresponding to the connecting edge and the main vehicle is regarded as a target vehicle; Acquire the yaw angle of the target vehicle at a previous moment, the control information at a previous moment, and the covariance matrix of the target vehicle at a previous moment, wherein the control information is information used to control the vehicle, so as to input the yaw angle of the target vehicle at a previous moment and the control information at a previous moment into a preset nonlinear state transfer function, and predict the yaw angle of the target vehicle at a current moment; Predicting the covariance matrix at the current moment according to the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at the previous moment, and the noise covariance matrix at the previous moment, and determining the measurement residual of the image sensor according to the yaw angle extracted from the image data and the nonlinear measurement function of the image sensor, and determining the measurement residual of the radar sensor according to the yaw angle extracted from the point cloud data and the nonlinear measurement function of the radar sensor; Determine the Kalman gain of the image sensor according to the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor; and determine the Kalman gain of the radar sensor according to the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor; The current yaw angle of the target vehicle is updated according to the two Kalman gains and the two measurement residuals to obtain a target yaw angle of the target vehicle.

2. The method according to claim 1, characterized in that Determining the Kalman gain of the image sensor according to the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor, and determining the Kalman gain of the radar sensor according to the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor, including: The Kalman gain is calculated by the following formula: Where i is 1 or 2, and are the Kalman gain of the image sensor and the Kalman gain of the radar sensor, P k is the covariance matrix at the current moment, and are the Jacobian matrix of the nonlinear measurement function of the image sensor and the Jacobian matrix of the nonlinear measurement function of the radar sensor, respectively, They are The transposed matrix and The transposed matrix of is the measurement noise covariance matrix of the image sensor.

3. The method according to claim 1, characterized in that: The method of updating the current yaw angle of the target vehicle according to the two Kalman gains and the two measurement residuals to obtain the target yaw angle of the target vehicle includes: The target yaw angle of the target vehicle is updated by the following steps: Among them, x k is the target yaw angle of the target vehicle, is the current yaw angle of the target vehicle, is the Kalman gain of the image sensor, is the measurement residual of the image sensor, is the Kalman gain of the radar sensor, is the measurement residual of the radar sensor.

4. The method according to claim 1, characterized in that: The predicting the covariance matrix at the current moment according to the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at the previous moment, and the noise covariance matrix at the previous moment includes: The covariance matrix at the current moment is predicted by the following formula: Among them, P k is the covariance matrix at the current moment, F k-1 is the Jacobian matrix of the nonlinear state transfer function, is the transposed matrix of the Jacobian matrix of the nonlinear state transfer function, Q k-1 is the noise covariance matrix at the previous moment.

5. The method according to claim 1, characterized in that The method further comprises: The covariance matrix at the current moment is updated by using the two Kalman gains and the Jacobian matrix of the measurement functions of the two sensors. The update can be implemented by the following formula: Among them, the P k is the covariance matrix at the current moment before updating, I is the identity matrix, is the Kalman gain of the image sensor, the is the Jacobian matrix of the measurement function of the image sensor, is the Kalman gain of the radar sensor, is the Jacobian matrix of the radar sensor’s measurement function, is the updated covariance matrix at the current moment.

6. The method according to any one of claims 1 to 5, characterized in that The determining vehicle information of at least one first vehicle according to the image data comprises: extracting target object features from the image data; Matching the target object feature with a preset feature in a preset database to obtain a target object feature that successfully matches the preset feature as a target vehicle feature; Acquire two-dimensional position information of the target vehicle feature in the image data; Determining three-dimensional position information of the target vehicle in three-dimensional space according to the two-dimensional position information of the target vehicle feature in the image data; The three-dimensional position information is converted into a rotation matrix or a quaternion to extract the yaw angle of the first vehicle from the rotation matrix or the quaternion.

7. The method according to any one of claims 1 to 5, characterized in that The determining vehicle information of at least one second vehicle according to the point cloud data comprises: Clustering the point cloud data to obtain at least one point cloud cluster, each of the point cloud clusters corresponding to a second vehicle; For each of the point cloud clusters, a covariance matrix is ​​constructed according to each point cloud data in the point cloud cluster, and the covariance matrix is ​​eigen-decomposed to obtain a signal subspace and a noise subspace; According to the orthogonality of the signal subspace and the noise subspace, a yaw angle of a second vehicle corresponding to the point cloud cluster is determined.

8. A vehicle yaw angle determination device, comprising: An acquisition module, used to acquire image data and point cloud data collected from the front of the host vehicle during driving, so as to extract vehicle information of at least one first vehicle from the image data, and to extract vehicle information of at least one second vehicle from the point cloud data, wherein the vehicle information includes: position, speed and yaw angle; a processing module, configured to respectively regard the at least one first vehicle and the at least one second vehicle as two non-intersecting vertex sets, and establish a connecting edge between the two non-intersecting vertex sets according to the position and the speed, so as to regard a vehicle having a minimum distance between the same vehicle corresponding to the connecting edge and the host vehicle as a target vehicle; A prediction module, used to obtain the yaw angle of the target vehicle at the last moment, the control information at the last moment and the covariance matrix of the target vehicle at the last moment, wherein the control information is information used to control the vehicle, so as to input the yaw angle of the target vehicle at the last moment and the control information at the last moment into a preset nonlinear state transfer function, and predict the yaw angle of the target vehicle at the current moment; A first determination module is used to predict the covariance matrix at a current moment according to the Jacobian matrix of the nonlinear state transfer function, the covariance matrix at a previous moment, and the noise covariance matrix at a previous moment, and to determine the measurement residual of the image sensor according to the yaw angle extracted from the image data and the nonlinear measurement function of the image sensor, and to determine the measurement residual of the radar sensor according to the yaw angle extracted from the point cloud data and the nonlinear measurement function of the radar sensor; a second determination module, configured to determine the Kalman gain of the image sensor according to the Jacobian matrix of the nonlinear measurement function of the image sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the image sensor, and to determine the Kalman gain of the radar sensor according to the Jacobian matrix of the nonlinear measurement function of the radar sensor, the covariance matrix at the current moment, and the measurement noise covariance matrix of the radar sensor; An updating module is used to update the current yaw angle of the target vehicle according to the two Kalman gains and the two measurement residuals to obtain a target yaw angle of the target vehicle.

9. An electronic device, characterized in that: include: processor; as well as A memory having executable codes stored thereon, which, when executed by the processor, causes the processor to execute the method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: An executable code is stored thereon, and when the executable code is executed by a processor of an electronic device, the processor is caused to execute the method as claimed in any one of claims 1 to 7.

Citation Information

Patent Citations

  • Vehicle auxiliary driving system, lane line space-time fitting system and method and FPGA chip

    CN113537100A

  • Environment adaptive perception target tracking method, system and equipment

    CN117522922A

  • All-weather vehicle-mounted sensing system based on multiple millimeter wave radars

    CN118485992A

  • State quantity estimation device, control device, and state quantity estimation method

    US20210201034A1

  • Lane-based automatic calibration of lidar on a vehicle

    US20240248186A1