A multi-constraint agent path smoothing method based on B-spline curve

Abstract

The application discloses a multi-constraint intelligent agent path smoothing method based on a B-spline curve and relates to the technical field of path planning.The technical points of the application include: acquiring multi-segment line path information and smoothing path constraint information;the multi-segment line path information includes starting segment multi-segment line path information, intermediate segment multi-segment line path information and terminal segment multi-segment line path information;the smoothing path constraint information includes starting point pose constraint, terminal point pose constraint and maximum curvature constraint;control points corresponding to an intelligent agent multi-segment line smoothing path are generated according to the multi-segment line path information and the smoothing path constraint information;global control points of the smoothing path are obtained according to the control points corresponding to the multi-segment line smoothing path, and the global control points are subjected to cubic quasi-uniform B-spline curve fitting to generate a global smoothing path.The application makes the smoothed path meet the starting point pose constraint, the terminal point pose constraint and strictly constrain the maximum curvature, and guarantees the approximation effect of the smoothing path on the multi-segment line path.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and more specifically to a multi-constraint intelligent agent path smoothing method based on B-spline curves. Background Technology

[0002] Path planning is one of the key technologies for achieving autonomous navigation of unmanned surface vessels (USVs). Classical path planning algorithms generate multi-segment paths composed of multiple path segments. When an USV follows a multi-segment path, it must stop at corners to complete a turn, which reduces the vessel's maneuverability and causes wear and tear on its components. Path smoothing is a secondary optimization step for multi-segment paths. The optimized path eliminates abrupt changes in the USV's speed and acceleration, improving the control accuracy of the unmanned equipment.

[0003] Unmanned surface vessel (USV) path planning methods can generate a polyline path composed of multiple line segments. However, in real-world operating environments, the starting and ending poses of the USV are subject to various constraints. Furthermore, to ensure the feasibility of the polyline path in practical operation and to prevent the USV from turning in place at the inflection points of the polyline path, path smoothing is necessary to guarantee the path C... 2 Continuity and maximum curvature limits.

[0004] Currently, the most common and frequently used methods for addressing the above problems are the Bézier curve method and the B-spline curve method. The Bézier curve method defines a continuous and smooth curve by adjusting the position and weight of control points to achieve a smooth path. However, the Bézier curve method cannot guarantee the C-axis stability of a smooth path. 2 Continuity may lead to points of high curvature in smooth paths; the B-spline curve method uses a set of basis functions and a series of control points to generate a parameterized path, although it can guarantee C 2 While maintaining continuity, existing B-spline curve-based algorithms cannot precisely control the maximum curvature of the path and cannot handle the pose constraints of the agent at the start and end points, resulting in poor approximation of polyline paths by smooth paths. Summary of the Invention

[0005] To address this issue, the present invention proposes a multi-constraint intelligent agent path smoothing method based on B-spline curves, aiming to solve or alleviate the above-mentioned problems.

[0006] A multi-constraint agent path smoothing method based on B-spline curves includes the following steps:

[0007] Step 1: Obtain polyline path information and smooth path constraint information; wherein the polyline path information includes the starting segment polyline path information, the middle segment polyline path information, and the ending segment polyline path information; the smooth path constraint information includes the starting point pose constraint, the ending point pose constraint, and the maximum curvature constraint;

[0008] Step 2: Generate control points corresponding to the polyline smooth path of the agent based on the polyline path information and the smooth path constraint information. The control points corresponding to the polyline smooth path include control points corresponding to the starting segment smooth path, the middle segment smooth path, and the ending segment smooth path, respectively.

[0009] Step 3: Obtain global control points for the smooth path based on the control points corresponding to the multi-segment smooth path, and perform cubic quasi-uniform B-spline curve fitting on the global control points to generate a global smooth path.

[0010] Furthermore, the starting pose constraint mentioned in step 1 is: given the pose of the agent at the starting point as (x start ,y start ,ψ start The direction vector of the first segment of the polyline is u0. The smooth path of the initial segment must pass through the starting point of the polyline path and be able to travel from the starting point with a heading angle ψ. start Turn to the u0 direction, where x start ,y start Let x be the starting position coordinates of the agent; the endpoint pose constraint is: given the agent's pose at the endpoint as (x, y) stop ,y stop ,ψ stop The direction vector of the final segment of the polyline is u. n The smooth path of the terminating segment needs to pass through the endpoint of the polyline and be able to reach from u n The direction is turned to the required endpoint heading angle ψ. stop , where x stop ,y stop The coordinates are the endpoint positions of the agent; the maximum curvature constraint is: the curvature value of the smooth path is less than or equal to the maximum curvature κ that can be executed when turning. max .

[0011] Furthermore, in step 2, the control points corresponding to each segment of the multi-segment smoothing path form a control polygon, and four consecutive control points in each segment form an isosceles trapezoid.

[0012] Furthermore, the process of generating control points corresponding to the initial smooth path in step 2 includes:

[0013] The control points consist of 7 points, denoted as P0, P1, ..., P6. The positions of P0, P1, and P2 are the coordinates of the starting point. P2, P3, P4, and P5 form an isosceles trapezoid. The positions of control points P3, P4, P5, and P6 are generated according to the following formula:

[0014] P3 = P2 + L side R(-θ0)u0

[0015]

[0016] P5 = P4 + L side R(k θ θ0)u0

[0017]

[0018] In the formula, L side Represents the values ​​of |P2P3| and |P4P5|; R(-θ0) represents the vector rotation matrix, where θ0 represents the angle of rotation from the starting heading to the first segment direction vector u0 of the polyline. WPT0 is the first path point, WPT1 is the second path point; β0 represents ∠P4P2WPT1; L top Represents the value of |P3P4|; k θ k represents the lateral angle proportionality coefficient of an isosceles trapezoid, 0 < k θ <1.

[0019] Furthermore, in step 2, the lengths of the two sides of the isosceles trapezoid formed by the control points P2, P3, P4, and P5 corresponding to the initial smooth path segment are L. side The calculation formula is:

[0020] L side =k side L top

[0021] In the formula, k side k is the side ratio coefficient. side ≥1;

[0022] The length of the apex of an isosceles trapezoid is L. top The calculation formula is:

[0023]

[0024] In the formula, This represents the angle between the apex and lateral sides of an isosceles trapezoid.

[0025] The formula for calculating the turning angle θ0 when the course changes from the starting point to the first segment direction vector u0 of the polyline is:

[0026]

[0027] Furthermore, the process of generating control points for the smooth path in the middle segment in step 2 includes:

[0028] The smooth path in the middle section contains multiple corners, and the i-th corner θ in the middle section... i Corresponding to 6 control points, represented as in Forming an isosceles trapezoid; based on different interpolation boundary angles, it is divided into inner control point interpolation and outer control point interpolation, then the control points... The positions generated are different; among them,

[0029] Corresponding to the interpolation of the inner control points, control points and Overlapping positions and If the positions overlap, then The formula for calculating the position is:

[0030]

[0031]

[0032]

[0033]

[0034]

[0035]

[0036] Corresponding to outer control point interpolation, control points The formula for calculating the position is:

[0037]

[0038]

[0039]

[0040]

[0041]

[0042]

[0043] In the formula, WPT i R represents the path point at the i-th corner; R(.) represents the vector rotation matrix; L top express The value of L; side express and The value of L; bot express and The value of α; i This represents the angle between the apex and lateral sides of an isosceles trapezoid. Indicates the angle θ i The two direction vectors on both sides of the point.

[0044] Furthermore, the control points corresponding to the smooth path in the middle segment in step 2. In the isosceles trapezoid formed by the two sides, the length L is... side The calculation formula is:

[0045] L side =k side L top

[0046] In the formula, k side k is the side ratio coefficient. side ≥1;

[0047] The length of the apex of an isosceles trapezoid is L. top The calculation formula is:

[0048]

[0049] In the formula, the interpolation value corresponds to the inner control point. Corresponding to the interpolation of the outer control points,

[0050] and The value L bot The calculation formula is:

[0051] L bot =k bot L top

[0052] In the formula, k bot k is the side ratio coefficient. bot >0.

[0053] Furthermore, in step 2, during the generation of control points corresponding to the smooth path of the intermediate segment, based on the polyline path information WPT0, WPT1, ..., WPT... n Calculate the corner θ between consecutive paths i Where i = 1, 2, 3, ..., n-1:

[0054]

[0055] Traverse multiple corners in the smooth path of the middle section and determine the corner θ. i Interpolation boundary angle θ min The size relationship: when θ i ≥θ min When interpolating the inner control points, when θ i <θ min External control point interpolation is performed at that time.

[0056] Furthermore, the process of generating control points corresponding to the smooth path of the termination segment in step 2 includes:

[0057] The control points consist of 7 points, denoted as P. m P m-1 P m-2 P m-3 P m-4 P m-5 P m-6 , where P m P m-1 P m-2 The positions are all the coordinates of the termination point, P m-2 P m-3 P m-4 P m-5 Forming an isosceles trapezoid; control point P m-3 P m-4 P m-5 P m-6 The position is generated according to the following formula:

[0058] P m-3 =P m-2 +L side R(θ n )u n

[0059]

[0060] P m-5 =P m-4 +L side R(-k θ θ n )u n

[0061]

[0062] In the formula, L side Represents both sides |P m-2 P m-3 |and|P m-4 P m-5 The value of |, R(θ) n ) represents the vector rotation matrix, θ n Indicates the direction u from the end segment of the polyline n Turn to destination heading ψ stop The corner, WPT n-1 With WPT n For the last two path points; β n ∠P m-4 P m-2 WPT n-1 L top|P m-3 P m-4 The value of |k θ The lateral angle proportionality coefficient, 0 < k θ <1.

[0063] Furthermore, the corresponding control point P in step 2 m-2 P m-3 P m-4 P m-5 In the isosceles trapezoid formed by the two sides, the lengths are L. side The calculation formula is:

[0064] L side =k side L top

[0065] In the formula, k side k is the side ratio coefficient. side >1; the length of the apex of the isosceles trapezoid is L. top The calculation formula is:

[0066]

[0067] In the formula, This represents the angle between the top side and the side side of the isosceles trapezoid;

[0068] The direction vector u of the end segment of the polyline n The turning angle θ towards the destination heading n The calculation formula is:

[0069]

[0070] The beneficial technical effects of this invention are:

[0071] This invention proposes a multi-constraint agent path smoothing method based on B-spline curves. By inserting a finite number of B-spline curve control points into the start, middle, and end segments of the agent's multi-segment path, the smoothing process of the multi-segment path is completed, so that the smoothed path satisfies the start pose constraint, the end pose constraint, and strictly constrains the maximum curvature, ensuring the approximation effect of the smoothed path on the multi-segment path.

[0072] This invention can ensure that the path is within C when the starting pose and ending pose are constrained. 2 Under certain conditions, it smoothly transitions to a polyline path and can precisely control the maximum curvature of the smoothed path; among them, two smoothing schemes are proposed at the inflection points of the polyline path, and based on the set parameters, it can achieve smoothing in C... 2 Under certain conditions, it makes the smooth path more closely approximate the polyline path and can precisely control the maximum curvature at the inflection point.

[0073] This invention can be combined with any other path planning method. As long as the agent's polyline path information and the constraint information of the smooth path are given, path smoothing can be achieved, which has good practicality. Attached Figure Description

[0074] The present invention can be better understood by referring to the description given below in conjunction with the accompanying drawings, which together with the detailed description below are incorporated in and form a part of this specification, and serve to further illustrate preferred embodiments of the invention and explain the principles and advantages of the invention.

[0075] Figure 1 This is a flowchart of a multi-constraint intelligent agent path smoothing method based on B-spline curves according to an embodiment of the present invention;

[0076] Figure 2 This is a schematic diagram of the multi-segment line path information and the starting and ending poses of the agent in an embodiment of the present invention;

[0077] Figure 3 This is a schematic diagram of the insertion of a control polygon into the starting segment in an embodiment of the present invention;

[0078] Figure 4 This is a schematic diagram of the control polygon for interpolation of inner control points in an embodiment of the present invention.

[0079] Figure 5 This is a schematic diagram of the control polygon for interpolation of the outer control points in an embodiment of the present invention;

[0080] Figure 6 This is a schematic diagram of the control polygon for inserting the termination segment in an embodiment of the present invention;

[0081] Figure 7 This is a schematic diagram of the smooth path generated after inserting control points in an embodiment of the present invention;

[0082] Figure 8 This is a parameter curve diagram of the smooth path in an embodiment of the present invention. Detailed Implementation

[0083] To enable those skilled in the art to better understand the present invention, exemplary embodiments or examples of the present invention will be described below in conjunction with the accompanying drawings. Obviously, the described embodiments or examples are merely some, not all, of the embodiments or examples of the present invention. All other embodiments or examples obtained by those skilled in the art based on the embodiments or examples of the present invention without inventive effort should fall within the scope of protection of the present invention.

[0084] For a polyline path {WPT0, WPT1, ..., WPT} with known path points...n Path smoothing involves generating a smooth path that satisfies multiple constraints based on the path points of a polyline, ensuring that the smooth path deviates minimally from the original polyline path. These constraints include C... 2 Continuity constraints, starting pose constraints, ending pose constraints, and maximum curvature constraints. For unmanned surface vessels, C 2 The continuity constraint requires that when an unmanned surface vessel moves on water, the smooth path must ensure continuity of path, velocity, and acceleration at every point; the starting pose constraint is: given the starting pose as (x start ,y start ,ψ start Given a polyline whose first segment has a direction vector of u0, a smooth path is required to pass through the starting point of the polyline path and be able to travel from the bow to the ψ direction. start Turn towards u0; the maximum curvature constraint is: the curvature value of the smooth path must be less than or equal to the maximum curvature that can be executed during the turn, i.e., κ≤κ. max The endpoint pose constraint is: given the agent's pose at the endpoint as (x... stop ,y stop ,ψ stop The direction vector of the polyline termination segment is u. n The smooth path of the terminating segment needs to pass through the endpoint of the polyline and be able to reach from u n The direction is turned to the required endpoint heading angle ψ. stop .

[0085] For path smoothing, B-spline curves are used for fitting. While B-spline curves have good tracking performance for multi-segment paths when smoothing large corners, they exhibit significant deviations when smoothing small corners.

[0086] Therefore, this invention proposes a multi-constraint agent path smoothing method based on B-spline curves, such as... Figure 1 As shown, the method includes the following steps:

[0087] Step 1: Obtain polyline path information and smooth path constraint information; wherein the polyline path information includes the starting segment polyline path information, the middle segment polyline path information, and the ending segment polyline path information; the smooth path constraint information includes the starting point pose constraint, the ending point pose constraint, and the maximum curvature constraint;

[0088] Step 2: Generate control points corresponding to the polyline smooth path of the agent based on the polyline path information and the smooth path constraint information. The control points corresponding to the polyline smooth path include control points corresponding to the starting segment smooth path, the middle segment smooth path, and the ending segment smooth path, respectively.

[0089] Step 3: Obtain global control points for the smooth path based on the control points corresponding to the multi-segment smooth path, and perform cubic quasi-uniform B-spline curve fitting on the global control points to generate a global smooth path.

[0090] The method begins with step 1. In step 1, polyline path information and smooth path constraint information are obtained.

[0091] According to an embodiment of the present invention, the polyline path information consists of n+1 location coordinates WPT0, WPT1, ..., WPT of the agent in the geodetic coordinate system. n WPT i =(x i ,y i The agent's starting position is at WPT0, and its ending position is at WPT. n The smooth path constraint information includes the starting pose constraint, the ending pose constraint, and the maximum curvature constraint. The starting pose constraint includes the starting agent pose (x... start ,y start ,ψ start Here, the intelligent agent is an unmanned ship, x start ,y start Let ψ be the initial coordinates of the unmanned surface vessel. start The initial heading angle of the unmanned vessel; the endpoint pose constraint includes the endpoint pose (x) stop ,y stop ,ψ stop ), x stop ,y stop Let ψ be the coordinates of the unmanned vessel's final destination. stop The endpoint bow angle of the unmanned vessel; the maximum curvature constraint is the maximum curvature κ that the agent can execute. max ; Polyline path information, starting point pose constraints, and ending point position constraints, such as Figure 2 As shown, in this embodiment, n = 4.

[0092] Then, step 2 is executed. In step 2, control points corresponding to the polyline smooth path of the agent are generated based on the polyline path information and the smooth path constraint information. The control points corresponding to the polyline smooth path include control points corresponding to the starting segment smooth path, the middle segment smooth path, and the ending segment smooth path, respectively. The control points corresponding to each segment of the polyline smooth path form a control polygon, and four consecutive control points in each segment form an isosceles trapezoid.

[0093] According to an embodiment of the present invention, firstly, in combination with Figure 3 Explain the process of generating control points for the smooth path in the initial segment.

[0094] Based on the initial polyline path information WPT0, WPT1, the first segment direction vector u0 of the polyline path can be obtained, with the direction of u0 pointing from the starting point to the next path point:

[0095]

[0096] Based on the starting pose constraint (x) start ,y start ,ψ start When the agent's starting point is located on the polyline path WPT0, the turning angle from the starting heading to the u0 direction can be obtained. It is defined that counterclockwise turning from the starting heading to the starting segment direction θ0 is positive, and clockwise turning to the starting segment direction θ0 is negative:

[0097]

[0098] For cubic B-spline curves, a repeatability of 3 for the initial control points will not affect the C-axis of the spline curve. 2 Since the system is continuous, control points P2 = P0 and P1 = P0 are chosen. Based on the maximum curvature constraint information κ... max The parameters of the smooth path control polygon for the initial segment can be obtained as follows:

[0099]

[0100]

[0101] L side =k side L top

[0102] Where α0 is the angle between the apex and lateral sides of the isosceles trapezoid; L side L is the length of the two sides; top Let k be the length of the apex side of the isosceles trapezoid. θ This represents the lateral angle scaling factor, 0 < k θ <1; θ0 represents the turning angle from the starting heading to the first segment direction vector u0 of the polyline; k side k represents the side proportioning coefficient. side ≥1, k side When the value is small, the curvature of the smooth path becomes uncontrollable, affecting the insertion of the control polygon at the next corner. side A larger value can prevent the smooth path from accurately approximating the polyline path. In this embodiment, k is chosen when generating the starting point control polygon. side =1.2, k θ =0.3.

[0103] For a control polygon formed by the initial segment P0, P1, ... P6, α0 represents the value of the included angles ∠P2P3P4 and ∠P3P4P5 of the control polygon, and kθ θ0 is the value of ∠P5P6P0; L top L represents the value of |P3P4|. side Let |P2P3| and |P4P5| represent the values. P0, P1, and P2 coincide with the starting coordinate WPT0. The calculation process for the control points of the initial smooth path segment is as follows:

[0104] P0 = WPT0

[0105] P1 = P0

[0106] P2 = P1

[0107] P3 = P2 + L side R(-θ0)u0

[0108]

[0109] P5 = P4 + L side R(k θ θ0)u0

[0110]

[0111] Wherein, the vector rotation matrix R(θ) is used to represent the vector rotating counterclockwise by θ degrees; d is the distance between P4 and line segment WPT0WPT1.

[0112] Next, the process of generating control points corresponding to the smooth path in the middle section will be explained.

[0113] This embodiment includes two interpolation methods: inner control point interpolation and outer control point interpolation. The two methods are distinguished by the interpolation boundary angle θ. min Based on the polyline path information WPT0, WPT1, ..., WPT n Calculate the corner θ between consecutive paths i Where i = 0, 1, ..., n-1:

[0114]

[0115] Traverse multiple corners in the smooth path of the middle section and determine the corner θ. i Interpolation boundary angle θ min The size relationship: when θ i ≥θ min When interpolating the inner control points, when θ i <θ min External control point interpolation is performed at that time.

[0116] Interpolation boundary angle θ min The calculation process is shown below.

[0117] Assuming we are performing inner control point interpolation, at the top edge L of the control polygon top Take the maximum value L max At that time, according to the maximum curvature constraint κ max The angle α of the control polygon can be obtained. min Implicit functions:

[0118]

[0119] Where, k side k is the side ratio coefficient. side ≥1; L max The value of L determines the degree to which the smooth path approximates the polyline path and the path length. max The smaller the value, the more corners are interpolated for the outer control points along the same polyline path, the longer the smooth path, and the closer the smooth path is to the polyline path; L max The larger the value, the more corners the inner control points interpolate on the same polyline path, the shorter the smooth path, and the less the smooth path approximates the polyline path.

[0120] Angle α can be solved using implicit functions. min α min To control the minimum value of the polygon's vertex angle; according to α min It can solve for the interpolation decomposition angle θ between the inner and outer control point interpolations. min :

[0121] θ min =|2α min -π|

[0122] Combination Figure 4 This section explains the interpolation method for the corresponding inner control points and the process for generating control points for the smooth path in the middle section.

[0123] According to θ i It can calculate the included angle α of the control polygon. i :

[0124]

[0125] Based on the maximum curvature constraint information κ max Controlling the included angle α of the polygon i With the side proportionality coefficient k side Calculate the side length L of the control polygon top With L side :

[0126]

[0127] L side =k side L top

[0128] Where, k side k is the side ratio coefficient. side ≥1, k side Taking a value that is too large will worsen the effect of the smooth path approximating the polyline path, while taking a value that is too small will make the curvature uncontrollable. In this embodiment, k is taken when interpolating the inner control points. side =1.4.

[0129] The smooth path in the middle section contains multiple corners, for each corner θ i Cause The resulting control polygon, α i Indicates the angle of the control polygon and The value of L top express The value of L side express and The value of; where and Overlapping positions and The positions coincide. Given a corner θ. i The direction vectors v on both sides i with w i , The corner angle θ can then be calculated based on the side length and geometric relationship of the control polygon. i The coordinates of the control point inserted at this location are as follows:

[0130]

[0131]

[0132]

[0133]

[0134]

[0135]

[0136] Combination Figure 5 This section explains the interpolation method for the outer control points and the process for generating control points for the smooth path in the middle section.

[0137] According to θ i It can calculate the included angle α of the control polygon. i :

[0138]

[0139] Based on the maximum curvature constraint κ maxControl the included angle α of the polygon i With geometric proportionality coefficient k side and k bot Calculate the side length L of the control polygon top L side and L bot :

[0140]

[0141] L side =k side L top

[0142] L bot =k bot L top

[0143] Where, k side k is the side ratio coefficient. side ≥1, k side A value that is too large will worsen the approximation of a polyline path by the smooth path; a value that is too small will make the curvature uncontrollable; k bot k is the side ratio coefficient. bot >0, k bot Taking a value that is too large will cause the smooth path to grow, while taking a value that is too small will make the curvature uncontrollable. In this example, k is taken when interpolating the outer control points. bot =0.5.

[0144] For the corner θ i Cause The resulting control polygon, α i Indicates the angle of the control polygon and The value of L top express The value of L side express and The value of L; bot express and The value of θ. Given the angle θ. i The direction vectors v on both sides i with w i , The corner angle θ can then be calculated based on the side length and geometric relationship of the control polygon. i The coordinates of the control point inserted at this location are:

[0145]

[0146]

[0147]

[0148]

[0149]

[0150]

[0151] Finally, combining Figure 6 Explain the process of generating control points for the smooth path of the terminal segment.

[0152] Based on the WPT (Multi-Segment Line Path Information) of the Termination Segment n-1 WPT n It can calculate the direction vector u of the terminating segment of a multi-segment path. n u n The direction from WPT n-1 Pointing to the endpoint of the polyline path WPT n :

[0153]

[0154] Based on the endpoint pose constraint information (x) stop ,y stop ,ψ stop When the agent's endpoint is located on a polyline path WPT n At that time, we can obtain u n The angle of change from the final direction to the final heading is defined as θ, which is a counter-clockwise turn from the final direction to the final heading. n If positive, turn clockwise to the destination heading θ n Negative:

[0155]

[0156] Similar to the insertion of the starting control polygon, for cubic quasi-uniform B-spline curves, a repeatability of 3 at the termination control point will also not affect the C-shape of the spline curve. 2 Since it is continuous, we take control point P. n-2 =P n-1 =P n According to the maximum curvature constraint κ max The parameters of the smooth path control polygon for the terminating segment can be obtained as follows:

[0157]

[0158]

[0159] L side =k side L top

[0160] Where, kside k is the side ratio coefficient. side ≥1; k θ Used to represent the lateral angle scaling factor, 0 < k θ <1; The impact of both on the smooth path is similar to that of the starting segment. In this embodiment, k is taken when generating the endpoint control polygon. side =1.2, k θ =0.3.

[0161] For the terminating segment P m P m-1 P m-2 P m-3 P m-4 P m-5 P m-6 The control polygon formed, α n Used to represent the included angle ∠P of the control polygon. m-5 P m-4 P m-3 With ∠P m-4 P m-3 P m-2 The value of k θ θ n For ∠P m-5 P m-6 P m The value of L; top |P m-4 P m-3 The value of |, L side |P m-2 P m-3 |and|P m-4 P m-5 The value of |P m P m-1 P m-2 and the endpoint coordinates (x) stop y stop If the control points of the smooth path of the terminal segment overlap, the calculation process is as follows:

[0162] P m =WPT n

[0163] P m-1 =P m

[0164] P m-2 =P m-1

[0165] P m-3 =P m-2 +L side R(θ n )u n

[0166]

[0167] P m-5 =P m-4 +L side R(-k θn )u n

[0168]

[0169] Wherein, the vector rotation matrix R(θ) is used to represent the vector's counterclockwise rotation by θ degrees, and d is P m-4 With line segment WPT n-1 WPT n The distance.

[0170] Finally, step 3 is executed. In step 3, global control points of the smooth path are obtained based on the control points corresponding to the multi-segment smooth path, and cubic quasi-uniform B-spline curve fitting is performed on the global control points to generate a global smooth path.

[0171] According to embodiments of the present invention, in combination Figure 7 and Figure 8 This describes the generated global smooth path. Based on the coordinates of multiple control points P = {P0, P1, ..., P2} obtained in step 2, the path consists of multiple paths. m It can generate a smooth cubic B-spline curve C(u):

[0172]

[0173]

[0174]

[0175] Where, N j，0 (u) is the basis function of the spline curve, u is a non-decreasing vector node, and p is the degree of the spline curve. In this embodiment, a quasi-uniform cubic B-spline curve is used, and p = 3 is taken. For m+1 control points with n+1 path points, the relationship between parameters m and n is: m = 6(n+1) + 1.

[0176] according to Figure 7 The shape of the smooth path shown illustrates that the smooth path generated by this invention satisfies the starting point pose constraint and the ending point pose constraint. Figure 8 The path curve, first derivative curve, and second derivative curve shown illustrate that the smooth path generated by this invention satisfies C. 2 Continuous; the curvature curve illustrates that the smooth path generated by this invention satisfies the maximum curvature constraint.

[0177] Although the invention has been described with respect to a limited number of embodiments, those skilled in the art will understand from the foregoing description that other embodiments are conceivable within the scope of the invention described herein. The disclosure of the invention is illustrative and not restrictive, and the scope of the invention is defined by the appended claims.

Claims

1. A multi-constraint intelligent agent path smoothing method based on B-spline curves, characterized in that, Includes the following steps: Step 1: Obtain polyline path information and smooth path constraint information; wherein the polyline path information includes the starting segment polyline path information, the middle segment polyline path information, and the ending segment polyline path information; the smooth path constraint information includes the starting point pose constraint, the ending point pose constraint, and the maximum curvature constraint; Step 2: Generate control points corresponding to the smoothed polyline path of the agent based on the polyline path information and the smoothed path constraint information. The control points corresponding to the smoothed polyline path include control points corresponding to the starting smoothed path, the middle smoothed path, and the ending smoothed path, respectively. The process of generating control points corresponding to the starting smoothed path includes: The control points consist of 7 points, represented as follows: ,in , , All positions are the coordinates of the starting point. , , , Forming an isosceles trapezoid; control points , , , The position is generated according to the following formula: ； In the formula, Indicates the length of the two sides, i.e. and The value, , This is the side proportion coefficient. ; Represents a vector rotation matrix. This represents the direction vector from the starting heading to the first segment of the polyline. The corner, , WPT0 is the first path point, and WPT1 is the second path point; express ; This represents the length of the apex side of an isosceles trapezoid, i.e. The value, , , which represents the value of the angle between the top side and the side side of an isosceles trapezoid; This represents the lateral angle proportionality coefficient of an isosceles trapezoid. ; The process of generating control points for the smooth path in the middle section includes: The smooth path in the middle section contains multiple corners, the middle section's first... a corner Corresponding to 6 control points, represented as , ,in Forming an isosceles trapezoid; based on different interpolation boundary angles, it is divided into inner control point interpolation and outer control point interpolation, then the control points... , The positions generated are different; among them, the interpolation of the corresponding inner control points, the control points and Overlapping positions If the positions overlap, then , The formula for calculating the position is: ； Corresponding to outer control point interpolation, control points , The formula for calculating the position is: ； In the formula, R represents the path point at the i-th corner; R(.) represents the vector rotation matrix; This represents the length of the apex side of an isosceles trapezoid, i.e. The value, Interpolation of the corresponding inner control points ; Corresponding to the interpolation of the outer control points, ; Indicates the length of the two sides, i.e. and The value, , This is the side proportion coefficient. ; and The value, = , This is the side proportion coefficient. >0; This represents the angle between the apex and lateral sides of an isosceles trapezoid. , , indicating a corner The two directional vectors on both sides are based on the multi-segment path information. Calculate the corners between consecutive paths , : ； Traverse multiple corners in the smooth path of the middle section and determine the corners. Interpolation boundary angle Size relationship: when When interpolating the inner control points, Interpolate the outer control points at the time; The process of generating control points for the smooth path of the terminal segment includes: The control points consist of 7 points, represented as follows: , , , , , , ,in , , The positions are all the coordinates of the termination point. , , , Forming an isosceles trapezoid; control points , , , The position is generated according to the following formula: ； In the formula, Indicates both sides and The value, , This is the side proportion coefficient. The length of the apex of the isosceles trapezoid is... The calculation formula is: ,express The value, , representing the angle between the apex and side of the isosceles trapezoid; determined by the direction vector of the final segment of the polyline. Turning angle towards the destination heading The calculation formula is: ; Represents a vector rotation matrix. Indicates the direction from the end segment of the polyline Turn to destination heading The corner, , and These are the last two path points; express ; This is the side angle proportionality coefficient. ; Step 3: Obtain global control points for the smooth path based on the control points corresponding to the multi-segment smooth path, and perform cubic quasi-uniform B-spline curve fitting on the global control points to generate a global smooth path.

2. The multi-constraint intelligent agent path smoothing method based on B-spline curves according to claim 1, characterized in that, The starting pose constraint mentioned in step 1 is: given the pose of the agent at the starting point as... The direction vector of the first segment of the polyline is The initial smooth path segment must pass through the starting point of the polyline path and be able to proceed from the starting point towards the angle. Turn to Direction, among which Let be the starting position coordinates of the agent; the endpoint pose constraint is: given the agent's pose at the endpoint as The direction vector of the final segment of the polyline is The smooth path of the terminal segment needs to pass through the endpoint of the polyline and be able to... The direction is turned to the required endpoint heading angle. ,in The coordinates are the endpoint positions of the agent; the maximum curvature constraint is: the curvature value of the smooth path is less than or equal to the maximum curvature that can be executed when turning. .

3. The multi-constraint intelligent agent path smoothing method based on B-spline curves according to claim 2, characterized in that, In step 2, the control points corresponding to each segment of the smoothed multi-segment path form a control polygon, and four consecutive control points in each segment form an isosceles trapezoid.

Images