Efficient driving interaction method and system based on backward reachable set safety guarantee
By solving the level set equation offline and building a table, combining online state search and threshold comparison, control instructions are generated, which solves the problem that intelligent driving vehicles are difficult to balance safety and comfort in uncertain environments, and achieves safe and efficient interactive control.
Patent Information
- Application Number
- CN202411162292.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-23
- Publication Date
- 2024-11-08
- Estimated Expiration
- 2044-08-23
AI Technical Summary
In an uncertain interactive environment, it is difficult for intelligent driving vehicles to find a balance between safety and comfort, making it difficult for control strategies to design safe and efficient interaction strategies.
By solving the horizontal set equation, the horizontal set function value is calculated offline, the mapping relationship between the vehicle state vector and the horizontal set function value is constructed, and the horizontal set function value is found online according to the vehicle state, and the threshold comparison is used to enter different control constraints. , generate control commands to control the vehicle's driving.
It realizes more accurately assessing risks in interactive scenarios, generates safe and efficient control instructions, avoids overly conservative control strategies, and improves vehicle traffic efficiency and passenger comfort.
Smart Images

Figure CN118907145A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of autonomous driving technology, and in particular to an efficient driving interaction method and system based on backward reachable set safety assurance. Background Art
[0002] With the continuous advancement of key technologies of intelligent networking, single-vehicle autonomous driving technology has gradually matured. In order to further act efficiently and safely in a dynamic and uncertain interactive environment, intelligent driving vehicles must consider the risks associated with their actions, as well as feedback interactions with other risk-sensitive agents, and avoid overly conservative control strategies. Ignoring either of these two elements makes it difficult to design a safe and efficient interactive strategy.
[0003] To this end, rigorous mathematical proof and reasoning are required to give a set of dangerous states, and to conduct interactive trials outside this set to strive for priority access, meet the expected goals of decision-making planning, and improve vehicle interaction efficiency. When approaching the edge of the set, additional control constraints must be imposed. Through a smooth control sequence, the vehicle can stay away from dangerous states while ensuring the riding comfort of passengers, avoiding the use of safety failure control strategies that maximize safety but lose comfort. Summary of the invention
[0004] Aiming at the problem that the existing intelligent driving vehicle control system in uncertain interactive environment is difficult to strike a balance between efficiency and comfort while generating a safe and reliable control sequence, the present invention provides an efficient control method and system based on backward reachable set safety assurance in an interactive scenario.
[0005] The technical solution adopted by the present invention is:
[0006] The present invention provides an efficient driving interaction method based on backward reachable set safety guarantee, comprising the following steps:
[0007] S1. Divide the sampling values of the vehicle state vector according to a certain grid size, calculate and store the level set function value offline by solving the level set equation, and construct a mapping relationship between the vehicle state vector and the level set function value to form a discrete table; when the level set function values are all zero, it corresponds to a zero level set, which corresponds to the safe distance boundary of the interactive vehicle relative to the self-vehicle;
[0008] S2, obtain the state vector of the vehicle online, and find the corresponding level set function value according to the discrete table;
[0009] S3, compare the found level set function value with the set threshold, and enter different control constraints according to the comparison result; if it is less than the first threshold, it enters the control hard constraint, and does not allow the vehicle to select a value other than the control amount; if it is greater than the second threshold, it enters the control soft constraint, and allows the vehicle to select the control amount within a certain range;
[0010] S4. Generate control instructions according to the control constraints to control the driving of the vehicle.
[0011] According to the above technical solution, the level set equation is constructed based on the positional relationship between the ego vehicle and the surrounding interactive vehicles and the overall system dynamics equation composed of the ego vehicle and the interactive vehicles.
[0012] According to the above technical solution, the state vector includes the position, speed, heading angle, yaw rate and the position, speed and heading angle of the interactive vehicle.
[0013] Following the above technical solution, after the vehicle state vector is obtained in step S2, it is filtered through an extended Kalman filter.
[0014] According to the above technical solution, if the level set function value is between the first threshold and the second threshold, no additional control constraint is introduced.
[0015] Following the above technical solution, in step S2, the level set function values are interpolated between the grid points to find the level set function value in any given state.
[0016] Following the above technical solution, in step S4, the control constraints are specifically substituted into the MPC solver to solve for safe and smooth control instructions.
[0017] The present invention also provides an efficient driving interaction system based on backward reachable set safety assurance, comprising:
[0018] A discrete table construction module is used to divide the sampled values of the vehicle state vector according to a certain grid size, calculate and store the level set function value offline by solving the level set equation, and construct a mapping relationship between the vehicle state vector and the level set function value to form a discrete table; when the level set function values are all zero, it corresponds to a zero level set, which corresponds to the safe distance boundary of the interactive vehicle relative to the self-vehicle;
[0019] The table lookup module is used to obtain the state vector of the vehicle online and find the corresponding level set function value according to the discrete table;
[0020] The control constraint selection module is used to compare the found level set function value with the set threshold value, and enter different control constraints according to the comparison result; if it is less than the first threshold value, it enters the control hard constraint, and does not allow the ego vehicle to select a value other than the control amount; if it is greater than the second threshold value, it enters the control soft constraint, and allows the ego vehicle to select the control amount within a certain range;
[0021] The control instruction generation module is used to generate control instructions according to control constraints to control the driving of the vehicle.
[0022] According to the above technical solution, the control constraint selection module is further configured to not introduce additional control constraints when the level set function value is between the first threshold and the second threshold.
[0023] The present invention also provides a computer storage medium, which stores a computer program that can be executed by a processor, and the computer program executes the efficient driving interaction method based on backward reachable set safety assurance described in the above technical solution.
[0024] The beneficial effects of the present invention are as follows: the present invention mainly utilizes the level set function value table obtained by offline solving the backward reachable set to explicitly express the risk level of the ego vehicle when the interactive vehicle interacts with the ego vehicle in different states; the level set function value in the current state can be more accurately found by online table search based on the collected vehicle state information; the corresponding control constraints are obtained based on the comparison between the level set function value and the threshold, and then the corresponding control instructions are generated to control the driving of the ego vehicle; the present invention avoids the impact of the method of generating emergency safety control only with safety as the guide on the traffic efficiency of the ego vehicle, the comfort of passengers and the normal driving of other traffic participants.
[0025] Furthermore, by filtering the vehicle status information collected by the sensor through an extended Kalman filter, interference information can be better deleted.
[0026] Furthermore, by comparing with the pre-set threshold, the safe range of the level set function value in the current state is found, and then different level set function gradient value constraints are given to narrow the set that can be used in the current control sequence.
[0027] Furthermore, in the process of online searching for the level set function value, the present invention performs interpolation processing between grid points to evaluate the level set function value in any given state, thereby meeting the real-time requirement.
[0028] Furthermore, the control constraint is introduced into the MPC solver to ensure that the solved control sequence will meet the evolution requirements of the level set function value.
[0029] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS
[0030] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.
[0031] Figure 1is a flow chart of an efficient driving interaction method based on backward reachable set safety assurance according to an embodiment of the present invention;
[0032] Figure 2 1 are several example slice diagrams of the level set function value of an embodiment of the present invention, where (a) is state 1 and (b) is state 2;
[0033] Figure 3 It is a flow chart for determining the safe range of the level set function value in the current state of an embodiment of the present invention. DETAILED DESCRIPTION
[0034] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.
[0035] It should be noted that the illustrations provided in the embodiments of the present invention are only used to illustrate the basic concept of the present invention in a schematic manner. Therefore, the drawings only show components related to the present invention rather than being drawn according to the number, shape and size of components in actual implementation. In actual implementation, the type, quantity and proportion of each component may be changed arbitrarily, and the component layout type may also be more complicated.
[0036] In the present invention, it is also necessary to explain that, if the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. appear, the orientation or position relationship indicated is based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present application and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present application. In addition, if the terms "first" and "second" appear, they are only used for description and distinction purposes, and cannot be understood as indicating or implying relative importance.
[0037] The present invention takes into account the impact of the behavior of the vehicle itself on the behavior of other vehicles in interactive scenarios, and improves the traffic efficiency in interactive scenarios by attempting to seize the right of way within a safe and permitted set range; in more dangerous conditions, a smoother method is used to generate a safe and reliable control sequence.
[0038] Example 1
[0039] like Figure 1 As shown, the efficient driving interaction method based on the safety guarantee of the backward reachable set in the embodiment of the present invention includes the following steps:
[0040] S1. Divide the sampling values of the vehicle state vector according to a certain grid size, calculate and store the level set function value offline by solving the level set equation, and construct a mapping relationship between the vehicle state vector and the level set function value to form a discrete table; when the level set function values are all zero, it corresponds to a zero level set, which corresponds to the safe distance boundary of the interactive vehicle relative to the self-vehicle;
[0041] S2, obtain the state vector of the vehicle online, and find the corresponding level set function value according to the discrete table;
[0042] S3. Compare the found level set function value with the set threshold, and enter different control constraints according to the comparison result; if it is less than the first threshold, it enters the control hard constraint, and the vehicle is not allowed to select a value other than the control amount; if it is greater than the second threshold, it enters the control soft constraint, and the vehicle is allowed to select the control amount within a certain range.
[0043] S4. Generate control instructions according to the control constraints to control the driving of the vehicle.
[0044] The level set method is a numerical technique that can express the complex topological changes of the boundary of the backward reachable set through simple set operations, handle complex surface parameterization problems from the perspective of numerical calculation, and finally give the safe distance boundary of the interactive vehicle relative to the self-vehicle. The level set equation used is as follows:
[0045]
[0046] in is the level set function that satisfies the Lipschitz continuity condition, f is the system dynamics equation, is the gradient of the level set function.
[0047] According to the distance boundary where the interactive vehicle may collide with the self-vehicle, a dangerous target set representing the final collision is predefined. The boundary of the target set is the level set function at the final value:
[0048]
[0049] Among them, x r and r They represent the horizontal and vertical coordinates of the interacting vehicle in the Cartesian coordinate system with the ego vehicle as the origin, i.e., the relative position; R represents the minimum safe distance. The larger the value of the level set function, the safer the current state.
[0050] The reason why the present invention solves the level set equation offline is that it is extremely difficult to solve the level set equation under the system dynamics equation of higher dimensions. Considering that the vehicle is a platform with limited computing power, the computing power demand is removed from the vehicle platform by gridding the relative state vectors of the self-vehicle and the interactive vehicle, and calculating the level set function value under the corresponding state offline. However, as a price, the self-vehicle will sacrifice a part of the storage space to record the discrete solution obtained offline, such as Figure 2 shown.
[0051] The relationship between the level set function value and the safety distance is as follows: Figure 2 As shown in the two states (a) and (b) in the figure, any position in the entire figure has its level set function value. When this value is 0, it is the zero level set, which is also the safety distance boundary. If this boundary is crossed, that is, the level set function value is less than 0, the interactive vehicle will inevitably collide with the ego vehicle. Therefore, it is required to impose a hard constraint when the level set function value is close to 0, so that the ego vehicle is far away from this boundary. Figure 2 The different states refer to the states of the self-vehicle and the interactive vehicle other than the horizontal and vertical relative positions. For example, in this embodiment, the vehicle state vector is a grid composed of 7-dimensional states, and the 7-dimensional state cannot be directly displayed, so two slices are taken, that is, the 5-dimensional state is fixed as a constant, and the remaining 2-dimensional state is a variable. Through such an operation, the mapping relationship between the 7-dimensional state and the 1-dimensional result can be extracted into a 2-dimensional state and a 1-dimensional result, and the mapping relationship can be directly displayed. Therefore, Figure 2 The 5 dimensions in the input are fixed, and the level set function values at different relative positions are displayed in a planar manner.
[0052] Furthermore, the design of the system dynamics equation f takes into account the relative motion characteristics between the ego vehicle and the interactive vehicle, and is the overall system dynamics equation consisting of the ego vehicle and the interactive vehicle.
[0053] The designed state vector x is as follows:
[0054]
[0055] Among them, x r and r They represent the horizontal and vertical coordinates of the interacting vehicle in the Cartesian coordinate system with the ego vehicle as the origin, i.e., the relative position; Expressed as relative heading angle; v rx 、v ey and v o They are respectively represented as the longitudinal and lateral speeds of the vehicle and the speed of the interacting vehicle; ω e It is represented as the yaw angular velocity of the vehicle.
[0056] Furthermore, the state vector gridding granularity is 27×20×9×20×10×20×5, which is uniformly distributed in the 7-dimensional space of state vector x∈[-6,20]×[-10,10]×[0,2π]×[1,20]×[-1,1]×[1,20]×[-1,1].
[0057] In step S2, the acquired vehicle state vectors may be filtered by an extended Kalman filter.
[0058] Furthermore, in the process of online searching for the level set function value, the present invention performs interpolation processing between grid points to evaluate the level set function value in any given state, thereby meeting the real-time requirements. At the same time, the method of replacing the most dangerous (for the ego vehicle) control state with the current control state of the interactive vehicle avoids the ego vehicle from taking an overly conservative control strategy while ensuring a safe response.
[0059] Specifically, the method for determining the level set function value in step S3 is as follows: if the current level set function value is less than the threshold value p1, a hard control constraint is introduced to prevent the control sequence selected by the controller from causing the ego vehicle to fall into an inevitable collision risk, thereby ensuring interactive safety; if the current level set function value is greater than the threshold value p2, a soft control constraint is introduced. When the controller selects an overly conservative control sequence, it will bear a larger objective function value as a penalty, thereby prompting the ego vehicle to adopt a control sequence that is conducive to improving traffic efficiency; if the current level set function value is greater than the threshold value p1 and less than the threshold value p2, no additional control constraint is introduced.
[0060] Among them, the control hard constraint set and the control soft constraint set are expressed as:
[0061]
[0062] Among them, U e (x) represents the control constraint set under state x, u e is the control quantity, satisfying the physical constraints of the vehicle execution unit:
[0063] u min ≤u e ≤u max
[0064] Step S4 can substitute the obtained control constraints into the MPC solver to solve the safe and smooth control instructions. The objective function and constraint conditions of the MPC used are:
[0065]
[0066] stx(k+1)=A k x(k)+B k u(k)
[0067] y(k)=C k x(k)
[0068] u(k)∈U e [x(k)]+S
[0069] Δu(k)=u(k)-u(k-1)
[0070] y min -ε≤y(k)≤y max +ε
[0071] u min ≤u(k)≤u max
[0072] Δu min ≤Δu(k)≤Δu max
[0073] In the formula, Q, R, ρ, and γ are the output deviation y(k+i)-y ref (k+i), control increment Δu(k+i), relaxation factor ε and weight matrix of S; P and C are state time domain and control time domain respectively; x(k), u(k), y(k) are state quantity, control quantity and output quantity at time k respectively; y max ,y min 、u max 、u min , Δu max , Δu min They are the upper and lower limits of the output quantity, control quantity and control increment respectively.
[0074] When the control constraint set is a hard constraint, S is zero, that is, the control amount is not allowed to take the set U e [x(k)]; when the control constraint set is a soft constraint, S is any positive value, that is, the control amount is allowed to take the set U e [x(k)], but the larger the value of S is, the larger the value of the objective function will be, which in turn limits the degree of relaxation when the soft constraint is applied.
[0075] Based on the obtained value of the level set function in the current state, the present invention introduces different control constraint strategies by comparing with the two set thresholds p1 and p2. Different from the general method of only setting the zero level set as the threshold for control triggering, and the triggered control instruction is the practice of safety maximization action, the control constraint generated by increasing or decreasing the level set function value is introduced into the MPC solution process, which ensures the traffic efficiency of the vehicle and the riding comfort of the passengers.
[0076] Example 2
[0077] The main steps of this embodiment are the same as those of embodiment 1, except for the method of solving the level set equation. In this embodiment, the level set function value is obtained by solving the viscosity solution of the Hamilton-Jacobi (HJ) partial differential equation.
[0078] The specific level set equation used is as follows:
[0079]
[0080] in is the level set function that satisfies the Lipschitz continuity condition, f is the system dynamics equation, is the gradient of the level set function.
[0081] The above level set equation is encoded into the terminal value HJ partial differential equation:
[0082]
[0083] in, is the Hamilton function, expressed as:
[0084]
[0085] Always less than or equal to 0, this is to ensure Always greater than or equal to 0, avoiding the level set function The reachable set of the definition shrinks over time.
[0086] Recorded as It is the level set function at the final value. The final value partial differential equation here is opposite to the initial value partial differential equation. The time in the final value partial differential equation is reversed from the current moment. Defined as:
[0087]
[0088] Among them, x r and r They represent the horizontal and vertical coordinates of the interacting vehicle in the Cartesian coordinate system with the ego vehicle as the origin, i.e., the relative position; R represents the minimum safe distance. The larger the value of the level set function, the safer the current state.
[0089] The designed state vector is as follows:
[0090]
[0091] Among them, x r and rThey represent the horizontal and vertical coordinates of the interacting vehicle in the Cartesian coordinate system with the ego vehicle as the origin, i.e., the relative position; Expressed as relative heading angle; v ex 、v ey and v o They are respectively represented as the longitudinal and lateral speeds of the vehicle and the speed of the interacting vehicle; ω e It is represented as the yaw angular velocity of the vehicle.
[0092] The state vector gridding granularity is 27×20×9×20×10×20×5, uniformly distributed in the 7-dimensional space of state vector x∈[-6,20]×[-10,10]×[0,2π]×[1,20]×[-1,1]×[1,20]×[-1,1].
[0093] Let the control amount u of the interacting vehicle be 0 =[ω o a o ] T , the control quantity u of the vehicle e =[δF x ] T , combined with the state vector x, the overall system dynamics equation f consisting of the self-driving car and the interactive vehicle can be obtained:
[0094]
[0095] Where δ is the front wheel turning angle of the vehicle; F x =F xf +F xr is the longitudinal driving force of the vehicle; I zz is the moment of inertia of the vehicle around the z-axis; d f d r are the distances from the center of mass of the vehicle to the center of the front and rear axles respectively;
[0096] Substitute the above formula into the HJ partial differential equation to calculate and store the level set function value.
[0097] The vehicle status data such as the position, speed, heading angle, yaw rate of the self-vehicle and the position, speed, heading angle of the interactive vehicle are obtained online, and filtered through the extended Kalman filter.
[0098] Interpolation is performed between grid points to find the value of the level set function at any given state. The interpolation method used is the radial basis function interpolation method, which has the general form:
[0099]
[0100] in is the basis function; iis the weight coefficient of the ith basis function. Considering the relatively complex mapping relationship between the relative state and the level set function and its gradient, the present invention adopts the Gaussian function as the basis function, that is:
[0101]
[0102] Among them ‖xx i ‖ is the Euclidean distance from the i-th point to the center point; σ is the variance of the Gaussian function, which controls the width of the basis function.
[0103] The found level set function value is compared with the set threshold, and different control constraints are obtained according to the state.
[0104] like Figure 3 As shown in the figure, the state determination method is: if the current level set function value is less than the threshold p1, a control hard constraint is introduced to prevent the control sequence selected by the controller from causing the ego vehicle to fall into an inevitable collision risk, thereby ensuring interactive safety; if the current level set function value is greater than the threshold p2, a control soft constraint is introduced. When the controller selects an overly conservative control sequence, it will bear a larger objective function value as a penalty, prompting the ego vehicle to adopt a control sequence that is conducive to improving traffic efficiency; if the current level set function value is greater than the threshold p1 and less than the threshold p2, no additional control constraints are introduced.
[0105] Among them, the control hard constraint set and the control soft constraint set are expressed as:
[0106]
[0107] Among them, U e (x) represents the control constraint set under state x, u e is the control quantity, satisfying the physical constraints of the vehicle execution unit:
[0108] u min ≤u e ≤u max
[0109] Bringing the control constraint set into the MPC solution process, the target problem is to minimize the following cost function under the premise of satisfying the following constraints:
[0110]
[0111] stx(k+1)=A k x(k)+B k u(k)
[0112] y(k)=C k x(k)
[0113] u(k)∈Ue [x(k)]+S
[0114] Δu(k)=u(k)-u(k-1)
[0115] y min -ε≤y(k)≤y max +ε
[0116] u min ≤u(k)≤u max
[0117] Δu min ≤Δu(k)≤Δu max
[0118] In the formula, Q, R, ρ, and γ are the output deviation y(k+i)-y ref (k+i), control increment Δu(k+i), relaxation factor ε and weight matrix of S; P and C are state time domain and control time domain respectively; x(k), u(k), y(k) are state quantity, control quantity and output quantity at time k respectively; y max ,y min 、u max 、u min , Δu max , Δu min They are the upper and lower limits of the output quantity, control quantity and control increment respectively.
[0119] When the control constraint set is a hard constraint, S is zero, that is, the control amount is not allowed to take the set U e [x(k)]; when the control constraint set is a soft constraint, S is any positive value, that is, the control amount is allowed to take the set U e [x(k)], but the larger the value of S is, the larger the value of the objective function will be, which in turn limits the degree of relaxation when the soft constraint is applied.
[0120] Example 3
[0121] The efficient driving interaction system based on the safety guarantee of the backward reachable set in the embodiment of the present invention is mainly used to implement the above method embodiment, which mainly includes:
[0122] A discrete table construction module is used to divide the sampled values of the vehicle state vector according to a certain grid size, calculate and store the level set function value offline by solving the level set equation, and construct a mapping relationship between the vehicle state vector and the level set function value to form a discrete table; when the level set function values are all zero, it corresponds to a zero level set, which corresponds to the safe distance boundary of the interactive vehicle relative to the self-vehicle;
[0123] The table lookup module is used to obtain the state vector of the vehicle online and find the corresponding level set function value according to the discrete table;
[0124] The control constraint selection module is used to compare the found level set function value with the set threshold, and enter different control constraints according to the comparison result; if it is less than the first threshold, it enters the control hard constraint, and the vehicle is not allowed to select a value other than the control amount; if it is greater than the second threshold, it enters the control soft constraint, and the vehicle is allowed to select the control amount within a certain range.
[0125] The control instruction generation module is used to generate control instructions according to control constraints to control the driving of the vehicle.
[0126] Furthermore, the control constraint selection module is also used for not introducing additional control constraints when the level set function value is between the first threshold and the second threshold.
[0127] Furthermore, the discrete table building module is also specifically used to convert the state quantity Sampling is performed according to the specified upper and lower limits and grid size, and the level set equation is solved. The obtained level set function value will be stored in the memory to form an offline data table. During driving, the extended Kalman filter filters the vehicle state information collected by the sensor. The more accurate state information after processing will be used to find the level set function value under the corresponding state online in the offline table. This module sacrifices a small amount of storage space in exchange for faster computing speed.
[0128] The control constraint selection module is also used to compare the current level set function value with the set threshold, find its safe range, give different level set function gradient value constraints, and narrow the set that can be used in the current control sequence.
[0129] Then, the control constraint is introduced into the MPC solver through the control instruction generation module, thereby ensuring that the solved control sequence will meet the evolution requirements of the level set function value.
[0130] The functions of each module mainly correspond to the method implementation example and will not be described in detail here.
[0131] The present application also provides a computer-readable storage medium, such as a flash memory, a hard disk, a multimedia card, a card-type memory (e.g., an SD or DX memory, etc.), a random access memory (RAM), a static random access memory (SRAM), a read-only memory (ROM), an electrically erasable programmable read-only memory (EEPROM), a programmable read-only memory (PROM), a magnetic memory, a disk, an optical disk, a server, an App application store, etc., on which a computer program is stored, and when the program is executed by a processor, a corresponding function is realized. When the computer-readable storage medium of this embodiment is executed by a processor, an efficient driving interaction method based on backward reachable set safety assurance of the method embodiment is realized.
[0132] It should be pointed out that, according to the needs of implementation, the various steps / components described in this application can be split into more steps / components, and two or more steps / components or partial operations of steps / components can be combined into new steps / components to achieve the purpose of the present invention.
[0133] The order of execution of each step in the above embodiment does not mean the order of execution. The order of execution of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present application.
[0134] It should be understood that those skilled in the art can make improvements or changes based on the above description, and all these improvements and changes should fall within the scope of protection of the appended claims of the present invention.
Claims
1. An efficient driving interaction method based on backward reachable set safety guarantee, It is characterized in that The following steps are involved: S1. Divide the sampling values of the vehicle state vector according to a certain grid size, calculate and store the level set function value offline by solving the level set equation, and construct a mapping relationship between the vehicle state vector and the level set function value to form a discrete table; when the level set function values are all zero, it corresponds to a zero level set, which corresponds to the safe distance boundary of the interactive vehicle relative to the self-vehicle; S2, obtain the state vector of the vehicle online, and find the corresponding level set function value according to the discrete table; S3, comparing the found level set function value with the set threshold, and entering different control constraints according to the comparison result; If it is less than the first threshold, it will enter the control hard constraint, and the vehicle is not allowed to select a value other than the control amount; if it is greater than the second threshold, it will enter the control soft constraint, and the vehicle is allowed to select the control amount within a certain range; S4. Generate control instructions according to the control constraints to control the driving of the vehicle.
2. The efficient driving interaction method based on backward reachable set safety guarantee according to claim 1, It is characterized in that The level set equation is constructed based on the positional relationship between the ego vehicle and the surrounding interactive vehicles and the overall system dynamics equation composed of the ego vehicle and the interactive vehicles.
3. The efficient driving interaction method based on backward reachable set safety guarantee according to claim 1, It is characterized in that The state vector includes position, velocity, heading angle, yaw rate and the position, velocity and heading angle of the interacting vehicle.
4. The efficient driving interaction method based on backward reachable set safety guarantee according to claim 1, It is characterized in that After the vehicle state vector is obtained in step S2, it is filtered by an extended Kalman filter.
5. The efficient driving interaction method based on backward reachable set safety guarantee according to claim 1, It is characterized in that If the level set function value is between the first threshold and the second threshold, no additional control constraints are introduced.
6. The efficient driving interaction method based on backward reachable set safety guarantee according to claim 1, It is characterized in that In step S2, the level set function values are interpolated between the grid points to find the level set function value at any given state.
7. The efficient driving interaction method based on backward reachable set safety guarantee according to claim 1, It is characterized in that In step S4, the control constraints are specifically substituted into the MPC solver to solve for safe and smooth control instructions.
8. An efficient driving interaction system based on backward reachable set safety assurance, It is characterized in that include: A discrete table construction module is used to divide the sampled values of the vehicle state vector according to a certain grid size, calculate and store the level set function value offline by solving the level set equation, and construct a mapping relationship between the vehicle state vector and the level set function value to form a discrete table; when the level set function values are all zero, it corresponds to a zero level set, which corresponds to the safe distance boundary of the interactive vehicle relative to the self-vehicle; The table lookup module is used to obtain the state vector of the vehicle online and find the corresponding level set function value according to the discrete table; The control constraint selection module is used to compare the found level set function value with the set threshold and enter different control constraints according to the comparison result; If it is less than the first threshold, it will enter the control hard constraint, and the vehicle is not allowed to select a value other than the control amount; if it is greater than the second threshold, it will enter the control soft constraint, and the vehicle is allowed to select the control amount within a certain range; The control instruction generation module is used to generate control instructions according to control constraints to control the driving of the vehicle.
9. The efficient driving interaction system based on backward reachable set safety guarantee according to claim 8, It is characterized in that The control constraint selection module is also used for not introducing additional control constraints when the level set function value is between the first threshold and the second threshold.
10. A computer storage medium, It is characterized in that A computer program executable by a processor is stored therein, and the computer program executes the efficient driving interaction method based on backward reachable set safety assurance as described in any one of claims 1 to 7.
Citation Information
Patent Citations
Time-optimal route planning method based on improved level set algorithm
CN106503837A
Mobile robot path planning method based on parameterized level set
CN110207709A
Method for calculating flight safety envelope of unmanned aerial vehicle based on nonlinear programming and reachability analysis
CN115047764A
Vehicular acceleration suppression device and vehicular acceleration suppression method
JP2015067220A
Wearable rehabilitation assistive device
KR1020240076073A
