阅读说明：本技术 一种基于参数曲线优化的导航路径规划方法、系统 (Navigation path planning method and system based on parameter curve optimization ) 是由 郑南宁 简志强 张崧翌 张稼慧 陈仕韬 于 2021-09-06 设计创作，主要内容包括：本发明公开一种基于参数曲线优化的导航路径规划方法、系统,具体过程如下：根据定位、激光雷达、以及地图信息生成规划配置；根据规划配置,使用Lazy Theta星算法进行初始路径搜索,得到初始路径；对初始路径进行几何分析得到一部分关键点,对所述关键点进行扩增,得到剩下的关键点；对所有关键点进行三次样条插值,得到的参数曲线；定义优化目标函数,将所述参数曲线作为目标函数输入,关键点作为决策变量,输出为参数曲线碰撞风险和平滑性的测量值；通过数值优化的方法对目标函数进行数值优化,得到导航路径；对路径的碰撞风险和平滑性进行优化,从而得到最终路径,在优化过程中,结合数值优化与几何优化,以提升优化效果。(The invention discloses a navigation path planning method and a system based on parameter curve optimization, which comprises the following specific processes: generating planning configuration according to the positioning, the laser radar and the map information; according to the planning configuration, carrying out initial path search by using a Lazy Theta star algorithm to obtain an initial path; performing geometric analysis on the initial path to obtain a part of key points, and amplifying the key points to obtain the rest key points; performing cubic spline interpolation on all key points to obtain a parameter curve; defining an optimization objective function, inputting the parameter curve as an objective function, taking the key point as a decision variable, and outputting a measured value of the collision risk and the smoothness of the parameter curve; performing numerical optimization on the objective function by a numerical optimization method to obtain a navigation path; and optimizing the collision risk and the smoothness of the path to obtain a final path, and combining numerical optimization and geometric optimization in the optimization process to improve the optimization effect.)

1. A navigation path planning method based on parameter curve optimization is characterized by comprising the following specific processes:

generating planning configuration according to the positioning, the laser radar and the map information;

according to the planning configuration, carrying out initial path search by using a Lazy Theta star algorithm to obtain an initial path;

performing geometric analysis on the initial path to obtain a part of key points, and amplifying the key points to obtain the rest key points;

performing cubic spline interpolation on all key points to obtain a parameter curve;

defining an objective function, taking the parameter curve as the input of the objective function, taking the key point as a decision variable, and outputting the measured value of the collision risk and the smoothness of the parameter curve; and optimizing the objective function through geometric optimization and numerical optimization to obtain the navigation path.

2. The method of claim 1, wherein a Douglas-Puke algorithm is used when geometric analysis is performed on the initial path to obtain a portion of the keypoints.

3. The method for planning a navigation path based on parametric curve optimization according to claim 1, wherein the amplification of the key points is specifically as follows:

calculating each section of parameter curve S_i(u) and corresponding straight line segmentsMaximum euclidean distance d of_iWhile simultaneously obtaining a straight line segmentUpper and curved line segment S_i(u) point of maximum distanceIf d is_iGreater than a given tolerance to deviation σ, the point is pointedInserting keypoints as new keypointsAndto (c) to (d);

the above process is repeated until no new key points need to be insertedIn (1), obtaining the key pointCurve of sum parameter

4. The method of claim 1, wherein the navigation path is planned according to current key pointsCarrying out cubic spline interpolation to obtain a parameter curveIs a piecewise function, a parametric curve, with respect to a parameter uSection i of (S)_iThe expression of (u) is shown in equation (1):

wherein the content of the first and second substances,representing two key pointsAndof between, of Euclidean distance,/₀＝0，Andis a parameter curveThe unknown parameter of (1); solving for these unknownsAfter the parameters are parameterized, an expression with an exact parameter curve can be obtained.

5. The navigation path planning method based on parametric curve optimization according to claim 1, wherein the objective function is specifically:

wherein the content of the first and second substances,as a parametric curveThe measurement of the risk of collision is carried out,as a parametric curveThe measurement of the smoothness is carried out by measuring,as a parametric curveOffset of two end points from planned start and end points, w_c，w_sAnd w_oIs a preset weight.

6. The method of claim 1, wherein the parametric curve is optimized for navigation path planningCarrying out discretization: passing through the sampling pointTo represent a parametric curve, where t +1 is the number of sampling points; calculating a collision risk measureIs calculated as shown in formula (3)

Wherein the content of the first and second substances,indicates to leaveIn the nearest barrier point, Filtermean is a self-defined function, gamma is a preset parameter, and gamma is more than or equal to 0 and less than or equal to 1;

number curveMeasurement of smoothnessIs calculated as shown in equation (4):

the Euclidean distance between each sampling point and the central point of the connecting line of the front sampling point and the rear sampling point is calculated by the formula (4);

curve of parametersOffset of two end points from planned start and end pointsIs calculated as shown in equation (5):

wherein the content of the first and second substances,andare respectively the initial pathStarting and ending points of, τ^(s)And τ^(g)The start and end point tolerated offsets, respectively.

7. The navigation path planning method based on parametric curve optimization according to claim 1, wherein the parametric curve is optimized by a method combining numerical optimization and geometric optimization, specifically as follows: firstly, key points are matched by using a numerical optimization methodOptimizing, judging the distance between the optimized key points, deleting the corresponding key points if the distance is less than a preset value, and carrying out cubic spline interpolation on the rest key points to obtain a new parameter curveIf the obtained parameter curve has no collision, the optimization process is ended, otherwise, each section S of the parameter curve is judged_i(u) whether a collision occurs, and if so, finding S_i(u) point closest to obstacleInsert it into key pointPerforming the following steps; the above processes are repeated continuously until the maximum iteration number it is reached, and an optimized parameter curve is obtainedI.e. the final global path.

8. The navigation path planning system based on parameter curve optimization is characterized by comprising a planning configuration generation module, an initial path acquisition module, a key point acquisition module, a parameter curve acquisition module and a path calculation module;

the planning configuration generation module is used for generating planning configuration according to the positioning information, the laser radar information and the map information;

the initial path acquisition module is used for searching an initial path by using a Lazy Theta algorithm according to the planning configuration to obtain the initial path;

the key point acquisition module is used for performing geometric analysis on the initial path to obtain a part of key points, and amplifying the key points to obtain the rest key points;

the parameter curve acquisition module is used for carrying out cubic spline interpolation on all key points to obtain a parameter curve;

a path calculation module defines a target function, the parameter curve is used as the input of the target function, the key point is used as a decision variable, and the measured value of the collision risk and the smoothness of the parameter curve is output; and optimizing the objective function through geometric optimization and numerical optimization to obtain the navigation path.

9. A computer device, characterized by comprising a processor and a memory, wherein the memory is used for storing a computer executable program, the processor reads the computer executable program from the memory and executes the computer executable program, and the processor can realize the navigation path planning method based on parameter curve optimization according to any one of claims 1 to 7 when executing the computer executable program.

10. A computer-readable storage medium, in which a computer program is stored, which, when being executed by a processor, is capable of implementing a method for navigation path planning based on parametric curve optimization according to any one of claims 1 to 7.

Technical Field

The invention belongs to the technical field of autonomous mobile robots, and particularly relates to a navigation path planning method and system based on parameter curve optimization.

Background

Global path planning is one of the important modules in an autonomous mobile robotic system. The method aims to generate a path capable of guiding the robot to move from a current position to a destination and provide navigation for local behavior planning. There are two key elements in global path planning, first collision risk and second smoothness.

The traditional global path planning method is divided into two categories, namely a planning method based on graph search and a planning algorithm based on sampling. These methods can generate a path from the current position to the end point, but the generated path cannot meet the requirements of high smoothness and minimized collision risk, and further path smoothing and optimization are required.

The current researchers provide that an initial path is generated by using a planning algorithm of graph search, then an optimization objective function is constructed, and optimization of path collision risk and smoothness is realized by using a conjugate gradient descent method. Although these methods have achieved good results, they still have problems, and cannot stably optimize the path, and may even cause a decrease in path smoothness.

This problem arises because these methods construct an objective function using the coordinates of all points on the path as decision variables. This results in a defined optimization problem that is highly dimensional and difficult to solve. At the same time, excessive redundancy of decision variables also results in that part of the variables may adversely affect the optimization process.

However, these methods are difficult to stably promote smoothness while minimizing the collision risk of the path. The reason is that the coordinates of all sampling points of the path are taken as an optimization target, so that the dimensionality of the constructed optimization problem is too high.

Disclosure of Invention

In order to solve the problems in the prior art, the invention provides a new global path planning method and a system, firstly, a Lazy Theta star algorithm is used for searching an initial path, after the initial path is obtained, key points are extracted from the path, a parameter path is constructed according to the key points, an optimized objective function is further obtained, and the collision risk and the smoothness of the path are optimized, so that a final path is obtained.

In order to achieve the purpose, the invention adopts the technical scheme that: a navigation path planning method based on parameter curve optimization specifically comprises the following processes:

generating planning configuration according to the positioning, the laser radar and the map information;

according to the planning configuration, carrying out initial path search by using a Lazy Theta star algorithm to obtain an initial path;

performing geometric analysis on the initial path to obtain a part of key points, and amplifying the key points to obtain the rest key points;

performing cubic spline interpolation on all key points to obtain a parameter curve;

defining an objective function, taking the parameter curve as the input of the objective function, taking the key point as a decision variable, and outputting the measured value of the collision risk and the smoothness of the parameter curve; and optimizing the objective function through geometric optimization and numerical optimization to obtain the navigation path.

And when the initial path is subjected to geometric analysis to obtain a part of key points, a Douglas-Puke algorithm is used.

The key point amplification is as follows:

calculating each section of parameter curve S_i(u) and corresponding straight line segmentsMaximum euclidean distance d of_iWhile simultaneously obtaining a straight line segmentUpper and curved line segment S_i(u) point of maximum distanceIf d is_iGreater than a given tolerance to deviation σ, the point is pointedInserting keypoints as new keypointsAndto (c) to (d);

the above process is repeated until no new key points need to be insertedIn (1), obtaining the key pointCurve of sum parameter

According to the current key pointCarrying out cubic spline interpolation to obtain a parameter curveIs a piecewise function, a parametric curve, with respect to a parameter uSection i of (S)_iThe expression of (u) is shown in equation (1):

wherein the content of the first and second substances,representing two key pointsAndof between, of Euclidean distance,/₀＝0，Andis a parameter curveThe unknown parameter of (1); the expression with exact parameter curve can be obtained after solving the unknown parameters.

The objective function is specifically:

wherein the content of the first and second substances,as a parametric curveThe measurement of the risk of collision is carried out,as a parametric curveThe measurement of the smoothness is carried out by measuring,as a parametric curveOffset of two end points from planned start and end points, w_c，w_sAnd w_oIs a preset weight.

Versus parameter curveCarrying out discretization: passing through the sampling pointTo represent a parametric curve, where t +1 is the number of sampling points; calculating a collision risk measureIs calculated as shown in formula (3)

Wherein the content of the first and second substances,indicates to leaveIn the nearest barrier point, Filtermean is a self-defined function, gamma is a preset parameter, and gamma is more than or equal to 0 and less than or equal to 1;

number curveMeasurement of smoothnessIs calculated as shown in equation (4):

the Euclidean distance between each sampling point and the central point of the connecting line of the front sampling point and the rear sampling point is calculated by the formula (4);

curve of parametersTwo end points and the starting point and the end point of the planOffset of pointIs calculated as shown in equation (5):

wherein the content of the first and second substances,andare respectively the initial pathStarting and ending points of, τ^(s)And τ^(g)The start and end point tolerated offsets, respectively.

The method for optimizing the parameter curve by combining numerical optimization and geometric optimization specifically comprises the following steps: firstly, key points are matched by using a numerical optimization methodOptimizing, judging the distance between the optimized key points, deleting the corresponding key points if the distance is less than a preset value, and carrying out cubic spline interpolation on the rest key points to obtain a new parameter curveIf the obtained parameter curve has no collision, the optimization process is ended, otherwise, each section S of the parameter curve is judged_i(u) whether a collision occurs, and if so, finding S_i(u) point closest to obstacleInsert it into key pointPerforming the following steps;the above processes are repeated continuously until the maximum iteration number it is reached, and an optimized parameter curve is obtainedI.e. the final global path.

On the other hand, the invention provides a navigation path planning system based on parameter curve optimization, which comprises a planning configuration generation module, an initial path acquisition module, a key point acquisition module, a parameter curve acquisition module and a path calculation module;

the planning configuration generation module is used for generating planning configuration according to the positioning information, the laser radar information and the map information;

the initial path acquisition module is used for searching an initial path by using a Lazy Theta algorithm according to the planning configuration to obtain the initial path;

the key point acquisition module is used for performing geometric analysis on the initial path to obtain a part of key points, and amplifying the key points to obtain the rest key points;

the parameter curve acquisition module is used for carrying out cubic spline interpolation on all key points to obtain a parameter curve;

defining and defining a target function by a path calculation module, taking the parameter curve as the input of the target function, taking the key point as a decision variable, and outputting the decision variable as a measured value of the collision risk and the smoothness of the parameter curve; and optimizing the objective function through geometric optimization and numerical optimization to obtain the navigation path.

The computer equipment comprises a processor and a memory, wherein the memory is used for storing a computer executable program, the processor reads the computer executable program from the memory and executes the computer executable program, and the processor can realize the navigation path planning method based on parameter curve optimization when executing the computer executable program.

A computer-readable storage medium, in which a computer program is stored, and when the computer program is executed by a processor, the navigation path planning method based on parameter curve optimization according to the present invention can be implemented.

Compared with the prior art, the invention has at least the following beneficial effects:

a new global path planning algorithm is provided, and the method can effectively reduce the collision risk of the path and improve the smoothness of the path by optimizing the parameterized and represented global path; a new optimization objective function is defined, so that the performance of the path can be fully improved through the numerical optimization of the objective function, the numerical optimization and the geometric optimization are combined in the path optimization process, the new optimization objective function is provided, and the optimization effect is improved; based on the method, the global path is not re-planned in real time, and the global path is updated only when the current global path is collided or the evaluation of the current global path is inferior to that of a newly planned global path.

Drawings

Fig. 1 is a schematic diagram of a path navigation and planning method framework.

FIG. 2 is a schematic diagram of the addition and deletion of key points.

Fig. 3 is a diagram of a mobile robot and its system architecture.

Detailed Description

The flow of the method provided by the invention is shown in figure 1 and can be divided into four steps. First, a planned configuration is generated based on the positioning, lidar, and map information. And secondly, according to the planning configuration, performing initial path search by using a Lazy Theta star algorithm to obtain an initial path shown by a red line in the figure 1. And thirdly, performing geometric analysis on the initial path to obtain a part of key points, and then amplifying the key points to obtain the rest key points. All the key points obtained in the third step are shown as red dots in fig. 1. Cubic spline interpolation is performed on these key points, and the resulting parameter curve is shown as a green curve in fig. 1. And fourthly, defining an optimization objective function, taking the parameter curve as objective function input, taking the key point as a decision variable, and performing numerical optimization. In the optimization process, key points are inserted and deleted simultaneously: judging whether a new key point needs to be inserted between the two key points; and judging whether each key point is redundant or not, and deleting the key points if the key points are redundant. The above process is repeated until the termination condition is satisfied. The adjusted key points are shown as blue points in fig. 1, and the optimized curve is shown as blue curve in fig. 1.

After using Lazy Theta star algorithm, an initial path can be obtainedBut now the path obtainedRather than having a continuous curvature, the present invention contemplates obtaining a path that is coincident with the initial pathSimilar and meets the curve of curvature continuous requirement. The method adopted by the invention is to carry out the initial pathExtracting key points on the basis ofUsing key pointsCarrying out cubic spline interpolation to obtain a parameter curveParameter curve obtained by the above processAnd pathSimilarity (prevent the initial value of the subsequent path optimization from being too poor), and the fewer the number of key points, the better (reduce the problem of the subsequent optimization).

To achieve the above objective, the method used in the present invention is shown as algorithm 1. The algorithm is divided into two parts: initial path geometry analysis (geotryanalysis) and keypoint amplification (keypointss inclusion). The former is to use as few key points as possible to characterize the initial path, and the latter is to prevent the interpolated parameter curve from being too different from the initial path.

The geometry analysis used the Douglas-Peker algorithm, and the key point amplification was shown as algorithm 2.

According to the current key pointCarrying out cubic spline interpolation to obtain a parameter curveIs a piecewise function with respect to the parameter u. Curve of parametersSection i of (S)_iThe expression of (u) is shown in equation 1.

Wherein the content of the first and second substances,representing two key pointsAndof between, of Euclidean distance,/₀＝0，Andis a parameter curveIs unknown. The exact expression of the parameter curve can be obtained by solving the unknown parameters, and the solving method is to solve the equation set shown in the formula 2.

Wherein l_iThe meaning is the same as that in formula 1.

Obtaining a parameter curveThen, calculating each section of parameter curve S_i(u) and corresponding straight line segmentsMaximum euclidean distance d of_i. At the same time, a straight line segment is obtainedUpper and curved line segment S_i(u) point of maximum distanceIf d is_iGreater than a given tolerance to deviation σ, the point is pointedInserting keypoints as new keypointsAndin the meantime. The above process is repeated until no new key points need to be insertedIn (5), the amplification of the key point is completed. To this end, key points as indicated by the dots in FIG. 1 can be obtainedAnd the parameter curve shown by the curve

Parameter curve obtained in the previous stepAlbeit approximately optimal in efficiency and satisfies curvature continuity. However, it is not optimal in terms of collision risk and smoothness. In order to optimize the collision risk and the smoothness of the parameter curve, a classical idea is to perform numerical optimization.

In detail, an objective function is definedObjective functionIs input as a parametric curveObjective functionThe output of (a) is a measure of the parameter curve collision risk and smoothness. The invention only needs to carry out the objective function optimization through a numerical optimization methodOptimization is carried out, so that the parameter curve can be optimizedThe risk of collision and smoothness. In addition, due to the parametric curveIs composed of key pointsThe decision variables of the above optimization problem are, uniquely determined, actuallyThus, the objective functionCan also be recorded as

After theoretical calculation analysis, multiple tests and tests, the finally determined objective function is shown in formula 3.

Wherein the content of the first and second substances,as a parametric curveThe measurement of the risk of collision is carried out,as a parametric curveThe measurement of the smoothness is carried out by measuring,as a parametric curveOffset of two end points from planned start and end points, w_c，w_sAnd w_oIs a preset weight. To simplify the calculation of the objective function, a parametric curve is usedDiscretization is carried out: the parametric curve is represented by a number of sample points, i.e. by assumingWhere t +1 is the number of sample points.

The calculation of (c) is shown in equation 4.

Wherein the content of the first and second substances,indicates to leaveThe nearest obstacle point. FilterMean is a custom function as shown in algorithm 3. Gamma is a preset parameter and meets the condition that gamma is more than or equal to 0 and less than or equal to 1.

Using the Filtermean function in calculating a collision risk measureOnly the influence of a part of obstacles closest to the curve on the curve is considered, and all obstacles are not considered. The reason for this is that the collision risk mainly originates from close-range obstacles. If all obstacles are considered, the curve may not be far away from the close-distance obstacle in the optimization process, but may be far away from the far-distance obstacle, which is contrary to our expectation. The current method solvesThis problem is solved by setting a safety distance threshold value and not considering the distance between the obstacle and the path if the distance is larger than the threshold value. This approach is problematic: if the threshold value is set to be too large, the effect is difficult to be achieved in a narrow scene; if the threshold value is set too small, the optimization is restricted by the threshold value, and it is difficult to achieve the ideal situation. In contrast, the method of the present invention does not have the above-described problems.

Number curveMeasurement of smoothnessThe calculation of (c) is shown in equation 5.

And the formula (5) calculates the Euclidean distance between each sampling point and the central point of the connecting line of the front and rear sampling points. That is, when the sampling point is at the center point of the line connecting the preceding and following sampling points, the ideal smoothness is considered to be achieved, and the purpose of using the FilterMean function in the invention is to make the optimization focus on the part of the curve which is the most unsmooth.

Curve of parametersOffset of two end points from planned start and end pointsThe calculation of (c) is shown in equation 6.

Wherein the content of the first and second substances,andare respectively the initial pathStarting and ending points of, τ^(s)And τ^(g)The start and end point tolerated offsets, respectively. In the optimization process, the starting point and the end point are not regarded as constraints, but are regarded as optimization items, so that the condition of the starting point or the end point is prevented from being too poor to influence the optimization of the whole parameter curve.

So far, an objective function is given as a wholeThe method of (3). The optimization for the objective function is a non-linear unconstrained optimization problem and there is no analytical solution for the derivative of the objective function. Therefore, the COBYLA algorithm can be used for optimization.

However, it is not sufficient to optimize the parameter curve by numerical optimization alone. For example, in the scenario shown above in fig. 2, it is difficult to achieve a desired planned path using only numerical optimization methods to adjust the coordinates of the keypoints. In the scenario shown in the lower part of fig. 2, too close key points cause curve jitter, which is also difficult to solve through numerical optimization. In solving these problems, the insertion and deletion of key points is a great help to solve the problem, as shown in fig. 2.

Therefore, the present invention proposes a method that combines numerical optimization and geometric optimization (inserting and deleting key points) to optimize the parametric curve, as shown in algorithm 4. Firstly, key points are matched by using a numerical optimization methodAnd (6) optimizing. And judging the distance between the optimized key points, and deleting the corresponding key points if the distance is too small. Carrying out cubic spline interpolation on the rest key points to obtain a new parameter curveAnd if the obtained parameter curve has no collision, ending the optimization process. Otherwise, each section S of the parameter curve is judged_i(u) whether a collision occurs. If collision, S is found_i(u) point closest to obstacleInsert it into key pointIn (1). The above process is repeated continuously until the maximum iteration number it is reached, and the optimization process is ended. At this time, an optimized parameter curve can be obtainedI.e. the final global path.

The method is realized on a self-developed mobile robot and is integrated into the whole robot system. The robot and its system used in the present invention are shown in fig. 3. The chassis of the robot is provided with a wheel speed meter and a single-line laser radar, and a computing unit of the robot uses NVIDIA Xavier NX. The system of the robot is shown in fig. 3, using AMCL for localization, TEB planner for local planning, and smoothing the speed of the local planning output. In this framework, the global planner is not re-planned in real-time. The global path is updated only when the current global path is collided or the current global path is evaluated inferior to the newly planned global path.

In addition, the invention provides a navigation path planning system based on parameter curve optimization, which comprises a planning configuration generation module, an initial path acquisition module, a key point acquisition module, a parameter curve acquisition module and a path calculation module;

the planning configuration generation module is used for generating planning configuration according to the positioning information, the laser radar information and the map information;

the initial path acquisition module is used for searching an initial path by using a Lazy Theta algorithm according to the planning configuration to obtain the initial path;

the key point acquisition module is used for performing geometric analysis on the initial path to obtain a part of key points, and amplifying the key points to obtain the rest key points;

the parameter curve acquisition module is used for carrying out cubic spline interpolation on all key points to obtain a parameter curve;

defining and defining a target function by a path calculation module, taking the parameter curve as the input of the target function, taking the key point as a decision variable, and outputting the decision variable as a measured value of the collision risk and the smoothness of the parameter curve; and optimizing the objective function through geometric optimization and numerical optimization to obtain the navigation path.

The invention can also provide a computer device, which comprises a processor and a memory, wherein the memory is used for storing a computer executable program, the processor reads part or all of the computer executable program from the memory and executes the computer executable program, and when the processor executes part or all of the computer executable program, the navigation path planning method based on parameter curve optimization can be realized.

In another aspect, the present invention provides a computer-readable storage medium, in which a computer program is stored, and when the computer program is executed by a processor, the navigation path planning method based on parameter curve optimization according to the present invention can be implemented.

The computer equipment can adopt an onboard computer, a notebook computer, a desktop computer or a workstation.

The processor may be a Central Processing Unit (CPU), a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), or an off-the-shelf programmable gate array (FPGA).

The memory of the invention can be an internal storage unit of a notebook computer, a desktop computer or a workstation, such as a memory and a hard disk; external memory units such as removable hard disks, flash memory cards may also be used.

Computer-readable storage media may include computer storage media and communication media. Computer storage media includes volatile and nonvolatile, removable and non-removable media implemented in any method or technology for storage of information such as computer readable instructions, data structures, program modules or other data. The computer-readable storage medium may include: a Read Only Memory (ROM), a Random Access Memory (RAM), a Solid State Drive (SSD), or an optical disc. The Random Access Memory may include a resistive Random Access Memory (ReRAM) and a Dynamic Random Access Memory (DRAM).