CN109835336B

CN109835336B - A constrained square wave speed planning method based on fuzzy algorithm

Info

Publication number: CN109835336B
Application number: CN201910122873.2A
Authority: CN
Inventors: 段建民; 马学峥; 花晓峰; 黄小龙
Original assignee: Beijing University of Technology
Current assignee: Beijing University of Technology
Priority date: 2019-02-19
Filing date: 2019-02-19
Publication date: 2021-05-14
Anticipated expiration: 2039-02-19
Also published as: CN109835336A

Abstract

The invention discloses a constrained square wave speed planning method based on a fuzzy algorithm, which belongs to the field of automatic driving vehicle speed planning. The method firstly determines the membership function of the input and output of the fuzzy algorithm and the fuzzy rule set of the fuzzy algorithm. Then, the global variables required by the subsequent algorithm are calculated, including the curvature signal queue of the planned path, the filtered curvature, and the global variables related to the square-wave fitting algorithm. Finally, square-wave fitting is performed on the curvature in the rolling window, and the velocity of the target trajectory point is calculated in combination with the vehicle and environmental state quantities, and then the vehicle dynamics are considered to perform quadratic planning for the planned velocity curve. Based on the human driving experience, the invention plans the speed trajectory more in line with the normal driver's driving habits, so that it is easier to realize the relevant functions of the unmanned vehicle and has better adaptability and safety.

Description

Fuzzy algorithm-based wavy vehicle speed planning method with constraint square

Technical Field

The invention belongs to the field of automatically driven vehicle speed planning. A future driving speed curve is solved by using a fuzzy algorithm and referring to the curvature of a front road, the current transverse tracking error and the ground friction coefficient in the driving process of a vehicle, and the unmanned actual environment is considered.

Background

By the end of 6 months in 2018, the number of Chinese automobiles is up to 2.29 hundred million, and 1381 million automobiles newly registered in the last half of 2018. The huge automobile holding capacity causes serious traffic jam and frequent casualty accidents, which become the problem that China must solve. The unmanned technology developed in recent years can effectively avoid traffic accidents caused by human factors. The unmanned automobile comprises an environment sensing module, a positioning module, a decision planning module and a bottom layer control module, wherein the problem of the arrival, the arrival and the arrival of the automobile is solved by a planning part in the decision planning module, the planning problem is often decomposed into two parts of path planning and speed planning by the existing research and practical application, and the reasonable speed planning not only can reduce the integral driving cost (time and energy consumption) of the automobile, but also can provide more comfortable and safer riding experience for passengers.

The existing speed planning method is mostly limited to the construction of an objective function through energy consumption or the realization of speed planning according to the state of a front vehicle based on an adaptive cruise system. Although these conventional speed planning methods are effective under the existing working conditions, for an unmanned system, the vehicle needs to select the driving speed more intelligently and autonomously without depending on a vehicle model, for example, in a large-curvature curve, a slippery road or a situation where a large error has occurred in the lateral tracking of the vehicle, the vehicle speed should be reduced to obtain better vehicle stability, and under the conditions of a straight road, a dry road with a large friction coefficient and better performance of an automatic steering system, the vehicle can be considered to run at a higher speed or the current economic hour speed of the vehicle, so as to obtain a shorter passing time or reduce energy consumption to ensure the driving range.

The velocity planning system is a multi-input single-output nonlinear system, and it is difficult to construct a mathematical model. The method is characterized in that a high speed, a medium speed or a low speed is tried to pass through the front road by judging factors such as the degree of curve and the degree of slippery of the front road, and a plurality of concepts of large, general and small exist in the process, so that compared with an optimal algorithm, a fuzzy algorithm is more suitable for calculating the speed or applying the scene. In the curvature calculation process, the curvature can be accurately calculated by a computer, the curvature can vibrate in a certain range inevitably, and the input vibration can influence the output result to a certain extent. Considering riding comfort, the speed should be kept at a stable value within a certain range, for example, a stable lower speed should be always maintained when entering a continuous turning working condition, and a high-speed area should not be planned because a section of lower curvature part appears in the continuous turning process, so a certain filtering algorithm and a square wave-shaped speed plan considering vehicle dynamics constraint are introduced into the algorithm, and a stable speed and softer speed switching process is obtained.

Disclosure of Invention

The running speed of the unmanned automobile is dynamically planned in consideration of input quantities such as the degree of curvature of a road ahead, the degree of slippery on the current road surface, the magnitude of lateral deviation and the like, in order to obtain good and comfortable riding experience, the planned speed is as stable as possible, large or high-frequency oscillation is avoided in a short time, and uncomfortable body feeling of a rider is avoided in the switching process of high, medium and low speed.

In order to achieve the purpose, the technical scheme adopted by the invention is a wavy vehicle speed planning method with a constraint square based on a fuzzy algorithm, and the method comprises the following implementation steps:

step 1, determining a membership function and a fuzzy rule of system input and output;

step 1.1, determining input and output signals and membership functions thereof;

inputs to the system are front road curvature, road surface friction coefficient, and lateral tracking error. The curvature fuzzy language value B is large, the M is medium, and the S is small, the fuzzy language value of the lateral tracking error is B, M, S, the fuzzy language value of the road surface friction coefficient VB is very large, and the fuzzy language value of the road surface friction coefficient B, S, VS is very small. The membership functions all adopt bell-shaped functions:

in the formula a_bell、b_bell、c_bellIs a parameter of a generalized bell-shaped membership function, wherein a_bellDetermining the position of the horizontal axis corresponding to the bell-shaped membership function with the membership degree of 0.5, b_bellThe greater the absolute value of (c), the faster the speed of change of membership, the opening of the bell-shaped function being directed downwards when taking a positive value, c_bellThe position of the center of the bell-shaped membership function is determined.

The system output is the driving speed of the target path point, the fuzzy language values VB, B and LB are relatively large, the M expression and LS expression are relatively small, the S expression is small, and the VS expression is very small. The membership function adopts a triangular function:

in the formula a_tri、b_tri、c_triThe positions of the horizontal axes of the three vertexes of the membership degree curve are determined for the parameters of the triangular membership functions.

Step 1.2, designing a fuzzy rule;

the driving speed range of the target path point output when the friction coefficient is large is larger, the upper limit is higher, the speed change is small when the friction coefficient is small, and the highest speed per hour is also smaller; the curvature is the most sensitive input quantity, when the curvature is increased, the output vehicle speed is integrally reduced, otherwise, the output vehicle speed is increased; lateral tracking error is inversely proportional to output vehicle speed, but is less sensitive on road segments where the coefficient of friction is greater and the road curvature is less.

Step 2, calculating a global variable;

step 2.1, calculating curvatures of all target path points;

and 3 continuous path points are selected each time from the planned path provided by the path planning system by using a sliding window with the window length of 3, and the curvature of the target path point is obtained by using the triangular relation between the three points. The curvatures of all target path points on the path are calculated by the method.

L in the formula_i-1,i、L_i,i+1、L_i-1,i+1The distance between every two of the three continuous target path points.

Step 2.2, carrying out moving average filtering aiming at the curvature;

the high-frequency vibration part existing in the curvature calculated by the triangular relation influences the output result of the system; under the continuous turning working condition, the change of the steering direction causes the change of the curvature absolute value to unreasonably describe the bending degree of the real road. To solve the above problem, a moving average filtering algorithm is used to smooth the curvature signal queue:

where K is the original curvature calculated by trigonometric relationship, K_filterFor the filtered curvature, 2n +1 is the sliding window length of the moving average filter.

Step 2.3, calculating thresholds and curvature references of different areas of square wave fitting;

dividing the curvature queue into different standard regions based on the absolute value of the global curvature queue, wherein transition regions exist between the standard regions, and the length of the standard regions is

L＝(K_filterMax-K_filterMin)/(N+q×(N-1))

Wherein K_filterMaxAnd K_filterMinThe maximum curvature value and the minimum curvature value in the global curvature queue are respectively, N is the number of the standard areas, and the ratio of the length of the q transition area to the length of the standard area.

And obtaining the length and the relation of the standard region and the transition region, wherein i is 1, …, and N is the serial number of the standard region:

T_min(i)＝K_filterMin+(i-1)×(1+q)×L

T_max(i)＝K_filterMin+(i-1)×(1+q)×L+L

after the upper threshold and the lower threshold of each standard area are obtained, the curvatures of all target path points in the corresponding standard area in the global curvature queue are subjected to arithmetic mean to obtain the area standard value of the current area

Wherein n is_pointNumber of target path points with curvature in the standard region, K_filterRegionIs the set of curvatures for the target waypoints whose curvatures are within the standard region.

Step 3, calculating the traveling speed of the target planning point;

step 3.1, acquiring input quantity required by a fuzzy algorithm and performing square wave fitting according to curvature;

comparing the curvature of the dynamically planned path with the region threshold in step 2.3, directly classifying and approximating the curvature as a region standard value, and keeping the curvature in the transition region consistent with the region standard value in the adjacent standard region in terms of safety:

wherein K_fuzzyInputThe curve value is used as the input quantity of the fuzzy algorithm after being approximated, r is the weight value of the area standard value in different standard areas, and K is calculated according to different planning strategies_fuzzyInputAnd (6) adjusting.

Step 3.2, calculating the output speed through a fuzzy algorithm;

calculating output by using a fuzzy algorithm, using a multiplier rule for a fuzzy implication relation, and adopting a weighted average method for a defuzzification method:

in the formula N_ruleNumber of fuzzy rules to activate, mu_kj(x_k)、μ_ej(x_e)、μ_mj(x_m) Respectively represents the membership degree, z, of the fuzzy subset in which the curvature, the transverse tracking error and the ground friction coefficient are respectively positioned in the activated 'j' fuzzy rule_jOutputting u for different fuzzy rules_speedThe abscissa value corresponding to the cusp of the membership function of the fuzzy subset.

Step 3.3, applying dynamic constraints to the obtained speed to perform re-planning;

the velocity output curve is adjusted again with a constant jerk, acceleration with an initial value of 0:

u_speed(k+1)＝u_speed(k)+a(k+1)

wherein a is the acceleration, a_maxIs the maximum acceleration (or maximum deceleration) for avoiding the discomfort of the passenger, and Δ a is a constant jerk, u_speedHigh、u_speedLowHigher and lower speeds, respectively, for different levels of speed conversion.

Drawings

FIG. 1 is a flow chart of the overall algorithm of the invention.

FIG. 2 is a schematic diagram of membership functions of input and output signals.

FIG. 3 is a fuzzy inference surface diagram.

Fig. 4 is a schematic view of an example runway.

FIG. 5 is a schematic diagram of curvature filtering and square wave fitting.

FIG. 6 is a schematic diagram of fuzzy algorithm output speed and re-planning with constraints.

Fig. 7 is a graphical illustration of runway example speed planning results.

Detailed Description

The invention is further described with reference to the following figures and detailed description.

The invention provides a method for planning a wavy vehicle speed with a constraint square based on a fuzzy algorithm, aiming at a speed planning system of an unmanned vehicle. The method firstly determines membership function and fuzzy rule of fuzzy algorithm input and output quantity, adopts the fuzzy algorithm to plan the reference speed of a target path point, solves the problem of difficult modeling of a speed planning system, and provides a new solution for the current situation that the speed planning system is designed by utilizing the traditional following model or the economic energy consumption speed per hour. Then, the curvature signal queue calculated by utilizing the triangular relation is filtered, and the global variable related to the square wave fitting algorithm is calculated, so that the problem that the curvature calculated by a computer is too frequent in fluctuation and is not in line with the thinking habit of normal automobile driving is solved. And finally, converting the obtained input quantity into an output speed through a fuzzy algorithm, and carrying out quadratic programming on the planned speed curve by considering the vehicle dynamics. Even the vehicle speed tracking system designed based on the non-dynamic constraint controller can provide stable and comfortable riding experience for passengers under the condition of stably tracking the final speed curve. The system flow chart of the related method is shown as the attached figure 1, and the specific implementation process comprises the following steps:

according to the driving habit, when the curvature of the road on the front side is large, a large centrifugal phenomenon is generated when the road passes at a high speed, and poor riding experience is brought to a rider; when the road surface is wet and slippery, the tire can slip, and the working conditions of emergent doubling and braking can be caused by maintaining high-speed running, so that the property and even life safety are endangered; when the automobile has larger deviation in tracking a target path, the speed is properly reduced to improve the transverse tracking performance of the automobile steering controller and reduce the transverse tracking error. On the contrary, the driving speed can be properly increased under the conditions that the front road is straight, the ground adhesion condition is good and the lateral tracking error is small.

The input is defined as three components of front road curvature, road surface friction coefficient and transverse tracking error, wherein the fuzzy language value of curvature and transverse tracking error is B to indicate large, M to indicate medium and S to indicate small, the fuzzy language value of road surface friction coefficient is VB to indicate very large, and B, S, VS to indicate very small. The membership function adopts a bell-shaped function:

the output is the driving speed of the target path point, the fuzzy language values VB, B and LB are larger, M, LS is smaller, and S, VS is adopted. The membership function adopts a triangular function:

a schematic diagram of the membership function of the input and output signals of the system is shown in FIG. 2, wherein FIGS. 2-a, 2-b, 2-c and 2-d are schematic diagrams of the membership function of curvature, lateral error, ground friction coefficient and speed, respectively.

Step 1.2, designing a fuzzy rule;

the road surface friction coefficient directly influences the ground gripping performance of the tire, so that the driving speed range of the output target path point is larger and the upper limit is higher when a large friction coefficient is set; when the friction coefficient is small, the vehicle speed change is small, and the highest speed per hour is also controlled within the safe speed.

The curvature of the road in front directly affects the body feeling of the rider and the lateral stability of the automobile, so the overall fuzzy rule takes the curvature as the most sensitive input quantity, when the curvature is increased, the input speed is reduced as a whole, and vice versa.

The transverse tracking error increase of the automobile in the bending process is a normal phenomenon, the automobile can be gradually converged after the automobile is bent, and different automobile states are reflected by considering the transverse tracking error under different working conditions, so that the sensitivity of the fuzzy rule to the transverse tracking error is small on a road section with a large friction coefficient and a small road curvature; on the road surface with small friction coefficient and large curvature, the sensitivity to the input quantity of the transverse tracking error is improved.

The specified curvature is K, the transverse error is E, the friction coefficient is Mu, the speed is V, a specific fuzzy rule set is designed as follows, and a schematic diagram of a fuzzy inference curved surface is shown in FIG. 3:

if K＝S and E＝S and Mu＝VS then V＝M

or if K＝S and E＝M and Mu＝VS then V＝M

or if K＝S and E＝B and Mu＝VS then V＝M

or if K＝M and E＝S and Mu＝VS then V＝LS

or if K＝M and E＝M and Mu＝VS then V＝LS

or if K＝M and E＝B and Mu＝VS then V＝S

or if K＝B and E＝S and Mu＝VS then V＝S

or if K＝B and E＝M and Mu＝VS then V＝VS

or if K＝B and E＝B and Mu＝VS then V＝VS

or if K＝S and E＝S and Mu＝S then V＝LB

or if K＝S and E＝M and Mu＝S then V＝LB

or if K＝S and E＝B and Mu＝S then V＝LB

or if K＝M and E＝S and Mu＝S then V＝M

or if K＝M and E＝M and Mu＝S then V＝M

or if K＝M and E＝B and Mu＝S then V＝LS

or if K＝B and E＝S and Mu＝S then V＝LS

or if K＝B and E＝M and Mu＝S then V＝S

or if K＝B and E＝B and Mu＝S then V＝S

or if K＝S and E＝S and Mu＝B then V＝B

or if K＝S and E＝M and Mu＝B then V＝B

or if K＝S and E＝B and Mu＝B then V＝B

or if K＝M and E＝S and Mu＝B then V＝LB

or if K＝M and E＝M and Mu＝B then V＝LB

or if K＝M and E＝B and Mu＝B then V＝M

or if K＝B and E＝S and Mu＝B then V＝M

or if K＝B and E＝M and Mu＝B then V＝LS

or if K＝B and E＝B and Mu＝B then V＝S

or if K＝S and E＝S and Mu＝VB then V＝VB

or if K＝S and E＝M and Mu＝VB then V＝VB

or if K＝S and E＝B and Mu＝VB then V＝VB

or if K＝M and E＝S and Mu＝VB then V＝B

or if K＝M and E＝M and Mu＝VB thenV＝B

or if K＝M and E＝B and Mu＝VB thenV＝LB

or if K＝B and E＝S and Mu＝VB then V＝M

or if K＝B and E＝M and Mu＝VB thenV＝LS

or if K＝B and E＝B and Mu＝VB thenV＝LS

step 2, calculating a global variable;

step 2.1, calculating curvatures of all target path points;

because the speed planning is established on the premise that a safe and collision-free path is provided by a path planning system, the number of speed points planned in a single time is not more than the number of the path planning points, and because of the consideration of safety and driving smoothness, a path planning algorithm often provides the farthest dynamic path planning result under the condition that an environment sensing system reaches the maximum, the speed planning can calculate the curvatures of all target path points at one time, and corresponding curvature results can be directly taken out from an internal memory when the next operation is carried out, and the calculation of the curvatures can be obtained through a simple triangular relation:

Step 2.2, carrying out moving average filtering aiming at the curvature;

the curvature calculated by using three continuous target path points generates high-frequency oscillation due to the close distance of the points, and under the working condition of continuous turning, a small-curvature road section is necessarily calculated in the process of alternating the bending direction of the path from left to right, but is not a straight line in macroscopic view. Both of the above cases result in the calculation of a target vehicle speed that is jittered or unreasonable; and the change of the output vehicle speed generated by the curvature change of the small curvature part and the short-distance slow-bending part in the two sections of straight roads in the continuous turning process is not in accordance with the normal driving habit, so that the filtering algorithm is required to be used for smoothing high-frequency vibration and weakening the curvature change under the relevant working conditions, and the adoption of the moving average filtering algorithm is considered:

according to the driving habits, the following steps are known: when the curve degree of the road at the front is larger or the road is a continuous curve, the driver often passes through the curve at a lower constant speed, and the driving speed cannot be adjusted in real time due to slight change of the curvature in the continuous curve process, so that the curvature queue is divided into different areas according to the absolute value, and the curvature queue in the same area is fitted to the same value, so that the subsequent fuzzy algorithm can output a stable speed curve.

L＝(K_filterMax-K_filterMin)/(N+q×(N-1))

T_min(i)＝K_filterMin+(i-1)×(1+q)×L

T_max(i)＝K_filterMin+(i-1)×(1+q)×L+L

Step 3, calculating the traveling speed of the target planning point;

the input quantity required by the fuzzy algorithm comprises a ground friction coefficient, a current transverse tracking error of the vehicle and the curvature of a front reference path point, the ground friction coefficient can be obtained by system presetting or analyzing the environment by using a sensor, the transverse tracking error can be directly calculated by using the position of the current vehicle and a target tracking track, the relevant information of the curvature of the front reference path is provided by a path planning system, the vibration of the curvature is directly obtained or calculated, the speed planning is not facilitated, the curvature of the dynamically planned path is compared with the area threshold value in the step 2.3, the curvature can be directly classified and approximated into an area standard value, and in consideration of safety, the curvature in a transition area is kept consistent with the area standard value in an adjacent standard area, wherein the area standard value is lower:

wherein K_fuzzyInputThe curve value can be used as fuzzy algorithm input quantity after approximation, r is the weight value of the area standard value in different standard areas, and K can be adjusted according to different planning strategies_fuzzyInputAnd (6) adjusting.

In combination with the runway example, starting from the coordinate (0, 0) point to the negative direction of the x-axis, the runway returns to the (0, 0) point after running for one circle along the runway, and the schematic diagram of the runway is shown in fig. 4. The results of the square wave fitting curvature according to this example are shown in fig. 5.

Step 3.2, calculating the output speed through a fuzzy algorithm;

after obtaining fuzzy algorithm input quantity, calculating through a membership function to obtain membership degrees of three input components of curvature, transverse tracking error and ground friction coefficient in different fuzzy subsets, wherein fuzzy implication relation utilizes a multiplier rule, and a defuzzification method adopts a weighted average method:

in the formula N_ruleNumber of fuzzy rules to activate, mu_kj(x_k)、μ_ej(x_e)、μ_mj(x_m) Respectively represents the membership degree, z, of the fuzzy subset in which the curvature, the transverse tracking error and the ground friction coefficient are respectively positioned in the activated 'j' fuzzy rule_jOutputting u for different fuzzy rules_speedThe abscissa value corresponding to the cusp of the membership function of the fuzzy subset. For the runway example, considering that the tracking performance is good, the planning speed of the target path point output by the fuzzy algorithm is shown as a blue cross-dashed line in fig. 6.

under the condition that the performance of a transverse tracking system is good and stable, a planned speed curve can generate certain tiny fluctuation due to the change of transverse errors, but the whole speed curve can present square wave-shaped speed curves with different levels of high, medium and low, however, the speed interconversion with different levels is realized in a jumping mode, although the tracking of the speed curve can be realized by utilizing a control system with constraints under the condition that the body feeling of a passenger is not influenced, the planning system has better applicability to the control system of the next level, so the planning system needs to carry out speed planning under the premise of considering the dynamic constraints of a vehicle or carry out secondary planning on the existing speed planning result.

The speed output curve is adjusted by constant acceleration and acceleration with an initial value of 0 on the premise that the speeds of different levels caused by curvatures of different standard areas are selected by taking no discomfort as a parameter at the mutual conversion:

u_speed(k+1)＝u_speed(k)+a(k+1)

wherein a is the acceleration, a_maxIs the maximum acceleration (or maximum deceleration) for avoiding the discomfort of the passenger, and Δ a is a constant jerk, u_speedHigh、u_speedLowHigher and lower speeds, respectively, for different levels of speed conversion. Considering the jerk of 0.2m/s³The resulting re-planning speed of the target waypoint is shown as a black "dotted solid line" in fig. 6.

Fig. 7 is a schematic diagram of the guideline results of the combination of the obtained speed and the runway example, and in order to more intuitively reflect the results of speed planning in different road sections, the diameter of the point in the defined result diagram is proportional to the speed, namely the size of the point reflects the speed. It can be seen from the results that the sudden change of speed due to the existence of the part with smaller curvature does not occur in the continuous turning process, but a moderate speed is stably output, a smaller speed is output on the road section with larger curvature, and a larger speed is output in the straight-line and small-curvature turning process. On the whole, the output speed is very stable, the speed is reduced in advance when the vehicle enters a bend, and the speed transition of different levels is very smooth, so that the effectiveness of the method is proved, and the use requirements of the unmanned vehicle speed planning system are met.

Claims

1. a bounded square wave shape vehicle speed planning method based on fuzzy algorithm, is characterized in that: the realization step of this method is as follows,

Step 1. Determine the membership function and fuzzy rules of the input and output of the system;

Step 2. Calculate global variables;

Step 3. Calculate the driving speed at the target planning point;

Step 1.1. Determine the input and output signals and their membership functions;

The input of the system is set as the curvature of the road ahead, the friction coefficient of the road surface and the lateral tracking error; the fuzzy language value of the curvature is B for large, M for medium, and S for small, and the fuzzy language value of the lateral tracking error is B, M, S, and the road friction The fuzzy language value of the coefficient is VB, which means very large, B means big, S means small, and VS means very small; the membership functions are all bell-shaped functions:

In the formula, a _bell , b _bell , and c _bell are the parameters of the generalized bell-shaped membership function, where a _bell determines the corresponding horizontal axis position when the membership degree of the bell-shaped membership function is 0.5. The greater the absolute value of b _bell , The faster the membership changes, the positive value of the opening of the clock-shaped function is downward, and c _bell determines the center position of the bell-shaped membership function; x is the driving speed of the target path point;

The output of the system is the driving speed of the target waypoint. The fuzzy language value is VB means very large, B means large, LB means relatively large, M means medium, LS means relatively small, S means small, VS means very small; the membership function adopts Triangular function:

In the formula, a _tri , b _tri , and c _tri are the parameters of the triangular membership function, which determine the position of the horizontal axis of the three vertices of the membership degree curve;

Step 1.2, design fuzzy rules;

When the friction coefficient is large, the output target path point has a larger range of driving speed and a higher upper limit. When the friction coefficient is small, the speed change is small, and the maximum speed is also small; the curvature is the most sensitive input. When the curvature increases, The output vehicle speed will decrease as a whole, and vice versa; the lateral tracking error is inversely proportional to the output vehicle speed, but the sensitivity is less on the road section with large friction coefficient and small road curvature;

Step 2.1. Calculate the curvature of all target path points;

Using a sliding window with a window length of 3, select 3 consecutive path points each time from the planned path provided by the path planning system, and use the triangular relationship between the three points to obtain the curvature of the target path point; through this method Calculate the curvature of all target waypoints on the path;

Li _-1,i , _Li,i+1 , _Li-1,i+1 in the formula are the distances between two consecutive three target path points, p is the process variable that simplifies the system formula, Calculated from _Li-1,i , _Li,i+1 , _Li-1,i+1 ;

Step 2.2, perform sliding average filtering for curvature;

The high-frequency vibration part of the curvature calculated by the triangular relationship will affect the system output results; under the condition of continuous turning, the change of the steering direction causes the change of the absolute value of the curvature to describe the real road curvature unreasonably; in order to solve the above problems , using the moving average filtering algorithm to smooth the curvature signal queue:

In the formula, K is the original curvature calculated by the triangular relationship, K _filter is the filtered curvature, 2n+1 is the sliding window length of the moving average filter; n is the one side of the sliding window of the moving average filter with i as the center point length;

Step 2.3. Calculate the thresholds and curvature references of different regions for square wave fitting;

Based on the absolute value of the global curvature queue, the curvature queue is divided into different standard areas, and there is a transition area between the standard areas, where the length of the standard area is

L=(K _filterMax -K _filterMin )/(N+q×(N-1))

where K _filterMax and K _filterMin are the maximum and minimum curvature values in the global curvature queue, respectively, N is the number of standard regions, and q is the ratio of the length of the transition region to the length of the standard region;

Through the obtained length and relationship between the standard area and the transition area, the upper and lower thresholds of each standard area can be obtained, where i=1,...,N is the serial number of the standard area:

T _min (i)=K _filterMin +(i-1)×(1+q)×L

T _max (i)=K _filterMin +(i-1)×(1+q)×L+L

After the upper and lower thresholds of each standard area are obtained, the curvature of all target path points in the corresponding standard area in the global curvature queue is arithmetically averaged to obtain the area standard value K of the current area.

where n _point is the number of target path points with curvature in the standard area, and K _filterRegion is the curvature set of target path points with curvature in the standard area;

Step 3.1. Obtain the input required by the fuzzy algorithm and perform square wave fitting for the curvature;

Compare the curvature of the dynamic programming path with the regional threshold in step 2.3, directly classify and approximate the curvature as the regional standard value, and for safety reasons, compare the curvature in the transition area with the regional standard value in the adjacent standard area. Low remains consistent:

Among them, K _fuzzyInput is the curvature value of the fuzzy algorithm input after approximation, r is the weight of the regional standard values in different standard regions, and K _fuzzyInput is adjusted according to different planning strategies; K _rollingPath is the target calculated in step 2.1. the original curvature of the waypoint;

Step 3.2, calculate the output speed through the fuzzy algorithm;

The fuzzy algorithm is used to calculate the output, the fuzzy implication relationship uses the opportunity rule, and the defuzzification method uses the weighted average method:

In the formula, N _rule is the number of activated fuzzy rules, μ _kj (x _k ), μ _ej (x _e ), μ _mj (x _m ) represent the three components of curvature, lateral tracking error and ground friction coefficient, respectively. In the activated "j" fuzzy rule, the membership degree of each fuzzy subset, z _j is the abscissa value corresponding to the cusp of the membership function of the fuzzy subset where the output u _speed is located in different fuzzy rules; x _k , x _e and x _m are the curvature of the target path point, the lateral tracking error and the friction coefficient, respectively;

Step 3.3, apply dynamic constraints to the obtained velocity for re-planning;

Adjust the velocity output curve again with a constant acceleration, with an initial value of 0 acceleration:

_uspeed (k+1)= _uspeed (k)+a(k+1)

In the formula, a is the acceleration, a _max is the maximum acceleration or maximum deceleration to prevent the passengers from feeling uncomfortable, Δa is the constant acceleration, u _speedHigh and u _speedLow are the higher and lower speeds when different levels of speed are converted; k is the sequence number of target waypoints.