A motion planning method for emergency lane keeping

Abstract

The application provides a motion planning method for emergency side parking, comprising the following steps: obtaining a traffic situation evaluation result, establishing a driving risk field according to the traffic situation evaluation result and calculating the risk field strength of each point in the driving risk field; identifying the target vehicle intention according to the risk field strength of the driving risk field where the vehicle is located, and deciding to execute the side parking motion planning method according to the target vehicle intention; constructing the executable path of the vehicle according to the kinematic model of the vehicle and the mechanical structure parameters of the vehicle; planning the speed of the vehicle based on a PID control algorithm; and modifying the target point coordinates in the path planning algorithm cycle to correct the executable path and the speed. The motion planning method for emergency side parking provided in the application improves the motion planning efficiency, realizes the adjustment of the parking posture of the ego vehicle, improves the safety, and enhances the robustness of the motion planning process.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving decision-making methods, and in particular to a motion planning method for intelligent connected vehicles under specific circumstances. Background Art

[0002] As the person who drives and controls the vehicle, the driver is the primary driver of vehicle movement in the traffic environment. However, precisely because vehicle movement is so dependent on the driver, traffic accidents are almost inevitable if the driver becomes unwell. Professional drivers face high workloads and work intensity, spending long hours behind the wheel, spending little time out of the vehicle, consuming little water, and eating irregularly, making them a high-risk group for sudden illnesses such as epilepsy, sudden death, and coronary heart disease. Preliminary research shows that traffic accidents involving loss of control due to sudden illness by professional drivers are frequent. To reduce the risk to drivers and passengers from such accidents and to ensure a safe traffic environment, an intelligent safety assistance system is needed that can safely and efficiently complete pull-over maneuvers autonomously in the event of driver incapacitation. This type of intelligent safety assistance system is currently lacking in commercial vehicles.

[0003] During autonomous parking, vehicles encounter complex road conditions, which include numerous potential risk events that require quantitative assessment. Accurate and adaptable risk assessment methods can ensure the safety of drivers and passengers and smooth traffic flow in emergency situations. Therefore, the out-of-control vehicle emergency response system should be based on risk assessment.

[0004] Existing autonomous decision-making is primarily based on the acquired state of the environment and the vehicle itself, determining the required driving behavior and trajectory. Among them, the driving risk field theory, based on the concept of artificial potential fields, a classic method in robotics, serves as a real-time two-dimensional risk assessment method. It points out the similarities between field theory and driving risk, quantifies driving risk using field theory, and reveals the human-vehicle-road interaction mechanism and the influence of various factors on driving safety. It can be used to predict the dynamic trend of driving risk and is suitable for the autonomous parking process of vehicles in emergency scenarios caused by driver emergencies. However, the corresponding traditional motion planning methods cannot well meet the requirements of safe and efficient pull-over parking in the event of driver disability: the path does not conform to the vehicle kinematic model, the parking posture is poor, and it is prone to periodic motion without reaching the target point.

[0005] Therefore, a technical solution is needed to overcome at least one of the above-mentioned defects of the existing traditional technology. Summary of the Invention

[0006] The present invention aims to solve one of the technical problems in the related art at least to a certain extent.

[0007] To this end, the purpose of the present invention is to propose a motion planning method for achieving emergency pull-over parking, which is used to plan a safe path so that the vehicle can park at the emergency parking lane in a reasonable posture in the shortest possible time and wait for subsequent rescue.

[0008] To achieve the above objectives, a first embodiment of the present invention provides a motion planning method for implementing emergency pull-over parking, comprising:

[0009] Obtaining a traffic situation assessment result, establishing a driving risk field based on the traffic situation assessment result, and calculating the risk field intensity of each point in the driving risk field;

[0010] Identifying the intention of a target vehicle based on the risk field strength of the driving risk field in which the vehicle is located, and executing a pull-over motion planning method based on the intention of the target vehicle;

[0011] constructing an executable path for the vehicle based on a vehicle kinematic model and mechanical structure parameters of the vehicle;

[0012] planning the speed of the vehicle based on a PID control algorithm;

[0013] By modifying the target point coordinates in the path planning algorithm loop, the executable path and the speed are corrected.

[0014] In addition, a motion planning method for implementing emergency pull-over parking according to the above embodiment of the present invention may also have the following additional technical features:

[0015] Furthermore, in one embodiment of the present invention, the theoretical model of the driving risk field includes three parts: the kinetic energy field formed by moving objects, the potential energy field formed by stationary objects, and the behavior field formed by the driver. The modeling is as follows:

[0016] Kinetic energy field formed by a moving object:

[0017]

[0018] Where: is the kinetic energy field strength generated by the moving object i at position j, M i is the equivalent mass of the moving object i, is the distance between moving objects i and j, v i is the speed of the moving object, θ i is the direction of the moving object's velocity and r ij The angle between G and R i , k1, k2 are all undetermined parameters;

[0019] The potential energy field formed by a stationary object:

[0020]

[0021] Where: is the potential energy field strength generated by object i at position j, M i is the equivalent mass of object i, is the distance between objects i and j, G, R i , k1 are parameters to be determined;

[0022]

[0023] Where: is the behavioral field strength generated by vehicle i at position j, is the kinetic energy field strength generated by vehicle i at position j, D ri is the driver risk factor;

[0024]

[0025] The three parts of the driving risk field mentioned above are synthesized to calculate the size and direction of the risk field strength formed by various traffic elements on the road to a certain moving vehicle.

[0026] Furthermore, in one embodiment of the present invention, the method of identifying the target vehicle's intention based on the risk field strength of the driving risk field in which the vehicle is located and executing the pull-over parking motion planning decision based on the target vehicle's intention includes:

[0027] Calculating a risk scenario for the vehicle and a risk field strength of the target vehicle relative to the vehicle;

[0028] If the lateral field strength of the road points in the opposite direction of the emergency lane, controlling the vehicle to execute a cruising method of decelerating along the current lane;

[0029] If the lateral field strength of the road points in the same direction as the emergency lane, the vehicle is controlled to execute a motion planning method for emergency pull-over parking.

[0030] Furthermore, in one embodiment of the present invention, planning the vehicle speed based on a PID control algorithm includes:

[0031] The input is the current vehicle speed v, and the output is the set target speed v t , the error function is err = v t -v, ignore the integral control term and rewrite the ideal PID control law of the continuous control system into the PD control law of the discrete control system:

[0032]

[0033] Where: K p is the proportional gain, T dis the differential time constant, Δt is the cycle time interval, i is the cycle number, u(i) is the output signal of the PD controller, that is, the speed change within the cycle time (Δt=0.01s), and err(i) is the difference between the vehicle speed and the target speed.

[0034] Furthermore, in one embodiment of the present invention, the PD speed control algorithm parameters are constrained as follows:

[0035]

[0036] Where: err(-1) is defined as the error function when the PD speed control algorithm is not started, and the maximum acceleration of the vehicle deceleration is obtained at the initial moment of the speed control algorithm, that is,

[0037] err[-1]=err[0],

[0038] but

[0039]

[0040] Furthermore, in one embodiment of the present invention, before modifying the target point coordinates in the path planning algorithm loop, the method further includes:

[0041] The original path-speed separation algorithm is coupled, and the path and speed are comprehensively considered to realize the path-speed coupled motion planning method.

[0042] To achieve the above objectives, a second embodiment of the present invention provides a motion planning device for implementing emergency pull-over parking, comprising the following modules:

[0043] A perception module is used to obtain traffic situation assessment results, establish a driving risk field based on the traffic situation assessment results, and calculate the risk field intensity of each point in the driving risk field;

[0044] A judgment module is used to identify the target vehicle's intention based on the risk field strength of the driving risk field in which the vehicle is located, and to decide to execute the pull-over parking motion planning method based on the target vehicle's intention;

[0045] A construction module, configured to construct an executable path for the vehicle according to a vehicle kinematic model and mechanical structure parameters of the vehicle;

[0046] A planning module, configured to plan the speed of the vehicle based on a PID control algorithm;

[0047] The optimization module is used to modify the target point coordinates in the path planning algorithm loop to correct the executable path and the speed.

[0048] Furthermore, in one embodiment of the present invention, it further includes: a coupling module for coupling the original path-speed separation algorithm, comprehensively considering the path and speed, and realizing a path-speed coupled motion planning method.

[0049] To achieve the above-mentioned purpose, an embodiment of the third aspect of the present invention proposes a computer device, characterized in that it includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the motion planning method for implementing emergency pull-over parking as described above.

[0050] To achieve the above-mentioned purpose, the fourth embodiment of the present invention proposes a computer-readable storage medium on which a computer program is stored, characterized in that when the computer program is executed by a processor, a motion planning method for realizing emergency pull-over parking as described above is implemented.

[0051] The motion planning method for implementing emergency pull-over parking proposed in the embodiment of the present invention implements decision-making and motion planning based on driving risk field theory, outputs a set of vehicle path points and speeds corresponding to each moment, for reference by downstream control systems, and ultimately realizes the development of an emergency pull-over parking safety assistance system to cope with driver disability situations. The advantages of this method are mainly as follows: for motion planning methods, most currently adopt path-speed separation planning methods, while this application comprehensively considers path and speed to implement a path-speed coupling motion planning method; it proposes a moving target idea to deal with one-dimensional targets such as emergency lanes, which not only shortens the pull-over path and improves motion planning efficiency, but also realizes the adjustment of the vehicle's parking posture, thereby improving safety and enhancing the robustness of the motion planning process. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the following description of the embodiments in conjunction with the accompanying drawings, in which:

[0053] Figure 1 A flow chart of a motion planning method for implementing emergency pull-over parking provided by an embodiment of the present invention.

[0054] Figure 2 Schematic diagram of the gradient calculation process of the artificial potential field model provided in an embodiment of the present invention.

[0055] Figure 3 A schematic diagram of a vehicle kinematic model provided by an embodiment of the present invention.

[0056] Figure 4 A schematic diagram of a road coordinate system provided by an embodiment of the present invention.

[0057] Figure 5 This is a schematic diagram of the algorithm for constraining the steering angle and steering wheel speed based on the vehicle's mechanical structure, as provided in an embodiment of the present invention.

[0058] Figure 6 This is a logic diagram of the PID control algorithm provided by an embodiment of the present invention.

[0059] Figure 7 This is a schematic diagram of the specific effect of the PD speed control algorithm provided by an embodiment of the present invention.

[0060] Figure 8 This is a schematic diagram of planning under different vehicle initial speeds provided by an embodiment of the present invention.

[0061] Figure 9 A schematic diagram of the emergency pull-over parking process provided by an embodiment of the present invention.

[0062] Figure 10 A schematic diagram of modifying the target point during an emergency pull-over parking process provided by an embodiment of the present invention.

[0063] Figure 11 This is a motion trajectory diagram of a vehicle in a road coordinate system provided by an embodiment of the present invention.

[0064] Figure 12 This is a diagram showing the temporal changes in the Y coordinate and heading angle of a vehicle in a road coordinate system during an emergency pull-over process provided by an embodiment of the present invention.

[0065] Figure 13 This is a schematic diagram showing a before-and-after comparison of the improved algorithm provided by an embodiment of the present invention.

[0066] Figure 14 This is an example diagram of the application of the correction algorithm provided in an embodiment of the present invention.

[0067] Figure 15 A schematic diagram of an ideal emergency stop risk area provided by an embodiment of the present invention.

[0068] Figure 16 A schematic flow chart of a motion planning device for implementing emergency pull-over parking provided by an embodiment of the present invention.

[0069] Figure 17 This is a flow chart of the motion planning method for emergency pull-over parking provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0070] The following describes embodiments of the present invention in detail, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, and are not to be construed as limiting the present invention.

[0071] The following describes a motion planning method and apparatus for implementing emergency pull-over parking according to an embodiment of the present invention with reference to the accompanying drawings.

[0072] Figure 1 A flowchart of a motion planning method for implementing emergency pull-over parking provided by an embodiment of the present invention.

[0073] like Figure 1 As shown, the motion planning method for implementing emergency pull-over parking includes the following steps:

[0074] S101: Obtain traffic situation assessment results, establish a driving risk field based on the traffic situation assessment results, and calculate the risk field intensity of each point in the driving risk field;

[0075] Driving risk field theory is a modeling theory used in vehicle risk assessment, based on the idea of ​​the traditional robotics path planning method—the artificial potential field path planning method.

[0076] The traditional artificial potential field path planning method is a virtual force method proposed by Khatib. Its basic idea is to abstract the robot's movement in the surrounding space as the movement of a point in the potential energy field acting on it. Its basic theory is field theory.

[0077] The research object of field theory is the field, which is the distribution of a physical quantity in a spatial region. The research content of field theory is the interaction between physical fields and matter. When applied to artificial potential field path planning methods, it studies the interaction between obstacles, target points and robots. In the field theory system, the field can be quantitatively described as a continuous and differentiable space-time function. The robot moves only in a two-dimensional plane, so the field function only needs to contain (x, y) two-dimensional coordinates, which can be simplified to Since the robot path planning is performed at discrete time points, the field function can be frozen at time t, which can be simplified to

[0078] For field functions Assume u = (cosθ, sinθ) is a direction, then at (x0, y0), the field function The derivative along the μ direction is

[0079]

[0080] According to the Cauchy-Schwarz inequality,

[0081]

[0082] The condition for the equality in the above formula to hold is that the direction μ and the vector

[0083]

[0084] have the same direction. This vector is called a field function The gradient at point (x0, y0) is denoted as In the gradient direction, the field function changes most dramatically.

[0085] Furthermore, if for a vector field There is a quantity field satisfy,

[0086]

[0087] Then the vector field is called a potential field, and the quantity field is a vector field The potential function of .

[0088] Based on field theory, the artificial potential field path planning method requires the robot to move along the path where the spatially distributed field function takes the lowest value. Therefore, its movement direction is naturally the gradient direction. The artificial potential field is modeled as follows:

[0089] Target gravitational field:

[0090]

[0091] Obstacle Repulsion Field:

[0092]

[0093] Where: p G (q) represents the distance to the target point, p(q) represents the distance to the obstacle, p0 represents the range of the obstacle repulsive field, k and η are both unknown coefficients. Taking this as the potential function, we can get the vector field by taking its negative gradient:

[0094] F att (q) = kp G (q),

[0095]

[0096] Therefore, the combined field strength generated by the target point and the repulsive field strength generated by the obstacle can be obtained, that is, the direction of the next movement, such as Figure 2 The complete path can be obtained by iterating in sequence as shown.

[0097] For simple artificial potential field models, the corresponding vector field function can be obtained by simply calculating the gradient. However, for more complex artificial potential field models, calculating the gradient is extremely difficult and tedious, and it is also difficult for a computer to numerically calculate the gradient of a point in a spatially distributed field function. Therefore, it is possible to directly model the spatially distributed vector field to simplify the calculation.

[0098] To explore better vehicle safety decision-making methods, some researchers have considered the vehicle's driving environment as a potential field and applied artificial potential field path planning methods to vehicle-following behavior modeling and the design of driver safety assistance systems. Furthermore, the risks posed by traffic factors to moving vehicles share similar characteristics with fields: the risks generated by the various components of the traffic system share the same spatiotemporal characteristics as fields; risks are generated by traffic factors, while fields are generated by field sources; the impact of traffic factors on driving safety also varies over time and space, and field quantities can be characterized as spatiotemporal functions; the impact of traffic factors on driving and the field strength are both directional. Therefore, using fields to describe driving risks is reasonable, and this is the underlying principle of driving risk field theory.

[0099] Furthermore, in one embodiment of the present invention, the driving risk field theoretical model is composed of three parts: the kinetic energy field formed by moving objects, the potential energy field formed by stationary objects, and the behavior field formed by the driver. The modeling is as follows:

[0100] Kinetic energy field formed by a moving object:

[0101]

[0102] Where: is the kinetic energy field strength generated by the moving object i at position j, M i is the equivalent mass of the moving object i, is the distance between moving objects i and j, v i is the speed of the moving object, θ i is the direction of the moving object's velocity and r ij The angle between G and R i , k1, k2 are all undetermined parameters;

[0103] The potential energy field formed by a stationary object:

[0104]

[0105] Where: is the potential energy field strength generated by object i at position j, M i is the equivalent mass of object i, is the distance between objects i and j, G, R i , k1 are parameters to be determined;

[0106]

[0107] Where: is the behavioral field strength generated by vehicle i at position j, is the kinetic energy field strength generated by vehicle i at position j, D ri is the driver risk factor;

[0108]

[0109] The three parts of the driving risk field mentioned above are synthesized to calculate the size and direction of the risk field strength formed by various traffic elements on the road to a certain moving vehicle.

[0110] S102: Identify the target vehicle's intention based on the risk field strength of the vehicle's driving risk field, and execute a pull-over motion planning method based on the target vehicle's intention;

[0111] After the present invention establishes the driving risk field at any point in space in step S101, the vehicle can quantitatively evaluate the road risk by the risk effects of other traffic elements on the road, and identify the intentions of vehicles in adjacent lanes. By modeling the targeted risk field for emergency pull-over scenarios under driver disability, the advantage of two-dimensional real-time quantification of risk can be fully utilized, thereby making efficient decisions. The ideal emergency stop risk field should be as follows: Figure 15 shown.

[0112] The wind fields generated by other vehicles in the figure are divided into four areas. The risk field in Area I should be larger. Its main purpose is to assist the ego vehicle in making lane-changing decisions for emergency pull-over parking. The risk field in this area should be related to the relative speed of the ego vehicle and other vehicles, so it can cope with various situations on the real road. The risk fields in Areas II and IV should be smaller. This is because if the ego vehicle is in these two positions of other vehicles, it means that the ego vehicle has decided to start an emergency stop. At this time, other vehicles should not exert too much influence on the ego vehicle. However, in order to cope with possible extreme situations, the wind field should not be 0. The risk field in Area III should be almost 0. This is because if the ego vehicle reaches this position of other vehicles, it should already be in the cruising and deceleration stage in the emergency lane. At this time, it does not need to be affected by other vehicles and only needs to continue the parking process.

[0113] If this risk field is constructed, the decision-making process will be greatly simplified: the risk field strength of the target and other vehicles to the ego vehicle is calculated. If the lateral field strength of the road points in the opposite direction of the emergency lane, the current state of vehicles in the adjacent lane poses a high risk to the ego vehicle's lane change behavior, and the cruise algorithm, which slows down and drives in the current lane, is executed. If the lateral field strength of the road points in the direction of the emergency lane, the current state of vehicles in the adjacent lane does not pose a high risk to the ego vehicle's lane change behavior, and the motion planning method for emergency pullover is executed. Because the cruise algorithm only operates when the ego vehicle remains in its original lane, a decision switch is implemented within the algorithm. Once the motion planning method for emergency pullover is executed, the cruise algorithm will not be executed again, even if the execution conditions of the cruise algorithm are met during pullover.

[0114] Furthermore, in one embodiment of the present invention, the target vehicle's intention is identified based on the risk field strength of the driving risk field in which the vehicle is located, and a pull-over parking motion planning method is executed based on the target vehicle's intention, including:

[0115] Calculate the risk scenario of the vehicle and the risk field strength of the target vehicle relative to the vehicle;

[0116] If the lateral field strength of the road points in the opposite direction of the emergency lane, the vehicle is controlled to execute a cruise method of decelerating along the current lane;

[0117] If the lateral field strength of the road points in the same direction as the emergency lane, the vehicle is controlled to execute a motion planning method for emergency pull-over parking.

[0118] S103: Constructing an executable path for the vehicle based on the vehicle kinematic model and the vehicle's mechanical structure parameters;

[0119] Considering the vehicle's kinematic constraints and its own mechanical structure improves the feasibility of the path planning method. The construction of an executable path for the vehicle requires attention to the following two aspects: the vehicle's kinematic model and the vehicle's mechanical structure constraints. This step imposes constraints on both:

[0120] Consider the vehicle kinematic model. Unlike robots, vehicles are larger and have more complex mechanical structures. Their motion needs to be described through geometric structure and kinematic analysis. Otherwise, the actual vehicle execution of the resulting path will be poor.

[0121] The vehicle kinematic model is now described as Figure 3 As shown in the figure: ψ is the heading angle of the vehicle, δ is the steering angle of the vehicle, is the front wheel speed, and L is the vehicle body length.

[0122] In the road coordinate system Oxy, the positions of the front and rear wheels of the vehicle can be described as:

[0123] Front wheel coordinates of the vehicle:

[0124]

[0125] Coordinates of the rear wheels of the vehicle:

[0126]

[0127] Taking the partial derivative of the coordinates with respect to time can obtain the rate of change of the coordinates with respect to time, that is, the speed of the front and rear wheels of the vehicle:

[0128] Front wheel speed of the vehicle:

[0129]

[0130] Vehicle rear wheel speed:

[0131]

[0132] The ego vehicle is considered as a rigid body, and O′ is the instantaneous center of motion of the ego vehicle body, that is, the velocity of the ego vehicle relative to the road coordinate system Oxy at this moment in the rigid body motion reference system is Taking O′ as the base point, the front wheel speed can be described as,

[0133]

[0134] Scalarize the above formula,

[0135]

[0136] Substitute the above formula into the rear wheel speed expression of the vehicle:

[0137] Vehicle rear wheel speed:

[0138]

[0139] Furthermore, the magnitude and direction of the velocity of the center of the vehicle can be calculated:

[0140]

[0141] Assume that the angle between the previously calculated combined risk direction and the positive direction of the x-axis in the road coordinate system Oxy is α, and the point at a distance of one cycle step from the center of the vehicle along the α direction is called the preview point, as shown in Figure 4 Point T is shown. In the figure: l is the cycle step length.

[0142] The steering angle corresponding to reaching the preview point can be obtained by inversely solving the expression of the vehicle's velocity direction:

[0143]

[0144]

[0145] The ideal steering angle of the vehicle can be obtained from the above formula, but it is often unattainable due to the mechanical structure constraints of the vehicle.

[0146] The steering angle and steering wheel speed are constrained according to the vehicle's mechanical structure. Figure 5 shown.

[0147] In the figure: δ0 is the maximum change in the steering angle within one cycle, that is, the maximum steering wheel rotation angle within one cycle; δ max is the maximum steering angle limited by the vehicle structure.

[0148] This gives the current steering angle that the vehicle should execute. The vehicle's position and posture at the next moment can be further calculated using the steering angle:

[0149]

[0150] So far, a complete loop structure has been obtained, and a path with strong vehicle executableness can be obtained through this planning method by iterating in a loop.

[0151] S104: Planning the vehicle speed based on the PID control algorithm;

[0152] The PID control algorithm is a control algorithm that combines the three links of proportional, integral and differential. It is the most mature and widely used control algorithm in continuous systems. The PID control algorithm calculates the proportional, integral and differential functions based on the input deviation value, and the calculation results are used to control the output. For continuous control systems, the ideal PID control law is:

[0153]

[0154] Where: K p is the proportional gain, T i is the integration time constant, T d is the differential time constant, u(t) is the output signal of the PID controller, and err(t) is the difference between the output and the input.

[0155] P represents proportional control, which acts on the input and can effectively adjust the input with the output as the center, but it is difficult to stabilize it at the output; I represents integral control, which can eliminate steady-state errors based on proportional control; D represents differential control, which acts on the input and can reduce the oscillation of the input near the output.

[0156] PID control algorithm is a closed-loop control, and its specific control logic is as follows Figure 6Therefore, due to the feedback adjustment mechanism of the PID control algorithm, it has good robustness and can better adapt to the artificial changes of the system input or output during the control process.

[0157] The PID control algorithm is widely used in lateral vehicle control due to its simple principle, ease of implementation, wide applicability, independent control parameters, and relatively simple parameter selection. Similarly, due to the aforementioned advantages of the PID control algorithm, the present invention uses the PID concept to perform vehicle speed planning.

[0158] Furthermore, in one embodiment of the present invention, planning the vehicle speed based on the PID control algorithm includes:

[0159] The input is the current vehicle speed v, and the output is the set target speed v t , the error function is err = v t -v, ignore the integral control term and rewrite the ideal PID control law of the continuous control system into the PD control law of the discrete control system:

[0160]

[0161] Where: K p is the proportional gain, T d is the differential time constant, Δt is the cycle time interval, i is the cycle number, u(i) is the output signal of the PD controller, that is, the speed change within the cycle time (Δt=0.01s), and err(i) is the difference between the vehicle speed and the target speed.

[0162] The specific control effect of PD speed control algorithm is as follows Figure 7 shown.

[0163] Constrain the parameters of the PD speed control algorithm. Although the vehicle should be decelerated to 0 as soon as possible to deal with emergency scenarios, the vehicle has an upper limit on its deceleration acceleration due to its dynamic model, that is, the maximum value of u has an upper limit, which is the parameter K. p There are restrictions on selection. Figure 7 As shown in Figure 2, the maximum acceleration of the vehicle deceleration is obtained at the initial moment of the speed control algorithm.

[0164] Furthermore, in one embodiment of the present invention, the PD speed control algorithm parameters are constrained as follows:

[0165]

[0166] Where: err(-1) is defined as the error function when the PD speed control algorithm is not started, and the maximum acceleration of the vehicle deceleration is obtained at the initial moment of the speed control algorithm, that is,

[0167] err[-1]=err[0],

[0168] but

[0169]

[0170] Under this constraint, the planning under different initial speeds of the vehicle is as follows: Figure 8 As shown, all of them are consistent with the vehicle dynamics model and have strong executability.

[0171] At this point, the general framework of the path planning and velocity planning parts of the motion planning of the present invention has been completed. However, in order to better achieve the original intention of the present invention, further optimization and improvement are still needed, namely the path-velocity coupled planning method.

[0172] To achieve safe and quick pullovers, separate path-speed planning methods are not ideal. First, these fixed target points cannot guarantee the ego vehicle's arrival without periodic oscillations in complex road conditions. Second, even if the ego vehicle can reach the fixed target point under certain circumstances, this path planning method cannot guarantee that the ego vehicle's posture will be parallel to the road at the time of parking. Finally, separate path-speed planning fails to account for both the ego vehicle's position and speed, potentially resulting in the ego vehicle's speed being reduced to zero before reaching the emergency lane or when its posture is not parallel to the road. Coupling the path-speed method can effectively address these issues, but coupling the previously separate algorithms requires revisions to the existing algorithms.

[0173] Correction 1: Modify the loop step length of the path planning method in step S103, as well as the permanent loop termination condition. In traditional path planning methods, the loop step length is a fixed value, while in the present invention, the discrete loop step length can be associated with the speed. The current speed of the vehicle can be expressed as the step length of the vehicle's movement within each loop time interval. The permanent loop termination condition of the traditional path planning method is that the distance between the vehicle position and the fixed target point is within a certain range. However, the present invention will abandon the idea of ​​a fixed target point, and since the vehicle speed is associated with the loop step length, the vehicle speed can be reduced to 0 as the permanent loop exit condition.

[0174] Modification 2: Modify the speed planning algorithm in step S104 to associate it with the vehicle's position and heading. To ensure that the vehicle decelerates to zero only when it reaches the emergency lane or when its posture meets the requirements, the output of the PID-based speed planning must be associated with the vehicle's position and heading. The simplest approach is to modify the speed planning target speed to associate it with the vehicle's position and heading.

[0175] This operation is equivalent to setting a guideline speed for each Y coordinate in the road coordinate system. For simplicity, let's assume that the guideline speed V is proportional to the lateral distance between the vehicle's current coordinate and the centerline of the emergency lane. Only when the vehicle reaches the centerline of the emergency lane, where the guideline speed is zero, can its speed be reduced to zero by the speed control algorithm. Figure 13 For comparison before and after improving the algorithm. Figure 13 As shown in the figure, due to the existence of the guide speed, the vehicle's deceleration is somewhat slowed down, which saves a certain amount of time in the lane change process when pulling over compared to the algorithm before the improvement. At the same time, the improved speed control algorithm has stronger robustness. It can deal with the sudden deceleration caused by avoiding vehicles in the adjacent lane during the pull-over process according to the guide speed, thereby improving the efficiency of the lane change process when pulling over and avoiding unnecessary time waste. An application example, such as Figure 14 shown.

[0176] In this application example, the vehicle has already slowed down to 10 km / s in the first lane to avoid a vehicle in the adjacent lane. The improved algorithm handles this situation, increasing the vehicle's speed and significantly saving time during the emergency stop and lane change process, thereby shortening the total time of the stop process.

[0177] Furthermore, in one embodiment of the present invention, before modifying the target point coordinates in the path planning algorithm loop, the method further includes:

[0178] The original path-speed separation algorithm is coupled, and the path and speed are comprehensively considered to realize the path-speed coupled motion planning method.

[0179] S105: The target point coordinates are modified in the path planning algorithm loop to correct the vehicle's executable path and vehicle speed.

[0180] Traditional path planning algorithms primarily address static scenarios, with the path planning target being a specific point in space. Therefore, a fixed target point meets model requirements and effectively provides guidance and traction. However, emergency pull-over scenarios are dynamic and require only that the vehicle safely stop in the emergency lane. The path planning target is a line, with no specific stopping location required. Therefore, using a fixed target point at a fixed location in the emergency lane would not effectively meet model requirements and could even hinder functional implementation.

[0181] To this end, the present invention makes the target point coordinates modifiable in the path planning algorithm loop, changes the function of the target point in the path planning algorithm, and optimizes the target point from a spatial point that must be reachable to a source of gravity that provides guidance and traction. This idea is called the variable target idea.

[0182] Under this concept, the target point, as the source of gravity, has different main functions at different stages of the emergency parking process, such as Figure 9 As shown:

[0183] In the first stage, the target mainly provides lateral attraction to enable the ego vehicle to start parking as quickly as possible; in the second stage, the ego vehicle's lateral position has passed the second lane. At this time, since the avoidance action to the vehicle in the second lane has been completed, the target mainly provides traction for the ego vehicle to adjust its parking posture. Based on the above analysis, the target position should be related to the lateral position of the ego vehicle in the lane, specifically Figure 10 As shown:

[0184] The target point should always be on the center line of the emergency lane, that is, moving parallel to the dashed line of the road in the figure. In the first stage, it should be below the dashed line I, and in the second stage, it should be above the dashed line II. Consider using the characteristic function to roughly fit the target point position:

[0185]

[0186] Where (x car ,y car ) are the ego vehicle coordinates, ψ is the ego vehicle heading angle, ψ0 is the ego vehicle heading angle from the first stage to the second stage, an adjustable parameter related to road width, and C is an undetermined constant that maintains the target point at a fixed distance ahead of the ego vehicle along the road. The transition from the first stage to the second stage is a decision made after the timing is determined to be ripe and is irreversible (one cannot return to the first stage from the second stage). Code implementation requires a motion planning switch: after executing the second stage, the switch is turned off, preventing execution even if the first stage conditional statements are met.

[0187] When the initial speed of the vehicle is 40km / s, the effect is as follows: Figure 11 、 12 shown.

[0188] Figure 11 It represents the motion trajectory of the vehicle in the road coordinate system and can meet the basic requirements of emergency parking. Figure 12 The time-varying patterns of the ego vehicle's Y coordinate and heading angle within the road coordinate system during the emergency pullover process are shown. Over time, the Y coordinate approaches the centerline of the emergency lane, while the heading angle approaches zero, indicating that the algorithm has essentially achieved the required posture control. However, slight fluctuations still occur during the pullover process, which can be optimized through the design of a road risk field.

[0189] The above is the complete motion planning method process for emergency parking. The flowchart is as follows: Figure 17 shown.

[0190] The motion planning method for implementing emergency pull-over parking proposed in an embodiment of the present invention implements decision-making and motion planning based on driving risk field theory, outputs a set of vehicle path points and speeds corresponding to each moment, for reference by downstream control systems, and ultimately realizes the development of an emergency pull-over parking safety assistance system to cope with driver disability situations. The innovation of this method lies mainly in: for motion planning methods, most currently adopt a path-speed separation planning method, which comprehensively considers the path and speed to implement a path-speed coupling motion planning method; it proposes a moving target idea to deal with one-dimensional targets such as emergency lanes, which not only shortens the pull-over path and improves the efficiency of motion planning, but also realizes the adjustment of the vehicle's parking posture, thereby improving safety and enhancing the robustness of the motion planning process.

[0191] In order to implement the above embodiment, the present invention further proposes a motion planning device for implementing emergency pull-over parking.

[0192] Figure 16 A schematic structural diagram of a motion planning device for implementing emergency pull-over parking provided in an embodiment of the present invention.

[0193] like Figure 16 As shown, the motion planning device for implementing emergency parking includes: a perception module 100, a judgment module 200, a construction module 300, a planning module 400, and an optimization module 500, wherein:

[0194] A perception module is used to obtain traffic situation assessment results, establish a driving risk field based on the traffic situation assessment results, and calculate the risk field intensity of each point in the driving risk field;

[0195] A judgment module is used to identify the target vehicle's intention based on the risk field strength of the driving risk field in which the vehicle is located, and to decide to execute the pull-over parking motion planning method based on the target vehicle's intention;

[0196] A construction module, used for constructing an executable path of the vehicle according to the vehicle kinematic model and the mechanical structure parameters of the vehicle;

[0197] Planning module, used to plan vehicle speed based on PID control algorithm;

[0198] The optimization module is used to modify the target point coordinates in the path planning algorithm loop to correct the vehicle's executable path and vehicle speed.

[0199] Furthermore, in one embodiment of the present invention, it further comprises:

[0200] The coupling module is used to couple the original path-speed separation algorithm, comprehensively consider the path and speed, and implement a path-speed coupled motion planning method.

[0201] To achieve the above-mentioned purpose, the third aspect of the present invention proposes a computer device, which is characterized in that it includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the motion planning method for emergency pull-over parking as described above.

[0202] To achieve the above-mentioned purpose, the fourth embodiment of the present invention proposes a computer-readable storage medium on which a computer program is stored, characterized in that when the computer program is executed by a processor, the motion planning method for realizing emergency pull-over parking as described above is implemented.

[0203] In the description of this specification, the reference terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" mean that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art can combine and combine different embodiments or examples described in this specification and features of different embodiments or examples without contradiction.

[0204] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of the technical features being referred to. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present invention, "plurality" means at least two, such as two, three, etc., unless otherwise specifically defined.

[0205] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limiting the present invention. A person skilled in the art may change, modify, replace, and modify the above embodiments within the scope of the present invention.

Claims

1. A motion planning method for implementing emergency pull-over parking, characterized in that: The following steps are involved: Obtaining a traffic situation assessment result, establishing a driving risk field based on the traffic situation assessment result, and calculating the risk field intensity of each point in the driving risk field; Identifying the intention of a target vehicle based on the risk field strength of the driving risk field in which the vehicle is located, and executing a pull-over motion planning method based on the intention of the target vehicle; constructing an executable path for the vehicle based on a vehicle kinematic model and mechanical structure parameters of the vehicle; planning the speed of the vehicle based on a PID control algorithm; Correcting the executable path and the speed by modifying the target point coordinates in a path planning algorithm loop; The method for identifying the target vehicle's intention based on the risk field strength of the driving risk field in which the vehicle is located and deciding to execute a pullover parking motion planning method based on the target vehicle's intention includes: calculating the risk scenario of the vehicle and the risk field strength of the target vehicle relative to the vehicle; if the lateral field strength of the road points in the opposite direction of the emergency lane, controlling the vehicle to execute a cruising method of decelerating along the current lane; and if the lateral field strength of the road points in the same direction as the emergency lane, controlling the vehicle to execute a motion planning method of emergency pullover parking; The speed planning of the vehicle based on the PID control algorithm includes: The input is the current vehicle speed v, and the output is the set target speed v t , the error function is err = v t -v, ignore the integral control term and rewrite the ideal PID control law of the continuous control system into the PD control law of the discrete control system: Where: K p is the proportional gain, T d is the differential time constant, Δt is the cycle time interval, i is the cycle number, u(i) is the output signal of the PD controller, that is, the speed change within the cycle time (Δt = 0.01s), and err(i) is the difference between the vehicle speed and the target speed; Also includes: The PD speed control algorithm parameters are constrained as follows: Where: err(-1) is defined as the error function when the PD speed control algorithm is not started, and the maximum acceleration of the vehicle deceleration is obtained at the initial moment of the speed control algorithm, that is, err[-1]=err[0], but 2. The method according to claim 1, characterized in that The theoretical model of the driving risk field includes three parts: the kinetic energy field formed by moving objects, the potential energy field formed by stationary objects, and the behavior field formed by drivers. The modeling is as follows: Kinetic energy field formed by a moving object: Where: is the kinetic energy field strength generated by the moving object i at position j, M i is the equivalent mass of the moving object i, is the distance between moving objects i and j, v i is the speed of the moving object, θ i The direction of the moving object's velocity and r ij The angle between G and R i , k1, k2 are all undetermined parameters; The potential energy field formed by a stationary object: Where: is the potential energy field strength generated by object i at position j, M i is the equivalent mass of object i, is the distance between objects i and j, G, R i , k1 are parameters to be determined; Where: is the behavioral field strength generated by vehicle i at position j, is the kinetic energy field strength generated by vehicle i at position j, D ri is the driver risk factor; The three parts of the driving risk field mentioned above are synthesized to calculate the size and direction of the risk field strength formed by various traffic elements on the road to a certain moving vehicle.

3. The method according to claim 1, characterized in that Before modifying the target point coordinates in the path planning algorithm loop, it also includes: The original path-speed separation algorithm is coupled, and the path and speed are comprehensively considered to realize the path-speed coupled motion planning method.

4. A motion planning device for implementing emergency pull-over parking, characterized in that: Includes the following modules: A perception module is used to obtain traffic situation assessment results, establish a driving risk field based on the traffic situation assessment results, and calculate the risk field intensity of each point in the driving risk field; A judgment module is used to identify the target vehicle's intention based on the risk field strength of the driving risk field in which the vehicle is located, and to decide to execute the pull-over parking motion planning method based on the target vehicle's intention; A construction module, configured to construct an executable path for the vehicle according to a vehicle kinematic model and mechanical structure parameters of the vehicle; A planning module, configured to plan the speed of the vehicle based on a PID control algorithm; an optimization module for modifying the executable path and the speed by modifying the target point coordinates in a path planning algorithm loop; The method for identifying the target vehicle's intention based on the risk field strength of the driving risk field in which the vehicle is located and deciding to execute a pullover parking motion planning method based on the target vehicle's intention includes: calculating the risk scenario of the vehicle and the risk field strength of the target vehicle relative to the vehicle; if the lateral field strength of the road points in the opposite direction of the emergency lane, controlling the vehicle to execute a cruising method of decelerating along the current lane; and if the lateral field strength of the road points in the same direction as the emergency lane, controlling the vehicle to execute a motion planning method of emergency pullover parking; The speed planning of the vehicle based on the PID control algorithm includes: The input is the current vehicle speed v, and the output is the set target speed v t , the error function is err = v t -v, ignore the integral control term and rewrite the ideal PID control law of the continuous control system into the PD control law of the discrete control system: Where: K p is the proportional gain, T d is the differential time constant, Δt is the cycle time interval, i is the cycle number, u(i) is the output signal of the PD controller, that is, the speed change within the cycle time (Δt = 0.01s), and err(i) is the difference between the vehicle speed and the target speed; Also includes: The PD speed control algorithm parameters are constrained as follows: Where: err(-1) is defined as the error function when the PD speed control algorithm is not started, and the maximum acceleration of the vehicle deceleration is obtained at the initial moment of the speed control algorithm, that is, err[-1]=err[0], but 5. The device according to claim 4, characterized in that Also includes: The coupling module is used to couple the original path-speed separation algorithm, comprehensively consider the path and speed, and implement a path-speed coupled motion planning method.

6. A computer device, characterized in that: The method comprises a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the method implements the motion planning method for implementing emergency pull-over parking as described in any one of claims 1 to 3.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the motion planning method for implementing emergency pull-over parking as described in any one of claims 1 to 3 is implemented.

Images