Estimation method, system and equipment of target travel path direction and storage medium

Abstract

The invention discloses a target travel direction estimation method, system and device and a storage medium, and relates to the technical field of automatic driving. The target travel path direction estimation method comprises the following steps: acquiring a first movement track of a target, and converting the first movement track into a second movement track; determining a dynamic track and a track length of the target according to the second motion track and a preset adaptive track length determination algorithm; and determining the travel path direction of the target according to the dynamic trajectory, the trajectory length and a preset principal component analysis algorithm. According to the target travel path direction estimation method provided by the invention, the dynamic trajectory of the target is constructed based on the preset adaptive trajectory length determination algorithm, and the trajectory length of the target is dynamically adjusted, so that system errors caused by instantaneous sudden change (such as violent steering) of the target travel path direction are reduced; and the path direction vector of the target is extracted from the motion trail of the target by using a principal component analysis method, so that the accuracy of the path direction estimation of the target is improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of automatic driving, in particular to a target moving direction estimation method, system, device and storage medium. BACKGROUND

[0002] In order to improve the perception effect of automatic driving, it is necessary to estimate the moving direction of the target in real time. The commonly used method is to perceive the moving direction of the instantaneous output through the sensor. However, due to the uncertainty of the sensor, the perception of the moving direction of the target has deviation, and in special cases, it loses reference value and has an error impact on the auxiliary driving function of the car. SUMMARY

[0003] The purpose of the present application is to provide a target moving direction estimation method, system, device and storage medium to improve the accuracy of target moving direction estimation.

[0004] Technical scheme: the target moving direction estimation method provided by the embodiments of the present application comprises: obtaining a first motion trajectory of the target and converting the first motion trajectory into a second motion trajectory; wherein the first motion trajectory is a motion trajectory of the target in a vehicle coordinate system; and the second motion trajectory is a motion trajectory of the target in a global coordinate system; determining a dynamic trajectory and a trajectory length of the target according to the second motion trajectory and a preset adaptive trajectory length determination algorithm; determining the moving direction of the target according to the dynamic trajectory, the trajectory length and a preset principal component analysis algorithm.

[0005] In some embodiments, the determination of the dynamic trajectory and the trajectory length of the target according to the second motion trajectory and the preset adaptive trajectory length determination algorithm comprises: determining a maximum allowed arc length corresponding to a trajectory point set of the second motion trajectory; determining the dynamic trajectory and the trajectory length according to the second motion trajectory, the maximum allowed arc length and a preset traversal condition.

[0006] In some embodiments, the determination of the dynamic trajectory and the trajectory length according to the second motion trajectory, the maximum allowed arc length and the preset traversal condition comprises: traversing the trajectory point set in reverse from the latest trajectory point of the second motion trajectory; determining the cumulative arc length of the target according to the latest trajectory point, the trajectory point set and a preset arc length estimation algorithm; stop traversing the set of trajectory points and determine the dynamic trajectory and the trajectory length according to the latest trajectory point and the set of traversed trajectory points in the case that the accumulated arc length is greater than the maximum allowable arc length.

[0007] In some embodiments, the method of determining the maximum allowable arc length comprises: acquiring an instantaneous speed of the target; determining the maximum allowable arc length according to the instantaneous speed, a preset curvature constraint coefficient and a preset minimum arc length.

[0008] In some embodiments, the determining the moving direction of the target according to the dynamic trajectory, the trajectory length and a preset principal component analysis algorithm comprises: constructing a trajectory data matrix according to the dynamic trajectory and the trajectory length; standardizing the trajectory data matrix and calculating a covariance matrix of the standardized trajectory data matrix; decomposing the covariance matrix to obtain a principal variation direction of the trajectory of the target; determining the moving direction of the target according to the principal variation direction of the trajectory and a preset motion continuity constraint condition.

[0009] In some embodiments, the method of estimating the moving direction of the target further comprises determining an eigenvalue ratio according to the covariance matrix; determining that the dynamic trajectory is a strong linear trajectory in the case that the eigenvalue ratio is greater than a preset ratio; returning to execute the step of determining the dynamic trajectory and the trajectory length of the target according to the second motion trajectory and a preset adaptive trajectory length determination algorithm in the case that the eigenvalue ratio is less than or equal to the preset ratio.

[0010] In some embodiments, after acquiring the first motion trajectory of the target, the method further comprises: performing motion compensation on the first motion trajectory according to a preset interpolation algorithm and a preset reverse motion compensation algorithm.

[0011] Correspondingly, the embodiments of the present application further provide a system for estimating the moving direction of a target, comprising: an acquisition module configured to acquire a first motion trajectory of the target; a conversion module configured to convert the first motion trajectory into a second motion trajectory; wherein the first motion trajectory is a motion trajectory of the target in a self-vehicle coordinate system; and the second motion trajectory is a motion trajectory of the target in a global coordinate system; a first determining module configured to determine a dynamic trajectory and a trajectory length of the target according to the second motion trajectory and a preset adaptive trajectory length determination algorithm; a second determining module configured to determine a moving direction of the target according to the dynamic trajectory, the trajectory length and a preset principal component analysis algorithm.

[0012] Correspondingly, the embodiment of the present application further provides an electronic device, comprising a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor implements the target moving direction estimation method as described above when executing the computer program.

[0013] Correspondingly, the embodiment of the present application further provides a computer readable storage medium having a computer program stored thereon, and the computer program is executed by a processor to implement the target moving direction estimation method as described above.

[0014] Advantages: Compared with the prior art, the target moving direction estimation method, system, device and storage medium provided by the embodiment of the present application, the target moving direction estimation method comprises: obtaining a first motion trajectory of a target, and converting the first motion trajectory into a second motion trajectory; wherein the first motion trajectory is a motion trajectory of the target in a self-vehicle coordinate system; the second motion trajectory is a motion trajectory of the target in a global coordinate system; determining a dynamic trajectory and a trajectory length of the target according to the second motion trajectory and a preset adaptive trajectory length determination algorithm; and determining a moving direction of the target according to the dynamic trajectory, the trajectory length and a preset principal component analysis algorithm. The target moving direction estimation method provided by the present application constructs a dynamic trajectory of the target based on a preset adaptive trajectory length determination algorithm, dynamically adjusts the trajectory length of the target, reduces the system error caused by the instantaneous mutation (such as sharp turning) of the target moving direction, and uses the principal component analysis method to extract the moving direction vector of the target from the motion trajectory of the target, thereby improving the accuracy of the target moving direction estimation. BRIEF DESCRIPTION OF DRAWINGS

[0015] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed in the embodiment description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0016] Figure 1 is a schematic diagram of a target moving direction filtering estimation based on a motion state provided in the embodiment of the present application; Figure 2 is a schematic diagram of Kalman filtering under non-Gaussian noise provided in the embodiment of the present application; Figure 3is a flowchart of a target moving direction estimation method provided in an embodiment of the present application; Figure 4 is a schematic diagram of the overall flow of a target moving direction estimation method provided in an embodiment of the present application; Figure 5 is a principle structure block diagram of a target moving direction estimation system provided in an embodiment of the present application; Figure 6 is a structure schematic diagram of an electronic device provided in an embodiment of the present application.

[0017] Reference signs: 101 - acquisition module; 102 - conversion module; 103 - first determination module; 104 - second determination module; 100 - target moving direction estimation system. DETAILED DESCRIPTION

[0018] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0019] It should be understood that although the terms first, second, etc. can be used herein to describe various components, these components should not be limited by these terms. These terms are used to distinguish one component from another component. Therefore, the first component discussed below can be called the second component without departing from the teachings of the present application. As used herein, the term "and / or" includes any one of the associated listed items and one or more of all combinations thereof.

[0020] Those skilled in the art can understand that the drawings are only schematic diagrams of example embodiments and can not be to scale. The modules or flows in the drawings are not necessarily essential for implementing the present application, and therefore cannot be used to limit the scope of protection of the present application.

[0021] Applicant has found that, based on the target moving direction filtering estimation of the motion state, the motion state is usually used to describe the target attribute, such as indicating the position, orientation, speed, turning rate, etc. of the target. The source of the target motion state information is mainly the sensor, including laser radar, millimeter wave radar, etc. Since there is a certain noise in the measurement of the sensor at each time, if only the orientation at the current time is used to represent the moving direction of the target, it is easy to appear a large deviation from the true result. Therefore, the target motion state needs to be filtered through multiple frames of measurement to obtain a more accurate estimation result. The commonly used filtering algorithm at present is Kalman filtering, which provides optimal estimation by combining prediction and measurement update to reduce error.

[0022] Figure 1 is a schematic diagram of a target moving direction filtering estimation based on a motion state provided in an embodiment of the present application. For example, refer to Figure 1 In the filtering stage, first, a predicted moving direction at the current time is obtained according to a target moving direction at the previous time (for example, a triangle point in a filtering node in Figure 1 , and then the target posterior moving direction (for example, a square point in a filtering node in Figure 1 , is obtained by updating using a measurement at the current time (for example, a circular point in a filtering stage in Figure 1 ). With the filtering, the target motion state gradually converges to the vicinity of the target real moving direction.

[0023] However, the target moving direction filtering estimation based on the motion state has the following deficiencies: First, the Kalman filtering algorithm is derived under the assumption that the noise obeys a Gaussian distribution. When the sensor perception noise does not obey the Gaussian distribution, the estimation result of the Kalman filtering does not conform to the optimal estimation, and there is a certain deviation, which affects the normal operation of the auxiliary driving function. Figure 2 is a schematic diagram of Kalman filtering under non-Gaussian noise provided in an embodiment of the present application. For example, refer to Figure 2 , the circular point is a measurement trajectory, and the triangle point is an updated trajectory. When the sensor perceives multiple consecutive error moving directions, the filtering result converges to the error moving direction.

[0024] Second, because the sensor perception moving direction has instability, the perception results of the front and back frames may have a large deviation. In the process of calculating the Kalman gain, the large deviation is easy to cause precision overflow, thereby bringing an irreversible filter system error, and even causing the filter to crash, which affects the normal operation of the auxiliary driving function.

[0025] Therefore, an embodiment of the present application provides a target moving direction estimation method, system, device and storage medium. The present application constructs a dynamic trajectory of the target by using a preset adaptive trajectory length determination algorithm, dynamically adjusts the trajectory length of the target, reduces the system error caused by the instantaneous mutation (such as sharp turning) of the target moving direction, and extracts the moving direction vector of the target from the motion trajectory of the target by using a principal component analysis method, thereby improving the accuracy of the target moving direction estimation.

[0026] Figure 3is a flowchart of a method for estimating a target moving direction provided in an embodiment of the present application. The method can be applied in a vehicle control system to accurately estimate the target moving direction. The method can be executed by a system for estimating the target moving direction, which can be implemented in software and / or hardware and can be configured in a processor or controller of the vehicle control system. Please refer to Figure 3 The method comprises the following steps: Step 110, obtaining a first motion trajectory of a target and converting the first motion trajectory into a second motion trajectory.

[0027] The first motion trajectory is a motion trajectory of the target in a vehicle coordinate system, and the second motion trajectory is a motion trajectory of the target in a global coordinate system (world coordinate system).

[0028] The target refers to other vehicles, pedestrians, etc. that need to estimate the moving direction of the current vehicle (i.e. the ego vehicle).

[0029] The first motion trajectory is a relative target trajectory, which is a target trajectory of the target in a relative coordinate system (such as the ego vehicle coordinate system). The first motion trajectory refers to a set of poses of the target relative to the ego vehicle at each time.

[0030] Specifically, obtaining the first motion trajectory of the target comprises: Sensor data acquisition and target detection and tracking: continuously acquiring surrounding environment perception data through the multi-modal sensors (including but not limited to visual camera, laser radar, millimeter wave radar, etc.) carried by the ego vehicle. Using target detection and tracking algorithms (such as visual detection based on deep learning, point cloud clustering tracking, multi-sensor fusion tracking, etc.) to process these perception data, to identify and track the target in real time. The target tracking result usually includes the position information of the target in the ego vehicle coordinate system and its size, speed (relative speed), etc. These position information represents the position of the target relative to the ego vehicle at a specific sampling time.

[0031] Constructing a target relative trajectory (first motion trajectory): arranging and combining the target tracking results at different times in time sequence to form a motion trajectory point sequence of the target in the ego vehicle coordinate system. The trajectory point sequence is the first motion trajectory. It should be noted that the target tracking result directly provides the position of the target in the ego vehicle coordinate system.

[0032] Specifically, since the target relative trajectory in the ego vehicle coordinate system will be affected by the state change of the ego vehicle, it is necessary to convert the target relative trajectory (i.e. the first motion trajectory) into an absolute trajectory (i.e. the second motion trajectory) in the world coordinate system (i.e. the global coordinate system) in order to determine the length of the target trajectory and determine the target moving direction subsequently.

[0033] In some embodiments, after obtaining the first motion trajectory of the target, the method further comprises: performing motion compensation on the first motion trajectory according to a preset interpolation algorithm and a preset reverse motion compensation algorithm. This process requires the use of ego vehicle positioning information, which refers to the real-time position and attitude (including position coordinates, heading angle, etc.) data of the ego vehicle in a global coordinate system (such as the World Geodetic System 1984 (WGS-84) or a local coordinate system) obtained by a Global Positioning System (GPS) or its enhanced system (such as a Real-Time Kinematic Global Positioning System (RTK-GPS)) mounted on the ego vehicle. The target tracking result refers to the result obtained by processing the environmental perception data collected by the multi-modal sensors mounted on the ego vehicle. The multi-modal sensors include but are not limited to visual cameras, Light Detection and Ranging (LiDAR), Millimeter Wave Radar, etc., and can be set according to actual conditions without specific limitation.

[0034] The preset interpolation algorithm includes linear interpolation and spherical linear interpolation (Slerp), etc.

[0035] Since the sensors in autonomous driving are installed on the ego vehicle, the output target detection information is generally given in the form of the ego vehicle coordinate system. The information between different frames needs to be compensated for ego vehicle motion, for example, turning motion will introduce certain errors. The global coordinate system as a "global reference perspective" can convert the data in the ego vehicle coordinate system to the global coordinate system in each frame for subsequent fusion, avoiding the noise introduced by the ego vehicle motion compensation process.

[0036] Specifically, the specific implementation process of performing motion compensation on the first motion trajectory according to the preset interpolation algorithm and the preset reverse motion compensation algorithm is as follows: First, perform pose interpolation: let the current frame time be , and the last frame time be . Based on the high-frequency pose stream (such as 100Hz GPS data or 100Hz Inertial Navigation System (INS) data) output by the ego vehicle positioning system, the last frame time to the current frame time is calculated by linear interpolation or spherical linear interpolation. The self-vehicle state at the current frame time is as follows: wherein, represents the self-vehicle pose in the global coordinate system at any time . represents the self-vehicle X-axis coordinate in the global coordinate system at any time . represents the self-vehicle Y-axis coordinate in the global coordinate system at any time . represents the heading angle of the self-vehicle in the global coordinate system at any time .

[0037] Then, reverse motion compensation is performed: the target relative position detected at the last frame time is compensated to the current frame time reference by using the self-vehicle pose (i.e., the self-vehicle state) at the last frame time obtained by pose interpolation, and the specific process is as follows: wherein, represents a rotation matrix; represents the target relative position after motion compensation at the last frame time . represents the target relative position detected at the last frame time . represents the heading angle of the self-vehicle in the global coordinate system at the current frame time . represents the self-vehicle displacement difference.

[0038] wherein, represents the self-vehicle X-axis coordinate in the global coordinate system at the current frame time . represents the self-vehicle Y-axis coordinate in the global coordinate system at the current frame time .

[0039] It can be seen that the technical scheme of the embodiment of the application can eliminate the target apparent displacement caused by the self-vehicle motion by performing motion compensation on the first motion trajectory before converting the first motion trajectory into the second motion trajectory (i.e., converting from the self-vehicle coordinate system to the global coordinate system), and ensure that the target positions of all frames are unified to the self-vehicle coordinate system reference at the current frame time , so as to realize time alignment of the cross-frame trajectory points and eliminate cross-frame coordinate offset.

[0040] ​​​​To avoid errors caused by positioning noise and sensor measurement noise, after motion compensation of the first motion trajectory, the embodiment of the application implements tracking filtering after conversion from the ego vehicle coordinate system to the global coordinate system (i.e., the process of converting from the first motion trajectory to the second motion trajectory), and the specific process is as follows: First, real-time global coordinate conversion is performed: the target relative position after motion compensation at the current frame time is mapped to the world coordinate system through homogeneous transformation, and the specific process is as follows:

[0041] Among them, represents the target position in the global coordinate system at the current frame time represents the target relative position after motion compensation at the current frame time

[0042] Then, global domain filtering optimization is performed: Kalman Filter (KF) or Extended Kalman Filter (EKF) is applied in the global coordinate system, the target motion model (such as constant velocity model CV and constant acceleration model CA) is taken as the state equation, and the global coordinate observation value is taken as the measurement equation, to obtain the state equation and the measurement equation. Among them, the target motion model is constant velocity model (Constant Velocity, CV) and constant acceleration model (Constant Acceleration, CA) and the like.

[0043] Among them, the state equation is: Among them, the measurement equation is: Among them, represents the state vector, including the target global position and the speed; represents the CV model state transition matrix; represents the state transition noise; represents the normal distribution model; represents the state transition noise covariance matrix; represents the measurement vector; represents the measurement matrix; represents the measurement noise; represents the measurement noise covariance matrix.

[0044] Among them, the state vector is: Among them, ​​​​​represents a lateral coordinate value of the target in the global coordinate system; represents a longitudinal coordinate value of the target in the global coordinate system; represents a lateral velocity of the target in the global coordinate system; represents a longitudinal velocity of the target in the global coordinate system.

[0045] Therefore, it can be known that the embodiment of the present application can realize the processing of positioning noise in a single filtering link by implementing tracking filtering in the global coordinate system, and avoid the error secondary amplification caused by the method of filtering in the ego coordinate system and then converting to the global coordinate system in the related technology. Therefore, it can be known that the scheme of the embodiment of the present application can eliminate the cross-frame coordinate offset through pose interpolation and reverse motion compensation, and perform Kalman filtering in the global coordinate system to generate an anti-noise trajectory.

[0046] In addition, the filtered and optimized target trajectory (i.e., the second motion trajectory) is stored in time sequence, and the trajectory storage structure is as follows: Among them, and constitute the basic input data set of the subsequent preset principal component analysis algorithm.

[0047] Step 120, determining the dynamic trajectory and the trajectory length of the target according to the second motion trajectory and the preset adaptive trajectory length determination algorithm.

[0048] Due to the uncertainty of the target motion state, that is, the target has a large curvature turning ability when moving at low speed (for example, a pedestrian turns sharply), and the target is constrained by dynamics when moving at high speed, and only allows small curvature turning (for example, a vehicle turns at high speed), the trajectory processing method of the related technology using a fixed time window will cause direction estimation deviation. Therefore, the embodiment of the present application can determine the dynamic trajectory of the target in real time and adaptively adjust the trajectory length of the target through the second motion trajectory and the preset adaptive trajectory length determination algorithm.

[0049] In some embodiments, the dynamic trajectory and the trajectory length of the target are determined according to the second motion trajectory and the preset adaptive trajectory length determination algorithm, specifically including the following steps: Step one, determining the maximum allowed arc length corresponding to the trajectory point set of the second motion trajectory.

[0050] Among them, the first motion trajectory and the second motion trajectory each contain a plurality of trajectory points, and the plurality of trajectory points constitute a trajectory point set. The trajectory point set has a plurality of arc lengths in the spatial domain. By determining the maximum allowed arc length of the trajectory point set of the second motion trajectory in the spatial domain, it is beneficial to further determine the dynamic trajectory and the trajectory length of the target according to the preset traversal condition.

[0051] ​In some embodiments, the method of determining the maximum allowable arc length comprises: obtaining an instantaneous speed of the target; and determining the maximum allowable arc length according to the instantaneous speed, a preset curvature constraint coefficient, and a preset minimum arc length.

[0052] Specifically, the maximum allowable arc length of the set of trajectory points in the spatial domain is set as , which is determined by the current motion state. The calculation formula of the maximum allowable arc length is as follows: ; wherein, represents the current instantaneous speed of the target; represents a curvature constraint coefficient (i.e., a preset curvature constraint coefficient), which is determined by the type of the target. For example, when the target is a pedestrian, the curvature constraint coefficient of the pedestrian is about 0.5 . When the target is a vehicle, the curvature constraint coefficient of the vehicle is about 0.2 .

[0053] wherein, represents a basic minimum arc length (i.e., a preset minimum arc length), which is used to ensure the effective amount of data at low speed. For example, the value of the basic minimum arc length can be 1.5 m, and can also be other values, which can be set according to actual conditions and are not limited herein.

[0054] Step two, determining the dynamic trajectory and the trajectory length according to the second motion trajectory, the maximum allowable arc length, and a preset traversal condition.

[0055] Specifically, the maximum allowable arc length of the second motion trajectory of the target can be dynamically adjusted through the real-time instantaneous speed of the target, so as to facilitate real-time determination of the dynamic trajectory of the target and dynamic adjustment of the trajectory length of the target according to the second motion trajectory, the maximum allowable arc length, and the preset traversal condition.

[0056] In some embodiments, determining the dynamic trajectory and the trajectory length according to the second motion trajectory, the maximum allowable arc length, and the preset traversal condition comprises: traversing the set of trajectory points from the latest trajectory point of the second motion trajectory in reverse; determining the cumulative arc length of the target according to the latest trajectory point, the set of trajectory points, and a preset arc length estimation algorithm; and stopping traversing the set of trajectory points when the cumulative arc length is greater than the maximum allowable arc length, and determining the dynamic trajectory and the trajectory length according to the latest trajectory point and the set of trajectory points that have been traversed.

[0057] Specifically, the implementation process of determining the dynamic trajectory and the trajectory length comprises: first, traversing the set of historical trajectory points of the set of trajectory points of the second motion trajectory from the latest trajectory point of the second motion trajectory in reverse.

[0058] ​​Then, according to the latest trajectory point (Pn) The following is abbreviated ) Reverse traversal, historical trajectory point set and preset arc length estimation algorithm determine the cumulative arc length of the target. The calculation formula of the cumulative arc length is: ; Wherein, represents the second motion trajectory point of the target at the moment; represents the current time index; The value of needs to meet the condition >= .

[0059] Secondly, when the cumulative arc length is greater than the maximum allowed arc length , stop traversal, and intercept the subset . The trajectory point set corresponding to the intercepted subset is the dynamic trajectory of the target, and the length of the trajectory point set corresponding to the intercepted subset is the trajectory length of the target.

[0060] Finally, the intercepted subset (i.e. the dynamic trajectory of the target and the trajectory length) is taken as the input of the preset principal component analysis algorithm to estimate the direction of the target's movement. And the technical solution of the embodiment of the application can dynamically adjust the maximum allowed arc length of the second motion trajectory of the target by obtaining the real-time instantaneous speed of the target, so as to facilitate real-time determination of the dynamic trajectory of the target and dynamic adjustment of the trajectory length of the target according to the second motion trajectory, the maximum allowed arc length and the preset traversal condition. And it can be realized to extend the trajectory length to suppress non-Gaussian noise in high-speed scene, shorten the trajectory length to maintain the agility of steering response in low-speed scene, and solve the estimation lag and oversmoothing problem caused by the fixed time window in related technology. For example, in the low-speed scene, the current instantaneous speed of the target is small, so the corresponding maximum allowed arc length is small, and a short trajectory is used to capture rapid direction changes. In the high-speed scene, the current instantaneous speed of the target is large, so the corresponding maximum allowed arc length is large, the trajectory is lengthened to smooth the noise and meet the motion continuity, which fundamentally avoids the direction estimation lag or oversmoothing problem caused by the fixed time window in the variable speed scene.

[0061] Step 130, determining the movement direction of the target according to the dynamic trajectory, the trajectory length and the preset principal component analysis algorithm.

[0062] Principal Component Analysis (PCA) is a dimensionality reduction technique used for data analysis and feature extraction. It simplifies data structure and removes redundancy by projecting the original data into a new coordinate system to maximize the preservation of data variance. In this process, PCA finds the direction that maximizes the data variance, which is similar to estimating the direction of travel through a target trajectory.

[0063] In the technical solution of this application embodiment, the trajectory data matrix is ​​first standardized using PCA technology, and the covariance matrix is ​​calculated. Then, the covariance matrix is ​​decomposed into eigenvalues ​​and eigenvectors. The eigenvectors represent the directions of the principal components, and the eigenvalues ​​represent the variances of the principal components. Finally, the original data is projected onto the principal components to obtain the dimensionality-reduced data.

[0064] In some embodiments, determining the target's trajectory direction based on the dynamic trajectory, trajectory length, and a preset principal component analysis algorithm includes: constructing a trajectory data matrix based on the dynamic trajectory and trajectory length; standardizing the trajectory data matrix and calculating the covariance matrix of the standardized trajectory data matrix; decomposing the covariance matrix to obtain the principal variation direction of the target's trajectory; and determining the target's trajectory direction based on the principal variation direction of the trajectory and preset motion continuity constraints.

[0065] Specifically, the process for determining the target's trajectory direction is as follows: First, construct a trajectory data matrix (two-dimensional data matrix). Arrange the n global coordinate points of the target trajectory within the time window into a data matrix according to time sequence, as follows: ; Then, the trajectory data matrix is ​​standardized. The standardization method is decentralized. Specifically, the trajectory data matrix... Decentralization is performed to eliminate positional bias. The specific formula is as follows: ; ; in, This represents the mean of the trajectory coordinates; This represents the decentralized trajectory data matrix; Represents the tensor product; This represents an n-dimensional vector of all 1s.

[0066] Secondly, the covariance matrix is ​​constructed, and its eigenvalues ​​are decomposed. Specifically, the covariance matrix is ​​calculated and decomposed based on the decentralized matrix data as follows: ; in, denotes a covariance matrix.

[0067] denotes a covariance matrix. wherein, denotes an eigenvector, eigenvalues and eigenvectors are obtained. Wherein, the eigenvalues are wherein, is greater than . The eigenvector is . Wherein, the maximum eigenvalue corresponding eigenvector is the main variation direction of the trajectory of the target.

[0068] Then, the target moving direction vector is determined. Specifically, the direction ambiguity of the main variation direction of the trajectory is eliminated by the preset motion continuity constraint condition. Wherein, the preset motion continuity constraint condition is: wherein, the displacement vector is: ; wherein, the direction symbol is: ; wherein, the direction vector is: ; Finally, the direction vector is output as the target moving direction unit vector, and thus the moving direction of the target is obtained. As can be seen, by constructing the trajectory point set into a two-dimensional data matrix and performing centering processing, the principal direction vector is extracted by eigenvalue decomposition of the covariance matrix, the direction polarity is determined based on the projection symbol of the displacement vector d of the start and end points of the trajectory, and the unit direction vector with confidence is output. The present application can realize dynamic construction of the target trajectory in the global coordinate system based on the self-positioning information and the target tracking result. Specifically, the moving direction vector of the target is extracted from the target trajectory by using the principal component analysis method, rather than directly relying on the instantaneous output of the moving direction perceived by the sensor. This method can effectively avoid the estimation error introduced by the non-Gaussian distribution characteristics of the sensor noise. At the same time, the method of calculating the moving direction based on the trajectory point set can significantly suppress the system error caused by the instantaneous mutation of the target moving direction (such as sharp turning), thereby improving the stability and robustness of the direction estimation.

[0069] In some embodiments, the method for estimating the target moving direction further comprises: determining an eigenvalue ratio according to the covariance matrix; determining that the dynamic trajectory is a strong linear trajectory when the eigenvalue ratio is greater than a preset ratio; and returning to perform the steps of determining the dynamic trajectory and the trajectory length of the target according to the second motion trajectory and the preset adaptive trajectory length determination algorithm when the eigenvalue ratio is less than or equal to the preset ratio.

[0070] wherein the eigenvalue ratio is calculated by the following formula: ; Specifically, the reliability of the target moving direction estimation is quantified by determining the eigenvalue ratio of the covariance matrix and comparing the eigenvalue ratio with the preset ratio.

[0071] In some embodiments, the embodiments of the present application further provide a method for updating the target moving method based on Kalman filtering. The present application constructs a filtering framework based on the direction vector, which supports Kalman filtering, extended Kalman filtering, unscented Kalman filtering and particle filtering and other implementation modes. For example, taking Kalman filtering as an example, the framework performs time sequence optimization on the moving direction extracted by PCA to solve the direction jump problem and improve the continuity of the estimation. The specific implementation process is as follows: First, the state space is established. The filtering state quantity is defined as a four-dimensional vector containing the direction vector and its change rate: ; wherein, represents the unit direction vector output by PCA. represents the direction change rate, which is used to represent the direction angular velocity.

[0072] Then, the state equation is designed. For example, the Constant Turn Rate and Velocity (CTRV) is used to design the state equation. Wherein, the Constant Turn Rate and Velocity (CTRV) is: ; wherein, represents the state transition noise covariance matrix; represents the state transition matrix, which is specifically as follows:

[0073] wherein, represents the PCA direction update interval. For example, the value range of the update interval is 50ms-200ms, and the specific value can be set according to the actual situation, which is not limited here. Wherein, represents the angular velocity change coefficient.

[0074] ; wherein, is a direction change rate attenuation coefficient. Exemplarily, the direction change rate attenuation coefficient takes a value in the range of 0.1-0.5, and a specific value can be set according to an actual situation, which is not specifically limited here.

[0075] wherein, is a state transition noise covariance matrix, and the structure thereof is: ; wherein, is a noise variance; is a unit matrix.

[0076] Secondly, an observation equation is constructed. A direction vector output by the PCA is directly observed: ; wherein, is a direction vector output by the PCA; is an observation matrix; is a process noise covariance matrix; is a noise covariance. The observation matrix and the noise covariance are designed as: ; ; wherein, is an observation trust coefficient (usually taken as 0.5-2.0), which is used to realize confidence adaptive weighting, for example, the lower the PCA confidence is, the greater the observation noise is.

[0077] Finally, filtering is performed, and a specific implementation process includes the following steps: Step one, initialization, specifically as follows: ; ; wherein, is an initial state of a moving direction; is a target moving direction estimated by the PCA for the first time; is an initial state covariance matrix; is a diagonal matrix; is an initial moving direction variance; is an initial moving direction variance.

[0078] Then, the moving direction estimated by the PCA for each frame is used: ​Prediction and update are performed.

[0079] Step two, prediction, the state at time is predicted to time .

[0080] ; ; wherein denotes the state at time predicted to time ; denotes the state transition matrix, which has been given above; denotes the state at time ; denotes the state at time predicted to time ; denotes the state transition noise covariance matrix.

[0081] Step three, update, the heading direction estimated by the PCA at time is used to update the state.

[0082] ; ; ; ; ; ; wherein denotes the prediction measurement bias; denotes the Kalman gain; denotes the updated state, denotes the updated covariance matrix.

[0083] Step four, normalization.

[0084] ; wherein denotes the filtered target heading direction; denotes the component of the updated state in the x-axis direction; denotes the component of the updated state in the y-axis direction.

[0085] The position vector normalization is the filtered heading direction at time .

[0086] ​Finally, the target moving direction obtained by filtering is converted into the ego-vehicle coordinate system and output. The output formula is: ; wherein, represents the target moving direction in the ego-vehicle coordinate system finally output; represents the direction vector of the ego-vehicle in the global coordinate system.

[0087] Figure 4 is a schematic diagram of the overall process of the target moving direction estimation method provided in the embodiments of the present application. For example, refer to Figure 4 , first, the target trajectory (i.e., the first motion trajectory) is converted to obtain a second motion trajectory. Then, the estimated trajectory length is determined, i.e., the length of the target trajectory is determined. Secondly, the target moving direction is estimated by principal component analysis. Then, Kalman filtering and target moving direction updating are performed. Finally, the target moving direction is converted from the global coordinate system to the ego-vehicle coordinate system and output.

[0088] As can be seen from the above, the technical scheme of the embodiments of the present application proposes a PCA moving direction real-time estimation algorithm based on a kinematic constraint adaptive spatial trajectory length mechanism. By dynamically adjusting the trajectory arc length threshold (i.e., the maximum allowed arc length) according to the target instantaneous speed, the effective trajectory point set is intercepted according to the spatial domain arc length threshold, which can extend the trajectory length in high-speed scenarios, suppress non-Gaussian noise, shorten the trajectory length in low-speed scenarios, maintain the agility of the steering response, and solve the estimation lag and oversmoothing problems caused by the traditional fixed time window. Moreover, based on the positioning information and tracking results, the target trajectory is dynamically constructed in the global coordinate system, and the PCA method is used to extract the moving direction from the target trajectory, rather than directly using the moving direction perceived by the sensor, so that the deviation caused by non-Gaussian noise of the sensor can be avoided. According to the target historical trajectory length, the moving direction is calculated (i.e., the preset adaptive trajectory length determination algorithm), which can effectively avoid the sudden change of the moving direction caused by the sudden change of the single-frame trajectory, thereby avoiding the filter system error or even filter collapse caused by the numerical mutation, and has good stability.

[0089] Figure 5 is a principle structure block diagram of a target moving direction estimation system provided in the embodiments of the present application. Correspondingly, the embodiments of the present application also provide a target moving direction estimation system, please refer to Figure 5The target moving direction estimation system 100 comprises: an acquisition module 101, configured to acquire a first motion trajectory of a target; a conversion module 102, configured to convert the first motion trajectory into a second motion trajectory; wherein the first motion trajectory is a motion trajectory of the target in a self-vehicle coordinate system; and the second motion trajectory is a motion trajectory of the target in a global coordinate system; a first determination module 103, configured to determine a dynamic trajectory and a trajectory length of the target according to the second motion trajectory and a preset adaptive trajectory length determination algorithm; and a second determination module 104, configured to determine a moving direction of the target according to the dynamic trajectory, the trajectory length and a preset principal component analysis algorithm.

[0090] The technical scheme of the embodiment of the present application provides a target moving direction estimation system, which constructs a dynamic trajectory of a target based on a preset adaptive trajectory length determination algorithm, dynamically adjusts the trajectory length of the target, reduces system errors caused by instantaneous mutation (such as sharp turning) of the moving direction of the target, and extracts a moving direction vector of the target from the motion trajectory of the target by using a principal component analysis method, thereby improving the accuracy of target moving direction estimation.

[0091] In some embodiments, the first determination module 103 is configured to: determine a maximum allowable arc length corresponding to a trajectory point set of the second motion trajectory; and determine the dynamic trajectory and the trajectory length according to the second motion trajectory, the maximum allowable arc length and a preset traversal condition.

[0092] In some embodiments, the first determination module 103 is further configured to: traverse the trajectory point set in a reverse direction from a latest trajectory point of the second motion trajectory; determine a cumulative arc length of the target according to the latest trajectory point, the trajectory point set and a preset arc length estimation algorithm; and stop traversing the trajectory point set and determine the dynamic trajectory and the trajectory length according to the latest trajectory point and the traversed trajectory point set when the cumulative arc length is greater than the maximum allowable arc length.

[0093] In some embodiments, the first determination module 103 is further configured to: acquire an instantaneous speed of the target; and determine the maximum allowable arc length according to the instantaneous speed, a preset curvature constraint coefficient and a preset minimum arc length.

[0094] In some embodiments, the second determination module 104 is further configured to: construct a trajectory data matrix according to the dynamic trajectory and the trajectory length; perform standardization processing on the trajectory data matrix and calculate a covariance matrix of the trajectory data matrix after the standardization processing; decompose the covariance matrix to obtain a principal variation direction of the trajectory of the target; and determine the moving direction of the target according to the principal variation direction of the trajectory and a preset motion continuity constraint condition.

[0095] In some embodiments, the estimation system of the target moving direction further comprises a third determining module configured to determine an eigenvalue ratio according to the covariance matrix; a fourth determining module configured to determine that the dynamic trajectory is a strong linear trajectory when the eigenvalue ratio is greater than a preset ratio; and return to execute the steps of determining the dynamic trajectory and the trajectory length of the target according to the second motion trajectory and the preset adaptive trajectory length determination algorithm when the eigenvalue ratio is less than or equal to the preset ratio.

[0096] In some embodiments, the estimation system of the target moving direction further comprises a compensation module configured to perform motion compensation on the first motion trajectory of the target according to a preset interpolation algorithm and a preset reverse motion compensation algorithm after the first motion trajectory is acquired.

[0097] Figure 6 is a structural schematic diagram of an electronic device provided in an embodiment of the present application. Correspondingly, the present application also provides an electronic device, please refer to Figure 6 The electronic device comprises a memory, a processor, and a computer program stored in the memory and executable on the processor, and the processor implements the steps of the above-mentioned estimation method of the target moving direction when executing the computer program. Since the above-mentioned estimation method of the target moving direction is described in detail, it will not be repeated here.

[0098] Correspondingly, the present application also provides a computer readable storage medium having a computer program stored thereon, and the computer program is executed by the processor to implement the steps of the above-mentioned estimation method of the target moving direction. Since the above-mentioned estimation method of the target moving direction is described in detail, it will not be repeated here.

[0099] In the above embodiments, the description of each embodiment has its own focus, and the parts not described in detail in a certain embodiment can be referred to the relevant description of other embodiments.

[0100] The estimation method, system, device and storage medium of the target moving direction provided by the embodiments of the present application are described in detail above, and specific examples are applied to explain the principles and implementation manners of the present application. The above description of the embodiments is only used to help understand the technical solutions and core ideas of the present application; those skilled in the art should understand that they can still modify the technical solutions recorded in the above embodiments, or make equivalent replacement to some technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A method of estimating a target heading direction, characterized by, The method comprises the following steps: acquiring a first motion trajectory of the target and converting the first motion trajectory into a second motion trajectory; wherein the first motion trajectory is a motion trajectory of the target in a self-vehicle coordinate system; and the second motion trajectory is a motion trajectory of the target in a global coordinate system; determining a dynamic trajectory and a trajectory length of the target according to the second motion trajectory and a preset adaptive trajectory length determination algorithm; determining a moving direction of the target according to the dynamic trajectory, the trajectory length and a preset principal component analysis algorithm.

2. The method of estimating the direction of travel of the object of claim 1, wherein, The step of determining the dynamic trajectory and the trajectory length of the target according to the second motion trajectory and the preset adaptive trajectory length determination algorithm comprises the following steps: determining a maximum allowable arc length corresponding to a trajectory point set of the second motion trajectory; determining the dynamic trajectory and the trajectory length according to the second motion trajectory, the maximum allowable arc length and a preset traversal condition.

3. The method of estimating the direction of travel of the object of claim 2, wherein, The step of determining the dynamic trajectory and the trajectory length according to the second motion trajectory, the maximum allowable arc length and the preset traversal condition comprises the following steps: traversing the trajectory point set in a reverse direction from a latest trajectory point of the second motion trajectory; determining a cumulative arc length of the target according to the latest trajectory point, the trajectory point set and a preset arc length estimation algorithm; stopping traversing the trajectory point set and determining the dynamic trajectory and the trajectory length according to the latest trajectory point and the traversed trajectory point set when the cumulative arc length is greater than the maximum allowable arc length.

4. The method of estimating the direction of travel of the object of claim 2, wherein, The method for determining the maximum allowable arc length comprises the following steps: acquiring an instantaneous speed of the target; determining the maximum allowable arc length according to the instantaneous speed, a preset curvature constraint coefficient and a preset minimum arc length.

5. The method of estimating the direction of travel of the object of claim 1, wherein, The step of determining the moving direction of the target according to the dynamic trajectory, the trajectory length and the preset principal component analysis algorithm comprises the following steps: constructing a trajectory data matrix according to the dynamic trajectory and the trajectory length; performing standardization processing on the trajectory data matrix and calculating a covariance matrix of the trajectory data matrix after the standardization processing; decomposing the covariance matrix to obtain a principal variation direction of the trajectory of the target; determining the moving direction of the target according to the principal variation direction of the trajectory and a preset motion continuity constraint condition.

6. The method of estimating the direction of travel of the object of claim 5, wherein, The method further comprises the following steps: determining an eigenvalue ratio according to the covariance matrix; determining that the dynamic trajectory is a strong linear trajectory when the eigenvalue ratio is greater than a preset ratio; returning to the step of determining the dynamic trajectory and the trajectory length of the target according to the second motion trajectory and the preset adaptive trajectory length determination algorithm when the eigenvalue ratio is less than or equal to the preset ratio.

7. The method of estimating the direction of travel of the object of claim 1, wherein, After acquiring the first motion trajectory of the target, the method further comprises the following step: performing motion compensation on the first motion trajectory according to a preset interpolation algorithm and a preset reverse motion compensation algorithm.

8. A system for estimating a target direction of travel, the system comprising: The method comprises the following steps: an acquiring module, configured to acquire a first motion trajectory of the target; a converting module, configured to convert the first motion trajectory into a second motion trajectory; wherein the first motion trajectory is a motion trajectory of the target in a self-vehicle coordinate system; and the second motion trajectory is a motion trajectory of the target in a global coordinate system; The first determining module is configured to determine a dynamic trajectory and a trajectory length of the target according to the second motion trajectory and a preset adaptive trajectory length determining algorithm. The second determining module is configured to determine a moving direction of the target according to the dynamic trajectory, the trajectory length and a preset principal component analysis algorithm.

9. An electronic device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the computer program to implement the target moving direction estimation method in any one of claims 1-7.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program is executed by the processor to implement the target moving direction estimation method in any one of claims 1-7.

Images