A monocular camera-based intelligent vehicle target tracking system and method

Abstract

The application claims a monocular camera-based intelligent vehicle target tracking system and method. The system comprises a sensor group and a target tracker. The sensor group comprises monocular vision, GNSS and INS sensors. The target tracker comprises a monocular vision target detection, communication, vehicle pose detection and target tracking module. The monocular vision target detection module obtains target orientation information from the monocular vision sensor; the communication module obtains GNSS differential information from a positioning base station; the vehicle pose detection module reads the measurement data of the GNSS and INS sensors and the GNSS differential information to calculate vehicle pose information; the target tracking module uses the vehicle maneuver information calculated from the vehicle pose information and the target orientation information to perform target tracking, obtaining the motion state information such as the orientation, distance and speed of the target. The application improves the monocular camera target tracking accuracy of the intelligent vehicle, especially the target tracking accuracy in a slope scene, and can effectively improve the reliability and safety of the assisted driving and automatic driving systems.

Description

Technical Field

[0001] This invention belongs to the field of intelligent vehicle environmental perception technology, specifically relating to an intelligent vehicle target tracking system and method based on a monocular camera. Background Technology

[0002] Intelligent vehicles are integrated systems encompassing environmental perception, dynamic planning and decision-making, and multi-level assisted driving. The environmental perception system uses onboard sensors, such as LiDAR, cameras, and millimeter-wave radar, to detect targets in the surrounding environment, providing reliable data for vehicle decision-making and control. Monocular cameras, being relatively inexpensive, are widely used in intelligent vehicle environmental perception systems. Therefore, understanding how to effectively utilize monocular cameras for target detection and improve the environmental perception capabilities of intelligent vehicles is of significant importance and practical value.

[0003] Chinese Patent Application No. CN202210548990.7 discloses a target tracking method, terminal device, and storage medium based on monocular vision. This method tracks the distance between a target and the vehicle, and the distance measurement depends on the type of the target. Another Chinese Patent Application No. CN201910029050.5 describes a monocular vision ranging method for both indoor and outdoor static environments. This method compensates for the object distance based on the image distance error formula and incident error formula of a monocular camera to obtain the target distance. A third Chinese Patent Application No. CN202110092093.5 describes a processing method for monocular vision ranging. This method preprocesses the acquired image, using a Gaussian filter for initial noise reduction and filtering, and then calculates the actual distance to the target. However, intelligent vehicles operate in complex environments, often encountering slope scenarios. The methods described above have limitations in handling target detection in slope scenarios.

[0004] This invention addresses the complexity of the operating environment of intelligent vehicles, especially in slope scenarios, where monocular vision systems suffer from significant errors in target detection distance. By utilizing more accurate target orientation information and the intelligent vehicle's own mobility information for target tracking, the relative motion between the target and the intelligent vehicle is estimated, improving target tracking accuracy. This enhances the monocular camera tracking accuracy of intelligent vehicles in slope scenarios, thereby improving the reliability and safety of assisted driving or autonomous driving. Summary of the Invention

[0005] This invention aims to solve the problems of the prior art mentioned above. It proposes an intelligent vehicle target tracking system and method based on a monocular camera. The technical solution of this invention is as follows:

[0006] A smart vehicle target tracking system based on a monocular camera includes a sensor array and a target tracker, wherein:

[0007] The sensor group includes a monocular vision sensor, a global navigation satellite system (GNSS) sensor, and an inertial navigation system (INS) sensor. The monocular vision sensor is used to acquire and output external environment image data, the GNSS sensor is used to acquire and output the intelligent vehicle's own latitude and longitude and heading angle information, and the INS sensor is used to acquire and output the intelligent vehicle's own acceleration and heading angle angular velocity information.

[0008] The target tracker includes a monocular vision target detection module, a communication module, a vehicle pose detection module, and a target tracking module. The monocular vision target detection module is used to obtain target orientation information from images. The communication module obtains GNSS differential information and sends it to the vehicle pose detection module. The vehicle pose detection module is used to read the vehicle's own latitude, longitude, and heading angle information obtained by the GNSS sensor, the vehicle's own acceleration and heading angle angular velocity information obtained by the INS sensor, and the GNSS differential information to calculate the vehicle's own pose information. The target tracking module uses the vehicle's own pose information to calculate vehicle motion information and target orientation information to track the target and obtain motion state information including the target's orientation, distance, and speed.

[0009] A target tracking method based on the system includes the following steps:

[0010] 2.1 Target Orientation Detection: The monocular vision target detection module acquires raw image information from the monocular vision sensor and calculates the target orientation measurement set in the vehicle coordinate system. Where n is the number of target measurements detected by the monocular vision sensor at time k;

[0011] 2.2 Differential Correction Acquisition: The communication module acquires GNSS differential information and sends it to the vehicle pose detection module;

[0012] 2.3 Vehicle Own Pose Information Calculation: The vehicle pose detection module reads the vehicle's own latitude, longitude, and heading angle information acquired by the GNSS sensor, the vehicle's own acceleration and angular velocity information acquired by the INS sensor, and the GNSS differential information from the communication module to calculate the vehicle's own pose information in a coordinate system fixed to the ground. Including X-direction position Y-direction position X-direction velocity Y-direction velocity X-direction acceleration Y-direction acceleration Heading angle angular velocity of heading angle

[0013] 2.4 Target State Tracking: The target tracking module uses the target orientation measurement set from the monocular vision target detection module. And the vehicle's own pose information through the vehicle pose detection module Calculated vehicle motion information Target tracking is performed to obtain motion state information such as the target's orientation, distance, and speed in the vehicle coordinate system.

[0014] Furthermore, in step 2.1, target orientation detection: the monocular vision target detection module acquires the raw image information from the monocular vision sensor and calculates the target orientation measurement set in the vehicle coordinate system. Specifically, it includes:

[0015] First, a convolutional neural network algorithm is used to extract target features. Then, a fully connected network is used to obtain the target's orientation information, and a coordinate system transformation is used to obtain the target orientation measurement set in the vehicle coordinate system.

[0016] Furthermore, step 2.4, the target tracking method, includes the following steps:

[0017] 3.1 Coordinate system establishment: Select a point on the ground as the origin, with the north direction as the Y-axis, the east direction as the X-axis, and the Z-axis perpendicular to the ground to establish a coordinate system O-XYZ that is fixed to the ground; with the center of the front of the vehicle as the origin, the front as the Y-axis, the right as the X-axis, and the Z-axis perpendicular to the ground to establish a vehicle coordinate system V-X'Y'Z'.

[0018] 3.2 System Modeling: Establish the target motion state model and measurement model;

[0019] 3.3 Target Motion State Prediction: Based on the target motion state equation described in step 3.2 and the target state estimate at the previous moment. With covariance matrix Calculate the one-step prediction value of the target state respectively and the covariance matrix of the one-step prediction error

[0020] 3.4 Measurement and Screening: Utilizing Calculate the predicted observations of the target based on the measurement model. Set a sector tracking gate to target the measurement set. The target candidate measurement set is obtained through screening.

[0021] 3.5 Data Association: Predicted values ​​of the target trajectory and target candidate measurement set Perform data association;

[0022] 3.6 Target State Filtering: A Kalman filter is used to filter the associated target to obtain the optimal estimate of the target's motion state at time k. and

[0023] Furthermore, step 3.2, system modeling, involves establishing a target motion state model and a measurement model, specifically including:

[0024] At time k, the target motion state model in the vehicle coordinate system V-X'Y'Z' is:

[0025]

[0026] In the vehicle coordinate system V-X'Y'Z', Represents the target's X-direction position, Y-direction position, X-direction velocity, and Y-direction velocity at time k; G is the target noise gain, and W... k-1 For the target process noise at time k-1, F V The motion state transition matrix of the target is represented. F represents the motion state of the target at time k-1; O This is the vehicle's own state transition matrix. This represents the vehicle's positional maneuver in the X direction, positional maneuver in the Y direction, change in velocity in the X direction, and change in velocity in the Y direction relative to the vehicle coordinate system V-X'Y'Z' at time k.

[0027] At time k, the target measurement model in the vehicle coordinate system V-X'Y'Z' is:

[0028]

[0029] Among them, U k It is the target measurement noise at time k.

[0030] Furthermore, step 3.6 involves vehicle mobility information. The calculation method includes the following steps:

[0031] 4.1 Calculation of Vehicle Pose Information: The vehicle pose detection module reads the vehicle's latitude, longitude, and heading angle information acquired by the GNSS sensor, the vehicle's acceleration and angular velocity information acquired by the INS sensor, and the GNSS differential information from the communication module, and then calculates the vehicle's pose information in the O-XYZ coordinate system. Including X-direction position Y-direction position X-direction velocity Y-direction velocity X-direction acceleration Y-direction acceleration Heading angle angular velocity of heading angle

[0032] 4.2 Calculate vehicle motion information: Based on the vehicle's own pose information in coordinate system O-XYZ in 4.1 and the vehicle's own position information at the previous moment Calculate vehicle motion information in This represents the vehicle's positional maneuver in the X direction, positional maneuver in the Y direction, change in velocity in the X direction, and change in velocity in the Y direction relative to the vehicle coordinate system V-X'Y'Z' at time k.

[0033] Furthermore, step 3.5, data association, involves predicting the target trajectory value. and target candidate measurement set Data association is performed, specifically including: based on the predicted values ​​of the target trajectory. and target candidate measurement set calculate Let represent the probability that the m-th measurement at time k originates from the target. This indicates the probability that no measurement originates from the target, where:

[0034]

[0035]

[0036] in Indicates candidate measurement It is a correct measurement event from the target, using This indicates that none of the candidate metrics are correct events, and j represents the number of metrics;

[0037] For unassociated measurement targets, they are considered potential new target trajectories. In the subsequent n tracking processes, if the number of successful associations exceeds the threshold C_new, they are treated as real target trajectories. For unassociated targets, they are considered target trajectories that may need to be destroyed. In the subsequent n tracking processes, if the number of failed associations exceeds the threshold C_disp, the trajectory is destroyed.

[0038] Furthermore, step 3.6, target state filtering, involves using a Kalman filter to filter the associated target, thereby obtaining the optimal estimate of the target's motion state at time k. and Specifically, it includes:

[0039] The filtering steps include: calculating the Kalman gain K. k The optimal state estimate of the target is obtained through prior estimation and measurement. Update the optimal estimated covariance

[0040]

[0041]

[0042]

[0043] in:

[0044]

[0045] R is the measurement noise covariance, H is the transformation matrix from state variables to measurements, and I is the identity matrix. Indicates the actual target measurement. Let j represent the probability that the m-th measurement at time k comes from the target event, and j represent the number of measurements.

[0046] The advantages and beneficial effects of this invention are as follows:

[0047] This invention addresses the issue that existing intelligent vehicle target tracking systems only consider the target's motion model during target tracking, neglecting the vehicle's own motion. It proposes a monocular camera-based intelligent vehicle target tracking system and method that fully considers the influence of the vehicle's own motion, utilizing both the target's orientation information and the intelligent vehicle's own maneuvering information for target tracking, estimating the relative motion between the target and the intelligent vehicle, and improving target tracking accuracy. Furthermore, this method enhances the target tracking accuracy of the intelligent vehicle monocular camera in slope scenarios, effectively improving the reliability and safety of assisted driving and autonomous driving systems. Attached Figure Description

[0048] Figure 1 This is the overall framework of a preferred embodiment of the present invention, which provides an intelligent vehicle target tracking system and method based on a monocular camera.

[0049] Figure 2 This is a flowchart of the intelligent vehicle target tracking system based on a monocular camera according to the present invention;

[0050] Figure 3 This is a schematic diagram of the coordinate system and vehicle coordinate system fixed to the ground as described in this invention;

[0051] Figure 4 This is a flowchart of the target tracking algorithm of the present invention;

[0052] Figure 5 This is a flowchart of the intelligent vehicle mobility information calculation process of the present invention. Detailed Implementation

[0053] The technical solutions of the embodiments of the present invention will be clearly and thoroughly described below with reference to the accompanying drawings. The described embodiments are merely some embodiments of the present invention.

[0054] The technical solution of the present invention to solve the above-mentioned technical problems is:

[0055] This invention addresses the need for target tracking in intelligent vehicle environmental perception systems, and the problem of large errors in target detection distance of monocular vision systems in slope scenarios. It designs an intelligent vehicle target tracking system and method based on a monocular camera.

[0056] The specific embodiments of the present invention are described below with reference to the accompanying drawings and specific examples.

[0057] Figure 1 The diagram shows the overall framework of an intelligent vehicle target tracking system based on a monocular camera, as described below:

[0058] Includes sensor arrays and target trackers;

[0059] The sensor group includes a monocular vision sensor, a GNSS sensor, and an INS sensor; the monocular vision sensor is used to acquire image data of the external environment; the GNSS sensor is used to acquire the latitude, longitude, and heading angle information of the intelligent vehicle itself; and the INS sensor is used to acquire the acceleration and angular velocity information of the heading angle of the intelligent vehicle itself.

[0060] The target tracker includes a monocular vision target detection module, a communication module, a vehicle pose detection module, and a target tracking module. The monocular vision target detection module obtains target orientation information from the image. The communication module connects to the positioning base station, obtains GNSS positioning correction information from the positioning base station, and sends it to the vehicle pose detection module. The vehicle pose detection module reads the vehicle's latitude, longitude, and heading angle information from the GNSS sensor, the vehicle's acceleration and angular velocity information from the INS sensor, and the GNSS positioning correction information to calculate the vehicle's pose information. The target tracking module uses the vehicle's pose information to calculate vehicle motion information and target orientation information to track the target, obtaining the target's orientation, distance, speed, and other motion state information.

[0061] Figure 2 The diagram shows a flowchart of an intelligent vehicle target tracking system based on a monocular camera. The process includes the following steps:

[0062] (1) Target orientation detection: The monocular vision target detection module acquires the raw image information from the monocular vision sensor and calculates the target orientation measurement set in the vehicle coordinate system. Where n is the number of target measurements detected by the monocular vision sensor at time k;

[0063] (2) Differential correction amount acquisition: The communication module acquires GNSS differential information and sends it to the vehicle pose detection module;

[0064] (3) Calculation of vehicle pose information: The vehicle pose detection module reads the vehicle's latitude, longitude and heading angle information obtained by the GNSS sensor, the vehicle's acceleration and angular velocity information obtained by the INS sensor, and the GNSS differential information from the communication module to calculate the vehicle's pose information in a coordinate system fixed to the ground. Including X-direction position Y-direction position X-direction velocity Y-direction velocity X-direction acceleration Y-direction acceleration Heading angle angular velocity of heading angle

[0065] (4) Target state tracking: The target tracking module uses the target orientation measurement set from the monocular vision target detection module. And the vehicle's own pose information through the vehicle pose detection module Calculated vehicle motion information Target tracking is performed to obtain motion state information such as the target's orientation, distance, and speed in the vehicle coordinate system;

[0066] Figure 3 The diagram shows a coordinate system fixed to the ground and a vehicle coordinate system.

[0067] (1) Select a point on the ground as the origin, take the north direction as the Y axis, the east direction as the X axis, and the Z axis perpendicular to the ground to establish a coordinate system O-XYZ that is fixed to the ground.

[0068] (2) Establish a vehicle coordinate system V-X'Y'Z' with the center of the front of the vehicle as the origin, the front as the Y-axis, the right as the X-axis, and the Z-axis perpendicular to the ground;

[0069] Figure 4 The image shows a target tracking method, which includes the following steps:

[0070] (1) Establishing a coordinate system: Establish as follows Figure 3 The coordinate system O-XYZ, which is fixed to the ground, and the vehicle coordinate system V-X'Y'Z' are shown.

[0071] (2) Establishment of target motion state model and measurement model:

[0072]

[0073]

[0074] This patent uses a uniform velocity model for the target motion state model and a uniform velocity turning model for the intelligent vehicle motion state model as examples for illustration, but the target motion model is not limited to these two models, among which:

[0075]

[0076]

[0077]

[0078] W k-1 The process noise follows a normal distribution, and T is the time interval between two frames of images acquired by the camera.

[0079] (3) Target motion state prediction: Based on the target motion state equation described in step (2) and the target state estimate at the previous moment. With covariance matrix Calculate the one-step prediction value of the target state respectively and the covariance matrix of the one-step prediction error

[0080] (4) Measurement and screening: using Calculate the predicted observations of the target based on the measurement model. Set a sector tracking gate to target the measurement set. The target candidate measurement set is obtained through screening. Where j is the number of candidate measurements for the target at time k;

[0081] (5) Data association: Based on the predicted values ​​of the target trajectory and target candidate measurement set calculate Let represent the probability that the m-th measurement at time k originates from the target. This indicates the probability that no measurement originates from the target, where:

[0082]

[0083]

[0084] in Indicates candidate measurement It is a correct measurement event from the target, using This indicates that none of the candidate measurement sets are correct events. For unassociated measurement targets, they are considered as potential new target trajectories. In the subsequent n tracking processes, if the number of successful associations exceeds the threshold C_new, they are treated as real target trajectories. For unassociated targets, they are considered as target trajectories that may need to be destroyed. In the subsequent n tracking processes, if the number of failed associations exceeds the threshold C_disp, the trajectory is destroyed.

[0085] (6) Target state filtering: The associated targets are filtered using a Kalman filter;

[0086] The filtering steps include: calculating the Kalman gain K. k The optimal state estimate of the target is obtained through prior estimation and measurement. Update the optimal estimated covariance

[0087]

[0088]

[0089]

[0090] in:

[0091]

[0092] R is the measurement noise covariance, H is the transformation matrix from state variables to measurements, and I is the identity matrix.

[0093] Figure 5 The diagram shown is a flowchart of intelligent vehicle motion information calculation. The process includes the following steps:

[0094] (1) Vehicle pose information calculation: The vehicle pose detection module reads the vehicle's own latitude, longitude and heading angle information obtained by the GNSS sensor, the vehicle's own acceleration and heading angle angular velocity information obtained by the INS sensor, and the GNSS differential information from the communication module, and then calculates the vehicle's own pose information in the O-XYZ coordinate system. Including X-direction position Y-direction position X-direction velocity Y-direction velocity X-direction acceleration Y-direction acceleration Heading angle angular velocity of heading angle

[0095] (2) Vehicle Motion Information Calculation: The first step is to determine whether the system is a valid maneuver: Taking the target motion state model as a uniform speed model and the intelligent vehicle motion state model as a uniform speed turning model as an example, but the target motion model is not limited to these two models. In this example, the order of the intelligent vehicle motion model is higher than the order of the target motion model, so the system is a valid maneuver; The second step is to calculate the vehicle's own pose information in the coordinate system O-XYZ in step (1). and the vehicle's own position information at the previous moment Calculate the vehicle's motion information relative to the coordinate system O-XYZ.

[0096]

[0097] The vehicle's motion information relative to the coordinate system O-XYZ Transformation to vehicle motion information relative to the vehicle coordinate system V-X'Y'Z' This represents the vehicle's positional maneuver in the X direction, positional maneuver in the Y direction, change in velocity in the X direction, and change in velocity in the Y direction relative to the vehicle coordinate system V-X'Y'Z' at time k.

[0098] The systems, devices, modules, or units described in the above embodiments can be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, a computer can be, for example, a personal computer, laptop computer, cellular phone, camera phone, smartphone, personal digital assistant, media player, navigation device, email device, game console, tablet computer, wearable device, or any combination of these devices.

[0099] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0100] The above embodiments should be understood as illustrative only and not as limiting the scope of protection of the present invention. After reading the description of the present invention, those skilled in the art can make various alterations or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. An intelligent vehicle target tracking system based on a monocular camera, characterized in that, Includes a sensor array and a target tracker, wherein: The sensor group includes a monocular vision sensor, a global navigation satellite system (GNSS) sensor, and an inertial navigation system (INS) sensor. The monocular vision sensor is used to acquire and output external environment image data, the GNSS sensor is used to acquire and output the intelligent vehicle's own latitude and longitude and heading angle information, and the INS sensor is used to acquire and output the intelligent vehicle's own acceleration and heading angle angular velocity information. The target tracker includes a monocular vision target detection module, a communication module, a vehicle pose detection module, and a target tracking module. The monocular vision target detection module obtains target orientation information from the image. The communication module obtains GNSS differential information and sends it to the vehicle pose detection module. The vehicle pose detection module reads the vehicle's latitude, longitude, and heading angle information obtained from the GNSS sensor, the vehicle's acceleration and heading angle angular velocity information obtained from the INS sensor, and the GNSS differential information to calculate the vehicle's pose information. The target tracking module uses the vehicle's pose information to calculate vehicle motion information and target orientation information to track the target and obtain the target's orientation, distance, and velocity motion state information. The target tracking process of the target tracker includes: 2.1 Target Orientation Detection: The monocular vision target detection module acquires raw image information from the monocular vision sensor and calculates the target orientation measurement set in the vehicle coordinate system. , where n is the number of target measurements detected by the monocular vision sensor at time k; 2.2 Differential Correction Acquisition: The communication module acquires GNSS differential information and sends it to the vehicle pose detection module; 2.3 Vehicle Own Pose Information Calculation: The vehicle pose detection module reads the vehicle's own latitude, longitude, and heading angle information acquired by the GNSS sensor, the vehicle's own acceleration and angular velocity information acquired by the INS sensor, and the GNSS differential information from the communication module to calculate the vehicle's own pose information in a coordinate system fixed to the ground. Including X-direction position Y-direction position X-direction velocity Y-direction velocity X-axis acceleration Y-axis acceleration Heading angle Angular velocity of heading angle ; 2.4 Target State Tracking: The target tracking module uses the target orientation measurement set from the monocular vision target detection module. And the vehicle's own pose information through the vehicle pose detection module Calculated vehicle motion information Target tracking is performed to obtain information on the target's orientation, distance, and velocity motion in the vehicle coordinate system. The target tracking method in step 2.4 includes the following steps: 3.1 Establishing a coordinate system: Select a point on the ground as the origin, with due north as the Y-axis, due east as the X-axis, and the Z-axis perpendicular to the ground, to establish a coordinate system fixed to the ground. Establish a vehicle coordinate system with the center of the front of the vehicle as the origin, the front as the Y-axis, the right as the X-axis, and the Z-axis perpendicular to the ground. ; 3.2 System Modeling: Establish the target motion state model and measurement model; At time k, the vehicle coordinate system The target motion state model is as follows: (1) In the vehicle coordinate system Down, Let k represent the target's position in the X direction, position in the Y direction, velocity in the X direction, and velocity in the Y direction at time k. For target noise gain, The target process noise at time k-1, The motion state transition matrix represents the target. This represents the motion state of the target at time k-1; This is the vehicle's own state transition matrix. This represents the vehicle's own position relative to the vehicle coordinate system at time k. Position maneuver in the X direction, position maneuver in the Y direction, change of velocity in the X direction, and change of velocity in the Y direction; At time k, the vehicle coordinate system The target measurement model is as follows: (2) in, It is the target measurement noise at time k; 3.3 Target Motion State Prediction: Based on the target motion state equation described in step 3.2 and the target state estimate at the previous moment. With covariance matrix Calculate the one-step prediction value of the target state respectively. and the covariance matrix of the one-step prediction error ; 3.4 Measurement and Screening: Utilizing Calculate the predicted observations of the target based on the measurement model. Set up a sector tracking gate to track the target measurement set. The target candidate measurement set is obtained through screening. ; 3.5 Data Association: Predicted values ​​of the target trajectory and target candidate measurement set Perform data association; for unassociated measurement targets, they are considered as potential new target trajectories. In the subsequent n tracking processes, if the number of successful associations exceeds the threshold C_new, they are treated as real target trajectories; for unassociated targets, they are considered as target trajectories that may need to be destroyed. In the subsequent n tracking processes, if the number of failed associations exceeds the threshold C_disp, the trajectory is destroyed. 3.6 Target State Filtering: A Kalman filter is used to filter the associated target to obtain the optimal estimate of the target's motion state at time k. and ; Step 3.2 Vehicle Mobility Information The calculation method includes the following steps: 4.1 Calculating Vehicle Pose Information: The vehicle pose detection module reads the vehicle's latitude, longitude, and heading angle information acquired by the GNSS sensor, the vehicle's acceleration and angular velocity information acquired by the INS sensor, and the GNSS differential information from the communication module, and then calculates the coordinate system. Vehicle position information Including X-direction position Y-direction position X-direction velocity Y-direction velocity X-axis acceleration Y-axis acceleration Heading angle Angular velocity of heading angle ; 4.2 Calculate vehicle motion information: Based on the coordinate system in 4.1 Vehicle position information and the vehicle's own position information at the previous moment Calculate vehicle movement information ,in This represents the vehicle's own position relative to the vehicle coordinate system at time k. Position maneuver in the X direction, position maneuver in the Y direction, change in velocity in the X direction, and change in velocity in the Y direction.

Images