CN109835336A - A kind of belt restraining square-wave-shaped speed planing method based on fuzzy algorithmic approach - Google Patents

Abstract

The belt restraining square-wave-shaped speed planing method based on fuzzy algorithmic approach that the invention discloses a kind of, belongs to automatic driving vehicle speed planning field.This method has determined the Fuzzy Rule Sets of fuzzy algorithmic approach input and output amount membership function and fuzzy algorithmic approach first.Then global variable needed for calculating subsequent algorithm, curvature signal queue, filtered curvature and the relevant global variable of square-wave-shaped fitting algorithm including having obtained planning path.Square-wave-shaped fitting finally is carried out to the curvature in rolling window and automobile, ambient condition meter is combined to calculate the speed of target trajectory point, considers further that dynamics of vehicle, the rate curve to cook up carries out quadratic programming.For the present invention from mankind's driving experience, the speed trajectory cooked up more meets normal driver's driving habit, to be easier to realize the correlation function of pilotless automobile and have better adaptability and safety.

Description

A kind of belt restraining square-wave-shaped speed planing method based on fuzzy algorithmic approach

Technical field

The invention belongs to automatic driving vehicle speed planning fields.Using fuzzy algorithmic approach, with reference in vehicle travel process Road ahead curvature, current horizontal tracing error and ground friction coefficient solve future travel rate curve, consider unmanned Actual environment further relates to the square-wave-shaped curve matching of a kind of Mean Filtering Algorithm for curvature and a kind of belt restraining in this method Method.

Background technique

In by the end of June, 2018 by, up to 2.29 hundred million, the registration automobile of first half of the year new registration in 2018 reaches China's automobile guarantee-quantity 13810000.Huge car ownership leads to that traffic congestion is serious, casualty accident takes place frequently, this, which has become China, to solve Certainly the problem of.The unmanned technology to grow up in recent years can effectively avoid the hair of traffic accident caused by human factor It is raw.Pilotless automobile includes environmental perception module, locating module, decision rule module and bottom control module, wherein decision It is which automobile go to that planning department in planning module, which decomposes certainly, how to be gone, when to the problem of, existing research and actually answer With planning problem is often resolved into path planning and two parts of speed planning, wherein reasonable speed planning can not only drop Whole traveling cost (time and energy consumption) of low automobile can also provide more comfortable safer seating body for occupant It tests.

Existing speed planning method is limited in by energy-output ratio building objective function or mostly based on adaptive Cruise system realizes speed planning according to preceding car state.Although these traditional speed planning modes are under existing operating condition It is largely effective, but for Unmanned Systems, automobile need not against car-following model it is more intelligent from Main selection running speed, such as the feelings of large error are had already appeared in Duct With Strong Curvature, wet and slippery road or automobile horizontal tracing Under condition, speed ought to be reduced to obtain better intact stability, and linear road, the biggish dry pavement of coefficient of friction and Under conditions of automatic steering system better performances, the economic speed per hour row with higher speed or current vehicle is can be considered in vehicle It sails, so that obtaining shorter by the time or reduce energy consumption and guarantee course continuation mileage.

Speed planning system is a multiple input single output, and is difficult to the nonlinear system of construct mathematical model.We are past Before passing through toward bending degree, the wet and slippery degree etc. by judging road ahead because of usually trial use high speed, middling speed or low speed There is the concept of many " big, general, small " in Fang Daolu, so being compared to optimal algorithm, fuzzy algorithmic approach is not during this By be for calculating speed or application scenarios it is all more particularly suitable.And in curvature calculating process, since computer can be accurate Calculating curvature size, curvature can inevitably shake in a certain range, and the concussion of input can influence output to a certain extent As a result.Consider the comfort taken, speed should keep a stable value in a certain range, such as enter continuous turning work A stable relatively low velocity should be maintained under condition always, without should because of occur in continued curve process one section of curvature compared with One section of high velocity is just cooked up in low part, so introducing certain filtering algorithm in algorithm and considering dynamics of vehicle constraint Square-wave-shaped speed planning, to obtain stable speed and more soft speed handoff procedure.

Summary of the invention

The travel speed of pilotless automobile should the wet and slippery degree for considering the bending degree of road ahead, current road, Dynamic Programming in the case where the input quantities such as the lateral deviation size having already appeared, and in order to obtain it is good, comfortably take body It tests, the speed of planning should be steady as far as possible, avoids occurring larger or high frequency concussion in a relatively short period of time, should not senior middle school Generating in the handoff procedure of low speed enables occupant generate uncomfortable body-sensing, thus The present invention gives the bands based on fuzzy algorithmic approach about Beam square-wave-shaped speed planing method, adaptive capacity to environment, the riding comfort of pilotless automobile can be promoted using this method And security performance.

To achieve the above object, the technical solution adopted by the present invention is a kind of belt restraining square-wave-shaped vehicle based on fuzzy algorithmic approach Fast planing method, steps are as follows for the realization of this method:

Step 1, the membership function and fuzzy rule for determining system input and output；

Step 1.1 determines input/output signal and its membership function；

The input of system is set to road ahead curvature, surface friction coefficient and horizontal tracing error.Curvature fuzzy language value Indicated in big, M expression for B, S indicates small, the fuzzy language value of horizontal tracing error is B, M, S, surface friction coefficient it is fuzzy Linguistic Value is that VB indicates that very big, B, S, VS indicate very small.Subordinating degree function is all made of bell shaped function:

A in formula_bell、b_bell、c_bellFor the parameter of generalized bell membership function, wherein a_bellDetermine bell membership letter When number degree of membership is 0.5, corresponding horizontal axis position, b_bellAbsolute value it is bigger, degree of membership variation speed it is faster, take positive value Open Side Down for clock shape function, c_bellThe center of bell membership function has been determined.

System output is the running speed of destination path point, fuzzy language value VB, B, LB indicate bigger, M indicate in, LS indicates that smaller, S indicates that small, VS indicates very small.Subordinating degree function uses triangular function:

A in formula_tri、b_tri、c_triFor the parameter of triangular membership, the cross on three vertex of degree of membership curve is determined Shaft position.

Step 1.2, design fuzzy rule；

The running speed range of the destination path point exported when big coefficient of friction is bigger, the upper limit is higher, when small coefficient of friction, Speed variation is small, and F-Zero is also smaller；Curvature is most sensitive input quantity, and when curvature increases, output speed will be whole Reduce, otherwise increases；Horizontal tracing error is inversely proportional with output speed, but the lesser road of and road curvature larger in coefficient of friction Susceptibility is smaller when in section.

Step 2 calculates global variable；

Step 2.1 calculates target complete path point curvature；

The sliding window for being 3 using length of window chooses 3 from the path cooked up that path planning system provides every time A continuous path point, the curvature of destination path point is acquired using the triangle relation between 3 points.It calculates by this method The curvature of target complete path point on path.

L in formula_i-1,i、L_i,i+1、L_i-1,i+1For the distance of continuous three destination paths point between any two.

Step 2.2 carries out moving average filter for curvature；

It will affect system by high-frequency vibration part present in the calculated curvature of triangle relation and export result；It is continuous to turn Under curved operating condition, the change of steering direction causes the variation of curvature absolute value unreasonable to the description of real roads bending degree.For Problem above is solved, moving average filter algorithm smooth curvature signal queue is utilized:

K is to pass through the calculated original curvature of triangle relation, K in formula_filterTo pass through filtered curvature, 2n+1 is to slide The sliding window length of dynamic average filter.

Step 2.3, the different zones threshold value and curvature reference quantity for calculating square wave fitting；

Using the order of magnitude of global curvature queue as foundation, curvature queue is divided into different standard areas, standard regions There are transitional region between domain, wherein the length of standard area is

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

Wherein K_filterMaxAnd K_filterMinMinimum and maximum curvature value in respectively global curvature queue, N is standard regions The number in domain, the ratio of q transitional region length and standard area length.

Length and relationship by obtained standard area and transitional region, can be in the hope of the threshold up and down of each standard area Value, wherein i=1 ..., N are the serial numbers of standard area:

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

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

It obtains all targets in global curvature queue in respective standard region after the upper lower threshold value of each standard area The curvature of path point, which is done sums, averagely obtains the regional standard value of current region

Wherein n_pointThe number for the destination path point for being curvature in standard area, K_filterRegionIt is curvature in standard regions The curvature set of destination path point in domain.

Step 3 calculates goal programming point running speed；

Step 3.1 obtains fuzzy algorithmic approach required input amount and carries out square-wave-shaped fitting for curvature；

Region threshold in the curvature of dynamic programming path and step 2.3 is compared, directly by curvature classification and approximation For regional standard value, and for security consideration, enable in curvature and the adjacent modular region in transitional region regional standard value compared with Low is consistent:

Wherein K_fuzzyInputFor the curvature value after approximation as fuzzy algorithmic approach input quantity, r is various criterion region inner region The weight of domain standard value, according to different planning strategies to K_fuzzyInputIt is adjusted.

Step 3.2 calculates output speed by fuzzy algorithmic approach；

It is calculated and is exported using fuzzy algorithmic approach, for Fuzzy implication relationship using rule is seized the opportunity, de-fuzzy method is flat using weighting Equal method:

N in formula_ruleFor the fuzzy rule number of activation, μ_kj(x_k)、μ_ej(x_e)、μ_mj(x_m) respectively represent curvature, cross To three components of tracking error and ground friction coefficient in " j " fuzzy rule being activated, respective place fuzzy subset's Degree of membership, z_jTo export u in different fuzzy rules_speedPlace fuzzy subset's subordinating degree function cusp corresponding to abscissa Value.

Step 3.3 applies Dynamic Constraints progress weight-normality stroke to the speed of acquisition；

It is regulated the speed again curve of output using constant acceleration, the acceleration that initial value is 0:

u_speed(k+1)=u_speed(k)+a(k+1)

A is acceleration, a in formula_maxIt is the peak acceleration (or maximum deceleration) for avoiding rider from generating uncomfortable body-sensing, Δ a is constant acceleration, u_speedHigh、u_speedLowHigher and lower speed when being the conversion of different brackets speed respectively.

Detailed description of the invention

Fig. 1 invention total algorithm flow chart.

The membership function schematic diagram of Fig. 2 input/output signal.

Fig. 3 fuzzy reasoning surface chart.

Fig. 4 runway example schematic.

The filtering of Fig. 5 curvature and square-wave-shaped are fitted schematic diagram.

Fig. 6 fuzzy algorithmic approach output speed and belt restraining weight-normality draw schematic diagram.

Fig. 7 runway example speed planning result schematic diagram.

Specific embodiment

Present invention will be further explained below with reference to the attached drawings and specific embodiments.

The present invention is directed to pilotless automobile speed planning system, proposes a kind of belt restraining square wave based on fuzzy algorithmic approach Shape speed planing method.This method has determined the subordinating degree function and fuzzy rule of fuzzy algorithmic approach input and output amount first, uses Fuzzy algorithmic approach cooks up the reference speed of destination path point, solves speed planning system modelling difficult problem, and to be confined to New resolving ideas is provided come the status of desin speed planning system using traditional car-following model or Economic Energy time-consuming speed.So It is relevant complete to having carried out filtering using the calculated curvature signal queue of triangle relation and having calculated square-wave-shaped fitting algorithm afterwards Office's variable, it is excessively frequent to solve computer calculated curvature fluctuation, does not meet asking for the thinking habit of normal automotive driving Topic.It is converted into output speed by input quantity has been obtained finally by fuzzy algorithmic approach, considers further that dynamics of vehicle, for the speed cooked up Curve carries out quadratic programming.Even, can also be in stabilization based on the unpowered speed tracking system for learning constraint controller design Tracking final speed curve in the case where, provided for occupant and stablize, comfortable experience by bus.The system stream of involved method Journey figure is as shown in Fig. 1, and specific implementation process is divided into step:

Step 1, the membership function and fuzzy rule for determining system input and output；

Step 1.1 determines input/output signal and its membership function；

According to driving habit, when front road curvature is larger, at a high speed by that can generate biggish centrifugal phenomenon, this can be given Occupant brings poor experience by bus；When road surface is more wet and slippery, tire will appear slipping phenomenon, and maintenance is run at high speed and can be led It causes in the operating condition for urgent doubling, brake occur to be that dangerous situation occurs, jeopardizes property or even life security；When automobile is on tracking target road When diameter has had already appeared relatively large deviation, speed should be reduced suitably to improve the horizontal tracing performance of motor turning controller, reduced Horizontal tracing error.Conversely, road ahead is straight, ground adhesion condition is good, in the lesser situation of horizontal tracing error, can With raising running speed appropriate.

Input is set to three road ahead curvature, surface friction coefficient and horizontal tracing error components, mean curvature and cross It is that B is indicated in big, M expression, S indicates small to the fuzzy language value of tracking error, the fuzzy language value of surface friction coefficient is VB Indicate that very big, B, S, VS indicate very small.Subordinating degree function uses bell shaped function:

Output is the running speed of destination path point, and fuzzy language value VB, B, LB indicate that bigger, M, LS expression is compared Small, S, VS.Subordinating degree function uses triangular function:

The membership function schematic diagram of system input/output signal is as shown in Fig. 2, wherein Fig. 2-a, 2-b, 2-c, 2-d are respectively Curvature, lateral error, ground friction coefficient and speed subordinating degree function schematic diagram.

Step 1.2, design fuzzy rule；

Surface friction coefficient directly affects the grip performance of tire, the destination path exported when so setting big coefficient of friction The running speed range of point is bigger, the upper limit is higher；When small coefficient of friction, speed variation is small, and F-Zero is also controlled in safety In speed.

The curvature of road ahead directly affects the body-sensing of occupant and the lateral stability of automobile, so whole fuzzy rule Then by using curvature as most sensitive input quantity, when curvature increases, input speed increases whole reduction.

Automobile during entering curved horizontal tracing error increase be normal phenomenon, can automobile go out it is curved after gradually restrain, Consider under different operating conditions the size of horizontal tracing error reacted different vehicle conditions, so it is larger in coefficient of friction and On the lesser section of road curvature, fuzzy rule is smaller to the susceptibility of horizontal tracing error；The smaller and curvature in coefficient of friction Biggish road surface promotes the susceptibility to horizontal tracing error input quantity.

Regulation curvature is K, lateral error E, coefficient of friction Mu, and speed V designs specific Fuzzy Rule Sets such as Under, fuzzy reasoning curved surface schematic diagram such as 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 calculates global variable；

Step 2.1 calculates target complete path point curvature；

Because being established when speed planning under the premise of a safety collisionless path has been given in path planning system, So single planning speed point number be not more than path planning point, for safety and drive ride comfort the considerations of, road Farthest active path planning is often provided in the case where context aware systems reach as far as possible for diameter planning algorithm as a result, institute The curvature that all destination path points can be gone out with one-time calculation with speed planning, when carrying out next step operation directly from memory Middle to take out corresponding curvature result, the calculating of curvature can be obtained by simple triangle relation:

L in formula_i-1,i、L_i,i+1、L_i-1,i+1For the distance of continuous three destination paths point between any two.

Step 2.2 carries out moving average filter for curvature；

High frequency oscillation can be generated because of the hypotelorism put using the calculated curvature of continuous three destination paths point, and And under the operating condition of continuous turning, small curvature section will necessarily be calculated during the bending direction in path or so is alternate, so And macroscopically seeing not is one section of straight line.Either way will lead to above is calculated in the presence of shake or unreasonable target carriage Speed；And the Curvature varying institute of bight portion is delayed in the short distance in the slight curvature and two sections of linear roads during continuous turning The variation for generating output speed does not meet normal driving habit, so needing to vibrate using filtering algorithm smooth high frequencies, cutting Curvature varying under weak correlation operating condition, is considered as moving average filter algorithm:

K is to pass through the calculated original curvature of triangle relation, K in formula_filterTo pass through filtered curvature, 2n+1 is to slide The sliding window length of dynamic average filter.

Step 2.3, the different zones threshold value and curvature reference quantity for calculating square wave fitting；

According to driving habit: when front road curvature degree is larger or be continued curve when, driver is often with one A lower constant vehicle speed passes through, and adjusts driving speed in real time without the slight change because of continued curve process mean curvature Degree, so curvature queue is divided into different zones according to the size of absolute value here, and the curvature queue in the same area is quasi- It is combined into the same value, fuzzy algorithmic approach subsequent in this way could export stable rate curve.

Using the order of magnitude of global curvature queue as foundation, curvature queue is divided into different standard areas, standard regions There are transitional region between domain, wherein the length of standard area is

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

Wherein K_filterMaxAnd K_filterMinMinimum and maximum curvature value in respectively global curvature queue, N is standard regions The number in domain, the ratio of q transitional region length and standard area length.

Length and relationship by obtained standard area and transitional region, can be in the hope of the threshold up and down of each standard area Value, wherein i=1 ..., N are the serial numbers of standard area:

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

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

It obtains all targets in global curvature queue in respective standard region after the upper lower threshold value of each standard area The curvature of path point, which is done sums, averagely obtains the regional standard value of current region

Wherein n_pointThe number for the destination path point for being curvature in standard area, K_filterRegionIt is curvature in standard regions The curvature set of destination path point in domain.

Step 3 calculates goal programming point running speed；

Step 3.1 obtains fuzzy algorithmic approach required input amount and carries out square-wave-shaped fitting for curvature；

Fuzzy algorithmic approach required input amount has the current horizontal tracing error of ground friction coefficient, vehicle and front reference path point Amount of curvature, ground friction coefficient can be analyzed to obtain by systemic presupposition or using sensors towards ambient, laterally with Track error can directly utilize the position of current vehicle and target following trajectory calculation, the related letter of the curvature of front reference path Breath provided by path planning system, directly obtain or be calculated curvature exist concussion, be unfavorable for speed planning, by will dynamic The curvature of planning path and the region threshold in step 2.3 compare, and directly curvature can be classified and approximation turns to regional standard Value, and for security consideration, enable the lower holding one of regional standard value in curvature and the adjacent modular region in transitional region It causes:

Wherein K_fuzzyInputFor the curvature value that can be used as fuzzy algorithmic approach input quantity after approximation, r is in various criterion region The weight of regional standard value, can be according to different planning strategies to K_fuzzyInputIt is adjusted.

In conjunction with runway example, puts from coordinate (0,0) to negative direction of the x-axis and set out, returned to after a week along running on track (0,0) Point, runway schematic diagram are as shown in Figure 4.It is as shown in Figure 5 that square-wave-shaped regressive curvature result figure is obtained according to the example.

Step 3.2 calculates output speed by fuzzy algorithmic approach；

After obtaining fuzzy algorithmic approach input quantity, curvature, horizontal tracing error and ground friction are calculated by membership function Degree of membership in each comfortable different fuzzy subsets of coefficient three input components, Fuzzy implication relationship utilize and seize the opportunity rule, deblurring Change method uses weighted mean method:

N in formula_ruleFor the fuzzy rule number of activation, μ_kj(x_k)、μ_ej(x_e)、μ_mj(x_m) respectively represent curvature, cross To three components of tracking error and ground friction coefficient in " j " fuzzy rule being activated, respective place fuzzy subset's Degree of membership, z_jTo export u in different fuzzy rules_speedPlace fuzzy subset's subordinating degree function cusp corresponding to abscissa Value.For above-mentioned runway example, consider that tracking performance is good, the planning speed of the destination path point exported by fuzzy algorithmic approach is such as In Fig. 6 shown in blue " fork dotted line ".

Step 3.3 applies Dynamic Constraints progress weight-normality stroke to the speed of acquisition；

Under conditions of horizontal tracing system performance is well stable, although the rate curve cooked up can also be because of lateral mistake The variation of difference generates certain small fluctuations, but high, medium and low different grades of square-wave-shaped rate curve can be presented in entirety, however Different grades of speed, which is mutually converted, to be realized in the form of jump, although the control system using belt restraining can be in not shadow The tracking of rate curve is realized in the case where ringing rider's body-sensing, but in view of planning system should be to the control system of next stage There is preferable applicability, so planning system should carry out speed planning under the premise of considering dynamics of vehicle constraint, or Quadratic programming is carried out in existing speed planning result.

The different grades of speed as caused by the curvature in various criterion region, not generate not aptamer at mutual conversion Sense is the premise that parameter is selected, and is regulated the speed curve of output by constant acceleration, the acceleration that initial value is 0:

u_speed(k+1)=u_speed(k)+a(k+1)

A is acceleration, a in formula_maxIt is the peak acceleration (or maximum deceleration) for avoiding rider from generating uncomfortable body-sensing, Δ a is constant acceleration, u_speedHigh、u_speedLowHigher and lower speed when being the conversion of different brackets speed respectively.It examines Consider acceleration 0.2m/s³, it is shown that the weight-normality of obtained destination path point draws black in speed such as Fig. 6 " point solid line ".

Fig. 7 is the simulation results schematic diagram that gained speed is combined with runway example, is not gone the same way to more intuitively react Section in speed planning as a result, define result figure midpoint diameter it is directly proportional to velocity magnitude, that is, the size put has reacted speed Size.As can be seen from the results, speed will not occur because of there are the lesser part of curvature during continuous turning Mutation, but stablize one moderate speed of output, lesser speed is exported in the biggish section of curvature, straight line, small curvature turn It is curved, export biggish speed.On the whole, output speed is very steady, enters curved pre-decelerating, different grades of speed transition It is very smooth, it was demonstrated that the validity of method meets the requirement of pilotless automobile speed planning system.

Claims (4)

1. a kind of belt restraining square-wave-shaped speed planing method based on fuzzy algorithmic approach, it is characterised in that: the realization step of this method It is as follows,

Step 1, the membership function and fuzzy rule for determining system input and output；

Step 2 calculates global variable；

Step 3 calculates goal programming point running speed.

2. a kind of belt restraining square-wave-shaped speed planing method based on fuzzy algorithmic approach according to claim 1, feature exist In:

Step 1.1 determines input/output signal and its membership function；

The input of system is set to road ahead curvature, surface friction coefficient and horizontal tracing error；Curvature fuzzy language value is B table Show in big, M expression, S indicates small, the fuzzy language value of horizontal tracing error is B, M, S, the fuzzy language value of surface friction coefficient Indicate that very big, B, S, VS indicate very small for VB；Subordinating degree function is all made of bell shaped function:

A in formula_bell、b_bell、c_bellFor the parameter of generalized bell membership function, wherein a_bellDetermine that bell membership function is subordinate to When degree is 0.5, corresponding horizontal axis position, b_bellAbsolute value it is bigger, degree of membership variation speed it is faster, take positive value clock-shaped Open Side Down for function, c_bellThe center of bell membership function has been determined；

System output is the running speed of destination path point, fuzzy language value VB, B, LB indicate bigger, M indicate in, LS table Show that smaller, S indicates that small, VS indicates very small；Subordinating degree function uses triangular function:

A in formula_tri、b_tri、c_triFor the parameter of triangular membership, the horizontal axis position on three vertex of degree of membership curve is determined It sets；

Step 1.2, design fuzzy rule；

The running speed range of the destination path point exported when big coefficient of friction is bigger, the upper limit is higher, when small coefficient of friction, speed Change small, and F-Zero is also smaller；Curvature is most sensitive input quantity, when curvature increases, output speed by whole reduction, Otherwise increase；Horizontal tracing error is inversely proportional with output speed, but on the lesser section of and road curvature larger in coefficient of friction When susceptibility it is smaller.

3. a kind of belt restraining square-wave-shaped speed planing method based on fuzzy algorithmic approach according to claim 1, feature exist In:

Step 2.1 calculates target complete path point curvature；

The sliding window for being 3 using length of window chooses 3 companies from the path cooked up that path planning system provides every time Continuous path point acquires the curvature of destination path point using the triangle relation between 3 points；Outbound path is calculated by this method The curvature of upper target complete path point；

L in formula_i-1,i、L_i,i+1、L_i-1,i+1For the distance of continuous three destination paths point between any two；

Step 2.2 carries out moving average filter for curvature；

It will affect system by high-frequency vibration part present in the calculated curvature of triangle relation and export result；Continuous turning work Under condition, the change of steering direction causes the variation of curvature absolute value unreasonable to the description of real roads bending degree；To solve Problem above utilizes moving average filter algorithm smooth curvature signal queue:

K is to pass through the calculated original curvature of triangle relation, K in formula_filterTo pass through filtered curvature, 2n+1 is that sliding is flat The sliding window length filtered；

Step 2.3, the different zones threshold value and curvature reference quantity for calculating square wave fitting；

Using the order of magnitude of global curvature queue as foundation, curvature queue is divided into different standard areas, standard area it Between there are transitional region, wherein the length of standard area is

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

Wherein K_filterMaxAnd K_filterMinMinimum and maximum curvature value in respectively global curvature queue, N are standard area Number, the ratio of q transitional region length and standard area length；

Length and relationship by obtained standard area and transitional region, can in the hope of the upper lower threshold value of each standard area, Wherein i=1 ..., N is the serial number of standard area:

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

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

It obtains all destination paths in global curvature queue in respective standard region after the upper lower threshold value of each standard area The curvature of point, which is done sums, averagely obtains the regional standard value of current region

Wherein n_pointThe number for the destination path point for being curvature in standard area, K_filterRegionIt is curvature in standard area Destination path point curvature set.

4. a kind of belt restraining square-wave-shaped speed planing method based on fuzzy algorithmic approach according to claim 1, feature exist In:

Step 3.1 obtains fuzzy algorithmic approach required input amount and carries out square-wave-shaped fitting for curvature；

Region threshold in the curvature of dynamic programming path and step 2.3 is compared, directly by curvature classification, simultaneously approximation turns to area Domain standard value, and for security consideration, enable the curvature in transitional region and regional standard value in adjacent modular region lower It is consistent:

Wherein K_fuzzyInputFor the curvature value after approximation as fuzzy algorithmic approach input quantity, r is various criterion region inner region standard The weight of value, according to different planning strategies to K_fuzzyInputIt is adjusted；

Step 3.2 calculates output speed by fuzzy algorithmic approach；

It is calculated and is exported using fuzzy algorithmic approach, Fuzzy implication relationship uses weighted mean method using rule, de-fuzzy method is seized the opportunity:

N in formula_ruleFor the fuzzy rule number of activation, μ_kj(x_k)、μ_ej(x_e)、μ_mj(x_m) respectively represent curvature, horizontal tracing Three components of error and ground friction coefficient are in " j " fuzzy rule being activated, the degree of membership of respective place fuzzy subset, z_jTo export u in different fuzzy rules_speedPlace fuzzy subset's subordinating degree function cusp corresponding to abscissa value；

Step 3.3 applies Dynamic Constraints progress weight-normality stroke to the speed of acquisition；

It is regulated the speed again curve of output using constant acceleration, the acceleration that initial value is 0:

u_speed(k+1)=u_speed(k)+a(k+1)

A is acceleration, a in formula_maxIt is the peak acceleration or maximum deceleration for avoiding rider from generating uncomfortable body-sensing, Δ a is perseverance Fixed acceleration, u_speedHigh、u_speedLowHigher and lower speed when being the conversion of different brackets speed respectively.