Vehicle obstacle avoidance trajectory planning method and system

Abstract

The invention relates to a vehicle obstacle avoidance trajectory planning method which comprises the following steps: in response to a trigger condition, planning an initial trajectory of a vehicle from a current pose to a target pose based on a vehicle kinematic model; the planning process of the initial trajectory comprises collision detection based on environment obstacle information sensed in real time, and the environment obstacle information comprises static obstacles and dynamic obstacles; and by taking the initial trajectory as an optimization initial value, performing trajectory optimization based on a preset constraint condition to obtain an optimized trajectory. Forward movement and backward movement generated by positive acceleration control and negative acceleration control are considered at the same time in the trajectory planning process, so that candidate movement strategies of the vehicle are multiplied, the vehicle can handle forward and lateral obstacles and can effectively cope with dangerous scenes such as rapid approaching of rear vehicles, and the vehicle driving safety is improved. The limitation that only forward obstacles are processed in the prior art is overcome, and the application scene and safety of the automatic driving function are improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of vehicle obstacle avoidance, in particular to a vehicle obstacle avoidance trajectory planning method and system. BACKGROUND

[0002] In recent years, with the continuous popularity of automobiles and the continuous development of electronic control technology, the market demand for automobile safety systems is increasing, and active safety technology, especially active collision avoidance technology, plays an increasingly important role in reducing traffic accidents. The current obstacle avoidance scheme needs to obtain prior information such as drivable lane, and the obstacle avoidance scheme is greatly limited for the part of the road segment where the prior information is missing. Moreover, most of the schemes are based on laser ranging modules to process forward obstacles, and there is a lack of corresponding unified processing scheme for rearward rapidly approaching vehicles and other obstacles. In the process of processing obstacles, the trajectory needs to be planned in real time, and for relatively complex solving scenarios, a lot of time is spent in the planning module, thereby affecting the subsequent decision control module and reducing comfort while greatly reducing safety.

[0003] For example, a vehicle low-speed automatic driving collision avoidance method and system disclosed in Chinese patent CN107145147A detects obstacles on the planned driving path, judges the collision risk, and finally selects steering avoidance or active braking. Although this scheme is effective in specific scenarios, the core architecture still has the above-mentioned defects: it mainly processes obstacles on the planned path in front of the vehicle, and its entire decision logic (including collision risk judgment and avoidance strategy selection) is designed for forward driving scenarios. For vehicles that cut in sideways, especially for dynamic obstacles such as vehicles that rapidly approach from the rear, this scheme lacks sensing and processing capabilities, forming a significant safety blind area. Moreover, it performs avoidance path planning or braking decision in real time after detecting obstacles. This highly dependent real-time calculation method will occupy a large amount of computing resources in complex scenarios, causing system decision delay, compressing the execution window of the subsequent processing module, and thus reducing the overall safety margin of the system. Therefore, there is an urgent need in the art for a vehicle obstacle avoidance trajectory planning scheme that can uniformly process obstacles in multiple directions (forward, rearward, and sideways) and greatly improve the real-time response capability and decision reliability of the system through a combination of pre-planning and real-time optimization to solve the above-mentioned defects. SUMMARY

[0004] The present application provides a vehicle obstacle avoidance trajectory planning method and system to solve the above problems.

[0005] In a first aspect, the present application provides a vehicle obstacle avoidance trajectory planning method, comprising the following steps: In response to a triggering condition, based on a vehicle kinematics model, an initial trajectory of the vehicle from its current pose to a target pose is planned; the initial trajectory planning process includes collision detection based on real-time perceived environmental obstacle information, which includes static obstacles and dynamic obstacles. Using the initial trajectory as the initial value for optimization, trajectory optimization is performed based on preset constraints to obtain the optimized trajectory; The optimized trajectory is output to the vehicle control unit to control the vehicle to perform tracking.

[0006] In conjunction with the first aspect, in one implementation, the step of planning the initial trajectory from the current pose to the target pose based on the vehicle kinematics model specifically includes: A bicycle kinematic model of the vehicle is established, and the bicycle kinematic model is discretized based on a preset time domain and a preset time step. Based on a discretized bicycle kinematics model, a forward exploration algorithm is used for trajectory search. During the expansion process of each node, collision detection is performed on candidate state nodes based on real-time perceived environmental obstacle information. Based on the evaluation function, the candidate state nodes that have passed the collision detection are selected for iterative expansion until the target pose is reached.

[0007] In conjunction with the first aspect, in one implementation, the collision detection of candidate state nodes specifically includes: The vehicle profile is calculated based on the pose information of the candidate state nodes. The vehicle outline is compared with the obstacle-occupied area determined based on real-time perceived environmental obstacle information, the obstacle-occupied area including static obstacle-occupied area and dynamic obstacle-occupied area. For areas occupied by static obstacles, determine whether the vehicle outline spatially overlaps with the area it occupies; For the area occupied by a dynamic obstacle, it is determined whether the vehicle outline spatially overlaps with the area it occupies within the preset time step, based on its current motion state. If any of the above spatial overlaps exist, the collision detection fails and the node is discarded.

[0008] In conjunction with the first aspect, in one implementation, the trajectory optimization based on preset constraints includes safety corridor constraints, which specifically include: An initial safe zone is formed with the trajectory points along the initial trajectory centered on the smallest bounding rectangle of the vehicle profile. based on the real-time perceived environmental obstacle information, expanding the boundary of the initial safety area outward until the distance between the safety corridor and the obstacle-occupied area determined based on the real-time perceived environmental obstacle information is equal to a first preset safety threshold, thereby forming the safety corridor; The trajectory points of the constrained optimization trajectory are located within the safety corridor.

[0009] In combination with the first aspect, in an implementation, the trajectory optimization based on the preset constraint conditions further includes a smoothing constraint, and the smoothing constraint includes a weighted sum of at least one of the following cost terms: a deviation degree cost from the initial trajectory; a first-order derivative term cost for promoting motion smoothness; a second-order derivative term cost for promoting ride comfort; a third-order derivative term cost for suppressing impact degree.

[0010] In combination with the first aspect, in an implementation, the trajectory optimization based on the preset constraint conditions further includes: a start pose constraint that the start point state of the trajectory is consistent with the current state of the vehicle; an end pose constraint that the end point state of the trajectory is consistent with a preset target state.

[0011] In combination with the first aspect, in an implementation, the trajectory optimization based on the preset constraint conditions further includes vehicle physical constraints, and the vehicle physical constraints include a longitudinal speed constraint, a longitudinal acceleration constraint, a steering angle constraint, and a steering angle change rate constraint.

[0012] In combination with the first aspect, in an implementation, the triggering condition includes at least one of the following: when the vehicle travels along the optimized trajectory, the distance between the real-time perceived environmental obstacle information and the vehicle is located in a preset safety interval within a preset time; a new instruction is received from a vehicle decision system or a human-computer interaction system.

[0013] In the second aspect, an embodiment of the present application provides a system based on a vehicle obstacle avoidance trajectory planning method, including: a data acquisition module configured to acquire a current pose and a target pose of a vehicle, and to acquire real-time environmental obstacle information; a control module connected with the data acquisition module, configured to, in response to a trigger condition, plan an initial trajectory of the vehicle from a current pose to a target pose based on a vehicle kinematics model, generate a safety corridor centered on the initial trajectory based on real-time perception of environmental obstacle information, perform trajectory optimization with the initial trajectory as an optimization initial value and with a constraint condition that trajectory points are located within the safety corridor to obtain an optimized trajectory, and output the optimized trajectory to a vehicle control unit; a control module connected with the processing module, configured to control the vehicle to track and execute.

[0014] In a third aspect, the embodiments of the present application provide a computer readable storage medium having a computer program stored thereon, the computer program being executed by a processor to implement the steps of the vehicle obstacle avoidance trajectory planning method described above.

[0015] The technical scheme provided by the embodiments of the present application has the following beneficial effects: 1. By introducing omnidirectional collision detection containing static and dynamic obstacles in the trajectory planning stage, the vehicle can cope with forward, lateral and rear obstacles approaching quickly, overcoming the limitation of the prior art which usually only deals with forward obstacles, fundamentally eliminating the safety blind area, and significantly improving the applicable scenarios and safety of the automatic driving function.

[0016] 2. By reconstructing the traditional perception-planning-decision-control process into planning-perception-optimization-decision-control, the vehicle can pre-plan the trajectory before driving, and only needs to quickly optimize the pre-planned trajectory according to real-time perception data during driving, which significantly reduces the calculation resource occupation and time delay in driving, leaves more time for the processing module, and thus greatly improves the response speed and driving safety of the vehicle in complex scenarios.

[0017] 3. The initial trajectory of the present application is generated based on the vehicle kinematics model, and the optimization process introduces smoothing constraints, so that the final output optimized trajectory not only can be accurately tracked by the vehicle, but also can effectively avoid the impact caused by sudden acceleration, deceleration and turning, thereby greatly improving the comfort experience of the passengers while ensuring obstacle avoidance safety. BRIEF DESCRIPTION OF DRAWINGS

[0018] In order to more clearly illustrate the technical schemes in the embodiments of the present application, the following will briefly introduce the drawings needed in the embodiment description. 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.

[0019] Figure 1 The main steps of the present application are shown in the schematic diagram. Figure 2 A flowchart is generated for the safety corridor of the application. DETAILED DESCRIPTION

[0020] In order for those skilled in the art to better understand the scheme of the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in combination with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than 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.

[0021] Embodiment one: Please refer to Figure 1 The embodiment of the present application provides a vehicle obstacle avoidance trajectory planning method, which comprises the following steps: S1, in response to a trigger condition, an initial trajectory of a vehicle from a current pose to a target pose is planned based on a vehicle kinematics model; collision detection based on real-time perception of environmental obstacle information is included in the planning process of the initial trajectory, and the environmental obstacle information includes static obstacles and dynamic obstacles; Specifically: S101, specific implementation of the vehicle kinematics model: A classic bicycle model is used for kinematic modeling of the vehicle, which simplifies a four-wheel vehicle into two front and rear wheels, and further simplifies the functions of the front and rear wheels into a virtual wheel located on the vehicle axis, and the continuous time equation is: ; ; ; ; ; Wherein, x, y are global coordinates representing the mass center of the vehicle, θ is the heading angle of the vehicle, L is the wheelbase of the vehicle, is the longitudinal velocity of the vehicle, a is the longitudinal acceleration of the vehicle, δ is the front wheel steering angle, is the rate of change of the front wheel steering angle; In order to make trajectory prediction, the continuous time equation needs to be discretized, and the Euler method is used in this embodiment, assuming that the discrete time step is Δt, and the state at the first time is: , and the control input is ; Discretization of the vehicle kinematics model can be obtained as follows: ; ; ; ; ; S102, Plan the initial trajectory: The initial state of the vehicle As the starting node for planning, the target pose node is set. This can be a precise pose or a region within a tolerance range (such as a target parking space), with a set time step of [value missing]. The prediction time domain is By systematically sampling different control inputs To generate the next candidate state node by expanding from the current state node. ; For each set of sampled control inputs Substituting this into the vehicle kinematics model in step S101, the calculation is obtained at the time step. The subsequent candidate state nodes ; Among them, control input Longitudinal acceleration in the sequence and the rate of change of the front wheel steering angle It can be a positive or negative value; >0, generated candidate state section Corresponding to forward acceleration state; <0, generated candidate state nodes Corresponding to forward deceleration state; The vehicle's heading angle θ and front wheel steering angle δ are controlled by δf, which determines the vehicle's turning direction. Therefore, a candidate state node... It can simultaneously include multiple candidate child nodes such as forward left turn, forward straight, forward right turn, decelerate left turn, decelerate straight, and decelerate right turn.

[0022] S103, Collision Detection: For each candidate state node Collision detection is performed based on its pose information ( ) Calculate the vehicle contour polygon in the global coordinate system at this time, obtain, fuse and generate real-time obstacle information in the global coordinate system through the vehicle real-time perception module (laser radar, camera, millimeter wave radar), and the information includes the current state of all detected obstacles (including static obstacles, and dynamic obstacles in front, side and rear) in the global coordinate system, and the current state includes position, speed and heading angle; for static obstacles, directly detect whether the vehicle contour polygon overlaps with the occupied area; and for dynamic obstacles, based on the current speed and heading angle, judge the occupied area extending in the current motion direction within the time step, detect whether the vehicle contour polygon overlaps with the occupied area, if there is any overlap, it is determined that there is a collision risk, and the candidate state node is discarded immediately; otherwise, it is retained.

[0023] S104, node selection and iterative exploration: For the retained candidate state node, the evaluation function is used to select the optimal node from the detected candidate state nodes for iterative expansion until the target pose node is reached; Specifically, in the embodiment, the evaluation function is used to quantify the closeness of the candidate state node to the target pose node , and its expression can be designed as: ; Wherein, represents the Euclidean distance between the candidate state node and the target pose node ; represents the difference between the heading angle of the candidate state node and the target pose node ; represents the difference between the speed of the candidate state node and the target pose node ; A, B and C are weight coefficients, which are used to balance the importance of each item in the evaluation function by pre-calibration; In each expansion step, the candidate state node with the minimum evaluation function value is selected as the trajectory node, and also as the next expansion node, to guide the search process to quickly converge to the target pose node , if the evaluation function values of multiple candidate state nodes are the same, the node with the speed closer to the target pose node or the node with the heading angle closer to the target pose node heading angle can be preferentially selected. ​

[0024] It can be understood that the above evaluation function is only a specific example of the embodiment and is not a limitation of the present application. In other embodiments of the present application, the evaluation function can be replaced or adjusted according to the actual application scene and requirements. For example, the evaluation function can further include one or more of the following costs: deviation from the global reference path; cost of control amount change amplitude (such as acceleration, steering rate) to encourage smooth control; risk field cost of the position, such as the tendency to move away from all obstacles; driving efficiency cost, such as encouraging to maintain a high speed.

[0025] S2, taking the initial trajectory as the optimization initial value, performing trajectory optimization based on the preset constraint condition to obtain an optimized trajectory; representing the trajectory nodes of the initial trajectory as a series of discrete state trajectory node sequences ; the optimization target is to optimize and generate a safe and comfortable optimized trajectory for these discrete state trajectory nodes, and the optimized trajectory node sequence can be represented as ; The trajectory optimization specifically includes smoothing constraints, safety corridor constraints, initial pose constraints, end pose constraints, and vehicle physical constraints. S201, safety corridor constraint; Referring to Figure 2 , the initial trajectory planned in the S1 step is obtained, and the initial trajectory is composed of a series of initial trajectory nodes; at the same time, a real-time obstacle map in a global coordinate system is obtained and generated by fusing a vehicle real-time perception module (laser radar, camera, millimeter wave radar), and the real-time obstacle map includes an occupied area of static obstacles and an occupied area of dynamic obstacles; along each initial trajectory node on the initial trajectory, a minimum circumscribed rectangle is generated according to the vehicle contour, and the circumscribed rectangles of all initial trajectory nodes are connected in a loop to form a narrow initial safety area wrapping the initial trajectory; the boundary of the above initial safety area is gradually and uniformly expanded along its normal direction, and in the expansion process, the distance between the expanded boundary and the occupied area of static obstacles and the occupied area of dynamic obstacles is detected in real time, and the expansion is stopped until the distance between the expanded boundary in any direction and the occupied area of static obstacles and the occupied area of dynamic obstacles is equal to a first preset safety threshold, thereby forming a safety corridor; each optimized trajectory node after optimization must be located within the safety corridor generated in step S2.

[0026] The specific value of the first preset safety threshold can be calibrated and set according to the vehicle platform performance, sensor configuration and different safety level requirements. Once the distance is less than the first preset safety threshold, the local boundary expansion is immediately stopped.

[0027] S202, smoothing constraint: which can be expressed by the following expression: ; wherein, is a trajectory node of the optimized trajectory, is a trajectory node of the initial trajectory; is a deviation degree cost of the optimized trajectory from the initial trajectory; is a first derivative between adjacent optimized trajectory nodes of the optimized trajectory, i.e., a velocity between the adjacent two points. Minimizing this cost can make the adjacent optimized trajectory nodes take the shortest route as much as possible, improve the motion smoothness, and reduce the occurrence of curve drawing and the like; is a second derivative between adjacent optimized trajectory nodes of the optimized trajectory, i.e., an acceleration. Minimizing this cost can make the vehicle maintain uniform acceleration and deceleration driving as much as possible, thereby avoiding frequent acceleration and deceleration to cause discomfort to the driver and passengers; is a third derivative between adjacent optimized trajectory nodes of the optimized trajectory, i.e., a jerk. Minimizing this constraint can avoid sudden changes in acceleration, thereby suppressing the impact degree and further ensuring the comfort of the driver and passengers; , , , are weight coefficients of the above cost terms. They are calibrated and set according to the expected system behavior based on a large number of typical driving scenarios (such as following, lane changing and emergency obstacle avoidance) through simulation and real vehicle testing in the vehicle development stage. In some embodiments, they can be dynamically adjusted according to the real-time driving scenario, such as when safety is the highest priority, the path proximity cost weight is correspondingly increased to ensure that the optimized trajectory closely follows the initial safe path; when comfort is the highest priority, the acceleration cost weight and the impact degree cost weight are correspondingly increased to suppress intense acceleration and deceleration. Through the above method, the weight coefficients can be given reasonable values to balance the safety, comfort and smoothness and other multiple targets in different working conditions.

[0028] S203, start pose constraint and end pose constraint: the start trajectory node of the optimized trajectory Must be consistent with the initial state of the vehicle ; Must meet the requirements of the preset target pose node ; .

[0029] S204, vehicle physical constraints: To ensure that the optimized trajectory can be actually executed by the vehicle, the state and control amount of the trajectory node must be subject to the physical limit constraints of the vehicle itself: Specifically, it includes the longitudinal speed constraint of the vehicle: ; The longitudinal acceleration constraint of the vehicle: ; The front wheel steering angle constraint of the vehicle: ; The change rate constraint of the front wheel steering angle of the vehicle: ; Wherein, , , , The longitudinal speed, longitudinal acceleration, front wheel steering angle and change rate of the front wheel steering angle of the trajectory node of the optimized trajectory, respectively, are determined by the physical characteristics of the vehicle platform.

[0030] S3, output the optimized trajectory to the vehicle control unit to control the vehicle to track and execute.

[0031] The optimized trajectory obtained in step S2 is sent to the vehicle control unit, and the vehicle control unit of the embodiment is preferably an ECU. Each trajectory node of the optimized trajectory contains position, longitudinal speed, heading angle, and front wheel steering angle information. After receiving the information, the vehicle control unit adjusts the steering angle of the steering wheel (i.e. controls the front wheel steering angle) to align the actual heading of the vehicle with the target heading of the trajectory node of the optimized trajectory, and to eliminate the lateral position deviation between the vehicle and the target path. By coordinating the control of the throttle opening and the brake pressure, the actual longitudinal speed of the vehicle tracks the target speed of the trajectory node of the optimized trajectory, so that the actual driving path of the vehicle is consistent with the optimized trajectory.

[0032] S301, trigger adjustment generation: In addition, if the distance between the vehicle and the latest obstacle is within the preset safety interval for a preset time based on the real-time perceived environmental obstacle information when the vehicle is driving along the optimized trajectory, or a new instruction is received from the vehicle decision system (such as a navigation system requesting a new target point) or the human-computer interaction system (such as the driver actively taking over or inputting a new instruction), it is immediately determined that the triggering condition is met. At this time, the system sends an interrupt instruction to the vehicle control unit, stops tracking the current optimized trajectory, and controls the current vehicle to enter a safety protection mode through the vehicle control unit, and immediately takes the current latest real-time state of the vehicle as a new starting pose, re-executes step S1, and starts a new round of trajectory planning and optimization to generate a new optimized trajectory that meets the latest environment and task requirements. The preset safety interval is an absolute safety region for vehicle driving, which includes a maximum distance from the vehicle body for early warning and a minimum distance from the vehicle body for absolute safety. When the real-time perceived obstacle information is continuously located in the preset safety interval for a preset time, the system controls the current vehicle to enter a safety protection mode through the vehicle control unit, and stops tracking the current optimized trajectory and starts a new round of trajectory planning and optimization.

[0033] In a specific embodiment, the safety protection mode preferably controls the vehicle to reduce speed to half of the current speed at a preset speed. The specific behavior and parameters of the safety protection mode are a configurable item, and the purpose is to ensure that the vehicle can automatically enter a state with the lowest risk to facilitate the planning of a new optimized trajectory that meets the latest environment and task requirements. In the vehicle development stage, the strategy and parameters of the safety protection mode can be calibrated and adjusted according to different vehicle performance (such as wheelbase, weight, braking ability), driving style preferences (such as comfort, sports), and specific application scenarios (such as urban roads, highways, automatic parking) through simulation and real vehicle testing.

[0034] The preset time in the embodiment is preferably 30s, and the specific value of the preset time and the preset safety interval can also be adjusted based on the real vehicle and the environment.

[0035] Embodiment two Based on the same technical concept as embodiment one, the application also provides an embodiment two, a system based on a vehicle obstacle avoidance trajectory planning method, mainly comprising: A data acquisition module, which is an interface for the system to interact with vehicle sensors and upper systems, is configured to: acquire the current pose of the vehicle (initial state ) provided by the positioning system (such as GPS, IMU), and the target pose (target pose node ) issued by the vehicle navigation system or decision system; Real-time acquisition of environmental obstacle information fused by environmental perception systems (such as laser radar, camera, millimeter wave radar).

[0036] The processing module is connected with the data acquisition module and is configured to execute the method steps of Embodiment One, and specifically includes the following functional units: A trajectory planning unit configured to plan an initial trajectory of the vehicle from the current pose to the target pose based on a vehicle kinematics model in response to a trigger condition (corresponding to step S1 in Embodiment One); A trajectory optimization unit configured to take the initial trajectory as an optimization initial value, take the condition that the trajectory points are located within the safety corridor as a constraint condition, and consider the smoothness constraint, the start / end pose constraint, and the vehicle physical constraint to perform trajectory optimization to obtain an optimized trajectory (corresponding to step S2 in Embodiment One); A monitoring and re-planning trigger unit configured to continuously monitor the vehicle state and the environmental information during trajectory execution, and immediately determine that the trigger condition is met and instruct the trajectory planning unit and the trajectory optimization unit to start a new round of planning and optimization when detecting that the distance between the real-time perception-based environmental obstacle information and the vehicle is located within a preset safety interval within a preset time or receiving a new instruction (corresponding to step S301 in Embodiment One); A control module connected with the processing module, which is configured to: Receive the optimized trajectory from the processing module; Decompose the trajectory into lateral control instructions (adjust the steering wheel steering angle) and longitudinal control instructions (control the throttle and brake), and send them to the actuators of the vehicle (such as the steering motor, the drive motor, and the electronic brake system); Control the vehicle to track and execute the optimized trajectory, and smoothly switch to a new optimized trajectory according to the re-planning trigger instruction.

[0037] Embodiment Three: Based on the same technical concept as Embodiment One, the present embodiment also provides a computer-readable storage medium for implementing the aforementioned vehicle obstacle avoidance trajectory planning method. Specifically, the computer-readable storage medium (for example, ROM / RAM, magnetic disk, optical disk, flash memory, etc.) has a computer program stored thereon, and the program is executed by a processor to implement all steps of the vehicle obstacle avoidance trajectory planning method of Embodiment One, so that the vehicle or computing device installed with the storage medium has the ability to execute the obstacle avoidance trajectory planning function of the present application.

[0038] In the description of the present application, it should be noted that the terms "upper", "lower", and the like are used for indicating the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as a limitation on the present application. Unless otherwise explicitly specified and limited, the terms "mounting", "connection", "connecting" should be interpreted broadly, for example, can be fixed connection, can also be detachable connection, or integral connection; can be mechanical connection, can also be electrical connection; can be directly connected, can also be indirectly connected through an intermediate medium, can be internal communication of two elements. For those skilled in the art, the specific meaning of the above terms in the present application can be understood according to the specific circumstances.

[0039] It should be noted that in the present application, relational terms such as "first" and "second" and the like are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. Moreover, the terms "include", "contain" or any other variants thereof are intended to cover non-exclusive inclusion, so that the process, method, article or device including a series of elements not only includes those elements, but also includes other elements not explicitly listed or inherent to such process, method, article or device. Without more limitations, the element defined by the statement "including a" does not exclude the presence of other identical elements in the process, method, article or device including the element.

[0040] The above is only a specific embodiment of the present application, which enables those skilled in the art to understand or implement the present application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application will not be limited to these embodiments shown herein, but will conform to the widest scope consistent with the principles and novel features applied herein.

Claims

1. A method for vehicle obstacle avoidance trajectory planning, characterized in that, The method comprises the following steps: in response to a triggering condition, planning an initial trajectory of a vehicle from a current pose to a target pose based on a vehicle kinematic model; the planning of the initial trajectory comprises collision detection based on real-time perception of environmental obstacle information, the environmental obstacle information comprising static obstacles and dynamic obstacles; taking the initial trajectory as an optimization initial value, trajectory optimization is performed based on preset constraints to obtain an optimized trajectory; outputting the optimized trajectory to a vehicle control unit to control the vehicle to track and execute.

2. The vehicle obstacle avoidance trajectory planning method of claim 1, wherein, the initial trajectory is planned based on the vehicle kinematic model, specifically comprising: establishing a bicycle kinematic model of the vehicle, and discretizing the bicycle kinematic model based on a preset time domain and a preset time step; based on the discretized bicycle kinematic model, a forward exploration algorithm is used for trajectory search, and in the expansion process of each node, collision detection is performed on the candidate state node based on real-time perception of environmental obstacle information; iterative expansion is performed from the candidate state node selected by collision detection according to an evaluation function until the target pose is reached.

3. The vehicle obstacle avoidance trajectory planning method of claim 1, wherein, the collision detection on the candidate state node specifically comprises: based on the pose information of the candidate state node, the vehicle contour is calculated; the vehicle contour is compared with the obstacle occupancy region determined based on real-time perception of environmental obstacle information, the obstacle occupancy region comprising a static obstacle occupancy region and a dynamic obstacle occupancy region; for the static obstacle occupancy region, it is judged whether the vehicle contour overlaps with the occupancy region; for the dynamic obstacle occupancy region, it is judged whether the vehicle contour overlaps with the occupancy region expanded within the preset time step based on the current motion state of the dynamic obstacle; if there is any spatial overlap, the collision detection is not passed, and the node is discarded.

4. The vehicle obstacle avoidance trajectory planning method of claim 3, wherein, the trajectory optimization based on the preset constraints comprises a safety corridor constraint, and the safety corridor constraint specifically comprises: an initial safety region is formed with the center being the minimum circumscribed rectangle of the vehicle contour along the trajectory point of the initial trajectory; based on real-time perception of environmental obstacle information, the boundary of the initial safety region is expanded outward until the distance between the initial safety region and the obstacle occupancy region determined based on real-time perception of environmental obstacle information is equal to a first preset safety threshold, thereby forming a safety corridor; the trajectory points of the optimized trajectory are constrained to be located within the safety corridor.

5. The vehicle obstacle avoidance trajectory planning method of claim 1, wherein, the trajectory optimization based on the preset constraints further comprises a smoothing constraint, and the smoothing constraint comprises a weighted sum of at least one of the following cost terms: a deviation degree cost from the initial trajectory; a first derivative term cost for promoting motion smoothness; a second derivative term cost for promoting ride comfort; a third derivative term cost for suppressing impact degree.

6. The vehicle obstacle avoidance trajectory planning method of claim 1, wherein, the trajectory optimization based on the preset constraints further comprises: a starting pose constraint that the starting point state of the trajectory is consistent with the current state of the vehicle; an end pose constraint that the end point state of the trajectory is consistent with the preset target state.

7. The vehicle obstacle avoidance trajectory planning method of claim 1, wherein, The trajectory optimization based on the preset constraint condition further comprises vehicle physical constraints, and the vehicle physical constraints comprise a longitudinal speed constraint, a longitudinal acceleration constraint, a steering angle constraint, and a steering angle change rate constraint.

8. The vehicle obstacle avoidance trajectory planning method of claim 1, wherein, The trigger condition comprises at least one of the following: When the vehicle travels along the optimized trajectory, the distance between the real-time perceived environmental obstacle information and the vehicle is located in a preset safety interval within a preset time. A new instruction is received from a vehicle decision system or a human-computer interaction system.

9. A system based on the vehicle obstacle avoidance trajectory planning method of claim 1, characterized in that, The method comprises the following steps: a data acquisition module configured to acquire a current pose and a target pose of the vehicle, and to acquire real-time environmental obstacle information; a processing module connected to the data acquisition module, configured to, in response to a trigger condition, plan an initial trajectory of the vehicle from the current pose to the target pose based on a vehicle kinematics model, perform collision detection based on real-time perceived environmental obstacle information in the planning process of the initial trajectory, perform trajectory optimization based on preset constraints with the initial trajectory as an optimization initial value to obtain an optimized trajectory, and output the optimized trajectory to a vehicle control unit; a control module connected to the processing module, configured to control the vehicle to track and execute.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, The computer program, when executed by a processor, implements the steps of the vehicle obstacle avoidance trajectory planning method according to claim 1. The trajectory optimization based on the preset constraint condition further comprises vehicle physical constraints, and the vehicle physical constraints comprise a longitudinal speed constraint, a longitudinal acceleration constraint, a steering angle constraint, and a steering angle change rate constraint.

Images