Tracking method and system of rail-mounted gantry crane target and vehicle

Abstract

The invention discloses a track-mounted gantry crane target tracking method and system and a vehicle, and the tracking method comprises the steps of target initialization, state prediction, target association and state updating, and provides a filter model with consistent and stable course and speed change aiming at the motion characteristics of a track-mounted gantry crane target, so as to output target information with more accurate and stable position and speed, and improve the tracking efficiency. And the perception precision is improved, so that a perception downstream module is assisted to carry out more accurate path planning.

Description

Technical Field

[0001] The present invention belongs to the technical field of automatic driving, and in particular relates to a method, system and vehicle for tracking a track-mounted target. Background Art

[0002] With the vigorous development of autonomous driving technology, commercial vehicle autonomous driving has gradually been implemented in ports and terminals. Compared with open urban roads, the autonomous driving environment of terminals is relatively simple, but when containers are transferred at ports and terminals, they rely on some special heavy equipment, such as rail cranes and quay cranes. Rail cranes and quay cranes only move on fixed tracks in most scenarios, but when vehicles interact with them, the distance is relatively close, and the passage area of ​​the container area is relatively small. Currently commonly used motion models such as constant speed model, constant acceleration model, constant turn rate and speed model, constant turn rate and acceleration model are mostly aimed at targets with high freedom of movement. Due to sensor noise, it is impossible to model the movement of special equipment such as quay cranes and rail cranes well, and it is easy to give a speed perpendicular to the track direction, which is inconsistent with the actual situation. Abnormal speed estimation brings certain difficulties to the route planning of autonomous driving. Summary of the invention

[0003] In view of the above problems, the main purpose of the present invention is to design a method, system and vehicle for tracking a rail-mounted target. The method constructs a filter model with consistent and stable changes in heading and speed to solve the difficulties caused by abnormal speeds that do not conform to actual conditions to autonomous driving route planning.

[0004] In order to achieve the above purpose, the present invention adopts the following technical solutions:

[0005] A method for tracking a track-mounted target, the method tracking a target moving along a fixed track, comprising the following steps:

[0006] Step 1: Initialize the target state information according to the sensor detection information, and obtain the initial state and covariance matrix of the tracked target;

[0007] Step 2: Predict the state and state covariance of the tracking target;

[0008] Step 3: Assign the sensor detection information to the corresponding tracking target through association calculation;

[0009] Step 4: Calculate the Kalman gain associated with the successfully tracked target;

[0010] Step 5: Update the target state through the target state quantity, state covariance, and Kalman gain;

[0011] Step 6: For detections that fail to be associated, go to step 1, use it to initialize a new tracking target, and execute steps 2-5.

[0012] As a further description of the present invention, in step 1, the state quantity of the target is X={x, y, v, θ}, wherein x is the longitudinal position in the vehicle coordinate system, y is the lateral position in the vehicle coordinate system, v is the speed of the target moving along its own heading direction, and θ is the target yaw angle in the vehicle coordinate system;

[0013] The covariance matrix of the target state is P, expressed as:

[0014]

[0015] Among them, x var is the initial variance of the target longitudinal position, y var is the initial variance of the target lateral position, v var is the initial variance of the target velocity, θ var is the initial variance of the target heading.

[0016] As a further description of the present invention, in step 2, the state and state covariance of the tracking target are predicted;

[0017] The target state transfer equation is:

[0018]

[0019] Where Δt is the time difference between observation and state;

[0020] The update expression of the target state covariance is:

[0021] P=JPJ T +Q,

[0022] Among them, J is the Jacobian matrix, P is the covariance matrix of the target state, and J T is the transpose of matrix J, Q is the process noise matrix;

[0023]

[0024] Where a is the target acceleration noise, a θ is the target yaw rate noise.

[0025] As a further description of the present invention, in step 3, target association is performed: the Euclidean distance value d between the measured and tracked targets is calculated as a similarity feature, and whether the association is successful is determined by setting a distance threshold;

[0026] Measured as a spatial point z(x m ,y m ) is expressed as:

[0027]

[0028] If d is less than the distance threshold, the association is considered successful, and if d is not less than the distance threshold, the association is considered failed.

[0029] As a further description of the present invention, in step 4, the Kalman gain K of the successfully associated target is calculated, and the expression is:

[0030] K=PH T (HPH T +R) -1 ,

[0031] Among them, P is the covariance matrix of the target state, H is the measurement matrix, and H T is the transpose of matrix H, R is the observation noise matrix;

[0032] The measurement matrix H is a matrix that maps the state quantity X to the measurement space. The linear relationship between the state quantity X and the measurement value z is expressed as:

[0033] z=HX+v,

[0034] Where, v is the measurement noise;

[0035] If it is a spatial point measurement, the expressions of the measurement matrix H and the observation noise matrix R are:

[0036]

[0037] in, To detect the variance of the target in the vertical position, is the variance of the detected target in the lateral position.

[0038] As a further description of the present invention, in step 5, the target state is updated, and the update formula is:

[0039]

[0040] Among them, X is the state quantity of the target, P is the covariance matrix of the target state, K is the Kalman gain, z is the measurement value, H is the measurement matrix, and I is the unit matrix.

[0041] A tracking system for a track-mounted target, comprising a target initialization module, a state prediction module, a target association module, and a state update module;

[0042] The target initialization module uses the sensor detection information to initialize the target for the first time, and obtains the initial state and covariance matrix of the tracked target;

[0043] The state prediction module obtains the predicted state and state covariance of the tracking target by calculation according to the historical tracking target information;

[0044] The target association module determines whether to assign the detection information to the tracking target based on the association calculation;

[0045] The state updating module performs state updating according to the prediction value of the state prediction module and the associated detection information to achieve target tracking.

[0046] A vehicle comprises the above-mentioned tracking system and executes the above-mentioned tracking method.

[0047] Compared with the prior art, the technical effects of the present invention are:

[0048] The present invention provides a method, system and vehicle for tracking a track-mounted target. The tracking method includes target initialization, state prediction, target association and state update. According to the motion characteristics of the track-mounted target, a filter model with a heading and a speed that change consistently and stably is proposed to output more accurate and stable target information of position and speed, thereby improving perception accuracy and assisting the perception downstream module to perform more accurate path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 It is a schematic diagram of the tracking method flow of the present invention;

[0050] Figure 2 It is a schematic diagram of the tracking system of the present invention. DETAILED DESCRIPTION

[0051] The present invention is described in detail below in conjunction with the accompanying drawings:

[0052] In one embodiment of the present invention, a method for tracking a track-mounted target is disclosed, referring to Figure 1 As shown, a constant heading motion model is designed to improve the stability of the filter in tracking the target speed and heading. Specifically, the method tracks a target moving along a fixed track and includes the following steps:

[0053] Step 1: Initialize the target state information according to the sensor detection information, and obtain the initial state and covariance matrix of the tracked target;

[0054] Step 2: Predict the state and state covariance of the tracking target;

[0055] Step 3: Assign the sensor detection information to the corresponding tracking target through association calculation;

[0056] Step 4: Calculate the Kalman gain associated with the successfully tracked target;

[0057] Step 5: Update the target state through the target state quantity, state covariance, and Kalman gain;

[0058] Step 6: For detections that fail to be associated, go to step 1, use it to initialize a new tracking target, and execute steps 2-5.

[0059] In this embodiment, in the above step 1, when the target appears for the first time, the state information of the target is initialized according to the sensor detection information. The state quantity of the target is X = {x, y, v, θ}, where x is the longitudinal position in the vehicle coordinate system, y is the lateral position in the vehicle coordinate system, v is the speed of the target moving along its own heading direction, and θ is the target yaw angle in the vehicle coordinate system;

[0060] The covariance matrix of the target state is P, expressed as:

[0061]

[0062] Among them, x var is the initial variance of the target longitudinal position, y var is the initial variance of the target lateral position, v var is the initial variance of the target velocity, θ var is the initial variance of the target heading. The initial variance is set based on experience, usually 10 times the variance after stabilization.

[0063] In this embodiment, in the above step 2, the state and state covariance of the tracked target are predicted;

[0064] The target state transfer equation is:

[0065]

[0066] Where Δt is the time difference between observation and state;

[0067] The update expression of the target state covariance is:

[0068] P=JPJ T +Q,

[0069] Among them, J is the Jacobian matrix, P is the covariance matrix of the target state, and J T is the transpose of matrix J, Q is the process noise matrix;

[0070]

[0071] Where a is the target acceleration noise, a θ is the target yaw rate noise.

[0072] In the present embodiment, in the above step 3, target association is performed: the Euclidean distance value d between the measured and tracked targets is calculated as the similarity feature, the smaller the Euclidean distance value, the more similar the measured and tracked targets are, and whether the association is successful is determined by setting a distance threshold. If the Euclidean distance between the measured and tracked targets is less than the distance threshold, the association is considered successful, otherwise the association is considered failed. Specifically, in the present embodiment, the rail hanging target is a large-scale equipment, and the distance threshold is an empirical value, which can be set to 2 or 3.

[0073] If the measurement is a spatial point z(x m ,y m ) is expressed as:

[0074]

[0075] If d is less than the distance threshold, the association is considered successful, and if d is not less than the distance threshold, the association is considered failed.

[0076] In this embodiment, in the above step 4, the Kalman gain of the successfully associated target is calculated, and the expression is:

[0077] K=PH T (HPH T +R) -1 ,

[0078] Among them, P is the covariance matrix of the target state, H is the measurement matrix, and H T is the transpose of matrix H, R is the observation noise matrix;

[0079] The measurement matrix H is a matrix that maps the state quantity X to the measurement space. The linear relationship between the state quantity X and the measurement value z is expressed as:

[0080] z=HX+v,

[0081] Where v is the measurement noise, which is usually assumed to be zero-mean white Gaussian noise; if it is a spatial point measurement, the expressions of the measurement matrix H and the observation noise matrix R are:

[0082]

[0083] in, To detect the variance of the target in the vertical position, is the variance of the detected target in the lateral position.

[0084] In this embodiment, in the above step 5, the target state is updated, and the expression is:

[0085]

[0086] Among them, X is the state quantity of the target, P is the covariance matrix of the target state, K is the Kalman gain, z is the measurement value, H is the measurement matrix, and I is the unit matrix.

[0087] In another embodiment of the present invention, a tracking system for a track-mounted target is disclosed, such as Figure 2 As shown, it includes a target initialization module, a state prediction module, a target association module, and a state update module;

[0088] The target initialization module uses the sensor detection information to initialize the target for the first time, and obtains the initial state and covariance matrix of the tracked target;

[0089] The state prediction module obtains the predicted state and state covariance of the tracking target by calculation according to the historical tracking target information;

[0090] The target association module determines whether to assign the detection information to the tracking target based on the association calculation;

[0091] The state updating module performs state updating according to the prediction value of the state prediction module and the associated detection information to achieve target tracking.

[0092] In another embodiment of the present invention, a vehicle is disclosed, comprising the above-mentioned tracking system and executing the above-mentioned tracking method.

[0093] Through the above embodiments, the technical solution of the present invention is disclosed. The method of the present invention proposes a filter model with consistent and stable changes in heading and speed for the motion characteristics of the track-mounted target, so as to output more accurate and stable target information of position and speed, thereby assisting the downstream model of the vehicle to perform more accurate path planning, avoiding the occurrence of abnormal speed that does not conform to the actual situation, stably estimating the speed, and improving the perception accuracy. The method of the present invention is suitable for moving targets traveling along the track or moving targets traveling on an approximately fixed straight path.

[0094] The above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Other modifications or equivalent substitutions made to the technical solution of the present invention by ordinary technicians in this field should be included in the scope of the claims of the present invention as long as they do not depart from the spirit and scope of the technical solution of the present invention.

Claims

1. A method for tracking a track-mounted target, characterized in that: The method tracks a target moving along a fixed track, comprising the following steps: Step 1: Initialize the target state information according to the sensor detection information, and obtain the initial state and covariance matrix of the tracked target; Step 2: Predict the state and state covariance of the tracking target; Step 3: Assign the sensor detection information to the corresponding tracking target through association calculation; Step 4: Calculate the Kalman gain associated with the successfully tracked target; Step 5: Update the target state through the target state quantity, state covariance, and Kalman gain; Step 6: For detections that fail to be associated, go to step 1, use it to initialize a new tracking target, and execute steps 2-5.

2. A method for tracking a track-mounted target according to claim 1, characterized in that: In step 1, the state of the target is X={x, y, v, θ}, where x is the longitudinal position in the vehicle coordinate system, y is the lateral position in the vehicle coordinate system, v is the speed of the target moving along its own heading direction, and θ is the target yaw angle in the vehicle coordinate system; the covariance matrix of the target state is P, and the expression is: Among them, x var is the initial variance of the target longitudinal position, y var is the initial variance of the target lateral position, v var is the initial variance of the target velocity, θ var is the initial variance of the target heading.

3. A method for tracking a track-mounted target according to claim 2, characterized in that: In the step 2, the state and state covariance of the tracked target are predicted; The target state transfer equation is: Where Δt is the time difference between observation and state; The update expression of the target state covariance is: P=JPJ T +Q, Among them, J is the Jacobian matrix, P is the covariance matrix of the target state, and J T is the transpose of matrix J, Q is the process noise matrix; Where a is the target acceleration noise, a θ is the target yaw rate noise.

4. A method for tracking a track-mounted target according to claim 1, characterized in that: In step 3, the Euclidean distance value d between the measured and tracked targets is calculated as a similarity feature, and a distance threshold is set to determine whether the association is successful; Measured as a spatial point z(x m ,y m ) is expressed as: If d is less than the distance threshold, the association is considered successful, and if d is not less than the distance threshold, the association is considered failed.

5. A method for tracking a track-mounted target according to claim 3, characterized in that: In step 4, the Kalman gain K is calculated, and the expression is: K=PH T (HPH T +R) -1 , Among them, P is the covariance matrix of the target state, H is the measurement matrix, and H T is the transpose of matrix H, R is the observation noise matrix; The measurement matrix H is a matrix that maps the state quantity X to the measurement space. The linear relationship between the state quantity X and the measurement value z is expressed as: z=HX+v, Where, v is the measurement noise; Then the expressions of spatial point measurement, measurement matrix H and observation noise matrix R are: in, To detect the variance of the target in the vertical position, is the variance of the detected target in the lateral position.

6. A method for tracking a track-mounted target according to claim 5, characterized in that: In step 5, the target state is updated, and the update formula is: Among them, P is the covariance matrix of the target state, K is the Kalman gain, z is the measurement value, H is the measurement matrix, and I is the unit matrix.

7. A tracking system for a track-mounted target according to any one of claims 1 to 6, characterized in that: It includes target initialization module, state prediction module, target association module and state update module; The target initialization module uses the sensor detection information to initialize the target for the first time, and obtains the initial state and covariance matrix of the tracked target; The state prediction module obtains the predicted state and state covariance of the tracking target by calculation according to the historical tracking target information; The target association module determines whether to assign the detection information to the tracking target based on the association calculation; The state updating module performs state updating according to the prediction value of the state prediction module and the associated detection information to achieve target tracking.

8. A vehicle, characterized in that: The tracking system comprises the tracking system described in claim 7 and executes the tracking method described in any one of claims 1 to 6.

Images