Intelligent vehicle track online re-planning method based on sensitivity simulation under sudden fault condition

Abstract

The invention relates to the technical field of intelligent vehicle trajectory planning, and particularly discloses an intelligent vehicle trajectory online re-planning method based on sensitivity simulation under a sudden fault condition, which is technically characterized by comprising the following steps: S1, constructing a migration trajectory based on sensitivity; s2, cutting a migration track; and S3, further improving the feasibility of re-planning the trajectory by using a tracking simulation technology. According to the method, on the basis of fully utilizing nominal trajectory information, high-performance and high-quality trajectory re-planning is realized, and the trajectory conforming to the real movement ability after the vehicle fault is planned in real time under the condition of the sudden driving fault; according to the method, a nominal trajectory is migrated into a reasonable new trajectory under a fault kinematics condition by using sensitivity information, then the trajectory is further cut, and then the kinematics feasibility of the trajectory is enhanced by using a simulation tracking method, so that a trajectory online re-planning result which is as reasonable as possible is generated. The complete scheme is named as the online trajectory replanning method based on sensitivity simulation.

Description

Technical Field

[0001] The patent of this invention relates to the technical field of intelligent vehicle trajectory planning, specifically, to an online re-planning method of intelligent vehicle trajectory based on sensitivity simulation under sudden fault conditions. Background Art

[0002] The trajectory planning of an intelligent vehicle aims to generate a space-time trajectory that reflects the local path and speed of the vehicle, conforming to the vehicle's internal kinematic constraints and external environment obstacle avoidance constraints, and optimizing the established performance indicators as much as possible while strictly ensuring that the above constraints are met.

[0003] Online trajectory replanning refers to the process of intelligent vehicle driving, which continuously uses the current instantaneous position and motion state of the vehicle as the starting point and a predetermined preview point in front of the driving road (usually a position that can be reached in about 10 seconds, generally within 80 to 120 meters) as the local end point to perform high-real-time and high-performance trajectory planning. Compared with trajectory planning, trajectory replanning emphasizes secondary processing based on the nominal trajectory planning results, such as Figure 1 As shown in a in , the previous trajectory planning has generated a light gray line for the smart car as the nominal trajectory planning result, and the vehicle is actually driving along the trajectory (the execution process of closed-loop control tracking), as shown in Figure 1 As shown in b, a sudden external blow or failure may occur halfway along the way, causing the turning response speed of the front wheels of the vehicle to drop to half of the original speed. The online trajectory replanning requires that the nominal trajectory ( Figure 1 The gray line in a of ) fully considers the latest vehicle motion capabilities and quickly generates a driving trajectory that meets the latest vehicle kinematic capabilities. This calculation process is called trajectory online replanning (see Figure 1 (the black dashed line in b).

[0004] It is not difficult to find that the replanned trajectory does not overlap with the nominal trajectory, and the turn appears to be more rounded and gentle, which is exactly a trajectory that conforms to the kinematic law of the vehicle after the fault. In addition, the trajectory replanning may not be able to connect to the original local preview point, so Figure 1 The gray line in b does not overlap with the end of the black dotted line. The present invention focuses on the online trajectory replanning technology under the condition of sudden failure (change in vehicle motion capability) during the online driving of the smart car.

[0005] At present, in academia or industry, there are two main technical solutions focusing on this problem: the first is emergency braking and sampling a series of allowable values ​​of the front wheel angular velocity according to the moving window method, and selecting a reasonable front wheel angle to stop the vehicle. This method fails to fully utilize the vehicle's full kinematic capabilities, and once a fault occurs, immediately stopping the vehicle will cause traffic jams, rear-end collisions and other derivative disasters, resulting in weak fault tolerance and error correction of autonomous driving trajectory planning. The second is to implement trajectory planning online, that is, to start a new trajectory planning process from the moment the fault occurs, which is often time-consuming, and the nominal planning results previously solved are not effectively utilized, resulting in unnecessary waste of computing resources. In short, the current market research on online trajectory replanning technology under sudden fault conditions is not systematic or complete enough.

[0006] To this end, the inventors of the present application propose an online replanning method for smart vehicle trajectories based on sensitivity simulation under sudden failure conditions to solve the above-mentioned problems in the prior art. Summary of the invention

[0007] The purpose of the present invention is to solve the prior art problems raised in the above background technology and to provide an online replanning method for smart vehicle trajectory based on sensitivity simulation under sudden failure conditions.

[0008] The above-mentioned purpose of the present invention is achieved like this:

[0009] The method for online replanning of intelligent vehicle trajectories under sudden fault conditions based on sensitivity simulation includes the following steps:

[0010] S1, Sensitivity-based migration trajectory construction;

[0011] S2, cutting of migration tracks;

[0012] S3. Use tracking simulation technology to further improve the feasibility of re-planning trajectory.

[0013] Furthermore, assuming that during the process of driving on the road, the vehicle suddenly experiences a change in the attenuation of its own mechanical structure, steering, acceleration and deceleration and other aspects of its movement ability, and such a change reflects a fault, then step S1 is specifically as follows:

[0014] First, the parameter list q is used to reflect the latest vehicle motion capability after the fault occurs, and the new parameter list is recorded as:

[0015] q new =[x lb_new x ub_new g lb_new g ub_new p new ];

[0016] Use the following formula to achieve trajectory migration, that is, quickly estimate q new The approximate optimal solution of Proposition 5 under the parameter z new :

[0017] z new =z norm +M·(q new -q norm ).;

[0018] The calculated migration trajectory is the trajectory from the nominal starting point to the end point, which is further processed.

[0019] Furthermore, step S2 is specifically as follows:

[0020] First, assume that the actual position of the vehicle at the time of the fault is a point P in the inertial coordinate system, and project the point onto the migration trajectory to obtain the projection point Q. Projection means finding the point closest to P along the migration trajectory, which is called Q.

[0021] The part from the starting point to Q of the migration trajectory is cut off, and the remaining part is retained. The remaining trajectory after cutting is the path.

[0022] Furthermore, step S3 is specifically as follows:

[0023] 1) First, a virtual chassis that meets the kinematic capabilities of the vehicle under the current fault condition is created in the scenario and placed at point P in the environment to reflect the current position of the vehicle. The chassis' variables such as speed, acceleration, front wheel angle, and angular velocity are required to be the actual state observation values ​​of the vehicle at this moment.

[0024] 2) The simulated vehicle starts from point P and uses the lateral and longitudinal control laws to track the path. The pure tracking control algorithm is selected for lateral control, and the PD control algorithm is adopted for longitudinal control. The chassis response effect of the vehicle driving as close to the path as possible under the control algorithm is simulated, and the trajectory is recorded and called final_path;

[0025] 3) After extracting the final_path, it is regarded as the trajectory online replanning result and output to completely complete the trajectory replanning task.

[0026] Compared with the prior art, the present invention has the following beneficial effects:

[0027] 1. The method of the present invention realizes high-performance and high-quality trajectory re-planning based on full utilization of nominal trajectory information, and plans a trajectory that meets the actual movement capability of the vehicle after the failure in real time under the condition of sudden driving failure;

[0028] 2. The method of the present invention uses sensitivity information to migrate the nominal trajectory into a reasonable new trajectory under the fault kinematic conditions, and then further cuts the trajectory, and then uses the simulation tracking method to enhance its kinematic feasibility, thereby generating an online replanning result of the trajectory that is as reasonable as possible;

[0029] 3. The method of the present invention adopts a tracking simulation method to simulate the process of the vehicle's virtual chassis trying to track the path, thereby deducing a feasible and correct starting point driving trajectory of the vehicle under the current real fault motion capability with extremely low computational cost (the computational freedom of the simulation process is 0). It is regarded as a trajectory replanning result that can naturally meet the boundary value constraints and kinematic constraints, and can effectively complete the trajectory replanning task. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 is a schematic diagram of trajectory replanning in an embodiment of the present invention;

[0031] Figure 2 is a schematic diagram of a trajectory of traveling from a nominal starting point to an end point in an embodiment of the present invention;

[0032] Figure 3 Schematic diagram of the cutting migration trajectory in an embodiment of the present invention. DETAILED DESCRIPTION

[0033] 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 intended to limit the present invention.

[0034] The implementation of the present invention is described in detail below in conjunction with specific embodiments. For those skilled in the art, the specific meanings of the above terms can be understood according to specific circumstances.

[0035] The following are preferred embodiments of the present invention.

[0036] The embodiment of the present invention provides an online replanning method for the trajectory of an intelligent vehicle based on sensitivity simulation under sudden fault conditions. The method of the present invention realizes high-performance and high-quality trajectory replanning based on full utilization of nominal trajectory information, and plans a trajectory that meets the actual movement capability of the vehicle after the fault in real time under the condition of sudden driving fault.

[0037] The core idea of ​​the method of the present invention is to use sensitivity information to migrate the nominal trajectory into a reasonable new trajectory under the fault kinematic conditions, and then further cut the trajectory, and then use the simulation tracking method to enhance its kinematic feasibility, so as to generate a trajectory online replanning result that is as reasonable as possible. We call the above complete scheme the trajectory online replanning method based on sensitivity simulation.

[0038] The method of the present invention comprises three steps, wherein the first step is trajectory migration based on sensitivity, the second step is migration trajectory cutting, and the third step is tracking simulation migration trajectory.

[0039] Before implementing step 1, the inventor of the present application introduces the nominal process of trajectory planning and requires the nominal trajectory planning process (i.e. Figure 1 The process of generating the gray line in a in ) must be solved based on the gradient optimization method, because only this method can obtain the sensitivity information of the result while generating the planning trajectory. For the integrity of the overall method, it is called step 0 (step 0 is not really part of the replanning process, but simply a pre-step).

[0040] The following will explain steps 0, 1, 2, and 3 respectively:

[0041] Step 0: Nominal trajectory planning process (used to derive the nominal optimal solution and sensitivity matrix)

[0042] The trajectory planning problem is a classic optimization problem, that is, solving the minimum value of the cost function polynomial under a series of constraints (such as obstacle avoidance constraints, vehicle kinematic constraints, and two-point boundary value constraints, etc.). The modeling process of the trajectory planning problem is not the core of this invention. For the sake of convenience, the optimization problem in the context of trajectory planning can be abstracted into the following format:

[0043]

[0044] Where x represents all decision variables; p represents the parameter or coefficient related to the vehicle's own kinematic capability in the proposition; before the fault occurs, p takes its nominal value p norm ;x lb , x ub are the left and right interval boundary constraints imposed on the decision variables; g lb ≤g(x,p)≤g ub Represents the complex implicit constraints in trajectory planning tasks.

[0045] For the above nominal trajectory planning problem (1), its Lagrangian function should be constructed as follows:

[0046]

[0047] The first-order optimality Karush-Kuhn-Tucker condition is then defined as follows:

[0048]

[0049] The Lagrange multiplier of the decision variable x For example, we found that the upper and lower boundary constraints cannot be effective at the same time. or Either of the two can be true, so let’s define the Lagrange multiplier of the decision variable x as The constrained Lagrange multiplier is now where λ x , g The symbol represents the effectiveness of the boundary constraint. Based on this idea, the original first-order optimality Karush-Kuhn-Tucker condition (i.e., Formula 3) can be further simplified to the following constraints:

[0050]

[0051] x lb ≤x≤x ub ,g lb ≤g(x,p)≤g ub ,(4c)

[0052]

[0053] The Lagrangian function is defined as L(x,p,λ g )≡f(x,p)+λ g g(x,p); is the valid set of decision variables x, and is a unit diagonal matrix, representing the validity of the upper and lower boundary constraints respectively; represents whether there are any valid boundary constraints, Indicates whether both the upper and lower bounds are invalid; the valid set of constraints g(x,p) is defined as Its definition is the same as the valid set of decision variables x; the two sets of valid sets form equations (4d) and (4e) that describe the complementary constraints in the first-order optimality Karush-Kuhn-Tucker condition (i.e., equation 3).

[0054] Then, integrating the above augmented cost function with the optimality condition constitutes a new optimization problem:

[0055]

[0056] The unknown part of the augmented optimization problem is defined as the augmented decision variable z = [xλ g ], the augmented parameter or coefficient q = [x lb x ub g lb g ubp]. Normally, nominal trajectory planning only requires numerical solution of problem (1), but we require numerical solution of proposition (5). The reason for doing this step is to derive more necessary auxiliary information for subsequent trajectory replanning, which is further explained in more detail below.

[0057] After solving Proposition (5) using the well-known interior point algorithm in the field of operations research, we can obtain z = [xλ g ], let’s record it as It should be noted that this optimal solution is the optimal solution for the nominal planning problem, and the nominal parameter is reflected in the generalized parameter q norm =[x lb x ub g lb g ub p norm ].

[0058] Next, define an implicit function F(z,q) in the following format:

[0059]

[0060] The explicit function z=G(q) between the unknown variable z and the parameter q can be determined by the implicit function F(z,q). The sensitivity analysis of the unknown variable z to the parameter q can be expressed as follows:

[0061]

[0062] Since the implicit function F(z,q) has continuous and non-zero partial derivatives with respect to the unknown variable z in the domain of definition Therefore, the derivative of the function G(q) exists. The next step is to use the famous implicit function theorem in the field of advanced mathematics to obtain the sensitivity matrix between the unknown variable z and the parameter q:

[0063]

[0064] The Jacobian matrix and The definition is as follows:

[0065]

[0066] In summary, the above formulas (8) and (9) define JG(z,q). In the embodiment of the present invention, the above-mentioned nominal optimal solution and nominal parameters are substituted into them to obtain the sensitivity matrix associated with the nominal optimal solution. The specific method is to substitute z=z in the above formula (8) norm and q=q normIn this way, all symbolic expressions in the sensitivity matrix are specifically assigned clear numerical values, thus obtaining a sensitivity matrix to be used later, which is abbreviated as M.

[0067] Step 1: Sensitivity-based migration trajectory construction

[0068] Assume that while driving on the road, the vehicle suddenly experiences a change in its mechanical structure, steering, acceleration and deceleration, and other aspects of its mobility, and this change reflects a fault. We first use the parameter list q to reflect the latest vehicle mobility after the fault occurs. Let's record the new parameter list as:

[0069] q new =[x lb_new x ub_new g lb_new g ub_new p new ]

[0070] Since the optimal solution of the nominal problem z is known norm , nominal parameter list q norm , sensitivity matrix M and the new parameter list q in the case of failure new , we can directly use the following formula to achieve trajectory migration, that is, quickly estimate q new The approximate optimal solution z of Proposition (5) under the parameter new :

[0071] z new =z norm +M·(q new -q norm ).(10)

[0072] It is not difficult to find that the calculation of the migration trajectory obtained by the above formula (10) is extremely simple. It only involves vector addition and matrix multiplication, and its calculation amount is much lower than the general trajectory planning process. The theoretical guarantee for achieving such a good effect is precisely the sensitivity matrix information M provided in step 0 and the incremental information of the parameter list before and after (q new -q norm ).

[0073] However, it must be pointed out that the migration trajectory calculated by the above method is the trajectory from the nominal starting point to the end point, e.g. Figure 2 The line effect of the migration trajectory is shown. At this time, the vehicle has moved a certain distance along the nominal trajectory, so the vehicle is not on the migration trajectory. The migration trajectory cannot be directly used as the result of trajectory replanning. Therefore, further subsequent steps should be performed for processing.

[0074] Step 2: Cutting the migration track

[0075] Step 2 is responsible for cutting the migration track for use in step 3. The specific steps are as follows.

[0076] First, assume that the actual position of the vehicle at the time of the fault is a point P in the inertial coordinate system, and project the point onto the migration trajectory to obtain the projection point Q. Projection here means finding the point closest to P along the migration trajectory, which is called Q. The part of the migration trajectory from the starting point to Q is cut off, and the remaining part is retained. For the convenience of description, the remaining trajectory after cutting is called path.

[0077] Step 3: Use tracking simulation technology to further improve the feasibility of replanning trajectory

[0078] Depend on Figure 3 As shown in the figure, it is not difficult to find that the cut part of the migration trajectory (the path mentioned above) starts from point Q, but the vehicle is at point P at this time, and the two do not match. Therefore, it is inappropriate to directly regard this green trajectory as the result of online trajectory replanning. To solve this problem, step 3 is now carried out. The specific steps are as follows.

[0079] First, in this scenario, a virtual chassis is created that meets the kinematic capabilities of the vehicle under the current fault condition and is placed at point P in the environment to faithfully reflect the current position of the vehicle. In addition, variables such as the chassis' speed, acceleration, front wheel angle, and angular velocity are required to be the actual state observation values ​​of the vehicle at this moment.

[0080] Then, the simulated vehicle starts from point P and tracks the path using lateral and longitudinal control laws. For lateral control, a pure tracking control algorithm is selected, and for longitudinal control, a PD control algorithm or other similar common solutions in the industry can be used. Under the control of the control algorithm, the simulated vehicle tries to get as close to the chassis response effect of the path as possible, and the trajectory is recorded and called final_path.

[0081] Then, after extracting the final_path, it is regarded as the trajectory online replanning result and outputted to completely complete the trajectory replanning task.

[0082] It is worth noting that the trajectory replanning method proposed in the present invention has strong real-time performance. This is because after an online fault occurs and triggers trajectory replanning, the calculations that really need to be implemented online only include the following parts:

[0083] The above formula (10), step 2 and step 3, where formula (10) only involves small-scale matrix addition and multiplication, and steps 2 and 3 are fast deduction simulation processes without degrees of freedom. Therefore, it can be completed in a very short time (on an industrial computer with a face value of less than 5,000 yuan, the average time consumed is within 3 milliseconds).

[0084] From a technical point of view, if the sensitivity technology, i.e., formula (10), is used alone, the clipped path obtained does not conform to the starting position of the trajectory replanning task, after all, the vehicle is at P, and the endpoint of the path is at Q. In order to eliminate the differences between PQ and not introduce new infeasibility, the method of the present invention adopts a tracking simulation method to simulate the process of the vehicle virtual chassis trying to track the path, thereby deducing a feasible and correct starting point driving trajectory of the vehicle under the current real fault motion capability with extremely low computational cost (the computational freedom of the simulation process is 0), and considering it as a trajectory replanning result that can naturally meet the boundary value constraints and kinematic constraints, thus effectively completing the trajectory replanning task.

[0085] In addition, through the application of the implementation method of the present invention, based on the statistical data of the use of autonomous driving mining trucks in open-pit mines over a period of time (4 months), autonomous driving mining trucks are prone to sudden failures in motion capabilities. After deploying the onboard industrial computer of mining trucks with a face value of less than 5,000 yuan, the average time to trigger the algorithm is 2.41 milliseconds, which is much lower than the average time of 182.28 milliseconds of other algorithms before. Therefore, it reflects the powerful decision-making planning ability and high-quality automatic driving ability of online trajectory replanning to quickly respond to vehicle failures.

[0086] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. An online replanning method for intelligent vehicle trajectories under sudden failure conditions based on sensitivity simulation, characterized in that: The following steps are involved: S1, Sensitivity-based migration trajectory construction; S2, cutting of migration tracks; S3. Use tracking simulation technology to further improve the feasibility of re-planning trajectory.

2. The method for online replanning of intelligent vehicle trajectory based on sensitivity simulation under sudden failure conditions according to claim 1 is characterized in that: Assuming that during driving on the road, the vehicle suddenly experiences a change in its own mechanical structure, steering, acceleration and deceleration and other aspects of its movement ability attenuation, and such a change reflects a fault, then step S1 is specifically as follows: First, the parameter list q is used to reflect the latest vehicle motion capability after the fault occurs, and the new parameter list is recorded as: q new =[x lb_new x ub_new g lb_new g ub_new p new ]; Use the following formula to achieve trajectory migration, that is, quickly estimate q new The approximate optimal solution of Proposition 5 under the parameter z new : z new =z norm +M·(q new -q norm ).; The calculated migration trajectory is the trajectory from the nominal starting point to the end point, which is further processed.

3. The online replanning method of intelligent vehicle trajectory based on sensitivity simulation under sudden failure conditions according to claim 2 is characterized in that: Step S2 is specifically as follows: First, assume that the actual position of the vehicle at the time of the fault is a point P in the inertial coordinate system, and project the point onto the migration trajectory to obtain the projection point Q. Projection means finding the point closest to P along the migration trajectory, which is called Q. The part from the starting point to Q of the migration trajectory is cut off, and the remaining part is retained. The remaining trajectory after cutting is the path.

4. The method for online replanning of intelligent vehicle trajectory based on sensitivity simulation under sudden failure conditions according to claim 3 is characterized in that: Step S3 is specifically as follows: 1) First, a virtual chassis that meets the kinematic capabilities of the vehicle under the current fault condition is created in the scenario and placed at point P in the environment to reflect the current position of the vehicle. The chassis' variables such as speed, acceleration, front wheel angle, and angular velocity are required to be the actual state observation values ​​of the vehicle at this moment. 2) The simulated vehicle starts from point P and uses the lateral and longitudinal control laws to track the path. The pure tracking control algorithm is selected for lateral control, and the PD control algorithm is adopted for longitudinal control. The chassis response effect of the vehicle driving as close to the path as possible under the control algorithm is simulated, and the trajectory is recorded and called final_path; 3) After extracting the final_path, it is regarded as the trajectory online replanning result and output to completely complete the trajectory replanning task.

Images