Obstacle trajectory adjustment methods, devices and electronic equipment
Patent Information
- Application Number
- CN202610992895.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-07-03
- Publication Date
- 2026-09-15
Smart Images
Figure CN122747950A_ABST
Abstract
Description
Technical Field
[0001] This application relates to the fields of autonomous driving, autonomous driving and driverless vehicle technology, and specifically to an obstacle trajectory adjustment method, device and electronic device. Background Technology
[0002] Autonomous driving planning systems need to predict the position and motion of surrounding dynamic obstacles over a future period in order to perform obstacle avoidance, following, yielding, lane changing, and speed planning. However, relevant obstacle trajectory prediction methods rely on single-frame perception of obstacle states for trajectory prediction. Single-frame perception information may contain jitter, resulting in low accuracy of predicted trajectories. Summary of the Invention
[0003] In view of the above problems, this application provides an obstacle trajectory adjustment method, apparatus and electronic device.
[0004] According to the first aspect of this application, an obstacle trajectory adjustment method is provided, comprising: in response to detecting a target obstacle in motion of a vehicle, mapping the initial obstacle state information of the target obstacle to a spatial coordinate system to obtain target obstacle state information; correcting the target obstacle state information based on historical state information to obtain corrected state information, wherein the historical state information characterizes the motion state of the target obstacle in a historical time period; processing the corrected state information using a trajectory prediction model to obtain an initial estimated trajectory of the target obstacle in a first future time period; and adjusting the initial estimated trajectory based on the historical estimated trajectory of the target obstacle in a second future time period to obtain the target trajectory, wherein the historical estimated trajectory is obtained by trajectory reasoning based on the historical state information and the corrected state information, and the first future time period and the second future time period have overlapping future moments.
[0005] According to an embodiment of this application, the historical predicted trajectory includes multiple first target trajectory points, and the initial predicted trajectory includes multiple second target trajectory points. The process of adjusting the initial predicted trajectory based on the historical predicted trajectory of the target obstacle in a second future time period to obtain the target trajectory includes: comparing the first target trajectory points with the second target trajectory points to obtain a trajectory point attribute difference; determining an adjustment weight based on the trajectory point attribute difference and a preset difference threshold; and fusing the first and second target trajectory points based on the adjustment weight to obtain a target trajectory point, wherein the target trajectory includes multiple target trajectory points.
[0006] According to an embodiment of this application, the corrected state information includes the corrected position of the target obstacle; wherein, trajectory reasoning based on historical state information and corrected state information to obtain a historical predicted trajectory includes: performing statistical processing on the historical state information to obtain historical motion trend information, which includes historical average speed, historical statistical acceleration, and historical statistical heading change rate; using the historical statistical heading change rate, historical statistical acceleration, historical average speed, and the time interval between the future time and the response time in the second future time period, trajectory reasoning is performed on the corrected position to obtain a historical predicted trajectory, wherein the state information of the first target trajectory point in the historical predicted trajectory includes the reasoned position, reasoned acceleration, reasoned speed, and reasoned heading change rate.
[0007] According to an embodiment of this application, correcting the target obstacle state information based on historical state information to obtain corrected state information includes: determining the state deviation between the historical state information and the target obstacle state information, wherein the state deviation includes multiple sub-state deviations, and the sub-state deviations include at least one of position deviation, heading deviation, and velocity deviation; and, if there is a sub-state deviation greater than a preset deviation threshold, fusing the historical state component in the historical state information corresponding to the sub-state deviation and the target state component in the target obstacle state information corresponding to the sub-state deviation to obtain corrected state information.
[0008] According to an embodiment of this application, the target obstacle state information includes the target velocity direction and the target heading direction; the correction of the target obstacle state information further includes: when the deviation between the target velocity direction and the target heading direction is greater than a preset angle threshold, correcting the target heading direction according to the target velocity direction to obtain a corrected heading direction, or; correcting the target velocity direction according to the target heading direction to obtain a corrected velocity direction, wherein the correction state information includes the corrected heading direction or the corrected velocity direction.
[0009] According to an embodiment of this application, it further includes: constraining the target trajectory using configuration parameters, wherein the configuration parameters include at least one of an upper limit value for acceleration, an upper limit value for deceleration, and an upper limit value for yaw rate.
[0010] According to an embodiment of this application, the method further includes: storing the corrected state information and the target trajectory in the historical storage queue of the target obstacle; and deleting from the historical storage queue of the target obstacle storage entries that have a cache duration greater than a preset upper limit and a time interval between the cache time and the response time greater than a preset time threshold, wherein the cache time represents the time when the data is stored in the historical storage queue.
[0011] The second aspect of this application provides an obstacle trajectory adjustment device, comprising: a response module, configured to, in response to detecting a target obstacle in motion, map the initial obstacle state information of the target obstacle into a spatial coordinate system to obtain target obstacle state information; a first correction module, configured to correct the target obstacle state information based on historical state information to obtain corrected state information, wherein the historical state information characterizes the motion state information of the target obstacle in a historical time period; a prediction module, configured to process the target obstacle state using a trajectory prediction model to obtain an initial estimated trajectory of the target obstacle in a first future time period; and a second correction module, configured to adjust the initial estimated trajectory based on the historical estimated trajectory of the target obstacle in a second future time period to obtain the target trajectory, wherein the historical estimated trajectory is obtained by trajectory reasoning based on the historical state information and the corrected state information, and the first future time period and the second future time period have overlapping future moments.
[0012] A third aspect of this application provides an electronic device, comprising: one or more processors; and a memory for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors perform the methods described above.
[0013] According to the obstacle trajectory adjustment method provided in this application, the target obstacle's state information is corrected using historical state information to obtain corrected state information, thereby eliminating the impact of perception noise and single-frame state jitter on subsequent predictions. A trajectory prediction model is used to predict the trajectory based on the corrected state information to obtain an initial trajectory representing the future movement trend of the target obstacle. Based on historical and corrected state information, a historical predicted trajectory is derived. This historical predicted trajectory carries the historical inertial trend of the obstacle's movement, effectively reducing false braking, planned speed oscillations, and incorrect obstacle avoidance caused by single-frame prediction jitter, thus improving the autonomous driving system's ability to handle dynamic obstacles. The accuracy and reliability of obstacle behavior prediction are improved. By using historical predicted trajectories to fuse and adjust the initial trajectory, a smooth transition of prediction results between adjacent planning cycles is achieved. This reduces trajectory breakpoints or jumps caused by prediction based on obstacle state information in a single frame, significantly improving the consistency and stability of obstacle trajectory prediction in the temporal dimension. It ensures that the initial prediction of the current cycle is consistent with the historical prediction of the previous cycle during overlapping periods, thereby achieving multi-source fusion of current frame trajectory prediction, historical state correction, and cross-cycle historical predicted trajectory. This effectively overcomes the technical problem of the lack of historical continuity caused by a single trajectory prediction model relying solely on the current obstacle state. Attached Figure Description
[0014] The above and other objects, features and advantages of this application will become clearer from the following description of embodiments of this application with reference to the accompanying drawings.
[0015] Figure 1 The diagram illustrates an application scenario of the obstacle trajectory adjustment method, apparatus, and electronic device according to embodiments of this application.
[0016] Figure 2 A flowchart of an obstacle trajectory adjustment method according to an embodiment of this application is shown.
[0017] Figure 3 A flowchart of the target trajectory points according to an embodiment of this application is shown.
[0018] Figure 4 A structural block diagram of an obstacle trajectory adjustment device according to an embodiment of this application is shown.
[0019] Figure 5 A block diagram of an electronic device suitable for implementing an obstacle trajectory adjustment method according to an embodiment of this application is shown. Detailed Implementation
[0020] The embodiments of this application will now be described with reference to the accompanying drawings. However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of this application. In the following detailed description, numerous specific details are set forth to provide a thorough understanding of the embodiments of this application for ease of explanation. However, it will be apparent that one or more embodiments may be implemented without these specific details. Furthermore, descriptions of well-known structures and technologies are omitted in the following description to avoid unnecessarily obscuring the concepts of this application.
[0021] The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of this application. The terms “comprising,” “including,” etc., as used herein indicate the presence of features, steps, operations, and / or components, but do not exclude the presence or addition of one or more other features, steps, operations, or components.
[0022] All terms used herein (including technical and scientific terms) have the meanings commonly understood by those skilled in the art, unless otherwise defined. It should be noted that the terms used herein are to be interpreted in a manner consistent with the context of this specification, and not in an idealized or overly rigid way.
[0023] When using expressions such as "at least one of A, B and C", they should generally be interpreted in accordance with the meaning that is commonly understood by those skilled in the art (e.g., "a system having at least one of A, B and C" should include, but is not limited to, a system having A alone, a system having B alone, a system having C alone, a system having A and B, a system having A and C, a system having B and C, and / or a system having A, B and C, etc.).
[0024] The constant revving and constant acceleration model can generate short-time predicted trajectories using the obstacle's current speed, acceleration, and steering trend, making it suitable for short-time motion prediction of vehicle-type targets. However, directly using single-frame perception states for prediction has the following shortcomings:
[0025] First, the position, velocity, and heading of the sensing output may fluctuate, causing the predicted trajectory to be discontinuous between adjacent frames.
[0026] Secondly, the speed and direction of the obstacle may not be consistent with the heading, and direct prediction will result in an incorrect trajectory.
[0027] Third, the acceleration and yaw rate estimated in a single frame may be unstable, leading to excessive curvature of the predicted trajectory or sudden changes in velocity.
[0028] Fourth, coordinate system transformation errors may cause the current state of an obstacle to be inconsistent with its historical state.
[0029] Fifth, discontinuous predicted trajectories can affect subsequent obstacle decisions and speed planning, causing false braking or planning erratic behavior.
[0030] In view of this, embodiments of this application provide an obstacle trajectory adjustment method. The method includes: in response to detecting a target obstacle in motion, mapping the initial obstacle state information of the target obstacle to a spatial coordinate system to obtain target obstacle state information; correcting the target obstacle state information based on historical state information to obtain corrected state information, where the historical state information characterizes the motion state of the target obstacle in a historical time period; processing the corrected state information using a trajectory prediction model to obtain an initial estimated trajectory of the target obstacle in a first future time period; and adjusting the initial estimated trajectory based on the historical estimated trajectory of the target obstacle in a second future time period to obtain the target trajectory.
[0031] In the technical solution of this application, the user information (including but not limited to user personal information, user image information, user device information, such as location information) and data (including but not limited to data used for analysis, stored data, and displayed data) involved are all information and data authorized by the user or fully authorized by all parties. Furthermore, the collection, storage, use, processing, transmission, provision, disclosure, and application of related data all comply with relevant laws, regulations, and standards, take necessary confidentiality measures, do not violate public order and good morals, and provide corresponding operation entry points for users to choose to authorize or refuse.
[0032] Figure 1 The diagram illustrates an application scenario of the obstacle trajectory adjustment method, apparatus, and electronic device according to embodiments of this application.
[0033] like Figure 1As shown, the application scenario 100 of the obstacle trajectory adjustment method, apparatus, and electronic device according to embodiments of this application includes a terminal device 101, a server 102, and a network 103. The network 103 serves as a medium for providing a communication link between the terminal device 101 and the server 102. The network 103 may include various connection types, such as wired, wireless communication links, or fiber optic cables. The test server 102 may be a server providing test services. The terminal device 101 may be various electronic devices with a display screen and supporting web browsing, including but not limited to smartphones, tablets, laptops, and desktop computers.
[0034] Server 102 can be a server that provides various services, such as a backend management server that supports websites browsed by users using terminal device 101 (for example only). The backend management server can analyze and process data such as received user requests, and feed back the processing results (such as web pages, information, or data obtained or generated according to user requests) to the terminal device.
[0035] It should be noted that the obstacle trajectory adjustment method provided in this application embodiment can generally be executed by the server 102. Accordingly, the obstacle trajectory adjustment device provided in this application embodiment can generally be installed in the server 102.
[0036] It should be understood that Figure 1 The number of terminal devices shown is merely illustrative. Depending on implementation needs, there can be any number of terminal devices and servers.
[0037] It should be noted that the sequence numbers of the operations in the following methods are for descriptive purposes only and should not be considered as indicating the execution order of the operations. Unless explicitly stated otherwise, the method does not need to be executed in the exact order shown.
[0038] Figure 2 A flowchart of an obstacle trajectory adjustment method according to an embodiment of this application is shown.
[0039] like Figure 2 As shown, the method includes operations S210 to S240.
[0040] In operation S210, in response to detecting a target obstacle in the vehicle's movement, the initial obstacle state information of the target obstacle is mapped to a spatial coordinate system to obtain the target obstacle state information.
[0041] In operation S220, the target obstacle status information is corrected based on historical status information to obtain corrected status information.
[0042] In operation S230, the trajectory prediction model is used to process and correct the state information to obtain the initial predicted trajectory of the target obstacle in the first future time period.
[0043] In operation S240, the initial predicted trajectory is adjusted based on the historical predicted trajectory of the target obstacle in the second future time period to obtain the target trajectory.
[0044] Target obstacles refer to dynamic or static objects that are identified and locked by onboard sensors during vehicle operation and require continuous tracking, such as vehicles ahead, pedestrians crossing the road, and bicycles parked on the side of the road.
[0045] Initial obstacle state information is the current measurement data output in the sensor coordinate system at the response time, which may include information such as the position, speed, size, and category of the target obstacle.
[0046] Spatial coordinate system mapping is the process of transforming the initial obstacle state information from the sensor coordinate system or local coordinate system to a unified global coordinate system or vehicle coordinate system through rigid body transformation, external parameter calibration, and coordinate transformation.
[0047] The target obstacle status information is obstacle status data in a unified standard coordinate system after being mapped to a spatial coordinate system, which has the basis for fusion with data from other modules.
[0048] For example, a vehicle is traveling at 10 m / s, and there is a truck 50 meters ahead. The vehicle-mounted LiDAR detects the truck and outputs initial obstacle status information: position (10, 2.0) meters, speed 8 m / s, heading angle 0°. Since the LiDAR is mounted on the front of the vehicle, its coordinate system origin is at the center of the front bumper. The system performs spatial coordinate system mapping, transforming the coordinates to the vehicle coordinate system with the rear axle center as the origin through a pre-calibrated rotation matrix and translation vector, obtaining the target obstacle status information: position (50.5, 2.1) meters, speed 8 m / s, heading angle 0°.
[0049] Historical state information is a sequence of actual states of a target obstacle recorded over multiple consecutive time periods in history, reflecting the recent movement trends and patterns of the obstacle.
[0050] After filtering, smoothing, or correcting anomalies in the target obstacle state information using historical state information, corrected state information is obtained.
[0051] The corrected state information is the state information after eliminating sensor noise, occlusion-induced jumps or drifts.
[0052] The trajectory prediction model can be constructed based on the constant turn rate and constant acceleration model. The trajectory prediction model is used to predict the trajectory of the target obstacle by applying the corrected state information.
[0053] The initial predicted trajectory is the predicted movement path of the target obstacle in the first future time period. The first future time period refers to the predicted time window after the start of the detection time, for example, 0 to 3 seconds after the start of the detection time.
[0054] For example, the system will correct the state information input to the trajectory prediction model. The model receives the corrected state information: position (50.4, 2.05), speed 8.2 m / s, heading 0°, and calculates that the truck will maintain a constant speed in the current lane during the first future time period (0-3 seconds in the future), outputting the initial predicted trajectory: at t+1.0s (58.6, 2.05), at t+2.0s (66.8, 2.05), and at t+3.0s (75.0, 2.05).
[0055] By using historical state information and corrected state information to perform trajectory reasoning for the second future time period, the historical predicted trajectory of the target obstacle in the second future time period is obtained. The historical predicted trajectory represents the system's prediction of the obstacle's future movement trend after integrating historical state information and current corrected information.
[0056] For example, by calculating historical average speed information based on historical state information and correcting the state information including the position information at 4:00, it is possible to infer the predicted trajectory of the target obstacle traveling at a constant speed in the second future time period, thereby obtaining the historical predicted trajectory.
[0057] The second future period is another future time window that overlaps with or connects with the first future period.
[0058] Based on the trajectory points in the historical predicted trajectory, the trajectory points at the same or similar future times in the initial predicted trajectory are smoothed to obtain the target trajectory.
[0059] In the embodiments of this application, the state information of the target obstacle is corrected using historical state information to obtain corrected state information, thereby eliminating the impact of perception noise and single-frame state jitter on subsequent predictions. A trajectory prediction model is used to predict the trajectory based on the corrected state information to obtain an initial trajectory representing the future movement trend of the target obstacle. A historical predicted trajectory is then derived based on the historical and corrected state information. This historical predicted trajectory carries the historical inertial trend of the obstacle's movement, effectively reducing false braking, planned speed oscillations, and incorrect obstacle avoidance caused by single-frame prediction jitter, thus improving the autonomous driving system's ability to predict dynamic obstacle behavior. The accuracy and reliability of the judgment are improved; by using historical predicted trajectories to fuse and adjust the initial trajectory, a smooth transition of prediction results between adjacent planning cycles is achieved, reducing trajectory breakpoints or jumps caused by prediction based on obstacle state information in a single frame. This significantly improves the consistency and stability of obstacle trajectory prediction in the temporal dimension, ensuring that the initial prediction of the current cycle is consistent with the historical prediction of the previous cycle during overlapping periods. This enables multi-source fusion of current frame trajectory prediction, historical state correction, and cross-cycle historical predicted trajectory, effectively overcoming the technical problem of the lack of historical continuity caused by a single trajectory prediction model relying solely on the current obstacle state.
[0060] According to an embodiment of this application, correcting the target obstacle state information based on historical state information to obtain corrected state information includes: determining the state deviation between the historical state information and the target obstacle state information, wherein the state deviation includes multiple sub-state deviations, and the sub-state deviations include at least one of position deviation, heading deviation, and velocity deviation; and, if there is a sub-state deviation greater than a preset deviation threshold, fusing the historical state component in the historical state information corresponding to the sub-state deviation and the target state component in the target obstacle state information corresponding to the sub-state deviation to obtain corrected state information.
[0061] State deviation is a measure of the difference between historical state information and target obstacle state information, used to assess the consistency between current detection values and historical trends.
[0062] Substate deviation is a decomposition of state deviation in a single dimension, including at least one of position deviation, heading deviation, and velocity deviation.
[0063] The preset deviation threshold is a judgment boundary set for each sub-state dimension, used to distinguish between normal fluctuations and abnormal jumps.
[0064] The historical state component is the historical value corresponding to the sub-state in the historical state information, such as historical position, historical heading, historical speed, and other historical values.
[0065] The target state component is the target detection value corresponding to the sub-state in the target obstacle state information. For example, target position, target heading, target velocity, etc.
[0066] The historical state components and target state components are smoothed according to the time interval and kinematic constraints to obtain the fused state components corresponding to the sub-state deviation. Based on the fused state components and the target detection values of the unchanged sub-states, the corrected state information is obtained.
[0067] When the sub-state deviation is greater than the preset deviation threshold corresponding to the sub-state, the historical state components of the sub-state deviation and the target state components can be weighted averaged, Kalman filtered, or fused with confidence to obtain the output result of the fusion action.
[0068] The system can first calculate the position deviation, heading deviation, and speed deviation in parallel; then compare each deviation with its corresponding preset deviation threshold; if the position deviation is greater than the preset position deviation threshold, the historical position and the target position are fused; if the heading deviation is greater than the preset heading deviation threshold, the historical heading and the target heading are fused; if the speed deviation is greater than the preset speed deviation threshold, the historical speed and the target speed are fused; sub-states that do not exceed the limits directly adopt the target state component, and finally combine them to form the corrected state information.
[0069] The corrected state information is the optimized state obtained after fusion processing, which eliminates abnormal jumps or noise in sub-states such as velocity, acceleration, or heading.
[0070] According to embodiments of this application, by comparing the target obstacle state information with historical state information in multiple dimensions, sub-state deviations such as position deviation, heading deviation, and speed deviation are calculated. Sub-state components exceeding preset deviation thresholds are fused to effectively filter out instantaneous sensor noise and single-frame detection jitter, avoiding direct input of noisy data into the trajectory prediction model, thereby significantly reducing the impact of perception noise and single-frame state jitter on the predicted trajectory. Through physical reachability clipping and kinematic constraint smoothing, speed changes, acceleration changes, and heading changes exceeding the vehicle dynamic limits are forcibly constrained to ensure that the corrected state sequence input into the prediction model conforms to the real motion law, thereby avoiding unreasonable speed abrupt changes, acceleration abrupt changes, and heading abrupt changes in the predicted trajectory.
[0071] According to an embodiment of this application, the target obstacle state information includes the target velocity direction and the target heading direction; the correction of the target obstacle state information further includes: when the deviation between the target velocity direction and the target heading direction is greater than a preset angle threshold, correcting the target heading direction according to the target velocity direction to obtain a corrected heading direction, or; correcting the target velocity direction according to the target heading direction to obtain a corrected velocity direction, wherein the correction state information includes the corrected heading direction or the corrected velocity direction.
[0072] The target velocity direction is the orientation of the vehicle's actual driving speed vector, derived from the velocity vector in the target obstacle state information.
[0073] The target heading direction is the obstacle orientation in the target obstacle status information.
[0074] The deviation is calculated based on the angle difference between the target speed direction and the target heading direction. The deviation reflects the degree of difference between the obstacle's travel direction and the vehicle's heading.
[0075] The preset angle threshold is a critical angle value used to determine whether the deviation is abnormal. If the deviation is greater than the preset angle threshold, it means that the angle difference between the target velocity direction and the target heading direction is large, and it is necessary to correct the heading based on the velocity direction or constrain the velocity direction based on the heading.
[0076] In one embodiment, if the deviation is greater than a preset angle threshold, the target heading direction is corrected based on the target velocity direction to obtain the corrected heading direction.
[0077] In another embodiment, if the deviation exceeds a preset angle threshold, the target velocity direction is corrected based on the target heading direction to obtain a corrected velocity direction. The corrected state information is the final state output containing the corrected heading or velocity direction.
[0078] For example, extract the target velocity direction θ1 and the target heading direction θ2, calculate the angle difference Δθ = |θ1 – θ2|, and compare |Δθ| with a preset angle threshold θ_T. If |Δθ| ≤ θ_T, it means the velocity direction and heading are basically consistent and no correction is needed; if |Δθ| > θ_T, it means there is an anomaly and correction is required.
[0079] When the speed measurement source has high accuracy and stable update frequency; the vehicle is in normal driving or sideslip state; and the heading measurement source is drifting or obstructed, the target heading direction is corrected according to the target speed direction, such as in scenarios like high-speed curve sideslip or low-friction road surface drift.
[0080] When the accuracy of the heading measurement source is high and the speed measurement noise is large, the target speed direction is corrected based on the target heading direction. For example, in congested traffic scenarios such as starting from a stop, reversing, and using turn signals while stationary.
[0081] According to an embodiment of this application, when the deviation between the target velocity direction and the target heading direction is greater than a preset angle threshold, a correction strategy is selected according to the scenario to eliminate the contradiction between velocity and heading, and to avoid the trajectory prediction model from outputting divergent trajectories based on incorrect assumptions, thereby significantly reducing prediction distortion caused by abnormal input.
[0082] Figure 3A flowchart of the target trajectory points according to an embodiment of this application is shown.
[0083] like Figure 3 As shown, the process includes operations S310 to S330.
[0084] In operation S310, the first target trajectory point is compared with the second target trajectory point to obtain the trajectory point attribute difference.
[0085] In operation S320, the adjustment weight is determined based on the difference in trajectory point attributes and the preset difference threshold.
[0086] In operation S330, based on the adjusted weights, the first target trajectory point and the second target trajectory point are fused to obtain the target trajectory point, which includes multiple target trajectory points.
[0087] The historical predicted trajectory includes multiple first target trajectory points, and the initial predicted trajectory includes multiple second target trajectory points.
[0088] The trajectory point attribute difference is a measure of the deviation between the first target trajectory point and the second target trajectory point in terms of attributes such as position, velocity, and heading. The preset difference threshold is a critical value for determining whether the deviation is abnormal.
[0089] The trajectory points of the first and second targets are compared item by item at the same future time, and the differences in trajectory point attributes are calculated. The Euclidean distance function can be used to calculate the positional difference; the absolute difference function can be used to calculate the speed difference or heading difference.
[0090] If the difference in trajectory point attributes is less than or equal to a preset difference threshold, it means that the historical predicted trajectory is similar to the initial predicted trajectory. The weight of the historical predicted trajectory is adjusted to be larger, for example, a weight of 0.7, to enhance continuity.
[0091] If the difference in trajectory point attributes exceeds a preset difference threshold, it indicates a change in the obstacle's movement intention. The weight of the initially estimated trajectory is increased to quickly respond to the new change. Based on this weight adjustment, the first and second target trajectory points are smoothed to obtain the target trajectory points. The operation described in the book is repeated for trajectory points at all overlapping moments to stitch together the complete target trajectory.
[0092] For example, the obstacle detection time is 10:00:00, the first future time period is from 10:00:00 to 10:00:03, and the second future time period is from 10:00:00 to 10:00:02. At the overlapping future time t=10:00:01: the position of the first target trajectory point is (60, 2.0), and the speed is 12 m / s; the position of the second target trajectory point is (58, 2.0), and the speed is 10 m / s. The trajectory point attribute difference is calculated. The position difference is 2 meters, and the speed difference is 2 m / s. The preset difference thresholds are: position threshold 3 meters, and speed threshold 4 m / s. Since 2 meters is less than 3 meters and 2 m / s is less than 4 m / s, the trajectory point attribute difference is less than the preset difference threshold. Therefore, the historical trend trajectory is trusted, and the weight of the historical predicted trajectory is increased. The position of the target trajectory point obtained by fusion is: 0.6×58 + 0.4×60 = 58.8 meters; the speed is: 0.6×10 + 0.4×12 = 10.8 m / s.
[0093] According to embodiments of this application, when the obstacle's movement is stable, the prediction connection between adjacent planning cycles is effectively smoothed by fusing historical predicted trajectories with high weights, reducing trajectory jitter caused by single-frame prediction noise, thereby improving the stability of obstacle decision-making, speed planning, and lane change safety judgment. When the actual movement of the obstacle changes, the predicted trajectory can quickly respond to sudden changes by dynamically reducing historical weights and increasing initial prediction weights, avoiding excessive lag. At the same time, by suppressing prediction jitter, the system reduces false braking, planned speed oscillations, or incorrect obstacle avoidance operations triggered by misjudging obstacle behavior, thereby improving the overall safety and ride comfort of the autonomous driving system.
[0094] According to an embodiment of this application, the corrected state information includes the corrected position of the target obstacle; wherein, trajectory reasoning based on historical state information and corrected state information to obtain a historical predicted trajectory includes: performing statistical processing on the historical state information to obtain historical motion trend information, which includes historical average speed, historical statistical acceleration, and historical statistical heading change rate; using the historical statistical heading change rate, historical statistical acceleration, historical average speed, and the time interval between the future time and the response time in the second future time period, trajectory reasoning is performed on the corrected position to obtain a historical predicted trajectory, wherein the state information of the first target trajectory point in the historical predicted trajectory includes the reasoned position, reasoned acceleration, reasoned speed, and reasoned heading change rate.
[0095] Historical state information is aggregated and calculated within a time window to obtain historical motion trend information, including historical average speed, historical statistical acceleration, and historical statistical rate of change of heading. For example, the historical average speed is obtained by averaging the velocity vectors at each historical moment; the historical statistical acceleration is obtained by averaging the accelerations at each historical moment or by performing least-squares fitting; and the historical statistical rate of change of heading is obtained by averaging the rates of change of heading at each adjacent moment.
[0096] Historical average speed reflects the overall speed and direction of motion; historical statistical acceleration reflects the overall acceleration and deceleration trend; historical statistical rate of change of heading reflects the overall turning trend.
[0097] The future moment is a discrete point in time within the second future time period. The response moment is the current detection moment, which is the time reference point that triggers trajectory inference.
[0098] For example, starting from the corrected position, using the historical average velocity as the initial velocity, the historical statistical acceleration as the uniform acceleration, and the historical statistical rate of change of heading as the uniform angular velocity, and combining the time interval between the future time and the response time, the inferred position, inferred acceleration, inferred velocity, and inferred rate of change of heading are obtained by recursion according to the uniformly accelerated circular motion model.
[0099] In one embodiment, the reasoning position p^(hist)(t1) at time t1 is obtained based on uniform reasoning as shown in formula (1):
[0100] p^(hist)(t1) = p_k + v (trend) × (t1-t0)t(1).
[0101] Where p_k represents the correction position, v (trend) represents the historical average velocity, and t0 represents the response time.
[0102] In another embodiment, the historical predicted trajectory is obtained based on a non-uniform velocity reasoning method. The reasoning velocity is obtained based on the historical average velocity, historical statistical acceleration, and time interval; the reasoning heading is obtained based on the corrected heading of the target obstacle, the historical statistical heading change rate, and time interval; the reasoning position is obtained based on the corrected position, the historical average velocity time interval, and the historical statistical acceleration. When the heading changes, the velocity direction is decomposed according to the current heading, thereby obtaining the state information of each first target trajectory point on the historical predicted trajectory.
[0103] According to the embodiments of this application, historical motion trends are extracted through statistical processing, avoiding random jitter caused by direct extrapolation of instantaneous states, making the historical predicted trajectory more consistent with the inertial motion law of obstacles; trajectory reasoning is performed using historical statistical heading change rate, historical statistical acceleration and historical average velocity, simplifying the complex obstacle motion into a parameterizable trend model, reducing computational complexity.
[0104] According to an embodiment of this application, it further includes: constraining the target trajectory using configuration parameters, wherein the configuration parameters include at least one of an upper limit value for acceleration, an upper limit value for deceleration, and an upper limit value for yaw rate.
[0105] The upper limit for acceleration is the maximum acceleration, the upper limit for deceleration is the maximum deceleration, and the upper limit for yaw rate is the maximum yaw rate.
[0106] Configuration parameters may include historical cache duration, maximum historical weight, maximum acceleration, maximum deceleration, and maximum yaw rate.
[0107] If the acceleration exceeds the maximum acceleration, the velocity and position of the target trajectory points in the target trajectory will be limited according to the maximum acceleration.
[0108] If the deceleration exceeds the maximum deceleration, the speed and position of the target trajectory points in the target trajectory will be limited according to the maximum deceleration.
[0109] If the yaw rate exceeds the maximum yaw rate, the heading change of the target trajectory points in the target trajectory is restricted, and the position is corrected accordingly.
[0110] If the time interval is too short or too long, the historical point is discarded or its correction weight is reduced.
[0111] According to an embodiment of this application, the method further includes: storing the corrected state information and the target trajectory in the historical storage queue of the target obstacle; and deleting from the historical storage queue of the target obstacle storage entries that have a cache duration greater than a preset upper limit and a time interval between the cache time and the response time greater than a preset time threshold, wherein the cache time represents the time when the data is stored in the historical storage queue.
[0112] The system maintains a historical storage queue for each obstacle. Each historical storage queue includes the obstacle number, timestamp, position, speed, heading, and historical predicted trajectory.
[0113] The historical storage queue is a circular cache data structure maintained for each target obstacle. It stores the past state and trajectory information of the obstacle in chronological order, supporting first-in-first-out (FIFO) or policy-based eviction. During each planning cycle, old states exceeding the maximum historical duration are deleted, retaining only reliable historical states close to the current frame time.
[0114] A storage entry is a single data unit in the historical storage queue, containing correction status information or target trajectory and its corresponding cache time.
[0115] The cache time represents the specific point in time when the storage entry is written to the historical storage queue, and is used to calculate data timeliness and trigger eviction.
[0116] The preset duration limit is the maximum time an entry is allowed to exist in the historical storage queue. Entries exceeding this limit are considered expired and must be deleted. The preset time threshold is a critical value used to identify and delete redundant expired historical data, determining whether the time interval between an entry and the current response time is too short.
[0117] Cache duration is the length of time an entry has existed from the time it was cached until the current response time.
[0118] At each response time, the corrected status information and target trajectory are encapsulated as storage entries, with the current response time being the cache time, and are appended to the tail of the historical storage queue of the target obstacle.
[0119] At each response time, the historical storage queue of the target obstacle is traversed, and a dual judgment is performed on each storage entry. If the cache duration exceeds the preset upper limit and the time interval exceeds the preset time threshold, the storage entry is deleted. The historical storage queue maintains a finite length through a dynamic balance between storing new entries and deleting old entries, ensuring controllable memory and data timeliness.
[0120] Figure 4 A structural block diagram of an obstacle trajectory adjustment device according to an embodiment of this application is shown.
[0121] like Figure 4 As shown, the obstacle trajectory adjustment device 400 of this embodiment includes a response module 410, a first correction module 420, a prediction module 430, and a second correction module 440.
[0122] The response module 410 is used to map the initial obstacle state information of the target obstacle into a spatial coordinate system in response to the detection of a target obstacle in the vehicle's movement, thereby obtaining the target obstacle state information.
[0123] The first correction module 420 is used to correct the target obstacle state information based on historical state information to obtain corrected state information. The historical state information represents the motion state information of the target obstacle in a historical period.
[0124] The prediction module 430 is used to process the state of the target obstacle using a trajectory prediction model to obtain the initial estimated trajectory of the target obstacle in the first future time period.
[0125] The second correction module 440 is used to adjust the initial predicted trajectory based on the historical predicted trajectory of the target obstacle in the second future time period to obtain the target trajectory. The historical predicted trajectory is obtained by trajectory reasoning based on historical state information and corrected state information. The first future time period and the second future time period have overlapping future moments.
[0126] According to an embodiment of this application, the second correction module 440 includes a comparison submodule, a determination submodule, and a fusion submodule.
[0127] The comparison submodule is used to compare the first target trajectory point with the second target trajectory point to obtain the difference in trajectory point attributes.
[0128] The determination submodule is used to determine the adjustment weights based on the difference in trajectory point attributes and the preset difference threshold.
[0129] The fusion submodule is used to fuse the first target trajectory point and the second target trajectory point based on the adjusted weights to obtain the target trajectory point, which includes multiple target trajectory points.
[0130] According to an embodiment of this application, the second correction module 440 further includes a processing submodule and an inference submodule.
[0131] The processing submodule is used to perform statistical processing on historical state information to obtain historical motion trend information, which includes historical average speed, historical statistical acceleration, and historical statistical rate of change of heading.
[0132] The inference submodule is used to perform trajectory inference on the corrected position using historical statistical heading change rate, historical statistical acceleration, historical average speed, and the time interval between the future time and the response time in the second future time period, to obtain the historical predicted trajectory. The state information of the first target trajectory point in the historical predicted trajectory includes the inference position, inference acceleration, inference speed, and inference heading change rate.
[0133] According to an embodiment of this application, the first correction module 420 includes a first correction submodule and a second correction submodule.
[0134] The first correction submodule is used to determine the state deviation between historical state information and target obstacle state information. The state deviation includes multiple sub-state deviations, which include at least one of position deviation, heading deviation, and speed deviation.
[0135] The second correction submodule is used to fuse the historical state component corresponding to the sub-state deviation in the historical state information and the target state component corresponding to the sub-state deviation in the target obstacle state information to obtain corrected state information when there is a sub-state deviation greater than a preset deviation threshold.
[0136] According to an embodiment of this application, the first correction module 420 further includes a third correction submodule and a fourth correction submodule.
[0137] The third correction submodule is used to correct the target heading direction based on the target speed direction when the deviation between the target speed direction and the target heading direction is greater than a preset angle threshold, so as to obtain the corrected heading direction.
[0138] The fourth correction submodule is used to correct the target velocity direction based on the target heading direction to obtain the corrected velocity direction. The correction status information includes the corrected heading direction or the corrected velocity direction.
[0139] According to an embodiment of this application, the obstacle trajectory adjustment device 400 further includes a storage module and a deletion module.
[0140] The storage module is used to store the corrected status information and target trajectory into the historical storage queue of the target obstacle.
[0141] The deletion module is used to delete storage entries from the historical storage queue of the target obstacle whose cache duration is greater than the preset maximum duration and whose time interval between the cache time and the response time is greater than the preset time threshold. The cache time represents the time when the entry was stored in the historical storage queue.
[0142] Optionally, any plurality of modules among the response module 410, the first correction module 420, the prediction module 430, and the second correction module 440 may be combined into one module, or any one of these modules may be split into multiple modules. Alternatively, at least part of the functionality of one or more of these modules may be combined with at least part of the functionality of other modules and implemented in one module. Optionally, at least one of the response module 410, the first correction module 420, the prediction module 430, and the second correction module 440 may be at least partially implemented as hardware circuitry, such as a field-programmable gate array (FPGA), a programmable logic array (PLA), a system-on-a-chip, a system-on-a-substrate, a system-on-package, an application-specific integrated circuit (ASIC), or any other reasonable means of integrating or packaging circuitry, or implemented in software, hardware, or firmware, or in any appropriate combination of any of these three implementation methods. Alternatively, at least one of the response module 410, the first correction module 420, the prediction module 430, and the second correction module 440 may be at least partially implemented as a computer program module, which, when run, can perform the corresponding function.
[0143] Figure 5 A block diagram of an electronic device suitable for implementing an obstacle trajectory adjustment method according to an embodiment of this application is shown.
[0144] Figure 5 The electronic device shown is merely an example and should not impose any limitation on the functionality and scope of use of the embodiments of this application.
[0145] like Figure 5 As shown, an electronic device 500 according to an embodiment of this application includes a processor 501, which can perform various appropriate actions and processes according to a program stored in ROM 502 (Read-Only Memory) or a program loaded from storage portion 508 into RAM 503 (Random Access Memory). The processor 501 may include, for example, a general-purpose microprocessor (e.g., a CPU), an instruction set processor and / or an associated chipset and / or a special-purpose microprocessor (e.g., an application-specific integrated circuit (ASIC)), etc. The processor 501 may also include onboard memory for caching purposes. The processor 501 may include a single processing unit or multiple processing units for performing different actions of the method flow according to an embodiment of this application.
[0146] RAM 503 stores various programs and data required for the operation of electronic device 500. Processor 501, ROM 502, and RAM 503 are interconnected via bus 504. Processor 501 executes various operations of the method flow according to embodiments of this application by executing programs in ROM 502 and / or RAM 503. It should be noted that programs may also be stored in one or more memories other than ROM 502 and RAM 503. Processor 501 may also execute various operations of the method flow according to embodiments of this application by executing programs stored in one or more memories.
[0147] In embodiments of this application, the electronic device 500 may further include an input / output (I / O) interface 505, which is also connected to the bus 504. The electronic device 500 may also include one or more of the following components connected to the input / output (I / O) interface 505: an input section 506 including a keyboard, mouse, etc.; an output section 507 including a cathode ray tube (CRT), liquid crystal display (LCD), etc., and a speaker, etc.; a storage section 508 including a hard disk, etc.; and a communication section 509 including a network interface card such as a LAN card, modem, etc. The communication section 509 performs communication processing via a network such as the Internet. A drive 510 is also connected to the input / output (I / O) interface 505 as needed. A removable medium 511, such as a disk, optical disk, magneto-optical disk, semiconductor memory, etc., is installed on the drive 510 as needed so that computer programs read from it can be installed into the storage section 508 as needed.
[0148] The embodiments of this application, and the method flow according to the embodiments of this application, can be implemented as a computer software program. For example, an embodiment of this application includes a computer program product comprising a computer program carried on a computer-readable storage medium, the computer program containing program code for performing the methods shown in the flowchart. In such an embodiment, the computer program can be downloaded and installed from a network via communication section 509, and / or installed from removable medium 511. When the computer program is executed by processor 501, it performs the functions defined in the system of the embodiments of this application. In the embodiments of this application, the systems, devices, apparatuses, modules, units, etc., described above can be implemented by computer program modules.
[0149] This application also provides a computer-readable storage medium, which may be included in the device / apparatus / system described in the above embodiments; or it may exist independently and not assembled into the device / apparatus / system. The computer-readable storage medium carries one or more programs, which, when executed, implement the obstacle trajectory adjustment method according to the embodiments of this application.
[0150] In embodiments of this application, the computer-readable storage medium can be a non-volatile computer-readable storage medium. Examples include, but are not limited to: portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination thereof. In this application, the computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.
[0151] For example, in embodiments of this application, a computer-readable storage medium may include the ROM 502 and / or RAM 503 described above and / or one or more memories other than ROM 502 and RAM 503.
[0152] Embodiments of this application also include a computer program product, which includes a computer program containing program code for performing the methods provided in the embodiments of this application. When the computer program product is run on an electronic device, the program code is used to enable the electronic device to implement the obstacle trajectory adjustment method provided in the embodiments of this application.
[0153] When the computer program is executed by the processor 501, it performs the functions defined in the system / apparatus of this application embodiment. In the embodiments of this application, the systems, apparatuses, modules, units, etc., described above can be implemented by computer program modules.
[0154] In one embodiment, the computer program may rely on a tangible storage medium such as an optical storage device or a magnetic storage device. In another embodiment, the computer program may also be transmitted and distributed in the form of signals over a network medium, and may be downloaded and installed via the communication section 509, and / or installed from a removable medium 511. The program code contained in the computer program can be transmitted using any suitable network medium, including but not limited to: wireless, wired, etc., or any suitable combination thereof.
[0155] In embodiments of this application, program code for executing the computer programs provided in the embodiments of this application can be written in any combination of one or more programming languages. Specifically, these computational programs can be implemented using high-level procedural and / or object-oriented programming languages, and / or assembly / machine languages. Programming languages include, but are not limited to, languages such as Java, C++, Python, "C", or similar programming languages. The program code can be executed entirely on the user's computing device, partially on the user's device, partially on a remote computing device, or entirely on a remote computing device or server. In cases involving remote computing devices, the remote computing device can be connected to the user's computing device via any type of network, including a local area network (LAN) or a wide area network (WAN), or it can be connected to an external computing device (e.g., via the Internet using an Internet service provider).
[0156] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram or flowchart, and combinations of blocks in a block diagram or flowchart, may be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions. Those skilled in the art will understand that the features described in the various embodiments of this application can be combined and / or combined in various ways, even if such combinations are not explicitly described in this application. In particular, without departing from the spirit and teachings of this application, the features described in the various embodiments of this application can be combined and / or combined in various ways. All such combinations and / or combinations fall within the scope of this application.
[0157] The embodiments of this application have been described above. However, these embodiments are merely illustrative and not intended to limit the scope of this application. Although various embodiments have been described above, this does not mean that the measures in the various embodiments cannot be used advantageously in combination. Without departing from the scope of this application, those skilled in the art can make various substitutions and modifications, all of which should fall within the scope of this application.
Claims
1. A method for adjusting the trajectory of an obstacle, characterized in that, The method includes: In response to detecting a target obstacle in the vehicle's motion, the initial obstacle state information of the target obstacle is mapped to a spatial coordinate system to obtain the target obstacle state information; The target obstacle's state information is corrected based on historical state information to obtain corrected state information, whereby the historical state information represents the target obstacle's motion state during a historical period. The corrected state information is processed using a trajectory prediction model to obtain the initial estimated trajectory of the target obstacle in the first future time period; Based on the historical predicted trajectory of the target obstacle in the second future time period, the initial predicted trajectory is adjusted to obtain the target trajectory. The historical predicted trajectory is obtained by trajectory reasoning based on the historical state information and the corrected state information. The first future time period and the second future time period have overlapping future moments.
2. The method according to claim 1, characterized in that, The historical predicted trajectory includes multiple first target trajectory points, and the initial predicted trajectory includes multiple second target trajectory points; The step of adjusting the initial estimated trajectory based on the historical estimated trajectory of the target obstacle in a second future time period to obtain the target trajectory includes: The first target trajectory point is compared with the second target trajectory point to obtain the trajectory point attribute difference. The adjustment weight is determined based on the difference in the trajectory point attributes and the preset difference threshold. Based on the adjusted weights, the first target trajectory point and the second target trajectory point are fused to obtain a target trajectory point, wherein the target trajectory includes multiple target trajectory points.
3. The method according to claim 2, characterized in that, The corrected status information includes the corrected position of the target obstacle; The process of trajectory reasoning based on the historical state information and the corrected state information to obtain the historical predicted trajectory includes: The historical state information is statistically processed to obtain historical motion trend information, which includes historical average speed, historical statistical acceleration, and historical statistical rate of change of heading. Using the historical statistical heading change rate, the historical statistical acceleration, the historical average speed, and the time interval between the future time and the response time in the second future time period, trajectory reasoning is performed on the corrected position to obtain the historical predicted trajectory. The state information of the first target trajectory point in the historical predicted trajectory includes the reasoned position, reasoned acceleration, reasoned speed, and reasoned heading change rate.
4. The method according to any one of claims 1 to 3, characterized in that, The step of correcting the target obstacle state information based on historical state information to obtain corrected state information includes: Determine the state deviation between the historical state information and the target obstacle state information. The state deviation includes multiple sub-state deviations, and the sub-state deviation includes at least one of position deviation, heading deviation, and velocity deviation. If the sub-state deviation is greater than a preset deviation threshold, the historical state component corresponding to the sub-state deviation in the historical state information and the target state component corresponding to the sub-state deviation in the target obstacle state information are fused to obtain corrected state information.
5. The method according to claim 4, characterized in that, The target obstacle state information includes the target velocity direction and the target heading direction; the correction of the target obstacle state information further includes: If the deviation between the target velocity direction and the target heading direction is greater than a preset angle threshold, the target heading direction is corrected according to the target velocity direction to obtain the corrected heading direction, or; The target velocity direction is corrected based on the target heading direction to obtain the corrected velocity direction. The corrected state information includes either the corrected heading direction or the corrected velocity direction.
6. The method according to claim 1 or 2, characterized in that, The method further includes: The target trajectory is constrained using configuration parameters, which include at least one of an upper limit value for acceleration, an upper limit value for deceleration, and an upper limit value for yaw rate.
7. The method according to claim 1 or 2, characterized in that, The method further includes: The corrected state information and the target trajectory are stored in the historical storage queue of the target obstacle; and From the historical storage queue of the target obstacle, delete storage entries whose cache duration is greater than a preset upper limit and whose time interval between the cache time and the response time is greater than a preset time threshold, where the cache time represents the time when the entry was stored in the historical storage queue.
8. An obstacle trajectory adjustment device, characterized in that, The device includes: The response module is used to map the initial obstacle state information of the target obstacle into a spatial coordinate system in response to the detection of a target obstacle in the vehicle's movement, so as to obtain the target obstacle state information. The first correction module is used to correct the target obstacle state information based on historical state information to obtain corrected state information, wherein the historical state information represents the motion state information of the target obstacle in a historical period. The prediction module is used to process the state of the target obstacle using a trajectory prediction model to obtain the initial estimated trajectory of the target obstacle in the first future time period. The second correction module is used to adjust the initial predicted trajectory based on the historical predicted trajectory of the target obstacle in the second future time period to obtain the target trajectory. The historical predicted trajectory is obtained by trajectory reasoning based on the historical state information and the corrected state information. The first future time period and the second future time period have overlapping future moments.
9. An electronic device, comprising: One or more processors; Memory, used to store one or more computer programs. The characteristic feature is that the one or more processors execute the one or more computer programs to implement the steps of the method according to any one of claims 1 to 7.
10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 7.