CN114859941A - Speed planning method under curved smooth path and storage medium - Google Patents

Speed planning method under curved smooth path and storage medium Download PDF

Info

Publication number
CN114859941A
CN114859941A CN202210781025.4A CN202210781025A CN114859941A CN 114859941 A CN114859941 A CN 114859941A CN 202210781025 A CN202210781025 A CN 202210781025A CN 114859941 A CN114859941 A CN 114859941A
Authority
CN
China
Prior art keywords
speed
path
robot
acceleration
point
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
Application number
CN202210781025.4A
Other languages
Chinese (zh)
Other versions
CN114859941B (en
Inventor
王为科
赵越
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Xiangong Intelligent Technology Co ltd
Original Assignee
Shanghai Xiangong Intelligent Technology Co ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shanghai Xiangong Intelligent Technology Co ltd filed Critical Shanghai Xiangong Intelligent Technology Co ltd
Priority to CN202210781025.4A priority Critical patent/CN114859941B/en
Publication of CN114859941A publication Critical patent/CN114859941A/en
Application granted granted Critical
Publication of CN114859941B publication Critical patent/CN114859941B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle

Abstract

The invention relates to a speed planning method under a curved smooth path and a storage medium, wherein the method comprises the following steps: step 1, acquiring corresponding information including speed, curvature and curvature radius of each path point on a planned path of a robot, calculating the maximum speed allowed by each path point under the constraint of centripetal force, and taking the minimum distance between the maximum speed and the maximum movement speed as the speed constraint of each path point to generate a speed space; step 2, traversing the speed constraints of all path points in the speed space to form a maximum speed upper bound curve, and taking the speed valley point of the maximum speed upper bound curve to generate a point index column to be planned; step 3, in the index array of the points to be planned, the index arrays of two adjacent path points are sequentially extracted, and the speed information of the path point with a smaller index array value is used as the initial speed of the planning
Figure 735719DEST_PATH_IMAGE002
Greater termination rate as a plan
Figure 127966DEST_PATH_IMAGE004
For S-shaped speed planning, and updating the planning path according to the planning result. Therefore, the centripetal acceleration constraint during bending and the self kinematics constraint thereof are satisfied, and the continuity of the acceleration is considered.

Description

Speed planning method under curved smooth path and storage medium
Technical Field
The invention relates to the technical field of mobile robot navigation, in particular to an improved speed planning algorithm under a curved smooth path based on an S-shaped speed planning algorithm, which is suitable for the constraint of the curved smooth path on robot speed planning.
Background
In the navigation process of the mobile robot, for a straight-line path, the field usually uses an S-shaped velocity planning algorithm to plan the reference velocity of a path point. Wherein the sigmoid velocity planning algorithm is a velocity planning algorithm that can increase an initial velocity sigmoid to a maximum velocity and decrease the initial velocity sigmoid from the maximum velocity sigmoid to an end velocity by decomposing the velocity into: 7 time intervals such as an acceleration adding section, a uniform acceleration section, an acceleration reducing section, a constant speed section, an acceleration and deceleration section, a uniform deceleration section and a deceleration reducing section are respectively calculated, and the characteristics of smoothness and continuous acceleration of the planned reference speed can be ensured.
However, when the navigation path of the mobile robot includes a curved path, if the reference speed is simply planned by using the S-shaped speed planning algorithm, the planned speed may not satisfy the centripetal acceleration constraint when the robot passes a curve, and the robot may drift when passing a curve.
Disclosure of Invention
Therefore, the main objective of the present invention is to provide a speed planning method and a storage medium under a curved smooth path, so as to satisfy the centripetal acceleration constraint and the self-kinematics constraint when the mobile robot passes a curve, and simultaneously consider the continuity of the acceleration.
To achieve the above object, according to one aspect of the present invention, there is provided a speed planning method under a curved smooth path, the steps including:
step 1, acquiring corresponding information including speed, curvature and curvature radius of each path point on a planned path of the robot so as to calculate the maximum speed allowed by each path point under the constraint of centripetal force of the robot
Figure 997003DEST_PATH_IMAGE001
And supply the maximum movement speed of the robot
Figure 978866DEST_PATH_IMAGE002
Taking small between them as the speed constraint of each path point
Figure 721694DEST_PATH_IMAGE003
For generating a velocity space;
step 2 traversing speed constraint of all path points in speed space
Figure 945740DEST_PATH_IMAGE003
Forming a maximum speed upper bound curve and taking a speed valley point to generate a point index row to be planned;
step 3, in the index rows of the points to be planned, the index rows of two adjacent path points are sequentially extracted, and the speed information of the path point with smaller index row value is used as the initial speed of the planning
Figure 457623DEST_PATH_IMAGE004
The speed information of the path point with larger index column value is used as the end speed of the plan
Figure 875966DEST_PATH_IMAGE005
And carrying out speed planning according to a speed curve algorithm, and updating a planned path according to a planning result.
In a possible preferred embodiment, step 3 further comprises: checking whether the speed value corresponding to each path point exceeds the speed constraint
Figure 341976DEST_PATH_IMAGE003
If the speed exceeds the preset speed limit value, the upper limit value of the speed of the current segment is programmed
Figure 871178DEST_PATH_IMAGE002
And (5) after the reduction, performing speed planning again until the requirement of speed constraint is met.
In a possible preferred embodiment, the step of generating the planned point index column includes: index column of subscript corresponding to speed valley point of maximum speed upper bound curve
Figure 503147DEST_PATH_IMAGE006
And adding the starting point
Figure 826812DEST_PATH_IMAGE007
And index column for subscript of end point
Figure 308347DEST_PATH_IMAGE008
To form an index column of points to be planned
Figure 375660DEST_PATH_IMAGE009
。
In a possible preferred embodiment, wherein the speed plan in step 3 is an S-shaped speed plan, the steps comprise: the velocity trajectory is divided into:
Figure 862136DEST_PATH_IMAGE010
the acceleration period is added to the acceleration period,
Figure 123747DEST_PATH_IMAGE011
the period of uniform acceleration is set to be,
Figure 62884DEST_PATH_IMAGE012
the acceleration period is reduced and the time period of acceleration,
Figure 668309DEST_PATH_IMAGE013
at a constant speed time interval,
Figure 274871DEST_PATH_IMAGE014
the time period of acceleration and deceleration is set,
Figure 704453DEST_PATH_IMAGE015
the period of uniform deceleration is set to be,
Figure 865307DEST_PATH_IMAGE016
during the deceleration period, the maximum acceleration which can be reached by the robot without considering the limitation of acceleration and deceleration is calculated
Figure 805581DEST_PATH_IMAGE017
With maximum deceleration
Figure 502535DEST_PATH_IMAGE018
：
Figure 604483DEST_PATH_IMAGE019
Figure 252633DEST_PATH_IMAGE020
Wherein
Figure 963975DEST_PATH_IMAGE021
The maximum acceleration of the robot is obtained;
according to maximum acceleration of robot
Figure 13971DEST_PATH_IMAGE022
And maximum deceleration
Figure 552399DEST_PATH_IMAGE023
And
Figure 953425DEST_PATH_IMAGE024
and
Figure 459668DEST_PATH_IMAGE018
is calculated from the relationship of
Figure 629750DEST_PATH_IMAGE025
，
Figure 807921DEST_PATH_IMAGE026
：
If it is
Figure 460357DEST_PATH_IMAGE027
Then, then
Figure 14966DEST_PATH_IMAGE028
，
Figure 305134DEST_PATH_IMAGE029
，
Figure 654206DEST_PATH_IMAGE030
If it is
Figure 265710DEST_PATH_IMAGE031
Then, then
Figure 624010DEST_PATH_IMAGE032
，
Figure 768683DEST_PATH_IMAGE033
，
Figure 318351DEST_PATH_IMAGE034
And update
Figure 915686DEST_PATH_IMAGE035
If it is
Figure 77677DEST_PATH_IMAGE036
，Then
Figure 76857DEST_PATH_IMAGE037
，
Figure 534776DEST_PATH_IMAGE038
，
Figure 353828DEST_PATH_IMAGE039
If it is
Figure 53931DEST_PATH_IMAGE040
，Then
Figure 671731DEST_PATH_IMAGE041
，
Figure 799087DEST_PATH_IMAGE042
，
Figure 636593DEST_PATH_IMAGE043
，And update
Figure 376273DEST_PATH_IMAGE044
Further calculating the acceleration segment distance
Figure 84466DEST_PATH_IMAGE045
And the distance of the deceleration section
Figure 648302DEST_PATH_IMAGE046
：
Figure 441946DEST_PATH_IMAGE047
Figure 716807DEST_PATH_IMAGE048
Determining the total length of the track
Figure 545086DEST_PATH_IMAGE049
And
Figure 279824DEST_PATH_IMAGE045
and
Figure 886447DEST_PATH_IMAGE046
the relationship of (1):
Figure 732043DEST_PATH_IMAGE050
if not, will
Figure 414828DEST_PATH_IMAGE002
After the reduction, each time interval and each route are recalculated until the requirement of speed constraint is met, and if the requirement of the speed constraint is met, the routes of the uniform speed segments are calculated
Figure 320468DEST_PATH_IMAGE051
And
Figure 587239DEST_PATH_IMAGE013
：
Figure 236526DEST_PATH_IMAGE052
Figure 304976DEST_PATH_IMAGE053
。
in a possible preferred embodiment, wherein the maximum speed
Figure 617402DEST_PATH_IMAGE001
The calculating step comprises: calculate each path point
Figure 872934DEST_PATH_IMAGE054
Corresponding first derivative
Figure 60333DEST_PATH_IMAGE055
Second derivative of
Figure 717710DEST_PATH_IMAGE056
Curvature of curvature
Figure 463687DEST_PATH_IMAGE057
And radius of curvature
Figure 940936DEST_PATH_IMAGE058
The information of (2):
Figure 932026DEST_PATH_IMAGE059
Figure 945374DEST_PATH_IMAGE060
Figure 98138DEST_PATH_IMAGE061
Figure 593842DEST_PATH_IMAGE062
according to the formula of circular motion:
Figure 857464DEST_PATH_IMAGE063
to obtain the acceleration of the robot
Figure 253548DEST_PATH_IMAGE064
Post-calculation
Figure 577213DEST_PATH_IMAGE065
Wherein
Figure 294633DEST_PATH_IMAGE066
Is the centripetal force of the robot,
Figure 863411DEST_PATH_IMAGE067
in order to be the quality of the robot,
Figure 615467DEST_PATH_IMAGE068
in order to accelerate the robot, the robot is provided with a plurality of wheels,
Figure 110033DEST_PATH_IMAGE069
is the current speed of the robot and is,
Figure 49170DEST_PATH_IMAGE070
is the radius of the circular motion of the robot.
To achieve the above object, according to another aspect of the present invention, there is also provided a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a computer processor, performs the steps of any of the above speed planning methods under a curved smooth path.
The speed planning method and the storage medium under the curved smooth path provided by the invention can provide a planning scheme which not only meets the physical condition constraint, but also can realize stable and rapid speed passing when the mobile robot runs on the curved path, and aims to meet the centripetal acceleration constraint and the self kinematics constraint when the mobile robot passes through a curve, and simultaneously consider the continuity of the acceleration, thereby avoiding the drift of the robot when the robot passes through the curve and being beneficial to improving the precision and the efficiency of the robot track tracking process.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments of the invention and, together with the description, serve to explain the invention and not to limit the invention. In the drawings:
FIG. 1 is a schematic representation of the steps of the velocity planning method of the present invention under a curved smooth path;
FIG. 2 is a schematic diagram of the calculation steps of the S-shaped velocity plan in the velocity planning method under the curved smooth path according to the present invention;
the straight line in fig. 3 represents the constraint of maximum speed and the curve represents the maximum speed under the constraint of curvature;
FIG. 4 is a graph of velocity constraints for each path point in the method of the present invention
Figure 418709DEST_PATH_IMAGE071
Generating a velocity space diagram;
FIG. 5 is a schematic diagram of velocity profiles in a velocity space in the method of the present invention.
Detailed Description
In order to make those skilled in the art better understand the technical solution of the present invention, the following will clearly and completely describe the specific technical solution of the present invention with reference to the embodiments to help those skilled in the art to further understand the present invention. It should be apparent that the embodiments described herein are only a few embodiments of the present invention, and not all embodiments. It should be noted that the embodiments and features of the embodiments in the present application can be combined with each other without departing from the inventive concept and without conflicting therewith by those skilled in the art. All other embodiments based on the embodiments of the present invention, which can be obtained by a person of ordinary skill in the art without any creative effort, shall fall within the disclosure and the protection scope of the present invention.
Furthermore, the terms "first," "second," "step 1," "step 2," and the like in the description and in the claims and the drawings of the present invention are used for distinguishing between similar elements and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used is interchangeable under appropriate circumstances such that the embodiments of the invention described herein are capable of operation in sequences other than those described herein. Also, the terms "including" and "having," as well as any variations thereof, are intended to cover non-exclusive inclusions. In addition, for those skilled in the art, the specific meanings of the above terms in the present case can be understood by combining the prior art according to specific situations.
When the speed of the mobile robot is planned in a curved path, if the upper limit of the motion speed of the robot is simply reduced to meet the constraint of centripetal force, the speed of the robot is too low in the navigation process, and the execution efficiency is affected.
If the minimum value of the constraint is simply taken into consideration for velocity planning by considering the constraint of the robot kinematics and the constraint of the centripetal force and the maximum acceleration of the position, the acceleration of the planned velocity trajectory will be discontinuous, which reduces the flexibility of the robot.
In the invention, the reference speed is planned based on an S-shaped speed planning algorithm and the self-restriction limit of the robot. When the speed of the mobile robot is planned on a curved path, the kinematic constraint problem of the robot is considered, and the influence of centripetal acceleration generated by path curvature on the motion of the robot is also considered.
Meanwhile, in order to plan a smooth velocity curve and satisfy the kinematic constraint and centripetal acceleration constraint of the robot, the inventor considers that the maximum velocity upper limit of each path point is firstly calculated according to the kinematic constraint and the centripetal constraint of the path of the robot, so as to generate a feasible velocity space. And then generating an index column of points to be planned according to the valley point of the upper bound curve of the speed space. And finally, sequentially extracting index columns of two adjacent points from the index columns to carry out S-shaped speed planning.
Specifically, referring to fig. 1 to 5, the speed planning method under a curved smooth path according to the present invention includes the steps of:
step 1
Obtaining the information of speed, curvature and curvature radius corresponding to each path point on the robot planning path, and calculating the maximum speed allowed by each path point under the centripetal force constraint of the robot through a circular motion calculation formula
Figure 25271DEST_PATH_IMAGE001
And then according to the kinematic parameters of the robot, namely the maximum motion speed allowed by the robot during the motion process
Figure 956318DEST_PATH_IMAGE002
With maximum speed allowed under the constraint of centripetal force
Figure 872497DEST_PATH_IMAGE001
Taking the minimum value between the two as the speed constraint of each path point
Figure 812772DEST_PATH_IMAGE003
For generating a velocity space as shown in fig. 4.
Path point information available parametric equations generated assuming a planned path in step 1
Figure 8261DEST_PATH_IMAGE072
From this, each path point can be calculated
Figure 844630DEST_PATH_IMAGE073
Corresponding first derivative
Figure 522473DEST_PATH_IMAGE074
Second derivative of
Figure 859DEST_PATH_IMAGE075
Curvature of curvature
Figure 50855DEST_PATH_IMAGE076
And radius of curvature
Figure 825169DEST_PATH_IMAGE077
The information of (2):
Figure 960615DEST_PATH_IMAGE078
Figure 242692DEST_PATH_IMAGE079
Figure 147194DEST_PATH_IMAGE080
Figure 89480DEST_PATH_IMAGE081
further, according to the kinematic equation satisfied by the circular motion, the acceleration of the robot can be obtained
Figure 712223DEST_PATH_IMAGE068
：
Figure 532411DEST_PATH_IMAGE082
Wherein the content of the first and second substances,
Figure 58464DEST_PATH_IMAGE066
is the centripetal force of the circular motion,
Figure 938695DEST_PATH_IMAGE067
in order to be the quality of the robot,
Figure 314313DEST_PATH_IMAGE068
in order to accelerate the robot, the robot is provided with a plurality of wheels,
Figure 938192DEST_PATH_IMAGE069
is the current speed of the robot and is,
Figure 581401DEST_PATH_IMAGE070
is the radius of the circular motion.
Since the centripetal force of the mobile robot is an inherent property and is determined by the mass of the robot body and the friction coefficient between the ground and the robot, the centripetal force can be calculated at a certain path point
Figure 366954DEST_PATH_IMAGE054
Upper curvature to velocity constraint
Figure 229868DEST_PATH_IMAGE001
：
Figure 391859DEST_PATH_IMAGE083
。
Step 2
Speed constraints for traversing all path points in a speed space
Figure 892504DEST_PATH_IMAGE003
And forming a maximum speed upper bound curve and taking a speed valley point to generate a point index column to be planned.
Since the path trajectory is in the form of discrete points, the maximum velocity constraint for all points in the velocity space can be traversed
Figure 848959DEST_PATH_IMAGE003
Forming an upper bound curve of maximum speed, and taking a subscript index column corresponding to a valley point of the maximum speed curve as shown in FIG. 5
Figure 933589DEST_PATH_IMAGE006
And adding the starting point
Figure 397806DEST_PATH_IMAGE007
And index column for subscript of end point
Figure 517072DEST_PATH_IMAGE008
Form an index column of points to be planned
Figure 644428DEST_PATH_IMAGE009
。
Step 3
In order to ensure the smoothness of the speed planning, the path points corresponding to two adjacent index row points are sequentially selected in the step to perform S-shaped speed planning.
For example, in the present embodiment, index rows of two adjacent path points need to be sequentially extracted from the index rows of the points to be planned, and the speed information of the path point with a smaller index row value is used as the initial speed of the planning
Figure 216355DEST_PATH_IMAGE004
The speed information of the path point with larger index column value is used as the end speed of the plan
Figure 233332DEST_PATH_IMAGE005
And the speed curve is redistributed to a path between two index row points after the planning is finished.
In addition, after the S-shaped speed planning is performed on the path between the two index row points, it is required to check whether the speed value corresponding to the path point exceeds the speed constraint
Figure 207104DEST_PATH_IMAGE003
If it exceeds, it will
Figure 770941DEST_PATH_IMAGE002
And (5) after the reduction, performing speed planning again until the requirement of speed constraint is met.
Specifically, in order to perform S-shaped velocity planning between two path points, the continuity of acceleration needs to be satisfied. In general, the velocity trajectory is composed of 7 time segments, each of which is
Figure 830164DEST_PATH_IMAGE010
The stage of adding an acceleration, wherein,
Figure 370604DEST_PATH_IMAGE011
a stage of uniform acceleration of the mixture,
Figure 464462DEST_PATH_IMAGE012
reduction and acceleration stageThe length of the section is equal to the length of the section,
Figure 199200DEST_PATH_IMAGE013
in the stage of uniform speed, the speed of the motor is controlled,
Figure 981605DEST_PATH_IMAGE014
the acceleration and deceleration stage is that the speed of the motor is increased and decreased,
Figure 827201DEST_PATH_IMAGE015
a stage of uniform speed reduction is carried out,
Figure 775565DEST_PATH_IMAGE016
and (5) reducing and decelerating. In the acceleration stage (
Figure 681204DEST_PATH_IMAGE025
) And a deceleration stage (
Figure 213555DEST_PATH_IMAGE026
) According to
Figure 597263DEST_PATH_IMAGE084
Figure 134554DEST_PATH_IMAGE085
Calculating the maximum acceleration that can be achieved without considering the acceleration and deceleration limits
Figure 211095DEST_PATH_IMAGE086
With maximum deceleration
Figure 968092DEST_PATH_IMAGE087
Wherein
Figure 421070DEST_PATH_IMAGE004
In order to be the initial speed of the vehicle,
Figure 78447DEST_PATH_IMAGE005
in order to terminate the speed, the speed of the motor is controlled,
Figure 824424DEST_PATH_IMAGE002
is the maximum speed allowed by the robot,
Figure 567252DEST_PATH_IMAGE021
is the maximum jerk.
Further, based on the maximum acceleration
Figure 558342DEST_PATH_IMAGE088
And maximum deceleration
Figure 335805DEST_PATH_IMAGE089
And the maximum acceleration that can be achieved without considering the limit of acceleration and deceleration
Figure 990034DEST_PATH_IMAGE086
And maximum deceleration
Figure 485737DEST_PATH_IMAGE087
Is calculated from the relationship of
Figure 14938DEST_PATH_IMAGE025
，
Figure 381329DEST_PATH_IMAGE026
：
If it is
Figure 734687DEST_PATH_IMAGE027
Then, then
Figure 186529DEST_PATH_IMAGE028
，
Figure 519421DEST_PATH_IMAGE090
，
Figure 495643DEST_PATH_IMAGE030
If it is
Figure 255789DEST_PATH_IMAGE031
，Then
Figure 194926DEST_PATH_IMAGE032
，
Figure 331509DEST_PATH_IMAGE033
，
Figure 436606DEST_PATH_IMAGE034
，And update
Figure 102074DEST_PATH_IMAGE035
If it is
Figure 794086DEST_PATH_IMAGE036
，Then
Figure 468781DEST_PATH_IMAGE037
，
Figure 165735DEST_PATH_IMAGE038
，
Figure 533263DEST_PATH_IMAGE039
If it is
Figure 712571DEST_PATH_IMAGE040
，Then
Figure 423913DEST_PATH_IMAGE091
，
Figure 739488DEST_PATH_IMAGE092
，
Figure 12337DEST_PATH_IMAGE043
，And update
Figure 413363DEST_PATH_IMAGE044
According to the calculation of the above-mentioned every time stage the acceleration segment distance can be obtained
Figure 931325DEST_PATH_IMAGE045
And the distance of the deceleration section
Figure 101407DEST_PATH_IMAGE046
：
Figure 810737DEST_PATH_IMAGE093
Figure 699058DEST_PATH_IMAGE094
According to the total length of the path
Figure 752203DEST_PATH_IMAGE049
And the acceleration section of the route
Figure 776790DEST_PATH_IMAGE045
And a deceleration section
Figure 657022DEST_PATH_IMAGE046
Whether or not the relationship of (1) is satisfied
Figure 268525DEST_PATH_IMAGE050
Judging, if not, indicating that the constant motion distance and time do not exist, and setting the maximum speed
Figure 626825DEST_PATH_IMAGE002
After the speed is reduced, the motion time and the path of each stage are recalculated until the requirement of speed constraint is met; if the judgment is satisfied, calculating the constant speed segment distance
Figure 37078DEST_PATH_IMAGE051
Travel time of constant speed section
Figure 88210DEST_PATH_IMAGE013
：
Figure 449659DEST_PATH_IMAGE095
Figure 80492DEST_PATH_IMAGE096
Calculating the reference speed of each path point in an iterative or analytic mode according to the calculated distance and time of each stage, checking whether the reference speed exceeds the upper limit of the speed space, and if the reference speed exceeds the upper limit of the speed space, maximizing the speed of the path
Figure 79672DEST_PATH_IMAGE002
And after the speed is reduced, calculating and checking the speed plan again until the planned reference speed is in the speed space, thereby distributing proper speed information for each path point on the whole path.
Example of calculation
In order to more intuitively present the main effects of the patent, the following algorithm gives examples of paths including: set the total length of the path as
Figure 549310DEST_PATH_IMAGE097
The curvature of the path satisfies the function
Figure 899520DEST_PATH_IMAGE098
(for a given curved smooth path, its curvature is continuous and therefore the description is not critical). Parameter information for S-shaped speed planning
Figure 599623DEST_PATH_IMAGE099
,
Figure 718889DEST_PATH_IMAGE100
,
Figure 79201DEST_PATH_IMAGE101
。
The example process according to the above embodiment:
1. first, traversing each path point, and knowing the maximum speed constraint and curvature-to-speed constraint corresponding to each path point, as shown in fig. 3. The straight line is the constraint of the maximum speed, and the curve is the maximum speed under the constraint of the curvature.
2. For each path point, take the maximum speed
Figure 916707DEST_PATH_IMAGE002
And maximum velocity under curvature constraint
Figure 154921DEST_PATH_IMAGE102
Smaller value of
Figure 895737DEST_PATH_IMAGE103
A velocity space under constraint may be obtained, as shown in fig. 4.
3. As shown in fig. 4, a valley point at the upper boundary of the velocity space is taken, and a point index column to be planned is generated. The sampling distance interval for the path in this example is
Figure 193995DEST_PATH_IMAGE104
So that the points to be planned are indexed by
Figure 253218DEST_PATH_IMAGE105
The path lengths of the point index row to be planned in the path are respectively
Figure 295123DEST_PATH_IMAGE106
。
4. The index rows of two adjacent points are sequentially extracted from the index rows, and S-speed planning is performed, so that the final effect is as shown in fig. 5, the speed curve is a curve which is curved and smooth, and it can be seen that each point on the curve is in the speed space.
In addition, it should be noted that, although the embodiment of the present invention mainly illustrates a scheme for performing speed planning based on an S-shaped speed planning algorithm and robot self-constraint limits, the purpose is to make the planned speed smooth. It will be appreciated by those skilled in the art that the examples of the present invention may be applied to other existing curve characteristics to form, for example: bezier curves, B-spline curves and the like replace the S-shaped speed planning algorithm in the scheme, and the speed curves are generated in an iterative optimization mode. Accordingly, such alternative embodiments are also within the scope of the present disclosure.
In another aspect, the present invention also provides a computer readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a computer processor, performs the steps of the method for velocity planning under a curved smooth path as described in the above example.
In summary, the speed planning method and the storage medium under the curved smooth path provided by the invention have the advantages that:
1. aiming at the speed planning problem under the curved smooth path, the speed planning is carried out in the speed space under the constraint of the maximum speed and the centripetal force, and the planned reference speed can be ensured to meet the speed constraint of the mobile robot in the operation process.
2. Aiming at the problem of speed smoothness in the operation process, the S-shaped speed planning algorithm is used for planning the reference speed, so that the acceleration of the planned reference speed is continuous, and the requirement on speed smoothness is met.
3. The acceleration is continuous, which means that the reference speed of the robot changes stably, and the accuracy and the efficiency of the tracking process of the robot track are improved.
The preferred embodiments of the invention disclosed above are intended to be illustrative only. The preferred embodiments are not intended to be exhaustive or to limit the invention to the precise embodiments disclosed. Obviously, many modifications and variations are possible in light of the above teaching. The embodiments were chosen and described in order to best explain the principles of the invention and the practical application, to thereby enable others skilled in the art to best utilize the invention. The invention is limited only by the claims and the full scope and equivalents thereof, and any modification, equivalent replacement, or improvement made within the spirit and principle of the invention should be included in the protection scope of the invention.
In addition, all or part of the steps of the method according to the above embodiments may be implemented by a program instructing related hardware, where the program is stored in a storage medium and includes several instructions to enable a single chip, a chip, or a processor (processor) to execute all or part of the steps of the method according to the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
In addition, any combination of various different implementation manners of the embodiments of the present invention is also possible, and the embodiments of the present invention should be considered as disclosed in the embodiments of the present invention as long as the combination does not depart from the spirit of the embodiments of the present invention.

Claims (6)

1. A method of velocity planning under a curved smooth path, the steps comprising:
step 1, acquiring corresponding information including speed, curvature and curvature radius of each path point on a planned path of the robot so as to calculate the maximum speed allowed by each path point under the constraint of centripetal force of the robot
Figure DEST_PATH_IMAGE001
And supply the maximum movement speed of the robot
Figure DEST_PATH_IMAGE002
Taking small between them as the speed constraint of each path point
Figure DEST_PATH_IMAGE003
For generating a velocity space;
step 2 traversing speed constraint of all path points in speed space
Figure 345097DEST_PATH_IMAGE003
Forming a maximum speed upper bound curve and taking a speed valley point to generate a point index row to be planned;
step 3, in the index rows of the points to be planned, the index rows of two adjacent path points are sequentially extracted,using the speed information of the path point with smaller index column value as the starting speed of the plan
Figure DEST_PATH_IMAGE004
The speed information of the path point with larger index column value is used as the end speed of the plan
Figure DEST_PATH_IMAGE005
And carrying out speed planning according to a speed curve algorithm, and updating a planned path according to a planning result.
2. The method for velocity planning under a curved and smooth path according to claim 1, wherein step 3 further comprises: checking whether the speed value corresponding to each path point exceeds the speed constraint
Figure 659928DEST_PATH_IMAGE003
If the speed exceeds the preset speed limit value, the upper limit value of the speed of the current segment is programmed
Figure 439403DEST_PATH_IMAGE002
And (5) after the reduction, performing speed planning again until the requirement of speed constraint is met.
3. The method for velocity planning under a curved smooth path according to claim 1, wherein the generating of the planned point index column comprises: index column of subscript corresponding to speed valley point of maximum speed upper bound curve
Figure DEST_PATH_IMAGE006
And adding the starting point
Figure DEST_PATH_IMAGE007
And index column for subscript of end point
Figure DEST_PATH_IMAGE008
To form an index column of points to be planned
Figure DEST_PATH_IMAGE009
。
4. The method for velocity planning under a curved smooth path according to claim 1, wherein the velocity planning in step 3 is an S-shaped velocity planning, the planning step comprising:
the velocity trajectory is divided into:
Figure DEST_PATH_IMAGE010
the acceleration period is added to the acceleration period,
Figure DEST_PATH_IMAGE011
the period of uniform acceleration is set to be,
Figure DEST_PATH_IMAGE012
the acceleration period is reduced and the time period of acceleration,
Figure DEST_PATH_IMAGE013
at the constant-speed time interval,
Figure DEST_PATH_IMAGE014
the time period of acceleration and deceleration is set,
Figure DEST_PATH_IMAGE015
the period of uniform deceleration is set to be,
Figure DEST_PATH_IMAGE016
during the deceleration period, the maximum acceleration which can be reached by the robot without considering the limitation of acceleration and deceleration is calculated
Figure DEST_PATH_IMAGE017
With maximum deceleration
Figure DEST_PATH_IMAGE018
：
Figure DEST_PATH_IMAGE019
Figure DEST_PATH_IMAGE020
Wherein
Figure DEST_PATH_IMAGE021
The maximum acceleration of the robot is obtained;
according to maximum acceleration of robot
Figure DEST_PATH_IMAGE022
And maximum deceleration
Figure DEST_PATH_IMAGE023
And with
Figure DEST_PATH_IMAGE024
And
Figure DEST_PATH_IMAGE025
is calculated from the relationship of
Figure DEST_PATH_IMAGE026
，
Figure DEST_PATH_IMAGE027
：
If it is
Figure DEST_PATH_IMAGE028
，Then
Figure DEST_PATH_IMAGE029
，
Figure DEST_PATH_IMAGE030
，
Figure DEST_PATH_IMAGE031
If it is
Figure DEST_PATH_IMAGE032
，Then
Figure DEST_PATH_IMAGE033
，
Figure DEST_PATH_IMAGE034
，
Figure DEST_PATH_IMAGE035
，And update
Figure DEST_PATH_IMAGE036
If it is
Figure DEST_PATH_IMAGE037
，Then
Figure DEST_PATH_IMAGE038
，
Figure DEST_PATH_IMAGE039
，
Figure DEST_PATH_IMAGE040
If it is
Figure DEST_PATH_IMAGE041
Then, then
Figure DEST_PATH_IMAGE042
，
Figure DEST_PATH_IMAGE043
，
Figure DEST_PATH_IMAGE044
And update
Figure DEST_PATH_IMAGE045
Further calculating the acceleration segment distance
Figure DEST_PATH_IMAGE046
And the distance of the deceleration section
Figure DEST_PATH_IMAGE047
：
Figure DEST_PATH_IMAGE048
Figure DEST_PATH_IMAGE049
Determining the total length of the track
Figure DEST_PATH_IMAGE050
And with
Figure 388728DEST_PATH_IMAGE046
And
Figure 71513DEST_PATH_IMAGE047
the relationship of (1):
Figure DEST_PATH_IMAGE051
if not, will
Figure 649256DEST_PATH_IMAGE002
After the speed is reduced, recalculating each time interval and path until the requirement of speed constraint is met, and if the judgment is met, calculating the path of the uniform speed segment
Figure DEST_PATH_IMAGE052
And
Figure 591061DEST_PATH_IMAGE013
：
Figure DEST_PATH_IMAGE053
Figure DEST_PATH_IMAGE054
。
5. the method for velocity planning under a curved, smooth path according to claim 1, wherein the maximum velocity
Figure 83091DEST_PATH_IMAGE001
The calculating step comprises:
calculate each path point
Figure DEST_PATH_IMAGE055
Corresponding first derivative
Figure DEST_PATH_IMAGE056
Second derivative of
Figure DEST_PATH_IMAGE057
Curvature of curvature
Figure DEST_PATH_IMAGE058
And radius of curvature
Figure DEST_PATH_IMAGE059
The information of (2):
Figure DEST_PATH_IMAGE060
Figure DEST_PATH_IMAGE061
Figure DEST_PATH_IMAGE062
Figure DEST_PATH_IMAGE063
according to the formula of circular motion:
Figure DEST_PATH_IMAGE064
to obtain the acceleration of the robot
Figure DEST_PATH_IMAGE065
Post-calculation
Figure DEST_PATH_IMAGE066
Wherein
Figure DEST_PATH_IMAGE067
Is the centripetal force of the robot,
Figure DEST_PATH_IMAGE068
in order to be the quality of the robot,
Figure 80435DEST_PATH_IMAGE065
in order to accelerate the robot, the robot is provided with a plurality of wheels,
Figure DEST_PATH_IMAGE069
is the current speed of the robot and is,
Figure DEST_PATH_IMAGE070
is the radius of the circular motion of the robot.
6. A computer-readable storage medium, on which a computer program is stored which, when being executed by a computer processor, carries out the steps of the method for velocity planning under a curved, smooth path according to any of claims 1 to 5.
CN202210781025.4A 2022-07-05 2022-07-05 Speed planning method under curved smooth path and storage medium Active CN114859941B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210781025.4A CN114859941B (en) 2022-07-05 2022-07-05 Speed planning method under curved smooth path and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210781025.4A CN114859941B (en) 2022-07-05 2022-07-05 Speed planning method under curved smooth path and storage medium

Publications (2)

Publication Number Publication Date
CN114859941A true CN114859941A (en) 2022-08-05
CN114859941B CN114859941B (en) 2022-09-23

Family

ID=82626443

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210781025.4A Active CN114859941B (en) 2022-07-05 2022-07-05 Speed planning method under curved smooth path and storage medium

Country Status (1)

Country Link
CN (1) CN114859941B (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116520822A (en) * 2023-03-13 2023-08-01 中国人民解放军国防科技大学 Smooth curvature parameterized representation path generation method
CN116540748A (en) * 2023-07-07 2023-08-04 上海仙工智能科技有限公司 Method and system for planning robot bypass path on navigation path
CN116701821A (en) * 2023-08-03 2023-09-05 季华实验室 Robot speed calculation method and device, electronic equipment and storage medium

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103488174A (en) * 2013-09-16 2014-01-01 北京邮电大学 Automatic guiding control method, device and system
WO2016045615A1 (en) * 2014-09-25 2016-03-31 科沃斯机器人有限公司 Robot static path planning method
CN108594815A (en) * 2018-04-20 2018-09-28 武汉大学 A kind of wheeled robot mobile route planing method stage by stage
US20190041858A1 (en) * 2017-08-03 2019-02-07 Mitsubishi Electric Research Laboratories, Inc. Motion Controller for Real-Time Continuous Curvature Path Planning
US20190146497A1 (en) * 2017-11-15 2019-05-16 Uber Technologies, Inc. Sparse Convolutional Neural Networks
CN110757455A (en) * 2019-10-25 2020-02-07 上海新时达机器人有限公司 Speed planning method
WO2022040748A1 (en) * 2020-08-27 2022-03-03 Technological Resources Pty. Limited Method and apparatus for coordinating multiple cooperative vehicle trajectories on shared road networks

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103488174A (en) * 2013-09-16 2014-01-01 北京邮电大学 Automatic guiding control method, device and system
WO2016045615A1 (en) * 2014-09-25 2016-03-31 科沃斯机器人有限公司 Robot static path planning method
US20190041858A1 (en) * 2017-08-03 2019-02-07 Mitsubishi Electric Research Laboratories, Inc. Motion Controller for Real-Time Continuous Curvature Path Planning
US20190146497A1 (en) * 2017-11-15 2019-05-16 Uber Technologies, Inc. Sparse Convolutional Neural Networks
CN108594815A (en) * 2018-04-20 2018-09-28 武汉大学 A kind of wheeled robot mobile route planing method stage by stage
CN110757455A (en) * 2019-10-25 2020-02-07 上海新时达机器人有限公司 Speed planning method
WO2022040748A1 (en) * 2020-08-27 2022-03-03 Technological Resources Pty. Limited Method and apparatus for coordinating multiple cooperative vehicle trajectories on shared road networks

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王杨斌: "改进Informed-RRT*的动态环境路径规划算法", 《电光与控制》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116520822A (en) * 2023-03-13 2023-08-01 中国人民解放军国防科技大学 Smooth curvature parameterized representation path generation method
CN116520822B (en) * 2023-03-13 2023-09-19 中国人民解放军国防科技大学 Smooth curvature parameterized representation path generation method
CN116540748A (en) * 2023-07-07 2023-08-04 上海仙工智能科技有限公司 Method and system for planning robot bypass path on navigation path
CN116540748B (en) * 2023-07-07 2023-10-31 上海仙工智能科技有限公司 Method and system for planning robot bypass path on navigation path
CN116701821A (en) * 2023-08-03 2023-09-05 季华实验室 Robot speed calculation method and device, electronic equipment and storage medium
CN116701821B (en) * 2023-08-03 2024-03-19 季华实验室 Robot speed calculation method and device, electronic equipment and storage medium

Also Published As

Publication number Publication date
CN114859941B (en) 2022-09-23

Similar Documents

Publication Publication Date Title
CN114859941B (en) Speed planning method under curved smooth path and storage medium
CN110297494B (en) Decision-making method and system for lane change of automatic driving vehicle based on rolling game
CN109540159B (en) Rapid and complete automatic driving track planning method
CN111238521B (en) Path planning method and system for unmanned vehicle
EP3730370A1 (en) Method and device for acquiring automatic driving track
CN111191847B (en) Distribution path planning method and system considering order polymerization degree
US7892078B2 (en) Racing line optimization
CN111898804B (en) Road self-adaptive sampling method for local planning
CN113448335A (en) Path planning method and device, vehicle and readable storage medium
CN113415288B (en) Sectional type longitudinal vehicle speed planning method, device, equipment and storage medium
CN108387232B (en) Flight object track planning method based on artificial potential field evolution algorithm
CN113593228B (en) Automatic driving cooperative control method for bottleneck area of expressway
CN113984080B (en) Layered local path planning method suitable for large complex scene
Bevilacqua et al. Particle swarm for path planning in a racing circuit simulation
CN110849385B (en) Track planning method and system based on double-layer heuristic search conjugate gradient descent
CN116734877A (en) Robot dynamic obstacle avoidance method based on improved A-algorithm and dynamic window method
Vesel Racing line optimization@ race optimal
CN114115268B (en) Path generation method, device, vehicle and storage medium
CN114117944B (en) Model updating method, device, equipment and readable storage medium
CN113753082B (en) Unmanned vehicle track updating method and device, control method and electronic equipment
CN114167857B (en) Control method and device of unmanned equipment
CN109791732A (en) Section determination device and section determining method
CN115092141A (en) Trajectory planning method and equipment applied to lane change and overtaking of automatic driving vehicle
JP2023531927A (en) Driving decision-making method, driving decision-making device, and chip
CN113370995A (en) Speed curve processing method and device, electric vehicle and electronic 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
GR01 Patent grant
GR01 Patent grant