Method for determining direction and track of vehicle coming in opposite direction at crossroad and related equipment

Abstract

The invention discloses a crossroad opposite coming vehicle orientation and trajectory determination method and related equipment. The method comprises the following steps: acquiring image information of an opposite vehicle; obtaining opposite vehicle corner point information based on the opposite vehicle image information; taking an intersection point closer to the vehicle as a reference point; acquiring reference point speed information and course angle information based on the reference point position information corresponding to the at least two signal periods; acquiring predicted position information and predicted course angle information of the vehicle according to the reference point speed information, the course angle information and the course angular speed information based on the basic motion model; performing one-step prediction by using a first Kalman filter according to the predicted position information and the predicted course angle information of the vehicle to obtain a state value at the next moment and a covariance thereof; based on the state value at the next moment, the covariance of the state value and the measurement data of the sensor, a second Kalman filter is used for correcting the predicted value, and the state estimation value and the covariance at the current moment are calculated; and determining an orientation and a trajectory based on the state estimation value and the covariance at the current moment.

Description

Technical Field

[0001] The present specification relates to the field of intelligent driving, and more specifically, the present application relates to a method for determining the position and trajectory of an oncoming vehicle at an intersection and related equipment. Background Art

[0002] Current autonomous driving systems mainly focus on detecting vehicles coming in the same direction, such as adaptive cruise control (ACC) and automatic emergency braking (AEB). However, these systems usually do not have the ability to judge oncoming vehicles, which makes their use scenarios relatively limited. In addition, existing systems usually rely on multiple millimeter-wave radars or other expensive equipment, which makes it difficult to maintain a competitive advantage in the cost-sensitive market.

[0003] Millimeter-wave radars have a weak ability to identify static objects and cannot effectively distinguish static objects such as roadside trees, buildings, and traffic signs, which may cause the system to falsely report or miss reports. The angular resolution of millimeter-wave radars is insufficient, making it difficult to accurately identify detailed features of vehicles, such as vehicle model and color, which affects recognition accuracy. When millimeter-wave radars are in severe weather conditions (such as rain and snow) or are affected by road pollution, their detection performance will drop significantly, or even fail to work properly, seriously affecting the stability of the system. Millimeter-wave radars generate a large amount of data, requiring complex data processing and algorithm optimization, which significantly increases the complexity and development difficulty of the system and reduces the efficiency of system implementation.

[0004] Due to these limitations, the existing technology does not perform well in intersection scenarios, and relies on millimeter-wave radar or other expensive sensor configurations, which makes the system costly and difficult to adapt to the current fierce market competition environment. Therefore, it is urgent to optimize and simplify the existing solutions to reduce the complexity and cost of the system. Summary of the invention

[0005] A series of simplified concepts are introduced in the Summary of the Invention section, which will be further described in detail in the Detailed Description of the Invention section. The Summary of the Invention section of this application does not mean to attempt to define the key features and essential technical features of the claimed technical solution, nor does it mean to attempt to determine the scope of protection of the claimed technical solution.

[0006] In a first aspect, the present application proposes a method for determining the position and trajectory of an oncoming vehicle at an intersection, comprising:

[0007] Obtaining oncoming vehicle image information;

[0008] Acquiring the oncoming vehicle corner point information based on the oncoming vehicle image information;

[0009] The intersection point that is closer to the vehicle is used as the reference point;

[0010] Acquire reference point velocity information and heading angle information based on the reference point position information corresponding to the at least two signal periods;

[0011] Based on the basic motion model, the vehicle predicted position information and predicted heading angle information are obtained according to the reference point speed information, the heading angle information and the heading angular velocity information;

[0012] A first Kalman filter is used to perform a one-step prediction based on the vehicle predicted position information and the predicted heading angle information to obtain a state value and its covariance at the next moment;

[0013] Based on the state value at the next moment and its covariance and the measurement data of the sensor, the predicted value is corrected by using the second Kalman filter to calculate the state estimation value and covariance at the current moment;

[0014] The direction and trajectory of the oncoming vehicle are determined based on the current state estimation value and covariance.

[0015] In a feasible implementation, when the recognition is complete, the above-mentioned corner point information of the oncoming vehicle is the 8 corner endpoints of the outer contour cuboid corresponding to the oncoming vehicle; when the recognition is incomplete, it is the partial corner endpoints of the outer contour cuboid corresponding to the above-mentioned vehicle.

[0016] In a feasible implementation manner, the obtaining of reference point velocity information and heading angle information based on the reference point position information corresponding to the at least two signal periods includes:

[0017] Performing differential solution based on the reference point position information corresponding to the at least two signal periods to obtain reference point velocity information;

[0018] Performing curve fitting based on the reference point position information corresponding to the at least two signal periods of the reference point to obtain heading angle information;

[0019] In a feasible implementation manner, when the oncoming vehicle is traveling on a curve, the basic motion model is a nonlinear motion model.

[0020] In a feasible implementation manner, when the oncoming vehicle is traveling on a straight road, the basic motion model is a linear motion model.

[0021] In a feasible implementation manner, the first Kalman filter is constructed based on a constant rotation rate and speed model.

[0022] In a feasible implementation manner, the filter gain K of the second Kalman filter is k for:

[0023]

[0024] J H is the Jacobian matrix of the sensor measurement matrix, is the Jacobian transposed matrix of the sensor measurement matrix, matrix R is the sensor measurement noise covariance matrix, P k is the state covariance at the current moment.

[0025] In a second aspect, the present application proposes a device for determining the position and trajectory of an oncoming vehicle at an intersection, comprising:

[0026] A first acquisition unit, used to acquire image information of an oncoming vehicle;

[0027] A second acquisition unit is used to acquire the oncoming vehicle corner point information based on the oncoming vehicle image information;

[0028] A first determining unit is used to use an intersection point that is closer to the vehicle as a reference point;

[0029] A third acquisition unit, configured to acquire reference point velocity information and heading angle information based on the reference point position information corresponding to the at least two signal periods;

[0030] A fourth acquisition unit, configured to acquire the vehicle predicted position information and predicted heading angle information according to the reference point speed information, the heading angle information and the heading angular velocity information based on a basic motion model;

[0031] A fifth acquisition unit, configured to perform a one-step prediction using a first Kalman filter according to the vehicle predicted position information and the predicted heading angle information, and acquire a state value and a covariance thereof at a next moment;

[0032] A calculation unit, configured to calculate a state estimation value and covariance at the current moment by using a second Kalman filter to correct the prediction value based on the state value at the next moment and its covariance and the measurement data of the sensor;

[0033] The second determination unit is used to determine the direction and trajectory of the oncoming vehicle based on the state estimation value and covariance at the current moment.

[0034] In a third aspect, an electronic device comprises: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor is used to implement the steps of a method for determining the position and trajectory of an oncoming vehicle at an intersection as described in any one of the first aspects above when executing the computer program stored in the memory.

[0035] In a fourth aspect, the present application further proposes a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method for determining the position and trajectory of an oncoming vehicle at an intersection of any one of the first aspects.

[0036] In summary, the existing millimeter-wave radar cannot accurately identify the detailed features of the vehicle, such as the model and color of the vehicle, in the detection of oncoming vehicles due to its low target resolution. This affects the effectiveness of the system in complex intersection scenes. The method proposed in this application uses a forward-looking visual camera and combines it with advanced image processing algorithms to more accurately extract the corner point information of the vehicle (such as the upper corner point and the outer rectangular corner point), thereby identifying the direction and trajectory of the oncoming vehicle. The visual device can obtain clearer images, identify more details, and improve the accurate judgment of the vehicle's motion state. The present invention: through the image information obtained by the camera, combined with the algorithm to identify the corner points of static objects and dynamic vehicles, it can effectively avoid the problem caused by the radar system's poor ability to recognize static objects. The camera can not only distinguish between static and dynamic objects, but also filter irrelevant static targets through algorithms, thereby improving the accuracy of the system. The present invention only relies on a system equipped with a surround-view camera and a forward-looking visual camera, which significantly reduces the complexity and cost of the system hardware. At the same time, the image data collected by the camera is processed to avoid the complex data processing requirements of the millimeter-wave radar in the auxiliary function of the intersection, making the system more economical and efficient, and adapting to the current cost-sensitive market demand. The camera system performs more stably under certain weather conditions. Combined with the image processing algorithm, even under complex weather conditions, it can track and predict oncoming vehicles through clear image information. In addition, the image processing algorithm can optimize the image quality under adverse environmental conditions through image enhancement technology to ensure the reliability and sustainability of the system. The present invention uses a Kalman filter to predict and correct the vehicle motion state. Through the first Kalman filter, a one-step prediction is performed, and the second Kalman filter is combined to correct the predicted value according to the measurement data of the sensor, so that the speed, position and heading angle of the vehicle can be updated more efficiently. The application of the two-stage Kalman filter can greatly improve the prediction accuracy of the oncoming vehicle, reduce errors, and achieve accurate tracking of the target vehicle's motion trajectory. By collecting image data through a surround-view camera and processing it in combination with an algorithm based on Kalman filtering, faster response speed and higher real-time performance can be achieved. In the case of large noise and environmental interference, the Kalman filter can improve the robustness of the system through multiple iterative corrections, making the calculation of the position and trajectory of oncoming vehicles more reliable. The present invention not only greatly reduces the complexity and cost of the system, but also improves the recognition and tracking accuracy of oncoming vehicles by optimizing the system structure and reducing the dependence on expensive hardware (such as millimeter-wave radar) and relying only on forward-looking visual cameras and algorithm design. In complex intersection scenarios, the present invention effectively compensates for the limitations of the millimeter-wave radar system, enhances the system's adaptability under a variety of weather and environmental conditions, and makes the vehicle's position and trajectory prediction more practical and stable. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Various other advantages and benefits will become apparent to those of ordinary skill in the art by reading the detailed description of the preferred embodiments below. The accompanying drawings are only for the purpose of illustrating the preferred embodiments and are not to be considered as limiting the present specification. Also, the same reference symbols are used throughout the accompanying drawings to represent the same components. In the accompanying drawings:

[0038] Figure 1 A schematic diagram of the process of determining the position and trajectory of an oncoming vehicle at an intersection provided in an embodiment of the present application;

[0039] Figure 2 A schematic diagram of a scenario for determining the position and trajectory of an oncoming vehicle at an intersection provided in an embodiment of the present application;

[0040] Figure 3 A schematic diagram of another scenario for determining the position and trajectory of an oncoming vehicle at an intersection provided in an embodiment of the present application;

[0041] Figure 4 A schematic diagram of another scenario for determining the position and trajectory of an oncoming vehicle at an intersection provided in an embodiment of the present application;

[0042] Figure 5 A schematic diagram of the structure of a device for determining the position and trajectory of an oncoming vehicle at an intersection provided in an embodiment of the present application;

[0043] Figure 6 A schematic diagram of the structure of an electronic device for determining the position and trajectory of oncoming vehicles at an intersection provided in an embodiment of the present application. DETAILED DESCRIPTION

[0044] The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments described herein can be implemented in an order other than that illustrated or described herein. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices. The technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments.

[0045] See also Figure 1, which is a schematic diagram of the process of determining the position and trajectory of an oncoming vehicle at an intersection provided by an embodiment of the present application, and may specifically include:

[0046] S110, obtaining image information of oncoming vehicles;

[0047] Exemplarily, image data of the oncoming vehicle is obtained from the sensor, and the image data will serve as the basis for extracting the vehicle corner information in the subsequent step. The image information can be obtained by a camera or other visual device and enter the next step as input data.

[0048] S120, obtaining oncoming vehicle corner point information based on the oncoming vehicle image information;

[0049] Exemplarily, based on the image information obtained in S110, the corner points of the target vehicle are identified. Corner points are prominent points on the outer contour of the vehicle, and are usually used to describe the spatial position and posture of the vehicle. In this step, the algorithm needs to analyze the image and extract corner point information related to the vehicle, such as the upper corner point of the vehicle, the corner point of the outer cuboid, etc.

[0050] S130, taking the intersection point that is closer to the vehicle as a reference point;

[0051] For example, among multiple corner points, an intersection point that is closer to the vehicle is selected as a reference point. This reference point represents the key position information of the oncoming vehicle, and subsequent motion prediction and heading angle estimation will be calculated based on the position change of this reference point.

[0052] S140, obtaining reference point velocity information and heading angle information based on the reference point position information corresponding to the at least two signal periods;

[0053] Exemplarily, the position information of the reference point is collected in at least two signal cycles. By comparing the position changes of the reference point in these cycles, the speed information of the reference point and the heading angle information of the target vehicle are calculated. The heading angle refers to the angle between the driving direction of the target vehicle and the Z axis, which is derived by the change of the reference point position over time.

[0054] S150, obtaining vehicle predicted position information and predicted heading angle information based on the reference point speed information, the heading angle information and the heading angular velocity information based on the basic motion model;

[0055] For example, based on the basic motion model, the future position and heading angle of the target vehicle are predicted using the reference point speed information, heading angle information, and heading angular velocity information calculated in the previous step. The basic motion model can describe the linear driving or curved driving of the vehicle. After combining these parameters, the future driving state of the vehicle can be estimated.

[0056] S160, performing a one-step prediction using a first Kalman filter according to the predicted vehicle position information and the predicted heading angle information, to obtain a state value and its covariance at the next moment;

[0057] Exemplarily, a first Kalman filter is used to perform a one-step state prediction based on the vehicle predicted position information and predicted heading angle information. This step outputs the state value and its covariance matrix at the next moment. The state value includes dynamic information such as the vehicle's position and speed, and the covariance matrix reflects the uncertainty of these predicted values.

[0058] S170, based on the state value at the next moment and its covariance and the measurement data of the sensor, using the second Kalman filter to correct the predicted value, and calculate the state estimation value and covariance at the current moment;

[0059] Exemplarily, the prediction result of the previous step is corrected by using the second Kalman filter in combination with the real-time measurement data of the sensor. The state estimation value and the covariance matrix at the current moment are updated by comparing the measurement data with the prediction result. This step can improve the estimation accuracy of the system for the current state and reduce the prediction error.

[0060] S180. Determine the direction and trajectory of the oncoming vehicle based on the current state estimation value and covariance.

[0061] For example, the specific position and motion trajectory of the oncoming vehicle are determined based on the current state estimation value and covariance. By integrating the vehicle's position information, speed and heading angle, the movement of the oncoming vehicle can be accurately tracked and the vehicle's trajectory information can be updated in real time.

[0062] In summary, the existing millimeter-wave radar cannot accurately identify the detailed features of the vehicle, such as the model and color of the vehicle, in the detection of oncoming vehicles due to its low target resolution. This affects the effectiveness of the system in complex intersection scenes. The method proposed in this application uses a forward-looking visual camera and combines it with advanced image processing algorithms to more accurately extract the corner point information of the vehicle (such as the upper corner point and the outer rectangular corner point), thereby identifying the direction and trajectory of the oncoming vehicle. The visual device can obtain clearer images, identify more details, and improve the accurate judgment of the vehicle's motion state. The present invention: through the image information obtained by the camera, combined with the algorithm to identify the corner points of static objects and dynamic vehicles, it can effectively avoid the problem caused by the radar system's poor ability to recognize static objects. The camera can not only distinguish between static and dynamic objects, but also filter irrelevant static targets through algorithms, thereby improving the accuracy of the system. The present invention only relies on a system equipped with a surround-view camera and a forward-looking visual camera, which significantly reduces the complexity and cost of the system hardware. At the same time, the image data collected by the camera is processed to avoid the complex data processing requirements of the millimeter-wave radar in the auxiliary function of the intersection, making the system more economical and efficient, and adapting to the current cost-sensitive market demand. The camera system performs more stably under certain weather conditions. Combined with the image processing algorithm, even under complex weather conditions, it can track and predict oncoming vehicles through clear image information. In addition, the image processing algorithm can optimize the image quality under adverse environmental conditions through image enhancement technology to ensure the reliability and sustainability of the system. The present invention uses a Kalman filter to predict and correct the vehicle motion state. Through the first Kalman filter, a one-step prediction is performed, and the second Kalman filter is combined to correct the predicted value according to the measurement data of the sensor, so that the speed, position and heading angle of the vehicle can be updated more efficiently. The application of the two-stage Kalman filter can greatly improve the prediction accuracy of the oncoming vehicle, reduce errors, and achieve accurate tracking of the target vehicle's motion trajectory. By collecting image data through a surround-view camera and processing it in combination with an algorithm based on Kalman filtering, faster response speed and higher real-time performance can be achieved. In the case of large noise and environmental interference, the Kalman filter can improve the robustness of the system through multiple iterative corrections, making the calculation of the position and trajectory of oncoming vehicles more reliable. The present invention not only greatly reduces the complexity and cost of the system, but also improves the recognition and tracking accuracy of oncoming vehicles by optimizing the system structure and reducing the dependence on expensive hardware (such as millimeter-wave radar) and relying only on forward-looking visual cameras and algorithm design. In complex intersection scenarios, the present invention effectively compensates for the limitations of the millimeter-wave radar system, enhances the system's adaptability under a variety of weather and environmental conditions, and makes the vehicle's position and trajectory prediction more practical and stable.

[0063] In some examples, when the recognition is complete, the above-mentioned corner point information of the oncoming vehicle is the eight corner endpoints of the outer contour rectangle corresponding to the oncoming vehicle; when the recognition is incomplete, it is the partial corner endpoints of the outer contour rectangle corresponding to the above-mentioned vehicle.

[0064] For example, the contours identified by the target vehicle are numbered. Note: Generally, each sensor output has its own output rules. The present invention is described in the following order. (No. 1 is at the bottom left, and marked counterclockwise); other targets are numbered in sequence (if identified), such as Figure 2 As shown. Based on the reference point, when the target vehicle goes straight through the intersection, the upper corner of the vehicle's outer rectangular parallelepiped is selected as the reference point (x reft ,y reft ,z reft ), which is the corner point 3 in the figure; when the target vehicle turns left or right through the intersection at time t, the upper corner point closest to the target vehicle's outer cuboid and the ego vehicle is selected as the reference point (x reft ,y reft ,z reft ). When the target vehicle goes straight through the intersection without changing direction, the reference point (x reft ,y reft ,z reft ) is still the corner point of the vehicle's outer cuboid at the previous moment.

[0065] like Figure 3 As shown, when the target vehicle first goes straight and then turns left to enter the left field blind spot and pass through the intersection, the reference point is first switched from corner point 3 to corner point 9, and then switches from corner point 9 to corner point 11 after entering the left field blind spot;

[0066] like Figure 4 As shown, when the target vehicle first goes straight and then turns right to enter the right field blind spot and pass through the intersection, the reference point is first switched from corner point 3 to corner point 10, and then switched from corner point 10 to corner point 12 after entering the right field blind spot.

[0067] In some examples, the obtaining of reference point velocity information and heading angle information based on the reference point position information corresponding to the at least two signal periods includes:

[0068] Performing differential solution based on the reference point position information corresponding to the at least two signal periods to obtain reference point velocity information;

[0069] Curve fitting is performed based on the reference point position information corresponding to the at least two signal periods of the reference point to obtain the heading angle information.

[0070] Exemplarily, reference point position information in at least two signal cycles.

[0071] Operation: Based on the position of the same reference point in two different signal cycles, perform differential solution to calculate the speed of the target vehicle in each direction. The specific formula is as follows:

[0072]

[0073] Among them, x reft and z reft are the lateral and longitudinal coordinates of the reference point in the vehicle coordinate system at the time of signal cycle t, and T is the signal cycle. Through this differential formula, the velocity information v of the reference point in the x and z directions can be obtained xt and v zt , that is, the vehicle's moving speed.

[0074] In a complex road traffic environment, vehicles often change lanes, turn, etc., which requires the smart car to be able to accurately and quickly identify its heading angle. Based on the axisymmetry of the vehicle, the heading angle of the target vehicle It can be expressed as the angle between its driving direction and the Z axis of the stereoscopic vision system coordinate system, as shown in the figure

[0075] In a short period of time (several sampling cycles or dozens of sampling cycles), the sensor measurement data is fitted piece by piece, and the motion of the target vehicle is represented by the following quadratic curve:

[0076] y=ax 2 +bx+c

[0077] The parameters a, b, and c of the quadratic curve can be obtained by the least squares estimation method, and the slope of the quadratic curve can be obtained by the derivation principle:

[0078]

[0079] The slope of the curve indicates the direction of its tangent, which is the instantaneous direction of movement of the target vehicle or the opposite direction. By taking the inverse tangent of the slope of the curve, we can get the angle between the tangent direction of the curve and the Z axis, which is the heading angle of the target vehicle relative to the observation vehicle:

[0080]

[0081] In some examples, when the oncoming vehicle is traveling on a curve, the basic motion model is a nonlinear motion model.

[0082] Exemplarily, in vehicle tracking based on the motion characteristics of vehicles at intersections, the present invention selects to set a basic vehicle motion model, and the state vector of the target vehicle is expressed as

[0083]

[0084] In the formula, the position of the target vehicle in the vehicle coordinate system is represented by the data set (z, x), v, ω The speed, yaw angle and yaw velocity of the target vehicle are v, ω.

[0085] When the vehicle is driving on a curve, when ω≠0, the function is:

[0086]

[0087] Wherein, Z is the longitudinal position of the target vehicle in the vehicle coordinate system (usually indicating the front and rear direction of the vehicle). X is the lateral position of the target vehicle in the vehicle coordinate system (usually indicating the left and right direction of the vehicle). V is the speed of the target vehicle (i.e., the speed of the vehicle's movement). is the yaw angle of the target vehicle, which indicates the angle between the vehicle's driving direction and the reference direction (usually the Z axis). ω is the yaw rate of the target vehicle, which indicates the rotation rate of the vehicle when turning. It describes the speed at which the vehicle's angle changes during driving. V is the vehicle's driving speed. It indicates the speed at which the vehicle travels along the curve and is used in the formula to calculate the change in position.

[0088] and These terms describe the periodic characteristics of the vehicle's position change over time when driving on a curve. When turning, the vehicle's position change is affected by the yaw angle and the influence of yaw angular velocity ω\.

[0089] In some examples, when the oncoming vehicle is traveling on a straight road, the basic motion model is a linear motion model.

[0090] For example, when the vehicle is traveling in a straight line, the state transfer function is

[0091]

[0092] The basic principle of the algorithm of the present invention is to use the state estimation value of the previous moment and the observation value of the current moment to calculate the filtering value of the current moment. The filtering process can be divided into two stages, namely the state prediction stage and the state update stage.

[0093] In some examples, the first Kalman filter is constructed based on a constant turn rate and speed model.

[0094] For example, it is assumed that the speed v and the turning rate (yaw rate) ω of the target vehicle are constant. That is, during the prediction, the vehicle's driving speed and turning rate remain unchanged. This assumption is suitable for short-term vehicle motion prediction, such as the short-term motion prediction of oncoming vehicles at an intersection.

[0095] The extended Kalman filter is based on nonlinear system processing. It linearizes the state equation of the nonlinear system and uses the first term approximation of Taylor series expansion. The state prediction equation is:

[0096] x k+1 =g(x k ,u)

[0097] Where: x k+1 Represents the state vector at the k+1th moment.

[0098] g(x k ,u) represents the state transfer function based on the constant turn rate and speed model, which describes the state change of the target vehicle from time k to time k+1.

[0099] X k is the state vector at the kth moment, including position, velocity, yaw angle, etc.

[0100] u represents the processing noise, which includes the influence of external disturbances on the vehicle state during the motion process.

[0101] Among them, g(x k ,u) represents the state transfer function of the constant rate and speed model, and u represents the processing noise. The one-step state prediction covariance is:

[0102]

[0103] Among them, J A It is the Jacobian matrix obtained by taking the partial derivative of each element in the state transfer function, and Q is the covariance matrix for processing noise.

[0104] In some examples, the filter gain K of the second Kalman filter is k for:

[0105]

[0106] J H is the Jacobian matrix of the sensor measurement matrix, is the Jacobian transposed matrix of the sensor measurement matrix, matrix R is the sensor measurement noise covariance matrix, and Pk is the state covariance at the current moment.

[0107] For example, in the formula, J H is the Jacobian matrix of the sensor measurement matrix, and R is the sensor measurement noise covariance matrix.

[0108] State estimation equation:

[0109] x k =x k +K k (Zk -h(x k ))

[0110] In the formula, Z k is the sensor measurement value, h(x k ) is a nonlinear mapping function.

[0111] State Estimation Covariance

[0112] P k =(1-K k J H ) k

[0113] Where I is the unit matrix.

[0114] like Figure 5 As shown, the present application proposes a device for determining the position and trajectory of an oncoming vehicle at an intersection, comprising:

[0115] A first acquisition unit 21, used to acquire image information of an oncoming vehicle;

[0116] A second acquisition unit 22, configured to acquire the oncoming vehicle corner point information based on the oncoming vehicle image information;

[0117] A first determining unit 23, configured to use an intersection point that is closer to the vehicle as a reference point;

[0118] The third acquisition unit 24 is used to acquire the reference point speed information and heading angle information based on the reference point position information corresponding to the at least two signal cycles.

[0119] A fourth acquisition unit 25 is used to acquire the vehicle predicted position information and predicted heading angle information according to the reference point speed information, the heading angle information and the heading angular velocity information based on the basic motion model;

[0120] A fifth acquisition unit 26, configured to perform a one-step prediction using a first Kalman filter according to the vehicle predicted position information and the predicted heading angle information, and acquire a state value and a covariance thereof at a next moment;

[0121] A calculation unit 27 is used to calculate the state estimation value and covariance at the current moment by using a second Kalman filter to correct the prediction value based on the state value and covariance at the next moment and the measurement data of the sensor;

[0122] The second determination unit 28 is used to determine the direction and trajectory of the oncoming vehicle based on the current state estimation value and covariance.

[0123] In some examples, when the recognition is complete, the above-mentioned corner point information of the oncoming vehicle is the eight corner endpoints of the outer contour rectangle corresponding to the oncoming vehicle; when the recognition is incomplete, it is the partial corner endpoints of the outer contour rectangle corresponding to the above-mentioned vehicle.

[0124] In some examples, the obtaining of reference point velocity information and heading angle information based on the reference point position information corresponding to the at least two signal periods includes:

[0125] Performing differential solution based on the reference point position information corresponding to the at least two signal periods to obtain reference point velocity information;

[0126] Performing curve fitting based on the reference point position information corresponding to the at least two signal periods of the reference point to obtain heading angle information;

[0127] In some examples, when the oncoming vehicle is traveling on a curve, the basic motion model is a nonlinear motion model.

[0128] In some examples, when the oncoming vehicle is traveling on a straight road, the basic motion model is a linear motion model.

[0129] In some examples, the first Kalman filter is constructed based on a constant turn rate and speed model.

[0130] In some examples, the filter gain K of the second Kalman filter is k for:

[0131]

[0132] J H is the Jacobian matrix of the sensor measurement matrix, is the Jacobian transposed matrix of the sensor measurement matrix, matrix R is the sensor measurement noise covariance matrix, P k is the state covariance at the current moment.

[0133] like Figure 6 As shown, an embodiment of the present application also provides an electronic device 300, including a memory 310, a processor 320, and a computer program 311 stored in the memory 310 and executable on the processor. When the processor 320 executes the computer program 311, the steps of any method for determining the direction and trajectory of oncoming vehicles at the above-mentioned intersection are implemented.

[0134] Since the electronic device introduced in this embodiment is a device used to implement a device for determining the direction and trajectory of oncoming vehicles at an intersection in an embodiment of the present application, based on the method introduced in the embodiment of the present application, technical personnel in this field can understand the specific implementation method of the electronic device of this embodiment and its various variations. Therefore, how the electronic device implements the method in the embodiment of the present application is not introduced in detail here. As long as the equipment used by technical personnel in this field to implement the method in the embodiment of the present application is within the scope of protection of this application.

[0135] In the specific implementation process, when the computer program 311 is executed by the processor, it can achieve Figure 1 Any implementation manner in the corresponding embodiments.

[0136] It should be noted that in the above embodiments, the description of each embodiment has its own emphasis, and for parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0137] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include computer-usable program code.

[0138] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded computer, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0139] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0140] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0141] An embodiment of the present application also provides a computer program product, which includes computer software instructions. When the computer software instructions are executed on a processing device, the processing device executes the process of determining the direction and trajectory of oncoming vehicles at an intersection in the corresponding embodiment.

[0142] The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on the computer, the process or function according to the embodiment of the present application is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from a website site, a computer, a server or a data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line (digital subscriber line, DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) mode to another website site, computer, server or data center. The computer-readable storage medium can be any available medium that a computer can store or a data storage device such as a server or a data center that includes one or more available media integration. The available medium can be a magnetic medium, (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a solid state drive (SSD)), etc.

[0143] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0144] In the several embodiments provided in the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms.

[0145] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0146] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0147] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the various embodiments of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, ROM), random access memory (Random Access Memory, RAM), disk or optical disk and other media that can store program codes.

[0148] The above embodiments are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A method for determining the position and trajectory of an oncoming vehicle at an intersection, characterized in that: include: Obtaining oncoming vehicle image information; Acquiring oncoming vehicle corner point information based on the oncoming vehicle image information; The intersection point that is closer to the vehicle is used as the reference point; Acquire reference point velocity information and heading angle information based on the reference point position information corresponding to the at least two signal periods; Acquire vehicle predicted position information and predicted heading angle information according to the reference point speed information, the heading angle information and the heading angular velocity information based on a basic motion model; A first Kalman filter is used to perform a one-step prediction based on the predicted vehicle position information and the predicted heading angle information to obtain a state value and its covariance at the next moment; Based on the state value at the next moment and its covariance and the measurement data of the sensor, the predicted value is corrected by using the second Kalman filter to calculate the state estimation value and covariance at the current moment; The direction and trajectory of the oncoming vehicle are determined based on the state estimation value and covariance at the current moment.

2. The method for determining the position and trajectory of an oncoming vehicle at an intersection according to claim 1, characterized in that: When the recognition is complete, the corner point information of the oncoming vehicle is the eight corner endpoints of the outer contour cuboid corresponding to the oncoming vehicle; when the recognition is incomplete, it is the partial corner endpoints of the outer contour cuboid corresponding to the vehicle.

3. The method for determining the position and trajectory of an oncoming vehicle at an intersection according to claim 1, characterized in that: The obtaining of reference point velocity information and heading angle information based on the reference point position information corresponding to the at least two signal periods includes: Performing a differential solution based on the reference point position information corresponding to the at least two signal periods to obtain reference point velocity information; Curve fitting is performed based on the reference point position information corresponding to the at least two signal periods of the reference point to obtain the heading angle information.

4. The method for determining the position and trajectory of an oncoming vehicle at an intersection according to claim 1, characterized in that: When the oncoming vehicle is traveling on a curve, the basic motion model is a nonlinear motion model.

5. The method for determining the position and trajectory of an oncoming vehicle at an intersection according to claim 1, characterized in that: When the oncoming vehicle is traveling on a straight road, the basic motion model is a linear motion model.

6. The method for determining the position and trajectory of an oncoming vehicle at an intersection according to claim 1, characterized in that: The first Kalman filter is constructed based on a constant turn rate and speed model.

7. The method for determining the position and trajectory of an oncoming vehicle at an intersection according to claim 1, characterized in that: The filter gain K of the second Kalman filter k for: J H is the Jacobian matrix of the sensor measurement matrix, is the Jacobian transposed matrix of the sensor measurement matrix, matrix R is the sensor measurement noise covariance matrix, P k is the state covariance at the current moment.

8. A device for determining the position and trajectory of an oncoming vehicle at an intersection, characterized in that: include: A first acquisition unit, used to acquire image information of an oncoming vehicle; A second acquisition unit, configured to acquire the oncoming vehicle corner point information based on the oncoming vehicle image information; A first determining unit is used to use an intersection point that is closer to the vehicle as a reference point; A third acquisition unit, configured to acquire reference point velocity information and heading angle information based on the reference point position information corresponding to the at least two signal periods; A fourth acquisition unit, configured to acquire vehicle predicted position information and predicted heading angle information according to the reference point speed information, the heading angle information and the heading angular velocity information based on a basic motion model; A fifth acquisition unit, configured to perform a one-step prediction using a first Kalman filter according to the predicted vehicle position information and the predicted heading angle information, and acquire a state value and a covariance thereof at a next moment; A calculation unit, configured to calculate a state estimation value and covariance at a current moment by using a second Kalman filter to correct the prediction value based on the state value at the next moment and its covariance and the measurement data of the sensor; The second determination unit is used to determine the direction and trajectory of the oncoming vehicle based on the state estimation value and covariance at the current moment.

9. An electronic device, comprising: A memory and a processor, wherein the processor is used to implement the steps of the method for determining the position and trajectory of an oncoming vehicle at an intersection as described in any one of claims 1 to 7 when executing a computer program stored in the memory.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method for determining the position and trajectory of an oncoming vehicle at an intersection as described in any one of claims 1-7 are implemented.

Images