CN110400023A - A kind of switching method and device of trajectory planning algorithm - Google Patents
A kind of switching method and device of trajectory planning algorithm Download PDFInfo
- Publication number
- CN110400023A CN110400023A CN201910703173.2A CN201910703173A CN110400023A CN 110400023 A CN110400023 A CN 110400023A CN 201910703173 A CN201910703173 A CN 201910703173A CN 110400023 A CN110400023 A CN 110400023A
- Authority
- CN
- China
- Prior art keywords
- track
- planning algorithm
- trajectory planning
- fusion
- switching
- 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.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
- G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
Landscapes
- Business, Economics & Management (AREA)
- Human Resources & Organizations (AREA)
- Engineering & Computer Science (AREA)
- Strategic Management (AREA)
- Economics (AREA)
- Marketing (AREA)
- Game Theory and Decision Science (AREA)
- Entrepreneurship & Innovation (AREA)
- Development Economics (AREA)
- Operations Research (AREA)
- Quality & Reliability (AREA)
- Tourism & Hospitality (AREA)
- Physics & Mathematics (AREA)
- General Business, Economics & Management (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Subject description discloses a kind of switching method of trajectory planning algorithm and devices, unmanned machine is in switching track planning algorithm, if it is smaller to switch the calculated track difference of both front and back trajectory planning algorithm, then merge two kinds of calculated tracks of trajectory planning algorithm, and according to fusion track traveling, switching track planning algorithm again, finally travelled according to the calculated track of trajectory planning algorithm after switching, it effectively improves unmanned machine and completes the stability switched to trajectory planning algorithm and ride comfort under steam, there is serious shake in unmanned machine when significantly improving scene switching, the case where loss of stability, guarantee the continuity of scene switching.
Description
Technical field
This application involves unmanned technical field more particularly to the switching methods and device of a kind of trajectory planning algorithm.
Background technique
With the continuous progress of science and technology, unmanned technology graduallys mature, and is become using unmanned machine instead of hand haulage
Main trend.
In existing unmanned machine technology, unmanned machine needs to make path planning (Path in operation
) and trajectory planning (Trajectory Planning) Planning.For path planning without the concern for barrier, solution is logical
Cross the problem of certain route arrives at the destination;And trajectory planning needs to consider barrier, solution is feelings in avoiding obstacles
The problem of under condition according to the route of planning.For example, passage path planning can cook up the route from certain to airport, and
The track how overtaken other vehicles in traveling can be cooked up by trajectory planning.
In the planning process of track, unmanned machine, which generally passes through, considers location information, to perception information of ambient enviroment etc.,
The travel route in a period of time is cooked up, and executes specific movement and changes driving status, such as parking, straight trip, lane change, turning
Deng.
However, the variability of environment causes unmanned machine that can not only utilize a kind of trajectory planning algorithm in reality scene
Calculate the track of all scenes.Therefore, generally directed to a kind of scene, nothing is calculated using the trajectory planning algorithm for being suitable for the scene
The track of people's equipment in order to adapt to new scene, then needs to calculate newly by switching track planning algorithm in scene changes
Track.
But when switching in unmanned machine traveling to trajectory planning algorithm, different trajectory planning in same scene
The calculated track of algorithm may be far from each other, and unmanned machine is caused to be possible to according to two trajectory planning algorithms calculated two
The excessive track of a difference, unmanned machine according to the calculated two different tracks in front and back when driving, if the two tracks
Situations such as difference is excessive, and unmanned machine then will appear serious shake, loss of stability, in some instances it may even be possible to trajectory planning algorithm occur
The case where handover failure, therefore, in the prior art, unmanned machine carries out cutting for trajectory planning algorithm after usually requiring parking again
It changes.
Summary of the invention
This specification embodiment provides the switching method and device of a kind of trajectory planning algorithm, with the existing skill of the solution of part
The above problem existing for art.
This specification embodiment adopts the following technical solutions:
A kind of switching method for trajectory planning algorithm that this specification provides, unmanned machine make the first trajectory planning algorithm
For currently employed trajectory planning algorithm, which comprises
Unmanned machine according to calculated first track of first trajectory planning algorithm when driving, however, it is determined that switching rail
Mark planning algorithm calculates the second track then by the second trajectory planning algorithm;
Determine the first difference value of first track and second track;
If first difference value is not more than preset first threshold value, merges first track and second track obtains
To intermediate track, continue to travel according to the intermediate track, and judge whether the intermediate track meets preset condition, if full
Foot, then using second trajectory planning algorithm as currently employed trajectory planning algorithm, according to second track after continuing
It sails, if not satisfied, first track is then merged again and second track obtains intermediate track, until obtained mid-rail
Until mark meets preset condition.
Optionally it is determined that the first difference value of first track and second track, specifically includes:
The identical sampled point of quantification from first track and second track respectively;
According to each sampled point determined, the average displacement error of first track and second track is determined, make
For the first difference value of first track and second track.
Optionally, judge whether the intermediate track meets preset condition, specifically include:
The identical sampled point of quantification from the intermediate track and second track respectively;
According to each sampled point determined, the average displacement error of the intermediate track and second track is determined, make
For the second difference value of the intermediate track and second track;
Judge whether second difference value is less than default second threshold, if being less than, it is determined that the intermediate track meets
Preset condition, if being not less than, it is determined that the intermediate track is unsatisfactory for preset condition.
Optionally, before fusion first track and second track obtain intermediate track, the method also includes:
Determine the number for obtaining intermediate track no more than the first default the number of iterations;
If the number for obtaining intermediate track is greater than the first default the number of iterations, stop travelling, and by second track
Planning algorithm continues to travel as currently employed trajectory planning algorithm according to second track.
Optionally, it merges first track and second track obtains intermediate track, specifically include:
First track is adjusted, fusion track is obtained;
According to first track, second track, the fusion track and preset cost function, cost letter is determined
Numerical value, wherein the cost function value characterizes the fusion track difference with first track, second track respectively
The sum of;
Judge whether the cost function value is greater than default third threshold value;
If so, readjusting first track to minimize cost function value as optimization aim, fusion rail is obtained
Mark, up to the number that cost function value is not more than the default third threshold value or obtains merging track reaches the second default iteration
Until number;
If it is not, using obtained fusion track as the intermediate track.
Optionally it is determined that cost function value, specifically includes:
Determine the first average displacement error and the first traveling deflection error of the fusion track and first track;Really
The the second average displacement error and the second driving direction error of fixed the fusion track and second track;
According to the first average displacement error and it is described first traveling deflection error, determine the fusion track with it is described
First comprehensive differences of the first track;According to the second average displacement error and the second driving direction error, institute is determined
State the second comprehensive differences of fusion track and second track;
According to corresponding second weight of corresponding first weight in first track and second track, described is determined
The weighted sum of one comprehensive differences and second comprehensive differences, as cost function value.
Optionally, first weight and obtaining merge track number it is negatively correlated, second weight with merged
The number of track is positively correlated.
Optionally, if first difference value is greater than preset first threshold value, stop travelling, and second track is advised
Cost-effective method continues to travel as currently employed trajectory planning algorithm according to second track.
This specification provides a kind of switching device of trajectory planning algorithm, described device using the first trajectory planning algorithm as
The currently employed trajectory planning algorithm of unmanned machine, described device include:
Computing module, for being travelled in described device according to calculated first track of first trajectory planning algorithm
When, however, it is determined that switching track planning algorithm calculates the second track then by the second trajectory planning algorithm;
Determining module, for determining the first difference value of first track and second track;
Judgment module, for judging whether first difference value is greater than preset first threshold value;
First switching module, when first difference value is not more than preset first threshold value, for merging first rail
Mark and second track obtain intermediate track, continue to travel according to the intermediate track, and whether judge the intermediate track
Meet preset condition, if satisfied, then advising second trajectory planning algorithm track currently employed as the unmanned machine
Cost-effective method controls the unmanned machine and continues to travel according to second track, if not satisfied, then merging first rail again
Mark and second track obtain intermediate track, until obtained intermediate track meets preset condition.
This specification provides a kind of computer readable storage medium, which is characterized in that the storage medium is stored with calculating
Machine program realizes the switching method of trajectory planning algorithm when the computer program is executed by processor.
A kind of unmanned machine that this specification provides, including memory, processor and storage on a memory and can located
The computer program run on reason device, which is characterized in that the processor realizes that above-mentioned trajectory planning is calculated when executing described program
The switching method of method.
This specification embodiment use at least one above-mentioned technical solution can reach it is following the utility model has the advantages that
This specification unmanned machine is in switching track planning algorithm, if switching both front and back trajectory planning algorithm calculates
Track difference it is smaller, then merge two kinds of calculated tracks of trajectory planning algorithm, and according to fusion track traveling, then switch rail
Mark planning algorithm finally travels according to the calculated track of trajectory planning algorithm after switching, effectively improves unmanned machine
The stability switched to trajectory planning algorithm and ride comfort, unmanned machine when significantly improving scene switching are completed under steam
There is the case where serious shake, loss of stability, guarantees the continuity of scene switching.
Detailed description of the invention
The drawings described herein are used to provide a further understanding of the present application, constitutes part of this application, this Shen
Illustrative embodiments and their description please are not constituted an undue limitation on the present application for explaining the application.In the accompanying drawings:
Fig. 1 is the switching method flow chart for the trajectory planning algorithm that this specification embodiment provides;
Fig. 2 is that the first track of fusion that this specification embodiment provides and the second track obtain the method flow of intermediate track
Figure;
Fig. 3 is the schematic diagram that the first track of adjustment that this specification embodiment provides obtains fusion track;
Fig. 4 is a kind of apparatus structure schematic diagram of the switching for trajectory planning algorithm that this specification embodiment provides;
Fig. 5 is the unmanned machine schematic diagram corresponding to Fig. 1 that this specification embodiment provides.
Specific embodiment
To keep the purposes, technical schemes and advantages of this specification clearer, it is embodied below in conjunction with this specification
Technical scheme is clearly and completely described in example and corresponding attached drawing.Obviously, described embodiment is only this Shen
Please a part of the embodiment, instead of all the embodiments.The embodiment of base in this manual, those of ordinary skill in the art exist
Every other embodiment obtained under the premise of creative work is not made, shall fall in the protection scope of this application.
Below in conjunction with attached drawing, the technical scheme provided by various embodiments of the present application will be described in detail.Fig. 1 is this specification embodiment
The switching method of the trajectory planning algorithm of offer, specifically can comprise the following steps that
S100: unmanned machine according to calculated first track of first trajectory planning algorithm when driving, however, it is determined that
Switching track planning algorithm calculates the second track then by the second trajectory planning algorithm.
In the present specification, unmanned machine mainly includes the Intelligent unattendeds steer such as unmanned vehicle, unmanned plane, is mainly used for
Instead of artificial sending objects.Unmanned machine needs to make trajectory planning under steam, comes avoiding obstacles and the road according to planning
Diameter traveling.
Assuming that unmanned machine is calculated the first trajectory planning algorithm as currently employed trajectory planning in a certain scene
Method, and travelled according to calculated first track of first trajectory planning algorithm.In the process of moving, unmanned machine can in real time or
Periodically judge whether to need switching track planning algorithm, however, it is determined that switching track planning algorithm, then according to current scene and
Unmanned machine driving information calculates the second track using the second trajectory planning algorithm.The rule of second track described in this specification
Cost-effective method is the unmanned machine trajectory planning algorithm to be switched to.
Specifically, unmanned machine is when judging whether to need switching track planning algorithm, it can be according to unmanned machine itself
Current location information or driving information judge whether the switching that scene has occurred, if occurrence scene switches, it is determined that need to switch
Trajectory planning algorithm, otherwise determination does not need switching track planning algorithm.For example, can be divided in electronic map in advance such as high
The different scenes such as fast highway scene, urban road scene, industrial area scene, shopping centre scene, residential quarter scene, then nobody sets
It is standby to judge whether itself current scene changes according to current location and electronic map, if so, after according to change
The corresponding relationship of scene and preset each scene and trajectory planning algorithm determines the trajectory planning algorithm for needing to be switched to, no
Then, continue to calculate track using current trajectory planning algorithm.This specification does not do any restrictions to default division scene.
Further, unmanned machine also can determine whether the switching i.e. by occurrence scene, however, it is determined that i.e. by occurrence scene
Switching, it is determined that need switching track planning algorithm, otherwise determination do not need switching track planning algorithm.Specifically, nobody
Equipment can according to current location, the track of current driving and electronic map, judge current location to later scene distance whether
In pre-determined distance, if so, being determined according to the later scene and the corresponding relationship of preset each scene and trajectory planning algorithm
Otherwise the trajectory planning algorithm for needing to be switched to continues to calculate track using current trajectory planning algorithm.
S102: the first difference value of first track and second track is determined.
In above-mentioned steps S100, after unmanned machine determination will be switched to the second trajectory planning algorithm, will wouldn't currently it adopt
Trajectory planning algorithm is switched to the second trajectory planning algorithm, but first according to second trajectory planning algorithm, determines the
Two tracks, then the first track (current driving track) and the second track (track to be switched) totally two can be obtained in unmanned machine at this time
Track.
After obtaining this two tracks, unmanned machine can quantification be identical from the first track and the second track respectively adopts
Sampling point determines the average displacement error (Average of the first track and the second track further according to each sampled point determined
Displacement Error, ADE), the first difference value ADE as the first track and the second trackA.First difference value
ADEACharacterize the difference of the first track and the second track.
Specifically, since there are numerous tracing points for the first track and the second track, it can not be true one by one to all tracing points
Positioning shift error, therefore the identical sampled point of quantification from the first track and the second track respectively, for example, according to preset rule
Set a distance neutralizes from the first track determine N number of sampled point in the second track respectively, or according to regulation time interval, respectively from
First track, which neutralizes, determines N number of sampled point in the second track.
According to each sampled point determined, it may be determined that ith sample in ith sample point and the second track in the first track
The displacement error of point.The average displacement error for determining all sampled points, the institute as first track and second track
State the first difference value ADEA, as shown in formula (1).
In formula (1), xiIndicate the abscissa of ith sample point in first track,Indicate second track
The abscissa of middle ith sample point, yiIndicate the ordinate of ith sample point in first track,Indicate described second
The ordinate of ith sample point in track.
Further, it in practical application scene, when calculating track using trajectory planning algorithm, usually first calculates
Several tracing points, then this several tracing point is sequentially connected to obtain track, therefore, in the present specification, using the first rail
Mark planning algorithm and the second trajectory planning algorithm are when calculating separately the first track and the second track, if calculated first track
Tracing point and the second track tracing point quantity it is identical, then unmanned machine can directly calculate both trajectory planning algorithms
Tracing point out is determined as sampled point.
S104: judging whether first difference value is greater than preset first threshold value, otherwise holds if so, executing step S106
Row step S108.
Preset first threshold value can be stored in advance in unmanned machine, as the first difference value ADEAWhen greater than preset first threshold value, say
The difference of bright first track and the second track is excessive, if unmanned machine direct switching track planning algorithm under steam, that is, be expert at
The second track traveling is directly switch in sailing, then will lead to stationarity seriously reduces, therefore can stop travelling, and by described second
Trajectory planning algorithm continues to travel as currently employed trajectory planning algorithm according to second track, that is, executes step
S106。
As the first difference value ADEAWhen no more than preset first threshold value, illustrate the difference of the first track and the second track compared with
It is small, unmanned machine can switching track planning algorithm under steam, and in order to further increase stability when switching, unmanned machine
The first track can be merged and the second track obtains intermediate track, continue to travel according to the intermediate track, and judge that intermediate track is
It is no to meet preset condition, if satisfied, then using the second trajectory planning algorithm as currently employed trajectory planning algorithm, according to second
Track continues to travel, if not satisfied, the first track is then merged again and the second track obtains intermediate track, until obtained centre
Until track meets preset condition, that is, execute step S108~S112.
S106: stop traveling, and using the second trajectory planning algorithm as currently employed trajectory planning algorithm, according to second
Continue to travel in track.
S104 through the above steps, when determining the first difference value ADEAWhen greater than the preset first threshold value, explanation
The difference of first track and the second track is excessive, if unmanned machine direct switching track planning algorithm under steam, that is, travelling
In be directly switch to the second track traveling, then will lead to stationarity seriously reduces, therefore can be calculated according to first trajectory planning
Regulation, which is drawn, stops traveling.
After stopping traveling, then switching track planning algorithm, it is advised the second trajectory planning algorithm as currently employed track
Cost-effective method finally continues to travel according to second track.
S108: merging first track and second track obtains intermediate track, continues according to the intermediate track
Traveling.
S104 through the above steps, as the first difference value ADEAWhen no more than the preset first threshold value, illustrate
The difference of one track and the second track is smaller, unmanned machine can switching track planning algorithm under steam, and in order to further mention
Stability when height switching, unmanned machine can merge in the process of moving the first track and the second track obtain without stopping traveling
To intermediate track, continue to travel according to the intermediate track.
Specifically, can adjust the first track by figure optimization algorithm, obtain intermediate track, figure optimization algorithm includes but not
It is limited to Elastic Band algorithm.
S110: judging whether the intermediate track meets preset condition, if satisfied, S112 is thened follow the steps, if not satisfied,
Then return step S108.
In the present specification, when unmanned machine judges whether the intermediate track meets preset condition, it may be determined that mid-rail
Second difference value ADE of mark and the second trackBIf the second difference value ADEBIt is excessive, then illustrate the difference of intermediate track and the second track
It is different excessive, be not suitable for directly being switched to the second track from intermediate track during traveling, illustrate that intermediate track is discontented at this time
Sufficient preset condition, if the second difference value ADEBIt is smaller, then illustrate that the difference of intermediate track and the second track is smaller, can travel
During be directly switched to the second track from intermediate track, illustrate that intermediate track meets preset condition at this time.
Specifically, unmanned machine can the identical sampled point of quantification from intermediate track and the second track, then root respectively
According to each sampled point determined, the average displacement error of intermediate track and the second track is determined, as intermediate track and the second rail
Second difference value ADE of markB, judge the second difference value ADEBWhether default second threshold is less than, if being less than, it is determined that institute
It states intermediate track and meets preset condition, step S112 is executed, if being not less than, it is determined that the intermediate track is unsatisfactory for default item
Part, return step S108.Wherein, the method and above-mentioned steps S102 of sampled point are determined from intermediate track and the second track respectively
In from the first track in the second track determine sampled point method it is identical, just no longer repeat one by one here.
S112: using the second trajectory planning algorithm as currently employed trajectory planning algorithm, according to second track after
It continues and sails.
If intermediate track and the second difference value of the second track are less than default second threshold, illustrate intermediate track and second
Track is closer to, and unmanned machine, can be directly in the process of moving by currently employed trajectory planning algorithm without parking at this time
It is switched to the second trajectory planning algorithm, and the track of current driving is switched to the second track by intermediate track.
It should be noted that unmanned machine is in the process of moving, can constantly be worked as according to trajectory planning algorithm, unmanned machine
The information such as the position at preceding place, current road conditions calculate track.Therefore, calculated track is all not quite similar every time.And it is above-mentioned
Fusion shown in step S108~S112 obtains being an iterative process on the process nature of intermediate track, meets if merged out
The intermediate track of preset condition is then travelled according to calculated intermediate track, if not merging out the centre for meeting preset condition
Track, then return step S108 starts next round iteration, that is, merging the first track and the second track again.
In above-mentioned iterative process, the front and back intermediate track that iteration merges out twice is also different, this is because: false
If the first track and the second track that preceding Single cell fusion is based on are unmanned machines in t0Moment is calculated, then Single cell fusion
The first track and the second track being based on are unmanned machines in t1Moment is calculated, as noted previously, as unmanned machine exists
In driving process, the information meter such as position, the current road conditions that can be constantly currently located according to trajectory planning algorithm, unmanned machine
Calculation track, and t0Moment and t1Position where moment unmanned machine and road conditions are different, so front and back is merged in iterative process twice
The first track and the second track that intermediate track is based on are also different, and two obtained intermediate tracks are also just different.
Further, in iterative process shown in above-mentioned steps S108~S112, the condition of iteration is jumped out in addition to including
To other than the intermediate track for meeting preset condition, it may also include the number of iterations and have reached the first default the number of iterations.That is, in step
Before rapid fusion first track S108 and second track obtain intermediate track, unmanned machine is it needs to be determined that obtain centre
The number of track is not more than the first default the number of iterations.
If the number for obtaining intermediate track is greater than the first default the number of iterations, illustrate to be difficult to obtain and the second track difference
Lesser intermediate track can stop iteration at this time, and stop travelling, then using second trajectory planning algorithm as currently adopting
Trajectory planning algorithm continues to travel according to the second track, that is, executes step S106.
Fig. 2 is that the first track of fusion that this specification embodiment provides and the second track obtain the method flow of intermediate track
Figure, as shown in Fig. 2, it specifically may include following step that fusion first track and second track, which obtain the process of intermediate track,
It is rapid:
S1080: first rail is adjusted to minimize cost function value as optimization aim according to preset cost function
Mark obtains fusion track.
Since the first trajectory planning algorithm is substantially optimization algorithm, in the present specification, unmanned machine can be most
Smallization cost function value is optimization aim, adjusts at least one parameter in the first trajectory planning algorithm, reuses adjusting parameter
The first trajectory planning algorithm afterwards calculates track, and obtained track is known as merging track.Cost function value is smaller, and explanation obtains
Fusion track closer to the second track, conversely, obtained fusion track is closer to the first track.Specific cost function
Form will be described below.
Fig. 3 is the schematic diagram that the first track of adjustment that this specification embodiment provides obtains fusion track, as shown in figure 3,
Obtain fusion track into the second track transition process in the first track, fusion track between the first track and the second track it
Between.
S1082: according to the first track, the second track, fusion track and preset cost function, cost function value is determined.
Wherein, cost function value characterization fusion track respectively with the sum of the first track, the difference of the second track.
After S1080 obtains fusion track through the above steps, it may be determined that the first average bit of fusion track and the first track
Shift error and the first traveling deflection error;Determine the second average displacement error and the second traveling side of fusion track and the second track
To error.
Further according to the first average displacement error and the first traveling deflection error, the first of fusion track and the first track is determined
Comprehensive differences;According to the second average displacement error and the second driving direction error, the second of fusion track and the second track is determined
Comprehensive differences.
As shown in formula (2), the first comprehensive differences f (k) are as follows:
In formula (2), x 'iIndicate the abscissa of ith sample point in fusion track, y 'iIt indicates in fusion track i-th
The ordinate of sampled point, βiIndicate the driving direction of ith sample point in the first track, β 'iIt indicates to adopt for i-th in intermediate track
The driving direction of sampling point.
Analogy formula (2), as shown in formula (3), the second comprehensive differences f (l) are as follows:
In formula (3),Indicate the driving direction of ith sample point in the second track.
Wherein, the method and above-mentioned steps S102 of sampled point are determined from the first track, the second track, fusion track respectively
In from the first track in the second track determine sampled point method it is identical, just no longer repeat one by one here.
Further, according to corresponding second weight of corresponding first weight in the first track and the second track, first is determined
The weighted sum of comprehensive differences and the second comprehensive differences, as cost function value.
As shown in formula (4), after merging for the first time, cost function cost are as follows:
In formula (4), α1For the initial value of the first weight, α2For the initial value of the second weight.
Further, the first weight and obtaining merge track number it is negatively correlated, the second weight with obtain merging track
Number is positively correlated, i.e., as the number of iterations of step S1080~S1086 gradually increases, the first weight is gradually reduced, the second weight
It is gradually increased, thus obtains formula (5).
In formula (5), αAFor the first weight, αBFor the second weight.
To minimize cost function value as optimization aim, at least one parameter in the first trajectory planning algorithm is adjusted, is made
The value for obtaining cost function cost is gradually reduced.Due to only merging track in the case where being cost function with above-mentioned formula (5)
Closer to the second track (that is, f (l) is smaller), the value of cost function cost just can be smaller, therefore, to minimize cost function value
For optimization aim, when adjusting the first trajectory planning algorithm, during continuous iteration, the first comprehensive differences f (k) gradually increases
Greatly, fusion track is gradually deviated from the first track, and the second comprehensive differences f (l) is gradually reduced, and fusion track moves closer to the second rail
Mark finally obtains the fusion of close enough second track so that fusion track be made gradually to migrate from the first track to the second track
Track.
Further, in practical application scene, distance weighting γ can be set to each sampled point, distance weighting can be deposited
Storage is in unmanned machine.Specifically, can according to sampled point at a distance from unmanned machine current location, preset each sampled point away from
From weight γ, sampled point is closer apart from unmanned machine current location, and distance weighting γ is bigger, and sampled point is current apart from unmanned machine
Position is remoter, and distance weighting γ is smaller.
Therefore, as shown in formula (6), cost function cost be may be expressed as:
In formula (6), γkFor the first track and the distance weighting for merging k-th of sampled point in track, γlTo merge track
With the distance weighting of first of sampled point in the second track.
In addition, in the present specification, directly can also regard f (l) as cost function, then still can be minimized cost letter at this time
Numerical value adjusts the first track, obtains the fusion track of close enough second track.
S1084: judging whether cost function value is greater than default third threshold value, if so, S1080 is returned to step, otherwise,
Execute step S1086.
Default third threshold value can be stored in advance in unmanned machine, and when cost function value is greater than default third threshold value, explanation is melted
The difference for closing the second track of track is excessive, in order to make to merge track between the first track and the second track, gradually to second
Track migration, unmanned machine need to be adjusted according to preset cost function to minimize cost function value as optimization aim again
First track obtains fusion track, that is, return to step S1080.
When cost function value is not more than default third threshold value, illustrate the difference for merging track and the first track, the second track
It is different smaller, track is merged between the first track and the second track, and unmanned machine can be using obtained fusion track as described in
Intermediate track, that is, execute step S1086.
S1086: using obtained fusion track as the intermediate track.
Further, in iterative process shown in above-mentioned steps S1080~S1086, the condition of iteration is jumped out in addition to including
It obtains meeting other than fusion track of the cost function value no more than default third threshold value, may also include to obtain the number of fusion track
Have reached the second default the number of iterations.
If the number for obtaining fusion track is greater than the second default the number of iterations, stop iteration, and by last time iteration
Obtained fusion track is as intermediate track, that is, executes step S1086.
Fig. 4 is a kind of apparatus structure schematic diagram of the switching for trajectory planning algorithm that this specification embodiment provides, described
The device trajectory planning algorithm that the first trajectory planning algorithm is currently employed as unmanned machine, described device include:
Computing module 401 is used in described device according to the calculated first track row of first trajectory planning algorithm
When sailing, however, it is determined that switching track planning algorithm calculates the second track then by the second trajectory planning algorithm;
Determining module 402, for determining the first difference value of first track and second track;
Judgment module 403, for judging whether first difference value is greater than preset first threshold value;
First switching module 404, when first difference value is not more than preset first threshold value, for merging described first
Track and second track obtain intermediate track, continue to travel according to the intermediate track, and judge that the intermediate track is
It is no to meet preset condition, if satisfied, the then track that second trajectory planning algorithm is currently employed as the unmanned machine
Planning algorithm controls the unmanned machine and continues to travel according to second track, if not satisfied, then merging described first again
Track and second track obtain intermediate track, until obtained intermediate track meets preset condition.
Optionally it is determined that module 402 is specifically used for, the quantification from first track and second track respectively
Identical sampled point;According to each sampled point determined, determine that the average displacement of first track and second track misses
Difference, the first difference value as first track and second track.
Optionally, judgment module 403 is specifically used for, respectively the quantification from the intermediate track and second track
Identical sampled point;According to each sampled point determined, determine that the average displacement of the intermediate track and second track misses
Difference, the second difference value as the intermediate track and second track;It is default to judge whether second difference value is less than
Second threshold, if being less than, it is determined that the intermediate track meets preset condition, if being not less than, it is determined that the intermediate track is not
Meet preset condition.
Optionally, the first switching module 404 is also used to, and merges first track and second track obtains mid-rail
Before mark, determine the number for obtaining intermediate track no more than the first default the number of iterations;If the number for obtaining intermediate track is greater than
First default the number of iterations, then stop travelling, and calculates second trajectory planning algorithm as currently employed trajectory planning
Method continues to travel according to second track.
Optionally, the first switching module 404 is specifically used for, and adjusts first track, obtains fusion track;According to described
First track, second track, the fusion track and preset cost function, determine cost function value, wherein the generation
Valence functional value characterize the fusion track respectively with the sum of first track, the difference of second track;Judge the generation
Whether valence functional value is greater than default third threshold value;If so, to minimize cost function value as optimization aim, described in readjustment
First track obtains fusion track, until cost function value is no more than the default third threshold value or obtains fusion track
Until number reaches the second default the number of iterations;If it is not, using obtained fusion track as the intermediate track.
Optionally, the first switching module 404 is specifically used for, and determines that the first of fusion track and first track is flat
Equal displacement error and the first traveling deflection error;Determine the second average displacement error of the fusion track and second track
With the second driving direction error;According to the first average displacement error and the first traveling deflection error, melt described in determination
Close the first comprehensive differences of track and first track;According to the second average displacement error and second driving direction
Error determines the second comprehensive differences of the fusion track and second track;According to first track corresponding first
Corresponding second weight of weight and second track, determines the weighting of first comprehensive differences and second comprehensive differences
And value, as cost function value.
Optionally, first weight and obtaining merge track number it is negatively correlated, second weight with merged
The number of track is positively correlated.
Optionally, described device further include: the second switching module 405 is greater than default first threshold in first difference value
When value, stop traveling for controlling the unmanned machine, and work as using second trajectory planning algorithm as the unmanned machine
The trajectory planning algorithm of preceding use controls the unmanned machine and continues to travel according to second track.
This specification embodiment additionally provides a kind of computer readable storage medium, which is stored with computer journey
Sequence, computer program can be used for executing the switching method for the trajectory planning algorithm that above-mentioned Fig. 1 is provided.
Based on the switching method of trajectory planning algorithm shown in FIG. 1, this specification embodiment also proposed nothing shown in fig. 5
The schematic configuration diagram of people's equipment.Such as Fig. 5, in hardware view, which includes processor, internal bus, network interface, interior
It deposits and nonvolatile memory, is also possible that hardware required for other business certainly.Processor is from non-volatile memories
It reads corresponding computer program in device then to run into memory, to realize cutting for trajectory planning algorithm described in above-mentioned Fig. 1
Change method.
Certainly, other than software realization mode, other implementations, such as logical device suppression is not precluded in this specification
Or mode of software and hardware combining etc., that is to say, that the executing subject of following process flow is not limited to each logic unit,
It is also possible to hardware or logical device.
In the 1990s, the improvement of a technology can be distinguished clearly be on hardware improvement (for example,
Improvement to circuit structures such as diode, transistor, switches) or software on improvement (improvement for method flow).So
And with the development of technology, the improvement of current many method flows can be considered as directly improving for hardware circuit.
Designer nearly all obtains corresponding hardware circuit by the way that improved method flow to be programmed into hardware circuit.Cause
This, it cannot be said that the improvement of a method flow cannot be realized with hardware entities module.For example, programmable logic device
(Programmable Logic Device, PLD) (such as field programmable gate array (Field Programmable Gate
Array, FPGA)) it is exactly such a integrated circuit, logic function determines device programming by user.By designer
Voluntarily programming comes a digital display circuit " integrated " on a piece of PLD, designs and makes without asking chip maker
Dedicated IC chip.Moreover, nowadays, substitution manually makes IC chip, this programming is also used instead mostly " is patrolled
Volume compiler (logic compiler) " software realizes that software compiler used is similar when it writes with program development,
And the source code before compiling also write by handy specific programming language, this is referred to as hardware description language
(Hardware Description Language, HDL), and HDL is also not only a kind of, but there are many kind, such as ABEL
(Advanced Boolean Expression Language)、AHDL(Altera Hardware Description
Language)、Confluence、CUPL(Cornell University Programming Language)、HDCal、JHDL
(Java Hardware Description Language)、Lava、Lola、MyHDL、PALASM、RHDL(Ruby
Hardware Description Language) etc., VHDL (Very-High-Speed is most generally used at present
Integrated Circuit Hardware Description Language) and Verilog.Those skilled in the art also answer
This understands, it is only necessary to method flow slightly programming in logic and is programmed into integrated circuit with above-mentioned several hardware description languages,
The hardware circuit for realizing the logical method process can be readily available.
Controller can be implemented in any suitable manner, for example, controller can take such as microprocessor or processing
The computer for the computer readable program code (such as software or firmware) that device and storage can be executed by (micro-) processor can
Read medium, logic gate, switch, specific integrated circuit (Application Specific Integrated Circuit,
ASIC), the form of programmable logic controller (PLC) and insertion microcontroller, the example of controller includes but is not limited to following microcontroller
Device: ARC 625D, Atmel AT91SAM, Microchip PIC18F26K20 and Silicone Labs C8051F320 are deposited
Memory controller is also implemented as a part of the control logic of memory.It is also known in the art that in addition to
Pure computer readable program code mode is realized other than controller, can be made completely by the way that method and step is carried out programming in logic
Controller is obtained to come in fact in the form of logic gate, switch, specific integrated circuit, programmable logic controller (PLC) and insertion microcontroller etc.
Existing identical function.Therefore this controller is considered a kind of hardware component, and to including for realizing various in it
The device of function can also be considered as the structure in hardware component.Or even, it can will be regarded for realizing the device of various functions
For either the software module of implementation method can be the structure in hardware component again.
System, device, module or the unit that above-described embodiment illustrates can specifically realize by computer chip or entity,
Or it is realized by the product with certain function.It is a kind of typically to realize that equipment is computer.Specifically, computer for example may be used
Think personal computer, laptop computer, cellular phone, camera phone, smart phone, personal digital assistant, media play
It is any in device, navigation equipment, electronic mail equipment, game console, tablet computer, wearable device or these equipment
The combination of equipment.
For convenience of description, it is divided into various units when description apparatus above with function to describe respectively.Certainly, implementing this
The function of each unit can be realized in the same or multiple software and or hardware when specification.
It should be understood by those skilled in the art that, the embodiment of the present invention can provide as method, system or computer program
Product.Therefore, complete hardware embodiment, complete software embodiment or reality combining software and hardware aspects can be used in the present invention
Apply the form of example.Moreover, it wherein includes the computer of computer usable program code that the present invention, which can be used in one or more,
The computer program implemented in usable storage medium (including but not limited to magnetic disk storage, CD-ROM, optical memory etc.) produces
The form of product.
The present invention be referring to according to the method for the embodiment of the present invention, the process of equipment (system) and computer program product
Figure and/or block diagram describe.It should be understood that every one stream in flowchart and/or the block diagram can be realized by computer program instructions
The combination of process and/or box in journey and/or box and flowchart and/or the block diagram.It can provide these computer programs
Instruct the processor of general purpose computer, special purpose computer, Embedded Processor or other programmable data processing devices to produce
A raw machine, so that being generated by the instruction that computer or the processor of other programmable data processing devices execute for real
The device for the function of being specified in present one or more flows of the flowchart and/or one or more blocks of the block diagram.
These computer program instructions, which may also be stored in, is able to guide computer or other programmable data processing devices with spy
Determine in the computer-readable memory that mode works, so that it includes referring to that instruction stored in the computer readable memory, which generates,
Enable the manufacture of device, the command device realize in one box of one or more flows of the flowchart and/or block diagram or
The function of being specified in multiple boxes.
These computer program instructions also can be loaded onto a computer or other programmable data processing device, so that counting
Series of operation steps are executed on calculation machine or other programmable devices to generate computer implemented processing, thus in computer or
The instruction executed on other programmable devices is provided for realizing in one or more flows of the flowchart and/or block diagram one
The step of function of being specified in a box or multiple boxes.
In a typical configuration, calculating equipment includes one or more processors (CPU), input/output interface, net
Network interface and memory.
Memory may include the non-volatile memory in computer-readable medium, random access memory (RAM) and/or
The forms such as Nonvolatile memory, such as read-only memory (ROM) or flash memory (flash RAM).Memory is computer-readable medium
Example.
Computer-readable medium includes permanent and non-permanent, removable and non-removable media can be by any method
Or technology come realize information store.Information can be computer readable instructions, data structure, the module of program or other data.
The example of the storage medium of computer includes, but are not limited to phase change memory (PRAM), static random access memory (SRAM), moves
State random access memory (DRAM), other kinds of random access memory (RAM), read-only memory (ROM), electric erasable
Programmable read only memory (EEPROM), flash memory or other memory techniques, read-only disc read only memory (CD-ROM) (CD-ROM),
Digital versatile disc (DVD) or other optical storage, magnetic cassettes, tape magnetic disk storage or other magnetic storage devices
Or any other non-transmission medium, can be used for storage can be accessed by a computing device information.As defined in this article, it calculates
Machine readable medium does not include temporary computer readable media (transitory media), such as the data-signal and carrier wave of modulation.
It should also be noted that, the terms "include", "comprise" or its any other variant are intended to nonexcludability
It include so that the process, method, commodity or the equipment that include a series of elements not only include those elements, but also to wrap
Include other elements that are not explicitly listed, or further include for this process, method, commodity or equipment intrinsic want
Element.In the absence of more restrictions, the element limited by sentence "including a ...", it is not excluded that including described want
There is also other identical elements in the process, method of element, commodity or equipment.
It will be understood by those skilled in the art that the embodiment of this specification can provide as the production of method, system or computer program
Product.Therefore, complete hardware embodiment, complete software embodiment or implementation combining software and hardware aspects can be used in this specification
The form of example.Moreover, it wherein includes the computer of computer usable program code that this specification, which can be used in one or more,
The computer program implemented in usable storage medium (including but not limited to magnetic disk storage, CD-ROM, optical memory etc.) produces
The form of product.
This specification can describe in the general context of computer-executable instructions executed by a computer, such as journey
Sequence module.Generally, program module include routines performing specific tasks or implementing specific abstract data types, programs, objects,
Component, data structure etc..This specification can also be practiced in a distributed computing environment, in these distributed computing environment
In, by executing task by the connected remote processing devices of communication network.In a distributed computing environment, program module
It can be located in the local and remote computer storage media including storage equipment.
All the embodiments in this specification are described in a progressive manner, same and similar portion between each embodiment
Dividing may refer to each other, and each embodiment focuses on the differences from other embodiments.Especially for system reality
For applying example, since it is substantially similar to the method embodiment, so being described relatively simple, related place is referring to embodiment of the method
Part explanation.
The foregoing is merely the embodiments of this specification, are not limited to this specification.For art technology
For personnel, this specification can have various modifications and variations.It is all made any within the spirit and principle of this specification
Modification, equivalent replacement, improvement etc., should be included within the scope of the claims of this specification.
Claims (11)
1. a kind of switching method of trajectory planning algorithm, which is characterized in that unmanned machine is using the first trajectory planning algorithm as working as
The trajectory planning algorithm of preceding use, which comprises
Unmanned machine according to calculated first track of first trajectory planning algorithm when driving, however, it is determined that switching track rule
Cost-effective method calculates the second track then by the second trajectory planning algorithm;
Determine the first difference value of first track and second track;
If first difference value is not more than preset first threshold value, first track is merged and during second track obtains
Between track, continue to travel according to the intermediate track, and judge whether the intermediate track meets preset condition, if satisfied, then
Using second trajectory planning algorithm as currently employed trajectory planning algorithm, continue to travel according to second track, if
It is unsatisfactory for, then merges first track again and second track obtains intermediate track, until obtained intermediate track is full
Until sufficient preset condition.
2. the method as described in claim 1, which is characterized in that determine the first poor of first track and second track
Different value, specifically includes:
The identical sampled point of quantification from first track and second track respectively;
According to each sampled point determined, the average displacement error of first track and second track is determined, as institute
State the first difference value of the first track and second track.
3. the method as described in claim 1, which is characterized in that judge whether the intermediate track meets preset condition, specifically
Include:
The identical sampled point of quantification from the intermediate track and second track respectively;
According to each sampled point determined, the average displacement error of the intermediate track and second track is determined, as institute
State the second difference value of intermediate track and second track;
Judge whether second difference value is less than default second threshold, if being less than, it is determined that the intermediate track meets default
Condition, if being not less than, it is determined that the intermediate track is unsatisfactory for preset condition.
4. the method as described in claim 1, which is characterized in that merge first track and second track obtains centre
Before track, the method also includes:
Determine the number for obtaining intermediate track no more than the first default the number of iterations;
If the number for obtaining intermediate track is greater than the first default the number of iterations, stop travelling, and by second trajectory planning
Algorithm continues to travel as currently employed trajectory planning algorithm according to second track.
5. the method as described in claim 1, which is characterized in that merge first track and second track obtains centre
Track specifically includes:
First track is adjusted, fusion track is obtained;
According to first track, second track, the fusion track and preset cost function, cost function is determined
Value, wherein the cost function value characterize it is described fusion track respectively with first track, second track difference it
With;
Judge whether the cost function value is greater than default third threshold value;
If so, readjusting first track to minimize cost function value as optimization aim, fusion track is obtained, directly
The number for being not more than the default third threshold value to cost function value or obtaining fusion track reaches the second default the number of iterations
Until;
If it is not, using obtained fusion track as the intermediate track.
6. method as claimed in claim 5, which is characterized in that determine cost function value, specifically include:
Determine the first average displacement error and the first traveling deflection error of the fusion track and first track;Determine institute
State the second average displacement error and the second driving direction error of fusion track and second track;
According to the first average displacement error and the first traveling deflection error, the fusion track and described first is determined
First comprehensive differences of track;According to the second average displacement error and the second driving direction error, melt described in determination
Close the second comprehensive differences of track and second track;
According to corresponding second weight of corresponding first weight in first track and second track, determine that described first is comprehensive
The weighted sum for closing difference and second comprehensive differences, as cost function value.
7. method as claimed in claim 6, which is characterized in that first weight and the number negative for obtaining merging track
It closes, second weight is positively correlated with the number for obtaining merging track.
8. the method as described in claim 1, which is characterized in that if first difference value is greater than preset first threshold value, institute
State method further include:
Stop traveling, and using second trajectory planning algorithm as currently employed trajectory planning algorithm, according to described second
Continue to travel in track.
9. a kind of switching device of trajectory planning algorithm, which is characterized in that described device is using the first trajectory planning algorithm as nothing
The currently employed trajectory planning algorithm of people's equipment, described device include:
Computing module, for described device according to calculated first track of first trajectory planning algorithm when driving, if
It determines switching track planning algorithm, then by the second trajectory planning algorithm, calculates the second track;
Determining module, for determining the first difference value of first track and second track;
Judgment module, for judging whether first difference value is greater than preset first threshold value;
First switching module, first difference value be not more than preset first threshold value when, for merge first track and
Second track obtains intermediate track, continues to travel according to the intermediate track, and judges whether the intermediate track meets
Preset condition, if satisfied, then calculating second trajectory planning algorithm trajectory planning currently employed as the unmanned machine
Method controls the unmanned machine and continues to travel according to second track, if not satisfied, then merge again first track and
Second track obtains intermediate track, until obtained intermediate track meets preset condition.
10. a kind of computer readable storage medium, which is characterized in that the storage medium is stored with computer program, the meter
The claims 1-8 any method is realized when calculation machine program is executed by processor.
11. a kind of unmanned machine including memory, processor and stores the calculating that can be run on a memory and on a processor
Machine program, which is characterized in that the processor realizes the claims 1-8 any method when executing described program.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910703173.2A CN110400023A (en) | 2019-07-31 | 2019-07-31 | A kind of switching method and device of trajectory planning algorithm |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910703173.2A CN110400023A (en) | 2019-07-31 | 2019-07-31 | A kind of switching method and device of trajectory planning algorithm |
Publications (1)
Publication Number | Publication Date |
---|---|
CN110400023A true CN110400023A (en) | 2019-11-01 |
Family
ID=68327048
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910703173.2A Pending CN110400023A (en) | 2019-07-31 | 2019-07-31 | A kind of switching method and device of trajectory planning algorithm |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110400023A (en) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112673406A (en) * | 2020-05-29 | 2021-04-16 | 华为技术有限公司 | Method and terminal device for identifying abnormal vehicle parameters in vehicle queue |
CN113009884A (en) * | 2019-12-19 | 2021-06-22 | 广州极飞科技股份有限公司 | Method, device, equipment and storage medium for controlling operation of unmanned equipment |
CN113771851A (en) * | 2020-05-20 | 2021-12-10 | 杭州海康威视数字技术股份有限公司 | Trajectory planning method and device |
CN114136331A (en) * | 2021-11-23 | 2022-03-04 | 常熟理工学院 | Driving habit evaluation method and system based on micro-electromechanical gyroscope and detection equipment |
-
2019
- 2019-07-31 CN CN201910703173.2A patent/CN110400023A/en active Pending
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113009884A (en) * | 2019-12-19 | 2021-06-22 | 广州极飞科技股份有限公司 | Method, device, equipment and storage medium for controlling operation of unmanned equipment |
CN113771851A (en) * | 2020-05-20 | 2021-12-10 | 杭州海康威视数字技术股份有限公司 | Trajectory planning method and device |
CN112673406A (en) * | 2020-05-29 | 2021-04-16 | 华为技术有限公司 | Method and terminal device for identifying abnormal vehicle parameters in vehicle queue |
CN114136331A (en) * | 2021-11-23 | 2022-03-04 | 常熟理工学院 | Driving habit evaluation method and system based on micro-electromechanical gyroscope and detection equipment |
CN114136331B (en) * | 2021-11-23 | 2024-07-19 | 常熟理工学院 | Driving habit evaluation method, system and detection equipment based on micro-electromechanical gyroscope |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110400023A (en) | A kind of switching method and device of trajectory planning algorithm | |
CN111114543B (en) | Trajectory prediction method and device | |
CN111190427B (en) | Method and device for planning track | |
CN110989636A (en) | Method and device for predicting track of obstacle | |
CN112306059B (en) | Training method, control method and device for control model | |
CN113419547B (en) | Multi-vehicle cooperative control method and device | |
CN112947495B (en) | Model training method, unmanned equipment control method and device | |
CN111090286B (en) | Unmanned vehicle motion state planning method and device | |
CN108600913A (en) | Whistle volume automatic control method, system, onboard control device and storage medium | |
CN113074748B (en) | Path planning method and device for unmanned equipment | |
CN111238523A (en) | Method and device for predicting motion trail | |
CN108583417A (en) | Track projecting method, track optical projection system, projection terminal and storage medium | |
CN111062372A (en) | Method and device for predicting obstacle track | |
CN110322054A (en) | A kind of optimization distribution method of highway section Traffic monitoring device | |
CN115762139B (en) | Method, device, equipment and storage medium for filtering intersection prediction track | |
CN113033527A (en) | Scene recognition method and device, storage medium and unmanned equipment | |
CN105711417A (en) | Electric vehicle speed-limiting system based on lane distinguishing | |
CN116373876A (en) | Method and device for correcting predicted running time of vehicle and vehicle | |
CN113848913B (en) | Control method and control device of unmanned equipment | |
CN110399582A (en) | A kind of method and device of page presentation | |
Naderi et al. | Optimal orientation for automated vehicles on large lane-free roundabouts | |
CN114167857B (en) | Control method and device of unmanned equipment | |
CN113815651B (en) | Unmanned equipment control method, unmanned equipment control device, unmanned equipment control equipment and storage medium | |
Mansour et al. | Towards traffic congestion-free through intelligent traffic control system | |
CN108764632A (en) | A kind of risk control method, device and equipment |
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 |