Multi-segment navigation path curvature continuous splicing optimization processor, method and storage medium
阅读说明：本技术 多段导航路径曲率连续拼接优化处理机、方法及存储介质 (Multi-segment navigation path curvature continuous splicing optimization processor, method and storage medium ) 是由 王为科 赵越 于 2021-10-25 设计创作，主要内容包括：本发明提供了一种多段导航路径曲率连续拼接优化处理机、方法及存储介质,其中方法步骤包括：S1 遍历多段预设子路径,筛查不可优化子路径；S2 以不可优化子路径为起始子路径,依次向其两端遍历,以筛查各可优化子路径中是否存在一阶贝塞尔曲线子路径、三阶贝塞尔曲线子路径,如存在则根据第一法则对可优化子路径进行优化后,再进行平滑处理,如不存在则执行步骤S3；S3 筛查各可优化子路径中是否存在五阶贝塞尔曲线子路径,如存在,则根据第二法则对剩余可优化子路径进行优化后,再进行平滑处理。籍此使得各子路径在拼接交点处切线方向连续并且曲率连续。(The invention provides a multi-section navigation path curvature continuous splicing optimization processor, a method and a storage medium, wherein the method comprises the following steps: s1 traversing multiple preset sub paths, and screening the sub paths which cannot be optimized; s2, the sub-paths which are not optimized are taken as initial sub-paths, traversal is carried out to the two ends of the initial sub-paths in sequence, so that whether a first-order Bezier curve sub-path and a third-order Bezier curve sub-path exist in each optimized sub-path is screened, if yes, the optimized sub-paths are optimized according to a first rule, then smoothing is carried out, and if not, the step S3 is executed; s3, screening whether the sub-paths of the fifth-order Bezier curve exist in each sub-path which can be optimized, if yes, optimizing the rest sub-paths of the sub-paths which can be optimized according to the second rule, and then smoothing. Thereby making the sub-paths tangentially continuous and curvature continuous at the splice intersection.)
1. A multi-segment navigation path curvature continuous splicing optimization method is characterized by comprising the following steps:
s1 traversing multiple preset sub paths, and screening the sub paths which cannot be optimized;
s2, the sub-paths which are not optimized are taken as initial sub-paths, traversal is carried out to the two ends of the initial sub-paths in sequence, so that whether a first-order Bezier curve sub-path and a third-order Bezier curve sub-path exist in each optimized sub-path is screened, if yes, the optimized sub-paths are optimized according to a first rule, then smoothing is carried out, and if not, the step S3 is executed;
s3, screening whether the sub-paths of the fifth-order Bezier curve exist in each sub-path which can be optimized, if yes, optimizing the rest sub-paths of the sub-paths which can be optimized according to the second rule, and then smoothing.
2. The method for optimizing multi-segment navigation path curvature continuous stitching according to claim 1, wherein the first rule comprises: and when the third-order Bezier curve sub-paths exist, counting the number of the first-order Bezier curve sub-paths in the paths at two adjacent ends, and performing classification optimization according to a third rule.
3. The method for optimizing multi-segment navigation path curvature continuous stitching according to claim 1, wherein the second rule comprises: and judging whether the front and rear sub-paths of the sub-path of the fifth-order Bezier curve are the fifth-order Bezier curve or not, and if so, optimizing according to a first strategy according to the types of the front and rear sub-paths.
4. The method for optimizing multi-segment navigation path curvature continuous stitching according to claim 2, wherein the third method comprises: when the number n1=1 of the first-order Bezier curve sub-paths in the paths at the two adjacent ends, setting the control point of the first-order Bezier curve sub-path to be selected in the tangential direction of the first-order Bezier curve sub-path adjacent to the first-order Bezier curve sub-path;
when n1=2, setting a control point of a first-order Bezier curve sub-path adjacent to the control point, and selecting an intersection point of extension lines of two first-order Bezier curve sub-paths adjacent to the control point;
when n1=0, counting the number n2 of third-order Bezier curves in two adjacent terminal paths;
when n2=1, judging whether the front-stage sub-path is a fifth-order Bezier curve and is locked, if not, performing smoothing processing by taking the third-order Bezier curve as a reference path; if so, smoothing by taking the fifth-order Bezier curve with the locked front end as a reference path;
when n2=2, performing smoothing processing by taking a front-stage third-order Bezier curve as a reference path;
when n2=0, it is determined whether the previous path is a fifth-order bezier curve sub-path and locked, and if so, smoothing is performed with the fifth-order bezier curve with the front end locked as a reference path.
5. The method of optimizing multi-segment navigation path curvature stitching according to claim 3, wherein the first strategy comprises:
c1, if the front sub-path and the rear sub-path are both first-order Bezier curves, the splicing sub-paths are optimized by adopting a fifth-order Bezier curve;
c2, if the front sub-path and the rear sub-path are both first-order Bezier curves, the splicing sub-paths are optimized by adopting the third-order Bezier curves;
c3, if the front sub-path is a first-order Bezier curve and the rear sub-path is a fifth-order Bezier curve, the splicing sub-paths are optimized by adopting the fifth-order Bezier curve;
c4, if the front sub-path is a first-order Bezier curve and the rear sub-path is a fifth-order Bezier curve, the splicing sub-paths are optimized by adopting a third-order Bezier curve;
c5, if the front-stage sub-path is a five-order Bezier curve and the rear-stage sub-path is also a five-order Bezier curve, the splicing sub-paths are optimized by adopting the five-order Bezier curve;
c6, if the front-stage sub-path is a five-order Bezier curve and the rear-stage sub-path is a three-order Bezier curve, the splicing sub-path is optimized by adopting the five-order Bezier curve;
c7, if the front sub-path is a third-order Bezier curve and the rear sub-path is a fifth-order Bezier curve, the splicing sub-paths are 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 splicing sub-path is optimized by adopting a fifth-order Bezier curve.
6. The method for optimizing the continuous splicing of the curvatures of the multi-segment navigation path according to claim 1, wherein the smoothing step comprises:
d1, solving a first derivative and a second derivative of the front-segment sub-path at the connecting point according to the control point coordinates of the Bezier curve of the front-segment sub-path and a derivation formula of the Bezier curve;
d2, solving a first derivative and a second derivative of the back-stage sub-path at the connecting point according to the control point coordinates of the Bezier curve of the back-stage sub-path by a derivation formula of the Bezier curve;
d3 sets the coordinates of the control points of the optimized sub-path to make the first and second derivatives of the bezier curve at the front and back connection points equal to the first and second derivatives at the front/back connection points, respectively.
7. The method for optimizing the continuous splicing of the curvatures of the multi-segment navigation path according to claim 6, wherein the derivation formula of the Bezier curve is expressed as:
wherein the content of the first and second substances,for the order of the bezier curve,is the first of the Bezier curveA control point, and，where the first derivative can be expressed as:
whereinThus, the first derivative of the bezier curve at the endpoints can be expressed as:
whereinIndicating its first derivative value at the starting point,representing the first derivative value at the end, and the second derivative value at the start and end of the Bezier curveAnd：
。
8. the method of optimizing continuous stitching of multi-segment navigation path curvatures of claim 1, wherein the non-optimizable sub-path includes at least one of: first order bezier curve sub-paths, preset locked sub-paths.
9. The utility model provides a multistage navigation path curvature is concatenation optimization processor in succession which characterized in that includes: the mobile robot acquires navigation data through the scanning module to be transmitted to the navigation path module, generates multi-segment sub-path data after processing and transmits the multi-segment sub-path data to the path splicing processing module, and the path splicing processing module splices the multi-segment sub-paths by adopting at least one of the following modes: splicing paths of the Bezier curves with the first order, the third order and the fifth order of the sub-paths by using a fifth-order Bezier curve, or splicing the Bezier curves with the first order and the third order of the sub-paths by using a third-order Bezier curve; then, the path splicing processing module optimizes the Bezier curve sub-path for splicing according to the multi-segment navigation path curvature continuous splicing optimization method of any one of claims 1 to 8, and then performs smoothing.
10. A readable storage medium having stored thereon a computer program, wherein the computer program, when being executed by a processor, is adapted to carry out the steps of the method for optimization of a multi-segment navigation path curvature continuous stitching according to any one of the claims 1 to 8.
Technical Field
The invention relates to the technical field of navigation path splicing optimization, in particular to a multi-section navigation path curvature continuous splicing optimization processor, a method and a storage medium.
Background
In the prior art, most path planning algorithms can plan a single-section continuous curvature path or a path with no more than three sections of continuous curvatures, but in the navigation path planning of an actual mobile robot, the problem of continuous splicing of curvatures of more than three sections of paths is often encountered.
However, the existing path planning technology cannot better handle such a situation. Therefore, the splicing of each sub-path section of the mobile robot is very hard, and the continuity of the tangent direction and the continuity of the curvature of each sub-path at the splicing intersection cannot be achieved, so that the existing mobile robot cannot run stably at the splicing path section, the change rate of the angular speed of the mobile robot cannot be continuous easily, and the precision and the efficiency of the mobile track tracking control of the mobile robot are influenced.
Disclosure of Invention
The invention mainly aims to provide a multi-segment navigation path curvature continuous splicing optimization processor, a method and a storage medium, so that each sub-path is continuous in the tangential direction and curvature at a splicing intersection point.
In order to achieve the above object, according to a first aspect of the present invention, there is provided a method for optimizing continuous splicing of curvatures of a multi-segment navigation path, the method comprising:
s1 traversing multiple preset sub paths, and screening the sub paths which cannot be optimized;
s2, the sub-paths which are not optimized are taken as initial sub-paths, traversal is carried out to the two ends of the initial sub-paths in sequence, so that whether a first-order Bezier curve sub-path and a third-order Bezier curve sub-path exist in each optimized sub-path is screened, if yes, the optimized sub-paths are optimized according to a first rule, then smoothing is carried out, and if not, the step S3 is executed;
s3, screening whether the sub-paths of the fifth-order Bezier curve exist in each sub-path which can be optimized, if yes, optimizing the rest sub-paths of the sub-paths which can be optimized according to the second rule, and then smoothing.
In a possible preferred embodiment, the non-optimizable sub-path comprises at least one of: first order bezier curve sub-paths, preset locked sub-paths.
In a possible preferred embodiment, said first law comprises: and when the third-order Bezier curve sub-paths exist, counting the number of the first-order Bezier curve sub-paths in the paths at two adjacent ends, and performing classification optimization according to a third rule.
In a possible preferred embodiment, said second law comprises: and judging whether the front and rear sub-paths of the sub-path of the fifth-order Bezier curve are the fifth-order Bezier curve or not, and if so, optimizing according to a first strategy according to the types of the front and rear sub-paths.
In a possible preferred embodiment, the third method then comprises: when the number n1=1 of the first-order Bezier curve sub-paths in the paths at the two adjacent ends, setting the control point of the first-order Bezier curve sub-path to be selected in the tangential direction of the first-order Bezier curve sub-path adjacent to the first-order Bezier curve sub-path;
when n1=2, setting a control point of a first-order Bezier curve sub-path adjacent to the control point, and selecting an intersection point of extension lines of two first-order Bezier curve sub-paths adjacent to the control point;
when n1=0, counting the number n2 of third-order Bezier curves in two adjacent terminal paths;
when n2=1, judging whether the front-stage sub-path is a fifth-order Bezier curve and is locked, if not, performing smoothing processing by taking the third-order Bezier curve as a reference path; if so, smoothing by taking the fifth-order Bezier curve with the locked front end as a reference path;
when n2=2, performing smoothing processing by taking a front-stage third-order Bezier curve as a reference path;
when n2=0, it is determined whether the previous path is a fifth-order bezier curve sub-path and locked, and if so, smoothing is performed with the fifth-order bezier curve with the front end locked as a reference path.
In a possible preferred embodiment, wherein the first policy comprises:
c1, if the front sub-path and the rear sub-path are both first-order Bezier curves, the splicing sub-paths are optimized by adopting a fifth-order Bezier curve;
c2, if the front sub-path and the rear sub-path are both first-order Bezier curves, the splicing sub-paths are optimized by adopting the third-order Bezier curves;
c3, if the front sub-path is a first-order Bezier curve and the rear sub-path is a fifth-order Bezier curve, the splicing sub-paths are optimized by adopting the fifth-order Bezier curve;
c4, if the front sub-path is a first-order Bezier curve and the rear sub-path is a fifth-order Bezier curve, the splicing sub-paths are optimized by adopting a third-order Bezier curve;
c5, if the front-stage sub-path is a five-order Bezier curve and the rear-stage sub-path is also a five-order Bezier curve, the splicing sub-paths are optimized by adopting the five-order Bezier curve;
c6, if the front-stage sub-path is a five-order Bezier curve and the rear-stage sub-path is a three-order Bezier curve, the splicing sub-path is optimized by adopting the five-order Bezier curve;
c7, if the front sub-path is a third-order Bezier curve and the rear sub-path is a fifth-order Bezier curve, the splicing sub-paths are 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 splicing sub-path is optimized by adopting a fifth-order Bezier curve.
In a possible preferred embodiment, the smoothing step comprises:
d1, solving a first derivative and a second derivative of the front-segment sub-path at the connecting point according to the control point coordinates of the Bezier curve of the front-segment sub-path and a derivation formula of the Bezier curve;
d2, solving a first derivative and a second derivative of the back-stage sub-path at the connecting point according to the control point coordinates of the Bezier curve of the back-stage sub-path by a derivation formula of the Bezier curve;
d3 sets the coordinates of the control points of the optimized sub-path to make the first and second derivatives of the bezier curve at the front and back connection points equal to the first and second derivatives at the front/back connection points, respectively.
In a possible preferred embodiment, the derivation formula of the bezier curve is expressed as:
wherein the content of the first and second substances,for the order of the bezier curve,is the first of the Bezier curveA control point, and，where the first derivative can be expressed as:
whereinThus, the first derivative of the bezier curve at the endpoints can be expressed as:
whereinIndicating its first derivative value at the starting point,representing the first derivative value at the end, and the second derivative values of the Bezier curve at the start and endAnd：
。
in order to achieve the above object, according to a second aspect of the present invention, there is also provided a multi-segment navigation path curvature continuous stitching optimization processor, including: the mobile robot acquires navigation data through the scanning module to be transmitted to the navigation path module, generates multi-segment sub-path data after processing and transmits the multi-segment sub-path data to the path splicing processing module, and the path splicing processing module splices the multi-segment sub-paths by adopting at least one of the following modes: splicing paths of the Bezier curves with the first order, the third order and the fifth order of the sub-paths by using a fifth-order Bezier curve, or splicing the Bezier curves with the first order and the third order of the sub-paths by using a third-order Bezier curve; then, the path splicing processing module optimizes the sub-paths of the bezier curve used for splicing according to the optimization method for continuously splicing the curvatures of the multiple navigation paths in any one of the first aspect and the second aspect of the invention, and then performs smoothing processing.
To achieve the above object, according to a third aspect of the present invention, there is further provided a readable storage medium, on which a computer program is stored, wherein the computer program, when executed by a processor, implements the steps of the method for optimizing the continuous splicing of curvature of multiple navigation paths according to any one of the first and second aspects of the present invention.
The multi-section navigation path curvature continuous splicing optimization processor, the method and the storage medium provided by the invention have the beneficial effects that:
1. aiming at the problem of how to select a smooth sequence of the track under the multi-section Bessel path, a scheme of using three times of circulation to optimize in sequence is provided, various conditions possibly occurring in the optimization are comprehensively considered, and the problem of continuous splicing of the curvatures of the multi-section path is solved.
2. The Bezier curve control points are selected according to the path information of the front section and the rear section, so that the paths can be effectively continuous in the tangential direction at the intersection points and continuous in curvature, and the mobile robot can run more stably.
3. The tangent direction of the path is continuous, which means that the angle change of the robot is continuous, and the curvature of the path is continuous, which means that the change rate of the angular speed of the robot is continuous, thus being beneficial to improving the precision and efficiency of the tracking control of the robot track.
Drawings
The accompanying drawings, which are incorporated in and constitute a part of this application, illustrate embodiments of the invention and, together with the description, serve to explain the invention and not to limit the invention. In the drawings:
FIG. 1 is a flowchart illustrating step S1 according to a first embodiment of the present invention;
FIG. 2 is a flowchart illustrating step S2 according to the first embodiment of the present invention;
FIG. 3 is a flowchart illustrating step S3 according to the first embodiment of the present invention;
FIG. 4 is a schematic diagram of the splicing optimization of Experimental example 1 in the first embodiment of the present invention;
FIG. 5 is a schematic diagram of the splicing optimization of Experimental example 2 in the first embodiment of the present invention;
FIG. 6 is a schematic diagram of the splicing optimization of Experimental example 3 in the first embodiment of the present invention;
FIG. 7 is a schematic diagram of splicing optimization of Experimental example 4 in the first embodiment of the present invention;
FIG. 8 is a schematic diagram of the splicing optimization of Experimental example 5 in the first embodiment of the present invention;
FIG. 9 is a schematic diagram of splicing optimization of Experimental example 6 in the first embodiment of the present invention;
FIG. 10 is a schematic diagram of the splicing optimization of Experimental example 7 in the first embodiment of the present invention;
FIG. 11 is a schematic diagram of splicing optimization of Experimental example 8 in the first embodiment of the present invention;
FIG. 12 is a diagram illustrating an initialization of an overall path according to a first embodiment of the present invention;
fig. 13 is a schematic diagram of the overall path after being optimized in step S2 according to the first embodiment of the present invention;
fig. 14 is a schematic diagram of the overall path after being optimized in step S3 according to the first embodiment of the present invention.
Detailed Description
The following describes in detail embodiments of the present invention. The following examples will assist those skilled in the art in further understanding the invention, but are not intended to limit the invention in any way. It should be noted that variations and modifications can be made by persons skilled in the art without departing from the spirit of the invention. All falling within the scope of the present invention.
It should be noted that the embodiments and features of the embodiments in the present application may be combined with each other without conflict. The present invention will be described in detail below with reference to the embodiments with reference to the attached drawings.
In order to make those skilled in the art better understand the technical solution of the present invention, the technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be obtained by a person skilled in the art without any inventive step based on the embodiments of the present invention, shall fall within the scope of the present invention.
It should be noted that the terms "first", "second", "S1", "S2", "C1", "D1", and the like in the description and claims of the present invention and the above-described drawings are used for distinguishing similar objects and not necessarily for describing a particular order or sequence. It is to be understood that the data so used is interchangeable under appropriate circumstances such that the embodiments of the invention described herein are capable of operation in sequences other than those illustrated or described herein. Furthermore, the terms "comprising" and "having," as well as any variations thereof, are intended to cover non-exclusive inclusions. Furthermore, unless expressly stated or limited otherwise, the terms "disposed," "mounted," "connected," and "connected" are to be construed broadly and may, for example, be fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; they may be connected directly or indirectly through intervening media, or they may be interconnected 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 combination with the prior art as the case may be.
In the following embodiments, the method for optimizing the continuous splicing of the curvatures of the multi-segment navigation path mainly aims at smoothly connecting multi-segment complex paths with different Bezier curve orders (mainly a first order, a third order and a fifth order). In the splicing optimization scheme provided by the invention, a path to be optimized is supposed to exist, wherein the path to be optimized is connected by a plurality of sub-paths with different Bessel orders.
Aiming at the path, three times of traversal of the path is strategically designed to solve the optimization sequence problem during the optimization of the multi-segment sub-path. When the splicing strategy is implemented, the track needs to be optimized and smoothed according to the sub-path of the front segment or the sub-paths of the front segment and the rear segment, so that the effect of smooth splicing is achieved.
In view of the concept of the invention, the paths of the first, third and fifth order bezier curves are spliced by using the fifth order bezier curve, or the paths of the first and third order bezier curves are spliced by using the third order bezier curve.
Meanwhile, when the paths are spliced, according to the coordinates of the control points of the sub paths of the Bezier curves at the front and rear stages, the first derivative and the second derivative information of the sub paths at the connecting points can be obtained through a derivative formula of the Bezier curves, so that the coordinates of the control points of the optimized Bezier curves can be reasonably arranged, the first second derivative of the Bezier curves is equal to the first second derivative of the curves at the front and rear stages at the connecting points, and the tangent continuity and the curvature continuity of the mobile robot at the connecting points of the paths can be realized.
(A)
Specifically, as shown in fig. 1 to 14, the method for optimizing continuous splicing of curvatures of multiple navigation paths provided by the first aspect of the present invention includes:
s1 as shown in FIG. 1, traversing multiple segments of preset sub-paths, and screening the sub-paths which are not optimized; wherein the non-optimizable sub-path comprises at least one sub-path of: first order bezier curve sub-paths, preset locked sub-paths.
S2 is as shown in fig. 2, the sub-paths that are not optimized are taken as the initial sub-paths, and the traversal is performed sequentially to both ends thereof to screen whether there exists a first order bezier curve sub-path and a third order bezier curve sub-path in each sub-path that is optimized, if yes, the sub-paths that are optimized according to the first rule, and then the smoothing process is performed, if not, the step S3 is performed.
Wherein the first rule comprises: and when the third-order Bezier curve sub-paths exist, counting the number of the first-order Bezier curve sub-paths in the paths at two adjacent ends, and performing classification optimization according to a third rule.
Wherein the third method comprises: when the number n1=1 of the first-order Bezier curve sub-paths in the paths at the two adjacent ends, setting the control point of the first-order Bezier curve sub-path to be selected in the tangential direction of the first-order Bezier curve sub-path adjacent to the first-order Bezier curve sub-path;
when n1=2, setting a control point of a first-order Bezier curve sub-path adjacent to the control point, and selecting an intersection point of extension lines of two first-order Bezier curve sub-paths adjacent to the control point;
when n1=0, counting the number n2 of third-order Bezier curves in two adjacent terminal paths;
when n2=1, judging whether the front-stage sub-path is a fifth-order Bezier curve and is locked, if not, performing smoothing processing by taking the third-order Bezier curve as a reference path; if so, smoothing by taking the fifth-order Bezier curve with the locked front end as a reference path;
when n2=2, performing smoothing processing by taking a front-stage third-order Bezier curve as a reference path;
when n2=0, it is determined whether the previous path is a fifth-order bezier curve sub-path and locked, and if so, smoothing is performed with the fifth-order bezier curve with the front end locked as a reference path.
S3, as shown in FIG. 3, screening whether a fifth-order Bezier curve sub-path exists in each optimizable sub-path, if so, optimizing the rest optimizable sub-paths according to a second rule, and then smoothing; wherein the second rule comprises: and judging whether the front and rear sub-paths of the sub-path of the fifth-order Bezier curve are the fifth-order Bezier curve or not, and if so, optimizing according to a first strategy according to the types of the front and rear sub-paths.
Considering the problem of smooth connection between a Bezier curve and two adjacent Bezier curves, assuming that the information of control points of the front and rear sub-paths is known, a middle splicing sub-path is designed through the known information to connect the front and rear sub-paths, and the effect of continuous tangential direction and curvature at the joint is achieved.
The smooth sub-path designed by the invention is preferably a third-order or fifth-order bezier curve, and in order to satisfy the condition of continuity of the tangential direction and the curvature, a method for selecting a control point of the bezier curve is provided in the embodiment according to the information of the front and rear sub-paths. Wherein a smooth path in the present invention is defined as a path in which the tangential direction is continuous and the curvature is continuous.
Specifically, the method comprises the following steps: wherein the first policy comprises:
c1, if the front sub-path and the rear sub-path are both first-order Bezier curves, the splicing sub-paths are optimized by adopting a fifth-order Bezier curve;
c2, if the front sub-path and the rear sub-path are both first-order Bezier curves, the splicing sub-paths are optimized by adopting the third-order Bezier curves;
c3, if the front sub-path is a first-order Bezier curve and the rear sub-path is a fifth-order Bezier curve, the splicing sub-paths are optimized by adopting the fifth-order Bezier curve;
c4, if the front sub-path is a first-order Bezier curve and the rear sub-path is a fifth-order Bezier curve, the splicing sub-paths are optimized by adopting a third-order Bezier curve;
c5, if the front-stage sub-path is a five-order Bezier curve and the rear-stage sub-path is also a five-order Bezier curve, the splicing sub-paths are optimized by adopting the five-order Bezier curve;
c6, if the front-stage sub-path is a five-order Bezier curve and the rear-stage sub-path is a three-order Bezier curve, the splicing sub-path is optimized by adopting the five-order Bezier curve;
c7, if the front sub-path is a third-order Bezier curve and the rear sub-path is a fifth-order Bezier curve, the splicing sub-paths are 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 splicing sub-path is optimized by adopting a fifth-order Bezier curve.
Wherein the smoothing step comprises:
d1, solving a first derivative and a second derivative of the front-segment sub-path at the connecting point according to the control point coordinates of the Bezier curve of the front-segment sub-path and a derivation formula of the Bezier curve;
d2, solving a first derivative and a second derivative of the back-stage sub-path at the connecting point according to the control point coordinates of the Bezier curve of the back-stage sub-path by a derivation formula of the Bezier curve;
d3 sets the coordinates of the control points of the optimized sub-path to make the first and second derivatives of the bezier curve at the front and back connection points equal to the first and second derivatives at the front/back connection points, respectively.
Wherein the design principle is as follows: and considering the front and rear sub-paths, selecting control points of a third-order or fifth-order Bezier curve of the evaluation sub-path according to the first-order and second-order derivatives at the tail 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 of the connection part of the control points and the front and rear sub-paths is continuous, and the curvature is continuous.
Wherein the derivation formula for a bezier curve is expressed as:
wherein the content of the first and second substances,for the order of the bezier curve,is the first of the Bezier curveA control pointAnd is and，where the first derivative can be expressed as:
whereinThus, the first derivative of the bezier curve at the endpoints can be expressed as:
whereinIndicating its first derivative value at the starting point,representing the first derivative value at the end, and the second derivative values of the Bezier curve at the start and endAnd：
。
through the formula, the numerical values of the first derivative and the second derivative of the Bezier curve sub-paths at the splicing points of the front section and the rear section can be conveniently calculated, and the numerical values of the intermediate Bezier curve splicing sub-paths at the end points can also 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 and curvature is continuous, and a Bezier curve can be formed by a parametric equationAnd (6) determining. From the formula of curvature:
the calculation formula of unit tangent vector is:
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 direction of the paths at the splicing point is continuous and the curvature is continuous.
As shown in fig. 13, a multi-step curve of the smoothed third-order bezier curve after the second cycle in step S2 is shown.
The third loop in step S3 involves a smooth connection problem for different types of curve paths. The second loop in step S2 can be simplified from the problem of trace splicing for a two-segment path.
Therefore, in the further optimization process, optimization is mainly performed on the smooth connection of the two sub-path splicing tracks, and as shown in fig. 14, a multi-segment curve of the smooth fifth-order bezier curve after the third loop is performed in step S3 is shown.
On the other hand, the invention investigates the problem of smooth joining of tracks, where smooth is defined as continuous in the tangential direction and continuous in curvature. The smooth splicing of the segmented paths not only can enable the robot to run more stably, but also can improve the precision of track tracking. According to the types of paths encountered in actual splicing, paths consisting of a first-order Bezier curve (straight line), a third-order Bezier curve and a fifth-order Bezier curve are respectively considered. Aiming at actual requirements, the scheme only considers the problem of smooth paths under 8 conditions. Moreover, the more complicated line smoothing problem can be solved by combining the 8 cases, and the optimization problem of the single track needed to be used in the second cycle can also be obtained by simplifying the 8 cases.
In the following experimental examples, use was made ofRepresents the j +1 control point of the ith bezier curve, and i =1, 2,3, j =0,1,2,3,4, 5.
Experimental example 1
Case 1: when the two front and rear tracks are straight lines and connected by using a 5 th order bezier curve, as shown in fig. 4.
Rule 1: since the tangent of the straight line is fixed and the second derivative thereof is constantly equal to 0, the control point selection rule can be simply analyzed without performing complicated operations through derivation. Thus 6 reference points can be chosen: the first one is selected at the end of the straight line and the second one is selected on the extension of the straight line so as to be in line with the tangent direction of the straight line. The third point is also selected in the direction of the extension of the previous line in order to make its second derivative 0.
Similarly, in order to ensure smooth connection between the path and the rear straight line, the fourth and fifth control points are selected from the extension line of the rear straight line, and the sixth control point is selected from the front end of the rear straight line. Note: the second and third control points are selected in the extension direction of the straight line and can be coincided, i.e. the second and third control points are selected in the extension direction of the straight line. Likewise, the fourth and fifth control points may also coincide, i.e.。
Example 1: the path of the front section is a straight lineThe rear end path is a straight lineSelecting according to the control point selection rule of case 1 =[0,0]Is the end point of the straight line,==[10,10], =[40,10], =[50,10],=[60,10]is the front end point of the rear segment straight line. The generated trajectory and control points are shown in fig. 4.
Experimental example 2
Case 2: when the front and rear two tracks are straight lines and are connected by using a 3-order bezier curve, as shown in fig. 5.
Rule 2: this case makes a special form of case 1, when the control pointTime, meansFour points coincide, and the bezier curve of order 5 in case 1 would then be degenerated to a bezier curve of order 3. At this point, 4 reference points need to be selected: the first one is selected from the front straight line and the second one is selected from the front straight line and the back straight lineThe fourth is selected at the front end of the following straight line at the intersection of the extension lines of the lines.
Example 2: the path of the front section is a straight lineThe rear end path is a straight lineAccording to the control point selection rule of case 2, selecting=[0,0]Is the end point of the straight line,==[10,10],=[60,10]is the front end point of the rear segment straight line. The generated trajectory and control points are shown in fig. 5.
Experimental example 3
Case 3: considering that the front-stage trajectory is a straight line and the rear-stage trajectory is a 5-order bezier curve, the 5-order bezier curve is used for splicing, as shown in fig. 6.
Rule 3: the control point of the later 5 th order Bezier curve is represented as (The middle 5 th order Bessel control point is represented by (. At this time, 6 reference points need to be selected in the following manner:
the end of the front straight line is selected,
the direction of the extension line of the front straight line is selected.
The front end of the back curve is selected so that=。
Selecting: should make sure thatAndaboutCentral symmetry, i.e.-=-To obtain=-+。
The selection of (a) should satisfy: (-)-(-)=(-)-(-) To obtain=-2*+2*。
Example 3: the path of the front section is a straight lineThe back-end path is a Bezier curve of 5 th order, an=[60,0],=[70,10],=[80,15] , =[110,20],=[120,30],=[140,50]. Selecting according to the control point selection rule of case 3=[0,0]Is the end point of the straight line,==[10,10],=[40,-25],=[50,-10],=[60,0]. The generated trajectory and control points are shown in fig. 6.
Experimental example 4
Case 4: when the front-stage trajectory is a straight line and the rear-stage trajectory is a 5-order bezier curve, the 3-order bezier curve is used for stitching, as shown in fig. 7.
Rule 4: the control point of the later 3 th order Bezier curve is represented as (The middle 5 th order Bessel control point is represented by (. At this time, 6 reference points need to be selected in the following manner:
the end of the front straight line is selected,
the direction of the extension line of the front straight line is selected.
The front end of the back curve is selected so that=。
The selection of (a) should satisfy: 3*-=5*(-) To obtain=-3*(-)/5。
The selection of (a) should satisfy: 3*2*[(-)-(-)] =5*4*[(-)-(-)]To obtain=3*(-2*+)/10-+2*。
Example 4: the path of the front section is a straight lineThe back-end path is a 3 rd order Bezier curve, an=[60,0],=[70,10],=[80,15] , =[90,30]. Selecting according to the control point selection rule of case 4=[0,0]Is the end point of the straight line,=[10,10],=[48,-13.5],=[54,-6],=[60,0]. The generated trajectory and control points are shown in fig. 7.
Experimental example 5
Case 5: when the front-stage trajectory is considered to be a 5-order bezier curve and the rear-stage trajectory is also a 5-order bezier curve, the 5-order bezier curve is used for splicing, as shown in fig. 8.
Rule 5: the 5 th order Bessel control point of the preceding paragraph is represented by (The control point of the latter 5 th order bezier curve is represented by (The middle 5 th order Bessel control point is represented by (. At this time, 6 reference points need to be selected in the following manner:
selecting the front straight line end to make=，
Selecting: should make sure thatAndaboutCentral symmetry, i.e.-=-To obtain=-+。
The selection of (a) should satisfy: (-)-(-)=(-)-(-) To obtain=-2*++ 2*-。
The front end of the back curve is selected so that=。
Selecting: should make sure thatAndaboutCentral symmetry, i.e.-=-To obtain=-+。
The selection of (a) should satisfy: (-)-(-)=(-)-(-) To obtain=-2*+2*。
Example 5: consider scenario 5 (when the front trace is a 5 th order bezier curve and the back trace is a 5 th order bezier curve, the 5 th order bezier curve is used for splicing)
The 5 th order bessel control points of the previous stage are:
=[-10，-10],=[0,0],=[5,20],=[20,10],=[30,20],=[40,10]the control point of the later 5 th order Bezier curve is=[140,50],=[150,60],=[160,65] , =[180,40],=[190,30] , =[200,50]Selecting according to the control point selection rule of case 5==[40,10]，=[50,0],=[60,-30],=[120,25],=[130,40],==[140,50]The generated trajectory and control points are shown in fig. 8.
Experimental example 6
Case 6: when the front-stage trajectory is considered to be a 5-order bezier curve and the rear-stage trajectory is considered to be a 3-order bezier curve, the 5-order bezier curve is used for splicing, as shown in fig. 9.
Rule 6: the 5 th order Bessel control point of the preceding paragraph is represented by (The control point of the 3 rd order bezier curve in the latter stage is represented by (The middle 5 th order Bessel control point is represented by (. At this time, 6 reference points need to be selected in the following manner:
selecting the front straight line end to make=，
Selecting: should make sure thatAndaboutCentral symmetry, i.e.-=-To obtain=-+。
The selection of (a) should satisfy: (-)-(-)=(-)-(-) To obtain=-2*++ 2*-。
The front end of the back curve is selected so that=。
The selection of (a) should satisfy: 3*(-)=5*(-) To obtain=-3*(-)/5。
The selection of (a) should satisfy: 3*2*[(-)-(-)] =5*4*[(-)-(-)]To obtain=3*(-2*+)/10-+2*。
Example 6: the 5 th order Bessel control point of the front section is
=[-10，-10],=[0,0],=[5,20],=[20,10],=[30,20],=[40,10]The control point of the later 5 th order Bezier curve is=[120,50],=[140,60],=[155,65] , =[160,40]Selecting according to the control point selection rule of case 6==[40,10]，=[50,0],=[60,-30],=[94.5,36.5],=[108,44],==[120,50]. The generated trajectory and control points are shown in fig. 9.
Experimental example 7
Case 7: when the front-stage trajectory is considered to be a 3-order bezier curve and the rear-stage trajectory is considered to be a 5-order bezier curve, the 5-order bezier curve is used for splicing, as shown in fig. 10.
Rule 7: the 3 rd order Bessel control point of the previous paragraph is represented by (The control point of the latter 5 th order bezier curve is represented by (The middle 5 th order Bessel control point is represented by (. At this time, 6 reference points need to be selected in the following manner:
selecting the front straight line end to make=，
The selection of (a) should satisfy: 5*(-)=3*(-) To obtain=3*(-)/5+。
The selection of (a) should satisfy: 5*4(-2*+)=3*2(-2*+) To obtain=3*(-2*+)/10+ 2*-。
The front end of the back curve is selected so that=。
Selecting: should make sure thatAndaboutCentral symmetry, i.e.-=-To obtain=-+。
The selection of (a) should satisfy: (-)-(-)=(-)-(-) To obtain=-2*+2*。
Example 7: the 3 rd order Bessel control point of the preceding stage is=[-10，-10], =[0,0],=[15,20],=[40,10]The control point of the later 5 th order Bezier curve is=[130,50],=[140,60],=[150,65] , =[170,40],=[175,30] , =[185,50]Selecting according to the control point selection rule of case 7=[40,10]，=[55,4],=[73,-11],=[110,25],=[120,40],=[130,50]. The generated trajectory and control points are shown in fig. 10.
Experimental example 8
Case 8: when the front-stage trajectory is considered to be a 3-order bezier curve and the rear-stage trajectory is also a 3-order bezier curve, the 5-order bezier curve is used for splicing, as shown in fig. 11.
Rule 8: the 3 rd order Bessel control point of the previous paragraph is represented by (The control point of the 3 rd order bezier curve in the latter stage is represented by (The middle 5 th order Bessel control point is represented by (. At this time, 6 reference points need to be selected in the following manner:
selecting the front straight line end to make=，
The selection of (a) should satisfy: 5*(-)=3*(-) To obtain=3*(-)/5+。
The selection of (a) should satisfy: 5*4(-2*+)=3*2(-2*+) To obtain=3*(-2*+)/10+ 2*-。
The front end of the back curve is selected so that=。
The selection of (a) should satisfy: 3*(-)=5*(-) To obtain=-3*(-)/5。
The selection of (a) should satisfy: 3*2*[(-)-(-)] =5*4*[(-)-(-)]To obtain=3*(-2*+)/10-+2*。
Example 8: the 3 rd order Bessel control point of the preceding stage is=[10,-10],=[23,20],=[30,30],=[35,10]The control point of the later 3 rd order Bezier curve is=[120,50],=[135,55],=[140,45] , =[150,40]Selecting according to the control point selection rule of case 8=[35,10]，=[38,-2],=[40.4,-23],=[99,39.5],=[111,47],=[120,50]. The generated trajectory and control points are shown in fig. 11.
In order to verify the correctness of the algorithm, in fig. 12, the initial path map uses a complex path including a plurality of sub-paths of different orders (1 st order, 3 rd order, 5 th order) to perform smooth connection of the tracks. It can be seen from fig. 12 that there is a significant tangential discontinuity in the initial path at the point of connection, and the continuity of curvature is less pronounced.
After the second loop (after optimizing the bezier curve of order 3) in step S2, the generated path is as shown in fig. 13, and the second loop optimizes the bezier curve of order three, which has been optimized differently for different adjacent curve types.
When the adjacent sub-path only has one straight line, the control point is selected on the extension line of the straight line, and when the adjacent path has two straight lines, the control point is selected at the intersection point on the extension lines of the two straight lines. After the third loop in step S3 (after the fifth-order bezier curve is optimized), the generated path is as shown in fig. 14, and it can be seen that the smoothness of the path is greatly improved by sequentially smoothing the remaining fifth-order bezier curves by the policy.
Thereby solving the problem of continuous splicing of multi-section path curvature. The navigation path of the mobile robot is continuous in the tangential direction at the intersection point and continuous in curvature, so 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 mobile robot are effectively improved.
(II)
In a second aspect of the present invention, there is provided a multi-segment path curvature continuous stitching processor for a mobile robot, which in a preferred embodiment comprises: the mobile robot acquires navigation data through the scanning module to be transmitted to the navigation path module, generates a plurality of sections of sub-path data after processing, and transmits the sub-path data to the path splicing processing module.
The path splicing processing module splices the multiple sub-paths by adopting at least one of the following modes: splicing paths of the Bezier curves with the first order, the third order and the fifth order of the sub-paths by using a fifth-order Bezier curve, or splicing the Bezier curves with the first order and the third order of the sub-paths by using a third-order Bezier curve; then, the path splicing processing module optimizes the sub-paths of the bezier curve used for splicing according to the optimization method for continuous splicing of the curvatures of the multiple navigation paths described in the first embodiment, and then performs smoothing processing.
(III)
In a third aspect of the present invention, there is provided the readable storage medium, in a preferred embodiment, having a computer program stored thereon, where the computer program, when executed by a processor, implements the steps of the method for optimizing continuous stitching of curvature of multiple navigation paths in the first embodiment.
The preferred embodiments of the invention disclosed above are intended to be illustrative only. The preferred embodiments are not intended to be exhaustive or to limit the invention to the precise embodiments disclosed. Obviously, many modifications and variations are possible in light of the above teaching. The embodiments were chosen and described in order to best explain the principles of the invention and the practical application, to thereby enable others skilled in the art to best utilize the invention. The invention is limited only by the claims and the full scope and equivalents thereof, and any modification, equivalent replacement, or improvement made within the spirit and principle of the invention should be included in the protection scope of the invention.
It will be appreciated by those skilled in the art that, in addition to implementing the system, apparatus and various modules thereof provided by the present invention in the form of pure computer readable program code, the same procedures may be implemented entirely by logically programming method steps such that the system, apparatus and various modules thereof provided by the present invention are implemented in the form of logic gates, switches, application specific integrated circuits, programmable logic controllers, embedded microcontrollers and the like. Therefore, the system, the device and the modules thereof provided by the present invention can be considered as a hardware component, and the modules included in the system, the device and the modules thereof for implementing various programs can also be considered as structures in the hardware component; modules for performing various functions may also be considered to be both software programs for performing the methods and structures within hardware components.
All or part of the steps of the method of the embodiments can be realized by instructing related hardware through a program, the program is stored in a storage medium, and the program comprises a plurality of instructions for enabling a single chip microcomputer, a chip or a processor (processor) to execute all or part of the steps of the method of the embodiments of the application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, and other various media capable of storing program codes.
In addition, any combination of various different implementation manners of the embodiments of the present invention is also possible, and the embodiments of the present invention should be considered as disclosed in the embodiments of the present invention as long as the combination does not depart from the spirit of the embodiments of the present invention.