CN113848922B - Method, device and storage medium for degenerate splicing of trajectory containing straight path - Google Patents

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

Method, device and storage medium for degenerate splicing of trajectory containing straight path

Technical Field

The present invention relates to the technical field of navigation path splicing optimization, and in particular to a degenerate splicing method, device and storage medium for a trajectory containing a straight path.

Background technique

Since Bezier curves are widely used in path planning, most of the current multi-segment path splicing algorithms and strategies only consider the path splicing problem composed of Bezier curves of different orders. However, in the actual path splicing problem of mobile robots, complex paths with multiple continuous straight lines that need to be spliced are often encountered. The splicing of straight line paths can only satisfy the tangential continuity of the connection points, and the curvature continuity cannot be guaranteed, which affects the robot's trajectory following accuracy and the smoothness of movement.

At this time, if a 3rd-order Bezier curve is used for splicing, when it is spliced with two straight lines, the two control points in the middle can only be located at the intersection of the extension lines of the two straight lines, and the curve lacks adjustability. When a 5th-order Bezier curve is used to splice two straight lines, although it can simultaneously meet the tangency with the straight line and maintain the adjustability of the curve. However, due to its excessive flexibility (having too many control points), it is difficult to adjust the linearity by manually dragging the control points. Therefore, the linearity can only be adjusted automatically through optimization methods. It can be seen that if manual adjustment is used, the complexity of manual adjustment during user use is increased, while automatic adjustment reduces the user's freedom of use and sacrifices a certain sense of experience.

Therefore, there is an urgent need in the art for a technology to solve the above problems, so as to reduce the complexity of user use while retaining the freedom of path adjustment, and to make the path tangent continuous and curvature continuous at the connection points.

Summary of the invention

The main purpose of the present invention is to provide a method, device and storage medium for degenerate splicing of trajectories containing straight path, so as to solve the problems in the background technology.

In order to achieve the above object, according to a first aspect of the present invention, a method for degenerate splicing of a trajectory containing a straight path is provided, the steps of which include:

S1 traverses multiple preset sub-paths and screens the sub-paths that can be optimized;

S2 takes the optimizable subpath as the starting point and determines whether the number of straight subpaths in the adjacent paths is equal to two. If yes, the optimizable subpath is optimized according to the first rule;

S3 performs degenerate smoothing processing on the optimizable subpath optimized in step S2.

The first rule includes: the sub-path can be optimized to use N-order Bezier curves to splice the sub-path trajectories, where N≥5.

Wherein, the degenerate smoothing processing step includes:

D1 calculates the first and second order derivative data of the straight line subpath;

D2 makes the first-order and second-order derivatives of the optimized Bezier curve of the optimized subpath equal to the first-order and second-order derivatives of the adjacent straight line subpath at the connection point;

D3 performs overlap processing on the control points of the optimized sub-path according to the second rule.

Among them, the second rule includes overlapping the control points according to at least one of the following methods: overlapping all control points to generate absolute control point coordinates; or all control points are divided into two groups, and the control points in the groups are in a continuous sequence, and the control points in each group overlap to generate the first control point and the second control point.

In a possible preferred implementation, the step of calculating the first-order and second-order derivatives of the Bezier curve that can optimize the subpath includes:

Solving the control point derivative formula of Bezier curve

Where n is the order of the Bezier curve, _Pi is the ith control point of the Bezier curve, and C(0) = _P0 , C(1) = _Pn , The first-order derivative can be expressed as:

Where, _Di = n(Pi ₊₁ - _Pi ), so the first-order derivative of the Bezier curve at the endpoint can be expressed as:

in, represents the first-order derivative value at the starting point,/> It represents the first-order derivative value at the end, and the second-order derivative value of the Bezier curve at the starting point and the end point/> With/> for:

In order to achieve the above-mentioned purpose, according to the second aspect of the present invention, there is also provided a navigation path curvature continuous splicing optimization processing device, which includes: a navigation path planning module, a path splicing processing module, wherein the mobile robot obtains navigation data through the scanning module to transmit it to the navigation path module, generates multiple segments of sub-path data after processing, and transmits them to the path splicing processing module, and the path splicing processing module performs degenerate splicing processing on the straight path trajectory according to the degenerate splicing method for the straight path trajectory containing the straight path as mentioned above.

In order to achieve the above-mentioned purpose, according to the second aspect of the present invention, a readable storage medium is further provided, on which a computer program is stored, wherein when the computer program is executed by a processor, the steps of the above-mentioned degenerate splicing method for a trajectory containing a straight path are implemented.

The degenerate splicing method, device and storage medium of the pair of straight path trajectories provided by the present invention have the following beneficial effects:

1. It overcomes the shortcomings of the fifth-order and above Bezier curves having too high a degree of freedom and the third-order Bezier curves having insufficient degrees of freedom when using Bezier curves to smoothly connect two straight paths. At the same time, it is able to reduce the complexity of user use while retaining the freedom of path adjustment.

2. At the same time, it can effectively make the Bezier curve path and the straight line path have continuous tangent direction and curvature at the intersection, so that the robot runs more smoothly.

3. The continuity of the tangent direction of the path means that the angle of the robot changes continuously, and the continuity of the curvature of the path means that the rate of change of the robot's angular velocity is continuous, which is conducive to improving the accuracy and efficiency of the robot's trajectory tracking control.

BRIEF DESCRIPTION OF THE DRAWINGS

The drawings constituting a part of this application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

FIG1 is a schematic diagram of the method steps of a first embodiment of the present invention;

FIG2 is a schematic diagram of a path after degeneration of an experimental example in the first embodiment of the present invention;

3 is a schematic diagram of adjusting control points on a degenerate path in the first embodiment of the present invention;

Detailed ways

The specific embodiments of the present invention are described in detail below. The following examples will help those skilled in the art to further understand the present invention, but do not limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiment of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiment of the present invention. Obviously, the described embodiment is only a part of the embodiment of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in the field without creative work should fall within the scope of protection of the present invention.

It should be noted that the terms "first", "second", "S1", "S2", "D1", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. In addition, unless otherwise clearly specified and limited, the terms "set", "layout", "install", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be an indirect connection through an intermediate medium, or it can be a connection between the two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to the specific circumstances and in combination with the prior art.

The degenerate splicing method for the pair of straight path trajectories provided by the present invention, in the following embodiments, mainly performs degenerate smoothing on multiple complex paths containing straight sub-path trajectories (straight lines, i.e., first-order Bezier curves). For this reason, in the splicing optimization scheme provided by the present invention, it is assumed that there is a path to be optimized composed of multiple Bezier curve sub-paths of different orders.

(one)

Specifically, as shown in FIG1 , the degenerate splicing method for the pair of trajectories containing straight path in this embodiment comprises the following steps:

S1 traverses multiple preset sub-paths, traverses each sub-path in the path, and determines the type of the sub-path to screen out the sub-path that can be optimized; if the traversed path is a straight sub-path or a sub-path that is locked and cannot be optimized, it is considered to be a non-optimizable path, so this sub-path is skipped and the traversal continues to the next one, thereby screening out the sub-path that can be optimized;

S2 When the traversed path is an optimizable sub-path, the optimizable sub-path is taken as the starting point to determine whether the number of straight sub-paths in the front and rear adjacent paths is equal to two. If so, it means that the current sub-path is spliced with two straight sub-paths at both ends, so it is the optimization target. Therefore, according to the first rule, the optimizable sub-path is optimized; the first rule includes: the optimizable sub-path uses N-order Bezier curves to splice sub-path trajectories, where N≥5.

S3 performs degenerate smoothing on the optimizable subpath optimized in step S2. The degenerate smoothing step includes: D1 calculating the first-order and second-order derivative data of the straight line subpath; D2 making the first-order and second-order derivatives of the Bezier curve of the optimized optimizable subpath equal to the first-order and second-order derivatives of the adjacent straight line subpath at the connection point; D3 performing coincidence processing on the control points of the optimized optimizable subpath according to the second rule.

The second rule includes performing coincidence processing on the control points according to at least one of the following methods:

1. Overlap all control points and generate absolute control point coordinates;

2. Or all control points are divided into two groups, and the control points in each group are continuous, and the control points in each group overlap to generate the first control point and the second control point.

The calculation steps of the first-order and second-order derivatives of the Bezier curve that can optimize the subpath include:

Solving the control point derivative formula of Bezier curve

Where n is the order of the Bezier curve, _Pi is the ith control point of the Bezier curve, and C(0) = _P0 , C(1) = _Pn , The first-order derivative can be expressed as:

Where, _Di = n(Pi ₊₁ - _Pi ), so the first-order derivative of the Bezier curve at the endpoint can be expressed as:

in, represents the first-order derivative value at the starting point,/> It represents the first-order derivative value at the end, and the second-order derivative value of the Bezier curve at the starting point and the end point/> With/> for:

Through the above formula, the values of the first-order and second-order derivatives of the Bezier path at the splicing point can be easily calculated, so that the first-order and second-order derivatives of the optimized Bezier curve of the optimized sub-path can be equal to the first-order and second-order derivatives of the adjacent straight line sub-path at the connection point, which can make the tangent direction and curvature between the spliced paths at the connection point continuous.

When the Bezier curve is on the x-y plane, its curvature at any time can be expressed as:

Specifically, since the straight line can be regarded as a special first-order Bezier curve C ₁ , its control points are the two end points of the straight line.

Therefore, the first-order derivative of its endpoint can be obtained as Where/> With/> are the two end points of the straight line, its second-order derivative is 0 and the curvature is 0. Among them,/> is the j-th control point of the i-th order Bezier curve.

For by The third-order Bezier curve C ₃ generated by four control points has a first-order derivative at the starting point of/> Its second-order derivative is/> The first-order derivative at the end point is/> Its second-order derivative is/>

For by The fifth-order Bezier curve C5 generated by six control points has a first-order derivative at the starting point of/> Its second-order derivative is/> The first-order derivative at the end point is/> Its second-order derivative is

When it is necessary to complete the smoothing from the straight sub-path to the third-order Bezier path. In order to make the straight sub-path and the Bezier path smooth, the first thing to do is to satisfy In order to ensure the continuity of the tangent direction of the straight line and the third-order Bezier curve, it is necessary to satisfy/> Where a is any real number that is not 0, that is, it is necessary to use/> In the direction of the tangent to the line.

Since the curvature of a straight line is 0, in order to make the curvature of the two paths equal at the junction, it can be seen from the curvature formula κ(t) of the Bezier curve at any time that we need to make With/> Collinear, that is, you need to make/> In the tangent direction of the line. Similarly, when you need to smooth the transition from a Bezier curve subpath to a straight line subpath, you also need to use /> Collinear with the following straight subpath.

Therefore, when using a third-order Bezier curve to smoothly connect the two straight lines, the control point Select the intersection of the two straight lines. At this time, because there are no extra movable control points, the generated Bezier curve cannot be adjusted.

Similar to the above analysis, when it is necessary to complete the smoothing from the straight line subpath to the fifth-order Bezier path, it is necessary to use And/> Collinear with the previous straight subpath. When you need to smooth from a fifth-order Bezier path to a straight subpath, you need to use/> Continuous at the collinear tangent point with the subsequent straight line subpath.

Therefore, when using the fifth-order Bezier curve to smoothly connect the two straight line sub-paths before and after, All four control points can be moved as long as they are in the tangent direction of the corresponding straight line. At this time, the user needs to arrange the four control points reasonably, but this will increase the complexity of user use.

For this purpose, the so-called degeneration in the present invention refers to the use of an N-order Bezier curve (N≥5) to smoothly connect the two straight line sub-paths before and after, such as using a fifth-order Bezier curve. With/> Overlap, /> With/> Overlap, that is By making the control points overlap, the original 4 control points that the user needs to specify are reduced to only 2 control points that the user needs to specify, that is, /> With/> Even 1 control point, that is /> Similar to the third-order Bezier curve, the degenerate fifth-order Bezier curve also requires the specification of two control points. The difference is that the two control points of the latter can be moved.

In addition, when the present invention splices two straight lines, although the above embodiment uses a fifth-order Bezier curve for degeneration, all control points are divided into two groups according to the order to form With/> coincide with the first control point,/> With/> In fact, a method for degenerating the seventh order can be similarly derived, that is, making the seventh-order Bezier path Overlap, /> Overlap. A degenerate ninth-order Bezier curve can also be derived similarly, that is, making the ninth-order Bezier path /> Overlap, /> The other higher-order degenerate Bezier curves are similar.

Similar to the even-order Bezier curve, for example, the sixth-order Bezier curve has 5 control points, because they cannot be evenly distributed into two groups, the method of degenerating the sixth-order Bezier curve control points can also arbitrarily divide all control points into two groups according to the order, such as Overlap, /> overlap; /> overlap; /> coincide, Overlap, etc., perform various permutations and combinations to simplify the first and second control points.

In addition, the degeneration of the aforementioned N-order Bezier curve can also combine all control points into one absolute control point, which can also achieve a degenerate effect.

Experimental example

Example: If the trajectories of the two sub-paths are both straight lines, use a degenerate fifth-order Bezier curve to connect them.

Specifically, the front path is a straight line y=x, x∈[-10, 0], and the back path is a straight line y=10, x∈[60, 90]. According to the control point selection rule, select is the end point of the straight line, The generated trajectory and control points are shown in Figure 2 in the attachment. When there are obstacles on the robot path or other reasons, and a new path needs to be redesigned, the two control points in the middle can be changed at will, such as/> The newly generated path is shown in Figure 3. Such a path can still be guaranteed to be smooth at the junction with the straight line.

In addition, in the above embodiments and experimental examples, only the degenerate splicing method for straight path trajectories is used. The subsequent smoothing algorithm between Bezier curves of different orders can refer to the following scheme or existing literature for processing.

For example, the curvature continuous splicing optimization method for arc navigation path segments includes the following steps:

Step S1 traverses multiple preset sub-paths, traverses each sub-path in the path, and determines the type of the sub-path to screen out optimizable sub-paths; if the traversed path is an arc trajectory sub-path, since the arc trajectory has no control points and is a non-optimizable path, this sub-path is skipped and traversal continues to the next one.

In step S2, when the traversed path is not an arc, since in the assumption, the navigation path includes: third-order and fifth-order Bezier curve sub-paths, and arc trajectory sub-paths, it is necessary to first determine whether it is one of the third-order or fifth-order Bezier curves, that is, the optimizable sub-path. At this time, the optimizable sub-path can be used as the starting point to determine the number of arc trajectory sub-paths in the adjacent path, and optimize the optimizable sub-path according to the first rule.

In this embodiment, the first rule includes:

Step C1: when the number of arc trajectory sub-paths in the adjacent path is one, determine the type of the currently optimizable sub-path: when its type is a third-order Bezier curve, optimize with the first scheme including: use the third-order Bezier curve to splice the sub-path trajectory; when its type is a fifth-order Bezier curve, optimize with the second scheme including: use the fifth-order Bezier curve to splice the sub-path trajectory;

Step C2: when the number of arc trajectory sub-paths in the adjacent path is two, determine the type of the current optimizable sub-path: when its type is a fifth-order Bezier curve, optimize with the third scheme including: use the fifth-order Bezier curve to splice the sub-path trajectories; when its type is a third-order Bezier curve, skip the sub-path and continue to traverse the next one;

In step C3, when the number of arc trajectory sub-paths in adjacent paths is zero, the connection problem of multiple Bezier curves should be considered. At this time, reference can be made to the solution of another application by the applicant, or the smoothing method mentioned in other documents can be used for processing, which is not limited in this embodiment.

Step S3 performs smoothing on the optimizable subpath optimized in step S2. The subpath smoothing in this embodiment is defined as a path where the tangent direction of the subpath at the connection point is continuous and the curvature is continuous.

For this reason, when the navigation track is smoothly connected, it is necessary to smoothly splice the track according to the derivative information of one arc path in the adjacent path, or the derivative information of two arc paths in the adjacent path. In this embodiment, the derivative information of the third-order or fifth-order Bezier curve at the connection point is preferably solved by the derivative formula of the Bezier curve, and the first-order and second-order derivatives of the Bezier curve and the first-order and second-order derivatives of the adjacent arc path can be equal at the connection point by reasonably arranging the coordinates of the control points of the Bezier curve. Thereby achieving the purpose of path smoothing.

Specifically, in a preferred embodiment, the smoothing step includes:

Step D1 calculates the first-order and second-order derivative data of the arc trajectory subpath;

Step D2 sets the coordinates of the control points of the optimized sub-path; making the first-order and second-order derivatives of its Bezier curve equal to the first-order and second-order derivatives of the adjacent arc trajectory sub-path at the connection point.

The derivative calculation steps of the arc trajectory sub-path include:

The arc trajectory parameter equation expression Calculate the first-order derivative at the end point of the arc path/> Second Derivative/> The first-order derivative at the front end of the rear path C ₃ /> Second Derivative/>

In a possible preferred embodiment, the derivative calculation step of the optimized optimizable subpath includes:

The derivative formula for solving the Bezier curve is:

Where n is the order of the Bezier curve, _Pi is the ith control point of the Bezier curve, and P(0) = _P0 , P(1) = _Pn , The first-order derivative can be expressed as:

Where D _i =n(P _i+1 -P _i ), so the first-order derivative of the Bezier curve at the endpoint can be expressed as:

in represents the first-order derivative value at the starting point,/> Indicates the first-order derivative value at the end. Similarly, the second-order derivative value of the Bezier curve at the starting point and the end can be obtained. With/>

By using the above formula, we can calculate the first and second order derivatives of the Bezier curve path at the front and back endpoints (i.e. the splicing points) after a given control point. The requirement for path splicing is to make the path tangent direction and curvature continuous at the connection point. The Bezier curve can be expressed by the parametric equation Determined by the curvature calculation formula:

The calculation formula for the unit tangent vector is:

From this, it can be seen that the continuity of the tangent direction and the curvature of the paths at the splicing points can be guaranteed only by making the first-order derivatives and second-order derivatives of the two paths continuous at the connection points.

On the other hand, in order to implement a smoothing algorithm between subsequent Bezier curves of different orders, this case also provides an implementation scheme in the following, which mainly performs smooth connection processing on multiple complex paths with different Bezier curve orders (mainly first order, third order and fifth order). For this reason, in the splicing optimization scheme provided by the present invention, it is assumed that there is a path to be optimized that is connected by several sub-paths with different Bezier orders.

For this path, the strategy is designed to traverse the path three times to solve the optimization order problem when optimizing multiple sub-paths. When implementing the splicing strategy, it is necessary to optimize and smooth the trajectory according to the sub-path of the previous segment or the sub-paths of the previous and next segments, so as to achieve the effect of smooth splicing.

Therefore, in the conception of this embodiment, the fifth-order Bezier curve is mainly used to perform path splicing on the first-order, third-order and fifth-order Bezier curves, or the third-order Bezier curve is used to splice the first-order and third-order Bezier curves.

At the same time, when performing path splicing, according to the control point coordinates of the front and rear Bezier curve sub-paths, the first-order derivative and second-order derivative information at the connection point can be obtained from the derivation formula of the Bezier curve. Therefore, the control point coordinates of the optimized Bezier curve can be reasonably arranged so that the first-order and second-order derivatives of the Bezier curve are equal to the first-order and second-order derivatives of the front and rear curves at the connection point, thereby realizing tangent continuity and curvature continuity of the mobile robot at the path connection point.

Specifically, the steps include:

S1 traverses multiple preset sub-paths to screen non-optimizable sub-paths; wherein the non-optimizable sub-paths include at least one of the following sub-paths: a first-order Bezier curve sub-path, and a preset locked sub-path.

S2 takes the non-optimizable subpath as the starting subpath, and traverses to both ends of it in turn to screen whether there is a first-order Bezier curve subpath or a third-order Bezier curve subpath in each optimizable subpath. If so, the optimizable subpath is optimized according to the first rule and then smoothed. If not, execute step S3.

The first rule includes: when a third-order Bezier curve sub-path is screened out, the number of first-order Bezier curve sub-paths in the adjacent two end paths is counted, and classification optimization is performed according to the third rule.

The third rule includes: when the number of first-order Bezier curve sub-paths in the adjacent two end paths n1=1, setting the control point to be selected in the tangent direction of the first-order Bezier curve sub-path adjacent to it;

When n1=2, the control point of the adjacent first-order Bezier curve subpath is set to the intersection of the extension lines of the two adjacent first-order Bezier curve subpaths;

When n1=0, count the number of third-order Bezier curves n2 in its adjacent two terminal paths;

When n2=1, determine whether the previous sub-path is a fifth-order Bezier curve and is locked. If not, use the third-order Bezier curve as the reference path for smoothing; if yes, use the fifth-order Bezier curve locked at the front end as the reference path for smoothing;

When n2=2, the first third-order Bezier curve is used as the reference path for smoothing;

When n2=0, it is determined whether the front path is a fifth-order Bezier curve subpath and is locked. If yes, the fifth-order Bezier curve locked at the front end is used as a reference path for smoothing.

S3 screens whether there is a fifth-order Bezier curve subpath in each optimizable subpath. If so, the remaining optimizable subpaths are optimized according to the second rule and then smoothed. The second rule includes: determining whether the front and rear subpaths of the fifth-order Bezier curve subpath are fifth-order Bezier curves. If so, optimizing according to the first strategy based on the types of the front and rear subpaths.

The problem of using a Bezier curve to smoothly connect two adjacent Bezier curves is taken into consideration. Assuming that the information of the control points of the front and rear sub-paths is known, an intermediate splicing sub-path is designed based on the known information to connect the front and rear sub-paths and achieve the effects of tangent direction continuity and curvature continuity at the connection point.

For this reason, the smooth subpath designed in this embodiment is preferably a third-order or fifth-order Bezier curve. In order to meet the conditions of tangent direction and curvature continuity, a method for selecting Bezier curve control points is provided in this embodiment based on the information of the previous and next subpaths. The smooth path in the present invention is defined as a path with continuous tangent direction and continuous curvature.

Specifically, the first strategy includes:

C1 If the two sub-paths before and after are both first-order Bezier curves, the splicing sub-path is optimized using a fifth-order Bezier curve;

C2: If both the previous and next sub-paths are first-order Bezier curves, the splicing sub-path is optimized using a third-order Bezier curve;

C3: If the first sub-path is a first-order Bezier curve and the second sub-path is a fifth-order Bezier curve, the splicing sub-path is optimized using the fifth-order Bezier curve;

C4: If the previous sub-path is a first-order Bezier curve and the subsequent sub-path is a fifth-order Bezier curve, the splicing sub-path is optimized using a third-order Bezier curve;

C5: If the first sub-path is a fifth-order Bezier curve and the second sub-path is also a fifth-order Bezier curve, the splicing sub-path is optimized using the fifth-order Bezier curve;

C6 If the first sub-path is a fifth-order Bezier curve and the second sub-path is a third-order Bezier curve, the splicing sub-path is optimized using the fifth-order Bezier curve;

C7: If the first sub-path is a third-order Bezier curve and the second sub-path is a fifth-order Bezier curve, the splicing sub-path is optimized using the fifth-order Bezier curve;

C8 If the previous sub-path is a third-order Bezier curve and the subsequent sub-path is also a third-order Bezier curve, the splicing sub-path is optimized using a fifth-order Bezier curve.

The smoothing step includes:

D1 solves the first-order derivative and second-order derivative of the previous subpath at the connection point according to the coordinates of the control points of the Bezier curve of the previous subpath and the derivative formula of the Bezier curve;

D2 solves the first-order derivative and second-order derivative of the latter subpath at the connection point according to the coordinates of the control points of the Bezier curve of the latter subpath and the derivative formula of the Bezier curve;

The D3 setting can optimize the coordinates of the control points of the subpath so that the first-order derivative and second-order derivative of the Bezier curve at the front and back connection points are equal to the first-order derivative and second-order derivative at the connection points of the front/back subpath, respectively.

The design principle is as follows: considering the front and rear sub-paths, the control points of the third-order or fifth-order Bezier curve of the evaluation sub-path are selected according to the first-order and second-order derivatives at the end point of the front sub-path and the first-order and second-order derivatives at the front end point of the rear sub-path, so that the tangent direction and curvature of the curve at the connection between the front and rear sub-paths are continuous.

The derivative formula for the Bezier curve is expressed as:

Where n is the order of the Bezier curve, _Pi is the ith control point of the Bezier curve, and P(0) = _P0 , P(1) = _Pn , The first-order derivative can be expressed as:

Where D _i =n(P _i+1 -P _i ), so the first-order derivative of the Bezier curve at the endpoint can be expressed as:

in represents the first-order derivative value at the starting point,/> Indicates the first-order derivative value at the end. Similarly, the second-order derivative value of the Bezier curve at the starting point and the end can be obtained./> With/>

Through the above formula, the values of the first-order and second-order derivatives of the front and rear Bezier curve sub-paths at the splicing point can be easily calculated. Similarly, the values of the middle Bezier curve splicing sub-path at the endpoints can also be expressed by the control points to be determined.

The requirement for path splicing is that the path should be continuous in tangent direction and curvature at the connection point. The Bezier curve can be expressed by the parametric equation Determined by the curvature calculation formula:

The calculation formula for the unit tangent vector is:

From this, it can be seen that the continuity of the tangent direction and the curvature of the paths at the splicing points can be guaranteed only by making the first-order derivatives and second-order derivatives of the two paths continuous at the connection points.

The third cycle in step S3 involves the problem of smooth connection of different types of curved paths. The second cycle in step S2 for the splicing of single-end paths can be simplified from the splicing of double-segment paths.

Therefore, in the further optimization process, the smooth connection of the two sub-path splicing trajectories is mainly optimized. On the other hand, the present embodiment studies the problem of smooth connection of trajectories, where smoothness is defined as continuity in the tangent direction and continuity in curvature. Smooth splicing of segmented paths can not only make the robot run more smoothly, but also improve the accuracy of trajectory tracking. According to the path type encountered during actual splicing, paths composed of first-order Bezier curves (straight lines), third-order Bezier curves and fifth-order Bezier curves are considered respectively. In view of actual needs, this scheme only considers path smoothing problems in 8 cases. Moreover, more complex line smoothing problems can be solved by combining these 8 cases, and the optimization problem of a single trajectory required in the above-mentioned second cycle can also be obtained by simplifying these 8 cases.

(two)

The second aspect of the present invention also provides a navigation path curvature continuous splicing optimization processing device, which includes: a navigation path planning module, a path splicing processing module, wherein the mobile robot obtains navigation data through the scanning module and transmits it to the navigation path module, generates multiple segments of sub-path data after processing, and transmits them to the path splicing processing module, and the path splicing processing module performs degenerate splicing processing on the straight path trajectory according to the degenerate splicing method for the straight path trajectory containing the straight path as mentioned above.

(three)

According to a second aspect of the present invention, a readable storage medium is provided, on which a computer program is stored, wherein when the computer program is executed by a processor, the steps of the above-mentioned method for degenerate splicing of trajectories containing straight path are implemented.

In summary, the degenerate splicing method, device and storage medium of a pair of straight path trajectories provided by the present invention overcome the shortcomings of the fifth-order and above Bezier curves having too high a degree of freedom and the third-order Bezier curves having insufficient degrees of freedom when using Bezier curves to smoothly splice two straight path trajectories. At the same time, the complexity of user use is reduced while retaining the freedom of path adjustment.

In addition, the present invention can also effectively make the Bezier curve path and the straight line path have continuous tangent direction and continuous curvature at the intersection, so that the robot runs more smoothly. It can be seen that the continuity of the tangent direction of the path means that the angle change of the robot is continuous, and the continuity of the curvature of the path means that the change rate of the robot's angular velocity is continuous, which is conducive to improving the accuracy and efficiency of the robot's trajectory tracking control.

The preferred embodiments of the present invention disclosed above are only used to help explain the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to the specific implementation methods described. Obviously, many modifications and changes can be made according to the content of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can understand and use the present invention well. The present invention is only limited by the claims and their full scope and equivalents. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Those skilled in the art can understand that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, the system, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be regarded as structures within the hardware component; the modules for implementing various functions can also be regarded as both software programs for implementing the method and structures within the hardware component.

In addition, all or part of the steps in the above-mentioned embodiment method can be completed by instructing the relevant hardware through a program, and the program is stored in a storage medium, including a number of instructions to enable a single-chip microcomputer, a chip or a processor to execute all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, RandomAccess Memory), disk or optical disk and other media that can store program codes.

In addition, various implementations of the embodiments of the present invention may be arbitrarily combined, and as long as they do not violate the concept of the embodiments of the present invention, they should also be regarded as the contents disclosed in the embodiments of the present invention.

Claims (4)

1. A method for degenerate splicing of a trajectory containing a straight path, characterized in that the steps include: S1 traverses multiple preset sub-paths and screens the sub-paths that can be optimized; S2 takes the optimizable subpath as the starting point and determines whether the number of straight subpaths in the adjacent paths is equal to two. If so, the optimizable subpath is optimized according to the first rule; S3: performing degenerate smoothing processing on the optimizable subpath optimized in step S2; The first rule includes: the sub-path can be optimized to use N-order Bezier curves to splice the sub-path trajectories, where N ≥ 5; The degenerate smoothing processing step comprises: D1 calculates the first and second order derivative data of the straight line subpath; D2 makes the first-order and second-order derivatives of the optimized Bezier curve of the optimized subpath equal to the first-order and second-order derivatives of the adjacent straight line subpath at the connection point; D3 performs coincidence processing on the control points of the optimized sub-paths according to the second rule; The second rule includes performing coincidence processing on the control points according to at least one of the following methods: Overlap all control points and generate absolute control point coordinates; Or all control points are divided into two groups, and the control points in each group are continuous in sequence, and the control points in each group overlap to generate the first control point and the second control point. 2. The method for degenerate splicing of a trajectory containing a straight path according to claim 1, characterized in that the step of calculating the first-order and second-order derivatives of the Bezier curve that can optimize the sub-path comprises: Solving the control point derivative formula of Bezier curve , in, is the order of the Bezier curve, /> It is the first/> of the Bezier curves control points, and The first-order derivative can be expressed as: , in, Therefore, the first-order derivative of the Bezier curve at the endpoint can be expressed as: , in, represents the first-order derivative value at the starting point,/> It represents the first-order derivative value at the end, and the second-order derivative value of the Bezier curve at the starting point and the end point/> With/> for: . 3. A navigation path curvature continuous splicing optimization processing device, characterized in that it includes: a navigation path planning module, a path splicing processing module, wherein the mobile robot obtains navigation data through a scanning module to transmit it to the navigation path module, generates multiple segments of sub-path data after processing, and transmits them to the path splicing processing module, and the path splicing processing module performs degenerate splicing processing on the straight path trajectory according to the degenerate splicing method for the straight path trajectory as described in any one of claims 1 to 2. 4. A readable storage medium having a computer program stored thereon, wherein when the computer program is executed by a processor, the steps of the degenerate splicing method for a trajectory containing a straight path as described in any one of claims 1 to 2 are implemented.