CN-121999046-A - Real-time trailer hinge angle estimation method and system based on feature point tracking

CN121999046ACN 121999046 ACN121999046 ACN 121999046ACN-121999046-A

Abstract

The application provides a real-time trailer hinging angle estimation method and system based on feature point tracking, wherein the method comprises the steps of acquiring a first image frame of a target trailer at the current moment and a second image frame of the target trailer at the previous moment; the method comprises the steps of determining a first characteristic point list of a first image frame according to a second characteristic point list in a second image frame, calculating according to the first characteristic point list and a preset characteristic point template to obtain a visual estimation angle and a confidence coefficient, updating a first Kalman filter at the current moment according to the visual estimation angle and the confidence coefficient to obtain an updated second Kalman filter, outputting an estimated value of the hinge angle at the current moment based on the second Kalman filter, and outputting the estimated value of the hinge angle at each moment in a preset time period according to the motion data of a target trailer in the preset time period, wherein the accuracy and the instantaneity of real-time estimation of the hinge angle of the trailer are improved.

Inventors

FU ZHOU
SONG KE
YU JI
LIU GUOQING
YANG GUANG
WANG QICHENG
HUANG LIANG

Assignees

深圳佑驾创新科技股份有限公司

Dates

Publication Date: 20260508
Application Date: 20260126

Claims (10)

1. The real-time trailer hinge angle estimation method based on feature point tracking is characterized by comprising the following steps of: acquiring a first image frame of a target trailer at the current moment and a second image frame of the target trailer at the previous moment; Determining a first characteristic point list of the first image frame according to a second characteristic point list in the second image frame; Calculating to obtain a visual estimation angle and a confidence coefficient according to the first characteristic point list and a preset characteristic point template; Updating a first Kalman filter at the current moment according to the visual estimation angle and the confidence coefficient, obtaining an updated second Kalman filter, and outputting an estimated value of the hinge angle at the current moment based on the second Kalman filter; And in a preset time period after the current time, carrying out real-time hinge angle estimation for a plurality of times through the second Kalman filter according to the motion data of the target trailer, and further outputting hinge angle estimated values of all times in the preset time period.
2. The method for real-time estimation of trailer articulation angle based on feature point tracking as set forth in claim 1, wherein said determining a first list of feature points for the first image frame based on a second list of feature points in the second image frame comprises: if the current moment is the optical flow tracking moment, predicting a plurality of first characteristic points in the first image frame through a preset optical flow algorithm according to a second characteristic point list in the second image frame, and further generating a first characteristic point list of the first image frame; If the current moment is the matching moment, carrying out feature point re-matching on the first image frame according to the feature point template, updating the second feature point list according to a re-matching result, and obtaining a first feature point list of the first image frame, wherein the feature point template is obtained by carrying out feature point calibration construction on a historical image frame sequence of the target trailer in a linear alignment state; wherein the time interval between two adjacent re-matching instants is larger than the time interval between two adjacent optical flow tracking instants.
3. The method for estimating the trailer hinging angle in real time based on feature point tracking according to claim 2, wherein if the current moment is an optical flow tracking moment, predicting a plurality of first feature points in the first image frame through a preset optical flow algorithm according to a second feature point list in the second image frame, and further generating a first feature point list of the first image frame, comprising: According to the second image frame and a preset target detection area, predicting corresponding positions of all second characteristic points in the second characteristic point list in the first image frame through the optical flow algorithm, and further determining all corresponding initial first characteristic points in the first image frame, wherein the initial first characteristic points comprise pixel positions, IDs and descriptors of all the initial first characteristic points; Screening the feature points of the initial first feature points according to a preset detection rule and the pixel positions of the second feature points to obtain the first feature points; And constructing and obtaining the first characteristic point list according to the ID, the pixel position and the descriptor of each first characteristic point.
4. The method for estimating the trailer hinging angle in real time based on feature point tracking according to claim 2, wherein if the current moment is a re-matching moment, performing feature point re-matching on the first image frame according to a preset feature point template, and updating the second feature point list according to a re-matching result, to obtain the first feature point list of the first image frame, comprising: Inputting the first image frame and a preset target detection area into a preset feature point detection network, so that the feature point detection network determines a plurality of candidate first feature points in the first image frame, wherein the candidate first feature points comprise pixel positions and descriptors of the candidate first feature points; Respectively determining matching feature points corresponding to third feature points in the feature point template according to the candidate first feature points, wherein for any third feature point, corresponding matching feature points are determined from the candidate first feature points in a nearest neighbor searching mode according to descriptors of the third feature points, and IDs of the matching feature points are determined according to corresponding relations; performing geometric verification on each matching characteristic point, and screening to obtain a plurality of re-matching characteristic points; and updating the second characteristic point list according to each re-matching characteristic point to obtain the first characteristic point list.
5. The method for estimating a trailer hinging angle in real time based on feature point tracking according to claim 4, wherein for any one of the third feature points, determining a corresponding matching feature point from each candidate first feature point in a nearest neighbor searching manner according to a descriptor of the third feature point, and determining an ID of the matching feature point according to a corresponding relation comprises: calculating feature distances between the descriptors of the third feature points and the descriptors of the candidate first feature points respectively; determining nearest neighbor points and next neighbor points of the third feature point from the candidate first feature points according to the feature distance; According to the feature distance corresponding to the nearest neighbor point and the secondary neighbor point, carrying out absolute distance threshold value test and ratio test on the nearest neighbor point and the secondary neighbor point; And if the absolute distance threshold value test and the ratio test are passed, taking the nearest neighbor point as a matching characteristic point corresponding to the third characteristic point, and taking the ID of the third characteristic point as the ID of the matching characteristic point.
6. The method for estimating a trailer hinging angle in real time based on feature point tracking as claimed in any one of claims 1-5, wherein the feature point template is obtained by performing feature point calibration construction on a historical image frame sequence of the target trailer in a straight line alignment state, and the method comprises the following steps: Acquiring a historical image frame sequence of the target trailer in a linear alignment state; Sequentially inputting each historical image frame in the historical image frame sequence to a preset characteristic point detection network, so that the characteristic point detection network sequentially extracts a historical characteristic point list of each historical image frame, wherein the historical characteristic point list comprises historical pixel positions, historical IDs and historical descriptors of each historical characteristic point; According to the historical image frame sequence, carrying out continuous optical flow tracking on each historical characteristic point through the optical flow algorithm, and counting to obtain the number of frames of each historical characteristic point which are continuously and successfully tracked; According to the number of frames of each history feature point which is continuously and successfully tracked and a preset frame number threshold, a plurality of stable feature points are obtained through screening from each history feature point; Performing geometric verification and motion consistency verification on each stable characteristic point, and screening to obtain a plurality of third characteristic points; And constructing and obtaining the characteristic point templates according to the historical pixel positions, the historical IDs and the historical descriptors of the third characteristic points.
7. The method for real-time estimating a trailer hinging angle based on feature point tracking according to any one of claims 1-5, wherein said calculating to obtain a visual estimation angle and a confidence level based on said first feature point list and said feature point template comprises: Respectively calculating first relative angles between each first feature point in the first feature point list and a first reference point, wherein the first reference point is a hinge point in the first image frame; Determining each corresponding fourth feature point from the feature point template according to the ID of each first feature point, and respectively calculating a second relative angle between each fourth feature point and a second reference point, wherein the second reference point is a hinge point in any historical image frame in the historical image frame sequence; Respectively calculating the angle difference between each first relative angle and the corresponding second relative angle to obtain the individual rotation angle of each first characteristic point; And according to the individual rotation angles, counting corresponding angle median and angle standard deviation, taking the angle median as the vision estimation angle, and taking the angle standard deviation as the confidence coefficient.
8. The method for real-time estimating a trailer hinging angle based on feature point tracking according to any one of claims 1-5, wherein updating the first kalman filter at the current moment according to the visual estimation angle and the confidence level, obtaining an updated second kalman filter, and outputting a hinging angle estimated value at the current moment based on the second kalman filter, comprises: acquiring a priori state estimation vector and a corresponding priori covariance matrix of the first Kalman filter at the current moment; determining observation noise at the current moment according to the confidence coefficient; according to the observation noise, the prior covariance matrix and a preset observation matrix, calculating to obtain a first Kalman gain of the first Kalman filter at the current moment; Calculating to obtain a state estimation vector of the target trailer at the current moment according to the first Kalman gain, the prior state estimation vector, the observation matrix and the visual estimation angle; calculating to obtain a covariance matrix of the state estimation vector according to the first Kalman gain, the observation matrix and the prior covariance matrix; updating the first Kalman filter according to the state estimation vector and the covariance matrix to obtain the second Kalman filter; and outputting the estimated value of the hinge angle at the current moment according to the state estimation vector in the second Kalman filter.
9. The method for real-time estimation of a trailer hinging angle based on feature point tracking according to claim 8, wherein the obtaining the prior state estimation vector and the corresponding prior covariance matrix of the first kalman filter at the current moment comprises: Acquiring real-time motion data of the target trailer at the current moment and a previous state estimation vector of the previous moment; According to the real-time motion data and the front state estimation vector, calculating and obtaining a priori state estimation vector of the target trailer at the current moment through a state transfer equation in the first Kalman filter; And calculating to obtain a priori covariance matrix of the prior state estimation vector according to the control noise, the process noise, the real-time motion data and the partial derivative value of the state transfer equation in the first Kalman filter.
10. The real-time trailer hinge angle estimation system based on the feature point tracking is characterized by comprising an acquisition module, a feature point determination module, a first estimation module, an updating module and a second estimation module; The acquisition module is used for acquiring a first image frame of the target trailer at the current moment and a second image frame of the target trailer at the previous moment; the characteristic point determining module is used for determining a first characteristic point list of the first image frame according to a second characteristic point list in the second image frame; The first estimation module is used for calculating and obtaining a visual estimation angle and a confidence coefficient according to the first characteristic point list and a preset characteristic point template; the updating module is used for updating the first Kalman filter at the current moment according to the visual estimation angle and the confidence coefficient, obtaining an updated second Kalman filter and outputting an estimated value of the hinge angle at the current moment based on the second Kalman filter; The second estimation module is configured to perform real-time estimation on the hinge angle for several times through the second kalman filter according to the motion data of the target trailer in a preset time period after the current time, and further output hinge angle estimation values at all times in the preset time period.

Description

Real-time trailer hinge angle estimation method and system based on feature point tracking Technical Field The application relates to the technical field of intelligent driving assistance, in particular to a real-time trailer hinging angle estimation method and system based on feature point tracking. Background In a special operation scene, the trailer and the tractor are connected through a hinge point to form a complex multi-body dynamics system. The articulation angle directly determines the instantaneous steering center and the travel path of the vehicle combination. When turning, changing the road or emergently avoiding, if the hinging angle is too big, dangerous "folding" phenomenon easily takes place, and the trailer gets rid of to tractor one side promptly, leads to out of control. In addition, during high speed travel, severe or frequent changes in angle may also induce lateral oscillations of the trailer, ultimately resulting in rollover. Thus, grasping the articulation angle accurately in real time is a prerequisite for predicting vehicle stability, triggering intervention by active safety systems (e.g., trailer version of electronic stability control ESC). Without accurate real-time angle data, these safety systems or intelligent driving assistance cannot determine whether the vehicle is in a dangerous state. In the prior art, a method based on deep learning key point detection has become a mainstream technical scheme. Although the method can realize estimation with certain precision under specific conditions, the inherent limitation of the method is increasingly prominent in practical large-scale application, firstly, the deep learning method is highly dependent on a large-scale and high-quality key point marking data set to cover various weather, illumination, trailer types and angle scenes, the data acquisition and manual marking cost is extremely high, the development period is long, secondly, the complex deep neural network carries out forward reasoning calculation on load, the hardware resource consumption is high under the requirement of high-frame-rate real-time processing, the real-time performance is difficult to ensure, and furthermore, the existing scheme generally carries out independent processing on video streams frame by frame, ignores the time sequence continuity of angle change between adjacent frames, and causes unnecessary inter-frame jitter and poor stability of output angles. In addition, the existing real-time trailer hinging angle estimation method is insufficient in environmental adaptability, the angle estimation model performance is severely limited by the environmental conditions of training data, the accuracy is remarkably reduced in the scenes with larger differences from training sets such as night, rain and fog, and the like, models are often required to be trained for different environments respectively to ensure robustness, and the complexity and maintenance cost of the system are increased. Meanwhile, when the trailer hinging region is shielded by cargoes or a car body part, the key points are easy to be subjected to missed detection or false detection, so that the angle calculation is failed or the error is increased rapidly. Disclosure of Invention Aiming at the technical problems, the application provides a real-time trailer hinge angle estimation method and system based on feature point tracking, which improve the accuracy and instantaneity of real-time trailer hinge angle estimation. In a first aspect, an embodiment of the present application provides a method for real-time estimating a trailer hinge angle based on feature point tracking, including: acquiring a first image frame of a target trailer at the current moment and a second image frame of the target trailer at the previous moment; Determining a first characteristic point list of the first image frame according to a second characteristic point list in the second image frame; Calculating to obtain a visual estimation angle and a confidence coefficient according to the first characteristic point list and a preset characteristic point template; Updating a first Kalman filter at the current moment according to the visual estimation angle and the confidence coefficient, obtaining an updated second Kalman filter, and outputting an estimated value of the hinge angle at the current moment based on the second Kalman filter; And in a preset time period after the current time, carrying out real-time hinge angle estimation for a plurality of times through the second Kalman filter according to the motion data of the target trailer, and further outputting hinge angle estimated values of all times in the preset time period. The embodiment of the application provides a real-time trailer hinge angle estimation method based on feature point tracking, which realizes high-precision and high-real-time hinge angle estimation by combining feature point tracking with real-time updating of a Kalman filter. First, at each i