Cotton bale target tracking method based on vehicle dynamics prediction and overlap ratio evaluation
By introducing vehicle dynamic prediction and overlap evaluation in cotton bale target tracking, the problem of insufficient tracking accuracy and robustness of traditional methods in complex environments is solved, and higher tracking accuracy and system stability are achieved.
Patent Information
- Application Number
- CN202510103096.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-01-22
- Publication Date
- 2025-05-27
AI Technical Summary
Traditional target tracking methods are difficult to ensure the accuracy and robustness of tracking in environments where light changes, complex backgrounds and irregular movement of the cotton bag, especially when the position of the cotton bag is changing greatly.
The cotton bag target tracking method based on vehicle dynamic prediction and coincidence evaluation was adopted, and the tracking accuracy was evaluated by establishing a clamped vehicle dynamic model and using extended Kalman filtering.
It effectively improves the tracking accuracy of the bale target and the robustness of the system, especially when the bale position changes greatly.
Smart Images
Figure CN120047487A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of target tracking algorithms, and particularly to a cotton bale target tracking method based on vehicle dynamics prediction and coincidence degree evaluation. Background Art
[0002] In a cotton gin factory, the automated detection and tracking of cotton bales is a key link in the production process. However, due to the complex environment and diverse stacking methods of cotton bales, traditional target tracking methods are difficult to ensure the accuracy and robustness of tracking under conditions such as light changes, complex backgrounds, and irregular cotton bale movements. Existing technologies mainly rely on static image recognition technologies and target matching methods. However, in a dynamic environment, as the position of the cotton bale continuously changes, a single image recognition method cannot cope with the challenges of cotton bale target tracking. Therefore, how to optimize the tracking strategy according to the dynamic characteristics of the cotton bale target, combined with real-time coincidence degree evaluation, and improve the accuracy and stability of tracking has become an urgent problem to be solved. Summary of the Invention
[0003] The object of the present invention is to provide a cotton bale target tracking method based on vehicle dynamics prediction and coincidence degree evaluation for the technical defects existing in the prior art.
[0004] The technical solution adopted to achieve the object of the present invention is as follows:
[0005] A cotton bale target tracking method based on vehicle dynamics prediction and coincidence degree evaluation, comprising the following steps:
[0006] Step 1, collect cotton bale image data, use a target detection algorithm to identify and locate the cotton bales in each frame of cotton bale image data, and obtain the cotton bale target detection frames in each frame of cotton bale image data;
[0007] Step 2, obtain the yaw angle ψ through GPS, construct a clamping vehicle dynamics model, determine dynamic parameters based on the dynamics model, and the dynamic parameters include the current frame speed v, sideslip angle β, and sideslip rate ω z , define the current frame vehicle state vector according to the yaw angle ψ and the dynamic parameters and determine the current motion state of the clamping vehicle;
[0008] Step 3, use the cotton bale target detection frames obtained in Step 1 as the input of the cotton bale target tracking method, assign tracking IDs to the cotton bale target detection frames, and based on the extended Kalman filter and according to the current frame vehicle state vector predict the motion trajectory and motion state of the cotton bale at the next moment, and predict the cotton bale target detection frames in the next frame to obtain the cotton bale predicted detection frames at the next moment;
[0009] Step 4: Calculate the overlap degree between the cotton bale target detection box and the cotton bale predicted detection box at the next moment, and evaluate the tracking accuracy based on the overlap degree.
[0010] In the above technical solution, each cotton bale target detection box includes a category, a confidence score, and rectangular box data. The category includes the front side and the side of the cotton bale, the confidence score is a numerical value from 0 to 1, and the rectangular box includes the center coordinates of the rectangular box in the pixel coordinate system of the cotton bale image.
[0011] In the above technical solution, the dynamics model of the clamping vehicle in Step 2 is shown in Expressions (1)-(5), and the relationship between the lateral force of the clamping vehicle and the tire side slip angle is shown in Expression (1):
[0012] F y =C·α (1)
[0013] In the formula, F y represents the lateral force of the clamping vehicle, C represents the tire cornering stiffness, and α represents the tire side slip angle.
[0014] The tire cornering force equation of the clamping vehicle is shown in Expressions (2) and (3):
[0015]
[0016] In the formula, F yf represents the tire lateral force of the front wheels, F yr represents the tire lateral force of the rear wheels, l f represents the distance from the front axle to the center of mass, l r represents the distance from the rear axle to the center of mass, C f represents the front wheel cornering stiffness, C r represents the rear wheel cornering stiffness, δ represents the rear wheel steering angle, β represents the side slip angle of the clamping vehicle, ω z represents the side slip rate of the clamping vehicle, and v represents the speed of the clamping vehicle.
[0017] The lateral dynamics equation of the clamping vehicle is shown in Expression (4):
[0018] m·α y =F yf +F yr (4)
[0019] In the formula, m represents the total mass of the clamping vehicle, α y represents the lateral acceleration of the clamping vehicle, F yf represents the tire lateral force of the front wheels, F yr represents the tire lateral force of the rear wheels.
[0020] The yaw dynamics equation of the clamping vehicle is shown in Expression (5):
[0021]
[0022] In the formula, I z represents the moment of inertia of the forklift around the vertical axis, represents the yaw angular velocity, l f represents the distance from the front axle to the center of mass, l r represents the distance from the rear axle to the center of mass, F yf represents the lateral tire force of the front wheels, F yr represents the lateral tire force of the rear wheels.
[0023] In the above technical solution, step 3 includes the following steps:
[0024] S3.1: Use the cotton bale target detection frame obtained in step 1 as the input of the cotton bale target tracking method, perform target separation according to the confidence score of the cotton bale target detection frame, discard the cotton bale target detection frames with a confidence score less than 0.3 for the cotton bale image data of the current frame, and assign tracking IDs to the cotton bale target detection frames with a confidence score not less than 0.3;
[0025] S3.2: Based on the extended Kalman filter, predict the changes of the cotton bale target detection frame according to the vehicle state vector of the current frame yaw angle change speed change side slip angle change and sideslip rate change According to the position change of the current cotton bale target detection frame yaw angle change speed change side slip angle change and sideslip rate change Define the state transition vector According to the vehicle state vector of the current frame and the state transition vector Obtain the state transition matrix F, and then calculate the covariance matrix P based on the state transition matrix F ; t ;
[0026] S3.3: Define the observation vector According to the observation vector and the state vector Derive the observation function h(x), and take the derivative of the observation function h(x) to obtain the observation matrix H;
[0027] S3.4: Calculate the Kalman gain K according to the covariance matrix P t and the observation matrix H, and calculate the predicted state vector according to the Kalman gain K and the observation matrix H According to the predicted state vector and the observation function h(x) to obtain the predicted observation vector According to the predicted state vector Predict the movement trajectory and movement state of the bale at the next moment. According to the predicted observation vector Predict the predicted detection frame of the bale at the next moment.
[0028] In the above technical solution, the expression of the current frame vehicle state vector is as follows:
[0029]
[0030] In the formula, represents the current frame vehicle state vector, x represents the abscissa of the clamping vehicle in the ground coordinate system, y represents the ordinate of the clamping vehicle in the ground coordinate system, ψ represents the yaw angle of the clamping vehicle, that is, the orientation of the clamping vehicle, v represents the speed of the clamping vehicle, β represents the sideslip angle of the clamping vehicle, ω z represents the sideslip rate of the clamping vehicle.
[0031] In the above technical solution, the expression of the position change of the bale target detection frame is as follows:
[0032]
[0033] In the formula, represents the change in the abscissa of the clamping vehicle in the ground coordinate system, v represents the speed of the clamping vehicle, represents the change in the ordinate of the clamping vehicle in the ground coordinate system, ψ represents the yaw angle of the clamping vehicle, β represents the sideslip angle of the clamping vehicle.
[0034] The expression of the yaw angle change of the clamping vehicle is as follows:
[0035]
[0036] In the formula, represents the change in the yaw angle of the clamping vehicle, ω z represents the sideslip rate of the clamping vehicle.
[0037] The expression of the speed change of the clamping vehicle is as follows:
[0038]
[0039] In the formula, represents the change in the speed of the clamping vehicle, and a is the vehicle acceleration.
[0040] The expression of the sideslip angle change of the clamping vehicle is as follows:
[0041]
[0042] In the formula, represents the change in the sideslip angle of the forklift, v represents the speed of the forklift, C f represents the cornering stiffness of the front wheels, C r represents the cornering stiffness of the rear wheels, δ represents the rear wheel steering angle, l f represents the distance from the front axle to the center of mass, l r represents the distance from the rear axle to the center of mass, ω z represents the sideslip rate of the forklift, m represents the total mass of the forklift.
[0043] The expression for the change in the sideslip rate of the forklift is as follows:
[0044]
[0045] In the formula, represents the change in the sideslip rate of the forklift, I z represents the moment of inertia of the forklift about the vertical axis, represents the yaw angular velocity, l f represents the distance from the front axle to the center of mass, l r represents the distance from the rear axle to the center of mass, C f represents the cornering stiffness of the front wheels, C r represents the cornering stiffness of the rear wheels, β represents the sideslip angle of the forklift, δ represents the rear wheel steering angle, v represents the speed of the forklift;
[0046] The state transition vector The expression is as follows:
[0047]
[0048] In the formula, represents the state transition vector, represents the change in the abscissa of the forklift in the ground coordinate system, represents the change in the ordinate of the forklift in the ground coordinate system, represents the change in the yaw angle of the forklift, that is, the change in the orientation of the forklift, represents the change in the speed of the forklift, represents the change in the sideslip angle of the forklift, represents the change in the sideslip rate of the forklift.
[0049] The calculation formula for the state transition matrix F is as follows:
[0050]
[0051] In the formula, x represents the abscissa of the forklift in the ground coordinate system, v represents the speed of the forklift, y represents the ordinate of the forklift in the ground coordinate system, ψ represents the yaw angle of the forklift, β represents the sideslip angle of the forklift, ω z represents the sideslip rate of the forklift, represents the change in the abscissa of the clamping forklift in the ground coordinate system, represents the change in the ordinate of the clamping forklift in the ground coordinate system, represents the change in the yaw angle of the clamping forklift, represents the change in the speed of the clamping forklift, represents the change in the sideslip angle of the clamping forklift, represents the change in the sideslip rate of the clamping forklift.
[0052] The expression of the covariance matrix is as follows:
[0053] P t = FP t-1 F T (14)
[0054] In the formula, P t represents the covariance matrix at time t, F represents the state transition matrix, and P t-1 represents the covariance matrix at time t - 1.
[0055] In the above technical solution, the expression of the observation vector is as follows:
[0056]
[0057] In the formula, x det represents the abscissa of the center point of the cotton bale target detection frame, y det represents the ordinate of the center point of the cotton bale target detection frame, w det represents the width of the cotton bale target detection frame, and h det represents the height of the cotton bale target detection frame.
[0058] The expression of the observation function is as follows:
[0059] h(x)= [x det , y det , w det , h det T = [x + Δxcos(ψ)-Δysin(ψ), y + Δxsin(ψ)+Δycos(ψ), k w0 ·(1 + αw·v)·(1 + β2), kh0·(1 + αh·v)·(1 + β2)T (16)
[0060] In the formula, k w0 、k h0 represent benchmark constants. The dimensions of the cotton bale target detection frame are measured at low speed and when the sideslip angle is zero. α w 、α h Represents the adjustment coefficient, indicating the influence degree of speed on the size of the cotton bale target detection frame, which is the proportionality coefficient obtained by calibrating the sizes of the cotton bale target frames at different speeds, β 2 Represents the influence of the side slip angle on the size of the cotton bale target detection frame, which is the proportionality coefficient obtained by calibrating the corresponding sizes of the cotton bale target detection frames at different side slip angles. x represents the abscissa of the clamping vehicle in the ground coordinate system, v represents the speed of the clamping vehicle, y represents the ordinate of the clamping vehicle in the ground coordinate system, and ψ represents the yaw angle of the clamping vehicle.
[0061] The expression of the observation matrix is as follows:
[0062]
[0063] In the formula, k w 、k h Represents the reference constant, x represents the abscissa of the clamping vehicle in the ground coordinate system, v represents the speed of the clamping vehicle, y represents the ordinate of the clamping vehicle in the ground coordinate system, ψ represents the yaw angle of the clamping vehicle, and β represents the side slip angle of the clamping vehicle.
[0064] In the above technical solution, the expression of the Kalman gain is as follows:
[0065] K = P t -H T (HP t H T ) -1 (18)
[0066] In the formula, K represents the Kalman gain, P t represents the covariance matrix at time t, H represents the observation matrix, and H T represents the inverted matrix of the observation matrix.
[0067] The expression of the predicted state vector is as follows:
[0068]
[0069] In the formula, represents the predicted state vector, represents the state vector at the current moment, K represents the Kalman gain, represents the observation vector.
[0070] The expression of the predicted observation vector is as follows:
[0071]
[0072] In the formula, x predict represents the abscissa of the center point of the predicted cotton bale detection frame, y predict represents the ordinate of the center point of the predicted cotton bale detection frame, w predictThe width of the predicted detection box of the cotton bale is h predict represents the height of the predicted detection box of the cotton bale.
[0073] In the above technical solution, the calculation formula for the overlap degree between the cotton bale target detection box at the next moment and the cotton bale predicted detection box is as follows:
[0074] IOU = Area of Union(rectA, rectB) / Area of Intersection(rectA, rectB) (21)
[0075] In the formula, rectA represents the cotton bale target detection box at the next moment, and rectB represents the cotton bale predicted detection box at the next moment.
[0076] In the above technical solution, step 4 includes the following steps:
[0077] S4.1: Calculate the overlap degree between the cotton bale target detection box at the next moment and the cotton bale predicted detection box, and calculate the weight value according to the overlap degree;
[0078] S4.2: When the weight value is not less than the threshold, it means that the cotton bale target detection box at the next moment and the cotton bale predicted detection box match successfully, indicating that the accuracy of the target tracking model meets the requirements, and the cotton bale at the next moment uses the tracking ID of the previous moment;
[0079] S4.3: When the weight value is less than the threshold, it means that the cotton bale target detection box at the next moment and the cotton bale predicted detection box do not match successfully, indicating that the accuracy of the target tracking model does not meet the requirements. Reassign a tracking ID for the cotton bale at the next moment and automatically adjust the dynamic model parameters.
[0080] Compared with the prior art, the beneficial effects of the present invention are:
[0081] 1. By establishing a dynamic model of the vehicle and combining the extended Kalman filter method to predict the movement trajectory of the cotton bale target, the present invention can effectively predict the future position of the cotton bale target, improve the tracking accuracy, and perform excellently especially when the position of the cotton bale changes greatly.
[0082] 2. By calculating the overlap degree between the cotton bale target detection box at the next moment and the cotton bale predicted detection box, evaluating the tracking quality, and adjusting the tracking strategy according to the overlap degree, the present invention enables the tracking algorithm to handle situations such as target position changes or occlusions, improving the robustness and stability of the system. BRIEF DESCRIPTION OF THE DRAWINGS
[0083] Figure 1 The working block diagram of the cotton bale target tracking method described in the present invention is shown. DETAILED DESCRIPTION OF THE INVENTION
[0084] The present invention will be further described in detail below in conjunction with specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.
[0085] A cotton bale target tracking method based on vehicle dynamics prediction and coincidence degree evaluation, see Figure 1 , including the following steps:
[0086] Step 1: Collect cotton bale image data, and use an object detection algorithm to identify and locate the cotton bales in each frame of cotton bale image data, so as to obtain the cotton bale target detection frames in each frame of cotton bale image data. Among them, each cotton bale target detection frame includes data such as category (front of cotton bale, side of cotton bale), confidence score (value 0-1), and rectangular frame (the center coordinates of the rectangular frame in the cotton bale image pixel coordinate system are (x, y), where x is the width of the rectangular frame and y is the height of the rectangular frame).
[0087] Among them, the collection of the cotton bale image data: The cotton bales are video-collected through a monitoring camera system installed in the cotton gin, and each frame of the video is used as the cotton bale image data.
[0088] The object detection algorithm in this embodiment is YOLO or SSD.
[0089] Step 2: Obtain the yaw angle ψ through GPS, and construct a dynamics model of the clamping vehicle (the vehicle that grabs the cotton bale). Based on the dynamics model, determine the dynamics parameters, and the dynamics parameters include the current frame (of the clamping vehicle) speed v, (of the clamping vehicle) sideslip angle β, and (of the clamping vehicle) sideslip rate ω z etc., and define the current frame vehicle state vector according to the yaw angle ψ and the dynamics parameters and determine the current motion state of the clamping vehicle.
[0090] The dynamics model of the clamping vehicle is shown in expressions (1)-(5), and the relationship between the lateral force of the clamping vehicle and the tire sideslip angle is shown in expression (1):
[0091] F y = C·α (1)
[0092] In the formula, F y represents the lateral force of the clamping vehicle, C represents the tire cornering stiffness, and α represents the tire sideslip angle (i.e., the angle between the actual driving direction of the tire and the tire center line).
[0093] The tire cornering force equation of the clamping vehicle is shown in expressions (2) and (3):
[0094]
[0095] In the formula, Fyf represents the lateral force of the front tire, F yr represents the lateral force of the rear tire, l f represents the distance from the front axle to the center of mass, l r represents the distance from the rear axle to the center of mass, C f represents the cornering stiffness of the front tire, C r represents the cornering stiffness of the rear tire, δ represents the rear wheel steering angle, β represents the side slip angle of the forklift, ω z represents the sideslip rate of the forklift, v represents the speed of the forklift.
[0096] The lateral dynamics equation of the forklift is shown in Expression (4):
[0097] m·α y = F yf + F yr (4)
[0098] In the formula, m represents the total mass of the forklift, α y represents the lateral acceleration of the forklift, F yf represents the lateral force of the front tire, F yr represents the lateral force of the rear tire.
[0099] The yaw dynamics equation of the forklift is shown in Expression (5):
[0100]
[0101] In the formula, I z represents the moment of inertia of the forklift about the vertical axis (Z-axis), represents the yaw angular velocity, l f represents the distance from the front axle to the center of mass, l r represents the distance from the rear axle to the center of mass, F yf represents the lateral force of the front tire, F yr represents the lateral force of the rear tire.
[0102] The expression of the current frame vehicle state vector is as follows:
[0103]
[0104] In the formula, represents the current frame vehicle state vector, x represents the abscissa of the forklift in the ground coordinate system, y represents the ordinate of the forklift in the ground coordinate system, ψ represents the yaw angle of the forklift, that is, the orientation of the forklift, v represents the speed of the forklift, β represents the side slip angle of the forklift, ω z represents the sideslip rate of the forklift.
[0105] Step 3: Use the cotton bale target detection box obtained in Step 1 as the input of the cotton bale target tracking method, assign a tracking ID to the cotton bale target detection box, and based on the Extended Kalman Filter (EKF) and according to the vehicle state vector of the current frame predict the motion trajectory and motion state of the cotton bale at the next moment, and predict the cotton bale target detection box in the next frame to obtain the cotton bale predicted detection box at the next moment.
[0106] The said Step 3 includes the following steps:
[0107] S3.1: Use the cotton bale target detection box obtained in Step 1 as the input of the cotton bale target tracking method, separate the targets according to the confidence scores of the cotton bale target detection boxes, discard the cotton bale target detection boxes with confidence scores less than 0.3 for the cotton bale image data of the first frame, and assign a tracking ID to the cotton bale target detection boxes with confidence scores not less than 0.3.
[0108] S3.2: Based on the said Extended Kalman Filter and according to the vehicle state vector of the current frame predict the change of the cotton bale target detection box (yaw angle change of the clamping vehicle) (speed change of the clamping vehicle) (side slip angle change of the clamping vehicle) and (side slip rate change of the clamping vehicle) According to the position change of the current cotton bale target detection box (yaw angle change of the clamping vehicle) (speed change of the clamping vehicle) (side slip angle change of the clamping vehicle) and (side slip rate change of the clamping vehicle) define the state transition vector According to the vehicle state vector of the current frame and the state transition vector obtain the state transition matrix F, and then calculate the covariance matrix P based on the state transition matrix F t .
[0109] The expression of the position change of the current cotton bale target detection box is as follows:
[0110]
[0111] In the formula, x represents the abscissa change of the clamping vehicle in the ground coordinate system, v represents the speed of the clamping vehicle, y represents the ordinate change of the clamping vehicle in the ground coordinate system, ψ represents the yaw angle of the clamping vehicle, and β represents the side slip angle of the clamping vehicle.
[0112] The expression of the yaw angle change of the clamping vehicle is as follows:
[0113]
[0114] In the formula, ψ represents the change in the yaw angle of the forklift, and ω z represents the sideslip rate of the forklift.
[0115] The expression for the speed change of the forklift is as follows:
[0116]
[0117] In the formula, v represents the speed change of the forklift, and a is the vehicle acceleration.
[0118] The expression for the change in the sideslip angle of the forklift is as follows:
[0119]
[0120] In the formula, β represents the change in the sideslip angle of the forklift, v represents the speed of the forklift, C f represents the cornering stiffness of the front wheels, C r represents the cornering stiffness of the rear wheels, δ represents the rear wheel steering angle, l f represents the distance from the front axle to the center of mass, l r represents the distance from the rear axle to the center of mass, ω z represents the sideslip rate of the forklift, and m represents the total mass of the forklift.
[0121] The expression for the change in the sideslip rate of the forklift is as follows:
[0122]
[0123] In the formula, ω z represents the change in the sideslip rate of the forklift, I z represents the moment of inertia of the forklift about the vertical axis (Z-axis), represents the yaw angular velocity, l f represents the distance from the front axle to the center of mass, l r represents the distance from the rear axle to the center of mass, C f represents the cornering stiffness of the front wheels, C r represents the cornering stiffness of the rear wheels, β represents the sideslip angle of the forklift, δ represents the rear wheel steering angle, and v represents the speed of the forklift.
[0124] The state transition vector is expressed as follows:
[0125]
[0126] In the formula, represents the state transition vector, represents the change in the abscissa of the forklift in the ground coordinate system, Represents the change in the vertical coordinate of the clamping vehicle in the ground coordinate system, Represents the change in the yaw angle of the clamping vehicle, that is, the change in the orientation of the clamping vehicle, Represents the change in the speed of the clamping vehicle, Represents the change in the sideslip angle of the clamping vehicle, Represents the change in the sideslip rate of the clamping vehicle.
[0127] The calculation formula of the state transition matrix is as follows:
[0128]
[0129] In the formula, x represents the abscissa of the clamping vehicle in the ground coordinate system, v represents the speed of the clamping vehicle, y represents the vertical coordinate of the clamping vehicle in the ground coordinate system, ψ represents the yaw angle of the clamping vehicle, β represents the sideslip angle of the clamping vehicle, ω z Represents the sideslip rate of the clamping vehicle, Represents the change in the abscissa of the clamping vehicle in the ground coordinate system, Represents the change in the vertical coordinate of the clamping vehicle in the ground coordinate system, Represents the change in the yaw angle of the clamping vehicle, Represents the change in the speed of the clamping vehicle, Represents the change in the sideslip angle of the clamping vehicle, Represents the change in the sideslip rate of the clamping vehicle.
[0130] The expression of the covariance matrix is as follows:
[0131] P t = FP t-1 F T (14)
[0132] In the formula, P t Represents the covariance matrix at time t (the current time), F represents the state transition matrix, P t-1 Represents the covariance matrix at time t - 1 (the previous time).
[0133] S3.3: Define the observation vector According to the observation vector and the state vector Derive the observation function h(x), and take the derivative of the observation function h(x) to obtain the observation matrix H.
[0134] The expression of the observation vector is as follows:
[0135]
[0136] In the formula, x det Represents the abscissa of the center point of the cotton bale target detection frame, y detRepresents the ordinate of the center point of the cotton bale target detection frame, w det Represents the width of the cotton bale target detection frame, h det Represents the height of the cotton bale target detection frame.
[0137] The expression of the observation function is as follows:
[0138] h(x) = [x det , y det , w det , h det T = [x + Δxcos(ψ) - Δysin(ψ), y + Δxsin(ψ) + Δycos(ψ), k w0 ·(1 + αw·v)·(1 + β2), kh0·(1 + αh·v)·(1 + β2)T (16)
[0139] In the formula, k w0 , k h0 Represent reference constants. When measuring the size of the cotton bale target detection frame at low speed and with a zero sideslip angle, α w , α h Represent adjustment coefficients, indicating the influence degree of speed on the size of the cotton bale target detection frame, which is the proportional coefficient obtained by calibrating the size of the cotton bale target frame at different speeds. β 2 Represents the influence of the sideslip angle on the size of the cotton bale target detection frame, which is the proportional coefficient obtained by calibrating the corresponding size of the cotton bale target detection frame at different sideslip angles. x represents the abscissa of the forklift in the ground coordinate system, v represents the speed of the forklift, y represents the ordinate of the forklift in the ground coordinate system, and ψ represents the yaw angle of the forklift.
[0140] The expression of the observation matrix is as follows:
[0141]
[0142] In the formula, k w , k h Represent reference constants. x represents the abscissa of the forklift in the ground coordinate system, v represents the speed of the forklift, y represents the ordinate of the forklift in the ground coordinate system, ψ represents the yaw angle of the forklift, and β represents the sideslip angle of the forklift.
[0143] S3.4: Calculate the Kalman gain K according to the covariance matrix P t and the observation matrix H. Calculate the predicted state vector according to the Kalman gain K and the observation matrix H According to the predicted state vector and the observation function h(x) to obtain the predicted observation vector According to the predicted state vector Predict the movement trajectory and movement state of the cotton bale at the next moment according to the predicted observation vector Predict the predicted detection frame of the cotton bale at the next moment
[0144] The expression of the Kalman gain is as follows
[0145] K = P t - H T (HP t H T ) -1 (18)
[0146] In the formula, K represents the Kalman gain, and P t represents the covariance matrix at time t (the current moment), H represents the observation matrix, and H T represents the inverted matrix of the observation matrix
[0147] The expression of the predicted state vector is as follows
[0148]
[0149] In the formula represents the predicted state vector represents the state vector at time t (the current moment), K represents the Kalman gain represents the observation vector
[0150] The expression of the predicted observation vector is as follows
[0151]
[0152] In the formula, x predict represents the abscissa of the center point of the predicted detection frame of the cotton bale, and y predict represents the ordinate of the center point of the predicted detection frame of the cotton bale, w predict represents the width of the predicted detection frame of the cotton bale, and h predict represents the height of the predicted detection frame of the cotton bale
[0153] Step 4, calculate the overlap degree between the target detection frame of the cotton bale and the predicted detection frame of the cotton bale at the next moment, and evaluate the accuracy of tracking according to the overlap degree
[0154] The calculation formula for the overlap degree between the target detection frame of the cotton bale and the predicted detection frame of the cotton bale at the next moment is as follows
[0155] IOU = Area of Union(rectA,rectB) / Area of Intersection(rectA,rectB)(21)
[0156] Where rectA represents the cotton bale target detection frame at the next moment, and rectB represents the cotton bale predicted detection frame at the next moment.
[0157] Step 4 includes the following steps:
[0158] S4.1: Calculate the coincidence degree between the cotton bale target detection frame and the cotton bale predicted detection frame at the next moment (the ratio of the overlapping area between the cotton bale target detection frame and the cotton bale predicted detection frame in the image pixel coordinate system to their combined area), and calculate the weight value according to the coincidence degree.
[0159] S4.2: When the weight value is not less than (greater than or equal to) the threshold (0.5), it means that the cotton bale target detection frame and the cotton bale predicted detection frame at the next moment are successfully matched, indicating that the accuracy of the target tracking model meets the requirements, and the tracking ID of the cotton bale at the next moment is the same as that of the previous moment.
[0160] S4.3: When the weight value is less than the threshold (0.5), it means that the cotton bale target detection frame and the cotton bale predicted detection frame at the next moment are mismatched, indicating that the accuracy of the target tracking model does not meet the requirements. Reassign a tracking ID for the cotton bale at the next moment and automatically adjust the dynamic model parameters.
[0161] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention.
Claims
1. A cotton bale target tracking method based on vehicle dynamics prediction and coincidence evaluation, characterized in that: The following steps are involved: Step 1, collecting cotton bale image data, using a target detection algorithm to identify and locate the cotton bales in each frame of cotton bale image data, and obtaining a cotton bale target detection frame in each frame of cotton bale image data; Step 2, obtain the yaw angle ψ through GPS, and build a dynamic model of the clamping vehicle, and determine the dynamic parameters based on the dynamic model, the dynamic parameters include the current frame speed v, the sideslip angle β and the yaw rate ω z , the vehicle state vector of the current frame is defined according to the yaw angle ψ and the dynamic parameters And determine the current motion state of the clamping vehicle; Step 3: Use the cotton bale target detection frame obtained in step 1 as the input of the cotton bale target tracking method, assign a tracking ID to the cotton bale target detection frame, and use the extended Kalman filter and the current frame vehicle state vector Predict the motion trajectory and motion state of the cotton bale at the next moment, and predict the cotton bale target detection frame of the next frame to obtain the cotton bale prediction detection frame at the next moment; Step 4: Calculate the overlap between the cotton bale target detection frame and the cotton bale prediction detection frame at the next moment, and evaluate the tracking accuracy based on the overlap.
2. The cotton bale target tracking method according to claim 1, characterized in that: Each cotton bale target detection frame includes a category, a confidence score and rectangular frame data, the category includes the front of the cotton bale and the side of the cotton bale, the confidence score is a value of 0-1, and the rectangular frame includes the center coordinates of the rectangular frame in the cotton bale image pixel coordinate system.
3. The cotton bale target tracking method according to claim 1, characterized in that: The dynamic model of the clamping vehicle in step 2 is shown in expressions (1)-(5), and the relationship between the lateral force of the clamping vehicle and the tire slip angle is shown in expression (1): F y =C·α (1) In the formula, F y represents the lateral force of the clamped vehicle, C represents the cornering stiffness of the tire, and α represents the tire slip angle. The tire cornering force equation of the clamping vehicle is shown in expressions (2) and (3): In the formula, F yf Represents the lateral force of the tire on the front wheel, F yr represents the lateral force of the rear tire, l f Represents the distance from the front axle to the center of mass, l r Represents the distance from the rear axle to the center of mass, C f Represents the front wheel cornering stiffness, C r represents the rear wheel cornering stiffness, δ represents the rear wheel steering angle, β represents the side slip angle of the clamped vehicle, ω z represents the yaw rate of the clamped vehicle, and v represents the speed of the clamped vehicle. The lateral dynamic equation of the clamping vehicle is shown in expression (4): m·a y =F yf +F yr (4) In the formula, m represents the total mass of the clamping vehicle, α y Represents the lateral acceleration of the clamping vehicle, F yf Represents the lateral force of the tire on the front wheel, F yr Represents the lateral force on the tire of the rear wheel. The yaw dynamics equation of the clamping vehicle is shown in expression (5): In the formula, I z represents the moment of inertia of the gripper around the vertical axis, represents the yaw angular velocity, l f Represents the distance from the front axle to the center of mass, l r Represents the distance from the rear axle to the center of mass, F yf Represents the lateral force of the tire on the front wheel, F yr Represents the lateral force on the tire of the rear wheel.
4. The cotton bale target tracking method according to claim 1, characterized in that: The step 3 comprises the following steps: S3.1: using the cotton bale target detection frame obtained in step 1 as the input of the cotton bale target tracking method, performing target separation according to the confidence score of the cotton bale target detection frame, abandoning the cotton bale target detection frame with a confidence score less than 0.3 for the cotton bale image data of the current frame, and assigning a tracking ID to the cotton bale target detection frame with a confidence score not less than 0.3; S3.2: Based on the extended Kalman filter according to the current frame vehicle state vector Predicting changes in cotton bale object detection boxes Yaw angle change Speed Change Side slip angle change and yaw rate changes , according to the position change of the current cotton bale target detection frame Yaw angle change Speed Change Side slip angle change and yaw rate changes Define the state transition vector According to the current frame vehicle state vector and the state transition vector Get the state transfer matrix F, and then calculate the covariance matrix P based on the state transfer matrix F t ; S3.3: Define the observation vector According to the observation vector and the state vector Derive the observation function h(x), and obtain the observation matrix H by taking the derivative of the observation function h(x); S3.4: According to the covariance matrix P t The Kalman gain K is calculated from the observation matrix H, and the predicted state vector is calculated based on the Kalman gain K and the observation matrix H. According to the predicted state vector And the observation function h(x) to get the predicted observation vector According to the predicted state vector Predict the trajectory and state of the cotton bale at the next moment, based on the predicted observation vector The predicted detection frame of the cotton bale at the next moment is obtained.
5. The cotton bale target tracking method according to claim 1, characterized in that: The vehicle state vector expression of the current frame is as follows: In the formula, represents the vehicle state vector of the current frame, x represents the horizontal coordinate of the clamping vehicle in the ground coordinate system, y represents the vertical coordinate of the clamping vehicle in the ground coordinate system, ψ represents the yaw angle of the clamping vehicle, that is, the direction of the clamping vehicle, v represents the speed of the clamping vehicle, β represents the sideslip angle of the clamping vehicle, ω z Represents the yaw rate of the clamped vehicle.
6. The cotton bale target tracking method according to claim 1, characterized in that: The position change expression of the cotton bale target detection frame is as follows: In the formula, represents the change of the horizontal coordinate of the clamping vehicle in the ground coordinate system, v represents the speed of the clamping vehicle, represents the change of the ordinate of the clamp vehicle in the ground coordinate system, ψ represents the yaw angle of the clamp vehicle, and β represents the sideslip angle of the clamp vehicle. The expression for the change of the yaw angle of the clamping vehicle is as follows: In the formula, Represents the change in the yaw angle of the clamping vehicle, ω z Represents the yaw rate of the clamped vehicle. The speed variation expression of the clamping vehicle is as follows: In the formula, represents the speed change of the clamping vehicle, and a is the vehicle acceleration. The side slip angle variation expression of the clamp vehicle is as follows: In the formula, represents the side slip angle change of the clamped vehicle, v represents the speed of the clamped vehicle, C f Represents the front wheel cornering stiffness, C r represents the rear wheel cornering stiffness, δ represents the rear wheel steering angle, l f Represents the distance from the front axle to the center of mass, l r Represents the distance from the rear axle to the center of mass, ω z represents the yaw rate of the clamped vehicle, and m represents the total mass of the clamped vehicle. The expression for the change of the yaw rate of the clamp vehicle is as follows: In the formula, Represents the change in the side slip rate of the clamped vehicle, I z represents the moment of inertia of the gripper around the vertical axis, represents the yaw angular velocity, l f Represents the distance from the front axle to the center of mass, l r Represents the distance from the rear axle to the center of mass, C f Represents the front wheel cornering stiffness, C r represents the rear wheel cornering stiffness, β represents the sideslip angle of the clamped vehicle, δ represents the rear wheel steering angle, and v represents the speed of the clamped vehicle; The state transition vector The expression is as follows: In the formula, represents the state transition vector, Represents the change of the horizontal coordinate of the clamping vehicle in the ground coordinate system, Represents the change of the vertical coordinate of the clamping vehicle in the ground coordinate system, Represents the change in the yaw angle of the clamping vehicle, that is, the change in the direction of the clamping vehicle. Represents the speed change of the clamping vehicle, Represents the change in the sideslip angle of the clamped vehicle, Represents the change in the yaw rate of the clamped vehicle. The calculation formula of the state transfer matrix F is as follows: In the formula, x represents the horizontal coordinate of the clamp car in the ground coordinate system, v represents the speed of the clamp car, y represents the vertical coordinate of the clamp car in the ground coordinate system, ψ represents the yaw angle of the clamp car, β represents the sideslip angle of the clamp car, ω z represents the yaw rate of the clamped vehicle, Represents the change of the horizontal coordinate of the clamping vehicle in the ground coordinate system, Represents the change of the vertical coordinate of the clamping vehicle in the ground coordinate system, Represents the change in the yaw angle of the clamping vehicle, Represents the speed change of the clamping vehicle, Represents the change in the sideslip angle of the clamped vehicle, Represents the change in the yaw rate of the clamped vehicle. The expression of the covariance matrix is as follows: P t =FP t-1 F T (14) Where P t represents the covariance matrix at time t, F represents the state transfer matrix, P t-1 Represents the covariance matrix at time t-1.
7. The cotton bale target tracking method according to claim 1, characterized in that: The expression of the observation vector is as follows: In the formula, x det Represents the horizontal coordinate of the center point of the cotton bale target detection box, y det represents the ordinate of the center point of the cotton bale target detection frame, w det Represents the width of the cotton bale target detection box, h det Represents the height of the cotton bale object detection box. The expression of the observation function is as follows: h(x)=[x det ,y det ,w det ,h det ] T =[x+Δxcos(ψ)-Δysin(ψ),y+Δxsin(ψ)+Δycos(ψ),k w0 (1+aw·v)·(1+β2),kh0·(1+αh·v)·(1+β2)T (16) In the formula, k w0 , k h0 represents the reference constant, measuring the size of the cotton bale target detection frame at low speed and zero sideslip angle, α w , α h Represents the adjustment coefficient, which indicates the influence of speed on the size of the cotton bale target detection frame. It is the proportional coefficient obtained by calibrating the size of the cotton bale target frame at different speeds. 2 Figure 3 shows the influence of the sideslip angle on the size of the cotton bale target detection frame. The proportional coefficient is obtained by calibrating the corresponding cotton bale target detection frame size under different sideslip angles. x represents the horizontal coordinate of the clamping vehicle in the ground coordinate system, v represents the speed of the clamping vehicle, y represents the vertical coordinate of the clamping vehicle in the ground coordinate system, and ψ represents the yaw angle of the clamping vehicle. The expression of the observation matrix is as follows: In the formula, k w , k h represents the reference constant, x represents the horizontal coordinate of the clamp vehicle in the ground coordinate system, v represents the speed of the clamp vehicle, y represents the vertical coordinate of the clamp vehicle in the ground coordinate system, ψ represents the yaw angle of the clamp vehicle, and β represents the sideslip angle of the clamp vehicle.
8. The cotton bale target tracking method according to claim 1, characterized in that: The Kalman gain expression is as follows: K=P t -H T (HP t H T ) -1 (18) Where K represents the Kalman gain, P t represents the covariance matrix at time t, H represents the observation matrix, and H T Represents the inverse of the observation matrix. The predicted state vector expression is as follows: In the formula, represents the predicted state vector, represents the state vector at the current moment, K represents the Kalman gain, represents the observation vector. The predicted observation vector expression is as follows: In the formula, x predict Represents the horizontal coordinate of the center point of the cotton bale prediction detection box, y predict Represents the ordinate of the center point of the cotton bale prediction detection box, w predict Table 2. Width of cotton bale prediction detection box, h predict Represents the height of the cotton bale prediction detection box.
9. The cotton bale target tracking method according to claim 1, characterized in that: The calculation formula for the overlap between the cotton bale target detection frame and the cotton bale prediction detection frame at the next moment is as follows: IOU=Area of Union(rectA,rectB) / Area of Intersection(rectA,rectB) (21) In the formula, rectA represents the cotton bale target detection frame at the next moment, and rectB represents the cotton bale prediction detection frame at the next moment.
10. The cotton bale target tracking method according to claim 1, characterized in that: The step 4 comprises the following steps: S4.1: Calculate the overlap between the cotton bale target detection frame and the cotton bale prediction detection frame at the next moment, and calculate the weight value according to the overlap; S4.2: When the weight value is not less than the threshold, it means that the cotton bale target detection frame at the next moment matches the cotton bale prediction detection frame successfully, which means that the accuracy of the target tracking model meets the requirements, and the cotton bale at the next moment uses the tracking ID of the previous moment; S4.3: When the weight value is less than the threshold, it means that the cotton bale target detection frame at the next moment fails to match the cotton bale prediction detection frame, which means that the accuracy of the target tracking model does not meet the requirements. The tracking ID is reallocated for the cotton bale at the next moment, and the dynamic model parameters are automatically adjusted.