Method and device for motion planning for emergency lane keeping

Abstract

The application discloses a motion planning method and device for realizing emergency side parking, wherein the method comprises the following steps: evaluating a current traffic situation according to perception information, establishing a driving risk field, calculating the risk field intensity and direction of each point; obtaining the risk field intensity of the ego vehicle based on the risk field intensity and direction of each point, evaluating the intention of other vehicles, and deciding whether to execute the motion planning strategy of emergency side parking according to the intention of other vehicles; and when the motion planning strategy of emergency side parking is executed, the speed of the ego vehicle is planned based on PID while being constrained based on the constraint conditions obtained from the kinematics constraints of the vehicle and the mechanical structure of the ego vehicle, until the emergency side parking is completed. Thus, the technical problem that in the prior art, the motion planning cannot be adapted to autonomous decision-making, so that it is difficult to guarantee the safety and efficiency of controlling the vehicle to perform emergency side parking in the case of driver disability is solved.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving and decision-making technology, and in particular to a motion planning method and device for implementing emergency pull-over parking. Background Art

[0002] As the person driving and controlling the vehicle, the driver is the primary driver of vehicle movement in the traffic environment. 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 labor intensity, spending long hours behind the wheel, spending little time off 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 accidents involving loss of control due to sudden illness by professional drivers are frequent. To reduce the impact on drivers and passengers, and to ensure a safe traffic environment, an intelligent safety assistance system is needed that can safely and efficiently complete pullover maneuvers even in the event of driver incapacitation.

[0003] During the autonomous parking process, the vehicle needs to face a complex road environment, which contains many potential risk events that need to be quantitatively evaluated. An accurate and adaptable risk assessment method can ensure the safety of drivers and passengers and the smooth flow of traffic in emergency scenarios. Therefore, the out-of-control vehicle emergency system should be based on risk assessment.

[0004] Autonomous decision-making can determine the required driving behavior and trajectory based on the acquired environmental and vehicle states. Among them, the driving risk field theory based on the classic method of artificial potential field in robotics is used as a real-time two-dimensional risk assessment method. It points out the similarities between field theory and driving risk. It uses field theory to quantify driving risk, reveals the human-vehicle-road interaction mechanism and the influence of various factors on driving safety, and can be used to predict the dynamic change trend of driving risk. It is suitable for the autonomous parking process of vehicles in emergency scenarios where the driver suddenly encounters a problem.

[0005] However, the motion planning methods in related technologies that are compatible with autonomous decision-making cannot 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 easy to fail to reach the target point and move periodically, which needs to be improved. Summary of the Invention

[0006] The present application provides a motion planning method and device for realizing emergency pull-over parking, so as to solve the technical problem in the related art that motion planning cannot adapt to autonomous decision-making, thereby making it difficult to ensure the safety and efficiency of controlling the emergency pull-over parking of the vehicle in the event of driver disability.

[0007] The first aspect of the present application provides a motion planning method for implementing emergency side parking, comprising the following steps: evaluating a current traffic situation according to perception information, and establishing a driving risk field, calculating the risk field strength and direction of each point; obtaining the risk field strength of the ego vehicle based on the risk field strength and direction of each point, and evaluating the intention of the other vehicle, to decide whether to execute the motion planning strategy of emergency side parking according to the intention of the other vehicle; and when executing the motion planning strategy of emergency side parking, constraining based on the constraint conditions obtained from the kinematic constraints of the vehicle and the mechanical structure of the ego vehicle, and planning the speed of the ego vehicle based on PID (Proportional Integral Derivative) until the emergency side parking is completed.

[0008] Optionally, in an embodiment of the present application, the establishment of the driving risk field and the calculation of the risk field strength and direction of each point comprise: based on the current traffic situation, obtaining a kinetic field formed by moving objects, a potential field formed by stationary objects and a behavior field formed by drivers; synthesizing the driving risk field according to the kinetic field, the potential field and the behavior field; and calculating the risk field strength and direction formed by various traffic elements on the road to any driving vehicle.

[0009] Optionally, in an embodiment of the present application, the decision whether to execute the motion planning strategy of emergency side parking according to the intention of the other vehicle comprises: if the transverse direction field strength of the road points to the opposite direction of the emergency lane, executing the cruise strategy of decelerating along the current lane; and if the transverse direction field strength of the road points to the same direction of the emergency lane, executing the motion planning strategy of emergency side parking.

[0010] Optionally, in an embodiment of the present application, the constraint based on the constraint conditions obtained from the kinematic constraints of the vehicle and the mechanical structure of the ego vehicle, and the planning of the speed of the ego vehicle based on PID, comprise: based on the association information between the speed of the ego vehicle and the cycle step length, taking the reduction of the speed of the ego vehicle to 0 as a always true loop exit condition.

[0011] Optionally, in an embodiment of the present application, the constraint based on the constraint conditions obtained from the kinematic constraints of the vehicle and the mechanical structure of the ego vehicle, and the planning of the speed of the ego vehicle based on PID, further comprise: modifying the target speed of the ego vehicle for speed planning to be associated with the position and heading information of the ego vehicle.

[0012] Optionally, in one embodiment of the present application, while constraining the vehicle based on the constraints obtained from the vehicle kinematic constraints and the mechanical structure of the vehicle, the speed of the vehicle is planned based on PID, and further includes: in the stage where the target mainly provides lateral gravity, determining the coordinates of the first target point based on the lateral position of the vehicle on the lane; in the stage where the lateral position of the vehicle has passed the second lane, determining the coordinates of the second target point based on the lateral position of the vehicle on the lane.

[0013] The second aspect of the present application provides a motion planning device for implementing emergency pull-over parking, including: a calculation module, which is used to evaluate the current traffic situation based on perception information, establish a driving risk field, and calculate the risk field strength at each point and direction; a judgment module, which is used to obtain the risk field strength at the vehicle based on the risk field strength and direction at each point, and evaluate the intention of the other vehicle, so as to decide whether to execute the motion planning strategy of emergency pull-over parking based on the intention of the other vehicle; and a planning module, which is used to plan the speed of the vehicle based on PID when executing the motion planning strategy of emergency pull-over parking, while constraining the constraints obtained from the vehicle kinematic constraints and the mechanical structure of the vehicle, until the emergency pull-over parking is completed.

[0014] Optionally, in one embodiment of the present application, the calculation module includes: an acquisition unit for acquiring the kinetic energy field formed by moving objects, the potential energy field formed by stationary objects, and the behavior field formed by the driver based on the current traffic situation; a synthesis unit for synthesizing a driving risk field based on the kinetic energy field, potential energy field, and behavior field; and a calculation unit for calculating the magnitude and direction of the risk field formed by various traffic elements on the road for any moving vehicle.

[0015] Optionally, in one embodiment of the present application, the judgment module includes: a first control unit, used to execute a cruising strategy of decelerating along the current lane when the lateral field strength of the road points in the opposite direction of the emergency lane; and a second control unit, used to execute the motion planning strategy of emergency pull-over parking when the lateral field strength of the road points in the same direction of the emergency lane.

[0016] Optionally, in one embodiment of the present application, the planning module includes: a judgment unit, configured to reduce the speed of the vehicle to 0 as a permanent loop exit condition based on association information between the speed of the vehicle and the loop step length.

[0017] Optionally, in one embodiment of the present application, the planning module further includes: a modification unit, configured to modify the target speed of the vehicle in the speed planning so as to establish an association with the position and heading information of the vehicle.

[0018] Optionally, in one embodiment of the present application, the planning module further includes: a first determination unit for determining the coordinates of a first target point based on the lateral position of the ego vehicle on the lane when the target mainly provides lateral gravity; and a second determination unit for determining the coordinates of a second target point based on the lateral position of the ego vehicle on the lane when the lateral position of the ego vehicle has passed the second lane.

[0019] The third aspect of the present application provides a vehicle, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the motion planning method for emergency pull-over parking as described in the above embodiment.

[0020] The fourth aspect of the present application provides a computer-readable storage medium, which stores a computer program. When the program is executed by a processor, it implements the motion planning method for implementing emergency pull-over parking as described above.

[0021] In the case of a driver disability, the embodiment of the present application can evaluate the current traffic situation and the intention of other vehicles based on perception information, plan a safe path based on the current traffic situation, decide whether to execute an emergency pull-over motion planning strategy based on the intention of other vehicles, and when executing the emergency pull-over motion planning strategy, while constraining the vehicle based on the constraints obtained from the vehicle's kinematic constraints and the mechanical structure of the vehicle, plan the speed of the vehicle based on PID until the emergency pull-over is completed, thereby realizing the vehicle's emergency response in the case of driver disability, so as to safely stop in a reasonable posture in the shortest possible time to await subsequent rescue. This solves the technical problem in the related art that motion planning cannot adapt to autonomous decision-making, making it difficult to ensure the safety and efficiency of controlling the vehicle's emergency pull-over in the case of driver disability.

[0022] Additional aspects and advantages of the present application will be given in part in the description below, and in part will become apparent from the description below, or will be learned through practice of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0024] Figure 1 A flowchart of a motion planning method for implementing emergency pull-over parking according to an embodiment of the present application;

[0025] Figure 2 A schematic diagram of a complete path of robot motion in an artificial potential field model of a motion planning method for implementing emergency pull-over parking according to one embodiment of the present application;

[0026] Figure 3 A schematic diagram of an ideal emergency parking risk field for implementing a motion planning method for emergency pull-over parking according to one embodiment of the present application;

[0027] Figure 4 A schematic diagram of a vehicle kinematic model for implementing a motion planning method for emergency pull-over parking according to one embodiment of the present application;

[0028] Figure 5 A schematic diagram of a vehicle kinematic model for implementing a motion planning method for emergency pull-over parking according to another embodiment of the present application;

[0029] Figure 6 A schematic diagram of a mechanical structure constraint algorithm for a vehicle in a motion planning method for implementing emergency pull-over parking according to one embodiment of the present application;

[0030] Figure 7 A control logic diagram of a PID control algorithm for implementing a motion planning method for emergency pull-over parking according to one embodiment of the present application;

[0031] Figure 8 Schematic diagram of a specific control effect of a PD (Proportional Derivative) speed control algorithm for implementing a motion planning method for emergency pull-over parking according to one embodiment of the present application;

[0032] Figure 9 Schematic diagram of a motion planning method for implementing emergency pull-over parking at different vehicle initial speeds according to one embodiment of the present application;

[0033] Figure 10 A schematic diagram showing a comparison before and after improvement of a speed planning algorithm for a motion planning method for implementing emergency pull-over parking according to one embodiment of the present application;

[0034] Figure 11 Schematic diagram of an application example of a modified speed planning algorithm for implementing a motion planning method for emergency pull-over parking according to one embodiment of the present application;

[0035] Figure 12 A schematic diagram of motion trajectories at different stages of an emergency pull-over parking process according to a motion planning method for implementing emergency pull-over parking according to one embodiment of the present application;

[0036] Figure 13 Schematic diagram of motion trajectories at different stages of an emergency pull-over parking process according to a motion planning method for implementing emergency pull-over parking according to another embodiment of the present application;

[0037] Figure 14A schematic diagram of a motion trajectory of a vehicle in a road coordinate system for implementing a motion planning method for emergency pull-over according to an embodiment of the present application;

[0038] Figure 15 A schematic diagram of the variation of the Y coordinate (left) and the vehicle heading angle (right) of a vehicle in a road coordinate system for implementing a motion planning method for emergency pull-over according to an embodiment of the present application over time during the process of emergency pull-over;

[0039] Figure 16 A flowchart of a motion planning method for emergency pull-over according to an embodiment of the present application;

[0040] Figure 17 A schematic diagram of a motion planning device for emergency pull-over according to an embodiment of the present application;

[0041] Figure 18 A schematic diagram of a vehicle according to an embodiment of the present application. DETAILED DESCRIPTION

[0042] Embodiments of the present application are described in detail below with reference to the accompanying drawings, in which the same or similar elements or elements having the same or similar functions are denoted by the same or similar reference numerals throughout. The embodiments described below by reference to the accompanying drawings are exemplary and are intended to explain the present application, and cannot be understood as limiting the present application.

[0043] The motion planning method and device for emergency pull-over according to the embodiments of the present application are described below with reference to the accompanying drawings. In view of the technical problems in the related art mentioned above that the motion planning cannot be adapted to autonomous decision-making, thereby making it difficult to ensure the safety and efficiency of controlling the vehicle to perform emergency pull-over in the case of driver incapacitation, the present application provides a motion planning method for emergency pull-over, in which in the case of driver incapacitation, the current traffic situation and the intentions of other vehicles can be evaluated based on perception information, a safe path can be planned based on the current traffic situation, it can be decided whether to execute the motion planning strategy for emergency pull-over based on the intentions of other vehicles, and when executing the motion planning strategy for emergency pull-over, the speed of the vehicle can be planned based on PID while being constrained by the constraint conditions obtained from the kinematics of the vehicle and the mechanical structure of the vehicle, until the emergency pull-over is completed, so as to realize the emergency response of the vehicle in the case of driver incapacitation, to safely stop in a reasonable posture in the shortest possible time, so as to wait for subsequent rescue. Thus, the technical problem in the related art that the motion planning cannot be adapted to autonomous decision-making, thereby making it difficult to ensure the safety and efficiency of controlling the vehicle to perform emergency pull-over in the case of driver incapacitation, is solved.

[0044] Specifically, Figure 1A flowchart of a motion planning method for implementing emergency pull-over parking provided in an embodiment of the present application.

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

[0046] In step S101, the current traffic situation is evaluated based on the perception information, and a driving risk field is established, and the magnitude and direction of the risk field at each point are calculated.

[0047] It can be understood that the driving risk field theory is a modeling theory applied to vehicle risk assessment, which is based on the idea of ​​the traditional robotics path planning method - the artificial potential field path planning method.

[0048] The basic idea of ​​the artificial potential field path planning method in related technologies is to abstract the movement of the robot in the surrounding space into the movement of a point in the potential energy field acting on it, and its basic theory is field theory.

[0049] 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

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

[0051]

[0052] According to the Cauchy-Schwarz inequality, we know that:

[0053]

[0054] The conditions for the equality in the above formula to hold are the direction u and the vector:

[0055]

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

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

[0058]

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

[0060] Based on the 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, so its movement direction is naturally the gradient direction.

[0061] The artificial potential field modeling can be as follows:

[0062] Target gravitational field:

[0063]

[0064] Obstacle Repulsion Field:

[0065]

[0066] Among them, 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, and k and η are both unknown coefficients.

[0067] Taking this as the potential function, we can get the vector field by taking its negative gradient:

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

[0069]

[0070] 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.

[0071] 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 very difficult and tedious, and it is difficult for a computer to numerically calculate the gradient of a certain point in the spatially distributed field function. Therefore, the spatially distributed vector field can be directly modeled to simplify the calculation.

[0072] In order to explore better vehicle safety decision-making methods, there are also related technologies that regard the vehicle's driving environment as a potential field and apply artificial potential field path planning methods to vehicle following behavior modeling and the design of driving safety assistance systems.

[0073] Furthermore, the risks posed by traffic factors to moving vehicles have similar characteristics to fields: the risks generated by the various components of the traffic system have 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 with time and space, and field quantities can be characterized as spatiotemporal functions; the impact of each traffic factor on driving and the field strength are both directional. Therefore, the embodiments of the present application can assess the current traffic situation based on sensory information, and it is reasonable to use fields to describe driving risks. This is the inherent principle of driving risk field theory, which establishes a driving risk field and calculates the magnitude and direction of the risk field at each point.

[0074] Optionally, in one embodiment of the present application, a driving risk field is established, and the strength and direction of the risk field at each point are calculated, including: based on the current traffic situation, obtaining the kinetic energy field formed by moving objects, the potential energy field formed by stationary objects, and the behavior field formed by the driver; synthesizing the driving risk field according to the kinetic energy field, potential energy field, and behavior field; calculating the strength and direction of the risk field formed by various traffic elements on the road for any moving vehicle.

[0075] In actual implementation, the embodiment of the present application can calculate the magnitude and direction of the risk field formed by various traffic factors on the road for any moving vehicle by establishing a driving risk field theoretical model. The driving risk field theoretical model can be 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 can be as follows:

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

[0077]

[0078] in, 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, and k2 are all parameters to be determined.

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

[0080]

[0081] in, 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 both unknown parameters.

[0082]

[0083] in, 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 Driver risk factors.

[0084]

[0085] By synthesizing the above three parts of the driving risk field, we can calculate the magnitude and direction of the risk field formed by various traffic elements on the road to a certain moving vehicle.

[0086] In step S102, the risk field strength at the vehicle is obtained based on the magnitude and direction of the risk field strength at each point, and the intention of the other vehicle is evaluated to decide whether to execute the motion planning strategy of emergency pull-over according to the intention of the other vehicle.

[0087] As a possible implementation, the embodiments of the present application can establish a driving risk field at any point in space through the above steps. The vehicle can then quantitatively assess the road risk by analyzing the impact of other traffic elements on the vehicle and identify the intentions of vehicles in adjacent lanes. By modeling a targeted risk field for emergency pull-over scenarios in the event of driver incapacitation, the embodiments of the present application can fully leverage the advantages of two-dimensional, real-time risk quantification, thereby enabling efficient decision-making.

[0088] Optionally, in one embodiment of the present application, a motion planning strategy for whether to execute an emergency pull-over parking is decided based on the intention of the other vehicle, including: if the lateral field strength of the road points in the opposite direction of the emergency lane, a cruising strategy of slowing down along the current lane is executed; if the lateral field strength of the road points in the same direction of the emergency lane, a motion planning strategy for emergency pull-over parking is executed.

[0089] In some embodiments, as Figure 3As shown in the figure, it is an ideal emergency stop risk field. The risk field generated by other vehicles in the figure can be 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 change decisions for emergency pull-over parking. The risk field in this area should be related to the relative speed between 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.

[0090] Specifically, if the aforementioned risk field is constructed, the decision-making method can be greatly simplified: The risk field strength of the target and other vehicles posing a risk 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 decelerates along the current lane, is executed. If the lateral field strength of the road points in the same direction as the emergency lane, the current state of vehicles in the adjacent lane poses no significant 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 conditions for executing the cruise algorithm are met during pullover.

[0091] In step S103 , when executing the motion planning strategy for emergency pull-over parking, the vehicle's speed is planned based on PID while being constrained based on the vehicle's kinematic constraints and the vehicle's mechanical structure until the emergency pull-over parking is completed.

[0092] During the actual execution process, the vehicle's kinematic constraints and its own mechanical structure are taken into consideration to improve the executability of the path planning method. In the embodiment of the present application, when executing the motion planning strategy for emergency pull-over parking, constraints are imposed based on the constraints obtained from the vehicle's kinematic constraints and the vehicle's mechanical structure, and the vehicle's speed is planned based on PID control until the vehicle completes the emergency pull-over parking, thereby achieving safe and efficient emergency parking motion planning.

[0093] Specifically, the vehicle kinematic constraints are:

[0094] Vehicles are different from robots. They are larger in size and have complex mechanical structures. Their motion needs to be described through geometric structure and kinematic analysis. Otherwise, the actual vehicle executableness of the obtained path is poor.

[0095] like Figure 4As shown in the figure, the kinematic model of the vehicle is 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.

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

[0097] Front wheel coordinates of the vehicle:

[0098]

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

[0100]

[0101] 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:

[0102] Front wheel speed of the vehicle:

[0103]

[0104] Vehicle rear wheel speed:

[0105]

[0106] 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,

[0107]

[0108] The embodiment of the present application can scalarize the above formula,

[0109]

[0110] In the embodiment of the present application, the above formula can be substituted into the rear wheel speed expression of the vehicle:

[0111] Vehicle rear wheel speed:

[0112]

[0113] Furthermore, the embodiment of the present application can calculate the magnitude and direction of the vehicle center velocity:

[0114]

[0115] In the embodiment of the present application, 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, such as Figure 5 Point T is shown in the figure, and l is the cycle step length.

[0116] By using the vehicle's velocity direction expression, the embodiment of the present application can inversely solve the steering angle required to reach the preview point:

[0117]

[0118] 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.

[0119] Mechanical structure of the vehicle:

[0120] The embodiment of the present application can constrain the steering angle and steering wheel speed according to the mechanical structure of the vehicle. The algorithm is as follows: Figure 6 As shown in the figure, δ0 is the maximum angle of the steering angle change 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.

[0121] The steering angle that the vehicle should currently perform is thus obtained. Based on the steering angle of the vehicle, the embodiment of the present application can further calculate the position and posture of the vehicle at the next moment:

[0122]

[0123] So far, the embodiment of the present application can obtain a complete loop structure, so that a path with strong executableness for the vehicle can be obtained through the planning method by sequentially iterating the loop.

[0124] Furthermore, the embodiment of the present application can plan the vehicle speed based on the PID control concept.

[0125] PID control algorithm is a control algorithm that combines the three links of proportion, integration and differentiation. It is the most mature and widely used control algorithm in continuous systems. The PID control algorithm calculates the input deviation value according to the functional relationship of proportion, integration and differentiation, and the calculation result is used to control the output. For continuous control systems, the ideal PID control law is

[0126]

[0127] Among them, 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.

[0128] 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. PID control algorithm is a closed-loop control, and its specific control logic is as follows: Figure 7 As shown, therefore, 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.

[0129] 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 above advantages of the PID control algorithm, the embodiments of the present application can use the PID concept to perform speed planning for the vehicle.

[0130] Applied to vehicle speed control, 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, since there is no steady-state error in the system, the integral control term can be simply ignored, so the ideal PID control law of the continuous control system is rewritten as the PD control law of the discrete control system:

[0131]

[0132] Among them, 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.

[0133] The specific control effect of the PD speed control algorithm can be as follows Figure 8 shown.

[0134] Furthermore, the embodiment of the present application can constrain the parameters of the PD speed control algorithm. Although the vehicle should be decelerated to 0 as soon as possible to cope 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, that is, the parameter K p There are restrictions on selection. Figure 8 As shown in the figure, the maximum acceleration of the vehicle deceleration is obtained at the initial moment of the speed control algorithm, that is,

[0135]

[0136] Among them, err(-1) is defined as the error function when the PD speed control algorithm is not started, that is,

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

[0138] but

[0139]

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

[0141] Optionally, in one embodiment of the present application, while constraining based on the constraints obtained from the vehicle kinematic constraints and the mechanical structure of the vehicle, the speed of the vehicle is planned based on PID, including: based on the correlation information between the speed of the vehicle and the loop step, reducing the speed of the vehicle to 0 as a permanent true loop exit condition.

[0142] In the actual implementation process, in order to achieve safe and efficient emergency pull-over parking, the embodiment of the present application needs to be further optimized and improved based on the planning method of path-speed coupling. First, the target point of the path planning method is fixed, which cannot ensure that the vehicle can arrive normally without periodic oscillation in a complex road environment; second, if the vehicle can arrive at the fixed target point normally under certain circumstances, the path planning method cannot guarantee that the vehicle's posture is parallel to the road at the time of parking; finally, due to the path-speed separation planning, it is impossible to take into account the vehicle's position information and speed information, and it may happen that the vehicle's speed has been reduced to 0 before it reaches the emergency lane or its posture is not parallel to the road. The embodiment of the present application can couple the path and speed, which can better solve the above problems, while coupling the originally separated algorithms requires the previous algorithm to be revised.

[0143] In some embodiments, the loop step length of the path planning method in the above steps and the permanent loop termination condition can be modified. The loop step length in the path method of the related art is a fixed value, while the discrete loop step length in the embodiment of the present application can be associated with the speed, and 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 path planning method of the related art is that the distance between the vehicle position and the fixed target point is within a certain range, while the fixed target point idea will be abandoned in the embodiment of the present application, and since the vehicle speed and the loop step length have been associated, the vehicle speed can be reduced to 0 as the permanent loop exit condition.

[0144] Optionally, in one embodiment of the present application, while constraining the vehicle based on the constraints obtained from the vehicle kinematic constraints and the mechanical structure of the vehicle, the speed of the vehicle is planned based on PID, and it also includes: modifying the target speed of the vehicle in the speed planning to establish an association with the position and heading information of the vehicle.

[0145] Specifically, the embodiment of the present application can modify the speed planning algorithm in the above steps and establish an association with the vehicle position and heading information. In order to enable the vehicle to decelerate to 0 when and only when it reaches the emergency lane or the vehicle posture meets the requirements, the embodiment of the present application can associate the speed planning output based on the PID idea with the vehicle position and heading information, that is, modify the target speed of the speed planning so that it is associated with the vehicle position and heading information.

[0146] The above operation is equivalent to setting a guide speed for each Y in the road coordinate system. For simplicity, the embodiment of the present application can set the guide speed V to be proportional to the lateral distance between the current coordinates of the vehicle and the center line of the emergency lane. Only when the vehicle reaches the center line of the emergency lane and the guide speed is 0, its speed can be reduced to 0 under the speed control algorithm.

[0147] Figure 10 Schematic diagram for comparison before and after the improved algorithm.

[0148] like Figure 10 As shown in the figure, due to the existence of the guide speed, the deceleration of the vehicle is slowed down to a certain extent. Compared with the algorithm before the improvement, the time of the lane change process during the pull-over parking is saved to a certain extent. At the same time, the improved speed control algorithm has stronger robustness. In response to the sudden deceleration caused by avoiding vehicles in the adjacent lane during the pull-over parking process, it can make corresponding acceleration and deceleration processing according to the guide speed, thereby improving the efficiency of the lane change process during the pull-over parking and avoiding unnecessary time waste.

[0149] like Figure 11 The figure shows an application example of the modified speed planning algorithm.

[0150] 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.

[0151] Optionally, in one embodiment of the present application, while constraining based on the constraints obtained from the vehicle kinematic constraints and the mechanical structure of the ego vehicle, the speed of the ego vehicle is planned based on PID, and it also includes: in the stage where the target mainly provides lateral gravity, determining the coordinates of the first target point based on the lateral position of the ego vehicle on the lane; in the stage where the lateral position of the ego vehicle has passed the second lane, determining the coordinates of the second target point based on the lateral position of the ego vehicle on the lane.

[0152] It's understandable that path planning algorithms in related technologies 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, an emergency pull-over scenario is dynamic and only requires the vehicle to safely stop in the emergency lane. Its path planning target is a line, with no specific stopping location required. Therefore, if a fixed target point is still used, set at a fixed location in the emergency lane, it would not meet the model requirements well and could even hinder functional implementation.

[0153] To this end, the embodiment of the present application can make the coordinates of the target point changeable in the path planning algorithm loop, changing the function of the target point in the path planning algorithm, and optimizing 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.

[0154] Under this concept, the target point can be used as a source of gravity, and its main functions are different at different stages of the emergency parking process, such as Figure 12 As shown in the figure, in the first stage, the target mainly provides lateral attraction to enable the ego vehicle to start pulling over 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 of the vehicle in the second lane has been completed, the target mainly provides traction for the ego vehicle to adjust its parking posture.

[0155] From the above analysis, the target position should be related to the lateral position of the vehicle on the lane, as follows: Figure 13 As shown, the target point should always be at the center line of the emergency lane, i.e. Figure 13 The target point moves parallel to the dotted line of the road. In the first stage, it should be below the dotted line I, and in the second stage, it should be above the dotted line II.

[0156] In the embodiment of the present application, the characteristic function may be used to roughly fit the target point position:

[0157]

[0158] Among them, (x car ,y car ) is the ego vehicle coordinate, ψ is the ego vehicle heading angle, ψ0 is the ego vehicle heading angle from the first stage to the second stage, which is an adjustable parameter related to the road width, and C is an undetermined constant that makes the target point a fixed distance ahead of the ego vehicle along the road direction.

[0159] The transition from the first stage to the second stage is a decision-making process that occurs after the timing is determined to be ripe. This is irreversible (one cannot return to the first stage from the second stage). When implementing the code, a motion planning switch must be added: after the second stage is executed, the switch must be turned off. This prevents execution even if the first stage conditional statements are met.

[0160] When the initial speed of the vehicle is 40km / h, the effect is as follows: Figure 14 、 15 shown.

[0161] Figure 14 It represents the motion trajectory of the vehicle in the road coordinate system and can meet the basic requirements of emergency parking. Figure 15 The time-varying patterns of the ego vehicle's Y coordinate and heading angle within the road coordinate system during an emergency pullover are shown. As time passes, 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 ego vehicle posture control. However, slight oscillations still occur during pullovers, which can be optimized through the design of a road risk field. This is not the focus of this study but is not explained in detail.

[0162] In summary, this application implements decision-making and motion planning based on driving risk field theory in real time, 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 deal with driver incapacitation. The innovation of this method lies mainly in: for motion planning methods, most currently adopt path-speed separation planning methods, which comprehensively consider path and speed to implement path-speed coupling motion planning methods; it proposes the idea of ​​moving targets 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.

[0163] Combine Figures 3 to 16 As shown, the working principle of the motion planning method for realizing emergency pull-over parking in an embodiment of the present application is described in detail with an embodiment.

[0164] like Figure 16 As shown, the embodiment of the present application may include the following steps:

[0165] Step S1601: By perceiving and assessing the traffic situation, a driving risk field is established and the strength of the risk field at each point is calculated. In actual implementation, the embodiment of the present application can establish a driving risk field theoretical model to calculate the strength and direction of the risk field formed by various traffic factors on the road for any moving vehicle. The driving risk field theoretical model can be composed of three parts: the kinetic energy field formed by moving objects, the potential energy field formed by stationary objects, and the behavioral field formed by the driver. The modeling can be as follows:

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

[0167]

[0168] in, 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, and k2 are all parameters to be determined.

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

[0170]

[0171] in, 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 both unknown parameters.

[0172]

[0173] in, 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 Driver risk factors.

[0174]

[0175] By synthesizing the above three parts of the driving risk field, we can calculate the magnitude and direction of the risk field formed by various traffic elements on the road to a certain moving vehicle.

[0176] Step S1602: Evaluate the intentions of other vehicles based on the risk field strength at the ego vehicle and decide whether to execute the pullover motion planning strategy. As one possible implementation, in this embodiment of the present application, after establishing the driving risk field at any point in space in the above steps, the ego vehicle can then quantitatively assess the road risk by considering the risk effects of other traffic elements on the ego vehicle and identify the intentions of vehicles in adjacent lanes, i.e., the intentions of other vehicles.

[0177] In some embodiments, as Figure 3As shown in the figure, it is an ideal emergency stop risk field. The risk field generated by other vehicles in the figure can be 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 change decisions for emergency pull-over parking. The risk field in this area should be related to the relative speed between 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.

[0178] Specifically, if the aforementioned risk field is constructed, the decision-making method can be greatly simplified: The risk field strength of the target and other vehicles posing a risk 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 decelerates along the current lane, is executed. If the lateral field strength of the road points in the same direction as the emergency lane, the current state of vehicles in the adjacent lane poses no significant 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 conditions for executing the cruise algorithm are met during pullover.

[0179] Step S1603: Consider the vehicle's own mechanical structure and kinematic constraints to improve the feasibility of the path planning method.

[0180] Specifically, the vehicle kinematic constraints are:

[0181] like Figure 4 As shown in the figure, the kinematic model of the vehicle is 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.

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

[0183] Front wheel coordinates of the vehicle:

[0184]

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

[0186]

[0187] 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:

[0188] Front wheel speed of the vehicle:

[0189]

[0190] Vehicle rear wheel speed:

[0191]

[0192] 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,

[0193]

[0194] The embodiment of the present application can scalarize the above formula,

[0195]

[0196] In the embodiment of the present application, the above formula can be substituted into the rear wheel speed expression of the vehicle:

[0197] Vehicle rear wheel speed:

[0198]

[0199] Furthermore, the embodiment of the present application can calculate the magnitude and direction of the vehicle center velocity:

[0200]

[0201] In the embodiment of the present application, 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, such as Figure 5 Point T is shown in the figure, and l is the cycle step length.

[0202] By using the vehicle's velocity direction expression, the embodiment of the present application can inversely solve the steering angle required to reach the preview point:

[0203]

[0204] 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.

[0205] Mechanical structure of the vehicle:

[0206] The embodiment of the present application can constrain the steering angle and steering wheel speed according to the mechanical structure of the vehicle. The algorithm is as follows: Figure 6 As shown in the figure, δ0 is the maximum angle of the steering angle change 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.

[0207] The steering angle that the vehicle should currently perform is thus obtained. Based on the steering angle of the vehicle, the embodiment of the present application can further calculate the position and posture of the vehicle at the next moment:

[0208]

[0209] So far, the embodiment of the present application can obtain a complete loop structure, so that a path with strong executableness for the vehicle can be obtained through the planning method by sequentially iterating the loop.

[0210] In some embodiments, the loop step length of the path planning method and the termination condition of the permanent loop in the above steps can be modified. In the embodiments of the present application, the idea of ​​a fixed target point can be abandoned, and since the vehicle speed is associated with the loop step length, the vehicle speed can be reduced to 0 as the exit condition of the permanent loop.

[0211] Step S1604: Planning the vehicle speed based on the PID control concept. In the embodiment of the present application, the vehicle speed can be planned based on the PID control concept.

[0212] For continuous control systems, the ideal PID control law is

[0213]

[0214] Among them, 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.

[0215] PID control algorithm is a closed-loop control, and its specific control logic is as follows Figure 7 As shown, therefore, 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.

[0216] Applied to vehicle speed control, 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, since the system does not exist steady-state error, can be simply ignored integral control term, so the ideal PID control law of continuous control system is rewritten as PD control law of discrete control system:

[0217]

[0218] Wherein, K p is a proportional gain, T d is a differential time constant, Δt is a cycle time interval, i is a cycle number, u(i) is a PD controller output signal, i.e. a speed change in a cycle time (Δt=0.01s), and err(i) is a difference between a vehicle speed and a target speed.

[0219] The specific control effect of the PD speed control algorithm can be shown as Figure 8 .

[0220] Further, the embodiment of the application can constrain the PD speed control algorithm parameters, although it should be reduced to 0 as soon as possible in response to an emergency scene, and the deceleration of the vehicle has an upper limit due to its dynamic model, i.e. the maximum value of u has an upper limit, i.e. the parameter K p is selected with a limit. As shown in Figure 8 , the maximum acceleration of the vehicle deceleration is obtained at the initial time of the speed control algorithm, i.e.

[0221]

[0222] Wherein, err(-1) is defined as an error function when the PD speed control algorithm is not started. That is

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

[0224] Then

[0225]

[0226] Under this constraint, the planning under different initial speeds of the vehicle is shown as Figure 9 , which conforms to the vehicle dynamics model and has strong executability.

[0227] Further, the embodiment of the application can modify the speed planning algorithm in the above steps and establish a correlation with the vehicle position and heading information. In order to enable the vehicle to decelerate to 0 only when the emergency lane is reached or the vehicle attitude meets the requirements, the embodiment of the application can correlate the speed planning output based on the PID idea with the vehicle position and heading information, i.e. modify the target speed of the speed planning to correlate with the vehicle position and heading information.

[0228] The above operation is equivalent to setting a guide speed for each Y in the road coordinate system. For simplicity, the embodiment of the present application can set the guide speed V to be proportional to the lateral distance between the current coordinates of the vehicle and the center line of the emergency lane. Only when the vehicle reaches the center line of the emergency lane and the guide speed is 0, its speed can be reduced to 0 under the speed control algorithm.

[0229] Figure 10 Schematic diagram for comparison before and after the improved algorithm.

[0230] like Figure 10 As shown in the figure, due to the existence of the guide speed, the deceleration of the vehicle is slowed down to a certain extent. Compared with the algorithm before the improvement, the time of the lane change process during the pull-over parking is saved to a certain extent. At the same time, the improved speed control algorithm has stronger robustness. In response to the sudden deceleration caused by avoiding vehicles in the adjacent lane during the pull-over parking process, it can make corresponding acceleration and deceleration processing according to the guide speed, thereby improving the efficiency of the lane change process during the pull-over parking and avoiding unnecessary time waste.

[0231] like Figure 11 The figure shows an application example of the modified speed planning algorithm.

[0232] 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.

[0233] Step S1605: Modify the target position within the loop by moving the target. This embodiment of the present application allows the target coordinates to be modified within the path planning algorithm loop, changing the function of the target in the path planning algorithm and optimizing the target from a spatial point that must be reachable to a source of gravity that provides guidance and traction.

[0234] The target point can be used as a source of attraction and has different main functions at different stages of the emergency parking process, such as Figure 12 As shown in the figure, in the first stage, the target mainly provides lateral attraction to enable the ego vehicle to start pulling over 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 of the vehicle in the second lane has been completed, the target mainly provides traction for the ego vehicle to adjust its parking posture.

[0235] From the above analysis, the target position should be related to the lateral position of the vehicle on the lane, as follows: Figure 13 As shown, the target point should always be at the center line of the emergency lane, i.e. Figure 13 The target point moves parallel to the dotted line of the road. In the first stage, it should be below the dotted line I, and in the second stage, it should be above the dotted line II.

[0236] In the embodiment of the present application, the characteristic function may be used to roughly fit the target point position:

[0237]

[0238] Among them, (x car ,y car ) is the ego vehicle coordinate, ψ is the ego vehicle heading angle, ψ0 is the ego vehicle heading angle from the first stage to the second stage, which is an adjustable parameter related to the road width, and C is an undetermined constant that makes the target point a fixed distance ahead of the ego vehicle along the road direction.

[0239] The transition from the first stage to the second stage is a decision-making process that occurs after the timing is determined to be ripe. This is irreversible (one cannot return to the first stage from the second stage). When implementing the code, a motion planning switch must be added: after the second stage is executed, the switch must be turned off. This prevents execution even if the first stage conditional statements are met.

[0240] When the initial speed of the vehicle is 40km / h, the effect is as follows: Figure 14 、 15 shown.

[0241] Figure 14 It represents the motion trajectory of the vehicle in the road coordinate system and can meet the basic requirements of emergency parking. Figure 15 The graphs show how the Y coordinate and heading angle of the ego vehicle in the road coordinate system change over time during the emergency pull-over process. It can be seen that over time, the Y coordinate in the road coordinate system approaches the centerline of the emergency lane, while the heading angle approaches 0, indicating that the algorithm has basically achieved the requirements for ego vehicle posture control.

[0242] According to the motion planning method for realizing emergency pull-over parking proposed in the embodiment of the present application, when the driver is incapacitated, the current traffic situation and the intention of other vehicles can be evaluated based on the perception information, a safe path can be planned based on the current traffic situation, and a motion planning strategy for whether to execute emergency pull-over parking can be decided based on the intention of other vehicles. When executing the motion planning strategy for emergency pull-over parking, the vehicle's speed can be planned based on PID while being constrained based on the constraints obtained from the vehicle's kinematic constraints and the mechanical structure of the vehicle until the emergency pull-over is completed, thereby realizing the vehicle's emergency response in the case of driver incapacity, so as to safely park in a reasonable posture in the shortest possible time, so as to await subsequent rescue. This solves the technical problem in the related art that motion planning cannot adapt to autonomous decision-making, making it difficult to ensure the safety and efficiency of controlling the vehicle's emergency pull-over parking in the case of driver incapacity.

[0243] Next, a motion planning device for implementing emergency pull-over parking according to an embodiment of the present application will be described with reference to the accompanying drawings.

[0244] Figure 17is a block schematic diagram of a motion planning device for emergency pull-over parking implemented by an embodiment of the present application.

[0245] As shown in the figure, the motion planning device for emergency pull-over parking 10 comprises a calculation module 100, a judgment module 200 and a planning module 300. Figure 17

[0246] Specifically, the calculation module 100 is configured to evaluate a current traffic situation according to perception information and establish a driving risk field, and calculate a risk field intensity and a direction of each point.

[0247] The judgment module 200 is configured to obtain a risk field intensity of the ego vehicle based on the risk field intensity and the direction of each point, and evaluate an intention of a he vehicle, so as to decide whether to execute a motion planning strategy for emergency pull-over parking according to the intention of the he vehicle.

[0248] The planning module 300 is configured to, when executing the motion planning strategy for emergency pull-over parking, constrain based on constraint conditions obtained from vehicle kinematics constraints and mechanical structure of the ego vehicle, and plan a speed of the ego vehicle based on PID until the emergency pull-over parking is completed.

[0249] Optionally, in an embodiment of the present application, the calculation module 100 comprises an acquisition unit, a synthesis unit and a calculation unit.

[0250] The acquisition unit is configured to acquire a kinetic field formed by moving objects, a potential field formed by stationary objects and a behavior field formed by drivers based on the current traffic situation.

[0251] The synthesis unit is configured to synthesize the driving risk field according to the kinetic field, the potential field and the behavior field.

[0252] The calculation unit is configured to calculate a risk field intensity and a direction of each point formed by various traffic elements on the road.

[0253] Optionally, in an embodiment of the present application, the judgment module 200 comprises a first control unit and a second control unit.

[0254] The first control unit is configured to execute a cruise strategy of decelerating along a current lane when a transverse direction field intensity of the road points to an opposite direction of an emergency lane.

[0255] The second control unit is configured to execute the motion planning strategy for emergency pull-over parking when the transverse direction field intensity of the road points to the same direction of the emergency lane.

[0256] Optionally, in an embodiment of the present application, the planning module 300 comprises a judgment unit.

[0257] ​The judgment unit is configured to reduce the speed of the vehicle to 0 as a permanent loop exit condition based on correlation information between the speed of the vehicle and the loop step length.

[0258] Optionally, in one embodiment of the present application, the planning module 300 further includes: a modification unit.

[0259] The modification unit is used to modify the target speed of the vehicle in the speed planning to establish an association with the position and heading information of the vehicle.

[0260] Optionally, in one embodiment of the present application, the planning module 300 further includes: a first determining unit and a second determining unit.

[0261] The first determination unit is configured to determine the coordinates of the first target point based on the lateral position of the vehicle on the lane when the target mainly provides lateral attraction.

[0262] The second determining unit is configured to determine the coordinates of the second target point based on the lateral position of the vehicle on the lane when the lateral position of the vehicle has passed the second lane.

[0263] It should be noted that the above explanation of the embodiment of the motion planning method for implementing emergency pull-over parking is also applicable to the motion planning device for implementing emergency pull-over parking in this embodiment, and will not be repeated here.

[0264] According to the motion planning device for realizing emergency pull-over parking proposed in the embodiment of the present application, when the driver is incapacitated, the current traffic situation and the intention of other vehicles can be evaluated based on the perception information, a safe path can be planned based on the current traffic situation, and a motion planning strategy for whether to execute emergency pull-over parking can be decided based on the intention of other vehicles. When executing the motion planning strategy for emergency pull-over parking, the vehicle's speed can be planned based on PID while being constrained based on the constraints obtained from the vehicle's kinematic constraints and the mechanical structure of the vehicle until the emergency pull-over is completed, thereby realizing the vehicle's emergency response in the case of driver incapacity, so as to safely park in a reasonable posture in the shortest possible time, so as to wait for subsequent rescue. Thus, the technical problem in the related art that motion planning cannot adapt to autonomous decision-making, making it difficult to ensure the safety and efficiency of controlling the vehicle's emergency pull-over parking in the case of driver incapacity, is solved.

[0265] Figure 18 A schematic diagram of the structure of a vehicle provided in an embodiment of the present application. The vehicle may include:

[0266] Memory 1801 , processor 1802 , and computer programs stored in the memory 1801 and executable on the processor 1802 .

[0267] The processor 1802 implements the motion planning method for implementing emergency lane keeping provided in the above embodiments when executing a program.

[0268] Further, the vehicle further comprises:

[0269] The communication interface 1803 is configured to communicate between the memory 1801 and the processor 1802.

[0270] The memory 1801 is configured to store a computer program executable on the processor 1802.

[0271] The memory 1801 can include a high-speed RAM memory, and can further include a non-volatile memory, for example, at least one disk memory.

[0272] If the memory 1801, the processor 1802 and the communication interface 1803 are independently implemented, the communication interface 1803, the memory 1801 and the processor 1802 can be connected to each other through a bus and complete communication between each other. The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 18 Only one thick line is used in the figure, but it does not mean that there is only one bus or only one type of bus.

[0273] Optionally, in a specific implementation, if the memory 1801, the processor 1802 and the communication interface 1803 are integrated on a chip, the memory 1801, the processor 1802 and the communication interface 1803 can complete communication between each other through an internal interface.

[0274] The processor 1802 can be a central processing unit (CPU), or an application specific integrated circuit (ASIC), or one or more integrated circuits configured to implement the embodiments of the present application.

[0275] The embodiments also provide a computer readable storage medium, having stored thereon a computer program, which, when executed by a processor, implements the motion planning method for implementing emergency lane keeping as above.

[0276] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" means 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 application. 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 N 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.

[0277] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be understood to indicate or imply relative importance or implicitly specify the number of technical features indicated. Thus, a feature specified as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of this application, "N" means at least two, for example, two, three, etc., unless otherwise specifically defined.

[0278] Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, fragment or portion of code comprising one or N executable instructions for implementing a custom logical function or process step, and the scope of the preferred embodiments of the present application includes alternative implementations in which functions may be performed in a different order than shown or discussed, including performing functions in a substantially simultaneous manner or in a reverse order depending on the functions involved, which should be understood by those skilled in the art to which the embodiments of the present application pertain.

[0279] The logic and / or steps represented in the flowcharts or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing the logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (e.g., a computer-based system, a system including a processor, or other system that can fetch and execute instructions from an instruction execution system, apparatus, or device). For purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport a program for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include the following: an electrical connection with one or N wires (electronic devices), a portable computer disk cartridge (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and programmable read-only memory (EPROM or flash memory), fiber optic devices, and a portable compact disc read-only memory (CDROM). In addition, the computer-readable medium may even be paper or other suitable medium on which the program is printed, since the program can be obtained electronically by optically scanning the paper or other medium and then editing, interpreting or processing it in other suitable ways as necessary, and then storing it in a computer memory.

[0280] It should be understood that various parts of the present application can be implemented using hardware, software, firmware, or a combination thereof. In the above embodiment, the N steps or methods can be implemented using software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented using hardware, as in another embodiment, any one of the following technologies known in the art or a combination thereof can be used to implement: a discrete logic circuit having a logic gate circuit for implementing a logic function on a data signal, an application-specific integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0281] Those skilled in the art will understand that all or part of the steps in the method of the above embodiment can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

[0282] In addition, the functional units in the various embodiments of the present application may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into a module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0283] The storage medium mentioned above may be a read-only memory, a magnetic disk, or an optical disk, etc. Although the embodiments of the present application have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present application. Persons skilled in the art may make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present application.

Claims

1. A motion planning method for implementing emergency pull-over parking, characterized in that: The following steps are involved: Evaluate the current traffic situation based on the perceived information, establish a driving risk field, and calculate the magnitude and direction of the risk field at each point; The risk field strength at the vehicle is determined based on the magnitude and direction of the risk field strength at each point, and the intention of the other vehicle is evaluated to determine whether to execute the motion planning strategy for emergency pull-over according to the intention of the other vehicle; as well as When executing the motion planning strategy for the emergency pull-over, the vehicle's speed is planned based on the PID controller while being constrained based on the vehicle's kinematic constraints and the vehicle's mechanical structure until the emergency pull-over is completed. The constraining based on the constraints obtained from the vehicle kinematic constraints and the mechanical structure of the ego vehicle includes: converting the front and rear wheel positions of the ego vehicle into coordinates in a road coordinate system, calculating the rate of change of the coordinates with respect to time to obtain the front and rear wheel speeds of the ego vehicle, and calculating the center motion speed and motion direction of the ego vehicle based on the front and rear wheel speeds, and using the motion direction to obtain the steering angle corresponding to the preview point; using the mechanical structure to constrain the steering angle and steering wheel speed to obtain the maximum steering wheel rotation angle within one cycle time of the maximum steering angle under the constraints; The PID-based speed planning of the ego vehicle until the emergency side parking is completed includes: planning the speed of the ego vehicle using a speed planning algorithm formed by a discrete control system PD control law, correcting the speed planning algorithm, and associating a target speed obtained by the corrected speed planning algorithm with the ego vehicle's position and heading information, adjusting target point coordinates using the effect of lateral gravity on the ego vehicle, and coupling a path formed by the target point coordinates with the target speed to perform speed planning until the emergency side parking is completed.

2. The method according to claim 1, characterized in that The establishment of the driving risk field and calculation of the magnitude and direction of the risk field at each point include: Based on the current traffic situation, obtaining a kinetic energy field formed by moving objects, a potential energy field formed by stationary objects, and a behavior field formed by a driver; synthesizing a driving risk field according to the kinetic energy field, potential energy field and behavior field; Calculate the magnitude and direction of the risk field generated by various traffic elements on the road to any moving vehicle.

3. The method according to claim 1, characterized in that The motion planning strategy for deciding whether to perform an emergency pull-over according to the other vehicle's intention includes: If the lateral field strength of the road points in the opposite direction of the emergency lane, the cruise strategy of slowing down along the current lane is executed; If the lateral field strength of the road points in the same direction as the emergency lane, the motion planning strategy for emergency pull-over parking is executed.

4. The method according to claim 1, wherein The method of planning the speed of the ego vehicle based on PID while constraining the ego vehicle based on the constraints obtained from the vehicle kinematic constraints and the mechanical structure of the ego vehicle includes: Based on the correlation information between the speed of the ego vehicle and the loop step length, the speed of the ego vehicle is reduced to 0 as a permanent loop exit condition.

5. The method according to claim 4, characterized in that The method of planning the speed of the ego vehicle based on PID while constraining the ego vehicle based on the constraints obtained from the vehicle kinematic constraints and the mechanical structure of the ego vehicle further includes: In a stage where the target mainly provides lateral attraction, determining the coordinates of a first target point based on the lateral position of the ego vehicle on the lane; When the lateral position of the vehicle has passed the second lane, the coordinates of the second target point are determined based on the lateral position of the vehicle on the lane.

6. A motion planning device for implementing emergency pull-over parking, characterized in that: include: The calculation module is used to evaluate the current traffic situation based on the perception information, establish a driving risk field, and calculate the magnitude and direction of the risk field at each point; A judgment module is used to determine the risk field strength at the vehicle based on the magnitude and direction of the risk field strength at each point, and to evaluate the intention of the other vehicle to decide whether to execute the motion planning strategy of emergency pull-over according to the intention of the other vehicle; as well as a planning module for, when executing the motion planning strategy for the emergency pull-over, planning the speed of the ego vehicle based on PID while applying constraints derived from vehicle kinematic constraints and the mechanical structure of the ego vehicle until the emergency pull-over is completed; The planning module includes: converting the front and rear wheel positions of the ego vehicle into coordinates in a road coordinate system, calculating the rate of change of the coordinates with respect to time to obtain the front and rear wheel speeds of the ego vehicle, and calculating the center movement speed and movement direction of the ego vehicle based on the front and rear wheel speeds, so as to obtain the steering angle corresponding to the preview point using the movement direction; using the mechanical structure to constrain the steering angle and steering wheel speed to obtain the maximum steering wheel rotation angle within one cycle time of the maximum steering angle under the constraints; The planning module includes: planning the speed of the ego vehicle using a speed planning algorithm formed by the PD control law of the discrete control system, correcting the speed planning algorithm, and associating the target speed obtained by the corrected speed planning algorithm with the ego vehicle's position and heading information, adjusting the target point coordinates using the influence of lateral gravity on the ego vehicle, and coupling the path formed by the target point coordinates with the target speed for speed planning until the emergency side parking is completed.

7. The device according to claim 6, characterized in that The calculation module includes: an acquisition unit, configured to acquire, based on the current traffic situation, a kinetic energy field formed by moving objects, a potential energy field formed by stationary objects, and a behavior field formed by a driver; a synthesis unit, configured to synthesize a driving risk field according to the kinetic energy field, potential energy field and behavior field; The calculation unit is used to calculate the magnitude and direction of the risk field generated by various traffic elements on the road to any moving vehicle.

8. The device according to claim 6, characterized in that The judgment module includes: a first control unit, configured to execute a cruising strategy of decelerating along a current lane when the lateral field strength of the road points in the opposite direction of the emergency lane; The second control unit is configured to execute the motion planning strategy for emergency pull-over parking when the lateral field strength of the road points in the same direction as the emergency lane.

9. The device according to claim 6, characterized in that The planning module includes: The judgment unit is configured to reduce the speed of the vehicle to 0 as a permanent loop exit condition based on the correlation information between the speed of the vehicle and the loop step length.

10. The device according to claim 9, characterized in that The planning module also includes: a first determining unit, configured to determine the coordinates of a first target point based on a lateral position of the ego vehicle on the lane during a stage in which the target mainly provides lateral attraction; The second determining unit is configured to determine the coordinates of the second target point based on the lateral position of the ego vehicle on the lane when the lateral position of the ego vehicle has passed the second lane.

11. A vehicle, characterized in that: include: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the motion planning method for implementing emergency pull-over parking as described in any one of claims 1 to 5.

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

Images