CN109765887B - Automatic driving control method - Google Patents

Abstract

The invention discloses an automatic driving control method, and belongs to the technical field of automatic driving. The existing control method is feasible for simple road conditions, but for a slightly complex traffic environment, a unified program framework is difficult to describe, and the conventional vehicle has poor comfort because the speed of the vehicle jumps due to frequent switching of states and targets. The invention adopts a spline interpolation algorithm, can accurately describe a complex route, directly calculates coordinate values of a plurality of intermediate points by obtaining discrete points such as turning coordinates and obstacle coordinates on the route, and then controls the motion of the unmanned automobile according to the interpolated coordinate values. The S-type acceleration and deceleration control can well realize smooth start and stop and speed switching of the vehicle, can achieve no sudden change of acceleration, and can well reduce the motion impact and the shock of the vehicle, thereby effectively improving the comfort of the automatic driving vehicle.

Description

Automatic driving control method

Technical Field

The invention relates to an automatic driving control method, and belongs to the technical field of automatic driving.

Background

The automatic driving technology of the automobile comprises a video camera, a radar sensor and a laser range finder for knowing surrounding traffic conditions, and is realized through a data center, and the automatic driving automobile is equivalent to a remote control automobile or an intelligent automobile of the data center. The intelligent driving technology is an extremely important component in the field of intelligent transportation, and with the rapid increase of computing power and the increasing demand for intelligent transportation automation, more and more national research institutes and company enterprises pay more attention to the field of intelligent driving.

The related documents and reports disclosed at present mainly use the judgment and matching of a plurality of modes to carry out the longitudinal control of the vehicle, the implementation is simpler, the frequent switching among the plurality of modes is easy, the traditional longitudinal control of the vehicle adopts a fusion algorithm of functions of a constant-speed cruise CC, an adaptive cruise ACC and an automatic emergency brake AEB, the algorithm is feasible for simple road conditions, but for a slightly complex traffic environment, a unified program framework is difficult to describe, and the traditional vehicle has the problem of vehicle speed jump due to the frequent switching of states and targets and is poor in comfort.

Disclosure of Invention

Aiming at the defects of the prior art, the invention aims to provide the comfortable automatic driving control method which can deal with the complex traffic environment, uniformly describe the driving route and reduce the speed jump of the unmanned automobile.

In order to achieve the purpose, the technical scheme of the invention is as follows:

an automatic driving control method comprising the steps of:

firstly, acquiring a path map through a manned automobile;

secondly, acquiring peripheral images of the unmanned vehicle through a camera, and comparing the peripheral images with corresponding positions of a path map to judge whether obstacles exist or not, wherein the obstacles comprise vehicles, people or road piles;

thirdly, measuring the distance between the unmanned automobile and the obstacle by using the laser radar

Fourthly, generating a driving route according to the route map and the distance information of the obstacles;

fifthly, planning a running route according to a spline interpolation method;

sixthly, performing speed control on the unmanned automobile by adopting an S-shaped acceleration and deceleration method;

and seventhly, the unmanned automobile runs according to the planned route and the preset speed.

The invention adopts a spline interpolation algorithm, can accurately describe a complex route, directly calculates coordinate values of a plurality of intermediate points by obtaining discrete points on the route, such as turning coordinates and obstacle coordinates, and then controls the motion of the unmanned automobile according to the interpolated coordinate values. The S-type acceleration and deceleration method is adopted to control the running speed of the unmanned automobile, and S-type acceleration and deceleration control has the advantages of smooth and uniform speed curve, stable movement, no jump and the like, and is comfortable. The S-type acceleration and deceleration control can well realize smooth start and stop and speed switching of the vehicle, can achieve no sudden change of acceleration, and can well reduce the motion impact and the shock of the vehicle, thereby effectively improving the comfort of the automatic driving vehicle.

The spline interpolation and S-type acceleration and deceleration control are mainly used in the aspect of numerical control machine tool machining, the machining precision and the machining efficiency of numerical control machining can be effectively improved, the bias of the prior art is broken through, the spline interpolation and S-type acceleration and deceleration control are applied to the automatic driving control technology, the design is ingenious, the scheme is detailed, the method is practical and feasible, and the applicability and the comfort of an automatic driving vehicle can be greatly improved.

As a preferred technical measure it is provided that,

for a complex route to be driven, firstly, the route needs to be accurately described, discrete points on the route are easy to obtain, if a conventional parameter curve is directly used for fitting according to the discrete points, the number of times of the parameter curve is high, and the driving route is difficult to accurately describe; therefore, the driving route is described by a spline curve;

the driving route is adjusted at any time according to the movement of the obstacle, and a spline interpolation control point is formed by adding starting and ending position points according to the position and the path characteristics of the obstacle; generating a passing point according to the control point and the necessary safety distance; generating a speed boundary control curve according to the moving speed and the path characteristics of the barrier and the legal speed limit;

the NURBS curve in the spline interpolation method consists of four factors, namely a control point, a node vector, a weight factor and a basis function; obtaining a node vector through a correlation formula according to the control point, further obtaining a basis function through the node vector, and generally giving a weight factor or calculating according to an offset; performing deformation calculation on the basis of a Hadeli-Giardian method according to a known control point to obtain a proper node vector; and calculating a weight factor according to the required offset, obtaining the weight factor and the node vector to obtain a basis function, and further constructing a NURBS curve by using the control points, the basis function and the weight factor.

As a preferred technical measure it is provided that,

calculating a node vector:

m + p +1 control points are known

p is the constructed basis function number; according to the distance between the control points, the proper section is calculatedA point vector value u;

firstly, the distance d between the control points is calculated_jThe formula is as follows:

wherein x_jIndicating a control point P_jThe abscissa of (a) and (b) the abscissa of (a)_jIndicating a control point P_jThe longitudinal axis coordinate of (a);

and (3) deforming by a recursive formula of a Hadeli-Giardian method, wherein the formula after deformation is as follows:

wherein u is_iExpressing the ith node, and further obtaining a node vector U recursion formula as follows:

the node vector is solved in the form: u ═ 0, 0, 0, U_p，u_p+1，...u_m+p1，m+1，m+1，m+1]；

NURBS expression:

the basis functions are obtained by using the node vectors, the NURBS basis functions have a plurality of construction forms, and the commonly used construction form is given by the following recursion formula, and the basis function N is determined by the following recursion mode_i，p(U) P NURBS basis functions corresponding to the node vector U:

p≥2

wherein i is the serial number of the basis function, a node vector U is given, and the required basis function can be deduced according to the recursion formula; and then canObtaining the i-th NURBS curve P_i(u) the expression is:

u∈[u_i+p，u_i+p+1]，i＝1，2，3...m+1。

the NURBS curve is a special parameter curve that can be expressed simply and efficiently for complex curves. The NURBS curve is a non-uniform rational B-spline, and can accurately represent a quadratic curve by introducing a weight factor and a denominator, namely, can accurately describe various complex paths.

As a preferred technical measure it is provided that,

effect of weighting factors on NURBS curves:

the distance between the NURBS curve and the control point can be adjusted by the weight factor, the larger the value of the weight factor is, the closer the NURBS curve is to the corresponding control point, the corresponding weight factor can be calculated by utilizing the offset according to a relational expression of the weight factor and the offset, the weight factor is substituted into the NURBS curve expression, the adjustment of the NURBS curve is further realized, the weight factor is determined according to the empirical value or the length and the width of the vehicle body, and the initial value of the weight factor is 1;

respectively calculating when u is equal to u_k，w_kWhen 1, the interpolation point p₁When u is equal to u_k，w_kWhen 0, the interpolation point p₀；

Further, the distance S can be obtained_k，K_kThe calculation formula is as follows:

wherein P is_KIs the kth control point, k being 0, 1, 2

The weight factor w can be obtained_kAnd offset d_kThe calculation formula is as follows:

as a preferred technical measure it is provided that,

the unmanned automobile moves from an initial position to a target position, the description of the posture change vector of the automobile body is related, an automobile body shaft is simplified into a straight rod, and the change posture of the straight rod in the space is described;

given discrete points through which the unmanned automobile needs to pass and the corresponding change angle of an automobile body shaft, describing the pose change from a starting point to a target point; according to the discrete points which need to be passed by the unmanned automobile and the space change angle of the automobile body shaft, the length of the automobile body shaft is generally fixed and unchanged; according to the effective length and the corresponding spatial variation angle, the passing points of the vehicle body shaft can be obtained by utilizing the vehicle body passing points, then two corresponding groups of control points are back-calculated through the NURBS theory, interpolation calculation is carried out according to the control points by utilizing the NURBS theory to generate a series of points, the points are substituted into a spline interpolation formula to carry out back solution calculation, and the position information at the front end and the rear end is obtained;

given that the discrete point through which the unmanned vehicle needs to pass is T_i＝(xt_i，yt_i，zt_i) And a set of varying angles of the body axis (α)_i，β_i) The length of the vehicle body is fixed and is set to be l, and the discrete passing point coordinate H of the corresponding vehicle body axis vector_i＝(xh_i，yh_i，zh_i) The calculation formula of (2) is as follows:

xh_i＝xt_i+lcosα_icosβ_i

yh_i＝yt_i+lcosα_isinβ_i

zh_i＝zt_i+lsinα_i

through the steps, the vehicle body passing point and the other group of passing points of the vehicle body axis vector can be obtained, so that the change of the vehicle body axis can be clearly described, and the corresponding control point can be obtained by knowing the passing point according to the NURSB curve generation theory;

in order to accurately describe the change of the unmanned automobile, a tangent vector of the track change of the unmanned automobile also needs to be known, the NURBS curve can be regarded as a parameter equation about u, the tangent vector of an automobile body interpolation track can be obtained through calculation of a derivation theory of the parameter equation, namely, the derivation is carried out on u by a basis function, and then the tangent vector is substituted into a related NURBS formula;

the calculation formula is as follows:

according to the control characteristics of the pose of the unmanned automobile, the invention provides that the pose change is described through two groups of related NURBS curves, the change relation between the curve offset and the weight factor is given, so that the NURBS curves can be flexibly changed, the tangent vector of the spline curve is simultaneously solved to facilitate the inverse solution calculation, the distance between the automobile and the obstacle is controlled through the weight factor, and the safety and perfect obstacle avoidance during the automobile driving are ensured to the maximum extent.

As a preferred technical measure it is provided that,

for the situation that the speeds of the initial position and the final position of the unmanned automobile are both zero; the S-type acceleration and deceleration method is a non-symmetrical S-type acceleration and deceleration control method:

the conventional point-to-point track planning adopts a symmetrical S-shaped acceleration and deceleration control method, and the running process is divided into seven sections, namely an acceleration section, a uniform acceleration section, an acceleration and deceleration section, a uniform velocity section, an acceleration and deceleration section, a uniform deceleration section and a deceleration and deceleration section; the time values of the four time periods of the acceleration section, the deceleration section, the acceleration section and the deceleration section are the same, and the time values of the uniform acceleration section and the uniform deceleration section are the same; considering the influence of the acceleration on the mechanism motion, the invention adopts an asymmetric S-shaped acceleration and deceleration control method, the acceleration section adopts larger acceleration in the beginning stage, and the deceleration section adopts smaller acceleration in the ending stage, so that the problems that the vehicle body stops and the precision and the efficiency cannot be considered because of larger vibration caused by overlarge acceleration are solved;

for a section of complete symmetrical S-shaped acceleration and deceleration control method, according to a time optimal algorithm, the trajectory planning of the method actually solves three time quantities: jerk time t_jTime of uniform acceleration t_aTime of uniform velocity t_vThe asymmetric S-type acceleration and deceleration control method is adopted, the track planning is more complicated than that of the symmetric S-type acceleration and deceleration control method, the time of an acceleration section is the same as that of an acceleration reduction section, the time of the acceleration section is the same as that of a deceleration reduction section, but the time of two variable acceleration time periods and the time of two variable deceleration time periods are not the same, the time of a uniform acceleration section is different from that of the uniform deceleration section, and the time unknowns are 5;

the variation relation between the acceleration rate and the time of each stage

Although the time values of the jerk section and the deceleration section are not equal, the derivation according to the following formula can show that the time value changes of the two stages are related to the change ratio before and after the jerk;

the constraint condition of the trajectory planning is not considered, the initial speed and the final speed are zero, and the acceleration is integrated with the time to obtain a speed value;

analyzing and calculating by adopting an area cut-and-fill method commonly used in engineering;

can solve t_j1、t_j2In the context of (a) or (b),

k is the jerk, i.e. the jerk value in the jerk phase is compared with the jerk value in the deceleration phase;

when t is_v1When the value is equal to 0, the starting and ending speed is zero according to the starting and ending speed constraint condition, and t is known_j1、t_j2Can obtain t by area cut-and-fill method_a1、t_a2A change relationship; the calculation formula is as follows:

the five time unknowns are converted into three time quantum;

time solving algorithm for each stage of asymmetric S-type acceleration and deceleration control

According to the time optimal principle, firstly, the limiting conditions are not considered, only an acceleration section, a deceleration section, an acceleration section and a deceleration section are arranged during trajectory planning, and other speed change sections and acceleration changes do not exist; therefore, order t_a1＝0，t_a2＝0， t_v1When it is equal to 0, t is calculated_j1，t_j2A value; according to the asymmetric S-type acceleration and deceleration control method, a binary cubic equation set related to speed and displacement can be obtained, and then t can be obtained_j1The calculation formula is as follows:

considering the speed, the acceleration limit requires that the maximum acceleration occurs at the end of the jerk segment due to the maximum speed, and the maximum acceleration occurs at the end of the jerk segment, and must also be given to t_j1The calculation formula for correction is as follows:

t obtained by the above correction of the time point_j1、t_j2Can meet the limiting requirements of speed and acceleration, but can not meet the requirement of running displacement generally, a uniform acceleration section, a uniform deceleration section and a uniform speed section exist generally, and t is ordered according to the time optimal principle _v10, like t_j1、t_j2Solving process, can obtain t_a1、t_a2Value, and then t is obtained_v1A value; obtaining five unknown time quantum related to asymmetric S-type acceleration and deceleration control to obtain velocity curve of whole motion track, further constructing relation between velocity and parameter u value by Taylor expansion, and obtaining variable u value and double NThe URBS curve theory completes interpolation operation, accurate description and speed control of the unmanned vehicle pose are achieved, and efficient and accurate operation of a spline interpolation formula is achieved.

The invention provides a simple and easily understood asymmetrical S-shaped acceleration and deceleration control method for controlling the running speed, the absolute value of the acceleration is different before and after, the acceleration value at the beginning of the acceleration stage is larger, and the acceleration value at the stopping of the deceleration stage is smaller, so that the problem that the running safety and comfort are influenced by the overlarge vibration of the vehicle when the vehicle stops can be effectively avoided, and the speed, safety and comfort problems are considered.

As a preferred technical measure it is provided that,

for the situation that the speed of the initial position and the speed of the final position of the unmanned automobile are not zero; the S-type acceleration and deceleration method is an S-type acceleration and deceleration control method with non-zero starting speed and ending speed:

the S-type acceleration and deceleration control method with the speed not equal to zero at the beginning and the end is divided into seven time periods in three change stages, wherein the three change stages are a uniform acceleration stage, a uniform acceleration stage and a uniform speed stage, and the uniform acceleration stage comprises four time periods: the acceleration section, the deceleration section, the acceleration and deceleration section and the deceleration section are arranged in the same plane; the jerk phase includes two time periods: an acceleration section and a deceleration section; the ramp phase includes only one time segment: a uniform velocity section; wherein the time of the acceleration section is the same as that of the deceleration section, and the relation between the starting speed and the ending speed and the acceleration and deceleration time and the relation between the displacement and the acceleration and deceleration time can be obtained as follows, wherein v_sIndicates the initial velocity, v_eRepresents the terminal velocity;

planning the track with the speed from beginning to end not being zero, wherein the processing idea is similar to that of the track planning with the speed from beginning to end being zero, time values of all stages are required to be solved, but the three-order track planning with the speed from beginning to end not being zero involves five time quanta, and only two equation sets exist, so that the solving difficulty is greater than that of the track planning with the speed from beginning to end being zero; the S-type acceleration and deceleration control method with non-zero starting and ending speeds involves more than ten time planning modes, and is particularly complicated if the S-type acceleration and deceleration control method is considered according to the classification mode; therefore, a more reasonable and simpler solving method is needed;

considering according to the driving situation, the driving task is hoped to be completed with the fastest driving efficiency, meanwhile, the precision requirement of the automobile needs to be met, and since the S-shaped acceleration and deceleration control method with the speed not equal to zero at the beginning and the end relates to the equation which is a quintuple cubic equation set and only has two equations, the direct solution according to the conventional equation set cannot be realized, the driving efficiency problem can be considered firstly, then the driving safety problem is considered, and the time planning scheme meeting the requirement, namely the time optimal algorithm, is obtained step by step;

uniform acceleration segment time solving method based on Newton iteration method

Considering the problem of driving efficiency firstly, the whole driving stage only has a uniform acceleration stage, and the uniform acceleration stage and the uniform speed stage do not exist, so that the driving mode has the highest driving efficiency; therefore, order t_a1＝0，t_a2＝0，t_v1When it is equal to 0, t is calculated_i1，t_j2A value;

substituting into the above equation results in a system of two-dimensional cubic equations for velocity and displacement:

the convergence value of the binary cubic equation system can be obtained by Newton iteration, and the equation system is firstly constructed into two functional formulas f (t)_j1，t_j2)、g(t_j1，t_j2)，：

The first partial derivative of the set of equations 3- (39) is calculated and formed into matrix A:

in the formula:

f(t_j1，t_j2) With respect to t_j1The first partial derivative of (a);

f(t_j1，t_j2) With respect to t_j2The first partial derivative of (a);

g(t_j1，t_j2) With respect to t_j1The first partial derivative of (a);

g(t_j1，t_j2) With respect to t_j2The first partial derivative of (a);

obtaining an inverse matrix A _ inv of the partial derivative matrix A, selecting a reasonable initial iteration value for Newton iteration, and planning the initial speed v in most cases_sAnd a termination velocity v_eAre located at the end of the speed curve, and in order to avoid the error exceeding, the allowable speed of the point is small and the speed difference is not large, therefore, t is known_j1And t_j2The final values of (a) are generally not very different; when selecting the initial value of iteration, let t_j1＝t_j2＝t_jObtaining t_jCalculating the formula:

let tj be t_j1The iteration initial value of (a), the iteration precision is set, and t_jIs assigned to t_j10，t_j20A handle t_j10、t_j20Respectively substituting the function expression and the matrix A to calculate a first iteration value t_j11，t_j21The calculation formula is as follows:

calculating an iteration precision esp using the calculated iteration value:

substituting the precision requirement into an iterative calculation formula and an iterative precision calculation formula to carry out iterative calculation, solving a convergence value meeting the requirement, and substituting the convergence value into subsequent calculation;

however, in general, such a driving method cannot meet the driving safety requirement, and may exceed the acceleration capability provided by the vehicle; the convergence value obtained by adopting the iterative method is only approximate to the optimal solution, and the obtained convergence value may have a large error with the actual value in consideration of the limits of speed and displacement, if the final convergence value is a set of negative number solutions, according to the actual situation, the final convergence value is not required, but a set of positive number solutions closest to the convergence solution is used as the final calculation result; thereby obtaining t meeting the requirement_j1、t_j2A value;

considering the speed and acceleration limits, when the deceleration section of the maximum speed ends, comparing the maximum speed with the allowed acceleration time of the maximum speed limit, and taking the smaller value, the calculation formula is as follows:

considering the requirement of acceleration limitation again, when the maximum acceleration appears at the end of the jerk segment, the maximum acceleration is compared with the allowed acceleration time of the maximum limited acceleration, and the smaller value is taken, and the calculation formula is as follows:

at this time t_j2And t_j1Correlation, t_j1Change then corresponding t_j2Recalculation is also required, and the calculation formula is as follows:

judging the variation time allowed by the maximum limit acceleration and the maximum acceleration required by the maximum acceleration reached in the acceleration and deceleration stage, wherein the time value can only be a smaller value, and the calculation formula is as follows:

this results in t meeting the maximum speed limit and maximum acceleration limit requirements_j1、t_j2A value of (d);

the values obtained by Newton's iteration method and iterative correction may have errors in some cases, and need to be corrected again, and the second correction is performed by using the speed starting and ending speed and the displacement limit requirement to obtain t meeting the requirement_j1、t_j2A value; to simultaneously take account of the first iteration correction result, t_j1、t_j2Can only be reduced but can not be increased; the velocity error v _ esp is calculated as follows:

setting a certain precision value ESP, if v _ ESP > ESP, t_j1Reducing a sampling period Ts, substituting into a speed error formula to judge the error, if the requirement is not met, continuously reducing until the requirement is met, and if t is met_j1When the time is reduced to zero, the requirement can not be met, and the circulation is quitted; if v _ ESP < -ESP, t is indicated_j2Reducing a sampling period Ts, substituting a speed error formula for error judgment, and repeating the process until the requirement is met;

and then t is determined by the displacement limit condition_j1，t_j2And performing iterative correction calculation, wherein the formula is as follows:

wherein the displacement error formula s _ esp is the difference between the actual displacement and the planned displacement value; if s _ esp > 0, indicating that the vehicle is travelling according to the calculated time, the actual displacement value will be less than the desired displacement value, which does not correspond to the actual requirement, and t should be reduced again_j1Since it involves a change in displacement, it is related to t_j1、t_j2Are all correlated, so t is corresponding_j2Also, t is available_j2The calculation formula of (a) is as follows:

substituting the displacement error formula into a displacement error formula to calculate an error value for comparison, repeating the process, and stopping the process when a value meeting the requirement is obtained; when s _ esp < 0 indicates that the planned displacement is smaller than the expected displacement, which is consistent with the actual situation, indicates that other speed change sections exist, and ends the iterative correction process;

the process can calculate the stage time t satisfying the uniform acceleration_j1、t_j2A value of (d);

time planning solution based on Newton iteration uniform acceleration time period

T obtained by the above correction of the time point_j1、t_j2Can meet the limit requirements of speed and acceleration, but generally can not meet the requirement of running displacement, and can also have a uniform acceleration section, a uniform deceleration section and a uniform speed section, and the time optimal principle is considered to enable t _v10, the system of equations for the start and end velocities and displacement limits can be found:

from the above theory, t is obtained by Newton's iteration method_a1、t_a2Then using iterative correction theory to correct the initial value of (1), and finally obtaining the uniform acceleration stage time t meeting the requirement_a1、t_a2A value of (d);

time value of uniform velocity phase

For the S-shaped acceleration and deceleration trajectory planning algorithm, seven speed change sections in three stages are involved, five time unknowns exist, and t can be obtained by utilizing the algorithm according to the principle of highest efficiency_j1、t_j2、t_a1、t_a2The value of time parameter is equal to 0 in the absence phase, now only one unknown quantity t remains_v1A handle t_j1、t_j2、t_a1、t_a2Substituting the equation, the equation becoming that for t_v1A linear equation of unity, directly able to solve for t_v1A value; the calculation formula can be obtained as follows:

t can be calculated by the above formula_v1；

The five time unknowns related to the algorithm are all obtained, and a speed change curve can be planned according to the planned time quantum, and then corresponding interpolation calculation is carried out.

The S-type acceleration and deceleration control relates to solving a multivariate high-order equation or inequality, and the final solution obtained by solving is required to be a nonnegative number. Therefore, the quinary nonlinear equation set involved in S-shaped acceleration and deceleration control is decomposed into two binary nonlinear equation sets and a unary linear equation according to a time optimal principle and limiting conditions of speed and acceleration, the acceleration and deceleration time meeting the requirement is gradually solved by adopting a Newton iteration method and an iterative correction mode, and a speed curve is further solved to accurately control the vehicle. The invention can effectively avoid the complex and tedious operation of the existing S-type acceleration and deceleration control, simplifies the classification of the acceleration and deceleration modes, and provides the acceleration and deceleration control method which has high precision and is easy to understand.

As a preferred technical measure it is provided that,

the spline interpolation algorithm is to determine the coordinates of the unmanned vehicle in each sampling period and to generate a curve in a densification manner, the process relates to the parameter calculation problem and needs to consider the problems of driving safety, driving efficiency and vehicle performance, so that various requirements are required on spline parameters; according to the speed value in each planned sampling period, the corresponding next interpolation point parameter u value is calculated by utilizing B-spline theoretical knowledge, and the calculation formula is shown as follows;

ts is the sampling period, v (t) the velocity of the planned calculation, u_iFor the parameter value of the last interpolation point, X '(u) is the first derivative of the abscissa of the spline curve, Y' (u) is the first derivative of the ordinate of the spline curve, X "(u) is the second derivative of the abscissa of the spline curve, and Y" (u) is the second derivative of the ordinate of the spline curve.

As a preferred technical measure it is provided that,

the situation that the speed of the curve of the planned speed is suddenly changed can occur, and the curve of the planned speed needs to be smoothly processed; in order to ensure the smoothness of the speed, the acceleration and deceleration capacity must be considered, and the smoothness is carried out according to the speed mutation degree and the acceleration change condition;

when the three-order S-type acceleration and deceleration control is adopted, the acceleration is the minimum control unit, and the acceleration changes continuously, so the magnitude of the acceleration in the same period must be considered in the smoothing treatment; because the speed sudden change in a certain sampling period needs to consider the acceleration and deceleration capacity in the front period and the rear period, and meanwhile, because the acceleration is not fixed and constant, the judgment needs to be carried out cycle by cycle, and the condition that the speed needs to be reduced in advance for a plurality of cycles is obtained;

firstly, considering forward from a sudden change speed point, calculating the reduction speed which needs several cycles to meet the bow height error limiting requirement; a cyclic accumulation mode can be adopted to judge that the limit requirement can be met in a plurality of periods; the calculation formula is shown as follows;

acC＝a_i-1+a_i-2+...a_i-n

wherein: n is the deceleration which needs to be carried out n periods in advance; a is_iThe acceleration value of the ith sampling period; acc is an acceleration value accumulated value of n sampling periods; v. of_iThe speed value of the ith sampling period; j is the maximum value of jerk.

As a preferred technical measure it is provided that,

describing a driving route by using a spline curve; considering the calculation precision and the calculation amount, the NURBS curve weight factor takes 1, the cubic B spline is taken, the basis function is solved by adopting a fixed node vector mode, and the node vector is as follows:

u＝{-3，-2，-1，0，1，2，3}

the effective calculation formula of u ═ 0, 1 is intercepted to obtain four basis functions of uniform cubic B splines:

the spline curve varies with the parameter u according to the relation:

wherein: p₁(u) denotes the l-th spline, p_1+iDenotes the ith control point, X, of the curve of the 1 st segment_l(u) denotes a control pointVector formed by the abscissa, Y₁(u) vector, x, consisting of control point ordinates_l+iRepresents the abscissa,. y, of the ith control point of segment 1_1+iThe ordinate of the ith control point of the 1 st segment is represented;

because the track needing to be traveled is an irregular path, the speed and direction changes of the unmanned automobile cannot be arbitrary; in a sampling period, the speed and the direction of the unmanned vehicle are generally fixed, and if the track has sudden change, the unmanned vehicle cannot drive an expected path under the condition of overlarge speed; therefore, the speed of the unmanned automobile is limited at some positions, such as position turning points, so that the driving safety is ensured; according to the relation between the running path and the actual motion track of the unmanned automobile, introducing the concept of bow height error, and analyzing the precision errors of the running path and the unmanned automobile; the corresponding speeds at the turning points of the coordinates are small, if the driving safety is ensured, the feeding speeds at the points must not be larger than the given speed limits, but if the speed of the whole path runs according to the minimum values of the speeds, the driving efficiency is influenced; therefore, the efficiency and the precision are comprehensively considered, and the whole path can be processed in a segmented mode according to the inflection points of the positions corresponding to the speed minimum values; analyzing and calculating each section by using an S-shaped path planning theory, and then performing interpolation planning by using a cubic spline theory:

the formula of curvature of the arcuate path is as follows:

the radius of curvature calculation formula is as follows:

knowing the control points and the sampling period, generating position coordinates between the control points by using a uniform spline theory according to the bow height error requirement, and solving the curvature radius so as to solve a speed limit curve; the velocity calculation formula under the limitation of the bow height error can be obtained:

wherein: e is the bow height error, r is the radius of the spline curve, T_sIs the sampling period.

Because the non-uniform rational B-spline has large calculation amount and is complicated in calculation, in some spline interpolation algorithms, in order to reduce calculation, a uniform cubic B-spline algorithm with unchanged node vectors is adopted. The method can greatly reduce the calculated amount, has little influence on the precision of the spline, and is widely applied to the spline interpolation calculation.

Compared with the prior art, the invention has the following beneficial effects:

the invention adopts a spline interpolation algorithm, can accurately describe a complex route, directly calculates coordinate values of a plurality of intermediate points by obtaining discrete points on the route, such as turning coordinates and obstacle coordinates, and then controls the motion of the unmanned automobile according to the interpolated coordinate values. The S-type acceleration and deceleration method is adopted to control the running speed of the unmanned automobile, and S-type acceleration and deceleration control has the advantages of smooth and uniform speed curve, stable movement, no jump and the like, and is comfortable. The S-type acceleration and deceleration control can better realize the smooth start and stop and speed switching of the vehicle, can realize no sudden change of acceleration, and can better reduce the motion impact and the shock of the vehicle, thereby effectively improving the comfort of the automatic driving vehicle.

The spline interpolation and S-type acceleration and deceleration control are mainly used in the aspect of numerical control machine tool machining, the machining precision and the machining efficiency of numerical control machining can be effectively improved, the bias of the prior art is broken through, the spline interpolation and S-type acceleration and deceleration control are applied to the automatic driving control technology, the design is ingenious, the scheme is detailed, the method is practical and feasible, and the applicability and the comfort of an automatic driving vehicle can be greatly improved.

Drawings

FIG. 1 changes in body pose;

fig. 2 the body tangent vector varies.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

On the contrary, the invention covers any alternatives, modifications, equivalents, and alternatives falling within the spirit and scope of the invention as defined by the appended claims. Furthermore, in the following detailed description of the present invention, certain specific details are set forth in order to provide a better understanding of the present invention. It will be apparent to one skilled in the art that the present invention may be practiced without these specific details.

An automatic driving control method comprising the steps of:

firstly, acquiring a path map through a manned automobile;

secondly, acquiring peripheral images of the unmanned vehicle through a camera, and comparing the peripheral images with corresponding positions of a path map to judge whether obstacles exist or not, wherein the obstacles comprise vehicles, people or road piles;

thirdly, measuring the distance between the unmanned automobile and the obstacle by using the laser radar

Fourthly, generating a driving route according to the route map and the distance information of the obstacles;

fifthly, planning a running route according to a spline interpolation method;

sixthly, performing speed control on the unmanned automobile by adopting an S-shaped acceleration and deceleration method;

and seventhly, the unmanned automobile runs according to the planned route and the preset speed.

The invention adopts a spline interpolation algorithm, can accurately describe a complex route, directly calculates coordinate values of a plurality of intermediate points by obtaining discrete points on the route, such as turning coordinates and obstacle coordinates, and then controls the motion of the unmanned automobile according to the interpolated coordinate values. The S-type acceleration and deceleration method is adopted to control the running speed of the unmanned automobile, and S-type acceleration and deceleration control has the advantages of smooth and uniform speed curve, stable movement, no jump and the like, and is comfortable. The S-type acceleration and deceleration control can better realize the smooth start and stop and speed switching of the vehicle, can realize no sudden change of acceleration, and can better reduce the motion impact and the shock of the vehicle, thereby effectively improving the comfort of the automatic driving vehicle.

The spline interpolation and S-type acceleration and deceleration control are mainly used in the aspect of numerical control machine tool machining, the machining precision and the machining efficiency of numerical control machining can be effectively improved, the bias of the prior art is broken through, the spline interpolation and S-type acceleration and deceleration control are applied to the automatic driving control technology, the design is ingenious, the scheme is detailed, the method is practical and feasible, and the applicability and the comfort of an automatic driving vehicle can be greatly improved.

For a complex route to be driven, firstly, the route needs to be accurately described, discrete points on the route are easy to obtain, if a conventional parameter curve is directly used for fitting according to the discrete points, the number of times of the parameter curve is high, and the driving route is difficult to accurately describe; therefore, the driving route is described by a spline curve;

the driving route is adjusted at any time according to the movement of the obstacle, and a spline interpolation control point is formed by adding starting and ending position points according to the position and the path characteristics of the obstacle; generating a passing point according to the control point and the necessary safety distance; generating a speed boundary control curve according to the moving speed and the path characteristics of the barrier and the legal speed limit;

the NURBS curve in the spline interpolation method consists of four factors, namely a control point, a node vector, a weight factor and a basis function; obtaining a node vector through a correlation formula according to the control point, further obtaining a basis function through the node vector, and generally giving a weight factor or calculating according to an offset; performing deformation calculation on the basis of a Hadeli-Giardian method according to a known control point to obtain a proper node vector; and calculating a weight factor according to the required offset, obtaining the weight factor and the node vector to obtain a basis function, and further constructing a NURBS curve by using the control points, the basis function and the weight factor.

Calculating a node vector:

m + p +1 control points are known

p is the constructed basis function number; calculating a proper node vector value u according to the distance between the control points;

firstly, the distance d between the control points is calculated_jThe formula is as follows:

wherein x_jIndicating a control point P_jThe abscissa of (a) and (b) the abscissa of (a)_jIndicating a control point P_jThe longitudinal axis coordinate of (a);

and (3) deforming by a recursive formula of a Hadeli-Giardian method, wherein the formula after deformation is as follows:

wherein u is_iExpressing the ith node, and further obtaining a node vector U recursion formula as follows:

the node vector is solved in the form: u ═ 0, 0, 0, U_p，u_p+1，...u_m+p1，m+1，m+1，m+1]；

NURBS expression:

the basis functions are obtained by using the node vectors, the NURBS basis functions have a plurality of construction forms, and the commonly used construction form is given by the following recursion formula, and the basis function N is determined by the following recursion mode_i，p(U) P NURBS basis functions corresponding to the node vector U:

p≥2

wherein i is the serial number of the basis function, a node vector U is given, and the required basis function can be deduced according to the recursion formula; further, the i-th NURBS curve P can be obtained_i(u) the expression is:

u∈[u_i+p，u_i+p+1]，i＝1，2，3...m+1

the NURBS curve is a special parameter curve that can be expressed simply and efficiently for complex curves. The NURBS curve is a non-uniform rational B-spline, and can accurately represent a quadratic curve by introducing a weight factor and a denominator, namely, can accurately describe various complex paths.

As a preferred technical measure it is provided that,

effect of weighting factors on NURBS curves:

the distance between the NURBS curve and the control point can be adjusted by the weight factor, the larger the value of the weight factor is, the closer the NURBS curve is to the corresponding control point, the corresponding weight factor can be calculated by utilizing the offset according to a relational expression of the weight factor and the offset, the weight factor is substituted into the NURBS curve expression, the adjustment of the NURBS curve is further realized, the weight factor is determined according to the empirical value or the length and the width of the vehicle body, and the initial value of the weight factor is 1;

respectively calculating when u is equal to u_k，w_kWhen 1, the interpolation point p₁When u is equal to u_k，w_kWhen 0, the interpolation point p₀；

Further, the distance S can be obtained_k，K_kThe calculation formula is as follows:

wherein P is_KIs the kth control point, k being 0, 1, 2

The weight factor w can be obtained_kAnd offset d_kThe calculation formula is as follows:

as a preferred technical measure it is provided that,

the unmanned automobile moves from an initial position to a target position, the description of the posture change vector of the automobile body is related, an automobile body shaft is simplified into a straight rod, and the change posture of the straight rod in the space is described;

given discrete points through which the unmanned automobile needs to pass and the corresponding change angle of an automobile body shaft, describing the pose change from a starting point to a target point; according to the discrete points which need to be passed by the unmanned automobile and the space change angle of the automobile body shaft, the length of the automobile body shaft is generally fixed and unchanged; according to the effective length and the corresponding spatial variation angle, the passing points of the vehicle body shaft can be obtained by utilizing the vehicle body passing points, then two corresponding groups of control points are back-calculated through the NURBS theory, interpolation calculation is carried out according to the control points by utilizing the NURBS theory to generate a series of points, the points are substituted into a spline interpolation formula to carry out back solution calculation, and the position information at the front end and the rear end is obtained;

given that the discrete point through which the unmanned vehicle needs to pass is T_i＝(xt_i，yt_i，zt_i) And a set of varying angles of the body axis (α)_i，β_i) The length of the vehicle body is fixed and is set to be l, and the discrete passing point coordinate H of the corresponding vehicle body axis vector_i＝(xh_i，yh_i，zh_i) The calculation formula of (2) is as follows:

xh_i＝xt_i+lcosα_icosβ_i

yh_i＝yt_i+lcosα_isinβ_i

zh_i＝zt_i+lsinα_i

through the steps, the vehicle body passing point and the other group of passing points of the vehicle body axis vector can be obtained, so that the change of the vehicle body axis can be clearly described, and the corresponding control point can be obtained by knowing the passing point according to the NURSB curve generation theory;

in order to accurately describe the change of the unmanned automobile, a tangent vector of the track change of the unmanned automobile also needs to be known, the NURBS curve can be regarded as a parameter equation about u, the tangent vector of an automobile body interpolation track can be obtained through calculation of a derivation theory of the parameter equation, namely, the derivation is carried out on u by a basis function, and then the tangent vector is substituted into a related NURBS formula;

the calculation formula is as follows:

according to the control characteristics of the pose of the unmanned automobile, the invention provides that the pose change is described through two groups of related NURBS curves, the change relation between the curve offset and the weight factor is given, so that the NURBS curves can be flexibly changed, the tangent vector of the spline curve is simultaneously solved to facilitate the inverse solution calculation, the distance between the automobile and the obstacle is controlled through the weight factor, and the safety and perfect obstacle avoidance during the automobile driving are ensured to the maximum extent.

An embodiment of the speed control of the present invention: for the situation that the speeds of the initial position and the final position of the unmanned automobile are both zero; the S-type acceleration and deceleration method is an asymmetric S-type acceleration and deceleration control method:

the conventional point-to-point track planning adopts a symmetrical S-shaped acceleration and deceleration control method, and the running process is divided into seven sections, namely an acceleration section, a uniform acceleration section, an acceleration and deceleration section, a uniform velocity section, an acceleration and deceleration section, a uniform deceleration section and a deceleration and deceleration section; the time values of the four time periods of the acceleration section, the deceleration section, the acceleration section and the deceleration section are the same, and the time values of the uniform acceleration section and the uniform deceleration section are the same; considering the influence of the acceleration on the mechanism motion, the invention adopts an asymmetric S-shaped acceleration and deceleration control method, the acceleration section adopts larger acceleration in the beginning stage, and the deceleration section adopts smaller acceleration in the ending stage, so that the problems that the vehicle body stops and the precision and the efficiency cannot be considered because of larger vibration caused by overlarge acceleration are solved;

for a section of complete symmetrical S-shaped acceleration and deceleration control method, according to a time optimal algorithm, the trajectory planning of the method actually solves three time quantities: jerk time t_jTime of uniform acceleration t_aTime of uniform velocity t_vThe asymmetric S-type acceleration and deceleration control method is adopted, the track planning is more complicated than that of the symmetric S-type acceleration and deceleration control method, the time of an acceleration section is the same as that of an acceleration reduction section, the time of the acceleration section is the same as that of a deceleration reduction section, but the time of two variable acceleration time periods and the time of two variable deceleration time periods are not the same, the time of a uniform acceleration section is different from that of the uniform deceleration section, and the time unknowns are 5;

the variation relation between the acceleration rate and the time of each stage

Although the time values of the jerk section and the deceleration section are not equal, the derivation according to the following formula can show that the time value changes of the two stages are related to the change ratio before and after the jerk;

the constraint condition of the trajectory planning is not considered, the initial speed and the final speed are zero, and the acceleration is integrated with the time to obtain a speed value;

analyzing and calculating by adopting an area cut-and-fill method commonly used in engineering;

can solve t_j1、t_j2In the context of (a) or (b),

k is the jerk, i.e. the jerk value in the jerk phase is compared with the jerk value in the deceleration phase;

when t is_v1When the value is equal to 0, the starting and ending speed is zero according to the starting and ending speed constraint condition, and t is known_j1、t_j2Can obtain t by area cut-and-fill method_a1、t_a2A change relationship; the calculation formula is as follows:

the five time unknowns are converted into three time quantum;

time solving algorithm for each stage of asymmetric S-type acceleration and deceleration control

According to the time optimal principle, firstly, the limiting conditions are not considered, only an acceleration section, a deceleration section, an acceleration section and a deceleration section are arranged during trajectory planning, and other speed change sections and acceleration changes do not exist; therefore, order t_a1＝0，t_a2＝0， t_v1When it is equal to 0, t is calculated_j1，t_j2A value; according to the asymmetric S-type acceleration and deceleration control method, a binary cubic equation set related to speed and displacement can be obtained, and then t can be obtained_j1The calculation formula is as follows:

considering the speed, the acceleration limit requires that the maximum acceleration occurs at the end of the jerk segment due to the maximum speed, and the maximum acceleration occurs at the end of the jerk segment, and must also be given to t_j1The calculation formula for correction is as follows:

t obtained by the above correction of the time point_j1、t_j2Can meet the limiting requirements of speed and acceleration, but can not meet the requirement of running displacement generally, a uniform acceleration section, a uniform deceleration section and a uniform speed section exist generally, and t is ordered according to the time optimal principle _v10, like t_j1、t_j2Solving process, can obtain t_a1、t_a2The value of l is further determined_v1A value; five unknown time quantum related to the asymmetric S-shaped acceleration and deceleration control is solved, a speed curve of the whole motion track can be obtained, a relational expression of speed and a parameter u value is further constructed by using a Taylor expansion, interpolation operation is completed according to the changed u value and the double-NURBS curve theory, accurate description and speed control of the unmanned vehicle pose are achieved, and efficient and accurate operation of a spline interpolation formula is achieved.

The invention provides a simple and easily understood asymmetrical S-shaped acceleration and deceleration control method for controlling the running speed, the absolute value of the acceleration is different before and after, the acceleration value at the beginning of the acceleration stage is larger, and the acceleration value at the stopping of the deceleration stage is smaller, so that the problem that the running safety and comfort are influenced by the overlarge vibration of the vehicle when the vehicle stops can be effectively avoided, and the speed, safety and comfort problems are considered.

Another embodiment of the present invention for speed control: for the situation that the speed of the initial position and the speed of the final position of the unmanned automobile are not zero; the S-type acceleration and deceleration method is an S-type acceleration and deceleration control method with non-zero starting speed and ending speed:

the S-type acceleration and deceleration control method with the speed not equal to zero at the beginning and the end is divided into seven time periods in three change stages, wherein the three change stages are a uniform acceleration stage, a uniform acceleration stage and a uniform speed stage, and the uniform acceleration stage comprises four time periods: the acceleration section, the deceleration section, the acceleration and deceleration section and the deceleration section are arranged in the same plane; the jerk phase includes two time periods: an acceleration section and a deceleration section; the ramp phase includes only one time segment: a uniform velocity section; wherein the time of the acceleration section is the same as that of the deceleration section, and the relation between the starting speed and the ending speed and the acceleration and deceleration time and the relation between the displacement and the acceleration and deceleration time can be obtained as follows, wherein v_sIndicates the initial velocity, v_eRepresents the terminal velocity;

planning the track with the speed from beginning to end not being zero, wherein the processing idea is similar to that of the track planning with the speed from beginning to end being zero, time values of all stages are required to be solved, but the three-order track planning with the speed from beginning to end not being zero involves five time quanta, and only two equation sets exist, so that the solving difficulty is greater than that of the track planning with the speed from beginning to end being zero; the S-type acceleration and deceleration control method with non-zero starting and ending speeds involves more than ten time planning modes, and is particularly complicated if the S-type acceleration and deceleration control method is considered according to the classification mode; therefore, a more reasonable and simpler solving method is needed;

considering according to the driving situation, the driving task is hoped to be completed with the fastest driving efficiency, meanwhile, the precision requirement of the automobile needs to be met, and since the S-shaped acceleration and deceleration control method with the speed not equal to zero at the beginning and the end relates to the equation which is a quintuple cubic equation set and only has two equations, the direct solution according to the conventional equation set cannot be realized, the driving efficiency problem can be considered firstly, then the driving safety problem is considered, and the time planning scheme meeting the requirement, namely the time optimal algorithm, is obtained step by step;

uniform acceleration segment time solving method based on Newton iteration method

Considering the problem of driving efficiency firstly, the whole driving stage only has a uniform acceleration stage, and the uniform acceleration stage and the uniform speed stage do not exist, so that the driving mode has the highest driving efficiency; therefore, order t_a1＝0，t_a2＝0，t_v1When it is equal to 0, t is calculated_j1，t_j2A value;

substituting into the above equation results in a system of two-dimensional cubic equations for velocity and displacement:

the convergence value of the binary cubic equation system can be obtained by Newton iteration, and the equation system is firstly constructed into two functional formulas f (t)_j1，t_j2)、g(t_j1，t_j2)，：

The first partial derivative of the set of equations 3- (39) is calculated and formed into matrix A:

in the formula:

f(t_j1，t_j2) With respect to t_j1The first partial derivative of (a);

f(t_j1，t_j2) With respect to t_j2The first partial derivative of (a);

g(t_j1，t_j2) With respect to t_j1The first partial derivative of (a);

g(t_j1，t_j2) With respect to t_j2The first partial derivative of (a);

obtaining an inverse matrix A _ inv of the partial derivative matrix A, selecting a reasonable initial iteration value for Newton iteration, and planning the initial speed v in most cases_sAnd a termination velocity v_eAre located at the end of the speed curve, and in order to avoid the error exceeding, the allowable speed of the point is small and the speed difference is not large, therefore, t is known_j1And t_j2The final values of (a) are generally not very different; when selecting the initial value of iteration, let t_j1＝t_j2＝t_jObtaining t_jCalculating the formula:

let tj be t_j1The iteration initial value of (a), the iteration precision is set, and t_jIs assigned to t_j10，t_j20A handle t_j10、t_j20Respectively substituting the function expression and the matrix A to calculate a first iteration value t_j11，t_j21The calculation formula is as follows:

calculating an iteration precision esp using the calculated iteration value:

substituting the precision requirement into an iterative calculation formula and an iterative precision calculation formula to carry out iterative calculation, solving a convergence value meeting the requirement, and substituting the convergence value into subsequent calculation;

however, in general, such a driving method cannot meet the driving safety requirement, and may exceed the acceleration capability provided by the vehicle; the convergence value obtained by adopting the iterative method is only approximate to the optimal solution, and the obtained convergence value may have a large error with the actual value in consideration of the limits of speed and displacement, if the final convergence value is a set of negative number solutions, according to the actual situation, the final convergence value is not required, but a set of positive number solutions closest to the convergence solution is used as the final calculation result; thereby obtaining t meeting the requirement_j1、t_j2A value;

considering the speed and acceleration limits, when the deceleration section of the maximum speed ends, comparing the maximum speed with the allowed acceleration time of the maximum speed limit, and taking the smaller value, the calculation formula is as follows:

considering the requirement of acceleration limitation again, when the maximum acceleration appears at the end of the jerk segment, the maximum acceleration is compared with the allowed acceleration time of the maximum limited acceleration, and the smaller value is taken, and the calculation formula is as follows:

at this time t_j2And t_j1Correlation, t_j1Change then corresponding t_j2Recalculation is also required, and the calculation formula is as follows:

judging the variation time allowed by the maximum limit acceleration and the maximum acceleration required by the maximum acceleration reached in the acceleration and deceleration stage, wherein the time value can only be a smaller value, and the calculation formula is as follows:

this results in t meeting the maximum speed limit and maximum acceleration limit requirements_j1、t_j2A value of (d);

the values obtained by Newton's iteration method and iterative correction may have errors in some cases, and need to be corrected again, and the second correction is performed by using the speed starting and ending speed and the displacement limit requirement to obtain t meeting the requirement_j1、t_j2A value; to simultaneously take account of the first iteration correction result, t_j1、t_j2Can only be reduced but can not be increased; the velocity error v _ esp is calculated as follows:

setting a certain precision value ESP, if v _ ESP> ESP, then t_j1Reducing a sampling period Ts, substituting into a speed error formula to judge the error, if the requirement is not met, continuously reducing until the requirement is met, and if t is met_j1When the time is reduced to zero, the requirement can not be met, and the circulation is quitted; if v _ ESP < -ESP, t is indicated_j2Reducing a sampling period Ts, substituting a speed error formula for error judgment, and repeating the process until the requirement is met;

and then t is determined by the displacement limit condition_j1，t_j2And performing iterative correction calculation, wherein the formula is as follows:

wherein the displacement error formula s _ esp is the difference between the actual displacement and the planned displacement value; if s _ esp > 0, indicating that the vehicle is travelling according to the calculated time, the actual displacement value will be less than the desired displacement value, which does not correspond to the actual requirement, and t should be reduced again_j1Since it involves a change in displacement, it is related to t_j1、t_j2Are all correlated, so t is corresponding_j2Also, t is available_j2The calculation formula of (a) is as follows:

substituting the displacement error formula into a displacement error formula to calculate an error value for comparison, repeating the process, and stopping the process when a value meeting the requirement is obtained; when s _ esp < 0 indicates that the planned displacement is smaller than the expected displacement, which is consistent with the actual situation, indicates that other speed change sections exist, and ends the iterative correction process;

the process can calculate the stage time t satisfying the uniform acceleration_j1、t_j2A value of (d);

time planning solution based on Newton iteration uniform acceleration time period

T obtained by the above correction of the time point_j1、t_j2Can meet the limit requirements of speed and acceleration, but generally can not meet the requirement of running displacement, and can also have a uniform acceleration section, a uniform deceleration section and a uniform speed section, and the time optimal principle is considered to enable t _v10, the system of equations for the start and end velocities and displacement limits can be found:

from the above theory, t is obtained by Newton's iteration method_a1、t_a2Then using iterative correction theory to correct the initial value of (1), and finally obtaining the uniform acceleration stage time t meeting the requirement_a1、t_a2A value of (d);

time value of uniform velocity phase

For the S-shaped acceleration and deceleration trajectory planning algorithm, seven speed change sections in three stages are involved, five time unknowns exist, and t can be obtained by utilizing the algorithm according to the principle of highest efficiency_j1、t_j2、t_a1、t_a2The value of time parameter is equal to 0 in the absence phase, now only one unknown quantity t remains_v1A handle t_j1、t_j2、t_a1、t_a2Substituting the equation, the equation becoming that for t_v1A linear equation of unity, directly able to solve for t_v1A value; the calculation formula can be obtained as follows:

t can be calculated by the above formula_v1；

The five time unknowns related to the algorithm are all obtained, and a speed change curve can be planned according to the planned time quantum, and then corresponding interpolation calculation is carried out.

The S-type acceleration and deceleration control relates to solving a multivariate high-order equation or inequality, and the final solution obtained by solving is required to be a nonnegative number. Therefore, the quinary nonlinear equation set involved in S-shaped acceleration and deceleration control is decomposed into two binary nonlinear equation sets and a unary linear equation according to a time optimal principle and limiting conditions of speed and acceleration, the acceleration and deceleration time meeting the requirement is gradually solved by adopting a Newton iteration method and an iterative correction mode, and a speed curve is further solved to accurately control the vehicle. The invention can effectively avoid the complex and tedious operation of the existing S-type acceleration and deceleration control, simplifies the classification of the acceleration and deceleration modes, and provides the acceleration and deceleration control method which has high precision and is easy to understand.

The spline interpolation algorithm is to determine the coordinates of the unmanned vehicle in each sampling period and to generate a curve in a densification manner, the process relates to the parameter calculation problem and needs to consider the problems of driving safety, driving efficiency and vehicle performance, so that various requirements are required on spline parameters; according to the speed value in each planned sampling period, the corresponding next interpolation point parameter u value is calculated by utilizing B-spline theoretical knowledge, and the calculation formula is shown as follows;

ts is the sampling period, v (t) the velocity of the planned calculation, u_iFor the parameter value of the last interpolation point, X '(u) is the first derivative of the abscissa of the spline curve, Y' (u) is the first derivative of the ordinate of the spline curve, X "(u) is the second derivative of the abscissa of the spline curve, and Y" (u) is the second derivative of the ordinate of the spline curve.

The situation that the speed of the curve of the planned speed is suddenly changed can occur, and the curve of the planned speed needs to be smoothly processed; in order to ensure the smoothness of the speed, the acceleration and deceleration capacity must be considered, and the smoothness is carried out according to the speed mutation degree and the acceleration change condition;

when the three-order S-type acceleration and deceleration control is adopted, the acceleration is the minimum control unit, and the acceleration changes continuously, so the magnitude of the acceleration in the same period must be considered in the smoothing treatment; because the speed sudden change in a certain sampling period needs to consider the acceleration and deceleration capacity in the front period and the rear period, and meanwhile, because the acceleration is not fixed and constant, the judgment needs to be carried out cycle by cycle, and the condition that the speed needs to be reduced in advance for a plurality of cycles is obtained;

firstly, considering forward from a sudden change speed point, calculating the reduction speed which needs several cycles to meet the bow height error limiting requirement; a cyclic accumulation mode can be adopted to judge that the limit requirement can be met in a plurality of periods; the calculation formula is shown as follows;

acC＝a_i-1+a_i-2+...a_i-n

wherein: n is the deceleration which needs to be carried out n periods in advance; a is_iThe acceleration value of the ith sampling period; acc is an acceleration value accumulated value of n sampling periods; v. of_iThe speed value of the ith sampling period; j is the maximum value of jerk.

As a preferred technical measure it is provided that,

describing a driving route by using a spline curve; considering the calculation precision and the calculation amount, the NURBS curve weight factor takes 1, the cubic B spline is taken, the basis function is solved by adopting a fixed node vector mode, and the node vector is as follows:

u＝{-3，-2，-1，0，1，2，3}

the effective calculation formula of intercepting U ═ 0, 1 can obtain four basis functions of even cubic B splines:

the spline curve varies with the parameter u according to the relation:

wherein: p₁(u) denotes the 1 st spline, p_1+iDenotes the ith control point, X, of the curve of the 1 st segment₁(u) vector Y consisting of the abscissa of the control points₁(u) vector, x, consisting of control point ordinates_l+iAbscissa, y, representing the ith control point of segment 1_1+iThe ordinate of the ith control point of the 1 st segment is represented;

because the track needing to be traveled is an irregular path, the speed and direction changes of the unmanned automobile cannot be arbitrary; in a sampling period, the speed and the direction of the unmanned vehicle are generally fixed, and if the track has sudden change, the unmanned vehicle cannot drive an expected path under the condition of overlarge speed; therefore, the speed of the unmanned automobile is limited at some positions, such as position turning points, so that the driving safety is ensured; according to the relation between the running path and the actual motion track of the unmanned automobile, introducing the concept of bow height error, and analyzing the precision errors of the running path and the unmanned automobile; the corresponding speeds at the turning points of the coordinates are small, if the driving safety is ensured, the feeding speeds at the points must not be larger than the given speed limits, but if the speed of the whole path runs according to the minimum values of the speeds, the driving efficiency is influenced; therefore, the efficiency and the precision are comprehensively considered, and the whole path can be processed in a segmented mode according to the inflection points of the positions corresponding to the speed minimum values; analyzing and calculating each section by using an S-shaped path planning theory, and then performing interpolation planning by using a cubic spline theory:

the formula of curvature of the arcuate path is as follows:

the radius of curvature calculation formula is as follows:

knowing the control points and the sampling period, generating position coordinates between the control points by using a uniform spline theory according to the bow height error requirement, and solving the curvature radius so as to solve a speed limit curve; the velocity calculation formula under the limitation of the bow height error can be obtained:

wherein: e is the bow height error, r is the spline curve radius, and Ts is the sampling period.

Because the non-uniform rational B-spline has large calculation amount and is complicated in calculation, in some spline interpolation algorithms, in order to reduce calculation, a uniform cubic B-spline algorithm with unchanged node vectors is adopted. The method can greatly reduce the calculated amount, has little influence on the precision of the spline, and is widely applied to the spline interpolation calculation.

The invention discloses an embodiment of controlling the pose and track of a double-NURBS curve unmanned automobile based on asymmetric S-type acceleration and deceleration control, which comprises the following steps:

the feasibility of the invention and the effect on vehicle operating speed were verified using a MATLAB GUI for simulations.

In order to further verify the feasibility of the invention, the vehicle is modeled in three dimensions in SolidWorks, and then the model is led into a MATLAB simulink module for simulation calculation. A set of discrete passing points of the unmanned vehicle and the body axis space angle change value are given, and the values are shown in table 1:

TABLE 1 vehicle body pose variation discrete data

The displacement is in kilometers and the angle is in degrees 100.

According to the invention, the control model carries out programming simulation in a matlab simulink module, the data in the table 1 are input into a motion control program, and simultaneously, the limiting conditions are given as follows: maximum limit acceleration j is 10000m/s³Maximum limit acceleration a_m＝1000m/s²Maximum limit speed v_m800m/s, with a period T_s1ms, jerk variableThe chemical conversion rate K was 4. Simulation experiments prove that the invention can control the unmanned automobile to efficiently and accurately move. The motion track of the unmanned vehicle is shown in fig. 1, and the partial tangent vector is shown in fig. 2.

The invention provides a double-NURBS curve unmanned vehicle pose and track control research based on asymmetric S-type acceleration and deceleration control, which utilizes double-NURBS curves to describe vehicle body axis vector changes, calculates tangent vectors of the unmanned vehicle track changes, realizes accurate control of vehicle body axis change poses, and calculates corresponding weight factors according to offset to realize accurate adjustment of vehicle body axis vectors. The asymmetrical S-shaped acceleration and deceleration control method can effectively improve the running speed and reduce the vibration problem caused by overlarge acceleration, and simulation experiments prove that the method is feasible.

Claims (10)

1. An automatic driving control method characterized by comprising the steps of:

firstly, acquiring a path map through a manned automobile;

secondly, acquiring peripheral images of the unmanned vehicle through a camera, and comparing the peripheral images with corresponding positions of a path map to judge whether obstacles exist or not, wherein the obstacles comprise vehicles, people or road piles;

thirdly, measuring the distance between the unmanned automobile and the obstacle by using the laser radar

Fourthly, generating a driving route according to the route map and the distance information of the obstacles;

fifthly, planning a running route according to a spline interpolation method;

sixthly, performing speed control on the unmanned automobile by adopting an S-shaped acceleration and deceleration method;

and seventhly, the unmanned automobile runs according to the planned route and the preset speed.

2. The automatic driving control method of claim 1,

the driving route is adjusted at any time according to the movement of the obstacle, and a spline interpolation control point is formed by adding starting and ending position points according to the position and the path characteristics of the obstacle; generating a passing point according to the control point and the necessary safety distance; generating a speed boundary control curve according to the moving speed and the path characteristics of the barrier and the legal speed limit;

the NURBS curve in the spline interpolation method consists of four factors, namely a control point, a node vector, a weight factor and a basis function; obtaining a node vector through a correlation formula according to the control point, further obtaining a basis function through the node vector, and generally giving a weight factor or calculating according to an offset; carrying out deformation calculation on the basis of a Hadeli-Giardian method according to a known control point to obtain a proper node vector; and calculating a weight factor according to the required offset, obtaining the weight factor and the node vector to obtain a basis function, and further constructing a NURBS curve by using the control points, the basis function and the weight factor.

3. The automatic driving control method according to claim 2,

calculating a node vector:

m + p +1 control points are known

p is the constructed basis function number; calculating a proper node vector value u according to the distance between the control points;

firstly, the distance d between the control points is calculated_jThe formula is as follows:

wherein x_jIndicating a control point P_jThe abscissa of (a) and (b) the abscissa of (a)_jIndicating a control point P_jThe longitudinal axis coordinate of (a);

and (3) deforming by a recursive formula of a Hadeli-Giardian method, wherein the formula after deformation is as follows:

wherein u is_iExpressing the ith node, and further obtaining a node vector U recursion formula as follows:

the node vector is solved in the form: u ═ 0, 0, 0, U_p，u_p+1，...u_m+p1，m+1，m+1，m+1]；

NURBS expression:

the basis functions are obtained by using the node vectors, the NURBS basis functions have a plurality of construction forms, and the commonly used construction form is given by the following recursion formula, and the basis function N is determined by the following recursion mode_i，p(U) P NURBS basis functions corresponding to the node vector U:

wherein i is the serial number of the basis function, a node vector U is given, and the required basis function can be deduced according to the recursion formula; further, the i-th NURBS curve P can be obtained_i(u) the expression is:

4. the automatic driving control method according to claim 3,

the distance between the NURBS curve and the control point can be adjusted by the weight factor, the larger the value of the weight factor is, the closer the NURBS curve is to the corresponding control point, the corresponding weight factor can be calculated by utilizing the offset according to a relational expression of the weight factor and the offset, the weight factor is substituted into the NURBS curve expression, the adjustment of the NURBS curve is further realized, the weight factor is determined according to the empirical value or the length and the width of the vehicle body, and the initial value of the weight factor is 1;

respectively calculating when u is equal to u_k，w_kWhen 1, the interpolation point p₁When u is equal to u_k，w_kWhen 0, the interpolation point p₀；

Further, the distance S can be obtained_k，K_kThe calculation formula is as follows:

wherein P is_KIs the kth control point, k being 0, 1, 2

The weight factor w can be obtained_kAnd offset d_kThe calculation formula is as follows:

5. the automatic driving control method according to claim 4,

the unmanned automobile moves from an initial position to a target position, the description of the posture change vector of the automobile body is related, an automobile body shaft is simplified into a straight rod, and the change posture of the straight rod in the space is described;

given discrete points through which the unmanned automobile needs to pass and the corresponding change angle of an automobile body shaft, describing the pose change from a starting point to a target point; according to the discrete points which need to be passed by the unmanned automobile and the space change angle of the automobile body shaft, the length of the automobile body shaft is generally fixed and unchanged; according to the effective length and the corresponding spatial variation angle, the passing points of the vehicle body shaft can be obtained by utilizing the vehicle body passing points, then two corresponding groups of control points are back-calculated through the NURBS theory, interpolation calculation is carried out according to the control points by utilizing the NURBS theory to generate a series of points, the points are substituted into a spline interpolation formula to carry out back solution calculation, and the position information at the front end and the rear end is obtained;

given that the discrete point through which the unmanned vehicle needs to pass is T_i＝(xt_i，yt_i，zt_i) And a set of varying angles of the body axis (α)_i，β_i) The length of the vehicle body is fixed and is set to be l, and the discrete passing point coordinate H of the corresponding vehicle body axis vector_i＝(xh_i，yh_i，zh_i) The calculation formula of (2) is as follows:

xh_i＝xt_i+l cosα_icosβ_i

yh_i＝yt_i+l cosα_isinβ_i

zh_i＝zt_i+l sinα_i

through the steps, the vehicle body passing point and the other group of passing points of the vehicle body axis vector can be obtained, so that the change of the vehicle body axis can be clearly described, and the corresponding control point can be obtained by knowing the passing point according to the NURSB curve generation theory;

calculating by a derivation theory of a parameter equation to obtain a tangent vector of the interpolation track of the vehicle body, namely, deriving u by a basis function, and then substituting into a related NURBS formula;

the calculation formula is as follows:

6. the automatic driving control method according to any one of claims 1 to 5,

for the situation that the speeds of the initial position and the final position of the unmanned automobile are both zero; the S-type acceleration and deceleration method is an asymmetric S-type acceleration and deceleration control method:

the conventional point-to-point track planning adopts a symmetrical S-shaped acceleration and deceleration control method, and the running process is divided into seven sections, namely an acceleration section, a uniform acceleration section, an acceleration and deceleration section, a uniform velocity section, an acceleration and deceleration section, a uniform deceleration section and a deceleration and deceleration section; the time values of the four time periods of the acceleration section, the deceleration section, the acceleration section and the deceleration section are the same, and the time values of the uniform acceleration section and the uniform deceleration section are the same; considering the influence of the acceleration on the mechanism motion, the invention adopts an asymmetric S-shaped acceleration and deceleration control method, wherein the acceleration section adopts larger acceleration in the beginning stage, and the deceleration section adopts smaller acceleration in the ending stage;

for a section of complete symmetrical S-shaped acceleration and deceleration control method, according to a time optimal algorithm, the trajectory planning of the method actually solves three time quantities: jerk time t_jTime of uniform acceleration t_aTime of uniform velocity t_vThe asymmetric S-type acceleration and deceleration control method is adopted, the trajectory planning is more complicated than that of the symmetric S-type acceleration and deceleration control method, the time of an acceleration section and a deceleration section is the same, the time of the acceleration section and the deceleration section is the same, but the time of two variable acceleration time periods and the time of two variable deceleration time periods are not the same, the time of a uniform acceleration section and the time of a uniform deceleration section are different, and the time unknown quantity is changed into 5;

the variation relation between the acceleration rate and the time of each stage

Although the time values of the jerk section and the deceleration section are not equal, the derivation according to the following formula can show that the time value change of the two stages is related to the change ratio before and after the jerk;

the constraint condition of the trajectory planning is not considered, the initial speed and the final speed are zero, and the acceleration is integrated with the time to obtain a speed value;

analyzing and calculating by adopting an area cut-and-fill method commonly used in engineering;

can solve t_j1、t_j2In the context of (a) or (b),

k is the jerk, i.e. the jerk value in the jerk phase is compared with the jerk value in the deceleration phase;

when t is_v1When the value is equal to 0, the starting and ending speed is zero according to the starting and ending speed constraint condition, and t is known_j1、t_j2Can obtain t by area cut-and-fill method_a1、t_a2A change relationship; the calculation formula is as follows:

the five time unknowns are converted into three time quantum;

time solving algorithm for each stage of asymmetric S-type acceleration and deceleration control

According to the time optimal principle, firstly, the limiting conditions are not considered, only an acceleration section, a deceleration section, an acceleration and deceleration section and a deceleration and deceleration section are arranged during trajectory planning, and other speed change sections and acceleration changes do not exist; therefore, order t_a1＝0，t_a2＝0，t_v1When it is equal to 0, t is calculated_j1，t_j2A value; according to the asymmetric S-type acceleration and deceleration control method, a binary cubic equation set related to speed and displacement can be obtained, and then t can be obtained_j1The calculation formula is as follows:

considering the speed, the acceleration limit requires that the maximum acceleration occurs at the end of the jerk segment due to the maximum speed, and the maximum acceleration occurs at the end of the jerk segment, and must also be applied to t_j1The calculation formula for correction is as follows:

t obtained by the above correction of the time point_j1、t_j2Can satisfy the speed and acceleration limitationThe requirement, but generally can not satisfy the requirement of the driving displacement, generally there will be a uniform acceleration section, a uniform deceleration section and a uniform velocity section, and t is ordered according to the time optimal principle_v10, like t_j1、t_j2Solving process, can obtain t_a1、t_a2Value, and then t is obtained_v1A value; five unknown time quantum related to asymmetric S-type acceleration and deceleration control is obtained, a speed curve of the whole motion track can be obtained, a relational expression of the speed and a parameter u value is constructed by using a Taylor expansion formula, and interpolation operation is completed according to a changed u value and the double NURBS curve theory.

7. The automatic driving control method according to any one of claims 1 to 5,

for the situation that the speed of the initial position and the speed of the final position of the unmanned automobile are not zero; the S-type acceleration and deceleration method is an S-type acceleration and deceleration control method with non-zero starting speed and ending speed:

the S-type acceleration and deceleration control method with the speed not equal to zero at the beginning and the end is divided into seven time periods in three change stages, wherein the three change stages are a uniform acceleration stage, a uniform acceleration stage and a uniform speed stage, and the uniform acceleration stage comprises four time periods: the acceleration section, the deceleration section, the acceleration and deceleration section and the deceleration section are arranged in the same plane; the jerk phase includes two time periods: an acceleration section and a deceleration section; the ramp phase includes only one time segment: a uniform velocity section; wherein the time of the acceleration section is the same as that of the deceleration section, and the relationship between the starting speed and the ending speed and the acceleration and deceleration time and the relationship between the displacement and the acceleration and deceleration time can be obtained as follows, wherein v_sIndicates the initial velocity, v_eRepresents the terminal velocity;

uniform acceleration segment time solving method based on Newton iteration method

Considering the problem of driving efficiency firstly, the whole driving stage only has a uniform acceleration stage, and the uniform acceleration stage and the uniform speed stage do not exist, so that the driving mode has the highest driving efficiency; therefore, order t_a1＝0，t_a2＝0，t_v1When it is equal to 0, t is calculated_j1，t_j2A value;

substituting into the above equation results in a system of two-dimensional cubic equations for velocity and displacement:

the convergence value of the binary cubic equation system can be obtained by Newton iteration, and the equation system is firstly constructed into two functional formulas f (t)_j1，t_j2)、g(t_j1，t_j2)，：

The first partial derivative of the set of equations 3- (39) is calculated and formed into matrix A:

in the formula:

f(t_j1，t_j2) With respect to t_j1The first partial derivative of (a);

f(t_j1，t_j2) With respect to t_j2The first partial derivative of (a);

g(t_j1，t_j2) With respect to t_j1The first partial derivative of (a);

g(t_j1，t_j2) With respect to t_j2The first partial derivative of (a);

obtaining an inverse matrix A _ inv of the partial derivative matrix A, selecting a reasonable initial iteration value for Newton iteration, and planning the initial speed v in most cases_sAnd a termination velocity v_eAll are located at the end point of the speed curve, and in order to avoid error overrun, the allowable speed of the point is generally small and the speed difference is not large, so t is known_j1And t_j2The final values of (a) are generally not very different; when selecting the initial value of iteration, let t_j1＝t_j2＝t_jObtaining t_jCalculating the formula:

setting the iteration precision by taking tj as an iteration initial value of tj1 and taking t as_jIs assigned to t_j10，t_j20A handle t_j10、t_j20Respectively substituting the function expression and the matrix A to calculate a first iteration value t_j11，t_j21The calculation formula is as follows:

calculating an iteration precision esp using the calculated iteration value:

substituting the precision requirement into an iterative calculation formula and an iterative precision calculation formula to carry out iterative calculation, solving a convergence value meeting the requirement, and substituting the convergence value into subsequent calculation;

considering the speed and acceleration limits, when the deceleration section of the maximum speed ends, comparing with the allowed acceleration time of the maximum limit speed, and taking the smaller value, the calculation formula is as follows:

considering the requirement of acceleration limitation again, when the maximum acceleration appears at the end of the jerk segment, the maximum acceleration is compared with the allowed acceleration time of the maximum limited acceleration, and the smaller value is taken, and the calculation formula is as follows:

at this time t_j2And t_j1Correlation, t_j1Change then corresponding t_j2Recalculation is also required, and the calculation formula is as follows:

judging the variation time allowed by the maximum limit acceleration and the maximum acceleration required by the maximum acceleration reached in the acceleration and deceleration stage, wherein the time value can only be a smaller value, and the calculation formula is as follows:

this results in t meeting the maximum speed limit and maximum acceleration limit requirements_j1、t_j2A value of (d);

the values obtained by Newton's iteration method and iterative correction may have errors in some cases, and need to be corrected again, and the second correction is performed by using the speed starting and ending speed and the displacement limit requirement to obtain t meeting the requirement_j1、t_j2A value; in order to simultaneously take account of the first iterative correctionAs a result, t_j1、t_j2Can only be reduced but can not be increased; the velocity error v _ esp is calculated as follows:

setting a certain precision value ESP, if v _ ESP > ESP, t_j1Reducing a sampling period Ts, substituting into a speed error formula to judge the error, if the requirement is not met, continuously reducing until the requirement is met, and if t is met_j1When the time is reduced to zero, the requirement can not be met, and the circulation is quitted; if v _ ESP < -ESP, t is indicated_j2Reducing a sampling period Ts, substituting a speed error formula for error judgment, and repeating the process until the requirement is met;

and then t is determined by the displacement limit condition_j1，t_j2And performing iterative correction calculation, wherein the formula is as follows:

wherein the displacement error formula s _ esp is the difference between the actual displacement and the planned displacement value; if s _ esp > 0, indicating that driving is being carried out according to the calculated time, the actual displacement value will be less than the desired displacement value, which does not correspond to the actual requirement, and therefore t should be reduced again_j1Since it involves a change in displacement, it is related to t_j1、t_j2Are all correlated, so t is corresponding_j2Also, t is available_j2The calculation formula of (a) is as follows:

substituting the displacement error formula into a displacement error formula to calculate an error value for comparison, repeating the process, and stopping the process when a value meeting the requirement is obtained; when s _ esp < 0 indicates that the planned displacement is smaller than the expected displacement, which is consistent with the actual situation, indicates that other speed change sections exist, and ends the iterative correction process;

time planning solution based on Newton iteration uniform acceleration time period

Let t consider the time optimal principle_v1＝0，

A system of equations for the start and end velocities and displacement limits can be derived:

from the above theory, t is obtained by Newton's iteration method_a1、t_a2Then using iterative correction theory to correct the initial value of (1), and finally obtaining the uniform acceleration stage time t meeting the requirement_a1、t_a2A value of (d);

time value of uniform velocity phase

For the S-shaped acceleration and deceleration trajectory planning algorithm, seven speed change sections in three stages are involved, five time unknowns are provided, and t can be obtained by utilizing the algorithm according to the principle of highest efficiency_j1、t_j2、t_a1、t_a2Value of (1), the value of the non-existent phase time parameter being equal to 0, now leaving only one unknown quantity t_v1A handle t_j1、t_j2、t_a1、t_a2Substituting the equation, the equation becoming that for t_v1A linear equation of unity, directly able to solve for t_v1A value; the calculation formula can be obtained as follows:

t can be calculated by the above formula_v1；

And (4) solving five time unknowns related to the algorithm, planning a speed change curve according to the planned time quantum, and further performing corresponding interpolation calculation.

8. The automatic driving control method of claim 7,

the spline interpolation algorithm is to determine the coordinates of the unmanned vehicle in each sampling period, to generate a curve in a densification manner, and to calculate the corresponding next interpolation point parameter u value by using B spline theoretical knowledge according to the speed value in each planned sampling period, wherein the calculation formula is shown as follows;

ts is the sampling period, v (t) the velocity of the planned calculation, u_iFor the parameter value of the last interpolation point, X '(u) is the first derivative of the abscissa of the spline curve, Y' (u) is the first derivative of the ordinate of the spline curve, X '(u) is the second derivative of the abscissa of the spline curve, and Y' (u) is the second derivative of the ordinate of the spline curve.

9. The automatic driving control method of claim 8,

the situation that the speed of the curve of the planned speed is suddenly changed can occur, and the curve of the planned speed needs to be subjected to smoothing treatment; in order to ensure the smoothness of the speed, the acceleration and deceleration capacity must be considered, and the smoothness processing is carried out according to the speed mutation degree and the acceleration change condition;

when the three-order S-type acceleration and deceleration control is adopted, the acceleration is the minimum control unit, and the acceleration changes continuously, so the magnitude of the acceleration in the same period must be considered in the smoothing treatment; because the speed mutation in a certain sampling period needs to consider the acceleration and deceleration capacity in the front and back periods, and meanwhile, because the acceleration is not fixed and constant, the judgment needs to be carried out cycle by cycle, and the condition that the speed needs to be reduced in advance for a plurality of cycles is obtained;

firstly, considering forward from a sudden change speed point, calculating the reduction speed which needs several cycles to meet the limitation requirement of the bow height error; a cyclic accumulation mode can be adopted to judge that the limit requirement can be met in a plurality of periods; the calculation formula is shown below;

acc＝a_i-1+a_i-2+...a_i-n

wherein: n is the deceleration which needs to be carried out n periods in advance; a is_iThe acceleration value of the ith sampling period; acc is an acceleration value accumulated value of n sampling periods; v. of_iThe speed value of the ith sampling period; j is the maximum value of jerk.

10. The automatic driving control method according to any one of claims 3 to 5,

describing a driving route by using a spline curve; considering the calculation precision and the calculation amount, the NURBS curve weight factor takes 1, the cubic B spline is taken, the basis function is solved by adopting a fixed node vector mode, and the node vector is as follows:

u＝{-3，-2，-1，0，1，2，3}

the effective calculation formula of u ═ 0, 1 is intercepted to obtain four basis functions of uniform cubic B splines:

the spline curve varies with the parameter u according to the relation:

wherein: p_l(u) represents the I-th spline, p_l+iDenotes the ith control point, X, of the curve of the section I_l(u) vector consisting of abscissa of control point, Y_l(u) vector, x, consisting of control point ordinates_l+iAbscissa, y, representing the ith control point of the I-th segment_l+iThe ordinate of the ith control point of the I-th segment is shown.

Images