CN113848922B - Degenerate splicing method and device for tracks containing straight paths and storage medium thereof - Google Patents
Degenerate splicing method and device for tracks containing straight paths and storage medium thereof Download PDFInfo
- Publication number
- CN113848922B CN113848922B CN202111151249.9A CN202111151249A CN113848922B CN 113848922 B CN113848922 B CN 113848922B CN 202111151249 A CN202111151249 A CN 202111151249A CN 113848922 B CN113848922 B CN 113848922B
- Authority
- CN
- China
- Prior art keywords
- path
- sub
- paths
- bezier curve
- order
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 238000009499 grossing Methods 0.000 claims abstract description 27
- 238000012216 screening Methods 0.000 claims abstract description 6
- 238000012545 processing Methods 0.000 claims description 18
- 238000004590 computer program Methods 0.000 claims description 5
- 238000009795 derivation Methods 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 4
- 238000005457 optimization Methods 0.000 description 15
- 238000004364 calculation method Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 230000007547 defect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000010187 selection method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Image Generation (AREA)
Abstract
The invention provides a degenerated splicing method and device for tracks containing straight paths and a storage medium thereof, wherein the method comprises the following steps: s1, traversing a plurality of sections of preset sub-paths, and screening the optimized sub-paths; s2, judging whether the existing number of the linear sub-paths in the front and rear adjacent paths is equal to two by taking the optimizable sub-paths as a starting point, and if so, optimizing the optimizable sub-paths according to a first rule; and S3, performing degenerate smoothing on the optimized sub-path obtained in the step S2. Thereby reducing the complexity of the user in use and allowing the path tangent to continue with the curvature at the point of connection while preserving the freedom of path adjustment.
Description
Technical Field
The invention relates to the technical field of navigation path splicing optimization, in particular to a method and a device for degenerating and splicing tracks containing straight paths and a storage medium thereof.
Background
Because Bezier curves are widely applied to path planning, only the path splicing problem formed by the Bezier curves with different orders is considered in most multi-section path splicing algorithms and strategies at present, but in the path splicing problem of an actual mobile robot, a plurality of continuous straight lines often meet the complex paths to be spliced, and the splicing position of the straight lines only can meet the tangential continuity of connecting points and cannot guarantee the curvature continuity, so that the track following precision and the moving stability of the robot are affected.
If the 3-order Bezier curves are adopted for splicing, when the 3-order Bezier curves are spliced with two straight lines, the two control points in the middle can only be positioned at the intersection point of the extension lines of the two straight lines, and the adjustability of the curves is lacking. When two straight lines are spliced by adopting a 5-order Bezier curve, the tangent to the straight lines can be simultaneously satisfied, and the adjustability of the curve is maintained. However, the method is too flexible (having too many control points), and is difficult to be applied to adjusting linearity by manually dragging the control points, so that linearity can be automatically adjusted only by an optimization method, and if manual adjustment is adopted, the complexity of manual adjustment is increased when a user uses the method, and the degree of freedom of the user is reduced by automatic adjustment, so that a certain experience is sacrificed.
For this reason, there is a need in the art for a technique to solve the above-mentioned problems, to reduce the complexity of the user in use while maintaining the freedom of path adjustment, and to make the path tangent continuous and curvature continuous at the connection point.
Disclosure of Invention
The invention mainly aims to provide a method and a device for degenerating and splicing tracks containing straight paths and a storage medium thereof, so as to solve the problems in the background technology.
To achieve the above object, according to a first aspect of the present invention, there is provided a degenerate stitching method for tracks comprising straight paths, comprising the steps of:
s1, traversing a plurality of sections of preset sub-paths, and screening the optimized sub-paths;
s2, judging whether the existing number of the linear sub-paths in the front and rear adjacent paths is equal to two by taking the optimizable sub-paths as a starting point, and if so, optimizing the optimizable sub-paths according to a first rule;
and S3, performing degenerate smoothing on the optimized sub-path obtained in the step S2.
Wherein the first rule comprises: the sub-path can be optimized, and the sub-path track is spliced by adopting an N-order Bezier curve, wherein N is more than or equal to 5.
Wherein the degenerating smoothing step comprises:
d1, calculating first-order and second-order derivative data of the linear sub-path;
D2, making the first and second derivatives of the optimized Bezier curve of the optimizable sub-path equal to the first and second derivatives of the adjacent linear sub-path at the connection point;
And D3, overlapping the optimized control points of the optimizable sub-paths according to a second rule.
Wherein the second rule includes overlapping control points according to at least one of the following: overlapping all control points to generate absolute control point coordinates; or all the control points are divided into two groups, the control points in the groups are sequentially continuous, and the control points in each group are overlapped to generate a first control point and a second control point.
In a possibly preferred embodiment, the calculating step of the first and second derivatives of the bezier curve of the optimizable sub-path includes:
Control point derivation formula for calculating Bezier curve
Where n is the order of the Bezier curve, P i is the ith control point of the Bezier curve, and C (0) =P 0,C(1)＝Pn,Wherein the first derivative can be expressed as:
Wherein D i＝n(Pi+1-Pi), the first derivative of the bezier curve at the end point can be expressed as:
Wherein, Representing the first derivative value thereof at the start point,/>Representing the first derivative value at the end of the curve, whereas the second derivative value/>, of the Bezier curve at the start and end pointsAnd/>The method comprises the following steps:
In order to achieve the above object, according to a second aspect of the present invention, there is also provided a navigation path curvature continuous stitching optimization processing device, comprising: the mobile robot acquires navigation data through the scanning module, transmits the navigation data to the navigation path module, generates a plurality of pieces of sub-path data after processing, and transmits the sub-path data to the path splicing processing module, and the path splicing processing module executes degeneracy splicing processing on the linear path track according to the degeneracy splicing method containing the linear path track.
To achieve the above object, according to a second aspect of the present invention, there is also provided a readable storage medium having stored thereon a computer program, wherein the computer program, when executed by a processor, performs the steps of the above-described degenerate stitching method with linear path trajectories.
The pair of degenerated splicing methods and devices containing linear path tracks and the storage medium thereof provided by the invention have the beneficial effects that:
1. the method overcomes the defects that when two straight paths are smoothly spliced by using Bezier curves, the degree of freedom of the Bezier curves with five or more steps is too high, and the degree of freedom of the Bezier curves with three steps is insufficient, and simultaneously reduces the complexity of a user in use while retaining the degree of freedom of path adjustment.
2. Meanwhile, the Bezier curve path and the straight line path can be effectively enabled to be continuous in tangential direction and curvature at the intersection point, so that the robot can run more stably.
3. The tangential direction of the path is continuous, which means that the angle change of the robot is continuous, the curvature of the path is continuous, which means that the change rate of the angular speed of the robot is continuous, and the accuracy and the efficiency of the track tracking control of the robot are improved.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate embodiments of the application and together with the description serve to explain the application. In the drawings:
FIG. 1 is a schematic diagram of the method steps of a first embodiment of the present invention;
FIG. 2 is a schematic diagram of a path after degeneracy of an experimental example in a first embodiment of the invention;
FIG. 3 is a schematic diagram of a first embodiment of the present invention with control point adjustments to a degenerate path;
Detailed Description
The following describes specific embodiments of the present invention in detail. The following examples will assist those skilled in the art in further understanding the present invention, but are not intended to limit the invention in any way. It should be noted that variations and modifications could be made by those skilled in the art without departing from the inventive concept. These are all within the scope of the present invention.
It should be noted that, without conflict, the embodiments of the present application and features of the embodiments may be combined with each other. The application will be described in detail below with reference to the drawings in connection with embodiments.
In order that those skilled in the art will better understand the present invention, a technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are only some embodiments of the present invention, not all embodiments. All other embodiments, based on the embodiments of the invention, which are obtained without inventive effort by a person of ordinary skill in the art, shall fall within the scope of the invention.
It should be noted that the terms "first", "second", "S1", "S2", "D1", and the like in the description and the claims of the present invention and the above drawings are used for distinguishing similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate such that the embodiments of the invention described herein may be implemented in sequences other than those illustrated or otherwise described herein. Furthermore, the terms "comprise" and "have," as well as any variations thereof, are intended to cover a non-exclusive inclusion. Furthermore, unless specified and limited otherwise, the terms "disposed," "configured," "mounted," "connected," and "connected" are to be construed broadly and may be, for example, fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the above terms in the present invention can be understood by those of ordinary skill in the art in view of the specific circumstances in combination with the prior art.
In the degenerate splicing method for the pair of paths containing the linear path track, in the following embodiment, degenerate smoothing is mainly performed on a plurality of sections of complex paths containing linear sub-path tracks (straight lines, namely first-order Bezier curves). Therefore, in the splicing optimization scheme provided by the invention, a path to be optimized is assumed to exist, wherein the path to be optimized consists of a plurality of sections of different-order Bezier curve sub-paths.
(One)
Specifically, as shown in fig. 1, the degenerate stitching method for the pair of tracks with straight paths in this embodiment includes the steps of:
S1, traversing a plurality of segments of preset sub-paths, traversing each sub-path in the paths, and judging the type of the sub-path to screen out an optimizable sub-path; if the traversed path is a straight-line sub-path or a sub-path locked from optimization, the path is considered to be an unoptimizable path, so that the path is skipped to continue traversing to the next path, and the optimizable sub-path is screened;
S2, when the traversed path is an optimizable sub-path, judging whether the existing number of linear sub-paths in the front and rear adjacent paths is equal to two by taking the optimizable sub-path as a starting point, if so, the two linear sub-paths are spliced at the head end and the tail end of the current sub-path, so that the current sub-path is an optimization target, and optimizing the optimizable sub-path according to a first rule; wherein the first rule comprises: the sub-path can be optimized, and the sub-path track is spliced by adopting an N-order Bezier curve, wherein N is more than or equal to 5.
And S3, performing degenerate smoothing on the optimized sub-path obtained in the step S2. Wherein the degenerately smoothing processing step comprises: d1, calculating first-order and second-order derivative data of the linear sub-path; d2, making the first and second derivatives of the optimized Bezier curve of the optimizable sub-path equal to the first and second derivatives of the adjacent linear sub-path at the connection point; and D3, overlapping the optimized control points of the optimizable sub-paths according to a second rule.
Wherein the second rule comprises overlapping control points according to at least one of:
1. overlapping all control points to generate absolute control point coordinates;
2. or all the control points are divided into two groups, the control points in the groups are sequentially continuous, and the control points in each group are overlapped to generate a first control point and a second control point.
Wherein the calculating step of the first and second derivatives of the Bezier curve of the optimizable sub-path comprises:
Control point derivation formula for calculating Bezier curve
Where n is the order of the Bezier curve, P i is the ith control point of the Bezier curve, and C (0) =P 0,C(1)＝Pn, Wherein the first derivative can be expressed as:
Wherein D i＝n(Pi+1-Pi), the first derivative of the bezier curve at the end point can be expressed as:
Wherein, Representing the first derivative value thereof at the start point,/>Representing the first derivative value at the end of the curve, whereas the second derivative value/>, of the Bezier curve at the start and end pointsAnd/>The method comprises the following steps:
by the formula, the numerical values of the first-order derivative and the second-order derivative of the Bezier paths at the splicing points can be conveniently calculated, so that the first-order derivative and the second-order derivative of the Bezier curves of the optimized sub-paths are equal to the first-order derivative and the second-order derivative of the adjacent linear sub-paths at the connecting points, and the tangential directions of the splicing paths at the connecting points can be continuous with the curvature.
When the Bezier curve is in the x-y plane, the curvature at any time can be expressed as:
Specifically, since a straight line can be regarded as a special first-order bezier curve C 1, its control points are two end points of the straight line.
Thus, the first derivative of the end point can be obtained asWherein/>And/>The two ends of the straight line are respectively provided with a second derivative of 0 and a curvature of 0. Wherein/>Is the j-th control point of the i-order Bezier curve.
For the group consisting ofThree-order Bezier curve C 3 generated by four control points, the first derivative at the starting point is/>Its second derivative is/> The first derivative at its end point is/>Its second derivative is/>
For the group consisting ofFive-order Bezier curve C5 generated by six control points, wherein the first derivative at the starting point is/>Its second derivative is/> The first derivative at its end point is/>Its second derivative is
When it is desired to complete the smoothing from the straight sub-path to the third order bessel path. In order to make the straight sub-path and Bessel path spliced smooth, the following conditions are satisfied firstlyIn order to ensure that the straight line is continuous with the tangential direction of the third-order Bezier curve, the requirement of/>Where a is any real number other than 0, i.e. it is necessary to make/>In the tangential direction of the straight line.
Since the straight line curvature is 0, in order to equalize the curvature of the two paths at the junction, it is necessary to make the curvature of the two paths equal as known by the curvature formula κ (t) at any time of the Bezier curveAnd/>Collinear, i.e. requiring to make/>In the tangential direction of the straight line. Similarly, when it is desired to complete the smoothing from the Bezier curve sub-path to the straight sub-path, it is also desired to make/>Collinear with the latter straight sub-path.
Therefore, when the third-order Bezier curve is used to smoothly connect the front and rear straight lines, the control point is requiredAnd selecting the intersection point of the tangent points of the two straight lines. At this time, the resulting Bezier curve is not adjustable because there are no redundant movable control points.
Similar to the above analysis, when it is desired to complete the smoothing from the straight sub-path to the fifth-order Bessel path, it is desired to makeAnd/>Collinear with the front straight sub-path. When it is desired to complete the smoothing from the fifth order Bezier path to the straight sub-path, it is desired to make/>Continuous at the collinear junction with the rear straight sub-path.
Therefore, when the five-order Bezier curve is used for smooth connection with the front and back straight-line sub-paths,The four control points can move only in the tangential direction of the corresponding straight line. At this time, the user needs to arrange four control points appropriately, but this will increase the complexity of the user's use.
The degeneracy of the invention is that when the N-order Bezier curve (N is larger than or equal to 5) is used for smooth connection with the front and back straight-line sub-paths, such as when the five-order Bezier curve is used, the method comprises the following steps ofAnd/>Coincidence,/>, ofAnd/>Coincidence, i.e.By overlapping the control points, the method reduces 4 control points originally required to be specified by the user to 2 control points only required to be specified by the user, namely/>And/>Even 1 control point, i.e./>Similarly to the third-order bezier curve, the degenerate fifth-order bezier curve also requires two control points to be specified, except that the two control points of the latter are movable.
In addition, the present invention, when the two straight lines are spliced, the above embodiment, while exemplified by degenerating using a five-order Bezier curve, divides all control points into two groups according to the order to formAnd/>Overlap as first control point,/>And/>An embodiment overlapping as a second control point. In practice, however, a degenerate seven-order approach can be deduced similarly, namely: let/>, seven-order Bessel pathCoincidence,/>, ofAnd (5) overlapping. The degenerate nine-order Bezier curve can also be similarly deduced, namely: let the nine-order Bessel Path/>Coincidence,/>, ofAnd (5) overlapping. The remaining higher order degenerate bezier curves are similar.
Similar to the even-order Bezier curve, for example, the six-order Bezier curve has 5 control points, because the control points cannot be equally distributed into two groups, the method for degenerating the six-order Bezier curve can also arbitrarily divide all the control points into two groups according to the sequence, for exampleCoincidence,/>, ofOverlapping; /(I)Overlapping; /(I)The two parts are overlapped together,And overlapping, and the like, performing various permutation and combination modes, and performing degeneracy to obtain a first control point and a second control point.
In addition, the degeneracy of the N-order Bezier curve can also be realized by overlapping all control points into one absolute control point.
Experimental example
Examples: if the tracks of the front and rear sub-paths are straight lines, the degenerate five-order Bezier curve is used for splicing.
Specifically: the front path is straight line y=x, x epsilon [ -10,0], the back path is straight line y=10, x epsilon [60, 90]. According to the control point selection rule, selectingIs the end point of the straight line,The generated trajectory and control points are as shown in figure 2 in the accessory. When there is an obstacle or other reason on the robot path, a new path needs to be redesigned, the two control points in the middle can be changed at will, such as/>, for exampleThe newly generated path is shown in fig. 3, and the smoothness of the joint of the path and the straight line can be ensured.
In addition, in the above embodiment and experimental example, only the degenerate splicing method for the linear path track is adopted, and the following scheme or the existing literature can be referred to for the subsequent smoothing algorithm between the bezier curves of different orders.
The curvature continuous splicing optimization method for the path segment containing the circular arc navigation comprises the following steps:
Step S1, traversing a plurality of preset sub-paths, traversing each sub-path in the paths, and judging the type of the sub-path to screen out an optimizable sub-path; if the traversed path is a circular arc track sub-path, the circular arc track is a non-optimizable path because no control point exists, so that the sub-path is skipped to continue to traverse to the next path.
Step S2, when the traversed path is not a circular arc, the navigation path contains, since in the assumption: the third-order and fifth-order Bezier curve sub-paths and the arc track sub-paths are needed to be judged whether to be one of the third-order or fifth-order Bezier curves, namely, the sub-paths can be optimized, at the moment, the optimized sub-paths can be used as starting points, the existence quantity of the arc track sub-paths in the adjacent paths is judged, and the optimized sub-paths are optimized according to a first rule.
In this embodiment, the first rule includes:
Step C1, when the number of arc track sub-paths in adjacent paths is one, judging the type of the current optimizable sub-path: when the type is a third-order Bezier curve, optimizing in a first scheme includes: sub-path track splicing is carried out by using a third-order Bezier curve; when the type is a fifth-order Bezier curve, optimizing according to the second scheme comprises: sub-path track splicing is carried out by using a fifth-order Bezier curve;
And C2, judging the type of the current optimizable sub-path when the number of the arc track sub-paths in the adjacent paths is two: when the type is a fifth-order Bezier curve, optimizing in the third mode comprises: sub-path track splicing is carried out by using a fifth-order Bezier curve; when the type is a third-order Bezier curve, skipping the sub-path, and continuing to traverse the next path;
Step C3, when the number of arc track sub-paths in the adjacent paths is zero, is considered to be the connection problem of the multiple segments of bezier curves, and the method may refer to the scheme of the applicant's patent application or the smoothing method mentioned in other documents, and the embodiment is not limited.
And step S3, smoothing the optimized sub-path optimized in the step S2. Wherein the sub-path smoothing process in the present embodiment is defined as a path in which the sub-paths are continuous in the tangential direction and continuous in curvature at the connection point.
Therefore, when the navigation tracks are smoothly connected, the tracks are required to be smoothly spliced according to the derivative information of one circular arc path in the adjacent paths or the derivative information of two circular arc paths in the adjacent paths. In this embodiment, derivative information of the third-order or fifth-order bezier curve at the connection point is preferably obtained by a derivative formula of the bezier curve, and by reasonably arranging coordinates of control points of the bezier curve, the first-order second derivative of the bezier curve and the first-order second derivative of an adjacent circular arc path can be equal at the connection point. Thereby achieving the purpose of path smoothing.
Specifically, in a preferred embodiment, the smoothing step includes:
Step D1, calculating first-order and second-order derivative data of a circular arc track sub-path;
Step D2, setting the control point coordinates of the optimized optimizable sub-path; the first and second derivatives of the Bezier curve are made equal to the first and second derivatives of the adjacent circular arc track sub-paths at the junction.
The derivative calculation step of the arc track sub-path comprises the following steps:
Expressed by circular arc track parameter equation Calculating the first derivative/>, at the end point of the arc pathSecond derivative/>First derivative/>, at the front end point of the back-end path C 3 Second derivative/>
In a possibly preferred embodiment, the step of calculating the derivative of the optimized optimizable sub-path comprises:
Solving a derivative formula of the Bezier curve:
Where n is the order of the Bezier curve, P i is the ith control point of the Bezier curve, and P (0) =P 0,P(1)＝Pn, Wherein the first derivative can be expressed as:
wherein D i＝n(Pi+1-Pi), the first derivative of the bezier curve at the end point can be expressed as:
Wherein the method comprises the steps of Representing the first derivative value thereof at the start point,/>Representing the first derivative value at the end and the second derivative value/>, at the start and end, of the Bezier curve, similarly obtainableAnd/>
By the above formula settlement, the values of the first and second derivatives of the Bezier curve path at the front and rear end points (i.e. the splice points) can be calculated after a given control point. The requirement of path splicing is that the path is continuous in tangential direction and curvature at the connection point, and the Bezier curve can be formed by a parameter equationAnd (5) determining. From the calculation formula of curvature:
The unit tangent vector calculation formula:
It can be seen that the first derivative and the second derivative of the two paths at the connecting point are continuous, so that the tangential continuity and curvature continuity of the paths at the splicing point can be ensured.
On the other hand, in order to implement a smoothing algorithm between subsequent bezier curves of different orders, an embodiment is also provided in the following, which mainly performs a smooth connection process for multiple complex paths with different bezier curve orders (mainly first order, third order and fifth order). In the splicing optimization scheme provided by the invention, it is assumed that one path to be optimized is formed by connecting a plurality of sub-paths with different Bessel orders.
For the path, three traversals of the path are strategically designed to solve the optimization sequence problem in multi-segment sub-path optimization. When the splicing strategy is implemented, the track needs to be optimized and smoothed respectively according to the sub-paths of the front section or the sub-paths of the front section and the rear section, so that the effect of smooth splicing is achieved.
In this embodiment, the first, third and fifth-order bezier curves are mainly spliced by using the fifth-order bezier curve to perform path splicing, or the third-order bezier curve is used to splice the first and third-order bezier curves.
Meanwhile, when the path is spliced, according to the control point coordinates of the sub paths of the front section and the rear section of Bezier curves, the information of the first derivative and the second derivative of the Bezier curves at the connection points can be obtained by the derivative formula of the Bezier curves, so that the control point coordinates of the optimized Bezier curves can be reasonably arranged, the first derivative of the Bezier curves is equal to the first derivative of the front section and the second derivative of the rear section of the Bezier curves at the connection points, and the tangential continuity and the curvature continuity of the mobile robot at the connection points of the paths can be realized.
Specifically, the method comprises the following steps:
s1, traversing a plurality of sections of preset sub-paths, and screening sub-paths which cannot be optimized; wherein the non-optimizable sub-path includes at least one of the following sub-paths: a first-order Bezier curve sub-path, a preset locked sub-path.
And S2, taking the non-optimizable sub-paths as initial sub-paths, sequentially traversing the sub-paths to the two ends of the initial sub-paths to screen whether first-order Bezier curve sub-paths and third-order Bezier curve sub-paths exist in each optimizable sub-path, if so, optimizing the optimizable sub-paths according to a first rule, then carrying out smoothing treatment, and if not, executing the step S3.
Wherein the first rule comprises: when the paths with the third-order Bezier curves exist in the screening, the number of the first-order Bezier curve paths in the paths at two adjacent ends of the paths is counted, and classification optimization is carried out according to a third method.
Wherein the third rule comprises: when the number n1 = 1 of the first-order Bezier curve sub-paths in the paths at two adjacent ends, setting the control points to select in the tangential direction of the first-order Bezier curve sub-paths adjacent to the control points;
When n1=2, setting control points of adjacent first-order Bezier curve sub-paths, and selecting the control points at the intersection points of extension lines of the two adjacent first-order Bezier curve sub-paths;
when n1=0, counting the number n2 of third-order Bezier curves in the adjacent two terminal paths;
When n2=1, judging whether the front-stage sub-path is a five-order Bezier curve and is locked, if not, smoothing by taking a three-order Bezier curve as a reference path; if yes, smoothing by taking the five-order Bezier curve with the locked front end as a reference path;
when n2=2, smoothing processing is performed by taking the former-stage third-order Bezier curve as a reference path;
when n2=0, judging whether the front-stage path is a sub-path of the five-order Bezier curve and is locked, if so, performing smoothing processing by taking the five-order Bezier curve with the locked front end as a reference path.
S3, screening whether five-order Bezier curve sub-paths exist in each optimizable sub-path, if so, optimizing the rest optimizable sub-paths according to a second rule, and then performing smoothing treatment; wherein the second rule comprises: judging whether the front and rear two-section sub-paths of the five-order Bezier curve sub-path are five-order Bezier curves, if so, optimizing according to a first strategy according to the types of the front and rear two-section paths.
In consideration of the problem of smooth connection between two adjacent Bezier curves and the Bezier curve, the information of the control points of the front and rear sub-paths is assumed to be known, and a middle splicing sub-path is designed according to the known information, so that the front and rear sub-paths are connected, and the effect of continuous tangential direction and continuous curvature is achieved at the joint.
The smooth sub-path designed for this embodiment is preferably a three-order or five-order bezier curve, and in order to satisfy the condition that the tangential direction and the curvature are continuous, the selection method of the bezier curve control point is provided in this embodiment according to the information of the front and rear segment sub-paths. Wherein a smooth path in the present invention is defined as a path that is continuous in tangential direction and continuous in curvature.
Specifically: wherein the first policy comprises:
if the front and rear sub-paths are both first-order Bezier curves, the spliced sub-paths are optimized by adopting fifth-order Bezier curves;
if the front and rear sub-paths are both first-order Bezier curves, the spliced sub-paths are optimized by adopting third-order Bezier curves;
if the front segment sub-path is a first-order Bezier curve, and the rear segment sub-path is a fifth-order Bezier curve, the spliced sub-path is optimized by adopting the fifth-order Bezier curve;
if the front segment sub-path is a first-order Bezier curve, and the rear segment sub-path is a fifth-order Bezier curve, the splicing sub-path is optimized by adopting a third-order Bezier curve;
If the front segment sub-path is a five-order Bezier curve, and the rear segment sub-path is also a five-order Bezier curve, the spliced sub-path is optimized by adopting the five-order Bezier curve;
if the front segment sub-path is a five-order Bezier curve and the rear segment sub-path is a three-order Bezier curve, the spliced sub-path is optimized by adopting the five-order Bezier curve;
If the front segment sub-path is a third-order Bezier curve, and the rear segment sub-path is a fifth-order Bezier curve, the spliced sub-path is optimized by adopting the fifth-order Bezier curve;
and C8, if the front-stage sub-path is a third-order Bezier curve, and the rear-stage sub-path is also a third-order Bezier curve, the spliced sub-path is optimized by adopting a fifth-order Bezier curve.
Wherein the smoothing step includes:
D1, according to the coordinates of the control points of the Bezier curve of the front-section sub-path, solving the first derivative and the second derivative of the front-section sub-path at the connecting point by a derivation formula of the Bezier curve;
d2, according to the coordinates of the control points of the Bezier curve of the back-end sub-path, solving the first derivative and the second derivative of the back-end sub-path at the connecting point by a derivation formula of the Bezier curve;
D3 sets the coordinates of the control points of the optimizable sub-paths so that the first derivative and the second derivative of the bezier curve at the front and rear connection points are equal to the first derivative and the second derivative at the front/rear section sub-path connection points respectively.
The design principle is as follows: considering the front and back sub-paths, selecting the control points of the third-order or fifth-order Bezier curves of the evaluation sub-paths according to the first-order and second-order derivatives at the tail end points of the front sub-paths and the first-order and second-order derivatives at the front end points of the back sub-paths, so that the tangential directions of the connection points of the three-order or fifth-order Bezier curves and the front and back sub-paths are continuous, and the curvature is continuous.
Wherein the derivative formula for the Bezier curve is expressed as:
Where n is the order of the Bezier curve, P i is the ith control point of the Bezier curve, and P (0) =P 0,P(1)＝Pn, Wherein the first derivative can be expressed as:
wherein D i＝n(Pi+1-Pi), the first derivative of the bezier curve at the end point can be expressed as:
Wherein the method comprises the steps of Representing the first derivative value thereof at the start point,/>Representing the first derivative value at the end and the second derivative value/>, at the start and end, of the Bezier curve, similarly obtainableAnd/>
By the formula, the values of the first-order derivative and the second-order derivative of the front-section and the back-section Bezier curve sub-paths at the splicing points can be conveniently calculated, and the values of the same intermediate Bezier curve splicing sub-paths at the end points can be represented by the control points to be determined.
The requirement of path splicing is that the tangential direction of the path at the connecting point is continuous with the curvature, and the Bezier curve can be formed by a parameter equationAnd (5) determining. From the calculation formula of curvature:
The unit tangent vector calculation formula:
It can be seen that the first derivative and the second derivative of the two paths at the connecting point are continuous, so that the tangential continuity and curvature continuity of the paths at the splicing point can be ensured.
Wherein the third loop performed in step S3 involves a smooth connection problem for different types of curve paths. The second loop in step S2 can simplify the problem of splicing the single-ended path from the problem of splicing the track of the double-section path.
Thus, in the further optimization process, the optimization is mainly performed for smooth connection of the spliced tracks of the two sub-paths. On the other hand, the present embodiment has studied the problem of smooth connection of tracks, where smooth is defined as tangential continuous and curvature continuous. Smooth splicing of the segmented paths can enable the robot to run more stably, and accuracy of track tracking can be improved. According to the type of the paths encountered during actual splicing, paths consisting of a first-order Bezier curve (straight line), a third-order Bezier curve and a fifth-order Bezier curve are considered respectively. Aiming at actual demands, the scheme only considers the problem of path smoothness under 8 conditions. And, the more complex line smoothing problem can be solved by combining these 8 cases, and the optimization problem of the single track required in the second cycle can be obtained by simplifying the case in these 8 cases.
(II)
In a second aspect of the present invention, there is also provided a navigation path curvature continuous stitching optimization processing device, including: the mobile robot acquires navigation data through the scanning module, transmits the navigation data to the navigation path module, generates a plurality of pieces of sub-path data after processing, and transmits the sub-path data to the path splicing processing module, and the path splicing processing module executes degeneracy splicing processing on the linear path track according to the degeneracy splicing method containing the linear path track.
(III)
In a second aspect of the present invention, there is also provided a readable storage medium having stored thereon a computer program, wherein the computer program, when executed by a processor, performs the steps of the above-described degenerate stitching method with linear path trajectories.
In summary, the pair of degenerated splicing methods, the device and the storage medium thereof containing the linear path tracks provided by the invention overcome the defects that the five-order or more Bezier curve has too high degree of freedom and the three-order Bezier curve has insufficient degree of freedom when the Bezier curve is used for smoothly splicing two linear paths, and simultaneously can reduce the complexity of a user in use while keeping the path adjustment degree of freedom.
In addition, the invention can effectively lead the Bezier curve path and the straight line path to be continuous in tangential direction and curvature at the intersection point, thereby leading the robot to run more stably. The tangential direction of the visible path is continuous, which means that the angle change of the robot is continuous, the curvature of the path is continuous, which means that the change rate of the angular speed of the robot is continuous, and the accuracy and the efficiency of the track tracking control of the robot are improved.
The preferred embodiments of the invention disclosed above are intended only to assist in the explanation of the invention. The preferred embodiments are not exhaustive or to limit the invention to the precise form 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 understand and utilize the invention. The invention is to be limited only by the following claims and their full scope and equivalents, and any modifications, equivalents, improvements, etc., which fall within the spirit and principles of the invention are intended to be included within the scope of the invention.
It will be appreciated by those skilled in the art that the system, apparatus and their respective modules provided by the present invention may be implemented entirely by logic programming method steps, in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers, etc., except for implementing the system, apparatus and their respective modules provided by the present invention in a purely computer readable program code. Therefore, the system, the apparatus, and the respective modules thereof provided by the present invention may be regarded as one hardware component, and the modules included therein for implementing various programs may also be regarded as structures within the hardware component; modules for implementing various functions may also be regarded as being either software programs for implementing the methods or structures within hardware components.
Furthermore, all or part of the steps in implementing the methods of the embodiments described above may be implemented by a program, where the program is stored in a storage medium and includes several instructions for causing a single-chip microcomputer, chip or processor (processor) to execute all or part of the steps in the methods of the embodiments of the application. And the aforementioned storage medium includes: a usb disk, a removable hard disk, a Read-only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.
In addition, any combination of various embodiments of the present invention may be performed, so long as the concept of the embodiments of the present invention is not violated, and the disclosure of the embodiments of the present invention should also be considered.
Claims (4)
1. A method of degenerate stitching for a path comprising a straight line, the steps comprising:
S1, traversing a plurality of sections of preset sub-paths, and screening the optimized sub-paths;
S2, judging whether the existing number of the linear sub-paths in the front and rear adjacent paths is equal to two by taking the optimizable sub-paths as a starting point, and if so, optimizing the optimizable sub-paths according to a first rule;
S3, performing degenerate smoothing on the optimized sub-path optimized in the step S2;
wherein the first rule comprises: the sub-path can be optimized, and the sub-path track is spliced by adopting an N-order Bezier curve, wherein N is more than or equal to 5;
The degenerate smoothing step comprises:
D1 Calculating first-order and second-order derivative data of the linear sub-path;
D2, making the first and second derivatives of the optimized Bezier curve of the optimizable sub-path equal to the first and second derivatives of the adjacent linear sub-path at the connection point;
D3, overlapping the optimized control points of the optimizable sub-paths according to a second rule;
the second rule includes overlapping control points according to at least one of the following:
Overlapping all control points to generate absolute control point coordinates;
or all the control points are divided into two groups, the control points in the groups are sequentially continuous, and the control points in each group are overlapped to generate a first control point and a second control point.
2. The method of degenerate stitching with a straight path trajectory as claimed in claim 1, wherein the step of calculating first and second derivatives of the bezier curve of the optimizable sub-path comprises:
Control point derivation formula for calculating Bezier curve
，
Wherein,Is the order of Bessel curve,/>Is Bezier curve/>Control points, andWherein the first derivative can be expressed as:
，
Wherein, The first derivative of the bezier curve at the end points can thus be expressed as:
，
Wherein, Representing the first derivative value thereof at the start point,/>Representing the first derivative value at the end of the curve, whereas the second derivative value/>, of the Bezier curve at the start and end pointsAnd/>The method comprises the following steps:
。
3. The utility model provides a navigation path curvature continuous concatenation optimizing process device which characterized in that includes: the navigation path planning module, the path splicing processing module, wherein the mobile robot obtains navigation data through the scanning module to transmit the navigation data to the navigation path module, generates a plurality of pieces of sub-path data after processing, and transmits the sub-path data to the path splicing processing module, and the path splicing processing module executes degenerate splicing processing on the linear path track according to the degenerate splicing method containing the linear path track as set forth in any one of claims 1 to 2.
4. A readable storage medium having stored thereon a computer program, which when executed by a processor performs the steps of the degenerate stitching method as claimed in any one of claims 1 to 2 for tracks containing straight paths.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111151249.9A CN113848922B (en) | 2021-09-29 | 2021-09-29 | Degenerate splicing method and device for tracks containing straight paths and storage medium thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111151249.9A CN113848922B (en) | 2021-09-29 | 2021-09-29 | Degenerate splicing method and device for tracks containing straight paths and storage medium thereof |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113848922A CN113848922A (en) | 2021-12-28 |
| CN113848922B true CN113848922B (en) | 2024-05-03 |
Family
ID=78977123
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111151249.9A Active CN113848922B (en) | 2021-09-29 | 2021-09-29 | Degenerate splicing method and device for tracks containing straight paths and storage medium thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113848922B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08249480A (en) * | 1994-12-15 | 1996-09-27 | Kao Corp | Cad system and device and method for converting data of bezier curve in cad system |
| CN107702723A (en) * | 2017-11-27 | 2018-02-16 | 安徽工程大学 | A kind of robot path planning method, storage medium and equipment |
| CN112269965A (en) * | 2020-08-10 | 2021-01-26 | 中国北方车辆研究所 | Continuous curvature path optimization method under incomplete constraint condition |
| CN113031592A (en) * | 2021-02-25 | 2021-06-25 | 杭州国辰机器人科技有限公司 | Robot path smoothing method and system based on fifth-order Bezier curve |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20200147796A1 (en) * | 2018-11-12 | 2020-05-14 | Cloudminds (Shenzhen) Robotics Systems Co., Ltd. | Moving method and device for a robot, robot, electronic apparatus and readable medium |
-
2021
- 2021-09-29 CN CN202111151249.9A patent/CN113848922B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH08249480A (en) * | 1994-12-15 | 1996-09-27 | Kao Corp | Cad system and device and method for converting data of bezier curve in cad system |
| CN107702723A (en) * | 2017-11-27 | 2018-02-16 | 安徽工程大学 | A kind of robot path planning method, storage medium and equipment |
| CN112269965A (en) * | 2020-08-10 | 2021-01-26 | 中国北方车辆研究所 | Continuous curvature path optimization method under incomplete constraint condition |
| CN113031592A (en) * | 2021-02-25 | 2021-06-25 | 杭州国辰机器人科技有限公司 | Robot path smoothing method and system based on fifth-order Bezier curve |
Non-Patent Citations (1)
| Title |
|---|
| 汽车造型特征的高效图像参数化重建;刘崇;《中国优秀硕士学位论文全文数据库 信息科技辑》;20150715;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113848922A (en) | 2021-12-28 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113686345B (en) | Multi-segment navigation path curvature continuous splicing optimization processor, method and storage medium | |
| US20200133289A1 (en) | Path tracking method and mobile robot using the same | |
| CN109947100B (en) | Path planning method and system and terminal equipment | |
| JP6717164B2 (en) | Operation route planning method | |
| CN105364928B (en) | Offline programming system | |
| CN109648571A (en) | Teaching trajectory reproducing method, system and the robot of industrial robot | |
| CN106227152A (en) | A kind of CNC processing smooth transient method and seamlessly transit device | |
| CN109048929B (en) | Method and device for generating motion trail of robot | |
| CN108413977A (en) | A kind of intelligent agricultural machinery automatic job exchange method and interactive system | |
| JP2004078516A (en) | Curve interpolation method | |
| KR20150104484A (en) | Method and apparatus for generating pathe of autonomous vehicle | |
| CN111123951B (en) | Biped robot and track following method and device thereof | |
| CN113848922B (en) | Degenerate splicing method and device for tracks containing straight paths and storage medium thereof | |
| CN111390902A (en) | Trajectory planning method, trajectory planning device, equipment and storage medium | |
| CN113865594B (en) | Method, device and storage medium for optimizing curvature continuous splicing of path segments containing circular arc navigation | |
| CN113650011B (en) | Method and device for planning splicing path of mechanical arm | |
| CN108279674A (en) | Intelligent moving method, device, robot and storage medium | |
| CN109254563A (en) | A kind of numerical control pie slice method and its filtering system | |
| CN106060896A (en) | Method and system for obtaining formation communication topology of unmanned aerial vehicle based on minimum arborescence | |
| CN109496286A (en) | Digital control system and its paths planning method and computer readable storage medium | |
| CN113741539B (en) | Unmanned aerial vehicle plant protection operation route planning method | |
| US12030185B2 (en) | Techniques for CAD-informed robotic assembly | |
| CN104317721A (en) | Regression test case selection method based on improved harmony search algorithm | |
| CN117666475B (en) | Continuous short line segment corner processing path smoothing method | |
| CN116520822B (en) | Smooth curvature parameterized representation path generation method |
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 |