CN109765887A - A kind of automatic Pilot control method - Google Patents
A kind of automatic Pilot control method Download PDFInfo
- Publication number
- CN109765887A CN109765887A CN201811578082.2A CN201811578082A CN109765887A CN 109765887 A CN109765887 A CN 109765887A CN 201811578082 A CN201811578082 A CN 201811578082A CN 109765887 A CN109765887 A CN 109765887A
- Authority
- CN
- China
- Prior art keywords
- acceleration
- value
- speed
- time
- section
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 133
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 33
- 230000001133 acceleration Effects 0.000 claims description 232
- 238000004364 calculation method Methods 0.000 claims description 86
- 238000013439 planning Methods 0.000 claims description 61
- 238000006073 displacement reaction Methods 0.000 claims description 44
- 230000008859 change Effects 0.000 claims description 37
- 238000005070 sampling Methods 0.000 claims description 27
- 230000008569 process Effects 0.000 claims description 23
- 230000004888 barrier function Effects 0.000 claims description 17
- 238000009795 derivation Methods 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 12
- 239000000203 mixture Substances 0.000 claims description 11
- 230000036461 convulsion Effects 0.000 claims description 8
- 230000008901 benefit Effects 0.000 claims description 6
- 230000003014 reinforcing effect Effects 0.000 claims description 6
- 230000015572 biosynthetic process Effects 0.000 claims description 5
- 230000009467 reduction Effects 0.000 claims description 4
- 238000009825 accumulation Methods 0.000 claims description 3
- 238000000280 densification Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims 1
- 230000009514 concussion Effects 0.000 abstract description 4
- 230000035772 mutation Effects 0.000 abstract description 4
- 238000012545 processing Methods 0.000 description 15
- 238000005457 optimization Methods 0.000 description 12
- 238000005516 engineering process Methods 0.000 description 5
- 238000003754 machining Methods 0.000 description 3
- 241000208340 Araliaceae Species 0.000 description 2
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 description 2
- 235000003140 Panax quinquefolius Nutrition 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 2
- 238000005452 bending Methods 0.000 description 2
- 235000013399 edible fruits Nutrition 0.000 description 2
- 238000009472 formulation Methods 0.000 description 2
- 235000008434 ginseng Nutrition 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- 238000012886 linear function Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000003786 synthesis reaction Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000008676 import Effects 0.000 description 1
- 238000002156 mixing Methods 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Landscapes
- Feedback Control In General (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a kind of automatic Pilot control methods, belong to automatic Pilot technical field.Existing control method is feasible for simple road conditions, but for slightly complicated traffic environment, it is difficult to is described using consolidator frame, for traditional vehicle because the frequent switching of state and target will cause car speed jump problem, comfort is poor.The present invention uses spline interpolation algorithm, accurate description can be carried out to complicated route, by obtaining point such as turn coordinate and obstacle article coordinate discrete on route, directly calculate the coordinate value of several intermediate points, and then the coordinate value gone out according to interpolation, control the movement of pilotless automobile.S type feed speed control can preferably realize the smooth start and stop and speed switching of vehicle, can accomplish accelerationless mutation, can preferably reduce elliptical gear and the concussion of vehicle, to effectively improve the comfort of automatic driving vehicle.
Description
Technical field
The present invention relates to a kind of automatic Pilot control methods, belong to automatic Pilot technical field.
Background technique
Automatic driving technology includes video camera, radar sensor and airborne laser range finder to understand the friendship of surrounding
Logical situation, and realized by data center, autonomous driving vehicle is equivalent to the remote-control car or intelligent vapour of data center
Vehicle.Intelligent driving technology is particularly important component part in terms of intelligent transportation field, with the rapid promotion of computing capability
And the increasingly promotion of intelligent transportation automation demanding, research institution, more and more countries increasingly focus on incorporated business
This field of intelligent driving.
Presently disclosed pertinent literature, report mainly carry out the longitudinal direction of vehicle using the judgement of multiple modes and matching
Control, realization is relatively simple, is easy the frequent switching between multiple modes, and traditional vehicle longitudinal control uses cruise
The blending algorithm of CC, adaptive cruise ACC and automatic emergency brake AEB function, this algorithm are feasible for simple road conditions, but
For slightly complicated traffic environment, it is difficult to be described using consolidator frame, traditional vehicle is because of state and target
Frequent switching will cause car speed jump problem, comfort is poor.
Summary of the invention
In view of the drawbacks of the prior art, the purpose of the present invention is to provide one kind to cope with complicated traffic environment, to row
It sails route and carries out Unify legislation, reduce the good automatic Pilot control method of the comfort of pilotless automobile velocity jump.
To achieve the above object, the technical solution of the present invention is as follows:
A kind of automatic Pilot control method, comprising the following steps:
The first step acquires path map by manned automobile；
Second step obtains pilotless automobile side images by camera, and compares with the corresponding position of path map
Judge that whether there are obstacles, the barrier includes vehicle or people or road stake；
Third step measures the distance between pilotless automobile and barrier by laser radar
4th step generates travel route according to path map and obstacle distance information；
5th step carries out route planning to travel route according to spline interpolation method；
6th step carries out speed control to pilotless automobile using S type acceleration and deceleration method；
7th step, pilotless automobile are travelled according to the route of planning and according to predetermined speed.
The present invention uses spline interpolation algorithm, can carry out accurate description to complicated route, by obtain on route from
Scattered point such as turn coordinate and obstacle article coordinate, directly calculate the coordinate value of several intermediate points, and then according to interpolation
Coordinate value out controls the movement of pilotless automobile.And using the traveling of S type acceleration and deceleration method control pilotless automobile
Speed, S type feed speed control has that rate curve is smooth, uniform, movement is steady, without advantages such as jumps, comfortable good.S type plus-minus
Speed control can preferably realize the smooth start and stop and speed switching of vehicle, can accomplish accelerationless mutation, can preferably reduce
The elliptical gear of vehicle and concussion, to effectively improve the comfort of automatic driving vehicle.
Existing spline interpolation and S type feed speed control are primary for numerically-controlled machine tool processing aspect, can effectively improve
The machining accuracy and processing efficiency of numerical control processing, the present invention break prior art prejudice, and spline interpolation and S type are added and subtracted
Speed control is applied in automatic Pilot control technology, is skillfully constructed, and scheme is detailed, practical, can be greatly improved and be driven automatically
Sail the applicability and comfort of vehicle.
As optimization technique measure,
Complicated route is travelled, it is necessary first to accurate description be carried out to route, be easier to obtain on route
Discrete point, if be directly fitted using conventional parameter curve according to discrete point, the number of parameter curve can compare
It is high, it is also difficult to accurate description travel route；Therefore travel route is described using spline curve；
The travel route is adjusted at any time according to the movement of barrier, according to the position of barrier and path feature
And the control point of spline interpolation is constituted plus whole story location point；Passed through a little according to control point and the generation of necessary safe distance；
According to the movement speed of barrier, path feature and legal speed limit formation speed boundary Control curve；
Nurbs curve in spline interpolation method is by four control point, knot vector, weight factor, basic function factors compositions；
Knot vector can be obtained by correlation formula according to control point, and then basic function, weight factor can be obtained by knot vector
It is general to give or be calculated according to offset；Become on the basis of one Judd's method of Hadley according to known control point
Shape calculates suitable knot vector；Weight factor is calculated further according to the offset of needs, obtains weight factor and knot vector
Basic function can be obtained, and then constructs nurbs curve using control point, basic function and weight factor.
As optimization technique measure,
Calculate node vector:
Known m+p+1 control pointP is the basic function number that is constructed；It is remote according to the distance between control point
Closely, suitable knot vector value u is calculated；
First calculate distance d between control pointj, formula are as follows:
Wherein xjIndicate control point PjHorizontal axis coordinate, yjIndicate control point PjOrdinate of orthogonal axes；
Hadley-Judd's method recurrence formula deforms, formula after deformation are as follows:
Wherein uiIt indicates i-th of node, and then knot vector U recurrence formula can be obtained are as follows:
The knot vector form found out are as follows: U=[0,0,0, up, up+1... um+p1, m+1, m+1, m+1]；
NURBS expression formula:
Basic function is acquired using knot vector, there are many kinds of structural form, general common structural forms for NURBS basic function
It is to be provided by following recurrence formula, the basic function N determined with following recursion modeI, p(u) it is known as corresponding to knot vector U's
P NURBS basic function:
p≥2
Wherein i is the sequence number of basic function, gives knot vector U, can derive institute according to recurrence formula above
The basic function needed；And then i-th section of nurbs curve P can be obtainedi(u), expression formula are as follows:
u∈[ui+p, ui+p+1], i=1,2,3...m+1.
Nurbs curve is a kind of special parameter curve, it can, efficient representation succinct to complex curve.Nurbs curve is
Non-uniform rational B-spline, it can accurately indicate conic section by introducing weight factor and denominator, can be accurate
The path of various complexity is described.
As optimization technique measure,
Influence of the weight factor to nurbs curve:
Weight factor can adjust nurbs curve at a distance from control point, and weight factor value is bigger, and nurbs curve is got over
Close to corresponding control point, according to the relational expression of weight factor and offset, can be calculated using offset corresponding weight because
Son substitutes into nurbs curve expression formula, and then realizes adjustment to nurbs curve, weight factor rule of thumb value or according to
Length of wagon and width determine that weight factor initial value all takes 1；
It calculates separately and works as u=uk, wkWhen=1, interpolated point p1, work as u=uk, wkWhen=0, interpolated point p0；
And then distance measurements S can be obtainedk, Kk, calculation formula is as follows:
Wherein PKIt is k-th of control point, k=0,1,2 ..., m+p
Weight factor w can be acquiredkWith offset dkBetween relational expression, calculation formula are as follows:
As optimization technique measure,
Pilotless automobile moves to target position from initial position, is related to the description of vehicle body pose diverse vector,
Vehicle body axis is reduced to a straight-bar, describes a straight-bar in the variation posture in space；
The discrete point and vehicle body axis corresponding change angle that given pilotless automobile needs to pass through, to describe it from
Initial point changes to the pose between target point；The discrete point and vehicle body shaft space for needing to pass through according to pilotless automobile become
Change angle, the length of vehicle body axis is usually to immobilize；According to effective length and corresponding spatial variations angle, vehicle body is utilized
By point, can acquire vehicle body axis goes out corresponding two groups of control points by point, and then by NURBS theory inverse, according to control
Point carries out interpolation calculating using NURBS theory, generates series of points, substitutes into the anti-solution of progress in spline interpolation formula and calculates, obtains
Location information at front and back end；
The discrete point that given pilotless automobile needs to pass through is Ti=(xti, yti, zti) and vehicle body axis one group of change
Change angle (αi, βi), the length of vehicle body is fixed, and l is set as, and the discrete of that corresponding vehicle body axial vector passes through a coordinate Hi=
(xhi, yhi, zhi) calculation formula are as follows:
xhi=xti+lcosαicosβi
yhi=yti+lcosαisinβi
zhi=zti+lsinαi
Vehicle body can be found out through the above steps by point and another group of vehicle body axial vector by point, such vehicle body axis
Variation can clearly be depicted, it is known that corresponding control can be obtained according to NURSB curve generative theory by putting
Point；
For the variation of accurate description pilotless automobile, it is also necessary to know the tangential of the trail change of pilotless automobile
Amount, since nurbs curve can regard the parametric equation about u as, can be obtained by the derivation theoretical calculation of parametric equation
The tangent vector of vehicle body interpolation track, i.e. basic function carry out derivation to u, then substitute into related NURBS formula again；
Calculation formula is as follows:
The present invention proposes to carry out by two groups of relevant nurbs curves according to the control feature of pilotless automobile pose
The variation of pose is described, and gives the variation relation of curve offset amount and weight factor, flexibly to change
Nurbs curve, while finding out the tangent vector of spline curve and being calculated convenient for anti-solution, vehicle and barrier are controlled by weight factor
The distance between, utmostly guarantee safety and perfect avoidance when vehicle driving.
As optimization technique measure,
The speed of initial position and final position for pilotless automobile is zero situation；S type acceleration and deceleration method
For asymmetric S type Acceleration-deceleration Control Method:
Conventional point-to-point trajectory planning, using symmetrical S type Acceleration-deceleration Control Method, operational process is divided into seven sections,
It is acceleration section, uniform acceleration section respectively, subtracts acceleration section, uniform velocity section, acceleration-deceleration section, even deceleration segment, slows down
Spend section；Wherein acceleration section, the even acceleration identical as this four period time values of braking section are subtracted that subtract accelerating sections, accelerating and decelerating part
It is identical with even deceleration segment time value to spend section；Consider influence of the acceleration to mechanism kinematic, the present invention uses asymmetrical S
Type Acceleration-deceleration Control Method, incipient stage, that is, acceleration section use larger acceleration, ending phase, that is, deceleration segment use compared with
Small acceleration is eliminated because of the larger vibration that acceleration is excessive to be occurred when vehicle body being caused to stop, cannot be considered in terms of precision with
The problem of efficiency；
Symmetric form S type Acceleration-deceleration Control Method complete for one section, it is real to its trajectory planning according to time optimal algorithm
Three time quantums: acceleration time t are just to solve on borderj, uniform acceleration time ta, uniform velocity time tv, use is asymmetric
Type S type Acceleration-deceleration Control Method, what trajectory planning can be more complicated than symmetric form S type Acceleration-deceleration Control Method is more, acceleration section with
The time for subtracting accelerating sections is identical, and accelerating and decelerating part is identical as the time for subtracting braking section, but two fluctuating acceleration periods and two
It is no longer identical to become the deceleration segment time, while uniform acceleration section and even deceleration segment time are also different, time unknown quantity becomes 5
It is a；
The variation relation of acceleration change rate and each phases-time
Although acceleration section and to subtract deceleration segment time value unequal, can be learnt according to the subsequent derivation of equation,
The time value variation in two stages is related to changing ratio before and after acceleration；
The constraint condition of trajectory planning is not considered, and whole story speed is zero, and acceleration obtains velocity amplitude to time integral；
Reinforcing method is cut using area common in engineering, carries out analytical calculation；
T can be solvedj1、tj2Relationship,
K be acceleration change rate, i.e., acceleration stage jerk value with subtract deceleration stage jerk value phase
Than；
Work as tv1It is zero according to whole story velocity restraint condition, that is, whole story speed, it is known that t when=0j1、tj2Variation relation, utilize
Area, which cuts reinforcing method, can obtain ta1、ta2Variation relation；Calculation formula is as follows:
Such five time unknown quantitys switch to seek three time quantums again；
Asymmetric each phases-time derivation algorithm of S type feed speed control
According to time optimal principle, restrictive condition is not considered first, when trajectory planning only has acceleration section, subtracts acceleration
Section and subtracts deceleration segment at acceleration-deceleration section, other velocity variations sections, acceleration change is not present；Therefore Schilling ta1=0, ta2
=0, tv1=0, seek tj1, tj2Value；It is available about speed and the binary of displacement three according to asymmetric S type Acceleration-deceleration Control Method
Equation of n th order n group, and then t can be obtainedj1, calculation formula is as follows:
Consider speed, acceleration limitation requires to occur at the end of subtracting acceleration section due to maximum speed, acceleration maximum value
At the end of appearing in acceleration section, it is necessary to tj1It is as follows to be modified calculation formula:
By the t found out above to the amendment at time pointj1、tj2It is able to satisfy the limitation requirement of velocity and acceleration, but general
It is unable to satisfy the requirement of traveling displacement, can generally have even accelerating sections, even braking section and at the uniform velocity section, according to time optimal principle
Enable tv1=0, similar tj1、tj2Solution procedure can acquire ta1、ta2Value, and then find out tv1Value；Find out asymmetric S type acceleration and deceleration
Five unknown time quantums being related to are made, the rate curve of entire motion profile can be obtained, and then construct using Taylor expansion
The relational expression of speed and parameter u value, according to the u value of variation and it is above-mentioned about double nurbs curve theories complete interpolation operation,
It realizes the accurate description and speed control to pilotless automobile pose, and then realizes spline interpolation formula efficiently, accurately
Running.
The present invention provides a kind of succinct, intelligible asymmetrical S type feed speed control side for the control of travel speed
The absolute value front and back of method, acceleration is different, and the jerk value for starting boost phase is larger, stops adding for decelerating phase
Velocity amplitude is smaller, and it is excessive because of vibrating when stopping effectively to avoid vehicle, and influences driving safety, comfortable problem, takes into account speed
Degree and safety, comfort problem.
As optimization technique measure,
The situation that the speed of initial position and final position for pilotless automobile is not zero；S type acceleration and deceleration method
The S type Acceleration-deceleration Control Method being not zero for whole story speed:
The S type Acceleration-deceleration Control Method that whole story speed is not zero is divided into seven periods of three kinds of changes phases, three variation ranks
Section is even acceleration stage, uniform acceleration stage, uniform velocity stage, and the even acceleration stage includes four periods: plus
Velocity shooting subtracts acceleration section, acceleration-deceleration section, subtracts deceleration segment；The uniform acceleration stage includes two periods: acceleration section,
Deceleration segment；The uniform velocity stage only includes a period: uniform velocity section；Wherein acceleration section and the time for subtracting accelerating sections
Identical, accelerating and decelerating part is identical as the time for subtracting braking section, can obtain the relationship of whole story speed and Acceleration and deceleration time and be displaced and add and subtract
The relationship of fast time is as follows, wherein vsIndicate beginning speed, veIndicate end speed；
The track being not zero to whole story speed is planned, similar with the trajectory planning roadmap that whole story speed is zero,
It needs to seek each phases-time value, but the three rank trajectory plannings that whole story speed is not zero are related to five time quantums, equation group is but
It is only two, therefore it is bigger than the trajectory planning that whole story speed is zero to solve difficulty；The S type acceleration and deceleration that whole story speed is not zero
It is several that the time planning mode that control method is related to has ten, just especially cumbersome if the classification mode according to it accounts for；
Therefore it needs to take a kind of more reasonable, easier method for solving；
Considered according to traveling situation, it is desirable to traveling task be completed with most fast running efficiency, while needing to meet automobile
Required precision is five yuan of cubic equation groups since the S type Acceleration-deceleration Control Method that whole story speed is not zero is related to equation, and
Only two equations, therefore can not be according to conventional equation group direct solution, therefore can first consider running efficiency problem, then
Consider driving safety problem, the i.e. time optimal algorithm of the time planning scheme gradually met the requirements；
The even acceleration section time based on Newton iteration method solves
First consider that running efficiency problem, entire travel phase only exist the even acceleration stage, uniform acceleration stage and even
Speed stage is all not present, this driving mode, running efficiency highest；Therefore Schilling ta1=0, ta2=0, tv1=0, seek ti1,
tj2Value；
The binary cubic equation group that can be obtained about speed and displacement is substituted into above formula:
Solving binary cubic equation group can be used the convergency value that Newton iteration finds out it, and equation group is configured to two first
Functional expression f (tj1, tj2)、g(tj1, tj2):
It seeks the first-order partial derivative of above-mentioned functional expression group 3- (39) and constitutes matrix A:
In formula:
f(tj1, tj2) about tj1First-order partial derivative；f(tj1, tj2) about tj2First-order partial derivative；
g(tj1, tj2) about tj1First-order partial derivative；g(tj1, tj2) about tj2First-order partial derivative；
The inverse matrix A_inv of partial derivative matrix A is acquired, Newton iteration needs to select reasonable iterative initial value, most of feelings
When carrying out trajectory planning under condition, initial velocity vsWith termination speed veIt is all located at rate curve endpoint, in order to avoid error transfinites,
The permissible velocity of the general point is smaller, and speed is not much different, therefore, it is known that tj1And tj2End value generally differ not
Greatly；When choosing iterative initial value, t is enabledj1=tj2=tj, t can be obtainedjCalculation formula:
Using tj as tj1Iterative initial value, be arranged iteration precision, tjIt is assigned to tj10, tj20, tj10、tj20Generation respectively
Enter above-mentioned functional expression and matrix A, and then calculates an iteration value tj11, tj21, calculation formula is as follows:
Iteration precision esp is calculated using calculated iterative value:
It according to required precision, substitutes into iterative calculation formula and iteration precision calculation formula is iterated calculating, find out full
The convergency value required enough substitutes into subsequent calculating；
But under normal circumstances, this driving mode is unable to satisfy driving safety requirement, can may also exceed vehicle institute energy
The acceleration capacity of offer；It is approximate optimal solution using the convergency value that iterative method is found out, and considers the limit of speed and displacement
System, the convergency value found out may there are large errors with actual value, if final convergency value is one group of negative solution, according to reality
Situation, what at this moment we needed is not just final convergency value, and one group of positive number solution for being closest to convergence solution is calculated as final
As a result；And then obtain satisfactory tj1、tj2Value；
Speed, acceleration limitation is considered, since maximum speed occurs at the end of subtracting acceleration section, with maximum limitation speed
The permitted acceleration time compares, and takes smaller value therein, and calculation formula is as follows:
It considers further that acceleration limitation requires, at the end of acceleration maximum value appears in acceleration section, adds with maximum limitation
The permitted acceleration time of speed compares, and takes smaller value therein, and calculation formula is as follows:
T at this timej2With tj1Correlation, tj1Change then corresponding tj2It is also required to recalculate, calculation formula is as follows:
When the acceleration maximum value for judging that the acceleration and deceleration stage reaches is also required to variation permitted with maximum limitation acceleration
Between, time value can only take smaller value, and calculation formula is as follows:
The t for meeting maximum limitation speed and maximum limitation acceleration requirement is obtained in this wayj1、tj2Value；
By Newton iteration method and the value of iterated revision found out, in some cases, it is understood that there may be error, need
It is corrected again, requires to correct for second of progress using fast whole story speed and displacement limitation, find out satisfactory tj1、tj2
Value；In order to combine first time iterated revision as a result, tj1、tj2Can only reduce not can increase；Velocity error v_esp is calculated such as
Shown in lower formula:
A certain accuracy value ESP is set, if v_esp > ESP, tj1It should reduce by a sampling period Ts, then substitute into speed
Degree error formula carries out error judgment to be continued to reduce if having not been met requirement, is reduced to always until meeting the requirements, such as
Fruit tj1Also it is unable to satisfy requirement when reducing to zero, then exits circulation；If v_esp <-ESP, shows tj2It should reduce by a sampling
Then cycle T s substitutes into velocity error formula and carries out error judgment, this process is repeated, until meeting the requirements；
And then by displacement restrictive condition to the t found outj1, tj2, it is iterated corrected Calculation, formula is as follows:
Wherein displacement error formula s_esp is actual displacement and the difference for planning shift value；If s_esp > 0 illustrates to press
It is travelled according to the calculated time, actual displacement value can be less than desired shift value, this is not inconsistent with actual requirement, therefore
T should be reduced againj1Value, due to what is involved is the variation of displacement, it and tj1、tj2It is all related, so corresponding tj2Also it wants
Change, t can be obtainedj2Calculation formula it is as follows:
Then it substitutes into displacement error formula and carries out calculating error amount and compare, this process repeatedly, when being met the requirements
Value stop this process；When s_esp < 0 illustrates that the displacement cooked up is less than desired shift value, this and actual conditions phase
Symbol illustrates to terminate iterated revision process there are also other velocity variations sections；
To sum up process, which can be found out, meets even acceleration phases-time tj1、tj2Value；
It is solved based on the even acceleration time section time planning of Newton iteration
By the t found out above to the amendment at time pointj1、tj2It is able to satisfy the limitation requirement of velocity and acceleration, but general
It is unable to satisfy the requirement of traveling displacement, may also have even accelerating sections, even braking section and at the uniform velocity section, considers time optimal principle
Enable tv1=0, it can be obtained about whole story speed and displacement limitation equation group:
According to above-mentioned theory, t is found out using Newton iteration methoda1、ta2Initial value, then utilize iterated revision theory carry out
Amendment, finally obtains the uniform acceleration phases-time t met the requirementsa1、ta2Value；
Uniform velocity phases-time value
Seven velocity variations sections of three phases are related to for S type acceleration and deceleration trajectory planning algorithm, there are five the time is unknown
Amount, can find out t using above-mentioned algorithm according to efficiency supreme principlej1、tj2、ta1、ta2Value, be not present phases-time ginseng
Numerical value is equal to 0, only remains next unknown quantity t nowv1, tj1、tj2、ta1、ta2Equation is substituted into, equation becomes about tv1One
First linear function can directly solve tv1Value；It is as follows calculation formula can be obtained:
T can be calculated by above-mentioned formulav1；
It has all been found out according to five time unknown quantitys that above-mentioned algorithm is related to, according to the time quantum of planning, can have been cooked up
Speed change curves, and then carry out corresponding interpolation calculating.
Since S type feed speed control is related to solving the polynary equation of higher degree or inequality, and solves and come most
Whole solution is nonnegative number, and traditional algorithm is to consider various plus-minus fast modes, and gradually judgement obtains time point, therefore results in meter
Calculation process is cumbersome, and duration is used in solution.It is proposed thus according to the restrictive condition of time optimal principle and speed, acceleration S type
Five yuan of Nonlinear System of Equations involved in feed speed control are decomposed into two binary nonlinear equation groups and a unitary is linear
Equation is gradually solved satisfactory Acceleration and deceleration time by the way of Newton iteration method and iterated revision, and then finds out speed
It writes music line, vehicle is accurately controlled.The present invention can effectively avoid complicated, the cumbersome fortune of existing S type feed speed control
It calculates, simplify acceleration and deceleration pattern classification, provide that a kind of precision is high, understandable Acceleration-deceleration Control Method.
As optimization technique measure,
Spline interpolation algorithm is the coordinate of determining each sampling period pilotless automobile, and the densification for carrying out curve is raw
At this process is related to parameters calculation, needs to consider that the performance of driving safety, running efficiency problem and vehicle is asked
Topic, so having many requirements to batten parameter；According to the velocity amplitude in each sampling period cooked up, managed using B-spline
Corresponding next interpolated points'parameter u value is found out by knowledge, calculation formula is as follows；
Ts is the sampling period, and v (t) plans the speed found out, uiFor the parameter value of a upper interpolated point, X ' (u) is that batten is bent
The first derivative of line abscissa, Y ' (u) are the first derivative of spline curve ordinate, and X " (u) is the two of spline curve abscissa
Order derivative, Y " (u) are the second dervative of spline curve ordinate.
As optimization technique measure,
To the curve for planning speed it is possible that the situation of velocity jump, at this moment needs to carry out light to planning rate curve
Sliding processing；To guarantee the smooth of speed, it is necessary to consider acceleration and deceleration ability, according to velocity jump degree, and consider that acceleration becomes
Change situation, carries out smooth treatment；
When using three rank S type feed speed controls, acceleration is minimum control unit, acceleration consecutive variations, so light
Acceleration magnitude in same period must be taken into consideration in sliding processing；Since velocity jump needs to consider front and back week in a certain sampling period
Acceleration and deceleration ability in phase needs period judgement one by one simultaneously because acceleration does not immobilize, obtain need it is several in advance
A period slows down；
It first has to consider forward from mutating speed point, calculates the reduction speed for needing several periods, it is high accidentally to be just able to satisfy bow
Difference limitation requires；The mode that circulative accumulation can be used judges that needing several periods to be just able to satisfy limitation requires；Calculation formula
As follows；
AcC=ai-1+ai-2+...ai-n
Wherein: n be need in advance n period slow down；aiFor the acceleration value in ith sample period；Acc is n
The acceleration value accumulated value in sampling period；viFor the velocity amplitude in ith sample period；J is the maximum value of acceleration.
As optimization technique measure,
Travel route is described using spline curve；Consider computational accuracy and calculation amount, nurbs curve weight because
Son takes 1, takes cubic B-spline, by the way of stationary nodes vector, finds out basic function, knot vector are as follows:
U={ -3, -2, -1,0,1,2,3 }
The effective calculation formula of interception u=[0,1] can obtain four basic functions of uniform cubic B-spline:
Spline curve with parameter u variation relation formula are as follows:
Wherein: P1(u) l sections of spline curve, p are indicated1+iIndicate i-th of control point of paragraph 1 curve, Xl(u) control is indicated
The vector of point abscissa composition, Y1(u) vector of control point ordinate composition, x are indicatedl+iIndicate i-th of control point of paragraph 1
Abscissa .y1+iIndicate the ordinate at i-th of control point of paragraph 1；
It is irregular path due to needing the track travelled, the speed and direction variation of pilotless automobile can not
It is arbitrary；Within a sampling period, the velocity magnitude of pilotless automobile and direction are generally fixed, if track have it is prominent
Become, in the case that the speed of pilotless automobile is excessive, can not just travel out expected path；This is just needed in some positions,
At the inflection point of position, the speed of pilotless automobile is limited, guarantees driving safety；According to driving path and pilotless automobile
Actual motion profile relationship introduces the concept for bending high error, carries out the trueness error of both analyses；At coordinate inflection point pair
The speed answered is smaller, if to guarantee driving safety, cannot centainly be greater than these given speed in the feed speed of these points
Limitation, but if entire path velocity just will affect running efficiency all in accordance with the operation of these speed minimums；Therefore synthesis is examined
Consider efficiency and precision problem, can the position inflection point according to corresponding to these speed minimums segment processing is carried out to entire path；
Analytical calculation, which is carried out, using S type path planning theory to every section then carries out interpolation planning with cubic spline theory again:
The curvature formulations of curved line are as follows:
Curvature radius calculation formula is as follows:
Known control point, sampling period are generated between control point according to high error requirements are bent using uniform spline theory
Position coordinates find out radius of curvature and then find out rate limitation curve；It can must bend the speed calculation formula under high error limitation:
Wherein: e is to bend high error, and r is spline curve radius, TsFor the sampling period.
Since non-uniform rational B-spline is computationally intensive, calculating is cumbersome, in some spline interpolation algorithms, in order to reduce meter
It calculates, using a kind of uniform cubic B-spline algorithm that knot vector is constant.It can greatly reduce calculation amount, and to the essence of batten
Degree influences very little, thus is widely used in spline interpolation calculating.
Compared with prior art, the invention has the following advantages:
The present invention uses spline interpolation algorithm, can carry out accurate description to complicated route, by obtain on route from
Scattered point such as turn coordinate and obstacle article coordinate, directly calculate the coordinate value of several intermediate points, and then according to interpolation
Coordinate value out controls the movement of pilotless automobile.And using the traveling of S type acceleration and deceleration method control pilotless automobile
Speed, S type feed speed control has that rate curve is smooth, uniform, movement is steady, without advantages such as jumps, comfortable good.S type plus-minus
Speed control can preferably realize the smooth start and stop and speed switching of vehicle, can accomplish accelerationless mutation, can preferably reduce
The elliptical gear of vehicle and concussion, to effectively improve the comfort of automatic driving vehicle.
Existing spline interpolation and S type feed speed control are primary for numerically-controlled machine tool processing aspect, can effectively improve
The machining accuracy and processing efficiency of numerical control processing, the present invention break prior art prejudice, and spline interpolation and S type are added and subtracted
Speed control is applied in automatic Pilot control technology, is skillfully constructed, and scheme is detailed, practical, can be greatly improved and be driven automatically
Sail the applicability and comfort of vehicle.
Detailed description of the invention
The variation of Fig. 1 vehicle body pose；
The variation of Fig. 2 vehicle body tangent vector.
Specific embodiment
In order to make the objectives, technical solutions, and advantages of the present invention clearer, with reference to the accompanying drawings and embodiments, right
The present invention is further elaborated.It should be appreciated that the specific embodiments described herein are merely illustrative of the present invention,
It is not intended to limit the present invention.
On the contrary, the present invention covers any substitution done on the essence and scope of the present invention being defined by the claims, repairs
Change, equivalent method and scheme.Further, in order to make the public have a better understanding the present invention, below to of the invention thin
It is detailed to describe some specific detail sections in section description.Part without these details for a person skilled in the art
The present invention can be also understood completely in description.
A kind of automatic Pilot control method, comprising the following steps:
The first step acquires path map by manned automobile；
Second step obtains pilotless automobile side images by camera, and compares with the corresponding position of path map
Judge that whether there are obstacles, the barrier includes vehicle or people or road stake；
Third step measures the distance between pilotless automobile and barrier by laser radar
4th step generates travel route according to path map and obstacle distance information；
5th step carries out route planning to travel route according to spline interpolation method；
6th step carries out speed control to pilotless automobile using S type acceleration and deceleration method；
7th step, pilotless automobile are travelled according to the route of planning and according to predetermined speed.
The present invention uses spline interpolation algorithm, can carry out accurate description to complicated route, by obtain on route from
Scattered point such as turn coordinate and obstacle article coordinate, directly calculate the coordinate value of several intermediate points, and then according to interpolation
Coordinate value out controls the movement of pilotless automobile.And using the traveling of S type acceleration and deceleration method control pilotless automobile
Speed, S type feed speed control has that rate curve is smooth, uniform, movement is steady, without advantages such as jumps, comfortable good.S type plus-minus
Speed control can preferably realize the smooth start and stop and speed switching of vehicle, can accomplish accelerationless mutation, can preferably reduce
The elliptical gear of vehicle and concussion, to effectively improve the comfort of automatic driving vehicle.
Existing spline interpolation and S type feed speed control are primary for numerically-controlled machine tool processing aspect, can effectively improve
The machining accuracy and processing efficiency of numerical control processing, the present invention break prior art prejudice, and spline interpolation and S type are added and subtracted
Speed control is applied in automatic Pilot control technology, is skillfully constructed, and scheme is detailed, practical, can be greatly improved and be driven automatically
Sail the applicability and comfort of vehicle.
Complicated route is travelled, it is necessary first to accurate description be carried out to route, be easier to obtain on route
Discrete point, if be directly fitted using conventional parameter curve according to discrete point, the number of parameter curve can compare
It is high, it is also difficult to accurate description travel route；Therefore travel route is described using spline curve；
The travel route is adjusted at any time according to the movement of barrier, according to the position of barrier and path feature
And the control point of spline interpolation is constituted plus whole story location point；Passed through a little according to control point and the generation of necessary safe distance；
According to the movement speed of barrier, path feature and legal speed limit formation speed boundary Control curve；
Nurbs curve in spline interpolation method is by four control point, knot vector, weight factor, basic function factors compositions；
Knot vector can be obtained by correlation formula according to control point, and then basic function, weight factor can be obtained by knot vector
It is general to give or be calculated according to offset；It is deformed on the basis of Hadley-Judd's method according to known control point
Calculate suitable knot vector；Weight factor is calculated further according to the offset of needs, obtains weight factor and knot vector just
Basic function can be obtained, and then constructs nurbs curve using control point, basic function and weight factor.
Calculate node vector:
Known m+p+1 control pointP is the basic function number that is constructed；It is remote according to the distance between control point
Closely, suitable knot vector value u is calculated；
First calculate distance d between control pointj, formula are as follows:
Wherein xjIndicate control point PjHorizontal axis coordinate, yjIndicate control point PjOrdinate of orthogonal axes；
Hadley-Judd's method recurrence formula deforms, formula after deformation are as follows:
Wherein uiIt indicates i-th of node, and then knot vector U recurrence formula can be obtained are as follows:
The knot vector form found out are as follows: U=[0,0,0, up, up+1... um+p1, m+1, m+1, m+1]；
NURBS expression formula:
Basic function is acquired using knot vector, there are many kinds of structural form, general common structural forms for NURBS basic function
It is to be provided by following recurrence formula, the basic function N determined with following recursion modeI, p(u) it is known as corresponding to knot vector U's
P NURBS basic function:
p≥2
Wherein i is the sequence number of basic function, gives knot vector U, can derive institute according to recurrence formula above
The basic function needed；And then i-th section of nurbs curve P can be obtainedi(u), expression formula are as follows:
u∈[ui+p, ui+p+1], i=1,2,3...m+1
Nurbs curve is a kind of special parameter curve, it can, efficient representation succinct to complex curve.Nurbs curve is
Non-uniform rational B-spline, it can accurately indicate conic section by introducing weight factor and denominator, can be accurate
The path of various complexity is described.
As optimization technique measure,
Influence of the weight factor to nurbs curve:
Weight factor can adjust nurbs curve at a distance from control point, and weight factor value is bigger, and nurbs curve is got over
Close to corresponding control point, according to the relational expression of weight factor and offset, can be calculated using offset corresponding weight because
Son substitutes into nurbs curve expression formula, and then realizes adjustment to nurbs curve, weight factor rule of thumb value or according to
Length of wagon and width determine that weight factor initial value all takes 1；
It calculates separately and works as u=uk, wkWhen=1, interpolated point p1, work as u=uk, wkWhen=0, interpolated point p0；
And then distance measurements S can be obtainedk, Kk, calculation formula is as follows:
Wherein PKIt is k-th of control point, k=0,1,2 ..., m+p
Weight factor w can be acquiredkWith offset dkBetween relational expression, calculation formula are as follows:
As optimization technique measure,
Pilotless automobile moves to target position from initial position, is related to the description of vehicle body pose diverse vector,
Vehicle body axis is reduced to a straight-bar, describes a straight-bar in the variation posture in space；
The discrete point and vehicle body axis corresponding change angle that given pilotless automobile needs to pass through, to describe it from
Initial point changes to the pose between target point；The discrete point and vehicle body shaft space for needing to pass through according to pilotless automobile become
Change angle, the length of vehicle body axis is usually to immobilize；According to effective length and corresponding spatial variations angle, vehicle body is utilized
By point, can acquire vehicle body axis goes out corresponding two groups of control points by point, and then by NURBS theory inverse, according to control
Point carries out interpolation calculating using NURBS theory, generates series of points, substitutes into the anti-solution of progress in spline interpolation formula and calculates, obtains
Location information at front and back end；
The discrete point that given pilotless automobile needs to pass through is Ti=(xti, yti, zti) and vehicle body axis one group of change
Change angle (αi, βi), the length of vehicle body is fixed, and l is set as, and the discrete of that corresponding vehicle body axial vector passes through a coordinate Hi=
(xhi, yhi, zhi) calculation formula are as follows:
xhi=xti+lcosαicosβi
yhi=yti+lcosαisinβi
zhi=zti+lsinαi
Vehicle body can be found out through the above steps by point and another group of vehicle body axial vector by point, such vehicle body axis
Variation can clearly be depicted, it is known that corresponding control can be obtained according to NURSB curve generative theory by putting
Point；
For the variation of accurate description pilotless automobile, it is also necessary to know the tangential of the trail change of pilotless automobile
Amount, since nurbs curve can regard the parametric equation about u as, can be obtained by the derivation theoretical calculation of parametric equation
The tangent vector of vehicle body interpolation track, i.e. basic function carry out derivation to u, then substitute into related NURBS formula again；
Calculation formula is as follows:
The present invention proposes to carry out by two groups of relevant nurbs curves according to the control feature of pilotless automobile pose
The variation of pose is described, and gives the variation relation of curve offset amount and weight factor, flexibly to change
Nurbs curve, while finding out the tangent vector of spline curve and being calculated convenient for anti-solution, vehicle and barrier are controlled by weight factor
The distance between, utmostly guarantee safety and perfect avoidance when vehicle driving.
A kind of speed control embodiment of the present invention: the speed of initial position and final position for pilotless automobile is equal
The situation for being zero；S type acceleration and deceleration method is asymmetric S type Acceleration-deceleration Control Method:
Conventional point-to-point trajectory planning, using symmetrical S type Acceleration-deceleration Control Method, operational process is divided into seven sections,
It is acceleration section, uniform acceleration section respectively, subtracts acceleration section, uniform velocity section, acceleration-deceleration section, even deceleration segment, slows down
Spend section；Wherein acceleration section, the even acceleration identical as this four period time values of braking section are subtracted that subtract accelerating sections, accelerating and decelerating part
It is identical with even deceleration segment time value to spend section；Consider influence of the acceleration to mechanism kinematic, the present invention uses asymmetrical S
Type Acceleration-deceleration Control Method, incipient stage, that is, acceleration section use larger acceleration, ending phase, that is, deceleration segment use compared with
Small acceleration is eliminated because of the larger vibration that acceleration is excessive to be occurred when vehicle body being caused to stop, cannot be considered in terms of precision with
The problem of efficiency；
Symmetric form S type Acceleration-deceleration Control Method complete for one section, it is real to its trajectory planning according to time optimal algorithm
Three time quantums: acceleration time t are just to solve on borderj, uniform acceleration time ta, uniform velocity time tv, use is asymmetric
Type S type Acceleration-deceleration Control Method, what trajectory planning can be more complicated than symmetric form S type Acceleration-deceleration Control Method is more, acceleration section with
The time for subtracting accelerating sections is identical, and accelerating and decelerating part is identical as the time for subtracting braking section, but two fluctuating acceleration periods and two
It is no longer identical to become the deceleration segment time, while uniform acceleration section and even deceleration segment time are also different, time unknown quantity becomes 5
It is a；
The variation relation of acceleration change rate and each phases-time
Although acceleration section and to subtract deceleration segment time value unequal, can be learnt according to the subsequent derivation of equation,
The time value variation in two stages is related to changing ratio before and after acceleration；
The constraint condition of trajectory planning is not considered, and whole story speed is zero, and acceleration obtains velocity amplitude to time integral；
Reinforcing method is cut using area common in engineering, carries out analytical calculation；
T can be solvedj1、tj2Relationship,
K be acceleration change rate, i.e., acceleration stage jerk value with subtract deceleration stage jerk value phase
Than；
Work as tv1It is zero according to whole story velocity restraint condition, that is, whole story speed, it is known that t when=0j1、tj2Variation relation, utilize
Area, which cuts reinforcing method, can obtain ta1、ta2Variation relation；Calculation formula is as follows:
Such five time unknown quantitys switch to seek three time quantums again；
Asymmetric each phases-time derivation algorithm of S type feed speed control
According to time optimal principle, restrictive condition is not considered first, when trajectory planning only has acceleration section, subtracts acceleration
Section and subtracts deceleration segment at acceleration-deceleration section, other velocity variations sections, acceleration change is not present；Therefore Schilling ta1=0, ta2
=0, tv1=0, seek tj1, tj2Value；It is available about speed and the binary of displacement three according to asymmetric S type Acceleration-deceleration Control Method
Equation of n th order n group, and then t can be obtainedj1, calculation formula is as follows:
Consider speed, acceleration limitation requires to occur at the end of subtracting acceleration section due to maximum speed, acceleration maximum value
At the end of appearing in acceleration section, it is necessary to tj1It is as follows to be modified calculation formula:
By the t found out above to the amendment at time pointj1、tj2It is able to satisfy the limitation requirement of velocity and acceleration, but general
It is unable to satisfy the requirement of traveling displacement, can generally have even accelerating sections, even braking section and at the uniform velocity section, according to time optimal principle
Enable tv1=0, similar tj1、tj2Solution procedure can acquire ta1、ta2Value, and then find out lv1Value；Find out asymmetric S type acceleration and deceleration
Five unknown time quantums being related to are made, the rate curve of entire motion profile can be obtained, and then construct using Taylor expansion
The relational expression of speed and parameter u value, according to the u value of variation and it is above-mentioned about double nurbs curve theories complete interpolation operation,
It realizes the accurate description and speed control to pilotless automobile pose, and then realizes spline interpolation formula efficiently, accurately
Running.
The present invention provides a kind of succinct, intelligible asymmetrical S type feed speed control side for the control of travel speed
The absolute value front and back of method, acceleration is different, and the jerk value for starting boost phase is larger, stops adding for decelerating phase
Velocity amplitude is smaller, and it is excessive because of vibrating when stopping effectively to avoid vehicle, and influences driving safety, comfortable problem, takes into account speed
Degree and safety, comfort problem.
Another kind speed control embodiment of the invention: the speed of initial position and final position for pilotless automobile
The situation being not zero；S type acceleration and deceleration method is the S type Acceleration-deceleration Control Method that whole story speed is not zero:
The S type Acceleration-deceleration Control Method that whole story speed is not zero is divided into seven periods of three kinds of changes phases, three variation ranks
Section is even acceleration stage, uniform acceleration stage, uniform velocity stage, and the even acceleration stage includes four periods: plus
Velocity shooting subtracts acceleration section, acceleration-deceleration section, subtracts deceleration segment；The uniform acceleration stage includes two periods: acceleration section,
Deceleration segment；The uniform velocity stage only includes a period: uniform velocity section；Wherein acceleration section and the time for subtracting accelerating sections
Identical, accelerating and decelerating part is identical as the time for subtracting braking section, can obtain the relationship of whole story speed and Acceleration and deceleration time and be displaced and add and subtract
The relationship of fast time is as follows, wherein vsIndicate beginning speed, veIndicate end speed；
The track being not zero to whole story speed is planned, similar with the trajectory planning roadmap that whole story speed is zero,
It needs to seek each phases-time value, but the three rank trajectory plannings that whole story speed is not zero are related to five time quantums, equation group is but
It is only two, therefore it is bigger than the trajectory planning that whole story speed is zero to solve difficulty；The S type acceleration and deceleration that whole story speed is not zero
It is several that the time planning mode that control method is related to has ten, just especially cumbersome if the classification mode according to it accounts for；
Therefore it needs to take a kind of more reasonable, easier method for solving；
Considered according to traveling situation, it is desirable to traveling task be completed with most fast running efficiency, while needing to meet automobile
Required precision is five yuan of cubic equation groups since the S type Acceleration-deceleration Control Method that whole story speed is not zero is related to equation, and
Only two equations, therefore can not be according to conventional equation group direct solution, therefore can first consider running efficiency problem, then
Consider driving safety problem, the i.e. time optimal algorithm of the time planning scheme gradually met the requirements；
The even acceleration section time based on Newton iteration method solves
First consider that running efficiency problem, entire travel phase only exist the even acceleration stage, uniform acceleration stage and even
Speed stage is all not present, this driving mode, running efficiency highest；Therefore Schilling ta1=0, ta2=0, tv1=0, seek tj1,
tj2Value；
The binary cubic equation group that can be obtained about speed and displacement is substituted into above formula:
Solving binary cubic equation group can be used the convergency value that Newton iteration finds out it, and equation group is configured to two first
Functional expression f (tj1, tj2)、g(tj1, tj2):
It seeks the first-order partial derivative of above-mentioned functional expression group 3- (39) and constitutes matrix A:
In formula:
f(tj1, tj2) about tj1First-order partial derivative；f(tj1, tj2) about tj2First-order partial derivative；
g(tj1, tj2) about tj1First-order partial derivative；g(tj1, tj2) about tj2First-order partial derivative；
The inverse matrix A_inv of partial derivative matrix A is acquired, Newton iteration needs to select reasonable iterative initial value, most of feelings
When carrying out trajectory planning under condition, initial velocity vsWith termination speed veIt is all located at rate curve endpoint, in order to avoid error transfinites,
The permissible velocity of the general point is smaller, and speed is not much different, therefore, it is known that tj1And tj2End value generally differ not
Greatly；When choosing iterative initial value, t is enabledj1=tj2=tj, t can be obtainedjCalculation formula:
Using tj as tj1Iterative initial value, be arranged iteration precision, tjIt is assigned to tj10, tj20, tj10、tj20Generation respectively
Enter above-mentioned functional expression and matrix A, and then calculates an iteration value tj11, tj21, calculation formula is as follows:
Iteration precision esp is calculated using calculated iterative value:
It according to required precision, substitutes into iterative calculation formula and iteration precision calculation formula is iterated calculating, find out full
The convergency value required enough substitutes into subsequent calculating；
But under normal circumstances, this driving mode is unable to satisfy driving safety requirement, can may also exceed vehicle institute energy
The acceleration capacity of offer；It is approximate optimal solution using the convergency value that iterative method is found out, and considers the limit of speed and displacement
System, the convergency value found out may there are large errors with actual value, if final convergency value is one group of negative solution, according to reality
Situation, what at this moment we needed is not just final convergency value, and one group of positive number solution for being closest to convergence solution is calculated as final
As a result；And then obtain satisfactory tj1、tj2Value；
Speed, acceleration limitation is considered, since maximum speed occurs at the end of subtracting acceleration section, with maximum limitation speed
The permitted acceleration time compares, and takes smaller value therein, and calculation formula is as follows:
It considers further that acceleration limitation requires, at the end of acceleration maximum value appears in acceleration section, adds with maximum limitation
The permitted acceleration time of speed compares, and takes smaller value therein, and calculation formula is as follows:
T at this timej2With tj1Correlation, tj1Change then corresponding tj2It is also required to recalculate, calculation formula is as follows:
When the acceleration maximum value for judging that the acceleration and deceleration stage reaches is also required to variation permitted with maximum limitation acceleration
Between, time value can only take smaller value, and calculation formula is as follows:
The t for meeting maximum limitation speed and maximum limitation acceleration requirement is obtained in this wayj1、tj2Value；
By Newton iteration method and the value of iterated revision found out, in some cases, it is understood that there may be error, need
It is corrected again, requires to correct for second of progress using fast whole story speed and displacement limitation, find out satisfactory tj1、tj2
Value；In order to combine first time iterated revision as a result, tj1、tj2Can only reduce not can increase；Velocity error v_esp is calculated such as
Shown in lower formula:
A certain accuracy value ESP is set, if v_esp > ESP, tj1It should reduce by a sampling period Ts, then substitute into speed
Degree error formula carries out error judgment to be continued to reduce if having not been met requirement, is reduced to always until meeting the requirements, such as
Fruit tj1Also it is unable to satisfy requirement when reducing to zero, then exits circulation；If v_esp <-ESP, shows tj2It should reduce by a sampling
Then cycle T s substitutes into velocity error formula and carries out error judgment, this process is repeated, until meeting the requirements；
And then by displacement restrictive condition to the t found outj1, tj2, it is iterated corrected Calculation, formula is as follows:
Wherein displacement error formula s_esp is actual displacement and the difference for planning shift value；If s_esp > 0 illustrates to press
It is travelled according to the calculated time, actual displacement value can be less than desired shift value, this is not inconsistent with actual requirement, therefore
T should be reduced againj1Value, due to what is involved is the variation of displacement, it and tj1、tj2It is all related, so corresponding tj2Also it wants
Change, t can be obtainedj2Calculation formula it is as follows:
Then it substitutes into displacement error formula and carries out calculating error amount and compare, this process repeatedly, when being met the requirements
Value stop this process；When s_esp < 0 illustrates that the displacement cooked up is less than desired shift value, this and actual conditions phase
Symbol illustrates to terminate iterated revision process there are also other velocity variations sections；
To sum up process, which can be found out, meets even acceleration phases-time tj1、tj2Value；
It is solved based on the even acceleration time section time planning of Newton iteration
By the t found out above to the amendment at time pointj1、tj2It is able to satisfy the limitation requirement of velocity and acceleration, but general
It is unable to satisfy the requirement of traveling displacement, may also have even accelerating sections, even braking section and at the uniform velocity section, considers time optimal principle
Enable tv1=0, it can be obtained about whole story speed and displacement limitation equation group:
According to above-mentioned theory, t is found out using Newton iteration methoda1、ta2Initial value, then utilize iterated revision theory carry out
Amendment, finally obtains the uniform acceleration phases-time t met the requirementsa1、ta2Value；
Uniform velocity phases-time value
Seven velocity variations sections of three phases are related to for S type acceleration and deceleration trajectory planning algorithm, there are five the time is unknown
Amount, can find out t using above-mentioned algorithm according to efficiency supreme principlej1、tj2、ta1、ta2Value, be not present phases-time ginseng
Numerical value is equal to 0, only remains next unknown quantity t nowv1, tj1、tj2、ta1、ta2Equation is substituted into, equation becomes about tv1One
First linear function can directly solve tv1Value；It is as follows calculation formula can be obtained:
T can be calculated by above-mentioned formulav1；
It has all been found out according to five time unknown quantitys that above-mentioned algorithm is related to, according to the time quantum of planning, can have been cooked up
Speed change curves, and then carry out corresponding interpolation calculating.
Since S type feed speed control is related to solving the polynary equation of higher degree or inequality, and solves and come most
Whole solution is nonnegative number, and traditional algorithm is to consider various plus-minus fast modes, and gradually judgement obtains time point, therefore results in meter
Calculation process is cumbersome, and duration is used in solution.It is proposed thus according to the restrictive condition of time optimal principle and speed, acceleration S type
Five yuan of Nonlinear System of Equations involved in feed speed control are decomposed into two binary nonlinear equation groups and a unitary is linear
Equation is gradually solved satisfactory Acceleration and deceleration time by the way of Newton iteration method and iterated revision, and then finds out speed
It writes music line, vehicle is accurately controlled.The present invention can effectively avoid complicated, the cumbersome fortune of existing S type feed speed control
It calculates, simplify acceleration and deceleration pattern classification, provide that a kind of precision is high, understandable Acceleration-deceleration Control Method.
Spline interpolation algorithm is the coordinate of determining each sampling period pilotless automobile, and the densification for carrying out curve is raw
At this process is related to parameters calculation, needs to consider that the performance of driving safety, running efficiency problem and vehicle is asked
Topic, so having many requirements to batten parameter；According to the velocity amplitude in each sampling period cooked up, managed using B-spline
Corresponding next interpolated points'parameter u value is found out by knowledge, calculation formula is as follows；
Ts is the sampling period, and v (t) plans the speed found out, uiFor the parameter value of a upper interpolated point, X ' (u) is that batten is bent
The first derivative of line abscissa, Y ' (u) are the first derivative of spline curve ordinate, and X " (u) is the two of spline curve abscissa
Order derivative, Y " (u) are the second dervative of spline curve ordinate.
To the curve for planning speed it is possible that the situation of velocity jump, at this moment needs to carry out light to planning rate curve
Sliding processing；To guarantee the smooth of speed, it is necessary to consider acceleration and deceleration ability, according to velocity jump degree, and consider that acceleration becomes
Change situation, carries out smooth treatment；
When using three rank S type feed speed controls, acceleration is minimum control unit, acceleration consecutive variations, so light
Acceleration magnitude in same period must be taken into consideration in sliding processing；Since velocity jump needs to consider front and back week in a certain sampling period
Acceleration and deceleration ability in phase needs period judgement one by one simultaneously because acceleration does not immobilize, obtain need it is several in advance
A period slows down；
It first has to consider forward from mutating speed point, calculates the reduction speed for needing several periods, it is high accidentally to be just able to satisfy bow
Difference limitation requires；The mode that circulative accumulation can be used judges that needing several periods to be just able to satisfy limitation requires；Calculation formula
As follows；
AcC=ai-1+ai-2+...ai-n
Wherein: n be need in advance n period slow down；aiFor the acceleration value in ith sample period；Acc is n
The acceleration value accumulated value in sampling period；viFor the velocity amplitude in ith sample period；J is the maximum value of acceleration.
As optimization technique measure,
Travel route is described using spline curve；Consider computational accuracy and calculation amount, nurbs curve weight because
Son takes 1, takes cubic B-spline, by the way of stationary nodes vector, finds out basic function, knot vector are as follows:
U={ -3, -2, -1,0,1,2,3 }
The effective calculation formula of interception U=[0,1] can obtain four basic functions of uniform cubic B-spline:
Spline curve with parameter u variation relation formula are as follows:
Wherein: P1(u) paragraph 1 spline curve, p are indicated1+iIndicate i-th of control point of paragraph 1 curve, X1(u) control is indicated
The vector of point abscissa composition, Y1(u) vector of control point ordinate composition, x are indicatedl+iIndicate i-th of control point of paragraph 1
Abscissa, y1+iIndicate the ordinate at i-th of control point of paragraph 1；
It is irregular path due to needing the track travelled, the speed and direction variation of pilotless automobile can not
It is arbitrary；Within a sampling period, the velocity magnitude of pilotless automobile and direction are generally fixed, if track have it is prominent
Become, in the case that the speed of pilotless automobile is excessive, can not just travel out expected path；This is just needed in some positions,
At the inflection point of position, the speed of pilotless automobile is limited, guarantees driving safety；According to driving path and pilotless automobile
Actual motion profile relationship introduces the concept for bending high error, carries out the trueness error of both analyses；At coordinate inflection point pair
The speed answered is smaller, if to guarantee driving safety, cannot centainly be greater than these given speed in the feed speed of these points
Limitation, but if entire path velocity just will affect running efficiency all in accordance with the operation of these speed minimums；Therefore synthesis is examined
Consider efficiency and precision problem, can the position inflection point according to corresponding to these speed minimums segment processing is carried out to entire path；
Analytical calculation, which is carried out, using S type path planning theory to every section then carries out interpolation planning with cubic spline theory again:
The curvature formulations of curved line are as follows:
Curvature radius calculation formula is as follows:
Known control point, sampling period are generated between control point according to high error requirements are bent using uniform spline theory
Position coordinates find out radius of curvature and then find out rate limitation curve；It can must bend the speed calculation formula under high error limitation:
Wherein: e is to bend high error, and r is spline curve radius, and Ts is the sampling period.
Since non-uniform rational B-spline is computationally intensive, calculating is cumbersome, in some spline interpolation algorithms, in order to reduce meter
It calculates, using a kind of uniform cubic B-spline algorithm that knot vector is constant.It can greatly reduce calculation amount, and to the essence of batten
Degree influences very little, thus is widely used in spline interpolation calculating.
The present invention is based on double nurbs curve pilotless automobile pose TRAJECTORY CONTROLs of asymmetric S type feed speed control realities
Apply example:
Emulation, which is carried out, using MATLAB GUI checks feasibility of the invention and to the influence of running velocity.
In order to be modeled in SolidWorks to three-dimensional vehicle, and then model further to verify feasibility of the invention
It imports MATLAB simulink module and carries out simulation calculation.It is given that one group of pilotless automobile is discrete passes through point and vehicle body
Shaft space angle change value, value are as shown in table 1:
1 vehicle body pose of table changes discrete data
The unit of displacement is km, and the unit of angle is degree * 100.
Controlling model is programmed emulation in matlab simulink module according to the present invention, 1 data input motion of table
It controls in program, while given restrictive condition: maximum limitation acceleration j=10000m/s3, maximum limitation acceleration am=
1000m/s2, maximum limitation speed vm=800m/s, using cycle Ts=1ms, acceleration change rate K=4.It is real by emulation
Verifying, the present invention can control pilotless automobile and efficiently, accurately move.Pilotless automobile motion profile such as Fig. 1 institute
Show, partially tangentially amount is as shown in Figure 2.
The invention proposes a kind of double nurbs curve pilotless automobile poses based on asymmetric S type feed speed control
TRAJECTORY CONTROL research describes vehicle body axial vector using double nurbs curves and changes, and calculates pilotless automobile trail change
Tangent vector, realize and vehicle body axis variation pose accurately controlled, corresponding weight factor is calculated according to offset and is able to achieve pair
The accurate adjustment of vehicle body axial vector.Travel speed can be effectively improved using asymmetric S type Acceleration-deceleration Control Method, reduced because adding
Vibration problem caused by acceleration is excessive, by emulation experiment, the verifying present invention is practical.
Claims (10)
1. a kind of automatic Pilot control method, which comprises the following steps:
The first step acquires path map by manned automobile；
Second step obtains pilotless automobile side images by camera, and compares and judge with the corresponding position of path map
Whether there are obstacles out, and the barrier includes vehicle or people or road stake；
Third step measures the distance between pilotless automobile and barrier by laser radar
4th step generates travel route according to path map and obstacle distance information；
5th step carries out route planning to travel route according to spline interpolation method；
6th step carries out speed control to pilotless automobile using S type acceleration and deceleration method；
7th step, pilotless automobile are travelled according to the route of planning and according to predetermined speed.
2. a kind of automatic Pilot control method as described in claim 1, which is characterized in that
The travel route is adjusted at any time according to the movement of barrier, according to the position of barrier and path feature and is added
Upper whole story location point constitutes the control point of spline interpolation；Passed through a little according to control point and the generation of necessary safe distance；According to
Movement speed, path feature and the legal speed limit formation speed boundary Control curve of barrier；
Nurbs curve in spline interpolation method is by four control point, knot vector, weight factor, basic function factors compositions；According to
Control point can obtain knot vector by correlation formula, and then can obtain basic function by knot vector, and weight factor is general
It gives or is calculated according to offset；Deformation calculating is carried out on the basis of one Judd's method of Hadley according to known control point
Suitable knot vector out；Weight factor is calculated further according to the offset of needs, weight factor is obtained and knot vector can
Basic function is obtained, and then constructs nurbs curve using control point, basic function and weight factor.
3. a kind of automatic Pilot control method as claimed in claim 2, which is characterized in that
Calculate node vector:
Known m+p+1 control pointP is the basic function number that is constructed；According to the distance between control point distance, meter
Calculate suitable knot vector value u；
First calculate distance d between control pointj, formula are as follows:
Wherein xjIndicate control point PjHorizontal axis coordinate, yjIndicate control point PjOrdinate of orthogonal axes；
Hadley-Judd's method recurrence formula deforms, formula after deformation are as follows:
Wherein uiIt indicates i-th of node, and then knot vector U recurrence formula can be obtained are as follows:
The knot vector form found out are as follows: U=[0,0,0, up, up+1... um+p1M+1, m+1, m+1]；
NURBS expression formula:
Acquire basic function using knot vector, NURBS basic function there are many kinds of structural form, general common structural form be by
What following recurrence formula provided, the basic function N determined with following recursion modeI, p(u) it is known as P times corresponding to knot vector U
NURBS basic function:
p≥2
Wherein i is the sequence number of basic function, gives knot vector U, required for capable of being derived according to recurrence formula above
Basic function；And then i-th section of nurbs curve P can be obtainedi(u), expression formula are as follows:
u∈[ui+p, ui+p+1], i=1,2,3...m+1.
4. a kind of automatic Pilot control method as claimed in claim 3, which is characterized in that
Weight factor can adjust nurbs curve at a distance from control point, and weight factor value is bigger, nurbs curve closer to
Corresponding control point can calculate corresponding weight factor, generation using offset according to the relational expression of weight factor and offset
Enter nurbs curve expression formula, and then realizes the adjustment to nurbs curve, weight factor rule of thumb value or according to vehicle body length
Degree and width determine that weight factor initial value all takes 1；
It calculates separately and works as u=uk, wkWhen=1, interpolated point p1, work as u=uk, wkWhen=0, interpolated point p0；
And then distance measurements S can be obtainedk, Kk, calculation formula is as follows:
Wherein PKIt is k-th of control point, k=0,1,2 ..., m+p
Weight factor w can be acquiredkWith offset dkBetween relational expression, calculation formula are as follows:
5. a kind of automatic Pilot control method as claimed in claim 4, which is characterized in that
Pilotless automobile moves to target position from initial position, is related to the description of vehicle body pose diverse vector, vehicle body
Axis is reduced to a straight-bar, describes a straight-bar in the variation posture in space；
The discrete point and vehicle body axis corresponding change angle that given pilotless automobile needs to pass through, to describe it from starting point
To the pose variation between target point；Discrete point and vehicle body shaft space the variation angle for needing to pass through according to pilotless automobile
Degree, the length of vehicle body axis are usually to immobilize；According to effective length and corresponding spatial variations angle, passed through using vehicle body
Point, can acquire vehicle body axis goes out corresponding two groups of control points by point, and then by NURBS theory inverse, according to control point benefit
Interpolation calculating is carried out with NURBS theory, generates series of points, the anti-solution of progress in spline interpolation formula is substituted into and calculates, obtain front and back
Location information at end；
The discrete point that given pilotless automobile needs to pass through is Ti=(xti, yti, zti) and vehicle body axis one group of variation angle
Spend (αi, βi), the length of vehicle body is fixed, and l is set as, and the discrete of that corresponding vehicle body axial vector passes through a coordinate Hi=(xhi,
yhi, zhi) calculation formula are as follows:
xhi=xti+l cosαicosβi
yhi=yti+l cosαisinβi
zhi=zti+l sinαi
Vehicle body can be found out through the above steps by point and another group of vehicle body axial vector by point, the change of such vehicle body axis
Change, which can clearly be depicted, to be come, it is known that can obtain corresponding control point according to NURSB curve generative theory by putting；
The tangent vector of vehicle body interpolation track is obtained by the derivation theoretical calculation of parametric equation, i.e. basic function carries out derivation to u, so
Substitute into related NURBS formula again afterwards；
Calculation formula is as follows:
6. a kind of automatic Pilot control method a method as claimed in any one of claims 1 to 5, which is characterized in that
The speed of initial position and final position for pilotless automobile is zero situation；S type acceleration and deceleration method is non-
Symmetric form S type Acceleration-deceleration Control Method:
Conventional point-to-point trajectory planning, using symmetrical S type Acceleration-deceleration Control Method, operational process is divided into seven sections, respectively
It is acceleration section, uniform acceleration section, subtracts acceleration section, uniform velocity section, acceleration-deceleration section, even deceleration segment, subtracts deceleration segment；
Wherein acceleration section, to subtract accelerating sections, accelerating and decelerating part identical as this four period time values of braking section are subtracted, uniform acceleration section and
Even deceleration segment time value is identical；Consider influence of the acceleration to mechanism kinematic, the present invention uses asymmetrical S type acceleration and deceleration
Control method, incipient stage, that is, acceleration section use larger acceleration, and ending phase, that is, deceleration segment is added using lesser
Speed；
Symmetric form S type Acceleration-deceleration Control Method complete for one section, according to time optimal algorithm, actually to its trajectory planning
Just it is to solve for three time quantums: acceleration time tj, uniform acceleration time ta, uniform velocity time tv, using asymmetric S type
Acceleration-deceleration Control Method, what trajectory planning can be more complicated than symmetric form S type Acceleration-deceleration Control Method is more, acceleration section and subtracts acceleration
The time of section is identical, and accelerating and decelerating part is identical as the time for subtracting braking section, but two fluctuating acceleration periods and two changes are slowed down
It is no longer identical to spend the section time, while uniform acceleration section and even deceleration segment time are also different, time unknown quantity becomes 5；
The variation relation of acceleration change rate and each phases-time
Although acceleration section and to subtract deceleration segment time value unequal, can be learnt according to the subsequent derivation of equation, two
The time value variation in stage is related to changing ratio before and after acceleration；
The constraint condition of trajectory planning is not considered, and whole story speed is zero, and acceleration obtains velocity amplitude to time integral；
Reinforcing method is cut using area common in engineering, carries out analytical calculation；
T can be solvedj1、tj2Relationship,
K is acceleration change rate, i.e. acceleration stage jerk value is compared with subtracting deceleration stage jerk value；
Work as tv1It is zero according to whole story velocity restraint condition, that is, whole story speed, it is known that t when=0j1、tj2Variation relation, utilize area
T can be obtained by cutting reinforcing methoda1、ta2Variation relation；Calculation formula is as follows:
Such five time unknown quantitys switch to seek three time quantums again；
Asymmetric each phases-time derivation algorithm of S type feed speed control
According to time optimal principle, do not consider restrictive condition first, when trajectory planning only has acceleration section, subtract acceleration section plus
Deceleration segment and subtract deceleration segment, other velocity variations sections, acceleration change is not present；Therefore Schilling ta1=0, ta2=0, tv1
=0, seek tj1, tj2Value；According to the available binary cubic equation about speed and displacement of asymmetric S type Acceleration-deceleration Control Method
Group, and then t can be obtainedj1, calculation formula is as follows:
Consider speed, acceleration limitation requires to occur at the end of subtracting acceleration section due to maximum speed, and acceleration maximum value occurs
At the end of acceleration section, it is necessary to tj1It is as follows to be modified calculation formula:
By the t found out above to the amendment at time pointj1、tj2It is able to satisfy the limitation requirement of velocity and acceleration, but generally can not
The requirement for meeting traveling displacement can generally have even accelerating sections, even braking section and at the uniform velocity section, enable t according to time optimal principlev1=
0, similar tj1、tj2Solution procedure can acquire ta1、ta2Value, and then find out tv1Value；Asymmetric S type feed speed control is found out to be related to
Five unknown time quantums, can obtain the rate curve of entire motion profile, so using Taylor expansion desin speed with
The relational expression of parameter u value, according to the u value of variation and it is above-mentioned about double nurbs curve theories complete interpolation operation, realize to nothing
The accurate description and speed control of people's driving pose.
7. a kind of automatic Pilot control method a method as claimed in any one of claims 1 to 5, which is characterized in that
The situation that the speed of initial position and final position for pilotless automobile is not zero；S type acceleration and deceleration method is to begin
The S type Acceleration-deceleration Control Method that end speed is not zero:
The S type Acceleration-deceleration Control Method that whole story speed is not zero is divided into seven periods of three kinds of changes phases, and three changes phases are
Even acceleration stage, uniform acceleration stage, uniform velocity stage, even acceleration stage include four periods: acceleration
Section subtracts acceleration section, acceleration-deceleration section, subtracts deceleration segment；The uniform acceleration stage includes two periods: acceleration section is slowed down
Spend section；The uniform velocity stage only includes a period: uniform velocity section；Wherein acceleration section is identical as the time for subtracting accelerating sections,
Accelerating and decelerating part is identical as the time for subtracting braking section, can obtain the relationship and displacement and Acceleration and deceleration time of whole story speed and Acceleration and deceleration time
Relationship it is as follows, wherein vsIndicate beginning speed, veIndicate end speed；
The track being not zero to whole story speed is planned, similar with the trajectory planning roadmap that whole story speed is zero, is needed
Each phases-time value is sought, but the three rank trajectory plannings that whole story speed is not zero are related to five time quantums, equation group is only
There are two, therefore it is bigger than the trajectory planning that whole story speed is zero to solve difficulty；The S type feed speed control side that whole story speed is not zero
It is several that the time planning mode that method is related to has ten, just especially cumbersome if the classification mode according to it accounts for；Therefore it needs
Take a kind of more reasonable, easier method for solving；
Considered according to traveling situation, it is desirable to traveling task be completed with most fast running efficiency, while needing to meet the precision of automobile
It is required that since the S type Acceleration-deceleration Control Method that whole story speed is not zero is related to equation for five yuan of cubic equation groups, and only two
A equation, therefore can not be according to conventional equation group direct solution, therefore can first consider running efficiency problem, consider further that traveling
Safety problem, the i.e. time optimal algorithm of the time planning scheme gradually met the requirements；
The even acceleration section time based on Newton iteration method solves
First consider that running efficiency problem, entire travel phase only exist even acceleration stage, uniform acceleration stage and uniform velocity
Stage is all not present, this driving mode, running efficiency highest；Therefore Schilling ta1=0, ta2=0, tv1=0, seek tj1, tj2Value；
The binary cubic equation group that can be obtained about speed and displacement is substituted into above formula:
Solving binary cubic equation group can be used the convergency value that Newton iteration finds out it, and equation group is configured to two functions first
Formula f (tj1, tj2)、g(tj1, tj2):
It seeks the first-order partial derivative of above-mentioned functional expression group 3- (39) and constitutes matrix A:
In formula:
f(tj1, tj2) about tj1First-order partial derivative；f(tj1, tj2) about tj2First-order partial derivative；
g(tj1, tj2) about tj1First-order partial derivative；g(tj1, tj2) about tj2First-order partial derivative；
The inverse matrix A_inv of partial derivative matrix A is acquired, Newton iteration needs to select reasonable iterative initial value, in most cases
When carrying out trajectory planning, initial velocity vsWith termination speed veIt is all located at rate curve endpoint, in order to avoid error transfinites, generally
The permissible velocity of the point is smaller, and speed is not much different, therefore, it is known that tj1And tj2End value be generally not much different；It is selecting
When taking iterative initial value, t is enabledj1=tj2=tj, t can be obtainedjCalculation formula:
Using tj as the iterative initial value of tj1, iteration precision is set, tjIt is assigned to tj10, tj20, tj10、tj20It substitutes into respectively above-mentioned
Functional expression and matrix A, and then calculate an iteration value tj11, tj21, calculation formula is as follows:
Iteration precision esp is calculated using calculated iterative value:
According to required precision, substitutes into iterative calculation formula and iteration precision calculation formula is iterated calculating, find out satisfaction and want
The convergency value asked substitutes into subsequent calculating；
But under normal circumstances, this driving mode is unable to satisfy driving safety requirement, can may also can be provided beyond vehicle
Acceleration capacity[i]；It is approximate optimal solution using the convergency value that iterative method is found out, and considers the limitation of speed and displacement,
The convergency value found out may there are large errors with actual value, if final convergency value is one group of negative solution, according to the actual situation,
What at this moment we needed is not just final convergency value, and is closest to one group of positive number solution of convergence solution as final calculation result；
And then obtain satisfactory tj1、tj2Value；
Speed, acceleration limitation is considered, since maximum speed occurs at the end of subtracting acceleration section, with permitting for maximum limitation speed
Perhaps acceleration time compares, and takes smaller value therein, and calculation formula is as follows:
It considers further that acceleration limitation requires, at the end of acceleration maximum value appears in acceleration section, limits acceleration with maximum
The permitted acceleration time compare, take smaller value therein, calculation formula is as follows:
T at this timej2With tj1Correlation, tj1Change then corresponding tj2It is also required to recalculate, calculation formula is as follows:
Judge that the acceleration maximum value that the acceleration and deceleration stage reaches is also required to limit the permitted transformation period of acceleration with maximum, when
Between value can only take smaller value, calculation formula is as follows:
The t for meeting maximum limitation speed and maximum limitation acceleration requirement is obtained in this wayj1、tj2Value；
By Newton iteration method and the value of iterated revision found out, in some cases, it is understood that there may be error, need to carry out
It corrects again, requires to correct for second of progress using fast whole story speed and displacement limitation, find out satisfactory tj1、tj2Value；For
First time iterated revision is combined as a result, tj1、tj2Can only reduce not can increase；Velocity error v_esp calculates following formula
It is shown:
A certain accuracy value ESP is set, if v_esp > ESP, tj1It should reduce by a sampling period Ts, then substitute into speed and miss
Poor formula carries out error judgment to be continued to reduce if having not been met requirement, is reduced to always until meeting the requirements, if tj1
Also it is unable to satisfy requirement when reducing to zero, then exits circulation；If v_esp <-ESP, shows tj2It should reduce by a sampling period
Then Ts substitutes into velocity error formula and carries out error judgment, this process is repeated, until meeting the requirements；
And then by displacement restrictive condition to the t found outj1, tj2, it is iterated corrected Calculation, formula is as follows:
Wherein displacement error formula s_esp is actual displacement and the difference for planning shift value；If s_esp > 0, illustrates according to meter
The time of calculating is travelled, and actual displacement value can be less than desired shift value, this is not inconsistent with actual requirement, therefore should be again
Secondary reduction tj1Value, due to what is involved is the variation of displacement, it and tj1、tj2It is all related, so corresponding tj2Also to change, it can
Obtain tj2Calculation formula it is as follows:
Then it substitutes into displacement error formula and carries out calculating error amount and compare, this process repeatedly, when the value met the requirements
Stop this process；When s_esp < 0 illustrates that the displacement cooked up is less than desired shift value, this is consistent with actual conditions, says
It is bright that there are also other velocity variations sections, end iterated revision processes；
To sum up process, which can be found out, meets even acceleration phases-time tj1、tj2Value；
It is solved based on the even acceleration time section time planning of Newton iteration
By the t found out above to the amendment at time pointj1、tj2It is able to satisfy the limitation requirement of velocity and acceleration, but generally can not
The requirement for meeting traveling displacement may also have even accelerating sections, even braking section and at the uniform velocity section, consideration time optimal principle enable tv1=
0, it can be obtained about whole story speed and displacement limitation equation group:
According to above-mentioned theory, t is found out using Newton iteration methoda1、ta2Initial value, be then modified using iterated revision theory,
Finally obtain the uniform acceleration phases-time t met the requirementsa1、ta2Value；
Uniform velocity phases-time value
Seven velocity variations sections of three phases are related to for S type acceleration and deceleration trajectory planning algorithm, there are five time unknown quantity, roots
T can be found out using above-mentioned algorithm according to efficiency supreme principlej1、tj2、ta1、ta2Value, phases-time parameter value being not present etc.
In 0, next unknown quantity t is only remained nowv1, tj1、tj2、ta1、ta2Equation is substituted into, equation becomes about tv1Unitary first power
Journey can directly solve tv1Value；It is as follows calculation formula can be obtained:
T can be calculated by above-mentioned formulav1；
It has all been found out according to five time unknown quantitys that above-mentioned algorithm is related to, according to the time quantum of planning, speed can be cooked up
Change curve, and then carry out corresponding interpolation calculating.
8. a kind of automatic Pilot control method as claimed in claim 7, which is characterized in that
Spline interpolation algorithm is the coordinate of determining each sampling period pilotless automobile, and the densification for carrying out curve generates, this
A process is related to parameters calculation, needs to consider the performance issue of driving safety, running efficiency problem and vehicle, so
There are many requirements to batten parameter；According to the velocity amplitude in each sampling period cooked up, asked using B-spline theoretical knowledge
Corresponding next interpolated points'parameter u value, calculation formula are as follows out；
Ts is the sampling period, and v (t) plans the speed found out, uiFor the parameter value of a upper interpolated point, X ' (u) is the horizontal seat of spline curve
Target first derivative, Y ' (u) are the first derivative of spline curve ordinate, and X " (u) is the second dervative of spline curve abscissa,
Y " (u) is the second dervative of spline curve ordinate.
9. a kind of automatic Pilot control method as claimed in claim 8, which is characterized in that
To the curve for planning speed it is possible that the situation of velocity jump, at this moment needs to carry out smooth place to planning rate curve
Reason；To guarantee the smooth of speed, it is necessary to consider acceleration and deceleration ability, according to velocity jump degree, and consider acceleration change feelings
Shape carries out smooth treatment；
When using three rank S type feed speed controls, acceleration is minimum control unit, acceleration consecutive variations, so smooth place
Acceleration magnitude in same period must be taken into consideration in reason；Since velocity jump needs to consider in the period of front and back in a certain sampling period
Acceleration and deceleration ability needs period judgement one by one simultaneously because acceleration does not immobilize, obtain need in advance several periods into
Row slows down；
It first has to consider forward from mutating speed point, calculates the reduction speed for needing several periods, be just able to satisfy the high limits of error of bow
System requires；The mode that circulative accumulation can be used judges that needing several periods to be just able to satisfy limitation requires；The following institute of calculation formula
Show；
Acc=ai-1+ai-2+…ai-n
Wherein: n be need in advance n period slow down；aiFor the acceleration value in ith sample period；Acc is n sampling week
The acceleration value accumulated value of phase；viFor the velocity amplitude in ith sample period；J is the maximum value of acceleration.
10. such as a kind of automatic Pilot control method as claimed in claim 3 to 5, which is characterized in that
Travel route is described using spline curve；Consider that computational accuracy and calculation amount, nurbs curve weight factor take
1, cubic B-spline is taken, by the way of stationary nodes vector, finds out basic function, knot vector are as follows:
U={ -3, -2, -1,0,1,2,3 }
The effective calculation formula of interception u=[0,1] can obtain four basic functions of uniform cubic B-spline:
Spline curve with parameter u variation relation formula are as follows:
Wherein: Pl(u) I section spline curve, p are indicatedl+iIndicate i-th of control point of I section curve, Xl(u) indicate that control point is horizontal
The vector of coordinate composition, Yl(u) vector of control point ordinate composition, x are indicatedl+iIndicate the horizontal seat at i-th of control point of I section
Mark, yl+iIndicate the ordinate at i-th of control point of I section.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811578082.2A CN109765887B (en) | 2018-12-21 | 2018-12-21 | Automatic driving control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811578082.2A CN109765887B (en) | 2018-12-21 | 2018-12-21 | Automatic driving control method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109765887A true CN109765887A (en) | 2019-05-17 |
| CN109765887B CN109765887B (en) | 2020-08-14 |
Family
ID=66451944
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811578082.2A Expired - Fee Related CN109765887B (en) | 2018-12-21 | 2018-12-21 | Automatic driving control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109765887B (en) |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111123952A (en) * | 2019-12-31 | 2020-05-08 | 华为技术有限公司 | Trajectory planning method and device |
| CN111739319A (en) * | 2019-10-18 | 2020-10-02 | 腾讯科技（深圳）有限公司 | Information processing method and device |
| CN112148002A (en) * | 2020-08-31 | 2020-12-29 | 西安交通大学 | Local trajectory planning method, system and device |
| CN112230641A (en) * | 2019-06-28 | 2021-01-15 | 比亚迪股份有限公司 | Method and device for generating braking curve of vehicle |
| CN112363515A (en) * | 2021-01-14 | 2021-02-12 | 上海交大智邦科技有限公司 | Mecanum wheel type AGV parking method based on visual positioning |
| WO2021088528A1 (en) * | 2019-11-07 | 2021-05-14 | 广东工业大学 | Outdoor driving system for unmanned vehicle |
| TWI740634B (en) * | 2020-09-08 | 2021-09-21 | 財團法人車輛研究測試中心 | Dynamic velocity planning method for autonomous vehicle and system thereof |
| CN113478479A (en) * | 2021-06-17 | 2021-10-08 | 北京工业大学 | Acceleration selection method based on industrial robot quintic polynomial trajectory planning intermediate point |
| CN113534826A (en) * | 2020-04-15 | 2021-10-22 | 苏州宝时得电动工具有限公司 | Attitude control method and device for self-moving equipment and storage medium |
| CN113835423A (en) * | 2020-06-08 | 2021-12-24 | 苏州艾吉威机器人有限公司 | B-spline curve-based path planning method, motion control method and system |
| CN114220283A (en) * | 2021-11-29 | 2022-03-22 | 中汽研（天津）汽车工程研究院有限公司 | Intersection smooth vehicle speed guiding method based on V2X |
| CN117666482A (en) * | 2023-12-11 | 2024-03-08 | 江南大学 | RGV asymmetric S-shaped speed curve speed regulation method and system |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101334283A (en) * | 2007-06-29 | 2008-12-31 | 爱信艾达株式会社 | Vehicle behavior learning apparatuses and programs |
| CN101607552A (en) * | 2008-06-20 | 2009-12-23 | 爱信艾达株式会社 | Drive assistance device and driving assistance method and driving auxiliary routine |
| JP2012043243A (en) * | 2010-08-20 | 2012-03-01 | Fanuc Ltd | Multiaxial processing machine numerical control device controlling tool tip point position |
| CN102765385A (en) * | 2011-05-05 | 2012-11-07 | 通用汽车环球科技运作有限责任公司 | System and method for adjusting smoothness for lane centering steering control |
| CN105849585A (en) * | 2013-12-26 | 2016-08-10 | 株式会社日立制作所 | Object recognition device |
| CN106167045A (en) * | 2015-05-21 | 2016-11-30 | Lg电子株式会社 | Human pilot auxiliary device and control method thereof |
| CN106233355A (en) * | 2014-04-15 | 2016-12-14 | 三菱电机株式会社 | Drive assistance device and driving assistance method |
| CN106527452A (en) * | 2016-12-30 | 2017-03-22 | 广州汽车集团股份有限公司 | Motion path planning method of pilotless automobile when encountering obstacle and system thereof |
| CN107396763A (en) * | 2017-08-29 | 2017-11-28 | 江苏师范大学 | A kind of hedgerow automatic pruning machine control system and control method |
| CN108549328A (en) * | 2018-03-22 | 2018-09-18 | 汇川技术（东莞）有限公司 | Adaptive speed method and system for planning |
-
2018
- 2018-12-21 CN CN201811578082.2A patent/CN109765887B/en not_active Expired - Fee Related
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101334283A (en) * | 2007-06-29 | 2008-12-31 | 爱信艾达株式会社 | Vehicle behavior learning apparatuses and programs |
| CN101607552A (en) * | 2008-06-20 | 2009-12-23 | 爱信艾达株式会社 | Drive assistance device and driving assistance method and driving auxiliary routine |
| JP2012043243A (en) * | 2010-08-20 | 2012-03-01 | Fanuc Ltd | Multiaxial processing machine numerical control device controlling tool tip point position |
| CN102765385A (en) * | 2011-05-05 | 2012-11-07 | 通用汽车环球科技运作有限责任公司 | System and method for adjusting smoothness for lane centering steering control |
| CN105849585A (en) * | 2013-12-26 | 2016-08-10 | 株式会社日立制作所 | Object recognition device |
| CN106233355A (en) * | 2014-04-15 | 2016-12-14 | 三菱电机株式会社 | Drive assistance device and driving assistance method |
| CN106167045A (en) * | 2015-05-21 | 2016-11-30 | Lg电子株式会社 | Human pilot auxiliary device and control method thereof |
| CN106527452A (en) * | 2016-12-30 | 2017-03-22 | 广州汽车集团股份有限公司 | Motion path planning method of pilotless automobile when encountering obstacle and system thereof |
| CN107396763A (en) * | 2017-08-29 | 2017-11-28 | 江苏师范大学 | A kind of hedgerow automatic pruning machine control system and control method |
| CN108549328A (en) * | 2018-03-22 | 2018-09-18 | 汇川技术（东莞）有限公司 | Adaptive speed method and system for planning |
Non-Patent Citations (2)
| Title |
|---|
| 杨亮亮，等: "始末速度不为零的S型加减速时间规划算法研究", 《机械工程学报》 * |
| 秦霞，等: "基于NURBS曲线的工业机器人位置插补算法研究", 《制造业自动化》 * |
Cited By (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112230641A (en) * | 2019-06-28 | 2021-01-15 | 比亚迪股份有限公司 | Method and device for generating braking curve of vehicle |
| CN111739319A (en) * | 2019-10-18 | 2020-10-02 | 腾讯科技（深圳）有限公司 | Information processing method and device |
| WO2021088528A1 (en) * | 2019-11-07 | 2021-05-14 | 广东工业大学 | Outdoor driving system for unmanned vehicle |
| CN111123952B (en) * | 2019-12-31 | 2021-12-31 | 华为技术有限公司 | Trajectory planning method and device |
| CN111123952A (en) * | 2019-12-31 | 2020-05-08 | 华为技术有限公司 | Trajectory planning method and device |
| CN113534826B (en) * | 2020-04-15 | 2024-02-23 | 苏州宝时得电动工具有限公司 | Attitude control method and device of self-mobile device and storage medium |
| CN113534826A (en) * | 2020-04-15 | 2021-10-22 | 苏州宝时得电动工具有限公司 | Attitude control method and device for self-moving equipment and storage medium |
| CN113835423B (en) * | 2020-06-08 | 2024-03-26 | 苏州艾吉威机器人有限公司 | Path planning method, motion control method and system based on B spline curve |
| CN113835423A (en) * | 2020-06-08 | 2021-12-24 | 苏州艾吉威机器人有限公司 | B-spline curve-based path planning method, motion control method and system |
| CN112148002A (en) * | 2020-08-31 | 2020-12-29 | 西安交通大学 | Local trajectory planning method, system and device |
| CN112148002B (en) * | 2020-08-31 | 2021-12-28 | 西安交通大学 | Local trajectory planning method, system and device |
| TWI740634B (en) * | 2020-09-08 | 2021-09-21 | 財團法人車輛研究測試中心 | Dynamic velocity planning method for autonomous vehicle and system thereof |
| CN112363515A (en) * | 2021-01-14 | 2021-02-12 | 上海交大智邦科技有限公司 | Mecanum wheel type AGV parking method based on visual positioning |
| CN112363515B (en) * | 2021-01-14 | 2021-05-04 | 上海交大智邦科技有限公司 | Mecanum wheel type AGV parking method based on visual positioning |
| CN113478479B (en) * | 2021-06-17 | 2023-08-01 | 北京工业大学 | Acceleration selection method based on five-time polynomial trajectory planning intermediate point of industrial robot |
| CN113478479A (en) * | 2021-06-17 | 2021-10-08 | 北京工业大学 | Acceleration selection method based on industrial robot quintic polynomial trajectory planning intermediate point |
| CN114220283A (en) * | 2021-11-29 | 2022-03-22 | 中汽研（天津）汽车工程研究院有限公司 | Intersection smooth vehicle speed guiding method based on V2X |
| CN117666482A (en) * | 2023-12-11 | 2024-03-08 | 江南大学 | RGV asymmetric S-shaped speed curve speed regulation method and system |
| CN117666482B (en) * | 2023-12-11 | 2024-07-12 | 江南大学 | RGV asymmetric S-shaped speed curve speed regulation method and system |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109765887B (en) | 2020-08-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109765887A (en) | A kind of automatic Pilot control method | |
| Zhang et al. | Optimal vehicle path planning using quadratic optimization for baidu apollo open platform | |
| Tuncali et al. | Utilizing S-TaLiRo as an automatic test generation framework for autonomous vehicles | |
| CN111830979B (en) | Track optimization method and device | |
| CN106372758B (en) | Path following method and device of auxiliary parking system | |
| Shamir | How should an autonomous vehicle overtake a slower moving vehicle: Design and analysis of an optimal trajectory | |
| CN112965476B (en) | High-speed unmanned vehicle trajectory planning system and method based on multi-window model | |
| CN101493687A (en) | Real time forward looking whole-process acceleration and deceleration controlled NURBS curve self-adapting subsection interpolation method | |
| CN109643496A (en) | Vehicle control system | |
| CN107844058A (en) | A kind of curve movement Discrete Dynamic Programming method | |
| CN113886764A (en) | Intelligent vehicle multi-scene track planning method based on Frenet coordinate system | |
| CN112158196B (en) | Automatic parking method and device | |
| CN114906173B (en) | Automatic driving decision method based on two-point pre-aiming driver model | |
| Graf et al. | Trajectory planning for automated vehicles using driver models | |
| Wang et al. | Speed profile optimization for enhanced passenger comfort: An optimal control approach | |
| CN113815679B (en) | Implementation method for autonomous driving control of high-speed train | |
| Ruof et al. | Real-time spatial trajectory planning for urban environments using dynamic optimization | |
| CN109686086A (en) | The training of fuzzy control network generates the method and device that speed is suggested at crossing | |
| Li et al. | A unified trajectory planning and tracking control framework for autonomous overtaking based on hierarchical mpc | |
| CN111857112B (en) | Automobile local path planning method and electronic equipment | |
| CN109634280A (en) | A kind of suspension pedal unit and the control method for being applicable in it | |
| Moller et al. | Frenetix Motion Planner: High-Performance and Modular Trajectory Planning Algorithm for Complex Autonomous Driving Scenarios | |
| Yang et al. | Deep Reinforcement Learning Lane-Changing Decision Algorithm for Intelligent Vehicles Combining LSTM Trajectory Prediction | |
| JP6617582B2 (en) | Vehicle steering control device | |
| Mutlu et al. | Case study on model free determination of optimal trajectories in highly automated driving |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20200714 Address after: 310023, room 908, 606 Xixi Road, Xihu District, Zhejiang, Hangzhou, China Applicant after: HANGZHOU AOCHAO TECHNOLOGY Co.,Ltd. Address before: 316021 Room 403, Unit 2, 31 Lotus Pond Moonlight District, Lincheng Street, Dinghai District, Zhoushan City, Zhejiang Province Applicant before: Fei Zixie |
|
| TA01 | Transfer of patent application right | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20200814 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |